Repository: newton-physics/newton
Branch: main
Commit: 04e4be2554be
Files: 730
Total size: 16.8 MB
Directory structure:
gitextract_nsnm_v9o/
├── .claude/
│ └── skills/
│ └── newton-api-design/
│ └── SKILL.md
├── .coderabbit.yml
├── .gitattributes
├── .github/
│ ├── ISSUE_TEMPLATE/
│ │ ├── 1-bug-report.yml
│ │ ├── 2-feature-request.yml
│ │ ├── 3-documentation.yml
│ │ └── config.yml
│ ├── PULL_REQUEST_TEMPLATE.md
│ ├── codecov.yml
│ └── workflows/
│ ├── aws_gpu_benchmarks.yml
│ ├── aws_gpu_tests.yml
│ ├── ci.yml
│ ├── docs-dev.yml
│ ├── docs-release.yml
│ ├── merge_queue_aws_gpu.yml
│ ├── minimum_deps_tests.yml
│ ├── mujoco_warp_tests.yml
│ ├── pr.yml
│ ├── pr_closed.yml
│ ├── pr_license_check.yml
│ ├── pr_target_aws_gpu_benchmarks.yml
│ ├── pr_target_aws_gpu_tests.yml
│ ├── push_aws_gpu.yml
│ ├── release.yml
│ ├── scheduled_nightly.yml
│ └── warp_nightly_tests.yml
├── .gitignore
├── .licenserc-docs.yaml
├── .licenserc.yaml
├── .pre-commit-config.yaml
├── .python-version
├── AGENTS.md
├── CHANGELOG.md
├── CITATION.cff
├── CLAUDE.md
├── CODE_OF_CONDUCT.md
├── CONTRIBUTING.md
├── LICENSE.md
├── README.md
├── SECURITY.md
├── asv/
│ └── benchmarks/
│ ├── __init__.py
│ ├── benchmark_ik.py
│ ├── benchmark_mujoco.py
│ ├── compilation/
│ │ ├── __init__.py
│ │ └── bench_example_load.py
│ ├── setup/
│ │ ├── __init__.py
│ │ └── bench_model.py
│ └── simulation/
│ ├── __init__.py
│ ├── bench_anymal.py
│ ├── bench_cable.py
│ ├── bench_cloth.py
│ ├── bench_contacts.py
│ ├── bench_heightfield.py
│ ├── bench_ik.py
│ ├── bench_mujoco.py
│ ├── bench_quadruped_xpbd.py
│ ├── bench_selection.py
│ ├── bench_sensor_tiled_camera.py
│ └── bench_viewer.py
├── asv.conf.json
├── docs/
│ ├── Makefile
│ ├── _ext/
│ │ ├── autodoc_filter.py
│ │ └── autodoc_wpfunc.py
│ ├── _static/
│ │ ├── custom.css
│ │ ├── gh-pages-404.html
│ │ └── switcher.json
│ ├── _templates/
│ │ ├── autosummary/
│ │ │ └── class.rst
│ │ ├── class.rst
│ │ ├── genindex.html
│ │ ├── py-modindex.html
│ │ ├── search.html
│ │ └── sidebar-nav-bs.html
│ ├── api/
│ │ ├── newton.rst
│ │ ├── newton_geometry.rst
│ │ ├── newton_ik.rst
│ │ ├── newton_math.rst
│ │ ├── newton_selection.rst
│ │ ├── newton_sensors.rst
│ │ ├── newton_solvers.rst
│ │ ├── newton_solvers_style3d.rst
│ │ ├── newton_usd.rst
│ │ ├── newton_utils.rst
│ │ └── newton_viewer.rst
│ ├── concepts/
│ │ ├── articulations.rst
│ │ ├── collisions.rst
│ │ ├── conventions.rst
│ │ ├── custom_attributes.rst
│ │ ├── extended_attributes.rst
│ │ ├── mass_inertia.rst
│ │ ├── sensors.rst
│ │ ├── sites.rst
│ │ ├── usd_parsing.rst
│ │ └── worlds.rst
│ ├── conf.py
│ ├── faq.rst
│ ├── generate_api.py
│ ├── guide/
│ │ ├── development.rst
│ │ ├── installation.rst
│ │ ├── overview.rst
│ │ ├── release.rst
│ │ ├── tutorials.rst
│ │ └── visualization.rst
│ ├── images/
│ │ └── examples/
│ │ ├── resize.bat
│ │ └── resize.sh
│ ├── index.rst
│ ├── integrations/
│ │ ├── index.rst
│ │ ├── isaac-lab.rst
│ │ └── mujoco.rst
│ ├── make.bat
│ ├── migration.rst
│ ├── print_api.py
│ ├── serve.py
│ └── tutorials/
│ └── 00_introduction.ipynb
├── newton/
│ ├── __init__.py
│ ├── _src/
│ │ ├── core/
│ │ │ ├── __init__.py
│ │ │ └── types.py
│ │ ├── geometry/
│ │ │ ├── __init__.py
│ │ │ ├── broad_phase_common.py
│ │ │ ├── broad_phase_nxn.py
│ │ │ ├── broad_phase_sap.py
│ │ │ ├── collision_convex.py
│ │ │ ├── collision_core.py
│ │ │ ├── collision_primitive.py
│ │ │ ├── contact_data.py
│ │ │ ├── contact_reduction.py
│ │ │ ├── contact_reduction_global.py
│ │ │ ├── contact_reduction_hydroelastic.py
│ │ │ ├── differentiable_contacts.py
│ │ │ ├── flags.py
│ │ │ ├── hashtable.py
│ │ │ ├── inertia.py
│ │ │ ├── kernels.py
│ │ │ ├── mpr.py
│ │ │ ├── multicontact.py
│ │ │ ├── narrow_phase.py
│ │ │ ├── raycast.py
│ │ │ ├── remesh.py
│ │ │ ├── sdf_contact.py
│ │ │ ├── sdf_hydroelastic.py
│ │ │ ├── sdf_mc.py
│ │ │ ├── sdf_texture.py
│ │ │ ├── sdf_utils.py
│ │ │ ├── simplex_solver.py
│ │ │ ├── support_function.py
│ │ │ ├── terrain_generator.py
│ │ │ ├── types.py
│ │ │ └── utils.py
│ │ ├── math/
│ │ │ ├── __init__.py
│ │ │ └── spatial.py
│ │ ├── sensors/
│ │ │ ├── __init__.py
│ │ │ ├── sensor_contact.py
│ │ │ ├── sensor_frame_transform.py
│ │ │ ├── sensor_imu.py
│ │ │ ├── sensor_raycast.py
│ │ │ ├── sensor_tiled_camera.py
│ │ │ └── warp_raytrace/
│ │ │ ├── __init__.py
│ │ │ ├── bvh.py
│ │ │ ├── gaussians.py
│ │ │ ├── lighting.py
│ │ │ ├── ray_intersect.py
│ │ │ ├── raytrace.py
│ │ │ ├── render.py
│ │ │ ├── render_context.py
│ │ │ ├── textures.py
│ │ │ ├── tiling.py
│ │ │ ├── types.py
│ │ │ └── utils.py
│ │ ├── sim/
│ │ │ ├── __init__.py
│ │ │ ├── articulation.py
│ │ │ ├── builder.py
│ │ │ ├── collide.py
│ │ │ ├── contacts.py
│ │ │ ├── control.py
│ │ │ ├── enums.py
│ │ │ ├── graph_coloring.py
│ │ │ ├── ik/
│ │ │ │ ├── __init__.py
│ │ │ │ ├── ik_common.py
│ │ │ │ ├── ik_lbfgs_optimizer.py
│ │ │ │ ├── ik_lm_optimizer.py
│ │ │ │ ├── ik_objectives.py
│ │ │ │ └── ik_solver.py
│ │ │ ├── model.py
│ │ │ └── state.py
│ │ ├── solvers/
│ │ │ ├── __init__.py
│ │ │ ├── featherstone/
│ │ │ │ ├── __init__.py
│ │ │ │ ├── kernels.py
│ │ │ │ └── solver_featherstone.py
│ │ │ ├── flags.py
│ │ │ ├── implicit_mpm/
│ │ │ │ ├── __init__.py
│ │ │ │ ├── contact_solver_kernels.py
│ │ │ │ ├── implicit_mpm_model.py
│ │ │ │ ├── implicit_mpm_solver_kernels.py
│ │ │ │ ├── rasterized_collisions.py
│ │ │ │ ├── render_grains.py
│ │ │ │ ├── rheology_solver_kernels.py
│ │ │ │ ├── solve_rheology.py
│ │ │ │ └── solver_implicit_mpm.py
│ │ │ ├── kamino/
│ │ │ │ ├── README.md
│ │ │ │ ├── __init__.py
│ │ │ │ ├── _src/
│ │ │ │ │ ├── __init__.py
│ │ │ │ │ ├── core/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── bodies.py
│ │ │ │ │ │ ├── builder.py
│ │ │ │ │ │ ├── control.py
│ │ │ │ │ │ ├── conversions.py
│ │ │ │ │ │ ├── data.py
│ │ │ │ │ │ ├── geometry.py
│ │ │ │ │ │ ├── gravity.py
│ │ │ │ │ │ ├── inertia.py
│ │ │ │ │ │ ├── joints.py
│ │ │ │ │ │ ├── materials.py
│ │ │ │ │ │ ├── math.py
│ │ │ │ │ │ ├── model.py
│ │ │ │ │ │ ├── shapes.py
│ │ │ │ │ │ ├── size.py
│ │ │ │ │ │ ├── state.py
│ │ │ │ │ │ ├── time.py
│ │ │ │ │ │ ├── types.py
│ │ │ │ │ │ └── world.py
│ │ │ │ │ ├── dynamics/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── delassus.py
│ │ │ │ │ │ ├── dual.py
│ │ │ │ │ │ └── wrenches.py
│ │ │ │ │ ├── geometry/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── aggregation.py
│ │ │ │ │ │ ├── contacts.py
│ │ │ │ │ │ ├── detector.py
│ │ │ │ │ │ ├── keying.py
│ │ │ │ │ │ ├── primitive/
│ │ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ │ ├── broadphase.py
│ │ │ │ │ │ │ ├── narrowphase.py
│ │ │ │ │ │ │ └── pipeline.py
│ │ │ │ │ │ └── unified.py
│ │ │ │ │ ├── integrators/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── euler.py
│ │ │ │ │ │ ├── integrator.py
│ │ │ │ │ │ └── moreau.py
│ │ │ │ │ ├── kinematics/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── constraints.py
│ │ │ │ │ │ ├── jacobians.py
│ │ │ │ │ │ ├── joints.py
│ │ │ │ │ │ ├── limits.py
│ │ │ │ │ │ └── resets.py
│ │ │ │ │ ├── linalg/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── blas.py
│ │ │ │ │ │ ├── conjugate.py
│ │ │ │ │ │ ├── core.py
│ │ │ │ │ │ ├── factorize/
│ │ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ │ ├── llt_blocked.py
│ │ │ │ │ │ │ ├── llt_blocked_semi_sparse.py
│ │ │ │ │ │ │ └── llt_sequential.py
│ │ │ │ │ │ ├── linear.py
│ │ │ │ │ │ ├── sparse_matrix.py
│ │ │ │ │ │ ├── sparse_operator.py
│ │ │ │ │ │ └── utils/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── matrix.py
│ │ │ │ │ │ ├── rand.py
│ │ │ │ │ │ └── range.py
│ │ │ │ │ ├── models/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── assets/
│ │ │ │ │ │ │ ├── basics/
│ │ │ │ │ │ │ │ ├── box_on_plane.usda
│ │ │ │ │ │ │ │ ├── box_pendulum.usda
│ │ │ │ │ │ │ │ ├── boxes_fourbar.usda
│ │ │ │ │ │ │ │ ├── boxes_hinged.usda
│ │ │ │ │ │ │ │ ├── boxes_nunchaku.usda
│ │ │ │ │ │ │ │ └── cartpole.usda
│ │ │ │ │ │ │ └── testing/
│ │ │ │ │ │ │ ├── geoms/
│ │ │ │ │ │ │ │ ├── test_geom_box.usda
│ │ │ │ │ │ │ │ ├── test_geom_capsule.usda
│ │ │ │ │ │ │ │ ├── test_geom_cone.usda
│ │ │ │ │ │ │ │ ├── test_geom_cylinder.usda
│ │ │ │ │ │ │ │ ├── test_geom_ellipsoid.usda
│ │ │ │ │ │ │ │ └── test_geom_sphere.usda
│ │ │ │ │ │ │ └── joints/
│ │ │ │ │ │ │ ├── test_joint_cartesian_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_cartesian_actuated_unary.usda
│ │ │ │ │ │ │ ├── test_joint_cartesian_passive.usda
│ │ │ │ │ │ │ ├── test_joint_cartesian_passive_unary.usda
│ │ │ │ │ │ │ ├── test_joint_cylindrical_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_cylindrical_actuated_unary.usda
│ │ │ │ │ │ │ ├── test_joint_cylindrical_passive.usda
│ │ │ │ │ │ │ ├── test_joint_cylindrical_passive_unary.usda
│ │ │ │ │ │ │ ├── test_joint_d6_6dof_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_d6_6dof_passive.usda
│ │ │ │ │ │ │ ├── test_joint_d6_cartesian_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_d6_cartesian_passive.usda
│ │ │ │ │ │ │ ├── test_joint_d6_cylindrical_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_d6_cylindrical_passive.usda
│ │ │ │ │ │ │ ├── test_joint_d6_prismatic_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_d6_prismatic_passive.usda
│ │ │ │ │ │ │ ├── test_joint_d6_revolute_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_d6_revolute_passive.usda
│ │ │ │ │ │ │ ├── test_joint_d6_spherical_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_d6_spherical_passive.usda
│ │ │ │ │ │ │ ├── test_joint_d6_universal_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_d6_universal_passive.usda
│ │ │ │ │ │ │ ├── test_joint_fixed.usda
│ │ │ │ │ │ │ ├── test_joint_fixed_unary.usda
│ │ │ │ │ │ │ ├── test_joint_prismatic_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_prismatic_actuated_unary.usda
│ │ │ │ │ │ │ ├── test_joint_prismatic_passive.usda
│ │ │ │ │ │ │ ├── test_joint_prismatic_passive_unary.usda
│ │ │ │ │ │ │ ├── test_joint_revolute_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_revolute_actuated_unary.usda
│ │ │ │ │ │ │ ├── test_joint_revolute_passive.usda
│ │ │ │ │ │ │ ├── test_joint_revolute_passive_unary.usda
│ │ │ │ │ │ │ ├── test_joint_spherical.usda
│ │ │ │ │ │ │ ├── test_joint_spherical_unary.usda
│ │ │ │ │ │ │ ├── test_joint_universal_actuated.usda
│ │ │ │ │ │ │ ├── test_joint_universal_actuated_unary.usda
│ │ │ │ │ │ │ ├── test_joint_universal_passive.usda
│ │ │ │ │ │ │ └── test_joint_universal_passive_unary.usda
│ │ │ │ │ │ └── builders/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── basics.py
│ │ │ │ │ │ ├── basics_newton.py
│ │ │ │ │ │ ├── testing.py
│ │ │ │ │ │ └── utils.py
│ │ │ │ │ ├── solver_kamino_impl.py
│ │ │ │ │ ├── solvers/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── fk.py
│ │ │ │ │ │ ├── metrics.py
│ │ │ │ │ │ ├── padmm/
│ │ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ │ ├── kernels.py
│ │ │ │ │ │ │ ├── math.py
│ │ │ │ │ │ │ ├── solver.py
│ │ │ │ │ │ │ └── types.py
│ │ │ │ │ │ └── warmstart.py
│ │ │ │ │ └── utils/
│ │ │ │ │ ├── __init__.py
│ │ │ │ │ ├── benchmark/
│ │ │ │ │ │ ├── .gitignore
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── __main__.py
│ │ │ │ │ │ ├── configs.py
│ │ │ │ │ │ ├── metrics.py
│ │ │ │ │ │ ├── problems.py
│ │ │ │ │ │ ├── render.py
│ │ │ │ │ │ └── runner.py
│ │ │ │ │ ├── control/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── animation.py
│ │ │ │ │ │ ├── pid.py
│ │ │ │ │ │ └── rand.py
│ │ │ │ │ ├── device.py
│ │ │ │ │ ├── io/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ └── usd.py
│ │ │ │ │ ├── logger.py
│ │ │ │ │ ├── profiles.py
│ │ │ │ │ ├── sim/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── datalog.py
│ │ │ │ │ │ ├── runner.py
│ │ │ │ │ │ ├── simulator.py
│ │ │ │ │ │ └── viewer.py
│ │ │ │ │ ├── sparse.py
│ │ │ │ │ └── viewer.py
│ │ │ │ ├── config.py
│ │ │ │ ├── examples/
│ │ │ │ │ ├── .gitignore
│ │ │ │ │ ├── __init__.py
│ │ │ │ │ ├── newton/
│ │ │ │ │ │ ├── example_anymal_d.py
│ │ │ │ │ │ ├── example_dr_legs.py
│ │ │ │ │ │ └── example_fourbar.py
│ │ │ │ │ ├── reset/
│ │ │ │ │ │ └── example_reset_dr_legs.py
│ │ │ │ │ ├── rl/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── camera_follow.py
│ │ │ │ │ │ ├── example_rl_bipedal.py
│ │ │ │ │ │ ├── example_rl_drlegs.py
│ │ │ │ │ │ ├── joystick.py
│ │ │ │ │ │ ├── observations.py
│ │ │ │ │ │ ├── simulation.py
│ │ │ │ │ │ ├── simulation_runner.py
│ │ │ │ │ │ ├── test_multi_env_dr_legs.py
│ │ │ │ │ │ └── utils.py
│ │ │ │ │ └── sim/
│ │ │ │ │ ├── example_sim_basics_all_heterogeneous.py
│ │ │ │ │ ├── example_sim_basics_box_on_plane.py
│ │ │ │ │ ├── example_sim_basics_box_pendulum.py
│ │ │ │ │ ├── example_sim_basics_boxes_fourbar.py
│ │ │ │ │ ├── example_sim_basics_boxes_hinged.py
│ │ │ │ │ ├── example_sim_basics_cartpole.py
│ │ │ │ │ ├── example_sim_dr_legs.py
│ │ │ │ │ ├── example_sim_dr_testmech.py
│ │ │ │ │ ├── example_sim_test_all_geoms.py
│ │ │ │ │ └── example_sim_test_all_joints.py
│ │ │ │ ├── solver_kamino.py
│ │ │ │ └── tests/
│ │ │ │ ├── .gitignore
│ │ │ │ ├── README.md
│ │ │ │ ├── __init__.py
│ │ │ │ ├── __main__.py
│ │ │ │ ├── test_core_builder.py
│ │ │ │ ├── test_core_geometry.py
│ │ │ │ ├── test_core_joints.py
│ │ │ │ ├── test_core_materials.py
│ │ │ │ ├── test_core_model.py
│ │ │ │ ├── test_core_shapes.py
│ │ │ │ ├── test_core_world.py
│ │ │ │ ├── test_dynamics_delassus.py
│ │ │ │ ├── test_dynamics_dual.py
│ │ │ │ ├── test_dynamics_wrenches.py
│ │ │ │ ├── test_geometry_aggregation.py
│ │ │ │ ├── test_geometry_contacts.py
│ │ │ │ ├── test_geometry_detector.py
│ │ │ │ ├── test_geometry_keying.py
│ │ │ │ ├── test_geometry_margin_gap.py
│ │ │ │ ├── test_geometry_primitive.py
│ │ │ │ ├── test_geometry_unified.py
│ │ │ │ ├── test_kinematics_constraints.py
│ │ │ │ ├── test_kinematics_jacobians.py
│ │ │ │ ├── test_kinematics_joints.py
│ │ │ │ ├── test_kinematics_limits.py
│ │ │ │ ├── test_linalg_core.py
│ │ │ │ ├── test_linalg_factorize_llt_blocked.py
│ │ │ │ ├── test_linalg_factorize_llt_sequential.py
│ │ │ │ ├── test_linalg_solve_cg.py
│ │ │ │ ├── test_linalg_solver_llt_blocked.py
│ │ │ │ ├── test_linalg_solver_llt_sequential.py
│ │ │ │ ├── test_linalg_sparse.py
│ │ │ │ ├── test_linalg_utils_matrix.py
│ │ │ │ ├── test_linalg_utils_rand.py
│ │ │ │ ├── test_solver_kamino.py
│ │ │ │ ├── test_solvers_forward_kinematics.py
│ │ │ │ ├── test_solvers_metrics.py
│ │ │ │ ├── test_solvers_padmm.py
│ │ │ │ ├── test_utils_control_animation.py
│ │ │ │ ├── test_utils_control_pid.py
│ │ │ │ ├── test_utils_control_rand.py
│ │ │ │ ├── test_utils_io_usd.py
│ │ │ │ ├── test_utils_logger.py
│ │ │ │ ├── test_utils_profiles.py
│ │ │ │ ├── test_utils_random.py
│ │ │ │ ├── test_utils_sim_simulator.py
│ │ │ │ └── utils/
│ │ │ │ ├── __init__.py
│ │ │ │ ├── checks.py
│ │ │ │ ├── diff_check.py
│ │ │ │ ├── extract.py
│ │ │ │ ├── make.py
│ │ │ │ ├── print.py
│ │ │ │ └── rand.py
│ │ │ ├── mujoco/
│ │ │ │ ├── __init__.py
│ │ │ │ ├── kernels.py
│ │ │ │ └── solver_mujoco.py
│ │ │ ├── semi_implicit/
│ │ │ │ ├── __init__.py
│ │ │ │ ├── kernels_body.py
│ │ │ │ ├── kernels_contact.py
│ │ │ │ ├── kernels_muscle.py
│ │ │ │ ├── kernels_particle.py
│ │ │ │ └── solver_semi_implicit.py
│ │ │ ├── solver.py
│ │ │ ├── style3d/
│ │ │ │ ├── __init__.py
│ │ │ │ ├── builder.py
│ │ │ │ ├── cloth.py
│ │ │ │ ├── collision/
│ │ │ │ │ ├── __init__.py
│ │ │ │ │ ├── bvh/
│ │ │ │ │ │ ├── __init__.py
│ │ │ │ │ │ ├── bvh.py
│ │ │ │ │ │ └── kernels.py
│ │ │ │ │ ├── collision.py
│ │ │ │ │ ├── collision_legacy.py
│ │ │ │ │ └── kernels.py
│ │ │ │ ├── kernels.py
│ │ │ │ ├── linear_solver.py
│ │ │ │ └── solver_style3d.py
│ │ │ ├── vbd/
│ │ │ │ ├── __init__.py
│ │ │ │ ├── particle_vbd_kernels.py
│ │ │ │ ├── rigid_vbd_kernels.py
│ │ │ │ ├── solver_vbd.py
│ │ │ │ └── tri_mesh_collision.py
│ │ │ └── xpbd/
│ │ │ ├── __init__.py
│ │ │ ├── kernels.py
│ │ │ └── solver_xpbd.py
│ │ ├── usd/
│ │ │ ├── __init__.py
│ │ │ ├── schema_resolver.py
│ │ │ ├── schemas.py
│ │ │ └── utils.py
│ │ ├── utils/
│ │ │ ├── __init__.py
│ │ │ ├── benchmark.py
│ │ │ ├── cable.py
│ │ │ ├── download_assets.py
│ │ │ ├── heightfield.py
│ │ │ ├── import_mjcf.py
│ │ │ ├── import_urdf.py
│ │ │ ├── import_usd.py
│ │ │ ├── import_utils.py
│ │ │ ├── mesh.py
│ │ │ ├── render.py
│ │ │ ├── selection.py
│ │ │ ├── texture.py
│ │ │ └── topology.py
│ │ └── viewer/
│ │ ├── __init__.py
│ │ ├── camera.py
│ │ ├── gl/
│ │ │ ├── gui.py
│ │ │ ├── icon.py
│ │ │ ├── opengl.py
│ │ │ └── shaders.py
│ │ ├── kernels.py
│ │ ├── picking.py
│ │ ├── viewer.py
│ │ ├── viewer_file.py
│ │ ├── viewer_gl.py
│ │ ├── viewer_null.py
│ │ ├── viewer_rerun.py
│ │ ├── viewer_usd.py
│ │ ├── viewer_viser.py
│ │ ├── viser/
│ │ │ └── static/
│ │ │ ├── assets/
│ │ │ │ ├── Sorter-Df0J3ZWJ.wasm
│ │ │ │ ├── SplatSortWorker-DiSpcAPr.js
│ │ │ │ ├── WebsocketServerWorker-C6PJJ7Dx.js
│ │ │ │ ├── __vite-browser-external-BIHI7g3E.js
│ │ │ │ ├── index-BVvA0mmR.css
│ │ │ │ └── index-H4DT9vxj.js
│ │ │ ├── index.html
│ │ │ ├── manifest.json
│ │ │ └── robots.txt
│ │ └── wind.py
│ ├── _version.py
│ ├── examples/
│ │ ├── __init__.py
│ │ ├── __main__.py
│ │ ├── assets/
│ │ │ ├── ant.usda
│ │ │ ├── anymal_walking_policy.pt
│ │ │ ├── axis_cube.usda
│ │ │ ├── bear.usd
│ │ │ ├── boxes_fourbar.usda
│ │ │ ├── bunny.usd
│ │ │ ├── cartpole.urdf
│ │ │ ├── cartpole.usda
│ │ │ ├── cartpole_single_pendulum.usda
│ │ │ ├── crazyflie.usd
│ │ │ ├── curvedSurface.usd
│ │ │ ├── humanoid.usda
│ │ │ ├── nv_ant.xml
│ │ │ ├── nv_humanoid.xml
│ │ │ ├── quadruped.urdf
│ │ │ ├── rj45_plug.usd
│ │ │ ├── sensor_contact_scene.usda
│ │ │ ├── tabletop.xml
│ │ │ └── unisex_shirt.usd
│ │ ├── basic/
│ │ │ ├── example_basic_conveyor.py
│ │ │ ├── example_basic_heightfield.py
│ │ │ ├── example_basic_joints.py
│ │ │ ├── example_basic_pendulum.py
│ │ │ ├── example_basic_plotting.py
│ │ │ ├── example_basic_shapes.py
│ │ │ ├── example_basic_urdf.py
│ │ │ ├── example_basic_viewer.py
│ │ │ ├── example_recording.py
│ │ │ └── example_replay_viewer.py
│ │ ├── cable/
│ │ │ ├── example_cable_bundle_hysteresis.py
│ │ │ ├── example_cable_pile.py
│ │ │ ├── example_cable_twist.py
│ │ │ └── example_cable_y_junction.py
│ │ ├── cloth/
│ │ │ ├── example_cloth_bending.py
│ │ │ ├── example_cloth_franka.py
│ │ │ ├── example_cloth_h1.py
│ │ │ ├── example_cloth_hanging.py
│ │ │ ├── example_cloth_poker_cards.py
│ │ │ ├── example_cloth_rollers.py
│ │ │ ├── example_cloth_style3d.py
│ │ │ └── example_cloth_twist.py
│ │ ├── contacts/
│ │ │ ├── example_brick_stacking.py
│ │ │ ├── example_contacts_rj45_plug.py
│ │ │ ├── example_nut_bolt_hydro.py
│ │ │ ├── example_nut_bolt_sdf.py
│ │ │ └── example_pyramid.py
│ │ ├── diffsim/
│ │ │ ├── example_diffsim_ball.py
│ │ │ ├── example_diffsim_bear.py
│ │ │ ├── example_diffsim_cloth.py
│ │ │ ├── example_diffsim_drone.py
│ │ │ ├── example_diffsim_soft_body.py
│ │ │ └── example_diffsim_spring_cage.py
│ │ ├── ik/
│ │ │ ├── example_ik_cube_stacking.py
│ │ │ ├── example_ik_custom.py
│ │ │ ├── example_ik_franka.py
│ │ │ └── example_ik_h1.py
│ │ ├── mpm/
│ │ │ ├── example_mpm_anymal.py
│ │ │ ├── example_mpm_beam_twist.py
│ │ │ ├── example_mpm_grain_rendering.py
│ │ │ ├── example_mpm_granular.py
│ │ │ ├── example_mpm_multi_material.py
│ │ │ ├── example_mpm_snow_ball.py
│ │ │ ├── example_mpm_twoway_coupling.py
│ │ │ └── example_mpm_viscous.py
│ │ ├── multiphysics/
│ │ │ ├── example_softbody_dropping_to_cloth.py
│ │ │ └── example_softbody_gift.py
│ │ ├── robot/
│ │ │ ├── example_robot_allegro_hand.py
│ │ │ ├── example_robot_anymal_c_walk.py
│ │ │ ├── example_robot_anymal_d.py
│ │ │ ├── example_robot_cartpole.py
│ │ │ ├── example_robot_g1.py
│ │ │ ├── example_robot_h1.py
│ │ │ ├── example_robot_panda_hydro.py
│ │ │ ├── example_robot_policy.py
│ │ │ └── example_robot_ur10.py
│ │ ├── selection/
│ │ │ ├── example_selection_articulations.py
│ │ │ ├── example_selection_cartpole.py
│ │ │ ├── example_selection_materials.py
│ │ │ └── example_selection_multiple.py
│ │ ├── sensors/
│ │ │ ├── example_sensor_contact.py
│ │ │ ├── example_sensor_imu.py
│ │ │ └── example_sensor_tiled_camera.py
│ │ └── softbody/
│ │ ├── example_softbody_franka.py
│ │ └── example_softbody_hanging.py
│ ├── geometry.py
│ ├── ik.py
│ ├── licenses/
│ │ ├── CC-BY-4.0.txt
│ │ ├── unittest-parallel-LICENSE.txt
│ │ └── viser_and_inter-font-family.txt
│ ├── math.py
│ ├── py.typed
│ ├── selection.py
│ ├── sensors.py
│ ├── solvers.py
│ ├── tests/
│ │ ├── __init__.py
│ │ ├── __main__.py
│ │ ├── assets/
│ │ │ ├── actuator_test.usda
│ │ │ ├── ant.usda
│ │ │ ├── ant_mixed.usda
│ │ │ ├── ant_multi.usda
│ │ │ ├── cartpole_mjc.usda
│ │ │ ├── constraints.xml
│ │ │ ├── cube_cylinder.usda
│ │ │ ├── four_link_chain_articulation.usda
│ │ │ ├── humanoid.urdf
│ │ │ ├── humanoid.usda
│ │ │ ├── mjcf_exclude_hyphen_test.xml
│ │ │ ├── mjcf_exclude_test.xml
│ │ │ ├── pendulum_revolute_vs_d6.usda
│ │ │ ├── revolute_articulation.usda
│ │ │ ├── simple_articulation_with_mesh.usda
│ │ │ ├── tetmesh_custom_attrs.usda
│ │ │ ├── tetmesh_multi.usda
│ │ │ ├── tetmesh_simple.usda
│ │ │ └── tetmesh_with_material.usda
│ │ ├── golden_data/
│ │ │ └── test_sensor_tiled_camera/
│ │ │ ├── color.npy
│ │ │ └── depth.npy
│ │ ├── test_actuators.py
│ │ ├── test_anymal_reset.py
│ │ ├── test_api.py
│ │ ├── test_body_force.py
│ │ ├── test_body_velocity.py
│ │ ├── test_broad_phase.py
│ │ ├── test_cable.py
│ │ ├── test_cloth.py
│ │ ├── test_collision_cloth.py
│ │ ├── test_collision_pipeline.py
│ │ ├── test_collision_primitives.py
│ │ ├── test_coloring.py
│ │ ├── test_cone_orientation.py
│ │ ├── test_contact_reduction.py
│ │ ├── test_contact_reduction_global.py
│ │ ├── test_control_force.py
│ │ ├── test_custom_attributes.py
│ │ ├── test_differentiable_contacts.py
│ │ ├── test_download_assets.py
│ │ ├── test_environment_group_collision.py
│ │ ├── test_equality_constraints.py
│ │ ├── test_example_browser.py
│ │ ├── test_examples.py
│ │ ├── test_fixed_tendon.py
│ │ ├── test_gjk.py
│ │ ├── test_hashtable.py
│ │ ├── test_heightfield.py
│ │ ├── test_hydroelastic.py
│ │ ├── test_ik.py
│ │ ├── test_ik_fk_kernels.py
│ │ ├── test_ik_lbfgs.py
│ │ ├── test_implicit_mpm.py
│ │ ├── test_implicit_mpm_flow_rule.py
│ │ ├── test_import_mjcf.py
│ │ ├── test_import_urdf.py
│ │ ├── test_import_usd.py
│ │ ├── test_inertia.py
│ │ ├── test_inertia_validation.py
│ │ ├── test_jacobian_mass_matrix.py
│ │ ├── test_joint_controllers.py
│ │ ├── test_joint_drive.py
│ │ ├── test_joint_limits.py
│ │ ├── test_kinematic_links.py
│ │ ├── test_kinematics.py
│ │ ├── test_match_labels.py
│ │ ├── test_menagerie_mujoco.py
│ │ ├── test_menagerie_usd_mujoco.py
│ │ ├── test_mesh_aabb.py
│ │ ├── test_model.py
│ │ ├── test_mujoco_general_actuators.py
│ │ ├── test_mujoco_solver.py
│ │ ├── test_narrow_phase.py
│ │ ├── test_obb.py
│ │ ├── test_parent_force.py
│ │ ├── test_pendulum_revolute_vs_d6.py
│ │ ├── test_physics_validation.py
│ │ ├── test_raycast.py
│ │ ├── test_recorder.py
│ │ ├── test_remesh.py
│ │ ├── test_rigid_contact.py
│ │ ├── test_robot_composer.py
│ │ ├── test_runtime_gravity.py
│ │ ├── test_schema_resolver.py
│ │ ├── test_sdf_compute.py
│ │ ├── test_sdf_primitive.py
│ │ ├── test_sdf_texture.py
│ │ ├── test_selection.py
│ │ ├── test_sensor_contact.py
│ │ ├── test_sensor_frame_transform.py
│ │ ├── test_sensor_imu.py
│ │ ├── test_sensor_raycast.py
│ │ ├── test_sensor_tiled_camera.py
│ │ ├── test_sensor_tiled_camera_forward_depth.py
│ │ ├── test_sensor_tiled_camera_particles_multiworld.py
│ │ ├── test_shape_colors.py
│ │ ├── test_shapes_no_bounce.py
│ │ ├── test_sites.py
│ │ ├── test_sites_mjcf_import.py
│ │ ├── test_sites_mujoco_export.py
│ │ ├── test_sites_usd_import.py
│ │ ├── test_softbody.py
│ │ ├── test_solver_vbd.py
│ │ ├── test_solver_xpbd.py
│ │ ├── test_spatial_tendon.py
│ │ ├── test_terrain_generator.py
│ │ ├── test_tolerance_clamping.py
│ │ ├── test_up_axis.py
│ │ ├── test_viewer_geometry_batching.py
│ │ ├── test_viewer_log_shapes.py
│ │ ├── test_viewer_particle_flags.py
│ │ ├── test_viewer_picking.py
│ │ ├── test_viewer_rerun_hidden.py
│ │ ├── test_viewer_rerun_init_args.py
│ │ ├── test_viewer_usd.py
│ │ ├── test_viewer_world_offsets.py
│ │ ├── test_warp_config_cli.py
│ │ ├── thirdparty/
│ │ │ ├── __init__.py
│ │ │ └── unittest_parallel.py
│ │ └── unittest_utils.py
│ ├── usd.py
│ ├── utils.py
│ └── viewer.py
├── pyproject.toml
└── scripts/
├── check_warp_array_syntax.py
└── ci/
├── dispatch_workflow_and_wait.py
└── update_docs_switcher.py
================================================
FILE CONTENTS
================================================
================================================
FILE: .claude/skills/newton-api-design/SKILL.md
================================================
---
name: newton-api-design
description: Use when designing, adding, or reviewing public API for the Newton physics engine — class names, method signatures, type hints, docstrings, or parameter conventions. Also use when unsure if new API conforms to project conventions.
---
# Newton API Design Conventions
Detailed patterns that supplement AGENTS.md. Read AGENTS.md first for the basics (prefix-first naming, PEP 604, Google-style docstrings, SI units, Sphinx cross-refs).
## Builder Method Signature Template
All `ModelBuilder.add_shape_*` methods follow this parameter order:
```python
def add_shape_cone(
self,
body: int,
xform: Transform | None = None,
# shape-specific params here (radius, height, etc.)
radius: float = 0.5,
height: float = 1.0,
cfg: ShapeConfig | None = None,
as_site: bool = False,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a cone collision shape to a body.
Args:
body: Index of the parent body. Use -1 for static shapes.
xform: Transform in parent body's local frame. If ``None``,
identity transform is used.
radius: Cone base radius [m].
height: Cone height [m].
cfg: Shape configuration. If ``None``, uses
:attr:`default_shape_cfg`.
as_site: If ``True``, creates a site instead of a collision shape.
label: Optional label for identifying the shape.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
Index of the newly added shape.
"""
```
**Key conventions:**
- `xform` (not `tf`, `transform`, or `pose`) — always `Transform | None = None`
- `cfg` (not `config`, `shape_config`) — always `ShapeConfig | None = None`
- `body`, `label`, `custom_attributes` — standard params on all builder methods
- Defaults are `None`, not constructed objects like `wp.transform()`
## Nested Classes
Use `IntEnum` (not `Enum` with strings) for enumerations:
```python
class Model:
class AttributeAssignment(IntEnum):
MODEL = 0
STATE = 1
```
When an `IntEnum` includes a `NONE` member, define it first at `0`:
```python
class GeoType(IntEnum):
NONE = 0
PLANE = 1
HFIELD = 2
```
This keeps the sentinel value stable and leaves room to append future real
members at the end instead of inserting them before a trailing `NONE`.
Dataclass field docstrings go on the line immediately below the field:
```python
@dataclass
class ShapeConfig:
density: float = 1000.0
"""The density of the shape material."""
ke: float = 2.5e3
"""The contact elastic stiffness."""
```
## Array Documentation Format
Document shape, dtype, and units in attribute docstrings:
```python
"""Rigid body velocities [m/s, rad/s], shape (body_count,), dtype :class:`spatial_vector`."""
"""Joint forces [N or N·m], shape (joint_dof_count,), dtype float."""
"""Contact points [m], shape [count, 3], float."""
```
For compound arrays, list per-component units:
```python
"""[0] k_mu [Pa], [1] k_lambda [Pa], ..."""
```
For **public API** attributes and method signatures, use bare `wp.array | None` and document the concrete dtype in the docstring (e.g., `dtype :class:\`vec3\``). Warp kernel parameters require concrete dtypes inline (`wp.array(dtype=wp.vec3)`) per AGENTS.md.
## Quick Checklist
When reviewing new API, verify:
- [ ] Parameters use project vocabulary (`xform`, `cfg`, `body`, `label`)
- [ ] Defaults are `None`, not constructed objects
- [ ] Nested enumerations use `IntEnum` with int values
- [ ] Enumerations with `NONE` define `NONE = 0` first
- [ ] Dataclass fields have docstrings on the line below
- [ ] Array docs include shape, dtype, and units
- [ ] Builder methods include `as_site`, `label`, `custom_attributes`
================================================
FILE: .coderabbit.yml
================================================
# yaml-language-server: $schema=https://coderabbit.ai/integrations/schema.v2.json
language: "en-US"
early_access: false
reviews:
profile: "chill"
request_changes_workflow: false
high_level_summary: true
poem: false
review_status: true
collapse_walkthrough: true
auto_review:
enabled: true
drafts: true
tools:
github-checks:
timeout_ms: 900000
path_instructions:
- path: "**"
instructions: |
Check that any user-facing change includes a corresponding entry in
CHANGELOG.md under the [Unreleased] section. Entries should use
imperative present tense ("Add X", not "Added X") and be placed under
the correct category (Added, Changed, Deprecated, Removed, or Fixed).
For Deprecated, Changed, and Removed entries, verify the entry includes
migration guidance telling users what replaces the old behavior.
chat:
auto_reply: true
issue_enrichment:
auto_enrich:
enabled: false
================================================
FILE: .gitattributes
================================================
*.jpg !text !filter !merge !diff
CHANGELOG.md merge=union
================================================
FILE: .github/ISSUE_TEMPLATE/1-bug-report.yml
================================================
name: Bug Report
description: Create a report to help us improve Newton.
title: "[BUG] "
labels: ["bug"]
body:
- type: markdown
attributes:
value: |
Thanks for taking the time to fill out this bug report!
- type: textarea
attributes:
label: Bug Description
description: >-
Describe the bug. What happened, and what did you expect to happen?
validations:
required: true
- type: textarea
attributes:
label: Reproduction Script
description: >-
If possible, provide a minimal script or code snippet that reproduces
the issue.
render: python
validations:
required: false
- type: textarea
attributes:
label: System Information
description: >-
Newton version, Warp version, Python version, OS, and GPU (if
relevant).
placeholder: |
- Newton: 0.x.x
- Warp: 1.x.x
- Python: 3.x
- OS: Ubuntu 22.04 / Windows 11 / macOS 14
- GPU: NVIDIA RTX ...
validations:
required: false
================================================
FILE: .github/ISSUE_TEMPLATE/2-feature-request.yml
================================================
name: Feature Request
description: Suggest an idea for Newton.
title: "[REQ] "
labels: ["feature request"]
body:
- type: textarea
attributes:
label: Description
description: >-
Describe the feature you'd like added or changed.
validations:
required: true
- type: textarea
attributes:
label: Motivation / Use Case
description: >-
Why would this be useful? What problem does it solve?
validations:
required: true
- type: textarea
attributes:
label: Alternatives Considered
description: >-
Have you considered any alternative approaches or workarounds?
validations:
required: false
================================================
FILE: .github/ISSUE_TEMPLATE/3-documentation.yml
================================================
name: Documentation
description: Report an error or suggest improvements for the documentation.
title: "[DOCS] "
labels: ["docs"]
body:
- type: dropdown
attributes:
label: Category
description: What kind of documentation issue is this?
options:
- Error in existing documentation
- Missing documentation
- Improvement suggestion
validations:
required: true
- type: textarea
attributes:
label: Description
description: >-
Describe the issue. Include links to the relevant documentation pages
if applicable.
validations:
required: true
================================================
FILE: .github/ISSUE_TEMPLATE/config.yml
================================================
blank_issues_enabled: true
contact_links:
- name: Question
url: https://github.com/newton-physics/newton/discussions
about: Ask questions about Newton in GitHub Discussions.
================================================
FILE: .github/PULL_REQUEST_TEMPLATE.md
================================================
## Description
## Checklist
- [ ] New or existing tests cover these changes
- [ ] The documentation is up to date with these changes
- [ ] `CHANGELOG.md` has been updated (if user-facing change)
## Test plan
## Bug fix
**Steps to reproduce:**
**Minimal reproduction:**
```python
import newton
# Code that demonstrates the bug
```
## New feature / API change
```python
import newton
# Code that demonstrates the new capability
```
================================================
FILE: .github/codecov.yml
================================================
coverage:
status:
project: off
patch: off
comment:
layout: "diff, flags, files"
fixes:
- "/var/snap/amazon-ssm-agent/[0-9]+/::"
================================================
FILE: .github/workflows/aws_gpu_benchmarks.yml
================================================
name: GPU Benchmarks on AWS EC2 (Reusable)
# This is a reusable workflow that uses machulav/ec2-github-runner to run GPU benchmarks.
# Called by:
# - pr_target_aws_gpu.yml (for pull requests)
# Workflow configuration variables
env:
AWS_REGION: us-east-2
AWS_INSTANCE_TYPE: g7e.2xlarge
AWS_VOLUME_SIZE: 92
AWS_VOLUME_TYPE: gp3
AWS_SECURITY_GROUP_IDS: sg-07807c44e7f2a368a
AWS_ROLE_ARN: arn:aws:iam::968945269301:role/newton-physics-newton-ec2-github-runner-role
AWS_ROLE_DURATION: 7200
HOME: /actions-runner
on:
workflow_call:
inputs:
ref:
description: 'Git ref to checkout'
required: true
type: string
default: ''
base_ref:
description: 'Base ref to compare against (for ASV)'
required: true
type: string
default: ''
secrets:
GH_PERSONAL_ACCESS_TOKEN:
required: true
jobs:
start-runner:
name: Start self-hosted EC2 runner
if: github.repository == 'newton-physics/newton'
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
outputs:
label: ${{ steps.start-ec2-runner.outputs.label }}
ec2-instance-id: ${{ steps.start-ec2-runner.outputs.ec2-instance-id }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Get the latest AWS Deep Learning Base GPU AMI
run: |
echo "Finding the latest AWS Deep Learning Base GPU AMI..."
LATEST_AMI_ID=$(aws ec2 describe-images --region ${{ env.AWS_REGION }} \
--owners amazon \
--filters 'Name=name,Values=Deep Learning Base AMI with Single CUDA (Ubuntu 22.04) ????????' 'Name=state,Values=available' \
--query 'reverse(sort_by(Images, &CreationDate))[:1].ImageId' \
--output text)
if [[ -z "$LATEST_AMI_ID" ]]; then
echo "❌ No AMI ID found. Exiting."
exit 1
fi
echo "Latest AMI ID found: $LATEST_AMI_ID"
echo "LATEST_AMI_ID=$LATEST_AMI_ID" >> "$GITHUB_ENV"
- name: Start EC2 runner
id: start-ec2-runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: start
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
ec2-instance-type: ${{ env.AWS_INSTANCE_TYPE }}
ec2-volume-size: ${{ env.AWS_VOLUME_SIZE }}
ec2-volume-type: ${{ env.AWS_VOLUME_TYPE }}
availability-zones-config: >
[
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-051b9d2e71acf8047", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"},
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-0c98bd06abe8ee5eb", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"}
]
pre-runner-script: |
if [ -d /opt/dlami/nvme ]; then
mkdir -p /opt/dlami/nvme/actions-runner/_work
mkdir -p /opt/dlami/nvme/actions-runner/.local
mkdir -p /opt/dlami/nvme/actions-runner/.cache
ln -s /opt/dlami/nvme/actions-runner/_work /actions-runner/_work
ln -s /opt/dlami/nvme/actions-runner/.local /actions-runner/.local
ln -s /opt/dlami/nvme/actions-runner/.cache /actions-runner/.cache
fi
aws-resource-tags: >
[
{"Key": "Name", "Value": "ec2-github-runner"},
{"Key": "created-by", "Value": "github-actions-newton-role"},
{"Key": "GitHub-Repository", "Value": "${{ github.repository }}"}
]
gpu-benchmarks:
name: Run GPU Benchmarks on AWS EC2
needs: start-runner # required to start the main job when the runner is ready
runs-on: ${{ needs.start-runner.outputs.label }} # run the job on the newly created runner
permissions:
contents: read
steps:
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
with:
ref: ${{ inputs.ref }}
fetch-depth: 0
- name: Install system dependencies
run: |
sudo apt-get update
sudo apt-get install -y libxrandr-dev libxinerama-dev libxcursor-dev libxi-dev \
libgl1-mesa-dev libglu1-mesa-dev
# Clean up apt cache immediately after install
sudo apt-get clean
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
run: uv python install
- name: Check disk space
run: df -h
- name: Set up ASV environment
run: uvx asv machine --yes
- name: Run Benchmarks
run: |
uvx --with virtualenv asv continuous \
--launch-method spawn \
--interleave-rounds \
--append-samples \
--no-only-changed \
-e -b Fast \
${{ inputs.base_ref }} \
${{ inputs.ref }}
continue-on-error: true
id: benchmark
- name: Show comparison on failure
if: steps.benchmark.outcome == 'failure'
run: |
uvx asv compare --split ${{ inputs.base_ref }} ${{ inputs.ref }}
exit 2
- name: Check disk space (post-benchmark)
if: always()
run: df -h
- name: Re-run instructions
if: failure()
run: |
# Create error annotations (appear at top of job summary)
echo "::error::DO NOT use 'Re-run failed jobs' - the EC2 runner no longer exists and your job will be queued forever."
echo "::error::USE 'Re-run all jobs' instead to start a fresh EC2 runner."
# Write to job summary (appears in Summary tab)
cat >> "$GITHUB_STEP_SUMMARY" << 'EOF'
## ⚠️ How to Re-run This Workflow
This workflow uses **ephemeral EC2 runners** that are terminated after each run.
| | Option | Result |
|---|--------|--------|
| ❌ | **Re-run failed jobs** | Runner no longer exists → job queued forever |
| ✅ | **Re-run all jobs** | Starts new EC2 runner → benchmarks re-run |
EOF
# Also print to log for completeness
cat << 'EOF'
================================================================================
⚠️ IMPORTANT: HOW TO RE-RUN THIS WORKFLOW
================================================================================
This workflow uses ephemeral EC2 runners that are terminated after each run.
❌ DO NOT select "Re-run failed jobs"
→ The runner no longer exists and your job will be queued forever.
✅ DO select "Re-run all jobs"
→ This will start a new EC2 runner and re-run the benchmarks.
================================================================================
EOF
stop-runner:
name: Stop self-hosted EC2 runner
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
needs:
- start-runner
- gpu-benchmarks
if: always() && github.repository == 'newton-physics/newton'
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Stop EC2 runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: stop
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
label: ${{ needs.start-runner.outputs.label }}
ec2-instance-id: ${{ needs.start-runner.outputs.ec2-instance-id }}
================================================
FILE: .github/workflows/aws_gpu_tests.yml
================================================
name: GPU Unit Tests on AWS EC2 (Reusable)
# This is a reusable workflow that uses machulav/ec2-github-runner to run GPU tests.
# Called by:
# - pr_target_aws_gpu_tests.yml (for pull requests)
# - merge_queue_aws_gpu.yml (for merge groups)
# - push_aws_gpu.yml (for pushes to main/release branches)
# - scheduled_nightly.yml (for nightly multi-GPU tests)
# Workflow configuration variables
env:
AWS_REGION: us-east-2
AWS_INSTANCE_TYPE: ${{ inputs.instance-type || 'g7e.2xlarge' }}
AWS_VOLUME_SIZE: ${{ inputs.volume-size || '92' }}
AWS_VOLUME_TYPE: gp3
AWS_SECURITY_GROUP_IDS: sg-07807c44e7f2a368a
AWS_ROLE_ARN: arn:aws:iam::968945269301:role/newton-physics-newton-ec2-github-runner-role
AWS_ROLE_DURATION: 7200
HOME: /actions-runner
on:
workflow_call:
inputs:
ref:
description: 'Git ref to checkout'
required: false
type: string
default: ''
instance-type:
description: 'EC2 instance type'
required: false
type: string
default: 'g7e.2xlarge'
volume-size:
description: 'EBS volume size in GB'
required: false
type: string
default: '92'
secrets:
GH_PERSONAL_ACCESS_TOKEN:
required: true
CODECOV_TOKEN:
required: true
workflow_dispatch:
inputs:
instance-type:
description: 'EC2 instance type'
required: false
type: string
default: 'g7e.2xlarge'
jobs:
start-runner:
name: Start self-hosted EC2 runner
if: github.repository == 'newton-physics/newton'
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
outputs:
label: ${{ steps.start-ec2-runner.outputs.label }}
ec2-instance-id: ${{ steps.start-ec2-runner.outputs.ec2-instance-id }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Get the latest AWS Deep Learning Base GPU AMI
run: |
echo "Finding the latest AWS Deep Learning Base GPU AMI..."
LATEST_AMI_ID=$(aws ec2 describe-images --region ${{ env.AWS_REGION }} \
--owners amazon \
--filters 'Name=name,Values=Deep Learning Base AMI with Single CUDA (Ubuntu 22.04) ????????' 'Name=state,Values=available' \
--query 'reverse(sort_by(Images, &CreationDate))[:1].ImageId' \
--output text)
if [[ -z "$LATEST_AMI_ID" ]]; then
echo "❌ No AMI ID found. Exiting."
exit 1
fi
echo "Latest AMI ID found: $LATEST_AMI_ID"
echo "LATEST_AMI_ID=$LATEST_AMI_ID" >> "$GITHUB_ENV"
- name: Start EC2 runner
id: start-ec2-runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: start
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
ec2-instance-type: ${{ env.AWS_INSTANCE_TYPE }}
ec2-volume-size: ${{ env.AWS_VOLUME_SIZE }}
ec2-volume-type: ${{ env.AWS_VOLUME_TYPE }}
availability-zones-config: >
[
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-051b9d2e71acf8047", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"},
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-0c98bd06abe8ee5eb", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"}
]
pre-runner-script: |
if [ -d /opt/dlami/nvme ]; then
mkdir -p /opt/dlami/nvme/actions-runner/_work
mkdir -p /opt/dlami/nvme/actions-runner/.local
mkdir -p /opt/dlami/nvme/actions-runner/.cache
ln -s /opt/dlami/nvme/actions-runner/_work /actions-runner/_work
ln -s /opt/dlami/nvme/actions-runner/.local /actions-runner/.local
ln -s /opt/dlami/nvme/actions-runner/.cache /actions-runner/.cache
fi
aws-resource-tags: >
[
{"Key": "Name", "Value": "ec2-github-runner"},
{"Key": "created-by", "Value": "github-actions-newton-role"},
{"Key": "GitHub-Repository", "Value": "${{ github.repository }}"}
]
gpu-unit-tests:
name: Run GPU Unit Tests on AWS EC2
needs: start-runner # required to start the main job when the runner is ready
runs-on: ${{ needs.start-runner.outputs.label }} # run the job on the newly created runner
permissions:
contents: read
env:
PYTHONFAULTHANDLER: "1" # Dump tracebacks on fatal signals (SIGSEGV, SIGABRT, etc.)
steps:
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
with:
ref: ${{ inputs.ref || github.sha }}
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
run: uv python install
- name: Restore Warp kernel cache
if: github.event_name != 'merge_group'
uses: actions/cache@668228422ae6a00e4ad889ee87cd7109ec5666a7 # v5.0.4
with:
path: /actions-runner/.cache/warp
key: warp-kernels-gpu-${{ hashFiles('uv.lock', 'newton/**/*.py') }}
restore-keys: |
warp-kernels-gpu-
- name: Check disk space
run: df -h
- name: Run Tests
run: uv run --extra dev --extra torch-cu13 -m newton.tests --no-cache-clear --junit-report-xml rspec.xml --coverage --coverage-xml coverage.xml
- name: Check disk space (post-test)
if: always()
run: df -h
- name: Test Summary
if: ${{ !cancelled() }}
uses: test-summary/action@31493c76ec9e7aa675f1585d3ed6f1da69269a86 # v2.4
with:
paths: "rspec.xml"
show: "fail"
- name: Upload coverage reports to Codecov
if: ${{ !cancelled() }}
uses: codecov/codecov-action@1af58845a975a7985b0beb0cbe6fbbb71a41dbad # v5.5.3
with:
disable_search: true
env_vars: AWS_INSTANCE_TYPE
files: ./coverage.xml
flags: unittests
token: ${{ secrets.CODECOV_TOKEN }}
- name: Re-run instructions
if: failure()
run: |
# Create error annotations (appear at top of job summary)
echo "::error::DO NOT use 'Re-run failed jobs' - the EC2 runner no longer exists and your job will be queued forever."
echo "::error::USE 'Re-run all jobs' instead to start a fresh EC2 runner."
# Write to job summary (appears in Summary tab)
cat >> "$GITHUB_STEP_SUMMARY" << 'EOF'
## ⚠️ How to Re-run This Workflow
This workflow uses **ephemeral EC2 runners** that are terminated after each run.
| | Option | Result |
|---|--------|--------|
| ❌ | **Re-run failed jobs** | Runner no longer exists → job queued forever |
| ✅ | **Re-run all jobs** | Starts new EC2 runner → tests re-run |
EOF
# Also print to log for completeness
cat << 'EOF'
================================================================================
⚠️ IMPORTANT: HOW TO RE-RUN THIS WORKFLOW
================================================================================
This workflow uses ephemeral EC2 runners that are terminated after each run.
❌ DO NOT select "Re-run failed jobs"
→ The runner no longer exists and your job will be queued forever.
✅ DO select "Re-run all jobs"
→ This will start a new EC2 runner and re-run the tests.
================================================================================
EOF
stop-runner:
name: Stop self-hosted EC2 runner
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
needs:
- start-runner
- gpu-unit-tests
if: always() && needs.start-runner.result != 'skipped' && github.repository == 'newton-physics/newton'
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Stop EC2 runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: stop
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
label: ${{ needs.start-runner.outputs.label }}
ec2-instance-id: ${{ needs.start-runner.outputs.ec2-instance-id }}
================================================
FILE: .github/workflows/ci.yml
================================================
name: CI
permissions:
contents: read
env:
PYTHONFAULTHANDLER: "1" # Dump tracebacks on fatal signals (SIGSEGV, SIGABRT, etc.)
on:
workflow_call:
workflow_dispatch:
push:
tags:
- v*
branches:
- main
- release-*
jobs:
minimal-import-test:
if: github.event_name != 'push' || github.repository == 'newton-physics/newton'
runs-on: ubuntu-latest
steps:
- name: Harden Runner
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: block
allowed-endpoints: >
download.pytorch.org:443
files.pythonhosted.org:443
github.com:443
pypi.nvidia.com:443
pypi.org:443
raw.githubusercontent.com:443
release-assets.githubusercontent.com:443
releases.astral.sh:443
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Test minimal import (Python 3.10)
run: uv run --python 3.10 python -c "import newton; print(f'Newton {newton.__version__} imported successfully')"
- name: Test minimal import (Python 3.11)
run: uv run --python 3.11 python -c "import newton; print(f'Newton {newton.__version__} imported successfully')"
- name: Test minimal import (Python 3.12)
run: uv run --python 3.12 python -c "import newton; print(f'Newton {newton.__version__} imported successfully')"
- name: Test minimal import (Python 3.13)
run: uv run --python 3.13 python -c "import newton; print(f'Newton {newton.__version__} imported successfully')"
- name: Test minimal import (Python 3.14)
run: uv run --python 3.14 python -c "import newton; print(f'Newton {newton.__version__} imported successfully')"
dependency-install-test:
needs: minimal-import-test
strategy:
fail-fast: false
matrix:
os:
[
ubuntu-latest,
ubuntu-24.04-arm,
windows-latest,
macos-latest,
]
runs-on: ${{ matrix.os }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Test dependency resolution (Python 3.10)
run: uv sync --dry-run --python 3.10 --extra dev
- name: Test dependency resolution (Python 3.11)
run: uv sync --dry-run --python 3.11 --extra dev
- name: Test dependency resolution (Python 3.12)
run: uv sync --dry-run --python 3.12 --extra dev
- name: Test dependency resolution (Python 3.13)
run: uv sync --dry-run --python 3.13 --extra dev
newton-unittests:
needs: dependency-install-test
strategy:
fail-fast: false
matrix:
os:
[
ubuntu-latest,
ubuntu-24.04-arm,
windows-latest,
macos-latest,
]
runs-on: ${{ matrix.os }}
env:
OS: ${{ matrix.os }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install system dependencies (Ubuntu ARM)
if: matrix.os == 'ubuntu-24.04-arm'
run: |
sudo apt-get update
sudo apt-get install -y libx11-dev libxrandr-dev libxinerama-dev libxcursor-dev libxi-dev libgl1-mesa-dev
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
uses: actions/setup-python@a309ff8b426b58ec0e2a45f0f869d46889d02405 # v6.2.0
with:
python-version-file: ".python-version"
- name: Restore Warp kernel cache
if: github.event_name != 'merge_group'
uses: actions/cache@668228422ae6a00e4ad889ee87cd7109ec5666a7 # v5.0.4
with:
path: |
~/.cache/warp
~/Library/Caches/warp
~\AppData\Local\NVIDIA\warp\Cache
key: warp-kernels-${{ runner.os }}-${{ runner.arch }}-${{ hashFiles('uv.lock', 'newton/**/*.py') }}
restore-keys: |
warp-kernels-${{ runner.os }}-${{ runner.arch }}-
- name: Run Tests
run: uv run --extra dev -m newton.tests --no-cache-clear --junit-report-xml rspec.xml --coverage --coverage-xml coverage.xml
- name: Test Summary
uses: test-summary/action@31493c76ec9e7aa675f1585d3ed6f1da69269a86 # v2.4
with:
paths: "rspec.xml"
show: "fail"
if: always()
- name: Upload test results to Codecov
if: ${{ !cancelled() }}
uses: codecov/codecov-action@1af58845a975a7985b0beb0cbe6fbbb71a41dbad # v5.5.3
with:
files: ./rspec.xml
report_type: test_results
token: ${{ secrets.CODECOV_TOKEN }}
- name: Upload coverage reports to Codecov
uses: codecov/codecov-action@1af58845a975a7985b0beb0cbe6fbbb71a41dbad # v5.5.3
with:
disable_search: true
env_vars: OS
files: ./coverage.xml
flags: unittests
token: ${{ secrets.CODECOV_TOKEN }}
================================================
FILE: .github/workflows/docs-dev.yml
================================================
name: Deploy dev documentation
on:
push:
branches:
- main
workflow_dispatch:
# Ensure only one deployment runs at a time
concurrency:
group: docs-deploy
cancel-in-progress: false
jobs:
build-and-deploy:
runs-on: ubuntu-latest
permissions:
contents: write
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
with:
fetch-depth: 0 # Need full history for gh-pages branch
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
uses: actions/setup-python@a309ff8b426b58ec0e2a45f0f869d46889d02405 # v6.2.0
with:
python-version-file: ".python-version"
- name: Install pandoc
uses: pandoc/actions/setup@86321b6dd4675f5014c611e05088e10d4939e09e # v1.1.1
- name: Build Sphinx documentation
run: uv run --extra docs --extra sim sphinx-build -j auto -b html docs docs/_build/html
env:
NEWTON_REQUIRE_PANDOC: "1"
- name: Deploy to gh-pages /latest/
run: |
set -e # Exit on any error
git config user.email "actions@github.com"
git config user.name "GitHub Actions"
# Save built docs and 404 template outside the repo before switching branches
mv docs/_build/html /tmp/docs-latest
cp docs/_static/gh-pages-404.html /tmp/gh-pages-404.html
# Switch to gh-pages branch (check existence first to avoid masking other fetch errors)
if git ls-remote --exit-code --heads origin gh-pages > /dev/null 2>&1; then
git fetch origin gh-pages:gh-pages
git checkout gh-pages
else
echo "Creating new gh-pages branch"
git checkout --orphan gh-pages
git rm -rf . || true
fi
# Remove old /latest/ and replace with new build
rm -rf latest
mv /tmp/docs-latest latest
# Deploy custom 404 page for redirecting old non-versioned URLs
cp /tmp/gh-pages-404.html 404.html
# Ensure .nojekyll exists
touch .nojekyll
# Check gh-pages size (warn if approaching GitHub Pages 1GB limit)
SIZE_KB=$(du -sk --exclude=.git . | cut -f1)
SIZE_MB=$((SIZE_KB / 1024))
echo "Current gh-pages size: ${SIZE_MB}MB"
if [ "$SIZE_MB" -gt 800 ]; then
echo "::warning::gh-pages branch is ${SIZE_MB}MB, approaching GitHub Pages 1GB limit. Consider pruning old versions."
fi
# Stage only the files we want deployed (avoid committing build artifacts
# like .venv/ that persist after switching branches)
git add latest 404.html .nojekyll
git diff --cached --quiet || git commit -m "Update dev docs from main@${GITHUB_SHA::8}"
git push origin gh-pages
================================================
FILE: .github/workflows/docs-release.yml
================================================
name: Deploy release documentation
on:
push:
tags:
- 'v*'
workflow_dispatch:
inputs:
version:
description: 'Version to build (e.g., 1.0.0)'
required: true
type: string
# Ensure only one deployment runs at a time
concurrency:
group: docs-deploy
cancel-in-progress: false
jobs:
build-and-deploy:
runs-on: ubuntu-latest
permissions:
contents: write
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
with:
fetch-depth: 0 # Need full history for gh-pages branch
- name: Set version from tag or input
id: version
env:
EVENT_NAME: ${{ github.event_name }}
INPUT_VERSION: ${{ inputs.version }}
run: |
if [ "$EVENT_NAME" = "push" ]; then
VERSION="${GITHUB_REF#refs/tags/v}"
else
VERSION="$INPUT_VERSION"
fi
echo "VERSION=$VERSION" >> $GITHUB_OUTPUT
echo "Version: $VERSION"
# Only deploy docs for stable releases (strict semver X.Y.Z)
# Pre-release tags (e.g., 1.0.0-rc.1, 1.0.0-beta) are skipped entirely
if [[ "$VERSION" =~ ^[0-9]+\.[0-9]+\.[0-9]+$ ]]; then
echo "SHOULD_DEPLOY=true" >> $GITHUB_OUTPUT
echo "Stable release detected: will deploy docs"
else
echo "SHOULD_DEPLOY=false" >> $GITHUB_OUTPUT
echo "Pre-release detected: skipping documentation deployment"
fi
- name: Install uv
if: steps.version.outputs.SHOULD_DEPLOY == 'true'
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
if: steps.version.outputs.SHOULD_DEPLOY == 'true'
uses: actions/setup-python@a309ff8b426b58ec0e2a45f0f869d46889d02405 # v6.2.0
with:
python-version-file: ".python-version"
- name: Install pandoc
if: steps.version.outputs.SHOULD_DEPLOY == 'true'
uses: pandoc/actions/setup@86321b6dd4675f5014c611e05088e10d4939e09e # v1.1.1
- name: Build Sphinx documentation
if: steps.version.outputs.SHOULD_DEPLOY == 'true'
run: uv run --extra docs --extra sim sphinx-build -j auto -b html docs docs/_build/html
env:
NEWTON_REQUIRE_PANDOC: "1"
- name: Deploy to gh-pages
if: steps.version.outputs.SHOULD_DEPLOY == 'true'
env:
VERSION: ${{ steps.version.outputs.VERSION }}
run: |
set -e # Exit on any error
git config user.email "actions@github.com"
git config user.name "GitHub Actions"
# Save built docs and 404 template outside the repo before switching branches
mv docs/_build/html /tmp/docs-release
cp docs/_static/gh-pages-404.html /tmp/gh-pages-404.html
# Switch to gh-pages branch (check existence first to avoid masking other fetch errors)
if git ls-remote --exit-code --heads origin gh-pages > /dev/null 2>&1; then
git fetch origin gh-pages:gh-pages
git checkout gh-pages
else
echo "Creating new gh-pages branch"
git checkout --orphan gh-pages
git rm -rf . || true
fi
# Deploy version directory (remove old if rebuilding)
rm -rf "$VERSION"
mv /tmp/docs-release "$VERSION"
# Update stable/ directory (copy, not symlink - symlinks unreliable on GH Pages)
rm -rf stable
cp -r "$VERSION" stable
# Update switcher.json (script is in the main branch, not gh-pages)
# Use origin/main because there's no local main branch on tag-triggered runs
git show origin/main:scripts/ci/update_docs_switcher.py > /tmp/update_docs_switcher.py
uv run --no-project /tmp/update_docs_switcher.py "$VERSION"
rm -f switcher.json.bak
# Ensure root index.html redirect exists
{
echo ''
echo ''
echo '
'
echo ' '
echo ' Redirecting to Newton Documentation '
echo ' '
echo ' '
echo ''
echo ''
echo ' Redirecting to Newton Documentation ...
'
echo ''
echo ''
} > index.html
# Deploy custom 404 page for redirecting old non-versioned URLs
cp /tmp/gh-pages-404.html 404.html
# Ensure .nojekyll exists
touch .nojekyll
# Check gh-pages size (warn if approaching GitHub Pages 1GB limit)
SIZE_KB=$(du -sk --exclude=.git . | cut -f1)
SIZE_MB=$((SIZE_KB / 1024))
echo "Current gh-pages size: ${SIZE_MB}MB"
if [ "$SIZE_MB" -gt 800 ]; then
echo "::warning::gh-pages branch is ${SIZE_MB}MB, approaching GitHub Pages 1GB limit. Consider pruning old versions."
fi
# Stage only the files we want deployed (avoid committing build artifacts
# like .venv/ that persist after switching branches)
git add "$VERSION" stable switcher.json index.html 404.html .nojekyll
git diff --cached --quiet || git commit -m "Release v$VERSION documentation"
git push origin gh-pages
================================================
FILE: .github/workflows/merge_queue_aws_gpu.yml
================================================
name: Merge Queue - AWS GPU
on:
merge_group:
jobs:
run-tests:
name: Run GPU Tests
if: github.repository == 'newton-physics/newton'
uses: ./.github/workflows/aws_gpu_tests.yml
with:
ref: ${{ github.sha }}
secrets: inherit
permissions:
id-token: write # Required for AWS OIDC authentication in start-runner/stop-runner
contents: read # Required for checkout in all jobs
================================================
FILE: .github/workflows/minimum_deps_tests.yml
================================================
name: Minimum Dependency Version Tests on AWS EC2 (Reusable)
# Standalone workflow that tests Newton with the lowest compatible versions
# of direct PyPI dependencies (as specified by version floors in pyproject.toml).
# Dispatched by scheduled_nightly.yml via the workflow_dispatch API.
env:
AWS_REGION: us-east-2
AWS_INSTANCE_TYPE: g7e.2xlarge
AWS_VOLUME_SIZE: 92
AWS_VOLUME_TYPE: gp3
AWS_SECURITY_GROUP_IDS: sg-07807c44e7f2a368a
AWS_ROLE_ARN: arn:aws:iam::968945269301:role/newton-physics-newton-ec2-github-runner-role
AWS_ROLE_DURATION: 3600
HOME: /actions-runner
on:
workflow_call:
secrets:
GH_PERSONAL_ACCESS_TOKEN:
required: true
CODECOV_TOKEN:
required: true
workflow_dispatch:
jobs:
start-runner:
name: Start self-hosted EC2 runner
if: github.repository == 'newton-physics/newton'
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
outputs:
label: ${{ steps.start-ec2-runner.outputs.label }}
ec2-instance-id: ${{ steps.start-ec2-runner.outputs.ec2-instance-id }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Get the latest AWS Deep Learning Base GPU AMI
run: |
echo "Finding the latest AWS Deep Learning Base GPU AMI..."
LATEST_AMI_ID=$(aws ec2 describe-images --region ${{ env.AWS_REGION }} \
--owners amazon \
--filters 'Name=name,Values=Deep Learning Base AMI with Single CUDA (Ubuntu 22.04) ????????' 'Name=state,Values=available' \
--query 'reverse(sort_by(Images, &CreationDate))[:1].ImageId' \
--output text)
if [[ -z "$LATEST_AMI_ID" ]]; then
echo "❌ No AMI ID found. Exiting."
exit 1
fi
echo "Latest AMI ID found: $LATEST_AMI_ID"
echo "LATEST_AMI_ID=$LATEST_AMI_ID" >> "$GITHUB_ENV"
- name: Start EC2 runner
id: start-ec2-runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: start
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
ec2-instance-type: ${{ env.AWS_INSTANCE_TYPE }}
ec2-volume-size: ${{ env.AWS_VOLUME_SIZE }}
ec2-volume-type: ${{ env.AWS_VOLUME_TYPE }}
availability-zones-config: >
[
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-051b9d2e71acf8047", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"},
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-0c98bd06abe8ee5eb", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"}
]
pre-runner-script: |
if [ -d /opt/dlami/nvme ]; then
mkdir -p /opt/dlami/nvme/actions-runner/_work
mkdir -p /opt/dlami/nvme/actions-runner/.local
mkdir -p /opt/dlami/nvme/actions-runner/.cache
ln -s /opt/dlami/nvme/actions-runner/_work /actions-runner/_work
ln -s /opt/dlami/nvme/actions-runner/.local /actions-runner/.local
ln -s /opt/dlami/nvme/actions-runner/.cache /actions-runner/.cache
fi
aws-resource-tags: >
[
{"Key": "Name", "Value": "ec2-github-runner"},
{"Key": "created-by", "Value": "github-actions-newton-role"},
{"Key": "GitHub-Repository", "Value": "${{ github.repository }}"}
]
minimum-deps-tests:
name: Run Tests with Minimum Dependency Versions
needs: start-runner
if: ${{ !cancelled() && needs.start-runner.result == 'success' }}
runs-on: ${{ needs.start-runner.outputs.label }}
timeout-minutes: 60
permissions:
contents: read
env:
PYTHONFAULTHANDLER: "1"
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
run: uv python install
- name: Resolve minimum dependency versions
run: |
uv lock --resolution lowest-direct
echo "Resolved dependency versions:"
uv tree --depth 1
- name: Run Tests
run: uv run --extra dev -m newton.tests --junit-report-xml rspec.xml
- name: Test Summary
if: ${{ !cancelled() }}
uses: test-summary/action@31493c76ec9e7aa675f1585d3ed6f1da69269a86 # v2.4
with:
paths: "rspec.xml"
show: "fail"
- name: Upload test results to Codecov
if: ${{ !cancelled() }}
continue-on-error: true
uses: codecov/codecov-action@1af58845a975a7985b0beb0cbe6fbbb71a41dbad # v5.5.3
with:
disable_search: true
files: ./rspec.xml
flags: minimum-deps-nightly
report_type: test_results
token: ${{ secrets.CODECOV_TOKEN }}
- name: Re-run instructions
if: failure()
run: |
echo "::error::DO NOT use 'Re-run failed jobs' - the EC2 runner no longer exists and your job will be queued forever."
echo "::error::USE 'Re-run all jobs' instead to start a fresh EC2 runner."
cat >> "$GITHUB_STEP_SUMMARY" << 'EOF'
## ⚠️ How to Re-run This Workflow
This workflow uses **ephemeral EC2 runners** that are terminated after each run.
| | Option | Result |
|---|--------|--------|
| ❌ | **Re-run failed jobs** | Runner no longer exists → job queued forever |
| ✅ | **Re-run all jobs** | Starts new EC2 runner → tests re-run |
EOF
stop-runner:
name: Stop self-hosted EC2 runner
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
needs:
- start-runner
- minimum-deps-tests
if: always() && needs.start-runner.result != 'skipped' && github.repository == 'newton-physics/newton'
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Stop EC2 runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: stop
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
label: ${{ needs.start-runner.outputs.label }}
ec2-instance-id: ${{ needs.start-runner.outputs.ec2-instance-id }}
================================================
FILE: .github/workflows/mujoco_warp_tests.yml
================================================
name: MuJoCo Warp Tests on AWS EC2 (Reusable)
# Standalone workflow that tests Newton with the latest mujoco-warp from source.
# Not currently dispatched by scheduled_nightly.yml; kept available for manual dispatch/reuse.
env:
AWS_REGION: us-east-2
AWS_INSTANCE_TYPE: g7e.2xlarge
AWS_VOLUME_SIZE: 92
AWS_VOLUME_TYPE: gp3
AWS_SECURITY_GROUP_IDS: sg-07807c44e7f2a368a
AWS_ROLE_ARN: arn:aws:iam::968945269301:role/newton-physics-newton-ec2-github-runner-role
AWS_ROLE_DURATION: 3600
HOME: /actions-runner
on:
workflow_call:
secrets:
GH_PERSONAL_ACCESS_TOKEN:
required: true
CODECOV_TOKEN:
required: true
workflow_dispatch:
jobs:
start-runner:
name: Start self-hosted EC2 runner
if: github.repository == 'newton-physics/newton'
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
outputs:
label: ${{ steps.start-ec2-runner.outputs.label }}
ec2-instance-id: ${{ steps.start-ec2-runner.outputs.ec2-instance-id }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Get the latest AWS Deep Learning Base GPU AMI
run: |
echo "Finding the latest AWS Deep Learning Base GPU AMI..."
LATEST_AMI_ID=$(aws ec2 describe-images --region ${{ env.AWS_REGION }} \
--owners amazon \
--filters 'Name=name,Values=Deep Learning Base AMI with Single CUDA (Ubuntu 22.04) ????????' 'Name=state,Values=available' \
--query 'reverse(sort_by(Images, &CreationDate))[:1].ImageId' \
--output text)
if [[ -z "$LATEST_AMI_ID" ]]; then
echo "❌ No AMI ID found. Exiting."
exit 1
fi
echo "Latest AMI ID found: $LATEST_AMI_ID"
echo "LATEST_AMI_ID=$LATEST_AMI_ID" >> "$GITHUB_ENV"
- name: Start EC2 runner
id: start-ec2-runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: start
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
ec2-instance-type: ${{ env.AWS_INSTANCE_TYPE }}
ec2-volume-size: ${{ env.AWS_VOLUME_SIZE }}
ec2-volume-type: ${{ env.AWS_VOLUME_TYPE }}
availability-zones-config: >
[
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-051b9d2e71acf8047", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"},
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-0c98bd06abe8ee5eb", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"}
]
pre-runner-script: |
if [ -d /opt/dlami/nvme ]; then
mkdir -p /opt/dlami/nvme/actions-runner/_work
mkdir -p /opt/dlami/nvme/actions-runner/.local
mkdir -p /opt/dlami/nvme/actions-runner/.cache
ln -s /opt/dlami/nvme/actions-runner/_work /actions-runner/_work
ln -s /opt/dlami/nvme/actions-runner/.local /actions-runner/.local
ln -s /opt/dlami/nvme/actions-runner/.cache /actions-runner/.cache
fi
aws-resource-tags: >
[
{"Key": "Name", "Value": "ec2-github-runner"},
{"Key": "created-by", "Value": "github-actions-newton-role"},
{"Key": "GitHub-Repository", "Value": "${{ github.repository }}"}
]
mujoco-warp-tests:
name: Run Tests with MuJoCo Warp from Source
needs: start-runner
if: ${{ !cancelled() && needs.start-runner.result == 'success' }}
runs-on: ${{ needs.start-runner.outputs.label }}
timeout-minutes: 60
permissions:
contents: read
env:
PYTHONFAULTHANDLER: "1"
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
run: uv python install
- name: Install Newton and override mujoco-warp with source build
run: |
uv sync --extra dev --extra torch-cu13
uv pip install --reinstall-package mujoco-warp "mujoco-warp @ git+https://github.com/google-deepmind/mujoco_warp.git"
- name: Print mujoco-warp version info
run: uv run --no-sync python -c "import mujoco_warp; print('mujoco_warp version:', getattr(mujoco_warp, '__version__', 'unknown'))"
- name: Check disk space
run: df -h
- name: Run Tests
run: uv run --no-sync -m newton.tests --junit-report-xml rspec.xml
- name: Check disk space (post-test)
if: always()
run: df -h
- name: Test Summary
if: ${{ !cancelled() }}
uses: test-summary/action@31493c76ec9e7aa675f1585d3ed6f1da69269a86 # v2.4
with:
paths: "rspec.xml"
show: "fail"
- name: Upload test results to Codecov
if: ${{ !cancelled() }}
continue-on-error: true
uses: codecov/codecov-action@1af58845a975a7985b0beb0cbe6fbbb71a41dbad # v5.5.3
with:
disable_search: true
files: ./rspec.xml
flags: mujoco-warp-nightly
report_type: test_results
token: ${{ secrets.CODECOV_TOKEN }}
- name: Re-run instructions
if: failure()
run: |
echo "::error::DO NOT use 'Re-run failed jobs' - the EC2 runner no longer exists and your job will be queued forever."
echo "::error::USE 'Re-run all jobs' instead to start a fresh EC2 runner."
cat >> "$GITHUB_STEP_SUMMARY" << 'EOF'
## ⚠️ How to Re-run This Workflow
This workflow uses **ephemeral EC2 runners** that are terminated after each run.
| | Option | Result |
|---|--------|--------|
| ❌ | **Re-run failed jobs** | Runner no longer exists → job queued forever |
| ✅ | **Re-run all jobs** | Starts new EC2 runner → tests re-run |
EOF
stop-runner:
name: Stop self-hosted EC2 runner
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
needs:
- start-runner
- mujoco-warp-tests
if: always() && needs.start-runner.result != 'skipped' && github.repository == 'newton-physics/newton'
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Stop EC2 runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: stop
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
label: ${{ needs.start-runner.outputs.label }}
ec2-instance-id: ${{ needs.start-runner.outputs.ec2-instance-id }}
================================================
FILE: .github/workflows/pr.yml
================================================
name: Pull Request
permissions:
contents: read
on:
merge_group:
pull_request:
branches:
- main
- "release-*"
concurrency:
group: ${{ github.workflow }}-on-${{ github.event_name }}-from-${{ github.ref_name }}
cancel-in-progress: true
jobs:
check-lockfile:
runs-on: ubuntu-latest
steps:
- name: Harden Runner
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: block
allowed-endpoints: >
download.pytorch.org:443
files.pythonhosted.org:443
github.com:443
pypi.nvidia.com:443
pypi.org:443
raw.githubusercontent.com:443
release-assets.githubusercontent.com:443
releases.astral.sh:443
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
uses: actions/setup-python@a309ff8b426b58ec0e2a45f0f869d46889d02405 # v6.2.0
with:
python-version-file: ".python-version"
- name: Check uv lockfile
run: uv lock --check
run-newton-tests:
uses: ./.github/workflows/ci.yml
secrets: inherit
pull-request-docs:
runs-on: ubuntu-latest
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: "Set up Python"
uses: actions/setup-python@a309ff8b426b58ec0e2a45f0f869d46889d02405 # v6.2.0
with:
python-version-file: ".python-version"
- name: Install pandoc
uses: pandoc/actions/setup@86321b6dd4675f5014c611e05088e10d4939e09e # v1.1.1
- name: Build Sphinx documentation
run: uv run --extra docs --extra sim sphinx-build -j auto -W -b html docs docs/_build/html
env:
NEWTON_REQUIRE_PANDOC: "1"
- name: Verify API docs are up-to-date
run: |
git diff --exit-code docs/api/ || {
echo "::error::API docs are stale. Run 'python docs/generate_api.py' and commit the changes."
exit 1
}
- name: Run Sphinx doctests
run: uv run --extra docs --extra sim sphinx-build -j auto -W -b doctest docs docs/_build/doctest
================================================
FILE: .github/workflows/pr_closed.yml
================================================
name: Cancel workflows on PR close
on:
pull_request_target:
types: [closed]
jobs:
cancel-pr-workflows:
name: Cancel in-progress PR workflows
runs-on: ubuntu-latest
permissions:
actions: write
timeout-minutes: 3
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Cancel in-progress runs for this PR
env:
GH_TOKEN: ${{ github.token }}
GH_REPO: ${{ github.repository }}
HEAD_BRANCH: ${{ github.event.pull_request.head.ref }}
PR_NUMBER: ${{ github.event.pull_request.number }}
CURRENT_RUN: ${{ github.run_id }}
run: |
echo "PR #$PR_NUMBER closed, cancelling workflows for branch: $HEAD_BRANCH"
# Cancel all in-progress and queued runs on this PR's head branch (except this one)
for status in in_progress queued; do
gh run list --branch "$HEAD_BRANCH" --status "$status" --json databaseId --jq '.[].databaseId' \
| while read -r run_id; do
if [ "$run_id" != "$CURRENT_RUN" ]; then
echo "Cancelling run $run_id"
if ! gh run cancel "$run_id"; then
echo "Warning: failed to cancel run $run_id" >&2
fi
fi
done
done
================================================
FILE: .github/workflows/pr_license_check.yml
================================================
name: License Check
permissions:
contents: read
pull-requests: read
on:
merge_group:
pull_request:
branches:
- main
- "release-*"
# Cancels in-progress runs of this workflow for the same pull request,
# but allows parallel runs for pushes to the main branch.
concurrency:
group: ${{ github.workflow }}-${{ github.head_ref }}
cancel-in-progress: true
jobs:
check-licenses:
runs-on: ubuntu-latest
steps:
- name: Harden Runner
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: block
allowed-endpoints: >
api.github.com:443
github.com:443
proxy.golang.org:443
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Check code license headers (Apache-2.0)
uses: apache/skywalking-eyes/header@61275cc80d0798a405cb070f7d3a8aaf7cf2c2c1 # v0.8.0
- name: Check docs license headers (CC-BY-4.0)
uses: apache/skywalking-eyes/header@61275cc80d0798a405cb070f7d3a8aaf7cf2c2c1 # v0.8.0
with:
config: .licenserc-docs.yaml
- name: Check notebook license headers (CC-BY-4.0)
run: |
status=0
for nb in $(find docs -name '*.ipynb' -not -path '*/.*' -not -path '*/_build/*'); do
if ! grep -q 'SPDX-License-Identifier: CC-BY-4.0' "$nb"; then
echo "ERROR: $nb missing CC-BY-4.0 SPDX header"
status=1
fi
done
exit $status
================================================
FILE: .github/workflows/pr_target_aws_gpu_benchmarks.yml
================================================
name: Pull Request - AWS GPU Benchmarks
on:
pull_request_target:
concurrency:
group: ${{ github.workflow }}-pr-${{ github.event.pull_request.number }}
cancel-in-progress: true
jobs:
check-author-membership:
name: Check Author Membership
runs-on: ubuntu-latest
permissions: {}
outputs:
membership_status: ${{ steps.check_org.outputs.membership_status }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Check user's organization membership
id: check_org
run: |
ASSOCIATION="${{ github.event.pull_request.author_association }}"
echo "Author's association with the repository: ${ASSOCIATION}"
if [[ "${ASSOCIATION}" == "MEMBER" || "${ASSOCIATION}" == "OWNER" || "${ASSOCIATION}" == "COLLABORATOR" ]]; then
echo "Author is a recognized member, owner, or collaborator."
echo "membership_status=CONFIRMED_MEMBER" >> "$GITHUB_OUTPUT"
else
# Set the output for other jobs to use
echo "membership_status=NOT_MEMBER" >> "$GITHUB_OUTPUT"
# Print a message explaining the status and its impact on workflows.
echo "--------------------------------------------------------------------------------" >&2
echo "Thank you for your contribution!" >&2
echo "This is the expected status for community contributors. Certain automated" >&2
echo "workflows are reserved for verified organization members." >&2
echo "" >&2
echo "--------------------------------------------------------------------------------" >&2
echo "❓ Are you a member of the 'newton-physics' organization and believe this is an error?" >&2
echo "" >&2
echo "This can happen if your organization membership is set to 'Private'. To fix this," >&2
echo "please make your membership 'Public' to enable all workflow triggers:" >&2
echo "" >&2
echo "1. Go to the organization's People page: https://github.com/orgs/newton-physics/people" >&2
echo "2. Find your username in the list." >&2
echo "3. Click the dropdown next to your name and change your visibility from 'Private' to 'Public'." >&2
echo "" >&2
echo "After updating your visibility, push a new commit to this PR to re-run the check." >&2
echo "--------------------------------------------------------------------------------" >&2
# Surface warnings as visible annotations (yellow banners in job view)
echo "::warning::This PR requires manual approval before GPU benchmarks can run (author is not a recognized org member)."
echo "::warning::If you are a newton-physics org member with private membership, make it public at https://github.com/orgs/newton-physics/people"
# Write to job summary (appears in Summary tab)
cat >> "$GITHUB_STEP_SUMMARY" << 'EOF'
## ⚠️ Manual Approval Required
This PR was authored by an external contributor. GPU benchmarks require manual approval from a maintainer before they can run.
### Are you a newton-physics org member?
If your membership is set to **Private**, the workflow cannot detect it. To fix:
1. Go to [newton-physics People](https://github.com/orgs/newton-physics/people)
2. Find your username and change visibility from **Private** to **Public**
3. Push a new commit to re-trigger the check
EOF
fi
require-approval:
name: Require Manual Approval for External PRs
runs-on: ubuntu-latest
permissions:
deployments: write # Required for creating deployment record when using environment
needs: check-author-membership
if: needs.check-author-membership.outputs.membership_status != 'CONFIRMED_MEMBER'
environment:
name: external-pr-approval
url: ${{ github.event.pull_request.html_url }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Approval granted
run: echo "Manual approval granted for external PR"
run-gpu-benchmarks:
name: Run GPU Benchmarks
needs:
- check-author-membership
- require-approval
if: github.repository == 'newton-physics/newton' && (!cancelled())
uses: ./.github/workflows/aws_gpu_benchmarks.yml
with:
ref: ${{ github.event.pull_request.head.sha }}
base_ref: ${{ github.event.pull_request.base.sha }}
secrets: inherit
permissions:
id-token: write # Required for AWS OIDC authentication in start-runner/stop-runner
contents: read # Required for checkout in all jobs
================================================
FILE: .github/workflows/pr_target_aws_gpu_tests.yml
================================================
name: Pull Request - AWS GPU Tests
on:
pull_request_target:
concurrency:
group: ${{ github.workflow }}-pr-${{ github.event.pull_request.number }}
cancel-in-progress: true
jobs:
check-author-membership:
name: Check Author Membership
runs-on: ubuntu-latest
permissions: {}
outputs:
membership_status: ${{ steps.check_org.outputs.membership_status }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Check user's organization membership
id: check_org
run: |
ASSOCIATION="${{ github.event.pull_request.author_association }}"
echo "Author's association with the repository: ${ASSOCIATION}"
if [[ "${ASSOCIATION}" == "MEMBER" || "${ASSOCIATION}" == "OWNER" || "${ASSOCIATION}" == "COLLABORATOR" ]]; then
echo "Author is a recognized member, owner, or collaborator."
echo "membership_status=CONFIRMED_MEMBER" >> "$GITHUB_OUTPUT"
else
# Set the output for other jobs to use
echo "membership_status=NOT_MEMBER" >> "$GITHUB_OUTPUT"
# Print a message explaining the status and its impact on workflows.
echo "--------------------------------------------------------------------------------" >&2
echo "Thank you for your contribution!" >&2
echo "This is the expected status for community contributors. Certain automated" >&2
echo "workflows are reserved for verified organization members." >&2
echo "" >&2
echo "--------------------------------------------------------------------------------" >&2
echo "❓ Are you a member of the 'newton-physics' organization and believe this is an error?" >&2
echo "" >&2
echo "This can happen if your organization membership is set to 'Private'. To fix this," >&2
echo "please make your membership 'Public' to enable all workflow triggers:" >&2
echo "" >&2
echo "1. Go to the organization's People page: https://github.com/orgs/newton-physics/people" >&2
echo "2. Find your username in the list." >&2
echo "3. Click the dropdown next to your name and change your visibility from 'Private' to 'Public'." >&2
echo "" >&2
echo "After updating your visibility, push a new commit to this PR to re-run the check." >&2
echo "--------------------------------------------------------------------------------" >&2
# Surface warnings as visible annotations (yellow banners in job view)
echo "::warning::This PR requires manual approval before GPU tests can run (author is not a recognized org member)."
echo "::warning::If you are a newton-physics org member with private membership, make it public at https://github.com/orgs/newton-physics/people"
# Write to job summary (appears in Summary tab)
cat >> "$GITHUB_STEP_SUMMARY" << 'EOF'
## ⚠️ Manual Approval Required
This PR was authored by an external contributor. GPU tests require manual approval from a maintainer before they can run.
### Are you a newton-physics org member?
If your membership is set to **Private**, the workflow cannot detect it. To fix:
1. Go to [newton-physics People](https://github.com/orgs/newton-physics/people)
2. Find your username and change visibility from **Private** to **Public**
3. Push a new commit to re-trigger the check
EOF
fi
require-approval:
name: Require Manual Approval for External PRs
runs-on: ubuntu-latest
permissions:
deployments: write # Required for creating deployment record when using environment
needs: check-author-membership
if: needs.check-author-membership.outputs.membership_status != 'CONFIRMED_MEMBER'
environment:
name: external-pr-approval
url: ${{ github.event.pull_request.html_url }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Approval granted
run: echo "Manual approval granted for external PR"
run-gpu-tests:
name: Run GPU Tests
needs:
- check-author-membership
- require-approval
if: github.repository == 'newton-physics/newton' && (!cancelled())
uses: ./.github/workflows/aws_gpu_tests.yml
with:
ref: ${{ github.event.pull_request.head.sha }}
secrets: inherit
permissions:
id-token: write # Required for AWS OIDC authentication in start-runner/stop-runner
contents: read # Required for checkout in all jobs
================================================
FILE: .github/workflows/push_aws_gpu.yml
================================================
name: Push - AWS GPU
# This workflow triggers GPU tests for pushes to main/release branches.
# It replaces the old push_aws_gpu_tests.yml workflow.
# Uses reusable workflows:
# - aws_gpu_tests.yml
on:
push:
paths-ignore:
- "docs/**"
- "**.md"
jobs:
run-tests:
if: github.repository == 'newton-physics/newton'
uses: ./.github/workflows/aws_gpu_tests.yml
secrets: inherit
permissions:
id-token: write # Required for AWS OIDC authentication in start-runner/stop-runner
contents: read # Required for checkout in all jobs
================================================
FILE: .github/workflows/release.yml
================================================
name: Release
# This workflow runs when a tag is pushed.
# The publish-to-pypi job requires manual approval via the 'pypi' environment protection rules
# configured in Settings > Environments > pypi.
on:
push:
tags:
- '*'
jobs:
build:
name: Build distribution
runs-on: ubuntu-latest
permissions:
contents: read
actions: write
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
with:
persist-credentials: false
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
run: uv python install
- name: Build a binary wheel
run: uv build --wheel
- name: Store the distribution packages
uses: actions/upload-artifact@b7c566a772e6b6bfb58ed0dc250532a479d7789f # v6.0.0
with:
name: python-package-distributions
path: dist/
publish-to-pypi:
name: Publish Python distribution to PyPI
if: github.repository == 'newton-physics/newton'
needs:
- build
runs-on: ubuntu-latest
environment:
name: pypi
url: https://pypi.org/p/newton
permissions:
id-token: write
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Download wheel and source tarball
uses: actions/download-artifact@37930b1c2abaa49bbe596cd826c3c89aef350131 # v7.0.0
with:
name: python-package-distributions
path: dist/
- name: Publish distribution to PyPI
uses: pypa/gh-action-pypi-publish@ed0c53931b1dc9bd32cbe73a98c7f6766f8a527e # v1.13.0
create-github-release:
name: Create GitHub Release
if: github.repository == 'newton-physics/newton'
needs:
- build
runs-on: ubuntu-latest
permissions:
contents: write
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Download wheel and source tarball
uses: actions/download-artifact@37930b1c2abaa49bbe596cd826c3c89aef350131 # v7.0.0
with:
name: python-package-distributions
path: dist/
- name: Create GitHub Release
run: |
gh release create ${{ github.ref_name }} \
--draft \
--title "Release ${{ github.ref_name }}" \
dist/*
env:
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
================================================
FILE: .github/workflows/scheduled_nightly.yml
================================================
name: Scheduled Nightly Tests
# Orchestrator that dispatches all nightly test suites sequentially.
# Each sub-workflow is triggered via the workflow_dispatch REST API and
# polled to completion before the next is started, ensuring only one
# EC2 instance is active at a time. Per-workflow history is preserved
# as separate workflow runs. One group's failure does not block
# subsequent groups.
on:
schedule:
- cron: '0 9 * * *' # Daily at 9 AM UTC (1 AM PST / 2 AM PDT)
workflow_dispatch:
jobs:
check-warp-update:
name: Check for new warp-lang nightly build
if: github.repository == 'newton-physics/newton'
runs-on: ubuntu-latest
permissions:
contents: read
outputs:
warp-updated: ${{ steps.check-update.outputs.warp-updated }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Update warp-lang in lock file
run: uv lock -P warp-lang --prerelease allow
- name: Check if warp-lang version changed
id: check-update
run: |
if git diff --quiet uv.lock; then
echo "No new warp-lang nightly build detected"
echo "warp-updated=false" >> "$GITHUB_OUTPUT"
else
echo "New warp-lang nightly build detected!"
echo "warp-updated=true" >> "$GITHUB_OUTPUT"
echo "Current warp-lang dependency tree:"
uv tree --package warp-lang
fi
run-nightly-tests:
name: Run nightly test suites
needs: [check-warp-update]
if: ${{ !cancelled() && github.repository == 'newton-physics/newton' }}
runs-on: ubuntu-latest
timeout-minutes: 180 # Budget for sequential dispatch+poll of all sub-workflows (typical total ~90 min)
permissions:
actions: write
contents: read
outputs:
gpu-tests-conclusion: ${{ steps.gpu-tests.outputs.conclusion }}
gpu-tests-url: ${{ steps.gpu-tests.outputs.run-url }}
minimum-deps-tests-conclusion: ${{ steps.minimum-deps-tests.outputs.conclusion }}
minimum-deps-tests-url: ${{ steps.minimum-deps-tests.outputs.run-url }}
warp-nightly-tests-conclusion: ${{ steps.warp-nightly-tests.outputs.conclusion }}
warp-nightly-tests-url: ${{ steps.warp-nightly-tests.outputs.run-url }}
env:
GH_TOKEN: ${{ github.token }}
REPO: ${{ github.repository }}
REF: ${{ github.ref }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
run: uv python install
- name: Dispatch and wait for GPU tests
id: gpu-tests
run: uv run --no-project scripts/ci/dispatch_workflow_and_wait.py aws_gpu_tests.yml -f "inputs[instance-type]=g7e.12xlarge"
- name: Dispatch and wait for Minimum Deps tests
id: minimum-deps-tests
run: uv run --no-project scripts/ci/dispatch_workflow_and_wait.py minimum_deps_tests.yml
- name: Dispatch and wait for Warp Nightly tests
id: warp-nightly-tests
if: needs.check-warp-update.result == 'success' && needs.check-warp-update.outputs.warp-updated == 'true'
run: uv run --no-project scripts/ci/dispatch_workflow_and_wait.py warp_nightly_tests.yml
notify-on-failure:
name: Notify on failure
needs: [run-nightly-tests]
if: |
!cancelled() &&
((needs.run-nightly-tests.outputs.gpu-tests-conclusion != '' &&
needs.run-nightly-tests.outputs.gpu-tests-conclusion != 'success') ||
(needs.run-nightly-tests.outputs.minimum-deps-tests-conclusion != '' &&
needs.run-nightly-tests.outputs.minimum-deps-tests-conclusion != 'success') ||
(needs.run-nightly-tests.outputs.warp-nightly-tests-conclusion != '' &&
needs.run-nightly-tests.outputs.warp-nightly-tests-conclusion != 'success'))
runs-on: ubuntu-latest
permissions:
issues: write
contents: read
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: File or update GitHub issue
uses: actions/github-script@ed597411d8f924073f98dfc5c65a23a2325f34cd # v8.0.0
with:
script: |
const suites = [
{
name: 'Multi-GPU tests',
workflow: 'aws_gpu_tests.yml',
conclusion: '${{ needs.run-nightly-tests.outputs.gpu-tests-conclusion }}',
url: '${{ needs.run-nightly-tests.outputs.gpu-tests-url }}'
},
{
name: 'Minimum deps tests',
workflow: 'minimum_deps_tests.yml',
conclusion: '${{ needs.run-nightly-tests.outputs.minimum-deps-tests-conclusion }}',
url: '${{ needs.run-nightly-tests.outputs.minimum-deps-tests-url }}'
},
{
name: 'Warp nightly tests',
workflow: 'warp_nightly_tests.yml',
conclusion: '${{ needs.run-nightly-tests.outputs.warp-nightly-tests-conclusion }}',
url: '${{ needs.run-nightly-tests.outputs.warp-nightly-tests-url }}'
}
];
// Fetch recent run history to show pass/fail trend in the issue table
// (default branch only, excludes cancelled runs)
async function getHistory(workflowFile) {
try {
const { data } = await github.rest.actions.listWorkflowRuns({
owner: context.repo.owner,
repo: context.repo.repo,
workflow_id: workflowFile,
branch: context.payload.repository?.default_branch || 'main',
per_page: 10,
status: 'completed',
exclude_pull_requests: true
});
const runs = data.workflow_runs
.filter(r => r.conclusion !== 'cancelled')
.slice(0, 5);
return runs.map(r => r.conclusion === 'success' ? '✅' : '❌').join('');
} catch (error) {
core.warning(`Failed to fetch history for ${workflowFile}: ${error.message}`);
return '';
}
}
const failed = [];
const rows = [];
for (const suite of suites) {
const history = await getHistory(suite.workflow);
const recentCol = history || '—';
if (!suite.conclusion) {
rows.push(`| ${suite.name} | ${recentCol} | ⏭️ Skipped | |`);
} else if (suite.conclusion === 'success') {
rows.push(`| ${suite.name} | ${recentCol} | ✅ Passed | [View logs](${suite.url}) |`);
} else {
rows.push(`| ${suite.name} | ${recentCol} | ❌ Failed | [View logs](${suite.url}) |`);
failed.push(suite.name);
}
}
const labels = ['stability', 'testing'];
const orchestratorUrl = `${context.serverUrl}/${context.repo.owner}/${context.repo.repo}/actions/runs/${context.runId}`;
const failedList = failed.join(', ');
const table = [
'| Test Suite | Recent | Status | Logs |',
'|---|---|---|---|',
...rows
].join('\n');
// Search for an existing open nightly failure issue to update
// instead of creating duplicates
const { data: candidates } = await github.rest.issues.listForRepo({
owner: context.repo.owner,
repo: context.repo.repo,
labels: labels.join(','),
state: 'open',
per_page: 100
});
const existing = candidates.find(i => i.title.startsWith('Nightly failure'));
try {
// If an existing issue is found, add a comment; otherwise
// create a new one with today's date
if (existing) {
await github.rest.issues.createComment({
owner: context.repo.owner,
repo: context.repo.repo,
issue_number: existing.number,
body: [
`@newton-physics/newton-ci-notify Nightly tests are still failing.`,
``,
table,
``,
`**Orchestrator run:** [View](${orchestratorUrl})`
].join('\n')
});
} else {
const date = new Date().toISOString().slice(0, 10);
await github.rest.issues.create({
owner: context.repo.owner,
repo: context.repo.repo,
title: `Nightly failure (${date}): ${failedList}`,
body: [
`@newton-physics/newton-ci-notify`,
``,
`The scheduled nightly workflow failed.`,
``,
table,
``,
`**Orchestrator run:** [View](${orchestratorUrl})`
].join('\n'),
labels: labels
});
}
} catch (error) {
core.error(`Failed to create/update notification issue: ${error.message}`);
core.error(`Test suites that failed: ${failedList}`);
throw error;
}
================================================
FILE: .github/workflows/warp_nightly_tests.yml
================================================
name: Warp Nightly Build Tests on AWS EC2 (Reusable)
# Standalone workflow that tests Newton with the latest nightly warp-lang builds.
# Called by scheduled_nightly.yml (only when a new warp-lang nightly is detected).
env:
AWS_REGION: us-east-2
AWS_INSTANCE_TYPE: g7e.2xlarge
AWS_VOLUME_SIZE: 92
AWS_VOLUME_TYPE: gp3
AWS_SECURITY_GROUP_IDS: sg-07807c44e7f2a368a
AWS_ROLE_ARN: arn:aws:iam::968945269301:role/newton-physics-newton-ec2-github-runner-role
AWS_ROLE_DURATION: 3600
HOME: /actions-runner
on:
workflow_call:
secrets:
GH_PERSONAL_ACCESS_TOKEN:
required: true
CODECOV_TOKEN:
required: true
workflow_dispatch:
jobs:
start-runner:
name: Start self-hosted EC2 runner
if: github.repository == 'newton-physics/newton'
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
outputs:
label: ${{ steps.start-ec2-runner.outputs.label }}
ec2-instance-id: ${{ steps.start-ec2-runner.outputs.ec2-instance-id }}
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Get the latest AWS Deep Learning Base GPU AMI
run: |
echo "Finding the latest AWS Deep Learning Base GPU AMI..."
LATEST_AMI_ID=$(aws ec2 describe-images --region ${{ env.AWS_REGION }} \
--owners amazon \
--filters 'Name=name,Values=Deep Learning Base AMI with Single CUDA (Ubuntu 22.04) ????????' 'Name=state,Values=available' \
--query 'reverse(sort_by(Images, &CreationDate))[:1].ImageId' \
--output text)
if [[ -z "$LATEST_AMI_ID" ]]; then
echo "❌ No AMI ID found. Exiting."
exit 1
fi
echo "Latest AMI ID found: $LATEST_AMI_ID"
echo "LATEST_AMI_ID=$LATEST_AMI_ID" >> "$GITHUB_ENV"
- name: Start EC2 runner
id: start-ec2-runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: start
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
ec2-instance-type: ${{ env.AWS_INSTANCE_TYPE }}
ec2-volume-size: ${{ env.AWS_VOLUME_SIZE }}
ec2-volume-type: ${{ env.AWS_VOLUME_TYPE }}
availability-zones-config: >
[
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-051b9d2e71acf8047", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"},
{"imageId": "${{ env.LATEST_AMI_ID }}", "subnetId": "subnet-0c98bd06abe8ee5eb", "securityGroupId": "${{ env.AWS_SECURITY_GROUP_IDS }}"}
]
pre-runner-script: |
if [ -d /opt/dlami/nvme ]; then
mkdir -p /opt/dlami/nvme/actions-runner/_work
mkdir -p /opt/dlami/nvme/actions-runner/.local
mkdir -p /opt/dlami/nvme/actions-runner/.cache
ln -s /opt/dlami/nvme/actions-runner/_work /actions-runner/_work
ln -s /opt/dlami/nvme/actions-runner/.local /actions-runner/.local
ln -s /opt/dlami/nvme/actions-runner/.cache /actions-runner/.cache
fi
aws-resource-tags: >
[
{"Key": "Name", "Value": "ec2-github-runner"},
{"Key": "created-by", "Value": "github-actions-newton-role"},
{"Key": "GitHub-Repository", "Value": "${{ github.repository }}"}
]
warp-nightly-tests:
name: Run Tests with Warp Nightly Build
needs: start-runner
if: ${{ !cancelled() && needs.start-runner.result == 'success' }}
runs-on: ${{ needs.start-runner.outputs.label }}
timeout-minutes: 60
permissions:
contents: read
env:
PYTHONFAULTHANDLER: "1"
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Checkout repository
uses: actions/checkout@8e8c483db84b4bee98b60c0593521ed34d9990e8 # v6.0.1
- name: Install uv
uses: astral-sh/setup-uv@37802adc94f370d6bfd71619e3f0bf239e1f3b78 # v7.6.0
with:
version: "0.11.0"
- name: Set up Python
run: uv python install
- name: Update lock file with latest warp-lang nightly build
run: uv lock -P warp-lang --prerelease allow
- name: Check disk space
run: df -h
- name: Run Tests
run: uv run --extra dev --extra torch-cu13 -m newton.tests --junit-report-xml rspec.xml
- name: Check disk space (post-test)
if: always()
run: df -h
- name: Test Summary
if: ${{ !cancelled() }}
uses: test-summary/action@31493c76ec9e7aa675f1585d3ed6f1da69269a86 # v2.4
with:
paths: "rspec.xml"
show: "fail"
- name: Upload test results to Codecov
if: ${{ !cancelled() }}
continue-on-error: true
uses: codecov/codecov-action@1af58845a975a7985b0beb0cbe6fbbb71a41dbad # v5.5.3
with:
disable_search: true
files: ./rspec.xml
flags: warp-nightly
report_type: test_results
token: ${{ secrets.CODECOV_TOKEN }}
- name: Re-run instructions
if: failure()
run: |
echo "::error::DO NOT use 'Re-run failed jobs' - the EC2 runner no longer exists and your job will be queued forever."
echo "::error::USE 'Re-run all jobs' instead to start a fresh EC2 runner."
cat >> "$GITHUB_STEP_SUMMARY" << 'EOF'
## ⚠️ How to Re-run This Workflow
This workflow uses **ephemeral EC2 runners** that are terminated after each run.
| | Option | Result |
|---|--------|--------|
| ❌ | **Re-run failed jobs** | Runner no longer exists → job queued forever |
| ✅ | **Re-run all jobs** | Starts new EC2 runner → tests re-run |
EOF
stop-runner:
name: Stop self-hosted EC2 runner
runs-on: ubuntu-latest
permissions:
id-token: write
contents: read
needs:
- start-runner
- warp-nightly-tests
if: always() && needs.start-runner.result != 'skipped' && github.repository == 'newton-physics/newton'
steps:
- name: Harden the runner (Audit all outbound calls)
uses: step-security/harden-runner@fa2e9d605c4eeb9fcad4c99c224cee0c6c7f3594 # v2.16.0
with:
egress-policy: audit
- name: Configure AWS credentials
uses: aws-actions/configure-aws-credentials@8df5847569e6427dd6c4fb1cf565c83acfa8afa7 # v6.0.0
with:
aws-region: ${{ env.AWS_REGION }}
role-to-assume: ${{ env.AWS_ROLE_ARN }}
role-duration-seconds: ${{ env.AWS_ROLE_DURATION }}
- name: Stop EC2 runner
uses: machulav/ec2-github-runner@a00f575a87f3a96ec6de9413d16eeb828a3cc0a8 # v2.5.2
with:
mode: stop
github-token: ${{ secrets.GH_PERSONAL_ACCESS_TOKEN }}
label: ${{ needs.start-runner.outputs.label }}
ec2-instance-id: ${{ needs.start-runner.outputs.ec2-instance-id }}
================================================
FILE: .gitignore
================================================
# Python-generated files
__pycache__/
*.py[oc]
build/
dist/
wheels/
*.egg-info
# Virtual Environments
.venv
# Tool cache
.cache/
.thumbs/
# Coverage reports
.coverage
.coverage.*
coverage.xml
htmlcov/
# Test Reports
rspec.xml
# Type Checking
.mypy_cache/
# IDEs & Editors
.vscode
.history/
.idea
# Claude Code
.claude/settings.local.json
.claude/worktrees/
.mcp.json
# Claude Code sandbox artifacts (empty placeholder files created in repo root)
.bash_profile
.bashrc
.profile
.zprofile
.zshrc
.gitconfig
.gitmodules
.ripgreprc
/HEAD
/config
/hooks
/objects
/refs
# Documentation
docs/_build/
docs/api/_generated/
# Airspeed Velocity
asv/env
asv/results
asv/html
# Log files, settings
MUJOCO_LOG.TXT
imgui.ini
newton/tests/outputs/
# Viser recordings
docs/_static/recordings/
*.viser
# Viser static files
docs/_static/viser/
# Notebook checkpoints
**/*.ipynb_checkpoints/
================================================
FILE: .licenserc-docs.yaml
================================================
header:
license:
spdx-id: CC-BY-4.0
copyright-owner: The Newton Developers
content: |
SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
SPDX-License-Identifier: CC-BY-4.0
# Pattern is matched against lowercased text where "(c)" is normalized
# to the word "copyright", so "Copyright (c)" becomes "copyright copyright".
pattern: |
spdx-filecopyrighttext: copyright copyright \d{4} the newton developers
spdx-license-identifier: cc-by-4\.0
paths:
- "**/*.rst"
paths-ignore:
- "docs/api/_generated/**"
- "docs/_templates/**"
language:
reStructuredText:
extensions:
- ".rst"
comment_style_id: DoubleDot
comment: on-failure
================================================
FILE: .licenserc.yaml
================================================
header:
license:
spdx-id: Apache-2.0
copyright-owner: The Newton Developers
content: |
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
# Pattern is matched against lowercased text where "(c)" is normalized
# to the word "copyright", so "Copyright (c)" becomes "copyright copyright".
pattern: |
spdx-filecopyrighttext: copyright copyright \d{4} the newton developers
spdx-license-identifier: apache-2\.0
paths:
- "**/*.py"
paths-ignore:
- "newton/_version.py"
- "asv/benchmarks/**/__init__.py"
- "newton/tests/thirdparty/__init__.py"
comment: on-failure
================================================
FILE: .pre-commit-config.yaml
================================================
ci:
autofix_commit_msg: |
[pre-commit.ci] auto code formatting
autofix_prs: false
autoupdate_branch: ""
autoupdate_commit_msg: "[pre-commit.ci] pre-commit autoupdate"
autoupdate_schedule: quarterly
# pre-commit.ci has no network access, but uv-lock needs to resolve
# dependencies from remote indexes (PyPI, nvidia, pytorch).
# Lockfile freshness is checked by a separate CI workflow instead.
skip:
- uv-lock
submodules: false
# See https://pre-commit.com for more information
# See https://pre-commit.com/hooks.html for more hooks
repos:
- repo: https://github.com/astral-sh/ruff-pre-commit
# Ruff version.
rev: v0.14.10
hooks:
# Run the linter.
- id: ruff
args: [--fix] # Apply fixes to resolve lint violations.
# Run the formatter.
- id: ruff-format
- repo: https://github.com/astral-sh/uv-pre-commit
# uv version.
rev: 0.9.21
hooks:
# Update the uv lockfile
- id: uv-lock
- repo: https://github.com/crate-ci/typos
rev: v1.42.0
hooks:
- id: typos
args: []
exclude: \.(js|css)$
- repo: local
hooks:
- id: check-warp-array-syntax
name: check warp array syntax
entry: python scripts/check_warp_array_syntax.py
language: system
types: [python]
exclude: ^(scripts/check_warp_array_syntax\.py|newton/_src/solvers/kamino/)
================================================
FILE: .python-version
================================================
3.12
================================================
FILE: AGENTS.md
================================================
# Newton Development Guidelines
- `newton/_src/` is internal. Examples and docs must not import from `newton._src`. Expose user-facing symbols via public modules (`newton/geometry.py`, `newton/solvers.py`, etc.).
- Breaking changes require a deprecation first. Do not remove or rename public API symbols without deprecating them in a prior release.
- Prefix-first naming for autocomplete: `ActuatorPD` (not `PDActuator`), `add_shape_sphere()` (not `add_sphere_shape()`).
- Prefer nested classes for self-contained helper types/enums.
- PEP 604 unions (`x | None`, not `Optional[x]`).
- Annotate Warp arrays with bracket syntax (`wp.array[wp.vec3]`, `wp.array2d[float]`, `wp.array[Any]`), not the parenthesized form (`wp.array(dtype=...)`). Use `wp.array[X]` for 1-D arrays, not `wp.array1d[X]`.
- Follow Google-style docstrings. Types in annotations, not docstrings. `Args:` use `name: description`.
- Sphinx cross-refs (`:class:`, `:meth:`) with shortest possible targets. Prefer public API paths; never use `newton._src`.
- SI units for physical quantities in public API docstrings: `"""Particle positions [m], shape [particle_count, 3]."""`. Joint-dependent: `[m or rad]`. Spatial vectors: `[N, N·m]`. Compound arrays: per-component. Skip non-physical fields.
- Run `docs/generate_api.py` when adding public API symbols.
- Avoid new required dependencies. Strongly prefer not adding optional ones — use Warp, NumPy, or stdlib.
- Create a feature branch before committing — never commit directly to `main`. Use `/feature-desc`.
- Imperative mood in commit messages ("Fix X", not "Fixed X"), ~50 char subject, body wraps at 72 chars explaining _what_ and _why_.
- Verify regression tests fail without the fix before committing.
- Pin GitHub Actions by SHA: `action@ # vX.Y.Z`. Check `.github/workflows/` for allowlisted hashes.
- In SPDX copyright lines, use the year the file was first created. Do not create date ranges or update the year when modifying a file.
Run `uvx pre-commit run -a` to lint/format before committing. Use `uv` for all commands; fall back to `venv`/`conda` if unavailable.
```bash
# Examples
uv sync --extra examples
uv run -m newton.examples basic_pendulum
```
## Tests
Always use `unittest`, not pytest.
```bash
uv run --extra dev -m newton.tests
uv run --extra dev -m newton.tests -k test_viewer_log_shapes # specific test
uv run --extra dev -m newton.tests -k test_basic.example_basic_shapes # example test
uv run --extra dev --extra torch-cu12 -m newton.tests # with PyTorch
```
### Testing guidelines
- Never call `wp.synchronize()` or `wp.synchronize_device()` right before `.numpy()` on a Warp array. This is redundant as `.numpy()` performs a synchronous device-to-host copy that completes all outstanding work.
```bash
# Benchmarks
uvx --with virtualenv asv run --launch-method spawn main^!
```
## PR Instructions
- If opening a pull request on GitHub, use the template in `.github/PULL_REQUEST_TEMPLATE.md`.
- If a change modifies user-facing behavior, append an entry at the end of the correct category (`Added`, `Changed`, `Deprecated`, `Removed`, `Fixed`) in `CHANGELOG.md`'s `[Unreleased]` section. Use imperative present tense ("Add X") and avoid internal implementation details.
- For `Deprecated`, `Changed`, and `Removed` entries, include migration guidance: "Deprecate `Model.geo_meshes` in favor of `Model.shapes`".
## Examples
- Follow the `Example` class format.
- Implement `test_final()` — runs after the example completes to verify simulation state is valid.
- Optionally implement `test_post_step()` — runs after every `step()` for per-step validation.
- Register in `README.md` with `python -m newton.examples ` command and a 320x320 jpg screenshot.
================================================
FILE: CHANGELOG.md
================================================
# Changelog
## [Unreleased]
### Added
- Add repeatable `--warp-config KEY=VALUE` CLI option for overriding `warp.config` attributes when running examples
- Add 3D texture-based SDF, replacing NanoVDB volumes in the mesh-mesh collision pipeline for improved performance and CPU compatibility.
- Add `--benchmark [SECONDS]` flag to examples for headless FPS measurement with warmup
- Interactive example browser in the GL viewer with tree-view navigation and switch/reset support
- Add `TetMesh` class and USD loading API for tetrahedral mesh geometry
- Support kinematic bodies in VBD solver
- Add brick stacking example
- Add box pyramid example and ASV benchmark for dense convex-on-convex contacts
- Add plotting example showing how to access and visualize per-step simulation diagnostics
- Add `exposure` property to GL renderer
- Add `snap_to` argument to `ViewerGL.log_gizmo()` to snap gizmos to a target world transform when the user releases them
- Expose `gizmo_is_using` attribute to detect whether a gizmo is actively being dragged
- Add per-axis gizmo filtering via `translate`/`rotate` parameters on `log_gizmo`
- Add conceptual overview and MuJoCo Warp integration primer to collision documentation
- Add configurable velocity basis for implicit MPM (`velocity_basis`, default `"Q1"`) with GIMP quadrature option (`integration_scheme="gimp"`)
- Add plastic viscosity, dilatancy, hardening and softening rate as per-particle MPM material properties (`mpm:viscosity`, `mpm:dilatancy`, `mpm:hardening_rate`, `mpm:softening_rate`)
- Add MPM beam twist, snow ball, and viscous coiling examples
- Add support for textures in `SensorTiledCamera` via `Config.enable_textures`
- Add `enable_ambient_lighting` and `enable_particles` options to `SensorTiledCamera.Config`
- Add `SensorTiledCamera.utils.convert_ray_depth_to_forward_depth()` to convert ray-distance depth to forward (planar) depth
- Add `newton.geometry.compute_offset_mesh()` for extracting offset surface meshes from any collision shape, and a viewer toggle to visualize gap + margin wireframes in the GL viewer
- Add differentiable rigid contacts (experimental) with respect to body poses via `CollisionPipeline` when `requires_grad=True`
- Add per-shape display colors via `ModelBuilder.shape_color`, `Model.shape_color`, and `color=` on `ModelBuilder.add_shape_*`; mesh shapes fall back to `Mesh.color` when available and viewers honor runtime `Model.shape_color` updates
- Add `ModelBuilder.inertia_tolerance` to configure the eigenvalue positivity and triangle inequality threshold used during inertia correction in `finalize()`
- Pin `newton-assets` and `mujoco_menagerie` downloads to specific commit SHAs for reproducible builds (`NEWTON_ASSETS_REF`, `MENAGERIE_REF`)
- Add `ref` parameter to `download_asset()` to allow overriding the pinned revision
- Add `total_force_friction` and `force_matrix_friction` to `SensorContact` for tangential (friction) force decomposition
- Add `compute_normals` and `compute_uvs` optional arguments to `Mesh.create_heightfield()` and `Mesh.create_terrain()`
- Add RJ45 plug-socket insertion example with SDF contacts, latch joint, and interactive gizmo
### Changed
- Unify heightfield and mesh collision pipeline paths; the separate `heightfield_midphase_kernel` and `shape_pairs_heightfield` buffer are removed in favor of the shared mesh midphase
- Replace per-shape `Model.shape_heightfield_data` / `Model.heightfield_elevation_data` with compact `Model.shape_heightfield_index` / `Model.heightfield_data` / `Model.heightfield_elevations`, matching the SDF indirection pattern
- Standardize `rigid_contact_normal` to point from shape 0 toward shape 1 (A-to-B), matching the documented convention. Consumers that previously negated the normal on read (XPBD, VBD, MuJoCo, Kamino) no longer need to.
- Replace `Model.sdf_data` / `sdf_volume` / `sdf_coarse_volume` with texture-based equivalents (`texture_sdf_data`, `texture_sdf_coarse_textures`, `texture_sdf_subgrid_textures`)
- Render inertia boxes as wireframe lines instead of solid boxes in the GL viewer to avoid occluding objects
- Make contact reduction normal binning configurable (polyhedron, scan directions, voxel budget) via constants in ``contact_reduction.py``
- Upgrade GL viewer lighting from Blinn-Phong to Cook-Torrance PBR with GGX distribution, Schlick-GGX geometry, Fresnel-weighted ambient, and ACES filmic tone mapping
- Change implicit MPM residual computation to consider both infinity and l2 norm
- Change implicit MPM hardening law from exponential to hyperbolic sine (`sinh(-h * log(Jp))`), no longer scales elastic modulus
- Change implicit MPM collider velocity mode names: `"forward"` / `"backward"` replace `"instantaneous"` / `"finite_difference"`
- Simplify `SensorContact` force output: add `total_force` (aggregate per sensing object) and `force_matrix` (per-counterpart breakdown, `None` when no counterparts)
- Add `sensing_obj_idx` (`list[int]`), `counterpart_indices` (`list[list[int]]`), `sensing_obj_type`, and `counterpart_type` attributes. Rename `include_total` to `measure_total`
- Replace verbose Apache 2.0 boilerplate with two-line SPDX-only license headers across all source and documentation files
- Add `custom_attributes` argument to `ModelBuilder.add_shape_convex_hull()`
- Improve wrench preservation in hydroelastic contacts with contact reduction.
- Show Newton deprecation warnings during example runs started via `python -m newton.examples ...` or `python -m newton.examples..`; pass `-W ignore::DeprecationWarning` if you need the previous quiet behavior.
- Reorder `ModelBuilder.add_shape_gaussian()` parameters so `xform` precedes `gaussian`, in line with other `add_shape_*` methods. Callers using positional arguments should switch to keyword form (`gaussian=..., xform=...`); passing a `Gaussian` as the second positional argument still works but emits a `DeprecationWarning`
- Rename `ModelBuilder.add_shape_ellipsoid()` parameters `a`, `b`, `c` to `rx`, `ry`, `rz`. Old names are still accepted as keyword arguments but emit a `DeprecationWarning`
- Rename `collide_plane_cylinder()` parameter `cylinder_center` to `cylinder_pos` for consistency with other collide functions
- Add optional `state` parameter to `SolverBase.update_contacts()` to align the base-class signature with Kamino and MuJoCo solvers
- Use `Literal` types for `SolverImplicitMPM.Config` string fields with fixed option sets (`solver`, `warmstart_mode`, `collider_velocity_mode`, `grid_type`, `transfer_scheme`, `integration_scheme`)
- Migrate `wp.array(dtype=X)` type annotations to `wp.array[X]` bracket syntax (Warp 1.12+).
### Deprecated
- Deprecate `ModelBuilder.default_body_armature`, the `armature` argument on `ModelBuilder.add_link()` / `ModelBuilder.add_body()`, and USD-authored body armature via `newton:armature` in favor of adding any isotropic artificial inertia directly to `inertia`
- Deprecate `SensorContact.net_force` in favor of `SensorContact.total_force` and `SensorContact.force_matrix`
- Deprecate `SensorContact(include_total=...)` in favor of `SensorContact(measure_total=...)`
- Deprecate `SensorContact.sensing_objs` in favor of `SensorContact.sensing_obj_idx`
- Deprecate `SensorContact.counterparts` and `SensorContact.reading_indices` in favor of `SensorContact.counterpart_indices`
- Deprecate `SensorContact.shape` (use `total_force.shape` and `force_matrix.shape` instead)
- Deprecate `SensorTiledCamera.render_context`; prefer `SensorTiledCamera.utils` and `SensorTiledCamera.render_config`.
- Deprecate `SensorTiledCamera.RenderContext`; use `SensorTiledCamera.RenderConfig` for config types and `SensorTiledCamera.render_config` / `SensorTiledCamera.utils` for runtime access.
- Deprecate `SensorTiledCamera.Config`; prefer `SensorTiledCamera.RenderConfig` and `SensorTiledCamera.utils`.
- Deprecate `Viewer.update_shape_colors()` in favor of writing directly to `Model.shape_color`
- Deprecate `ModelBuilder.add_shape_ellipsoid()` parameters `a`, `b`, `c` in favor of `rx`, `ry`, `rz`
- Deprecate passing a `Gaussian` as the second positional argument to `ModelBuilder.add_shape_gaussian()`; use the `gaussian=` keyword argument instead
- Deprecate `SensorTiledCamera.utils.assign_random_colors_per_world()` and `assign_random_colors_per_shape()` in favor of per-shape colors via `ModelBuilder.add_shape_*(color=...)`
### Removed
- Remove `Heightfield.finalize()` and stop storing raw pointers for heightfields in `Model.shape_source_ptr`; heightfield collision data is accessed via `Model.shape_heightfield_index` / `Model.heightfield_data` / `Model.heightfield_elevations`
- Remove `robot_humanoid` example in favor of `basic_plotting` which uses the same humanoid model with diagnostics visualization
### Fixed
- Fix inertia validation spuriously inflating small but physically valid eigenvalues for lightweight components (< ~50 g) by using a relative threshold instead of an absolute 1e-6 cutoff
- Restore keyboard camera movement while hovering gizmos so keyboard controls remain active when the pointer is over gizmos
- Resolve USD asset references recursively in `resolve_usd_from_url` so nested stages are fully downloaded
- Unify CPU and GPU inertia validation to produce identical results for zero-mass bodies with `bound_mass`, singular inertia, non-symmetric tensors, and triangle-inequality boundary cases
- Fix `UnboundLocalError` crash in detailed inertia validation when eigenvalue decomposition encounters NaN/Inf input
- Handle NaN/Inf mass and inertia deterministically in both validation paths (zero out mass and inertia)
- Update `ModelBuilder` internal state after fast-path (GPU kernel) inertia validation so it matches the returned `Model`
- Fix MJCF mesh scale resolution to use the mesh asset's own class rather than the geom's default class, avoiding incorrect vertex scaling for models like Robotiq 2F-85 V4
- Fix articulated bodies drifting laterally on the ground in XPBD solver by solving rigid contacts before joints
- Fix viewer crash with `imgui_bundle>=1.92.6` when editing colors by normalizing `color_edit3` input/output in `_edit_color3`
- Fix `hide_collision_shapes=True` not hiding collision meshes that have bound PBR materials
- Filter inactive particles in viewer so only particles with `ParticleFlags.ACTIVE` are rendered
- Fix concurrent asset download races on Windows by using content-addressed cache directories
- Show prismatic joints in the GL viewer when "Show Joints" is enabled
- Fix body `gravcomp` not being written to the MuJoCo spec, causing it to be absent from XML saved via `save_to_mjcf`
- Fix `compute_world_offsets` grid ordering to match terrain grid row-major order so replicated world indices align with terrain block indices
- Fix `eq_solimp` not being written to the MuJoCo spec for equality constraints, causing it to be absent from XML saved via `save_to_mjcf`
- Fix WELD equality constraint quaternion written in xyzw format instead of MuJoCo's wxyz format in the spec, causing incorrect orientation in XML saved via `save_to_mjcf`
- Fix `update_contacts` not populating `rigid_contact_point0`/`rigid_contact_point1` when using `use_mujoco_contacts=True`
- Fix VSync toggle having no effect in `ViewerGL` on Windows 8+ due to a pyglet bug where `DwmFlush()` is never called when `_always_dwm` is True
- Fix loop joint coordinate mapping in the MuJoCo solver so joints after a loop joint read/write at correct qpos/qvel offsets
- Fix viewer crash when contact buffer overflows by clamping contact count to buffer size
- Decompose loop joint constraints by DOF type (WELD for fixed, CONNECT-pair for revolute, single CONNECT for ball) instead of always emitting 2x CONNECT
- Fix inertia box wireframe rotation for isotropic and axisymmetric bodies in viewer
- Implicit MPM solver now uses `mass=0` for kinematic particles instead of `ACTIVE` flag
- Suppress macOS OpenGL warning about unloadable textures by binding a 1x1 white fallback texture when no albedo or environment texture is set
- Fix MuJoCo solver freeze when immovable bodies (kinematic, static, or fixed-root) generate contacts with degenerate invweight
- Fix forward-kinematics child-origin linear velocity for articulated translated joints
- Fix `ModelBuilder.approximate_meshes()` to handle the duplication of per-shape custom attributes that results from convex decomposition
- Fix `get_tetmesh()` winding order for left-handed USD meshes
- Fix contact force conversion in `SolverMuJoCo` to include friction (tangential) components
- Fix URDF inertial parameters parsing in parse_urdf so inertia tensor is correctly calculated as R@I@R.T
- Fix Poisson surface reconstruction segfault under parallel test execution by defaulting to single-threaded Open3D Poisson (`n_threads=1`)
- Fix overly conservative broadphase AABB for mesh shapes by using the pre-computed local AABB with a rotated-box transform instead of a bounding-sphere fallback, eliminating false contacts between distant meshes
- Fix heightfield bounding-sphere radius underestimating Z extent for asymmetric height ranges (e.g. `min_z=0, max_z=10`)
- Fix VBD self-contact barrier C2 discontinuity at `d = tau` caused by a factor-of-two error in the log-barrier coefficient
- Fix fast inertia validation treating near-symmetric tensors within `np.allclose()` default tolerances as corrections, aligning CPU and GPU validation warnings
## [1.0.0] - 2026-03-10
Initial public release.
================================================
FILE: CITATION.cff
================================================
cff-version: 1.2.0
message: "If you use Newton, please cite it using this metadata."
title: "Newton: GPU-accelerated physics simulation for robotics and simulation research"
type: software
authors:
- name: "The Newton Contributors"
url: "https://github.com/newton-physics/newton"
license: Apache-2.0
date-released: "2025-04-22"
================================================
FILE: CLAUDE.md
================================================
@AGENTS.md
================================================
FILE: CODE_OF_CONDUCT.md
================================================
By participating in this community, you agree to abide by the Linux Foundation [Code of Conduct](https://lfprojects.org/policies/code-of-conduct/).
================================================
FILE: CONTRIBUTING.md
================================================
# Overview
Newton is a project of the Linux Foundation and aims to be governed in a transparent, accessible way for the benefit of the community. All participation in this project is open and not bound to corporate affiliation. Participants are all bound to the Linux Foundation [Code of Conduct](https://lfprojects.org/policies/code-of-conduct/).
# General Guidelines and Legal
Please refer to [the contribution guidelines](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md) in the `newton-governance` repository for general information, project membership, and legal requirements for making contributions to Newton.
# Contributing to Newton
Newton welcomes contributions from the community. In order to avoid any surprises and to increase the chance of contributions being merged, we encourage contributors to communicate their plans proactively by opening a GitHub Issue or starting a Discussion in the corresponding repository.
Please also refer to the [development guide](https://newton-physics.github.io/newton/latest/guide/development.html).
There are several ways to participate in the Newton community:
## Questions, Discussions, Suggestions
* Help answer questions or contribute to technical discussion in [GitHub Discussions](https://github.com/newton-physics/newton/discussions) and Issues.
* If you have a question, suggestion or discussion topic, start a new [GitHub Discussion](https://github.com/newton-physics/newton/discussions) if there is no existing topic.
* Once somebody shares a satisfying answer to the question, click "Mark as answer".
* [GitHub Issues](https://github.com/newton-physics/newton/issues) should only be used for bugs and features. Specifically, issues that result in a code or documentation change. We may convert issues to discussions if these conditions are not met.
## Reporting a Bug
* Check in the [GitHub Issues](https://github.com/newton-physics/newton/issues) if a report for the bug already exists.
* If the bug has not been reported yet, open a new Issue.
* Use a short, descriptive title and write a clear description of the bug.
* Document the Git hash or release version where the bug is present, and the hardware and environment by including the output of `nvidia-smi`.
* Add executable code samples or test cases with instructions for reproducing the bug.
## Documentation Issues
* Create a new issue if there is no existing report, or
* directly submit a fix following the "Fixing a Bug" workflow below.
## Fixing a Bug
* Ensure that the bug report issue has no assignee yet. If the issue is assigned and there is no linked PR, you're welcome to ask about the current status by commenting on the issue.
* Write a fix and regression unit test for the bug following the [style guide](https://newton-physics.github.io/newton/latest/guide/development.html#style-guide).
* Open a new pull request for the fix and test.
* Write a description of the bug and the fix.
* Mention related issues in the description: E.g. if the patch fixes Issue \#33, write Fixes \#33.
* Have a signed CLA on file (see [Legal Requirements](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#legal-requirements)).
* Have the pull request approved by a [Project Member](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#project-members) and merged into the codebase.
## Improving Performance
* Write an optimization that improves an existing or new benchmark following the [style guide](https://newton-physics.github.io/newton/latest/guide/development.html#style-guide).
* Open a new pull request with the optimization, and the benchmark, if applicable.
* Write a description of the performance optimization.
* Mention related issues in the description: E.g. if the optimization addresses Issue \#42, write Addresses \#42.
* Have a signed CLA on file (see [Legal Requirements](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#legal-requirements)).
* Have the pull request approved by a [Project Member](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#project-members) and merged into the codebase.
## Adding a Feature or Solver
* Discuss your proposal ideally before starting with implementation. Open a GitHub Issue or Discussion to:
* propose and motivate the new feature or solver;
* detail technical specifications;
* and list changes or additions to the Newton API.
* Wait for feedback from [Project Members](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#project-members) before proceeding.
* Implement the feature or solver following the [style guide](https://newton-physics.github.io/newton/latest/guide/development.html#style-guide).
* Add comprehensive testing and benchmarking for the new feature or solver.
* Ensure all existing tests pass and that existing benchmarks do not regress.
* Update or add documentation for the new feature or solver.
* Have a signed CLA on file (see [Legal Requirements](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#legal-requirements)).
* Have the pull request approved by a [Project Member](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#project-members) and merged into the codebase.
## Adding Simulation Assets
* Before proposing to add any assets to the Newton project, make sure that the assets are properly licensed for use and distribution. If you are unsure about the license, open a new discussion.
* The Newton project hosts possibly large simulation assets such as models, textures, datasets, or pre-trained policies in the [newton-assets](https://github.com/newton-physics/newton-assets) repository to keep the main newton repository small.
* Therefore, along with a pull request in the main newton repository that relies on new assets, open a corresponding pull request in the [newton-assets](https://github.com/newton-physics/newton-assets) repository.
* Follow the instructions in the [README](https://github.com/newton-physics/newton-assets) of the newton-assets repository.
* Have a signed CLA on file (see [Legal Requirements](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#legal-requirements)).
* Have the pull request approved by a [Project Member](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md#project-members) and merged into the asset repository.
================================================
FILE: LICENSE.md
================================================
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
================================================
FILE: README.md
================================================
[](https://opensource.org/licenses/Apache-2.0)

[](https://codecov.io/gh/newton-physics/newton)
[](https://github.com/newton-physics/newton/actions/workflows/push_aws_gpu.yml)
# Newton
Newton is a GPU-accelerated physics simulation engine built upon [NVIDIA Warp](https://github.com/NVIDIA/warp), specifically targeting roboticists and simulation researchers.
Newton extends and generalizes Warp's ([deprecated](https://github.com/NVIDIA/warp/discussions/735)) `warp.sim` module, and integrates
[MuJoCo Warp](https://github.com/google-deepmind/mujoco_warp) as its primary backend. Newton emphasizes GPU-based computation, [OpenUSD](https://openusd.org/) support, differentiability, and user-defined extensibility, facilitating rapid iteration and scalable robotics simulation.
Newton is a [Linux Foundation](https://www.linuxfoundation.org/) project that is community-built and maintained. Code is licensed under [Apache-2.0](https://github.com/newton-physics/newton/blob/main/LICENSE.md). Documentation is licensed under [CC-BY-4.0](https://creativecommons.org/licenses/by/4.0/). Additional and third-party license texts are available in [`newton/licenses`](https://github.com/newton-physics/newton/tree/main/newton/licenses).
Newton was initiated by [Disney Research](https://www.disneyresearch.com/), [Google DeepMind](https://deepmind.google/), and [NVIDIA](https://www.nvidia.com/).
## Requirements
- **Python** 3.10+
- **OS:** Linux (x86-64, aarch64), Windows (x86-64), or macOS (CPU only)
- **GPU:** NVIDIA GPU (Maxwell or newer), driver 545 or newer (CUDA 12). No local CUDA Toolkit installation required. macOS runs on CPU.
For detailed system requirements and tested configurations, see the [installation guide](https://newton-physics.github.io/newton/latest/guide/installation.html).
## Quickstart
```bash
pip install "newton[examples]"
python -m newton.examples basic_pendulum
```
To install from source with [uv](https://docs.astral.sh/uv/), see the [installation guide](https://newton-physics.github.io/newton/latest/guide/installation.html).
## Examples
Before running the examples below, install Newton with the examples extra:
```bash
pip install "newton[examples]"
```
If you installed from source with uv, substitute `uv run` for `python` in the commands below.
Basic Examples
python -m newton.examples basic_pendulum
python -m newton.examples basic_urdf
python -m newton.examples basic_viewer
python -m newton.examples basic_shapes
python -m newton.examples basic_joints
python -m newton.examples basic_conveyor
python -m newton.examples basic_heightfield
python -m newton.examples recording
python -m newton.examples replay_viewer
python -m newton.examples basic_plotting
Robot Examples
python -m newton.examples robot_cartpole
python -m newton.examples robot_g1
python -m newton.examples robot_h1
python -m newton.examples robot_anymal_d
python -m newton.examples robot_anymal_c_walk
python -m newton.examples robot_policy
python -m newton.examples robot_ur10
python -m newton.examples robot_panda_hydro
python -m newton.examples robot_allegro_hand
Cable Examples
python -m newton.examples cable_twist
python -m newton.examples cable_y_junction
python -m newton.examples cable_bundle_hysteresis
python -m newton.examples cable_pile
Cloth Examples
python -m newton.examples cloth_bending
python -m newton.examples cloth_hanging
python -m newton.examples cloth_style3d
python -m newton.examples cloth_h1
python -m newton.examples cloth_twist
python -m newton.examples cloth_franka
python -m newton.examples cloth_rollers
python -m newton.examples cloth_poker_cards
Inverse Kinematics Examples
python -m newton.examples ik_franka
python -m newton.examples ik_h1
python -m newton.examples ik_custom
python -m newton.examples ik_cube_stacking
MPM Examples
python -m newton.examples mpm_granular
python -m newton.examples mpm_anymal
python -m newton.examples mpm_twoway_coupling
python -m newton.examples mpm_grain_rendering
python -m newton.examples mpm_multi_material
python -m newton.examples mpm_viscous
python -m newton.examples mpm_beam_twist
python -m newton.examples mpm_snow_ball
Sensor Examples
python -m newton.examples sensor_contact
python -m newton.examples sensor_tiled_camera
python -m newton.examples sensor_imu
Selection Examples
python -m newton.examples selection_cartpole
python -m newton.examples selection_materials
python -m newton.examples selection_articulations
python -m newton.examples selection_multiple
DiffSim Examples
python -m newton.examples diffsim_ball
python -m newton.examples diffsim_cloth
python -m newton.examples diffsim_drone
python -m newton.examples diffsim_spring_cage
python -m newton.examples diffsim_soft_body
python -m newton.examples diffsim_bear
Multi-Physics Examples
python -m newton.examples softbody_gift
python -m newton.examples softbody_dropping_to_cloth
Contacts Examples
python -m newton.examples nut_bolt_hydro
python -m newton.examples nut_bolt_sdf
python -m newton.examples brick_stacking
python -m newton.examples pyramid
python -m newton.examples contacts_rj45_plug
Softbody Examples
python -m newton.examples softbody_hanging
python -m newton.examples softbody_franka
### Example Options
The examples support the following command-line arguments:
| Argument | Description | Default |
| --------------- | --------------------------------------------------------------------------------------------------- | ---------------------------- |
| `--viewer` | Viewer type: `gl` (OpenGL window), `usd` (USD file output), `rerun` (ReRun), or `null` (no viewer). | `gl` |
| `--device` | Compute device to use, e.g., `cpu`, `cuda:0`, etc. | `None` (default Warp device) |
| `--num-frames` | Number of frames to simulate (for USD output). | `100` |
| `--output-path` | Output path for USD files (required if `--viewer usd` is used). | `None` |
Some examples may add additional arguments (see their respective source files for details).
### Example Usage
```bash
# List available examples
python -m newton.examples
# Run with the USD viewer and save to my_output.usd
python -m newton.examples basic_viewer --viewer usd --output-path my_output.usd
# Run on a selected device
python -m newton.examples basic_urdf --device cuda:0
# Combine options
python -m newton.examples basic_viewer --viewer gl --num-frames 500 --device cpu
```
## Contributing and Development
See the [contribution guidelines](https://github.com/newton-physics/newton-governance/blob/main/CONTRIBUTING.md) and the [development guide](https://newton-physics.github.io/newton/latest/guide/development.html) for instructions on how to contribute to Newton.
## Support and Community Discussion
For questions, please consult the [Newton documentation](https://newton-physics.github.io/newton/latest/guide/overview.html) first before creating [a discussion in the main repository](https://github.com/newton-physics/newton/discussions).
## Code of Conduct
By participating in this community, you agree to abide by the Linux Foundation [Code of Conduct](https://lfprojects.org/policies/code-of-conduct/).
## Project Governance, Legal, and Members
Please see the [newton-governance repository](https://github.com/newton-physics/newton-governance) for more information about project governance.
================================================
FILE: SECURITY.md
================================================
Please refer to the [SECURITY.md](https://github.com/newton-physics/newton-governance/blob/main/SECURITY.md) in the newton-governance repository.
================================================
FILE: asv/benchmarks/__init__.py
================================================
================================================
FILE: asv/benchmarks/benchmark_ik.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# Used for benchmarking the Newton IK solver.
#
# This module provides shared logic for IK benchmarks on the
# Franka Emika Panda robot.
###########################################################################
from __future__ import annotations
import numpy as np
import warp as wp
import newton
import newton.ik as ik
import newton.utils
def create_franka_model() -> newton.Model:
builder = newton.ModelBuilder()
builder.num_rigid_contacts_per_world = 0
builder.default_shape_cfg.density = 100.0
asset_path = newton.utils.download_asset("franka_emika_panda") / "urdf/fr3.urdf"
builder.add_urdf(asset_path, floating=False, scale=1.0)
return builder.finalize(requires_grad=False)
def random_solutions(model: newton.Model, n: int, rng: np.random.Generator) -> np.ndarray:
n_coords = model.joint_coord_count
lower = model.joint_limit_lower.numpy()[:n_coords]
upper = model.joint_limit_upper.numpy()[:n_coords]
span = upper - lower
mask = np.abs(span) > 1e5
span[mask] = 0.0
q = rng.random((n, n_coords)) * span + lower
q[:, mask] = 0.0
return q.astype(np.float32)
def build_ik_solver(model: newton.Model, n_problems: int, ee_links: tuple[int, ...]):
zero_pos = [wp.zeros(n_problems, dtype=wp.vec3) for _ in ee_links]
zero_rot = [wp.zeros(n_problems, dtype=wp.vec4) for _ in ee_links]
objectives = []
for ee, link in enumerate(ee_links):
objectives.append(ik.IKObjectivePosition(link, wp.vec3(), zero_pos[ee]))
for ee, link in enumerate(ee_links):
objectives.append(
ik.IKObjectiveRotation(
link,
wp.quat_identity(),
zero_rot[ee],
canonicalize_quat_err=False,
)
)
objectives.append(
ik.IKObjectiveJointLimit(
model.joint_limit_lower,
model.joint_limit_upper,
weight=1.0,
)
)
solver = ik.IKSolver(
model,
n_problems,
objectives,
sampler=ik.IKSampler.ROBERTS,
n_seeds=64,
lambda_factor=4.0,
jacobian_mode=ik.IKJacobianType.ANALYTIC,
)
return (
solver,
objectives[: len(ee_links)],
objectives[len(ee_links) : 2 * len(ee_links)],
)
def fk_targets(solver, model: newton.Model, q_batch: np.ndarray, ee_links: tuple[int, ...]):
batch_size = q_batch.shape[0]
solver._fk_two_pass(
model,
wp.array(q_batch, dtype=wp.float32),
solver.body_q,
solver.X_local,
batch_size,
)
wp.synchronize_device()
bq = solver.body_q.numpy()[:batch_size]
ee = np.asarray(ee_links)
return bq[:, ee, :3].copy(), bq[:, ee, 3:7].copy()
def eval_success(solver, model, q_best, tgt_pos, tgt_rot, ee_links, pos_thresh_m, ori_thresh_rad):
batch_size = q_best.shape[0]
solver._fk_two_pass(
model,
wp.array(q_best, dtype=wp.float32),
solver.body_q,
solver.X_local,
batch_size,
)
wp.synchronize_device()
bq = solver.body_q.numpy()[:batch_size]
ee = np.asarray(ee_links)
pos_err = np.linalg.norm(bq[:, ee, :3] - tgt_pos, axis=-1).max(axis=-1)
def _qmul(a, b):
# Quaternions stored as (x, y, z, w) — scalar-last, matching Warp convention.
x1, y1, z1, w1 = np.moveaxis(a, -1, 0)
x2, y2, z2, w2 = np.moveaxis(b, -1, 0)
return np.stack(
(
w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
),
axis=-1,
)
tgt_conj = np.concatenate([-tgt_rot[..., :3], tgt_rot[..., 3:]], axis=-1)
rel = _qmul(tgt_conj, bq[:, ee, 3:7])
rot_err = (2 * np.arctan2(np.linalg.norm(rel[..., :3], axis=-1), np.abs(rel[..., 3]))).max(axis=-1)
success = (pos_err < pos_thresh_m) & (rot_err < ori_thresh_rad)
return success
================================================
FILE: asv/benchmarks/benchmark_mujoco.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# Used for benchmarking MjWarp.
#
# This script allows us to choose between several predefined robots and
# provides a large range of customizable options.
#
###########################################################################
import time
import numpy as np
import warp as wp
wp.config.enable_backward = False
import newton
import newton.examples
import newton.utils
from newton.sensors import SensorContact
from newton.utils import EventTracer
ROBOT_CONFIGS = {
"humanoid": {
"solver": "newton",
"integrator": "implicitfast",
"njmax": 80,
"nconmax": 25,
"ls_parallel": False,
"cone": "pyramidal",
"sensing_bodies": ["*thigh*", "*shin*", "*foot*", "*arm*"],
},
"g1": {
"solver": "newton",
"integrator": "implicitfast",
"njmax": 210,
"nconmax": 35,
"ls_parallel": False,
"cone": "pyramidal",
},
"h1": {
"solver": "newton",
"integrator": "implicitfast",
"njmax": 65,
"nconmax": 15,
"ls_parallel": False,
"cone": "pyramidal",
},
"cartpole": {
"solver": "newton",
"integrator": "implicitfast",
"njmax": 5,
"nconmax": 0,
"ls_parallel": False,
"cone": "pyramidal",
},
"ant": {
"solver": "newton",
"integrator": "implicitfast",
"njmax": 38,
"nconmax": 15,
"ls_parallel": False,
"cone": "pyramidal",
},
"quadruped": {
"solver": "newton",
"integrator": "implicitfast",
"njmax": 75,
"nconmax": 50,
"ls_parallel": False,
"cone": "pyramidal",
},
"allegro": {
"solver": "newton",
"integrator": "implicitfast",
"njmax": 60,
"nconmax": 40,
"ls_parallel": False,
"cone": "elliptic",
},
"kitchen": {
"setup_builder": lambda x: _setup_kitchen(x),
"njmax": 3800,
"nconmax": 900,
},
"tabletop": {
"setup_builder": lambda x: _setup_tabletop(x),
"njmax": 100,
"nconmax": 20,
},
}
def _setup_humanoid(articulation_builder):
articulation_builder.add_mjcf(
newton.examples.get_asset("nv_humanoid.xml"),
ignore_names=["floor", "ground"],
up_axis="Z",
parse_sites=False, # AD: remove once asset is fixed
enable_self_collisions=False, # Keep False for consistent benchmark performance
)
# Setting root pose
root_dofs = 7
articulation_builder.joint_q[:3] = [0.0, 0.0, 1.5]
return root_dofs
def _setup_g1(articulation_builder):
articulation_builder.default_joint_cfg = newton.ModelBuilder.JointDofConfig(
limit_ke=1.0e3, limit_kd=1.0e1, friction=1e-5
)
articulation_builder.default_shape_cfg.ke = 5.0e4
articulation_builder.default_shape_cfg.kd = 5.0e2
articulation_builder.default_shape_cfg.kf = 1.0e3
articulation_builder.default_shape_cfg.mu = 0.75
asset_path = newton.utils.download_asset("unitree_g1")
articulation_builder.add_usd(
str(asset_path / "usd" / "g1_isaac.usd"),
xform=wp.transform(wp.vec3(0, 0, 0.8)),
collapse_fixed_joints=True,
enable_self_collisions=False,
hide_collision_shapes=True,
)
for i in range(6, articulation_builder.joint_dof_count):
articulation_builder.joint_target_ke[i] = 1000.0
articulation_builder.joint_target_kd[i] = 5.0
# approximate meshes for faster collision detection
articulation_builder.approximate_meshes("bounding_box")
root_dofs = 7
return root_dofs
def _setup_h1(articulation_builder):
articulation_builder.default_joint_cfg = newton.ModelBuilder.JointDofConfig(
limit_ke=1.0e3, limit_kd=1.0e1, friction=1e-5
)
articulation_builder.default_shape_cfg.ke = 5.0e4
articulation_builder.default_shape_cfg.kd = 5.0e2
articulation_builder.default_shape_cfg.kf = 1.0e3
articulation_builder.default_shape_cfg.mu = 0.75
asset_path = newton.utils.download_asset("unitree_h1")
asset_file = str(asset_path / "usd" / "h1_minimal.usda")
articulation_builder.add_usd(
asset_file,
ignore_paths=["/GroundPlane"],
collapse_fixed_joints=False,
enable_self_collisions=False,
hide_collision_shapes=True,
)
# approximate meshes for faster collision detection
articulation_builder.approximate_meshes("bounding_box")
for i in range(articulation_builder.joint_dof_count):
articulation_builder.joint_target_ke[i] = 150
articulation_builder.joint_target_kd[i] = 5
root_dofs = 7
return root_dofs
def _setup_cartpole(articulation_builder):
articulation_builder.default_shape_cfg.density = 100.0
articulation_builder.default_joint_cfg.armature = 0.1
articulation_builder.add_usd(
newton.examples.get_asset("cartpole_single_pendulum.usda"),
enable_self_collisions=False,
collapse_fixed_joints=True,
)
armature_inertia = wp.mat33(np.eye(3, dtype=np.float32)) * 0.1
for i in range(articulation_builder.body_count):
articulation_builder.body_inertia[i] = articulation_builder.body_inertia[i] + armature_inertia
# set initial joint positions (cartpole has 2 joints: prismatic slider + revolute pole)
# joint_q[0] = slider position, joint_q[1] = pole angle
articulation_builder.joint_q[0] = 0.0 # slider at origin
articulation_builder.joint_q[1] = 0.3 # pole tilted
# Setting root pose
root_dofs = 1
return root_dofs
def _setup_ant(articulation_builder):
articulation_builder.add_usd(
newton.examples.get_asset("ant.usda"),
collapse_fixed_joints=True,
)
# Setting root pose
root_dofs = 7
articulation_builder.joint_q[:3] = [0.0, 0.0, 1.5]
return root_dofs
def _setup_quadruped(articulation_builder):
articulation_builder.default_joint_cfg.armature = 0.01
articulation_builder.default_shape_cfg.ke = 1.0e4
articulation_builder.default_shape_cfg.kd = 1.0e2
articulation_builder.default_shape_cfg.kf = 1.0e2
articulation_builder.default_shape_cfg.mu = 1.0
articulation_builder.add_urdf(
newton.examples.get_asset("quadruped.urdf"),
xform=wp.transform([0.0, 0.0, 0.7], wp.quat_identity()),
floating=True,
enable_self_collisions=False,
)
armature_inertia = wp.mat33(np.eye(3, dtype=np.float32)) * 0.01
for i in range(articulation_builder.body_count):
articulation_builder.body_inertia[i] = articulation_builder.body_inertia[i] + armature_inertia
root_dofs = 7
return root_dofs
def _setup_allegro(articulation_builder):
asset_path = newton.utils.download_asset("wonik_allegro")
asset_file = str(asset_path / "usd" / "allegro_left_hand_with_cube.usda")
articulation_builder.add_usd(
asset_file,
xform=wp.transform(wp.vec3(0, 0, 0.5)),
enable_self_collisions=True,
ignore_paths=[".*Dummy", ".*CollisionPlane", ".*goal", ".*DexCube/visuals"],
)
# set joint targets and joint drive gains
for i in range(articulation_builder.joint_dof_count):
articulation_builder.joint_target_ke[i] = 150
articulation_builder.joint_target_kd[i] = 5
articulation_builder.joint_target_pos[i] = 0.0
root_dofs = 1
return root_dofs
def _setup_kitchen(articulation_builder):
asset_path = newton.utils.download_asset("kitchen")
asset_file = str(asset_path / "mjcf" / "kitchen.xml")
articulation_builder.add_mjcf(
asset_file,
collapse_fixed_joints=True,
enable_self_collisions=False, # Keep False for consistent benchmark performance
)
# Change pose of the robot to minimize overlap
articulation_builder.joint_q[:2] = [1.5, -1.5]
def _setup_tabletop(articulation_builder):
articulation_builder.add_mjcf(
newton.examples.get_asset("tabletop.xml"),
collapse_fixed_joints=True,
)
class Example:
def __init__(
self,
robot="humanoid",
environment="None",
stage_path=None,
world_count=1,
use_cuda_graph=True,
use_mujoco_cpu=False,
randomize=False,
headless=False,
actuation="None",
solver=None,
integrator=None,
solver_iteration=None,
ls_iteration=None,
njmax=None,
nconmax=None,
builder=None,
ls_parallel=None,
cone=None,
):
fps = 600
self.sim_time = 0.0
self.benchmark_time = 0.0
self.frame_dt = 1.0 / fps
self.sim_substeps = 10
self.contacts = None
self.sim_dt = self.frame_dt / self.sim_substeps
self.world_count = world_count
self.use_cuda_graph = use_cuda_graph
self.use_mujoco_cpu = use_mujoco_cpu
self.actuation = actuation
# set numpy random seed
self.seed = 123
self.rng = np.random.default_rng(self.seed)
if not stage_path:
stage_path = "example_" + robot + ".usd"
if builder is None:
builder = Example.create_model_builder(robot, world_count, environment, randomize, self.seed)
# finalize model
self.model = builder.finalize()
self.solver = Example.create_solver(
self.model,
robot,
use_mujoco_cpu=use_mujoco_cpu,
environment=environment,
solver=solver,
integrator=integrator,
solver_iteration=solver_iteration,
ls_iteration=ls_iteration,
njmax=njmax,
nconmax=nconmax,
ls_parallel=ls_parallel,
cone=cone,
)
if stage_path and not headless:
self.renderer = newton.viewer.ViewerGL()
self.renderer.set_model(self.model)
self.renderer.set_world_offsets((4.0, 4.0, 0.0))
else:
self.renderer = None
self.control = self.model.control()
self.state_0, self.state_1 = self.model.state(), self.model.state()
newton.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, self.state_0)
self.sensor_contact = None
sensing_bodies = ROBOT_CONFIGS.get(robot, {}).get("sensing_bodies", None)
if sensing_bodies is not None:
self.sensor_contact = SensorContact(self.model, sensing_obj_bodies=sensing_bodies, counterpart_bodies="*")
self.contacts = newton.Contacts(
self.solver.get_max_contact_count(),
0,
device=self.model.device,
requested_attributes=self.model.get_requested_contact_attributes(),
)
self.graph = None
if self.use_cuda_graph:
# simulate() allocates memory via a clone, so we can't use graph capture if the device does not support mempools
cuda_graph_comp = wp.get_device().is_cuda and wp.is_mempool_enabled(wp.get_device())
if not cuda_graph_comp:
print("Cannot use graph capture. Graph capture is disabled.")
else:
with wp.ScopedCapture() as capture:
self.simulate()
self.graph = capture.graph
def simulate(self):
for _ in range(self.sim_substeps):
self.state_0.clear_forces()
self.solver.step(self.state_0, self.state_1, self.control, self.contacts, self.sim_dt)
self.state_0, self.state_1 = self.state_1, self.state_0
if self.sensor_contact is not None:
self.solver.update_contacts(self.contacts, self.state_0)
self.sensor_contact.update(self.state_0, self.contacts)
def step(self):
if self.actuation == "random":
joint_target = wp.array(self.rng.uniform(-1.0, 1.0, size=self.model.joint_dof_count), dtype=wp.float32)
wp.copy(self.control.joint_target_pos, joint_target)
wp.synchronize_device()
start_time = time.time()
if self.use_cuda_graph:
wp.capture_launch(self.graph)
else:
self.simulate()
wp.synchronize_device()
end_time = time.time()
self.benchmark_time += end_time - start_time
self.sim_time += self.frame_dt
def render(self):
if self.renderer is None:
return
self.renderer.begin_frame(self.sim_time)
self.renderer.log_state(self.state_0)
self.renderer.end_frame()
@staticmethod
def create_model_builder(robot, world_count, environment="None", randomize=False, seed=123) -> newton.ModelBuilder:
rng = np.random.default_rng(seed)
articulation_builder = newton.ModelBuilder()
articulation_builder.default_shape_cfg.ke = 1.0e3
articulation_builder.default_shape_cfg.kd = 1.0e2
newton.solvers.SolverMuJoCo.register_custom_attributes(articulation_builder)
if robot == "humanoid":
root_dofs = _setup_humanoid(articulation_builder)
elif robot == "g1":
root_dofs = _setup_g1(articulation_builder)
elif robot == "h1":
root_dofs = _setup_h1(articulation_builder)
elif robot == "cartpole":
root_dofs = _setup_cartpole(articulation_builder)
elif robot == "ant":
root_dofs = _setup_ant(articulation_builder)
elif robot == "quadruped":
root_dofs = _setup_quadruped(articulation_builder)
elif robot == "allegro":
root_dofs = _setup_allegro(articulation_builder)
else:
raise ValueError(f"Name of the provided robot not recognized: {robot}")
custom_setup_fn = ROBOT_CONFIGS.get(environment, {}).get("setup_builder", None)
if custom_setup_fn is not None:
custom_setup_fn(articulation_builder)
builder = newton.ModelBuilder()
builder.replicate(articulation_builder, world_count)
if randomize:
njoint = len(articulation_builder.joint_q)
for i in range(world_count):
istart = i * njoint
builder.joint_q[istart + root_dofs : istart + njoint] = rng.uniform(
-1.0, 1.0, size=(njoint - root_dofs)
).tolist()
builder.default_shape_cfg.ke = 1.0e3
builder.default_shape_cfg.kd = 1.0e2
if robot != "cartpole":
# Disable all collisions for the cartpole benchmark
builder.add_ground_plane()
return builder
@staticmethod
def create_solver(
model,
robot,
*,
use_mujoco_cpu=False,
environment="None",
solver=None,
integrator=None,
solver_iteration=None,
ls_iteration=None,
njmax=None,
nconmax=None,
ls_parallel=None,
cone=None,
):
solver_iteration = solver_iteration if solver_iteration is not None else 100
ls_iteration = ls_iteration if ls_iteration is not None else 50
solver = solver if solver is not None else ROBOT_CONFIGS[robot]["solver"]
integrator = integrator if integrator is not None else ROBOT_CONFIGS[robot]["integrator"]
njmax = njmax if njmax is not None else ROBOT_CONFIGS[robot]["njmax"]
nconmax = nconmax if nconmax is not None else ROBOT_CONFIGS[robot]["nconmax"]
ls_parallel = ls_parallel if ls_parallel is not None else ROBOT_CONFIGS[robot]["ls_parallel"]
cone = cone if cone is not None else ROBOT_CONFIGS[robot]["cone"]
njmax += ROBOT_CONFIGS.get(environment, {}).get("njmax", 0)
nconmax += ROBOT_CONFIGS.get(environment, {}).get("nconmax", 0)
return newton.solvers.SolverMuJoCo(
model,
use_mujoco_cpu=use_mujoco_cpu,
solver=solver,
integrator=integrator,
iterations=solver_iteration,
ls_iterations=ls_iteration,
njmax=njmax,
nconmax=nconmax,
ls_parallel=ls_parallel,
cone=cone,
)
def print_trace(trace, indent, steps):
if indent == 0:
print("================= Profiling =================")
for k, v in trace.items():
times, sub_trace = v
print(" " * indent + f"{k}: {times / steps:.4f}")
print_trace(sub_trace, indent + 1, steps)
if indent == 0:
step_time = trace["step"][0]
step_trace = trace["step"][1]
mujoco_warp_step_time = step_trace["_mujoco_warp_step"][0]
overhead = 100.0 * (step_time - mujoco_warp_step_time) / step_time
print("---------------------------------------------")
print(f"Newton overhead:\t{overhead:.2f} %")
print("=============================================")
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("--robot", type=str, default="humanoid", help="Name of the robot to simulate.")
parser.add_argument("--env", type=str, default="None", help="Name of the environment where the robot is located.")
parser.add_argument(
"--event-trace", default=False, action=argparse.BooleanOptionalAction, help="Print profiling information."
)
parser.add_argument("--device", type=str, default=None, help="Override the default Warp device.")
parser.add_argument(
"--stage-path",
type=lambda x: None if x == "None" else str(x),
default=None,
help="Path to the output USD file.",
)
parser.add_argument("--num-frames", type=int, default=12000, help="Total number of frames.")
parser.add_argument("--world-count", type=int, default=1, help="Total number of simulated worlds.")
parser.add_argument("--use-cuda-graph", default=True, action=argparse.BooleanOptionalAction)
parser.add_argument(
"--use-mujoco-cpu",
default=False,
action=argparse.BooleanOptionalAction,
help="Use Mujoco-C CPU (Not yet supported).",
)
parser.add_argument(
"--headless", default=False, action=argparse.BooleanOptionalAction, help="Run the simulation in headless mode."
)
parser.add_argument(
"--random-init", default=False, action=argparse.BooleanOptionalAction, help="Randomize initial pose."
)
parser.add_argument(
"--actuation",
type=str,
default="None",
choices=["None", "random"],
help="Type of action to apply at each step.",
)
parser.add_argument(
"--solver", type=str, default=None, choices=["cg", "newton"], help="Mujoco model constraint solver used."
)
parser.add_argument(
"--integrator", type=str, default=None, choices=["euler", "rk4", "implicit"], help="Mujoco integrator used."
)
parser.add_argument("--solver-iteration", type=int, default=None, help="Number of solver iterations.")
parser.add_argument("--ls-iteration", type=int, default=None, help="Number of linesearch iterations.")
parser.add_argument("--njmax", type=int, default=None, help="Maximum number of constraints per world.")
parser.add_argument("--nconmax", type=int, default=None, help="Maximum number of collision per world.")
parser.add_argument(
"--ls-parallel", default=None, action=argparse.BooleanOptionalAction, help="Use parallel line search."
)
parser.add_argument("--cone", type=str, default=None, choices=["pyramidal", "elliptic"], help="Friction cone type.")
args = parser.parse_known_args()[0]
if args.use_mujoco_cpu:
args.use_mujoco_cpu = False
print("The option ``use-mujoco-cpu`` is not yet supported. Disabling it.")
trace = {}
with EventTracer(enabled=args.event_trace) as tracer:
with wp.ScopedDevice(args.device):
example = Example(
robot=args.robot,
environment=args.env,
stage_path=args.stage_path,
world_count=args.world_count,
use_cuda_graph=args.use_cuda_graph,
use_mujoco_cpu=args.use_mujoco_cpu,
randomize=args.random_init,
headless=args.headless,
actuation=args.actuation,
solver=args.solver,
integrator=args.integrator,
solver_iteration=args.solver_iteration,
ls_iteration=args.ls_iteration,
njmax=args.njmax,
nconmax=args.nconmax,
ls_parallel=args.ls_parallel,
cone=args.cone,
)
# Print simulation configuration summary
LABEL_WIDTH = 25
TOTAL_WIDTH = 45
title = " Simulation Configuration "
print(f"\n{title.center(TOTAL_WIDTH, '=')}")
print(f"{'Simulation Steps':<{LABEL_WIDTH}}: {args.num_frames * example.sim_substeps}")
print(f"{'World Count':<{LABEL_WIDTH}}: {args.world_count}")
print(f"{'Robot Type':<{LABEL_WIDTH}}: {args.robot}")
print(f"{'Timestep (dt)':<{LABEL_WIDTH}}: {example.sim_dt:.6f}s")
print(f"{'Randomize Initial Pose':<{LABEL_WIDTH}}: {args.random_init!s}")
print("-" * TOTAL_WIDTH)
# Map MuJoCo solver enum back to string
solver_value = example.solver.mj_model.opt.solver
solver_map = {0: "PGS", 1: "CG", 2: "Newton"} # mjSOL_PGS = 0, mjSOL_CG = 1, mjSOL_NEWTON = 2
actual_solver = solver_map.get(solver_value, f"unknown({solver_value})")
# Map MuJoCo integrator enum back to string
integrator_map = {
0: "Euler",
1: "RK4",
2: "Implicit",
3: "Implicitfast",
} # mjINT_EULER = 0, mjINT_RK4 = 1, mjINT_IMPLICIT = 2, mjINT_IMPLICITFAST = 3
actual_integrator = integrator_map.get(example.solver.mj_model.opt.integrator, "unknown")
# Map MuJoCo cone enum back to string
cone_value = example.solver.mj_model.opt.cone
cone_map = {0: "pyramidal", 1: "elliptic"} # mjCONE_PYRAMIDAL = 0, mjCONE_ELLIPTIC = 1
actual_cone = cone_map.get(cone_value, f"unknown({cone_value})")
# Get actual max constraints and contacts from MuJoCo Warp data
actual_njmax = example.solver.mjw_data.njmax
actual_nconmax = (
example.solver.mjw_data.naconmax // args.world_count
if args.world_count > 0
else example.solver.mjw_data.naconmax
)
print(f"{'Solver':<{LABEL_WIDTH}}: {actual_solver}")
print(f"{'Integrator':<{LABEL_WIDTH}}: {actual_integrator}")
# print(f"{'Parallel Line Search':<{LABEL_WIDTH}}: {example.solver.mj_model.opt.ls_parallel}")
print(f"{'Cone':<{LABEL_WIDTH}}: {actual_cone}")
print(f"{'Solver Iterations':<{LABEL_WIDTH}}: {example.solver.mj_model.opt.iterations}")
print(f"{'Line Search Iterations':<{LABEL_WIDTH}}: {example.solver.mj_model.opt.ls_iterations}")
print(f"{'Max Constraints / world':<{LABEL_WIDTH}}: {actual_njmax}")
print(f"{'Max Contacts / world':<{LABEL_WIDTH}}: {actual_nconmax}")
print(f"{'Joint DOFs':<{LABEL_WIDTH}}: {example.model.joint_dof_count}")
print(f"{'Body Count':<{LABEL_WIDTH}}: {example.model.body_count}")
print("-" * TOTAL_WIDTH)
print(f"{'Execution Device':<{LABEL_WIDTH}}: {wp.get_device()}")
print(f"{'Use CUDA Graph':<{LABEL_WIDTH}}: {example.use_cuda_graph!s}")
print("=" * TOTAL_WIDTH + "\n")
for _ in range(args.num_frames):
example.step()
example.render()
if args.event_trace:
trace = tracer.add_trace(trace, tracer.trace())
if args.event_trace:
print_trace(trace, 0, args.num_frames * example.sim_substeps)
================================================
FILE: asv/benchmarks/compilation/__init__.py
================================================
================================================
FILE: asv/benchmarks/compilation/bench_example_load.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import subprocess
import sys
import warp as wp
wp.config.enable_backward = False
wp.config.quiet = True
from asv_runner.benchmarks.mark import skip_benchmark_if
class SlowExampleRobotAnymal:
warmup_time = 0
repeat = 2
number = 1
timeout = 600
def setup(self):
wp.build.clear_lto_cache()
wp.build.clear_kernel_cache()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_load(self):
"""Time the amount of time it takes to load and run one frame of the example."""
command = [
sys.executable,
"-m",
"newton.examples.robot.example_robot_anymal_c_walk",
"--num-frames",
"1",
"--viewer",
"null",
]
# Run the script as a subprocess
subprocess.run(command, capture_output=True, text=True, check=True)
class SlowExampleRobotCartpole:
warmup_time = 0
repeat = 2
number = 1
timeout = 600
def setup(self):
wp.build.clear_lto_cache()
wp.build.clear_kernel_cache()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_load(self):
"""Time the amount of time it takes to load and run one frame of the example."""
command = [
sys.executable,
"-m",
"newton.examples.robot.example_robot_cartpole",
"--num-frames",
"1",
"--viewer",
"null",
]
# Run the script as a subprocess
subprocess.run(command, capture_output=True, text=True, check=True)
class SlowExampleClothFranka:
warmup_time = 0
repeat = 2
number = 1
def setup(self):
wp.build.clear_lto_cache()
wp.build.clear_kernel_cache()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_load(self):
"""Time the amount of time it takes to load and run one frame of the example."""
command = [
sys.executable,
"-m",
"newton.examples.cloth.example_cloth_franka",
"--num-frames",
"1",
"--viewer",
"null",
]
# Run the script as a subprocess
subprocess.run(command, capture_output=True, text=True, check=True)
class SlowExampleClothTwist:
warmup_time = 0
repeat = 2
number = 1
def setup(self):
wp.build.clear_lto_cache()
wp.build.clear_kernel_cache()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_load(self):
"""Time the amount of time it takes to load and run one frame of the example."""
command = [
sys.executable,
"-m",
"newton.examples.cloth.example_cloth_twist",
"--num-frames",
"1",
"--viewer",
"null",
]
# Run the script as a subprocess
subprocess.run(command, capture_output=True, text=True, check=True)
class SlowExampleBasicUrdf:
warmup_time = 0
repeat = 2
number = 1
timeout = 600
def setup(self):
wp.build.clear_lto_cache()
wp.build.clear_kernel_cache()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_load(self):
"""Time the amount of time it takes to load and run one frame of the example."""
command = [
sys.executable,
"-m",
"newton.examples.basic.example_basic_urdf",
"--num-frames",
"1",
"--viewer",
"null",
]
# Run the script as a subprocess
subprocess.run(command, capture_output=True, text=True, check=True)
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"SlowExampleBasicUrdf": SlowExampleBasicUrdf,
"SlowExampleRobotAnymal": SlowExampleRobotAnymal,
"SlowExampleRobotCartpole": SlowExampleRobotCartpole,
"SlowExampleClothFranka": SlowExampleClothFranka,
"SlowExampleClothTwist": SlowExampleClothTwist,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/setup/__init__.py
================================================
================================================
FILE: asv/benchmarks/setup/bench_model.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import gc
import os
import sys
# Force headless mode for CI environments before any pyglet imports
os.environ["PYGLET_HEADLESS"] = "1"
import warp as wp
wp.config.enable_backward = False
wp.config.quiet = True
from asv_runner.benchmarks.mark import skip_benchmark_if
parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
sys.path.append(parent_dir)
from benchmark_mujoco import Example
from newton.viewer import ViewerGL
class KpiInitializeModel:
params = (["humanoid", "g1", "cartpole"], [8192])
param_names = ["robot", "world_count"]
rounds = 1
repeat = 3
number = 1
min_run_count = 1
timeout = 3600
def setup(self, robot, world_count):
wp.init()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_initialize_model(self, robot, world_count):
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
_model = builder.finalize()
wp.synchronize_device()
class KpiInitializeSolver:
params = (["humanoid", "g1", "cartpole", "ant"], [8192])
param_names = ["robot", "world_count"]
rounds = 1
repeat = 3
number = 1
min_run_count = 1
timeout = 3600
def setup(self, robot, world_count):
wp.init()
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
self._model = builder.finalize()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_initialize_solver(self, robot, world_count):
self._solver = Example.create_solver(self._model, robot, use_mujoco_cpu=False)
wp.synchronize_device()
def teardown(self, robot, world_count):
del self._solver
del self._model
class KpiInitializeViewerGL:
params = (["g1"], [8192])
param_names = ["robot", "world_count"]
rounds = 1
repeat = 3
number = 1
min_run_count = 1
def setup(self, robot, world_count):
wp.init()
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
self._model = builder.finalize()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_initialize_renderer(self, robot, world_count):
# Setting up the renderer
self.renderer = ViewerGL(headless=True)
self.renderer.set_model(self._model)
wp.synchronize_device()
self.renderer.close()
def teardown(self, robot, world_count):
del self._model
class FastInitializeModel:
params = (["humanoid", "g1", "cartpole"], [256])
param_names = ["robot", "world_count"]
rounds = 1
repeat = 3
number = 1
min_run_count = 1
def setup_cache(self):
# Load a small model to cache the kernels
for robot in self.params[0]:
builder = Example.create_model_builder(robot, 1, randomize=False, seed=123)
model = builder.finalize(device="cpu")
del model
del builder
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_initialize_model(self, robot, world_count):
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
_model = builder.finalize()
wp.synchronize_device()
def peakmem_initialize_model_cpu(self, robot, world_count):
gc.collect()
with wp.ScopedDevice("cpu"):
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
model = builder.finalize()
del model
class FastInitializeSolver:
params = (["humanoid", "g1", "cartpole"], [256])
param_names = ["robot", "world_count"]
rounds = 1
repeat = 3
number = 1
min_run_count = 1
def setup(self, robot, world_count):
wp.init()
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
self._model = builder.finalize()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_initialize_solver(self, robot, world_count):
self._solver = Example.create_solver(self._model, robot, use_mujoco_cpu=False)
wp.synchronize_device()
def teardown(self, robot, world_count):
del self._solver
del self._model
class FastInitializeViewerGL:
params = (["g1"], [256])
param_names = ["robot", "world_count"]
rounds = 1
repeat = 3
number = 1
min_run_count = 1
def setup(self, robot, world_count):
wp.init()
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
self._model = builder.finalize()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_initialize_renderer(self, robot, world_count):
# Setting up the renderer
self.renderer = ViewerGL(headless=True)
self.renderer.set_model(self._model)
wp.synchronize_device()
self.renderer.close()
def teardown(self, robot, world_count):
del self._model
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"KpiInitializeModel": KpiInitializeModel,
"FastInitializeModel": FastInitializeModel,
"KpiInitializeSolver": KpiInitializeSolver,
"FastInitializeSolver": FastInitializeSolver,
"KpiInitializeViewerGL": KpiInitializeViewerGL,
"FastInitializeViewerGL": FastInitializeViewerGL,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/__init__.py
================================================
================================================
FILE: asv/benchmarks/simulation/bench_anymal.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from asv_runner.benchmarks.mark import skip_benchmark_if
wp.config.enable_backward = False
wp.config.quiet = True
import newton
import newton.examples
from newton.examples.robot.example_robot_anymal_c_walk import Example
class FastExampleAnymalPretrained:
repeat = 3
number = 1
def setup(self):
self.num_frames = 50
if hasattr(newton.examples, "default_args"):
args = newton.examples.default_args()
else:
args = None
self.example = Example(newton.viewer.ViewerNull(num_frames=self.num_frames), args)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
for _ in range(self.num_frames):
self.example.step()
wp.synchronize_device()
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"FastExampleAnymalPretrained": FastExampleAnymalPretrained,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_cable.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from asv_runner.benchmarks.mark import skip_benchmark_if
wp.config.enable_backward = False
wp.config.quiet = True
import newton.examples
from newton.examples.cable.example_cable_pile import Example as ExampleCablePile
from newton.viewer import ViewerNull
class FastExampleCablePile:
number = 1
rounds = 2
repeat = 2
def setup(self):
self.num_frames = 30
if hasattr(newton.examples, "default_args"):
args = newton.examples.default_args()
else:
args = None
self.example = ExampleCablePile(ViewerNull(num_frames=self.num_frames), args)
wp.synchronize_device()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
newton.examples.run(self.example, args=None)
wp.synchronize_device()
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"FastExampleCablePile": FastExampleCablePile,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_cloth.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from asv_runner.benchmarks.mark import skip_benchmark_if
wp.config.quiet = True
import newton.examples
from newton.examples.cloth.example_cloth_franka import Example as ExampleClothManipulation
from newton.examples.cloth.example_cloth_twist import Example as ExampleClothTwist
from newton.viewer import ViewerNull
class FastExampleClothManipulation:
timeout = 300
repeat = 3
number = 1
def setup(self):
self.num_frames = 30
if hasattr(newton.examples, "default_args"):
args = newton.examples.default_args()
else:
args = None
self.example = ExampleClothManipulation(ViewerNull(num_frames=self.num_frames), args)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
newton.examples.run(self.example, args=None)
wp.synchronize_device()
class FastExampleClothTwist:
repeat = 5
number = 1
def setup(self):
self.num_frames = 100
if hasattr(newton.examples, "default_args"):
args = newton.examples.default_args()
else:
args = None
self.example = ExampleClothTwist(ViewerNull(num_frames=self.num_frames), args)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
newton.examples.run(self.example, None)
wp.synchronize_device()
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"FastExampleClothManipulation": FastExampleClothManipulation,
"FastExampleClothTwist": FastExampleClothTwist,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_contacts.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from asv_runner.benchmarks.mark import SkipNotImplemented, skip_benchmark_if
wp.config.enable_backward = False
wp.config.quiet = True
import importlib
import newton.examples
from newton.viewer import ViewerNull
ISAACGYM_ENVS_REPO_URL = "https://github.com/isaac-sim/IsaacGymEnvs.git"
ISAACGYM_NUT_BOLT_FOLDER = "assets/factory/mesh/factory_nut_bolt"
try:
from newton.examples import download_external_git_folder as _download_external_git_folder
except ImportError:
from newton._src.utils.download_assets import download_git_folder as _download_external_git_folder
def _import_example_class(module_names: list[str]):
"""Import and return the ``Example`` class from candidate modules.
Args:
module_names: Ordered module names to try importing.
Returns:
The first successfully imported module's ``Example`` class.
Raises:
SkipNotImplemented: If none of the module names can be imported.
"""
for module_name in module_names:
try:
module = importlib.import_module(module_name)
except ModuleNotFoundError:
continue
return module.Example
raise SkipNotImplemented
class FastExampleContactSdfDefaults:
"""Benchmark the SDF nut-bolt example default configuration."""
repeat = 2
number = 1
def setup_cache(self):
_download_external_git_folder(ISAACGYM_ENVS_REPO_URL, ISAACGYM_NUT_BOLT_FOLDER)
def setup(self):
example_cls = _import_example_class(
[
"newton.examples.contacts.example_nut_bolt_sdf",
]
)
self.num_frames = 20
if hasattr(newton.examples, "default_args") and hasattr(example_cls, "create_parser"):
args = newton.examples.default_args(example_cls.create_parser())
self.example = example_cls(ViewerNull(num_frames=self.num_frames), args)
else:
self.example = example_cls(
viewer=ViewerNull(num_frames=self.num_frames),
world_count=100,
num_per_world=1,
scene="nut_bolt",
solver="mujoco",
test_mode=False,
)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
for _ in range(self.num_frames):
self.example.step()
wp.synchronize_device()
class FastExampleContactHydroWorkingDefaults:
"""Benchmark the hydroelastic nut-bolt example default configuration."""
repeat = 2
number = 1
def setup_cache(self):
_download_external_git_folder(ISAACGYM_ENVS_REPO_URL, ISAACGYM_NUT_BOLT_FOLDER)
def setup(self):
example_cls = _import_example_class(
[
"newton.examples.contacts.example_nut_bolt_hydro",
]
)
self.num_frames = 20
if hasattr(newton.examples, "default_args") and hasattr(example_cls, "create_parser"):
args = newton.examples.default_args(example_cls.create_parser())
self.example = example_cls(ViewerNull(num_frames=self.num_frames), args)
else:
self.example = example_cls(
viewer=ViewerNull(num_frames=self.num_frames),
world_count=20,
num_per_world=1,
scene="nut_bolt",
solver="mujoco",
test_mode=False,
)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
for _ in range(self.num_frames):
self.example.step()
wp.synchronize_device()
class FastExampleContactPyramidDefaults:
"""Benchmark the box pyramid example with default configuration."""
repeat = 2
number = 1
def setup(self):
example_cls = _import_example_class(
[
"newton.examples.contacts.example_pyramid",
]
)
self.num_frames = 20
if hasattr(newton.examples, "default_args") and hasattr(example_cls, "create_parser"):
args = newton.examples.default_args(example_cls.create_parser())
self.example = example_cls(ViewerNull(num_frames=self.num_frames), args)
else:
self.example = example_cls(
viewer=ViewerNull(num_frames=self.num_frames),
solver="xpbd",
test_mode=False,
)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
for _ in range(self.num_frames):
self.example.step()
wp.synchronize_device()
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"FastExampleContactSdfDefaults": FastExampleContactSdfDefaults,
"FastExampleContactHydroWorkingDefaults": FastExampleContactHydroWorkingDefaults,
"FastExampleContactPyramidDefaults": FastExampleContactPyramidDefaults,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_heightfield.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from asv_runner.benchmarks.mark import SkipNotImplemented, skip_benchmark_if
wp.config.enable_backward = False
wp.config.quiet = True
import numpy as np
import newton
def _build_heightfield_scene(num_bodies=200, nrow=100, ncol=100):
"""Build a scene with many spheres dropped onto a large heightfield."""
builder = newton.ModelBuilder()
hx, hy = 20.0, 20.0
x = np.linspace(-hx, hx, ncol)
y = np.linspace(-hy, hy, nrow)
xx, yy = np.meshgrid(x, y)
elevation = np.sin(xx * 0.5) * np.cos(yy * 0.5) * 1.0
hfield = newton.Heightfield(data=elevation, nrow=nrow, ncol=ncol, hx=hx, hy=hy)
builder.add_shape_heightfield(heightfield=hfield)
# Grid of spheres above the terrain
grid_size = int(np.ceil(np.sqrt(num_bodies)))
spacing = 2.0 * hx / (grid_size + 1)
count = 0
for i in range(grid_size):
for j in range(grid_size):
if count >= num_bodies:
break
x_pos = -hx + spacing * (i + 1)
y_pos = -hy + spacing * (j + 1)
body = builder.add_body(
xform=wp.transform(p=wp.vec3(x_pos, y_pos, 3.0), q=wp.quat_identity()),
)
builder.add_shape_sphere(body=body, radius=0.3)
count += 1
model = builder.finalize()
model.rigid_contact_max = num_bodies * 20
return model
class HeightfieldCollision:
"""Benchmark heightfield collision with many spheres on a 100x100 grid."""
repeat = 8
number = 1
def setup(self):
cuda_graph_comp = wp.get_device().is_cuda and wp.is_mempool_enabled(wp.get_device())
if not cuda_graph_comp:
raise SkipNotImplemented
self.num_frames = 50
self.model = _build_heightfield_scene(num_bodies=200, nrow=100, ncol=100)
self.solver = newton.solvers.SolverXPBD(self.model, iterations=10)
self.contacts = self.model.contacts()
self.state_0 = self.model.state()
self.state_1 = self.model.state()
self.control = self.model.control()
self.sim_substeps = 10
self.sim_dt = (1.0 / 100.0) / self.sim_substeps
wp.synchronize_device()
with wp.ScopedCapture() as capture:
for _sub in range(self.sim_substeps):
self.state_0.clear_forces()
self.model.collide(self.state_0, self.contacts)
self.solver.step(self.state_0, self.state_1, self.control, self.contacts, self.sim_dt)
self.state_0, self.state_1 = self.state_1, self.state_0
self.graph = capture.graph
wp.synchronize_device()
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
for _frame in range(self.num_frames):
wp.capture_launch(self.graph)
wp.synchronize_device()
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"HeightfieldCollision": HeightfieldCollision,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_ik.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import os
import sys
import numpy as np
import warp as wp
from asv_runner.benchmarks.mark import SkipNotImplemented, skip_benchmark_if
wp.config.quiet = True
wp.config.enable_backward = False
parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
sys.path.append(parent_dir)
from benchmark_ik import build_ik_solver, create_franka_model, eval_success, fk_targets, random_solutions
class _IKBenchmark:
"""Utility base class for IK benchmarks."""
params = None
param_names = ["batch_size"]
repeat = None
number = 1
rounds = 2
EE_LINKS = (9,)
ITERATIONS = 16
STEP_SIZE = 1.0
POS_THRESH_M = 5e-3
ORI_THRESH_RAD = 0.05
SEED = 123
NUM_SOLVES = 50
def setup(self, batch_size):
if not (wp.get_device().is_cuda and wp.is_mempool_enabled(wp.get_device())):
raise SkipNotImplemented
self.model = create_franka_model()
self.solver, self.pos_obj, self.rot_obj = build_ik_solver(self.model, batch_size, self.EE_LINKS)
self.n_coords = self.model.joint_coord_count
rng = np.random.default_rng(self.SEED)
q_gt = random_solutions(self.model, batch_size, rng)
self.tgt_p, self.tgt_r = fk_targets(self.solver, self.model, q_gt, self.EE_LINKS)
self.winners_d = wp.zeros((batch_size, self.n_coords), dtype=wp.float32)
self.seeds_d = wp.zeros((batch_size, self.n_coords), dtype=wp.float32)
# Set targets
for ee in range(len(self.EE_LINKS)):
self.pos_obj[ee].set_target_positions(
wp.array(self.tgt_p[:, ee].astype(np.float32, copy=False), dtype=wp.vec3)
)
self.rot_obj[ee].set_target_rotations(
wp.array(self.tgt_r[:, ee].astype(np.float32, copy=False), dtype=wp.vec4)
)
with wp.ScopedCapture() as cap:
self.solver.step(self.seeds_d, self.winners_d, iterations=self.ITERATIONS, step_size=self.STEP_SIZE)
self.solve_graph = cap.graph
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_solve(self, batch_size):
for _ in range(self.NUM_SOLVES):
wp.capture_launch(self.solve_graph)
wp.synchronize_device()
def teardown(self, batch_size):
q_best = self.winners_d.numpy()
success = eval_success(
self.solver,
self.model,
q_best,
self.tgt_p,
self.tgt_r,
self.EE_LINKS,
self.POS_THRESH_M,
self.ORI_THRESH_RAD,
)
if not success.all():
n_failed = int((~success).sum())
raise RuntimeError(f"IK failed for {n_failed}/{batch_size} problems")
class FastIKSolve(_IKBenchmark):
params = ([512],)
repeat = 6
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"FastSolve": FastIKSolve,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_mujoco.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import os
import sys
import warp as wp
wp.config.enable_backward = False
wp.config.quiet = True
from asv_runner.benchmarks.mark import SkipNotImplemented, skip_benchmark_if
parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
sys.path.append(parent_dir)
from benchmark_mujoco import Example
from newton.utils import EventTracer
@wp.kernel
def apply_random_control(state: wp.uint32, joint_target: wp.array[float]):
tid = wp.tid()
joint_target[tid] = wp.randf(state) * 2.0 - 1.0
class _FastBenchmark:
"""Utility base class for fast benchmarks."""
num_frames = None
robot = None
number = 1
rounds = 2
repeat = None
world_count = None
random_init = None
environment = "None"
def setup(self):
if not hasattr(self, "builder") or self.builder is None:
self.builder = Example.create_model_builder(
self.robot, self.world_count, randomize=self.random_init, seed=123
)
self.example = Example(
stage_path=None,
robot=self.robot,
randomize=self.random_init,
headless=True,
actuation="None",
use_cuda_graph=True,
builder=self.builder,
environment=self.environment,
)
wp.synchronize_device()
# Recapture the graph with control application included
cuda_graph_comp = wp.get_device().is_cuda and wp.is_mempool_enabled(wp.get_device())
if not cuda_graph_comp:
raise SkipNotImplemented
else:
state = wp.rand_init(self.example.seed)
with wp.ScopedCapture() as capture:
wp.launch(
apply_random_control,
dim=(self.example.model.joint_dof_count,),
inputs=[state],
outputs=[self.example.control.joint_target_pos],
)
self.example.simulate()
self.graph = capture.graph
wp.synchronize_device()
def time_simulate(self):
for _ in range(self.num_frames):
wp.capture_launch(self.graph)
wp.synchronize_device()
class _KpiBenchmark:
"""Utility base class for KPI benchmarks."""
param_names = ["world_count"]
num_frames = None
params = None
robot = None
samples = None
ls_iteration = None
random_init = None
environment = "None"
def setup(self, world_count):
if not hasattr(self, "builder") or self.builder is None:
self.builder = {}
if world_count not in self.builder:
self.builder[world_count] = Example.create_model_builder(
self.robot, world_count, randomize=self.random_init, seed=123
)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def track_simulate(self, world_count):
total_time = 0.0
for _iter in range(self.samples):
example = Example(
stage_path=None,
robot=self.robot,
randomize=self.random_init,
headless=True,
actuation="random",
use_cuda_graph=True,
builder=self.builder[world_count],
ls_iteration=self.ls_iteration,
environment=self.environment,
)
wp.synchronize_device()
for _ in range(self.num_frames):
example.step()
wp.synchronize_device()
total_time += example.benchmark_time
return total_time * 1000 / (self.num_frames * example.sim_substeps * world_count * self.samples)
track_simulate.unit = "ms/world-step"
class _NewtonOverheadBenchmark:
"""Utility base class for measuring Newton overhead."""
param_names = ["world_count"]
num_frames = None
params = None
robot = None
samples = None
ls_iteration = None
random_init = None
def setup(self, world_count):
if not hasattr(self, "builder") or self.builder is None:
self.builder = {}
if world_count not in self.builder:
self.builder[world_count] = Example.create_model_builder(
self.robot, world_count, randomize=self.random_init, seed=123
)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def track_simulate(self, world_count):
trace = {}
with EventTracer(enabled=True) as tracer:
for _iter in range(self.samples):
example = Example(
stage_path=None,
robot=self.robot,
randomize=self.random_init,
headless=True,
actuation="random",
world_count=world_count,
use_cuda_graph=True,
builder=self.builder[world_count],
ls_iteration=self.ls_iteration,
)
for _ in range(self.num_frames):
example.step()
trace = tracer.add_trace(trace, tracer.trace())
step_time = trace["step"][0]
step_trace = trace["step"][1]
mujoco_warp_step_time = step_trace["_mujoco_warp_step"][0]
overhead = 100.0 * (step_time - mujoco_warp_step_time) / step_time
return overhead
track_simulate.unit = "%"
class FastCartpole(_FastBenchmark):
num_frames = 50
robot = "cartpole"
repeat = 8
world_count = 256
random_init = True
environment = "None"
class KpiCartpole(_KpiBenchmark):
params = [[8192]]
num_frames = 50
robot = "cartpole"
samples = 4
ls_iteration = 3
random_init = True
environment = "None"
class FastG1(_FastBenchmark):
num_frames = 25
robot = "g1"
repeat = 2
world_count = 256
random_init = True
environment = "None"
class KpiG1(_KpiBenchmark):
params = [[8192]]
num_frames = 50
robot = "g1"
timeout = 900
samples = 2
ls_iteration = 10
random_init = True
environment = "None"
class FastNewtonOverheadG1(_NewtonOverheadBenchmark):
params = [[256]]
num_frames = 25
robot = "g1"
repeat = 2
samples = 1
random_init = True
class KpiNewtonOverheadG1(_NewtonOverheadBenchmark):
params = [[8192]]
num_frames = 50
robot = "g1"
timeout = 900
samples = 2
ls_iteration = 10
random_init = True
class FastHumanoid(_FastBenchmark):
num_frames = 50
robot = "humanoid"
repeat = 8
world_count = 256
random_init = True
environment = "None"
class KpiHumanoid(_KpiBenchmark):
params = [[8192]]
num_frames = 100
robot = "humanoid"
samples = 4
ls_iteration = 15
random_init = True
environment = "None"
class FastNewtonOverheadHumanoid(_NewtonOverheadBenchmark):
params = [[256]]
num_frames = 50
robot = "humanoid"
repeat = 8
samples = 1
random_init = True
class KpiNewtonOverheadHumanoid(_NewtonOverheadBenchmark):
params = [[8192]]
num_frames = 100
robot = "humanoid"
samples = 4
ls_iteration = 15
random_init = True
class FastAllegro(_FastBenchmark):
num_frames = 100
robot = "allegro"
repeat = 2
world_count = 256
random_init = False
environment = "None"
class KpiAllegro(_KpiBenchmark):
params = [[8192]]
num_frames = 300
robot = "allegro"
samples = 2
ls_iteration = 10
random_init = False
environment = "None"
class FastKitchenG1(_FastBenchmark):
num_frames = 25
robot = "g1"
repeat = 2
world_count = 32
random_init = True
environment = "kitchen"
class KpiKitchenG1(_KpiBenchmark):
params = [[512]]
num_frames = 50
robot = "g1"
timeout = 900
samples = 2
ls_iteration = 10
random_init = True
environment = "kitchen"
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"FastCartpole": FastCartpole,
"FastG1": FastG1,
"FastHumanoid": FastHumanoid,
"FastAllegro": FastAllegro,
"FastKitchenG1": FastKitchenG1,
"FastNewtonOverheadG1": FastNewtonOverheadG1,
"FastNewtonOverheadHumanoid": FastNewtonOverheadHumanoid,
"KpiCartpole": KpiCartpole,
"KpiG1": KpiG1,
"KpiHumanoid": KpiHumanoid,
"KpiAllegro": KpiAllegro,
"KpiKitchenG1": KpiKitchenG1,
"KpiNewtonOverheadG1": KpiNewtonOverheadG1,
"KpiNewtonOverheadHumanoid": KpiNewtonOverheadHumanoid,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_quadruped_xpbd.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from asv_runner.benchmarks.mark import skip_benchmark_if
wp.config.enable_backward = False
wp.config.quiet = True
import newton
import newton.examples
from newton.examples.basic.example_basic_urdf import Example
class FastExampleQuadrupedXPBD:
repeat = 10
number = 1
def setup(self):
self.num_frames = 1000
if hasattr(newton.examples, "default_args") and hasattr(Example, "create_parser"):
args = newton.examples.default_args(Example.create_parser())
args.world_count = 200
self.example = Example(newton.viewer.ViewerNull(num_frames=self.num_frames), args)
else:
self.example = Example(newton.viewer.ViewerNull(num_frames=self.num_frames), 200)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
for _ in range(self.num_frames):
self.example.step()
wp.synchronize_device()
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"FastExampleQuadrupedXPBD": FastExampleQuadrupedXPBD,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_selection.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from asv_runner.benchmarks.mark import skip_benchmark_if
wp.config.enable_backward = False
wp.config.quiet = True
import newton
import newton.examples
from newton.examples.selection.example_selection_cartpole import Example
class FastExampleSelectionCartpoleMuJoCo:
repeat = 10
number = 1
def setup(self):
self.num_frames = 200
if hasattr(newton.examples, "default_args") and hasattr(Example, "create_parser"):
args = newton.examples.default_args(Example.create_parser())
self.example = Example(newton.viewer.ViewerNull(num_frames=self.num_frames), args)
else:
self.example = Example(
viewer=newton.viewer.ViewerNull(num_frames=self.num_frames), world_count=16, verbose=False
)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_simulate(self):
for _ in range(self.num_frames):
self.example.step()
wp.synchronize_device()
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"FastExampleSelectionCartpoleMuJoCo": FastExampleSelectionCartpoleMuJoCo,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv/benchmarks/simulation/bench_sensor_tiled_camera.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from asv_runner.benchmarks.mark import skip_benchmark_if
wp.config.enable_backward = False
wp.config.quiet = True
import math
import newton
from newton.sensors import SensorTiledCamera
NICE_NAMES = {}
def nice_name(value):
def decorator(func):
func._nice_name = value
return func
return decorator
def nice_name_collector():
def decorator(instance):
for name, attr in instance.__dict__.items():
if nice_name := getattr(attr, "_nice_name", None):
NICE_NAMES[name] = nice_name
return instance
return decorator
@nice_name_collector()
class SensorTiledCameraBenchmark:
param_names = ["resolution", "world_count", "iterations"]
params = ([64], [4096], [50])
def setup(self, resolution: int, world_count: int, iterations: int):
self.device = wp.get_preferred_device()
franka = newton.ModelBuilder()
franka.add_urdf(
newton.utils.download_asset("franka_emika_panda") / "urdf/fr3_franka_hand.urdf",
floating=False,
)
scene = newton.ModelBuilder()
scene.replicate(franka, world_count)
scene.add_ground_plane()
self.model = scene.finalize()
self.state = self.model.state()
newton.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, self.state)
self.camera_transforms = wp.array(
[
[
wp.transformf(
wp.vec3f(2.4, 0.0, 0.8),
wp.quatf(0.4187639653682709, 0.4224344491958618, 0.5708873867988586, 0.5659270882606506),
)
]
* world_count
],
dtype=wp.transformf,
)
self.tiled_camera_sensor = SensorTiledCamera(model=self.model)
self.tiled_camera_sensor.utils.create_default_light(enable_shadows=False)
self.tiled_camera_sensor.utils.assign_checkerboard_material_to_all_shapes()
self.camera_rays = self.tiled_camera_sensor.utils.compute_pinhole_camera_rays(
resolution, resolution, math.radians(45.0)
)
self.color_image = self.tiled_camera_sensor.utils.create_color_image_output(resolution, resolution)
self.depth_image = self.tiled_camera_sensor.utils.create_depth_image_output(resolution, resolution)
self.tiled_camera_sensor.sync_transforms(self.state)
# Warmup Kernels
self.tiled_camera_sensor.render_config.render_order = SensorTiledCamera.RenderOrder.TILED
self.tiled_camera_sensor.render_config.tile_width = 8
self.tiled_camera_sensor.render_config.tile_height = 8
for out_color, out_depth in [(True, True), (True, False), (False, True)]:
for _ in range(iterations):
self.tiled_camera_sensor.update(
None,
self.camera_transforms,
self.camera_rays,
color_image=self.color_image if out_color else None,
depth_image=self.depth_image if out_depth else None,
)
self.tiled_camera_sensor.render_config.render_order = SensorTiledCamera.RenderOrder.PIXEL_PRIORITY
for out_color, out_depth in [(True, True), (True, False), (False, True)]:
for _ in range(iterations):
self.tiled_camera_sensor.update(
None,
self.camera_transforms,
self.camera_rays,
color_image=self.color_image if out_color else None,
depth_image=self.depth_image if out_depth else None,
)
@nice_name("Rendering (Pixel)")
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_rendering_pixel_priority_color_depth(self, resolution: int, world_count: int, iterations: int):
self.tiled_camera_sensor.render_config.render_order = SensorTiledCamera.RenderOrder.PIXEL_PRIORITY
for _ in range(iterations):
self.tiled_camera_sensor.update(
None,
self.camera_transforms,
self.camera_rays,
color_image=self.color_image,
depth_image=self.depth_image,
refit_bvh=False,
)
wp.synchronize()
@nice_name("Rendering (Pixel) (Color Only)")
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_rendering_pixel_priority_color_only(self, resolution: int, world_count: int, iterations: int):
self.tiled_camera_sensor.render_config.render_order = SensorTiledCamera.RenderOrder.PIXEL_PRIORITY
for _ in range(iterations):
self.tiled_camera_sensor.update(
None,
self.camera_transforms,
self.camera_rays,
color_image=self.color_image,
refit_bvh=False,
)
wp.synchronize()
@nice_name("Rendering (Pixel) (Depth Only)")
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_rendering_pixel_priority_depth_only(self, resolution: int, world_count: int, iterations: int):
self.tiled_camera_sensor.render_config.render_order = SensorTiledCamera.RenderOrder.PIXEL_PRIORITY
for _ in range(iterations):
self.tiled_camera_sensor.update(
None,
self.camera_transforms,
self.camera_rays,
depth_image=self.depth_image,
refit_bvh=False,
)
wp.synchronize()
@nice_name("Rendering (Tiled)")
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_rendering_tiled_color_depth(self, resolution: int, world_count: int, iterations: int):
self.tiled_camera_sensor.render_config.render_order = SensorTiledCamera.RenderOrder.TILED
self.tiled_camera_sensor.render_config.tile_width = 8
self.tiled_camera_sensor.render_config.tile_height = 8
for _ in range(iterations):
self.tiled_camera_sensor.update(
None,
self.camera_transforms,
self.camera_rays,
color_image=self.color_image,
depth_image=self.depth_image,
refit_bvh=False,
)
wp.synchronize()
@nice_name("Rendering (Tiled) (Color Only)")
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_rendering_tiled_color_only(self, resolution: int, world_count: int, iterations: int):
self.tiled_camera_sensor.render_config.render_order = SensorTiledCamera.RenderOrder.TILED
self.tiled_camera_sensor.render_config.tile_width = 8
self.tiled_camera_sensor.render_config.tile_height = 8
for _ in range(iterations):
self.tiled_camera_sensor.update(
None,
self.camera_transforms,
self.camera_rays,
color_image=self.color_image,
refit_bvh=False,
)
wp.synchronize()
@nice_name("Rendering (Tiled) (Depth Only)")
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_rendering_tiled_depth_only(self, resolution: int, world_count: int, iterations: int):
self.tiled_camera_sensor.render_config.render_order = SensorTiledCamera.RenderOrder.TILED
self.tiled_camera_sensor.render_config.tile_width = 8
self.tiled_camera_sensor.render_config.tile_height = 8
for _ in range(iterations):
self.tiled_camera_sensor.update(
None,
self.camera_transforms,
self.camera_rays,
depth_image=self.depth_image,
refit_bvh=False,
)
wp.synchronize()
def print_fps(name: str, duration: float, resolution: int, world_count: int, iterations: int):
camera_count = 1
title = f"{name}"
if iterations > 1:
title += " average"
average = f"{duration * 1000.0 / iterations:.2f} ms"
fps = f"({(1.0 / (duration / iterations) * (world_count * camera_count)):,.2f} fps)"
print(f"{title} {'.' * (50 - len(title) - len(average))} {average} {fps if iterations > 1 else ''}")
def print_fps_results(results: dict[tuple[str, tuple[int, int, int]], float]):
print("")
print("=== Benchmark Results (FPS) ===")
for (method_name, params), avg in results.items():
print_fps(NICE_NAMES.get(method_name, method_name), avg, *params)
print("")
if __name__ == "__main__":
from newton.utils import run_benchmark
results = run_benchmark(SensorTiledCameraBenchmark)
print_fps_results(results)
================================================
FILE: asv/benchmarks/simulation/bench_viewer.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import os
import sys
# Force headless mode for CI environments before any pyglet imports
os.environ["PYGLET_HEADLESS"] = "1"
import warp as wp
wp.config.enable_backward = False
wp.config.quiet = True
from asv_runner.benchmarks.mark import skip_benchmark_if
parent_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
sys.path.append(parent_dir)
from benchmark_mujoco import Example
from newton.viewer import ViewerGL
class KpiViewerGL:
params = (["g1"], [8192])
param_names = ["robot", "world_count"]
rounds = 1
repeat = 3
number = 1
min_run_count = 1
def setup(self, robot, world_count):
wp.init()
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
self._model = builder.finalize()
self._state = self._model.state()
# Setting up the renderer
self.renderer = ViewerGL(headless=True)
self.renderer.set_model(self._model)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_rendering_frame(self, robot, world_count):
# Rendering one frame
self.renderer.begin_frame(0.0)
self.renderer.log_state(self._state)
self.renderer.end_frame()
wp.synchronize_device()
def teardown(self, robot, world_count):
self.renderer.close()
del self.renderer
del self._model
del self._state
class FastViewerGL:
params = (["g1"], [256])
param_names = ["robot", "world_count"]
rounds = 1
repeat = 3
number = 1
min_run_count = 1
def setup(self, robot, world_count):
wp.init()
builder = Example.create_model_builder(robot, world_count, randomize=True, seed=123)
# finalize model
self._model = builder.finalize()
self._state = self._model.state()
# Setting up the renderer
self.renderer = ViewerGL(headless=True)
self.renderer.set_model(self._model)
@skip_benchmark_if(wp.get_cuda_device_count() == 0)
def time_rendering_frame(self, robot, world_count):
# Rendering one frame
self.renderer.begin_frame(0.0)
self.renderer.log_state(self._state)
self.renderer.end_frame()
wp.synchronize_device()
def teardown(self, robot, world_count):
self.renderer.close()
del self.renderer
del self._model
del self._state
if __name__ == "__main__":
import argparse
from newton.utils import run_benchmark
benchmark_list = {
"KpiViewerGL": KpiViewerGL,
"FastViewerGL": FastViewerGL,
}
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
"-b", "--bench", default=None, action="append", choices=benchmark_list.keys(), help="Run a single benchmark."
)
args = parser.parse_known_args()[0]
if args.bench is None:
benchmarks = benchmark_list.keys()
else:
benchmarks = args.bench
for key in benchmarks:
benchmark = benchmark_list[key]
run_benchmark(benchmark)
================================================
FILE: asv.conf.json
================================================
{
"version": 1,
"project": "newton",
"project_url": "https://github.com/newton-physics/newton",
"repo": ".",
"branches": ["main"],
"dvcs": "git",
"environment_type": "virtualenv",
"show_commit_url": "https://github.com/newton-physics/newton/commit/",
"benchmark_dir": "asv/benchmarks",
"env_dir": "asv/env",
"results_dir": "asv/results",
"html_dir": "asv/html",
"build_cache_size": 20,
"default_benchmark_timeout": 600,
"build_command": ["python -m build --wheel -o {build_cache_dir} {build_dir}"],
"install_command": [
"python -m pip install -U numpy",
"python -m pip install -U warp-lang==1.12.0 --index-url=https://pypi.nvidia.com/",
"python -m pip install -U mujoco==3.6.0",
"python -m pip install -U mujoco-warp==3.6.0",
"python -m pip install -U torch==2.10.0+cu130 --index-url https://download.pytorch.org/whl/cu130",
"in-dir={env_dir} python -m pip install {wheel_file}[dev]"
]
}
================================================
FILE: docs/Makefile
================================================
# Minimal makefile for Sphinx documentation
#
# You can set these variables from the command line, and also
# from the environment for the first two.
SPHINXOPTS ?= -j auto
SPHINXBUILD ?= sphinx-build
SOURCEDIR = .
BUILDDIR = _build
# Put it first so that "make" without argument is like "make help".
help:
@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
.PHONY: help Makefile
# Catch-all target: route all unknown targets to Sphinx using the new
# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS).
%: Makefile
@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
================================================
FILE: docs/_ext/autodoc_filter.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import sys
from typing import Any
# NOTE: This file is *imported by Sphinx* when building the docs.
# It must therefore avoid heavy third-party imports that might not be
# available in the documentation environment.
# ---------------------------------------------------------------------------
# Skip handler implementation
def _should_skip_member(
app: Any, # Sphinx application (unused)
what: str,
name: str,
obj: Any,
skip: bool,
options: Any, # autodoc options (unused)
) -> bool | None:
"""Determine whether *obj* should be skipped by autodoc.
We apply two simple rules that make API pages cleaner:
1. Private helpers (names that start with an underscore but are not
special dunder methods) are hidden unless they are explicitly
marked as public via a ``:meta public:`` field.
2. Public members that have **no** docstring are hidden. This keeps the
generated documentation focused on the public, documented API.
Returning ``True`` tells Sphinx to skip the member, ``False`` to include
it, and ``None`` to fall back to Sphinx's default behaviour.
"""
# Respect decisions made by other handlers first.
if skip:
return True
# Keep dunder methods that are explicitly requested elsewhere.
if name.startswith("__") and name.endswith("__"):
return None # keep default behaviour
# Skip private helpers (single underscore) that are not explicitly public.
if name.startswith("_"):
# Let users override via :meta public:
doc = getattr(obj, "__doc__", "") or ""
if ":meta public:" not in doc:
return True
return None
# If the member is public but undocumented, decide based on its nature.
doc = getattr(obj, "__doc__", None)
if not doc:
# Keep an undocumented callable **only** if it overrides a documented
# attribute from a base-class. This covers cases like ``step`` in
# solver subclasses while still hiding brand-new helpers that have no
# documentation.
is_callable = callable(obj) or isinstance(
obj,
property | staticmethod | classmethod,
)
if is_callable and what == "class":
# Try to determine the parent class from the qualified name.
qualname = getattr(obj, "__qualname__", "")
parts = qualname.split(".")
if len(parts) >= 2:
cls_name = parts[-2]
module = sys.modules.get(obj.__module__)
parent_cls = getattr(module, cls_name, None)
if isinstance(parent_cls, type):
for base in parent_cls.__mro__[1:]:
if hasattr(base, name):
return None # overrides something -> keep it
# Otherwise hide the undocumented member to keep the page concise.
return True
# Default: do not override Sphinx's decision.
return None
def setup(app): # type: ignore[override]
"""Hook into the Sphinx build."""
app.connect("autodoc-skip-member", _should_skip_member)
# Tell Sphinx our extension is parallel-safe.
return {
"parallel_read_safe": True,
"parallel_write_safe": True,
}
================================================
FILE: docs/_ext/autodoc_wpfunc.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Sphinx extension to document Warp `@wp.func` functions.
This extension registers a custom *autodoc* documenter that recognises
`warp.types.Function` objects (created by the :pyfunc:`warp.func` decorator),
unwraps them to their original Python function (stored in the ``.func``
attribute) and then delegates all further processing to the standard
:class:`sphinx.ext.autodoc.FunctionDocumenter`.
With this in place, *autosummary* and *autodoc* treat Warp kernels exactly like
regular Python functions: the original signature and docstring are used and
`__all__` filtering works as expected.
"""
from __future__ import annotations
import inspect
from typing import Any
from sphinx.ext.autodoc import FunctionDocumenter
# NOTE: We do **not** import warp at module import time. Doing so would require
# CUDA and other heavy deps during the Sphinx build. Instead, detection of a
# Warp function is performed purely via *duck typing* (checking attributes and
# class name) so the extension is safe even when Warp cannot be imported.
class WarpFunctionDocumenter(FunctionDocumenter):
"""Autodoc documenter that unwraps :pyclass:`warp.types.Function`."""
objtype = "warpfunc"
directivetype = "function"
# Ensure we run *before* the builtin FunctionDocumenter (higher priority)
priority = FunctionDocumenter.priority + 10
# ---------------------------------------------------------------------
# Helper methods
# ---------------------------------------------------------------------
@staticmethod
def _looks_like_warp_function(obj: Any) -> bool:
"""Return *True* if *obj* appears to be a `warp.types.Function`."""
cls = obj.__class__
return getattr(cls, "__name__", "") == "Function" and hasattr(obj, "func")
@classmethod
def can_document_member(
cls,
member: Any,
member_name: str,
isattr: bool,
parent,
) -> bool:
"""Return *True* when *member* is a Warp function we can handle."""
return cls._looks_like_warp_function(member)
# ------------------------------------------------------------------
# Autodoc overrides - we proxy to the underlying Python function.
# ------------------------------------------------------------------
def _unwrap(self):
"""Return the original Python function or *self.object* as fallback."""
orig = getattr(self.object, "func", None)
if orig and inspect.isfunction(orig):
return orig
return self.object
# Each of these hooks replaces *self.object* with the unwrapped function
# *before* delegating to the base implementation.
def format_args(self):
self.object = self._unwrap()
return super().format_args()
def get_doc(self, *args: Any, **kwargs: Any) -> list[list[str]]:
self.object = self._unwrap()
return super().get_doc(*args, **kwargs)
def add_directive_header(self, sig: str) -> None:
self.object = self._unwrap()
super().add_directive_header(sig)
# ----------------------------------------------------------------------------
# Sphinx extension entry point
# ----------------------------------------------------------------------------
def setup(app): # type: ignore[override]
"""Register the :class:`WarpFunctionDocumenter` with *app*."""
app.add_autodocumenter(WarpFunctionDocumenter, override=True)
# Declare the extension safe for parallel reading/writing
return {
"parallel_read_safe": True,
"parallel_write_safe": True,
}
================================================
FILE: docs/_static/custom.css
================================================
/* Reduce spacing between class members */
.py-attribute, .py-method, .py-property, .py-class {
margin-top: 0.5em !important;
margin-bottom: 0.5em !important;
}
/* Reduce spacing in docstring sections */
.section > dl > dt {
margin-top: 0.2em !important;
margin-bottom: 0.2em !important;
}
/* Reduce padding in parameter lists */
.field-list .field {
margin-bottom: 0.2em !important;
padding-bottom: 0.2em !important;
}
/* Reduce spacing between headings and content */
h2, h3, h4, h5, h6 {
margin-top: 1em !important;
margin-bottom: 0.5em !important;
}
/* Reduce spacing in enum documentation */
.py.class .py.attribute {
margin-bottom: 0.5em !important;
}
/* Reduce spacing between enum values */
dl.py.attribute {
margin-bottom: 0.3em !important;
}
================================================
FILE: docs/_static/gh-pages-404.html
================================================
Page Not Found - Newton Documentation
Page Not Found
The page you requested does not exist.
Visit the Newton documentation .
================================================
FILE: docs/_static/switcher.json
================================================
[
{
"name": "dev",
"version": "dev",
"url": "https://newton-physics.github.io/newton/latest/"
}
]
================================================
FILE: docs/_templates/autosummary/class.rst
================================================
{{ fullname | escape | underline }}
.. autoclass:: {{ fullname }}
:members:
:undoc-members:
:show-inheritance:
:member-order: groupwise
================================================
FILE: docs/_templates/class.rst
================================================
{{ fullname | escape | underline}}
.. currentmodule:: {{ module }}
.. autoclass:: {{ objname }}
:members:
:inherited-members:
:member-order: groupwise
{% block methods %}
{% if methods %}
.. rubric:: {{ _('Methods') }}
.. autosummary::
{% for item in methods %}
~{{ name }}.{{ item }}
{%- endfor %}
{% endif %}
{% endblock %}
{% block attributes %}
{% if attributes %}
.. rubric:: {{ _('Attributes') }}
.. autosummary::
{% for item in attributes %}
~{{ name }}.{{ item }}
{%- endfor %}
{% endif %}
{% endblock %}
================================================
FILE: docs/_templates/genindex.html
================================================
{#
basic/genindex.html
~~~~~~~~~~~~~~~~~~~
Template for an "all indices" page.
:copyright: Copyright 2007-2023 by the Sphinx team, see AUTHORS.
:license: BSD, see LICENSE for details.
#}
{% extends "layout.html" %}
{% set title = _('Index') %}
{% block body %}
{{ _('Index') }}
{% for key, dummy in genindexentries %}
{{ key }} {% if not loop.last %}| {% endif %}
{% endfor %}
{%- for key, entries in genindexentries %}
{{ key }}
{%- for column in entries|slice_index(2) %}
{%- for entryname, (links, subitems, _) in column %}
{%- if links %}
{{ entryname }}
{%- for islast, link in links[1:] -%}
{%- if loop.first %}, {% endif -%}
{{ loop.index }}
{%- if not loop.last %}, {% endif -%}
{%- endfor %}
{%- else %}
{{ entryname }}
{%- endif %}
{%- if subitems %}
{%- for subentryname, subentrylinks in subitems %}
{{ subentryname }}
{%- for islast, link in subentrylinks[1:] -%}
{%- if loop.first %}, {% endif -%}
{{ loop.index }}
{%- if not loop.last %}, {% endif -%}
{%- endfor -%}
{%- endfor %}
{%- endif -%}
{%- endfor %}
{%- endfor %}
{% endfor %}
{% endblock %}
================================================
FILE: docs/_templates/py-modindex.html
================================================
{#
basic/modindex.html
~~~~~~~~~~~~~~~~~~~
Template for the module index.
:copyright: Copyright 2007-2023 by the Sphinx team, see AUTHORS.
:license: BSD, see LICENSE for details.
#}
{% extends "layout.html" %}
{% set title = _('Python Module Index') %}
{% block body %}
{{ _('Python Module Index') }}
{% if modulenames %}
{%- for letter in letters %}
{{ letter }} {% if not loop.last %} | {% endif %}
{%- endfor %}
{% endif %}
{% if modulenames %}
{%- for letter, entries in modindexentries %}
{{ letter }}
{%- for name, (uri, synopsis, platform, deprecated) in entries %}
{% if deprecated %}
{% endif %}
{%- if uri %}
{{ name }}
{%- else %}
{{ name }}
{%- endif %}
{% if deprecated %}
{% endif %}
{% if synopsis %} ({{ synopsis }}) {% endif %}
{% if platform %} ({{ platform }}) {% endif %}
{%- endfor %}
{%- endfor %}
{% else %}
{{ _('No modules found in project.') }}
{% endif %}
{% endblock %}
================================================
FILE: docs/_templates/search.html
================================================
{#
basic/search.html
~~~~~~~~~~~~~~~~
Template for the search page.
:copyright: Copyright 2007-2023 by the Sphinx team, see AUTHORS.
:license: BSD, see LICENSE for details.
#}
{% extends "layout.html" %}
{% set title = _('Search') %}
{% block scripts %}
{{ super() }}
{% endblock %}
{% block extrahead %}
{{ super() }}
{% endblock %}
{% block body %}
{{ _('Search') }}
{% block scriptwarning %}
{% trans %}Please activate JavaScript to enable the search functionality.{% endtrans %}
{% endblock %}
{% block searchresults %}
{% endblock %}
{% endblock %}
================================================
FILE: docs/_templates/sidebar-nav-bs.html
================================================
{# Displays the TOC-subtree for pages nested under the currently active top-level TOCtree element. #}
{{ _("Table of Contents") }}
{{- generate_toctree_html(
"sidebar",
startdepth=0,
show_nav_level=theme_show_nav_level | int,
maxdepth=theme_navigation_depth | int,
collapse=theme_collapse_navigation | tobool,
includehidden=theme_sidebar_includehidden | tobool,
titles_only=True
)
-}}
================================================
FILE: docs/api/newton.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton
======
.. py:module:: newton
.. currentmodule:: newton
.. toctree::
:hidden:
newton_geometry
newton_ik
newton_math
newton_selection
newton_sensors
newton_solvers
newton_usd
newton_utils
newton_viewer
.. rubric:: Submodules
- :doc:`newton.geometry `
- :doc:`newton.ik `
- :doc:`newton.math `
- :doc:`newton.selection `
- :doc:`newton.sensors `
- :doc:`newton.solvers `
- :doc:`newton.usd `
- :doc:`newton.utils `
- :doc:`newton.viewer `
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
Axis
BodyFlags
CollisionPipeline
Contacts
Control
EqType
Gaussian
GeoType
Heightfield
JointTargetMode
JointType
Mesh
Model
ModelBuilder
ParticleFlags
SDF
ShapeFlags
State
TetMesh
.. rubric:: Functions
.. autosummary::
:toctree: _generated
:signatures: long
AxisType
eval_fk
eval_ik
eval_jacobian
eval_mass_matrix
.. rubric:: Constants
.. list-table::
:header-rows: 1
* - Name
- Value
* - ``MAXVAL``
- ``10000000000.0``
* - ``__version__``
- ``1.1.0.dev0``
================================================
FILE: docs/api/newton_geometry.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.geometry
===============
.. py:module:: newton.geometry
.. currentmodule:: newton.geometry
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
BroadPhaseAllPairs
BroadPhaseExplicit
BroadPhaseSAP
HydroelasticSDF
NarrowPhase
.. rubric:: Functions
.. autosummary::
:toctree: _generated
:signatures: long
collide_box_box
collide_capsule_box
collide_capsule_capsule
collide_plane_box
collide_plane_capsule
collide_plane_cylinder
collide_plane_ellipsoid
collide_plane_sphere
collide_sphere_box
collide_sphere_capsule
collide_sphere_cylinder
collide_sphere_sphere
compute_inertia_shape
compute_offset_mesh
create_empty_sdf_data
sdf_box
sdf_capsule
sdf_cone
sdf_cylinder
sdf_mesh
sdf_plane
sdf_sphere
transform_inertia
================================================
FILE: docs/api/newton_ik.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.ik
=========
Public inverse-kinematics API for defining objectives and solving IK problems.
.. py:module:: newton.ik
.. currentmodule:: newton.ik
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
IKJacobianType
IKObjective
IKObjectiveJointLimit
IKObjectivePosition
IKObjectiveRotation
IKOptimizer
IKOptimizerLBFGS
IKOptimizerLM
IKSampler
IKSolver
================================================
FILE: docs/api/newton_math.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.math
===========
.. py:module:: newton.math
.. currentmodule:: newton.math
.. rubric:: Functions
.. autosummary::
:toctree: _generated
:signatures: long
boltzmann
leaky_max
leaky_min
normalize_with_norm
orthonormal_basis
quat_between_axes
quat_between_vectors_robust
quat_decompose
quat_velocity
safe_div
smooth_max
smooth_min
transform_twist
transform_wrench
vec_abs
vec_allclose
vec_inside_limits
vec_leaky_max
vec_leaky_min
vec_max
vec_min
velocity_at_point
================================================
FILE: docs/api/newton_selection.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.selection
================
.. py:module:: newton.selection
.. currentmodule:: newton.selection
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
ArticulationView
================================================
FILE: docs/api/newton_sensors.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.sensors
==============
.. py:module:: newton.sensors
.. currentmodule:: newton.sensors
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
SensorContact
SensorFrameTransform
SensorIMU
SensorRaycast
SensorTiledCamera
================================================
FILE: docs/api/newton_solvers.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.solvers
==============
Solvers are used to integrate the dynamics of a Newton model.
The typical workflow is to construct a :class:`~newton.Model` and a :class:`~newton.State` object, then use a solver to advance the state forward in time
via the :meth:`~newton.solvers.SolverBase.step` method:
.. mermaid::
:config: {"theme": "forest", "themeVariables": {"lineColor": "#76b900"}}
flowchart LR
subgraph Input["Input Data"]
M[newton.Model]
S[newton.State]
C[newton.Control]
K[newton.Contacts]
DT[Time step dt]
end
STEP["solver.step()"]
subgraph Output["Output Data"]
SO["newton.State (updated)"]
end
%% Connections
M --> STEP
S --> STEP
C --> STEP
K --> STEP
DT --> STEP
STEP --> SO
Supported Features
------------------
.. list-table::
:header-rows: 1
:widths: auto
:stub-columns: 0
* - Solver
- :abbr:`Integration (Available methods for integrating the dynamics)`
- Rigid bodies
- :ref:`Articulations `
- Particles
- Cloth
- Soft bodies
- Differentiable
* - :class:`~newton.solvers.SolverFeatherstone`
- Explicit
- ✅
- ✅ generalized coordinates
- ✅
- 🟨 no self-collision
- ✅
- 🟨 basic :sup:`2`
* - :class:`~newton.solvers.SolverImplicitMPM`
- Implicit
- ❌
- ❌
- ✅
- ❌
- ❌
- ❌
* - :class:`~newton.solvers.SolverKamino`
- Semi-implicit: Euler, Moreau-Jean
- ✅ maximal coordinates
- ✅ maximal coordinates
- ❌
- ❌
- ❌
- ❌
* - :class:`~newton.solvers.SolverMuJoCo`
- Explicit, Semi-implicit, Implicit
- ✅ :sup:`1`
- ✅ generalized coordinates
- ❌
- ❌
- ❌
- ❌
* - :class:`~newton.solvers.SolverSemiImplicit`
- Semi-implicit
- ✅
- ✅ maximal coordinates
- ✅
- 🟨 no self-collision
- ✅
- 🟨 basic :sup:`2`
* - :class:`~newton.solvers.SolverStyle3D`
- Implicit
- ❌
- ❌
- ✅
- ✅
- ❌
- ❌
* - :class:`~newton.solvers.SolverVBD`
- Implicit
- ✅
- 🟨 :ref:`limited joint support `
- ✅
- ✅
- ❌
- ❌
* - :class:`~newton.solvers.SolverXPBD`
- Implicit
- ✅
- ✅ maximal coordinates
- ✅
- 🟨 no self-collision
- 🟨 experimental
- ❌
| :sup:`1` Uses its own collision pipeline from MuJoCo/mujoco_warp by default,
unless ``use_mujoco_contacts`` is set to ``False``.
| :sup:`2` ``basic`` means Newton includes several examples that use these solvers in diffsim workflows,
see :ref:`Differentiability` for further details.
.. _Joint feature support:
Joint Feature Support
---------------------
Not every solver supports every joint type or joint property.
The tables below document which joint features each solver handles.
Only :class:`~newton.solvers.SolverFeatherstone` and :class:`~newton.solvers.SolverMuJoCo`
operate on :ref:`articulations ` (generalized/reduced coordinates).
The maximal-coordinate solvers (:class:`~newton.solvers.SolverSemiImplicit`,
:class:`~newton.solvers.SolverXPBD`, and :class:`~newton.solvers.SolverKamino`)
enforce joints as pairwise body constraints but do not use the articulation kinematic-tree structure.
:class:`~newton.solvers.SolverVBD` supports a subset of joint types via soft constraints (AVBD).
:class:`~newton.solvers.SolverStyle3D` and :class:`~newton.solvers.SolverImplicitMPM` do not support joints.
**Joint types**
.. list-table::
:header-rows: 1
:widths: auto
:stub-columns: 1
* - Joint type
- :class:`~newton.solvers.SolverFeatherstone`
- :class:`~newton.solvers.SolverSemiImplicit`
- :class:`~newton.solvers.SolverXPBD`
- :class:`~newton.solvers.SolverMuJoCo`
- :class:`~newton.solvers.SolverVBD`
- :class:`~newton.solvers.SolverKamino`
* - PRISMATIC
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
* - REVOLUTE
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
* - BALL
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
* - FIXED
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
* - FREE
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
* - DISTANCE
- 🟨 :sup:`1`
- 🟨 :sup:`1`
- |yes|
- |no|
- |no|
- |no|
* - D6
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
- |no|
* - CABLE
- |no|
- |no|
- |no|
- |no|
- |yes|
- |no|
| :sup:`1` DISTANCE joints are treated as FREE (no distance constraint enforcement).
**Joint properties**
.. list-table::
:header-rows: 1
:widths: auto
:stub-columns: 1
* - Property
- :class:`~newton.solvers.SolverFeatherstone`
- :class:`~newton.solvers.SolverSemiImplicit`
- :class:`~newton.solvers.SolverXPBD`
- :class:`~newton.solvers.SolverMuJoCo`
- :class:`~newton.solvers.SolverVBD`
- :class:`~newton.solvers.SolverKamino`
* - :attr:`~newton.Model.joint_enabled`
- |no|
- |yes|
- |yes|
- |no|
- |yes|
- |no|
* - :attr:`~newton.Model.joint_armature`
- |yes|
- |no|
- |no|
- |yes|
- |no|
- |yes|
* - :attr:`~newton.Model.joint_friction`
- |no|
- |no|
- |no|
- |yes|
- |no|
- |no|
* - :attr:`~newton.Model.joint_limit_lower` / :attr:`~newton.Model.joint_limit_upper`
- |yes|
- |yes| :sup:`2`
- |yes|
- |yes|
- |yes|
- |yes|
* - :attr:`~newton.Model.joint_limit_ke` / :attr:`~newton.Model.joint_limit_kd`
- |yes|
- |yes| :sup:`2`
- |no|
- |yes|
- |yes| :sup:`4`
- |no|
* - :attr:`~newton.Model.joint_effort_limit`
- |no|
- |no|
- |no|
- |yes|
- |no|
- |no|
* - :attr:`~newton.Model.joint_velocity_limit`
- |no|
- |no|
- |no|
- |no|
- |no|
- |no|
| :sup:`2` Not enforced for BALL joints in SemiImplicit.
**Actuation and control**
.. list-table::
:header-rows: 1
:widths: auto
:stub-columns: 1
* - Feature
- :class:`~newton.solvers.SolverFeatherstone`
- :class:`~newton.solvers.SolverSemiImplicit`
- :class:`~newton.solvers.SolverXPBD`
- :class:`~newton.solvers.SolverMuJoCo`
- :class:`~newton.solvers.SolverVBD`
- :class:`~newton.solvers.SolverKamino`
* - :attr:`~newton.Model.joint_target_ke` / :attr:`~newton.Model.joint_target_kd`
- |yes|
- |yes| :sup:`2`
- |yes|
- |yes|
- |yes| :sup:`4`
- |yes|
* - :attr:`~newton.Model.joint_target_mode`
- |no|
- |no|
- |no|
- |yes|
- |no|
- |yes|
* - :attr:`~newton.Control.joint_f` (feedforward forces)
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
- |yes|
**Constraints**
.. list-table::
:header-rows: 1
:widths: auto
:stub-columns: 1
* - Feature
- :class:`~newton.solvers.SolverFeatherstone`
- :class:`~newton.solvers.SolverSemiImplicit`
- :class:`~newton.solvers.SolverXPBD`
- :class:`~newton.solvers.SolverMuJoCo`
- :class:`~newton.solvers.SolverVBD`
- :class:`~newton.solvers.SolverKamino`
* - Equality constraints (CONNECT, WELD, JOINT)
- |no|
- |no|
- |no|
- |yes|
- |no|
- |no|
* - Mimic constraints
- |no|
- |no|
- |no|
- |yes| :sup:`3`
- |no|
- |no|
| :sup:`3` Mimic constraints in MuJoCo are supported for REVOLUTE and PRISMATIC joints only.
| :sup:`4` VBD interprets ``joint_target_kd`` and ``joint_limit_kd`` as dimensionless Rayleigh damping coefficients (``D = kd * ke``), not absolute units.
.. _Differentiability:
Differentiability
-----------------
Differentiable simulation in Newton typically runs a forward rollout inside
``wp.Tape()``, computes a scalar loss from the simulated state, and then calls
``tape.backward(loss)`` to populate gradients on differentiable state,
control, or model arrays. In practice, this starts by calling
:meth:`~newton.ModelBuilder.finalize` with ``requires_grad=True``.
.. testcode::
import warp as wp
import newton
@wp.kernel
def loss_kernel(particle_q: wp.array[wp.vec3], target: wp.vec3, loss: wp.array[float]):
delta = particle_q[0] - target
loss[0] = wp.dot(delta, delta)
builder = newton.ModelBuilder()
builder.add_particle(pos=wp.vec3(0.0, 0.0, 0.0), vel=wp.vec3(1.0, 0.0, 0.0), mass=1.0)
model = builder.finalize(requires_grad=True)
solver = newton.solvers.SolverSemiImplicit(model)
state_in = model.state()
state_out = model.state()
control = model.control()
loss = wp.zeros(1, dtype=float, requires_grad=True)
target = wp.vec3(0.25, 0.0, 0.0)
tape = wp.Tape()
with tape:
state_in.clear_forces()
solver.step(state_in, state_out, control, None, 1.0 / 60.0)
wp.launch(
loss_kernel,
dim=1,
inputs=[state_out.particle_q, target],
outputs=[loss],
)
tape.backward(loss)
initial_velocity_grad = state_in.particle_qd.grad.numpy()
assert float(initial_velocity_grad[0, 0]) < 0.0
See the `DiffSim examples on GitHub`_ for the current reference workflows.
.. _DiffSim examples on GitHub: https://github.com/newton-physics/newton/tree/main/newton/examples/diffsim
.. |yes| unicode:: U+2705
.. |no| unicode:: U+274C
.. py:module:: newton.solvers
.. currentmodule:: newton.solvers
.. toctree::
:hidden:
newton_solvers_style3d
.. rubric:: Submodules
- :doc:`newton.solvers.style3d `
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
SolverBase
SolverFeatherstone
SolverImplicitMPM
SolverKamino
SolverMuJoCo
SolverNotifyFlags
SolverSemiImplicit
SolverStyle3D
SolverVBD
SolverXPBD
================================================
FILE: docs/api/newton_solvers_style3d.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.solvers.style3d
======================
Style3D solver module.
This module provides helper functions for setting up Style3D cloth assets.
Use :class:`~newton.solvers.SolverStyle3D` as the canonical public solver
import path.
.. py:module:: newton.solvers.style3d
.. currentmodule:: newton.solvers.style3d
.. rubric:: Functions
.. autofunction:: add_cloth_grid
.. autofunction:: add_cloth_mesh
================================================
FILE: docs/api/newton_usd.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.usd
==========
Utilities for working with the Universal Scene Description (USD) format.
This module provides both low-level USD utility helpers and public schema
resolver types used by :meth:`newton.ModelBuilder.add_usd`.
.. py:module:: newton.usd
.. currentmodule:: newton.usd
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
PrimType
SchemaResolver
SchemaResolverMjc
SchemaResolverNewton
SchemaResolverPhysx
.. rubric:: Functions
.. autosummary::
:toctree: _generated
:signatures: long
find_tetmesh_prims
get_attribute
get_attributes_in_namespace
get_custom_attribute_declarations
get_custom_attribute_values
get_float
get_gprim_axis
get_mesh
get_quat
get_scale
get_tetmesh
get_transform
has_applied_api_schema
has_attribute
type_to_warp
value_to_warp
================================================
FILE: docs/api/newton_utils.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.utils
============
.. py:module:: newton.utils
.. currentmodule:: newton.utils
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
EventTracer
MeshAdjacency
.. rubric:: Functions
.. autosummary::
:toctree: _generated
:signatures: long
bourke_color_map
color_graph
compute_world_offsets
create_cable_stiffness_from_elastic_moduli
create_parallel_transport_cable_quaternions
create_straight_cable_points
create_straight_cable_points_and_quaternions
download_asset
event_scope
load_texture
normalize_texture
plot_graph
remesh_mesh
run_benchmark
solidify_mesh
string_to_warp
================================================
FILE: docs/api/newton_viewer.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
newton.viewer
=============
.. py:module:: newton.viewer
.. currentmodule:: newton.viewer
.. rubric:: Classes
.. autosummary::
:toctree: _generated
:nosignatures:
ViewerBase
ViewerFile
ViewerGL
ViewerNull
ViewerRerun
ViewerUSD
ViewerViser
================================================
FILE: docs/concepts/articulations.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. currentmodule:: newton
.. _Articulations:
Articulations
=============
Articulations are a way to represent a collection of rigid bodies that are connected by joints.
.. _Articulation parameterization:
Generalized and maximal coordinates
-----------------------------------
There are two types of parameterizations to describe the configuration of an articulation:
generalized coordinates and maximal coordinates.
Generalized (sometimes also called "reduced") coordinates describe an articulation in terms of its joint positions and velocities.
For example, a double-pendulum articulation has two revolute joints, so its generalized state consists of two joint angles in :attr:`newton.State.joint_q` and two corresponding joint velocities in :attr:`newton.State.joint_qd`.
See the table below for the number of generalized coordinates for each joint type.
For a floating-base articulation (one connected to the world by a free joint), the generalized coordinates also include the base link pose: a 3D position and an XYZW quaternion.
Maximal coordinates describe the configuration of an articulation in terms of the body link positions and velocities.
Each rigid body's pose is represented by 7 parameters (3D position and XYZW quaternion) in :attr:`newton.State.body_q`,
and its velocity by 6 parameters (3D linear and 3D angular) in :attr:`newton.State.body_qd`.
To convert between these two representations, we use forward and inverse kinematics:
forward kinematics (:func:`newton.eval_fk`) converts generalized coordinates to maximal coordinates, and inverse kinematics (:func:`newton.eval_ik`) converts maximal coordinates to generalized coordinates.
Newton supports both parameterizations, and each solver chooses which one it treats as the primary articulation state representation.
For example, :class:`~newton.solvers.SolverMuJoCo` and :class:`~newton.solvers.SolverFeatherstone`
use generalized coordinates, while :class:`~newton.solvers.SolverXPBD`,
:class:`~newton.solvers.SolverSemiImplicit`, and :class:`~newton.solvers.SolverVBD`
use maximal coordinates.
Note that collision detection, e.g., via :meth:`newton.Model.collide` requires the maximal coordinates to be current in the state.
To showcase how an articulation state is initialized using reduced coordinates, let's consider an example where we create an articulation with a single revolute joint and initialize
its joint angle to 0.5 and joint velocity to 10.0:
.. testcode::
builder = newton.ModelBuilder()
# create an articulation with a single revolute joint
body = builder.add_link()
builder.add_shape_box(body) # add a shape to the body to add some inertia
joint = builder.add_joint_revolute(parent=-1, child=body, axis=wp.vec3(0.0, 0.0, 1.0)) # add a revolute joint to the body
builder.add_articulation([joint]) # create articulation from the joint
builder.joint_q[-1] = 0.5
builder.joint_qd[-1] = 10.0
model = builder.finalize()
state = model.state()
# The generalized coordinates have been initialized by the revolute joint:
assert all(state.joint_q.numpy() == [0.5])
assert all(state.joint_qd.numpy() == [10.0])
While the generalized coordinates have been initialized by the values we set through the :attr:`newton.ModelBuilder.joint_q` and :attr:`newton.ModelBuilder.joint_qd` definitions,
the body poses (maximal coordinates) are still initialized by the identity transform (since we did not provide a ``xform`` argument to the :meth:`newton.ModelBuilder.add_link` call, it defaults to the identity transform).
This is not a problem for generalized-coordinate solvers, as they do not use the body poses (maximal coordinates) to represent the state of the articulation but only the generalized coordinates.
In order to update the body poses (maximal coordinates), we need to use the forward kinematics function :func:`newton.eval_fk`:
.. testcode::
newton.eval_fk(model, state.joint_q, state.joint_qd, state)
Now, the body poses (maximal coordinates) have been updated by the forward kinematics and a maximal-coordinate solver can simulate the scene starting from these initial conditions.
As mentioned above, this call is not needed for generalized-coordinate solvers.
When declaring an articulation using the :class:`~newton.ModelBuilder`, the rigid body poses (maximal coordinates :attr:`newton.State.body_q`) are initialized by the ``xform`` argument:
.. testcode::
builder = newton.ModelBuilder()
tf = wp.transform(wp.vec3(1.0, 2.0, 3.0), wp.quat_from_axis_angle(wp.vec3(0.0, 0.0, 1.0), 0.5 * wp.pi))
body = builder.add_body(xform=tf)
builder.add_shape_box(body) # add a shape to the body to add some inertia
model = builder.finalize()
state = model.state()
# The body poses (maximal coordinates) are initialized by the xform argument:
assert all(state.body_q.numpy()[0] == [*tf])
# Note: add_body() automatically creates a free joint, so generalized coordinates exist:
assert len(state.joint_q) == 7 # 7 DOF for a free joint (3 position + 4 quaternion)
In this setup, we have a body with a box shape that both maximal-coordinate and generalized-coordinate solvers can simulate.
Since :meth:`~newton.ModelBuilder.add_body` automatically adds a free joint, the body already has the necessary degrees of freedom in generalized coordinates (:attr:`newton.State.joint_q`).
.. testcode::
builder = newton.ModelBuilder()
tf = wp.transform(wp.vec3(1.0, 2.0, 3.0), wp.quat_from_axis_angle(wp.vec3(0.0, 0.0, 1.0), 0.5 * wp.pi))
body = builder.add_link(xform=tf)
builder.add_shape_box(body) # add a shape to the body to add some inertia
joint = builder.add_joint_free(body) # add a free joint to connect the body to the world
builder.add_articulation([joint]) # create articulation from the joint
# The free joint's coordinates (joint_q) are initialized by its child body's pose,
# so we do not need to specify them here
# builder.joint_q[-7:] = *tf
model = builder.finalize()
state = model.state()
# The body poses (maximal coordinates) are initialized by the xform argument:
assert all(state.body_q.numpy()[0] == [*tf])
# Now, the generalized coordinates are initialized by the free joint:
assert len(state.joint_q) == 7
assert all(state.joint_q.numpy() == [*tf])
This scene can now be simulated by both maximal-coordinate and generalized-coordinate solvers.
.. _Kinematic links:
Kinematic links and bodies
--------------------------
Newton distinguishes three motion modes for rigid bodies:
**Static**
Does not move. Typical examples are world-attached shapes or links attached to world with a fixed joint.
**Kinematic**
Moves only from user-prescribed state updates. It can have joint DOFs (free, revolute, etc.), but external forces do not accelerate it.
**Dynamic**
Moves from forces, constraints, and contacts during solver integration.
Kinematic bodies are created through the ``is_kinematic=True`` flag on :meth:`~newton.ModelBuilder.add_link`
or :meth:`~newton.ModelBuilder.add_body`. Only root links (joint parent ``-1``) may be kinematic.
Setting a non-root link to kinematic raises a :class:`ValueError` during articulation construction.
Common combinations
^^^^^^^^^^^^^^^^^^^
The following patterns are valid and commonly used:
1. **Kinematic free-base body**: ``add_body(is_kinematic=True)`` (free joint root).
2. **Kinematic articulated root**: root link is kinematic and attached to world with a non-fixed joint
(for example revolute), with dynamic descendants.
3. **Static fixed-root body**: root link is kinematic and attached to world with a fixed joint.
This has zero joint DOFs and behaves as static.
.. testcode:: articulation-kinematic-combinations
builder = newton.ModelBuilder()
# 1) Kinematic free-base body (add_body creates free joint + articulation)
kinematic_free = builder.add_body(is_kinematic=True, mass=1.0)
# 2) Kinematic revolute root with a dynamic child
root = builder.add_link(is_kinematic=True, mass=1.0)
child = builder.add_link(mass=1.0)
j_root = builder.add_joint_revolute(parent=-1, child=root, axis=newton.Axis.Z)
j_child = builder.add_joint_revolute(parent=root, child=child, axis=newton.Axis.Z)
builder.add_articulation([j_root, j_child])
# 3) Static fixed-root body (zero joint DOFs)
static_root = builder.add_link(is_kinematic=True, mass=1.0)
j_static = builder.add_joint_fixed(parent=-1, child=static_root)
builder.add_articulation([j_static])
model = builder.finalize()
.. list-table:: Static vs kinematic vs dynamic bodies/links
:header-rows: 1
:widths: 22 26 26 26
* - Property
- Static
- Kinematic
- Dynamic
* - Typical definition
- World-attached shape, or root link fixed to world
- ``is_kinematic=True`` on a root link/body with free/revolute/etc. joint
- Default link/body (no kinematic flag)
* - Joint DOFs
- 0 for fixed-root links
- Joint-dependent (free/revolute/D6/etc.)
- Joint-dependent
* - Position/velocity state
- Constant (not integrated)
- User-prescribed ``q``/``qd`` (or ``body_q``/``body_qd`` depending on solver coordinates)
- Integrated by solver from dynamics
* - Response to applied force/torque
- No acceleration
- No acceleration (force-immune for own motion)
- Accelerates according to dynamics
* - Collision/contact participation
- Yes (acts as obstacle/support)
- Yes (can push dynamic bodies while following prescribed motion)
- Yes
* - Mass/inertia (see :ref:`Mass and Inertia`)
- Not used for motion when fixed
- Preserved for body properties and future dynamic switching
- Fully used by dynamics
* - Mass matrix / constraint role
- No active DOFs when fixed to world
- Solver-dependent infinite-mass approximation along kinematic DOFs
- Standard articulated mass matrix
* - Typical applications
- Environment geometry, fixtures
- Conveyors, robot bases on trajectories, scripted mechanism roots
- Physically simulated robots and objects
Velocity consistency for prescribed motion
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
For prescribed motion, it is up to the user to keep position and velocity updates consistent across time.
In particular, ``qd`` should be consistent with the finite-differenced motion implied by ``q``.
For scalar coordinates, this is the familiar ``q_next = q + qd * dt`` relation; quaternion-based coordinates
(for example FREE/BALL joint) require manifold-consistent quaternion integration instead of direct addition.
When writing kinematic state values:
- For generalized-coordinate workflows, write :attr:`newton.State.joint_q` and :attr:`newton.State.joint_qd`,
then call :func:`newton.eval_fk` so maximal coordinates (for collisions and body-space consumers) are current.
- For maximal-coordinate workflows, write :attr:`newton.State.body_q` and :attr:`newton.State.body_qd` directly.
Rigid-body solver behavior
^^^^^^^^^^^^^^^^^^^^^^^^^^
The rigid-body solvers (:class:`~newton.solvers.SolverMuJoCo`,
:class:`~newton.solvers.SolverFeatherstone`, :class:`~newton.solvers.SolverXPBD`,
:class:`~newton.solvers.SolverSemiImplicit`, :class:`~newton.solvers.SolverVBD`)
support the same user-facing kinematic authoring model:
- Kinematic links keep their declared joint type (free/revolute/etc.).
- A kinematic root attached to world by a fixed joint remains fixed (zero DOFs).
- Kinematic links participate in collisions/contacts and can impart motion to dynamic bodies.
- Applied forces do not drive kinematic motion; motion is user-prescribed.
Implementation details differ by coordinate formulation:
- Generalized-coordinate solvers (:class:`~newton.solvers.SolverMuJoCo`,
:class:`~newton.solvers.SolverFeatherstone`) treat kinematic motion through prescribed joint state.
- Maximal-coordinate solvers (:class:`~newton.solvers.SolverXPBD`,
:class:`~newton.solvers.SolverSemiImplicit`, :class:`~newton.solvers.SolverVBD`)
use prescribed body transforms/twists.
- Contact handling of kinematic bodies is not identical across the solvers. :class:`~newton.solvers.SolverXPBD`,
:class:`~newton.solvers.SolverVBD`, :class:`~newton.solvers.SolverMuJoCo`, and
:class:`~newton.solvers.SolverFeatherstone` treat kinematic bodies like
infinite-mass colliders for contact response, while
:class:`~newton.solvers.SolverSemiImplicit` currently preserves prescribed state but
does not zero inverse mass/inertia inside its contact solver. Contacts against
kinematic bodies can therefore be softer under SemiImplicit.
In :class:`~newton.solvers.SolverMuJoCo`, kinematic DOFs are regularized with a
large internal armature value; see :ref:`Kinematic Links and Fixed Roots ` for details.
.. _Joint types:
Joint types
-----------
.. list-table::
:header-rows: 1
:widths: auto
:stub-columns: 0
* - Joint Type
- Description
- Coordinates in ``joint_q``
- DOFs in ``joint_qd``
* - ``JointType.PRISMATIC``
- Prismatic (slider) joint with 1 linear degree of freedom
- 1
- 1
* - ``JointType.REVOLUTE``
- Revolute (hinge) joint with 1 angular degree of freedom
- 1
- 1
* - ``JointType.BALL``
- Ball (spherical) joint with quaternion state representation
- 4
- 3
* - ``JointType.FIXED``
- Fixed (static) joint with no degrees of freedom
- 0
- 0
* - ``JointType.FREE``
- Free (floating) joint with 6 degrees of freedom in velocity space
- 7 (3D position + 4D quaternion)
- 6 (see :ref:`Twist conventions in Newton `)
* - ``JointType.DISTANCE``
- Distance joint that keeps two bodies at a distance within its joint limits
- 7
- 6
* - ``JointType.D6``
- Generic D6 joint with up to 3 translational and 3 rotational degrees of freedom
- up to 6
- up to 6
* - ``JointType.CABLE``
- Cable joint with 1 linear (stretch/shear) and 1 angular (bend/twist) degree of freedom
- 2
- 2
D6 joints are the most general joint type in Newton and can be used to represent any combination of translational and rotational degrees of freedom.
Prismatic, revolute, planar, and universal joints can be seen as special cases of the D6 joint.
Definition of ``joint_q``
^^^^^^^^^^^^^^^^^^^^^^^^^
The :attr:`newton.Model.joint_q` array stores the default generalized joint positions
for all joints in the model and is used to initialize :attr:`newton.State.joint_q`.
Both arrays share the same per-joint layout.
For scalar-coordinate joints (for example this D6 joint), the positional coordinates can be queried as follows:
.. testsetup:: articulation-joint-layout
builder = newton.ModelBuilder()
body = builder.add_link()
builder.add_shape_box(body, hx=0.1, hy=0.1, hz=0.1)
joint = builder.add_joint_d6(
parent=-1,
child=body,
linear_axes=[newton.ModelBuilder.JointDofConfig(axis=newton.Axis.X, limit_lower=-0.5, limit_upper=0.5)],
angular_axes=[newton.ModelBuilder.JointDofConfig(axis=newton.Axis.Z, limit_lower=-1.0, limit_upper=1.0)],
)
builder.add_articulation([joint])
model = builder.finalize()
state = model.state()
control = model.control()
joint_id = 0
joint_q_start = model.joint_q_start.numpy()
joint_qd_start = model.joint_qd_start.numpy()
joint_q = state.joint_q.numpy()
joint_qd = state.joint_qd.numpy()
joint_dof_dim = model.joint_dof_dim.numpy()
joint_axis = model.joint_axis.numpy()
joint_limit_lower = model.joint_limit_lower.numpy()
joint_target_pos = control.joint_target_pos.numpy()
joint_f = control.joint_f.numpy()
.. testcode:: articulation-joint-layout
q_start = joint_q_start[joint_id]
coord_count = joint_dof_dim[joint_id, 0] + joint_dof_dim[joint_id, 1]
# now the positional coordinates can be queried as follows:
q = joint_q[q_start : q_start + coord_count]
q0 = q[0]
q1 = q[1]
Definition of ``joint_qd``
^^^^^^^^^^^^^^^^^^^^^^^^^^
The :attr:`newton.Model.joint_qd` array stores the default generalized joint velocities
for all joints in the model and is used to initialize :attr:`newton.State.joint_qd`.
The generalized joint forces at :attr:`newton.Control.joint_f` use the same DOF order.
Several other arrays also use this same DOF-ordered layout, indexed from
:attr:`newton.Model.joint_qd_start` rather than :attr:`newton.Model.joint_q_start`.
This includes :attr:`newton.Model.joint_axis`, joint limits and other per-DOF
properties defined via :class:`newton.ModelBuilder.JointDofConfig`, and the
position targets at :attr:`newton.Control.joint_target_pos`.
For every joint, these per-DOF arrays are stored consecutively, with linear DOFs
first and angular DOFs second. Use :attr:`newton.Model.joint_dof_dim` to query
how many of each a joint has.
The velocity DOFs for each joint can be queried as follows:
.. testcode:: articulation-joint-layout
qd_start = joint_qd_start[joint_id]
dof_count = joint_dof_dim[joint_id, 0] + joint_dof_dim[joint_id, 1]
# now the velocity DOFs can be queried as follows:
qd = joint_qd[qd_start : qd_start + dof_count]
qd0 = qd[0]
qd1 = qd[1]
# the generalized joint forces can be queried as follows:
f = joint_f[qd_start : qd_start + dof_count]
f0 = f[0]
f1 = f[1]
The same start index can be used to query other per-DOF arrays for that joint:
.. testcode:: articulation-joint-layout
num_linear_dofs = joint_dof_dim[joint_id, 0]
num_angular_dofs = joint_dof_dim[joint_id, 1]
# all per-DOF arrays for this joint start at this index:
dof_start = joint_qd_start[joint_id]
# the axis vector for the first linear DOF
first_lin_axis = joint_axis[dof_start]
# the position target for this linear DOF
first_lin_target = joint_target_pos[dof_start]
# the joint limit of this linear DOF
first_lin_limit = joint_limit_lower[dof_start]
# the axis vector for the first angular DOF comes after all linear DOFs
first_ang_axis = joint_axis[dof_start + num_linear_dofs]
# the position target for this angular DOF
first_ang_target = joint_target_pos[dof_start + num_linear_dofs]
# the joint limit of this angular DOF
first_ang_limit = joint_limit_lower[dof_start + num_linear_dofs]
assert (num_linear_dofs, num_angular_dofs) == (1, 1)
assert np.allclose(first_lin_axis, [1.0, 0.0, 0.0])
assert np.allclose(first_ang_axis, [0.0, 0.0, 1.0])
assert np.allclose([first_lin_limit, first_ang_limit], [-0.5, -1.0])
Common articulation workflows
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Center ``joint_q`` between joint limits with Warp kernels
"""""""""""""""""""""""""""""""""""""""""""""""""""""""""
Joint limits are stored in DOF order (``joint_qd`` layout), while ``joint_q`` stores generalized
joint coordinates (which may include quaternion coordinates for free/ball joints).
The pattern below sets each scalar coordinate to the midpoint between its lower and upper limits.
A robust pattern is:
1. Loop over joints.
2. Use ``Model.joint_qd_start`` to find the first DOF index for each joint.
3. Use ``Model.joint_dof_dim`` to get the number of linear and angular DOFs for that joint.
4. Use ``Model.joint_q_start`` to find where that joint starts in ``State.joint_q``.
5. Center only scalar coordinates (for example, revolute/prismatic axes) and skip quaternion joints.
.. testsetup:: articulation-center-joint-q
builder = newton.ModelBuilder()
parent = builder.add_link()
child = builder.add_link(xform=wp.transform(wp.vec3(1.0, 0.0, 0.0), wp.quat_identity()))
builder.add_shape_box(parent, hx=0.1, hy=0.1, hz=0.1)
builder.add_shape_box(child, hx=0.1, hy=0.1, hz=0.1)
j0 = builder.add_joint_revolute(
parent=-1,
child=parent,
axis=wp.vec3(0.0, 0.0, 1.0),
limit_lower=-1.0,
limit_upper=1.0,
)
j1 = builder.add_joint_revolute(
parent=parent,
child=child,
axis=wp.vec3(0.0, 0.0, 1.0),
parent_xform=wp.transform(wp.vec3(1.0, 0.0, 0.0), wp.quat_identity()),
child_xform=wp.transform_identity(),
limit_lower=0.0,
limit_upper=2.0,
)
builder.add_articulation([j0, j1])
model = builder.finalize()
state = model.state()
.. testcode:: articulation-center-joint-q
@wp.kernel
def center_joint_q_from_limits(
joint_q_start: wp.array(dtype=wp.int32),
joint_qd_start: wp.array(dtype=wp.int32),
joint_dof_dim: wp.array2d(dtype=wp.int32),
joint_type: wp.array(dtype=wp.int32),
joint_limit_lower: wp.array(dtype=float),
joint_limit_upper: wp.array(dtype=float),
joint_q: wp.array(dtype=float),
):
joint_id = wp.tid()
# First DOF index for this joint in qd-order arrays (limits/axes/forces)
qd_begin = joint_qd_start[joint_id]
dof_count = joint_dof_dim[joint_id, 0] + joint_dof_dim[joint_id, 1]
# Start index for this joint in generalized coordinates q
q_begin = joint_q_start[joint_id]
# Skip free/ball joints because their q entries include quaternion coordinates.
jt = joint_type[joint_id]
if (
jt == newton.JointType.FREE
or jt == newton.JointType.BALL
or jt == newton.JointType.DISTANCE
):
return
# For scalar joints, q coordinates align with this joint's total DOF count.
for local_dof in range(dof_count):
qd_idx = qd_begin + local_dof
q_idx = q_begin + local_dof
lower = joint_limit_lower[qd_idx]
upper = joint_limit_upper[qd_idx]
if wp.isfinite(lower) and wp.isfinite(upper):
joint_q[q_idx] = 0.5 * (lower + upper)
# Launch over all joints in the model
wp.launch(
kernel=center_joint_q_from_limits,
dim=model.joint_count,
inputs=[
model.joint_q_start,
model.joint_qd_start,
model.joint_dof_dim,
model.joint_type,
model.joint_limit_lower,
model.joint_limit_upper,
state.joint_q,
],
)
# Recompute transforms after editing generalized coordinates
newton.eval_fk(model, state.joint_q, state.joint_qd, state)
ArticulationView: selection interface for RL and batched control
""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
:class:`newton.selection.ArticulationView` is the high-level interface for selecting a subset
of articulations and accessing their joints/links/DOFs with stable tensor shapes. This is
especially useful in RL pipelines where the same observation/action logic is applied to many
parallel environments.
Construct a view by matching articulation keys with a pattern and optional filters:
.. testsetup:: articulation-view
builder = newton.ModelBuilder()
for i in range(2):
root = builder.add_link(
xform=wp.transform(wp.vec3(float(i) * 2.0, 0.0, 0.0), wp.quat_identity())
)
tip = builder.add_link(
xform=wp.transform(wp.vec3(float(i) * 2.0 + 1.0, 0.0, 0.0), wp.quat_identity())
)
builder.add_shape_box(root, hx=0.1, hy=0.1, hz=0.1)
builder.add_shape_box(tip, hx=0.1, hy=0.1, hz=0.1)
j_root = builder.add_joint_free(parent=-1, child=root)
j_tip = builder.add_joint_revolute(
parent=root,
child=tip,
axis=wp.vec3(0.0, 0.0, 1.0),
parent_xform=wp.transform(wp.vec3(1.0, 0.0, 0.0), wp.quat_identity()),
child_xform=wp.transform_identity(),
)
builder.add_articulation([j_root, j_tip], label=f"robot_{i}")
model = builder.finalize()
state = model.state()
.. testcode:: articulation-view
# select all articulations whose key starts with "robot"
view = newton.selection.ArticulationView(model, pattern="robot*")
assert view.count == 2
# select only scalar-joint articulations (exclude quaternion-root joint types)
scalar_view = newton.selection.ArticulationView(
model,
pattern="robot*",
include_joint_types=[newton.JointType.PRISMATIC, newton.JointType.REVOLUTE],
exclude_joint_types=[newton.JointType.FREE, newton.JointType.BALL],
)
assert scalar_view.get_dof_positions(state).shape == (1, 2, 1)
Use views to read/write batched state slices (joint positions/velocities, root transforms,
link transforms) without manual index bookkeeping.
Move articulations in world space
"""""""""""""""""""""""""""""""""
Use :meth:`newton.selection.ArticulationView.set_root_transforms` to move selected articulations:
.. testcode:: articulation-view
view = newton.selection.ArticulationView(model, pattern="robot*")
root_tf = view.get_root_transforms(state).numpy()
# shift +0.2 m along world x for all selected articulations
root_tf[..., 0] += 0.2
view.set_root_transforms(state, root_tf)
# recompute link transforms from generalized coordinates
newton.eval_fk(model, state.joint_q, state.joint_qd, state)
assert np.allclose(view.get_root_transforms(state).numpy()[0, :, 0], [0.2, 2.2])
For floating-base articulations (root joint type ``FREE`` or ``DISTANCE``), this updates
the root coordinates in ``joint_q``.
For non-floating-base articulations (for example ``FIXED`` or a world-attached
``REVOLUTE`` root), ``set_root_transforms()`` moves the articulation by writing
``Model.joint_X_p`` because there is no root pose stored in state coordinates.
Use ``ArticulationView`` to inspect and modify selected articulations
"""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""
``ArticulationView`` provides stable, per-articulation access to links, joints, DOFs, and attributes:
.. testcode:: articulation-view
view = newton.selection.ArticulationView(model, pattern="robot*")
scalar_view = newton.selection.ArticulationView(
model,
pattern="robot*",
include_joint_types=[newton.JointType.PRISMATIC, newton.JointType.REVOLUTE],
exclude_joint_types=[newton.JointType.FREE, newton.JointType.BALL],
)
# inspect
q = scalar_view.get_dof_positions(state) # shape [world_count, articulation_count, dof_count]
qd = scalar_view.get_dof_velocities(state) # shape [world_count, articulation_count, dof_count]
link_q = view.get_link_transforms(state) # shape [world_count, articulation_count, link_count]
assert q.shape == (1, 2, 1)
assert qd.shape == (1, 2, 1)
assert link_q.shape == (1, 2, 2)
# edit selected articulation values in-place
q_np = q.numpy()
q_np[..., 0] = 0.0
scalar_view.set_dof_positions(state, q_np)
assert np.allclose(scalar_view.get_dof_positions(state).numpy()[0, :, 0], 0.0)
# if model attributes are edited through the view, notify the solver afterwards
# solver.notify_model_changed()
.. _FK-IK:
Forward / Inverse Kinematics
----------------------------
Articulated rigid-body mechanisms are kinematically described by the joints that connect the bodies as well as the
relative transform from the parent and child body to the respective anchor frames of the joint in the parent and child body:
.. image:: /_static/joint_transforms.png
:width: 400
:align: center
.. list-table:: Variable names in the articulation kernels
:widths: 10 90
:header-rows: 1
* - Symbol
- Description
* - x_wp
- World transform of the parent body (stored at :attr:`State.body_q`)
* - x_wc
- World transform of the child body (stored at :attr:`State.body_q`)
* - x_pj
- Transform from the parent body to the joint parent anchor frame (defined by :attr:`Model.joint_X_p`)
* - x_cj
- Transform from the child body to the joint child anchor frame (defined by :attr:`Model.joint_X_c`)
* - x_j
- Joint transform from the joint parent anchor frame to the joint child anchor frame
In the forward kinematics, the joint transform is determined by the joint coordinates (generalized joint positions :attr:`State.joint_q` and velocities :attr:`State.joint_qd`).
Given the parent body's world transform :math:`x_{wp}` and the joint transform :math:`x_{j}`, the child body's world transform :math:`x_{wc}` is computed as:
.. math::
x_{wc} = x_{wp} \cdot x_{pj} \cdot x_{j} \cdot x_{cj}^{-1}.
.. autofunction:: newton.eval_fk
:noindex:
.. autofunction:: newton.eval_ik
:noindex:
.. _Orphan joints:
Orphan joints
-------------
An **orphan joint** is a joint that is not part of any articulation. This situation can arise when:
* The USD asset does not define a ``PhysicsArticulationRootAPI`` on any prim, so no articulations are discovered during parsing.
* A joint connects two bodies that are not under any ``PhysicsArticulationRootAPI`` prim, even though other articulations exist in the scene.
When orphan joints are detected during USD parsing (:meth:`~newton.ModelBuilder.add_usd`), Newton issues a warning that lists the affected joint paths.
**Validation and finalization**
By default, :meth:`~newton.ModelBuilder.finalize` validates that every joint belongs to an articulation and raises a :class:`ValueError` if orphan joints are found.
To proceed with orphan joints, skip this validation:
.. testsetup:: articulation-orphan-joints
builder = newton.ModelBuilder()
body = builder.add_link()
builder.add_shape_box(body, hx=0.1, hy=0.1, hz=0.1)
builder.add_joint_revolute(parent=-1, child=body, axis=newton.Axis.Z)
.. testcode:: articulation-orphan-joints
model = builder.finalize(skip_validation_joints=True)
**Solver compatibility**
Only maximal-coordinate solvers (:class:`~newton.solvers.SolverXPBD`, :class:`~newton.solvers.SolverSemiImplicit`) support orphan joints.
Generalized-coordinate solvers (:class:`~newton.solvers.SolverFeatherstone`, :class:`~newton.solvers.SolverMuJoCo`) require every joint to belong to an articulation.
================================================
FILE: docs/concepts/collisions.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. currentmodule:: newton
.. _Collisions:
Collisions
==========
Newton provides a GPU-accelerated collision detection system with:
- **Full shape-pair coverage** — every shape type collides with every other shape type
(see :ref:`Shape Compatibility`).
- **Mesh-mesh contacts** via precomputed SDFs for O(1) distance queries on complex
geometry.
- **Hydroelastic contacts** that sample contacts across the contact surface for improved
fidelity in torsional friction and force distribution, especially in non-convex and
manipulation scenarios.
- **Drop-in replacement for MuJoCo's contacts** — use Newton's pipeline with
:class:`~solvers.SolverMuJoCo` for advanced contact models (see
:ref:`MuJoCo Warp Integration`).
This page starts with a :ref:`conceptual overview ` of how geometry
representations and narrow phase algorithms combine, then covers each stage in detail.
.. _Collision Overview:
Conceptual Overview
-------------------
Newton's collision pipeline runs in two stages: a **broad phase** that quickly
eliminates shape pairs whose bounding boxes do not overlap, followed by a **narrow
phase** that computes the actual contact geometry for surviving pairs.
The narrow phase algorithm used for a given pair depends on how the shapes are
represented:
.. mermaid::
:config: {"theme": "forest", "themeVariables": {"lineColor": "#76b900"}}
flowchart LR
BP["Broad Phase (AABB culling)"] --> Triage
subgraph Triage ["Pair Triage"]
G1["Convex / primitive pairs"]
G2["Mesh pairs (BVH or SDF)"]
end
subgraph NP ["Narrow Phase"]
A["MPR / GJK"]
B["Distance queries + contact reduction"]
C["Hydroelastic + contact reduction"]
end
G1 --> A --> Contacts
G2 --> B --> Contacts
G2 -.->|"both shapes hydroelastic"| C --> Contacts
**Geometry representations**
1. **Convex hulls and primitives** — sphere, box, capsule, cylinder, cone, ellipsoid,
and convex mesh shapes expose canonical support functions. These feed directly into
the **MPR/GJK** narrow phase which produces contact points without further reduction.
See :ref:`Narrow Phase`.
2. **Live BVH queries** — triangle meshes that do *not* have a precomputed SDF are
queried through Warp's BVH (Bounding Volume Hierarchy). This path computes on-the-fly
distance queries and generates contacts with optional contact reduction. It works out
of the box but can be slow for high-triangle-count meshes. Hydroelastic contacts are
**not** available on this path. See :ref:`Mesh Collisions`.
3. **Precomputed SDFs** — calling ``mesh.build_sdf(...)`` on a mesh precomputes a
signed distance field that provides O(1) distance lookups. Primitive shapes can also
generate SDF grids via ``ShapeConfig`` SDF parameters (see :ref:`Shape Configuration`).
This path supports both **distance-query** and **hydroelastic** contact generation
(with contact reduction). Hydroelastic contacts require SDF on *both* shapes in a
pair. See :ref:`Mesh Collisions` and :ref:`Hydroelastic Contacts`.
.. note::
**Contact reduction** applies to the SDF-based and hydroelastic paths where many raw
contacts are generated from distance field queries. The direct MPR/GJK path for
convex pairs produces a small number of contacts and does not require reduction. See
:ref:`Contact Reduction`.
.. tip::
For scenes with expensive collision (SDF or hydroelastic), running ``collide`` once
per frame instead of every substep can significantly improve performance. See
:ref:`Common Patterns` for the different collision-frequency patterns.
.. _Contact Model:
Contact Geometry
^^^^^^^^^^^^^^^^
The output of the narrow phase is a set of **contacts**: lightweight geometric
descriptors that decouple the solver from the underlying shape complexity. A mesh may
contain hundreds of thousands of triangles, but the collision pipeline distills the
interaction into a manageable number of contacts that the solver can process efficiently.
Each contact carries the following geometric data:
.. figure:: ../images/contact_model.svg
:alt: Contact geometry: normal, contact points, contact distance
:width: 70%
:align: center
A contact between two shapes (A and B). The **contact normal** (blue, unit length)
points from shape A to shape B. **Body-frame contact points** (yellow) are stored in
each body's local frame. The **contact midpoint** (red) — the average of the two
world-space contact points — is not stored but is useful for visualization and
debugging. The **contact distance** encodes the signed separation or penetration depth.
- **Contact normal** (world frame) — a unit vector pointing from shape A toward shape B.
- **Contact points** (body frame) — the contact location on each shape
(``rigid_contact_point0/1``), stored in the parent body's local coordinate frame.
- **Contact distance** — the signed separation between the two contact points along the
normal. Negative values indicate penetration.
Because contacts are self-contained geometric objects, the solver never needs to query
mesh triangles or SDF grids — it only works with the contact arrays stored in
:class:`~Contacts`. See :ref:`Contact Generation` for the full data layout.
.. _MuJoCo Warp Integration:
MuJoCo Warp Integration
^^^^^^^^^^^^^^^^^^^^^^^^
:class:`~solvers.SolverMuJoCo` (the MuJoCo Warp backend) ships with its own
built-in collision pipeline that handles convex primitive contacts. For many use cases
this is sufficient and requires no extra setup.
Newton's collision pipeline can also **replace** MuJoCo's contact generation, enabling
SDF-based mesh-mesh contacts and hydroelastic contacts that MuJoCo's built-in pipeline
does not support.
Examples:
- **Hydroelastic mesh contacts** —
:github:`newton/examples/contacts/example_nut_bolt_hydro.py`
- **SDF mesh contacts** —
:github:`newton/examples/contacts/example_nut_bolt_sdf.py`
- **Robot manipulation with SDF** —
:github:`newton/examples/contacts/example_brick_stacking.py`
See :ref:`Solver Integration` for the full code pattern showing how to configure
this.
.. _Collision Pipeline:
Collision Pipeline
------------------
Newton's collision pipeline implementation supports multiple broad phase algorithms and advanced contact models (SDF-based, hydroelastic, cylinder/cone primitives). See :ref:`Collision Pipeline Details` for details.
Basic usage:
.. testsetup:: pipeline-basics
import warp as wp
import newton
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0.0, 0.0, 2.0), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.5)
model = builder.finalize()
state = model.state()
.. testcode:: pipeline-basics
# Default: creates CollisionPipeline with EXPLICIT broad phase (precomputed pairs)
contacts = model.contacts()
model.collide(state, contacts)
# Or create a pipeline explicitly to choose broad phase mode
from newton import CollisionPipeline
pipeline = CollisionPipeline(
model,
broad_phase="sap",
)
contacts = pipeline.contacts()
pipeline.collide(state, contacts)
.. _Quick Start:
Quick Start
-----------
A minimal end-to-end example that creates shapes, runs collision detection, and steps the
solver (see also the :doc:`Introduction tutorial ` and
``example_basic_shapes.py`` — :github:`newton/examples/basic/example_basic_shapes.py`):
.. testcode:: quickstart
import warp as wp
import newton
builder = newton.ModelBuilder()
builder.add_ground_plane()
# Dynamic sphere
body = builder.add_body(xform=wp.transform((0.0, 0.0, 2.0), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.5)
model = builder.finalize()
solver = newton.solvers.SolverXPBD(model, iterations=5)
state_0 = model.state()
state_1 = model.state()
control = model.control()
contacts = model.contacts()
dt = 1.0 / 60.0 / 10.0
for frame in range(120):
for substep in range(10):
state_0.clear_forces()
model.collide(state_0, contacts)
solver.step(state_0, state_1, control, contacts, dt)
state_0, state_1 = state_1, state_0
.. _Supported Shape Types:
Supported Shape Types
---------------------
Newton supports the following geometry types via :class:`~GeoType`:
.. list-table::
:header-rows: 1
:widths: 20 80
* - Type
- Description
* - ``PLANE``
- Infinite plane (ground)
* - ``HFIELD``
- Heightfield terrain (2D elevation grid)
* - ``SPHERE``
- Sphere primitive
* - ``CAPSULE``
- Cylinder with hemispherical ends
* - ``BOX``
- Axis-aligned box
* - ``CYLINDER``
- Cylinder
* - ``CONE``
- Cone
* - ``ELLIPSOID``
- Ellipsoid
* - ``MESH``
- Triangle mesh (arbitrary, including non-convex)
* - ``CONVEX_MESH``
- Convex hull mesh
.. note::
**SDF is collision data, not a standalone shape type.** For mesh shapes, build and attach
an SDF explicitly with ``mesh.build_sdf(...)`` and then pass that mesh to
``builder.add_shape_mesh(...)``. For primitive hydroelastic workflows, SDF generation uses
``ShapeConfig`` SDF parameters.
.. _Shapes and Bodies:
Shapes and Rigid Bodies
-----------------------
Collision shapes are attached to rigid bodies. Each shape has:
- **Body index** (``shape_body``): The rigid body this shape is attached to. Use ``body=-1`` for static/world-fixed shapes.
- **Local transform** (``shape_transform``): Position and orientation relative to the body frame.
- **Scale** (``shape_scale``): 3D scale factors applied to the shape geometry.
- **Margin** (``shape_margin``): Surface offset that shifts where contact points are placed. See :ref:`Margin and gap semantics `.
- **Gap** (``shape_gap``): Extra detection distance that shifts when contacts are generated. See :ref:`Margin and gap semantics `.
- **Source geometry** (``shape_source``): Reference to the underlying geometry object (e.g., :class:`~Mesh`).
During collision detection, shapes are transformed to world space using their parent body's pose:
.. code-block:: python
# Shape world transform = body_pose * shape_local_transform
X_world_shape = body_q[shape_body] * shape_transform[shape_id]
Contacts are generated between shapes, not bodies. Depending on the type of solver, the motion of the bodies is affected by forces or constraints that resolve the penetrations between their attached shapes.
.. _Collision Filtering:
Collision Filtering
-------------------
The collision pipeline uses filtering rules based on world indices and collision groups.
.. _World IDs:
World Indices
^^^^^^^^^^^^^
World indices enable multi-world simulations, primarily for reinforcement learning, where objects belonging to different worlds coexist but do not interact through contacts:
- **Index -1**: Global entities that collide with all worlds (e.g., ground plane)
- **Index 0, 1, 2, ...**: World-specific entities that only interact within their world
.. testcode:: world-indices
builder = newton.ModelBuilder()
# Global ground (default world -1, collides with all worlds)
builder.add_ground_plane()
# Robot template
robot_builder = newton.ModelBuilder()
body = robot_builder.add_link()
robot_builder.add_shape_sphere(body, radius=0.5)
joint = robot_builder.add_joint_free(body)
robot_builder.add_articulation([joint])
# Instantiate in separate worlds - robots won't collide with each other
builder.add_world(robot_builder) # World 0
builder.add_world(robot_builder) # World 1
model = builder.finalize()
For heterogeneous worlds, use :meth:`~ModelBuilder.begin_world` and :meth:`~ModelBuilder.end_world`.
For large-scale parallel simulations (e.g., RL), :meth:`~ModelBuilder.replicate` stamps
out many copies of a template environment builder into separate worlds in one call:
.. testcode:: replicate
# Template environment: one sphere per world
env_builder = newton.ModelBuilder()
body = env_builder.add_body()
env_builder.add_shape_sphere(body, radius=0.5)
# Combined builder: global geometry + 1024 replicated worlds
main = newton.ModelBuilder()
main.add_ground_plane() # global (world -1), shared across all worlds
main.replicate(env_builder, world_count=1024)
model = main.finalize()
.. note::
MJWarp does not currently support heterogeneous environments (different models per world).
World indices are stored in :attr:`~Model.shape_world`, :attr:`~Model.body_world`, etc.
.. _Collision Groups:
Collision Groups
^^^^^^^^^^^^^^^^
Collision groups control which shapes collide within the same world:
- **Group 0**: Collisions disabled
- **Positive groups (1, 2, ...)**: Collide with same group or any negative group
- **Negative groups (-1, -2, ...)**: Collide with shapes in any positive or negative group, except shapes in the same negative group
.. list-table::
:header-rows: 1
:widths: 15 15 15 55
* - Group A
- Group B
- Collide?
- Reason
* - 0
- Any
- ❌
- Group 0 disables collision
* - 1
- 1
- ✅
- Same positive group
* - 1
- 2
- ❌
- Different positive groups
* - 1
- -2
- ✅
- Positive with any negative
* - -1
- -1
- ❌
- Same negative group
* - -1
- -2
- ✅
- Different negative groups
.. testcode:: collision-groups
builder = newton.ModelBuilder()
# Group 1: only collides with group 1 and negative groups
body1 = builder.add_body()
builder.add_shape_sphere(body1, radius=0.5, cfg=builder.ShapeConfig(collision_group=1))
# Group -1: collides with everything (except other -1)
body2 = builder.add_body()
builder.add_shape_sphere(body2, radius=0.5, cfg=builder.ShapeConfig(collision_group=-1))
model = builder.finalize()
**Self-collision within articulations**
Self-collisions within an articulation can be enabled or disabled with ``enable_self_collisions`` when loading models. By default, adjacent body collisions (parent-child pairs connected by joints) are disabled via ``collision_filter_parent=True``.
.. code-block:: python
# Enable self-collisions when loading models
builder.add_usd("robot.usda", enable_self_collisions=True)
builder.add_mjcf("robot.xml", enable_self_collisions=True)
# Or control per-shape (also applies to max-coordinate jointed bodies)
cfg = builder.ShapeConfig(collision_group=-1, collision_filter_parent=False)
**Controlling particle collisions**
Use ``has_shape_collision`` and ``has_particle_collision`` for fine-grained control over what a shape collides with. Setting both to ``False`` is equivalent to ``collision_group=0``.
.. testcode:: particle-collision
builder = newton.ModelBuilder()
# Shape that only collides with particles (not other shapes)
cfg = builder.ShapeConfig(has_shape_collision=False, has_particle_collision=True)
# Shape that only collides with other shapes (not particles)
cfg = builder.ShapeConfig(has_shape_collision=True, has_particle_collision=False)
UsdPhysics Collision Filtering
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Newton follows the `UsdPhysics collision filtering specification `_,
which provides two complementary mechanisms for controlling which shapes collide:
1. **Collision Groups** - Group-based filtering using ``UsdPhysicsCollisionGroup``
2. **Pairwise Filtering** - Explicit shape pair exclusions using ``physics:filteredPairs``
**Collision Groups**
In UsdPhysics, shapes can be assigned to collision groups defined by ``UsdPhysicsCollisionGroup`` prims.
When importing USD files, Newton reads the ``collisionGroups`` attribute from each shape and maps
each unique collision group name to a positive integer ID (starting from 1). Shapes in different
collision groups will not collide with each other unless their groups are configured to interact.
.. code-block:: usda
# Define a collision group in USD
def "CollisionGroup_Robot" (
prepend apiSchemas = ["PhysicsCollisionGroup"]
) {
}
# Assign shape to a collision group
def Sphere "RobotPart" (
prepend apiSchemas = ["PhysicsCollisionAPI"]
) {
rel physics:collisionGroup =
}
When loading this USD, Newton automatically assigns each collision group a unique integer ID
and sets the shape's ``collision_group`` accordingly.
**Pairwise Filtering**
For fine-grained control, UsdPhysics supports explicit pair filtering via the ``physics:filteredPairs``
relationship. This allows excluding specific shape pairs from collision detection regardless of their
collision groups.
.. code-block:: usda
# Exclude specific shape pairs in USD
def Sphere "ShapeA" (
prepend apiSchemas = ["PhysicsCollisionAPI"]
) {
rel physics:filteredPairs = []
}
Newton reads these relationships during USD import and converts them to
:attr:`~ModelBuilder.shape_collision_filter_pairs`.
**Collision Enabled Flag**
Shapes with ``physics:collisionEnabled=false`` are excluded from all collisions by adding filter
pairs against all other shapes in the scene.
Shape Collision Filter Pairs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
The :attr:`~ModelBuilder.shape_collision_filter_pairs` list stores explicit shape pair exclusions.
This is Newton's internal representation for pairwise filtering (including pairs imported from
UsdPhysics ``physics:filteredPairs`` relationships).
.. testcode:: filter-pairs
builder = newton.ModelBuilder()
# Add shapes
body = builder.add_body()
shape_a = builder.add_shape_sphere(body, radius=0.5)
shape_b = builder.add_shape_box(body, hx=0.5, hy=0.5, hz=0.5)
# Exclude this specific pair from collision detection
builder.add_shape_collision_filter_pair(shape_a, shape_b)
Filter pairs are automatically populated in several cases:
- **Adjacent bodies**: Parent-child body pairs connected by joints (when ``collision_filter_parent=True``). Also applies to max-coordinate jointed bodies.
- **Same-body shapes**: Shapes attached to the same rigid body
- **Disabled self-collision**: All shape pairs within an articulation when ``enable_self_collisions=False``
- **USD filtered pairs**: Pairs defined by ``physics:filteredPairs`` relationships in USD files
- **USD collision disabled**: Shapes with ``physics:collisionEnabled=false`` (filtered against all other shapes)
The resulting filter pairs are stored in :attr:`~Model.shape_collision_filter_pairs` as a set of
``(shape_index_a, shape_index_b)`` tuples (canonical order: ``a < b``).
**USD Import Example**
.. code-block:: python
# Newton automatically imports UsdPhysics collision filtering
builder = newton.ModelBuilder()
builder.add_usd("scene.usda")
# Collision groups and filter pairs are now populated:
# - shape_collision_group: integer IDs mapped from UsdPhysicsCollisionGroup
# - shape_collision_filter_pairs: pairs from physics:filteredPairs relationships
model = builder.finalize()
.. _Collision Pipeline Details:
Broad Phase and Shape Compatibility
-----------------------------------
:class:`~CollisionPipeline` provides configurable broad phase algorithms:
.. list-table::
:header-rows: 1
:widths: 15 85
* - Mode
- Description
* - **NxN**
- All-pairs AABB broad phase. O(N²), optimal for small scenes (<100 shapes).
* - **SAP**
- Sweep-and-prune AABB broad phase. O(N log N), better for larger scenes with spatial coherence.
* - **EXPLICIT**
- Uses precomputed shape pairs (default). Combines static pair efficiency with advanced contact algorithms.
.. testsetup:: broad-phase
import warp as wp
import newton
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0.0, 0.0, 2.0), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.5)
model = builder.finalize()
state = model.state()
.. testcode:: broad-phase
from newton import CollisionPipeline
# Default: EXPLICIT (precomputed pairs)
pipeline = CollisionPipeline(model)
# NxN for small scenes
pipeline = CollisionPipeline(model, broad_phase="nxn")
# SAP for larger scenes
pipeline = CollisionPipeline(model, broad_phase="sap")
contacts = pipeline.contacts()
pipeline.collide(state, contacts)
.. _Shape Compatibility:
Shape Compatibility
^^^^^^^^^^^^^^^^^^^
Shape compatibility summary (rigid + soft particle-shape):
.. list-table::
:header-rows: 1
:widths: 11 7 7 7 7 7 7 7 7 7 7 7 7
* -
- Plane
- HField
- Sphere
- Capsule
- Box
- Cylinder
- Cone
- Ellipsoid
- ConvexHull
- Mesh
- SDF
- Particle
* - **Plane**
- [1]
- [1]
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
* - **HField**
- [1]
- [1]
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅⚠️
- ✅
- ✅
* - **Sphere**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
* - **Capsule**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
* - **Box**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
* - **Cylinder**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
* - **Cone**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
* - **Ellipsoid**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
* - **ConvexHull**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
* - **Mesh**
- ✅
- ✅⚠️
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅⚠️
- ✅⚠️
- ✅
* - **SDF**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅⚠️
- ✅
- ✅
* - **Particle**
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- ✅
- [2]
**Legend:** ⚠️ = Can be slow for meshes with high triangle counts; performance can
often be improved by attaching a precomputed SDF to the mesh (``mesh.build_sdf(...)``).
| [1] Plane and heightfield shapes are static (world-attached) in Newton; static-static pairs are filtered from rigid collision generation.
| [2] Particle-particle interactions are handled by the particle/soft-body solver self-collision path, not by the shape compatibility pipeline in this table.
.. note::
``Particle`` in this table refers to soft particle-shape contacts generated
automatically by the collision pipeline. These contacts additionally require
the shape to have particle collision enabled
(``ShapeFlags.COLLIDE_PARTICLES`` / ``ShapeConfig.has_particle_collision``).
For examples, see cloth and cable scenes that use the collision pipeline for
particle-shape contacts.
.. note::
**Heightfield representation:** A heightfield (``HFIELD``) stores a regular 2D grid
of elevation samples. For rigid contacts, Newton uses dedicated heightfield
narrow-phase routes: heightfield-vs-convex uses per-cell triangle GJK/MPR, while
mesh-vs-heightfield routes through the mesh/SDF path with on-the-fly triangle
extraction from the grid. For soft contacts, the collision pipeline automatically
samples the heightfield signed distance and normal.
.. note::
**SDF** in this table refers to shapes with precomputed SDF data. There is no
``GeoType.SDF`` enum value; this row is a conceptual collision mode for shapes
carrying SDF resources. Mesh SDFs are attached through ``mesh.build_sdf(...)``
and provide O(1) distance queries.
.. _Narrow Phase:
Narrow Phase Algorithms
-----------------------
After broad phase identifies candidate pairs, the narrow phase generates contact points.
The algorithm used depends on the shape types in each pair.
.. _Convex Primitive Contacts:
Convex Primitive Contacts
^^^^^^^^^^^^^^^^^^^^^^^^^
**MPR (Minkowski Portal Refinement) and GJK**
MPR is the primary algorithm for convex shape pairs. It uses support mapping functions to
find the closest points between shapes via Minkowski difference sampling. Works with all
convex primitives (sphere, box, capsule, cylinder, cone, ellipsoid) and convex meshes.
Newton uses MPR for penetration depth computation (not EPA); GJK handles the
separated-shapes distance query.
**Multi-contact generation**
For convex primitive pairs, multiple contact points are generated for stable stacking and
resting contacts. The collision pipeline estimates buffer sizes based on the model; you
can override this value with ``rigid_contact_max`` when instantiating the pipeline.
.. _Mesh Collisions:
Mesh Collision Handling
^^^^^^^^^^^^^^^^^^^^^^^
Mesh collisions use different strategies depending on the pair type:
**Mesh vs Primitive (e.g., Sphere, Box)**
Uses BVH (Bounding Volume Hierarchy) queries to find nearby triangles, then generates contacts between primitive vertices and triangle surfaces, plus triangle vertices against the primitive.
**Mesh vs Plane**
Projects mesh vertices onto the plane and generates contacts for vertices below the plane surface.
**Mesh vs Mesh**
Two approaches available:
1. **BVH-based** (default when no SDF configured): Iterates mesh vertices against the other mesh's BVH.
Performance scales with triangle count - can be very slow for complex meshes.
2. **SDF-based** (recommended): Uses precomputed signed distance fields for fast queries.
For mesh shapes, call ``mesh.build_sdf(...)`` once and reuse the mesh.
.. warning::
If SDF is not precomputed, mesh-mesh contacts fall back to on-the-fly BVH distance queries
which are **significantly slower**. For production use with complex meshes, precompute and
attach SDF data on meshes:
.. code-block:: python
my_mesh.build_sdf(max_resolution=64)
builder.add_shape_mesh(body, mesh=my_mesh)
.. tip::
**Build an SDF on every mesh that can collide**, even when high-precision contacts are
not required. A low-resolution SDF (e.g., ``max_resolution=64``) uses very little memory
yet still provides O(1) distance queries that are dramatically faster than the BVH
fallback. Without an SDF, mesh-vs-mesh and mesh-vs-primitive contacts must walk the BVH
for every query point, which dominates collision cost in most scenes. Attaching even a
coarse SDF eliminates this bottleneck.
:meth:`~Mesh.build_sdf` accepts several optional keyword arguments
(defaults shown in parentheses):
.. code-block:: python
mesh.build_sdf(
max_resolution=256, # Max voxels along longest AABB axis; must be divisible by 8 (None)
narrow_band_range=(-0.005, 0.005), # SDF narrow band [m] ((-0.1, 0.1))
margin=0.005, # Extra AABB padding [m] (0.05)
shape_margin=0.001, # Shrink SDF surface inward [m] (0.0)
scale=(1.0, 1.0, 1.0), # Bake non-unit scale into the SDF (None)
)
``max_resolution`` sets the voxel count along the longest AABB axis (must be divisible by 8);
voxel size is uniform across all axes. Use ``target_voxel_size`` instead to specify resolution
in meters directly — it takes precedence over ``max_resolution`` when both are provided. Use
``narrow_band_range`` to limit the SDF computation to a thin shell around the surface (saves
memory and build time). Set the SDF ``margin`` to at least the sum of the shape's :ref:`margin and gap ` so the SDF covers the
full contact detection range. Pass ``scale`` when the shape will be added with non-unit scale
to bake it into the SDF grid. ``shape_margin`` is mainly useful for hydroelastic collision
where a compliant-layer offset is desired.
**Mesh simplification for collision**
For imported models (URDF, MJCF, USD) whose visual meshes are too detailed for efficient
collision, :meth:`~ModelBuilder.approximate_meshes` replaces mesh collision shapes
with convex hulls, bounding boxes, or convex decompositions:
.. code-block:: python
builder.add_usd("robot.usda")
# Replace all collision meshes with convex hulls (default)
builder.approximate_meshes()
# Or target specific shapes and keep visual geometry
builder.approximate_meshes(
method="convex_hull",
shape_indices=non_finger_shapes,
keep_visual_shapes=True,
)
Supported methods: ``"convex_hull"`` (default), ``"bounding_box"``, ``"bounding_sphere"``,
``"coacd"`` (convex decomposition), ``"vhacd"``.
.. note::
``approximate_meshes()`` modifies the builder's shape geometry in-place. By default
(``keep_visual_shapes=False``), the original mesh is replaced for both collision and
rendering. Pass ``keep_visual_shapes=True`` to preserve the original mesh as a
visual-only shape alongside the simplified collision shape.
.. _Contact Reduction:
Contact Reduction
^^^^^^^^^^^^^^^^^
Contact reduction is enabled by default. For scenes with many mesh-mesh interactions that generate thousands of contacts, reduction selects a significantly smaller representative set that maintains stable contact behavior while improving solver performance.
**How it works:**
1. Contacts are binned by normal direction (polyhedron face directions)
2. Within each bin, contacts are scored by spatial distribution and penetration depth
3. Representative contacts are selected to preserve coverage and depth cues
To disable reduction, set ``reduce_contacts=False`` when creating the pipeline.
**Configuring contact reduction (HydroelasticSDF.Config):**
For hydroelastic and SDF-based contacts, use :class:`~geometry.HydroelasticSDF.Config` to tune reduction behavior:
.. testsetup:: hydro-config
import warp as wp
import newton
from newton import CollisionPipeline
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0.0, 0.0, 2.0), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.5)
model = builder.finalize()
.. testcode:: hydro-config
from newton.geometry import HydroelasticSDF
config = HydroelasticSDF.Config(
reduce_contacts=True, # Enable contact reduction (default)
buffer_fraction=0.2, # Reduce GPU buffer allocations (default: 1.0)
normal_matching=True, # Align reduced normals with aggregate force
anchor_contact=False, # Optional center-of-pressure anchor contact
)
pipeline = CollisionPipeline(model, sdf_hydroelastic_config=config)
**Other reduction options:**
.. list-table::
:header-rows: 1
:widths: 30 70
* - Parameter
- Description
* - ``normal_matching``
- Rotates selected contact normals so their weighted sum aligns with the aggregate force direction
from all unreduced contacts. Preserves net force direction after reduction. Default: True.
* - ``anchor_contact``
- Adds an anchor contact at the center of pressure for each normal bin to better preserve moments.
Default: False.
* - ``margin_contact_area``
- Lower bound on contact area. Hydroelastic stiffness is ``area * k_eff``, but contacts
within the contact margin that are not yet penetrating (speculative contacts) have zero
geometric area. This provides a floor value so they still generate repulsive force. Default: 0.01.
.. _Shape Configuration:
Shape Configuration
-------------------
Shape collision behavior is controlled via :class:`~ModelBuilder.ShapeConfig`:
**Collision control:**
.. list-table::
:header-rows: 1
:widths: 30 70
* - Parameter
- Description
* - ``collision_group``
- Collision group ID. 0 disables collisions. Default: 1.
* - ``collision_filter_parent``
- Filter collisions with adjacent body (parent in articulation or connected via joint). Default: True.
* - ``has_shape_collision``
- Whether shape collides with other shapes. Default: True.
* - ``has_particle_collision``
- Whether shape collides with particles. Default: True.
**Geometry parameters:**
.. list-table::
:header-rows: 1
:widths: 25 75
* - Parameter
- Description
* - ``margin``
- Surface offset used by narrow phase. Pairwise effect is additive (``m_a + m_b``): contacts are evaluated against the signed distance to the margin-shifted surfaces, so resting separation is ``m_a + m_b``. Helps thin shells/cloth stability and reduces self-intersections. Default: 0.0.
* - ``gap``
- Additional detection threshold. Pairwise effect is additive (``g_a + g_b``). Broad phase expands each shape AABB by ``(margin + gap)`` per shape; narrow phase then keeps a candidate contact when ``d <= g_a + g_b`` (with ``d`` measured relative to margin-shifted surfaces). Increasing gap detects contacts earlier and helps reduce tunneling. Default: None (uses ``builder.rigid_gap``, which defaults to 0.1).
* - ``is_solid``
- Whether shape is solid or hollow. Affects inertia and SDF sign. Default: True.
* - ``is_hydroelastic``
- Whether the shape uses SDF-based hydroelastic contacts. Both shapes in a pair must have this enabled. See :ref:`Hydroelastic Contacts`. Default: False.
* - ``kh``
- Contact stiffness for hydroelastic collisions. Used by MuJoCo, Featherstone, SemiImplicit when ``is_hydroelastic=True``. Default: 1.0e10.
.. _margin-gap-semantics:
**Margin and gap semantics (where vs when):**
- **Where contacts are placed** is controlled by ``margin``.
- **When contacts are generated** is controlled by ``gap``.
For a shape pair ``(a, b)``:
- Pair margin: ``m = margin_a + margin_b``
- Pair gap: ``g = gap_a + gap_b``
- Surface distance (true geometry, no offsets): ``s``
- Contact-space distance used by Newton: ``d = s - m``
Contacts are generated when:
.. math::
d \leq g \quad\Leftrightarrow\quad s \leq (m + g)
Broad phase uses the same idea by expanding each shape AABB by:
.. math::
margin_i + gap_i
This keeps broad-phase culling and narrow-phase contact generation consistent.
The solver enforces ``d >= 0``, so objects at rest settle with surfaces separated
by ``margin_a + margin_b``.
.. figure:: ../images/margin_and_gap.svg
:alt: Margin and gap contact generation phases
:width: 90%
:align: center
Margin sets contact location (surface offset), while gap adds speculative
detection distance on top of margin. Left: no contact generated. Middle:
contact generated but not yet active. Right: active contact support.
**SDF configuration (primitive generation defaults):**
.. list-table::
:header-rows: 1
:widths: 30 70
* - Parameter
- Description
* - ``sdf_max_resolution``
- Maximum SDF grid dimension (must be divisible by 8) for primitive SDF generation.
* - ``sdf_target_voxel_size``
- Target voxel size for primitive SDF generation. Takes precedence over ``sdf_max_resolution``.
* - ``sdf_narrow_band_range``
- SDF narrow band distance range (inner, outer). Default: (-0.1, 0.1).
The :meth:`~ModelBuilder.ShapeConfig.configure_sdf` helper sets SDF and hydroelastic
options in one call:
.. testcode:: configure-sdf
builder = newton.ModelBuilder()
cfg = builder.ShapeConfig()
cfg.configure_sdf(max_resolution=64, is_hydroelastic=True, kh=1.0e11)
Example (mesh SDF workflow):
.. code-block:: python
cfg = builder.ShapeConfig(
collision_group=-1, # Collide with everything
margin=0.001, # 1mm margin
gap=0.01, # 1cm detection gap
)
my_mesh.build_sdf(max_resolution=64)
builder.add_shape_mesh(body, mesh=my_mesh, cfg=cfg)
**Builder default gap:**
The builder's ``rigid_gap`` (default 0.1) applies to shapes without explicit ``gap``. Alternatively, use ``builder.default_shape_cfg.gap``.
.. _Common Patterns:
Common Patterns
---------------
**Creating static/ground geometry**
Use ``body=-1`` to attach shapes to the static world frame:
.. testcode:: static-geometry
builder = newton.ModelBuilder()
# Static ground plane
builder.add_ground_plane() # Convenience method
# Or manually create static shapes
builder.add_shape_plane(body=-1, xform=wp.transform_identity())
builder.add_shape_box(body=-1, hx=5.0, hy=5.0, hz=0.1) # Static floor
**Setting default shape configuration**
Use ``builder.default_shape_cfg`` to set defaults for all shapes:
.. testcode:: default-shape-cfg
builder = newton.ModelBuilder()
# Set defaults before adding shapes
builder.default_shape_cfg.ke = 1.0e6
builder.default_shape_cfg.kd = 1000.0
builder.default_shape_cfg.mu = 0.5
builder.default_shape_cfg.is_hydroelastic = True
builder.default_shape_cfg.sdf_max_resolution = 64 # Primitive SDF defaults
**Collision frequency in the simulation loop**
There are two common patterns for when to call ``collide`` relative to the substep loop:
.. testsetup:: sim-loop
import warp as wp
import newton
from newton import CollisionPipeline
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0.0, 0.0, 2.0), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.5)
model = builder.finalize()
solver = newton.solvers.SolverXPBD(model, iterations=5)
pipeline = CollisionPipeline(model, broad_phase="sap")
state_0 = model.state()
state_1 = model.state()
control = model.control()
contacts = model.contacts()
num_frames = 2
sim_substeps = 3
sim_dt = 1.0 / 60.0 / sim_substeps
collide_every_n = 2
*Every substep* (most accurate, used by most basic examples):
.. testcode:: sim-loop
for frame in range(num_frames):
for substep in range(sim_substeps):
model.collide(state_0, contacts)
solver.step(state_0, state_1, control, contacts, dt=sim_dt)
state_0, state_1 = state_1, state_0
*Once per frame* (faster, common for hydroelastic/SDF-heavy scenes):
.. testcode:: sim-loop
for frame in range(num_frames):
contacts = model.collide(state_0, collision_pipeline=pipeline)
for substep in range(sim_substeps):
solver.step(state_0, state_1, control, contacts, dt=sim_dt)
state_0, state_1 = state_1, state_0
Another pattern is to run collision detection every N substeps for a middle ground:
.. testcode:: sim-loop
for frame in range(num_frames):
for substep in range(sim_substeps):
if substep % collide_every_n == 0:
pipeline.collide(state_0, contacts)
solver.step(state_0, state_1, control, contacts, dt=sim_dt)
state_0, state_1 = state_1, state_0
**Soft contacts (particle-shape)**
Soft contacts are generated automatically when particles are present. They use a separate margin:
.. testsetup:: soft-contacts
import warp as wp
import newton
from newton import CollisionPipeline
builder = newton.ModelBuilder()
builder.add_ground_plane()
builder.add_particle(pos=wp.vec3(0, 0, 1), vel=wp.vec3(0, 0, 0), mass=1.0)
model = builder.finalize()
state = model.state()
.. testcode:: soft-contacts
# Set soft contact margin
pipeline = CollisionPipeline(model, soft_contact_margin=0.01)
contacts = pipeline.contacts()
pipeline.collide(state, contacts)
# Access soft contact data
n_soft = contacts.soft_contact_count.numpy()[0]
particles = contacts.soft_contact_particle.numpy()[:n_soft]
shapes = contacts.soft_contact_shape.numpy()[:n_soft]
.. _Contact Generation:
Contact Data
------------
The :class:`~Contacts` class stores the results from the collision detection step
and is consumed by the solver :meth:`~solvers.SolverBase.step` method for contact handling.
**Rigid contacts:**
.. list-table::
:header-rows: 1
:widths: 35 65
* - Attribute
- Description
* - ``rigid_contact_count``
- Number of active rigid contacts (scalar).
* - ``rigid_contact_shape0``, ``rigid_contact_shape1``
- Indices of colliding shapes.
* - ``rigid_contact_point0``, ``rigid_contact_point1``
- Contact point on each shape (body frame). This is the narrow-phase contact
location used by the solver for the normal constraint and lever-arm computation.
* - ``rigid_contact_offset0``, ``rigid_contact_offset1``
- Body-frame friction-anchor offset per shape, equal to the contact normal scaled
by ``effective_radius + margin``. Added to the contact point to form a shifted
friction anchor that accounts for rotational effects of finite contact thickness
in tangential friction calculations.
* - ``rigid_contact_normal``
- Contact normal, pointing from shape 0 toward shape 1 (world frame).
* - ``rigid_contact_margin0``, ``rigid_contact_margin1``
- Per-shape thickness: effective radius + margin (scalar).
**Soft contacts (particle-shape):**
.. list-table::
:header-rows: 1
:widths: 35 65
* - Attribute
- Description
* - ``soft_contact_count``
- Number of active soft contacts.
* - ``soft_contact_particle``
- Particle indices.
* - ``soft_contact_shape``
- Shape indices.
* - ``soft_contact_body_pos``, ``soft_contact_body_vel``
- Contact position and velocity on shape.
* - ``soft_contact_normal``
- Contact normal.
**Extended contact attributes** (see :ref:`extended_contact_attributes`):
.. list-table::
:header-rows: 1
:widths: 22 78
* - Attribute
- Description
* - :attr:`~Contacts.force`
- Contact spatial forces (used by :class:`~sensors.SensorContact`)
Example usage:
.. testsetup:: contact-data
import warp as wp
import newton
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0.0, 0.0, 2.0), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.5)
model = builder.finalize()
state = model.state()
.. testcode:: contact-data
contacts = model.contacts()
model.collide(state, contacts)
n = contacts.rigid_contact_count.numpy()[0]
points0 = contacts.rigid_contact_point0.numpy()[:n]
points1 = contacts.rigid_contact_point1.numpy()[:n]
normals = contacts.rigid_contact_normal.numpy()[:n]
# Shape indices
shape0 = contacts.rigid_contact_shape0.numpy()[:n]
shape1 = contacts.rigid_contact_shape1.numpy()[:n]
.. _Differentiable Contacts:
Differentiable Contacts
-----------------------
When ``requires_grad=True``, the :class:`~newton.Contacts` object provides an
additional set of **differentiable** rigid-contact arrays that participate in
:class:`wp.Tape` autodiff. These arrays give first-order gradients of contact
distance and world-space contact points with respect to body poses
(``state.body_q``).
.. note::
Rigid-contact differentiability is **experimental**. Accuracy and fitness for
real-world optimization or learning workflows should be validated case by case
before relying on these gradients.
Making the full narrow-phase pipeline differentiable end-to-end would be
prohibitively expensive and numerically fragile — iterative GJK/MPR solvers,
BVH traversals, and discrete contact-set changes all introduce discontinuities
or ill-conditioned gradients. Newton therefore keeps the narrow phase frozen
(``enable_backward=False``) and applies a lightweight **post-processing** step:
it re-reads the contact geometry produced by the narrow phase (body-local
points, world normal, margins) and reconstructs the world-space quantities
through the differentiable ``body_q``. The result is a first-order
tangent-plane approximation that is cheap, stable, and sufficient for most
gradient-based optimization and reinforcement-learning workflows.
**Differentiable arrays** (allocated only when ``requires_grad=True``):
.. list-table::
:header-rows: 1
:widths: 40 60
* - Attribute
- Description
* - ``rigid_contact_diff_distance``
- Signed contact distance [m] (negative = penetration).
* - ``rigid_contact_diff_normal``
- World-space contact normal (A → B).
* - ``rigid_contact_diff_point0_world``
- World-space contact point on shape 0 [m].
* - ``rigid_contact_diff_point1_world``
- World-space contact point on shape 1 [m].
Gradients flow through the contact points and distance; the normal direction is
treated as a frozen constant.
.. testsetup:: diff-contacts
import warp as wp
import newton
.. testcode:: diff-contacts
builder = newton.ModelBuilder(gravity=0.0)
body = builder.add_body(xform=wp.transform((0.0, 0.0, 0.3)))
builder.add_shape_sphere(body=body, radius=0.5)
builder.add_ground_plane()
model = builder.finalize(requires_grad=True)
pipeline = newton.CollisionPipeline(model)
contacts = pipeline.contacts()
state = model.state(requires_grad=True)
with wp.Tape() as tape:
pipeline.collide(state, contacts)
# Backpropagate through differentiable distance
tape.backward(grads={
contacts.rigid_contact_diff_distance: wp.ones(
contacts.rigid_contact_max, dtype=float
)
})
grad_body_q = tape.gradients[state.body_q]
.. note::
The standard (non-differentiable) rigid-contact arrays
(``rigid_contact_point0``, ``rigid_contact_normal``, etc.) are unaffected and
remain available for solvers. The ``rigid_contact_diff_*`` arrays are an
additional output intended for gradient-based optimization and ML workflows.
.. _Creating Contacts:
Creating and Populating Contacts
--------------------------------
:meth:`~Model.contacts` creates a :class:`~Contacts` buffer using a default
:class:`~CollisionPipeline` (EXPLICIT broad phase, cached on first call).
:meth:`~Model.collide` populates it and returns the :class:`~Contacts` object:
.. testsetup:: creating-contacts
import warp as wp
import newton
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0.0, 0.0, 2.0), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.5)
model = builder.finalize()
state = model.state()
.. testcode:: creating-contacts
contacts = model.contacts()
model.collide(state, contacts)
The contacts buffer can be reused across steps -- ``collide`` clears it each time.
Both methods accept an optional ``collision_pipeline`` keyword to override the default
pipeline. When ``contacts`` is omitted from ``collide``, a buffer is allocated automatically:
.. testcode:: creating-contacts
from newton import CollisionPipeline
pipeline = CollisionPipeline(
model,
broad_phase="sap",
rigid_contact_max=50000,
)
# Option A: explicit buffer
contacts = pipeline.contacts()
pipeline.collide(state, contacts)
# Option B: use model helpers with a custom pipeline
contacts = model.contacts(collision_pipeline=pipeline)
model.collide(state, contacts)
# Option C: let collide allocate the buffer for you
contacts = model.collide(state, collision_pipeline=pipeline)
.. _Hydroelastic Contacts:
Hydroelastic Contacts
---------------------
Hydroelastic contacts are an **opt-in** feature that generates contact areas (not just points) using SDF-based collision detection. This provides more realistic and continuous force distribution, particularly useful for robotic manipulation scenarios.
**Default behavior (hydroelastic disabled):**
When ``is_hydroelastic=False`` (default), shapes use **hard SDF contacts** - point contacts computed from SDF distance queries. This is efficient and suitable for most rigid body simulations.
**Opt-in hydroelastic behavior:**
When ``is_hydroelastic=True`` on **both** shapes in a pair, the system generates distributed contact areas instead of point contacts. This is useful for:
- More stable and continuous contact forces for non-convex shape interactions
- Better force distribution across large contact patches
- Realistic friction behavior for flat-on-flat contacts
**Requirements:**
- Both shapes in a pair must have ``is_hydroelastic=True``
- Shapes must have SDF data available:
- mesh shapes: call ``mesh.build_sdf(...)``
- primitive shapes: use ``sdf_max_resolution`` or ``sdf_target_voxel_size`` in ``ShapeConfig``
- For non-unit shape scale, the attached SDF must be scale-baked
- Only volumetric shapes supported (not planes, heightfields, or non-watertight meshes)
.. testcode:: hydroelastic
builder = newton.ModelBuilder()
body = builder.add_body()
cfg = builder.ShapeConfig(
is_hydroelastic=True, # Opt-in to hydroelastic contacts
sdf_max_resolution=64, # Required for hydroelastic
kh=1.0e11, # Contact stiffness
)
builder.add_shape_box(body, hx=0.5, hy=0.5, hz=0.5, cfg=cfg)
**How it works:**
1. SDF intersection finds overlapping regions between shapes
2. Marching cubes extracts the contact iso-surface
3. Contact points are distributed across the surface area
4. Optional contact reduction selects representative points
**Hydroelastic stiffness (kh):**
The ``kh`` parameter on each shape controls area-dependent contact stiffness. For a pair, the effective stiffness is computed as the harmonic mean: ``k_eff = 2 * k_a * k_b / (k_a + k_b)``. Tune this for desired penetration behavior.
Contact reduction options for hydroelastic contacts are configured via :class:`~geometry.HydroelasticSDF.Config` (see :ref:`Contact Reduction`).
Hydroelastic memory can be tuned with ``buffer_fraction`` on
:class:`~geometry.HydroelasticSDF.Config`. This scales broadphase, iso-refinement,
and hydroelastic face-contact buffer allocations as a fraction of the worst-case
size. Lower values reduce memory usage but also reduce overflow headroom.
.. testcode:: hydro-buffer
from newton.geometry import HydroelasticSDF
config = HydroelasticSDF.Config(
reduce_contacts=True,
buffer_fraction=0.2, # 20% of worst-case (default: 1.0)
)
The default ``buffer_fraction`` is ``1.0`` (full worst-case allocation). Lowering it
reduces GPU memory usage but may cause overflow in dense contact scenes.
If runtime overflow warnings appear, increase ``buffer_fraction`` (or stage-specific
``buffer_mult_*`` values) until warnings disappear in your target scenes.
.. _Contact Material Properties:
Contact Materials
-----------------
Shape material properties control contact resolution. Configure via :class:`~ModelBuilder.ShapeConfig`:
.. list-table::
:header-rows: 1
:widths: 10 25 18 9 19 19
* - Property
- Description
- Solvers
- Default
- ShapeConfig
- Model Array
* - ``mu``
- Dynamic friction coefficient
- All
- 1.0
- :attr:`~ModelBuilder.ShapeConfig.mu`
- :attr:`~Model.shape_material_mu`
* - ``ke``
- Contact elastic stiffness
- SemiImplicit, Featherstone, MuJoCo
- 2.5e3
- :attr:`~ModelBuilder.ShapeConfig.ke`
- :attr:`~Model.shape_material_ke`
* - ``kd``
- Contact damping
- SemiImplicit, Featherstone, MuJoCo
- 100.0
- :attr:`~ModelBuilder.ShapeConfig.kd`
- :attr:`~Model.shape_material_kd`
* - ``kf``
- Friction damping coefficient
- SemiImplicit, Featherstone
- 1000.0
- :attr:`~ModelBuilder.ShapeConfig.kf`
- :attr:`~Model.shape_material_kf`
* - ``ka``
- Adhesion distance
- SemiImplicit, Featherstone
- 0.0
- :attr:`~ModelBuilder.ShapeConfig.ka`
- :attr:`~Model.shape_material_ka`
* - ``restitution``
- Bounciness (requires ``enable_restitution=True`` in solver)
- XPBD
- 0.0
- :attr:`~ModelBuilder.ShapeConfig.restitution`
- :attr:`~Model.shape_material_restitution`
* - ``mu_torsional``
- Resistance to spinning at contact
- XPBD, MuJoCo
- 0.005
- :attr:`~ModelBuilder.ShapeConfig.mu_torsional`
- :attr:`~Model.shape_material_mu_torsional`
* - ``mu_rolling``
- Resistance to rolling motion
- XPBD, MuJoCo
- 0.0001
- :attr:`~ModelBuilder.ShapeConfig.mu_rolling`
- :attr:`~Model.shape_material_mu_rolling`
* - ``kh``
- Hydroelastic stiffness
- SemiImplicit, Featherstone, MuJoCo
- 1.0e10
- :attr:`~ModelBuilder.ShapeConfig.kh`
- :attr:`~Model.shape_material_kh`
.. note::
Material properties interact differently with each solver. ``ke``, ``kd``, ``kf``, and ``ka``
are used by force-based solvers (SemiImplicit, Featherstone, MuJoCo), while ``restitution``
only applies to XPBD. See the :doc:`../api/newton_solvers` API reference for solver-specific
behavior.
Example:
.. testcode:: material-config
builder = newton.ModelBuilder()
cfg = builder.ShapeConfig(
mu=0.8, # High friction
ke=1.0e6, # Stiff contact
kd=1000.0, # Damping
restitution=0.5, # Bouncy (XPBD only)
)
.. _USD Collision:
USD Integration
---------------
Custom collision properties can be authored in USD:
.. code-block:: usda
def Xform "Box" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsCollisionAPI"]
) {
custom int newton:collision_group = 1
custom int newton:world = 0
custom float newton:contact_ke = 100000.0
custom float newton:contact_kd = 1000.0
custom float newton:contact_kf = 1000.0
custom float newton:contact_ka = 0.0
custom float newton:margin = 0.00001
}
See :doc:`custom_attributes` and :doc:`usd_parsing` for details.
.. _Performance:
Performance
-----------
- Use **EXPLICIT** (default) when collision pairs are limited (<100 shapes with most pairs filtered)
- Use **SAP** for >100 shapes with spatial coherence
- Use **NxN** for small scenes (<100 shapes) or uniform spatial distribution
- Minimize global entities (world=-1) as they interact with all worlds
- Use positive collision groups to reduce candidate pairs
- Use world indices for parallel simulations (essential for RL with many environments)
- Contact reduction is enabled by default for mesh-heavy scenes
- Pass ``rigid_contact_max`` to :class:`~CollisionPipeline` to limit memory in complex scenes
- Use :meth:`~ModelBuilder.approximate_meshes` to replace detailed visual meshes with convex hulls for collision
- Use ``viewer.log_contacts(contacts, state)`` in the render loop to visualize contact points and normals for debugging
**Troubleshooting**
- **No contacts generated?** Check that both shapes have compatible ``collision_group`` values (group 0 disables collision) and belong to the same world index.
- **Mesh-mesh contacts slow?** Attach an SDF with ``mesh.build_sdf(...)`` — without it, Newton falls back to O(N) BVH vertex queries.
- **Objects tunneling through each other?** Increase ``gap`` to detect contacts earlier, or increase substep count (decrease simulation ``dt``).
- **Hydroelastic buffer overflow warnings?** Increase ``buffer_fraction`` in :class:`~geometry.HydroelasticSDF.Config`.
**CUDA graph capture**
On CUDA devices, the simulation loop (including ``collide`` and ``solver.step``) can be
captured into a CUDA graph with ``wp.ScopedCapture`` for reduced kernel launch overhead.
Place ``collide`` inside the captured region so it is replayed each frame:
.. code-block:: python
if wp.get_device().is_cuda:
with wp.ScopedCapture() as capture:
model.collide(state_0, contacts)
for _ in range(sim_substeps):
solver.step(state_0, state_1, control, contacts, dt)
state_0, state_1 = state_1, state_0
graph = capture.graph
# Each frame:
wp.capture_launch(graph)
.. _Solver Integration:
Solver Integration
------------------
Newton's collision pipeline works with all built-in solvers
(:class:`~solvers.SolverXPBD`, :class:`~solvers.SolverVBD`,
:class:`~solvers.SolverSemiImplicit`, :class:`~solvers.SolverFeatherstone`,
:class:`~solvers.SolverMuJoCo`). Pass the :class:`~Contacts`
object to :meth:`~solvers.SolverBase.step`:
.. code-block:: python
solver.step(state_0, state_1, control, contacts, dt)
**MuJoCo solver** (see also :ref:`MuJoCo Warp Integration`)
By default (``use_mujoco_contacts=True``), :class:`~solvers.SolverMuJoCo` runs its own
contact generation and the ``contacts`` argument to ``step`` should be ``None``.
To replace MuJoCo's contact generation with Newton's pipeline — enabling advanced contact models
(SDF, hydroelastic) — set ``use_mujoco_contacts=False`` and pass a populated
:class:`~Contacts` object to :meth:`~solvers.SolverMuJoCo.step`:
.. testsetup:: mujoco-solver
import warp as wp
import newton
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0.0, 0.0, 2.0), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.5)
model = builder.finalize()
state_0 = model.state()
state_1 = model.state()
control = model.control()
num_steps = 2
.. testcode:: mujoco-solver
pipeline = newton.CollisionPipeline(model, broad_phase="sap")
solver = newton.solvers.SolverMuJoCo(
model,
use_mujoco_contacts=False,
)
contacts = pipeline.contacts()
for step in range(num_steps):
pipeline.collide(state_0, contacts)
solver.step(state_0, state_1, control, contacts, dt=1.0/60.0)
state_0, state_1 = state_1, state_0
.. _Advanced Customization:
Advanced Customization
----------------------
:class:`~CollisionPipeline` covers the vast majority of use cases, but Newton also
exposes the underlying broad phase, narrow phase, and primitive collision building blocks
for users who need full control — for example, writing contacts in a custom format,
implementing a domain-specific culling strategy, or integrating Newton's collision
detection into an external solver.
**Pipeline stages**
The standard pipeline runs three stages:
1. **AABB computation** — shape bounding boxes in world space.
2. **Broad phase** — identifies candidate shape pairs whose AABBs overlap.
3. **Narrow phase** — generates contacts for each candidate pair.
You can replace or compose these stages independently.
**Broad phase classes**
All broad phase classes expose a ``launch`` method that writes candidate pairs
(``wp.array(dtype=wp.vec2i)``) and a pair count:
.. list-table::
:header-rows: 1
:widths: 25 75
* - Class
- Description
* - :class:`~geometry.BroadPhaseAllPairs`
- All-pairs O(N²) AABB test. Accepts ``shape_world`` and optional ``shape_flags``.
* - :class:`~geometry.BroadPhaseSAP`
- Sweep-and-prune. Same interface, with optional ``sweep_thread_count_multiplier``
and ``sort_type`` tuning parameters.
* - :class:`~geometry.BroadPhaseExplicit`
- Tests precomputed ``shape_pairs`` against AABBs. No constructor arguments.
.. code-block:: python
from newton.geometry import BroadPhaseSAP
bp = BroadPhaseSAP(model.shape_world, model.shape_flags)
bp.launch(
shape_lower=shape_aabb_lower,
shape_upper=shape_aabb_upper,
shape_gap=model.shape_gap,
shape_collision_group=model.shape_collision_group,
shape_world=model.shape_world,
shape_count=model.shape_count,
candidate_pair=candidate_pair_buffer,
candidate_pair_count=candidate_pair_count,
device=device,
)
**Narrow phase**
:class:`~geometry.NarrowPhase` accepts the candidate pairs from any broad phase and
generates contacts:
.. code-block:: python
from newton.geometry import NarrowPhase
narrow_phase = NarrowPhase(
max_candidate_pairs=10000,
reduce_contacts=True,
device=device,
)
narrow_phase.launch(
candidate_pair=candidate_pairs,
candidate_pair_count=pair_count,
shape_types=...,
shape_data=...,
shape_transform=...,
# ... remaining geometry arrays from Model
contact_pair=out_pairs,
contact_position=out_positions,
contact_normal=out_normals,
contact_penetration=out_depths,
contact_count=out_count,
device=device,
)
To write contacts in a custom format, pass a ``contact_writer_warp_func`` (a Warp
``@wp.func``) to the constructor to define the per-contact write logic, then call
``launch_custom_write`` instead of ``launch``, providing a ``writer_data`` struct that
matches your writer function. Together these give full control over how and where contacts
are stored.
**Primitive collision functions**
For per-pair queries outside the pipeline, ``newton.geometry`` exports Warp device
functions (``@wp.func``) for specific shape combinations:
- ``collide_sphere_sphere``, ``collide_sphere_capsule``, ``collide_sphere_box``,
``collide_sphere_cylinder``
- ``collide_capsule_capsule``, ``collide_capsule_box``
- ``collide_box_box``
- ``collide_plane_sphere``, ``collide_plane_capsule``, ``collide_plane_box``,
``collide_plane_cylinder``, ``collide_plane_ellipsoid``
These return signed distance (negative = penetration), contact position, and contact
normal. Multi-contact variants (e.g., ``collide_box_box``) return fixed-size vectors with
unused slots set to ``MAXVAL``. Because they are ``@wp.func``, they must be called from
within Warp kernels.
**GJK, MPR, and multi-contact generators**
For convex shapes that lack a dedicated ``collide_*`` function, Newton provides
factory functions that create Warp device functions from a support-map interface:
- ``create_solve_mpr(support_func)`` — Minkowski Portal Refinement for boolean
collision and signed distance.
- ``create_solve_closest_distance(support_func)`` — GJK closest-point query.
- ``create_solve_convex_multi_contact(support_func, writer_func, post_process_contact)``
— generates a stable multi-contact manifold and writes results through a callback.
.. note::
These factory functions are internal building blocks available from
``newton._src.geometry``. They are not part of the public API and may change between
releases, but are accessible for advanced users building custom narrow-phase routines.
See Also
--------
**Imports:**
.. testcode:: see-also-imports
import newton
from newton import (
CollisionPipeline,
Contacts,
GeoType,
)
from newton.geometry import (
BroadPhaseAllPairs,
BroadPhaseExplicit,
BroadPhaseSAP,
HydroelasticSDF,
NarrowPhase,
)
**API Reference:**
- :meth:`~Model.contacts` - Create a contacts buffer (accepts ``collision_pipeline=``)
- :meth:`~Model.collide` - Run collision detection (accepts ``collision_pipeline=``, returns :class:`~Contacts`)
- :class:`~CollisionPipeline` - Collision pipeline with configurable broad phase
- ``broad_phase`` - Broad phase algorithm: ``"nxn"``, ``"sap"``, or ``"explicit"``
- :class:`~Contacts` - Contact data container
- :class:`~GeoType` - Shape geometry types
- :class:`~ModelBuilder.ShapeConfig` - Shape configuration options
- :meth:`~ModelBuilder.ShapeConfig.configure_sdf` - Set SDF and hydroelastic options in one call
- :class:`~geometry.HydroelasticSDF.Config` - Hydroelastic contact configuration
- :meth:`~CollisionPipeline.contacts` - Allocate a contacts buffer for a custom pipeline
- :meth:`~Mesh.build_sdf` - Precompute SDF for a mesh
- :meth:`~ModelBuilder.approximate_meshes` - Replace mesh collision shapes with simpler geometry
- :meth:`~ModelBuilder.replicate` - Stamp out multi-world copies of a template builder
- :class:`~geometry.BroadPhaseAllPairs`, :class:`~geometry.BroadPhaseSAP`, :class:`~geometry.BroadPhaseExplicit` - Broad phase implementations
- :class:`~geometry.NarrowPhase` - Narrow phase contact generation
**Model attributes:**
- :attr:`~Model.shape_collision_group` - Per-shape collision groups
- :attr:`~Model.shape_world` - Per-shape world indices
- :attr:`~Model.shape_gap` - Per-shape contact gaps (detection threshold)
- :attr:`~Model.shape_margin` - Per-shape margin values (signed distance padding)
**Related documentation:**
- :doc:`../api/newton_solvers` - Solver API reference (material property behavior per solver)
- :doc:`custom_attributes` - USD custom attributes for collision properties
- :doc:`usd_parsing` - USD import options including collision settings
- :doc:`sites` - Non-colliding reference points
================================================
FILE: docs/concepts/conventions.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Conventions
===========
This document covers the various conventions used across physics engines and graphics frameworks when working with Newton and other simulation systems.
Primer: Reference Points for Rigid-Body Spatial Force and Velocity
------------------------------------------------------------------
Newton uses rigid-body spatial forces (wrenches) and velocities (twists) in its API. These spatial vectors are defined with respect to a **reference point**.
When shifting the reference point, the force and velocity are updated in order to preserve the effect of the wrench, and the velocity field described by the twist, with respect to the new reference point.
The 6D wrench and twist are composed of a linear and an angular 3D-vector component, and in the context of these spatial vectors, their reference-point dependence is as follows:
- **Point-independent components**: linear force :math:`\mathbf{f}`, angular velocity :math:`\boldsymbol{\omega}`.
- **Point-dependent components**: angular torque (moment) :math:`\boldsymbol{\tau}`, linear velocity :math:`\mathbf{v}`.
Shifting the reference point by :math:`\mathbf{r} = (\mathbf{p}_{\text{new}} - \mathbf{p}_{\text{old}})` changes the point-dependent vector components as follows:
.. math::
\boldsymbol{\tau}_{\text{new}} = \boldsymbol{\tau} + \mathbf{r} \times \mathbf{f}, \qquad
\mathbf{v}_{\text{new}} = \mathbf{v} + \boldsymbol{\omega} \times \mathbf{r}.
Keep this distinction in mind below: In addition to the coordinate frame that wrenches and twists are expressed in,
Newton documentation states the **reference point** that it expects. If you compute e.g. a wrench with respect to a different reference point, you must shift it to the expected reference point.
Spatial Twist Conventions
--------------------------
Twists in Modern Robotics
~~~~~~~~~~~~~~~~~~~~~~~~~~
In robotics, a **twist** is a 6-dimensional velocity vector combining angular
and linear velocity. *Modern Robotics* (Lynch & Park) defines two equivalent
representations of a rigid body's twist, depending on the coordinate frame
used:
* **Body twist** (:math:`V_b`):
uses the body's *body frame* (often at the body's center of mass).
Here :math:`\omega_b` is the angular velocity expressed in the body frame,
and :math:`v_b` is the linear velocity of a point at the body origin
(e.g. the COM) expressed in the body frame.
Thus :math:`V_b = (\omega_b,\;v_b)` gives the body's own-frame view of its
motion.
* **Spatial twist** (:math:`V_s`):
uses the fixed *space frame* (world/inertial frame).
:math:`v_s` represents the linear velocity of a hypothetical point on the
moving body that is instantaneously at the world origin, and
:math:`\omega_s` is the angular velocity expressed in world coordinates. Equivalently,
.. math::
v_s \;=\; \dot p \;-\; \omega_s \times p,
where :math:`p` is the vector from the world origin to the body origin.
Hence :math:`V_s = (v_s,\;\omega_s)` is called the **spatial twist**.
*Note:* :math:`v_s` is **not** simply the COM velocity
(that would be :math:`\dot p`); it is the velocity of the *world origin* as
if rigidly attached to the body.
In summary, *Modern Robotics* lets us express the same physical motion either
in the body frame or in the world frame. The angular velocity is identical
up to coordinate rotation; the linear component depends on the chosen
reference point (world origin vs. body origin).
Physics-Engine Conventions (Drake, MuJoCo, Isaac)
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Most physics engines store the **COM linear velocity** together with the
**angular velocity** of the body, typically both in world coordinates. This
corresponds conceptually to a twist taken at the COM and expressed in the
world frame, though details vary:
* **Drake**
Drake's multibody library uses full spatial vectors with explicit frame
names. The default, :math:`V_{MB}^{E}`, reads "velocity of frame *B*
measured in frame *M*, expressed in frame *E*." In normal use
:math:`V_{WB}^{W}` (body *B* in world *W*, expressed in *W*) contains
:math:`(\omega_{WB}^{W},\;v_{WB_o}^{W})`, i.e. both components in the world
frame. This aligns with the usual physics-engine convention.
* **MuJoCo**
MuJoCo employs a *mixed-frame* format for free bodies:
the linear part :math:`(v_x,v_y,v_z)` is the velocity of the **body frame
origin** (i.e., where ``qpos[0:3]`` is located) in the world frame, while the
angular part :math:`(\omega_x,\omega_y,\omega_z)` is expressed in the **body
frame**. The choice follows from quaternion integration (angular velocities
"live" in the quaternion's tangent space, a local frame). Note that when the
body's center of mass (``body_ipos``) is offset from the body frame origin,
the linear velocity is *not* the CoM velocity—see :ref:`MuJoCo conversion `
below for the relationship.
* **Isaac Lab / Isaac Gym**
NVIDIA's Isaac tools provide **both** linear and angular velocities in the
world frame. The root-state tensor returns
:math:`(v_x,v_y,v_z,\;\omega_x,\omega_y,\omega_z)` all expressed globally.
This matches Bullet/ODE/PhysX practice.
.. _Twist conventions:
Newton Conventions
~~~~~~~~~~~~~~~~~~
**Newton** follows the standard physics engine convention for most solvers,
aligning with Isaac Lab's approach.
Newton's public ``spatial_vector`` arrays use ``(linear, angular)`` ordering,
unlike Warp's native ``(angular, linear)`` convention. This applies to arrays
such as :attr:`newton.State.body_qd` and :attr:`newton.State.body_f`.
Newton's :attr:`State.body_qd ` stores **both** linear and angular velocities
in the world frame.
.. code-block:: python
@wp.kernel
def get_body_twist(body_qd: wp.array(dtype=wp.spatial_vector)):
body_id = wp.tid()
# body_qd is a 6D wp.spatial_vector in world frame
twist = body_qd[body_id]
# linear velocity is the velocity of the body's center of mass in world frame
linear_velocity = twist[0:3]
# angular velocity is the angular velocity of the body in world frame
angular_velocity = twist[3:6]
wp.launch(get_body_twist, dim=model.body_count, inputs=[state.body_qd])
The linear velocity represents the COM velocity in world
coordinates, while the angular velocity is also expressed in world coordinates.
This matches the Isaac Lab convention exactly. Note that Newton will automatically
convert from this convention to MuJoCo's mixed-frame format when using the
SolverMuJoCo, including both the angular velocity frame conversion (world ↔ body)
and the linear velocity reference point conversion (CoM ↔ body frame origin).
Summary of Conventions
~~~~~~~~~~~~~~~~~~~~~~
.. list-table::
:header-rows: 1
:widths: 28 27 27 18
* - **System**
- **Linear velocity (translation)**
- **Angular velocity (rotation)**
- **Twist term**
* - *Modern Robotics* — **Body twist**
- Body origin (chosen point; often COM), body frame
- Body frame
- "Body twist" (:math:`V_b`)
* - *Modern Robotics* — **Spatial twist**
- World origin, world frame
- World frame
- "Spatial twist" (:math:`V_s`)
* - **Drake**
- Body-frame origin :math:`B_o` (not necessarily COM), world frame
- World frame
- Spatial velocity :math:`V_{WB}^{W}`
* - **MuJoCo**
- Body-frame origin, world frame
- Body frame
- Mixed-frame 6-vector
* - **Isaac Gym / Sim**
- COM, world frame
- World frame
- "Root" linear/angular velocity
* - **PhysX**
- COM, world frame
- World frame
- Not named "twist"; typically treated as :math:`[\mathbf{v}_{com}^W;\ \boldsymbol{\omega}^W]`
* - **Newton** (except the Featherstone solver; see below)
- COM, world frame
- World frame
- :attr:`~newton.State.body_qd`
.. warning::
:class:`~newton.solvers.SolverFeatherstone` does not correctly handle angular velocity
for free-floating bodies with **non-zero center of mass offsets**. The body may not
rotate purely about its CoM.
This issue is tracked at https://github.com/newton-physics/newton/issues/1366.
Mapping Between Representations
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
**Body ↔ Spatial (Modern Robotics)**
For body pose :math:`T_{sb}=(R,p)`:
.. math::
\omega_s \;=\; R\,\omega_b,
\qquad
v_s \;=\; R\,v_b \;+\; \omega_s \times p.
This is :math:`V_s = \mathrm{Ad}_{(R,p)}\,V_b`;
the inverse uses :math:`R^{\mathsf T}` and :math:`-R^{\mathsf T}p`.
**Physics engine → MR**
Given engine values :math:`(v_{\text{com}}^{W},\;\omega^{W})`
(world-frame COM velocity and angular velocity):
1. Spatial twist at COM
:math:`V_{WB}^{W} = (v_{\text{com}}^{W},\;\omega^{W})`
2. Body-frame twist
:math:`\omega_b = R^{\mathsf T}\omega^{W}`,
:math:`v_b = R^{\mathsf T}v_{\text{com}}^{W}`.
3. Shift to another origin offset :math:`r` from COM:
:math:`v_{\text{origin}}^{W} = v_{\text{com}}^{W} + \omega^{W}\times r^{W}`,
where :math:`r^{W}=R\,r`.
.. _MuJoCo conversion:
**MuJoCo conversion**
Two conversions are needed between Newton and MuJoCo:
1. **Angular velocity frame**: Rotate MuJoCo's body-frame angular velocity by
:math:`R` to obtain the world-frame angular velocity (or vice versa):
.. math::
\omega^{W} = R\,\omega^{B}, \qquad \omega^{B} = R^{\mathsf T}\omega^{W}
2. **Linear velocity reference point**: MuJoCo's linear velocity is at the
body frame origin, while Newton uses the CoM velocity. When the body has a
non-zero CoM offset :math:`r` (``body_ipos`` in MuJoCo, ``body_com`` in
Newton), convert using:
.. math::
v_{\text{origin}}^{W} = v_{\text{com}}^{W} - \omega^{W} \times r^{W},
\qquad
v_{\text{com}}^{W} = v_{\text{origin}}^{W} + \omega^{W} \times r^{W}
where :math:`r^{W} = R\,r^{B}` is the CoM offset expressed in world coordinates.
In all cases the conversion boils down to the **reference point**
(COM vs. another point) and the **frame** (world vs. body) used for each
component. Physics is unchanged; any linear velocity at one point follows
:math:`v_{\text{new}} = v + \omega\times r`.
Spatial Wrench Conventions
--------------------------
Newton represents external rigid-body forces as **spatial wrenches** in
:attr:`State.body_f `. The 6D wrench is stored in world
frame as:
.. math::
\mathbf{w} = \begin{bmatrix} \mathbf{f} \\ \boldsymbol{\tau} \end{bmatrix},
where :math:`\mathbf{f}` is the **linear force** and :math:`\boldsymbol{\tau}`
is the **moment about the body's center of mass (COM)**, both expressed in
world coordinates. The reference point matters for the moment term, so shifting
the wrench to a point offset by :math:`\mathbf{r}` changes the torque as:
.. math::
\boldsymbol{\tau}_{\text{new}} = \boldsymbol{\tau} + \mathbf{r} \times \mathbf{f}.
This convention is used in all Newton solvers, except for :class:`~newton.solvers.SolverFeatherstone`, which does not correctly handle torque application for free-floating bodies with **non-zero center of mass offsets**.
.. warning::
:class:`~newton.solvers.SolverFeatherstone` does not correctly handle torque application
for free-floating bodies with **non-zero center of mass offsets**. A pure torque will
incorrectly cause the CoM to translate instead of remaining stationary.
This issue is tracked at https://github.com/newton-physics/newton/issues/1366.
The array of joint forces (torques) in generalized coordinates is stored in :attr:`Control.joint_f `.
For free joints, the corresponding 6 dimensions in this array are the spatial wrench applied at the body's center of mass (COM)
in world frame, following exactly the same convention as :attr:`State.body_f `.
.. note::
MuJoCo represents free-joint generalized forces in a mixed-frame convention in ``qfrc_applied``. To preserve Newton's
COM-wrench semantics, :class:`~newton.solvers.SolverMuJoCo` applies free-joint
:attr:`Control.joint_f ` through ``xfrc_applied`` (world-frame wrench at the COM) and
uses ``qfrc_applied`` only for non-free joints. This keeps free-joint ``joint_f`` behavior aligned with
:attr:`State.body_f `.
We avoid converting free-joint wrenches into ``qfrc_applied`` directly because ``qfrc_applied`` is **generalized-force
space**, not a physical wrench. For free joints the 6-DOF basis depends on the current ``cdof`` (subtree COM frame),
and the rotational components are expressed in the body frame. A naive world-to-body rotation is insufficient because
the correct mapping is the Jacobian-transpose operation used internally by MuJoCo (the same path as ``xfrc_applied``).
Routing through ``xfrc_applied`` ensures the wrench is interpreted at the COM in world coordinates and then mapped to
generalized forces consistently with MuJoCo's own dynamics.
Quaternion Ordering Conventions
--------------------------------
Different physics engines and graphics frameworks use different conventions
for storing quaternion components. This can cause significant confusion when
transferring data between systems or when interfacing with external libraries.
The quaternion :math:`q = w + xi + yj + zk` where :math:`w` is the scalar
(real) part and :math:`(x, y, z)` is the vector (imaginary) part, can be
stored in memory using different orderings:
.. list-table:: Quaternion Component Ordering
:header-rows: 1
:widths: 30 35 35
* - **System**
- **Storage Order**
- **Description**
* - **Newton / Warp**
- ``(x, y, z, w)``
- Vector part first, scalar last
* - **Isaac Lab / Isaac Sim**
- ``(w, x, y, z)``
- Scalar first, vector part last
* - **MuJoCo**
- ``(w, x, y, z)``
- Scalar first, vector part last
* - **USD (Universal Scene Description)**
- ``(x, y, z, w)``
- Vector part first, scalar last
**Important Notes:**
* **Mathematical notation** typically writes quaternions as :math:`q = w + xi + yj + zk`
or :math:`q = (w, x, y, z)`, but this doesn't dictate storage order.
* **Conversion between systems** requires careful attention to component ordering.
For example, converting from Isaac Lab to Newton requires reordering:
``newton_quat = (isaac_quat[1], isaac_quat[2], isaac_quat[3], isaac_quat[0])``
* **Rotation semantics** remain the same regardless of storage order—only the
memory layout differs.
* **Warp's quat type** uses ``(x, y, z, w)`` ordering, accessible via:
``quat[0]`` (x), ``quat[1]`` (y), ``quat[2]`` (z), ``quat[3]`` (w).
When working with multiple systems, always verify quaternion ordering in your
data pipeline to avoid unexpected rotations or orientations.
Coordinate System and Up Axis Conventions
------------------------------------------
Different physics engines, graphics frameworks, and content creation tools use
different conventions for coordinate systems and up axis orientation. This can
cause significant confusion when transferring assets between systems or when
setting up physics simulations from existing content.
The **up axis** determines which coordinate axis points "upward" in the world,
affecting gravity direction, object placement, and overall scene orientation.
.. list-table:: Coordinate System and Up Axis Conventions
:header-rows: 1
:widths: 30 20 25 25
* - **System**
- **Up Axis**
- **Handedness**
- **Notes**
* - **Newton**
- ``Z`` (default)
- Right-handed
- Configurable via ``Axis.X/Y/Z``
* - **MuJoCo**
- ``Z`` (default)
- Right-handed
- Standard robotics convention
* - **USD**
- ``Y`` (default)
- Right-handed
- Configurable as ``Y`` or ``Z``
* - **Isaac Lab / Isaac Sim**
- ``Z`` (default)
- Right-handed
- Follows robotics conventions
**Important Design Principle:**
Newton itself is **coordinate system agnostic** and can work with any choice
of up axis. The physics calculations and algorithms do not depend on a specific
coordinate system orientation. However, it becomes essential to track the
conventions used by various assets and data sources to enable proper conversion
and integration at runtime.
**Common Integration Scenarios:**
* **USD to Newton**: Convert from USD's Y-up (or Z-up) to Newton's configured up axis
* **MuJoCo to Newton**: Convert from MuJoCo's Z-up to Newton's configured up axis
* **Mixed asset pipelines**: Track up axis per asset and apply appropriate transforms
**Conversion Between Systems:**
When converting assets between coordinate systems with different up axes,
apply the appropriate rotation transforms:
* **Y-up ↔ Z-up**: 90° rotation around the X-axis
* **Maintain right-handedness**: Ensure coordinate system handedness is preserved
**Example Configuration:**
.. code-block:: python
import newton
# Configure Newton for Z-up coordinate system (robotics convention)
builder = newton.ModelBuilder(up_axis=newton.Axis.Z, gravity=-9.81)
# Or use Y-up (graphics/animation convention)
builder = newton.ModelBuilder(up_axis=newton.Axis.Y, gravity=-9.81)
# Gravity vector will automatically align with the chosen up axis:
# - Y-up: gravity = (0, -9.81, 0)
# - Z-up: gravity = (0, 0, -9.81)
Collision Primitive Conventions
-------------------------------
This section documents the conventions used for collision primitive shapes in Newton and compares them with other physics engines and formats. Understanding these conventions is essential when:
* Creating collision geometry programmatically with ModelBuilder
* Debugging unexpected collision behavior after asset import
* Understanding center of mass calculations for asymmetric shapes
Newton Collision Primitives
~~~~~~~~~~~~~~~~~~~~~~~~~~~
Newton defines collision primitives with consistent conventions across all shape types. The following table summarizes the key parameters and properties for each primitive:
.. list-table:: Newton Collision Primitive Specifications
:header-rows: 1
:widths: 15 20 35 30
* - **Shape**
- **Origin**
- **Parameters**
- **Notes**
* - **Box**
- Geometric center
- ``hx``, ``hy``, ``hz`` (half-extents)
- Edges aligned with local axes
* - **Sphere**
- Center
- ``radius``
- Uniform in all directions
* - **Capsule**
- Geometric center
- ``radius``, ``half_height``
- Extends along Z-axis; half_height excludes hemispherical caps
* - **Cylinder**
- Geometric center
- ``radius``, ``half_height``
- Extends along Z-axis
* - **Cone**
- Geometric center
- ``radius`` (base), ``half_height``
- Extends along Z-axis; base at -half_height, apex at +half_height
* - **Plane**
- Shape frame origin
- ``width``, ``length`` (or 0,0 for infinite)
- Normal along +Z of shape frame
* - **Mesh**
- User-defined
- Vertex and triangle arrays
- General triangle mesh (can be non-convex)
**Shape Orientation and Alignment**
All Newton primitives that have a primary axis (capsule, cylinder, cone) are aligned along the Z-axis in their local coordinate frame. The shape's transform determines its final position and orientation in the world or parent body frame.
**Center of Mass Considerations**
For most primitives, the center of mass coincides with the geometric origin. The cone is a notable exception:
* **Cone COM**: Located at ``(0, 0, -half_height/2)`` in the shape's local frame, which is 1/4 of the total height from the base toward the apex.
Collision Primitive Conventions Across Engines
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
The following tables compare how different engines and formats define common collision primitives:
**Sphere Primitives**
.. list-table::
:header-rows: 1
:widths: 25 35 40
* - **System**
- **Parameter Convention**
- **Notes**
* - **Newton**
- ``radius``
- Origin at center
* - **MuJoCo**
- ``size[0]`` = radius
- Origin at center
* - **USD (UsdGeomSphere)**
- ``radius`` attribute
- Origin at center
* - **USD Physics**
- ``radius`` attribute
- Origin at center
**Box Primitives**
.. list-table::
:header-rows: 1
:widths: 25 35 40
* - **System**
- **Parameter Convention**
- **Notes**
* - **Newton**
- Half-extents (``hx``, ``hy``, ``hz``)
- Distance from center to face
* - **MuJoCo**
- Half-sizes in ``size`` attribute
- Can use ``fromto`` (Newton importer doesn't support)
* - **USD (UsdGeomCube)**
- ``size`` attribute (full dimensions)
- Edge length, not half-extent
* - **USD Physics**
- ``halfExtents`` attribute
- Matches Newton convention
**Capsule Primitives**
.. list-table::
:header-rows: 1
:widths: 25 35 40
* - **System**
- **Parameter Convention**
- **Notes**
* - **Newton**
- ``radius``, ``half_height`` (excludes caps)
- Total length = 2*(radius + half_height)
* - **MuJoCo**
- ``size[0]`` = radius, ``size[1]`` = half-length (excludes caps)
- Can also use ``fromto`` for endpoints
* - **USD (UsdGeomCapsule)**
- ``radius``, ``height`` (excludes caps)
- Full height of cylindrical portion
* - **USD Physics**
- ``radius``, ``halfHeight`` (excludes caps)
- Similar to Newton
**Cylinder Primitives**
.. list-table::
:header-rows: 1
:widths: 25 35 40
* - **System**
- **Parameter Convention**
- **Notes**
* - **Newton**
- ``radius``, ``half_height``
- Extends along Z-axis
* - **MuJoCo**
- ``size[0]`` = radius, ``size[1]`` = half-length
- Can use ``fromto``; Newton's MJCF importer maps to capsule
* - **USD (UsdGeomCylinder)**
- ``radius``, ``height`` (full height)
- Visual shape
* - **USD Physics**
- ``radius``, ``halfHeight``
- Newton's USD importer creates actual cylinders
**Cone Primitives**
.. list-table::
:header-rows: 1
:widths: 25 35 40
* - **System**
- **Parameter Convention**
- **Notes**
* - **Newton**
- ``radius`` (base), ``half_height``
- COM offset at -half_height/2
* - **MuJoCo**
- Not supported
- N/A
* - **USD (UsdGeomCone)**
- ``radius``, ``height`` (full height)
- Visual representation
* - **USD Physics**
- ``radius``, ``halfHeight``
- Physics representation
**Plane Primitives**
.. list-table::
:header-rows: 1
:widths: 25 35 40
* - **System**
- **Definition Method**
- **Normal Direction**
* - **Newton**
- Transform-based or plane equation
- +Z of shape frame
* - **MuJoCo**
- Size and orientation in body frame
- +Z of geom frame
* - **USD**
- No standard plane primitive
- Implementation-specific
**Mesh Primitives**
.. list-table::
:header-rows: 1
:widths: 25 35 40
* - **System**
- **Mesh Type**
- **Notes**
* - **Newton**
- General triangle mesh
- Can be non-convex
* - **MuJoCo**
- Convex hull only for collision
- Visual mesh can be non-convex
* - **USD (UsdGeomMesh)**
- General polygon mesh
- Visual representation
* - **USD Physics**
- Implementation-dependent
- May use convex approximation
Import Handling
~~~~~~~~~~~~~~~
Newton's importers automatically handle convention differences when loading assets. No manual conversion is required when using these importers—they automatically transform shapes to Newton's conventions.
================================================
FILE: docs/concepts/custom_attributes.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. currentmodule:: newton
.. _custom_attributes:
Custom Attributes
=================
Newton's simulation model uses flat buffer arrays to represent physical properties and simulation state. These arrays can be extended with user-defined custom attributes to store application-specific data alongside the standard physics quantities.
Use Cases
---------
Custom attributes enable a wide range of simulation extensions:
* **Per-body properties**: Store thermal properties, material composition, sensor IDs, or hardware specifications
* **Advanced control**: Store PD gains, velocity limits, control modes, or actuator parameters per-joint or per-DOF
* **Visualization**: Attach colors, labels, rendering properties, or UI metadata to simulation entities
* **Multi-physics coupling**: Store quantities like surface stress, temperature fields, or electromagnetic properties
* **Reinforcement learning**: Store observation buffers, reward weights, optimization parameters, or policy-specific data directly on entities
* **Solver-specific data**: Store contact pair parameters, tendon properties, or other solver-specific entity types
Custom attributes follow Newton's flat array indexing scheme, enabling efficient GPU-parallel access while maintaining flexibility for domain-specific extensions.
Overview
--------
Newton organizes simulation data into four primary objects, each containing flat arrays indexed by simulation entities:
* **Model Object** (:class:`~newton.Model`) - Static configuration and physical properties that remain constant during simulation
* **State Object** (:class:`~newton.State`) - Dynamic quantities that evolve during simulation
* **Control Object** (:class:`~newton.Control`) - Control inputs and actuator commands
* **Contact Object** (:class:`~newton.Contacts`) - Contact-specific properties
Custom attributes extend these objects with user-defined arrays that follow the same indexing scheme as Newton's built-in attributes. The ``CONTACT`` assignment attaches attributes to the :class:`~newton.Contacts` object created during collision detection.
Declaring Custom Attributes
----------------------------
Custom attributes must be declared before use via the :meth:`newton.ModelBuilder.add_custom_attribute` method. Each declaration specifies:
* **name**: Attribute name
* **frequency**: Determines array size and indexing—either a :class:`~newton.Model.AttributeFrequency` enum value (e.g., ``BODY``, ``SHAPE``, ``JOINT``, ``JOINT_DOF``, ``JOINT_COORD``, ``ARTICULATION``, ``ONCE``) or a string for custom frequencies
* **dtype**: Warp data type (``wp.float32``, ``wp.vec3``, ``wp.quat``, etc.) or ``str`` for string attributes stored as Python lists
* **assignment**: Which simulation object owns the attribute (``MODEL``, ``STATE``, ``CONTROL``, ``CONTACT``)
* **default** (optional): Default value for unspecified entities. When omitted, a sensible zero-value is derived from the dtype (``0`` for scalars, identity for quaternions, ``False`` for booleans, ``""`` for strings)
* **namespace** (optional): Hierarchical organization for grouping related attributes
* **references** (optional): For multi-world merging, specifies how values are transformed (e.g., ``"body"``, ``"shape"``, ``"world"``, or a custom frequency key)
* **values** (optional): Pre-populated values — ``dict[int, Any]`` for enum frequencies or ``list[Any]`` for custom string frequencies
When **no namespace** is specified, attributes are added directly to their assignment object (e.g., ``model.temperature``). When a **namespace** is provided, Newton creates a namespace container (e.g., ``model.mujoco.damping``).
.. testcode::
from newton import Model, ModelBuilder
import warp as wp
builder = ModelBuilder()
# Default namespace attributes - added directly to assignment objects
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="temperature",
frequency=Model.AttributeFrequency.BODY,
dtype=wp.float32,
default=20.0, # Explicit default value
assignment=Model.AttributeAssignment.MODEL
)
)
# → Accessible as: model.temperature
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="velocity_limit",
frequency=Model.AttributeFrequency.BODY,
dtype=wp.vec3,
default=(1.0, 1.0, 1.0), # Default vector value
assignment=Model.AttributeAssignment.STATE
)
)
# → Accessible as: state.velocity_limit
# Namespaced attributes - organized under namespace containers
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="float_attr",
frequency=Model.AttributeFrequency.BODY,
dtype=wp.float32,
default=0.5,
assignment=Model.AttributeAssignment.MODEL,
namespace="namespace_a"
)
)
# → Accessible as: model.namespace_a.float_attr
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="bool_attr",
frequency=Model.AttributeFrequency.SHAPE,
dtype=wp.bool,
default=False,
assignment=Model.AttributeAssignment.MODEL,
namespace="namespace_a"
)
)
# → Accessible as: model.namespace_a.bool_attr
# Articulation frequency attributes - one value per articulation
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="articulation_stiffness",
frequency=Model.AttributeFrequency.ARTICULATION,
dtype=wp.float32,
default=100.0,
assignment=Model.AttributeAssignment.MODEL
)
)
# → Accessible as: model.articulation_stiffness
# ONCE frequency attributes - a single global value
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="gravity_scale",
frequency=Model.AttributeFrequency.ONCE,
dtype=wp.float32,
default=1.0,
assignment=Model.AttributeAssignment.MODEL
)
)
# → Accessible as: model.gravity_scale (array of length 1)
# String dtype attributes - stored as Python lists, not Warp arrays
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="body_description",
frequency=Model.AttributeFrequency.BODY,
dtype=str,
default="unnamed",
assignment=Model.AttributeAssignment.MODEL
)
)
# → Accessible as: model.body_description (Python list[str])
**Default Value Behavior:**
When entities don't explicitly specify custom attribute values, the default value is used:
.. testcode::
# First body uses the default value (20.0)
body1 = builder.add_body(mass=1.0)
# Second body overrides with explicit value
body2 = builder.add_body(
mass=1.0,
custom_attributes={"temperature": 37.5}
)
# Articulation attributes: create articulations with custom values
# Each add_articulation creates one articulation at the next index
for i in range(3):
base = builder.add_link(mass=1.0)
joint = builder.add_joint_free(child=base)
builder.add_articulation(
joints=[joint],
custom_attributes={
"articulation_stiffness": 100.0 + float(i) * 50.0 # 100, 150, 200
}
)
# After finalization, access attributes
model = builder.finalize()
temps = model.temperature.numpy()
arctic_stiff = model.articulation_stiffness.numpy()
print(f"Body 1: {temps[body1]}") # 20.0 (default)
print(f"Body 2: {temps[body2]}") # 37.5 (authored)
# Articulation indices reflect all articulations in the model
# (including any implicit ones from add_body)
print(f"Articulations: {len(arctic_stiff)}")
print(f"Last articulation stiffness: {arctic_stiff[-1]}") # 200.0
.. testoutput::
Body 1: 20.0
Body 2: 37.5
Articulations: 5
Last articulation stiffness: 200.0
.. note::
Uniqueness is determined by the full identifier (namespace + name):
- ``model.float_attr`` (key: ``"float_attr"``) and ``model.namespace_a.float_attr`` (key: ``"namespace_a:float_attr"``) can coexist
- ``model.float_attr`` (key: ``"float_attr"``) and ``state.namespace_a.float_attr`` (key: ``"namespace_a:float_attr"``) can coexist
- ``model.float_attr`` (key: ``"float_attr"``) and ``state.float_attr`` (key: ``"float_attr"``) cannot coexist - same key
- ``model.namespace_a.float_attr`` and ``state.namespace_a.float_attr`` cannot coexist - same key ``"namespace_a:float_attr"``
**Registering Solver Attributes:**
Before loading assets, register solver-specific attributes:
.. testcode:: custom-attrs-solver
from newton import ModelBuilder
from newton.solvers import SolverMuJoCo
builder_mujoco = ModelBuilder()
SolverMuJoCo.register_custom_attributes(builder_mujoco)
# Now build your scene...
body = builder_mujoco.add_link()
joint = builder_mujoco.add_joint_free(body)
builder_mujoco.add_articulation([joint])
shape = builder_mujoco.add_shape_box(body=body, hx=0.1, hy=0.1, hz=0.1)
model_mujoco = builder_mujoco.finalize()
assert hasattr(model_mujoco, "mujoco")
assert hasattr(model_mujoco.mujoco, "condim")
MuJoCo boolean custom attributes use a ``parse_bool`` transformer (registered by :meth:`~newton.solvers.SolverMuJoCo.register_custom_attributes`) that handles strings (``"true"``/``"false"``), integers, and native booleans.
Authoring Custom Attributes
----------------------------
After declaration, values are assigned through the standard entity creation API (``add_body``, ``add_shape``, ``add_joint``). For default namespace attributes, use the attribute name directly. For namespaced attributes, use the format ``"namespace:attr_name"``.
.. testcode::
# Create a body with both default and namespaced attributes
body_id = builder.add_body(
mass=1.0,
custom_attributes={
"temperature": 37.5, # default → model.temperature
"velocity_limit": [2.0, 2.0, 2.0], # default → state.velocity_limit
"namespace_a:float_attr": 0.5, # namespaced → model.namespace_a.float_attr
}
)
# Create a shape with a namespaced attribute
shape_id = builder.add_shape_box(
body=body_id,
hx=0.1, hy=0.1, hz=0.1,
custom_attributes={
"namespace_a:bool_attr": True, # → model.namespace_a.bool_attr
}
)
**Joint Frequency Types:**
For joints, Newton provides three frequency types to store different granularities of data:
* **JOINT frequency** → One value per joint
* **JOINT_DOF frequency** → Values per degree of freedom (list, dict, or scalar for single-DOF joints)
* **JOINT_COORD frequency** → Values per position coordinate (list, dict, or scalar for single-coordinate joints)
For ``JOINT_DOF`` and ``JOINT_COORD`` frequencies, values can be provided in three formats:
1. **List format**: Explicit values for all DOFs/coordinates (e.g., ``[100.0, 200.0]`` for 2-DOF joint)
2. **Dict format**: Sparse specification mapping indices to values (e.g., ``{0: 100.0, 2: 300.0}`` sets only DOF 0 and 2)
3. **Scalar format**: Single value for single-DOF/single-coordinate joints, automatically expanded to a list
.. testcode::
# Declare joint attributes with different frequencies
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="int_attr",
frequency=Model.AttributeFrequency.JOINT,
dtype=wp.int32
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="float_attr_dof",
frequency=Model.AttributeFrequency.JOINT_DOF,
dtype=wp.float32
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="float_attr_coord",
frequency=Model.AttributeFrequency.JOINT_COORD,
dtype=wp.float32
)
)
# Create a D6 joint with 2 DOFs (1 linear + 1 angular) and 2 coordinates
parent = builder.add_link(mass=1.0)
child = builder.add_link(mass=1.0)
cfg = ModelBuilder.JointDofConfig
joint_id = builder.add_joint_d6(
parent=parent,
child=child,
linear_axes=[cfg(axis=[1, 0, 0])], # 1 linear DOF
angular_axes=[cfg(axis=[0, 0, 1])], # 1 angular DOF
custom_attributes={
"int_attr": 5, # JOINT frequency: single value
"float_attr_dof": [100.0, 200.0], # JOINT_DOF frequency: list with 2 values (one per DOF)
"float_attr_coord": [0.5, 0.7], # JOINT_COORD frequency: list with 2 values (one per coordinate)
}
)
builder.add_articulation([joint_id])
# Scalar format for single-DOF joints (automatically expanded to list)
parent2 = builder.add_link(mass=1.0)
child2 = builder.add_link(mass=1.0)
revolute_joint = builder.add_joint_revolute(
parent=parent2,
child=child2,
axis=[0, 0, 1],
custom_attributes={
"float_attr_dof": 150.0, # Scalar for 1-DOF joint (expanded to [150.0])
"float_attr_coord": 0.8, # Scalar for 1-coord joint (expanded to [0.8])
}
)
builder.add_articulation([revolute_joint])
# Dict format for sparse specification (only set specific DOF/coord indices)
parent3 = builder.add_link(mass=1.0)
child3 = builder.add_link(mass=1.0)
d6_joint = builder.add_joint_d6(
parent=parent3,
child=child3,
linear_axes=[cfg(axis=[1, 0, 0]), cfg(axis=[0, 1, 0])], # 2 linear DOFs
angular_axes=[cfg(axis=[0, 0, 1])], # 1 angular DOF
custom_attributes={
"float_attr_dof": {0: 100.0, 2: 300.0}, # Dict: only DOF 0 and 2 specified
}
)
builder.add_articulation([d6_joint])
Accessing Custom Attributes
----------------------------
After finalization, custom attributes become accessible as Warp arrays. Default namespace attributes are accessed directly on their assignment object, while namespaced attributes are accessed through their namespace container.
.. testcode::
# Finalize the model
model = builder.finalize()
state = model.state()
# Access default namespace attributes (direct access on assignment objects)
temperatures = model.temperature.numpy()
velocity_limits = state.velocity_limit.numpy()
print(f"Temperature: {temperatures[body_id]}")
print(f"Velocity limit: {velocity_limits[body_id]}")
# Access namespaced attributes (via namespace containers)
namespace_a_body_floats = model.namespace_a.float_attr.numpy()
namespace_a_shape_bools = model.namespace_a.bool_attr.numpy()
print(f"Namespace A body float: {namespace_a_body_floats[body_id]}")
print(f"Namespace A shape bool: {bool(namespace_a_shape_bools[shape_id])}")
.. testoutput::
Temperature: 37.5
Velocity limit: [2. 2. 2.]
Namespace A body float: 0.5
Namespace A shape bool: True
Custom attributes follow the same GPU/CPU synchronization rules as built-in attributes and can be modified during simulation.
USD Integration
---------------
Custom attributes can be authored in USD files using a declaration-first pattern, similar to the Python API. Declarations are placed on the PhysicsScene prim, and individual prims can then assign values to these attributes.
**Declaration Format (on PhysicsScene prim):**
.. code-block:: usda
def PhysicsScene "physicsScene" {
# Default namespace attributes
custom float newton:float_attr = 0.0 (
customData = {
string assignment = "model"
string frequency = "body"
}
)
custom float3 newton:vec3_attr = (0.0, 0.0, 0.0) (
customData = {
string assignment = "state"
string frequency = "body"
}
)
# ARTICULATION frequency attribute
custom float newton:articulation_stiffness = 100.0 (
customData = {
string assignment = "model"
string frequency = "articulation"
}
)
# Custom namespace attributes
custom float newton:namespace_a:some_attrib = 150.0 (
customData = {
string assignment = "control"
string frequency = "joint_dof"
}
)
custom bool newton:namespace_a:bool_attr = false (
customData = {
string assignment = "model"
string frequency = "shape"
}
)
}
**Assignment Format (on individual prims):**
.. code-block:: usda
def Xform "robot_arm" (
prepend apiSchemas = ["PhysicsRigidBodyAPI"]
) {
# Override declared attributes with custom values
custom float newton:float_attr = 850.0
custom float3 newton:vec3_attr = (1.0, 0.5, 0.3)
custom float newton:namespace_a:some_attrib = 250.0
}
def Mesh "gripper" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsCollisionAPI"]
) {
custom bool newton:namespace_a:bool_attr = true
}
After importing the USD file, attributes are accessible following the same patterns as programmatically declared attributes:
.. testcode::
:skipif: True
from newton import ModelBuilder
builder_usd = ModelBuilder()
builder_usd.add_usd("robot_arm.usda")
model = builder_usd.finalize()
state = model.state()
control = model.control()
# Access default namespace attributes
float_values = model.float_attr.numpy()
vec3_values = state.vec3_attr.numpy()
# Access namespaced attributes
namespace_a_floats = control.namespace_a.some_attrib.numpy()
namespace_a_bools = model.namespace_a.bool_attr.numpy()
For more information about USD integration and the schema resolver system, see :doc:`usd_parsing`.
MJCF and URDF Integration
--------------------------
Custom attributes can also be parsed from MJCF and URDF files. Each :class:`~newton.ModelBuilder.CustomAttribute` has optional fields for controlling how values are extracted from these formats:
* :attr:`~newton.ModelBuilder.CustomAttribute.mjcf_attribute_name` — name of the XML attribute to read (defaults to the attribute ``name``)
* :attr:`~newton.ModelBuilder.CustomAttribute.mjcf_value_transformer` — callable that converts the XML string value to the target dtype
* :attr:`~newton.ModelBuilder.CustomAttribute.urdf_attribute_name` — name of the XML attribute to read (defaults to the attribute ``name``)
* :attr:`~newton.ModelBuilder.CustomAttribute.urdf_value_transformer` — callable that converts the XML string value to the target dtype
These are primarily used by solver integrations (e.g., :meth:`~newton.solvers.SolverMuJoCo.register_custom_attributes` registers MJCF transformers for MuJoCo-specific attributes like ``condim``, ``priority``, and ``solref``). When no transformer is provided, values are parsed using a generic string-to-Warp converter.
.. code-block:: python
# Example: register an attribute that reads "damping" from MJCF joint elements
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="custom_damping",
frequency=Model.AttributeFrequency.JOINT_DOF,
dtype=wp.float32,
default=0.0,
namespace="myns",
mjcf_attribute_name="damping", # reads
)
)
Validation and Constraints
---------------------------
The custom attribute system enforces several constraints to ensure correctness:
* Attributes must be declared via ``add_custom_attribute()`` before use (raises ``AttributeError`` otherwise)
* Each attribute must be used with entities matching its declared frequency (raises ``ValueError`` otherwise)
* Each full attribute identifier (namespace + name) can only be declared once with a specific assignment, frequency, and dtype
* The same attribute name can exist in different namespaces because they create different full identifiers
Custom Frequencies
==================
While enum frequencies (``BODY``, ``SHAPE``, ``JOINT``, etc.) cover most use cases, some data structures have counts independent of built-in entity types. Custom frequencies address this by allowing a string instead of an enum for the :attr:`~newton.ModelBuilder.CustomAttribute.frequency` parameter.
**Example use case:** MuJoCo's ```` elements define contact pairs between geometries. These pairs have their own count independent of bodies or shapes, and their indices must be remapped when merging worlds.
Registering Custom Frequencies
------------------------------
Custom frequencies must be **registered before use** via :meth:`~newton.ModelBuilder.add_custom_frequency` using a :class:`~newton.ModelBuilder.CustomFrequency` object. This explicit registration ensures clarity about which entity types exist and enables optional USD parsing support.
.. testsetup:: custom-freqs
from newton import Model, ModelBuilder
builder = ModelBuilder()
.. testcode:: custom-freqs
# Register a custom frequency
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="item",
namespace="myns",
)
)
The frequency key follows the same namespace rules as attribute keys: if a namespace is provided, it is prepended to the name (e.g., ``"mujoco:pair"``). When declaring a custom attribute, the :attr:`~newton.ModelBuilder.CustomAttribute.frequency` string must match this full key.
Declaring Custom Frequency Attributes
-------------------------------------
Once a custom frequency is registered, pass a string instead of an enum for the :attr:`~newton.ModelBuilder.CustomAttribute.frequency` parameter when adding attributes:
.. testcode:: custom-freqs
# First register the custom frequency
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(name="pair", namespace="mujoco")
)
# Then add attributes using that frequency
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_geom1",
frequency="mujoco:pair", # Custom frequency (string)
dtype=wp.int32,
namespace="mujoco",
)
)
.. note::
Attempting to add an attribute with an unregistered custom frequency will raise a ``ValueError``.
Adding Values
-------------
Custom frequency values are appended using :meth:`~newton.ModelBuilder.add_custom_values`:
.. testcode:: custom-freqs
# Declare attributes sharing the "myns:item" frequency
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(name="item_id", frequency="myns:item", dtype=wp.int32, namespace="myns")
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(name="item_value", frequency="myns:item", dtype=wp.float32, namespace="myns")
)
# Append values together
builder.add_custom_values(**{
"myns:item_id": 100,
"myns:item_value": 2.5,
})
builder.add_custom_values(**{
"myns:item_id": 101,
"myns:item_value": 3.0,
})
# Finalize (requires at least one articulation)
_body = builder.add_link()
_joint = builder.add_joint_free(_body)
builder.add_articulation([_joint])
model = builder.finalize()
print(model.myns.item_id.numpy())
print(model.myns.item_value.numpy())
.. testoutput:: custom-freqs
[100 101]
[2.5 3. ]
For convenience, :meth:`~newton.ModelBuilder.add_custom_values_batch` appends multiple rows in a single call:
.. code-block:: python
builder.add_custom_values_batch([
{"myns:item_id": 100, "myns:item_value": 2.5},
{"myns:item_id": 101, "myns:item_value": 3.0},
])
**Validation:** All attributes sharing a custom frequency must have the same count at ``finalize()`` time. This catches synchronization bugs early.
USD Parsing Support
-------------------
Custom frequencies can support automatic USD parsing:
In this section, a *row* means one appended set of values for a custom frequency
(that is, one index entry across all attributes in that frequency, equivalent to
one call to :meth:`~newton.ModelBuilder.add_custom_values`).
* :attr:`~newton.ModelBuilder.CustomFrequency.usd_prim_filter` selects which prims should emit rows.
* :attr:`~newton.ModelBuilder.CustomFrequency.usd_entry_expander` (optional) expands one prim into multiple rows.
.. code-block:: python
def is_actuator_prim(prim, context):
"""Return True for prims with type name ``MjcActuator``."""
return prim.GetTypeName() == "MjcActuator"
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="actuator",
namespace="mujoco",
usd_prim_filter=is_actuator_prim,
)
)
For one-to-many mappings (one prim -> many rows):
.. code-block:: python
def is_tendon_prim(prim, context):
return prim.GetTypeName() == "MjcTendon"
def expand_joint_rows(prim, context):
return [
{"mujoco:tendon_joint": 4, "mujoco:tendon_coef": 0.5},
{"mujoco:tendon_joint": 8, "mujoco:tendon_coef": 0.5},
]
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="tendon_joint",
namespace="mujoco",
usd_prim_filter=is_tendon_prim,
usd_entry_expander=expand_joint_rows,
)
)
When :meth:`~newton.ModelBuilder.add_usd` runs:
1. Parses standard entities (bodies, shapes, joints, etc.).
2. Collects custom frequencies that define :attr:`~newton.ModelBuilder.CustomFrequency.usd_prim_filter`.
3. Traverses prims once (including instance proxies via ``Usd.TraverseInstanceProxies()``).
4. For each prim, evaluate matching frequencies in registration order:
- If :attr:`~newton.ModelBuilder.CustomFrequency.usd_entry_expander` is set, one row is appended per emitted dictionary,
and default per-attribute USD extraction for that frequency is skipped for that prim.
- Otherwise, one row is appended from the frequency's declared attributes.
Callback inputs:
* ``usd_prim_filter(prim, context)`` and ``usd_entry_expander(prim, context)`` receive
the same context shape.
* ``context`` is a small dictionary:
- ``prim``: current USD prim (same object as the ``prim`` argument)
- ``builder``: current :class:`~newton.ModelBuilder` instance
- ``result``: dictionary returned by :meth:`~newton.ModelBuilder.add_usd`
.. note::
Important behavior:
- Frequency callbacks are evaluated in deterministic registration order for each visited prim.
- If a frequency defines :attr:`~newton.ModelBuilder.CustomFrequency.usd_entry_expander`, then for every matched
prim in that frequency, the expander output is the only source of row values.
- In that expander code path, the normal :class:`~newton.ModelBuilder.CustomAttribute` USD parsing path is skipped
for that frequency/prim. In other words,
:attr:`~newton.ModelBuilder.CustomAttribute.usd_attribute_name` and
:attr:`~newton.ModelBuilder.CustomAttribute.usd_value_transformer` are not evaluated for those rows.
- Example: if frequency ``"mujoco:tendon_joint"`` has an expander and attribute
``CustomAttribute(name="tendon_coef", frequency="mujoco:tendon_joint", ...)``, then ``tendon_coef`` is populated
only from keys returned by the expander rows. If a row omits ``"mujoco:tendon_coef"``, the value is treated as
``None`` and the attribute default is applied at finalize time.
This mechanism lets solvers such as MuJoCo define USD-native schemas and parse them automatically
during model import.
Deriving Values from Prim Data (Wildcard Attribute)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
By default, each custom attribute reads its value from a specific USD attribute on the prim (e.g., ``newton:myns:my_attr``). Sometimes, however, you want to **compute** an attribute value from arbitrary prim data rather than reading a single named attribute. This is what setting :attr:`~newton.ModelBuilder.CustomAttribute.usd_attribute_name` to ``"*"`` is for.
When :attr:`~newton.ModelBuilder.CustomAttribute.usd_attribute_name` is set to ``"*"``, the attribute's :attr:`~newton.ModelBuilder.CustomAttribute.usd_value_transformer` is called for **every prim** matching the attribute's frequency — regardless of which USD attributes exist on that prim. The transformer receives ``None`` as the value (since there is no specific attribute to read) and a context dictionary containing the prim and the attribute definition.
A :attr:`~newton.ModelBuilder.CustomAttribute.usd_value_transformer` **must** be provided when using ``"*"``; omitting it raises a :class:`ValueError`.
**Example:** Suppose your USD stage contains "sensor" prims, each with an arbitrary ``sensor:position`` attribute. You want to store the distance from the origin as a custom attribute, computed at parse time:
.. code-block:: python
import warp as wp
import numpy as np
# 1. Register the custom frequency with a filter that selects sensor prims
def is_sensor(prim, context):
return prim.GetName().startswith("Sensor")
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="sensor",
namespace="myns",
usd_prim_filter=is_sensor,
)
)
# 2. Define a transformer that computes the distance from prim data
def compute_distance(value, context):
pos = context["prim"].GetAttribute("sensor:position").Get()
return wp.float32(float(np.linalg.norm(pos)))
# 3. Register the attribute with usd_attribute_name="*"
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="distance",
frequency="myns:sensor",
dtype=wp.float32,
default=0.0,
namespace="myns",
usd_attribute_name="*",
usd_value_transformer=compute_distance,
)
)
# 4. Parse the USD stage (assuming `stage` is an existing Usd.Stage)
builder.add_usd(stage)
model = builder.finalize()
# Access the computed values
distances = model.myns.distance.numpy()
The transformer context dictionary contains:
* ``"prim"``: The current USD prim.
* ``"attr"``: The :class:`~newton.ModelBuilder.CustomAttribute` being evaluated.
* When called from :meth:`~newton.ModelBuilder.add_usd` custom-frequency parsing,
context also includes ``"result"`` (the ``add_usd`` return dictionary) and
``"builder"`` (the current :class:`~newton.ModelBuilder`).
This pattern is useful when:
* The value you need doesn't exist as a single USD attribute (it must be derived from multiple attributes, prim metadata, or relationships).
* You want to run the same computation for every prim of a given frequency without requiring an authored attribute on each prim.
* You need to look up related entities (for example, resolving a prim relationship
to a body index through ``context["result"]["path_body_map"]``).
Multi-World Merging
-------------------
When using ``add_builder()``, ``add_world()``, or ``replicate()`` in multi-world simulations, the :attr:`~newton.ModelBuilder.CustomAttribute.references` field specifies how attribute values should be transformed:
.. testcode:: custom-merge
from newton import Model, ModelBuilder
builder = ModelBuilder()
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(name="pair", namespace="mujoco")
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_world",
frequency="mujoco:pair",
dtype=wp.int32,
namespace="mujoco",
references="world", # Replaced with the builder-managed current world during merge
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_geom1",
frequency="mujoco:pair",
dtype=wp.int32,
namespace="mujoco",
references="shape", # Offset by shape count during merge
)
)
Supported reference types:
* Any built-in entity type (e.g., ``"body"``, ``"shape"``, ``"joint"``, ``"joint_dof"``, ``"joint_coord"``, ``"articulation"``) — offset by entity count
* ``"world"`` — replaced with the builder-managed ``current_world`` for the active merge context
* Custom frequency keys (e.g., ``"mujoco:pair"``) — offset by that frequency's count
Querying Counts
---------------
Use :meth:`~newton.Model.get_custom_frequency_count` to get the count for a custom frequency (raises ``KeyError`` if unknown):
.. testcode:: custom-merge
# Finalize (requires at least one articulation)
_body = builder.add_link()
_joint = builder.add_joint_free(_body)
builder.add_articulation([_joint])
model = builder.finalize()
pair_count = model.get_custom_frequency_count("mujoco:pair")
# Or check directly without raising:
pair_count = model.custom_frequency_counts.get("mujoco:pair", 0)
.. note::
When querying, use the full frequency key with namespace prefix (e.g., ``"mujoco:pair"``). This matches how attribute keys work: ``model.get_attribute_frequency("mujoco:condim")`` for a namespaced attribute.
ArticulationView Limitations
----------------------------
Custom frequency attributes are generally not accessible via :class:`~newton.selection.ArticulationView` because they represent entity types that aren't tied to articulation structure. The one exception is the ``mujoco:tendon`` frequency, which is supported. For per-articulation data, use enum frequencies like ``ARTICULATION``, ``JOINT``, or ``BODY``.
================================================
FILE: docs/concepts/extended_attributes.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. currentmodule:: newton
.. _extended_attributes:
Extended Attributes
===================
Newton's :class:`~newton.State` and :class:`~newton.Contacts` objects can optionally carry extra arrays that are not always needed.
These *extended attributes* are allocated on demand when explicitly requested, reducing memory usage for simulations that don't need them.
.. _extended_contact_attributes:
Extended Contact Attributes
---------------------------
Extended contact attributes are optional arrays on :class:`~newton.Contacts` (e.g., contact forces for sensors).
Request them via :meth:`Model.request_contact_attributes ` or :meth:`ModelBuilder.request_contact_attributes ` before creating a :class:`~newton.Contacts` object.
.. testcode::
import newton
builder = newton.ModelBuilder()
body = builder.add_body(mass=1.0)
builder.add_shape_sphere(body, radius=0.1)
model = builder.finalize()
# Request the "force" extended attribute directly
model.request_contact_attributes("force")
contacts = model.contacts()
print(contacts.force is not None)
.. testoutput::
True
Some components request attributes transparently. For example,
:class:`~newton.sensors.SensorContact` requests ``"force"`` at init time, so
creating the sensor before allocating contacts is sufficient:
.. testcode::
import warp as wp
import newton
from newton.sensors import SensorContact
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0, 0, 0.1), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.1, label="ball")
model = builder.finalize()
sensor = SensorContact(model, sensing_obj_shapes="ball")
contacts = model.contacts()
print(contacts.force is not None)
.. testoutput::
True
The canonical list is :attr:`Contacts.EXTENDED_ATTRIBUTES `:
.. list-table::
:header-rows: 1
:widths: 22 78
* - Attribute
- Description
* - :attr:`~newton.Contacts.force`
- Contact spatial forces (used by :class:`~newton.sensors.SensorContact`)
.. _extended_state_attributes:
Extended State Attributes
-------------------------
Extended state attributes are optional arrays on :class:`~newton.State` (e.g., accelerations for sensors).
Request them via :meth:`Model.request_state_attributes ` or :meth:`ModelBuilder.request_state_attributes ` before calling :meth:`Model.state() `.
.. testcode::
import newton
builder = newton.ModelBuilder()
body = builder.add_body(mass=1.0)
builder.request_state_attributes("body_qdd")
model = builder.finalize()
state = model.state()
print(state.body_qdd is not None)
.. testoutput::
True
The canonical list is :attr:`State.EXTENDED_ATTRIBUTES `:
.. list-table::
:header-rows: 1
:widths: 22 78
* - Attribute
- Description
* - :attr:`~newton.State.body_qdd`
- Rigid-body spatial accelerations (used by :class:`~newton.sensors.SensorIMU`)
* - :attr:`~newton.State.body_parent_f`
- Rigid-body parent interaction wrenches
* - ``State.mujoco.qfrc_actuator``
- Actuator forces in generalized (joint DOF) coordinates, namespaced under ``state.mujoco.qfrc_actuator``.
Only populated by :class:`~newton.solvers.SolverMuJoCo`.
Notes
-----
- Some components transparently request attributes they need. For example, :class:`~newton.sensors.SensorIMU` requests ``body_qdd`` and :class:`~newton.sensors.SensorContact` requests ``force``.
Create sensors before allocating State/Contacts for this to work automatically.
- Solvers populate extended attributes they support. Currently, :class:`~newton.solvers.SolverMuJoCo` populates ``body_qdd``, ``body_parent_f``, ``mujoco:qfrc_actuator``, and ``force``.
================================================
FILE: docs/concepts/mass_inertia.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. _Mass and Inertia:
Mass and Inertia
================
Every dynamic rigid body in Newton needs positive mass and a physically
meaningful inertia tensor for stable simulation. Newton provides three ways to
assign these properties:
1. **Direct specification** on the body via :meth:`~newton.ModelBuilder.add_link`.
2. **Density-based inference** from collision shapes added with
:class:`~newton.ModelBuilder.ShapeConfig`.
3. **File import** (USD, MJCF, URDF), where mass properties are parsed from
the source format and mapped to Newton's internal representation.
For the distinction between static, kinematic, and dynamic bodies see
:ref:`Articulations`.
Best practices
--------------
.. tip::
**Dynamic bodies should have positive mass.**
If a body has no shapes with density, set ``mass`` and ``inertia``
explicitly on :meth:`~newton.ModelBuilder.add_link`.
.. tip::
**Use** ``is_kinematic=True`` **for prescribed motion** — do not rely on
zero mass to make a body immovable. See :ref:`Articulations` for details.
.. tip::
**Prefer density-based inference** when possible. Letting Newton compute
mass and inertia from shape geometry keeps mass properties consistent with
collision geometry and avoids manual bookkeeping.
.. tip::
**Use** ``lock_inertia=True`` **to protect hand-specified mass properties**
from subsequent shape additions. This is the mechanism the MJCF importer
uses when an ```` element is present.
.. tip::
**Check finalize warnings.** Set ``validate_inertia_detailed=True`` on
:class:`~newton.ModelBuilder` during development to get per-body warnings
for any mass or inertia values that were corrected.
.. _Specifying mass and inertia:
Specifying mass and inertia
---------------------------
Direct specification on the body
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
:meth:`~newton.ModelBuilder.add_link` accepts the following inertial
parameters:
- ``mass`` — body mass [kg]. Defaults to ``0.0``.
- ``inertia`` — 3x3 inertia tensor [kg m\ :sup:`2`] relative to the center
of mass. Defaults to the zero matrix.
- ``com`` — center-of-mass offset [m] from the body origin. Defaults to the
origin.
- ``armature`` — artificial scalar inertia [kg m\ :sup:`2`] added to the
diagonal of the inertia tensor. Useful for regularization.
These values serve as the initial mass properties of the body. If shapes with
positive density are subsequently added, their contributions are accumulated on
top of these initial values (see below).
Automatic inference from shape density
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
When a shape is added via one of the ``add_shape_*()`` methods with
:attr:`ShapeConfig.density ` > 0, Newton
automatically computes mass, center of mass, and inertia tensor from the shape
geometry and accumulates the result onto the parent body.
The accumulation follows three steps:
1. **Mass**: the shape's mass is added to the body's total mass.
2. **Center of mass**: the body COM is recomputed as the mass-weighted average
of the existing body COM and the shape's COM (transformed to body frame).
3. **Inertia**: both the existing body inertia and the shape inertia are
shifted to the new COM using the parallel-axis theorem (Steiner's theorem),
then summed.
This means multiple shapes on the same body compose additively.
.. testcode:: mass-accumulation
import numpy as np
import warp as wp
import newton
builder = newton.ModelBuilder()
# Body with initial mass 2.0 kg
body = builder.add_link(mass=2.0)
# Shape adds mass from density: a 1m-radius sphere at 1000 kg/m^3
builder.add_shape_sphere(body, radius=1.0, cfg=builder.ShapeConfig(density=1000.0))
sphere_mass = 4.0 / 3.0 * np.pi * 1000.0 # ~4189 kg
assert abs(builder.body_mass[body] - (2.0 + sphere_mass)) < 1.0
Special cases:
- **Planes and heightfields** never contribute mass, regardless of density.
- **Sites** enforce ``density=0`` and never contribute mass.
- ``lock_inertia=True`` on :meth:`~newton.ModelBuilder.add_link` prevents
subsequent shape additions from modifying the body's mass, COM, or inertia.
- **Hollow shapes** (``ShapeConfig.is_solid=False``) compute shell inertia
by subtracting the inner volume's contribution from the outer, using
:attr:`ShapeConfig.margin ` as shell thickness.
.. _Mass resolution during file import:
Mass resolution during file import
-----------------------------------
When importing from USD, MJCF, or URDF, each format has its own rules for how
mass properties are authored, inferred, or overridden. Regardless of format,
shape-derived contributions all flow through the same accumulation logic
described above.
The common pattern across formats is:
- **Explicit inertial data takes precedence** when present (``UsdPhysics.MassAPI``
for USD, ```` for MJCF/URDF).
- **Shape-based inference** is the fallback when explicit data is missing —
mass and inertia are computed from collision geometry and density.
- Both MJCF and URDF importers accept ``ignore_inertial_definitions=True`` to
skip explicit inertial data and always infer from shapes.
For format-specific details, see:
- **USD**: :ref:`usd_parsing` — covers the ``MassAPI`` precedence cascade,
``ComputeMassProperties`` callback, and collider density priority.
- **MJCF**: :meth:`~newton.ModelBuilder.add_mjcf` — follows MuJoCo semantics
where ```` fully overrides geom-derived mass (via
``lock_inertia``), and geoms contribute via density when ```` is
absent.
- **URDF**: :meth:`~newton.ModelBuilder.add_urdf` — uses ````
directly when present; falls back to collision geometry with default density
otherwise. By default visual shapes do not contribute mass; however,
``parse_visuals_as_colliders=True`` promotes visual geometry into the
collider set, making it mass-contributing at ``default_shape_density``.
.. _Validation and correction at finalize:
Validation and correction at finalize
--------------------------------------
During :meth:`~newton.ModelBuilder.finalize`, Newton validates and optionally
corrects mass and inertia properties for all bodies.
Compiler settings
^^^^^^^^^^^^^^^^^
The following attributes on :class:`~newton.ModelBuilder` control validation
behavior:
.. list-table::
:header-rows: 1
:widths: 30 15 55
* - Setting
- Default
- Description
* - :attr:`~newton.ModelBuilder.balance_inertia`
- ``True``
- Fix triangle-inequality violations on principal moments by shifting
eigenvalues uniformly.
* - :attr:`~newton.ModelBuilder.bound_mass`
- ``None``
- Minimum mass value. If set, clamps small masses up to this value.
* - :attr:`~newton.ModelBuilder.bound_inertia`
- ``None``
- Minimum inertia eigenvalue. If set, ensures all principal moments are
at least this value.
* - :attr:`~newton.ModelBuilder.validate_inertia_detailed`
- ``False``
- When ``True``, uses CPU validation with per-body warnings. When
``False``, uses a fast GPU kernel that returns only the count of
corrected bodies.
Checks performed
^^^^^^^^^^^^^^^^
The detailed (``validate_inertia_detailed=True``) and fast (default) validation
paths apply the same conceptual checks, but the detailed path emits per-body
warnings. Work is underway to unify the two implementations.
The following checks are applied in order:
1. **Negative mass** — set to zero.
2. **Small positive mass** below ``bound_mass`` — clamped (if ``bound_mass``
is set).
3. **Zero mass with non-zero inertia** — inertia zeroed.
4. **Inertia symmetry** — enforced via :math:`(I + I^T) / 2`.
5. **Positive definiteness** — negative eigenvalues adjusted.
6. **Eigenvalue bounds** — all eigenvalues clamped to at least
``bound_inertia`` (if set).
7. **Triangle inequality** — principal moments must satisfy
:math:`I_1 + I_2 \geq I_3`. If violated and ``balance_inertia`` is
``True``, eigenvalues are shifted uniformly to satisfy the inequality.
.. note::
:meth:`~newton.ModelBuilder.collapse_fixed_joints` merges mass and inertia
across collapsed bodies *before* validation runs.
.. _Shape inertia reference:
Shape inertia reference
-----------------------
The table below summarizes the mass formula for each shape type when density
is positive. For the full inertia tensor expressions, see
:func:`~newton.geometry.compute_inertia_shape`.
.. list-table::
:header-rows: 1
:widths: 18 35 47
* - Shape
- Mass
- Notes
* - Sphere
- :math:`\tfrac{4}{3} \pi r^3 \rho`
- Hollow: shell inertia (outer minus inner).
* - Box
- :math:`8\, h_x h_y h_z\, \rho`
- Half-extents.
* - Capsule
- hemisphere caps + cylinder
- :math:`(\tfrac{4}{3}\pi r^3 + 2\pi r^2 h)\,\rho`
* - Cylinder
- :math:`\pi r^2 h\, \rho`
-
* - Cone
- :math:`\tfrac{1}{3} \pi r^2 h\, \rho`
- Center of mass offset from base.
* - Ellipsoid
- :math:`\tfrac{4}{3} \pi a\,b\,c\, \rho`
- Semi-axes.
* - Mesh
- Integrated from triangles
- Cached on :class:`~newton.Mesh` when available; recomputed from
vertices otherwise. Supports solid and hollow.
* - Plane
- Always 0
- Regardless of density.
* - Heightfield
- Always 0
- Regardless of density.
Hollow shapes (``ShapeConfig.is_solid=False``) compute shell inertia by
subtracting the inner volume's contribution, using
:attr:`ShapeConfig.margin ` as shell thickness.
================================================
FILE: docs/concepts/sensors.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Sensors
=======
Sensors in Newton provide a way to extract measurements and observations from the simulation. They compute derived
quantities that are commonly needed for control, reinforcement learning, robotics applications, and analysis.
Overview
--------
Most Newton sensors follow a common pattern:
1. **Initialization**: Configure the sensor with the model and specify what to measure
2. **Update**: Call ``sensor.update(state, ...)`` during the simulation loop to compute measurements
3. **Access**: Read results from sensor attributes (typically as Warp arrays)
.. note::
Sensors automatically request any :doc:`extended attributes ` they need
(e.g. ``body_qdd``, ``Contacts.force``) at init, so ``State`` and ``Contacts`` objects created afterwards will
include them.
``SensorContact`` additionally requires a call to ``solver.update_contacts()`` before ``sensor.update()``.
``SensorTiledCamera`` writes results to output arrays passed into ``update()`` rather than storing them as sensor
attributes.
.. testcode::
import warp as wp
import newton
from newton.sensors import SensorIMU
# Build the model
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0, 0, 1), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.1)
builder.add_site(body, label="imu_0")
model = builder.finalize()
# 1. Create sensor and specify what to measure
imu = SensorIMU(model, sites="imu_*")
# Create solver and state
solver = newton.solvers.SolverMuJoCo(model)
state = model.state()
# Simulation loop
for _ in range(100):
state.clear_forces()
solver.step(state, state, None, None, dt=1.0 / 60.0)
# 2. Compute measurements from the current state
imu.update(state)
# 3. Results stored on sensor attributes
acc = imu.accelerometer.numpy() # (n_sensors, 3) linear acceleration
gyro = imu.gyroscope.numpy() # (n_sensors, 3) angular velocity
print("accelerometer shape:", acc.shape)
print("gyroscope shape:", gyro.shape)
.. testoutput::
accelerometer shape: (1, 3)
gyroscope shape: (1, 3)
.. _label-matching:
Label Matching
--------------
Several Newton APIs accept **label patterns** to select bodies, shapes, joints, sites, etc. by name. Parameters that
support label matching accept one of the following:
- A **list of integer indices** -- selects directly by index.
- A **single string pattern** -- selects all entries whose label matches the pattern via :func:`fnmatch.fnmatch`
(supports ``*`` and ``?`` wildcards).
- A **list of string patterns** -- selects all entries whose label matches at least one of the patterns.
Examples::
# single pattern: all shapes whose label starts with "foot_"
SensorIMU(model, sites="foot_*")
# list of patterns: union of two groups
SensorContact(model, sensing_obj_shapes=["*Plate*", "*Flap*"])
# list of indices: explicit selection
SensorFrameTransform(model, shapes=[0, 3, 7], reference_sites=[1])
Available Sensors
-----------------
Newton provides five sensor types. See the
:doc:`API reference <../api/newton_sensors>` for constructor arguments,
attributes, and usage examples.
* :class:`~newton.sensors.SensorContact` -- contact forces between bodies or shapes, including friction decomposition,
with optional per-counterpart breakdown.
* :class:`~newton.sensors.SensorFrameTransform` -- relative transforms of shapes/sites with respect to reference sites.
* :class:`~newton.sensors.SensorIMU` -- linear acceleration and angular velocity at site frames.
* :class:`~newton.sensors.SensorRaycast` -- ray-based depth images from a virtual camera.
* :class:`~newton.sensors.SensorTiledCamera` -- raytraced color and depth rendering across multiple worlds.
Extended Attributes
-------------------
Some sensors depend on extended attributes that are not allocated by default:
- ``SensorIMU`` requires ``State.body_qdd`` (rigid-body accelerations). By
default it requests this from the model at construction, so subsequent
``model.state()`` calls allocate it automatically.
- ``SensorContact`` requires ``Contacts.force`` (per-contact spatial force
wrenches). By default it requests this from the model at construction, so
subsequent ``model.contacts()`` calls allocate it automatically. The solver
must also support populating contact forces.
Performance Considerations
--------------------------
Sensors are designed to be efficient and GPU-friendly, computing results in
parallel where possible. Create each sensor once during setup and reuse it
every step -- this lets Newton pre-allocate output arrays and avoid per-frame
overhead.
Sensors that depend on extended attributes (e.g. ``body_qdd``,
``Contacts.force``) may add nontrivial cost to the solver step itself, since
the solver must compute and store these additional quantities regardless of
whether the sensor is evaluated after each step.
See Also
--------
* :doc:`sites` -- using sites as sensor attachment points and reference frames
* :doc:`../api/newton_sensors` -- full sensor API reference
* :doc:`extended_attributes` -- optional ``State``/``Contacts`` arrays required by some sensors
* ``newton.examples.sensors.example_sensor_contact`` -- SensorContact example
* ``newton.examples.sensors.example_sensor_imu`` -- SensorIMU example
* ``newton.examples.sensors.example_sensor_tiled_camera`` -- SensorTiledCamera example
================================================
FILE: docs/concepts/sites.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Sites (Abstract Markers)
========================
**Sites** are abstract reference points that don't participate in physics simulation or collision detection. They are lightweight markers used for:
* Sensor attachment points (IMU, camera, raycast origins)
* Frame of reference definitions for measurements
* Debugging and visualization reference points
* Spatial tendon attachment points and routing
Overview
--------
Sites in Newton are implemented as a special type of shape with the following properties:
* **No collision**: Sites never collide with any objects (shapes or particles)
* **No mass contribution**: Sites have zero density and don't affect body inertia
* **Transform-based**: Sites have position and orientation relative to their parent body
* **Shape types**: Sites can use any geometric primitive (sphere, box, capsule, etc.) for visualization
* **Visibility**: Sites can be visible (for debugging) or invisible (for runtime use)
Creating Sites
--------------
Sites are created using the ``add_site()`` method on ModelBuilder:
.. testcode:: sites-basic
builder = newton.ModelBuilder()
# Create a body
body = builder.add_body(mass=1.0)
# Add a site at body origin
imu_site = builder.add_site(
body=body,
label="imu"
)
# Add a site with offset and rotation
camera_site = builder.add_site(
body=body,
xform=wp.transform(
wp.vec3(0.5, 0, 0.2), # Position
wp.quat_from_axis_angle(wp.vec3(0, 1, 0), 3.14159/4) # Orientation
),
type=newton.GeoType.BOX,
scale=(0.05, 0.05, 0.02),
visible=True,
label="camera"
)
Sites can also be attached to the world frame (body=-1) to create fixed reference points:
.. testcode:: sites-world
builder = newton.ModelBuilder()
# World-frame reference site
world_origin = builder.add_site(
body=-1,
xform=wp.transform(wp.vec3(0, 0, 0), wp.quat_identity()),
label="world_origin"
)
Alternative: Using Shape Methods with ``as_site=True``
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Sites can also be created using shape creation methods (``add_shape_sphere``, ``add_shape_box``, ``add_shape_capsule``, ``add_shape_cylinder``, ``add_shape_ellipsoid``, ``add_shape_cone``) by passing ``as_site=True``. This is particularly useful when programmatically generating shapes or conditionally creating sites:
.. testcode:: sites-shape-methods
builder = newton.ModelBuilder()
body = builder.add_body(mass=1.0)
# Create sites using shape methods
sphere_site = builder.add_shape_sphere(
body=body,
radius=0.05,
as_site=True,
label="sphere_marker"
)
box_site = builder.add_shape_box(
body=body,
hx=0.1, hy=0.1, hz=0.1,
as_site=True,
label="box_marker"
)
# Useful for conditional creation
is_sensor_point = True
shape_idx = builder.add_shape_sphere(
body=body,
radius=0.05,
as_site=is_sensor_point, # Conditionally a site
label="measurement_point"
)
When ``as_site=True``, the shape is automatically configured with all site invariants (no collision, zero density, collision_group=0), regardless of any custom configuration passed.
Importing Sites
---------------
Sites are automatically imported from MJCF and USD files, with optional control over what gets loaded.
MJCF Import
~~~~~~~~~~~
MuJoCo sites are directly mapped to Newton sites, preserving type, position, orientation, and size:
.. code-block:: xml
By default, sites are loaded along with collision and visual shapes. You can control this behavior with the ``parse_sites`` and ``parse_visuals`` parameters:
.. code-block:: python
builder = newton.ModelBuilder()
# Load only collision shapes and sites (no visual shapes)
builder.add_mjcf("robot.xml", parse_sites=True, parse_visuals=False)
# Load only collision shapes (no sites or visual shapes)
builder.add_mjcf("robot.xml", parse_sites=False, parse_visuals=False)
USD Import
~~~~~~~~~~
Sites in USD are identified by the ``MjcSiteAPI`` schema applied to geometric primitives:
.. code-block:: usda
def Xform "robot" (
prepend apiSchemas = ["PhysicsRigidBodyAPI"]
) {
def Sphere "imu_site" (
prepend apiSchemas = ["MjcSiteAPI"]
) {
double radius = 0.02
double3 xformOp:translate = (0.1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate"]
}
}
Similar to MJCF import, you can control whether sites and visual shapes are loaded using the ``load_sites`` and ``load_visual_shapes`` parameters:
.. code-block:: python
builder = newton.ModelBuilder()
# Load only collision shapes and sites (no visual shapes)
builder.add_usd("robot.usda", load_sites=True, load_visual_shapes=False)
# Load only collision shapes (no sites or visual shapes)
builder.add_usd("robot.usda", load_sites=False, load_visual_shapes=False)
By default, both ``load_sites`` and ``load_visual_shapes`` are set to ``True``.
Using Sites with Sensors
------------------------
Sites are commonly used as reference frames for sensors, particularly :class:`~newton.sensors.SensorFrameTransform` which computes relative poses between objects and reference frames.
For detailed information on using sites with sensors, see :doc:`sensors`.
MuJoCo Interoperability
-----------------------
When using ``SolverMuJoCo``, Newton sites are automatically exported to MuJoCo's native site representation:
.. testcode:: sites-mujoco
from newton.solvers import SolverMuJoCo
# Create a simple model with a site
builder = newton.ModelBuilder()
body = builder.add_body(mass=1.0, inertia=wp.mat33(np.eye(3)))
site = builder.add_site(body=body, label="sensor")
model = builder.finalize()
# Create MuJoCo solver (sites are exported by default)
solver = SolverMuJoCo(model)
Sites are exported with their visual properties (color, size) and can be used with MuJoCo's native sensors and actuators. To disable site export, pass ``include_sites=False`` to :class:`~newton.solvers.SolverMuJoCo`.
Implementation Details
----------------------
Sites are internally represented as shapes with the :attr:`~newton.ShapeFlags.SITE` flag set. This allows them to leverage Newton's existing shape infrastructure while maintaining distinct behavior:
* Sites are filtered out from collision detection pipelines
* Site density is automatically set to zero during creation
* Sites can be identified at runtime by checking the :attr:`~newton.ShapeFlags.SITE` flag on ``model.shape_flags``
This implementation approach provides maximum flexibility while keeping the codebase maintainable and avoiding duplication.
See Also
--------
* :doc:`sensors` — Using sites with sensors for measurements
* :doc:`custom_attributes` — Attaching custom data to sites and other entities
* :doc:`../api/newton_sensors` — Full sensor API reference
* :doc:`usd_parsing` — Details on USD schema handling
================================================
FILE: docs/concepts/usd_parsing.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. _usd_parsing:
USD Parsing and Schema Resolver System
========================================
Newton provides USD (Universal Scene Description) ingestion and schema resolver pipelines that enable integration of physics assets authored for different simulation solvers.
Understanding USD and UsdPhysics
--------------------------------
USD (Universal Scene Description) is Pixar's open-source framework for interchange of 3D computer graphics data. It provides an ecosystem for describing 3D scenes with hierarchical composition, animation, and metadata.
UsdPhysics is the standard USD schema for physics simulation, defining for instance:
* Rigid bodies (``UsdPhysics.RigidBodyAPI``)
* Collision shapes (``UsdPhysics.CollisionAPI``)
* Joints and constraints (``UsdPhysics.Joint``)
* Materials and contact properties (``UsdPhysics.MaterialAPI``)
* Scene-level physics settings (``UsdPhysics.Scene``)
However, UsdPhysics provides only a basic foundation. Different physics solvers like PhysX and MuJoCo often require additional attributes not covered by these standard schemas.
PhysX and MuJoCo have their own schemas for describing physics assets. While some of these attributes are *conceptually* common between many solvers, many are solver-specific.
Even among the common attributes, the names and semantics may differ and they are only conceptually similar. Therefore, some transformation is needed to make these attributes usable by Newton.
Newton's schema resolver system automatically handles these differences, allowing assets authored for any solver to work with Newton's simulation. See :ref:`schema_resolvers` for more details.
Newton's USD Import System
--------------------------
Newton's :meth:`newton.ModelBuilder.add_usd` method provides a USD import pipeline that:
* Parses standard UsdPhysics schema for basic rigid body simulation setup
* Resolves common solver attributes that are conceptually similar between different solvers through configurable schema resolvers
* Handles priority-based attribute resolution when multiple solvers define conflicting values for conceptually similar properties
* Collects solver-specific attributes preserving solver-native attributes for potential use in the solver
* Supports parsing of custom Newton model/state/control attributes for specialized simulation requirements
Mass and Inertia Precedence
---------------------------
.. seealso::
:ref:`Mass and Inertia` for general concepts: the programmatic API,
density-based inference, and finalize-time validation.
For rigid bodies with ``UsdPhysics.MassAPI`` applied, Newton resolves each inertial property
(mass, inertia, center of mass) independently. Authored attributes take precedence;
``UsdPhysics.RigidBodyAPI.ComputeMassProperties(...)`` provides baseline values for the rest.
1. Authored ``physics:mass``, ``physics:diagonalInertia``, and ``physics:centerOfMass`` are
applied directly when present. If ``physics:principalAxes`` is missing, identity rotation
is used.
2. When ``physics:mass`` is authored but ``physics:diagonalInertia`` is not, the inertia
accumulated from collision shapes is scaled by ``authored_mass / accumulated_mass``.
3. For any remaining unresolved properties, Newton falls back to
``UsdPhysics.RigidBodyAPI.ComputeMassProperties(...)``.
In this fallback path, collider contributions use a two-level precedence:
a. If collider ``UsdPhysics.MassAPI`` has authored ``mass`` and ``diagonalInertia``, those
authored values are converted to unit-density collider mass information.
b. Otherwise, Newton derives unit-density collider mass information from collider geometry.
A collider is skipped (with warning) only if neither path provides usable collider mass
information.
.. note::
The callback payload provided by Newton in this path is unit-density collider shape
information (volume/COM/inertia basis). Collider density authored via ``UsdPhysics.MassAPI``
(for example, ``physics:density``) or via bound ``UsdPhysics.MaterialAPI`` is still applied
by USD during ``ComputeMassProperties(...)``. In other words, unit-density callback data does
not mean authored densities are ignored.
If resolved mass is non-positive, inverse mass is set to ``0``.
.. tip::
For the most predictable results, fully author ``physics:mass``, ``physics:diagonalInertia``,
``physics:principalAxes``, and ``physics:centerOfMass`` on each rigid body. This avoids any
fallback heuristics and is also the fastest import path since ``ComputeMassProperties(...)``
can be skipped entirely.
.. _schema_resolvers:
Schema Resolvers
----------------
Schema resolvers bridge the gap between solver-specific USD schemas and Newton's internal representation. They remap attributes authored for PhysX, MuJoCo, or other solvers to the equivalent Newton properties, handle priority-based resolution when multiple solvers define the same attribute, and collect solver-native attributes for inspection or custom pipelines.
.. note::
The ``schema_resolvers`` argument in :meth:`newton.ModelBuilder.add_usd` is an experimental feature that may be removed or changed significantly in the future.
Solver Attribute Remapping
~~~~~~~~~~~~~~~~~~~~~~~~~~
When working with USD assets authored for other physics solvers like PhysX or MuJoCo, Newton's schema resolver system can automatically remap various solver attributes to Newton's internal representation. This enables Newton to use physics properties from assets originally designed for other simulators without manual conversion.
The following tables show examples of how solver-specific attributes are mapped to Newton's internal representation. Some attributes map directly while others require mathematical transformations.
**PhysX Attribute Remapping Examples:**
The table below shows PhysX attribute remapping examples:
.. list-table:: PhysX Attribute Remapping
:header-rows: 1
:widths: 30 30 40
* - **PhysX Attribute**
- **Newton Equivalent**
- **Transformation**
* - ``physxJoint:armature``
- ``armature``
- Direct mapping
* - ``physxArticulation:enabledSelfCollisions``
- ``self_collision_enabled`` (per articulation)
- Direct mapping
**Newton articulation remapping:**
On articulation root prims (with ``PhysicsArticulationRootAPI`` or ``NewtonArticulationRootAPI``), the following is resolved:
.. list-table:: Newton Articulation Remapping
:header-rows: 1
:widths: 30 30 40
* - **Newton Attribute**
- **Resolved key**
- **Transformation**
* - ``newton:selfCollisionEnabled``
- ``self_collision_enabled``
- Direct mapping
The parser resolves ``self_collision_enabled`` from either ``newton:selfCollisionEnabled`` or ``physxArticulation:enabledSelfCollisions`` (in resolver priority order). The ``enable_self_collisions`` argument to :meth:`newton.ModelBuilder.add_usd` is used as the default when neither attribute is authored.
**MuJoCo Attribute Remapping Examples:**
The table below shows MuJoCo attribute remapping examples, including both direct mappings and transformations:
.. list-table:: MuJoCo Attribute Remapping
:header-rows: 1
:widths: 30 30 40
* - **MuJoCo Attribute**
- **Newton Equivalent**
- **Transformation**
* - ``mjc:armature``
- ``armature``
- Direct mapping
* - ``mjc:margin``, ``mjc:gap``
- ``margin``
- ``margin = mjc:margin - mjc:gap``
**Example USD with remapped attributes:**
The following USD example demonstrates how PhysX attributes are authored in a USD file. The schema resolver automatically applies the transformations shown in the table above during import:
.. code-block:: usda
#usda 1.0
def PhysicsScene "Scene" (
prepend apiSchemas = ["PhysxSceneAPI"]
) {
# PhysX scene settings that Newton can understand
uint physxScene:maxVelocityIterationCount = 16 # → max_solver_iterations = 16
}
def RevoluteJoint "elbow_joint" (
prepend apiSchemas = ["PhysxJointAPI", "PhysxLimitAPI:angular"]
) {
# PhysX joint attributes remapped to Newton
float physxJoint:armature = 0.1 # → armature = 0.1
# PhysX limit attributes (applied via PhysxLimitAPI:angular)
float physxLimit:angular:stiffness = 1000.0 # → limit_angular_ke = 1000.0
float physxLimit:angular:damping = 10.0 # → limit_angular_kd = 10.0
# Initial joint state
float state:angular:physics:position = 1.57 # → joint_q = 1.57 rad
}
def Mesh "collision_shape" (
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysxCollisionAPI"]
) {
# PhysX collision settings (gap = contactOffset - restOffset)
float physxCollision:contactOffset = 0.05
float physxCollision:restOffset = 0.01 # → gap = 0.04
}
Priority-Based Resolution
~~~~~~~~~~~~~~~~~~~~~~~~~
When multiple physics solvers define conflicting attributes for the same property, the user can define which solver attributes should be preferred by configuring the resolver order.
**Resolution Hierarchy:**
The attribute resolution process follows a three-layer fallback hierarchy to determine which value to use:
1. **Authored Values**: Resolvers are queried in priority order; the first resolver that finds an authored value on the prim returns it and remaining resolvers are not consulted.
2. **Importer Defaults**: If no authored value is found, Newton's importer uses a property-specific fallback (e.g. ``builder.default_joint_cfg.armature`` for joint armature). This takes precedence over schema-level defaults.
3. **Approximated Schema Defaults**: If neither an authored value nor an importer default is available, Newton falls back to a hardcoded approximation of each solver's schema default, defined in Newton's resolver mapping. These approximations will be replaced by actual USD schema defaults in a future release.
**Configuring Resolver Priority:**
The order of resolvers in the ``schema_resolvers`` list determines priority, with earlier entries taking precedence. To demonstrate this, consider a USD asset where the same joint has conflicting armature values authored for different solvers:
.. code-block:: usda
def RevoluteJoint "shoulder_joint" {
float newton:armature = 0.01
float physxJoint:armature = 0.02
float mjc:armature = 0.03
}
By changing the order of resolvers in the ``schema_resolvers`` list, different attribute values will be selected from the same USD file. The following examples show how the same asset produces different results based on resolver priority:
.. testcode::
:skipif: True
from newton import ModelBuilder
from newton.usd import SchemaResolverMjc, SchemaResolverNewton, SchemaResolverPhysx
builder = ModelBuilder()
# Configuration 1: Newton priority
result_newton = builder.add_usd(
source="conflicting_asset.usda",
schema_resolvers=[SchemaResolverNewton(), SchemaResolverPhysx(), SchemaResolverMjc()]
)
# Result: Uses newton:armature = 0.01
# Configuration 2: PhysX priority
builder2 = ModelBuilder()
result_physx = builder2.add_usd(
source="conflicting_asset.usda",
schema_resolvers=[SchemaResolverPhysx(), SchemaResolverNewton(), SchemaResolverMjc()]
)
# Result: Uses physxJoint:armature = 0.02
# Configuration 3: MuJoCo priority
builder3 = ModelBuilder()
result_mjc = builder3.add_usd(
source="conflicting_asset.usda",
schema_resolvers=[SchemaResolverMjc(), SchemaResolverNewton(), SchemaResolverPhysx()]
)
# Result: Uses mjc:armature = 0.03
Solver-Specific Attribute Collection
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Some attributes are solver-specific and cannot be directly used by Newton's simulation. The schema resolver system preserves these solver-specific attributes during import, making them accessible as part of the parsing results. This is useful for:
* Debugging and inspection of solver-specific properties
* Future compatibility when Newton adds support for additional attributes
* Custom pipelines that need to access solver-native properties
* Sim-to-sim transfer where you might need to rebuild assets for other solvers
**Solver-Specific Attribute Namespaces:**
Each solver has its own namespace prefixes for solver-specific attributes. The table below shows the namespace conventions and provides examples of attributes that would be collected from each solver:
.. list-table:: Solver-Specific Namespaces
:header-rows: 1
:widths: 20 40 40
* - **Engine**
- **Namespace Prefixes**
- **Example Attributes**
* - **PhysX**
- ``physx``, ``physxScene``, ``physxRigidBody``, ``physxCollision``, ``physxArticulation``
- ``physxArticulation:enabledSelfCollisions``, ``physxSDFMeshCollision:meshScale``
* - **MuJoCo**
- ``mjc``
- ``mjc:model:joint:testMjcJointScalar``, ``mjc:state:joint:testMjcJointVec3``
* - **Newton**
- ``newton``
- ``newton:maxHullVertices``, ``newton:contactGap``
**Accessing Collected Solver-Specific Attributes:**
The collected attributes are returned in the result dictionary and can be accessed by solver namespace:
.. testcode::
:skipif: True
from newton import ModelBuilder
from newton.usd import SchemaResolverNewton, SchemaResolverPhysx
builder = ModelBuilder()
result = builder.add_usd(
source="physx_humanoid.usda",
schema_resolvers=[SchemaResolverPhysx(), SchemaResolverNewton()],
)
# Access collected solver-specific attributes
solver_attrs = result.get("schema_attrs", {})
if "physx" in solver_attrs:
physx_attrs = solver_attrs["physx"]
for prim_path, attrs in physx_attrs.items():
if "physxJoint:armature" in attrs:
armature_value = attrs["physxJoint:armature"]
print(f"PhysX joint {prim_path} has armature: {armature_value}")
Custom Attributes from USD
--------------------------
USD assets can define custom attributes that become part of the model/state/control attributes, see :ref:`custom_attributes` for more information.
Besides the programmatic way of defining custom attributes through the :meth:`newton.ModelBuilder.add_custom_attribute` method, Newton's USD importer also supports declaring custom attributes from within a USD stage.
**Overview:**
Custom attributes enable users to:
* Extend Newton's data model with application-specific properties
* Store per-body/joint/dof/shape data directly in USD assets
* Implement custom simulation behaviors driven by USD-authored data
* Organize related attributes using namespaces
**Declaration-First Pattern:**
Custom attributes must be declared on the ``PhysicsScene`` prim with metadata before being used on individual prims:
1. **Declare on PhysicsScene**: Define attributes with ``customData`` metadata specifying assignment and frequency
2. **Assign on individual prims**: Override default values using shortened attribute names
**Declaration Format:**
.. code-block:: usda
custom newton:namespace:attr_name = default_value (
customData = {
string assignment = "model|state|control|contact"
string frequency = "body|shape|joint|joint_dof|joint_coord|articulation"
}
)
Where:
* **namespace** (optional): Custom namespace for organizing related attributes (omit for default namespace)
* **attr_name**: User-defined attribute name
* **assignment**: Storage location (``model``, ``state``, ``control``, ``contact``)
* **frequency**: Per-entity granularity (``body``, ``shape``, ``joint``, ``joint_dof``, ``joint_coord``, ``articulation``)
**Supported Data Types:**
The system automatically infers data types from authored USD values. The following table shows the mapping between USD types and Warp types used internally by Newton:
.. list-table:: Custom Attribute Data Types
:header-rows: 1
:widths: 25 25 50
* - **USD Type**
- **Warp Type**
- **Example**
* - ``float``
- ``wp.float32``
- Scalar values
* - ``bool``
- ``wp.bool``
- Boolean flags
* - ``int``
- ``wp.int32``
- Integer values
* - ``float2``
- ``wp.vec2``
- 2D vectors
* - ``float3``
- ``wp.vec3``
- 3D vectors, positions
* - ``float4``
- ``wp.vec4``
- 4D vectors
* - ``quatf``/``quatd``
- ``wp.quat``
- Quaternions (with automatic reordering)
**Assignment Types:**
The ``assignment`` field in the declaration determines where the custom attribute data will be stored. The following table describes each assignment type and its typical use cases:
.. list-table:: Custom Attribute Assignments
:header-rows: 1
:widths: 15 25 60
* - **Assignment**
- **Storage Location**
- **Use Cases**
* - ``model``
- ``Model`` object
- Static configuration, physical properties, metadata
* - ``state``
- ``State`` object
- Dynamic quantities, targets, sensor readings
* - ``control``
- ``Control`` object
- Control parameters, actuator settings, gains
* - ``contact``
- Contact container
- Contact-specific properties (future use)
**USD Authoring with Custom Attributes:**
The following USD example demonstrates the complete workflow for authoring custom attributes. Note how attributes are first declared on the ``PhysicsScene`` with their metadata, then assigned with specific values on individual prims:
.. code-block:: usda
# robot_with_custom_attrs.usda
#usda 1.0
def PhysicsScene "physicsScene" {
# Declare custom attributes with metadata (default namespace)
custom float newton:mass_scale = 1.0 (
customData = {
string assignment = "model"
string frequency = "body"
}
)
custom float3 newton:local_marker = (0.0, 0.0, 0.0) (
customData = {
string assignment = "model"
string frequency = "body"
}
)
custom bool newton:is_sensor = false (
customData = {
string assignment = "model"
string frequency = "body"
}
)
custom float3 newton:target_position = (0.0, 0.0, 0.0) (
customData = {
string assignment = "state"
string frequency = "body"
}
)
# Declare namespaced custom attributes (namespace_a)
custom float newton:namespace_a:mass_scale = 1.0 (
customData = {
string assignment = "state"
string frequency = "body"
}
)
custom float newton:namespace_a:gear_ratio = 1.0 (
customData = {
string assignment = "model"
string frequency = "joint"
}
)
custom float2 newton:namespace_a:pid_gains = (0.0, 0.0) (
customData = {
string assignment = "control"
string frequency = "joint"
}
)
# Articulation frequency attribute
custom float newton:articulation_stiffness = 100.0 (
customData = {
string assignment = "model"
string frequency = "articulation"
}
)
}
def Xform "robot_body" (
prepend apiSchemas = ["PhysicsRigidBodyAPI"]
) {
# Assign values to declared attributes (default namespace)
custom float newton:mass_scale = 1.5
custom float3 newton:local_marker = (0.1, 0.2, 0.3)
custom bool newton:is_sensor = true
custom float3 newton:target_position = (1.0, 2.0, 3.0)
# Assign values to namespaced attributes (namespace_a)
custom float newton:namespace_a:mass_scale = 2.5
}
def RevoluteJoint "joint1" {
# Assign joint attributes (namespace_a)
custom float newton:namespace_a:gear_ratio = 2.25
custom float2 newton:namespace_a:pid_gains = (100.0, 10.0)
}
# Articulation frequency attributes must be defined on the prim with PhysicsArticulationRootAPI
def Xform "robot_articulation" (
prepend apiSchemas = ["PhysicsArticulationRootAPI"]
) {
# Assign articulation-level attributes
custom float newton:articulation_stiffness = 150.0
}
.. note::
Attributes with ``frequency = "articulation"`` store per-articulation values and must be
authored on USD prims that have the ``PhysicsArticulationRootAPI`` schema applied.
**Accessing Custom Attributes in Python:**
After importing the USD file with the custom attributes shown above, they become accessible as properties on the appropriate objects (``Model``, ``State``, or ``Control``) based on their assignment. The following example shows how to import and access these attributes:
.. code-block:: python
from newton import ModelBuilder
builder = ModelBuilder()
# Import the USD file with custom attributes (from example above)
result = builder.add_usd(
source="robot_with_custom_attrs.usda",
)
model = builder.finalize()
state = model.state()
control = model.control()
# Access default namespace model-assigned attributes
body_mass_scale = model.mass_scale.numpy() # Per-body scalar
local_markers = model.local_marker.numpy() # Per-body vec3
sensor_flags = model.is_sensor.numpy() # Per-body bool
# Access default namespace state-assigned attributes
target_positions = state.target_position.numpy() # Per-body vec3
# Access namespaced attributes (namespace_a)
# Note: Same attribute name can exist in different namespaces with different assignments
namespaced_mass = state.namespace_a.mass_scale.numpy() # Per-body scalar (state assignment)
gear_ratios = model.namespace_a.gear_ratio.numpy() # Per-joint scalar
pid_gains = control.namespace_a.pid_gains.numpy() # Per-joint vec2
arctic_stiff = model.articulation_stiffness.numpy() # Per-articulation scalar
**Namespace Isolation:**
Attributes with the same name in different namespaces are completely independent and stored separately. This allows the same attribute name to be used for different purposes across namespaces. In the example above, ``mass_scale`` appears in both the default namespace (as a model attribute) and in ``namespace_a`` (as a state attribute). These are treated as completely separate attributes with independent values, assignments, and storage locations.
Limitations
-----------
Importing USD files where many (> 30) mesh colliders are under the same rigid body
can result in a crash in ``UsdPhysics.LoadUsdPhysicsFromRange``. This is a known
thread-safety issue in OpenUSD and will be fixed in a future release of
``usd-core``. It can be worked around by setting the work concurrency limit to 1
before ``pxr`` initializes its thread pool.
.. note::
Setting the concurrency limit to 1 disables multi-threaded USD processing
globally and may degrade performance of other OpenUSD workloads in the same
process.
Choose **one** of the two approaches below — do not combine them.
``PXR_WORK_THREAD_LIMIT`` is evaluated once when ``pxr`` is first imported and
cached for the lifetime of the process; after that point,
``Work.SetConcurrencyLimit()`` cannot override it. Conversely, if the env var
*is* set, calling ``Work.SetConcurrencyLimit()`` has no effect.
**Option A — environment variable (before any USD import):**
.. code-block:: python
import os
os.environ["PXR_WORK_THREAD_LIMIT"] = "1" # must precede any pxr import
from newton import ModelBuilder
builder = ModelBuilder()
result = builder.add_usd(
source="rigid_body_with_many_mesh_colliders.usda",
)
**Option B —** ``Work.SetConcurrencyLimit`` **(only when the env var is not set):**
.. code-block:: python
from pxr import Work
import os
if "PXR_WORK_THREAD_LIMIT" not in os.environ:
Work.SetConcurrencyLimit(1)
from newton import ModelBuilder
builder = ModelBuilder()
result = builder.add_usd(
source="rigid_body_with_many_mesh_colliders.usda",
)
.. seealso::
`threadLimits.h`_ (API reference) and `threadLimits.cpp`_ (implementation)
document the precedence rules between the environment variable and the API.
.. _threadLimits.h: https://openusd.org/dev/api/thread_limits_8h.html
.. _threadLimits.cpp: https://github.com/PixarAnimationStudios/OpenUSD/blob/release/pxr/base/work/threadLimits.cpp
================================================
FILE: docs/concepts/worlds.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. _Worlds:
Worlds
======
Newton enables multiple independent simulations, referred to as *worlds*, within a single :class:`~newton.Model` object.
Each *world* provides an index-based grouping of all primary simulation entities such as particles, bodies, shapes, joints, articulations and equality constraints.
Overview
--------
GPU-accelerated operations in Newton often involve parallelizing over an entire set of model entities, e.g. bodies, shapes or joints, without needing to consider which specific world they belong to.
However, some operations, such as those part of Collision Detection (CD), can exploit world-based grouping to effectively filter out potential collisions between shapes that belong to different worlds.
Moreover, world-based grouping can also facilitate partitioning of thread grids according to both world indices and the number of entities per world.
Such operations facilitate support for simulating multiple, and potentially heterogeneous, worlds defined within a :class:`~newton.Model` instance.
Lastly, world-based grouping also enables selectively operating on only the entities that belong to a specific world, i.e. masking, as well as partitioning of the :class:`~newton.Model` and :class:`~newton.State` data.
.. note::
Support for fully heterogeneous simulations is still under active development and quite experimental.
At present time, although the :class:`~newton.ModelBuilder` and :class:`~newton.Model` objects support instantiating worlds with different disparate entities, not all solvers are able to simulate them.
Moreover, the selection API still operates under the assumption of model homogeneity, but this is expected to also support heterogeneous simulations in the near future.
.. _World assignment:
World Assignment
----------------
World assignment is managed by :class:`~newton.ModelBuilder` when entities are added through methods such as :meth:`~newton.ModelBuilder.add_body`,
:meth:`~newton.ModelBuilder.add_joint`, and :meth:`~newton.ModelBuilder.add_shape`. Assignment can either be global (world index ``-1``) or specific to a particular world (indices ``0, 1, 2, ...``).
The supported workflows are:
* Add entities before the first call to :meth:`~newton.ModelBuilder.begin_world` or after the last call to :meth:`~newton.ModelBuilder.end_world` to place them in the global world (index ``-1``), or between those calls to place them in a specific world.
* Create worlds from a sub-builder with :meth:`~newton.ModelBuilder.add_world` or :meth:`~newton.ModelBuilder.replicate`.
Within a world scope, each entity is assigned the current world index. The :attr:`~newton.ModelBuilder.current_world` attribute is a read-only property that reflects the active builder context and should not be set directly.
The following example creates two different worlds within a single model:
.. testcode::
import warp as wp
import newton
builder = newton.ModelBuilder()
# Global entity at front (world -1)
builder.add_ground_plane()
# World 0: two free-floating spheres
builder.begin_world()
b0 = builder.add_body(mass=1.0)
b1 = builder.add_body(mass=1.0)
builder.add_shape_sphere(body=b0, radius=0.1)
builder.add_shape_sphere(body=b1, radius=0.1)
builder.end_world()
# World 1: fixed-base revolute articulation with boxes
builder.begin_world()
link0 = builder.add_link(mass=1.0)
j0 = builder.add_joint_fixed(parent=-1, child=link0)
link1 = builder.add_link(mass=2.0)
j1 = builder.add_joint_revolute(parent=link0, child=link1)
builder.add_articulation(joints=[j0, j1])
builder.add_shape_box(body=link0, hx=0.1, hy=0.1, hz=0.1)
builder.add_shape_box(body=link1, hx=0.1, hy=0.1, hz=0.1)
builder.end_world()
# Global entity at back (world -1)
builder.add_shape_box(body=-1, hx=0.5, hy=0.5, hz=0.05)
model = builder.finalize()
In this example, we create a model with two worlds (world ``0`` and world ``1``) containing different bodies, shapes and joints, as well as two global entities (the ground plane at the front and a static box at the back, both with world index ``-1``).
For homogeneous multi-world scenes, prefer :meth:`~newton.ModelBuilder.add_world` or :meth:`~newton.ModelBuilder.replicate` instead of manually repeating world scopes for each copy.
.. _World grouping:
World Grouping
--------------
The :class:`~newton.ModelBuilder` maintains internal lists that track the world assignment of each entity added to it.
When :meth:`~newton.ModelBuilder.finalize` is called, the :class:`~newton.Model` object generated will contain arrays that store the world indices for each entity type.
Specifically, the entity types that currently support world grouping include:
- Particles: :attr:`~newton.Model.particle_world`
- Bodies: :attr:`~newton.Model.body_world`
- Shapes: :attr:`~newton.Model.shape_world`
- Joints: :attr:`~newton.Model.joint_world`
- Articulations: :attr:`~newton.Model.articulation_world`
- Equality Constraints: :attr:`~newton.Model.equality_constraint_world`
The corresponding world grouping arrays for the example above are:
.. testcode::
print("Body worlds:", model.body_world.numpy().tolist())
print("Shape worlds:", model.shape_world.numpy().tolist())
print("Joint worlds:", model.joint_world.numpy().tolist())
.. testoutput::
Body worlds: [0, 0, 1, 1]
Shape worlds: [-1, 0, 0, 1, 1, -1]
Joint worlds: [0, 0, 1, 1]
.. _World starts:
World Start Indices & Dimensions
--------------------------------
In addition to the world grouping arrays, the :class:`~newton.Model` object will also contain Warp arrays that store the per-world starting indices for each entity type.
These arrays include:
- Particles: :attr:`~newton.Model.particle_world_start`
- Bodies: :attr:`~newton.Model.body_world_start`
- Shapes: :attr:`~newton.Model.shape_world_start`
- Joints: :attr:`~newton.Model.joint_world_start`
- Articulations: :attr:`~newton.Model.articulation_world_start`
- Equality Constraints: :attr:`~newton.Model.equality_constraint_world_start`
To handle the special case of joint entities, that vary in the number of DOFs, coordinates and constraints, the model also provides arrays that store the per-world starting indices in these specific dimensions:
- Joint DOFs: :attr:`~newton.Model.joint_dof_world_start`
- Joint Coordinates: :attr:`~newton.Model.joint_coord_world_start`
- Joint Constraints: :attr:`~newton.Model.joint_constraint_world_start`
All ``*_world_start`` arrays adopt a special format that facilitates accounting of the total number of entities in each world as well as the global world (index ``-1``) at the front and back of each per-entity array such as :attr:`~newton.Model.body_world`.
Specifically, each ``*_world_start`` array contains ``world_count + 2`` entries, with the first ``world_count`` entries corresponding to starting indices of each ``world >= 0`` world,
the second-to-last entry corresponding to the starting index of the global entities at the back (world index ``-1``), and the last entry corresponding to total number of entities or dimensions in the model.
With this format, we can easily compute the number of entities per world by computing the difference between consecutive entries in these arrays (since they are essentially cumulative sums),
as well as the total number of global entities by summing the first entry with the difference of the last two.
Continuing the same example, we can compute the per-world shape counts as follows:
.. testcode::
print("world_count:", model.world_count)
# Shape start indices per world
# Entries: [start_world_0, start_world_1, start_global_back, total_shapes]
shape_start = model.shape_world_start.numpy()
print("Shape starts:", shape_start.tolist())
# Compute per-world shape counts
world_shape_counts = [
int(shape_start[i + 1] - shape_start[i])
for i in range(model.world_count)
]
# Global shapes: those at the front (before start_world_0) plus at the back
global_shape_count = int(shape_start[0]) + int(shape_start[-1] - shape_start[-2])
print("Shape counts per world:", world_shape_counts)
print("Global shape count:", global_shape_count)
.. testoutput::
world_count: 2
Shape starts: [1, 3, 5, 6]
Shape counts per world: [2, 2]
Global shape count: 2
.. _Convenience methods:
Convenience Methods: ``add_world`` and ``replicate``
----------------------------------------------------
While :meth:`~newton.ModelBuilder.begin_world` and :meth:`~newton.ModelBuilder.end_world` give full control, Newton provides two convenience methods for the most common multi-world patterns:
**add_world**: adds a pre-built :class:`~newton.ModelBuilder` as a new world in a single call (combines ``begin_world`` / :meth:`~newton.ModelBuilder.add_builder` / ``end_world``):
.. testcode::
import newton
# Build a simple two-link arm
arm = newton.ModelBuilder()
link0 = arm.add_link(mass=1.0)
j0 = arm.add_joint_fixed(parent=-1, child=link0)
link1 = arm.add_link(mass=1.0)
j1 = arm.add_joint_revolute(parent=link0, child=link1)
arm.add_articulation(joints=[j0, j1])
arm.add_shape_box(body=link0, hx=0.1, hy=0.1, hz=0.1)
arm.add_shape_box(body=link1, hx=0.1, hy=0.1, hz=0.1)
# Create a scene with two instances of the same arm
scene = newton.ModelBuilder()
scene.add_ground_plane()
scene.add_world(arm)
scene.add_world(arm)
multi_arm_model = scene.finalize()
print("world_count:", multi_arm_model.world_count)
.. testoutput::
world_count: 2
**replicate**: creates ``N`` copies of a builder, each as its own world, with optional spatial offsets.
.. tip::
Using physical ``spacing`` to separate replicated worlds moves bodies away from the origin,
which can reduce numerical stability. For visual separation, prefer using viewer-level world
offsets (e.g. ``viewer.set_world_offsets()``) while keeping ``spacing=(0, 0, 0)`` so that all
worlds remain at the origin in the physics simulation.
.. testcode::
import newton
arm = newton.ModelBuilder()
link0 = arm.add_link(mass=1.0)
j0 = arm.add_joint_fixed(parent=-1, child=link0)
link1 = arm.add_link(mass=1.0)
j1 = arm.add_joint_revolute(parent=link0, child=link1)
arm.add_articulation(joints=[j0, j1])
scene = newton.ModelBuilder()
scene.add_ground_plane()
scene.replicate(arm, world_count=4, spacing=(2.0, 0.0, 0.0))
replicated_model = scene.finalize()
print("world_count:", replicated_model.world_count)
print("body_count:", replicated_model.body_count)
.. testoutput::
world_count: 4
body_count: 8
.. _Per-world gravity:
Per-World Gravity
-----------------
Each world can have its own gravity vector, which is useful for simulating different environments
(e.g., Earth gravity in one world, lunar gravity in another).
Per-world gravity can be configured at build time via the ``gravity`` argument of :meth:`~newton.ModelBuilder.begin_world`,
or modified at runtime via :meth:`~newton.Model.set_gravity`:
.. note::
Global entities (world index ``-1``) use the gravity of world ``0``.
Keep this in mind when mixing global and world-specific entities with different gravity vectors.
.. testcode::
import warp as wp
import newton
robot_builder = newton.ModelBuilder()
robot_builder.add_body(mass=1.0)
scene = newton.ModelBuilder()
scene.add_world(robot_builder)
scene.add_world(robot_builder)
model = scene.finalize()
# Set different gravity for each world
model.set_gravity((0.0, 0.0, -9.81), world=0) # Earth
model.set_gravity((0.0, 0.0, -1.62), world=1) # Moon
print("Gravity shape:", model.gravity.numpy().shape)
.. testoutput::
Gravity shape: (2, 3)
.. _World-entity partitioning:
World-Entity GPU Thread Partitioning
------------------------------------
Another important use of world grouping is to facilitate partitioning of GPU thread grids according to both world indices and the number of entities per world, i.e. into 2D world-entity grids.
For example:
.. code-block:: python
import warp as wp
import newton
@wp.kernel
def world_body_2d_kernel(
body_world_start: wp.array(dtype=wp.int32),
body_qd: wp.array(dtype=wp.spatial_vectorf),
):
world_id, body_world_id = wp.tid()
world_start = body_world_start[world_id]
num_bodies_in_world = body_world_start[world_id + 1] - world_start
if body_world_id < num_bodies_in_world:
global_body_id = world_start + body_world_id
twist = body_qd[global_body_id]
# ... perform computations on twist ...
# Create model with multiple worlds
builder = newton.ModelBuilder()
# ... add entities to multiple worlds ...
model = builder.finalize()
# Define number of entities per world (e.g., bodies)
body_world_start = model.body_world_start.numpy()
num_bodies_per_world = [
body_world_start[i + 1] - body_world_start[i]
for i in range(model.world_count)
]
# Launch kernel with 2D grid: (world_count, max_entities_per_world)
state = model.state()
wp.launch(
world_body_2d_kernel,
dim=(model.world_count, max(num_bodies_per_world)),
inputs=[model.body_world_start, state.body_qd],
)
This kernel thread partitioning allows each thread to uniquely identify both the world it is operating on (via ``world_id``) and the relative entity index w.r.t that world (via ``body_world_id``).
The world-relative index is useful in certain operations such as accessing the body-specific column of constraint Jacobian matrices in maximal-coordinate formulations, which are stored in contiguous blocks per world.
This relative index can then be mapped to the global entity index within the model by adding the corresponding starting index from the ``*_world_start`` arrays.
Note that in the simpler case of a homogeneous model consisting of identical worlds, the ``max(num_bodies_per_world)`` reduces to a constant value, and this effectively becomes a *batched* operation.
For the more general heterogeneous case, the kernel needs to account for the varying number of entities per world, and an important pattern arises w.r.t 2D thread indexing and memory allocations that applies to all per-entity and per-world arrays.
Essentially, ``sum(num_bodies_per_world)`` equals the total number of *world-local* bodies (i.e. ``body_world_start[-2] - body_world_start[0]``), which excludes any global entities (world index ``-1``).
Note that ``model.body_count`` may be larger than this sum when global bodies are present, since it includes both world-local and global entities (see :attr:`~newton.Model.body_world_start` for the explicit distinction).
The maximum ``max(num_bodies_per_world)`` determines the second dimension of the 2D thread grid used to launch the kernel.
However, since different worlds may have different numbers of bodies, some threads in the 2D grid will be inactive for worlds with fewer bodies than the maximum.
Therefore, kernels need to check whether the relative entity index is within bounds for the current world before performing any operations, as shown in the example above.
This pattern of computing ``sum`` and ``max`` of per-world entity counts provides a consistent way to handle memory allocations and thread grid dimensions for heterogeneous multi-world simulations in Newton.
See Also
--------
* :class:`~newton.ModelBuilder`
* :class:`~newton.Model`
* :meth:`~newton.ModelBuilder.begin_world`
* :meth:`~newton.ModelBuilder.end_world`
* :meth:`~newton.ModelBuilder.add_world`
* :meth:`~newton.ModelBuilder.replicate`
* :meth:`~newton.Model.set_gravity`
================================================
FILE: docs/conf.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
# Configuration file for the Sphinx documentation builder.
#
# For the full list of built-in configuration values, see the documentation:
# https://www.sphinx-doc.org/en/master/usage/configuration.html
import datetime
import importlib
import inspect
import os
import re
import shutil
import sys
from pathlib import Path
from typing import Any
# Set environment variable to indicate we're in a Sphinx build.
# This is inherited by subprocesses (e.g., Jupyter kernels run by nbsphinx).
os.environ["NEWTON_SPHINX_BUILD"] = "1"
# Determine the Git version/tag from CI environment variables.
# 1. Check for GitHub Actions' variable.
# 2. Check for GitLab CI's variable.
# 3. Fallback to 'main' for local builds.
github_version = os.environ.get("GITHUB_REF_NAME") or os.environ.get("CI_COMMIT_REF_NAME") or "main"
# -- Project information -----------------------------------------------------
# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information
project = "Newton Physics"
copyright = f"{datetime.date.today().year}, The Newton Developers. Documentation licensed under CC-BY-4.0"
author = "The Newton Developers"
# Read version from pyproject.toml
# TODO: When minimum Python version is >=3.11, replace with:
# import tomllib
# with open(project_root / "pyproject.toml", "rb") as f:
# project_version = tomllib.load(f)["project"]["version"]
project_root = Path(__file__).parent.parent
try:
with open(project_root / "pyproject.toml", encoding="utf-8") as f:
content = f.read()
project_section = re.search(r"^\[project\]\s*\n(.*?)(?=^\[|\Z)", content, re.MULTILINE | re.DOTALL)
if not project_section:
raise ValueError("Could not find [project] section in pyproject.toml")
match = re.search(r'^version\s*=\s*"([^"]+)"', project_section.group(1), re.MULTILINE)
if not match:
raise ValueError("Could not find version in [project] section of pyproject.toml")
project_version = match.group(1)
except Exception as e:
print(f"Error reading version from pyproject.toml: {e}", file=sys.stderr)
sys.exit(1)
release = project_version
# -- Nitpicky mode -----------------------------------------------------------
# Set nitpicky = True to warn about all broken cross-references (e.g. missing
# intersphinx targets, typos in :class:/:func:/:attr: roles, etc.). Useful for
# auditing docs but noisy during regular development.
nitpicky = False
# -- General configuration ---------------------------------------------------
# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration
# Add docs/ and docs/_ext to Python import path so custom extensions and
# sibling scripts (e.g. generate_api) can be imported.
_docs_path = str(Path(__file__).parent)
if _docs_path not in sys.path:
sys.path.append(_docs_path)
_ext_path = Path(__file__).parent / "_ext"
if str(_ext_path) not in sys.path:
sys.path.append(str(_ext_path))
extensions = [
"myst_parser", # Parse markdown files
"nbsphinx", # Process Jupyter notebooks
"sphinx.ext.autodoc",
"sphinx.ext.napoleon", # Convert docstrings to reStructuredText
"sphinx.ext.intersphinx",
"sphinx.ext.autosummary",
"sphinx.ext.extlinks", # Markup to shorten external links
"sphinx.ext.githubpages",
"sphinx.ext.doctest", # Test code snippets in docs
"sphinx.ext.mathjax", # Math rendering support
"sphinx.ext.linkcode", # Add GitHub source links to documentation
"sphinxcontrib.mermaid",
"sphinx_copybutton",
"sphinx_design",
"sphinx_tabs.tabs",
"autodoc_filter",
"autodoc_wpfunc",
]
# -- nbsphinx configuration ---------------------------------------------------
# Configure notebook execution mode for nbsphinx
nbsphinx_execute = "auto"
# Timeout for notebook execution (in seconds)
nbsphinx_timeout = 600
# Allow errors in notebook execution (useful for development)
nbsphinx_allow_errors = False
templates_path = ["_templates"]
exclude_patterns = [
"_build",
"Thumbs.db",
".DS_Store",
"sphinx-env/**",
"sphinx-env",
"**/site-packages/**",
"**/lib/**",
]
# nbsphinx requires pandoc to convert Jupyter notebooks. When pandoc is not
# installed we exclude the notebook tutorials so the rest of the docs can still
# be built locally without a hard error. CI workflows install pandoc explicitly
# so published docs always include the tutorials.
#
# Set NEWTON_REQUIRE_PANDOC=1 to turn the missing-pandoc warning into an error
# (used in CI to guarantee tutorials are never silently skipped).
if shutil.which("pandoc") is None:
if os.environ.get("NEWTON_REQUIRE_PANDOC", "") == "1":
raise RuntimeError(
"pandoc is required but not found. Install pandoc "
"(https://pandoc.org/installing.html) or unset NEWTON_REQUIRE_PANDOC."
)
exclude_patterns.append("tutorials/**")
print(
"WARNING: pandoc not found - Jupyter notebook tutorials will be "
"skipped. Install pandoc (https://pandoc.org/installing.html) to "
"build the complete documentation."
)
intersphinx_mapping = {
"python": ("https://docs.python.org/3", None),
"numpy": ("https://numpy.org/doc/stable", None),
"jax": ("https://docs.jax.dev/en/latest", None),
"pytorch": ("https://docs.pytorch.org/docs/stable", None),
"warp": ("https://nvidia.github.io/warp", None),
"usd": ("https://docs.omniverse.nvidia.com/kit/docs/pxr-usd-api/latest", None),
}
# Map short USD type names (from ``from pxr import Usd``) to their fully-qualified
# ``pxr.*`` paths so intersphinx can resolve them against the USD inventory.
# Note: this only affects annotations processed by autodoc, not autosummary stubs.
autodoc_type_aliases = {
"Usd.Prim": "pxr.Usd.Prim",
"Usd.Stage": "pxr.Usd.Stage",
"UsdGeom.XformCache": "pxr.UsdGeom.XformCache",
"UsdGeom.Mesh": "pxr.UsdGeom.Mesh",
"UsdShade.Material": "pxr.UsdShade.Material",
"UsdShade.Shader": "pxr.UsdShade.Shader",
}
source_suffix = {
".rst": "restructuredtext",
".md": "markdown",
}
extlinks = {
"github": (f"https://github.com/newton-physics/newton/blob/{github_version}/%s", "%s"),
}
doctest_global_setup = """
from typing import Any
import numpy as np
import warp as wp
import newton
# Suppress warnings by setting warp_showwarning to an empty function
def empty_warning(*args, **kwargs):
pass
wp.utils.warp_showwarning = empty_warning
wp.config.quiet = True
wp.init()
"""
# -- Autodoc configuration ---------------------------------------------------
# put type hints inside the description instead of the signature (easier to read)
autodoc_typehints = "description"
# default argument values of functions will be not evaluated on generating document
autodoc_preserve_defaults = True
autodoc_typehints_description_target = "documented"
toc_object_entries_show_parents = "hide"
autodoc_default_options = {
"members": True,
"member-order": "groupwise",
"special-members": "__init__",
"undoc-members": False,
"exclude-members": "__weakref__",
"imported-members": True,
"autosummary": True,
}
# fixes errors with Enum docstrings
autodoc_inherit_docstrings = False
# Mock imports for modules that are not installed by default
autodoc_mock_imports = ["jax", "torch", "paddle"]
autosummary_generate = True
autosummary_ignore_module_all = False
autosummary_imported_members = True
# -- Options for HTML output -------------------------------------------------
# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output
html_title = "Newton Physics"
html_theme = "pydata_sphinx_theme"
html_static_path = ["_static"]
html_css_files = ["custom.css"]
html_show_sourcelink = False
# PyData theme configuration
html_theme_options = {
# Remove navigation from the top navbar
# "navbar_start": ["navbar-logo"],
# "navbar_center": [],
# "navbar_end": ["search-button"],
# Navigation configuration
# "font_size": "14px", # or smaller
"navigation_depth": 1,
"show_nav_level": 1,
"show_toc_level": 2,
"collapse_navigation": False,
# Show the indices in the sidebar
"show_prev_next": False,
"use_edit_page_button": False,
"logo": {
"image_light": "_static/newton-logo-light.png",
"image_dark": "_static/newton-logo-dark.png",
"text": f"Newton Physics ({release}) ",
"alt_text": "Newton Physics Logo",
},
# Keep the right-hand page TOC on by default, but remove it on the
# solver API page where several wide comparison tables benefit from the
# extra content width.
"secondary_sidebar_items": {
"**": ["page-toc", "edit-this-page", "sourcelink"],
"api/newton_solvers": [],
},
# "primary_sidebar_end": ["indices.html", "sidebar-ethical-ads.html"],
}
html_sidebars = {"**": ["sidebar-nav-bs.html"], "index": ["sidebar-nav-bs.html"]}
# Version switcher configuration for multi-version docs on GitHub Pages
# See: https://pydata-sphinx-theme.readthedocs.io/en/stable/user_guide/version-dropdown.html
# Determine if we're in a CI build and which version
_is_ci = os.environ.get("GITHUB_ACTIONS") == "true"
_is_release = os.environ.get("GITHUB_REF", "").startswith("refs/tags/v")
# Configure version switcher
html_theme_options["switcher"] = {
"json_url": "https://newton-physics.github.io/newton/switcher.json",
"version_match": release if _is_release else "dev",
}
# Add version switcher to navbar
html_theme_options["navbar_end"] = ["theme-switcher", "version-switcher", "navbar-icon-links"]
# Footer configuration — show copyright (includes CC-BY-4.0 notice)
html_theme_options["footer_start"] = ["copyright"]
html_theme_options["footer_end"] = ["theme-version"]
# Disable switcher JSON validation during local builds (file not accessible locally)
if not _is_ci:
html_theme_options["check_switcher"] = False
# -- Math configuration -------------------------------------------------------
# MathJax configuration for proper LaTeX rendering
mathjax3_config = {
"tex": {
"packages": {"[+]": ["amsmath", "amssymb", "amsfonts"]},
"inlineMath": [["$", "$"], ["\\(", "\\)"]],
"displayMath": [["$$", "$$"], ["\\[", "\\]"]],
"processEscapes": True,
"processEnvironments": True,
"tags": "ams",
"macros": {
"RR": "{\\mathbb{R}}",
"bold": ["{\\mathbf{#1}}", 1],
"vec": ["{\\mathbf{#1}}", 1],
},
},
"options": {
"processHtmlClass": ("tex2jax_process|mathjax_process|math|output_area"),
"ignoreHtmlClass": "annotation",
},
}
# -- Linkcode configuration --------------------------------------------------
# create back links to the Github Python source file
# called automatically by sphinx.ext.linkcode
def linkcode_resolve(domain: str, info: dict[str, str]) -> str | None:
"""
Determine the URL corresponding to Python object using introspection
"""
if domain != "py":
return None
if not info["module"]:
return None
module_name = info["module"]
# Only handle newton modules
if not module_name.startswith("newton"):
return None
try:
# Import the module and get the object
module = importlib.import_module(module_name)
if "fullname" in info:
# Get the specific object (function, class, etc.)
obj_name = info["fullname"]
if hasattr(module, obj_name):
obj = getattr(module, obj_name)
else:
return None
else:
# No specific object, link to the module itself
obj = module
# Get the file where the object is actually defined
source_file = None
line_number = None
try:
source_file = inspect.getfile(obj)
# Get line number if possible
try:
_, line_number = inspect.getsourcelines(obj)
except (TypeError, OSError):
pass
except (TypeError, OSError):
# Check if it's a Warp function with wrapped original function
if hasattr(obj, "func") and callable(obj.func):
try:
original_func = obj.func
source_file = inspect.getfile(original_func)
try:
_, line_number = inspect.getsourcelines(original_func)
except (TypeError, OSError):
pass
except (TypeError, OSError):
pass
# If still no source file, fall back to the module file
if not source_file:
try:
source_file = inspect.getfile(module)
except (TypeError, OSError):
return None
if not source_file:
return None
# Convert absolute path to relative path from project root
project_root = os.path.dirname(os.path.dirname(__file__))
rel_path = os.path.relpath(source_file, project_root)
# Normalize path separators for URLs
rel_path = rel_path.replace("\\", "/")
# Add line fragment if we have a line number
line_fragment = f"#L{line_number}" if line_number else ""
# Construct GitHub URL
github_base = "https://github.com/newton-physics/newton"
return f"{github_base}/blob/{github_version}/{rel_path}{line_fragment}"
except (ImportError, AttributeError, TypeError):
return None
def _copy_viser_client_into_output_static(*, outdir: Path) -> None:
"""Ensure the Viser web client assets are available at `{outdir}/_static/viser/`.
This avoids relying on repo-relative `html_static_path` entries (which can break under `uv`),
and avoids writing generated assets into `docs/_static` in the working tree.
"""
dest_dir = outdir / "_static" / "viser"
src_candidates: list[Path] = []
# Repo checkout layout (most common for local builds).
src_candidates.append(project_root / "newton" / "_src" / "viewer" / "viser" / "static")
# Installed package layout (e.g. building docs from an environment where `newton` is installed).
try:
import newton # noqa: PLC0415
src_candidates.append(Path(newton.__file__).resolve().parent / "_src" / "viewer" / "viser" / "static")
except Exception:
pass
src_dir = next((p for p in src_candidates if (p / "index.html").is_file()), None)
if src_dir is None:
# Don't hard-fail doc builds; the viewer docs can still build without the embedded client.
expected = ", ".join(str(p) for p in src_candidates)
print(
f"Warning: could not find Viser client assets to copy. Expected `index.html` under one of: {expected}",
file=sys.stderr,
)
return
dest_dir.mkdir(parents=True, exist_ok=True)
shutil.copytree(src_dir, dest_dir, dirs_exist_ok=True)
def _on_builder_inited(_app: Any) -> None:
outdir = Path(_app.builder.outdir)
_copy_viser_client_into_output_static(outdir=outdir)
def setup(app: Any) -> None:
# Regenerate API .rst files so builds always reflect the current public API.
from generate_api import generate_all # noqa: PLC0415
generate_all()
app.connect("builder-inited", _on_builder_inited)
================================================
FILE: docs/faq.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Frequently Asked Questions (FAQ)
================================
This FAQ addresses general questions about Newton. For technical questions and answers, please refer to `GitHub Discussions `_.
What is Newton?
---------------
Newton is an open-source, GPU-accelerated, extensible, and differentiable physics engine for robotics built on `NVIDIA Warp `_, initially focused on high-performance robot learning and `MuJoCo Warp `_ integration.
Newton supports OpenUSD, URDF, and MJCF asset formats and provides multiple solver backends within a unified architecture for simulation, learning, and extensibility.
Newton is a `Linux Foundation `_ project initiated by `Disney Research `_, `Google DeepMind `_, and `NVIDIA `_. The project is community-built and maintained under the permissive `Apache-2.0 license `_.
What is the difference between Warp and Newton?
-----------------------------------------------
`Warp `_ is a Python framework for writing high-performance, differentiable GPU kernels for physics simulation and spatial computing. Newton is a full physics engine built on Warp that adds high-level simulation APIs, interchangeable solvers, and asset I/O for robotics.
What is the difference between ``warp.sim`` and Newton?
-------------------------------------------------------
``warp.sim`` was the predecessor to Newton, developed by NVIDIA as a module in Warp. It was deprecated in Warp 1.8 and removed in Warp 1.10. See the :doc:`migration` to Newton.
Does Newton support coupling of solvers for multiphysics or co-simulation?
--------------------------------------------------------------------------
Yes, Newton is explicitly designed to be extensible with multiple solver implementations for rich multiphysics scenarios. Newton provides APIs for users to implement coupling between solvers, and we have successfully demonstrated one-way coupling in examples such as cloth manipulation by a robotic arm and a quadruped walking through non-rigid terrain. Two-way coupling and implicit coupling between select solvers are on the Newton roadmap.
Does Newton support MuJoCo simulation?
--------------------------------------
Newton leverages `MuJoCo Warp `_ as a key solver, which is a reimplementation of MuJoCo in Warp for GPU acceleration, developed and maintained by Google DeepMind and NVIDIA.
Newton can import assets in MJCF, URDF, and OpenUSD formats, making it compatible with MuJoCo at both asset and solver levels.
Is Newton exposed and accessible in Isaac Lab and Isaac Sim?
------------------------------------------------------------
Yes, an experimental Newton integration is available in Isaac Lab and under active development. Initial training environments include quadruped and biped locomotion, and basic manipulation. Read more on `the integration `_.
Newton integration with Isaac Sim as a physics backend is under development.
Is Newton a standalone framework?
---------------------------------
Yes, Newton and its modern Python API can be used as a standalone simulation framework. See the :doc:`api/newton` or the `Quickstart Guide `_ for more information.
Does Newton provide visualization capabilities?
-----------------------------------------------
Newton provides basic visualization for debugging purposes. Read more in the :doc:`guide/visualization` Guide.
For rich real-time graphics, users commonly pair Newton with Isaac Lab, which provides advanced rendering. Users can also export simulation outputs to a time-sampled USD that can be visualized, for example, in `NVIDIA Omniverse `_ or `Isaac Sim `_.
How can I contribute to Newton?
-------------------------------
Newton welcomes community contributions. Please see the `Contribution Guide `_ and :doc:`guide/development` Guide for more information.
Can I use Newton to develop my own custom solver?
-------------------------------------------------
Yes, Newton is designed to be highly extensible, supporting custom solvers, integrators, and numerical methods within its modular architecture.
What is PhysX?
--------------
`PhysX `_ is an open-source, multi-physics SDK that provides scalable simulation across CPUs and GPUs, widely used for industrial digital-twin simulation in Omniverse, and robotics simulation in Isaac Sim and Isaac Lab.
It features a unified simulation framework for reduced-coordinate articulations and rigid bodies, deformable bodies and cloth (FEM), fluids/particles (PBD), vehicle dynamics, and character controllers.
Will Newton replace PhysX?
--------------------------
No, the two engines serve different primary goals: Newton targets robot learning and extensible multiphysics with differentiability, while PhysX focuses on industrial digital-twin simulation as a mature, multi-platform real-time physics SDK that is actively maintained and updated.
Isaac Lab's experimental Newton integration does not support PhysX, but Isaac Lab plans to continue supporting PhysX as a simulation backend.
Can PhysX work in Newton?
-------------------------
No. However, different approaches for supporting PhysX as a Newton solver option are under consideration.
================================================
FILE: docs/generate_api.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Generate concise API .rst files for selected modules.
This helper scans a list of *top-level* modules, reads their ``__all__`` lists
(and falls back to public attributes if ``__all__`` is missing), and writes one
reStructuredText file per module with an ``autosummary`` directive. When
Sphinx later builds the documentation (with ``autosummary_generate = True``),
individual stub pages will be created automatically for every listed symbol.
The generated files live in ``docs/api/`` (git-ignored by default).
Usage (from the repository root):
python docs/generate_api.py
Adjust ``MODULES`` below to fit your project.
"""
from __future__ import annotations
import importlib
import inspect
import pkgutil
import shutil
import sys
from pathlib import Path
from types import ModuleType
import warp as wp # type: ignore
# -----------------------------------------------------------------------------
# Configuration
# -----------------------------------------------------------------------------
# Add project root to import path so that `import newton` works when the script
# is executed from the repository root without installing the package.
REPO_ROOT = Path(__file__).resolve().parent.parent
sys.path.insert(0, str(REPO_ROOT))
# Modules for which we want API pages. Feel free to modify.
MODULES: list[str] = [
"newton",
"newton.geometry",
"newton.ik",
"newton.math",
"newton.selection",
"newton.sensors",
"newton.solvers",
"newton.usd",
"newton.utils",
"newton.viewer",
]
# Output directory (relative to repo root)
OUTPUT_DIR = REPO_ROOT / "docs" / "api"
# Where autosummary should place generated stub pages (relative to each .rst
# file). Keeping them alongside the .rst files avoids clutter elsewhere.
TOCTREE_DIR = "_generated" # sub-folder inside OUTPUT_DIR
# -----------------------------------------------------------------------------
# Helpers
# -----------------------------------------------------------------------------
def public_symbols(mod: ModuleType) -> list[str]:
"""Return the list of public names for *mod* (honours ``__all__``)."""
if hasattr(mod, "__all__") and isinstance(mod.__all__, list | tuple):
return list(mod.__all__)
def is_public(name: str) -> bool:
if name.startswith("_"):
return False
return not inspect.ismodule(getattr(mod, name))
return sorted(filter(is_public, dir(mod)))
def _is_solver_only_module(mod: ModuleType) -> bool:
"""Return True when the module only exposes its solver class."""
names = getattr(mod, "__all__", None)
public = list(names) if isinstance(names, (list, tuple)) else public_symbols(mod)
return len(public) == 1 and public[0].startswith("Solver")
def solver_submodule_pages() -> list[str]:
"""Return solver submodules that expose more than the solver class."""
modules: list[str] = []
solvers_pkg = importlib.import_module("newton._src.solvers")
public_solvers = importlib.import_module("newton.solvers")
for info in pkgutil.iter_modules(solvers_pkg.__path__):
if not info.ispkg:
continue
if not hasattr(public_solvers, info.name):
continue
internal_name = f"{solvers_pkg.__name__}.{info.name}"
try:
mod = importlib.import_module(internal_name)
except Exception:
# Optional dependency missing; skip doc generation for this solver.
continue
if _is_solver_only_module(mod):
continue
public_name = f"newton.solvers.{info.name}"
modules.append(public_name)
return modules
def write_module_page(mod_name: str) -> None:
"""Create an .rst file for *mod_name* under *OUTPUT_DIR*."""
is_solver_submodule = mod_name.startswith("newton.solvers.") and mod_name != "newton.solvers"
if is_solver_submodule:
sub_name = mod_name.split(".", 2)[2]
module = importlib.import_module(f"newton._src.solvers.{sub_name}")
else:
module = importlib.import_module(mod_name)
symbols = public_symbols(module)
if is_solver_submodule:
# Keep solver classes centralized in newton.solvers.
symbols = [name for name in symbols if not name.startswith("Solver")]
classes: list[str] = []
functions: list[str] = []
constants: list[str] = []
modules: list[str] = []
for name in symbols:
attr = getattr(module, name)
# ------------------------------------------------------------------
# Class-like objects
# ------------------------------------------------------------------
if inspect.isclass(attr) or wp.types.type_is_struct(attr):
classes.append(name)
continue
# ------------------------------------------------------------------
# Constants / simple values
# ------------------------------------------------------------------
if wp.types.type_is_value(type(attr)) or isinstance(attr, str):
constants.append(name)
continue
# ------------------------------------------------------------------
# Submodules
# ------------------------------------------------------------------
if inspect.ismodule(attr):
modules.append(name)
continue
# ------------------------------------------------------------------
# Everything else → functions section
# ------------------------------------------------------------------
functions.append(name)
title = mod_name
underline = "=" * len(title)
lines: list[str] = [
".. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers",
".. SPDX-License-Identifier: CC-BY-4.0",
"",
title,
underline,
"",
]
# Module docstring if available
doc = (module.__doc__ or "").strip()
if doc:
lines.extend([doc, ""])
lines.extend([f".. py:module:: {mod_name}", f".. currentmodule:: {mod_name}", ""])
# Render a simple bullet list of submodules (no autosummary/toctree) to
# avoid generating stub pages that can cause duplicate descriptions.
if modules and not is_solver_submodule:
modules.sort()
lines.extend(
[
".. toctree::",
" :hidden:",
"",
]
)
for sub in modules:
modname = f"{mod_name}.{sub}"
docname = modname.replace(".", "_")
lines.append(f" {docname}")
lines.append("")
lines.extend([".. rubric:: Submodules", ""])
# Link to sibling generated module pages without creating autosummary stubs.
for sub in modules:
modname = f"{mod_name}.{sub}"
docname = modname.replace(".", "_")
lines.append(f"- :doc:`{modname} <{docname}>`")
lines.append("")
if classes:
classes.sort()
lines.extend([".. rubric:: Classes", ""])
if is_solver_submodule:
for cls in classes:
lines.extend([f".. autoclass:: {cls}", ""])
else:
lines.extend(
[
".. autosummary::",
f" :toctree: {TOCTREE_DIR}",
" :nosignatures:",
"",
]
)
lines.extend([f" {cls}" for cls in classes])
lines.append("")
if functions:
functions.sort()
lines.extend([".. rubric:: Functions", ""])
if is_solver_submodule:
for fn in functions:
lines.extend([f".. autofunction:: {fn}", ""])
else:
lines.extend(
[
".. autosummary::",
f" :toctree: {TOCTREE_DIR}",
" :signatures: long",
"",
]
)
lines.extend([f" {fn}" for fn in functions])
lines.append("")
if constants:
constants.sort()
lines.extend(
[
".. rubric:: Constants",
"",
".. list-table::",
" :header-rows: 1",
"",
" * - Name",
" - Value",
]
)
for const in constants:
value = getattr(module, const, "?")
# unpack the warp scalar value, we can remove this
# when the warp.types.scalar_base supports __str__()
if type(value) in wp.types.scalar_types:
value = getattr(value, "value", value)
lines.extend(
[
f" * - ``{const}``",
f" - ``{value}``",
]
)
lines.append("")
outfile = OUTPUT_DIR / f"{mod_name.replace('.', '_')}.rst"
outfile.parent.mkdir(parents=True, exist_ok=True)
outfile.write_text("\n".join(lines), encoding="utf-8")
print(f"Wrote {outfile.relative_to(REPO_ROOT)} ({len(symbols)} symbols)")
# -----------------------------------------------------------------------------
# Public entry point
# -----------------------------------------------------------------------------
def generate_all() -> None:
"""Regenerate all API ``.rst`` files under :data:`OUTPUT_DIR`."""
# delete previously generated files
shutil.rmtree(OUTPUT_DIR, ignore_errors=True)
extra_solver_modules = solver_submodule_pages()
all_modules = MODULES + [mod for mod in extra_solver_modules if mod not in MODULES]
for mod in all_modules:
write_module_page(mod)
# -----------------------------------------------------------------------------
# Script entry
# -----------------------------------------------------------------------------
if __name__ == "__main__":
generate_all()
print("\nDone. Add docs/api/index.rst to your TOC or glob it in.")
================================================
FILE: docs/guide/development.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Development
===========
This document is a guide for developers who want to contribute to the project or understand its internal workings in more detail.
Please refer to `CONTRIBUTING.md `_ for how to best contribute to Newton and relevant legal information (CLA).
Installation
------------
For regular end-user installation, see the :doc:`installation` guide.
To install Newton from source for development or contribution, first clone the
repository:
.. code-block:: console
git clone https://github.com/newton-physics/newton.git
cd newton
Method 1: Using uv (Recommended)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Install `uv `_ if you don't have it already:
.. tab-set::
:sync-group: os
.. tab-item:: macOS / Linux
:sync: linux
.. code-block:: console
curl -LsSf https://astral.sh/uv/install.sh | sh
.. tab-item:: Windows
:sync: windows
.. code-block:: console
powershell -ExecutionPolicy ByPass -c "irm https://astral.sh/uv/install.ps1 | iex"
Then create a local project environment with the ``dev`` dependency extras:
.. code-block:: console
uv sync --extra dev
After syncing, the ``dev`` extras are available to all ``uv run`` commands
without needing to pass ``--extra dev`` each time. For example, to list all
available examples:
.. code-block:: console
uv run -m newton.examples
See the :ref:`extra-dependencies` section of the installation guide for a
description of all available extras.
Method 2: Using pip in a Virtual Environment
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
To manually manage a virtual environment, create and activate one first:
.. tab-set::
:sync-group: os
.. tab-item:: macOS / Linux
:sync: linux
.. code-block:: console
python -m venv .venv
source .venv/bin/activate
.. tab-item:: Windows (console)
:sync: windows
.. code-block:: console
python -m venv .venv
.venv\Scripts\activate.bat
.. tab-item:: Windows (PowerShell)
:sync: windows-ps
.. code-block:: console
python -m venv .venv
.venv\Scripts\Activate.ps1
Then locally install Newton in editable mode with its development dependencies:
.. code-block:: console
pip install -e ".[dev]" --extra-index-url https://pypi.nvidia.com/
The ``--extra-index-url`` flag points pip to the NVIDIA package index, which is
required to find ``warp-lang`` versions newer than those available on PyPI.
Python Dependency Management
----------------------------
uv lockfile management
^^^^^^^^^^^^^^^^^^^^^^
When using uv, the `lockfile `__
(``uv.lock``) is used to resolve project dependencies into exact versions for reproducibility among different machines.
We maintain a lockfile in the root of the repository that pins exact versions of all dependencies and their transitive dependencies.
Sometimes, a dependency in the lockfile needs to be updated to a newer version.
This can be done by running ``uv lock -P ``:
.. code-block:: console
uv lock -P warp-lang --prerelease allow
uv lock -P mujoco-warp
The ``--prerelease allow`` flag is needed for dependencies that use pre-release versions (e.g. ``warp-lang``).
uv also provides a command to update all dependencies in the lockfile:
.. code-block:: console
uv lock -U
Remember to commit ``uv.lock`` after running a command that updates the lockfile.
Running the tests
-----------------
The Newton test suite supports both ``uv`` and standard ``venv`` workflows,
and by default runs in up to eight parallel processes. The tests can be run
in a serial manner with ``--serial-fallback``.
Pass ``--help`` to either run method below to see all available flags.
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
# install development extras and run tests
uv run --extra dev -m newton.tests
.. tab-item:: venv
:sync: venv
.. code-block:: console
# install dev extras (including testing & coverage deps)
python -m pip install -e ".[dev]"
# run tests
python -m newton.tests
Most tests run when the ``dev`` extras are installed. The tests using PyTorch
to run inference on an RL policy are skipped if the ``torch`` dependency is
not installed. In order to run these tests, include the ``torch-cu12`` or
``torch-cu13`` extras matching your NVIDIA driver's CUDA support:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
# install development extras and run tests
uv run --extra dev --extra torch-cu12 -m newton.tests
.. tab-item:: venv
:sync: venv
.. code-block:: console
# install both dev and torch-cu12 extras (need to pull from PyTorch CUDA 12.8 wheel index)
python -m pip install --extra-index-url https://download.pytorch.org/whl/cu128 -e ".[dev,torch-cu12]"
# run tests
python -m newton.tests
Specific Newton examples can be tested in isolation via the ``-k`` argument:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
# test the basic_shapes example
uv run --extra dev -m newton.tests.test_examples -k test_basic.example_basic_shapes
.. tab-item:: venv
:sync: venv
.. code-block:: console
# test the basic_shapes example
python -m newton.tests.test_examples -k test_basic.example_basic_shapes
To generate a coverage report:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
# append the coverage flags:
uv run --extra dev -m newton.tests --coverage --coverage-html htmlcov
.. tab-item:: venv
:sync: venv
.. code-block:: console
# append the coverage flags and make sure `coverage[toml]` is installed (it comes in `[dev]`)
python -m newton.tests --coverage --coverage-html htmlcov
The file ``htmlcov/index.html`` can be opened with a web browser to view the coverage report.
Code formatting and linting
---------------------------
`Ruff `_ is used for Python linting and code formatting.
`pre-commit `_ can be used to ensure that local code complies with Newton's checks.
From the top of the repository, run:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
uvx pre-commit run -a
.. tab-item:: venv
:sync: venv
.. code-block:: console
python -m pip install pre-commit
pre-commit run -a
To automatically run pre-commit hooks with ``git commit``:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
uvx pre-commit install
.. tab-item:: venv
:sync: venv
.. code-block:: console
pre-commit install
The hooks can be uninstalled with ``pre-commit uninstall``.
Typos
-----
To proactively catch spelling mistakes, Newton uses the `typos `_ tool. Typos scans source files for common misspellings and is integrated into our pre-commit hooks, so spelling errors in both code and documentation are flagged when you run or install pre-commit (see above). You can also run ``typos`` manually if needed. Refer to the `typos documentation `_ for more details on usage and configuration options.
Dealing with false positives
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Typos may occasionally flag legitimate project-specific terminology, domain terms, or variable names as misspellings (false positives). To handle these, the Newton codebase configures typos in ``pyproject.toml`` at the repository root.
False positives are managed as follows:
- **File exclusions:** The ``[tool.typos]`` section includes ``files.extend-exclude`` to ignore matching files and directories, such as ``examples/assets`` and specific model or asset file types (e.g., ``*.urdf``, ``*.usd``).
- **Word allowlist:** Words or acronyms that would otherwise be flagged can be listed in ``[tool.typos.default.extend-words]`` (e.g., ``ba``, ``HAA``).
- **Identifier allowlist:** Specific identifiers, such as variable or constant names, can be declared in ``[tool.typos.default.extend-identifiers]`` (e.g., ``PNGs``).
When typos reports a word that is valid within the Newton codebase, you can add it to the appropriate section in ``pyproject.toml`` to suppress future warnings. After updating, re-run typos (or pre-commit) to confirm that the word is ignored. Use these options to keep the codebase clean while ensuring needed flexibility for accepted project-specific words and identifiers.
License headers
---------------
Every source file in the repository must carry a 2-line `SPDX `_ license
header. The project's Apache-2.0 license is in ``LICENSE.md`` at the repository root, and additional and third-party license texts are available in the ``newton/licenses`` directory, so no further
boilerplate is required in individual files.
The required headers depend on the file type:
- **Python files** (``.py``):
.. code-block:: python
# SPDX-FileCopyrightText: Copyright (c) The Newton Developers
# SPDX-License-Identifier: Apache-2.0
- **Documentation files** (``.rst``) — CC-BY-4.0:
.. code-block:: rst
.. SPDX-FileCopyrightText: Copyright (c) The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
- **Jupyter notebooks** (``.ipynb``) — CC-BY-4.0 (plain text in the first cell, no comment prefix):
.. code-block:: text
SPDX-FileCopyrightText: Copyright (c) The Newton Developers
SPDX-License-Identifier: CC-BY-4.0
Use the year the file was **first created**. Do not update the year when modifying an
existing file, and do not use year ranges — git history is the authoritative record of
when changes were made.
A CI check (``pr_license_check.yml``) enforces headers on every pull request using
`Apache SkyWalking Eyes `_.
To run the license checks locally with Docker before pushing:
.. code-block:: console
# Check Python source headers (Apache-2.0)
docker run -it --rm -v $(pwd):/github/workspace apache/skywalking-eyes header check
# Check documentation headers (CC-BY-4.0)
docker run -it --rm -v $(pwd):/github/workspace apache/skywalking-eyes -c .licenserc-docs.yaml header check
Using a local Warp installation with uv
---------------------------------------
Use the following steps to run Newton with a local build of Warp:
.. code-block:: console
uv venv
source .venv/bin/activate
uv sync --extra dev
uv pip install -e "warp-lang @ ../warp"
The Warp initialization message should then properly reflect the local Warp installation instead of the locked version,
e.g. when running ``python -m newton.examples basic_pendulum``.
.. _building-the-documentation:
Building the documentation
--------------------------
To build the documentation locally, ensure you have the documentation dependencies installed.
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
rm -rf docs/_build
uv run --extra docs --extra sim sphinx-build -j auto -W -b html docs docs/_build/html
.. tab-item:: venv
:sync: venv
.. code-block:: console
python -m pip install -e ".[docs]"
cd path/to/newton/docs && make html
The built documentation will be available in ``docs/_build/html``.
.. note::
The documentation build requires `pandoc `_ for converting Jupyter notebooks.
While ``pypandoc_binary`` is included in the ``[docs]`` dependencies, some systems may require
pandoc to be installed separately:
- **Ubuntu/Debian:** ``sudo apt-get install pandoc``
- **macOS:** ``brew install pandoc``
- **Windows:** Download from https://pandoc.org/installing.html or ``choco install pandoc``
Serving the documentation locally
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
After building the documentation, you can serve it locally using the ``docs/serve.py`` script.
This is particularly useful for testing interactive features like the Viser 3D visualizations
in the tutorial notebooks, which require proper MIME types for WebAssembly and JavaScript modules.
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
uv run docs/serve.py
.. tab-item:: venv
:sync: venv
.. code-block:: console
python docs/serve.py
Then open http://localhost:8000 in your browser. You can specify a custom port with ``--port``:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
uv run docs/serve.py --port 8080
.. tab-item:: venv
:sync: venv
.. code-block:: console
python docs/serve.py --port 8080
.. note::
Using Python's built-in ``http.server`` or simply opening the HTML files directly
will not work correctly for the interactive Viser visualizations, as they require
specific CORS headers and MIME types that ``serve.py`` provides.
Documentation Versioning
------------------------
Newton's documentation is versioned and hosted on GitHub Pages. Multiple versions
are available simultaneously, with a version switcher dropdown in the navigation bar.
How It Works
^^^^^^^^^^^^
The ``gh-pages`` branch contains versioned documentation in subdirectories:
.. code-block:: text
/
├── index.html # Redirects to /stable/
├── switcher.json # Version manifest for dropdown
├── stable/ # Copy of latest release
├── latest/ # Dev docs from main branch
├── 1.1.0/ # Release versions
└── 1.0.0/
Two GitHub Actions workflows manage deployment:
- **docs-dev.yml**: Deploys to ``/latest/`` on every push to ``main``
- **docs-release.yml**: Deploys to ``/X.Y.Z/`` and updates ``/stable/`` on version tags
Deploying Documentation
^^^^^^^^^^^^^^^^^^^^^^^
**Dev docs** are deployed automatically when changes are pushed to ``main``.
**Release docs** are deployed when a version tag is pushed:
.. code-block:: bash
git tag v1.0.0
git push origin v1.0.0
Only strict semver tags (``vX.Y.Z``) trigger release deployments. Pre-release tags
like ``v1.0.0-rc.1`` are ignored.
Manual Operations
^^^^^^^^^^^^^^^^^
**Removing a version** (rare):
1. Check out the ``gh-pages`` branch
2. Delete the version directory (e.g., ``rm -rf 1.0.0``)
3. Edit ``switcher.json`` to remove the entry
4. Commit and push
**Rebuilding all docs** (disaster recovery): Check out each version tag, build its
docs with Sphinx, and deploy to the corresponding directory on ``gh-pages``. Update
``switcher.json`` after each version using ``scripts/ci/update_docs_switcher.py``.
API documentation
-----------------
Newton's API reference is auto-generated from the ``__all__`` lists of its public modules.
The script ``docs/generate_api.py`` produces reStructuredText files under ``docs/api/`` (git-ignored)
that Sphinx processes via ``autosummary`` to create individual pages for every public symbol.
Whenever you add, remove, or rename a public symbol in one of the public modules
(``newton``, ``newton.geometry``, ``newton.solvers``, ``newton.sensors``, etc.),
regenerate the API pages:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
uv run python docs/generate_api.py
.. tab-item:: venv
:sync: venv
.. code-block:: console
python docs/generate_api.py
After running the script, rebuild the documentation to verify the result (see
:ref:`building-the-documentation` above).
.. note::
Only symbols listed in a module's ``__all__`` (or, as a fallback, its public
attributes) are included. If a new class or function in ``newton/_src/`` should
be visible to users, re-export it through the appropriate public module first.
Testing documentation code snippets
-----------------------------------
The ``doctest`` Sphinx builder is used to ensure that code snippets in the documentation remain up-to-date.
The doctests can be run with:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
uv run --extra docs --extra sim sphinx-build -j auto -W -b doctest docs docs/_build/doctest
.. tab-item:: venv
:sync: venv
.. code-block:: console
python -m sphinx -j auto -W -b doctest docs docs/_build/doctest
For more information, see the `sphinx.ext.doctest `__
documentation.
Changelog
---------
Newton maintains a ``CHANGELOG.md`` at the repository root.
When a pull request modifies user-facing behavior, add an entry under the
``[Unreleased]`` section in the appropriate category:
- **Added** — new features
- **Changed** — changes to existing functionality (include migration guidance)
- **Deprecated** — features that will be removed (include migration guidance,
e.g. "Deprecate ``Model.geo_meshes`` in favor of ``Model.shapes``")
- **Removed** — removed features (include migration guidance)
- **Fixed** — bug fixes
Use imperative present tense ("Add X", not "Added X") and keep entries concise.
Internal implementation details (refactors, CI tweaks) that do not affect users
should **not** be listed.
Style Guide
-----------
- Follow PEP 8 for Python code.
- Use Google-style docstrings (compatible with Napoleon extension).
- Write clear, concise commit messages.
- Keep pull requests focused on a single feature or bug fix.
- Use kebab-case instead of snake_case for command line arguments, e.g. ``--use-cuda-graph`` instead of ``--use_cuda_graph``.
Writing examples
----------------
Examples live in ``newton/examples//example__.py`` (e.g.
``newton/examples/basic/example_basic_pendulum.py``). Each file defines an ``Example``
class with the following interface:
.. code-block:: python
class Example:
def __init__(self, viewer, args):
"""Build the model, create solver/state/control, and set up the viewer."""
...
def step(self):
"""Advance the simulation by one frame (typically with substeps)."""
...
def render(self):
"""Update the viewer with the current state."""
...
def test_final(self):
"""Validate the final simulation state. Required for CI."""
...
def test_post_step(self):
"""Optional per-step validation, called after every step() in test mode."""
...
Every example **must** implement ``test_final()`` (or ``test_post_step()``, or both).
The test harness runs examples with ``--viewer null --test`` and calls these methods to
verify simulation correctness. An example that implements neither will raise
``NotImplementedError`` in CI.
Discovery and registration
^^^^^^^^^^^^^^^^^^^^^^^^^^
Examples are discovered automatically: any file matching
``newton/examples//example_*.py`` is picked up by ``newton.examples.get_examples()``.
The short name used on the command line is the filename without the ``example_`` prefix and
``.py`` extension (e.g. ``basic_pendulum``).
New examples must also be registered in the examples ``README.md`` with a
``python -m newton.examples `` command and a 320x320 jpg screenshot.
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
# list all available examples
uv run -m newton.examples
# run an example by short name
uv run -m newton.examples basic_pendulum
# run in headless test mode (used by CI)
uv run -m newton.examples basic_pendulum --viewer null --test
.. tab-item:: venv
:sync: venv
.. code-block:: console
# list all available examples
python -m newton.examples
# run an example by short name
python -m newton.examples basic_pendulum
# run in headless test mode (used by CI)
python -m newton.examples basic_pendulum --viewer null --test
Asset version pinning
---------------------
Several Newton tests and examples rely on external assets hosted in separate Git
repositories. To ensure that any given Newton commit always downloads the same
asset versions, each repository is pinned to a specific commit SHA. The pinned
revisions are defined as constants in ``newton/_src/utils/download_assets.py``:
- ``NEWTON_ASSETS_REF`` — pinned SHA for the ``newton-assets`` repository
- ``MENAGERIE_REF`` — pinned SHA for the ``mujoco_menagerie`` repository
Updating pinned revisions
^^^^^^^^^^^^^^^^^^^^^^^^^
When upstream assets change and the new versions need to be adopted:
1. Look up the new commit SHA from the asset repository.
2. Update the corresponding ``*_REF`` constant in ``download_assets.py``.
3. Run the full test suite to verify that no tests break with the new assets.
4. Commit the SHA update together with any test adjustments.
Overriding the pinned revision
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
The ``download_asset()`` function accepts a ``ref`` parameter that overrides the
default pinned SHA.
Roadmap and Future Work
-----------------------
(Placeholder for future roadmap and planned features)
- Advanced solver coupling
- More comprehensive sensor models
- Expanded robotics examples
See the `GitHub Discussions `__ and `GitHub Roadmap `__ for ongoing feature planning.
Benchmarking with airspeed velocity
-----------------------------------
The Newton repository contains a benchmarking suite implemented using the `airspeed velocity `__ framework.
The full set of benchmarks is intended to be run on a machine with a CUDA-capable GPU.
To get started, install airspeed velocity from PyPI:
.. code-block:: console
python -m pip install asv
.. tip::
With ``uv``, airspeed velocity can be run without installing it into the
project environment by using ``uvx``:
.. code-block:: console
uvx --with virtualenv asv run --launch-method spawn ...
If airspeed velocity has not been previously run on the machine, it will need to be initialized with:
.. code-block:: console
asv machine --yes
To run the benchmarks, run the following command from the root of the repository:
.. tab-set::
:sync-group: shell
.. tab-item:: Unix
:sync: unix
.. code-block:: console
asv run --launch-method spawn main^!
.. tab-item:: Windows
:sync: windows
.. code-block:: console
asv run --launch-method spawn main^^!
.. note::
On Windows CMD, the ``^`` character is an escape character, so it must be doubled (``^^``) to be interpreted literally.
The benchmarks discovered by airspeed velocity are in the ``asv/benchmarks`` directory. This command runs the
benchmark code from the ``asv/benchmarks`` directory against the code state of the ``main`` branch. Note that
the benchmark definitions themselves are not checked out from different branches—only the code being
benchmarked is.
Benchmarks can also be run against a range of commits using the ``commit1..commit2`` syntax.
This is useful for comparing performance across several recent changes:
.. tab-set::
:sync-group: shell
.. tab-item:: Unix
:sync: unix
.. code-block:: console
asv run --launch-method spawn HEAD~4..HEAD
.. tab-item:: Windows
:sync: windows
.. code-block:: console
asv run --launch-method spawn HEAD~4..HEAD
Note that the older commit has to come first.
Commit hashes can be used instead of relative references:
.. tab-set::
:sync-group: shell
.. tab-item:: Unix
:sync: unix
.. code-block:: console
asv run --launch-method spawn abc1234..def5678
.. tab-item:: Windows
:sync: windows
.. code-block:: console
asv run --launch-method spawn abc1234..def5678
Running benchmarks standalone
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Benchmark files can also be run directly as Python scripts, without the airspeed velocity
harness. This is useful for quick iteration during development since it skips the
environment setup that airspeed velocity performs. Each benchmark file under
``asv/benchmarks/`` supports a ``--bench`` flag to select specific benchmark classes:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
uv run python asv/benchmarks/simulation/bench_mujoco.py --bench FastAllegro
.. tab-item:: venv
:sync: venv
.. code-block:: console
python asv/benchmarks/simulation/bench_mujoco.py --bench FastAllegro
When ``--bench`` is omitted, all benchmarks in the file are run. The ``--bench`` flag can
be repeated to select multiple benchmarks:
.. tab-set::
:sync-group: env
.. tab-item:: uv
:sync: uv
.. code-block:: console
uv run python asv/benchmarks/simulation/bench_mujoco.py --bench FastAllegro --bench FastG1
.. tab-item:: venv
:sync: venv
.. code-block:: console
python asv/benchmarks/simulation/bench_mujoco.py --bench FastAllegro --bench FastG1
Tips for writing benchmarks
^^^^^^^^^^^^^^^^^^^^^^^^^^^
Rather than running the entire benchmark suite, use the ``--bench BENCH, -b BENCH`` flag to filter the benchmarks
to just the ones under development:
.. tab-set::
:sync-group: shell
.. tab-item:: Unix
:sync: unix
.. code-block:: console
asv run --launch-method spawn main^! --bench FastG1
.. tab-item:: Windows
:sync: windows
.. code-block:: console
asv run --launch-method spawn main^^! --bench FastG1
The most time-consuming benchmarks are those that measure the time it takes to load and run one frame of the example
starting from an empty kernel cache.
These benchmarks have names ending with ``time_load``. It is sometimes convenient to exclude these benchmarks
from running by using the following command:
.. tab-set::
:sync-group: shell
.. tab-item:: Unix
:sync: unix
.. code-block:: console
asv run --launch-method spawn main^! -b '^(?!.*time_load$).*'
.. tab-item:: Windows
:sync: windows
.. code-block:: console
asv run --launch-method spawn main^^! -b "^^(?!.*time_load$).*"
While airspeed velocity has built-in mechanisms to determine automatically how to collect measurements,
it is often useful to manually specify benchmark attributes like ``repeat`` and ``number`` to control the
number of times a benchmark is run and the number of times a benchmark is repeated.
.. code-block:: python
class PretrainedSimulate:
repeat = 3
number = 1
As the airspeed documentation on `benchmark attributes `__ notes,
the ``setup`` and ``teardown`` methods are not run between the ``number`` iterations that make up a sample.
These benchmark attributes should be tuned to ensure that the benchmark runs in a reasonable amount of time while
also ensuring that the benchmark is run a sufficient number of times to get a statistically meaningful result.
The ``--durations all`` flag can be passed to the ``asv run`` command to show the durations of all benchmarks,
which is helpful for ensuring that a single benchmark is not requiring an abnormally long amount of time compared
to the other benchmarks.
Release process
---------------
See :doc:`release` for the full release workflow, including versioning,
branching strategy, testing criteria, and publication steps.
.. toctree::
:hidden:
release
================================================
FILE: docs/guide/installation.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. currentmodule:: newton
Installation
============
This guide covers the recommended way to install Newton from PyPI. For
installing from source or using ``uv``, see the :doc:`development` guide.
.. _system-requirements:
System Requirements
-------------------
Minimum Requirements
^^^^^^^^^^^^^^^^^^^^
.. list-table::
:widths: 25 30 45
:header-rows: 1
* - Requirement
- Minimum
- Notes
* - Python
- 3.10
- 3.11+ recommended
* - OS
- Linux (x86-64, aarch64), Windows (x86-64), or macOS (CPU only)
- macOS has no GPU acceleration; see :ref:`cpu-limitations` below
* - NVIDIA GPU
- Compute capability 5.0+ (Maxwell)
- Any GeForce GTX 9xx or newer
* - NVIDIA Driver
- 545 or newer (CUDA 12)
- 550 or newer (CUDA 12.4) recommended for best performance
* - CUDA
- 12, 13
- No local CUDA Toolkit required; `Warp `__ bundles its own runtime
CUDA Compatibility
^^^^^^^^^^^^^^^^^^
.. list-table::
:widths: 25 75
:header-rows: 1
* - CUDA Version
- Notes
* - 12.3+
- Required for reliable CUDA graph capture
* - 12.4+
- Recommended for best performance
* - 13
- Supported
Tested Configurations
^^^^^^^^^^^^^^^^^^^^^
Newton releases are tested on the following configurations:
.. list-table::
:widths: 25 75
:header-rows: 1
* - Component
- Configuration
* - OS
- Ubuntu 22.04/24.04 (x86-64 + ARM64), Windows, macOS (CPU only)
* - GPU
- NVIDIA Ada Lovelace, Blackwell
* - Python
- 3.10, 3.12, 3.14 (import-only)
* - CUDA
- 12, 13
Platform-Specific Requirements
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
**Linux aarch64 (ARM64)**
On ARM64 Linux systems (such as NVIDIA Jetson Thor and DGX Spark), installing the ``examples`` extras currently requires
X11 development libraries to build ``imgui_bundle`` from source:
.. code-block:: console
sudo apt-get update
sudo apt-get install -y libx11-dev libxrandr-dev libxinerama-dev libxcursor-dev libxi-dev libgl1-mesa-dev
.. _cpu-limitations:
CPU-Only Limitations
^^^^^^^^^^^^^^^^^^^^
Newton can run on CPU (including macOS), but the following features require an
NVIDIA GPU and are unavailable in CPU-only mode:
- **SDF collision** — signed-distance-field computation requires CUDA
(``wp.Volume`` is GPU-only).
- **Mesh-mesh contacts** — SDF-based mesh-mesh collision is silently skipped on CPU.
- **Hydroelastic contacts** — depends on the SDF system.
- **Tiled camera sensor** — GPU-accelerated raytraced rendering.
- **Implicit MPM solver** — designed for GPU execution with CUDA graph support.
- **Tile-based VBD solve** — uses GPU tile API; gracefully disabled on CPU.
Installing Newton
-----------------
Basic installation:
.. code-block:: console
pip install newton
Install with the ``examples`` extra to run the built-in examples (includes simulation and visualization dependencies):
.. code-block:: console
pip install "newton[examples]"
We recommend installing Newton inside a virtual environment to avoid conflicts
with other packages:
.. tab-set::
:sync-group: os
.. tab-item:: macOS / Linux
:sync: linux
.. code-block:: console
python -m venv .venv
source .venv/bin/activate
pip install "newton[examples]"
.. tab-item:: Windows (console)
:sync: windows
.. code-block:: console
python -m venv .venv
.venv\Scripts\activate.bat
pip install "newton[examples]"
.. tab-item:: Windows (PowerShell)
:sync: windows-ps
.. code-block:: console
python -m venv .venv
.venv\Scripts\Activate.ps1
pip install "newton[examples]"
.. note::
Users on Python 3.10 may experience issues when installing ``imgui_bundle`` (a dependency of the
``examples`` extra). If you encounter installation errors, we recommend upgrading to a later
Python version, or follow the :doc:`development` guide to install Newton using ``uv``.
Running Examples
^^^^^^^^^^^^^^^^
After installing Newton with the ``examples`` extra, run an example with:
.. code-block:: console
python -m newton.examples basic_pendulum
Run an example that runs RL policy inference. Choose the extra matching your
NVIDIA driver's CUDA support (``torch-cu12`` for CUDA 12.x, ``torch-cu13`` for
CUDA 13.x) and the corresponding pytorch wheel (e.g, ``128`` for CUDA 12.8); run ``nvidia-smi``
to check the supported CUDA version (shown in the top-right corner of the output):
.. code-block:: console
pip install newton[torch-cu12] --extra-index-url https://download.pytorch.org/whl/cu128
python -m newton.examples robot_anymal_c_walk
See a list of all available examples:
.. code-block:: console
python -m newton.examples
Quick Start
^^^^^^^^^^^
After installing Newton, you can build
models, create solvers, and run simulations directly from Python. A typical
workflow looks like this:
.. code-block:: python
import warp as wp
import newton
# Build a model
builder = newton.ModelBuilder()
builder.add_mjcf("robot.xml") # or add_urdf() / add_usd()
builder.add_ground_plane()
model = builder.finalize()
# Create a solver and allocate state
solver = newton.solvers.SolverMuJoCo(model)
state_0 = model.state()
state_1 = model.state()
control = model.control()
contacts = model.contacts()
newton.eval_fk(model, model.joint_q, model.joint_qd, state_0)
# Step the simulation
for step in range(1000):
state_0.clear_forces()
model.collide(state_0, contacts)
solver.step(state_0, state_1, control, contacts, 1.0 / 60.0 / 4.0)
state_0, state_1 = state_1, state_0
For robot-learning workflows with parallel environments (as used by
`Isaac Lab `_), you can replicate a
robot template across many worlds and step them all simultaneously on the GPU:
.. code-block:: python
# Build a single robot template
template = newton.ModelBuilder()
template.add_mjcf("humanoid.xml")
# Replicate into parallel worlds
builder = newton.ModelBuilder()
builder.replicate(template, world_count=1024)
builder.add_ground_plane()
model = builder.finalize()
# The solver steps all 1024 worlds in parallel
solver = newton.solvers.SolverMuJoCo(model)
See the :doc:`/guide/overview` guide and :doc:`/integrations/isaac-lab`
for more details.
.. _extra-dependencies:
Extra Dependencies
------------------
Newton's only mandatory dependency is `NVIDIA Warp `_.
Additional optional dependency sets are defined in ``pyproject.toml``:
.. list-table::
:widths: 20 80
:header-rows: 1
* - Set
- Purpose
* - ``sim``
- Simulation dependencies, including MuJoCo
* - ``importers``
- Asset import and mesh processing dependencies
* - ``remesh``
- Remeshing dependencies (Open3D, pyfqmr) for :func:`newton.utils.remesh_mesh`
* - ``examples``
- Dependencies for running examples, including visualization (includes ``sim`` + ``importers``)
* - ``torch-cu12``
- PyTorch (CUDA 12) for running RL policy examples (includes ``examples``)
* - ``torch-cu13``
- PyTorch (CUDA 13) for running RL policy examples (includes ``examples``)
* - ``notebook``
- Jupyter notebook support with Rerun visualization (includes ``examples``)
* - ``dev``
- Dependencies for development and testing (includes ``examples``)
* - ``docs``
- Dependencies for building the documentation
Some extras transitively include others. For example, ``examples`` pulls in both
``sim`` and ``importers``, and ``dev`` pulls in ``examples``. You only need to
install the most specific set for your use case.
.. _versioning:
Versioning
----------
Newton currently uses the following versioning scheme. This may evolve
depending on the needs of the project and its users.
Newton uses a **major.minor.micro** versioning scheme, similar to
`Python itself `__:
* New **major** versions are reserved for major reworks of Newton causing
disruptive incompatibility (or reaching the 1.0 milestone).
* New **minor** versions are feature releases with a new set of features.
May contain deprecations, breaking changes, and removals.
* New **micro** versions are bug-fix releases. In principle, there are no
new features. The first release of a new minor version always includes
the micro version (e.g., ``1.1.0``), though informal references may
shorten it (e.g., "Newton 1.1").
Prerelease Versions
^^^^^^^^^^^^^^^^^^^
In addition to stable releases, Newton uses the following prerelease
version formats:
* **Development builds** (``major.minor.micro.dev0``): The version string
used in the source code on the main branch between stable releases
(e.g., ``1.1.0.dev0``).
* **Release candidates** (``major.minor.microrcN``): Pre-release versions
for QA testing before a stable release, starting with ``rc1`` and
incrementing (e.g., ``1.1.0rc1``). Usually not published to PyPI.
Prerelease versions should be considered unstable and are not subject
to the same compatibility guarantees as stable releases.
Next Steps
----------
- Run ``python -m newton.examples`` to see all available examples and check out the :doc:`visualization` guide to learn how to interact with the example simulations.
- Check out the :doc:`development` guide to learn how to contribute to Newton, or how to use alternative installation methods.
================================================
FILE: docs/guide/overview.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Newton Physics Documentation
============================
.. image:: /_static/newton-banner.jpg
:alt: Newton Physics Engine Banner
:align: center
:class: newton-banner
.. raw:: html
**Newton** is a GPU-accelerated, extensible, and differentiable physics simulation engine designed for robotics, research, and advanced simulation workflows. Built on top of NVIDIA Warp and integrating MuJoCo Warp, Newton provides high-performance simulation, modern Python APIs, and a flexible architecture for both users and developers.
Key Features
------------
* **GPU-accelerated**: Leverages NVIDIA Warp for fast, scalable simulation.
* **Multiple solver implementations**: XPBD, VBD, MuJoCo, Featherstone, SemiImplicit.
* **Modular design**: Easily extendable with new solvers and components.
* **Differentiable**: Supports differentiable simulation for machine learning and optimization.
* **Rich Import/Export**: Load models from URDF, MJCF, USD, and more.
* **Open Source**: Maintained by Disney Research, Google DeepMind, and NVIDIA.
.. admonition:: Learn More
:class: tip
Start with the :doc:`introduction tutorial ` for a
hands-on walkthrough. For a deeper conceptual introduction, see the
`DeepWiki Newton Physics page `__.
.. _guide-core-concepts:
Core Concepts
-------------
.. mermaid::
:config: {"theme": "forest", "themeVariables": {"lineColor": "#76b900"}}
flowchart LR
subgraph Authoring["Model Authoring"]
direction LR
P[Python API] --> A[ModelBuilder]
subgraph Imported["Imported assets"]
direction TB
U[URDF]
M[MJCF]
S[USD]
end
U --> G[Importer]
M --> G
S --> G
G --> A
end
B[Model]
subgraph Loop["Simulation Loop"]
direction LR
C[State] --> D[Solver]
J[Control] --> D
E[Contacts] --> D
D --> C2[Updated state]
end
subgraph Outputs["Outputs"]
direction TB
K[Sensors]
F[Viewer]
end
A -->|builds| B
B --> C
B --> J
B --> E
C2 --> K
E --> K
C2 --> F
- :class:`~newton.ModelBuilder`: The entry point for constructing
simulation models from primitives or imported assets.
- :class:`~newton.Model`: Encapsulates the physical structure,
parameters, and configuration of the simulation world, including
bodies, joints, shapes, and physical properties.
- :class:`~newton.State`: Represents the dynamic state at a given time,
including positions and velocities that solvers update each step.
Optional :doc:`extended attributes <../concepts/extended_attributes>`
store derived quantities such as rigid-body accelerations for
sensors.
- :class:`~newton.Contacts`: Stores the active contact set produced by
:meth:`Model.collide `, with optional extended
attributes such as contact forces for sensing and analysis.
- :class:`~newton.Control`: Encodes control inputs such as joint targets
and forces applied during the simulation loop.
- :doc:`Solver <../api/newton_solvers>`: Advances the simulation by
integrating physics, handling contacts, and enforcing constraints.
Newton provides multiple solver backends, including XPBD, VBD,
MuJoCo, Featherstone, and SemiImplicit.
- :doc:`Sensors <../concepts/sensors>`: Compute observations from
:class:`~newton.State`, :class:`~newton.Contacts`, sites, and shapes.
Many sensors rely on optional :doc:`extended attributes
<../concepts/extended_attributes>` that store derived solver outputs.
- **Importer**: Loads models from external formats via
:meth:`~newton.ModelBuilder.add_urdf`,
:meth:`~newton.ModelBuilder.add_mjcf`, and
:meth:`~newton.ModelBuilder.add_usd`.
- :doc:`Viewer `: Visualizes the simulation in real time
or offline.
Simulation Loop
---------------
1. Build or import a model with :class:`~newton.ModelBuilder`.
2. Finalize the builder into a :class:`~newton.Model`.
3. Create any sensors, then allocate one or more
:class:`~newton.State` objects plus :class:`~newton.Control` inputs
and :class:`~newton.Contacts`.
4. Call :meth:`Model.collide ` to populate the
contact set for the current state.
5. Step a :doc:`solver <../api/newton_solvers>` using the current
state, control, and contacts.
6. Update sensors, inspect outputs, render, or export the results.
Quick Links
-----------
- :doc:`installation` — Setup Newton and run a first example in a couple of minutes
- :doc:`tutorials` — Browse the guide's tutorial landing page
- :doc:`Introduction tutorial ` — Walk through a first hands-on tutorial
- :doc:`../faq` — Frequently asked questions
- :doc:`development` — For developers and code contributors
- :doc:`../api/newton` — Full API reference
:ref:`Full Index `
================================================
FILE: docs/guide/release.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Release Process
===============
This document describes how to prepare, publish, and follow up on a Newton
release. It is intended for release engineers and maintainers.
Overview
--------
Newton follows PEP 440 versioning as described in the
:ref:`versioning ` section of the installation guide.
Releases are published to `PyPI `__ and
documentation is deployed to
`GitHub Pages `__.
Version source of truth
^^^^^^^^^^^^^^^^^^^^^^^
The version string lives in the ``[project]`` table of ``pyproject.toml``.
All other version references (PyPI metadata, documentation) are derived from
this file. At runtime, ``newton/_version.py`` reads the version from
installed package metadata via ``importlib.metadata``.
Dependency versioning strategy
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
``pyproject.toml`` specifies **minimum** compatible versions
(e.g. ``warp-lang>=1.12.0``). ``uv.lock`` pins the **latest known-good**
versions for reproducible installs.
Exception: on the **release branch**, ``mujoco`` and ``mujoco-warp`` use
**compatible-release** pins (e.g. ``mujoco~=3.5.0``) to allow patch
updates while locking the minor version. MuJoCo follows
`semantic versioning from 3.5.0 onward `__,
so patch releases are safe to pick up automatically. ``main`` uses a
version floor like other dependencies.
.. _deprecation-timeline:
Deprecation timeline
^^^^^^^^^^^^^^^^^^^^
Following `Warp's deprecation policy
`__,
a deprecated feature is maintained for **two full minor release cycles**
after deprecation (e.g. deprecated in 1.2.0 → removed in 1.4.0).
Deprecations and removals only happen in minor releases, never in patch
releases.
Pre-release planning
--------------------
.. list-table::
:widths: 5 95
:header-rows: 0
* - ☐
- Determine target version (``X.Y.Z``).
* - ☐
- Confirm dependency versions and availability: warp-lang, mujoco,
mujoco-warp, newton-usd-schemas, newton-actuators.
* - ☐
- Set timeline: code freeze → RC1 → testing window → GA.
* - ☐
- Conduct public API audit:
- Review all new/changed symbols since the last release for unintended
breaking changes.
- Verify deprecated symbols carry proper deprecation warnings and
migration guidance (see :ref:`deprecation-timeline`).
- Confirm new public API has complete docstrings and is included in
Sphinx docs (run ``docs/generate_api.py``).
* - ☐
- Communicate the timeline to the community.
Code freeze and release branch creation
---------------------------------------
.. list-table::
:widths: 5 95
:header-rows: 0
* - ☐
- Create ``release-X.Y`` branch from ``main`` and push it.
* - ☐
- On **main**: bump the version in ``pyproject.toml`` to ``X.(Y+1).0.dev0`` and run
``docs/generate_api.py``.
* - ☐
- On **release-X.Y**: bump the version in ``pyproject.toml`` to ``X.Y.ZrcN`` and
run ``docs/generate_api.py``.
* - ☐
- On **release-X.Y**: update dependencies in ``pyproject.toml`` from dev
to RC versions where applicable, then regenerate ``uv.lock``
(``uv lock``) and commit it.
* - ☐
- Push tag ``vX.Y.Zrc1``. This triggers the ``release.yml`` workflow
(build wheel → PyPI publish with manual approval).
* - ☐
- RC1 published to PyPI (approve in GitHub environment).
Release candidate stabilization
-------------------------------
Bug fixes merge to ``main`` first, then are cherry-picked to
``release-X.Y``. Cherry-pick relevant commits from ``main`` onto a feature
branch and open a pull request targeting ``release-X.Y`` — never push
directly to the release branch.
For each new RC (``rc2``, ``rc3``, …) bump the version in
``pyproject.toml`` and run ``docs/generate_api.py``, then tag and push.
Resolve any cherry-pick conflicts or missing dependent cherry-picks that
cause CI failures before tagging.
.. _testing-criteria:
Testing criteria
^^^^^^^^^^^^^^^^
The release engineer and maintainers decide which issues must be fixed
before GA and which can ship as known issues documented in the release
notes. Features explicitly marked **experimental** have a lower bar —
regressions in experimental APIs do not necessarily block a release.
As a guideline, an RC is typically ready for GA when:
- All examples run without crashes, excessive warnings, or visual
artifacts (``uv run -m newton.examples ``).
- Testing covers **Windows and Linux**, **all supported Python versions**,
and both **latest and minimum-spec CUDA drivers** (see
:ref:`system requirements ` in the installation guide).
- PyPI installation of the RC works in a clean environment: ``pip install``
succeeds, ``import newton`` works, and examples and tests can be run from
the installed wheel (``pip install newton==X.Y.ZrcN``).
- No unexpected regressions compared to the previous release have been
identified.
.. list-table::
:widths: 5 95
:header-rows: 0
* - ☐
- All release-targeted fixes cherry-picked from ``main``.
* - ☐
- :ref:`Testing criteria ` satisfied.
* - ☐
- No outstanding release-blocking issues.
.. _final-release:
Final GA release
----------------
Before proceeding, obtain explicit go/no-go approval from the
maintainers. Do not start the final release steps until sign-off is
confirmed.
All steps below are performed on the **release-X.Y** branch unless noted
otherwise.
.. list-table::
:widths: 5 95
:header-rows: 0
* - ☐
- Go/no-go approval obtained from maintainers.
* - ☐
- Finalize ``CHANGELOG.md``: rename ``[Unreleased]`` →
``[X.Y.Z] - YYYY-MM-DD``. Review the entries for:
- **Missing entries** — cross-check merged PRs since the last GA
release (or patch) to catch changes that were not recorded in the
changelog.
- **Redundant entries** — consolidate or remove duplicates for changes
within the same release period (e.g. a bug fix for a feature added
in the same cycle should not appear as both an "Added" and a "Fixed"
entry).
* - ☐
- Update ``README.md`` documentation links to point to versioned URLs
(e.g. ``/X.Y.Z/guide.html`` instead of ``/latest/``).
* - ☐
- Verify all dependency pins in ``pyproject.toml`` use stable
(non-pre-release) versions.
* - ☐
- Regenerate ``uv.lock`` (``uv lock``) and verify that no pre-release
dependencies remain in the lock file.
* - ☐
- Bump the version in ``pyproject.toml`` to ``X.Y.Z`` (remove the RC suffix) and
run ``docs/generate_api.py``.
* - ☐
- Commit and push tag ``vX.Y.Z``. Automated workflows trigger:
- ``release.yml``: builds wheel, publishes to PyPI (requires manual
approval), creates a draft GitHub Release.
- ``docs-release.yml``: deploys docs to ``/X.Y.Z/`` and ``/stable/``
on gh-pages, updates ``switcher.json``.
* - ☐
- PyPI publish approved and verified: ``pip install newton==X.Y.Z``.
* - ☐
- GitHub Release un-drafted and published.
* - ☐
- Docs live at ``/X.Y.Z/`` and ``/stable/``: verify links and version
switcher.
* - ☐
- Release announcement posted.
Post-release
------------
.. list-table::
:widths: 5 95
:header-rows: 0
* - ☐
- Merge back the changelog from ``release-X.Y`` to ``main``: move
entries included in the release from ``[Unreleased]`` to a new
``[X.Y.Z]`` section. (``[Unreleased]`` is a permanent header in
the changelog that always exists on ``main``.)
* - ☐
- Verify PyPI installation works in a clean environment.
* - ☐
- Verify published docs render correctly.
Patch releases
--------------
Patch releases continue cherry-picking fixes to the existing
``release-X.Y`` branch. For example, ``1.0.1`` follows ``1.0.0``.
Follow the same :ref:`final-release` flow — bump version, update changelog,
tag, and push. There is no need to create a new branch or bump ``main``.
================================================
FILE: docs/guide/tutorials.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Tutorials
=========
This section contains tutorials to help you get started with Newton.
You can also explore the examples in the ``newton/examples/`` directory for more use cases.
.. toctree::
:maxdepth: 2
:caption: Tutorial Notebooks
/tutorials/00_introduction
================================================
FILE: docs/guide/visualization.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. currentmodule:: newton
Visualization
=============
Newton provides multiple viewer backends for different visualization needs, from real-time rendering to offline recording and external integrations.
Common Interface
----------------
All viewer backends inherit from :class:`~newton.viewer.ViewerBase` and share a common interface:
**Core loop methods** — every viewer uses the same simulation loop pattern:
- :meth:`~newton.viewer.ViewerBase.set_model` — assign a :class:`~newton.Model` and optionally limit the number of rendered worlds with ``max_worlds``
- :meth:`~newton.viewer.ViewerBase.begin_frame` — start a new frame with the current simulation time
- :meth:`~newton.viewer.ViewerBase.log_state` — update the viewer with the current :class:`~newton.State` (body transforms, particle positions, etc.)
- :meth:`~newton.viewer.ViewerBase.end_frame` — finish the frame and present it
- :meth:`~newton.viewer.ViewerBase.is_running` — check whether the viewer is still open (useful as a loop condition)
- :meth:`~newton.viewer.ViewerBase.is_paused` — check whether the simulation is paused (toggled with ``SPACE`` in :class:`~newton.viewer.ViewerGL`)
- :meth:`~newton.viewer.ViewerBase.close` — close the viewer and release resources
**Camera and layout:**
- :meth:`~newton.viewer.ViewerBase.set_camera` — set camera position, pitch, and yaw
- :meth:`~newton.viewer.ViewerBase.set_world_offsets` — arrange multiple worlds in a grid with a given spacing along each axis
**Custom visualization** — draw debug overlays on top of the simulation:
- :meth:`~newton.viewer.ViewerBase.log_lines` — draw line segments (e.g. rays, normals, force vectors)
- :meth:`~newton.viewer.ViewerBase.log_points` — draw a point cloud (e.g. contact locations, particle positions)
- :meth:`~newton.viewer.ViewerBase.log_contacts` — visualize :class:`~newton.Contacts` as normal lines at contact points
- :meth:`~newton.viewer.ViewerBase.log_gizmo` — display a transform gizmo (position + orientation axes)
- :meth:`~newton.viewer.ViewerBase.log_scalar` / :meth:`~newton.viewer.ViewerBase.log_array` — log numeric data for backend-specific visualization (e.g. time-series plots in Rerun)
**Limiting rendered worlds**: When training with many parallel environments, rendering all worlds can impact performance.
All viewers support the ``max_worlds`` parameter to limit visualization to a subset of environments:
.. testcode:: viewer-max-worlds
builder = newton.ModelBuilder()
body = builder.add_body(mass=1.0)
model = builder.finalize()
# Only render the first 4 environments
viewer = newton.viewer.ViewerNull()
viewer.set_model(model, max_worlds=4)
Real-time Viewers
-----------------
OpenGL Viewer
~~~~~~~~~~~~~
Newton provides :class:`~newton.viewer.ViewerGL`, a simple OpenGL viewer for interactive real-time visualization of simulations.
The viewer requires pyglet (version >= 2.1.6) and imgui_bundle (version >= 1.92.0) to be installed.
Constructor parameters:
- ``width``: Window width in pixels (default: ``1920``)
- ``height``: Window height in pixels (default: ``1080``)
- ``vsync``: Enable vertical sync (default: ``False``)
- ``headless``: Run without a visible window, useful for off-screen rendering (default: ``False``)
.. code-block:: python
viewer = newton.viewer.ViewerGL()
viewer.set_model(model)
# at every frame:
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
# check if the simulation is paused (toggled with SPACE key):
if viewer.is_paused():
pass # simulation stepping is paused
**Interactive forces and input:**
:meth:`~newton.viewer.ViewerGL.apply_forces` applies viewer-driven forces (object picking with right-click, wind) to the simulation state.
Call it each frame before stepping the solver:
.. code-block:: python
viewer.apply_forces(state)
solver.step(model, state, ...)
:meth:`~newton.viewer.ViewerGL.is_key_down` queries whether a key is currently pressed.
Keys can be specified as single-character strings (``'w'``), special key names (``'space'``, ``'escape'``), or pyglet key constants:
.. code-block:: python
if viewer.is_key_down('r'):
state = model.state() # reset
**Headless mode and frame capture:**
In headless mode (``headless=True``), the viewer renders off-screen without opening a window.
Use :meth:`~newton.viewer.ViewerGL.get_frame` to retrieve the rendered image as a GPU array:
.. code-block:: python
viewer = newton.viewer.ViewerGL(headless=True)
viewer.set_model(model)
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
# Returns a wp.array with shape (height, width, 3), dtype wp.uint8
frame = viewer.get_frame()
**Custom UI panels:**
:meth:`~newton.viewer.ViewerGL.register_ui_callback` adds custom imgui UI elements to the viewer.
The ``position`` parameter controls placement: ``"side"`` (default), ``"stats"``, ``"free"``, or ``"panel"``:
.. code-block:: python
def my_ui(ui):
import imgui_bundle.imgui as imgui
imgui.text("Hello from custom UI!")
viewer.register_ui_callback(my_ui, position="side")
Keyboard shortcuts when working with the OpenGL Viewer:
.. list-table:: Keyboard Shortcuts
:header-rows: 1
* - Key(s)
- Description
* - ``W``, ``A``, ``S``, ``D`` (or arrow keys) + mouse drag
- Move the camera like in a FPS game
* - ``H``
- Toggle Sidebar
* - ``SPACE``
- Pause/continue the simulation
* - ``Right Click``
- Pick objects
**Troubleshooting:**
If you encounter an OpenGL context error on Linux with Wayland:
.. code-block:: text
OpenGL.error.Error: Attempt to retrieve context when no valid context
Set the PyOpenGL platform before running:
.. code-block:: bash
export PYOPENGL_PLATFORM=glx
This is a known issue when running OpenGL applications on Wayland display servers.
Recording and Offline Viewers
-----------------------------
Recording to File (ViewerFile)
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
The :class:`~newton.viewer.ViewerFile` backend records simulation data to JSON or binary files for later replay or analysis.
This is useful for capturing simulations for debugging, sharing results, or post-processing.
**File formats:**
- ``.json``: Human-readable JSON format (no additional dependencies)
- ``.bin``: Binary CBOR2 format (more efficient, requires ``cbor2`` package)
To use binary format, install the optional dependency:
.. code-block:: bash
pip install cbor2
**Recording a simulation:**
.. testcode:: viewer-file
import tempfile, os
builder = newton.ModelBuilder()
body = builder.add_body(mass=1.0)
model = builder.finalize()
state = model.state()
# Record to JSON format (human-readable, no extra dependencies)
output_path = os.path.join(tempfile.mkdtemp(), "simulation.json")
viewer = newton.viewer.ViewerFile(output_path)
viewer.set_model(model)
sim_time = 0.0
for _ in range(5):
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
sim_time += 1.0 / 60.0
# Close to save the recording
viewer.close()
.. testoutput:: viewer-file
:options: +NORMALIZE_WHITESPACE, +ELLIPSIS
...
**Loading and playing back recordings:**
Use :class:`~newton.viewer.ViewerFile` to load a recording, then restore the model and state for a given frame. Use :class:`~newton.viewer.ViewerGL` (or another rendering viewer) to visualize.
.. testcode:: viewer-file
# Load a recording for playback
viewer_file = newton.viewer.ViewerFile(output_path)
viewer_file.load_recording()
# Restore the model and state from the recording
model = newton.Model()
viewer_file.load_model(model)
print(f"Frames: {viewer_file.get_frame_count()}")
state = model.state()
viewer_file.load_state(state, frame_id=0) # frame index in [0, get_frame_count())
.. testoutput:: viewer-file
Frames: 5
For a complete example with UI controls for scrubbing and playback, see ``newton/examples/basic/example_replay_viewer.py``.
Key parameters:
- ``output_path``: Path to the output file (format determined by extension: .json or .bin)
- ``auto_save``: If True, automatically save periodically during recording (default: ``True``)
- ``save_interval``: Number of frames between auto-saves when auto_save=True (default: ``100``)
- ``max_history_size``: Maximum number of frames to keep in memory (default: ``None`` for unlimited)
Rendering to USD
~~~~~~~~~~~~~~~~
Instead of rendering in real-time, you can also render the simulation as a time-sampled USD stage to be visualized in Omniverse or other USD-compatible tools using the :class:`~newton.viewer.ViewerUSD` backend.
Constructor parameters:
- ``output_path``: Path to the output USD file
- ``fps``: Frames per second for time sampling (default: ``60``)
- ``up_axis``: USD up axis, ``"Y"`` or ``"Z"`` (default: ``"Z"``)
- ``num_frames``: Maximum number of frames to record, or ``None`` for unlimited (default: ``100``)
- ``scaling``: Uniform scaling applied to the scene root (default: ``1.0``)
.. code-block:: python
viewer = newton.viewer.ViewerUSD(output_path="simulation.usd", fps=60, up_axis="Z")
viewer.set_model(model)
# at every frame:
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
# Save and close the USD file
viewer.close()
External Integrations
---------------------
Rerun Viewer
~~~~~~~~~~~~
The :class:`~newton.viewer.ViewerRerun` backend integrates with the `rerun `_ visualization library,
enabling real-time or offline visualization with advanced features like time scrubbing and data inspection.
**Installation**: Requires the rerun-sdk package:
.. code-block:: bash
pip install rerun-sdk
Constructor parameters:
- ``app_id``: Application ID for Rerun (default: ``"newton-viewer"``). Use different IDs to differentiate between parallel viewer instances.
- ``address``: Optional server address to connect to a remote Rerun server. If provided, connects to the specified server.
- ``serve_web_viewer``: Serve a web viewer over HTTP and open it in the browser (default: ``True``). If ``False``, spawns a native Rerun viewer.
- ``web_port``: Port for the web viewer (default: ``9090``)
- ``grpc_port``: Port for the gRPC server (default: ``9876``)
- ``keep_historical_data``: Keep historical state data in the viewer for time scrubbing (default: ``False``)
- ``keep_scalar_history``: Keep scalar time-series history (default: ``True``)
- ``record_to_rrd``: Optional path to save a ``.rrd`` recording file
**Usage**:
.. code-block:: python
# Default usage: spawns a local viewer
viewer = newton.viewer.ViewerRerun(
app_id="newton-simulation"
)
# Or specify a custom server address for remote viewing
viewer = newton.viewer.ViewerRerun(
address="rerun+http://127.0.0.1:9876/proxy",
app_id="newton-simulation"
)
viewer.set_model(model)
# at every frame:
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
By default, the viewer will run without keeping historical state data in the viewer to keep the memory usage constant when sending transform updates via :meth:`~newton.viewer.ViewerBase.log_state`.
This is useful for visualizing long and complex simulations that would quickly fill up the web viewer's memory if the historical data was kept.
If you want to keep the historical state data in the viewer, you can set the ``keep_historical_data`` flag to ``True``.
The rerun viewer provides a web-based interface with features like:
- Time scrubbing and playback controls
- 3D scene navigation
- Data inspection and filtering
- Recording and export capabilities
**Jupyter notebook support**
The ViewerRerun backend automatically detects if it is running inside a Jupyter notebook environment and automatically generates an output widget for the viewer
during the construction of :class:`~newton.viewer.ViewerRerun`.
The rerun SDK provides a Jupyter notebook extension that allows you to visualize rerun data in a Jupyter notebook.
You can use ``uv`` to start Jupyter lab with the required dependencies (or install the extension manually with ``pip install rerun-sdk[notebook]``):
.. code-block:: bash
uv run --extra notebook jupyter lab
Then, you can use the rerun SDK in a Jupyter notebook by importing the ``rerun`` module and creating a viewer instance.
.. code-block:: python
viewer = newton.viewer.ViewerRerun(keep_historical_data=True)
viewer.set_model(model)
frame_dt = 1 / 60.0
sim_time = 0.0
for frame in range(500):
# simulate, step the solver, etc.
solver.step(...)
# visualize
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
sim_time += frame_dt
viewer.show_notebook() # or simply `viewer` to display the viewer in the notebook
.. image:: /images/rerun_notebook_example.png
:width: 1000
:align: left
The history of states will be available in the viewer to scrub through the simulation timeline.
Viser Viewer
~~~~~~~~~~~~
The :class:`~newton.viewer.ViewerViser` backend integrates with the `viser `_ visualization library,
providing web-based 3D visualization that works in any browser and has native Jupyter notebook support.
**Installation**: Requires the viser package:
.. code-block:: bash
pip install viser
**Usage**:
.. code-block:: python
# Default usage: starts a web server on port 8080
viewer = newton.viewer.ViewerViser(port=8080)
# Open http://localhost:8080 in your browser to view the simulation
viewer.set_model(model)
# at every frame:
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
# Close the viewer when done
viewer.close()
Key parameters:
- ``port``: Port number for the web server (default: ``8080``)
- ``label``: Optional label for the browser window title
- ``verbose``: If True, print the server URL when starting (default: ``True``)
- ``share``: If True, create a publicly accessible URL via viser's share feature
- ``record_to_viser``: Path to record the visualization to a ``.viser`` file for later playback
**Recording and playback**
ViewerViser can record simulations to ``.viser`` files for later playback:
.. code-block:: python
# Record to a .viser file
viewer = newton.viewer.ViewerViser(record_to_viser="my_simulation.viser")
viewer.set_model(model)
# Run simulation...
for frame in range(500):
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
sim_time += frame_dt
# Save the recording
viewer.save_recording()
The recorded ``.viser`` file can be played back using the viser HTML player.
**Jupyter notebook support**
ViewerViser has native Jupyter notebook integration. When recording is enabled, calling ``show_notebook()``
will display an embedded player with timeline controls:
.. code-block:: python
viewer = newton.viewer.ViewerViser(record_to_viser="simulation.viser")
viewer.set_model(model)
# Run simulation...
for frame in range(500):
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
sim_time += frame_dt
# Display in notebook with timeline controls
viewer.show_notebook() # or simply `viewer` at the end of a cell
When no recording is active, ``show_notebook()`` displays the live server in an IFrame.
The viser viewer provides features like:
- Real-time 3D visualization in any web browser
- Interactive camera controls (pan, zoom, orbit)
- GPU-accelerated batched mesh rendering
- Recording and playback capabilities
- Public URL sharing via viser's share feature
Utility Viewers
---------------
Null Viewer
~~~~~~~~~~~
The :class:`~newton.viewer.ViewerNull` provides a no-operation viewer for headless environments or automated testing where visualization is not required.
It simply counts frames and provides stub implementations for all viewer methods.
.. testcode:: viewer-null
builder = newton.ModelBuilder()
body = builder.add_body(mass=1.0)
model = builder.finalize()
state = model.state()
sim_time = 0.0
viewer = newton.viewer.ViewerNull(num_frames=10)
viewer.set_model(model)
while viewer.is_running():
viewer.begin_frame(sim_time)
viewer.log_state(state)
viewer.end_frame()
sim_time += 1.0 / 60.0
print(f"Ran {viewer.frame_count} frames")
.. testoutput:: viewer-null
Ran 10 frames
This is particularly useful for:
- Performance benchmarking without rendering overhead
- Automated testing in CI/CD pipelines
- Running simulations on headless servers
- Batch processing of simulations
Custom Visualization
--------------------
In addition to rendering simulation state with :meth:`~newton.viewer.ViewerBase.log_state`, you can draw custom debug overlays using the ``log_*`` methods available on all viewers.
**Drawing lines:**
Use :meth:`~newton.viewer.ViewerBase.log_lines` to draw line segments — useful for visualizing forces, rays, or normals:
.. code-block:: python
# Draw force vectors at body positions
viewer.log_lines(
"/debug/forces",
starts=positions, # wp.array(dtype=wp.vec3)
ends=positions + forces, # wp.array(dtype=wp.vec3)
colors=(1.0, 0.0, 0.0), # red
width=0.005,
)
**Drawing points:**
Use :meth:`~newton.viewer.ViewerBase.log_points` to draw a point cloud:
.. code-block:: python
viewer.log_points(
"/debug/targets",
points=target_positions, # wp.array(dtype=wp.vec3)
radii=0.02, # uniform radius, or wp.array(dtype=wp.float32)
colors=(0.0, 1.0, 0.0), # green
)
**Visualizing contacts:**
Use :meth:`~newton.viewer.ViewerBase.log_contacts` to draw contact normals from a :class:`~newton.Contacts` object.
The viewer's ``show_contacts`` flag (toggled in the :class:`~newton.viewer.ViewerGL` sidebar) controls visibility:
.. code-block:: python
viewer.log_contacts(contacts, state)
**Transform gizmos:**
Use :meth:`~newton.viewer.ViewerBase.log_gizmo` to display a coordinate-frame gizmo at a given transform:
.. code-block:: python
viewer.log_gizmo("/debug/target_frame", wp.transform(pos, rot))
**Camera and world layout:**
Set the camera programmatically with :meth:`~newton.viewer.ViewerBase.set_camera`:
.. code-block:: python
viewer.set_camera(pos=wp.vec3(5.0, 2.0, 3.0), pitch=-0.3, yaw=0.5)
When visualizing multiple worlds, use :meth:`~newton.viewer.ViewerBase.set_world_offsets` to arrange them in a grid
(must be called after :meth:`~newton.viewer.ViewerBase.set_model`):
.. code-block:: python
viewer.set_world_offsets(spacing=(5.0, 5.0, 0.0))
Choosing the Right Viewer
-------------------------
.. list-table:: Viewer Comparison
:header-rows: 1
* - Viewer
- Use Case
- Output
- Dependencies
* - :class:`~newton.viewer.ViewerGL`
- Interactive development and debugging
- Real-time display
- pyglet, imgui_bundle
* - :class:`~newton.viewer.ViewerFile`
- Recording for replay/sharing
- .json or .bin files
- None
* - :class:`~newton.viewer.ViewerUSD`
- Integration with 3D pipelines
- .usd files
- usd-core
* - :class:`~newton.viewer.ViewerRerun`
- Advanced visualization and analysis
- Web interface
- rerun-sdk
* - :class:`~newton.viewer.ViewerViser`
- Browser-based visualization and Jupyter notebooks
- Web interface, .viser files
- viser
* - :class:`~newton.viewer.ViewerNull`
- Headless/automated environments
- None
- None
================================================
FILE: docs/images/examples/resize.bat
================================================
@echo off
REM resize all PNGs to half size, crop to square, and convert to JPG
for %%f in (*.png) do (
echo Processing %%f...
REM First crop to square (keep as PNG), then resize to half and convert to JPG
ffmpeg -y -i "%%f" -vf "crop=min(iw\,ih):min(iw\,ih)" -update 1 "%%~nf_temp.png"
ffmpeg -y -i "%%~nf_temp.png" -vf "scale=iw/2:ih/2,format=yuv420p" -update 1 -q:v 2 "%%~nf.jpg"
del "%%~nf_temp.png"
)
================================================
FILE: docs/images/examples/resize.sh
================================================
# resize all pngs in a directory by 2x in dimension, crop to square, + convert to jpg
for f in *.png; do ffmpeg -y -i "$f" -vf "crop=min(iw\,ih):min(iw\,ih),scale=iw/2:ih/2,format=yuv420p" -update 1 -q:v 3 "${f%.png}.jpg"; done
================================================
FILE: docs/index.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. raw:: html
Newton Physics
===============
.. toctree::
:maxdepth: 1
:hidden:
:caption: User Guide
Overview
guide/installation
guide/visualization
guide/tutorials
Development
.. toctree::
:maxdepth: 1
:hidden:
:caption: Concepts
Worlds
Articulations
Mass and Inertia
Sites
Sensors
Conventions
USD Parsing
Custom Attributes
Extended Attributes
Collisions and Contacts
.. toctree::
:maxdepth: 1
:hidden:
:caption: API Reference
api/newton
api/newton_geometry
api/newton_ik
api/newton_math
api/newton_selection
api/newton_sensors
api/newton_solvers
api/newton_usd
api/newton_utils
api/newton_viewer
.. toctree::
:hidden:
:caption: Further Reading
FAQ
Migration Guide
Integrations
.. toctree::
:hidden:
:caption: Project Links
GitHub
================================================
FILE: docs/integrations/index.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Newton Integrations
====================
.. toctree::
:maxdepth: 1
isaac-lab
mujoco
================================================
FILE: docs/integrations/isaac-lab.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
Isaac Lab Integration
=====================
For details about Isaac Lab support for Newton, see the
`Isaac Lab documentation `_.
================================================
FILE: docs/integrations/mujoco.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. currentmodule:: newton
MuJoCo Integration
==================
:class:`~newton.solvers.SolverMuJoCo` translates a Newton :class:`~newton.Model`
into a `MuJoCo `_ model and steps it
with `mujoco_warp `_.
Because MuJoCo has its own modelling conventions, some Newton properties are
mapped differently or not at all. The sections below describe each conversion
in detail.
Coordinate conventions
----------------------
**Quaternion order.**
Newton stores quaternions as ``(x, y, z, w)``; MuJoCo uses ``(w, x, y, z)``.
The solver converts between the two automatically. Be aware of this when
inspecting raw MuJoCo model or data objects (e.g. via ``save_to_mjcf`` or
the ``mj_model``/``mj_data`` attributes on the solver).
Joint types
-----------
.. list-table::
:header-rows: 1
:widths: 25 35 40
* - Newton type
- MuJoCo type(s)
- Notes
* - ``FREE``
- ``mjJNT_FREE``
- Initial pose taken from ``body_q``.
* - ``BALL``
- ``mjJNT_BALL``
- Per-axis actuators mapped via ``gear``.
* - ``REVOLUTE``
- ``mjJNT_HINGE``
-
* - ``PRISMATIC``
- ``mjJNT_SLIDE``
-
* - ``D6``
- Up to 3 × ``mjJNT_SLIDE`` + 3 × ``mjJNT_HINGE``
- Each active linear/angular DOF becomes a separate MuJoCo joint
(``_lin``/``_ang`` suffixes, with numeric indices when multiple axes
are active).
* - ``FIXED``
- *(no joint)*
- The child body is nested directly under its parent. If the fixed joint
connects to the world, the body is created as a **mocap** body.
Geometry types
--------------
.. list-table::
:header-rows: 1
:widths: 25 25 50
* - Newton type
- MuJoCo type
- Notes
* - ``SPHERE``
- ``mjGEOM_SPHERE``
-
* - ``CAPSULE``
- ``mjGEOM_CAPSULE``
-
* - ``CYLINDER``
- ``mjGEOM_CYLINDER``
-
* - ``BOX``
- ``mjGEOM_BOX``
-
* - ``ELLIPSOID``
- ``mjGEOM_ELLIPSOID``
-
* - ``PLANE``
- ``mjGEOM_PLANE``
- Must be attached to the world body. Rendered size defaults to
``5 × 5 × 5``.
* - ``HFIELD``
- ``mjGEOM_HFIELD``
- Heightfield data is normalized ``[0, 1]``; the geom origin is shifted
by ``min_z`` so the lowest point is at the correct world height.
* - ``MESH`` / ``CONVEX_MESH``
- ``mjGEOM_MESH``
- MuJoCo only supports **convex** collision meshes. Non-convex meshes are
convex-hulled automatically, which changes the collision boundary.
``maxhullvert`` is forwarded from the mesh source when set.
**Sites** (shapes with the ``SITE`` flag) are converted to MuJoCo sites, which
are non-colliding reference frames used for sensor attachment and spatial
tendons.
Shape parameters
----------------
**Friction.**
Newton's ``shape_material_mu``, ``shape_material_mu_torsional``, and
``shape_material_mu_rolling`` map directly to MuJoCo's three-element
geom ``friction`` vector: ``(sliding, torsional, rolling)``.
**Stiffness and damping (solref).**
Newton's ``shape_material_ke`` (stiffness) and ``shape_material_kd``
(damping) are converted to MuJoCo's geom ``solref`` ``(timeconst, dampratio)``
pair. When either value is zero or negative, the solver falls back to MuJoCo's defaults
(``timeconst = 0.02``, ``dampratio = 1.0``).
**Joint-limit stiffness and damping (solref_limit).**
``joint_limit_ke`` and ``joint_limit_kd`` are forwarded as negative
``solref_limit`` values ``(-ke, -kd)``, which MuJoCo interprets as direct
stiffness/damping rather than time-constant/damp-ratio.
**Margin.**
Newton's ``shape_margin`` maps to MuJoCo ``geom_margin``.
**MuJoCo-specific custom attributes.**
Many MuJoCo-specific parameters are stored in Newton's ``mujoco``
custom-attribute namespace and forwarded to the MuJoCo model when present.
These cover geom properties (``condim``, ``geom_priority``, ``geom_solimp``,
``geom_solmix``), joint properties (``dof_passive_stiffness``,
``dof_passive_damping``, ``jnt_actgravcomp``, ``dof_springref``, ``dof_ref``,
``limit_margin``, ``solimplimit``, ``solreffriction``, ``solimpfriction``),
equality constraints (``eq_solref``), tendons, general actuators, and solver
options. See :doc:`/concepts/custom_attributes` for the full list.
Collision filtering
-------------------
Newton uses integer ``shape_collision_group`` labels to control which shapes
can collide. MuJoCo uses ``contype``/``conaffinity`` bitmasks with a different
semantic: two geoms collide when
``(contype_A & conaffinity_B) || (contype_B & conaffinity_A)`` is non-zero.
The solver bridges the two systems with **graph coloring**. Shapes that must
*not* collide are assigned the same color; each color maps to one bit in
``contype``. ``conaffinity`` is set to the complement so that same-color
geoms never match. Up to 32 colors are supported (one per ``contype`` bit).
If the graph requires more than 32 colors, shapes with color index ≥ 32 fall
back to MuJoCo defaults (``contype=1``, ``conaffinity=1``) and will collide
with all other shapes, silently bypassing the intended filtering.
Non-colliding shapes (no ``COLLIDE_SHAPES`` flag, or ``collision_group == 0``)
get ``contype = conaffinity = 0``.
Additionally, body pairs for which all shape-shape combinations are filtered
are registered as ```` elements.
Mass and inertia
----------------
Bodies with positive mass have their mass, center-of-mass offset (``ipos``),
and inertia tensor set explicitly (``explicitinertial="true"``). When the
inertia tensor is diagonal the solver uses ``diaginertia``; otherwise it uses
``fullinertia``.
Zero-mass bodies (e.g. sensor frames) omit mass and inertia entirely, letting
MuJoCo derive them from child geoms (``inertiafromgeom="auto"``).
Actuators
---------
Newton's per-DOF ``joint_target_mode`` determines which MuJoCo general
actuators are created:
.. list-table::
:header-rows: 1
:widths: 25 75
* - Mode
- MuJoCo actuator(s)
* - ``POSITION``
- One actuator: ``gainprm = [kp]``, ``biasprm = [0, -kp, -kd]``.
* - ``VELOCITY``
- One actuator: ``gainprm = [kd]``, ``biasprm = [0, 0, -kd]``.
* - ``POSITION_VELOCITY``
- Two actuators — a position actuator (``gainprm = [kp]``,
``biasprm = [0, -kp, 0]``) and a velocity actuator
(``gainprm = [kd]``, ``biasprm = [0, 0, -kd]``).
* - ``NONE``
- No actuator created for this DOF.
``joint_effort_limit`` is forwarded as ``actfrcrange`` on the joint (for
prismatic, revolute, and D6 joints) or as ``forcerange`` on the actuator (for
ball joints).
Additional MuJoCo general actuators (motors, etc.) can be attached through
custom attributes and are appended after the joint-target actuators.
Equality constraints
--------------------
.. list-table::
:header-rows: 1
:widths: 20 20 60
* - Newton type
- MuJoCo type
- Notes
* - ``CONNECT``
- ``mjEQ_CONNECT``
- Anchor forwarded in ``data[0:3]``.
* - ``WELD``
- ``mjEQ_WELD``
- Relative pose and torque scale forwarded.
* - ``JOINT``
- ``mjEQ_JOINT``
- Polynomial coefficients forwarded in ``data[0:5]``.
* - Mimic
- ``mjEQ_JOINT``
- ``coef0`` / ``coef1`` mapped to polynomial coefficients.
Only ``REVOLUTE`` and ``PRISMATIC`` joints are supported.
``eq_solref`` custom attributes are forwarded when present.
Solver options
--------------
Solver parameters follow a three-level resolution priority:
1. **Constructor argument** — value passed to :class:`~newton.solvers.SolverMuJoCo`.
2. **Custom attribute** (``model.mujoco.``) — supports per-world values.
These attributes are typically populated automatically when importing USD or
MJCF assets.
3. **Default** — the table below lists Newton defaults alongside MuJoCo
defaults for reference.
.. list-table::
:header-rows: 1
:widths: 25 25 25 25
* - Option
- Newton default
- MuJoCo default
- Notes
* - ``solver``
- ``newton``
- ``newton``
-
* - ``integrator``
- ``implicitfast``
- ``euler``
- ``implicitfast`` provides better stability for stiff systems.
* - ``cone``
- ``pyramidal``
- ``pyramidal``
-
* - ``iterations``
- 100
- 100
-
* - ``ls_iterations``
- 50
- 50
-
Multi-world support
-------------------
When ``separate_worlds=True`` (the default for GPU mode with multiple worlds),
the solver builds a
MuJoCo model from the **first world** only and replicates it across all worlds
via ``mujoco_warp``. This requires all Newton worlds to be structurally
identical (same bodies, joints, and shapes). Global entities (those with a
negative world index) may only include static shapes — they are shared across
all worlds without replication.
Collision pipeline
------------------
By default :class:`~newton.solvers.SolverMuJoCo` uses MuJoCo's built-in
collision detection (``use_mujoco_contacts=True``). Alternatively, you can set
``use_mujoco_contacts=False`` and pass contacts computed by Newton's own
collision pipeline into :meth:`~newton.solvers.SolverMuJoCo.step`. Newton's
pipeline supports non-convex meshes, SDF-based contacts, and hydroelastic
contacts, which are not available through MuJoCo's collision detection.
Caveats
-------
**geom_gap is always zero.**
MuJoCo's ``gap`` parameter controls *inactive* contact generation — contacts
that are detected but do not produce constraint forces until penetration
exceeds the gap threshold. Newton does not use this concept: when the MuJoCo
collision pipeline is active it runs every step, so there is no benefit to
keeping inactive contacts around. Setting ``geom_gap > 0`` would inflate
``geom_margin``, which disables MuJoCo's multicontact and degrades contact
quality. Therefore :class:`~newton.solvers.SolverMuJoCo` always sets
``geom_gap = 0`` regardless of the Newton :attr:`~newton.ModelBuilder.ShapeConfig.gap`
value. MJCF/USD ``gap`` values are still imported into
:attr:`~newton.ModelBuilder.ShapeConfig.gap` in the Newton model, but they are
not forwarded to the MuJoCo solver.
**shape_collision_radius is ignored.**
MuJoCo computes bounding-sphere radii (``geom_rbound``) internally from the
geometry definition. Newton's ``shape_collision_radius`` is not forwarded.
**Non-convex meshes are convex-hulled.**
MuJoCo only supports convex collision geometry. If a Newton ``MESH`` shape
is non-convex, MuJoCo will automatically compute its convex hull, changing
the effective collision boundary.
**Velocity limits are not forwarded.**
Newton's ``joint_velocity_limit`` has no MuJoCo equivalent and is ignored.
**Body ordering must be depth-first.**
The solver sorts joints in depth-first topological order for MuJoCo's
kinematic tree. If the Newton model's joint order differs, a warning is
emitted because kinematics may diverge.
.. _mujoco-kinematic-links-and-fixed-roots:
Kinematic Links and Fixed Roots
-------------------------------
Newton only allows ``is_kinematic=True`` on articulation roots, so in the
MuJoCo exporter a "kinematic link" always means a kinematic root body. Any
descendants of that root can still be dynamic and are exported normally.
At runtime, :class:`~newton.solvers.SolverMuJoCo` keeps kinematic roots
user-prescribed rather than dynamically integrated:
- When converting MuJoCo state back to Newton, the previous Newton
:attr:`newton.State.joint_q` and :attr:`newton.State.joint_qd` values are
passed through for kinematic roots instead of being overwritten from MuJoCo's
integrated ``qpos`` and ``qvel``.
- Applied body wrenches and joint forces targeting kinematic bodies are ignored
on the MuJoCo side.
- Kinematic bodies still participate in contacts, so they can act as moving or
fixed obstacles for dynamic bodies.
During export, :class:`~newton.solvers.SolverMuJoCo` maps roots according to
their joint type:
- **Kinematic roots with non-fixed joints** are exported as ordinary MuJoCo
joints with the same Newton joint type and DOFs. The solver assigns a very
large internal armature to those DOFs so MuJoCo treats them like prescribed,
effectively infinite-mass coordinates.
- **Roots attached to world with a fixed joint** are exported as MuJoCo mocap
bodies. This applies to both kinematic and non-kinematic Newton roots
attached to world by :class:`~newton.JointType.FIXED`. MuJoCo has no joint
coordinates for a fixed root, so Newton drives the pose through
``mjData.mocap_pos`` and ``mjData.mocap_quat`` instead.
- **World-attached shapes that are not part of an articulation** remain
ordinary static MuJoCo geometry rather than mocap bodies.
If you edit :attr:`newton.Model.joint_X_p` or :attr:`newton.Model.joint_X_c`
for a fixed-root articulation after constructing the solver, call
``solver.notify_model_changed(newton.solvers.SolverNotifyFlags.JOINT_PROPERTIES)``
to synchronize the updated fixed-root poses into MuJoCo.
================================================
FILE: docs/make.bat
================================================
@ECHO OFF
pushd %~dp0
REM Command file for Sphinx documentation
if "%SPHINXBUILD%" == "" (
set SPHINXBUILD=sphinx-build
)
set SOURCEDIR=.
set BUILDDIR=_build
%SPHINXBUILD% >NUL 2>NUL
if errorlevel 9009 (
echo.
echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
echo.installed, then set the SPHINXBUILD environment variable to point
echo.to the full path of the 'sphinx-build' executable. Alternatively you
echo.may add the Sphinx directory to PATH.
echo.
echo.If you don't have Sphinx installed, grab it from
echo.https://www.sphinx-doc.org/
exit /b 1
)
if "%1" == "" goto help
%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
goto end
:help
%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
:end
popd
================================================
FILE: docs/migration.rst
================================================
.. SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
.. SPDX-License-Identifier: CC-BY-4.0
.. currentmodule:: newton
``warp.sim`` Migration Guide
============================
This guide is designed for users seeking to migrate their applications from ``warp.sim`` to Newton.
Solvers
-------
+------------------------------------------------------------------------------+-------------------------------------------------------------------------------------+
| **warp.sim** | **Newton** |
+------------------------------------------------------------------------------+-------------------------------------------------------------------------------------+
|:class:`warp.sim.FeatherstoneIntegrator` |:class:`newton.solvers.SolverFeatherstone` |
+------------------------------------------------------------------------------+-------------------------------------------------------------------------------------+
|:class:`warp.sim.SemiImplicitIntegrator` |:class:`newton.solvers.SolverSemiImplicit` |
+------------------------------------------------------------------------------+-------------------------------------------------------------------------------------+
|:class:`warp.sim.VBDIntegrator` |:class:`newton.solvers.SolverVBD` |
+------------------------------------------------------------------------------+-------------------------------------------------------------------------------------+
|:class:`warp.sim.XPBDIntegrator` |:class:`newton.solvers.SolverXPBD` |
+------------------------------------------------------------------------------+-------------------------------------------------------------------------------------+
| ``integrator.simulate(self.model, self.state0, self.state1, self.dt, None)`` | ``solver.step(self.state0, self.state1, self.control, None, self.dt)`` |
+------------------------------------------------------------------------------+-------------------------------------------------------------------------------------+
Importers
---------
+-----------------------------------------------+---------------------------------------------------------+
| **warp.sim** | **Newton** |
+-----------------------------------------------+---------------------------------------------------------+
|:func:`warp.sim.parse_urdf` |:meth:`newton.ModelBuilder.add_urdf` |
+-----------------------------------------------+---------------------------------------------------------+
|:func:`warp.sim.parse_mjcf` |:meth:`newton.ModelBuilder.add_mjcf` |
+-----------------------------------------------+---------------------------------------------------------+
|:func:`warp.sim.parse_usd` |:meth:`newton.ModelBuilder.add_usd` |
+-----------------------------------------------+---------------------------------------------------------+
The joint-specific arguments to the importers have been removed.
Instead, you can set the default joint properties on a :class:`newton.ModelBuilder` instance in the :attr:`newton.ModelBuilder.default_joint_cfg` attribute.
For example, ``limit_lower`` is now defined using ``builder.default_joint_cfg.limit_lower``, where ``builder`` is an instance of :class:`newton.ModelBuilder`.
Similarly, the shape contact parameters have been removed from the importers.
Instead, you can set the default contact parameters on a :class:`newton.ModelBuilder` instance in the :attr:`newton.ModelBuilder.default_shape_cfg` object before loading the asset.
For example, ``ke`` is now defined using ``builder.default_shape_cfg.ke``, where ``builder`` is an instance of :class:`newton.ModelBuilder`.
The MJCF and URDF importers both have an ``up_axis`` argument that defaults to +Z.
All importers will rotate the asset now to match the builder's ``up_axis`` (instead of overwriting the ``up_axis`` in the builder, as was the case previously for the USD importer).
:meth:`newton.ModelBuilder.add_usd` accepts both file paths and URLs directly, so a separate
``resolve_usd_from_url()`` helper is usually unnecessary when migrating from ``warp.sim``.
The MJCF importer from Warp sim only uses the ``geom_density`` defined in the MJCF for sphere and box shapes but ignores these definitions for other shape types (which will receive the default density specified by the ``density`` argument to ``wp.sim.parse_mjcf``). The Newton MJCF importer now considers the ``geom_density`` for all shape types. This change may yield to different simulation results and may require tuning contact and other simulation parameters to achieve similar results in Newton compared to Warp sim.
``Model``
---------
:attr:`newton.Model.shape_is_solid` is now of dtype ``bool`` instead of ``wp.uint8``.
The ``Model.ground`` attribute and the special ground collision handling have been removed. Instead, you need to manually add a ground plane via :meth:`newton.ModelBuilder.add_ground_plane`.
Newton's public ``spatial_vector`` arrays now use ``(linear, angular)`` ordering.
For example, :attr:`newton.State.body_qd` stores ``(lin_vel, ang_vel)``, whereas
``warp.sim`` followed Warp's native ``(ang_vel, lin_vel)`` convention. See
:ref:`Twist conventions`.
The attributes related to joint axes now have the same dimension as the joint DOFs, which is
:attr:`newton.Model.joint_dof_count`. :attr:`newton.Model.joint_axis` remains available and is
indexed per DOF; use :attr:`newton.Model.joint_qd_start` and :attr:`newton.Model.joint_dof_dim`
to locate a joint's slice in the per-DOF arrays.
For free and D6 joints, Newton stores linear DOFs before angular DOFs in per-axis arrays. In
particular, floating-base slices of :attr:`newton.State.joint_qd`, :attr:`newton.Control.joint_f`,
:attr:`newton.Control.joint_target_pos`, and :attr:`newton.Control.joint_target_vel` use
``(lin_vel, ang_vel)`` ordering, whereas ``warp.sim`` used ``(ang_vel, lin_vel)``.
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| **warp.sim** | **Newton** |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| ``Model.shape_geo_src`` | :attr:`Model.shape_source` |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| ``Model.shape_geo`` | Removed ``ShapeGeometry`` struct |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| ``Model.shape_geo.type``, ``Model.shape_geo.scale``, etc. | :attr:`Model.shape_type`, :attr:`Model.shape_scale`, etc. |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| ``Model.shape_geo.source`` | :attr:`Model.shape_source_ptr` |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| ``Model.shape_materials`` | Removed ``ShapeMaterial`` struct |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| ``Model.shape_materials.ke``, ``Model.shape_materials.kd``, etc. | :attr:`Model.shape_material_ke`, :attr:`Model.shape_material_kd`, etc. |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| ``Model.rigid_contact_torsional_friction`` | :attr:`Model.shape_material_mu_torsional` (now per-shape array) |
| | |
| | Note: these coefficients are now interpreted as absolute values rather than being scaled by the friction coefficient. |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
| ``Model.rigid_contact_rolling_friction`` | :attr:`Model.shape_material_mu_rolling` (now per-shape array) |
| | |
| | Note: these coefficients are now interpreted as absolute values rather than being scaled by the friction coefficient. |
+------------------------------------------------------------------+-----------------------------------------------------------------------------------------------------------------------+
Forward and Inverse Kinematics
------------------------------
The signatures of the :func:`newton.eval_fk` and :func:`newton.eval_ik` functions have been slightly modified to make the mask argument optional:
+--------------------------------------------------------+------------------------------------------------------------------------+
| **warp.sim** | **Newton** |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``eval_fk(model, joint_q, joint_qd, mask, state)`` | ``eval_fk(model, joint_q, joint_qd, state, mask=None)`` |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``eval_ik(model, state, joint_q, joint_qd)`` | ``eval_ik(model, state, joint_q, joint_qd, mask=None)`` |
+--------------------------------------------------------+------------------------------------------------------------------------+
``Control``
-----------
The :class:`newton.Control` interface is split by responsibility:
:attr:`newton.Control.joint_target_pos` and :attr:`newton.Control.joint_target_vel` store per-DOF
position and velocity targets, :attr:`newton.Control.joint_act` stores feedforward actuator input,
and :attr:`newton.Control.joint_f` stores generalized forces/torques. Unlike ``warp.sim``,
``joint_act`` is no longer the target array.
In order to match the MuJoCo convention, :attr:`~newton.Control.joint_f` includes the DOFs of the
free joints as well, so its dimension is :attr:`newton.Model.joint_dof_count`.
``JointMode`` has been replaced by :class:`newton.JointTargetMode`. Direct force control
corresponds to :attr:`newton.JointTargetMode.EFFORT` together with
:attr:`newton.Control.joint_f`, while simultaneous position and velocity target control uses
:attr:`newton.JointTargetMode.POSITION_VELOCITY` together with
:attr:`newton.Control.joint_target_pos` and :attr:`newton.Control.joint_target_vel`.
``ModelBuilder``
----------------
The default up axis of the builder is now Z instead of Y.
Analogously, the geometry types plane, capsule, cylinder, and cone now have their up axis set to the Z axis instead of Y by default.
+--------------------------------------------------------+------------------------------------------------------------------------+
| **warp.sim** | **Newton** |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``ModelBuilder.add_body(origin=..., m=...)`` | ``ModelBuilder.add_body(xform=..., mass=...)`` |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``ModelBuilder._add_shape()`` | :meth:`newton.ModelBuilder.add_shape` |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``ModelBuilder.add_shape_*(pos=..., rot=...)`` | ``ModelBuilder.add_shape_*(xform=...)`` |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``ModelBuilder.add_shape_*(..., ke=..., ka=..., ...)`` | ``ModelBuilder.add_shape_*(cfg=ShapeConfig(ke=..., ka=..., ...))`` |
| | see :class:`newton.ModelBuilder.ShapeConfig` |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``ModelBuilder.add_joint_*(..., target=...)`` | ``ModelBuilder.add_joint_*(..., target_pos=..., target_vel=...)`` |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``ModelBuilder(up_vector=(0, 1, 0))`` | ``ModelBuilder(up_axis="Y")`` or ``ModelBuilder(up_axis=Axis.Y)`` |
+--------------------------------------------------------+------------------------------------------------------------------------+
| ``JointAxis`` | :class:`newton.ModelBuilder.JointDofConfig` |
+--------------------------------------------------------+------------------------------------------------------------------------+
It is now possible to set the up axis of the builder using the :attr:`~newton.ModelBuilder.up_axis` attribute,
which can be defined from any value compatible with the :obj:`~newton.AxisType` alias.
:attr:`newton.ModelBuilder.up_vector` is now a read-only property computed from :attr:`newton.ModelBuilder.up_axis`.
The ``ModelBuilder.add_joint_*()`` functions now use ``None`` defaults that are filled in from
the fields of :attr:`newton.ModelBuilder.default_joint_cfg`.
Newton uses ``world_count`` throughout the public API (for example in
:meth:`newton.ModelBuilder.replicate` and :attr:`newton.Model.world_count`); older ``num_envs``
terminology is obsolete.
The ``ModelBuilder.add_joint*()`` methods no longer accept ``linear_compliance`` and ``angular_compliance`` arguments
and the ``Model`` no longer stores them as attributes.
Instead, you can pass them as arguments to the :class:`newton.solvers.SolverXPBD` constructor. Note that now these values
apply to all joints and cannot be set individually per joint anymore. So far we have not found applications that require
per-joint compliance settings and have decided to remove this feature for memory efficiency.
The :meth:`newton.ModelBuilder.add_joint_free()` method now initializes the positional dofs of the free joint with the child body's transform (``body_q``).
The universal and compound joints have been removed in favor of the more general D6 joint.
Collisions
----------
+-----------------------------------------------+--------------------------------------------------------------+
| **warp.sim** | **Newton** |
+-----------------------------------------------+--------------------------------------------------------------+
| ``contacts = model.collide(state)`` | ``contacts = model.collide(state)`` |
+-----------------------------------------------+--------------------------------------------------------------+
:meth:`~newton.Model.collide` allocates and returns a contacts buffer when ``contacts`` is omitted.
For more control, create a :class:`~newton.CollisionPipeline` directly.
Renderers
---------
+-----------------------------------------------+----------------------------------------------+
| **warp.sim** | **Newton** |
+-----------------------------------------------+----------------------------------------------+
|:class:`warp.sim.render.UsdRenderer` |:class:`newton.viewer.ViewerUSD` |
+-----------------------------------------------+----------------------------------------------+
|:class:`warp.sim.render.OpenGLRenderer` |:class:`newton.viewer.ViewerGL` |
+-----------------------------------------------+----------------------------------------------+
================================================
FILE: docs/print_api.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import importlib
import inspect
def public_symbols(mod) -> list[str]:
"""Return the list of public names for *mod* (honours ``__all__``)."""
if hasattr(mod, "__all__") and isinstance(mod.__all__, list | tuple):
return list(mod.__all__)
def is_public(name: str) -> bool:
if name.startswith("_"):
return False
return not inspect.ismodule(getattr(mod, name))
return sorted(filter(is_public, dir(mod)))
def get_symbols(mod_name: str):
module = importlib.import_module(mod_name)
all_symbols = public_symbols(module)
children = []
for name in all_symbols:
attr = getattr(module, name)
if inspect.ismodule(attr):
children.append(get_symbols(f"{mod_name}.{name}"))
else:
children.append(name)
return (mod_name.split(".")[-1], children)
def print_symbols(sym_dict, indent=0):
name, children = sym_dict[0], sym_dict[1]
print(f"{' ' * indent}{name}:")
symbols = []
submodules = []
for child in children:
if isinstance(child, str):
symbols.append(child)
else:
submodules.append(child)
# sort
symbols.sort()
submodules.sort(key=lambda x: x[0])
for sym in symbols:
print(f"{' ' * (indent + 1)}{sym}")
print()
for sub in submodules:
print_symbols(sub, indent=indent + 1)
print_symbols(get_symbols("newton"))
================================================
FILE: docs/serve.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Simple HTTP server with proper MIME types for WASM and JS module files.
This script is used to test the built Sphinx documentation locally,
including the Viser static viewer which requires correct MIME types.
Usage:
# From the repository root:
python docs/serve.py
# Or specify a custom port:
python docs/serve.py --port 8080
Then open http://localhost:8000 in your browser.
"""
import argparse
import http.server
import mimetypes
import os
import sys
from pathlib import Path
from typing import ClassVar
# Add/override MIME types for proper module loading
mimetypes.add_type("application/wasm", ".wasm")
mimetypes.add_type("application/javascript", ".js")
mimetypes.add_type("application/javascript", ".mjs")
mimetypes.add_type("text/css", ".css")
mimetypes.add_type("application/json", ".json")
class CORSHTTPRequestHandler(http.server.SimpleHTTPRequestHandler):
"""HTTP handler with proper MIME type support and CORS headers."""
# Explicit extensions map for strict MIME type checking
extensions_map: ClassVar[dict[str, str]] = { # pyright: ignore[reportIncompatibleVariableOverride]
".wasm": "application/wasm",
".js": "application/javascript",
".css": "text/css",
".html": "text/html",
".json": "application/json",
".png": "image/png",
".jpg": "image/jpeg",
".jpeg": "image/jpeg",
".gif": "image/gif",
".svg": "image/svg+xml",
".ico": "image/x-icon",
".hdr": "image/vnd.radiance", # HDR image format for viser HDRI backgrounds
".txt": "text/plain",
".viser": "application/octet-stream",
".ttf": "font/ttf",
"": "application/octet-stream",
}
def guess_type(self, path):
"""Guess the MIME type of a file with proper WASM/JS support."""
_, ext = os.path.splitext(path)
ext = ext.lower()
if ext in self.extensions_map:
return self.extensions_map[ext]
mimetype, _ = mimetypes.guess_type(path)
if mimetype is None:
return "application/octet-stream"
return mimetype
def end_headers(self):
"""Add CORS headers for local development."""
self.send_header("Access-Control-Allow-Origin", "*")
self.send_header("Access-Control-Allow-Methods", "GET, OPTIONS")
self.send_header("Access-Control-Allow-Headers", "*")
super().end_headers()
def do_OPTIONS(self):
"""Handle CORS preflight requests."""
self.send_response(200)
self.end_headers()
def main():
parser = argparse.ArgumentParser(description="Serve the built Sphinx documentation with proper MIME types.")
parser.add_argument("--port", "-p", type=int, default=8000, help="Port to serve on (default: 8000)")
parser.add_argument(
"--directory",
"-d",
type=str,
default=None,
help="Directory to serve (default: docs/_build/html)",
)
args = parser.parse_args()
# Determine the directory to serve
if args.directory:
serve_dir = Path(args.directory)
else:
# Default to docs/_build/html relative to this script
script_dir = Path(__file__).parent
serve_dir = script_dir / "_build" / "html"
if not serve_dir.exists():
print(f"Error: Directory {serve_dir} does not exist.")
print("Please build the documentation first with:")
print(" uv run --extra docs --extra sim sphinx-build -b html docs docs/_build/html")
sys.exit(1)
os.chdir(serve_dir)
with http.server.HTTPServer(("", args.port), CORSHTTPRequestHandler) as httpd:
print(f"Serving documentation at: http://localhost:{args.port}")
print(f"Directory: {serve_dir.absolute()}")
print()
print("Press Ctrl+C to stop")
try:
httpd.serve_forever()
except KeyboardInterrupt:
print("\nShutting down...")
if __name__ == "__main__":
main()
================================================
FILE: docs/tutorials/00_introduction.ipynb
================================================
{
"cells": [
{
"cell_type": "raw",
"metadata": {
"nbsphinx": "hidden",
"vscode": {
"languageId": "raw"
}
},
"source": "SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers\nSPDX-License-Identifier: CC-BY-4.0"
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Introduction to Newton Physics\n",
"\n",
"The main components for a Newton simulation are:\n",
"\n",
"1. **Model**: Encapsulates the physical structure, parameters, and configuration.\n",
"2. **State**: Represents the dynamic state (positions, velocities, etc.).\n",
"3. **Solver**: Steps the simulation by integrating physics and resolving contact, joint, and other constraints on the simulated objects.\n",
"\n",
"See also the [Overview page](https://newton-physics.github.io/newton/stable/guide/overview.html).\n",
"\n",
"In this tutorial, we show how to set up the model and state for a simple scene of a few rigid bodies with different collision geometries falling on a ground plane, and how to step the simulation. We'll also show how to visualize the resulting simulation."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Setup and Imports\n",
"\n",
"First, let's import the necessary libraries:\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from pathlib import Path\n",
"\n",
"import warp as wp\n",
"from pxr import Usd\n",
"\n",
"import newton\n",
"\n",
"# these dependencies are needed to load example assets and ingest meshes from USD\n",
"import newton.examples\n",
"import newton.usd"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Building a Model with ModelBuilder\n",
"\n",
"The `ModelBuilder` is the primary way to construct simulation scenes. Newton provides multiple approaches for building models:\n",
"\n",
"1. **Programmatic construction**: Add bodies, shapes, joints, and constraints directly using `add_body()`, `add_shape_*()`, `add_joint_*()` methods\n",
"2. **Import from asset files**: Load complete models from URDF, MJCF (MuJoCo XML), or USD files using `add_urdf()`, `add_mjcf()`, and `add_usd()`\n",
"3. **Parallel environments for RL**: Use `replicate()` to create multiple copies of an environment for parallel training, or combine different models with `add_builder()` for modular scene construction\n",
"\n",
"In this tutorial, we'll focus on programmatic construction to understand the fundamentals. For examples showing asset import and parallelization, see `newton/examples/basic/example_basic_urdf.py` and `newton/examples/robot/`.\n",
"\n",
"Let's create a simple scene with various collision shapes falling onto a ground plane.\n",
"\n",
"### Step 1: Create the ModelBuilder\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Create a new model builder\n",
"builder = newton.ModelBuilder()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Step 2: Add Rigid Bodies with Collision Shapes\n",
"\n",
"Bodies are the fundamental dynamic entities in Newton. Each body can have one or more collision shapes attached to it.\n",
"\n",
"The workflow is:\n",
"1. Create a body with `add_body()` - returns a body index\n",
"2. Attach shapes to the body using `add_shape_*()` methods\n",
"3. Shapes contribute to the body's mass/inertia\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": "# Add a ground plane (infinite static plane at z=0)\nbuilder.add_ground_plane()\n\n# Height from which to drop shapes\ndrop_z = 2.0\n\n# SPHERE\nsphere_pos = wp.vec3(0.0, -4.0, drop_z)\nbody_sphere = builder.add_body(\n xform=wp.transform(p=sphere_pos, q=wp.quat_identity()),\n label=\"sphere\", # Optional: human-readable identifier\n)\nbuilder.add_shape_sphere(body_sphere, radius=0.5)\n\n# CAPSULE\ncapsule_pos = wp.vec3(0.0, -2.0, drop_z)\nbody_capsule = builder.add_body(xform=wp.transform(p=capsule_pos, q=wp.quat_identity()), label=\"capsule\")\nbuilder.add_shape_capsule(body_capsule, radius=0.3, half_height=0.7)\n\n# CYLINDER\ncylinder_pos = wp.vec3(0.0, 0.0, drop_z)\nbody_cylinder = builder.add_body(xform=wp.transform(p=cylinder_pos, q=wp.quat_identity()), label=\"cylinder\")\nbuilder.add_shape_cylinder(body_cylinder, radius=0.4, half_height=0.6)\n\n# Multi-Shape Collider\nmulti_shape_pos = wp.vec3(0.0, 2.0, drop_z)\nbody_multi_shape = builder.add_body(xform=wp.transform(p=multi_shape_pos, q=wp.quat_identity()), label=\"multi_shape\")\n\n# Now attach both a sphere and a box to the multi-shape body\n# body-local shape offsets, offset sphere in x so the body will topple over\nsphere_offset = wp.vec3(0.1, 0.0, -0.3)\nbox_offset = wp.vec3(0.0, 0.0, 0.3)\nbuilder.add_shape_sphere(body_multi_shape, wp.transform(p=sphere_offset, q=wp.quat_identity()), radius=0.25)\nbuilder.add_shape_box(body_multi_shape, wp.transform(p=box_offset, q=wp.quat_identity()), hx=0.25, hy=0.25, hz=0.25)\n\nprint(f\"Added {builder.body_count} bodies with collision shapes\")"
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Step 3: Add a Mesh Body\n",
"\n",
"Newton can also simulate bodies with triangle-mesh collision shapes. Let's load a mesh from a USD file:\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": "# Load a mesh from a USD file\nusd_stage = Usd.Stage.Open(newton.examples.get_asset(\"bunny.usd\"))\ndemo_mesh = newton.usd.get_mesh(usd_stage.GetPrimAtPath(\"/root/bunny\"))\n\n# Add the mesh as a rigid body\nmesh_pos = wp.vec3(0.0, 4.0, drop_z - 0.5)\nbody_mesh = builder.add_body(xform=wp.transform(p=mesh_pos, q=wp.quat(0.5, 0.5, 0.5, 0.5)), label=\"bunny\")\nbuilder.add_shape_mesh(body_mesh, mesh=demo_mesh)\n\nprint(f\"Added mesh body with {demo_mesh.vertices.shape[0]} vertices\")"
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Step 4: Finalize the Model\n",
"\n",
"Once all bodies and shapes are added, we finalize the model. This converts the Python data structures into GPU-optimized arrays and makes the model ready for simulation.\n",
"\n",
"Newton runs on GPU by default (if a GPU is available); you may force the compute device to CPU by setting `use_cpu` to `True` below."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Optional: Run the simulation on CPU\n",
"use_cpu = False\n",
"if use_cpu:\n",
" wp.set_device(\"cpu\") # alternatively, pass device=\"cpu\" to the finalize method\n",
"\n",
"# Finalize the model - this creates the simulation-ready Model object\n",
"model = builder.finalize()\n",
"\n",
"print(f\"Model finalized for device {model.device}:\")\n",
"print(f\" Bodies: {model.body_count}\")\n",
"print(f\" Shapes: {model.shape_count}\")\n",
"print(f\" Joints: {model.joint_count}\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating States and Control\n",
"\n",
"After finalizing the model, we can create the objects that hold time-varying data, i.e. the data that is changing with each simulation step.\n",
"\n",
"- **State**: Holds positions, velocities, and forces\n",
"- **Control**: Holds user-set control inputs (joint torques, motor commands, etc.)\n",
"- **Contacts**: Holds contacts generated by the [collision pipeline](https://newton-physics.github.io/newton/stable/concepts/collisions.html) for processing in the solver\n",
"\n",
"Some solvers rely on input and output state to be separated in memory when running the `solver.step()` method, including XPBD.\n",
"The MuJoCo solver, on the other hand, can be run with just a single state passed in as both input and output states.\n",
"\n",
"Note that for differentiable simulations you may need to allocate a new state for every substep of the simulation.\n",
"To accommodate these different use cases, we leave the memory management of the `State` and `Control` objects up to the user."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Create two state objects for time integration\n",
"state_0 = model.state() # Current state\n",
"state_1 = model.state() # Next state\n",
"\n",
"# The control object is not used in this example, but we create it for completeness\n",
"control = model.control()\n",
"\n",
"# Allocate Contacts buffer\n",
"contacts = model.contacts()\n",
"\n",
"print(\"State, Contacts and Control objects created\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Setting Up the Solver\n",
"\n",
"Newton provides multiple solver implementations. For this example, we use **XPBD** (Extended Position-Based Dynamics).\n",
"\n",
"For other available solvers and their features/strengths, please refer to the [Solvers feature overview](https://newton-physics.github.io/newton/stable/api/newton_solvers.html#supported-features).\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Create the XPBD solver with 10 constraint iterations\n",
"solver = newton.solvers.SolverXPBD(model, iterations=10)\n",
"\n",
"print(f\"Solver created: {type(solver).__name__}\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Configuring the Simulation Loop\n",
"\n",
"Let's set up the simulation parameters and create a simulation function:\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Simulation parameters\n",
"fps = 60 # Frames per second for visualization\n",
"frame_dt = 1.0 / fps # Time step per frame\n",
"sim_substeps = 10 # Number of physics substeps per frame\n",
"sim_dt = frame_dt / sim_substeps # Physics time step\n",
"\n",
"print(\"Simulation configured:\")\n",
"print(f\" Frame rate: {fps} Hz\")\n",
"print(f\" Frame dt: {frame_dt:.4f} s\")\n",
"print(f\" Physics substeps: {sim_substeps}\")\n",
"print(f\" Physics dt: {sim_dt:.4f} s\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### The Simulation Function\n",
"\n",
"The core simulation loop executed for each substep follows this pattern:\n",
"\n",
"1. Clear forces in `State` that may have been set by the solver or user in the previous step\n",
"2. Apply external forces\n",
"3. Detect collisions\n",
"4. Step the solver forward in time\n",
"5. Swap state buffers\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"def simulate():\n",
" \"\"\"Run multiple physics substeps for one frame.\"\"\"\n",
" global state_0, state_1\n",
"\n",
" for _ in range(sim_substeps):\n",
" # 1. Clear forces in input state\n",
" state_0.clear_forces()\n",
"\n",
" # 2. Apply control targets/forces, and viewer picking forces if using the OpenGL viewer\n",
" # update_control(state_0, control)\n",
" # viewer.apply_forces(state_0)\n",
"\n",
" # 3. Detect collisions\n",
" model.collide(state_0, contacts)\n",
"\n",
" # 4. Step the simulation by one physics timestep\n",
" solver.step(state_in=state_0, state_out=state_1, control=control, contacts=contacts, dt=sim_dt)\n",
"\n",
" # 5. Swap states (next becomes current)\n",
" state_0, state_1 = state_1, state_0"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## GPU Acceleration with CUDA Graphs\n",
"\n",
"For maximum performance on CUDA devices, we can capture the simulation loop as a CUDA graph. This reduces kernel launch overhead significantly:\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Capture the simulation as a CUDA graph (if running on GPU)\n",
"if wp.get_device().is_cuda:\n",
" with wp.ScopedCapture() as capture:\n",
" simulate()\n",
" graph = capture.graph\n",
" print(\"CUDA graph captured for optimized execution\")\n",
"else:\n",
" graph = None\n",
" print(\"Running on CPU (no CUDA graph)\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Visualization\n",
"\n",
"There are several viewer types available in Newton that can be used to display and/or debug simulations, see the [Visualization section in the docs](https://newton-physics.github.io/newton/stable/guide/visualization.html).\n",
"\n",
"In this example, we use the [Viser](https://github.com/nerfstudio-project/viser) viewer, which launches a web server and can be embedded in Jupyter notebooks or viewed in a browser."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Create the Viser viewer with a path to save the recording\n",
"recording_path = Path(\"../_static/recordings/00_introduction.viser\").resolve()\n",
"recording_path.parent.mkdir(parents=True, exist_ok=True)\n",
"viewer = newton.viewer.ViewerViser(verbose=False, record_to_viser=str(recording_path))\n",
"\n",
"# Set the model (this logs the static geometry)\n",
"viewer.set_model(model)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Running the Simulation\n",
"\n",
"Now let's run the simulation and visualize it! We'll simulate 500 frames (about 8 seconds at 60 fps).\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Run the simulation\n",
"num_frames = 500\n",
"sim_time = 0.0 # Current simulation time in seconds\n",
"for _ in range(num_frames):\n",
" # Execute the simulation (use CUDA graph if available)\n",
" if graph:\n",
" wp.capture_launch(graph)\n",
" else:\n",
" simulate()\n",
"\n",
" # Log the current state to the viewer\n",
" viewer.begin_frame(sim_time)\n",
" viewer.log_state(state_0)\n",
"\n",
" # Log contacts to the viewer (not supported by the Viser viewer)\n",
" viewer.log_contacts(contacts, state_0)\n",
" viewer.end_frame()\n",
"\n",
" # Advance simulation time\n",
" sim_time += frame_dt\n",
"\n",
"print(f\"\\nSimulation complete! Total time: {sim_time:.2f} seconds\")\n",
"\n",
"viewer"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Next Steps\n",
"\n",
"To learn more about Newton:\n",
"\n",
"- **Examples**: Explore the `newton/examples/` directory for more complex scenarios\n",
"- **Documentation**: Visit [newton-physics.github.io](https://newton-physics.github.io/newton/stable/)\n"
]
}
],
"metadata": {
"kernelspec": {
"display_name": ".venv",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.11.12"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
================================================
FILE: newton/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
# ==================================================================================
# core
# ==================================================================================
from ._src.core import (
MAXVAL,
Axis,
AxisType,
)
from ._version import __version__
__all__ = [
"MAXVAL",
"Axis",
"AxisType",
"__version__",
]
# ==================================================================================
# geometry
# ==================================================================================
from ._src.geometry import (
SDF,
Gaussian,
GeoType,
Heightfield,
Mesh,
ParticleFlags,
ShapeFlags,
TetMesh,
)
__all__ += [
"SDF",
"Gaussian",
"GeoType",
"Heightfield",
"Mesh",
"ParticleFlags",
"ShapeFlags",
"TetMesh",
]
# ==================================================================================
# sim
# ==================================================================================
from ._src.sim import ( # noqa: E402
BodyFlags,
CollisionPipeline,
Contacts,
Control,
EqType,
JointTargetMode,
JointType,
Model,
ModelBuilder,
State,
eval_fk,
eval_ik,
eval_jacobian,
eval_mass_matrix,
)
__all__ += [
"BodyFlags",
"CollisionPipeline",
"Contacts",
"Control",
"EqType",
"JointTargetMode",
"JointType",
"Model",
"ModelBuilder",
"State",
"eval_fk",
"eval_ik",
"eval_jacobian",
"eval_mass_matrix",
]
# ==================================================================================
# submodule APIs
# ==================================================================================
from . import geometry, ik, math, selection, sensors, solvers, usd, utils, viewer # noqa: E402
__all__ += [
"geometry",
"ik",
"math",
"selection",
"sensors",
"solvers",
"usd",
"utils",
"viewer",
]
================================================
FILE: newton/_src/core/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from ..math import (
quat_between_axes,
)
from .types import (
MAXVAL,
Axis,
AxisType,
)
__all__ = [
"MAXVAL",
"Axis",
"AxisType",
"quat_between_axes",
]
================================================
FILE: newton/_src/core/types.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Common definitions for types and constants."""
from __future__ import annotations
from collections.abc import Callable, Sequence
from enum import IntEnum
from typing import TYPE_CHECKING, Any, Literal, TypeVar
import numpy as np
import warp as wp
from warp import DeviceLike as Devicelike
_F = TypeVar("_F", bound=Callable[..., Any])
def _override_noop(func: _F, /) -> _F:
"""Fallback no-op decorator when override is unavailable."""
return func
if TYPE_CHECKING:
from typing_extensions import override
else:
try:
from typing import override as _override
except ImportError:
try:
from typing_extensions import override as _override
except ImportError:
_override = _override_noop
override = _override
warp_int_types = (wp.int8, wp.uint8, wp.int16, wp.uint16, wp.int32, wp.uint32, wp.int64, wp.uint64)
def flag_to_int(flag):
"""Converts a flag (Warp constant) to an integer."""
if type(flag) in warp_int_types:
return flag.value
return int(flag)
Vec2 = list[float] | tuple[float, float] | wp.vec2
"""A 2D vector represented as a list or tuple of 2 floats."""
Vec3 = list[float] | tuple[float, float, float] | wp.vec3
"""A 3D vector represented as a list or tuple of 3 floats."""
Vec4 = list[float] | tuple[float, float, float, float] | wp.vec4
"""A 4D vector represented as a list or tuple of 4 floats."""
Vec6 = list[float] | tuple[float, float, float, float, float, float] | wp.spatial_vector
"""A 6D vector represented as a list or tuple of 6 floats or a ``warp.spatial_vector``."""
Quat = list[float] | tuple[float, float, float, float] | wp.quat
"""A quaternion represented as a list or tuple of 4 floats (in XYZW order)."""
Mat22 = list[float] | wp.mat22
"""A 2x2 matrix represented as a list of 4 floats or a ``warp.mat22``."""
Mat33 = list[float] | wp.mat33
"""A 3x3 matrix represented as a list of 9 floats or a ``warp.mat33``."""
Transform = tuple[Vec3, Quat] | wp.transform
"""A 3D transformation represented as a tuple of 3D translation and rotation quaternion (in XYZW order)."""
# Warp vector types
vec5 = wp.types.vector(length=5, dtype=wp.float32)
vec10 = wp.types.vector(length=10, dtype=wp.float32)
# Large finite value used as sentinel (matches MuJoCo's mjMAXVAL)
MAXVAL = 1e10
"""Large finite sentinel value for 'no limit' / 'no hit' / 'invalid' markers.
Use this instead of infinity to avoid verify_fp false positives.
For comparisons with volume-sampled data, use `>= wp.static(MAXVAL * 0.99)` to handle
interpolation-induced floating-point errors.
"""
class Axis(IntEnum):
"""Enumeration of axes in 3D space."""
X = 0
"""X-axis."""
Y = 1
"""Y-axis."""
Z = 2
"""Z-axis."""
@classmethod
def from_string(cls, axis_str: str) -> Axis:
"""
Convert a string representation of an axis ("x", "y", or "z") to the corresponding Axis enum member.
Args:
axis_str: The axis as a string. Should be "x", "y", or "z" (case-insensitive).
Returns:
The corresponding Axis enum member.
Raises:
ValueError: If the input string does not correspond to a valid axis.
"""
axis_str = axis_str.lower()
if axis_str == "x":
return cls.X
elif axis_str == "y":
return cls.Y
elif axis_str == "z":
return cls.Z
raise ValueError(f"Invalid axis string: {axis_str}")
@classmethod
def from_any(cls, value: AxisType) -> Axis:
"""
Convert a value of various types to an Axis enum member.
Args:
value: The value to convert. Can be an Axis, str, or int-like.
Returns:
The corresponding Axis enum member.
Raises:
TypeError: If the value cannot be converted to an Axis.
ValueError: If the string or integer does not correspond to a valid Axis.
"""
if isinstance(value, cls):
return value
if isinstance(value, str):
return cls.from_string(value)
if type(value) in {int, wp.int32, wp.int64, np.int32, np.int64}:
return cls(value)
raise TypeError(f"Cannot convert {type(value)} to Axis")
@override
def __str__(self):
return self.name.capitalize()
@override
def __repr__(self):
return f"Axis.{self.name.capitalize()}"
@override
def __eq__(self, other):
if isinstance(other, str):
return self.name.lower() == other.lower()
if type(other) in {int, wp.int32, wp.int64, np.int32, np.int64}:
return self.value == int(other)
return NotImplemented
@override
def __hash__(self):
return hash(self.name)
def to_vector(self) -> tuple[float, float, float]:
"""
Return the axis as a 3D unit vector.
Returns:
The unit vector corresponding to the axis.
"""
if self == Axis.X:
return (1.0, 0.0, 0.0)
elif self == Axis.Y:
return (0.0, 1.0, 0.0)
else:
return (0.0, 0.0, 1.0)
def to_vec3(self) -> wp.vec3:
"""
Return the axis as a warp.vec3 unit vector.
Returns:
The unit vector corresponding to the axis.
"""
return wp.vec3(*self.to_vector())
def quat_between_axes(self, other: Axis) -> wp.quat:
"""
Return the quaternion between two axes.
"""
return wp.quat_between_vectors(self.to_vec3(), other.to_vec3())
AxisType = Axis | Literal["X", "Y", "Z"] | Literal[0, 1, 2] | int | str
"""Type that can be used to represent an axis, including the enum, string, and integer representations."""
def axis_to_vec3(axis: AxisType | Vec3) -> wp.vec3:
"""Convert an axis representation to a 3D vector."""
if isinstance(axis, list | tuple | np.ndarray):
return wp.vec3(*axis)
elif wp.types.type_is_vector(type(axis)):
return axis
else:
return Axis.from_any(axis).to_vec3()
__all__ = [
"MAXVAL",
"Axis",
"AxisType",
"Devicelike",
"Mat22",
"Mat33",
"Quat",
"Sequence",
"Transform",
"Vec2",
"Vec3",
"Vec4",
"Vec6",
"flag_to_int",
"override",
"vec5",
"vec10",
]
================================================
FILE: newton/_src/geometry/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .broad_phase_common import test_group_pair, test_world_and_group_pair
from .broad_phase_nxn import BroadPhaseAllPairs, BroadPhaseExplicit
from .broad_phase_sap import BroadPhaseSAP
from .collision_primitive import (
collide_box_box,
collide_capsule_box,
collide_capsule_capsule,
collide_plane_box,
collide_plane_capsule,
collide_plane_cylinder,
collide_plane_ellipsoid,
collide_plane_sphere,
collide_sphere_box,
collide_sphere_capsule,
collide_sphere_cylinder,
collide_sphere_sphere,
)
from .flags import ParticleFlags, ShapeFlags
from .inertia import compute_inertia_shape, compute_inertia_sphere, transform_inertia
from .sdf_utils import SDF
from .terrain_generator import create_mesh_heightfield, create_mesh_terrain
from .types import (
Gaussian,
GeoType,
Heightfield,
Mesh,
TetMesh,
)
from .utils import compute_shape_radius
__all__ = [
"SDF",
"BroadPhaseAllPairs",
"BroadPhaseExplicit",
"BroadPhaseSAP",
"Gaussian",
"GeoType",
"Heightfield",
"Mesh",
"ParticleFlags",
"ShapeFlags",
"TetMesh",
"collide_box_box",
"collide_capsule_box",
"collide_capsule_capsule",
"collide_plane_box",
"collide_plane_capsule",
"collide_plane_cylinder",
"collide_plane_ellipsoid",
"collide_plane_sphere",
"collide_sphere_box",
"collide_sphere_capsule",
"collide_sphere_cylinder",
"collide_sphere_sphere",
"compute_inertia_shape",
"compute_inertia_sphere",
"compute_shape_radius",
"create_mesh_heightfield",
"create_mesh_terrain",
"test_group_pair",
"test_world_and_group_pair",
"transform_inertia",
]
================================================
FILE: newton/_src/geometry/broad_phase_common.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Common utilities for broad phase collision detection.
Provides shared functions for AABB overlap tests, world/group filtering,
and pair output used by both NxN and SAP broad phase implementations.
"""
from typing import Any
import numpy as np
import warp as wp
from .flags import ShapeFlags
@wp.func
def check_aabb_overlap(
box1_lower: wp.vec3,
box1_upper: wp.vec3,
box1_cutoff: float,
box2_lower: wp.vec3,
box2_upper: wp.vec3,
box2_cutoff: float,
) -> bool:
cutoff_combined = box1_cutoff + box2_cutoff
return (
box1_lower[0] <= box2_upper[0] + cutoff_combined
and box1_upper[0] >= box2_lower[0] - cutoff_combined
and box1_lower[1] <= box2_upper[1] + cutoff_combined
and box1_upper[1] >= box2_lower[1] - cutoff_combined
and box1_lower[2] <= box2_upper[2] + cutoff_combined
and box1_upper[2] >= box2_lower[2] - cutoff_combined
)
@wp.func
def binary_search(values: wp.array[Any], value: Any, lower: int, upper: int) -> int:
while lower < upper:
mid = (lower + upper) >> 1
if values[mid] > value:
upper = mid
else:
lower = mid + 1
return upper
@wp.func
def _vec2i_less(p: wp.vec2i, q: wp.vec2i) -> bool:
"""Lexicographic less-than for vec2i.
Args:
p: First vector to compare.
q: Second vector to compare.
Returns:
True if p < q lexicographically, i.e. p[0] < q[0] or (p[0] == q[0] and p[1] < q[1]).
"""
if p[0] < q[0]:
return True
if p[0] > q[0]:
return False
return p[1] < q[1]
@wp.func
def _vec2i_equal(p: wp.vec2i, q: wp.vec2i) -> bool:
"""Element-wise equality for vec2i.
Args:
p: First vector to compare.
q: Second vector to compare.
Returns:
True if p[0] == q[0] and p[1] == q[1].
"""
return p[0] == q[0] and p[1] == q[1]
@wp.func
def is_pair_excluded(
pair: wp.vec2i,
filter_pairs: wp.array[wp.vec2i],
num_filter_pairs: int,
) -> bool:
"""Check whether a shape pair is in the sorted exclusion list via binary search.
Args:
pair: Canonical shape pair (min, max) to look up.
filter_pairs: Lexicographically sorted array of excluded shape pairs.
Each entry must be canonical (min, max).
num_filter_pairs: Number of valid entries in ``filter_pairs``.
Returns:
True if ``pair`` is found in ``filter_pairs``, False otherwise.
Returns False immediately when ``num_filter_pairs`` is 0.
"""
if num_filter_pairs <= 0:
return False
low = int(0)
high = num_filter_pairs - 1
while low <= high:
mid = (low + high) >> 1
m = filter_pairs[mid]
if _vec2i_equal(pair, m):
return True
if _vec2i_less(pair, m):
high = mid - 1
else:
low = mid + 1
return False
@wp.func
def write_pair(
pair: wp.vec2i,
candidate_pair: wp.array[wp.vec2i],
candidate_pair_count: wp.array[int], # Size one array
max_candidate_pair: int,
):
pairid = wp.atomic_add(candidate_pair_count, 0, 1)
if pairid >= max_candidate_pair:
return
candidate_pair[pairid] = pair
# Collision filtering
@wp.func
def test_group_pair(group_a: int, group_b: int) -> bool:
"""Test if two collision groups should interact.
Args:
group_a: First collision group ID. Positive values indicate groups that only collide with themselves (and with negative groups).
Negative values indicate groups that collide with everything except their negative counterpart.
Zero indicates no collisions.
group_b: Second collision group ID. Same meaning as group_a.
Returns:
bool: True if the groups should collide, False if they should not.
"""
if group_a == 0 or group_b == 0:
return False
if group_a > 0:
return group_a == group_b or group_b < 0
if group_a < 0:
return group_a != group_b
@wp.func
def test_world_and_group_pair(world_a: int, world_b: int, collision_group_a: int, collision_group_b: int) -> bool:
"""Test if two entities should collide based on world indices and collision groups.
World indices define which simulation world an entity belongs to:
- Index -1: Global entities that collide with all worlds
- Indices 0, 1, 2, ...: World-specific entities
Collision rules:
1. Entities from different worlds (except -1) do not collide
2. Global entities (index -1) collide with all worlds
3. Within the same world, collision groups determine interactions
Args:
world_a: World index of first entity
world_b: World index of second entity
collision_group_a: Collision group of first entity
collision_group_b: Collision group of second entity
Returns:
bool: True if the entities should collide, False otherwise
"""
# Check world indices first
if world_a != -1 and world_b != -1 and world_a != world_b:
return False
# If same world or at least one is global (-1), check collision groups
return test_group_pair(collision_group_a, collision_group_b)
def precompute_world_map(shape_world: np.ndarray | list[int], shape_flags: np.ndarray | list[int] | None = None):
"""Precompute an index map that groups shapes by world ID with shared shapes.
This method creates an index mapping where shapes belonging to the same world
(non-negative world ID) are grouped together, and shared shapes
(world ID -1) are appended to each world's slice.
A dedicated segment at the end contains only world -1 objects for handling
-1 vs -1 collisions without duplication.
Optionally filters out shapes that should not participate in collision detection
based on their flags (e.g., visual-only shapes without COLLIDE_SHAPES flag).
Args:
shape_world: Array of world IDs. Must contain only:
- World ID -1: Global/shared entities that collide with all worlds
- World IDs >= 0: World-specific entities (0, 1, 2, ...)
World IDs < -1 are not supported and will raise ValueError.
shape_flags: Optional array of shape flags. If provided, only shapes with the
COLLIDE_SHAPES flag (bit 1) set will be included in the output map. This allows
efficient filtering of visual-only shapes that shouldn't participate in collision.
Raises:
ValueError: If shape_flags is provided and lengths don't match shape_world, or if
any world IDs are < -1.
Returns:
tuple: (index_map, slice_ends)
- index_map: 1D array of indices into shape_world, arranged such that:
* Each regular world's indices are followed by all world -1 (shared) indices
* A final segment contains only world -1 (shared) indices
Only includes shapes that pass the collision flag filter.
- slice_ends: 1D array containing the end index (exclusive) of each world's slice
in the index_map (including the dedicated -1 segment at the end)
"""
# Ensure shape_world is a numpy array (might be a list from builder)
if not isinstance(shape_world, np.ndarray):
shape_world = np.array(shape_world)
# Filter out non-colliding shapes if flags are provided
if shape_flags is not None:
# Ensure shape_flags is also a numpy array
if not isinstance(shape_flags, np.ndarray):
shape_flags = np.array(shape_flags)
if shape_flags.shape[0] != shape_world.shape[0]:
raise ValueError("shape_flags and shape_world must have the same length")
colliding_mask = (shape_flags & ShapeFlags.COLLIDE_SHAPES) != 0
else:
colliding_mask = np.ones(len(shape_world), dtype=bool)
# Apply collision filter to get valid indices
valid_indices = np.where(colliding_mask)[0]
# Work with filtered world IDs
filtered_world_ids = shape_world[valid_indices]
# Validate world IDs: only -1, 0, 1, 2, ... are allowed
invalid_worlds = shape_world[(shape_world < -1)]
if len(invalid_worlds) > 0:
unique_invalid = np.unique(invalid_worlds)
raise ValueError(
f"Invalid world IDs detected: {unique_invalid.tolist()}. "
f"Only world ID -1 (global/shared) and non-negative IDs (0, 1, 2, ...) are supported."
)
# Count world -1 (global entities) in filtered set -> num_shared
# Only world -1 is treated as shared; kernels special-case -1 for deduplication
negative_mask = filtered_world_ids == -1
num_shared = np.sum(negative_mask)
# Get indices of world -1 (shared) entries in the valid set
shared_local_indices = np.where(negative_mask)[0]
# Map back to original shape indices
shared_indices = valid_indices[shared_local_indices]
# Count how many distinct positive (or zero) world IDs are in filtered set -> world_count
# Get unique positive/zero world IDs
positive_mask = filtered_world_ids >= 0
positive_world_ids = filtered_world_ids[positive_mask]
unique_worlds = np.unique(positive_world_ids)
world_count = len(unique_worlds)
# Calculate total size of result
# Each world gets its own indices + all shared indices
# Plus one additional segment at the end with only shared indices
num_positive = np.sum(positive_mask)
total_size = num_positive + (num_shared * world_count) + num_shared
# Allocate output arrays (world_count + 1 to include dedicated -1 segment)
index_map = np.empty(total_size, dtype=np.int32)
slice_ends = np.empty(world_count + 1, dtype=np.int32)
# Build the index map
current_pos = 0
for world_idx, world_id in enumerate(unique_worlds):
# Get indices for this world in the filtered set
world_local_indices = np.where(filtered_world_ids == world_id)[0]
# Map back to original shape indices
world_indices = valid_indices[world_local_indices]
world_shape_count = len(world_indices)
# Copy world-specific indices (using original shape indices)
index_map[current_pos : current_pos + world_shape_count] = world_indices
current_pos += world_shape_count
# Append shared (negative) indices (using original shape indices)
index_map[current_pos : current_pos + num_shared] = shared_indices
current_pos += num_shared
# Store the end position of this slice
slice_ends[world_idx] = current_pos
# Add dedicated segment at the end with only world -1 objects
index_map[current_pos : current_pos + num_shared] = shared_indices
current_pos += num_shared
slice_ends[world_count] = current_pos
return index_map, slice_ends
================================================
FILE: newton/_src/geometry/broad_phase_nxn.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""NxN (all-pairs) broad phase collision detection.
Provides O(N^2) broad phase using AABB overlap tests. Simple and effective
for small scenes (<100 shapes). For larger scenes, use SAP broad phase.
See Also:
:class:`BroadPhaseSAP` in ``broad_phase_sap.py`` for O(N log N) performance.
"""
from __future__ import annotations
import numpy as np
import warp as wp
from ..core.types import Devicelike
from .broad_phase_common import (
check_aabb_overlap,
is_pair_excluded,
precompute_world_map,
test_world_and_group_pair,
write_pair,
)
@wp.kernel(enable_backward=False)
def _nxn_broadphase_precomputed_pairs(
# Input arrays
shape_bounding_box_lower: wp.array[wp.vec3],
shape_bounding_box_upper: wp.array[wp.vec3],
shape_gap: wp.array[float], # Optional per-shape effective gaps (can be empty if AABBs pre-expanded)
nxn_shape_pair: wp.array[wp.vec2i],
# Output arrays
candidate_pair: wp.array[wp.vec2i],
candidate_pair_count: wp.array[int], # Size one array
max_candidate_pair: int,
):
elementid = wp.tid()
pair = nxn_shape_pair[elementid]
shape1 = pair[0]
shape2 = pair[1]
# Check if gaps are provided (empty array means AABBs are pre-expanded)
gap1 = 0.0
gap2 = 0.0
if shape_gap.shape[0] > 0:
gap1 = shape_gap[shape1]
gap2 = shape_gap[shape2]
if check_aabb_overlap(
shape_bounding_box_lower[shape1],
shape_bounding_box_upper[shape1],
gap1,
shape_bounding_box_lower[shape2],
shape_bounding_box_upper[shape2],
gap2,
):
write_pair(
pair,
candidate_pair,
candidate_pair_count,
max_candidate_pair,
)
@wp.func
def _get_lower_triangular_indices(index: int, matrix_size: int) -> tuple[int, int]:
total = (matrix_size * (matrix_size - 1)) >> 1
if index >= total:
# In Warp, we can't throw, so return an invalid pair
return -1, -1
low = int(0)
high = matrix_size - 1
while low < high:
mid = (low + high) >> 1
count = (mid * (2 * matrix_size - mid - 1)) >> 1
if count <= index:
low = mid + 1
else:
high = mid
r = low - 1
f = (r * (2 * matrix_size - r - 1)) >> 1
c = (index - f) + r + 1
return r, c
@wp.func
def _find_world_and_local_id(
tid: int,
world_cumsum_lower_tri: wp.array[int],
):
"""Binary search to find world ID and local ID from thread ID.
Args:
tid: Global thread ID
world_cumsum_lower_tri: Cumulative sum of lower triangular elements per world
Returns:
tuple: (world_id, local_id) - World ID and local index within that world
"""
world_count = world_cumsum_lower_tri.shape[0]
# Find world_id using binary search
# Declare as dynamic variables for loop mutation
low = int(0)
high = int(world_count - 1)
world_id = int(0)
while low <= high:
mid = (low + high) >> 1
if tid < world_cumsum_lower_tri[mid]:
high = mid - 1
world_id = mid
else:
low = mid + 1
# Calculate local index within this world
local_id = tid
if world_id > 0:
local_id = tid - world_cumsum_lower_tri[world_id - 1]
return world_id, local_id
@wp.kernel(enable_backward=False)
def _nxn_broadphase_kernel(
# Input arrays
shape_bounding_box_lower: wp.array[wp.vec3],
shape_bounding_box_upper: wp.array[wp.vec3],
shape_gap: wp.array[float], # Optional per-shape effective gaps (can be empty if AABBs pre-expanded)
collision_group: wp.array[int], # per-shape
shape_world: wp.array[int], # per-shape world indices
world_cumsum_lower_tri: wp.array[int], # Cumulative sum of lower tri elements per world
world_slice_ends: wp.array[int], # End indices of each world slice
world_index_map: wp.array[int], # Index map into source geometry
num_regular_worlds: int, # Number of regular world segments (excluding dedicated -1 segment)
filter_pairs: wp.array[wp.vec2i], # Sorted excluded pairs (empty if none)
num_filter_pairs: int,
# Output arrays
candidate_pair: wp.array[wp.vec2i],
candidate_pair_count: wp.array[int], # Size one array
max_candidate_pair: int,
):
tid = wp.tid()
# Find which world this thread belongs to and the local index within that world
world_id, local_id = _find_world_and_local_id(tid, world_cumsum_lower_tri)
# Get the slice boundaries for this world in the index map
world_slice_start = 0
if world_id > 0:
world_slice_start = world_slice_ends[world_id - 1]
world_slice_end = world_slice_ends[world_id]
# Number of geometries in this world
num_shapes_in_world = world_slice_end - world_slice_start
# Convert local_id to pair indices within the world
local_shape1, local_shape2 = _get_lower_triangular_indices(local_id, num_shapes_in_world)
# Map to actual geometry indices using the world_index_map
shape1_tmp = world_index_map[world_slice_start + local_shape1]
shape2_tmp = world_index_map[world_slice_start + local_shape2]
# Ensure canonical ordering (smaller index first)
# After mapping, the indices might not preserve local_shape1 < local_shape2 ordering
shape1 = wp.min(shape1_tmp, shape2_tmp)
shape2 = wp.max(shape1_tmp, shape2_tmp)
# Get world and collision groups
world1 = shape_world[shape1]
world2 = shape_world[shape2]
collision_group1 = collision_group[shape1]
collision_group2 = collision_group[shape2]
# Skip pairs where both geometries are global (world -1), unless we're in the dedicated -1 segment
# The dedicated -1 segment is the last segment (world_id >= num_regular_worlds)
is_dedicated_minus_one_segment = world_id >= num_regular_worlds
if world1 == -1 and world2 == -1 and not is_dedicated_minus_one_segment:
return
# Check both world and collision groups
if not test_world_and_group_pair(world1, world2, collision_group1, collision_group2):
return
# Check if gaps are provided (empty array means AABBs are pre-expanded)
gap1 = 0.0
gap2 = 0.0
if shape_gap.shape[0] > 0:
gap1 = shape_gap[shape1]
gap2 = shape_gap[shape2]
# Check AABB overlap
if check_aabb_overlap(
shape_bounding_box_lower[shape1],
shape_bounding_box_upper[shape1],
gap1,
shape_bounding_box_lower[shape2],
shape_bounding_box_upper[shape2],
gap2,
):
# Skip explicitly excluded pairs (e.g. shape_collision_filter_pairs)
if num_filter_pairs > 0 and is_pair_excluded(wp.vec2i(shape1, shape2), filter_pairs, num_filter_pairs):
return
write_pair(
wp.vec2i(shape1, shape2),
candidate_pair,
candidate_pair_count,
max_candidate_pair,
)
class BroadPhaseAllPairs:
"""A broad phase collision detection class that performs N x N collision checks between all geometry pairs.
This class performs collision detection between all possible pairs of geometries by checking for
axis-aligned bounding box (AABB) overlaps. It uses a lower triangular matrix approach to avoid
checking each pair twice.
The collision checks take into account per-geometry cutoff distances and collision groups. Two geometries
will only be considered as a candidate pair if:
1. Their AABBs overlap when expanded by their cutoff distances
2. Their collision groups allow interaction (determined by test_group_pair())
The class outputs an array of candidate collision pairs that need more detailed narrow phase collision
checking.
"""
def __init__(
self,
shape_world: wp.array[wp.int32] | np.ndarray,
shape_flags: wp.array[wp.int32] | np.ndarray | None = None,
device: Devicelike | None = None,
) -> None:
"""Initialize the broad phase with world ID information.
Args:
shape_world: Array of world IDs (numpy or warp array).
Positive/zero values represent distinct worlds, negative values represent
shared entities that belong to all worlds.
shape_flags: Optional array of shape flags (numpy or warp array). If provided,
only shapes with the COLLIDE_SHAPES flag will be included in collision checks.
This efficiently filters out visual-only shapes.
device: Device to store the precomputed arrays on. If None, uses CPU for numpy
arrays or the device of the input warp array.
"""
# Convert to numpy if it's a warp array
if isinstance(shape_world, wp.array):
shape_world_np = shape_world.numpy()
if device is None:
device = shape_world.device
else:
shape_world_np = shape_world
if device is None:
device = "cpu"
# Convert shape_flags to numpy if provided
shape_flags_np = None
if shape_flags is not None:
if isinstance(shape_flags, wp.array):
shape_flags_np = shape_flags.numpy()
else:
shape_flags_np = shape_flags
# Precompute the world map (filters out non-colliding shapes if flags provided)
index_map_np, slice_ends_np = precompute_world_map(shape_world_np, shape_flags_np)
# Calculate number of regular worlds (excluding dedicated -1 segment at end)
# Must be derived from filtered slices since precompute_world_map applies flags
# slice_ends_np has length (num_filtered_worlds + 1), where +1 is the dedicated -1 segment
num_regular_worlds = max(0, len(slice_ends_np) - 1)
# Calculate cumulative sum of lower triangular elements per world
# For each world, compute n*(n-1)/2 where n is the number of geometries in that world
world_count = len(slice_ends_np)
world_cumsum_lower_tri_np = np.zeros(world_count, dtype=np.int32)
start_idx = 0
cumsum = 0
for world_idx in range(world_count):
end_idx = slice_ends_np[world_idx]
# Number of geometries in this world (including shared geometries)
num_geoms_in_world = end_idx - start_idx
# Number of lower triangular elements for this world
num_lower_tri = (num_geoms_in_world * (num_geoms_in_world - 1)) // 2
cumsum += num_lower_tri
world_cumsum_lower_tri_np[world_idx] = cumsum
start_idx = end_idx
# Store as warp arrays
self.world_index_map = wp.array(index_map_np, dtype=wp.int32, device=device)
self.world_slice_ends = wp.array(slice_ends_np, dtype=wp.int32, device=device)
self.world_cumsum_lower_tri = wp.array(world_cumsum_lower_tri_np, dtype=wp.int32, device=device)
# Store total number of kernel threads needed (last element of cumsum)
self.num_kernel_threads = int(world_cumsum_lower_tri_np[-1]) if world_count > 0 else 0
# Store number of regular worlds (for distinguishing dedicated -1 segment)
self.num_regular_worlds = int(num_regular_worlds)
def launch(
self,
shape_lower: wp.array[wp.vec3], # Lower bounds of shape bounding boxes
shape_upper: wp.array[wp.vec3], # Upper bounds of shape bounding boxes
shape_gap: wp.array[float] | None, # Optional per-shape effective gaps
shape_collision_group: wp.array[int], # Collision group ID per box
shape_world: wp.array[int], # World index per box
shape_count: int, # Number of active bounding boxes
# Outputs
candidate_pair: wp.array[wp.vec2i], # Array to store overlapping shape pairs
candidate_pair_count: wp.array[int],
device: Devicelike | None = None, # Device to launch on
filter_pairs: wp.array[wp.vec2i] | None = None, # Sorted excluded pairs
num_filter_pairs: int | None = None,
) -> None:
"""Launch the N x N broad phase collision detection.
This method performs collision detection between all possible pairs of geometries by checking for
axis-aligned bounding box (AABB) overlaps. It uses a lower triangular matrix approach to avoid
checking each pair twice.
Args:
shape_lower: Array of lower bounds for each shape's AABB
shape_upper: Array of upper bounds for each shape's AABB
shape_gap: Optional array of per-shape effective gaps. If None or empty array,
assumes AABBs are pre-expanded (gaps = 0). If provided, gaps are added during overlap checks.
shape_collision_group: Array of collision group IDs for each shape. Positive values indicate
groups that only collide with themselves (and with negative groups). Negative values indicate
groups that collide with everything except their negative counterpart. Zero indicates no collisions.
shape_world: Array of world indices for each shape. Index -1 indicates global entities
that collide with all worlds. Indices 0, 1, 2, ... indicate world-specific entities.
shape_count: Number of active bounding boxes to check
candidate_pair: Output array to store overlapping shape pairs
candidate_pair_count: Output array to store number of overlapping pairs found
device: Device to launch on. If None, uses the device of the input arrays.
The method will populate candidate_pair with the indices of shape pairs (i,j) where i < j whose AABBs overlap
(with optional margin expansion), whose collision groups allow interaction, and whose world indices are
compatible (same world or at least one is global). Pairs in filter_pairs (if provided) are excluded.
The number of pairs found will be written to candidate_pair_count[0].
"""
max_candidate_pair = candidate_pair.shape[0]
candidate_pair_count.zero_()
if device is None:
device = shape_lower.device
# If no gaps provided, pass empty array (kernel will use 0.0 gaps)
if shape_gap is None:
shape_gap = wp.empty(0, dtype=wp.float32, device=device)
# Exclusion filter: empty array and 0 when not provided or empty
if filter_pairs is None or filter_pairs.shape[0] == 0:
filter_pairs_arr = wp.empty(0, dtype=wp.vec2i, device=device)
n_filter = 0
else:
filter_pairs_arr = filter_pairs
n_filter = num_filter_pairs if num_filter_pairs is not None else filter_pairs.shape[0]
# Launch with the precomputed number of kernel threads
wp.launch(
_nxn_broadphase_kernel,
dim=self.num_kernel_threads,
inputs=[
shape_lower,
shape_upper,
shape_gap,
shape_collision_group,
shape_world,
self.world_cumsum_lower_tri,
self.world_slice_ends,
self.world_index_map,
self.num_regular_worlds,
filter_pairs_arr,
n_filter,
],
outputs=[candidate_pair, candidate_pair_count, max_candidate_pair],
device=device,
record_tape=False,
)
class BroadPhaseExplicit:
"""A broad phase collision detection class that only checks explicitly provided geometry pairs.
This class performs collision detection only between geometry pairs that are explicitly specified,
rather than checking all possible pairs. This can be more efficient when the set of potential
collision pairs is known ahead of time.
The class checks for axis-aligned bounding box (AABB) overlaps between the specified geometry pairs,
taking into account per-geometry cutoff distances.
"""
def __init__(self) -> None:
pass
def launch(
self,
shape_lower: wp.array[wp.vec3], # Lower bounds of shape bounding boxes
shape_upper: wp.array[wp.vec3], # Upper bounds of shape bounding boxes
shape_gap: wp.array[float] | None, # Optional per-shape effective gaps
shape_pairs: wp.array[wp.vec2i], # Precomputed pairs to check
shape_pair_count: int,
# Outputs
candidate_pair: wp.array[wp.vec2i], # Array to store overlapping shape pairs
candidate_pair_count: wp.array[int],
device: Devicelike | None = None, # Device to launch on
) -> None:
"""Launch the explicit pairs broad phase collision detection.
This method checks for AABB overlaps only between explicitly specified shape pairs,
rather than checking all possible pairs. It populates the candidate_pair array with
indices of shape pairs whose AABBs overlap.
Args:
shape_lower: Array of lower bounds for shape bounding boxes
shape_upper: Array of upper bounds for shape bounding boxes
shape_gap: Optional array of per-shape effective gaps. If None or empty array,
assumes AABBs are pre-expanded (gaps = 0). If provided, gaps are added during overlap checks.
shape_pairs: Array of precomputed shape pairs to check
shape_pair_count: Number of shape pairs to check
candidate_pair: Output array to store overlapping shape pairs
candidate_pair_count: Output array to store number of overlapping pairs found
device: Device to launch on. If None, uses the device of the input arrays.
The method will populate candidate_pair with the indices of shape pairs whose AABBs overlap
(with optional margin expansion), but only checking the explicitly provided pairs.
"""
max_candidate_pair = candidate_pair.shape[0]
candidate_pair_count.zero_()
if device is None:
device = shape_lower.device
# If no gaps provided, pass empty array (kernel will use 0.0 gaps)
if shape_gap is None:
shape_gap = wp.empty(0, dtype=wp.float32, device=device)
wp.launch(
kernel=_nxn_broadphase_precomputed_pairs,
dim=shape_pair_count,
inputs=[
shape_lower,
shape_upper,
shape_gap,
shape_pairs,
candidate_pair,
candidate_pair_count,
max_candidate_pair,
],
device=device,
record_tape=False,
)
================================================
FILE: newton/_src/geometry/broad_phase_sap.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Sweep and Prune (SAP) broad phase collision detection.
Provides O(N log N) broad phase by projecting AABBs onto an axis and using
sorted interval overlap tests. More efficient than NxN for larger scenes.
See Also:
:class:`BroadPhaseAllPairs` in ``broad_phase_nxn.py`` for simpler O(N²) approach.
"""
from __future__ import annotations
from typing import Literal
import numpy as np
import warp as wp
from ..core.types import Devicelike
from .broad_phase_common import (
binary_search,
check_aabb_overlap,
is_pair_excluded,
precompute_world_map,
test_world_and_group_pair,
write_pair,
)
wp.set_module_options({"enable_backward": False})
SAPSortMode = Literal["segmented", "tile"]
def _normalize_sort_mode(mode: str) -> SAPSortMode:
normalized = mode.strip().lower()
if normalized not in ("segmented", "tile"):
raise ValueError(f"Unsupported SAP sort mode: {mode!r}. Expected 'segmented' or 'tile'.")
return normalized
@wp.func
def _sap_project_aabb(
elementid: int,
direction: wp.vec3, # Must be normalized
shape_bounding_box_lower: wp.array[wp.vec3],
shape_bounding_box_upper: wp.array[wp.vec3],
shape_gap: wp.array[float], # Optional per-shape effective gaps (can be empty if AABBs pre-expanded)
) -> wp.vec2:
lower = shape_bounding_box_lower[elementid]
upper = shape_bounding_box_upper[elementid]
# Check if margins are provided (empty array means AABBs are pre-expanded)
gap = 0.0
if shape_gap.shape[0] > 0:
gap = shape_gap[elementid]
half_size = 0.5 * (upper - lower)
half_size = wp.vec3(half_size[0] + gap, half_size[1] + gap, half_size[2] + gap)
radius = wp.dot(direction, half_size)
center = wp.dot(direction, 0.5 * (lower + upper))
return wp.vec2(center - radius, center + radius)
@wp.func
def binary_search_segment(
arr: wp.array[float],
base_idx: int,
value: float,
start: int,
end: int,
) -> int:
"""Binary search in a segment of a 1D array.
Args:
arr: The array to search in
base_idx: Base index offset for this segment
value: Value to search for
start: Start index (relative to base_idx)
end: End index (relative to base_idx)
Returns:
Index (relative to base_idx) where value should be inserted
"""
low = int(start)
high = int(end)
while low < high:
mid = (low + high) // 2
if arr[base_idx + mid] < value:
low = mid + 1
else:
high = mid
return low
def _create_tile_sort_kernel(tile_size: int):
"""Create a tile-based sort kernel for a specific tile size.
This uses Warp's tile operations for efficient shared-memory sorting.
Note: tile_size should match max_geoms_per_world and can be any value.
Args:
tile_size: Size of each tile (should match max_geoms_per_world)
Returns:
A Warp kernel that performs segmented tile-based sorting
"""
@wp.kernel(enable_backward=False)
def tile_sort_kernel(
sap_projection_lower: wp.array[float],
sap_sort_index: wp.array[int],
max_geoms_per_world: int,
):
"""Tile-based segmented sort kernel.
Each thread block processes one world's data using shared memory.
Loads tile_size elements (equal to max_geoms_per_world).
Padding values (1e30) will sort to the end automatically.
"""
world_id = wp.tid()
# Calculate base index for this world
base_idx = world_id * max_geoms_per_world
# Load data into tiles (shared memory)
# tile_size is a closure variable, treated as compile-time constant by Warp
keys = wp.tile_load(sap_projection_lower, shape=(tile_size,), offset=(base_idx,), storage="shared")
values = wp.tile_load(sap_sort_index, shape=(tile_size,), offset=(base_idx,), storage="shared")
# Perform in-place sorting on shared memory
wp.tile_sort(keys, values)
# Store sorted data back to global memory
wp.tile_store(sap_projection_lower, keys, offset=(base_idx,))
wp.tile_store(sap_sort_index, values, offset=(base_idx,))
return tile_sort_kernel
@wp.kernel(enable_backward=False)
def _sap_project_kernel(
direction: wp.vec3, # Must be normalized
shape_bounding_box_lower: wp.array[wp.vec3],
shape_bounding_box_upper: wp.array[wp.vec3],
shape_gap: wp.array[float], # Optional per-shape effective gaps (can be empty if AABBs pre-expanded)
world_index_map: wp.array[int],
world_slice_ends: wp.array[int],
max_shapes_per_world: int,
# Outputs (1D arrays with manual indexing)
sap_projection_lower_out: wp.array[float],
sap_projection_upper_out: wp.array[float],
sap_sort_index_out: wp.array[int],
):
world_id, local_shape_id = wp.tid()
# Calculate 1D index: world_id * max_shapes_per_world + local_shape_id
idx = world_id * max_shapes_per_world + local_shape_id
# Get slice boundaries for this world
world_slice_start = 0
if world_id > 0:
world_slice_start = world_slice_ends[world_id - 1]
world_slice_end = world_slice_ends[world_id]
num_shapes_in_world = world_slice_end - world_slice_start
# Check if this thread is within valid range
if local_shape_id >= num_shapes_in_world:
# Pad with invalid values
sap_projection_lower_out[idx] = 1e30
sap_projection_upper_out[idx] = 1e30
sap_sort_index_out[idx] = -1
return
# Map to actual geometry index
shape_id = world_index_map[world_slice_start + local_shape_id]
# Project AABB onto direction
range = _sap_project_aabb(shape_id, direction, shape_bounding_box_lower, shape_bounding_box_upper, shape_gap)
sap_projection_lower_out[idx] = range[0]
sap_projection_upper_out[idx] = range[1]
sap_sort_index_out[idx] = local_shape_id
@wp.kernel(enable_backward=False)
def _sap_range_kernel(
world_slice_ends: wp.array[int],
max_shapes_per_world: int,
sap_projection_lower_in: wp.array[float],
sap_projection_upper_in: wp.array[float],
sap_sort_index_in: wp.array[int],
sap_range_out: wp.array[int],
):
world_id, local_shape_id = wp.tid()
# Calculate 1D index
idx = world_id * max_shapes_per_world + local_shape_id
# Get number of geometries in this world
world_slice_start = 0
if world_id > 0:
world_slice_start = world_slice_ends[world_id - 1]
world_slice_end = world_slice_ends[world_id]
num_shapes_in_world = world_slice_end - world_slice_start
if local_shape_id >= num_shapes_in_world:
sap_range_out[idx] = 0
return
# Current bounding shape (after sort, this is the original local geometry index)
# Note: sap_sort_index_in[idx] contains the original local geometry index of the
# geometry that's now at position local_shape_id in the sorted array
sort_idx = sap_sort_index_in[idx]
# Invalid shape (padding)
if sort_idx < 0:
sap_range_out[idx] = 0
return
# Get upper bound for this shape
# sort_idx is the original local geometry index, so we use it to index into
# sap_projection_upper_in (which is NOT sorted, only sap_projection_lower_in is sorted)
upper_idx = world_id * max_shapes_per_world + sort_idx
upper = sap_projection_upper_in[upper_idx]
# Binary search for the limit in this world's segment
# We need to search in the range [local_shape_id + 1, num_shapes_in_world)
world_base_idx = world_id * max_shapes_per_world
limit = binary_search_segment(
sap_projection_lower_in, world_base_idx, upper, local_shape_id + 1, num_shapes_in_world
)
limit = wp.min(num_shapes_in_world, limit)
# Range of shapes for the sweep and prune process
sap_range_out[idx] = limit - local_shape_id - 1
@wp.func
def _process_single_sap_pair(
pair: wp.vec2i,
shape_bounding_box_lower: wp.array[wp.vec3],
shape_bounding_box_upper: wp.array[wp.vec3],
shape_gap: wp.array[float], # Optional per-shape effective gaps (can be empty if AABBs pre-expanded)
candidate_pair: wp.array[wp.vec2i],
candidate_pair_count: wp.array[int], # Size one array
max_candidate_pair: int,
filter_pairs: wp.array[wp.vec2i], # Sorted excluded pairs (empty if none)
num_filter_pairs: int,
):
shape1 = pair[0]
shape2 = pair[1]
# Skip explicitly excluded pairs (e.g. shape_collision_filter_pairs)
if num_filter_pairs > 0 and is_pair_excluded(pair, filter_pairs, num_filter_pairs):
return
# Check if margins are provided (empty array means AABBs are pre-expanded)
gap1 = 0.0
gap2 = 0.0
if shape_gap.shape[0] > 0:
gap1 = shape_gap[shape1]
gap2 = shape_gap[shape2]
if check_aabb_overlap(
shape_bounding_box_lower[shape1],
shape_bounding_box_upper[shape1],
gap1,
shape_bounding_box_lower[shape2],
shape_bounding_box_upper[shape2],
gap2,
):
write_pair(
pair,
candidate_pair,
candidate_pair_count,
max_candidate_pair,
)
@wp.kernel(enable_backward=False)
def _sap_broadphase_kernel(
# Input arrays
shape_bounding_box_lower: wp.array[wp.vec3],
shape_bounding_box_upper: wp.array[wp.vec3],
shape_gap: wp.array[float], # Optional per-shape effective gaps (can be empty if AABBs pre-expanded)
collision_group: wp.array[int],
shape_world: wp.array[int], # World indices
world_index_map: wp.array[int],
world_slice_ends: wp.array[int],
sap_sort_index_in: wp.array[int], # 1D array with manual indexing
sap_cumulative_sum_in: wp.array[int], # Flattened [world_count * max_shapes]
world_count: int,
max_shapes_per_world: int,
nsweep_in: int,
num_regular_worlds: int, # Number of regular world segments (excluding dedicated -1 segment)
filter_pairs: wp.array[wp.vec2i], # Sorted excluded pairs (empty if none)
num_filter_pairs: int,
# Output arrays
candidate_pair: wp.array[wp.vec2i],
candidate_pair_count: wp.array[int], # Size one array
max_candidate_pair: int,
):
tid = wp.tid()
total_work_packages = sap_cumulative_sum_in[world_count * max_shapes_per_world - 1]
workid = tid
while workid < total_work_packages:
# Binary search to find which (world, local_shape) this work package belongs to
flat_id = binary_search(sap_cumulative_sum_in, workid, 0, world_count * max_shapes_per_world)
# Calculate j from flat_id and workid
j = flat_id + workid + 1
if flat_id > 0:
j -= sap_cumulative_sum_in[flat_id - 1]
# Convert flat_id to world and local indices
world_id = flat_id // max_shapes_per_world
i = flat_id % max_shapes_per_world
j = j % max_shapes_per_world
# Get slice boundaries for this world
world_slice_start = 0
if world_id > 0:
world_slice_start = world_slice_ends[world_id - 1]
world_slice_end = world_slice_ends[world_id]
num_shapes_in_world = world_slice_end - world_slice_start
# Check validity: ensure indices are within bounds
if i >= num_shapes_in_world or j >= num_shapes_in_world:
workid += nsweep_in
continue
# Skip self-pairs (i == j) and invalid pairs (i > j) - pairs must have distinct geometries with i < j
if i >= j:
workid += nsweep_in
continue
# Get sorted local indices using manual indexing
idx_i = world_id * max_shapes_per_world + i
idx_j = world_id * max_shapes_per_world + j
local_shape1 = sap_sort_index_in[idx_i]
local_shape2 = sap_sort_index_in[idx_j]
# Check for invalid indices (padding)
if local_shape1 < 0 or local_shape2 < 0:
workid += nsweep_in
continue
# Map to actual geometry indices
shape1_tmp = world_index_map[world_slice_start + local_shape1]
shape2_tmp = world_index_map[world_slice_start + local_shape2]
# Skip if mapped to the same geometry (shouldn't happen, but defensive check)
if shape1_tmp == shape2_tmp:
workid += nsweep_in
continue
# Ensure canonical ordering
shape1 = wp.min(shape1_tmp, shape2_tmp)
shape2 = wp.max(shape1_tmp, shape2_tmp)
# Get collision and world groups
col_group1 = collision_group[shape1]
col_group2 = collision_group[shape2]
world1 = shape_world[shape1]
world2 = shape_world[shape2]
# Skip pairs where both geometries are global (world -1), unless we're in the dedicated -1 segment
# The dedicated -1 segment is the last segment (world_id >= num_regular_worlds)
is_dedicated_minus_one_segment = world_id >= num_regular_worlds
if world1 == -1 and world2 == -1 and not is_dedicated_minus_one_segment:
workid += nsweep_in
continue
# Check both world and collision groups
if test_world_and_group_pair(world1, world2, col_group1, col_group2):
_process_single_sap_pair(
wp.vec2i(shape1, shape2),
shape_bounding_box_lower,
shape_bounding_box_upper,
shape_gap,
candidate_pair,
candidate_pair_count,
max_candidate_pair,
filter_pairs,
num_filter_pairs,
)
workid += nsweep_in
class BroadPhaseSAP:
"""Sweep and Prune (SAP) broad phase collision detection.
This class implements the sweep and prune algorithm for broad phase collision detection.
It efficiently finds potentially colliding pairs of objects by sorting their bounding box
projections along a fixed axis and checking for overlaps.
"""
def __init__(
self,
shape_world: wp.array[wp.int32] | np.ndarray,
shape_flags: wp.array[wp.int32] | np.ndarray | None = None,
sweep_thread_count_multiplier: int = 5,
sort_type: Literal["segmented", "tile"] = "segmented",
tile_block_dim: int | None = None,
device: Devicelike | None = None,
) -> None:
"""Initialize arrays for sweep and prune broad phase collision detection.
Args:
shape_world: Array of world indices for each shape (numpy or warp array).
Represents which world each shape belongs to for world-aware collision detection.
shape_flags: Optional array of shape flags (numpy or warp array). If provided,
only shapes with the COLLIDE_SHAPES flag will be included in collision checks.
This efficiently filters out visual-only shapes.
sweep_thread_count_multiplier: Multiplier for number of threads used in sweep phase
sort_type: SAP sort mode. Use ``"segmented"`` (default) for
``wp.utils.segmented_sort_pairs`` or ``"tile"`` for
tile-based sorting via ``wp.tile_sort``.
tile_block_dim: Block dimension for tile-based sorting (optional, auto-calculated if None).
If None, will be set to next power of 2 >= ``max_shapes_per_world``, capped at 512.
Minimum value is 32 (required by wp.tile_sort). If provided, will be clamped to [32, 1024].
device: Device to store the precomputed arrays on. If None, uses CPU for numpy
arrays or the device of the input warp array.
"""
self.sweep_thread_count_multiplier = sweep_thread_count_multiplier
self.sort_type = _normalize_sort_mode(sort_type)
self.tile_block_dim_override = tile_block_dim # Store user override if provided
# Convert to numpy if it's a warp array
if isinstance(shape_world, wp.array):
shape_world_np = shape_world.numpy()
if device is None:
device = shape_world.device
else:
shape_world_np = shape_world
if device is None:
device = "cpu"
# Convert shape_flags to numpy if provided
shape_flags_np = None
if shape_flags is not None:
if isinstance(shape_flags, wp.array):
shape_flags_np = shape_flags.numpy()
else:
shape_flags_np = shape_flags
# Precompute the world map (filters out non-colliding shapes if flags provided)
index_map_np, slice_ends_np = precompute_world_map(shape_world_np, shape_flags_np)
# Calculate number of regular worlds (excluding dedicated -1 segment at end)
# Must be derived from filtered slices since precompute_world_map applies flags
# slice_ends_np has length (num_filtered_worlds + 1), where +1 is the dedicated -1 segment
num_regular_worlds = max(0, len(slice_ends_np) - 1)
# Store as warp arrays
self.world_index_map = wp.array(index_map_np, dtype=wp.int32, device=device)
self.world_slice_ends = wp.array(slice_ends_np, dtype=wp.int32, device=device)
# Calculate world information
self.world_count = len(slice_ends_np)
self.num_regular_worlds = int(num_regular_worlds)
self.max_shapes_per_world = 0
start_idx = 0
for end_idx in slice_ends_np:
num_shapes = end_idx - start_idx
self.max_shapes_per_world = max(self.max_shapes_per_world, num_shapes)
start_idx = end_idx
# Create tile sort kernel if using tile-based sorting
self.tile_sort_kernel = None
if self.sort_type == "tile":
# Calculate block_dim: next power of 2 >= max_shapes_per_world, capped at 512
if self.tile_block_dim_override is not None:
self.tile_block_dim = max(32, min(self.tile_block_dim_override, 1024))
else:
block_dim = 1
while block_dim < self.max_shapes_per_world:
block_dim *= 2
self.tile_block_dim = max(32, min(block_dim, 512))
# tile_size should match max_shapes_per_world (actual data size)
# tile_block_dim is for thread block configuration and can be larger
self.tile_size = int(self.max_shapes_per_world)
self.tile_sort_kernel = _create_tile_sort_kernel(self.tile_size)
# Allocate 1D arrays for per-world SAP data
# Note: projection_lower and sort_index need 2x space for segmented sort scratch memory
total_elements = int(self.world_count * self.max_shapes_per_world)
self.sap_projection_lower = wp.zeros(2 * total_elements, dtype=wp.float32, device=device)
self.sap_projection_upper = wp.zeros(total_elements, dtype=wp.float32, device=device)
self.sap_sort_index = wp.zeros(2 * total_elements, dtype=wp.int32, device=device)
self.sap_range = wp.zeros(total_elements, dtype=wp.int32, device=device)
self.sap_cumulative_sum = wp.zeros(total_elements, dtype=wp.int32, device=device)
# Segment indices for segmented sort (needed for graph capture)
# [0, max_shapes_per_world, 2*max_shapes_per_world, ..., world_count*max_shapes_per_world]
segment_indices_np = np.array(
[i * self.max_shapes_per_world for i in range(self.world_count + 1)], dtype=np.int32
)
self.segment_indices = wp.array(segment_indices_np, dtype=wp.int32, device=device)
def launch(
self,
shape_lower: wp.array[wp.vec3], # Lower bounds of shape bounding boxes
shape_upper: wp.array[wp.vec3], # Upper bounds of shape bounding boxes
shape_gap: wp.array[float] | None, # Optional per-shape effective gaps
shape_collision_group: wp.array[int], # Collision group ID per box
shape_world: wp.array[int], # World index per box
shape_count: int, # Number of active bounding boxes
# Outputs
candidate_pair: wp.array[wp.vec2i], # Array to store overlapping shape pairs
candidate_pair_count: wp.array[int],
device: Devicelike | None = None, # Device to launch on
filter_pairs: wp.array[wp.vec2i] | None = None, # Sorted excluded pairs
num_filter_pairs: int | None = None,
) -> None:
"""Launch the sweep and prune broad phase collision detection with per-world segmented sort.
This method performs collision detection between geometries using a sweep and prune algorithm along a fixed axis.
It processes each world independently using segmented sort, which is more efficient than global sorting
when geometries are organized into separate worlds.
Args:
shape_lower: Array of lower bounds for each shape's AABB
shape_upper: Array of upper bounds for each shape's AABB
shape_gap: Optional array of per-shape effective gaps. If None or empty array,
assumes AABBs are pre-expanded (gaps = 0). If provided, gaps are added during overlap checks.
shape_collision_group: Array of collision group IDs for each shape. Positive values indicate
groups that only collide with themselves (and with negative groups). Negative values indicate
groups that collide with everything except their negative counterpart. Zero indicates no collisions.
shape_world: Array of world indices for each shape. Index -1 indicates global entities
that collide with all worlds. Indices 0, 1, 2, ... indicate world-specific entities.
shape_count: Number of active bounding boxes to check (not used in world-based approach)
candidate_pair: Output array to store overlapping shape pairs
candidate_pair_count: Output array to store number of overlapping pairs found
device: Device to launch on. If None, uses the device of the input arrays.
The method will populate candidate_pair with the indices of shape pairs whose AABBs overlap
(with optional margin expansion), whose collision groups allow interaction, and whose worlds are
compatible (same world or at least one is global). Pairs in filter_pairs (if provided) are excluded.
The number of pairs found will be written to candidate_pair_count[0].
"""
# TODO: Choose an optimal direction
# random fixed direction
direction = wp.vec3(0.5935, 0.7790, 0.1235)
direction = wp.normalize(direction)
max_candidate_pair = candidate_pair.shape[0]
candidate_pair_count.zero_()
if device is None:
device = shape_lower.device
# If no gaps provided, pass empty array (kernel will use 0.0 gaps)
if shape_gap is None:
shape_gap = wp.empty(0, dtype=wp.float32, device=device)
# Exclusion filter: empty array and 0 when not provided or empty
if filter_pairs is None or filter_pairs.shape[0] == 0:
filter_pairs_arr = wp.empty(0, dtype=wp.vec2i, device=device)
n_filter = 0
else:
filter_pairs_arr = filter_pairs
n_filter = num_filter_pairs if num_filter_pairs is not None else filter_pairs.shape[0]
# Project AABBs onto the sweep axis for each world
wp.launch(
kernel=_sap_project_kernel,
dim=(self.world_count, self.max_shapes_per_world),
inputs=[
direction,
shape_lower,
shape_upper,
shape_gap,
self.world_index_map,
self.world_slice_ends,
self.max_shapes_per_world,
self.sap_projection_lower,
self.sap_projection_upper,
self.sap_sort_index,
],
device=device,
record_tape=False,
)
# Perform segmented sort - each world is sorted independently
# Two strategies: tile-based (faster for certain sizes) or segmented (more flexible)
if self.sort_type == "tile" and self.tile_sort_kernel is not None:
# Use tile-based sort with shared memory
wp.launch_tiled(
kernel=self.tile_sort_kernel,
dim=self.world_count,
inputs=[
self.sap_projection_lower,
self.sap_sort_index,
self.max_shapes_per_world,
],
block_dim=self.tile_block_dim,
device=device,
record_tape=False,
)
else:
# Use segmented sort (default)
# The count is the number of actual elements to sort (not including scratch space)
wp.utils.segmented_sort_pairs(
keys=self.sap_projection_lower,
values=self.sap_sort_index,
count=self.world_count * self.max_shapes_per_world,
segment_start_indices=self.segment_indices,
)
# Compute range of overlapping geometries for each geometry in each world
wp.launch(
kernel=_sap_range_kernel,
dim=(self.world_count, self.max_shapes_per_world),
inputs=[
self.world_slice_ends,
self.max_shapes_per_world,
self.sap_projection_lower,
self.sap_projection_upper,
self.sap_sort_index,
self.sap_range,
],
device=device,
record_tape=False,
)
# Compute cumulative sum of ranges
wp.utils.array_scan(self.sap_range, self.sap_cumulative_sum, True)
# Estimate number of sweep threads
total_elements = self.world_count * self.max_shapes_per_world
nsweep_in = int(self.sweep_thread_count_multiplier * total_elements)
# Perform the sweep and generate candidate pairs
wp.launch(
kernel=_sap_broadphase_kernel,
dim=nsweep_in,
inputs=[
shape_lower,
shape_upper,
shape_gap,
shape_collision_group,
shape_world,
self.world_index_map,
self.world_slice_ends,
self.sap_sort_index,
self.sap_cumulative_sum,
self.world_count,
self.max_shapes_per_world,
nsweep_in,
self.num_regular_worlds,
filter_pairs_arr,
n_filter,
],
outputs=[
candidate_pair,
candidate_pair_count,
max_candidate_pair,
],
device=device,
record_tape=False,
)
================================================
FILE: newton/_src/geometry/collision_convex.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
High-level collision detection functions for convex shapes.
Fused MPR + GJK approach with shared support functions and frame transform:
1. MPR with small inflate — exact normals for overlapping and near-touching shapes.
Exits early for separated shapes (just a few support queries).
2. Only if MPR finds no overlap: GJK for accurate speculative contacts.
Both algorithms share support mapping code and the relative-frame coordinate
transform, reducing compiled code size and register pressure.
"""
from typing import Any
import warp as wp
from .contact_data import ContactData
from .mpr import create_solve_mpr, create_support_map_function
from .multicontact import create_build_manifold
from .simplex_solver import create_solve_closest_distance
def create_solve_convex_multi_contact(support_func: Any, writer_func: Any, post_process_contact: Any):
"""Factory: fused MPR+GJK multi-contact solver with shared support code."""
# Create support functions ONCE — shared between MPR and GJK.
support_funcs = create_support_map_function(support_func)
solve_mpr = create_solve_mpr(support_func, _support_funcs=support_funcs)
solve_gjk = create_solve_closest_distance(support_func, _support_funcs=support_funcs)
@wp.func
def solve_convex_multi_contact(
geom_a: Any,
geom_b: Any,
orientation_a: wp.quat,
orientation_b: wp.quat,
position_a: wp.vec3,
position_b: wp.vec3,
combined_margin: float,
data_provider: Any,
contact_threshold: float,
skip_multi_contact: bool,
writer_data: Any,
contact_template: ContactData,
) -> int:
# Shared relative-frame transform (computed once for both algorithms).
relative_orientation_b = wp.quat_inverse(orientation_a) * orientation_b
relative_position_b = wp.quat_rotate_inv(orientation_a, position_b - position_a)
# Enlarge a little bit to avoid contact flickering when the signed distance is close to 0.
# This ensures MPR consistently detects resting contacts, preventing alternation between
# MPR and GJK across frames for near-touching shapes.
enlarge = 1e-4
# MPR with small inflate for overlapping shapes.
# Exits early (few support queries) when shapes are separated.
collision, point_a, point_b, normal, penetration = wp.static(solve_mpr.core)(
geom_a,
geom_b,
relative_orientation_b,
relative_position_b,
combined_margin + enlarge,
data_provider,
)
if collision:
signed_distance = -penetration + enlarge
else:
# GJK fallback for separated shapes -- proven accurate normals/distances.
_separated, point_a, point_b, normal, signed_distance = wp.static(solve_gjk.core)(
geom_a,
geom_b,
relative_orientation_b,
relative_position_b,
combined_margin,
data_provider,
)
if skip_multi_contact or signed_distance > contact_threshold:
# Transform to world space only for the single-contact early-out.
point = 0.5 * (point_a + point_b)
point = wp.quat_rotate(orientation_a, point) + position_a
normal_ws = wp.quat_rotate(orientation_a, normal)
contact_data = contact_template
contact_data.contact_point_center = point
contact_data.contact_normal_a_to_b = normal_ws
contact_data.contact_distance = signed_distance
contact_data = post_process_contact(
contact_data, geom_a, position_a, orientation_a, geom_b, position_b, orientation_b
)
writer_func(contact_data, writer_data, -1)
return 1
# Generate multi-contact manifold -- pass A-local-frame data directly
# to avoid redundant world-space round-trip.
count = wp.static(
create_build_manifold(support_func, writer_func, post_process_contact, _support_funcs=support_funcs)
)(
geom_a,
geom_b,
orientation_a,
position_a,
relative_orientation_b,
relative_position_b,
point_a,
point_b,
normal,
data_provider,
writer_data,
contact_template,
)
return count
return solve_convex_multi_contact
def create_solve_convex_single_contact(support_func: Any, writer_func: Any, post_process_contact: Any):
"""Factory: fused MPR+GJK single-contact solver with shared support code."""
# Create support functions ONCE — shared between MPR and GJK.
support_funcs = create_support_map_function(support_func)
solve_mpr = create_solve_mpr(support_func, _support_funcs=support_funcs)
solve_gjk = create_solve_closest_distance(support_func, _support_funcs=support_funcs)
@wp.func
def solve_convex_single_contact(
geom_a: Any,
geom_b: Any,
orientation_a: wp.quat,
orientation_b: wp.quat,
position_a: wp.vec3,
position_b: wp.vec3,
combined_margin: float,
data_provider: Any,
contact_threshold: float,
writer_data: Any,
contact_template: ContactData,
) -> int:
# Shared relative-frame transform (computed once for both algorithms).
relative_orientation_b = wp.quat_inverse(orientation_a) * orientation_b
relative_position_b = wp.quat_rotate_inv(orientation_a, position_b - position_a)
# Enlarge a little bit to avoid contact flickering when the signed distance is close to 0.
# This ensures MPR consistently detects resting contacts, preventing alternation between
# MPR and GJK across frames for near-touching shapes.
enlarge = 1e-4
# MPR with small inflate for overlapping shapes.
collision, point_a, point_b, normal, penetration = wp.static(solve_mpr.core)(
geom_a,
geom_b,
relative_orientation_b,
relative_position_b,
combined_margin + enlarge,
data_provider,
)
if collision:
signed_distance = -penetration + enlarge
else:
# GJK fallback for separated shapes.
_separated, point_a, point_b, normal, signed_distance = wp.static(solve_gjk.core)(
geom_a,
geom_b,
relative_orientation_b,
relative_position_b,
combined_margin,
data_provider,
)
# Transform results back to world space (once).
point = 0.5 * (point_a + point_b)
point = wp.quat_rotate(orientation_a, point) + position_a
normal = wp.quat_rotate(orientation_a, normal)
contact_data = contact_template
contact_data.contact_point_center = point
contact_data.contact_normal_a_to_b = normal
contact_data.contact_distance = signed_distance
contact_data = post_process_contact(
contact_data, geom_a, position_a, orientation_a, geom_b, position_b, orientation_b
)
writer_func(contact_data, writer_data, -1)
return 1
return solve_convex_single_contact
================================================
FILE: newton/_src/geometry/collision_core.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from typing import Any
import warp as wp
from ..core.types import vec5
from .broad_phase_common import binary_search
from .collision_convex import create_solve_convex_multi_contact, create_solve_convex_single_contact
from .contact_data import ContactData
from .support_function import (
GenericShapeData,
GeoTypeEx,
SupportMapDataProvider,
pack_mesh_ptr,
support_map,
unpack_mesh_ptr,
)
from .types import GeoType
# Configuration flag for multi-contact generation
ENABLE_MULTI_CONTACT = True
# Configuration flag for tiled BVH queries (experimental)
ENABLE_TILE_BVH_QUERY = True
# Type definitions for multi-contact manifolds
_mat53f = wp.types.matrix((5, 3), wp.float32)
# Type definitions for single-contact mode
_vec1 = wp.types.vector(1, wp.float32)
@wp.func
def is_discrete_shape(shape_type: int) -> bool:
"""A discrete shape can be represented with a finite amount of flat polygon faces."""
return (
shape_type == GeoType.BOX
or shape_type == GeoType.CONVEX_MESH
or shape_type == GeoTypeEx.TRIANGLE
or shape_type == GeoType.PLANE
)
@wp.func
def project_point_onto_plane(point: wp.vec3, plane_point: wp.vec3, plane_normal: wp.vec3) -> wp.vec3:
"""
Project a point onto a plane defined by a point and normal.
Args:
point: The point to project
plane_point: A point on the plane
plane_normal: Normal vector of the plane (should be normalized)
Returns:
The projected point on the plane
"""
to_point = point - plane_point
distance_to_plane = wp.dot(to_point, plane_normal)
projected_point = point - plane_normal * distance_to_plane
return projected_point
@wp.func
def compute_plane_normal_from_contacts(
points: _mat53f,
normal: wp.vec3,
signed_distances: vec5,
count: int,
) -> wp.vec3:
"""
Compute plane normal from reconstructed plane points.
Reconstructs the plane points from contact data and computes the plane normal
using fan triangulation to find the largest area triangle for numerical stability.
Args:
points: Contact points matrix (5x3)
normal: Initial contact normal (used for reconstruction)
signed_distances: Signed distances vector (5 elements)
count: Number of contact points
Returns:
Normalized plane normal from the contact points
"""
if count < 3:
# Not enough points to form a triangle, return original normal
return normal
# Reconstruct plane points from contact data
# Use first point as anchor for fan triangulation
# Contact points are at midpoint, move to discrete surface (plane)
p0 = points[0] + normal * (signed_distances[0] * 0.5)
# Find the triangle with the largest area for numerical stability
# This avoids issues with nearly collinear points
best_normal = wp.vec3(0.0, 0.0, 0.0)
max_area_sq = float(0.0)
for i in range(1, count - 1):
# Reconstruct plane points for this triangle
pi = points[i] + normal * (signed_distances[i] * 0.5)
pi_next = points[i + 1] + normal * (signed_distances[i + 1] * 0.5)
# Compute cross product for triangle (p0, pi, pi_next)
edge1 = pi - p0
edge2 = pi_next - p0
cross = wp.cross(edge1, edge2)
area_sq = wp.dot(cross, cross)
if area_sq > max_area_sq:
max_area_sq = area_sq
best_normal = cross
# Normalize, avoid zero
len_n = wp.sqrt(wp.max(1.0e-12, max_area_sq))
plane_normal = best_normal / len_n
# Ensure normal points in same direction as original normal
if wp.dot(plane_normal, normal) < 0.0:
plane_normal = -plane_normal
return plane_normal
@wp.func
def no_post_process_contact(
contact_data: ContactData,
shape_a: GenericShapeData,
pos_a_adjusted: wp.vec3,
rot_a: wp.quat,
shape_b: GenericShapeData,
pos_b_adjusted: wp.vec3,
rot_b: wp.quat,
) -> ContactData:
return contact_data
@wp.func
def post_process_minkowski_only(
contact_data: ContactData,
shape_a: GenericShapeData,
pos_a_adjusted: wp.vec3,
rot_a: wp.quat,
shape_b: GenericShapeData,
pos_b_adjusted: wp.vec3,
rot_b: wp.quat,
) -> ContactData:
"""Lean post-processor: Minkowski sphere/capsule adjustment only, no axial rolling."""
type_a = shape_a.shape_type
type_b = shape_b.shape_type
normal = contact_data.contact_normal_a_to_b
radius_eff_a = contact_data.radius_eff_a
radius_eff_b = contact_data.radius_eff_b
if type_a == GeoType.SPHERE or type_a == GeoType.CAPSULE:
contact_data.contact_point_center = contact_data.contact_point_center + normal * (radius_eff_a * 0.5)
contact_data.contact_distance = contact_data.contact_distance - radius_eff_a
if type_b == GeoType.SPHERE or type_b == GeoType.CAPSULE:
contact_data.contact_point_center = contact_data.contact_point_center - normal * (radius_eff_b * 0.5)
contact_data.contact_distance = contact_data.contact_distance - radius_eff_b
return contact_data
@wp.func
def post_process_axial_on_discrete_contact(
contact_data: ContactData,
shape_a: GenericShapeData,
pos_a_adjusted: wp.vec3,
rot_a: wp.quat,
shape_b: GenericShapeData,
pos_b_adjusted: wp.vec3,
rot_b: wp.quat,
) -> ContactData:
"""
Post-process a single contact for minkowski objects and axial shape rolling.
This function handles:
1. Minkowski objects (spheres/capsules): Adjusts contact point and distance for rounded geometry
2. Axial shapes on discrete surfaces: Projects contact point for rolling stabilization
Args:
contact_data: Contact data to post-process
shape_a: Shape data for shape A
pos_a_adjusted: Position of shape A
rot_a: Orientation of shape A
shape_b: Shape data for shape B
pos_b_adjusted: Position of shape B
rot_b: Orientation of shape B
Returns:
Post-processed contact data
"""
type_a = shape_a.shape_type
type_b = shape_b.shape_type
normal = contact_data.contact_normal_a_to_b
radius_eff_a = contact_data.radius_eff_a
radius_eff_b = contact_data.radius_eff_b
# 1. Minkowski object processing for spheres and capsules
# Adjust contact point and distance for sphere/capsule A
if type_a == GeoType.SPHERE or type_a == GeoType.CAPSULE:
contact_data.contact_point_center = contact_data.contact_point_center + normal * (radius_eff_a * 0.5)
contact_data.contact_distance = contact_data.contact_distance - radius_eff_a
# Adjust contact point and distance for sphere/capsule B
if type_b == GeoType.SPHERE or type_b == GeoType.CAPSULE:
contact_data.contact_point_center = contact_data.contact_point_center - normal * (radius_eff_b * 0.5)
contact_data.contact_distance = contact_data.contact_distance - radius_eff_b
# 2. Axial shape rolling stabilization (cylinders and cones on discrete surfaces)
is_discrete_a = is_discrete_shape(type_a)
is_discrete_b = is_discrete_shape(type_b)
is_axial_a = type_a == GeoType.CYLINDER or type_a == GeoType.CONE
is_axial_b = type_b == GeoType.CYLINDER or type_b == GeoType.CONE
# Only process if we have discrete vs axial configuration
if (is_discrete_a and is_axial_b) or (is_discrete_b and is_axial_a):
# Extract the axial shape parameters
if is_discrete_a and is_axial_b:
shape_axis = wp.quat_rotate(rot_b, wp.vec3(0.0, 0.0, 1.0))
shape_radius = shape_b.scale[0]
shape_half_height = shape_b.scale[1]
is_cone = type_b == GeoType.CONE
shape_pos = pos_b_adjusted
axial_normal = normal
else: # is_discrete_b and is_axial_a
shape_axis = wp.quat_rotate(rot_a, wp.vec3(0.0, 0.0, 1.0))
shape_radius = shape_a.scale[0]
shape_half_height = shape_a.scale[1]
is_cone = type_a == GeoType.CONE
shape_pos = pos_a_adjusted
axial_normal = -normal # Flip normal for shape A
# Check if shape is in rolling configuration
axis_normal_dot = wp.abs(wp.dot(shape_axis, axial_normal))
# Compute threshold based on shape type
is_rolling = False
if is_cone:
# For a cone rolling on its base, the axis makes an angle with the normal
cone_half_angle = wp.atan2(shape_radius, 2.0 * shape_half_height)
tolerance_angle = wp.static(2.0 * wp.pi / 180.0) # 2 degrees
lower_threshold = wp.sin(cone_half_angle - tolerance_angle)
upper_threshold = wp.sin(cone_half_angle + tolerance_angle)
if axis_normal_dot >= lower_threshold and axis_normal_dot <= upper_threshold:
is_rolling = True
else:
# For cylinder: axis should be perpendicular to normal (dot product ≈ 0)
perpendicular_threshold = wp.static(wp.sin(2.0 * wp.pi / 180.0))
if axis_normal_dot <= perpendicular_threshold:
is_rolling = True
# If rolling, project contact point onto the projection plane
if is_rolling:
projection_plane_normal = wp.normalize(wp.cross(shape_axis, axial_normal))
point_on_projection_plane = shape_pos
# Project the contact point
projected_point = project_point_onto_plane(
contact_data.contact_point_center, point_on_projection_plane, projection_plane_normal
)
# Update the contact with the projected point
contact_data.contact_point_center = projected_point
return contact_data
def create_compute_gjk_mpr_contacts(
writer_func: Any,
post_process_contact: Any = post_process_axial_on_discrete_contact,
support_func: Any = None,
):
"""
Factory function to create a compute_gjk_mpr_contacts function with a specific writer function.
Args:
writer_func: Function to write contact data (signature: (ContactData, writer_data) -> None)
post_process_contact: Function to post-process contact data
support_func: Support mapping function (defaults to support_map)
Returns:
A compute_gjk_mpr_contacts function with the writer function baked in
"""
if support_func is None:
support_func = support_map
@wp.func
def compute_gjk_mpr_contacts(
shape_a_data: GenericShapeData,
shape_b_data: GenericShapeData,
rot_a: wp.quat,
rot_b: wp.quat,
pos_a_adjusted: wp.vec3,
pos_b_adjusted: wp.vec3,
rigid_gap: float,
shape_a: int,
shape_b: int,
margin_a: float,
margin_b: float,
writer_data: Any,
):
"""
Compute contacts between two shapes using GJK/MPR algorithm and write them.
Args:
shape_a_data: Generic shape data for shape A (contains shape_type)
shape_b_data: Generic shape data for shape B (contains shape_type)
rot_a: Orientation of shape A
rot_b: Orientation of shape B
pos_a_adjusted: Adjusted position of shape A
pos_b_adjusted: Adjusted position of shape B
rigid_gap: Contact gap for rigid bodies
shape_a: Index of shape A
shape_b: Index of shape B
margin_a: Per-shape margin offset for shape A (signed distance padding)
margin_b: Per-shape margin offset for shape B (signed distance padding)
writer_data: Data structure for contact writer
"""
data_provider = SupportMapDataProvider()
radius_eff_a = float(0.0)
radius_eff_b = float(0.0)
small_radius = 0.0001
# Get shape types from shape data
type_a = shape_a_data.shape_type
type_b = shape_b_data.shape_type
# Special treatment for minkowski objects
if type_a == GeoType.SPHERE or type_a == GeoType.CAPSULE:
radius_eff_a = shape_a_data.scale[0]
shape_a_data.scale[0] = small_radius
if type_b == GeoType.SPHERE or type_b == GeoType.CAPSULE:
radius_eff_b = shape_b_data.scale[0]
shape_b_data.scale[0] = small_radius
# Pre-pack ContactData template with static information
contact_template = ContactData()
contact_template.radius_eff_a = radius_eff_a
contact_template.radius_eff_b = radius_eff_b
contact_template.margin_a = margin_a
contact_template.margin_b = margin_b
contact_template.shape_a = shape_a
contact_template.shape_b = shape_b
contact_template.gap_sum = rigid_gap
if wp.static(ENABLE_MULTI_CONTACT):
wp.static(create_solve_convex_multi_contact(support_func, writer_func, post_process_contact))(
shape_a_data,
shape_b_data,
rot_a,
rot_b,
pos_a_adjusted,
pos_b_adjusted,
0.0, # combined_margin
data_provider,
rigid_gap + radius_eff_a + radius_eff_b + margin_a + margin_b,
type_a == GeoType.SPHERE
or type_b == GeoType.SPHERE
or type_a == GeoType.ELLIPSOID
or type_b == GeoType.ELLIPSOID,
writer_data,
contact_template,
)
else:
wp.static(create_solve_convex_single_contact(support_func, writer_func, post_process_contact))(
shape_a_data,
shape_b_data,
rot_a,
rot_b,
pos_a_adjusted,
pos_b_adjusted,
0.0, # combined_margin
data_provider,
rigid_gap + radius_eff_a + radius_eff_b + margin_a + margin_b,
writer_data,
contact_template,
)
return compute_gjk_mpr_contacts
@wp.func
def compute_tight_aabb_from_support(
shape_data: GenericShapeData,
orientation: wp.quat,
center_pos: wp.vec3,
data_provider: SupportMapDataProvider,
) -> tuple[wp.vec3, wp.vec3]:
"""
Compute tight AABB for a shape using support function.
Args:
shape_data: Generic shape data
orientation: Shape orientation (quaternion)
center_pos: Center position of the shape
data_provider: Support map data provider
Returns:
Tuple of (aabb_min, aabb_max) in world space
"""
# Transpose orientation matrix to transform world axes to local space
# Convert quaternion to 3x3 rotation matrix and transpose (inverse rotation)
rot_mat = wp.quat_to_matrix(orientation)
rot_mat_t = wp.transpose(rot_mat)
# Transform world axes to local space (multiply by transposed rotation = inverse rotation)
local_x = wp.vec3(rot_mat_t[0, 0], rot_mat_t[1, 0], rot_mat_t[2, 0])
local_y = wp.vec3(rot_mat_t[0, 1], rot_mat_t[1, 1], rot_mat_t[2, 1])
local_z = wp.vec3(rot_mat_t[0, 2], rot_mat_t[1, 2], rot_mat_t[2, 2])
# Compute AABB extents by evaluating support function in local space
# Dot products are done in local space to avoid expensive rotations
min_x = float(0.0)
max_x = float(0.0)
min_y = float(0.0)
max_y = float(0.0)
min_z = float(0.0)
max_z = float(0.0)
if shape_data.shape_type == GeoType.CONVEX_MESH:
# Single-pass AABB: iterate over vertices once, project onto all 3 axes.
# This replaces 6 separate support_map calls (each iterating all vertices)
# with 1 pass that computes min/max projections simultaneously.
mesh_ptr = unpack_mesh_ptr(shape_data.auxiliary)
mesh = wp.mesh_get(mesh_ptr)
mesh_scale = shape_data.scale
num_verts = mesh.points.shape[0]
# Pre-scale axes: dot(local_axis, scale*v) == dot(scale*local_axis, v)
scaled_x = wp.cw_mul(local_x, mesh_scale)
scaled_y = wp.cw_mul(local_y, mesh_scale)
scaled_z = wp.cw_mul(local_z, mesh_scale)
min_x = float(1.0e10)
max_x = float(-1.0e10)
min_y = float(1.0e10)
max_y = float(-1.0e10)
min_z = float(1.0e10)
max_z = float(-1.0e10)
for i in range(num_verts):
p = mesh.points[i]
vx = wp.dot(p, scaled_x)
vy = wp.dot(p, scaled_y)
vz = wp.dot(p, scaled_z)
min_x = wp.min(min_x, vx)
max_x = wp.max(max_x, vx)
min_y = wp.min(min_y, vy)
max_y = wp.max(max_y, vy)
min_z = wp.min(min_z, vz)
max_z = wp.max(max_z, vz)
else:
# Generic path: 6 support evaluations for other shape types (all O(1))
support_point = support_map(shape_data, local_x, data_provider)
max_x = wp.dot(local_x, support_point)
support_point = support_map(shape_data, local_y, data_provider)
max_y = wp.dot(local_y, support_point)
support_point = support_map(shape_data, local_z, data_provider)
max_z = wp.dot(local_z, support_point)
support_point = support_map(shape_data, -local_x, data_provider)
min_x = wp.dot(local_x, support_point)
support_point = support_map(shape_data, -local_y, data_provider)
min_y = wp.dot(local_y, support_point)
support_point = support_map(shape_data, -local_z, data_provider)
min_z = wp.dot(local_z, support_point)
# AABB in world space (add world position to extents)
aabb_min = wp.vec3(min_x, min_y, min_z) + center_pos
aabb_max = wp.vec3(max_x, max_y, max_z) + center_pos
return aabb_min, aabb_max
@wp.func
def compute_bounding_sphere_from_aabb(aabb_lower: wp.vec3, aabb_upper: wp.vec3) -> tuple[wp.vec3, float]:
"""
Compute a bounding sphere from an AABB.
Returns:
Tuple of (center, radius) where center is the AABB center and radius is half the diagonal.
"""
center = 0.5 * (aabb_lower + aabb_upper)
half_extents = 0.5 * (aabb_upper - aabb_lower)
radius = wp.length(half_extents)
return center, radius
@wp.func
def convert_infinite_plane_to_cube(
shape_data: GenericShapeData,
plane_rotation: wp.quat,
plane_position: wp.vec3,
other_position: wp.vec3,
other_radius: float,
) -> tuple[GenericShapeData, wp.vec3]:
"""
Convert an infinite plane into a cube proxy for GJK/MPR collision detection.
Since GJK/MPR cannot handle infinite planes, we create a finite cube where:
- The cube is positioned with its top face at the plane surface
- The cube's lateral dimensions are sized based on the other object's bounding sphere
- The cube extends only 'downward' from the plane (half-space in -Z direction in plane's local frame)
Args:
shape_data: The plane's shape data (should have shape_type == GeoType.PLANE)
plane_rotation: The plane's orientation (plane normal is along local +Z)
plane_position: The plane's position in world space
other_position: The other object's position in world space
other_radius: Bounding sphere radius of the colliding object
Returns:
Tuple of (modified_shape_data, adjusted_position):
- modified_shape_data: GenericShapeData configured as a BOX
- adjusted_position: The cube's center position (centered on other object projected to plane)
"""
result = GenericShapeData()
result.shape_type = GeoType.BOX
# Size the cube based on the other object's bounding sphere radius
# Make it large enough to always contain potential contact points
# The lateral dimensions (x, y) should be at least 2x the radius to ensure coverage
lateral_size = other_radius * 10.0
# The depth (z) should be large enough to encompass the potential collision region
# Half-space behavior: cube extends only below the plane surface (negative Z)
depth = other_radius * 10.0
# Set the box half-extents
# x, y: lateral coverage (parallel to plane)
# z: depth perpendicular to plane
result.scale = wp.vec3(lateral_size, lateral_size, depth)
# Position the cube center at the plane surface, directly under/over the other object
# Project the other object's position onto the plane
plane_normal = wp.quat_rotate(plane_rotation, wp.vec3(0.0, 0.0, 1.0))
to_other = other_position - plane_position
distance_along_normal = wp.dot(to_other, plane_normal)
# Point on plane surface closest to the other object
plane_surface_point = other_position - plane_normal * distance_along_normal
# Position cube center slightly below the plane surface so the top face is at the surface
# Since the cube has half-extent 'depth', its top face is at center + depth*normal
# We want: center + depth*normal = plane_surface, so center = plane_surface - depth*normal
adjusted_position = plane_surface_point - plane_normal * depth
return result, adjusted_position
@wp.func
def check_infinite_plane_bsphere_overlap(
shape_data_a: GenericShapeData,
shape_data_b: GenericShapeData,
pos_a: wp.vec3,
pos_b: wp.vec3,
quat_a: wp.quat,
quat_b: wp.quat,
bsphere_center_a: wp.vec3,
bsphere_center_b: wp.vec3,
bsphere_radius_a: float,
bsphere_radius_b: float,
) -> bool:
"""
Check if an infinite plane overlaps with another shape's bounding sphere.
Treats the plane as a half-space: objects on or below the plane (negative side of the normal)
are considered to overlap and will generate contacts.
Returns True if they overlap, False otherwise.
Uses data already extracted by extract_shape_data.
"""
type_a = shape_data_a.shape_type
type_b = shape_data_b.shape_type
scale_a = shape_data_a.scale
scale_b = shape_data_b.scale
# Check if either shape is an infinite plane
is_infinite_plane_a = (type_a == GeoType.PLANE) and (scale_a[0] == 0.0 and scale_a[1] == 0.0)
is_infinite_plane_b = (type_b == GeoType.PLANE) and (scale_b[0] == 0.0 and scale_b[1] == 0.0)
# If neither is an infinite plane, return True (no culling)
if not (is_infinite_plane_a or is_infinite_plane_b):
return True
# Determine which is the plane and which is the other shape
if is_infinite_plane_a:
plane_pos = pos_a
plane_quat = quat_a
other_center = bsphere_center_b
other_radius = bsphere_radius_b
else:
plane_pos = pos_b
plane_quat = quat_b
other_center = bsphere_center_a
other_radius = bsphere_radius_a
# Compute plane normal (plane's local +Z axis in world space)
plane_normal = wp.quat_rotate(plane_quat, wp.vec3(0.0, 0.0, 1.0))
# Distance from sphere center to plane (positive = above plane, negative = below plane)
center_dist = wp.dot(other_center - plane_pos, plane_normal)
# Treat plane as a half-space: objects on or below the plane (negative side) generate contacts
# Remove absolute value to only check penetration side
return center_dist <= other_radius
def create_find_contacts(writer_func: Any, support_func: Any = None, post_process_contact: Any = None):
"""
Factory function to create a find_contacts function with a specific writer function.
Args:
writer_func: Function to write contact data (signature: (ContactData, writer_data) -> None)
support_func: Support mapping function (defaults to support_map)
post_process_contact: Post-processing function (defaults to post_process_axial_on_discrete_contact)
Returns:
A find_contacts function with the writer function baked in
"""
if support_func is None:
support_func = support_map
if post_process_contact is None:
post_process_contact = post_process_axial_on_discrete_contact
@wp.func
def find_contacts(
pos_a: wp.vec3,
pos_b: wp.vec3,
quat_a: wp.quat,
quat_b: wp.quat,
shape_data_a: GenericShapeData,
shape_data_b: GenericShapeData,
is_infinite_plane_a: bool,
is_infinite_plane_b: bool,
bsphere_radius_a: float,
bsphere_radius_b: float,
rigid_gap: float,
shape_a: int,
shape_b: int,
margin_a: float,
margin_b: float,
writer_data: Any,
):
"""
Find contacts between two shapes using GJK/MPR algorithm and write them using the writer function.
Args:
pos_a: Position of shape A in world space
pos_b: Position of shape B in world space
quat_a: Orientation of shape A
quat_b: Orientation of shape B
shape_data_a: Generic shape data for shape A (contains shape_type)
shape_data_b: Generic shape data for shape B (contains shape_type)
is_infinite_plane_a: Whether shape A is an infinite plane
is_infinite_plane_b: Whether shape B is an infinite plane
bsphere_radius_a: Bounding sphere radius of shape A
bsphere_radius_b: Bounding sphere radius of shape B
rigid_gap: Contact gap for rigid bodies
shape_a: Index of shape A
shape_b: Index of shape B
margin_a: Per-shape margin offset for shape A (signed distance padding)
margin_b: Per-shape margin offset for shape B (signed distance padding)
writer_data: Data structure for contact writer
"""
if writer_data.contact_count[0] >= writer_data.contact_max:
return
# Convert infinite planes to cube proxies for GJK/MPR compatibility
# Use the OTHER object's radius to properly size the cube
# Only convert if it's an infinite plane (finite planes can be handled normally)
pos_a_adjusted = pos_a
if is_infinite_plane_a:
# Position the cube based on the OTHER object's position (pos_b)
# Note: convert_infinite_plane_to_cube modifies shape_data_a.shape_type to BOX
shape_data_a, pos_a_adjusted = convert_infinite_plane_to_cube(
shape_data_a, quat_a, pos_a, pos_b, bsphere_radius_b + rigid_gap
)
pos_b_adjusted = pos_b
if is_infinite_plane_b:
# Position the cube based on the OTHER object's position (pos_a)
# Note: convert_infinite_plane_to_cube modifies shape_data_b.shape_type to BOX
shape_data_b, pos_b_adjusted = convert_infinite_plane_to_cube(
shape_data_b, quat_b, pos_b, pos_a, bsphere_radius_a + rigid_gap
)
# Compute and write contacts using GJK/MPR
wp.static(
create_compute_gjk_mpr_contacts(
writer_func, post_process_contact=post_process_contact, support_func=support_func
)
)(
shape_data_a,
shape_data_b,
quat_a,
quat_b,
pos_a_adjusted,
pos_b_adjusted,
rigid_gap,
shape_a,
shape_b,
margin_a,
margin_b,
writer_data,
)
return find_contacts
@wp.func
def pre_contact_check(
shape_a: int,
shape_b: int,
pos_a: wp.vec3,
pos_b: wp.vec3,
quat_a: wp.quat,
quat_b: wp.quat,
shape_data_a: GenericShapeData,
shape_data_b: GenericShapeData,
aabb_a_lower: wp.vec3,
aabb_a_upper: wp.vec3,
aabb_b_lower: wp.vec3,
aabb_b_upper: wp.vec3,
pair: wp.vec2i,
mesh_id_a: wp.uint64,
mesh_id_b: wp.uint64,
shape_pairs_mesh: wp.array[wp.vec2i],
shape_pairs_mesh_count: wp.array[int],
shape_pairs_mesh_plane: wp.array[wp.vec2i],
shape_pairs_mesh_plane_cumsum: wp.array[int],
shape_pairs_mesh_plane_count: wp.array[int],
mesh_plane_vertex_total_count: wp.array[int],
shape_pairs_mesh_mesh: wp.array[wp.vec2i],
shape_pairs_mesh_mesh_count: wp.array[int],
):
"""
Perform pre-contact checks for early rejection and special case handling.
Args:
shape_a: Index of shape A
shape_b: Index of shape B
pos_a: Position of shape A in world space
pos_b: Position of shape B in world space
quat_a: Orientation of shape A
quat_b: Orientation of shape B
shape_data_a: Generic shape data for shape A (contains shape_type and scale)
shape_data_b: Generic shape data for shape B (contains shape_type and scale)
aabb_a_lower: Lower bound of AABB for shape A
aabb_a_upper: Upper bound of AABB for shape A
aabb_b_lower: Lower bound of AABB for shape B
aabb_b_upper: Upper bound of AABB for shape B
pair: Shape pair indices
mesh_id_a: Mesh ID pointer for shape A (wp.uint64(0) if not a mesh)
mesh_id_b: Mesh ID pointer for shape B (wp.uint64(0) if not a mesh)
shape_pairs_mesh: Output array for mesh collision pairs
shape_pairs_mesh_count: Counter for mesh collision pairs
shape_pairs_mesh_plane: Output array for mesh-plane collision pairs
shape_pairs_mesh_plane_cumsum: Cumulative sum array for mesh-plane vertices
shape_pairs_mesh_plane_count: Counter for mesh-plane collision pairs
mesh_plane_vertex_total_count: Total vertex count for mesh-plane collisions
shape_pairs_mesh_mesh: Output array for mesh-mesh collision pairs
shape_pairs_mesh_mesh_count: Counter for mesh-mesh collision pairs
Returns:
Tuple of (skip_pair, is_infinite_plane_a, is_infinite_plane_b, bsphere_radius_a, bsphere_radius_b)
"""
# Get shape types from shape data
type_a = shape_data_a.shape_type
type_b = shape_data_b.shape_type
# Check if shapes are infinite planes (scale.x == 0 and scale.y == 0)
# Scale is already in shape_data, no need for array lookup
is_infinite_plane_a = (type_a == GeoType.PLANE) and (shape_data_a.scale[0] == 0.0 and shape_data_a.scale[1] == 0.0)
is_infinite_plane_b = (type_b == GeoType.PLANE) and (shape_data_b.scale[0] == 0.0 and shape_data_b.scale[1] == 0.0)
# Early return: both shapes are infinite planes
if is_infinite_plane_a and is_infinite_plane_b:
return True, is_infinite_plane_a, is_infinite_plane_b, float(0.0), float(0.0)
# Compute bounding spheres from AABBs instead of using mesh bounding spheres
bsphere_center_a, bsphere_radius_a = compute_bounding_sphere_from_aabb(aabb_a_lower, aabb_a_upper)
bsphere_center_b, bsphere_radius_b = compute_bounding_sphere_from_aabb(aabb_b_lower, aabb_b_upper)
# Check if infinite plane vs bounding sphere overlap - early rejection
if not check_infinite_plane_bsphere_overlap(
shape_data_a,
shape_data_b,
pos_a,
pos_b,
quat_a,
quat_b,
bsphere_center_a,
bsphere_center_b,
bsphere_radius_a,
bsphere_radius_b,
):
return True, is_infinite_plane_a, is_infinite_plane_b, bsphere_radius_a, bsphere_radius_b
# Check for mesh vs infinite plane collision - special handling
# After sorting, type_a <= type_b, so we only need to check one direction
if type_a == GeoType.PLANE and type_b == GeoType.MESH:
# Check if plane is infinite (scale x and y are zero) - use scale from shape_data
if shape_data_a.scale[0] == 0.0 and shape_data_a.scale[1] == 0.0:
# Get mesh vertex count using the provided mesh_id
if mesh_id_b != wp.uint64(0):
mesh_obj = wp.mesh_get(mesh_id_b)
vertex_count = mesh_obj.points.shape[0]
# Add to mesh-plane collision buffer with cumulative vertex count
mesh_plane_idx = wp.atomic_add(shape_pairs_mesh_plane_count, 0, 1)
if mesh_plane_idx < shape_pairs_mesh_plane.shape[0]:
# Store shape indices (mesh, plane)
shape_pairs_mesh_plane[mesh_plane_idx] = wp.vec2i(shape_b, shape_a)
# Store inclusive cumulative vertex count in separate array for better cache locality
cumulative_count_before = wp.atomic_add(mesh_plane_vertex_total_count, 0, vertex_count)
cumulative_count_inclusive = cumulative_count_before + vertex_count
shape_pairs_mesh_plane_cumsum[mesh_plane_idx] = cumulative_count_inclusive
return True, is_infinite_plane_a, is_infinite_plane_b, bsphere_radius_a, bsphere_radius_b
# Check for mesh-mesh collisions - add to separate buffer for specialized handling
if type_a == GeoType.MESH and type_b == GeoType.MESH:
# Add to mesh-mesh collision buffer using atomic counter
mesh_mesh_pair_idx = wp.atomic_add(shape_pairs_mesh_mesh_count, 0, 1)
if mesh_mesh_pair_idx < shape_pairs_mesh_mesh.shape[0]:
shape_pairs_mesh_mesh[mesh_mesh_pair_idx] = pair
return True, is_infinite_plane_a, is_infinite_plane_b, bsphere_radius_a, bsphere_radius_b
# Check for other mesh collisions (mesh vs non-mesh) - add to separate buffer for specialized handling
if type_a == GeoType.MESH or type_b == GeoType.MESH:
# Add to mesh collision buffer using atomic counter
mesh_pair_idx = wp.atomic_add(shape_pairs_mesh_count, 0, 1)
if mesh_pair_idx < shape_pairs_mesh.shape[0]:
shape_pairs_mesh[mesh_pair_idx] = pair
return True, is_infinite_plane_a, is_infinite_plane_b, bsphere_radius_a, bsphere_radius_b
return False, is_infinite_plane_a, is_infinite_plane_b, bsphere_radius_a, bsphere_radius_b
@wp.func
def mesh_vs_convex_midphase(
idx_in_thread_block: int,
mesh_shape: int,
non_mesh_shape: int,
X_mesh_ws: wp.transform,
X_ws: wp.transform,
mesh_id: wp.uint64,
shape_type: wp.array[int],
shape_data: wp.array[wp.vec4],
shape_source_ptr: wp.array[wp.uint64],
rigid_gap: float,
triangle_pairs: wp.array[wp.vec3i],
triangle_pairs_count: wp.array[int],
):
"""
Perform mesh vs convex shape midphase collision detection.
This function finds all mesh triangles that overlap with the convex shape's AABB
by querying the mesh BVH. The results are output as triangle pairs for further
narrow-phase collision detection.
Args:
mesh_shape: Index of the mesh shape
non_mesh_shape: Index of the non-mesh (convex) shape
X_mesh_ws: Mesh world-space transform
X_ws: Non-mesh shape world-space transform
mesh_id: Mesh BVH ID
shape_type: Array of shape types
shape_data: Array of shape data (vec4: scale.xyz, margin.w)
shape_source_ptr: Array of mesh/SDF source pointers
rigid_gap: Contact gap for rigid bodies
triangle_pairs: Output array for triangle pairs (mesh_shape, non_mesh_shape, tri_index)
triangle_pairs_count: Counter for triangle pairs
"""
# Get inverse mesh transform (world to mesh local space)
X_mesh_sw = wp.transform_inverse(X_mesh_ws)
# Compute transform from non-mesh shape local space to mesh local space
# X_mesh_shape = X_mesh_sw * X_ws
X_mesh_shape = wp.transform_multiply(X_mesh_sw, X_ws)
pos_in_mesh = wp.transform_get_translation(X_mesh_shape)
orientation_in_mesh = wp.transform_get_rotation(X_mesh_shape)
# Create generic shape data for non-mesh shape
geo_type = shape_type[non_mesh_shape]
data_vec4 = shape_data[non_mesh_shape]
scale = wp.vec3(data_vec4[0], data_vec4[1], data_vec4[2])
generic_shape_data = GenericShapeData()
generic_shape_data.shape_type = geo_type
generic_shape_data.scale = scale
generic_shape_data.auxiliary = wp.vec3(0.0, 0.0, 0.0)
# For CONVEX_MESH, pack the mesh pointer
if geo_type == GeoType.CONVEX_MESH:
generic_shape_data.auxiliary = pack_mesh_ptr(shape_source_ptr[non_mesh_shape])
data_provider = SupportMapDataProvider()
# Compute tight AABB directly in mesh local space for optimal fit
aabb_lower, aabb_upper = compute_tight_aabb_from_support(
generic_shape_data, orientation_in_mesh, pos_in_mesh, data_provider
)
# Add small margin for contact detection
margin_vec = wp.vec3(rigid_gap, rigid_gap, rigid_gap)
aabb_lower = aabb_lower - margin_vec
aabb_upper = aabb_upper + margin_vec
if wp.static(ENABLE_TILE_BVH_QUERY):
# Query mesh BVH for overlapping triangles in mesh local space using tiled version
query = wp.tile_mesh_query_aabb(mesh_id, aabb_lower, aabb_upper)
result_tile = wp.tile_mesh_query_aabb_next(query)
# Continue querying while we have results
# Each iteration, each thread in the block gets one result (or -1)
while wp.tile_max(result_tile)[0] >= 0:
# Each thread processes its result from the tile
tri_index = wp.untile(result_tile)
# Add this triangle pair to the output buffer if valid
# Store (mesh_shape, non_mesh_shape, tri_index) to guarantee mesh is always first
has_tri = 0
if tri_index >= 0:
has_tri = 1
count_tile = wp.tile(has_tri)
inclusive_scan = wp.tile_scan_inclusive(count_tile)
offset = 0
if idx_in_thread_block == wp.block_dim() - 1:
offset = wp.atomic_add(triangle_pairs_count, 0, inclusive_scan[wp.block_dim() - 1])
offset_broadcast_tile = wp.tile(offset)
offset_broadcast = offset_broadcast_tile[wp.block_dim() - 1]
if tri_index >= 0:
# out_idx = wp.atomic_add(triangle_pairs_count, 0, 1)
out_idx = offset_broadcast + inclusive_scan[idx_in_thread_block] - has_tri
if out_idx < triangle_pairs.shape[0]:
triangle_pairs[out_idx] = wp.vec3i(mesh_shape, non_mesh_shape, tri_index)
result_tile = wp.tile_mesh_query_aabb_next(query)
else:
query = wp.mesh_query_aabb(mesh_id, aabb_lower, aabb_upper)
tri_index = wp.int32(0)
while wp.mesh_query_aabb_next(query, tri_index):
# Add this triangle pair to the output buffer if valid
# Store (mesh_shape, non_mesh_shape, tri_index) to guarantee mesh is always first
if tri_index >= 0:
out_idx = wp.atomic_add(triangle_pairs_count, 0, 1)
if out_idx < triangle_pairs.shape[0]:
triangle_pairs[out_idx] = wp.vec3i(mesh_shape, non_mesh_shape, tri_index)
@wp.func
def find_pair_from_cumulative_index(
global_idx: int,
cumulative_sums: wp.array[int],
pair_count: int,
) -> tuple[int, int]:
"""
Binary search to find which pair a global index belongs to.
This function is useful for mapping a flat global index to a (pair_index, local_index)
tuple when work is distributed across multiple pairs with varying sizes.
Args:
global_idx: Global index to search for
cumulative_sums: Array of inclusive cumulative sums (end indices for each pair)
pair_count: Number of pairs
Returns:
Tuple of (pair_index, local_index_within_pair)
"""
# Use binary_search to find first index where cumulative_sums[i] > global_idx
# This gives us the bucket that contains global_idx
pair_idx = binary_search(cumulative_sums, global_idx, 0, pair_count)
# Get cumulative start for this pair to calculate local index
cumulative_start = int(0)
if pair_idx > 0:
cumulative_start = int(cumulative_sums[pair_idx - 1])
local_idx = global_idx - cumulative_start
return pair_idx, local_idx
@wp.func
def get_triangle_shape_from_mesh(
mesh_id: wp.uint64,
mesh_scale: wp.vec3,
X_mesh_ws: wp.transform,
tri_idx: int,
) -> tuple[GenericShapeData, wp.vec3]:
"""
Extract triangle shape data from a mesh.
This function retrieves a specific triangle from a mesh and creates a GenericShapeData
structure for collision detection. The triangle is represented in world space with
vertex A as the origin.
Args:
mesh_id: The mesh ID (use wp.mesh_get to retrieve the mesh object)
mesh_scale: Scale to apply to mesh vertices
X_mesh_ws: Mesh world-space transform
tri_idx: Triangle index in the mesh
Returns:
Tuple of (shape_data, v0_world) where:
- shape_data: GenericShapeData with triangle geometry (type=TRIANGLE, scale=B-A, auxiliary=C-A)
- v0_world: First vertex position in world space (used as triangle origin)
"""
# Get the mesh object from the ID
mesh = wp.mesh_get(mesh_id)
# Extract triangle vertices from mesh (indices are stored as flat array: i0, i1, i2, i0, i1, i2, ...)
idx0 = mesh.indices[tri_idx * 3 + 0]
idx1 = mesh.indices[tri_idx * 3 + 1]
idx2 = mesh.indices[tri_idx * 3 + 2]
# Get vertex positions in mesh local space (with scale applied)
v0_local = wp.cw_mul(mesh.points[idx0], mesh_scale)
v1_local = wp.cw_mul(mesh.points[idx1], mesh_scale)
v2_local = wp.cw_mul(mesh.points[idx2], mesh_scale)
# Transform vertices to world space
v0_world = wp.transform_point(X_mesh_ws, v0_local)
v1_world = wp.transform_point(X_mesh_ws, v1_local)
v2_world = wp.transform_point(X_mesh_ws, v2_local)
# Create triangle shape data: vertex A at origin, B-A in scale, C-A in auxiliary
shape_data = GenericShapeData()
shape_data.shape_type = int(GeoTypeEx.TRIANGLE)
shape_data.scale = v1_world - v0_world # B - A
shape_data.auxiliary = v2_world - v0_world # C - A
return shape_data, v0_world
# OBB collisions by Separating Axis Theorem
@wp.func
def get_box_axes(q: wp.quat) -> wp.mat33:
"""Get the 3 local axes of a box from its quaternion rotation"""
# Box local axes (x, y, z)
local_x = wp.vec3(1.0, 0.0, 0.0)
local_y = wp.vec3(0.0, 1.0, 0.0)
local_z = wp.vec3(0.0, 0.0, 1.0)
# Rotate local axes to world space using warp's built-in method
axis_x = wp.quat_rotate(q, local_x)
axis_y = wp.quat_rotate(q, local_y)
axis_z = wp.quat_rotate(q, local_z)
return wp.matrix_from_rows(axis_x, axis_y, axis_z)
@wp.func
def project_box_onto_axis(transform: wp.transform, extents: wp.vec3, axis: wp.vec3) -> wp.vec2:
"""Project a box onto an axis and return [min, max] projection values"""
# Get box axes and extents
axes = get_box_axes(wp.transform_get_rotation(transform))
# Project box center onto axis
center_proj = wp.dot(wp.transform_get_translation(transform), axis)
# Project each axis of the box onto the separating axis and get the extent
extent = 0.0
extent += extents[0] * wp.abs(wp.dot(axes[0], axis)) # x-axis contribution
extent += extents[1] * wp.abs(wp.dot(axes[1], axis)) # y-axis contribution
extent += extents[2] * wp.abs(wp.dot(axes[2], axis)) # z-axis contribution
return wp.vec2(center_proj - extent, center_proj + extent)
@wp.func
def test_axis_separation(
transform_a: wp.transform, extents_a: wp.vec3, transform_b: wp.transform, extents_b: wp.vec3, axis: wp.vec3
) -> bool:
"""Test if two boxes are separated along a given axis. Returns True if separated."""
# Normalize the axis (handle zero-length axes)
axis_len = wp.length(axis)
if axis_len < 1e-8:
return False # Invalid axis, assume no separation
normalized_axis = axis / axis_len
# Project both boxes onto the axis
proj_a = project_box_onto_axis(transform_a, extents_a, normalized_axis)
proj_b = project_box_onto_axis(transform_b, extents_b, normalized_axis)
# Check if projections overlap - if no overlap, boxes are separated
return proj_a[1] < proj_b[0] or proj_b[1] < proj_a[0]
@wp.func
def sat_box_intersection(
transform_a: wp.transform, extents_a: wp.vec3, transform_b: wp.transform, extents_b: wp.vec3
) -> bool:
"""
Test if two oriented boxes intersect using the Separating Axis Theorem.
Args:
transform_a: Transform of first box (position and rotation)
extents_a: Half-extents of first box
transform_b: Transform of second box (position and rotation)
extents_b: Half-extents of second box
Returns:
bool: True if boxes intersect, False if separated
"""
# Get the axes for both boxes
axes_a = get_box_axes(wp.transform_get_rotation(transform_a))
axes_b = get_box_axes(wp.transform_get_rotation(transform_b))
# Test the 15 potential separating axes
# Test face normals of box A (3 axes)
for i in range(3):
if test_axis_separation(transform_a, extents_a, transform_b, extents_b, axes_a[i]):
return False # Boxes are separated
# Test face normals of box B (3 axes)
for i in range(3):
if test_axis_separation(transform_a, extents_a, transform_b, extents_b, axes_b[i]):
return False # Boxes are separated
# Test cross products of edge directions (9 axes: 3x3 combinations)
for i in range(3):
for j in range(3):
cross_axis = wp.cross(axes_a[i], axes_b[j])
if test_axis_separation(transform_a, extents_a, transform_b, extents_b, cross_axis):
return False # Boxes are separated
# If no separating axis found, boxes intersect
return True
================================================
FILE: newton/_src/geometry/collision_primitive.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# ATTENTION
#
# This file is used by MJWarp. Be careful when making changes. Verify that
# everything in MJWarp still works after your changes. Try to avoid too
# many dependencies on other files.
#
###########################################################################
"""Collision detection functions for primitive geometric shapes.
Conventions:
- Normal: points from first geometry into second geometry (unit length)
- Position: midpoint between the two contacting surfaces
- Distance: negative = penetration, positive = separation, zero = touching
- Geometry order: collide_A_B() means A is geom 0, B is geom 1
Returns (single contact): (distance: float, position: vec3, normal: vec3)
Returns (multi contact): (distances: vecN, positions: matNx3, normals: vecN or matNx3)
Use MAXVAL for unpopulated contact slots.
"""
import math
import warp as wp
from ..core.types import MAXVAL
from ..math import normalize_with_norm, safe_div
# Local type definitions for use within kernels
_vec8f = wp.types.vector(8, wp.float32)
_mat23f = wp.types.matrix((2, 3), wp.float32)
_mat43f = wp.types.matrix((4, 3), wp.float32)
_mat83f = wp.types.matrix((8, 3), wp.float32)
MINVAL = 1e-15
# For near-upright cylinders on planes, switch to fixed tripod mode when the
# axis-vs-plane-normal angle is below this threshold [deg].
CYLINDER_FLAT_MODE_DEG = 22.5
CYLINDER_FLAT_MODE_COS = float(math.cos(math.radians(CYLINDER_FLAT_MODE_DEG)))
@wp.func
def closest_segment_point(a: wp.vec3, b: wp.vec3, pt: wp.vec3) -> wp.vec3:
"""Returns the closest point on the a-b line segment to a point pt."""
ab = b - a
t = wp.dot(pt - a, ab) / (wp.dot(ab, ab) + 1e-6)
return a + wp.clamp(t, 0.0, 1.0) * ab
@wp.func
def closest_segment_point_and_dist(a: wp.vec3, b: wp.vec3, pt: wp.vec3) -> tuple[wp.vec3, float]:
"""Returns closest point on the line segment and the distance squared."""
closest = closest_segment_point(a, b, pt)
dist = wp.dot((pt - closest), (pt - closest))
return closest, dist
@wp.func
def closest_segment_to_segment_points(a0: wp.vec3, a1: wp.vec3, b0: wp.vec3, b1: wp.vec3) -> tuple[wp.vec3, wp.vec3]:
"""Returns closest points between two line segments."""
dir_a, len_a = normalize_with_norm(a1 - a0)
dir_b, len_b = normalize_with_norm(b1 - b0)
half_len_a = len_a * 0.5
half_len_b = len_b * 0.5
a_mid = a0 + dir_a * half_len_a
b_mid = b0 + dir_b * half_len_b
trans = a_mid - b_mid
dira_dot_dirb = wp.dot(dir_a, dir_b)
dira_dot_trans = wp.dot(dir_a, trans)
dirb_dot_trans = wp.dot(dir_b, trans)
denom = 1.0 - dira_dot_dirb * dira_dot_dirb
orig_t_a = (-dira_dot_trans + dira_dot_dirb * dirb_dot_trans) / (denom + 1e-6)
orig_t_b = dirb_dot_trans + orig_t_a * dira_dot_dirb
t_a = wp.clamp(orig_t_a, -half_len_a, half_len_a)
t_b = wp.clamp(orig_t_b, -half_len_b, half_len_b)
best_a = a_mid + dir_a * t_a
best_b = b_mid + dir_b * t_b
new_a, d1 = closest_segment_point_and_dist(a0, a1, best_b)
new_b, d2 = closest_segment_point_and_dist(b0, b1, best_a)
if d1 < d2:
return new_a, best_b
return best_a, new_b
# core
@wp.func
def collide_plane_sphere(
plane_normal: wp.vec3, plane_pos: wp.vec3, sphere_pos: wp.vec3, sphere_radius: float
) -> tuple[float, wp.vec3]:
# TODO(team): docstring
dist = wp.dot(sphere_pos - plane_pos, plane_normal) - sphere_radius
pos = sphere_pos - plane_normal * (sphere_radius + 0.5 * dist)
return dist, pos
@wp.func
def collide_sphere_sphere(
# In:
pos1: wp.vec3,
radius1: float,
pos2: wp.vec3,
radius2: float,
) -> tuple[float, wp.vec3, wp.vec3]:
"""Sphere-sphere collision calculation.
Args:
pos1: Center position of the first sphere
radius1: Radius of the first sphere
pos2: Center position of the second sphere
radius2: Radius of the second sphere
Returns:
Tuple containing:
dist: Distance between sphere surfaces (negative if overlapping)
pos: Contact position
n: Contact normal vector
"""
dir = pos2 - pos1
dist = wp.length(dir)
if dist == 0.0:
n = wp.vec3(1.0, 0.0, 0.0)
else:
n = dir / dist
dist = dist - (radius1 + radius2)
pos = pos1 + n * (radius1 + 0.5 * dist)
return dist, pos, n
@wp.func
def collide_sphere_capsule(
# In:
sphere_pos: wp.vec3,
sphere_radius: float,
capsule_pos: wp.vec3,
capsule_axis: wp.vec3,
capsule_radius: float,
capsule_half_length: float,
) -> tuple[float, wp.vec3, wp.vec3]:
"""Core contact geometry calculation for sphere-capsule collision.
Args:
sphere_pos: Center position of the sphere
sphere_radius: Radius of the sphere
capsule_pos: Center position of the capsule
capsule_axis: Axis direction of the capsule
capsule_radius: Radius of the capsule
capsule_half_length: Half length of the capsule
Returns:
Tuple containing:
dist: Distance between surfaces (negative if overlapping)
pos: Contact position (midpoint between closest surface points)
normal: Contact normal vector (from sphere toward capsule)
"""
# Calculate capsule segment
segment = capsule_axis * capsule_half_length
# Find closest point on capsule centerline to sphere center
pt = closest_segment_point(capsule_pos - segment, capsule_pos + segment, sphere_pos)
# Use sphere-sphere collision between sphere and closest point
return collide_sphere_sphere(sphere_pos, sphere_radius, pt, capsule_radius)
@wp.func
def collide_capsule_capsule(
# In:
cap1_pos: wp.vec3,
cap1_axis: wp.vec3,
cap1_radius: float,
cap1_half_length: float,
cap2_pos: wp.vec3,
cap2_axis: wp.vec3,
cap2_radius: float,
cap2_half_length: float,
) -> tuple[wp.vec2, wp.types.matrix((2, 3), wp.float32), wp.vec3]:
"""Core contact geometry calculation for capsule-capsule collision.
Args:
cap1_pos: Center position of the first capsule
cap1_axis: Axis direction of the first capsule
cap1_radius: Radius of the first capsule
cap1_half_length: Half length of the first capsule
cap2_pos: Center position of the second capsule
cap2_axis: Axis direction of the second capsule
cap2_radius: Radius of the second capsule
cap2_half_length: Half length of the second capsule
Returns:
Tuple containing:
contact_dist: Vector of contact distances (MAXVAL for invalid contacts)
contact_pos: Matrix of contact positions (one per row)
contact_normal: Shared contact normal vector (from capsule 1 toward capsule 2)
"""
contact_dist = wp.vec2(MAXVAL, MAXVAL)
contact_pos = _mat23f()
contact_normal = wp.vec3()
# Calculate scaled axes and center difference
axis1 = cap1_axis * cap1_half_length
axis2 = cap2_axis * cap2_half_length
dif = cap1_pos - cap2_pos
# Compute matrix coefficients and determinant
ma = wp.dot(axis1, axis1)
mb = -wp.dot(axis1, axis2)
mc = wp.dot(axis2, axis2)
u = -wp.dot(axis1, dif)
v = wp.dot(axis2, dif)
det = ma * mc - mb * mb
# Non-parallel axes: 1 contact
if wp.abs(det) >= MINVAL:
inv_det = 1.0 / det
x1 = (mc * u - mb * v) * inv_det
x2 = (ma * v - mb * u) * inv_det
if x1 > 1.0:
x1 = 1.0
x2 = (v - mb) / mc
elif x1 < -1.0:
x1 = -1.0
x2 = (v + mb) / mc
if x2 > 1.0:
x2 = 1.0
x1 = wp.clamp((u - mb) / ma, -1.0, 1.0)
elif x2 < -1.0:
x2 = -1.0
x1 = wp.clamp((u + mb) / ma, -1.0, 1.0)
# Find nearest points
vec1 = cap1_pos + axis1 * x1
vec2 = cap2_pos + axis2 * x2
dist, pos, normal = collide_sphere_sphere(vec1, cap1_radius, vec2, cap2_radius)
contact_dist[0] = dist
contact_pos[0] = pos
contact_normal = normal
# Parallel axes: 2 contacts (use first contact's normal for both)
else:
# First contact: positive end of capsule 1
vec1 = cap1_pos + axis1
x2 = wp.clamp((v - mb) / mc, -1.0, 1.0)
vec2 = cap2_pos + axis2 * x2
dist, pos, normal = collide_sphere_sphere(vec1, cap1_radius, vec2, cap2_radius)
contact_dist[0] = dist
contact_pos[0] = pos
contact_normal = normal # Use first contact's normal for both
# Second contact: negative end of capsule 1
vec1 = cap1_pos - axis1
x2 = wp.clamp((v + mb) / mc, -1.0, 1.0)
vec2 = cap2_pos + axis2 * x2
dist, pos, _normal = collide_sphere_sphere(vec1, cap1_radius, vec2, cap2_radius)
contact_dist[1] = dist
contact_pos[1] = pos
return contact_dist, contact_pos, contact_normal
@wp.func
def collide_plane_capsule(
# In:
plane_normal: wp.vec3,
plane_pos: wp.vec3,
capsule_pos: wp.vec3,
capsule_axis: wp.vec3,
capsule_radius: float,
capsule_half_length: float,
) -> tuple[wp.vec2, wp.types.matrix((2, 3), wp.float32), wp.mat33]:
"""Core contact geometry calculation for plane-capsule collision.
Args:
plane_normal: Normal vector of the plane
plane_pos: Position point on the plane
capsule_pos: Center position of the capsule
capsule_axis: Axis direction of the capsule
capsule_radius: Radius of the capsule
capsule_half_length: Half length of the capsule
Returns:
Tuple containing:
contact_dist: Vector of contact distances
contact_pos: Matrix of contact positions (one per row)
contact_frame: Contact frame for both contacts
"""
n = plane_normal
axis = capsule_axis
# align contact frames with capsule axis
b, b_norm = normalize_with_norm(axis - n * wp.dot(n, axis))
if b_norm < 0.5:
if -0.5 < n[1] and n[1] < 0.5:
b = wp.vec3(0.0, 1.0, 0.0)
else:
b = wp.vec3(0.0, 0.0, 1.0)
c = wp.cross(n, b)
frame = wp.mat33(n[0], n[1], n[2], b[0], b[1], b[2], c[0], c[1], c[2])
segment = axis * capsule_half_length
# First contact (positive end of capsule)
dist1, pos1 = collide_plane_sphere(n, plane_pos, capsule_pos + segment, capsule_radius)
# Second contact (negative end of capsule)
dist2, pos2 = collide_plane_sphere(n, plane_pos, capsule_pos - segment, capsule_radius)
dist = wp.vec2(dist1, dist2)
pos = _mat23f(pos1[0], pos1[1], pos1[2], pos2[0], pos2[1], pos2[2])
return dist, pos, frame
@wp.func
def collide_plane_ellipsoid(
# In:
plane_normal: wp.vec3,
plane_pos: wp.vec3,
ellipsoid_pos: wp.vec3,
ellipsoid_rot: wp.mat33,
ellipsoid_size: wp.vec3,
) -> tuple[float, wp.vec3, wp.vec3]:
"""Core contact geometry calculation for plane-ellipsoid collision.
Args:
plane_normal: Normal vector of the plane
plane_pos: Position point on the plane
ellipsoid_pos: Center position of the ellipsoid
ellipsoid_rot: Rotation matrix of the ellipsoid
ellipsoid_size: Size (radii) of the ellipsoid along each axis
Returns:
Tuple containing:
dist: Distance from ellipsoid surface to plane (negative if penetrating)
pos: Contact position on ellipsoid surface
normal: Contact normal vector (plane normal direction)
"""
sphere_support = -wp.normalize(wp.cw_mul(wp.transpose(ellipsoid_rot) @ plane_normal, ellipsoid_size))
pos = ellipsoid_pos + ellipsoid_rot @ wp.cw_mul(sphere_support, ellipsoid_size)
dist = wp.dot(plane_normal, pos - plane_pos)
pos = pos - plane_normal * dist * 0.5
return dist, pos, plane_normal
@wp.func
def collide_plane_box(
# In:
plane_normal: wp.vec3,
plane_pos: wp.vec3,
box_pos: wp.vec3,
box_rot: wp.mat33,
box_size: wp.vec3,
margin: float = 0.0,
) -> tuple[wp.vec4, wp.types.matrix((4, 3), wp.float32), wp.vec3]:
"""Core contact geometry calculation for plane-box collision.
Args:
plane_normal: Normal vector of the plane
plane_pos: Position point on the plane
box_pos: Center position of the box
box_rot: Rotation matrix of the box
box_size: Half-extents of the box along each axis
margin: Contact margin for early contact generation (default: 0.0)
Returns:
Tuple containing:
contact_dist: Vector of contact distances (MAXVAL for unpopulated contacts)
contact_pos: Matrix of contact positions (one per row)
contact_normal: contact normal vector
"""
corner = wp.vec3()
center_dist = wp.dot(box_pos - plane_pos, plane_normal)
dist = wp.vec4(MAXVAL)
pos = _mat43f()
# Test all corners and keep up to 4 deepest (most negative) contacts.
# Track the current worst kept contact by index for O(1) replacement checks.
ncontact = wp.int32(0)
worst_idx = wp.int32(0)
for i in range(8):
# get corner in local coordinates
corner.x = wp.where((i & 1) != 0, box_size.x, -box_size.x)
corner.y = wp.where((i & 2) != 0, box_size.y, -box_size.y)
corner.z = wp.where((i & 4) != 0, box_size.z, -box_size.z)
# get corner in global coordinates relative to box center
corner = box_rot @ corner
# Compute distance to plane and skip corners beyond margin.
ldist = wp.dot(plane_normal, corner)
cdist = center_dist + ldist
if cdist > margin:
continue
cpos = corner + box_pos - 0.5 * plane_normal * cdist
if ncontact < 4:
dist[ncontact] = cdist
pos[ncontact] = cpos
if ncontact == 0 or cdist > dist[worst_idx]:
worst_idx = ncontact
ncontact += 1
else:
if cdist < dist[worst_idx]:
dist[worst_idx] = cdist
pos[worst_idx] = cpos
# Recompute worst index (largest distance among kept contacts).
worst_idx = 0
if dist[1] > dist[worst_idx]:
worst_idx = 1
if dist[2] > dist[worst_idx]:
worst_idx = 2
if dist[3] > dist[worst_idx]:
worst_idx = 3
return dist, pos, plane_normal
@wp.func
def collide_sphere_cylinder(
# In:
sphere_pos: wp.vec3,
sphere_radius: float,
cylinder_pos: wp.vec3,
cylinder_axis: wp.vec3,
cylinder_radius: float,
cylinder_half_height: float,
) -> tuple[float, wp.vec3, wp.vec3]:
"""Core contact geometry calculation for sphere-cylinder collision.
Args:
sphere_pos: Center position of the sphere
sphere_radius: Radius of the sphere
cylinder_pos: Center position of the cylinder
cylinder_axis: Axis direction of the cylinder
cylinder_radius: Radius of the cylinder
cylinder_half_height: Half height of the cylinder
Returns:
Tuple containing:
dist: Distance between surfaces (negative if overlapping)
pos: Contact position (midpoint between closest surface points)
normal: Contact normal vector (from sphere toward cylinder)
"""
vec = sphere_pos - cylinder_pos
x = wp.dot(vec, cylinder_axis)
a_proj = cylinder_axis * x
p_proj = vec - a_proj
p_proj_sqr = wp.dot(p_proj, p_proj)
collide_side = wp.abs(x) < cylinder_half_height
collide_cap = p_proj_sqr < (cylinder_radius * cylinder_radius)
if collide_side and collide_cap:
dist_cap = cylinder_half_height - wp.abs(x)
dist_radius = cylinder_radius - wp.sqrt(p_proj_sqr)
if dist_cap < dist_radius:
collide_side = False
else:
collide_cap = False
# side collision
if collide_side:
pos_target = cylinder_pos + a_proj
return collide_sphere_sphere(sphere_pos, sphere_radius, pos_target, cylinder_radius)
# cap collision
elif collide_cap:
if x > 0.0:
# top cap
pos_cap = cylinder_pos + cylinder_axis * cylinder_half_height
plane_normal = cylinder_axis
else:
# bottom cap
pos_cap = cylinder_pos - cylinder_axis * cylinder_half_height
plane_normal = -cylinder_axis
dist, pos = collide_plane_sphere(plane_normal, pos_cap, sphere_pos, sphere_radius)
return dist, pos, -plane_normal # flip normal after position calculation
# corner collision
else:
inv_len = safe_div(1.0, wp.sqrt(p_proj_sqr))
p_proj = p_proj * (cylinder_radius * inv_len)
cap_offset = cylinder_axis * (wp.sign(x) * cylinder_half_height)
pos_corner = cylinder_pos + cap_offset + p_proj
return collide_sphere_sphere(sphere_pos, sphere_radius, pos_corner, 0.0)
@wp.func
def collide_plane_cylinder(
# In:
plane_normal: wp.vec3,
plane_pos: wp.vec3,
cylinder_pos: wp.vec3,
cylinder_axis: wp.vec3,
cylinder_radius: float,
cylinder_half_height: float,
) -> tuple[wp.vec4, wp.types.matrix((4, 3), wp.float32), wp.vec3]:
"""Core contact geometry calculation for plane-cylinder collision.
Uses two contact modes:
- Flat-surface mode (near upright): fixed-orientation stable tripod on
the near cap plus one deepest rim point.
- Rolling mode: 1 deepest rim point + 2 side-generator contacts + 1
near-cap rim contact (one generator typically merges with the deepest
point, leaving 3 contacts).
Args:
plane_normal: Normal vector of the plane.
plane_pos: Position point on the plane.
cylinder_pos: Center position of the cylinder.
cylinder_axis: Axis direction of the cylinder.
cylinder_radius: Radius of the cylinder.
cylinder_half_height: Half height of the cylinder.
Returns:
Tuple containing:
contact_dist: Vector of contact distances (MAXVAL for unpopulated).
contact_pos: Matrix of contact positions (one per row).
contact_normal: Contact normal (plane normal).
"""
contact_dist = wp.vec4(MAXVAL)
contact_pos = _mat43f()
n = plane_normal
axis = cylinder_axis
# Orient axis toward the plane
dot_na = wp.dot(n, axis)
if dot_na > 0.0:
axis = -axis
dot_na = -dot_na
# Near cap center (the cap closer to the plane)
cap_center = cylinder_pos + axis * cylinder_half_height
# Build cap-plane directions.
# perp_align tracks the deepest radial direction wrt the plane.
perp_align = -n + axis * dot_na
perp_align_len_sq = wp.dot(perp_align, perp_align)
has_align = perp_align_len_sq > 1e-10
if has_align:
perp_align = perp_align * (1.0 / wp.sqrt(perp_align_len_sq))
# perp_fixed gives a deterministic world-anchored rim orientation.
ref = wp.vec3(1.0, 0.0, 0.0)
if wp.abs(wp.dot(axis, ref)) > 0.9:
ref = wp.vec3(0.0, 1.0, 0.0)
perp_fixed = ref - axis * wp.dot(axis, ref)
perp_fixed = wp.normalize(perp_fixed)
abs_dot = -dot_na # in [0, 1], where 1 is upright
flat_mode_cos = wp.static(CYLINDER_FLAT_MODE_COS)
in_flat_surface_mode = abs_dot >= flat_mode_cos
deepest_perp = wp.where(has_align, perp_align, perp_fixed)
deepest_pt = cap_center + deepest_perp * cylinder_radius
deepest_d = wp.dot(deepest_pt - plane_pos, n)
deepest_pos = deepest_pt - n * (deepest_d * 0.5)
# Emit deepest contact first, then add extra contacts only if they are
# sufficiently far from deepest in world space.
contact_dist[0] = deepest_d
contact_pos[0] = deepest_pos
ncontact = wp.int32(1)
merge_threshold = 0.01 * wp.max(cylinder_radius, cylinder_half_height)
merge_threshold_sq = merge_threshold * merge_threshold
# Near-upright flat mode: fixed tripod + deepest point (no blending).
# The tripod orientation is purely fixed/world-anchored and does not
# depend on the deepest-point direction.
if in_flat_surface_mode:
u_fixed = perp_fixed * cylinder_radius
v_fixed = wp.cross(axis, perp_fixed) * cylinder_radius
# Stable tripod (120-degree spacing) in fixed orientation.
c120 = float(-0.5)
s120 = float(0.8660254)
pt0 = cap_center + u_fixed
d0 = wp.dot(pt0 - plane_pos, n)
pos0 = pt0 - n * (d0 * 0.5)
if ncontact < 4 and wp.length_sq(pos0 - deepest_pos) > merge_threshold_sq:
contact_dist[ncontact] = d0
contact_pos[ncontact] = pos0
ncontact += 1
pt1 = cap_center + c120 * u_fixed + s120 * v_fixed
d1 = wp.dot(pt1 - plane_pos, n)
pos1 = pt1 - n * (d1 * 0.5)
if ncontact < 4 and wp.length_sq(pos1 - deepest_pos) > merge_threshold_sq:
contact_dist[ncontact] = d1
contact_pos[ncontact] = pos1
ncontact += 1
pt2 = cap_center + c120 * u_fixed - s120 * v_fixed
d2 = wp.dot(pt2 - plane_pos, n)
pos2 = pt2 - n * (d2 * 0.5)
if ncontact < 4 and wp.length_sq(pos2 - deepest_pos) > merge_threshold_sq:
contact_dist[ncontact] = d2
contact_pos[ncontact] = pos2
ncontact += 1
else:
# Rolling mode: side generators plus the cap-rim point facing the plane.
perp_roll = wp.where(has_align, perp_align, perp_fixed)
u = perp_roll * cylinder_radius
v = wp.cross(axis, perp_roll) * cylinder_radius
# Candidate 0: top side generator (+u)
pt = cylinder_pos + axis * cylinder_half_height + u
d = wp.dot(pt - plane_pos, n)
pos = pt - n * (d * 0.5)
if ncontact < 4 and wp.length_sq(pos - deepest_pos) > merge_threshold_sq:
contact_dist[ncontact] = d
contact_pos[ncontact] = pos
ncontact += 1
# Candidate 1: bottom side generator (+u)
pt = cylinder_pos - axis * cylinder_half_height + u
d = wp.dot(pt - plane_pos, n)
pos = pt - n * (d * 0.5)
if ncontact < 4 and wp.length_sq(pos - deepest_pos) > merge_threshold_sq:
contact_dist[ncontact] = d
contact_pos[ncontact] = pos
ncontact += 1
# Keep only the cap-rim point that faces the plane.
pt_pos_v = cap_center + v
d_pos_v = wp.dot(pt_pos_v - plane_pos, n)
pt_neg_v = cap_center - v
d_neg_v = wp.dot(pt_neg_v - plane_pos, n)
use_pos_v = d_pos_v <= d_neg_v
pt = wp.where(use_pos_v, pt_pos_v, pt_neg_v)
d = wp.where(use_pos_v, d_pos_v, d_neg_v)
pos = pt - n * (d * 0.5)
if ncontact < 4 and wp.length_sq(pos - deepest_pos) > merge_threshold_sq:
contact_dist[ncontact] = d
contact_pos[ncontact] = pos
ncontact += 1
return contact_dist, contact_pos, n
@wp.func
def _compute_rotmore(face_idx: int) -> wp.mat33:
rotmore = wp.mat33(0.0)
if face_idx == 0:
rotmore[0, 2] = -1.0
rotmore[1, 1] = +1.0
rotmore[2, 0] = +1.0
elif face_idx == 1:
rotmore[0, 0] = +1.0
rotmore[1, 2] = -1.0
rotmore[2, 1] = +1.0
elif face_idx == 2:
rotmore[0, 0] = +1.0
rotmore[1, 1] = +1.0
rotmore[2, 2] = +1.0
elif face_idx == 3:
rotmore[0, 2] = +1.0
rotmore[1, 1] = +1.0
rotmore[2, 0] = -1.0
elif face_idx == 4:
rotmore[0, 0] = +1.0
rotmore[1, 2] = +1.0
rotmore[2, 1] = -1.0
elif face_idx == 5:
rotmore[0, 0] = -1.0
rotmore[1, 1] = +1.0
rotmore[2, 2] = -1.0
return rotmore
@wp.func
def collide_box_box(
# In:
box1_pos: wp.vec3,
box1_rot: wp.mat33,
box1_size: wp.vec3,
box2_pos: wp.vec3,
box2_rot: wp.mat33,
box2_size: wp.vec3,
margin: float = 0.0,
) -> tuple[wp.types.vector(8, wp.float32), wp.types.matrix((8, 3), wp.float32), wp.types.matrix((8, 3), wp.float32)]:
"""Core contact geometry calculation for box-box collision.
Args:
box1_pos: Center position of the first box
box1_rot: Rotation matrix of the first box
box1_size: Half-extents of the first box along each axis
box2_pos: Center position of the second box
box2_rot: Rotation matrix of the second box
box2_size: Half-extents of the second box along each axis
margin: Distance threshold for early contact generation (default: 0.0).
When positive, contacts are generated before boxes overlap.
Returns:
Tuple containing:
contact_dist: Vector of contact distances (MAXVAL for unpopulated contacts)
contact_pos: Matrix of contact positions (one per row)
contact_normals: Matrix of contact normal vectors (one per row)
"""
# Initialize output matrices
contact_dist = _vec8f()
for i in range(8):
contact_dist[i] = MAXVAL
contact_pos = _mat83f()
contact_normals = _mat83f()
contact_count = 0
# Compute transforms between box's frames
pos21 = wp.transpose(box1_rot) @ (box2_pos - box1_pos)
pos12 = wp.transpose(box2_rot) @ (box1_pos - box2_pos)
rot21 = wp.transpose(box1_rot) @ box2_rot
rot12 = wp.transpose(rot21)
rot21abs = wp.matrix_from_rows(wp.abs(rot21[0]), wp.abs(rot21[1]), wp.abs(rot21[2]))
rot12abs = wp.transpose(rot21abs)
plen2 = rot21abs @ box2_size
plen1 = rot12abs @ box1_size
# Compute axis of maximum separation
s_sum_3 = 3.0 * (box1_size + box2_size)
separation = wp.float32(margin + s_sum_3[0] + s_sum_3[1] + s_sum_3[2])
axis_code = wp.int32(-1)
# First test: consider boxes' face normals
for i in range(3):
c1 = -wp.abs(pos21[i]) + box1_size[i] + plen2[i]
c2 = -wp.abs(pos12[i]) + box2_size[i] + plen1[i]
if c1 < -margin or c2 < -margin:
return contact_dist, contact_pos, contact_normals
if c1 < separation:
separation = c1
axis_code = i + 3 * wp.int32(pos21[i] < 0) + 0 # Face of box1
if c2 < separation:
separation = c2
axis_code = i + 3 * wp.int32(pos12[i] < 0) + 6 # Face of box2
clnorm = wp.vec3(0.0)
inv = wp.bool(False)
cle1 = wp.int32(0)
cle2 = wp.int32(0)
# Second test: consider cross products of boxes' edges
for i in range(3):
for j in range(3):
# Compute cross product of box edges (potential separating axis)
if i == 0:
cross_axis = wp.vec3(0.0, -rot12[j, 2], rot12[j, 1])
elif i == 1:
cross_axis = wp.vec3(rot12[j, 2], 0.0, -rot12[j, 0])
else:
cross_axis = wp.vec3(-rot12[j, 1], rot12[j, 0], 0.0)
cross_length = wp.length(cross_axis)
if cross_length < MINVAL:
continue
cross_axis /= cross_length
box_dist = wp.dot(pos21, cross_axis)
c3 = wp.float32(0.0)
# Project box half-sizes onto the potential separating axis
for k in range(3):
if k != i:
c3 += box1_size[k] * wp.abs(cross_axis[k])
if k != j:
c3 += box2_size[k] * rot21abs[i, 3 - k - j] / cross_length
c3 -= wp.abs(box_dist)
# Early exit: no collision if separated along this axis
if c3 < -margin:
return contact_dist, contact_pos, contact_normals
# Track minimum separation and which edge-edge pair it occurs on
if c3 < separation * (1.0 - 1e-12):
separation = c3
# Determine which corners/edges are closest
cle1 = 0
cle2 = 0
for k in range(3):
if k != i and (int(cross_axis[k] > 0) ^ int(box_dist < 0)):
cle1 += 1 << k
if k != j:
if int(rot21[i, 3 - k - j] > 0) ^ int(box_dist < 0) ^ int((k - j + 3) % 3 == 1):
cle2 += 1 << k
axis_code = 12 + i * 3 + j
clnorm = cross_axis
inv = box_dist < 0
# No axis with separation < margin found
if axis_code == -1:
return contact_dist, contact_pos, contact_normals
points = _mat83f()
depth = _vec8f()
max_con_pair = 8
# 8 contacts should suffice for most configurations
if axis_code < 12:
# Handle face-vertex collision
face_idx = axis_code % 6
box_idx = axis_code // 6
rotmore = _compute_rotmore(face_idx)
r = rotmore @ wp.where(box_idx, rot12, rot21)
p = rotmore @ wp.where(box_idx, pos12, pos21)
ss = wp.abs(rotmore @ wp.where(box_idx, box2_size, box1_size))
s = wp.where(box_idx, box1_size, box2_size)
rt = wp.transpose(r)
lx, ly, hz = ss[0], ss[1], ss[2]
p[2] -= hz
clcorner = wp.int32(0) # corner of non-face box with least axis separation
for i in range(3):
if r[2, i] < 0:
clcorner += 1 << i
lp = p
for i in range(wp.static(3)):
lp += rt[i] * s[i] * wp.where(clcorner & 1 << i, 1.0, -1.0)
m = wp.int32(1)
dirs = wp.int32(0)
cn1 = wp.vec3(0.0)
cn2 = wp.vec3(0.0)
for i in range(3):
if wp.abs(r[2, i]) < 0.5:
if not dirs:
cn1 = rt[i] * s[i] * wp.where(clcorner & (1 << i), -2.0, 2.0)
else:
cn2 = rt[i] * s[i] * wp.where(clcorner & (1 << i), -2.0, 2.0)
dirs += 1
k = dirs * dirs
# Find potential contact points
n = wp.int32(0)
for i in range(k):
for q in range(2):
# lines_a and lines_b (lines between corners) computed on the fly
lav = lp + wp.where(i < 2, wp.vec3(0.0), wp.where(i == 2, cn1, cn2))
lbv = wp.where(i == 0 or i == 3, cn1, cn2)
if wp.abs(lbv[q]) > MINVAL:
br = 1.0 / lbv[q]
for j in range(-1, 2, 2):
l = ss[q] * wp.float32(j)
c1 = (l - lav[q]) * br
if c1 < 0 or c1 > 1:
continue
c2 = lav[1 - q] + lbv[1 - q] * c1
if wp.abs(c2) > ss[1 - q]:
continue
points[n] = lav + c1 * lbv
n += 1
if dirs == 2:
ax = cn1[0]
bx = cn2[0]
ay = cn1[1]
by = cn2[1]
C = safe_div(1.0, ax * by - bx * ay)
for i in range(4):
llx = wp.where(i // 2, lx, -lx)
lly = wp.where(i % 2, ly, -ly)
x = llx - lp[0]
y = lly - lp[1]
u = (x * by - y * bx) * C
v = (y * ax - x * ay) * C
if u > 0 and v > 0 and u < 1 and v < 1:
points[n] = wp.vec3(llx, lly, lp[2] + u * cn1[2] + v * cn2[2])
n += 1
for i in range(1 << dirs):
tmpv = lp + wp.float32(i & 1) * cn1 + wp.float32((i & 2) != 0) * cn2
if tmpv[0] > -lx and tmpv[0] < lx and tmpv[1] > -ly and tmpv[1] < ly:
points[n] = tmpv
n += 1
m = n
n = wp.int32(0)
for i in range(m):
if points[i][2] > margin:
continue
if i != n:
points[n] = points[i]
points[n, 2] *= 0.5
depth[n] = points[n, 2] * 2.0
n += 1
# Set up contact frame
rw = wp.where(box_idx, box2_rot, box1_rot) @ wp.transpose(rotmore)
pw = wp.where(box_idx, box2_pos, box1_pos)
normal = wp.where(box_idx, -1.0, 1.0) * wp.transpose(rw)[2]
else:
# Handle edge-edge collision
edge1 = (axis_code - 12) // 3
edge2 = (axis_code - 12) % 3
# Set up non-contacting edges ax1, ax2 for box2 and pax1, pax2 for box 1
ax1 = wp.int32(1 - (edge2 & 1))
ax2 = wp.int32(2 - (edge2 & 2))
pax1 = wp.int32(1 - (edge1 & 1))
pax2 = wp.int32(2 - (edge1 & 2))
if rot21abs[edge1, ax1] < rot21abs[edge1, ax2]:
ax1, ax2 = ax2, ax1
if rot12abs[edge2, pax1] < rot12abs[edge2, pax2]:
pax1, pax2 = pax2, pax1
rotmore = _compute_rotmore(wp.where(cle1 & (1 << pax2), pax2, pax2 + 3))
# Transform coordinates for edge-edge contact calculation
p = rotmore @ pos21
rnorm = rotmore @ clnorm
r = rotmore @ rot21
rt = wp.transpose(r)
s = wp.abs(wp.transpose(rotmore) @ box1_size)
lx, ly, hz = s[0], s[1], s[2]
p[2] -= hz
# Calculate closest box2 face
points[0] = (
p
+ rt[ax1] * box2_size[ax1] * wp.where(cle2 & (1 << ax1), 1.0, -1.0)
+ rt[ax2] * box2_size[ax2] * wp.where(cle2 & (1 << ax2), 1.0, -1.0)
)
points[1] = points[0] - rt[edge2] * box2_size[edge2]
points[0] += rt[edge2] * box2_size[edge2]
points[2] = (
p
+ rt[ax1] * box2_size[ax1] * wp.where(cle2 & (1 << ax1), -1.0, 1.0)
+ rt[ax2] * box2_size[ax2] * wp.where(cle2 & (1 << ax2), 1.0, -1.0)
)
points[3] = points[2] - rt[edge2] * box2_size[edge2]
points[2] += rt[edge2] * box2_size[edge2]
n = 4
# Set up coordinate axes for contact face of box2
axi_lp = points[0]
axi_cn1 = points[1] - points[0]
axi_cn2 = points[2] - points[0]
# Check if contact normal is valid
if wp.abs(rnorm[2]) < MINVAL:
return contact_dist, contact_pos, contact_normals # Shouldn't happen
# Calculate inverse normal for projection
innorm = wp.where(inv, -1.0, 1.0) / rnorm[2]
pu = _mat43f()
# Project points onto contact plane
for i in range(4):
pu[i] = points[i]
c_scl = points[i, 2] * wp.where(inv, -1.0, 1.0) * innorm
points[i] -= rnorm * c_scl
pts_lp = points[0]
pts_cn1 = points[1] - points[0]
pts_cn2 = points[2] - points[0]
n = wp.int32(0)
for i in range(4):
for q in range(2):
la = pts_lp[q] + wp.where(i < 2, 0.0, wp.where(i == 2, pts_cn1[q], pts_cn2[q]))
lb = wp.where(i == 0 or i == 3, pts_cn1[q], pts_cn2[q])
lc = pts_lp[1 - q] + wp.where(i < 2, 0.0, wp.where(i == 2, pts_cn1[1 - q], pts_cn2[1 - q]))
ld = wp.where(i == 0 or i == 3, pts_cn1[1 - q], pts_cn2[1 - q])
# linesu_a and linesu_b (lines between corners) computed on the fly
lua = axi_lp + wp.where(i < 2, wp.vec3(0.0), wp.where(i == 2, axi_cn1, axi_cn2))
lub = wp.where(i == 0 or i == 3, axi_cn1, axi_cn2)
if wp.abs(lb) > MINVAL:
br = 1.0 / lb
for j in range(-1, 2, 2):
if n == max_con_pair:
break
l = s[q] * wp.float32(j)
c1 = (l - la) * br
if c1 < 0 or c1 > 1:
continue
c2 = lc + ld * c1
if wp.abs(c2) > s[1 - q]:
continue
if (lua[2] + lub[2] * c1) * innorm > margin:
continue
points[n] = lua * 0.5 + c1 * lub * 0.5
points[n, q] += 0.5 * l
points[n, 1 - q] += 0.5 * c2
depth[n] = points[n, 2] * innorm * 2.0
n += 1
nl = n
ax = pts_cn1[0]
bx = pts_cn2[0]
ay = pts_cn1[1]
by = pts_cn2[1]
C = safe_div(1.0, ax * by - bx * ay)
for i in range(4):
if n == max_con_pair:
break
llx = wp.where(i // 2, lx, -lx)
lly = wp.where(i % 2, ly, -ly)
x = llx - pts_lp[0]
y = lly - pts_lp[1]
u = (x * by - y * bx) * C
v = (y * ax - x * ay) * C
if nl == 0:
if (u < 0 or u > 1) and (v < 0 or v > 1):
continue
elif u < 0 or v < 0 or u > 1 or v > 1:
continue
u = wp.clamp(u, 0.0, 1.0)
v = wp.clamp(v, 0.0, 1.0)
w = 1.0 - u - v
vtmp = pu[0] * w + pu[1] * u + pu[2] * v
points[n] = wp.vec3(llx, lly, 0.0)
vtmp2 = points[n] - vtmp
tc1 = wp.length_sq(vtmp2)
if vtmp[2] > 0 and tc1 > margin * margin:
continue
points[n] = 0.5 * (points[n] + vtmp)
depth[n] = wp.sqrt(tc1) * wp.where(vtmp[2] < 0, -1.0, 1.0)
n += 1
nf = n
for i in range(4):
if n >= max_con_pair:
break
x = pu[i, 0]
y = pu[i, 1]
if nl == 0 and nf != 0:
if (x < -lx or x > lx) and (y < -ly or y > ly):
continue
elif x < -lx or x > lx or y < -ly or y > ly:
continue
c1 = wp.float32(0)
for j in range(2):
if pu[i, j] < -s[j]:
c1 += (pu[i, j] + s[j]) * (pu[i, j] + s[j])
elif pu[i, j] > s[j]:
c1 += (pu[i, j] - s[j]) * (pu[i, j] - s[j])
c1 += pu[i, 2] * innorm * pu[i, 2] * innorm
if pu[i, 2] > 0 and c1 > margin * margin:
continue
tmp_p = wp.vec3(pu[i, 0], pu[i, 1], 0.0)
for j in range(2):
if pu[i, j] < -s[j]:
tmp_p[j] = -s[j] * 0.5
elif pu[i, j] > s[j]:
tmp_p[j] = +s[j] * 0.5
tmp_p += pu[i]
points[n] = tmp_p * 0.5
depth[n] = wp.sqrt(c1) * wp.where(pu[i, 2] < 0, -1.0, 1.0)
n += 1
# Set up contact data for all points
rw = box1_rot @ wp.transpose(rotmore)
pw = box1_pos
normal = wp.where(inv, -1.0, 1.0) * rw @ rnorm
contact_count = n
# Copy contact data to output matrices
for i in range(contact_count):
points[i, 2] += hz
pos = rw @ points[i] + pw
contact_dist[i] = depth[i]
contact_pos[i] = pos
contact_normals[i] = normal
return contact_dist, contact_pos, contact_normals
@wp.func
def collide_sphere_box(
# In:
sphere_pos: wp.vec3,
sphere_radius: float,
box_pos: wp.vec3,
box_rot: wp.mat33,
box_size: wp.vec3,
) -> tuple[float, wp.vec3, wp.vec3]:
"""Core contact geometry calculation for sphere-box collision.
Args:
sphere_pos: Center position of the sphere
sphere_radius: Radius of the sphere
box_pos: Center position of the box
box_rot: Rotation matrix of the box
box_size: Half-extents of the box along each axis
Returns:
Tuple containing:
dist: Distance between surfaces (negative if overlapping)
pos: Contact position (midpoint between closest surface points)
normal: Contact normal vector (from sphere toward box)
"""
center = wp.transpose(box_rot) @ (sphere_pos - box_pos)
clamped = wp.max(-box_size, wp.min(box_size, center))
clamped_dir, dist = normalize_with_norm(clamped - center)
# sphere center inside box (use robust epsilon for numerical stability across platforms)
if dist <= 1e-6:
closest = 2.0 * (box_size[0] + box_size[1] + box_size[2])
k = wp.int32(0)
for i in range(6):
face_dist = wp.abs(wp.where(i % 2, 1.0, -1.0) * box_size[i // 2] - center[i // 2])
if closest > face_dist:
closest = face_dist
k = i
nearest = wp.vec3(0.0)
nearest[k // 2] = wp.where(k % 2, -1.0, 1.0)
pos = center + nearest * (sphere_radius - closest) / 2.0
contact_normal = box_rot @ nearest
contact_distance = -closest - sphere_radius
else:
deepest = center + clamped_dir * sphere_radius
pos = 0.5 * (clamped + deepest)
contact_normal = box_rot @ clamped_dir
contact_distance = dist - sphere_radius
contact_position = box_pos + box_rot @ pos
return contact_distance, contact_position, contact_normal
@wp.func
def collide_capsule_box(
# In:
capsule_pos: wp.vec3,
capsule_axis: wp.vec3,
capsule_radius: float,
capsule_half_length: float,
box_pos: wp.vec3,
box_rot: wp.mat33,
box_size: wp.vec3,
) -> tuple[wp.vec2, wp.types.matrix((2, 3), wp.float32), wp.types.matrix((2, 3), wp.float32)]:
"""Core contact geometry calculation for capsule-box collision.
Args:
capsule_pos: Center position of the capsule
capsule_axis: Axis direction of the capsule
capsule_radius: Radius of the capsule
capsule_half_length: Half length of the capsule
box_pos: Center position of the box
box_rot: Rotation matrix of the box
box_size: Half-extents of the box along each axis
Returns:
Tuple containing:
contact_dist: Vector of contact distances (MAXVAL for unpopulated contacts)
contact_pos: Matrix of contact positions (one per row)
contact_normals: Matrix of contact normal vectors (one per row)
"""
# Based on the mjc implementation
boxmatT = wp.transpose(box_rot)
pos = boxmatT @ (capsule_pos - box_pos)
axis = boxmatT @ capsule_axis
halfaxis = axis * capsule_half_length # halfaxis is the capsule direction
axisdir = wp.int32(halfaxis[0] > 0.0) + 2 * wp.int32(halfaxis[1] > 0.0) + 4 * wp.int32(halfaxis[2] > 0.0)
bestdistmax = 2.0 * (capsule_radius + capsule_half_length + box_size[0] + box_size[1] + box_size[2])
# keep track of closest point
bestdist = wp.float32(bestdistmax)
bestsegmentpos = wp.float32(-12)
# cltype: encoded collision configuration
# cltype / 3 == 0 : lower corner is closest to the capsule
# == 2 : upper corner is closest to the capsule
# == 1 : middle of the edge is closest to the capsule
# cltype % 3 == 0 : lower corner is closest to the box
# == 2 : upper corner is closest to the box
# == 1 : middle of the capsule is closest to the box
cltype = wp.int32(-4)
# clface: index of the closest face of the box to the capsule
# -1: no face is closest (edge or corner is closest)
# 0, 1, 2: index of the axis perpendicular to the closest face
clface = wp.int32(-12)
# first: consider cases where a face of the box is closest
for i in range(-1, 2, 2):
axisTip = pos + wp.float32(i) * halfaxis
boxPoint = wp.vec3(axisTip)
n_out = wp.int32(0)
ax_out = wp.int32(-1)
for j in range(3):
if boxPoint[j] < -box_size[j]:
n_out += 1
ax_out = j
boxPoint[j] = -box_size[j]
elif boxPoint[j] > box_size[j]:
n_out += 1
ax_out = j
boxPoint[j] = box_size[j]
if n_out > 1:
continue
dist = wp.length_sq(boxPoint - axisTip)
if dist < bestdist:
bestdist = dist
bestsegmentpos = wp.float32(i)
cltype = -2 + i
clface = ax_out
# second: consider cases where an edge of the box is closest
clcorner = wp.int32(-123) # which corner is the closest
cledge = wp.int32(-123) # which axis
bestboxpos = wp.float32(0.0)
for i in range(8):
for j in range(3):
if i & (1 << j) != 0:
continue
c2 = wp.int32(-123)
# box_pt is the starting point (corner) on the box
box_pt = wp.cw_mul(
wp.vec3(
wp.where(i & 1, 1.0, -1.0),
wp.where(i & 2, 1.0, -1.0),
wp.where(i & 4, 1.0, -1.0),
),
box_size,
)
box_pt[j] = 0.0
# find closest point between capsule and the edge
dif = box_pt - pos
u = -box_size[j] * dif[j]
v = wp.dot(halfaxis, dif)
ma = box_size[j] * box_size[j]
mb = -box_size[j] * halfaxis[j]
mc = capsule_half_length * capsule_half_length
det = ma * mc - mb * mb
if wp.abs(det) < MINVAL:
continue
idet = 1.0 / det
# sX : X=1 means middle of segment. X=0 or 2 one or the other end
x1 = wp.float32((mc * u - mb * v) * idet)
x2 = wp.float32((ma * v - mb * u) * idet)
s1 = wp.int32(1)
s2 = wp.int32(1)
if x1 > 1:
x1 = 1.0
s1 = 2
x2 = safe_div(v - mb, mc)
elif x1 < -1:
x1 = -1.0
s1 = 0
x2 = safe_div(v + mb, mc)
x2_over = x2 > 1.0
if x2_over or x2 < -1.0:
if x2_over:
x2 = 1.0
s2 = 2
x1 = safe_div(u - mb, ma)
else:
x2 = -1.0
s2 = 0
x1 = safe_div(u + mb, ma)
if x1 > 1:
x1 = 1.0
s1 = 2
elif x1 < -1:
x1 = -1.0
s1 = 0
dif -= halfaxis * x2
dif[j] += box_size[j] * x1
# encode relative positions of the closest points
ct = s1 * 3 + s2
dif_sq = wp.length_sq(dif)
if dif_sq < bestdist - MINVAL:
bestdist = dif_sq
bestsegmentpos = x2
bestboxpos = x1
# ct<6 means closest point on box is at lower end or middle of edge
c2 = ct // 6
clcorner = i + (1 << j) * c2 # index of closest box corner
cledge = j # axis index of closest box edge
cltype = ct # encoded collision configuration
best = wp.float32(0.0)
p = wp.vec2(pos.x, pos.y)
dd = wp.vec2(halfaxis.x, halfaxis.y)
s = wp.vec2(box_size.x, box_size.y)
secondpos = wp.float32(-4.0)
uu = dd.x * s.y
vv = dd.y * s.x
w_neg = dd.x * p.y - dd.y * p.x < 0
best = wp.float32(-1.0)
ee1 = uu - vv
ee2 = uu + vv
if wp.abs(ee1) > best:
best = wp.abs(ee1)
c1 = wp.where((ee1 < 0) == w_neg, 0, 3)
if wp.abs(ee2) > best:
best = wp.abs(ee2)
c1 = wp.where((ee2 > 0) == w_neg, 1, 2)
if cltype == -4: # invalid type
return wp.vec2(MAXVAL), _mat23f(), _mat23f()
if cltype >= 0 and cltype // 3 != 1: # closest to a corner of the box
c1 = axisdir ^ clcorner
# Calculate relative orientation between capsule and corner
# There are two possible configurations:
# 1. Capsule axis points toward/away from corner
# 2. Capsule axis aligns with a face or edge
if c1 != 0 and c1 != 7: # create second contact point
if c1 == 1 or c1 == 2 or c1 == 4:
mul = 1
else:
mul = -1
c1 = 7 - c1
# "de" and "dp" distance from first closest point on the capsule to both ends of it
# mul is a direction along the capsule's axis
if c1 == 1:
ax = 0
ax1 = 1
ax2 = 2
elif c1 == 2:
ax = 1
ax1 = 2
ax2 = 0
elif c1 == 4:
ax = 2
ax1 = 0
ax2 = 1
if axis[ax] * axis[ax] > 0.5: # second point along the edge of the box
m = 2.0 * safe_div(box_size[ax], wp.abs(halfaxis[ax]))
secondpos = wp.min(1.0 - wp.float32(mul) * bestsegmentpos, m)
else: # second point along a face of the box
# check for overshoot again
m = 2.0 * wp.min(
safe_div(box_size[ax1], wp.abs(halfaxis[ax1])),
safe_div(box_size[ax2], wp.abs(halfaxis[ax2])),
)
secondpos = -wp.min(1.0 + wp.float32(mul) * bestsegmentpos, m)
secondpos *= wp.float32(mul)
elif cltype >= 0 and cltype // 3 == 1: # we are on box's edge
# Calculate relative orientation between capsule and edge
# Two possible configurations:
# - T configuration: c1 = 2^n (no additional contacts)
# - X configuration: c1 != 2^n (potential additional contacts)
c1 = axisdir ^ clcorner
c1 &= 7 - (1 << cledge) # mask out edge axis to determine configuration
if c1 == 1 or c1 == 2 or c1 == 4: # create second contact point
if cledge == 0:
ax1 = 1
ax2 = 2
if cledge == 1:
ax1 = 2
ax2 = 0
if cledge == 2:
ax1 = 0
ax2 = 1
ax = cledge
# find which face the capsule has a lower angle, and switch the axis
if wp.abs(axis[ax1]) > wp.abs(axis[ax2]):
ax1 = ax2
ax2 = 3 - ax - ax1
# mul determines direction along capsule axis for second contact point
if c1 & (1 << ax2):
mul = 1
secondpos = 1.0 - bestsegmentpos
else:
mul = -1
secondpos = 1.0 + bestsegmentpos
# now find out whether we point towards the opposite side or towards one of the sides
# and also find the farthest point along the capsule that is above the box
e1 = 2.0 * safe_div(box_size[ax2], wp.abs(halfaxis[ax2]))
secondpos = wp.min(e1, secondpos)
if ((axisdir & (1 << ax)) != 0) == ((c1 & (1 << ax2)) != 0):
e2 = 1.0 - bestboxpos
else:
e2 = 1.0 + bestboxpos
e1 = box_size[ax] * safe_div(e2, wp.abs(halfaxis[ax]))
secondpos = wp.min(e1, secondpos)
secondpos *= wp.float32(mul)
elif cltype < 0:
# similarly we handle the case when one capsule's end is closest to a face of the box
# and find where is the other end pointing to and clamping to the farthest point
# of the capsule that's above the box
# if the closest point is inside the box there's no need for a second point
if clface != -1: # create second contact point
mul = wp.where(cltype == -3, 1, -1)
secondpos = 2.0
tmp1 = pos - halfaxis * wp.float32(mul)
for i in range(3):
if i != clface:
ha_r = safe_div(wp.float32(mul), halfaxis[i])
e1 = (box_size[i] - tmp1[i]) * ha_r
if 0 < e1 and e1 < secondpos:
secondpos = e1
e1 = (-box_size[i] - tmp1[i]) * ha_r
if 0 < e1 and e1 < secondpos:
secondpos = e1
secondpos *= wp.float32(mul)
# create sphere in original orientation at first contact point
s1_pos_l = pos + halfaxis * bestsegmentpos
s1_pos_g = box_rot @ s1_pos_l + box_pos
# collide with sphere using core function
dist1, pos1, normal1 = collide_sphere_box(s1_pos_g, capsule_radius, box_pos, box_rot, box_size)
if secondpos > -3: # secondpos was modified
s2_pos_l = pos + halfaxis * (secondpos + bestsegmentpos)
s2_pos_g = box_rot @ s2_pos_l + box_pos
# collide with sphere using core function
dist2, pos2, normal2 = collide_sphere_box(s2_pos_g, capsule_radius, box_pos, box_rot, box_size)
else:
dist2 = MAXVAL
pos2 = wp.vec3()
normal2 = wp.vec3()
return (
wp.vec2(dist1, dist2),
_mat23f(pos1[0], pos1[1], pos1[2], pos2[0], pos2[1], pos2[2]),
_mat23f(normal1[0], normal1[1], normal1[2], normal2[0], normal2[1], normal2[2]),
)
================================================
FILE: newton/_src/geometry/contact_data.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Contact data structures for collision detection.
This module defines the core contact data structures used throughout the collision detection system.
"""
import warp as wp
# Bit flag and mask used to encode heightfield shape indices in collision pair buffers.
SHAPE_PAIR_HFIELD_BIT = wp.int32(1 << 30)
SHAPE_PAIR_INDEX_MASK = wp.int32((1 << 30) - 1)
@wp.struct
class ContactData:
"""
Internal contact representation for collision detection.
This struct stores contact information between two colliding shapes before conversion
to solver-specific formats. It serves as an intermediate representation passed between
collision detection algorithms and contact writer functions.
Attributes:
contact_point_center: Center point of the contact region in world space
contact_normal_a_to_b: Unit normal vector pointing from shape A to shape B
contact_distance: Signed distance between shapes (negative indicates penetration)
radius_eff_a: Effective radius of shape A (for rounded shapes like spheres/capsules)
radius_eff_b: Effective radius of shape B (for rounded shapes like spheres/capsules)
margin_a: Collision surface margin offset for shape A
margin_b: Collision surface margin offset for shape B
shape_a: Index of the first shape in the collision pair
shape_b: Index of the second shape in the collision pair
gap_sum: Pairwise summed contact gap threshold that determines if a contact should be written
contact_stiffness: Contact stiffness. 0.0 means no stiffness was set.
contact_damping: Contact damping scale. 0.0 means no damping was set.
contact_friction_scale: Friction scaling factor. 0.0 means no friction was set.
"""
contact_point_center: wp.vec3
contact_normal_a_to_b: wp.vec3
contact_distance: float
radius_eff_a: float
radius_eff_b: float
margin_a: float
margin_b: float
shape_a: int
shape_b: int
gap_sum: float
contact_stiffness: float
contact_damping: float
contact_friction_scale: float
@wp.func
def contact_passes_gap_check(
contact_data: ContactData,
) -> bool:
"""
Check if a contact passes the gap threshold check and should be written.
Args:
contact_data: ContactData struct containing contact information
Returns:
True if the contact distance is within the contact gap threshold, False otherwise
"""
total_separation_needed = (
contact_data.radius_eff_a + contact_data.radius_eff_b + contact_data.margin_a + contact_data.margin_b
)
# Distance calculation matching box_plane_collision
contact_normal_a_to_b = wp.normalize(contact_data.contact_normal_a_to_b)
a_contact_world = contact_data.contact_point_center - contact_normal_a_to_b * (
0.5 * contact_data.contact_distance + contact_data.radius_eff_a
)
b_contact_world = contact_data.contact_point_center + contact_normal_a_to_b * (
0.5 * contact_data.contact_distance + contact_data.radius_eff_b
)
diff = b_contact_world - a_contact_world
distance = wp.dot(diff, contact_normal_a_to_b)
d = distance - total_separation_needed
return d <= contact_data.gap_sum
================================================
FILE: newton/_src/geometry/contact_reduction.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Contact reduction utilities for mesh collision.
This module provides constants, helper functions, and shared-memory utilities
used by the contact reduction system. The reduction selects a representative
subset (up to ``MAX_CONTACTS_PER_PAIR`` contacts per shape pair) that preserves
simulation stability.
**Configuration:**
Edit the constants in the "Contact Reduction Configuration" block below to
tune the reduction. All values are plain Python integers evaluated at module
import time and consumed via ``wp.static()`` in kernels, so there is zero
runtime overhead. Changing them requires restarting the process (standard Warp
kernel-caching behavior).
.. note::
Only the default ``"icosahedron"`` polyhedron configuration is currently
tested on CI. Other polyhedra (dodecahedron, octahedron, hexahedron) are
functional but should be considered experimental.
**Contact Reduction Strategy Overview:**
When complex meshes collide, thousands of triangle pairs may generate contacts.
Contact reduction selects a representative subset that preserves simulation
stability while keeping memory and computation bounded.
The reduction uses three complementary strategies:
1. **Spatial Extreme Slots** (``NUM_NORMAL_BINS`` x ``NUM_SPATIAL_DIRECTIONS``)
For each normal bin (polyhedron face), finds the most extreme contacts in
evenly-spaced 2D scan directions on the face plane. This builds the convex
hull / support polygon boundary, critical for stable stacking.
2. **Per-Bin Max-Depth Slots** (``NUM_NORMAL_BINS`` x 1)
Each normal bin tracks its deepest contact unconditionally. This ensures
deeply penetrating contacts from any normal direction are never dropped.
Critical for gear-like contacts with varied normal orientations.
3. **Voxel-Based Depth Slots** (``NUM_VOXEL_DEPTH_SLOTS``)
The mesh is divided into a virtual voxel grid. Each voxel independently
tracks its deepest contact, providing spatial coverage and preventing
sudden contact jumps when different mesh regions become deepest.
See Also:
:class:`GlobalContactReducer` in ``contact_reduction_global.py`` for the
hashtable-based approach used for mesh-mesh (SDF) collisions.
"""
import warp as wp
# =====================================================================
# Contact Reduction Configuration
# =====================================================================
# Polyhedron for normal binning. Determines NUM_NORMAL_BINS.
# "icosahedron" -> 20 bins (default, finer normal resolution)
# "dodecahedron" -> 12 bins (good balance)
# "octahedron" -> 8 bins (cheaper, coarser)
# "hexahedron" -> 6 bins (cheapest, coarsest)
#
# NOTE: Only the default "icosahedron" configuration is currently tested
# on CI. Other polyhedra are functional but should be considered
# experimental. Use at your own discretion.
NORMAL_BINNING_POLYHEDRON = "icosahedron"
# Scan directions per normal bin (2D extremes on each face plane).
# Range 3-6. More directions = more accurate convex hull but more slots.
NUM_SPATIAL_DIRECTIONS = 6
# Voxel-based depth slots for spatial coverage.
NUM_VOXEL_DEPTH_SLOTS = 100
# =====================================================================
# Hard architectural limit — keeps per-pair indices representable in 8 bits.
MAX_CONTACTS_PER_PAIR = 255
# ---------------------------------------------------------------------------
# Derived constants (do not edit — computed from the config above)
# ---------------------------------------------------------------------------
_POLYHEDRON_BINS = {
"hexahedron": 6,
"octahedron": 8,
"dodecahedron": 12,
"icosahedron": 20,
}
NUM_NORMAL_BINS = _POLYHEDRON_BINS[NORMAL_BINNING_POLYHEDRON]
_total_slots = NUM_NORMAL_BINS * (NUM_SPATIAL_DIRECTIONS + 1) + NUM_VOXEL_DEPTH_SLOTS
assert _total_slots <= MAX_CONTACTS_PER_PAIR, (
f"Total reduction slots ({_total_slots}) exceed MAX_CONTACTS_PER_PAIR "
f"({MAX_CONTACTS_PER_PAIR}). Reduce NUM_SPATIAL_DIRECTIONS, "
f"NUM_VOXEL_DEPTH_SLOTS, or switch to a coarser polyhedron."
)
# http://stereopsis.com/radix.html
@wp.func_native("""
uint32_t i = reinterpret_cast(f);
uint32_t mask = (uint32_t)(-(int)(i >> 31)) | 0x80000000;
return i ^ mask;
""")
def float_flip(f: float) -> wp.uint32: ...
@wp.func_native("""
#if defined(__CUDA_ARCH__)
__syncthreads();
#endif
""")
def synchronize(): ...
# =====================================================================
# Polyhedron face normals
# =====================================================================
# Each polyhedron is stored as a flat tuple of (x, y, z) triples.
# Only the selected polyhedron is compiled into a Warp matrix constant.
# fmt: off
_HEXAHEDRON_NORMALS = (
1.0, 0.0, 0.0,
-1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, -1.0, 0.0,
0.0, 0.0, 1.0,
0.0, 0.0, -1.0,
)
_OCTAHEDRON_NORMALS = (
0.57735027, 0.57735027, 0.57735027,
0.57735027, 0.57735027, -0.57735027,
-0.57735027, 0.57735027, 0.57735027,
-0.57735027, 0.57735027, -0.57735027,
0.57735027, -0.57735027, 0.57735027,
0.57735027, -0.57735027, -0.57735027,
-0.57735027, -0.57735027, 0.57735027,
-0.57735027, -0.57735027, -0.57735027,
)
# Dodecahedron face normals (= normalised icosahedron vertices).
# Ordered: top group (0-3, Y > 0), equatorial (4-7, Y = 0),
# bottom group (8-11, Y < 0).
# a = 1/sqrt(1+phi^2) ~ 0.52573111, b = phi/sqrt(1+phi^2) ~ 0.85065081
_DODECAHEDRON_NORMALS = (
# Top group (faces 0-3, Y > 0)
0.52573111, 0.85065081, 0.0,
-0.52573111, 0.85065081, 0.0,
0.0, 0.52573111, 0.85065081,
0.0, 0.52573111, -0.85065081,
# Equatorial band (faces 4-7, Y = 0)
0.85065081, 0.0, 0.52573111,
0.85065081, 0.0, -0.52573111,
-0.85065081, 0.0, 0.52573111,
-0.85065081, 0.0, -0.52573111,
# Bottom group (faces 8-11, Y < 0)
0.0, -0.52573111, 0.85065081,
0.0, -0.52573111, -0.85065081,
0.52573111, -0.85065081, 0.0,
-0.52573111, -0.85065081, 0.0,
)
# Icosahedron face normals (20 faces).
# Ordered: top cap (0-4, Y ~ +0.79), equatorial belt (5-14, |Y| ~ 0.19),
# bottom cap (15-19, Y ~ -0.79). This layout enables contiguous range
# searches (top-only, top+equat, equat+bottom, bottom-only).
_ICOSAHEDRON_NORMALS = (
# Top cap (faces 0-4, Y ~ +0.795)
0.49112338, 0.79465455, 0.35682216,
-0.18759243, 0.79465450, 0.57735026,
-0.60706190, 0.79465450, 0.0,
-0.18759237, 0.79465450, -0.57735026,
0.49112340, 0.79465455, -0.35682210,
# Equatorial belt (faces 5-14, |Y| ~ 0.188)
0.98224690, -0.18759257, 0.0,
0.79465440, 0.18759239, -0.57735030,
0.30353096, -0.18759252, 0.93417233,
0.79465440, 0.18759243, 0.57735030,
-0.79465450, -0.18759249, 0.57735030,
-0.30353105, 0.18759243, 0.93417240,
-0.79465440, -0.18759240, -0.57735030,
-0.98224690, 0.18759254, 0.0,
0.30353096, -0.18759250, -0.93417233,
-0.30353084, 0.18759246, -0.93417240,
# Bottom cap (faces 15-19, Y ~ -0.795)
0.18759249, -0.79465440, 0.57735026,
-0.49112338, -0.79465450, 0.35682213,
-0.49112338, -0.79465455, -0.35682213,
0.18759243, -0.79465440, -0.57735026,
0.60706200, -0.79465440, 0.0,
)
# fmt: on
_POLYHEDRON_NORMALS_DATA = {
"hexahedron": _HEXAHEDRON_NORMALS,
"octahedron": _OCTAHEDRON_NORMALS,
"dodecahedron": _DODECAHEDRON_NORMALS,
"icosahedron": _ICOSAHEDRON_NORMALS,
}
_face_normals_mat_type = wp.types.matrix(shape=(NUM_NORMAL_BINS, 3), dtype=wp.float32)
FACE_NORMALS = _face_normals_mat_type(*_POLYHEDRON_NORMALS_DATA[NORMAL_BINNING_POLYHEDRON])
# Backward-compatible alias used by tests.
DODECAHEDRON_FACE_NORMALS = (
_face_normals_mat_type(*_DODECAHEDRON_NORMALS)
if NORMAL_BINNING_POLYHEDRON == "dodecahedron"
else wp.types.matrix(shape=(12, 3), dtype=wp.float32)(*_DODECAHEDRON_NORMALS)
)
@wp.func
def get_slot(normal: wp.vec3) -> int:
"""Return the normal-bin index whose face normal best matches *normal*.
Each polyhedron has a compile-time specialization selected via
``NORMAL_BINNING_POLYHEDRON``:
* **hexahedron** (6 faces) — O(1) axis-aligned comparison, no dot products.
* **dodecahedron** (12 faces) — Y-based range pruning over
top / equatorial / bottom groups (4-8 dot products).
* **icosahedron** (20 faces) — Y-based range pruning over
top cap / equatorial belt / bottom cap (5-15 dot products).
* **octahedron** and any other polyhedron — full linear scan.
Args:
normal: Normal vector to match.
Returns:
Index of the best matching face in ``FACE_NORMALS``.
"""
if wp.static(NORMAL_BINNING_POLYHEDRON == "hexahedron"):
# Faces are axis-aligned: 0=+X, 1=-X, 2=+Y, 3=-Y, 4=+Z, 5=-Z.
ax = wp.abs(normal[0])
ay = wp.abs(normal[1])
az = wp.abs(normal[2])
if ax >= ay and ax >= az:
if normal[0] >= 0.0:
return 0
else:
return 1
elif ay >= az:
if normal[1] >= 0.0:
return 2
else:
return 3
else:
if normal[2] >= 0.0:
return 4
else:
return 5
elif wp.static(NORMAL_BINNING_POLYHEDRON == "dodecahedron"):
up_dot = normal[1]
# Conservative thresholds: only skip regions when clearly in a polar cap.
# Face layout: 0-3 = top group, 4-7 = equatorial, 8-11 = bottom group.
if up_dot > 0.65:
start_idx = 0
end_idx = 4
elif up_dot < -0.65:
start_idx = 8
end_idx = 12
elif up_dot >= 0.0:
start_idx = 0
end_idx = 8
else:
start_idx = 4
end_idx = 12
best_slot = start_idx
max_dot = wp.dot(normal, FACE_NORMALS[start_idx])
for i in range(start_idx + 1, end_idx):
d = wp.dot(normal, FACE_NORMALS[i])
if d > max_dot:
max_dot = d
best_slot = i
return best_slot
elif wp.static(NORMAL_BINNING_POLYHEDRON == "icosahedron"):
up_dot = normal[1]
# Face layout: 0-4 = top cap, 5-14 = equatorial belt, 15-19 = bottom cap.
if up_dot > 0.65:
start_idx = 0
end_idx = 5
elif up_dot < -0.65:
start_idx = 15
end_idx = 20
elif up_dot >= 0.0:
start_idx = 0
end_idx = 15
else:
start_idx = 5
end_idx = 20
best_slot = start_idx
max_dot = wp.dot(normal, FACE_NORMALS[start_idx])
for i in range(start_idx + 1, end_idx):
d = wp.dot(normal, FACE_NORMALS[i])
if d > max_dot:
max_dot = d
best_slot = i
return best_slot
else:
best_slot = 0
max_dot = wp.dot(normal, FACE_NORMALS[0])
for i in range(1, wp.static(NUM_NORMAL_BINS)):
d = wp.dot(normal, FACE_NORMALS[i])
if d > max_dot:
max_dot = d
best_slot = i
return best_slot
@wp.func
def project_point_to_plane(bin_normal_idx: wp.int32, point: wp.vec3) -> wp.vec2:
"""Project a 3D point onto the 2D plane of a normal-bin face.
Creates a local 2D coordinate system on the face plane using the face
normal and constructs orthonormal basis vectors u and v.
Args:
bin_normal_idx: Index of the face in ``FACE_NORMALS``.
point: 3D point to project.
Returns:
2D coordinates of the point in the face's local coordinate system.
"""
face_normal = FACE_NORMALS[bin_normal_idx]
if wp.abs(face_normal[1]) < 0.9:
ref = wp.vec3(0.0, 1.0, 0.0)
else:
ref = wp.vec3(1.0, 0.0, 0.0)
u = wp.normalize(ref - wp.dot(ref, face_normal) * face_normal)
v = wp.cross(face_normal, u)
return wp.vec2(wp.dot(point, u), wp.dot(point, v))
@wp.func
def get_spatial_direction_2d(dir_idx: int) -> wp.vec2:
"""Get evenly-spaced 2D direction for spatial binning.
Args:
dir_idx: Direction index in the range 0..NUM_SPATIAL_DIRECTIONS-1.
Returns:
Unit 2D vector at angle ``dir_idx * 2pi / NUM_SPATIAL_DIRECTIONS``.
"""
angle = float(dir_idx) * (2.0 * wp.pi / float(wp.static(NUM_SPATIAL_DIRECTIONS)))
return wp.vec2(wp.cos(angle), wp.sin(angle))
def compute_num_reduction_slots() -> int:
"""Total reduction slots per shape pair.
Returns:
``NUM_NORMAL_BINS * (NUM_SPATIAL_DIRECTIONS + 1) + NUM_VOXEL_DEPTH_SLOTS``,
guaranteed to be at most ``MAX_CONTACTS_PER_PAIR`` (255).
"""
return NUM_NORMAL_BINS * (NUM_SPATIAL_DIRECTIONS + 1) + NUM_VOXEL_DEPTH_SLOTS
@wp.func
def compute_voxel_index(
pos_local: wp.vec3,
aabb_lower: wp.vec3,
aabb_upper: wp.vec3,
resolution: wp.vec3i,
) -> int:
"""Compute voxel index for a position in local space.
Args:
pos_local: Position in mesh local space
aabb_lower: Local AABB lower bound
aabb_upper: Local AABB upper bound
resolution: Voxel grid resolution (nx, ny, nz)
Returns:
Linear voxel index in [0, nx*ny*nz)
"""
size = aabb_upper - aabb_lower
# Normalize position to [0, 1]
rel = wp.vec3(0.0, 0.0, 0.0)
if size[0] > 1e-6:
rel = wp.vec3((pos_local[0] - aabb_lower[0]) / size[0], rel[1], rel[2])
if size[1] > 1e-6:
rel = wp.vec3(rel[0], (pos_local[1] - aabb_lower[1]) / size[1], rel[2])
if size[2] > 1e-6:
rel = wp.vec3(rel[0], rel[1], (pos_local[2] - aabb_lower[2]) / size[2])
# Clamp to [0, 1) and map to voxel indices
nx = resolution[0]
ny = resolution[1]
nz = resolution[2]
vx = wp.clamp(int(rel[0] * float(nx)), 0, nx - 1)
vy = wp.clamp(int(rel[1] * float(ny)), 0, ny - 1)
vz = wp.clamp(int(rel[2] * float(nz)), 0, nz - 1)
return vx + vy * nx + vz * nx * ny
def create_shared_memory_pointer_block_dim_func(
add: int,
):
"""Create a shared memory pointer function for a block-dimension-dependent array size.
Args:
add: Number of additional int elements beyond WP_TILE_BLOCK_DIM.
Returns:
A Warp function that returns a pointer to shared memory
"""
snippet = f"""
#if defined(__CUDA_ARCH__)
constexpr int array_size = WP_TILE_BLOCK_DIM +{add};
__shared__ int s[array_size];
auto ptr = &s[0];
return (uint64_t)ptr;
#else
return (uint64_t)0;
#endif
"""
@wp.func_native(snippet)
def get_shared_memory_pointer() -> wp.uint64: ...
return get_shared_memory_pointer
def create_shared_memory_pointer_block_dim_mul_func(
mul: int,
):
"""Create a shared memory pointer whose size scales with the block dimension.
Allocates ``WP_TILE_BLOCK_DIM * mul`` int32 elements of shared memory.
Args:
mul: Multiplier applied to WP_TILE_BLOCK_DIM.
Returns:
A Warp function that returns a pointer to shared memory
"""
snippet = f"""
#if defined(__CUDA_ARCH__)
constexpr int array_size = WP_TILE_BLOCK_DIM * {mul};
__shared__ int s[array_size];
auto ptr = &s[0];
return (uint64_t)ptr;
#else
return (uint64_t)0;
#endif
"""
@wp.func_native(snippet)
def get_shared_memory_pointer() -> wp.uint64: ...
return get_shared_memory_pointer
get_shared_memory_pointer_block_dim_plus_2_ints = create_shared_memory_pointer_block_dim_func(2)
================================================
FILE: newton/_src/geometry/contact_reduction_global.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Global GPU contact reduction using hashtable-based tracking.
This module provides a global contact reduction system that uses a hashtable
to track the best contacts across shape pairs, normal bins, and scan directions.
Unlike the shared-memory based approach in ``contact_reduction.py``, this works
across the entire GPU without block-level synchronization constraints.
**When to Use:**
- Used for mesh-mesh (SDF) collisions where contacts span multiple GPU blocks
- The shared-memory approach in ``contact_reduction.py`` is used for mesh-plane
and mesh-convex where all contacts for a pair fit in one block
**Contact Reduction Strategy:**
The same three-strategy approach as the shared-memory reduction in
``contact_reduction.py``. Slot counts depend on the configuration in that
module (``NUM_NORMAL_BINS``, ``NUM_SPATIAL_DIRECTIONS``,
``NUM_VOXEL_DEPTH_SLOTS``, ``MAX_CONTACTS_PER_PAIR``).
1. **Spatial Extreme Slots** (``NUM_SPATIAL_DIRECTIONS`` per normal bin)
- Builds support polygon boundary for stable stacking
- Only contacts with depth < beta participate
2. **Per-Bin Max-Depth Slots** (1 per normal bin)
- Tracks deepest contact per normal direction
- Critical for gear-like contacts with varied normal orientations
- Participates unconditionally (not gated by beta)
3. **Voxel-Based Depth Slots** (``NUM_VOXEL_DEPTH_SLOTS`` total per pair)
- Tracks deepest contact per mesh-local voxel region
- Ensures early detection of contacts at mesh centers
- Prevents sudden contact jumps between frames
**Implementation Details:**
- Contacts stored in global buffer (struct of arrays: position_depth, normal, shape_pairs)
- Hashtable key: ``(shape_a, shape_b, bin_id)`` where ``bin_id`` is
``0..NUM_NORMAL_BINS-1`` for normal bins and higher indices for voxel groups
- Each normal bin entry has ``NUM_SPATIAL_DIRECTIONS + 1`` value slots
- Voxels are grouped by ``NUM_SPATIAL_DIRECTIONS + 1``:
``bin_id = NUM_NORMAL_BINS + (voxel_idx // group_size)``
- Atomic max on packed (score, contact_id) selects winners
See Also:
``contact_reduction.py`` for shared utility functions, configuration
constants, and detailed algorithm documentation.
"""
from __future__ import annotations
from typing import Any
import warp as wp
from newton._src.geometry.hashtable import (
HASHTABLE_EMPTY_KEY,
HashTable,
hashtable_find_or_insert,
)
from ..utils.heightfield import HeightfieldData, get_triangle_shape_from_heightfield
from .collision_core import (
create_compute_gjk_mpr_contacts,
get_triangle_shape_from_mesh,
)
from .contact_data import ContactData
from .contact_reduction import (
NUM_NORMAL_BINS,
NUM_SPATIAL_DIRECTIONS,
NUM_VOXEL_DEPTH_SLOTS,
compute_voxel_index,
float_flip,
get_slot,
get_spatial_direction_2d,
project_point_to_plane,
)
from .support_function import extract_shape_data
from .types import GeoType
# Fixed beta threshold for contact reduction - small positive value to avoid flickering
# from numerical noise while effectively selecting only near-penetrating contacts for
# the support polygon.
BETA_THRESHOLD = 0.0001 # 0.1mm
VALUES_PER_KEY = NUM_SPATIAL_DIRECTIONS + 1
# Vector type for tracking exported contact IDs (used in export kernels)
exported_ids_vec_type = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.int32)
@wp.func
def is_contact_already_exported(
contact_id: int,
exported_ids: wp.types.vector(length=VALUES_PER_KEY, dtype=wp.int32),
num_exported: int,
) -> bool:
"""Check if a contact_id is already in the exported list.
Args:
contact_id: The contact ID to check
exported_ids: Vector of already exported contact IDs
num_exported: Number of valid entries in exported_ids
Returns:
True if contact_id is already in the list, False otherwise
"""
j = int(0)
while j < num_exported:
if exported_ids[j] == contact_id:
return True
j = j + 1
return False
@wp.func
def compute_effective_radius(shape_type: int, shape_scale: wp.vec4) -> float:
"""Compute effective radius for a shape based on its type.
For shapes that can be represented as Minkowski sums with a sphere (sphere, capsule),
the effective radius is the sphere radius component. For other shapes, it's 0.
Args:
shape_type: The GeoType of the shape
shape_scale: Shape scale data (vec4, xyz are scale components)
Returns:
Effective radius (scale[0] for sphere/capsule, 0 otherwise)
"""
if shape_type == GeoType.SPHERE or shape_type == GeoType.CAPSULE:
return shape_scale[0]
return 0.0
# =============================================================================
# Reduction slot functions (specific to contact reduction)
# =============================================================================
# These functions handle the slot-major value storage used for contact reduction.
# Memory layout is slot-major (SoA) for coalesced GPU access:
# [slot0_entry0, slot0_entry1, ..., slot0_entryN, slot1_entry0, ...]
@wp.func
def reduction_update_slot(
entry_idx: int,
slot_id: int,
value: wp.uint64,
values: wp.array[wp.uint64],
capacity: int,
):
"""Update a reduction slot using atomic max.
Use this after hashtable_find_or_insert() to write multiple values
to the same entry without repeated hash lookups.
Args:
entry_idx: Entry index from hashtable_find_or_insert()
slot_id: Which value slot to write to (0 to values_per_key-1)
value: The uint64 value to max with existing value
values: Values array in slot-major layout
capacity: Hashtable capacity (number of entries)
"""
value_idx = slot_id * capacity + entry_idx
# Check before atomic to reduce contention
if values[value_idx] < value:
wp.atomic_max(values, value_idx, value)
@wp.func
def reduction_insert_slot(
key: wp.uint64,
slot_id: int,
value: wp.uint64,
keys: wp.array[wp.uint64],
values: wp.array[wp.uint64],
active_slots: wp.array[wp.int32],
) -> bool:
"""Insert or update a value in a specific reduction slot.
Convenience function that combines hashtable_find_or_insert()
and reduction_update_slot(). For inserting multiple values to
the same key, prefer using those functions separately.
Args:
key: The uint64 key to insert
slot_id: Which value slot to write to (0 to values_per_key-1)
value: The uint64 value to insert or max with
keys: The hash table keys array (length must be power of two)
values: Values array in slot-major layout
active_slots: Array of size (capacity + 1) tracking active entry indices.
Returns:
True if insertion/update succeeded, False if the table is full
"""
capacity = keys.shape[0]
entry_idx = hashtable_find_or_insert(key, keys, active_slots)
if entry_idx < 0:
return False
reduction_update_slot(entry_idx, slot_id, value, values, capacity)
return True
# =============================================================================
# Contact key/value packing
# =============================================================================
# Bit layout for hashtable key (63 bits used, bit 63 kept 0 for signed/unsigned safety):
# Key is (shape_a, shape_b, bin_id) - NO slot_id (slots are handled via values_per_key)
# - Bits 0-26: shape_a (27 bits, up to ~134M shapes)
# - Bits 27-54: shape_b (28 bits, up to ~268M shapes)
# - Bits 55-62: bin_id (8 bits, 0-255, supports normal bins + voxel groups)
# - Bit 63: unused (kept 0 for signed/unsigned compatibility)
# Total: 63 bits used
SHAPE_A_BITS = wp.constant(wp.uint64(27))
SHAPE_A_MASK = wp.constant(wp.uint64((1 << 27) - 1))
SHAPE_B_BITS = wp.constant(wp.uint64(28))
SHAPE_B_MASK = wp.constant(wp.uint64((1 << 28) - 1))
BIN_BITS = wp.constant(wp.uint64(8))
BIN_MASK = wp.constant(wp.uint64((1 << 8) - 1))
@wp.func
def make_contact_key(shape_a: int, shape_b: int, bin_id: int) -> wp.uint64:
"""Create a hashtable key from shape pair and bin.
Args:
shape_a: First shape index
shape_b: Second shape index
bin_id: Bin index (``0..NUM_NORMAL_BINS-1`` for normal bins, higher for voxel groups)
Returns:
64-bit key for hashtable lookup (only 63 bits used)
"""
key = wp.uint64(shape_a) & SHAPE_A_MASK
key = key | ((wp.uint64(shape_b) & SHAPE_B_MASK) << SHAPE_A_BITS)
# bin_id goes at bits 55-62 (after 27 + 28 = 55 bits for shape IDs)
key = key | ((wp.uint64(bin_id) & BIN_MASK) << wp.uint64(55))
return key
@wp.func
def make_contact_value(score: float, contact_id: int) -> wp.uint64:
"""Pack score and contact_id into hashtable value for atomic max.
High 32 bits: float_flip(score) - makes floats comparable as unsigned ints
Low 32 bits: contact_id - identifies which contact in the buffer
Args:
score: Spatial projection score (higher is better)
contact_id: Index into the contact buffer
Returns:
64-bit value for hashtable (atomic max will select highest score)
"""
return (wp.uint64(float_flip(score)) << wp.uint64(32)) | wp.uint64(contact_id)
@wp.func_native("""
return static_cast(packed & 0xFFFFFFFFull);
""")
def unpack_contact_id(packed: wp.uint64) -> int:
"""Extract contact_id from packed value."""
...
@wp.func
def encode_oct(n: wp.vec3) -> wp.vec2:
"""Encode a unit normal into octahedral 2D representation.
Projects the unit vector onto an octahedron and flattens to 2D.
Near-uniform precision, stable numerics, no trig needed.
"""
l1 = wp.abs(n[0]) + wp.abs(n[1]) + wp.abs(n[2])
if l1 < 1.0e-20:
return wp.vec2(0.0, 0.0)
inv_l1 = 1.0 / l1
ox = n[0] * inv_l1
oy = n[1] * inv_l1
oz = n[2] * inv_l1
if oz < 0.0:
sign_x = 1.0
if ox < 0.0:
sign_x = -1.0
sign_y = 1.0
if oy < 0.0:
sign_y = -1.0
new_x = (1.0 - wp.abs(oy)) * sign_x
new_y = (1.0 - wp.abs(ox)) * sign_y
ox = new_x
oy = new_y
return wp.vec2(ox, oy)
@wp.func
def decode_oct(e: wp.vec2) -> wp.vec3:
"""Decode octahedral 2D representation back to a unit normal.
Inverse of encode_oct. Lossless within float precision.
"""
nz = 1.0 - wp.abs(e[0]) - wp.abs(e[1])
nx = e[0]
ny = e[1]
if nz < 0.0:
sign_x = 1.0
if nx < 0.0:
sign_x = -1.0
sign_y = 1.0
if ny < 0.0:
sign_y = -1.0
new_x = (1.0 - wp.abs(ny)) * sign_x
new_y = (1.0 - wp.abs(nx)) * sign_y
nx = new_x
ny = new_y
return wp.normalize(wp.vec3(nx, ny, nz))
@wp.struct
class GlobalContactReducerData:
"""Struct for passing GlobalContactReducer arrays to kernels.
This struct bundles all the arrays needed for global contact reduction
so they can be passed as a single argument to warp kernels/functions.
"""
# Contact buffer arrays
position_depth: wp.array[wp.vec4]
normal: wp.array[wp.vec2] # Octahedral-encoded unit normal (see encode_oct/decode_oct)
shape_pairs: wp.array[wp.vec2i]
contact_count: wp.array[wp.int32]
capacity: int
# Optional hydroelastic data
# contact_area: area of contact surface element (per contact)
contact_area: wp.array[wp.float32]
# Cached normal-bin hashtable entry index per contact
contact_nbin_entry: wp.array[wp.int32]
# Effective stiffness coefficient k_a*k_b/(k_a+k_b) per hashtable entry
# Constant for a given shape pair, stored once per entry instead of per contact
entry_k_eff: wp.array[wp.float32]
# Aggregate force per hashtable entry (indexed by ht_capacity)
# Used for hydroelastic stiffness calculation: c_stiffness = k_eff * |agg_force| / total_depth
# Accumulates sum(area * depth * normal) for all penetrating contacts per entry
agg_force: wp.array[wp.vec3]
# Weighted position sum per hashtable entry (for anchor contact computation)
# Accumulates sum(area * depth * position) for penetrating contacts
# Divide by weight_sum to get center of pressure (anchor position)
weighted_pos_sum: wp.array[wp.vec3]
# Weight sum per hashtable entry (for anchor contact normalization)
# Accumulates sum(area * depth) for penetrating contacts
weight_sum: wp.array[wp.float32]
# Total depth of reduced (winning) contacts per normal bin entry.
total_depth_reduced: wp.array[wp.float32]
# Total depth-weighted normal of reduced (winning) contacts per normal bin entry.
total_normal_reduced: wp.array[wp.vec3]
# Hashtable arrays
ht_keys: wp.array[wp.uint64]
ht_values: wp.array[wp.uint64]
ht_active_slots: wp.array[wp.int32]
ht_insert_failures: wp.array[wp.int32]
ht_capacity: int
ht_values_per_key: int
@wp.kernel(enable_backward=False)
def _clear_active_kernel(
# Hashtable arrays
ht_keys: wp.array[wp.uint64],
ht_values: wp.array[wp.uint64],
ht_active_slots: wp.array[wp.int32],
# Hydroelastic per-entry arrays
agg_force: wp.array[wp.vec3],
weighted_pos_sum: wp.array[wp.vec3],
weight_sum: wp.array[wp.float32],
entry_k_eff: wp.array[wp.float32],
total_depth_reduced: wp.array[wp.float32],
total_normal_reduced: wp.array[wp.vec3],
agg_moment_unreduced: wp.array[wp.float32],
agg_moment_reduced: wp.array[wp.float32],
agg_moment2_reduced: wp.array[wp.float32],
ht_capacity: int,
values_per_key: int,
num_threads: int,
):
"""Kernel to clear active hashtable entries (keys, values, and hydroelastic aggregates).
Uses grid-stride loop for efficient thread utilization.
Each thread handles one value slot, with key and aggregate clearing done once per entry.
Memory layout for values is slot-major (SoA):
[slot0_entry0, slot0_entry1, ..., slot0_entryN, slot1_entry0, ...]
"""
tid = wp.tid()
# Read count from GPU - stored at active_slots[capacity]
count = ht_active_slots[ht_capacity]
# Total work items: count entries * values_per_key slots per entry
total_work = count * values_per_key
# Grid-stride loop: each thread processes one value slot
i = tid
while i < total_work:
# Compute which entry and which slot within that entry
active_idx = i / values_per_key
local_idx = i % values_per_key
entry_idx = ht_active_slots[active_idx]
# Clear keys and hydroelastic aggregates only once per entry (when processing slot 0)
if local_idx == 0:
ht_keys[entry_idx] = HASHTABLE_EMPTY_KEY
# Clear hydroelastic aggregates if arrays are not empty
if agg_force.shape[0] > 0:
agg_force[entry_idx] = wp.vec3(0.0, 0.0, 0.0)
weighted_pos_sum[entry_idx] = wp.vec3(0.0, 0.0, 0.0)
weight_sum[entry_idx] = 0.0
entry_k_eff[entry_idx] = 0.0
total_depth_reduced[entry_idx] = 0.0
total_normal_reduced[entry_idx] = wp.vec3(0.0, 0.0, 0.0)
if agg_moment_unreduced.shape[0] > 0:
agg_moment_unreduced[entry_idx] = 0.0
agg_moment_reduced[entry_idx] = 0.0
agg_moment2_reduced[entry_idx] = 0.0
# Clear this value slot (slot-major layout)
value_idx = local_idx * ht_capacity + entry_idx
ht_values[value_idx] = wp.uint64(0)
i += num_threads
@wp.kernel(enable_backward=False)
def _zero_count_and_contacts_kernel(
ht_active_slots: wp.array[wp.int32],
contact_count: wp.array[wp.int32],
ht_insert_failures: wp.array[wp.int32],
ht_capacity: int,
):
"""Zero the active slots count and contact count."""
ht_active_slots[ht_capacity] = 0
contact_count[0] = 0
ht_insert_failures[0] = 0
class GlobalContactReducer:
"""Global contact reduction using hashtable-based tracking.
This class manages:
1. A global contact buffer storing contact data (struct of arrays)
2. A hashtable tracking the best contact per (shape_pair, bin, slot)
Slot counts depend on the configuration in ``contact_reduction.py``.
**Hashtable Structure:**
- Key: ``(shape_a, shape_b, bin_id)`` packed into 64 bits
- bin_id ``0..NUM_NORMAL_BINS-1``: Normal bins (polyhedron faces)
- Higher bin_ids: Voxel groups
**Slot Layout per Normal Bin Entry** (``NUM_SPATIAL_DIRECTIONS + 1`` slots):
- Slots ``0..NUM_SPATIAL_DIRECTIONS-1``: Spatial direction extremes (depth < beta)
- Last slot: Maximum depth contact for the bin (unconditional)
**Slot Layout per Voxel Group Entry** (``NUM_SPATIAL_DIRECTIONS + 1`` slots):
- Each slot tracks the deepest contact for one voxel in the group
- ``bin_id = NUM_NORMAL_BINS + (voxel_idx // group_size)``
**Contact Data Storage:**
Packed for efficient memory access:
- position_depth: vec4(position.x, position.y, position.z, depth)
- normal: vec2(octahedral-encoded unit normal)
- shape_pairs: vec2i(shape_a, shape_b)
- contact_area: float (optional, per contact, for hydroelastic contacts)
Attributes:
capacity: Maximum number of contacts that can be stored
values_per_key: Number of value slots per hashtable entry (``NUM_SPATIAL_DIRECTIONS + 1``)
position_depth: vec4 array storing position.xyz and depth
normal: vec2 array storing octahedral-encoded contact normal
shape_pairs: vec2i array storing (shape_a, shape_b) per contact
contact_area: float array storing contact area per contact (for hydroelastic)
entry_k_eff: float array storing effective stiffness per hashtable entry (for hydroelastic)
contact_count: Atomic counter for allocated contacts
hashtable: HashTable for tracking best contacts (keys only)
ht_values: Values array for hashtable (managed here, not by HashTable)
"""
def __init__(
self,
capacity: int,
device: str | None = None,
store_hydroelastic_data: bool = False,
store_moment_data: bool = False,
):
"""Initialize the global contact reducer.
Args:
capacity: Maximum number of contacts to store
device: Warp device (e.g., "cuda:0", "cpu")
store_hydroelastic_data: If True, allocate arrays for contact_area and entry_k_eff
store_moment_data: If True, allocate moment accumulator arrays for friction
moment matching. Only needed when ``moment_matching=True``.
"""
self.capacity = capacity
self.device = device
self.store_hydroelastic_data = store_hydroelastic_data
self.values_per_key = NUM_SPATIAL_DIRECTIONS + 1
# Contact buffer (struct of arrays)
self.position_depth = wp.zeros(capacity, dtype=wp.vec4, device=device)
self.normal = wp.zeros(capacity, dtype=wp.vec2, device=device) # Octahedral-encoded normals
self.shape_pairs = wp.zeros(capacity, dtype=wp.vec2i, device=device)
# Optional hydroelastic data arrays
if store_hydroelastic_data:
self.contact_area = wp.zeros(capacity, dtype=wp.float32, device=device)
self.contact_nbin_entry = wp.zeros(capacity, dtype=wp.int32, device=device)
else:
self.contact_area = wp.zeros(0, dtype=wp.float32, device=device)
self.contact_nbin_entry = wp.zeros(0, dtype=wp.int32, device=device)
# Per-contact dedup flags for cross-entry deduplication during export
self.exported_flags = wp.zeros(capacity, dtype=wp.int32, device=device)
# Atomic counter for contact allocation
self.contact_count = wp.zeros(1, dtype=wp.int32, device=device)
# Count failed hashtable inserts (e.g., table full)
self.ht_insert_failures = wp.zeros(1, dtype=wp.int32, device=device)
# Hashtable sizing: estimate unique (shape_pair, bin) keys needed
# - NUM_NORMAL_BINS + ceil(NUM_VOXEL_DEPTH_SLOTS / values_per_key) bins per pair
# - Dense hydroelastic contacts: many contacts share the same bin
# - Assume ~8 contacts per unique key on average (conservative for dense contacts)
# - Provides 2x load factor headroom within the /4 estimate
# - If table fills, contacts gracefully skip reduction (still in buffer)
hashtable_size = max(capacity // 4, 1024) # minimum 1024 for small scenes
self.hashtable = HashTable(hashtable_size, device=device)
# Values array for hashtable - managed here, not by HashTable
# This is contact-reduction-specific (slot-major layout with values_per_key slots)
self.ht_values = wp.zeros(self.hashtable.capacity * self.values_per_key, dtype=wp.uint64, device=device)
# Aggregate force per hashtable entry (for hydroelastic stiffness calculation)
# Accumulates sum(area * depth * normal) for all penetrating contacts per entry
if store_hydroelastic_data:
self.agg_force = wp.zeros(self.hashtable.capacity, dtype=wp.vec3, device=device)
self.weighted_pos_sum = wp.zeros(self.hashtable.capacity, dtype=wp.vec3, device=device)
self.weight_sum = wp.zeros(self.hashtable.capacity, dtype=wp.float32, device=device)
# k_eff per entry (constant per shape pair, set once on first insert)
self.entry_k_eff = wp.zeros(self.hashtable.capacity, dtype=wp.float32, device=device)
# Total depth of reduced contacts per normal bin (accumulated from all winning contacts)
self.total_depth_reduced = wp.zeros(self.hashtable.capacity, dtype=wp.float32, device=device)
# Total depth-weighted normal of reduced contacts per normal bin
self.total_normal_reduced = wp.zeros(self.hashtable.capacity, dtype=wp.vec3, device=device)
# Moment accumulators for moment matching (friction scale adjustment)
if store_moment_data:
self.agg_moment_unreduced = wp.zeros(self.hashtable.capacity, dtype=wp.float32, device=device)
self.agg_moment_reduced = wp.zeros(self.hashtable.capacity, dtype=wp.float32, device=device)
self.agg_moment2_reduced = wp.zeros(self.hashtable.capacity, dtype=wp.float32, device=device)
else:
self.agg_moment_unreduced = wp.zeros(0, dtype=wp.float32, device=device)
self.agg_moment_reduced = wp.zeros(0, dtype=wp.float32, device=device)
self.agg_moment2_reduced = wp.zeros(0, dtype=wp.float32, device=device)
else:
self.agg_force = wp.zeros(0, dtype=wp.vec3, device=device)
self.weighted_pos_sum = wp.zeros(0, dtype=wp.vec3, device=device)
self.weight_sum = wp.zeros(0, dtype=wp.float32, device=device)
self.entry_k_eff = wp.zeros(0, dtype=wp.float32, device=device)
self.total_depth_reduced = wp.zeros(0, dtype=wp.float32, device=device)
self.total_normal_reduced = wp.zeros(0, dtype=wp.vec3, device=device)
self.agg_moment_unreduced = wp.zeros(0, dtype=wp.float32, device=device)
self.agg_moment_reduced = wp.zeros(0, dtype=wp.float32, device=device)
self.agg_moment2_reduced = wp.zeros(0, dtype=wp.float32, device=device)
def clear(self):
"""Clear all contacts and reset the reducer (full clear)."""
self.contact_count.zero_()
self.ht_insert_failures.zero_()
self.hashtable.clear()
self.ht_values.zero_()
def clear_active(self):
"""Clear only the active entries (efficient for sparse usage).
Uses a combined kernel that clears both hashtable keys, values, and aggregate force,
followed by a small kernel to zero the counters.
"""
# Use fixed thread count for efficient GPU utilization
num_threads = min(1024, self.hashtable.capacity)
# Single kernel clears keys, values, and hydroelastic aggregates for active entries
wp.launch(
_clear_active_kernel,
dim=num_threads,
inputs=[
self.hashtable.keys,
self.ht_values,
self.hashtable.active_slots,
self.agg_force,
self.weighted_pos_sum,
self.weight_sum,
self.entry_k_eff,
self.total_depth_reduced,
self.total_normal_reduced,
self.agg_moment_unreduced,
self.agg_moment_reduced,
self.agg_moment2_reduced,
self.hashtable.capacity,
self.values_per_key,
num_threads,
],
device=self.device,
)
# Zero the counts in a separate kernel
wp.launch(
_zero_count_and_contacts_kernel,
dim=1,
inputs=[
self.hashtable.active_slots,
self.contact_count,
self.ht_insert_failures,
self.hashtable.capacity,
],
device=self.device,
)
def get_data_struct(self) -> GlobalContactReducerData:
"""Get a GlobalContactReducerData struct for passing to kernels.
Returns:
A GlobalContactReducerData struct containing all arrays.
"""
data = GlobalContactReducerData()
data.position_depth = self.position_depth
data.normal = self.normal
data.shape_pairs = self.shape_pairs
data.contact_count = self.contact_count
data.capacity = self.capacity
data.contact_area = self.contact_area
data.contact_nbin_entry = self.contact_nbin_entry
data.entry_k_eff = self.entry_k_eff
data.agg_force = self.agg_force
data.weighted_pos_sum = self.weighted_pos_sum
data.weight_sum = self.weight_sum
data.total_depth_reduced = self.total_depth_reduced
data.total_normal_reduced = self.total_normal_reduced
data.ht_keys = self.hashtable.keys
data.ht_values = self.ht_values
data.ht_active_slots = self.hashtable.active_slots
data.ht_insert_failures = self.ht_insert_failures
data.ht_capacity = self.hashtable.capacity
data.ht_values_per_key = self.values_per_key
return data
@wp.func
def export_contact_to_buffer(
shape_a: int,
shape_b: int,
position: wp.vec3,
normal: wp.vec3,
depth: float,
reducer_data: GlobalContactReducerData,
) -> int:
"""Store a contact in the buffer without reduction.
Args:
shape_a: First shape index
shape_b: Second shape index
position: Contact position in world space
normal: Contact normal
depth: Penetration depth (negative = penetrating)
reducer_data: GlobalContactReducerData with all arrays
Returns:
Contact ID if successfully stored, -1 if buffer full
"""
# Allocate contact slot. On overflow, contact_count keeps incrementing
# past capacity so (contact_count - capacity) gives the drop count.
contact_id = wp.atomic_add(reducer_data.contact_count, 0, 1)
if contact_id >= reducer_data.capacity:
return -1
# Store contact data (packed into vec4, normal octahedral-encoded into vec2)
reducer_data.position_depth[contact_id] = wp.vec4(position[0], position[1], position[2], depth)
reducer_data.normal[contact_id] = encode_oct(normal)
reducer_data.shape_pairs[contact_id] = wp.vec2i(shape_a, shape_b)
return contact_id
@wp.func
def reduce_contact_in_hashtable(
contact_id: int,
reducer_data: GlobalContactReducerData,
beta: float,
shape_transform: wp.array[wp.transform],
shape_collision_aabb_lower: wp.array[wp.vec3],
shape_collision_aabb_upper: wp.array[wp.vec3],
shape_voxel_resolution: wp.array[wp.vec3i],
):
"""Register a buffered contact in the reduction hashtable.
Uses single beta threshold for contact reduction with two strategies:
1. **Normal-binned slots** (``NUM_NORMAL_BINS`` x ``NUM_SPATIAL_DIRECTIONS + 1``):
- Spatial direction slots for contacts with depth < beta
- 1 max-depth slot per normal bin (always participates)
2. **Voxel-based depth slots** (``NUM_VOXEL_DEPTH_SLOTS`` voxels, grouped):
- Voxels are grouped by ``NUM_SPATIAL_DIRECTIONS + 1``
- Each slot tracks the deepest contact in that voxel region
- Provides spatial coverage independent of contact normal
Args:
contact_id: Index of contact in buffer
reducer_data: Reducer data
beta: Depth threshold (contacts with depth < beta participate in spatial competition)
shape_transform: Per-shape world transforms (for transforming position to local space)
shape_collision_aabb_lower: Per-shape local AABB lower bounds
shape_collision_aabb_upper: Per-shape local AABB upper bounds
shape_voxel_resolution: Per-shape voxel grid resolution
"""
# Read contact data from buffer (normal is octahedral-encoded)
pd = reducer_data.position_depth[contact_id]
normal = decode_oct(reducer_data.normal[contact_id])
pair = reducer_data.shape_pairs[contact_id]
position = wp.vec3(pd[0], pd[1], pd[2])
depth = pd[3]
shape_a = pair[0] # Mesh shape
shape_b = pair[1] # Convex shape
aabb_lower = shape_collision_aabb_lower[shape_a]
aabb_upper = shape_collision_aabb_upper[shape_a]
ht_capacity = reducer_data.ht_capacity
# === Part 1: Normal-binned reduction (spatial extremes + max-depth per bin) ===
# Get normal bin from polyhedron face matching
bin_id = get_slot(normal)
# Project position to 2D plane of the polyhedron face
pos_2d = project_point_to_plane(bin_id, position)
# Key is (shape_a, shape_b, bin_id)
key = make_contact_key(shape_a, shape_b, bin_id)
# Find or create the hashtable entry ONCE, then write directly to slots
entry_idx = hashtable_find_or_insert(key, reducer_data.ht_keys, reducer_data.ht_active_slots)
if entry_idx >= 0:
use_beta = depth < beta * wp.length(aabb_upper - aabb_lower)
for dir_i in range(wp.static(NUM_SPATIAL_DIRECTIONS)):
if use_beta:
dir_2d = get_spatial_direction_2d(dir_i)
score = wp.dot(pos_2d, dir_2d)
value = make_contact_value(score, contact_id)
slot_id = dir_i
reduction_update_slot(entry_idx, slot_id, value, reducer_data.ht_values, ht_capacity)
max_depth_value = make_contact_value(-depth, contact_id)
reduction_update_slot(
entry_idx, wp.static(NUM_SPATIAL_DIRECTIONS), max_depth_value, reducer_data.ht_values, ht_capacity
)
else:
wp.atomic_add(reducer_data.ht_insert_failures, 0, 1)
# === Part 2: Voxel-based reduction (deepest contact per voxel) ===
# Transform contact position from world space to shape_a's local space
X_shape_ws = shape_transform[shape_a]
X_ws_shape = wp.transform_inverse(X_shape_ws)
position_local = wp.transform_point(X_ws_shape, position)
# Compute voxel index using shape_a's local AABB
voxel_res = shape_voxel_resolution[shape_a]
voxel_idx = compute_voxel_index(position_local, aabb_lower, aabb_upper, voxel_res)
# Clamp voxel index to valid range
voxel_idx = wp.clamp(voxel_idx, 0, wp.static(NUM_VOXEL_DEPTH_SLOTS - 1))
voxels_per_group = wp.static(NUM_SPATIAL_DIRECTIONS + 1)
voxel_group = voxel_idx // voxels_per_group
voxel_local_slot = voxel_idx % voxels_per_group
voxel_bin_id = wp.static(NUM_NORMAL_BINS) + voxel_group
voxel_key = make_contact_key(shape_a, shape_b, voxel_bin_id)
voxel_entry_idx = hashtable_find_or_insert(voxel_key, reducer_data.ht_keys, reducer_data.ht_active_slots)
if voxel_entry_idx >= 0:
# Use -depth so atomic_max selects most penetrating (most negative depth)
voxel_value = make_contact_value(-depth, contact_id)
reduction_update_slot(voxel_entry_idx, voxel_local_slot, voxel_value, reducer_data.ht_values, ht_capacity)
else:
wp.atomic_add(reducer_data.ht_insert_failures, 0, 1)
@wp.func
def export_and_reduce_contact(
shape_a: int,
shape_b: int,
position: wp.vec3,
normal: wp.vec3,
depth: float,
reducer_data: GlobalContactReducerData,
beta: float,
shape_transform: wp.array[wp.transform],
shape_collision_aabb_lower: wp.array[wp.vec3],
shape_collision_aabb_upper: wp.array[wp.vec3],
shape_voxel_resolution: wp.array[wp.vec3i],
) -> int:
"""Export contact to buffer and register in hashtable for reduction."""
contact_id = export_contact_to_buffer(shape_a, shape_b, position, normal, depth, reducer_data)
if contact_id >= 0:
reduce_contact_in_hashtable(
contact_id,
reducer_data,
beta,
shape_transform,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
)
return contact_id
@wp.func
def export_and_reduce_contact_centered(
shape_a: int,
shape_b: int,
position: wp.vec3,
normal: wp.vec3,
depth: float,
centered_position: wp.vec3,
X_ws_voxel_shape: wp.transform,
aabb_lower_voxel: wp.vec3,
aabb_upper_voxel: wp.vec3,
voxel_res: wp.vec3i,
reducer_data: GlobalContactReducerData,
) -> int:
"""Export contact to buffer and register in hashtable matching thread-block behavior.
Differs from :func:`export_and_reduce_contact` in three ways that match
the shared-memory reduction used for mesh-mesh contacts:
- Spatial projection uses *centered_position* (midpoint-centered)
- Beta threshold is fixed at 0.0001 m (not scale-relative)
- Voxel grid uses the caller-specified AABB/transform (tri_shape's)
Pre-prunes contacts by non-atomically reading current slot values before
allocating a buffer slot. Contacts that cannot beat any existing winner
are skipped entirely, reducing buffer pressure.
Args:
shape_a: First shape index (for hashtable key)
shape_b: Second shape index (for hashtable key)
position: World-space contact position (stored in buffer)
normal: Contact normal (a-to-b)
depth: Penetration depth
centered_position: Midpoint-centered position for spatial projection
X_ws_voxel_shape: World-to-local transform for voxel computation
aabb_lower_voxel: Local AABB lower for voxel grid
aabb_upper_voxel: Local AABB upper for voxel grid
voxel_res: Voxel grid resolution
reducer_data: Global reducer data
"""
ht_capacity = reducer_data.ht_capacity
use_beta = depth < wp.static(BETA_THRESHOLD)
# === Normal bin: find/create hashtable entry ===
bin_id = get_slot(normal)
pos_2d = project_point_to_plane(bin_id, centered_position)
key = make_contact_key(shape_a, shape_b, bin_id)
entry_idx = hashtable_find_or_insert(key, reducer_data.ht_keys, reducer_data.ht_active_slots)
# === Pre-prune normal bin: non-atomic reads ===
might_win = False
if entry_idx >= 0:
# Check max-depth slot first (cheapest — no direction computation)
max_depth_probe = make_contact_value(-depth, 0)
if reducer_data.ht_values[wp.static(NUM_SPATIAL_DIRECTIONS) * ht_capacity + entry_idx] < max_depth_probe:
might_win = True
if not might_win and use_beta:
for dir_i in range(wp.static(NUM_SPATIAL_DIRECTIONS)):
if not might_win:
dir_2d = get_spatial_direction_2d(dir_i)
score = wp.dot(pos_2d, dir_2d)
probe = make_contact_value(score, 0)
if reducer_data.ht_values[dir_i * ht_capacity + entry_idx] < probe:
might_win = True
# === Voxel bin: only look up if normal bin didn't already show a win ===
position_local = wp.transform_point(X_ws_voxel_shape, position)
voxel_idx = compute_voxel_index(position_local, aabb_lower_voxel, aabb_upper_voxel, voxel_res)
voxel_idx = wp.clamp(voxel_idx, 0, wp.static(NUM_VOXEL_DEPTH_SLOTS - 1))
voxels_per_group = wp.static(NUM_SPATIAL_DIRECTIONS + 1)
voxel_group = voxel_idx // voxels_per_group
voxel_local_slot = voxel_idx % voxels_per_group
voxel_bin_id = wp.static(NUM_NORMAL_BINS) + voxel_group
voxel_key = make_contact_key(shape_a, shape_b, voxel_bin_id)
voxel_entry_idx = -1
if not might_win:
voxel_entry_idx = hashtable_find_or_insert(voxel_key, reducer_data.ht_keys, reducer_data.ht_active_slots)
if voxel_entry_idx >= 0:
voxel_probe = make_contact_value(-depth, 0)
if reducer_data.ht_values[voxel_local_slot * ht_capacity + voxel_entry_idx] < voxel_probe:
might_win = True
if not might_win:
return -1
# === Allocate buffer slot (only for contacts that might win) ===
contact_id = export_contact_to_buffer(shape_a, shape_b, position, normal, depth, reducer_data)
if contact_id < 0:
return -1
# === Register in hashtable with real contact_id ===
if entry_idx >= 0:
for dir_i in range(wp.static(NUM_SPATIAL_DIRECTIONS)):
if use_beta:
dir_2d = get_spatial_direction_2d(dir_i)
score = wp.dot(pos_2d, dir_2d)
value = make_contact_value(score, contact_id)
reduction_update_slot(entry_idx, dir_i, value, reducer_data.ht_values, ht_capacity)
max_depth_value = make_contact_value(-depth, contact_id)
reduction_update_slot(
entry_idx, wp.static(NUM_SPATIAL_DIRECTIONS), max_depth_value, reducer_data.ht_values, ht_capacity
)
else:
wp.atomic_add(reducer_data.ht_insert_failures, 0, 1)
# Deferred voxel entry lookup for contacts that won via normal bin
if voxel_entry_idx < 0:
voxel_entry_idx = hashtable_find_or_insert(voxel_key, reducer_data.ht_keys, reducer_data.ht_active_slots)
if voxel_entry_idx >= 0:
voxel_value = make_contact_value(-depth, contact_id)
reduction_update_slot(voxel_entry_idx, voxel_local_slot, voxel_value, reducer_data.ht_values, ht_capacity)
else:
wp.atomic_add(reducer_data.ht_insert_failures, 0, 1)
return contact_id
@wp.kernel(enable_backward=False)
def reduce_buffered_contacts_kernel(
reducer_data: GlobalContactReducerData,
shape_transform: wp.array[wp.transform],
shape_collision_aabb_lower: wp.array[wp.vec3],
shape_collision_aabb_upper: wp.array[wp.vec3],
shape_voxel_resolution: wp.array[wp.vec3i],
total_num_threads: int,
):
"""Register buffered contacts in the hashtable for reduction.
Uses the fixed BETA_THRESHOLD (0.1mm) for spatial competition.
Contacts with depth < beta participate in spatial extreme competition.
"""
tid = wp.tid()
# Get total number of contacts written
num_contacts = reducer_data.contact_count[0]
# Early exit if no contacts (fast path for empty work)
if num_contacts == 0:
return
# Cap at capacity
num_contacts = wp.min(num_contacts, reducer_data.capacity)
# Grid stride loop over contacts
for i in range(tid, num_contacts, total_num_threads):
reduce_contact_in_hashtable(
i,
reducer_data,
wp.static(BETA_THRESHOLD),
shape_transform,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
)
# =============================================================================
# Helper functions for contact unpacking and writing
# =============================================================================
@wp.func
def unpack_contact(
contact_id: int,
position_depth: wp.array[wp.vec4],
normal: wp.array[wp.vec2],
):
"""Unpack contact data from the buffer.
Normal is stored as octahedral-encoded vec2 and decoded back to vec3.
Args:
contact_id: Index into the contact buffer
position_depth: Contact buffer for position.xyz + depth
normal: Contact buffer for octahedral-encoded normal
Returns:
Tuple of (position, normal, depth)
"""
pd = position_depth[contact_id]
n = decode_oct(normal[contact_id])
position = wp.vec3(pd[0], pd[1], pd[2])
depth = pd[3]
return position, n, depth
@wp.func
def write_contact_to_reducer(
contact_data: Any, # ContactData struct
reducer_data: GlobalContactReducerData,
output_index: int, # Unused, kept for API compatibility with write_contact_simple
):
"""Writer function that stores contacts in GlobalContactReducer for reduction.
This follows the same signature as write_contact_simple in narrow_phase.py,
so it can be used with create_compute_gjk_mpr_contacts and other contact
generation functions.
Note: Beta threshold is applied later in create_reduce_buffered_contacts_kernel,
not at write time. This reduces register pressure on contact generation kernels.
Args:
contact_data: ContactData struct from contact computation
reducer_data: GlobalContactReducerData struct with all reducer arrays
output_index: Unused, kept for API compatibility
"""
# Extract contact info from ContactData
position = contact_data.contact_point_center
normal = contact_data.contact_normal_a_to_b
depth = contact_data.contact_distance
shape_a = contact_data.shape_a
shape_b = contact_data.shape_b
# Store contact ONLY (registration to hashtable happens in a separate kernel)
# This reduces register pressure on the contact generation kernel
export_contact_to_buffer(
shape_a=shape_a,
shape_b=shape_b,
position=position,
normal=normal,
depth=depth,
reducer_data=reducer_data,
)
def create_export_reduced_contacts_kernel(writer_func: Any):
"""Create a kernel that exports reduced contacts using a custom writer function.
The kernel processes one hashtable ENTRY per thread (not one value slot).
Each entry has VALUES_PER_KEY value slots (``NUM_SPATIAL_DIRECTIONS`` spatial + 1 max-depth).
The thread reads all slots, collects unique contact IDs, and exports each
unique contact once.
This naturally deduplicates: one thread handles one (shape_pair, bin) entry
and can locally track which contact IDs it has already exported.
Args:
writer_func: A warp function with signature (ContactData, writer_data, int) -> None.
The third argument is an output_index (-1 indicates the writer should allocate
a new slot). This follows the same pattern as narrow_phase.py's write_contact_simple.
Returns:
A warp kernel that can be launched to export reduced contacts.
"""
# Define vector type for tracking exported contact IDs
exported_ids_vec = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.int32)
@wp.kernel(enable_backward=False, module="unique")
def export_reduced_contacts_kernel(
# Hashtable arrays
ht_keys: wp.array[wp.uint64],
ht_values: wp.array[wp.uint64],
ht_active_slots: wp.array[wp.int32],
# Contact buffer arrays
position_depth: wp.array[wp.vec4],
normal: wp.array[wp.vec2], # Octahedral-encoded
shape_pairs: wp.array[wp.vec2i],
# Global dedup flags: one int per buffer contact, for cross-entry deduplication
exported_flags: wp.array[wp.int32],
# Shape data for extracting margin and effective radius
shape_types: wp.array[int],
shape_data: wp.array[wp.vec4],
# Per-shape contact gaps
shape_gap: wp.array[float],
# Writer data (custom struct)
writer_data: Any,
# Grid stride parameters
total_num_threads: int,
):
"""Export reduced contacts to the writer.
Uses grid stride loop to iterate over active hashtable ENTRIES.
For each entry, reads all value slots, collects unique contact IDs,
and exports each unique contact once. Uses atomic flags per contact_id
for cross-entry deduplication (same contact winning multiple entries).
"""
tid = wp.tid()
# Get number of active entries (stored at index = ht_capacity)
ht_capacity = ht_keys.shape[0]
num_active = ht_active_slots[ht_capacity]
# Early exit if no active entries (fast path for empty work)
if num_active == 0:
return
# Grid stride loop over active entries
for i in range(tid, num_active, total_num_threads):
# Get the hashtable entry index
entry_idx = ht_active_slots[i]
# Track exported contact IDs for this entry (intra-entry dedup)
exported_ids = exported_ids_vec()
num_exported = int(0)
# Read all value slots for this entry (slot-major layout)
for slot in range(wp.static(VALUES_PER_KEY)):
value = ht_values[slot * ht_capacity + entry_idx]
# Skip empty slots (value = 0)
if value == wp.uint64(0):
continue
# Extract contact ID from low 32 bits
contact_id = unpack_contact_id(value)
# Skip if already exported within this entry
if is_contact_already_exported(contact_id, exported_ids, num_exported):
continue
# Record this contact ID for intra-entry dedup
exported_ids[num_exported] = contact_id
num_exported = num_exported + 1
# Cross-entry dedup: same contact can win slots in different entries
# (e.g., normal-bin AND voxel entry). Atomic flag per contact_id.
old_flag = wp.atomic_add(exported_flags, contact_id, 1)
if old_flag > 0:
continue
# Unpack contact data
position, contact_normal, depth = unpack_contact(contact_id, position_depth, normal)
# Get shape pair
pair = shape_pairs[contact_id]
shape_a = pair[0]
shape_b = pair[1]
# Extract margin offsets from shape_data (stored in w component)
margin_offset_a = shape_data[shape_a][3]
margin_offset_b = shape_data[shape_b][3]
# Compute effective radius for spheres, capsules, and cones
radius_eff_a = compute_effective_radius(shape_types[shape_a], shape_data[shape_a])
radius_eff_b = compute_effective_radius(shape_types[shape_b], shape_data[shape_b])
# Use additive per-shape contact gap (matching broad/narrow phase)
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
gap_sum = gap_a + gap_b
# Create ContactData struct
contact_data = ContactData()
contact_data.contact_point_center = position
contact_data.contact_normal_a_to_b = contact_normal
contact_data.contact_distance = depth
contact_data.radius_eff_a = radius_eff_a
contact_data.radius_eff_b = radius_eff_b
contact_data.margin_a = margin_offset_a
contact_data.margin_b = margin_offset_b
contact_data.shape_a = shape_a
contact_data.shape_b = shape_b
contact_data.gap_sum = gap_sum
# Call the writer function
writer_func(contact_data, writer_data, -1)
return export_reduced_contacts_kernel
@wp.kernel(enable_backward=False, module="unique")
def mesh_triangle_contacts_to_reducer_kernel(
shape_types: wp.array[int],
shape_data: wp.array[wp.vec4],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
shape_gap: wp.array[float],
shape_heightfield_index: wp.array[wp.int32],
heightfield_data: wp.array[HeightfieldData],
heightfield_elevations: wp.array[wp.float32],
triangle_pairs: wp.array[wp.vec3i],
triangle_pairs_count: wp.array[int],
reducer_data: GlobalContactReducerData,
total_num_threads: int,
):
"""Process mesh/heightfield-triangle contacts and store them in GlobalContactReducer.
This kernel processes triangle pairs (mesh-or-hfield shape, convex-shape, triangle_index)
and computes contacts using GJK/MPR, storing results in the GlobalContactReducer for
subsequent reduction and export.
Uses grid stride loop over triangle pairs.
"""
tid = wp.tid()
num_triangle_pairs = triangle_pairs_count[0]
for i in range(tid, num_triangle_pairs, total_num_threads):
if i >= triangle_pairs.shape[0]:
break
triple = triangle_pairs[i]
shape_a = triple[0] # Mesh or heightfield shape
shape_b = triple[1] # Convex shape
tri_idx = triple[2]
type_a = shape_types[shape_a]
if type_a == GeoType.HFIELD:
# Heightfield triangle
hfd = heightfield_data[shape_heightfield_index[shape_a]]
X_ws_a = shape_transform[shape_a]
shape_data_a, v0_world = get_triangle_shape_from_heightfield(hfd, heightfield_elevations, X_ws_a, tri_idx)
else:
# Mesh triangle (mesh_id already validated by midphase)
mesh_id_a = shape_source[shape_a]
scale_data_a = shape_data[shape_a]
mesh_scale_a = wp.vec3(scale_data_a[0], scale_data_a[1], scale_data_a[2])
X_ws_a = shape_transform[shape_a]
shape_data_a, v0_world = get_triangle_shape_from_mesh(mesh_id_a, mesh_scale_a, X_ws_a, tri_idx)
# Extract shape B data
pos_b, quat_b, shape_data_b, _scale_b, margin_offset_b = extract_shape_data(
shape_b,
shape_transform,
shape_types,
shape_data,
shape_source,
)
# Set pos_a to be vertex A (origin of triangle in local frame)
pos_a = v0_world
quat_a = wp.quat_identity() # Triangle has no orientation
# Extract margin offset for shape A (signed distance padding)
margin_offset_a = shape_data[shape_a][3]
# Use additive per-shape contact gap for detection threshold
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
gap_sum = gap_a + gap_b
# Compute and write contacts using GJK/MPR
wp.static(create_compute_gjk_mpr_contacts(write_contact_to_reducer))(
shape_data_a,
shape_data_b,
quat_a,
quat_b,
pos_a,
pos_b,
gap_sum,
shape_a,
shape_b,
margin_offset_a,
margin_offset_b,
reducer_data,
)
================================================
FILE: newton/_src/geometry/contact_reduction_hydroelastic.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Hydroelastic contact reduction using hashtable-based tracking.
This module provides hydroelastic-specific contact reduction functionality,
building on the core ``GlobalContactReducer`` from ``contact_reduction_global.py``.
**Hydroelastic Contact Features:**
- Aggregate stiffness calculation: ``c_stiffness = k_eff * |agg_force| / total_depth``
- Normal matching: rotates reduced normals to align with aggregate force direction
- Anchor contact: synthetic contact at center of pressure for moment balance
**Usage:**
Use ``HydroelasticContactReduction`` for the high-level API, or call the individual
kernels for more control over the pipeline.
See Also:
:class:`GlobalContactReducer` in ``contact_reduction_global.py`` for the
core contact reduction system.
"""
from __future__ import annotations
from dataclasses import dataclass
from typing import Any
import warp as wp
from newton._src.geometry.hashtable import hashtable_find_or_insert
from .contact_data import ContactData
from .contact_reduction import (
NUM_NORMAL_BINS,
NUM_SPATIAL_DIRECTIONS,
NUM_VOXEL_DEPTH_SLOTS,
compute_voxel_index,
get_slot,
get_spatial_direction_2d,
project_point_to_plane,
)
from .contact_reduction_global import (
BETA_THRESHOLD,
BIN_MASK,
VALUES_PER_KEY,
GlobalContactReducer,
GlobalContactReducerData,
decode_oct,
export_contact_to_buffer,
is_contact_already_exported,
make_contact_key,
make_contact_value,
reduction_update_slot,
unpack_contact_id,
)
# =============================================================================
# Constants for hydroelastic export
# =============================================================================
EPS_LARGE = 1e-8
EPS_SMALL = 1e-20
MIN_FRICTION_SCALE = 1e-2
@wp.func
def _compute_normal_matching_rotation(
selected_normal_sum: wp.vec3,
agg_force_vec: wp.vec3,
agg_force_mag: wp.float32,
) -> wp.quat:
"""Compute rotation quaternion that aligns selected_normal_sum with agg_force direction."""
rotation_q = wp.quat_identity()
selected_mag = wp.length(selected_normal_sum)
if selected_mag > EPS_LARGE and agg_force_mag > EPS_LARGE:
selected_dir = selected_normal_sum / selected_mag
agg_dir = agg_force_vec / agg_force_mag
cross = wp.cross(selected_dir, agg_dir)
cross_mag = wp.length(cross)
dot_val = wp.dot(selected_dir, agg_dir)
if cross_mag > EPS_LARGE:
axis = cross / cross_mag
angle = wp.acos(wp.clamp(dot_val, -1.0, 1.0))
rotation_q = wp.quat_from_axis_angle(axis, angle)
elif dot_val < 0.0:
perp = wp.vec3(1.0, 0.0, 0.0)
if wp.abs(wp.dot(selected_dir, perp)) > 0.9:
perp = wp.vec3(0.0, 1.0, 0.0)
axis = wp.normalize(wp.cross(selected_dir, perp))
rotation_q = wp.quat_from_axis_angle(axis, 3.14159265359)
return rotation_q
@wp.func
def _effective_stiffness(k_a: wp.float32, k_b: wp.float32) -> wp.float32:
denom = k_a + k_b
if denom <= 0.0:
return 0.0
return (k_a * k_b) / denom
# =============================================================================
# Hydroelastic contact buffer function
# =============================================================================
@wp.func
def export_hydroelastic_contact_to_buffer(
shape_a: int,
shape_b: int,
position: wp.vec3,
normal: wp.vec3,
depth: float,
area: float,
k_eff: float,
reducer_data: GlobalContactReducerData,
) -> int:
"""Store a hydroelastic contact in the buffer with area and stiffness.
Extends :func:`export_contact_to_buffer` by storing additional hydroelastic
data (area and effective stiffness).
Args:
shape_a: First shape index
shape_b: Second shape index
position: Contact position in world space
normal: Contact normal
depth: Penetration depth (negative = penetrating, standard convention)
area: Contact surface area
k_eff: Effective stiffness coefficient k_a*k_b/(k_a+k_b)
reducer_data: GlobalContactReducerData with all arrays
Returns:
Contact ID if successfully stored, -1 if buffer full
"""
# Use base function to store common contact data
contact_id = export_contact_to_buffer(shape_a, shape_b, position, normal, depth, reducer_data)
if contact_id >= 0:
# Store hydroelastic-specific data (k_eff is stored per entry, not per contact)
reducer_data.contact_area[contact_id] = area
return contact_id
# =============================================================================
# Hydroelastic reduction kernels
# =============================================================================
def get_reduce_hydroelastic_contacts_kernel():
"""Create a hydroelastic contact reduction kernel.
Returns:
A Warp kernel that registers buffered contacts in the hashtable.
"""
@wp.kernel(enable_backward=False)
def reduce_hydroelastic_contacts_kernel(
reducer_data: GlobalContactReducerData,
shape_material_k_hydro: wp.array[wp.float32],
shape_transform: wp.array[wp.transform],
shape_collision_aabb_lower: wp.array[wp.vec3],
shape_collision_aabb_upper: wp.array[wp.vec3],
shape_voxel_resolution: wp.array[wp.vec3i],
agg_moment_unreduced: wp.array[wp.float32],
total_num_threads: int,
):
"""Register hydroelastic contacts in the hashtable for reduction.
Populates all hashtable slots (spatial extremes, max-depth, voxel) with
real contact_ids from the buffer.
"""
tid = wp.tid()
num_contacts = reducer_data.contact_count[0]
if num_contacts == 0:
return
num_contacts = wp.min(num_contacts, reducer_data.capacity)
for i in range(tid, num_contacts, total_num_threads):
pd = reducer_data.position_depth[i]
normal = decode_oct(reducer_data.normal[i])
pair = reducer_data.shape_pairs[i]
position = wp.vec3(pd[0], pd[1], pd[2])
depth = pd[3]
shape_a = pair[0]
shape_b = pair[1]
aabb_lower = shape_collision_aabb_lower[shape_b]
aabb_upper = shape_collision_aabb_upper[shape_b]
ht_capacity = reducer_data.ht_capacity
# === Part 1: Normal-binned reduction ===
bin_id = get_slot(normal)
key = make_contact_key(shape_a, shape_b, bin_id)
entry_idx = hashtable_find_or_insert(key, reducer_data.ht_keys, reducer_data.ht_active_slots)
# Cache normal-bin entry index for downstream kernels (avoids repeated hash lookups)
if reducer_data.contact_nbin_entry.shape[0] > 0:
reducer_data.contact_nbin_entry[i] = entry_idx
if entry_idx >= 0:
# k_eff is constant for a shape pair, so redundant writes are safe.
reducer_data.entry_k_eff[entry_idx] = _effective_stiffness(
shape_material_k_hydro[shape_a], shape_material_k_hydro[shape_b]
)
aabb_size = wp.length(aabb_upper - aabb_lower)
use_beta = depth < wp.static(BETA_THRESHOLD) * aabb_size
if use_beta:
ws = reducer_data.weight_sum[entry_idx]
anchor = reducer_data.weighted_pos_sum[entry_idx] / ws
pos_2d_centered = project_point_to_plane(bin_id, position - anchor)
pen_weight = wp.max(-depth, 0.0)
for dir_i in range(wp.static(NUM_SPATIAL_DIRECTIONS)):
dir_2d = get_spatial_direction_2d(dir_i)
score = wp.dot(pos_2d_centered, dir_2d) * pen_weight
value = make_contact_value(score, i)
reduction_update_slot(entry_idx, dir_i, value, reducer_data.ht_values, ht_capacity)
max_depth_value = make_contact_value(-depth, i)
reduction_update_slot(
entry_idx,
wp.static(NUM_SPATIAL_DIRECTIONS),
max_depth_value,
reducer_data.ht_values,
ht_capacity,
)
if agg_moment_unreduced.shape[0] > 0 and depth < 0.0:
pen_mag = -depth
ws = reducer_data.weight_sum[entry_idx]
if ws > EPS_SMALL:
anchor_pos = reducer_data.weighted_pos_sum[entry_idx] / ws
lever = wp.length(wp.cross(position - anchor_pos, normal))
area_i = reducer_data.contact_area[i]
wp.atomic_add(agg_moment_unreduced, entry_idx, area_i * pen_mag * lever)
else:
wp.atomic_add(reducer_data.ht_insert_failures, 0, 1)
# === Part 2: Voxel-based reduction ===
voxel_res = shape_voxel_resolution[shape_b]
voxel_idx = compute_voxel_index(position, aabb_lower, aabb_upper, voxel_res)
voxel_idx = wp.clamp(voxel_idx, 0, wp.static(NUM_VOXEL_DEPTH_SLOTS - 1))
voxels_per_group = wp.static(NUM_SPATIAL_DIRECTIONS + 1)
voxel_group = voxel_idx // voxels_per_group
voxel_local_slot = voxel_idx % voxels_per_group
voxel_bin_id = wp.static(NUM_NORMAL_BINS) + voxel_group
voxel_key = make_contact_key(shape_a, shape_b, voxel_bin_id)
voxel_entry_idx = hashtable_find_or_insert(voxel_key, reducer_data.ht_keys, reducer_data.ht_active_slots)
if voxel_entry_idx >= 0:
reducer_data.entry_k_eff[voxel_entry_idx] = _effective_stiffness(
shape_material_k_hydro[shape_a], shape_material_k_hydro[shape_b]
)
voxel_value = make_contact_value(-depth, i)
reduction_update_slot(
voxel_entry_idx,
voxel_local_slot,
voxel_value,
reducer_data.ht_values,
ht_capacity,
)
else:
wp.atomic_add(reducer_data.ht_insert_failures, 0, 1)
return reduce_hydroelastic_contacts_kernel
# =============================================================================
# Hydroelastic export kernel factory
# =============================================================================
def _create_accumulate_reduced_depth_kernel():
"""Create a kernel that accumulates winning contact depths and normals per normal bin.
Returns:
A Warp kernel that accumulates ``total_depth_reduced`` and
``total_normal_reduced``.
"""
exported_ids_vec = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.int32)
@wp.kernel(enable_backward=False)
def accumulate_reduced_depth_kernel(
ht_keys: wp.array[wp.uint64],
ht_values: wp.array[wp.uint64],
ht_active_slots: wp.array[wp.int32],
position_depth: wp.array[wp.vec4],
normal: wp.array[wp.vec2],
contact_nbin_entry: wp.array[wp.int32],
total_depth_reduced: wp.array[wp.float32],
total_normal_reduced: wp.array[wp.vec3],
total_num_threads: int,
):
"""Accumulate winning contact depths and normals per normal bin.
For each active hashtable entry (normal bin or voxel bin), iterates
over unique winning contacts and atomically adds their penetrating
depths to the corresponding normal bin's ``total_depth_reduced`` and
their depth-weighted normals to ``total_normal_reduced``.
"""
tid = wp.tid()
ht_capacity = ht_keys.shape[0]
num_active = ht_active_slots[ht_capacity]
if num_active == 0:
return
for i in range(tid, num_active, total_num_threads):
entry_idx = ht_active_slots[i]
# Extract bin_id from the stored key to distinguish normal vs voxel bins.
stored_key = ht_keys[entry_idx]
entry_bin_id = int((stored_key >> wp.uint64(55)) & BIN_MASK)
p1_ids = exported_ids_vec()
p1_count = int(0)
for slot in range(wp.static(VALUES_PER_KEY)):
value = ht_values[slot * ht_capacity + entry_idx]
if value == wp.uint64(0):
continue
contact_id = unpack_contact_id(value)
if is_contact_already_exported(contact_id, p1_ids, p1_count):
continue
p1_ids[p1_count] = contact_id
p1_count = p1_count + 1
pd = position_depth[contact_id]
depth = pd[3]
if depth < 0.0:
if entry_bin_id < wp.static(NUM_NORMAL_BINS):
nbin_idx = entry_idx
else:
nbin_idx = contact_nbin_entry[contact_id]
if nbin_idx >= 0:
pen_mag = -depth
contact_normal = decode_oct(normal[contact_id])
wp.atomic_add(total_depth_reduced, nbin_idx, pen_mag)
wp.atomic_add(total_normal_reduced, nbin_idx, pen_mag * contact_normal)
return accumulate_reduced_depth_kernel
def _create_accumulate_moments_kernel(normal_matching: bool = True):
"""Create a kernel that accumulates unreduced and reduced friction moments per normal bin.
Args:
normal_matching: If True, rotate reduced contact normals using the aggregate
force direction before computing lever arms.
Returns:
A Warp kernel that populates ``agg_moment_unreduced``,
``agg_moment_reduced``, and ``agg_moment2_reduced``.
"""
exported_ids_vec = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.int32)
@wp.kernel(enable_backward=False)
def accumulate_moments_kernel(
ht_keys: wp.array[wp.uint64],
ht_values: wp.array[wp.uint64],
ht_active_slots: wp.array[wp.int32],
position_depth: wp.array[wp.vec4],
normal: wp.array[wp.vec2],
contact_nbin_entry: wp.array[wp.int32],
weighted_pos_sum: wp.array[wp.vec3],
weight_sum: wp.array[wp.float32],
agg_force: wp.array[wp.vec3],
total_normal_reduced: wp.array[wp.vec3],
agg_moment_reduced: wp.array[wp.float32],
agg_moment2_reduced: wp.array[wp.float32],
total_num_threads: int,
):
"""Accumulate reduced friction moments per normal bin."""
tid = wp.tid()
ht_capacity = ht_keys.shape[0]
# Reduced moment over winning contacts
num_active = ht_active_slots[ht_capacity]
if num_active == 0:
return
for i in range(tid, num_active, total_num_threads):
entry_idx = ht_active_slots[i]
stored_key = ht_keys[entry_idx]
entry_bin_id = int((stored_key >> wp.uint64(55)) & BIN_MASK)
p2_ids = exported_ids_vec()
p2_count = int(0)
for slot in range(wp.static(VALUES_PER_KEY)):
value = ht_values[slot * ht_capacity + entry_idx]
if value == wp.uint64(0):
continue
contact_id = unpack_contact_id(value)
if is_contact_already_exported(contact_id, p2_ids, p2_count):
continue
p2_ids[p2_count] = contact_id
p2_count = p2_count + 1
pd = position_depth[contact_id]
depth = pd[3]
if depth >= 0.0:
continue
pen_mag = -depth
contact_normal = decode_oct(normal[contact_id])
# Determine normal-bin index using cached entry
if entry_bin_id < wp.static(NUM_NORMAL_BINS):
nbin_idx = entry_idx
else:
nbin_idx = contact_nbin_entry[contact_id]
if nbin_idx < 0:
continue
ws = weight_sum[nbin_idx]
if ws <= EPS_SMALL:
continue
anchor_pos = weighted_pos_sum[nbin_idx] / ws
# Optionally rotate normal to match aggregate force direction
rotated_normal = contact_normal
if wp.static(normal_matching):
nbin_agg_force = agg_force[nbin_idx]
nbin_agg_mag = wp.length(nbin_agg_force)
if nbin_agg_mag > EPS_LARGE:
nbin_nsum = total_normal_reduced[nbin_idx]
rot_q = _compute_normal_matching_rotation(nbin_nsum, nbin_agg_force, nbin_agg_mag)
rotated_normal = wp.normalize(wp.quat_rotate(rot_q, contact_normal))
pos = wp.vec3(pd[0], pd[1], pd[2])
lever = wp.length(wp.cross(pos - anchor_pos, rotated_normal))
wp.atomic_add(agg_moment_reduced, nbin_idx, pen_mag * lever)
wp.atomic_add(agg_moment2_reduced, nbin_idx, pen_mag * lever * lever) # second moment
return accumulate_moments_kernel
def create_export_hydroelastic_reduced_contacts_kernel(
writer_func: Any,
margin_contact_area: float,
normal_matching: bool = True,
anchor_contact: bool = False,
moment_matching: bool = False,
):
"""Create a kernel that exports reduced hydroelastic contacts using a custom writer function.
Computes contact stiffness using the aggregate stiffness formula:
c_stiffness = k_eff * |agg_force| / total_depth_reduced
where:
- agg_force = sum(area * |depth| * normal) for ALL contacts in the normal bin
- total_depth_reduced = sum(|depth|) for all winning contacts (normal bin + voxel)
that map to the normal bin, pre-accumulated by ``accumulate_reduced_depth_kernel``
This ensures the total contact force matches the aggregate force from all original
contacts, with the force distributed over ALL reduced contacts (including voxel-based).
.. important::
``accumulate_reduced_depth_kernel`` (from
:func:`_create_accumulate_reduced_depth_kernel`) **must** be launched
before this kernel so that ``total_depth_reduced`` is fully populated.
Args:
writer_func: A warp function with signature (ContactData, writer_data, int) -> None
margin_contact_area: Contact area to use for non-penetrating contacts at the margin
normal_matching: If True, rotate contact normals so their weighted sum aligns with aggregate force
anchor_contact: If True, add an anchor contact at the center of pressure for each entry
moment_matching: If True, adjust per-contact friction scales so that
the maximum friction moment per normal bin is preserved between
reduced and unreduced contacts.
Returns:
A warp kernel that can be launched to export reduced hydroelastic contacts.
"""
# Define vector types for tracking exported contact data
exported_ids_vec = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.int32)
exported_depths_vec = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.float32)
# Cache decoded normals (vec3 per slot) to avoid double decode_oct
# Stored as 3 separate float vectors (Warp doesn't support vector-of-vec3)
exported_nx_vec = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.float32)
exported_ny_vec = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.float32)
exported_nz_vec = wp.types.vector(length=VALUES_PER_KEY, dtype=wp.float32)
@wp.kernel(enable_backward=False)
def export_hydroelastic_reduced_contacts_kernel(
# Hashtable arrays
ht_keys: wp.array[wp.uint64],
ht_values: wp.array[wp.uint64],
ht_active_slots: wp.array[wp.int32],
# Aggregate data per entry (from generate kernel)
agg_force: wp.array[wp.vec3],
weighted_pos_sum: wp.array[wp.vec3],
weight_sum: wp.array[wp.float32],
# Contact buffer arrays
position_depth: wp.array[wp.vec4],
normal: wp.array[wp.vec2], # Octahedral-encoded
shape_pairs: wp.array[wp.vec2i],
contact_area: wp.array[wp.float32],
entry_k_eff: wp.array[wp.float32],
contact_nbin_entry: wp.array[wp.int32],
# Pre-accumulated total depth of winning contacts per normal bin
total_depth_reduced: wp.array[wp.float32],
# Pre-accumulated depth-weighted normal sum of winning contacts per normal bin
total_normal_reduced: wp.array[wp.vec3],
# Pre-accumulated friction moments per normal bin (for moment matching)
agg_moment_unreduced: wp.array[wp.float32],
agg_moment_reduced: wp.array[wp.float32],
agg_moment2_reduced: wp.array[wp.float32],
# Shape data for margin
shape_gap: wp.array[float],
shape_transform: wp.array[wp.transform],
# Writer data (custom struct)
writer_data: Any,
# Grid stride parameters
total_num_threads: int,
):
"""
Export reduced hydroelastic contacts to the writer with aggregate stiffness.
"""
tid = wp.tid()
# Get number of active entries (stored at index = ht_capacity)
ht_capacity = ht_keys.shape[0]
num_active = ht_active_slots[ht_capacity]
# Early exit if no active entries
if num_active == 0:
return
for i in range(tid, num_active, total_num_threads):
# Get the hashtable entry index
entry_idx = ht_active_slots[i]
# === First pass: collect unique contacts and compute aggregates ===
exported_ids = exported_ids_vec()
exported_depths = exported_depths_vec()
# Cache decoded normals to avoid double decode_oct in second pass
cached_nx = exported_nx_vec()
cached_ny = exported_ny_vec()
cached_nz = exported_nz_vec()
num_exported = int(0)
max_pen_depth = float(0.0) # Maximum penetration magnitude (positive value)
k_eff_first = float(0.0)
shape_a_first = int(0)
shape_b_first = int(0)
# Read all value slots for this entry (slot-major layout)
for slot in range(wp.static(VALUES_PER_KEY)):
value = ht_values[slot * ht_capacity + entry_idx]
# Skip empty slots (value = 0)
if value == wp.uint64(0):
continue
# Extract contact ID from low 32 bits
contact_id = unpack_contact_id(value)
# Skip if already exported
if is_contact_already_exported(contact_id, exported_ids, num_exported):
continue
# Unpack contact data (decode oct-normal once, cache for second pass)
pd = position_depth[contact_id]
contact_normal = decode_oct(normal[contact_id])
depth = pd[3]
# Record this contact, its depth, and cached normal
exported_ids[num_exported] = contact_id
exported_depths[num_exported] = depth
cached_nx[num_exported] = contact_normal[0]
cached_ny[num_exported] = contact_normal[1]
cached_nz[num_exported] = contact_normal[2]
num_exported = num_exported + 1
# Track max penetration and normal matching (depth < 0 = penetrating)
if depth < 0.0:
pen_magnitude = -depth
max_pen_depth = wp.max(max_pen_depth, pen_magnitude)
# Store first contact's shape pair (same for all contacts in the entry)
if k_eff_first == 0.0:
k_eff_first = entry_k_eff[entry_idx]
pair = shape_pairs[contact_id]
shape_a_first = pair[0]
shape_b_first = pair[1]
# Skip entries with no contacts
if num_exported == 0:
continue
# === Compute stiffness and optional features based on entry type ===
# Normal bin entries (bin_id < NUM_NORMAL_BINS): have aggregate force, use aggregate stiffness
# Voxel bin entries (bin_id >= NUM_NORMAL_BINS): no aggregate force, use per-contact stiffness
agg_force_vec = agg_force[entry_idx]
agg_force_mag = wp.length(agg_force_vec)
# Aggregate force direction must be well-conditioned for
# normal matching and anchor features.
has_reliable_agg_direction = agg_force_mag > wp.static(EPS_LARGE)
# Compute anchor position (center of pressure) for normal bin entries
anchor_pos = wp.vec3(0.0, 0.0, 0.0)
add_anchor = 0
entry_weight_sum = weight_sum[entry_idx]
if wp.static(anchor_contact) and has_reliable_agg_direction and max_pen_depth > 0.0:
if entry_weight_sum > wp.static(EPS_SMALL):
anchor_pos = weighted_pos_sum[entry_idx] / entry_weight_sum
add_anchor = 1
# Compute total_depth including anchor contribution
# Use pre-accumulated total_depth_reduced which includes all winning contacts
# (both normal bin and voxel bin) that map to this normal bin.
anchor_depth = max_pen_depth # Anchor uses max penetration depth (positive magnitude)
entry_total_depth = total_depth_reduced[entry_idx]
# Compute normal matching rotation quaternion from pre-accumulated
rotation_q = wp.quat_identity()
nbin_normal_sum = total_normal_reduced[entry_idx]
if wp.static(normal_matching) and has_reliable_agg_direction:
rotation_q = _compute_normal_matching_rotation(nbin_normal_sum, agg_force_vec, agg_force_mag)
# When normal matching is enabled, use |total_normal_reduced| as the
# effective depth denominator. This compensates for the magnitude loss
# caused by cancellation in the depth-weighted normal sum so that
# F_reduced = k_eff * agg_force exactly.
if wp.static(normal_matching):
effective_depth = wp.length(nbin_normal_sum)
if effective_depth < wp.static(EPS_LARGE):
effective_depth = entry_total_depth
else:
effective_depth = entry_total_depth
total_depth_with_anchor = effective_depth + wp.float32(add_anchor) * anchor_depth
# Compute shared stiffness: c_stiffness = k_eff * |agg_force| / total_depth
shared_stiffness = float(0.0)
if agg_force_mag > wp.static(EPS_SMALL) and total_depth_with_anchor > 0.0:
shared_stiffness = k_eff_first * agg_force_mag / total_depth_with_anchor
# Moment matching: hybrid uniform / per-contact strategy.
moment_alpha = float(0.0)
moment_L_avg = float(0.0)
uniform_friction_scale = float(1.0)
anchor_friction_scale = float(1.0)
if wp.static(moment_matching):
m_unr = agg_moment_unreduced[entry_idx]
m_red = agg_moment_reduced[entry_idx] # S1 = sum(pen * lever)
m_red2 = agg_moment2_reduced[entry_idx] # S2 = sum(pen * lever^2)
s0_total = entry_total_depth + wp.float32(add_anchor) * anchor_depth
if (
m_unr > wp.static(EPS_SMALL)
and s0_total > wp.static(EPS_SMALL)
and m_red > wp.static(EPS_SMALL)
and agg_force_mag > wp.static(EPS_SMALL)
):
m_target = m_unr * total_depth_with_anchor / agg_force_mag
if m_target < m_red:
# Overshoot: uniform scale down
uniform_friction_scale = m_target / m_red
else:
# Undershoot: per-contact alpha scaling
moment_L_avg = m_red / s0_total
variance = m_red2 * s0_total - m_red * m_red
if variance > wp.static(EPS_SMALL):
moment_alpha = wp.clamp((m_target - m_red) * m_red / variance, 0.0, 1.0)
# Anchor compensation:
# - Overshoot: anchor_fs = 1 + (S0/anchor_depth)*(1 - uniform_fs)
# - Undershoot: anchor_fs = 1 - alpha
if add_anchor == 1 and anchor_depth > 0.0:
anchor_friction_scale = wp.max(
wp.static(MIN_FRICTION_SCALE),
1.0 + (entry_total_depth / anchor_depth) * (1.0 - uniform_friction_scale) - moment_alpha,
)
# Get transform and gap sum (same for all contacts in the entry)
transform_b = shape_transform[shape_b_first]
gap_a = shape_gap[shape_a_first]
gap_b = shape_gap[shape_b_first]
gap_sum = gap_a + gap_b
# === Second pass: export contacts ===
for idx in range(num_exported):
contact_id = exported_ids[idx]
depth = exported_depths[idx]
# Read position from buffer; use cached decoded normal
pd = position_depth[contact_id]
position = wp.vec3(pd[0], pd[1], pd[2])
contact_normal = wp.vec3(cached_nx[idx], cached_ny[idx], cached_nz[idx])
# Get shape pair
pair = shape_pairs[contact_id]
shape_a = pair[0]
shape_b = pair[1]
# Apply normal matching rotation for penetrating contacts (depth < 0)
final_normal = contact_normal
area_i = contact_area[contact_id]
c_friction_scale = float(1.0)
if has_reliable_agg_direction:
# --- Normal-bin entry ---
if wp.static(normal_matching) and depth < 0.0:
final_normal = wp.normalize(wp.quat_rotate(rotation_q, contact_normal))
c_stiffness = shared_stiffness
if shared_stiffness == 0.0:
# Normal-bin entry but aggregate stiffness unavailable
if depth < 0.0:
c_stiffness = area_i * k_eff_first
else:
c_stiffness = wp.static(margin_contact_area) * k_eff_first
# Moment matching friction adjustment
if wp.static(moment_matching) and depth < 0.0:
if moment_L_avg > wp.static(EPS_SMALL):
# Undershoot: per-contact scaling
lever_i = wp.length(wp.cross(position - anchor_pos, final_normal))
c_friction_scale = wp.max(
wp.static(MIN_FRICTION_SCALE),
1.0 + moment_alpha * (lever_i - moment_L_avg) / moment_L_avg,
)
else:
# Overshoot: uniform scaling
c_friction_scale = uniform_friction_scale
else:
# --- Voxel-bin entry: use cached normal-bin index ---
nbin_entry_idx = contact_nbin_entry[contact_id]
if nbin_entry_idx >= 0 and depth < 0.0:
nbin_agg_force = agg_force[nbin_entry_idx]
nbin_agg_mag = wp.length(nbin_agg_force)
# Normal matching from the normal bin's rotation
if wp.static(normal_matching) and nbin_agg_mag > wp.static(EPS_LARGE):
voxel_nsum = total_normal_reduced[nbin_entry_idx]
voxel_rot_q = _compute_normal_matching_rotation(voxel_nsum, nbin_agg_force, nbin_agg_mag)
final_normal = wp.normalize(wp.quat_rotate(voxel_rot_q, contact_normal))
# Stiffness from the normal bin's aggregate
if wp.static(normal_matching):
nbin_effective_depth_no_anchor = wp.length(total_normal_reduced[nbin_entry_idx])
if nbin_effective_depth_no_anchor < wp.static(EPS_LARGE):
nbin_effective_depth_no_anchor = total_depth_reduced[nbin_entry_idx]
else:
nbin_effective_depth_no_anchor = total_depth_reduced[nbin_entry_idx]
nbin_effective_depth = nbin_effective_depth_no_anchor
nbin_anchor_depth = float(0.0)
if wp.static(anchor_contact) and nbin_agg_mag > wp.static(EPS_LARGE):
nbin_max_depth_value = ht_values[
wp.static(NUM_SPATIAL_DIRECTIONS) * ht_capacity + nbin_entry_idx
]
if nbin_max_depth_value != wp.uint64(0):
nbin_max_depth_contact_id = unpack_contact_id(nbin_max_depth_value)
nbin_max_depth = position_depth[nbin_max_depth_contact_id][3]
if nbin_max_depth < 0.0:
nbin_anchor_depth = -nbin_max_depth
if weight_sum[nbin_entry_idx] > wp.static(EPS_SMALL) and nbin_anchor_depth > 0.0:
nbin_effective_depth = nbin_effective_depth_no_anchor + nbin_anchor_depth
if nbin_agg_mag > wp.static(EPS_SMALL) and nbin_effective_depth > 0.0:
c_stiffness = k_eff_first * nbin_agg_mag / nbin_effective_depth
else:
c_stiffness = area_i * k_eff_first
# Moment matching friction adjustment (voxel entry)
if wp.static(moment_matching):
voxel_m_unr = agg_moment_unreduced[nbin_entry_idx]
voxel_s1 = agg_moment_reduced[nbin_entry_idx]
voxel_s2 = agg_moment2_reduced[nbin_entry_idx]
nbin_entry_total_depth = total_depth_reduced[nbin_entry_idx]
voxel_s0 = nbin_entry_total_depth + nbin_anchor_depth
if (
voxel_m_unr > wp.static(EPS_SMALL)
and voxel_s0 > wp.static(EPS_SMALL)
and voxel_s1 > wp.static(EPS_SMALL)
and nbin_agg_mag > wp.static(EPS_SMALL)
):
voxel_m_target = voxel_m_unr * nbin_effective_depth / nbin_agg_mag
if voxel_m_target < voxel_s1:
# Overshoot: uniform scale down
c_friction_scale = voxel_m_target / voxel_s1
else:
# Undershoot: per-contact alpha scaling
voxel_L_avg = voxel_s1 / voxel_s0
voxel_variance = voxel_s2 * voxel_s0 - voxel_s1 * voxel_s1
voxel_alpha = float(0.0)
if voxel_variance > wp.static(EPS_SMALL):
voxel_alpha = wp.clamp(
(voxel_m_target - voxel_s1) * voxel_s1 / voxel_variance, 0.0, 1.0
)
voxel_anchor_pos = wp.vec3(0.0, 0.0, 0.0)
nbin_ws = weight_sum[nbin_entry_idx]
if nbin_ws > wp.static(EPS_SMALL):
voxel_anchor_pos = weighted_pos_sum[nbin_entry_idx] / nbin_ws
voxel_lever = wp.length(wp.cross(position - voxel_anchor_pos, final_normal))
if voxel_L_avg > wp.static(EPS_SMALL):
c_friction_scale = wp.max(
wp.static(MIN_FRICTION_SCALE),
1.0 + voxel_alpha * (voxel_lever - voxel_L_avg) / voxel_L_avg,
)
elif depth < 0.0:
# Penetrating contact with no normal bin: per-contact area.
c_stiffness = area_i * k_eff_first
else:
# Non-penetrating margin contact.
c_stiffness = wp.static(margin_contact_area) * k_eff_first
# Transform contact to world space
normal_world = wp.transform_vector(transform_b, final_normal)
pos_world = wp.transform_point(transform_b, position)
# Create ContactData struct
# contact_distance = 2 * depth (depth is already negative for penetrating)
# This gives negative contact_distance for penetrating contacts
contact_data = ContactData()
contact_data.contact_point_center = pos_world
contact_data.contact_normal_a_to_b = normal_world
contact_data.contact_distance = 2.0 * depth # depth is negative = penetrating
contact_data.radius_eff_a = 0.0
contact_data.radius_eff_b = 0.0
contact_data.margin_a = 0.0
contact_data.margin_b = 0.0
contact_data.shape_a = shape_a
contact_data.shape_b = shape_b
contact_data.gap_sum = gap_sum
contact_data.contact_stiffness = c_stiffness
contact_data.contact_friction_scale = wp.float32(c_friction_scale)
# Call the writer function
writer_func(contact_data, writer_data, -1)
# === Export anchor contact if enabled ===
if add_anchor == 1:
# Anchor normal is aligned with aggregate force direction
anchor_normal = wp.normalize(agg_force_vec)
anchor_normal_world = wp.transform_vector(transform_b, anchor_normal)
anchor_pos_world = wp.transform_point(transform_b, anchor_pos)
# Create ContactData for anchor
# anchor_depth is positive magnitude, so negate for standard convention
contact_data = ContactData()
contact_data.contact_point_center = anchor_pos_world
contact_data.contact_normal_a_to_b = anchor_normal_world
contact_data.contact_distance = -2.0 * anchor_depth # anchor_depth is positive magnitude
contact_data.radius_eff_a = 0.0
contact_data.radius_eff_b = 0.0
contact_data.margin_a = 0.0
contact_data.margin_b = 0.0
contact_data.shape_a = shape_a_first
contact_data.shape_b = shape_b_first
contact_data.gap_sum = gap_sum
contact_data.contact_stiffness = shared_stiffness
contact_data.contact_friction_scale = wp.float32(anchor_friction_scale)
# Call the writer function for anchor
writer_func(contact_data, writer_data, -1)
return export_hydroelastic_reduced_contacts_kernel
# =============================================================================
# Hydroelastic Contact Reduction API
# =============================================================================
@dataclass
class HydroelasticReductionConfig:
"""Configuration for hydroelastic contact reduction.
Attributes:
normal_matching: If True, rotate reduced contact normals so their weighted
sum aligns with the aggregate force direction.
anchor_contact: If True, add an anchor contact at the center of pressure
for each normal bin. The anchor contact helps preserve moment balance.
moment_matching: If True, adjust per-contact friction scales so that the
maximum friction moment per normal bin is preserved between reduced
and unreduced contacts. Automatically enables ``anchor_contact``.
margin_contact_area: Contact area used for non-penetrating contacts at the margin.
"""
normal_matching: bool = True
anchor_contact: bool = False
moment_matching: bool = False
margin_contact_area: float = 1e-2
class HydroelasticContactReduction:
"""High-level API for hydroelastic contact reduction.
This class encapsulates the hydroelastic contact reduction pipeline, providing
a clean interface that hides the low-level kernel launch details. It manages:
1. A ``GlobalContactReducer`` for contact storage and hashtable tracking
2. The reduction kernels for hashtable registration
3. The export kernel for writing reduced contacts
**Usage Pattern:**
The typical usage in a contact generation pipeline is:
1. Call ``clear()`` at the start of each frame
2. Write contacts to the buffer using ``export_hydroelastic_contact_to_buffer``
in your contact generation kernel (use ``get_data_struct()`` to get the data)
3. Call ``reduce()`` to register contacts in the hashtable
4. Call ``export()`` to write reduced contacts using the writer function
Example:
.. code-block:: python
# Initialize once
config = HydroelasticReductionConfig(normal_matching=True)
reduction = HydroelasticContactReduction(
capacity=100000,
device="cuda:0",
writer_func=my_writer_func,
config=config,
)
# Each frame
reduction.clear()
# Launch your contact generation kernel that uses:
# export_hydroelastic_contact_to_buffer(..., reduction.get_data_struct())
reduction.reduce(shape_transform, shape_sdf_data, grid_size)
reduction.export(shape_gap, shape_transform, writer_data, grid_size)
Attributes:
reducer: The underlying ``GlobalContactReducer`` instance.
config: The ``HydroelasticReductionConfig`` for this instance.
contact_count: Array containing the number of contacts in the buffer.
See Also:
:func:`export_hydroelastic_contact_to_buffer`: Warp function for writing
contacts to the buffer from custom kernels.
:class:`GlobalContactReducerData`: Struct for passing reducer data to kernels.
"""
def __init__(
self,
capacity: int,
device: str | None = None,
writer_func: Any = None,
config: HydroelasticReductionConfig | None = None,
):
"""Initialize the hydroelastic contact reduction system.
Args:
capacity: Maximum number of contacts to store in the buffer.
device: Warp device (e.g., "cuda:0", "cpu"). If None, uses default device.
writer_func: Warp function for writing decoded contacts. Must have signature
``(ContactData, writer_data, int) -> None``.
config: Configuration options. If None, uses default ``HydroelasticReductionConfig``.
"""
if config is None:
config = HydroelasticReductionConfig()
# Moment matching requires anchor contact for lever-arm reference
if config.moment_matching and not config.anchor_contact:
config.anchor_contact = True
self.config = config
self.device = device
# Create the underlying reducer with hydroelastic data storage enabled
self.reducer = GlobalContactReducer(
capacity=capacity,
device=device,
store_hydroelastic_data=True,
store_moment_data=config.moment_matching,
)
# Create reduction kernel
self._reduce_kernel = get_reduce_hydroelastic_contacts_kernel()
self._accumulate_depth_kernel = _create_accumulate_reduced_depth_kernel()
# Create moment accumulation kernel (only when moment matching is enabled)
self._accumulate_moments_kernel = None
if config.moment_matching:
self._accumulate_moments_kernel = _create_accumulate_moments_kernel(
normal_matching=config.normal_matching,
)
# Create the export kernel with the configured options
self._export_kernel = create_export_hydroelastic_reduced_contacts_kernel(
writer_func=writer_func,
margin_contact_area=config.margin_contact_area,
normal_matching=config.normal_matching,
anchor_contact=config.anchor_contact,
moment_matching=config.moment_matching,
)
@property
def contact_count(self) -> wp.array:
"""Array containing the current number of contacts in the buffer."""
return self.reducer.contact_count
@property
def capacity(self) -> int:
"""Maximum number of contacts that can be stored."""
return self.reducer.capacity
def get_data_struct(self) -> GlobalContactReducerData:
"""Get the data struct for passing to Warp kernels.
Returns:
A ``GlobalContactReducerData`` struct containing all arrays needed
for contact storage and reduction.
"""
return self.reducer.get_data_struct()
def clear(self):
"""Clear all contacts and reset for a new frame.
This efficiently clears only the active hashtable entries and resets
the contact counter. Call this at the start of each simulation step.
"""
self.reducer.clear_active()
def reduce(
self,
shape_material_k_hydro: wp.array,
shape_transform: wp.array,
shape_collision_aabb_lower: wp.array,
shape_collision_aabb_upper: wp.array,
shape_voxel_resolution: wp.array,
grid_size: int,
):
"""Register buffered contacts in the hashtable for reduction.
This launches the reduction kernel that processes all contacts in the
buffer and registers them in the hashtable based on spatial extremes,
max-depth per normal bin, and voxel-based slots.
Aggregate accumulation (agg_force, weighted_pos_sum, weight_sum) is
always performed in the generate kernel, so this method only handles
hashtable slot registration.
Args:
shape_material_k_hydro: Per-shape hydroelastic material stiffness (dtype: float).
shape_transform: Per-shape world transforms (dtype: wp.transform).
shape_collision_aabb_lower: Per-shape local AABB lower bounds (dtype: wp.vec3).
shape_collision_aabb_upper: Per-shape local AABB upper bounds (dtype: wp.vec3).
shape_voxel_resolution: Per-shape voxel grid resolution (dtype: wp.vec3i).
grid_size: Number of threads for the kernel launch.
"""
reducer_data = self.reducer.get_data_struct()
wp.launch(
kernel=self._reduce_kernel,
dim=[grid_size],
inputs=[
reducer_data,
shape_material_k_hydro,
shape_transform,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
self.reducer.agg_moment_unreduced,
grid_size,
],
device=self.device,
record_tape=False,
)
def export(
self,
shape_gap: wp.array,
shape_transform: wp.array,
writer_data: Any,
grid_size: int,
):
"""Export reduced contacts using the writer function.
This first launches the accumulation kernel so that
``total_depth_reduced`` and ``total_normal_reduced`` are fully
populated before the export kernel reads them (the implicit
synchronisation between ``wp.launch()`` calls acts as the required
global memory barrier).
Args:
shape_gap: Per-shape contact gap (detection threshold) (dtype: float).
shape_transform: Per-shape world transforms (dtype: wp.transform).
writer_data: Data struct for the writer function.
grid_size: Number of threads for the kernel launch.
"""
# --- accumulate winning-contact depths per normal bin (Phase 1) ---
wp.launch(
kernel=self._accumulate_depth_kernel,
dim=[grid_size],
inputs=[
self.reducer.hashtable.keys,
self.reducer.ht_values,
self.reducer.hashtable.active_slots,
self.reducer.position_depth,
self.reducer.normal,
self.reducer.contact_nbin_entry,
self.reducer.total_depth_reduced,
self.reducer.total_normal_reduced,
grid_size,
],
device=self.device,
)
# --- accumulate reduced friction moments per normal bin (Phase 1.5) ---
if self._accumulate_moments_kernel is not None:
wp.launch(
kernel=self._accumulate_moments_kernel,
dim=[grid_size],
inputs=[
self.reducer.hashtable.keys,
self.reducer.ht_values,
self.reducer.hashtable.active_slots,
self.reducer.position_depth,
self.reducer.normal,
self.reducer.contact_nbin_entry,
self.reducer.weighted_pos_sum,
self.reducer.weight_sum,
self.reducer.agg_force,
self.reducer.total_normal_reduced,
self.reducer.agg_moment_reduced,
self.reducer.agg_moment2_reduced,
grid_size,
],
device=self.device,
)
# --- export reduced contacts (Phase 2) ---
wp.launch(
kernel=self._export_kernel,
dim=[grid_size],
inputs=[
self.reducer.hashtable.keys,
self.reducer.ht_values,
self.reducer.hashtable.active_slots,
self.reducer.agg_force,
self.reducer.weighted_pos_sum,
self.reducer.weight_sum,
self.reducer.position_depth,
self.reducer.normal,
self.reducer.shape_pairs,
self.reducer.contact_area,
self.reducer.entry_k_eff,
self.reducer.contact_nbin_entry,
self.reducer.total_depth_reduced,
self.reducer.total_normal_reduced,
self.reducer.agg_moment_unreduced,
self.reducer.agg_moment_reduced,
self.reducer.agg_moment2_reduced,
shape_gap,
shape_transform,
writer_data,
grid_size,
],
device=self.device,
record_tape=False,
)
def reduce_and_export(
self,
shape_material_k_hydro: wp.array,
shape_transform: wp.array,
shape_collision_aabb_lower: wp.array,
shape_collision_aabb_upper: wp.array,
shape_voxel_resolution: wp.array,
shape_gap: wp.array,
writer_data: Any,
grid_size: int,
):
"""Convenience method to reduce and export in one call.
Combines ``reduce()`` and ``export()`` into a single method call.
Args:
shape_material_k_hydro: Per-shape hydroelastic material stiffness (dtype: float).
shape_transform: Per-shape world transforms (dtype: wp.transform).
shape_collision_aabb_lower: Per-shape local AABB lower bounds (dtype: wp.vec3).
shape_collision_aabb_upper: Per-shape local AABB upper bounds (dtype: wp.vec3).
shape_voxel_resolution: Per-shape voxel grid resolution (dtype: wp.vec3i).
shape_gap: Per-shape contact gap (detection threshold) (dtype: float).
writer_data: Data struct for the writer function.
grid_size: Number of threads for the kernel launch.
"""
self.reduce(
shape_material_k_hydro,
shape_transform,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
grid_size,
)
self.export(shape_gap, shape_transform, writer_data, grid_size)
================================================
FILE: newton/_src/geometry/differentiable_contacts.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Post-processing kernels that augment rigid contacts with differentiable data.
The narrow-phase collision kernels use ``enable_backward=False`` so they are
never recorded on a :class:`wp.Tape`. This module provides lightweight kernels
that re-read the frozen contact geometry (body-local points, world normal,
margins) produced by the narrow phase and reconstruct world-space quantities
through the *differentiable* body transforms ``body_q``.
The resulting arrays carry ``requires_grad=True`` and participate in autodiff,
giving first-order (tangent-plane) gradients of contact distance and world-space
contact points with respect to body poses. The frozen world-space normal passes
through unchanged — gradients flow through the contact *points* and *distance*
but **not** through the normal direction.
"""
from __future__ import annotations
import warp as wp
@wp.kernel
def differentiable_contact_augment_kernel(
body_q: wp.array[wp.transform],
shape_body: wp.array[int],
contact_count: wp.array[int],
contact_shape0: wp.array[int],
contact_shape1: wp.array[int],
contact_point0: wp.array[wp.vec3],
contact_point1: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
contact_margin0: wp.array[float],
contact_margin1: wp.array[float],
# outputs
out_distance: wp.array[float],
out_normal: wp.array[wp.vec3],
out_point0_world: wp.array[wp.vec3],
out_point1_world: wp.array[wp.vec3],
):
"""Differentiable contact augmentation.
Transforms body-local contact points into world space through the
differentiable ``body_q`` and computes the signed contact distance.
The world-space normal is passed through from the narrow phase as-is
(frozen, no orientation gradients).
Outputs (per contact):
* ``out_distance`` — signed gap ``dot(n, p_b - p_a) - thickness`` [m].
* ``out_normal`` — world-space contact normal (frozen, equals input).
* ``out_point0_world`` — contact point on shape A in world space [m].
* ``out_point1_world`` — contact point on shape B in world space [m].
"""
tid = wp.tid()
count = contact_count[0]
if tid >= count:
return
shape_a = contact_shape0[tid]
shape_b = contact_shape1[tid]
body_a = -1
if shape_a >= 0:
body_a = shape_body[shape_a]
body_b = -1
if shape_b >= 0:
body_b = shape_body[shape_b]
X_wb_a = wp.transform_identity()
X_wb_b = wp.transform_identity()
if body_a >= 0:
X_wb_a = body_q[body_a]
if body_b >= 0:
X_wb_b = body_q[body_b]
bx_a = wp.transform_point(X_wb_a, contact_point0[tid])
bx_b = wp.transform_point(X_wb_b, contact_point1[tid])
n = contact_normal[tid]
thickness = contact_margin0[tid] + contact_margin1[tid]
d = wp.dot(n, bx_b - bx_a) - thickness
out_distance[tid] = d
out_normal[tid] = n
out_point0_world[tid] = bx_a
out_point1_world[tid] = bx_b
def launch_differentiable_contact_augment(
contacts,
body_q: wp.array,
shape_body: wp.array,
device=None,
):
"""Launch the differentiable contact augmentation kernel.
Gradients flow through the contact points and distance but the normal
direction is frozen (constant).
Args:
contacts: :class:`~newton.Contacts` instance with differentiable arrays allocated.
body_q: Body transforms, shape ``(body_count,)``, dtype :class:`wp.transform`.
shape_body: Per-shape body index, shape ``(shape_count,)``, dtype ``int``.
device: Warp device.
"""
wp.launch(
kernel=differentiable_contact_augment_kernel,
dim=contacts.rigid_contact_max,
inputs=[
body_q,
shape_body,
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_normal,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
],
outputs=[
contacts.rigid_contact_diff_distance,
contacts.rigid_contact_diff_normal,
contacts.rigid_contact_diff_point0_world,
contacts.rigid_contact_diff_point1_world,
],
device=device,
)
================================================
FILE: newton/_src/geometry/flags.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from enum import IntEnum
# Particle flags
class ParticleFlags(IntEnum):
"""
Flags for particle properties.
"""
ACTIVE = 1 << 0
"""Indicates that the particle is active."""
# Shape flags
class ShapeFlags(IntEnum):
"""
Flags for shape properties.
"""
VISIBLE = 1 << 0
"""Indicates that the shape is visible."""
COLLIDE_SHAPES = 1 << 1
"""Indicates that the shape collides with other shapes."""
COLLIDE_PARTICLES = 1 << 2
"""Indicates that the shape collides with particles."""
SITE = 1 << 3
"""Indicates that the shape is a site (non-colliding reference point)."""
HYDROELASTIC = 1 << 4
"""Indicates that the shape uses hydroelastic collision."""
__all__ = [
"ParticleFlags",
"ShapeFlags",
]
================================================
FILE: newton/_src/geometry/hashtable.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""GPU-friendly hash table for concurrent key-to-index mapping.
This module provides a generic hash table that maps keys to entry indices.
It is designed for GPU kernels where many threads insert concurrently.
Key features:
- Thread-safe insertion using atomic compare-and-swap
- Active entry tracking for efficient clearing
- Power-of-two capacity for fast modulo via bitwise AND
The hash table does NOT store values - it only maps keys to entry indices.
Callers can use these indices to access their own value storage.
"""
from __future__ import annotations
import warp as wp
# Note on uint64 constants: HASHTABLE_EMPTY_KEY and HASH_MIX_MULTIPLIER are
# defined with wp.uint64() at module scope rather than cast inside kernels.
# When a literal is cast inside a @wp.kernel or @wp.func (e.g., wp.uint64(x)),
# Warp first creates an intermediate variable with an incorrect type (signed),
# then casts to the target type. Defining the typed value at global scope and
# referencing it directly in kernels avoids this intermediate.
# On CPU builds, users may still see: "warning: integer literal is too large to
# be represented in a signed integer type, interpreting as unsigned". This is
# benign—no truncation or data loss occurs.
# See also: https://github.com/NVIDIA/warp/issues/485
# Sentinel value for empty slots
_HASHTABLE_EMPTY_KEY_VALUE = 0xFFFFFFFFFFFFFFFF
HASHTABLE_EMPTY_KEY = wp.constant(wp.uint64(_HASHTABLE_EMPTY_KEY_VALUE))
# Multiplier constant from MurmurHash3's 64-bit finalizer (fmix64)
HASH_MIX_MULTIPLIER = wp.constant(wp.uint64(0xFF51AFD7ED558CCD))
def _next_power_of_two(n: int) -> int:
"""Round up to the next power of two."""
if n <= 0:
return 1
n -= 1
n |= n >> 1
n |= n >> 2
n |= n >> 4
n |= n >> 8
n |= n >> 16
n |= n >> 32
return n + 1
@wp.func
def _hashtable_hash(key: wp.uint64, capacity_mask: int) -> int:
"""Compute hash index using a simplified mixer."""
h = key
h = h ^ (h >> wp.uint64(33))
h = h * HASH_MIX_MULTIPLIER
h = h ^ (h >> wp.uint64(33))
return int(h) & capacity_mask
@wp.func
def hashtable_find(
key: wp.uint64,
keys: wp.array[wp.uint64],
) -> int:
"""Find a key and return its entry index (read-only lookup).
This function locates an existing entry without inserting. Use this for
read-only lookups in second-pass kernels where entries should already exist.
Args:
key: The uint64 key to find
keys: The hash table keys array (length must be power of two)
Returns:
Entry index (>= 0) if found, -1 if not found
"""
capacity = keys.shape[0]
capacity_mask = capacity - 1
idx = _hashtable_hash(key, capacity_mask)
# Linear probing with a maximum of 'capacity' attempts
for _i in range(capacity):
# Read to check if key exists
stored_key = keys[idx]
if stored_key == key:
# Key found - return its index
return idx
if stored_key == HASHTABLE_EMPTY_KEY:
# Hit an empty slot - key doesn't exist
return -1
# Collision with different key - linear probe to next slot
idx = (idx + 1) & capacity_mask
# Searched entire table without finding key
return -1
@wp.func
def hashtable_find_or_insert(
key: wp.uint64,
keys: wp.array[wp.uint64],
active_slots: wp.array[wp.int32],
) -> int:
"""Find or insert a key and return the entry index.
This function locates an existing entry or creates a new one for the key.
The returned entry index can be used to access caller-managed value storage.
Args:
key: The uint64 key to find or insert
keys: The hash table keys array (length must be power of two)
active_slots: Array of size (capacity + 1) tracking active entry indices.
active_slots[capacity] is the count of active entries.
Returns:
Entry index (>= 0) if successful, -1 if the table is full
"""
capacity = keys.shape[0]
capacity_mask = capacity - 1
idx = _hashtable_hash(key, capacity_mask)
# Linear probing with a maximum of 'capacity' attempts
for _i in range(capacity):
# Read first to check if key exists (keys only transition EMPTY -> KEY)
stored_key = keys[idx]
if stored_key == key:
# Key already exists - return its index
return idx
if stored_key == HASHTABLE_EMPTY_KEY:
# Try to claim this slot
old_key = wp.atomic_cas(keys, idx, HASHTABLE_EMPTY_KEY, key)
if old_key == HASHTABLE_EMPTY_KEY:
# We claimed an empty slot - this is a NEW entry
# Add to active slots list
active_idx = wp.atomic_add(active_slots, capacity, 1)
if active_idx < capacity:
active_slots[active_idx] = idx
return idx
elif old_key == key:
# Another thread just inserted the same key - use it
return idx
# else: Another thread claimed with different key - continue probing
# Collision with different key - linear probe to next slot
idx = (idx + 1) & capacity_mask
# Table is full
return -1
@wp.kernel(enable_backward=False)
def _hashtable_clear_keys_kernel(
keys: wp.array[wp.uint64],
active_slots: wp.array[wp.int32],
capacity: int,
num_threads: int,
):
"""Kernel to clear only the active keys in the hash table.
Uses grid-stride loop for efficient thread utilization.
Reads count from GPU memory - works because all threads read before any writes.
"""
tid = wp.tid()
# Read count from GPU - stored at active_slots[capacity]
# All threads read this value before any modifications happen
count = active_slots[capacity]
# Grid-stride loop: each thread processes multiple entries if needed
i = tid
while i < count:
entry_idx = active_slots[i]
keys[entry_idx] = HASHTABLE_EMPTY_KEY
i += num_threads
@wp.kernel(enable_backward=False)
def _zero_count_kernel(
active_slots: wp.array[wp.int32],
capacity: int,
):
"""Zero the count element after clearing."""
active_slots[capacity] = 0
class HashTable:
"""Generic hash table for concurrent key-to-index mapping.
Uses open addressing with linear probing. Designed for GPU kernels
where many threads insert concurrently.
This hash table does NOT store values - it only maps keys to entry indices.
Callers can use the entry indices to access their own value storage with
whatever layout they prefer.
Attributes:
capacity: Maximum number of unique keys (power of two)
keys: Warp array storing the keys
active_slots: Array tracking active slot indices (size = capacity + 1)
device: The device where the table is allocated
"""
def __init__(self, capacity: int, device: str | None = None):
"""Initialize an empty hash table.
Args:
capacity: Maximum number of unique keys. Rounded up to power of two.
device: Warp device (e.g., "cuda:0", "cpu").
"""
self.capacity = _next_power_of_two(capacity)
self.device = device
# Allocate arrays
self.keys = wp.zeros(self.capacity, dtype=wp.uint64, device=device)
self.active_slots = wp.zeros(self.capacity + 1, dtype=wp.int32, device=device)
self.clear()
def clear(self):
"""Clear all entries in the hash table."""
self.keys.fill_(_HASHTABLE_EMPTY_KEY_VALUE)
self.active_slots.zero_()
def clear_active(self):
"""Clear only the active entries. CUDA graph capture compatible.
Uses two kernel launches:
1. Clear all active hashtable keys using grid-stride loop
2. Zero the count element
The two-kernel approach is needed to avoid race conditions on CPU where
threads execute sequentially.
"""
# Use fixed thread count to cover the GPU (65536 = 256 blocks x 256 threads)
# Grid-stride loop handles any number of active entries
num_threads = min(65536, self.capacity)
wp.launch(
_hashtable_clear_keys_kernel,
dim=num_threads,
inputs=[self.keys, self.active_slots, self.capacity, num_threads],
device=self.device,
)
# Zero the count in a separate kernel to avoid CPU race condition
wp.launch(
_zero_count_kernel,
dim=1,
inputs=[self.active_slots, self.capacity],
device=self.device,
)
================================================
FILE: newton/_src/geometry/inertia.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Helper functions for computing rigid body inertia properties."""
from __future__ import annotations
import warnings
import numpy as np
import warp as wp
from .types import (
GeoType,
Heightfield,
Mesh,
Vec3,
)
# Relative tolerance for eigenvalue positivity checks. An eigenvalue is
# considered "near-zero" only when it is smaller than this fraction of the
# largest eigenvalue. This prevents spurious inflation of physically correct
# but small inertia values (e.g. lightweight gripper pads).
_INERTIA_REL_TOL = 1.0e-6
# Absolute floor for the eigenvalue check when max_eigenvalue itself is ~0
# (degenerate tensor). Must be well below the smallest physically meaningful
# eigenvalue we want to preserve (order ~1e-7 for lightweight gripper pads).
_INERTIA_ABS_FLOOR = 1.0e-10
# Absolute value used when an eigenvalue correction *is* triggered. This
# keeps the corrected tensor well-conditioned (e.g. singular inertia [0,0,0]
# becomes [1e-6, 1e-6, 1e-6]).
_INERTIA_ABS_ADJUSTMENT = 1.0e-6
# Match numpy's default np.allclose() tolerances when deciding whether a
# nearly-symmetric tensor should be treated as unchanged.
_INERTIA_SYMMETRY_RTOL = 1.0e-5
_INERTIA_SYMMETRY_ATOL = 1.0e-8
def compute_inertia_sphere(density: float, radius: float) -> tuple[float, wp.vec3, wp.mat33]:
"""Helper to compute mass and inertia of a solid sphere
Args:
density: The sphere density [kg/m³]
radius: The sphere radius [m]
Returns:
A tuple of (mass, center of mass, inertia) with inertia specified around the center of mass
"""
v = 4.0 / 3.0 * wp.pi * radius * radius * radius
m = density * v
Ia = 2.0 / 5.0 * m * radius * radius
I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ia]])
return (m, wp.vec3(), I)
def compute_inertia_capsule(density: float, radius: float, half_height: float) -> tuple[float, wp.vec3, wp.mat33]:
"""Helper to compute mass and inertia of a solid capsule extending along the z-axis
Args:
density: The capsule density [kg/m³]
radius: The capsule radius [m]
half_height: Half-length of the cylindrical section (excluding hemispherical caps) [m]
Returns:
A tuple of (mass, center of mass, inertia) with inertia specified around the center of mass
"""
h = 2.0 * half_height # full height of the cylindrical section
ms = density * (4.0 / 3.0) * wp.pi * radius * radius * radius
mc = density * wp.pi * radius * radius * h
# total mass
m = ms + mc
# adapted from ODE
Ia = mc * (0.25 * radius * radius + (1.0 / 12.0) * h * h) + ms * (
0.4 * radius * radius + 0.375 * radius * h + 0.25 * h * h
)
Ib = (mc * 0.5 + ms * 0.4) * radius * radius
# For Z-axis orientation: I_xx = I_yy = Ia, I_zz = Ib
I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]])
return (m, wp.vec3(), I)
def compute_inertia_cylinder(density: float, radius: float, half_height: float) -> tuple[float, wp.vec3, wp.mat33]:
"""Helper to compute mass and inertia of a solid cylinder extending along the z-axis
Args:
density: The cylinder density [kg/m³]
radius: The cylinder radius [m]
half_height: The half-height of the cylinder along the z-axis [m]
Returns:
A tuple of (mass, center of mass, inertia) with inertia specified around the center of mass
"""
h = 2.0 * half_height # full height
m = density * wp.pi * radius * radius * h
Ia = 1 / 12 * m * (3 * radius * radius + h * h)
Ib = 1 / 2 * m * radius * radius
# For Z-axis orientation: I_xx = I_yy = Ia, I_zz = Ib
I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]])
return (m, wp.vec3(), I)
def compute_inertia_cone(density: float, radius: float, half_height: float) -> tuple[float, wp.vec3, wp.mat33]:
"""Helper to compute mass and inertia of a solid cone extending along the z-axis
Args:
density: The cone density [kg/m³]
radius: The cone base radius [m]
half_height: The half-height of the cone (distance from geometric center to base or apex) [m]
Returns:
A tuple of (mass, center of mass, inertia) with inertia specified around the center of mass
"""
h = 2.0 * half_height # full height
m = density * wp.pi * radius * radius * h / 3.0
# Center of mass is at -h/4 from the geometric center
# Since the cone has base at -h/2 and apex at +h/2, the COM is 1/4 of the height from base toward apex
com = wp.vec3(0.0, 0.0, -h / 4.0)
# Inertia about the center of mass
Ia = 3 / 20 * m * radius * radius + 3 / 80 * m * h * h
Ib = 3 / 10 * m * radius * radius
# For Z-axis orientation: I_xx = I_yy = Ia, I_zz = Ib
I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]])
return (m, com, I)
def compute_inertia_ellipsoid(density: float, rx: float, ry: float, rz: float) -> tuple[float, wp.vec3, wp.mat33]:
"""Helper to compute mass and inertia of a solid ellipsoid
The ellipsoid is centered at the origin with semi-axes rx, ry, rz along the x, y, z axes respectively.
Args:
density: The ellipsoid density [kg/m³]
rx: The semi-axis along the x-axis [m]
ry: The semi-axis along the y-axis [m]
rz: The semi-axis along the z-axis [m]
Returns:
A tuple of (mass, center of mass, inertia) with inertia specified around the center of mass
"""
# Volume of ellipsoid: V = (4/3) * pi * rx * ry * rz
v = (4.0 / 3.0) * wp.pi * rx * ry * rz
m = density * v
# Inertia tensor for a solid ellipsoid about its center of mass:
# Ixx = (1/5) * m * (ry² + rz²)
# Iyy = (1/5) * m * (rx² + rz²)
# Izz = (1/5) * m * (rx² + ry²)
Ixx = (1.0 / 5.0) * m * (ry * ry + rz * rz)
Iyy = (1.0 / 5.0) * m * (rx * rx + rz * rz)
Izz = (1.0 / 5.0) * m * (rx * rx + ry * ry)
I = wp.mat33([[Ixx, 0.0, 0.0], [0.0, Iyy, 0.0], [0.0, 0.0, Izz]])
return (m, wp.vec3(), I)
def compute_inertia_box_from_mass(mass: float, hx: float, hy: float, hz: float) -> wp.mat33:
"""Helper to compute 3x3 inertia matrix of a solid box with given mass and half-extents.
Args:
mass: The box mass [kg]
hx: The box half-extent along the x-axis [m]
hy: The box half-extent along the y-axis [m]
hz: The box half-extent along the z-axis [m]
Returns:
A 3x3 inertia matrix with inertia specified around the center of mass
"""
Ia = 1.0 / 3.0 * mass * (hy * hy + hz * hz)
Ib = 1.0 / 3.0 * mass * (hx * hx + hz * hz)
Ic = 1.0 / 3.0 * mass * (hx * hx + hy * hy)
I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ib, 0.0], [0.0, 0.0, Ic]])
return I
def compute_inertia_box(density: float, hx: float, hy: float, hz: float) -> tuple[float, wp.vec3, wp.mat33]:
"""Helper to compute mass and inertia of a solid box
Args:
density: The box density [kg/m³]
hx: The box half-extent along the x-axis [m]
hy: The box half-extent along the y-axis [m]
hz: The box half-extent along the z-axis [m]
Returns:
A tuple of (mass, center of mass, inertia) with inertia specified around the center of mass
"""
v = 8.0 * hx * hy * hz
m = density * v
I = compute_inertia_box_from_mass(m, hx, hy, hz)
return (m, wp.vec3(), I)
@wp.func
def triangle_inertia(
v0: wp.vec3,
v1: wp.vec3,
v2: wp.vec3,
):
vol = wp.dot(v0, wp.cross(v1, v2)) / 6.0 # tetra volume (0,v0,v1,v2)
first = vol * (v0 + v1 + v2) / 4.0 # first-order integral
# second-order integral (symmetric)
o00, o11, o22 = wp.outer(v0, v0), wp.outer(v1, v1), wp.outer(v2, v2)
o01, o02, o12 = wp.outer(v0, v1), wp.outer(v0, v2), wp.outer(v1, v2)
o01t, o02t, o12t = wp.transpose(o01), wp.transpose(o02), wp.transpose(o12)
second = (vol / 10.0) * (o00 + o11 + o22)
second += (vol / 20.0) * (o01 + o01t + o02 + o02t + o12 + o12t)
return vol, first, second
@wp.kernel
def compute_solid_mesh_inertia(
indices: wp.array[int],
vertices: wp.array[wp.vec3],
# outputs
volume: wp.array[float],
first: wp.array[wp.vec3],
second: wp.array[wp.mat33],
):
i = wp.tid()
p = vertices[indices[i * 3 + 0]]
q = vertices[indices[i * 3 + 1]]
r = vertices[indices[i * 3 + 2]]
v, f, s = triangle_inertia(p, q, r)
wp.atomic_add(volume, 0, v)
wp.atomic_add(first, 0, f)
wp.atomic_add(second, 0, s)
@wp.kernel
def compute_hollow_mesh_inertia(
indices: wp.array[int],
vertices: wp.array[wp.vec3],
thickness: wp.array[float],
# outputs
volume: wp.array[float],
first: wp.array[wp.vec3],
second: wp.array[wp.mat33],
):
tid = wp.tid()
i = indices[tid * 3 + 0]
j = indices[tid * 3 + 1]
k = indices[tid * 3 + 2]
vi = vertices[i]
vj = vertices[j]
vk = vertices[k]
normal = -wp.normalize(wp.cross(vj - vi, vk - vi))
ti = normal * thickness[i]
tj = normal * thickness[j]
tk = normal * thickness[k]
# wedge vertices
vi0 = vi - ti
vi1 = vi + ti
vj0 = vj - tj
vj1 = vj + tj
vk0 = vk - tk
vk1 = vk + tk
v_total = 0.0
f_total = wp.vec3(0.0)
s_total = wp.mat33(0.0)
v, f, s = triangle_inertia(vi0, vj0, vk0)
v_total += v
f_total += f
s_total += s
v, f, s = triangle_inertia(vj0, vk1, vk0)
v_total += v
f_total += f
s_total += s
v, f, s = triangle_inertia(vj0, vj1, vk1)
v_total += v
f_total += f
s_total += s
v, f, s = triangle_inertia(vj0, vi1, vj1)
v_total += v
f_total += f
s_total += s
v, f, s = triangle_inertia(vj0, vi0, vi1)
v_total += v
f_total += f
s_total += s
v, f, s = triangle_inertia(vj1, vi1, vk1)
v_total += v
f_total += f
s_total += s
v, f, s = triangle_inertia(vi1, vi0, vk0)
v_total += v
f_total += f
s_total += s
v, f, s = triangle_inertia(vi1, vk0, vk1)
v_total += v
f_total += f
s_total += s
wp.atomic_add(volume, 0, v_total)
wp.atomic_add(first, 0, f_total)
wp.atomic_add(second, 0, s_total)
def compute_inertia_mesh(
density: float,
vertices: list[Vec3] | np.ndarray,
indices: list[int] | np.ndarray,
is_solid: bool = True,
thickness: list[float] | float = 0.001,
) -> tuple[float, wp.vec3, wp.mat33, float]:
"""
Compute the mass, center of mass, inertia, and volume of a triangular mesh.
Args:
density: The density of the mesh material.
vertices: A list of vertex positions (3D coordinates).
indices: A list of triangle indices (each triangle is defined by 3 vertex indices).
is_solid: If True, compute inertia for a solid mesh; if False, for a hollow mesh using the given thickness.
thickness: Thickness of the mesh if it is hollow. Can be a single value or a list of values for each vertex.
Returns:
A tuple containing:
- mass: The mass of the mesh.
- com: The center of mass (3D coordinates).
- I: The inertia tensor (3x3 matrix).
- volume: The signed volume of the mesh.
"""
indices = np.array(indices).flatten()
num_tris = len(indices) // 3
# Allocating for mass and inertia
com_warp = wp.zeros(1, dtype=wp.vec3)
I_warp = wp.zeros(1, dtype=wp.mat33)
vol_warp = wp.zeros(1, dtype=float)
wp_vertices = wp.array(vertices, dtype=wp.vec3)
wp_indices = wp.array(indices, dtype=int)
if is_solid:
wp.launch(
kernel=compute_solid_mesh_inertia,
dim=num_tris,
inputs=[
wp_indices,
wp_vertices,
],
outputs=[
vol_warp,
com_warp,
I_warp,
],
)
else:
if isinstance(thickness, float):
thickness = [thickness] * len(vertices)
wp.launch(
kernel=compute_hollow_mesh_inertia,
dim=num_tris,
inputs=[
wp_indices,
wp_vertices,
wp.array(thickness, dtype=float),
],
outputs=[
vol_warp,
com_warp,
I_warp,
],
)
V_tot = float(vol_warp.numpy()[0]) # signed volume
F_tot = com_warp.numpy()[0] # first moment
S_tot = I_warp.numpy()[0] # second moment
# If the winding is inward, flip signs
if V_tot < 0:
V_tot = -V_tot
F_tot = -F_tot
S_tot = -S_tot
mass = density * V_tot
if V_tot > 0.0:
com = F_tot / V_tot
else:
com = F_tot
S_tot *= density # include density
I_origin = np.trace(S_tot) * np.eye(3) - S_tot # inertia about origin
r = com
I_com = I_origin - mass * ((r @ r) * np.eye(3) - np.outer(r, r))
return mass, wp.vec3(*com), wp.mat33(*I_com), V_tot
@wp.func
def transform_inertia(mass: float, inertia: wp.mat33, offset: wp.vec3, quat: wp.quat) -> wp.mat33:
"""
Compute a rigid body's inertia tensor expressed in a new coordinate frame.
The transformation applies (1) a rotation by quaternion ``quat`` and
(2) a parallel-axis shift by vector ``offset`` (Steiner's theorem).
Let ``R`` be the rotation matrix corresponding to ``quat``. The returned
inertia tensor :math:`\\mathbf{I}'` is
.. math::
\\mathbf{I}' \\,=\\, \\mathbf{R}\\,\\mathbf{I}\\,\\mathbf{R}^\\top
\\, + \\, m\\big(\\lVert\\mathbf{p}\\rVert^2\\,\\mathbf{I}_3
\\, - \\, \\mathbf{p}\\,\\mathbf{p}^\\top\\big),
where :math:`\\mathbf{I}_3` is the :math:`3\\times3` identity matrix.
Args:
mass: Mass of the rigid body.
inertia: Inertia tensor expressed in the body's local frame, relative
to its center of mass.
offset: Position vector from the new frame's origin to the body's
center of mass.
quat: Orientation of the body relative to the new frame, expressed
as a quaternion.
Returns:
wp.mat33: The transformed inertia tensor expressed in the new frame.
"""
R = wp.quat_to_matrix(quat)
# Steiner's theorem
return R @ inertia @ wp.transpose(R) + mass * (
wp.dot(offset, offset) * wp.mat33(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) - wp.outer(offset, offset)
)
def compute_inertia_shape(
type: int,
scale: Vec3,
src: Mesh | Heightfield | None,
density: float,
is_solid: bool = True,
thickness: list[float] | float = 0.001,
) -> tuple[float, wp.vec3, wp.mat33]:
"""Computes the mass, center of mass and 3x3 inertia tensor of a shape
Args:
type: The type of shape (GeoType.SPHERE, GeoType.BOX, etc.)
scale: The scale of the shape
src: The source shape (Mesh or Heightfield)
density: The density of the shape
is_solid: Whether the shape is solid or hollow
thickness: The thickness of the shape (used for collision detection, and inertia computation of hollow shapes)
Returns:
The mass, center of mass and 3x3 inertia tensor of the shape
"""
if density == 0.0 or type == GeoType.PLANE: # zero density means fixed
return 0.0, wp.vec3(), wp.mat33()
if type == GeoType.SPHERE:
solid = compute_inertia_sphere(density, scale[0])
if is_solid:
return solid
else:
assert isinstance(thickness, float), "thickness must be a float for a hollow sphere geom"
hollow = compute_inertia_sphere(density, scale[0] - thickness)
return solid[0] - hollow[0], solid[1], solid[2] - hollow[2]
elif type == GeoType.BOX:
# scale stores half-extents (hx, hy, hz)
solid = compute_inertia_box(density, scale[0], scale[1], scale[2])
if is_solid:
return solid
else:
assert isinstance(thickness, float), "thickness must be a float for a hollow box geom"
hollow = compute_inertia_box(density, scale[0] - thickness, scale[1] - thickness, scale[2] - thickness)
return solid[0] - hollow[0], solid[1], solid[2] - hollow[2]
elif type == GeoType.CAPSULE:
# scale[0] = radius, scale[1] = half_height
solid = compute_inertia_capsule(density, scale[0], scale[1])
if is_solid:
return solid
else:
assert isinstance(thickness, float), "thickness must be a float for a hollow capsule geom"
hollow = compute_inertia_capsule(density, scale[0] - thickness, scale[1] - thickness)
return solid[0] - hollow[0], solid[1], solid[2] - hollow[2]
elif type == GeoType.CYLINDER:
# scale[0] = radius, scale[1] = half_height
solid = compute_inertia_cylinder(density, scale[0], scale[1])
if is_solid:
return solid
else:
assert isinstance(thickness, float), "thickness must be a float for a hollow cylinder geom"
hollow = compute_inertia_cylinder(density, scale[0] - thickness, scale[1] - thickness)
return solid[0] - hollow[0], solid[1], solid[2] - hollow[2]
elif type == GeoType.CONE:
# scale[0] = radius, scale[1] = half_height
solid = compute_inertia_cone(density, scale[0], scale[1])
if is_solid:
return solid
else:
assert isinstance(thickness, float), "thickness must be a float for a hollow cone geom"
hollow = compute_inertia_cone(density, scale[0] - thickness, scale[1] - thickness)
m_shell = solid[0] - hollow[0]
if m_shell <= 0.0:
raise ValueError(
f"Hollow cone shell has non-positive mass ({m_shell:.6g}). "
f"The thickness ({thickness}) must be smaller than both the "
f"radius ({scale[0]}) and half_height ({scale[1]})."
)
# Cones have non-zero COM so outer and inner cones have different COMs;
# compute the shell COM as the weighted difference, then shift both
# inertia tensors to the shell COM before subtracting (parallel-axis theorem).
com_s = np.array(solid[1])
com_i = np.array(hollow[1])
com_shell = (solid[0] * com_s - hollow[0] * com_i) / m_shell
def _shift_inertia(mass, I_mat, com_from, com_to):
d = com_to - np.array(com_from)
return np.array(I_mat).reshape(3, 3) + mass * (np.dot(d, d) * np.eye(3) - np.outer(d, d))
I_shell = _shift_inertia(solid[0], solid[2], com_s, com_shell) - _shift_inertia(
hollow[0], hollow[2], com_i, com_shell
)
return m_shell, wp.vec3(*com_shell), wp.mat33(*I_shell.flatten())
elif type == GeoType.ELLIPSOID:
# scale stores semi-axes (rx, ry, rz)
solid = compute_inertia_ellipsoid(density, scale[0], scale[1], scale[2])
if is_solid:
return solid
else:
assert isinstance(thickness, float), "thickness must be a float for a hollow ellipsoid geom"
hollow = compute_inertia_ellipsoid(
density, scale[0] - thickness, scale[1] - thickness, scale[2] - thickness
)
return solid[0] - hollow[0], solid[1], solid[2] - hollow[2]
elif type == GeoType.HFIELD or type == GeoType.GAUSSIAN:
# Heightfields are always static terrain; Gaussians are render-only (zero mass, zero inertia)
return 0.0, wp.vec3(), wp.mat33()
elif type == GeoType.MESH or type == GeoType.CONVEX_MESH:
assert src is not None, "src must be provided for mesh or convex hull shapes"
if src.has_inertia and src.mass > 0.0 and src.is_solid == is_solid:
m, c, I = src.mass, src.com, src.inertia
scale = wp.vec3(scale)
sx, sy, sz = scale
mass_ratio = sx * sy * sz * density
m_new = m * mass_ratio
c_new = wp.cw_mul(c, scale)
Ixx = I[0, 0] * (sy**2 + sz**2) / 2 * mass_ratio
Iyy = I[1, 1] * (sx**2 + sz**2) / 2 * mass_ratio
Izz = I[2, 2] * (sx**2 + sy**2) / 2 * mass_ratio
Ixy = I[0, 1] * sx * sy * mass_ratio
Ixz = I[0, 2] * sx * sz * mass_ratio
Iyz = I[1, 2] * sy * sz * mass_ratio
I_new = wp.mat33([[Ixx, Ixy, Ixz], [Ixy, Iyy, Iyz], [Ixz, Iyz, Izz]])
return m_new, c_new, I_new
elif type == GeoType.MESH or type == GeoType.CONVEX_MESH:
assert isinstance(src, Mesh), "src must be a Mesh for mesh or convex hull shapes"
# fall back to computing inertia from mesh geometry
vertices = np.array(src.vertices) * np.array(scale)
m, c, I, _vol = compute_inertia_mesh(density, vertices, src.indices, is_solid, thickness)
return m, c, I
raise ValueError(f"Unsupported shape type: {type}")
def verify_and_correct_inertia(
mass: float,
inertia: wp.mat33,
balance_inertia: bool = True,
bound_mass: float | None = None,
bound_inertia: float | None = None,
body_label: str | None = None,
) -> tuple[float, wp.mat33, bool]:
"""Verify and correct inertia values similar to MuJoCo's balanceinertia compiler setting.
This function checks for invalid inertia values and corrects them if needed. It performs
the following checks and corrections:
1. Ensures mass is non-negative (and bounded if specified)
2. Ensures inertia diagonal elements are non-negative (and bounded if specified)
3. Ensures inertia matrix satisfies triangle inequality (principal moments satisfy Ixx + Iyy >= Izz etc.)
4. Optionally balances inertia to satisfy the triangle inequality exactly
Eigenvalue positivity is checked using a relative threshold
(``_INERTIA_REL_TOL * max_eigenvalue``) so that lightweight components with
small but physically valid inertia are not spuriously inflated. When
correction *is* needed, the adjustment uses a small absolute floor
(``_INERTIA_ABS_ADJUSTMENT``) to keep the result well-conditioned.
Args:
mass: The mass of the body [kg].
inertia: The 3x3 inertia tensor [kg*m^2].
balance_inertia: If True, adjust inertia to exactly satisfy triangle inequality (like MuJoCo's balanceinertia).
bound_mass: If specified, clamp mass to be at least this value [kg].
bound_inertia: If specified, clamp inertia diagonal elements to be at least this value [kg*m^2].
body_label: Optional label/name of the body for more informative warnings.
Returns:
A tuple of (corrected_mass, corrected_inertia, was_corrected) where was_corrected
indicates if any corrections were made
"""
was_corrected = False
corrected_mass = mass
inertia_array = np.array(inertia).reshape(3, 3)
corrected_inertia = inertia_array.copy()
# Format body identifier for warnings
body_id = f" for body '{body_label}'" if body_label else ""
# Check for NaN/Inf in mass or inertia
if not np.isfinite(mass) or not np.all(np.isfinite(inertia_array)):
warnings.warn(
f"NaN/Inf detected in mass or inertia{body_id}, zeroing out mass and inertia",
stacklevel=2,
)
return 0.0, wp.mat33(np.zeros((3, 3))), True
# Check and correct mass
if mass < 0:
warnings.warn(f"Negative mass {mass} detected{body_id}, setting to 0", stacklevel=2)
corrected_mass = 0.0
was_corrected = True
elif bound_mass is not None and mass < bound_mass and mass > 0:
warnings.warn(f"Mass {mass} is below bound {bound_mass}{body_id}, clamping", stacklevel=2)
corrected_mass = bound_mass
was_corrected = True
# For zero mass, inertia should also be zero
if corrected_mass == 0.0:
if np.any(inertia_array != 0):
warnings.warn(f"Zero mass body{body_id} should have zero inertia, correcting", stacklevel=2)
corrected_inertia = np.zeros((3, 3))
was_corrected = True
return corrected_mass, wp.mat33(corrected_inertia), was_corrected
# Unconditionally symmetrize inertia matrix (idempotent for symmetric tensors)
symmetrized = (inertia_array + inertia_array.T) / 2
if not np.allclose(
inertia_array,
symmetrized,
rtol=_INERTIA_SYMMETRY_RTOL,
atol=_INERTIA_SYMMETRY_ATOL,
):
warnings.warn(f"Inertia matrix{body_id} is not symmetric, making it symmetric", stacklevel=2)
was_corrected = True
corrected_inertia = symmetrized
# Compute eigenvalues (principal moments) for validation
try:
eigenvalues = np.linalg.eigvals(corrected_inertia)
# Check for negative or near-zero eigenvalues (ensure positive-definite).
# The threshold is relative to the largest eigenvalue so that small but
# physically valid inertia (lightweight components) is not inflated.
max_eig = np.max(eigenvalues)
eig_threshold = max(_INERTIA_REL_TOL * max_eig, _INERTIA_ABS_FLOOR)
if np.any(eigenvalues < eig_threshold):
warnings.warn(
f"Eigenvalues below threshold detected{body_id}: {eigenvalues}, correcting inertia",
stacklevel=2,
)
# Make positive definite by adjusting eigenvalues
min_eig = np.min(eigenvalues)
adjustment = eig_threshold - min_eig + _INERTIA_ABS_ADJUSTMENT
corrected_inertia += np.eye(3) * adjustment
eigenvalues += adjustment
was_corrected = True
# Apply inertia bounds to eigenvalues if specified
if bound_inertia is not None:
min_eig = np.min(eigenvalues)
if min_eig < bound_inertia:
warnings.warn(
f"Minimum eigenvalue {min_eig} is below bound {bound_inertia}{body_id}, adjusting", stacklevel=2
)
adjustment = bound_inertia - min_eig
corrected_inertia += np.eye(3) * adjustment
eigenvalues += adjustment
was_corrected = True
# Sort eigenvalues to get principal moments
principal_moments = np.sort(eigenvalues)
I1, I2, I3 = principal_moments
# Check triangle inequality on principal moments
# For a physically valid inertia tensor: I1 + I2 >= I3 (with tolerance)
# Use float32 machine epsilon scaled by I3 as numerical noise floor.
tri_tol = max(np.finfo(np.float32).eps * I3, _INERTIA_ABS_FLOOR)
has_violations = I1 + I2 < I3 - tri_tol
except np.linalg.LinAlgError:
warnings.warn(f"Failed to compute eigenvalues for inertia tensor{body_id}, making it diagonal", stacklevel=2)
was_corrected = True
# Fallback: use diagonal elements
trace = np.trace(corrected_inertia)
if trace <= 0:
trace = _INERTIA_ABS_ADJUSTMENT
corrected_inertia = np.eye(3) * (trace / 3.0)
has_violations = False
principal_moments = [trace / 3.0, trace / 3.0, trace / 3.0]
eigenvalues = np.array(principal_moments)
if has_violations:
warnings.warn(
f"Inertia tensor{body_id} violates triangle inequality with principal moments ({I1:.6f}, {I2:.6f}, {I3:.6f})",
stacklevel=2,
)
if balance_inertia:
# For non-diagonal matrices, we need to adjust while preserving the rotation
deficit = I3 - I1 - I2
if deficit > 0:
# Simple approach: add scalar to all eigenvalues to ensure validity
# This preserves eigenvectors exactly
# We need: (I1 + a) + (I2 + a) >= I3 + a
# Which simplifies to: I1 + I2 + a >= I3
# So: a >= I3 - I1 - I2 = deficit
adjustment = deficit + _INERTIA_ABS_ADJUSTMENT
# Add scalar*I to shift all eigenvalues equally
corrected_inertia = corrected_inertia + np.eye(3) * adjustment
was_corrected = True
# Update principal moments
new_I1 = I1 + adjustment
new_I2 = I2 + adjustment
new_I3 = I3 + adjustment
warnings.warn(
f"Balanced principal moments{body_id} from ({I1:.6f}, {I2:.6f}, {I3:.6f}) to "
f"({new_I1:.6f}, {new_I2:.6f}, {new_I3:.6f})",
stacklevel=2,
)
# Final check: ensure the corrected inertia matrix is positive definite
if has_violations and balance_inertia:
# Need to recompute after balancing since we modified the matrix
try:
eigenvalues = np.linalg.eigvals(corrected_inertia)
except np.linalg.LinAlgError:
warnings.warn(f"Failed to compute eigenvalues of inertia matrix{body_id}", stacklevel=2)
eigenvalues = np.array([0.0, 0.0, 0.0])
# Check final eigenvalues
if np.any(eigenvalues <= 0) or np.any(~np.isfinite(eigenvalues)):
warnings.warn(
f"Corrected inertia matrix{body_id} is not positive definite, this should not happen", stacklevel=2
)
# As a last resort, make it positive definite by adding a small value to diagonal.
min_eigenvalue = (
np.min(eigenvalues[np.isfinite(eigenvalues)])
if np.any(np.isfinite(eigenvalues))
else -_INERTIA_ABS_ADJUSTMENT
)
epsilon = abs(min_eigenvalue) + _INERTIA_ABS_ADJUSTMENT
corrected_inertia[0, 0] += epsilon
corrected_inertia[1, 1] += epsilon
corrected_inertia[2, 2] += epsilon
was_corrected = True
return corrected_mass, wp.mat33(corrected_inertia), was_corrected
@wp.kernel(enable_backward=False, module="unique")
def validate_and_correct_inertia_kernel(
body_mass: wp.array[wp.float32],
body_inertia: wp.array[wp.mat33],
body_inv_mass: wp.array[wp.float32],
body_inv_inertia: wp.array[wp.mat33],
balance_inertia: wp.bool,
bound_mass: wp.float32,
bound_inertia: wp.float32,
correction_count: wp.array[wp.int32], # Output: atomic counter of corrected bodies
):
"""Warp kernel for parallel inertia validation and correction.
This kernel performs basic validation and correction but doesn't support:
- Warning messages (handled by caller)
- Complex iterative balancing (falls back to simple correction)
"""
tid = wp.tid()
mass = body_mass[tid]
inertia = body_inertia[tid]
original_inertia = inertia
was_corrected = False
# Detect NaN/Inf in mass or any inertia coefficient and zero out
if (
not wp.isfinite(mass)
or not wp.isfinite(inertia[0, 0])
or not wp.isfinite(inertia[0, 1])
or not wp.isfinite(inertia[0, 2])
or not wp.isfinite(inertia[1, 0])
or not wp.isfinite(inertia[1, 1])
or not wp.isfinite(inertia[1, 2])
or not wp.isfinite(inertia[2, 0])
or not wp.isfinite(inertia[2, 1])
or not wp.isfinite(inertia[2, 2])
):
mass = 0.0
inertia = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
was_corrected = True
# Check for negative mass
if mass < 0.0:
mass = 0.0
was_corrected = True
# Apply mass bound (only to positive mass; zero mass means static/fixed body)
if bound_mass > 0.0 and mass < bound_mass and mass > 0.0:
mass = bound_mass
was_corrected = True
# For zero mass, inertia should be zero
if mass == 0.0:
was_corrected = was_corrected or (wp.ddot(inertia, inertia) > 0.0)
inertia = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
else:
# Symmetrize inertia matrix: (I + I^T) / 2
sym01 = (inertia[0, 1] + inertia[1, 0]) * 0.5
sym02 = (inertia[0, 2] + inertia[2, 0]) * 0.5
sym12 = (inertia[1, 2] + inertia[2, 1]) * 0.5
sym = wp.mat33(
inertia[0, 0],
sym01,
sym02,
sym01,
inertia[1, 1],
sym12,
sym02,
sym12,
inertia[2, 2],
)
tol01 = _INERTIA_SYMMETRY_ATOL + _INERTIA_SYMMETRY_RTOL * wp.abs(sym01)
tol02 = _INERTIA_SYMMETRY_ATOL + _INERTIA_SYMMETRY_RTOL * wp.abs(sym02)
tol12 = _INERTIA_SYMMETRY_ATOL + _INERTIA_SYMMETRY_RTOL * wp.abs(sym12)
if (
wp.abs(inertia[0, 1] - sym01) > tol01
or wp.abs(inertia[1, 0] - sym01) > tol01
or wp.abs(inertia[0, 2] - sym02) > tol02
or wp.abs(inertia[2, 0] - sym02) > tol02
or wp.abs(inertia[1, 2] - sym12) > tol12
or wp.abs(inertia[2, 1] - sym12) > tol12
):
was_corrected = True
inertia = sym
# Use eigendecomposition for proper validation
_eigvecs, eigvals = wp.eig3(inertia)
# Sort eigenvalues to get principal moments (I1 <= I2 <= I3)
I1, I2, I3 = eigvals[0], eigvals[1], eigvals[2]
if I1 > I2:
I1, I2 = I2, I1
if I2 > I3:
I2, I3 = I3, I2
if I1 > I2:
I1, I2 = I2, I1
# Check for negative or near-zero eigenvalues (ensure positive-definite).
# Use a relative threshold so lightweight components are not inflated.
eig_threshold = wp.max(1.0e-6 * I3, 1.0e-10)
if I1 < eig_threshold:
adjustment = eig_threshold - I1 + 1.0e-6
# Add scalar to all eigenvalues
I1 += adjustment
I2 += adjustment
I3 += adjustment
inertia = inertia + wp.mat33(adjustment, 0.0, 0.0, 0.0, adjustment, 0.0, 0.0, 0.0, adjustment)
was_corrected = True
# Apply eigenvalue bounds
if bound_inertia > 0.0 and I1 < bound_inertia:
adjustment = bound_inertia - I1
I1 += adjustment
I2 += adjustment
I3 += adjustment
inertia = inertia + wp.mat33(adjustment, 0.0, 0.0, 0.0, adjustment, 0.0, 0.0, 0.0, adjustment)
was_corrected = True
# Check triangle inequality: I1 + I2 >= I3 (with tolerance)
tri_tol = wp.max(1.1920929e-7 * I3, 1.0e-10) # float32 eps * I3
if balance_inertia and (I1 + I2 < I3 - tri_tol):
deficit = I3 - I1 - I2
adjustment = deficit + 1.0e-6
# Add scalar*I to fix triangle inequality
inertia = inertia + wp.mat33(adjustment, 0.0, 0.0, 0.0, adjustment, 0.0, 0.0, 0.0, adjustment)
was_corrected = True
output_inertia = inertia if was_corrected else original_inertia
# Write back corrected values
body_mass[tid] = mass
body_inertia[tid] = output_inertia
# Update inverse mass
if mass > 0.0:
body_inv_mass[tid] = 1.0 / mass
else:
body_inv_mass[tid] = 0.0
# Update inverse inertia
if mass > 0.0:
body_inv_inertia[tid] = wp.inverse(output_inertia)
else:
body_inv_inertia[tid] = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
if was_corrected:
wp.atomic_add(correction_count, 0, 1)
================================================
FILE: newton/_src/geometry/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ..utils.heightfield import HeightfieldData, sample_sdf_grad_heightfield
from .broad_phase_common import binary_search
from .flags import ParticleFlags, ShapeFlags
from .types import (
Axis,
GeoType,
)
@wp.func
def triangle_closest_point_barycentric(a: wp.vec3, b: wp.vec3, c: wp.vec3, p: wp.vec3):
ab = b - a
ac = c - a
ap = p - a
d1 = wp.dot(ab, ap)
d2 = wp.dot(ac, ap)
if d1 <= 0.0 and d2 <= 0.0:
return wp.vec3(1.0, 0.0, 0.0)
bp = p - b
d3 = wp.dot(ab, bp)
d4 = wp.dot(ac, bp)
if d3 >= 0.0 and d4 <= d3:
return wp.vec3(0.0, 1.0, 0.0)
vc = d1 * d4 - d3 * d2
v = d1 / (d1 - d3)
if vc <= 0.0 and d1 >= 0.0 and d3 <= 0.0:
return wp.vec3(1.0 - v, v, 0.0)
cp = p - c
d5 = wp.dot(ab, cp)
d6 = wp.dot(ac, cp)
if d6 >= 0.0 and d5 <= d6:
return wp.vec3(0.0, 0.0, 1.0)
vb = d5 * d2 - d1 * d6
w = d2 / (d2 - d6)
if vb <= 0.0 and d2 >= 0.0 and d6 <= 0.0:
return wp.vec3(1.0 - w, 0.0, w)
va = d3 * d6 - d5 * d4
w = (d4 - d3) / ((d4 - d3) + (d5 - d6))
if va <= 0.0 and (d4 - d3) >= 0.0 and (d5 - d6) >= 0.0:
return wp.vec3(0.0, 1.0 - w, w)
denom = 1.0 / (va + vb + vc)
v = vb * denom
w = vc * denom
return wp.vec3(1.0 - v - w, v, w)
@wp.func
def triangle_closest_point(a: wp.vec3, b: wp.vec3, c: wp.vec3, p: wp.vec3):
"""
feature_type type:
TRI_CONTACT_FEATURE_VERTEX_A
TRI_CONTACT_FEATURE_VERTEX_B
TRI_CONTACT_FEATURE_VERTEX_C
TRI_CONTACT_FEATURE_EDGE_AB : at edge A-B
TRI_CONTACT_FEATURE_EDGE_AC : at edge A-C
TRI_CONTACT_FEATURE_EDGE_BC : at edge B-C
TRI_CONTACT_FEATURE_FACE_INTERIOR
"""
ab = b - a
ac = c - a
ap = p - a
d1 = wp.dot(ab, ap)
d2 = wp.dot(ac, ap)
if d1 <= 0.0 and d2 <= 0.0:
feature_type = TRI_CONTACT_FEATURE_VERTEX_A
bary = wp.vec3(1.0, 0.0, 0.0)
return a, bary, feature_type
bp = p - b
d3 = wp.dot(ab, bp)
d4 = wp.dot(ac, bp)
if d3 >= 0.0 and d4 <= d3:
feature_type = TRI_CONTACT_FEATURE_VERTEX_B
bary = wp.vec3(0.0, 1.0, 0.0)
return b, bary, feature_type
cp = p - c
d5 = wp.dot(ab, cp)
d6 = wp.dot(ac, cp)
if d6 >= 0.0 and d5 <= d6:
feature_type = TRI_CONTACT_FEATURE_VERTEX_C
bary = wp.vec3(0.0, 0.0, 1.0)
return c, bary, feature_type
vc = d1 * d4 - d3 * d2
if vc <= 0.0 and d1 >= 0.0 and d3 <= 0.0:
v = d1 / (d1 - d3)
feature_type = TRI_CONTACT_FEATURE_EDGE_AB
bary = wp.vec3(1.0 - v, v, 0.0)
return a + v * ab, bary, feature_type
vb = d5 * d2 - d1 * d6
if vb <= 0.0 and d2 >= 0.0 and d6 <= 0.0:
v = d2 / (d2 - d6)
feature_type = TRI_CONTACT_FEATURE_EDGE_AC
bary = wp.vec3(1.0 - v, 0.0, v)
return a + v * ac, bary, feature_type
va = d3 * d6 - d5 * d4
if va <= 0.0 and (d4 - d3) >= 0.0 and (d5 - d6) >= 0.0:
v = (d4 - d3) / ((d4 - d3) + (d5 - d6))
feature_type = TRI_CONTACT_FEATURE_EDGE_BC
bary = wp.vec3(0.0, 1.0 - v, v)
return b + v * (c - b), bary, feature_type
denom = 1.0 / (va + vb + vc)
v = vb * denom
w = vc * denom
feature_type = TRI_CONTACT_FEATURE_FACE_INTERIOR
bary = wp.vec3(1.0 - v - w, v, w)
return a + v * ab + w * ac, bary, feature_type
@wp.func
def _sdf_point_to_z_up(point: wp.vec3, up_axis: int):
if up_axis == int(Axis.X):
return wp.vec3(point[1], point[2], point[0])
if up_axis == int(Axis.Y):
return wp.vec3(point[0], point[2], point[1])
return point
@wp.func
def _sdf_capped_cone_z(bottom_radius: float, top_radius: float, half_height: float, point_z_up: wp.vec3):
q = wp.vec2(wp.length(wp.vec2(point_z_up[0], point_z_up[1])), point_z_up[2])
k1 = wp.vec2(top_radius, half_height)
k2 = wp.vec2(top_radius - bottom_radius, 2.0 * half_height)
if q[1] < 0.0:
ca = wp.vec2(q[0] - wp.min(q[0], bottom_radius), wp.abs(q[1]) - half_height)
else:
ca = wp.vec2(q[0] - wp.min(q[0], top_radius), wp.abs(q[1]) - half_height)
denom = wp.dot(k2, k2)
t = 0.0
if denom > 0.0:
t = wp.clamp(wp.dot(k1 - q, k2) / denom, 0.0, 1.0)
cb = q - k1 + k2 * t
sign = 1.0
if cb[0] < 0.0 and ca[1] < 0.0:
sign = -1.0
return sign * wp.sqrt(wp.min(wp.dot(ca, ca), wp.dot(cb, cb)))
@wp.func
def sdf_sphere(point: wp.vec3, radius: float):
"""Compute signed distance to a sphere for ``Mesh.create_sphere`` geometry.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radius [m]: Sphere radius.
Returns:
Signed distance [m], negative inside, zero on surface, positive outside.
"""
return wp.length(point) - radius
@wp.func
def sdf_sphere_grad(point: wp.vec3, radius: float):
"""Compute outward SDF gradient for ``sdf_sphere``.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radius [m]: Sphere radius (unused, kept for API symmetry).
Returns:
Unit-length gradient direction in local frame.
"""
_ = radius
eps = 1.0e-8
p_len = wp.length(point)
if p_len > eps:
return point / p_len
return wp.vec3(0.0, 0.0, 1.0)
@wp.func
def sdf_box(point: wp.vec3, hx: float, hy: float, hz: float):
"""Compute signed distance to an axis-aligned box.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
hx [m]: Half-extent along X.
hy [m]: Half-extent along Y.
hz [m]: Half-extent along Z.
Returns:
Signed distance [m], negative inside, zero on surface, positive outside.
"""
# adapted from https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm
qx = abs(point[0]) - hx
qy = abs(point[1]) - hy
qz = abs(point[2]) - hz
e = wp.vec3(wp.max(qx, 0.0), wp.max(qy, 0.0), wp.max(qz, 0.0))
return wp.length(e) + wp.min(wp.max(qx, wp.max(qy, qz)), 0.0)
@wp.func
def sdf_box_grad(point: wp.vec3, hx: float, hy: float, hz: float):
"""Compute outward SDF gradient for ``sdf_box``.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
hx [m]: Half-extent along X.
hy [m]: Half-extent along Y.
hz [m]: Half-extent along Z.
Returns:
Unit-length (or axis-aligned) outward gradient direction.
"""
qx = abs(point[0]) - hx
qy = abs(point[1]) - hy
qz = abs(point[2]) - hz
# exterior case
if qx > 0.0 or qy > 0.0 or qz > 0.0:
x = wp.clamp(point[0], -hx, hx)
y = wp.clamp(point[1], -hy, hy)
z = wp.clamp(point[2], -hz, hz)
return wp.normalize(point - wp.vec3(x, y, z))
sx = wp.sign(point[0])
sy = wp.sign(point[1])
sz = wp.sign(point[2])
# x projection
if (qx > qy and qx > qz) or (qy == 0.0 and qz == 0.0):
return wp.vec3(sx, 0.0, 0.0)
# y projection
if (qy > qx and qy > qz) or (qx == 0.0 and qz == 0.0):
return wp.vec3(0.0, sy, 0.0)
# z projection
return wp.vec3(0.0, 0.0, sz)
@wp.func
def sdf_capsule(point: wp.vec3, radius: float, half_height: float, up_axis: int = int(Axis.Y)):
"""Compute signed distance to a capsule for ``Mesh.create_capsule`` geometry.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radius [m]: Capsule radius.
half_height [m]: Half-height of the cylindrical section.
up_axis: Capsule long axis as ``int(newton.Axis.*)``.
Returns:
Signed distance [m], negative inside, zero on surface, positive outside.
"""
point_z_up = _sdf_point_to_z_up(point, up_axis)
if point_z_up[2] > half_height:
return wp.length(wp.vec3(point_z_up[0], point_z_up[1], point_z_up[2] - half_height)) - radius
if point_z_up[2] < -half_height:
return wp.length(wp.vec3(point_z_up[0], point_z_up[1], point_z_up[2] + half_height)) - radius
return wp.length(wp.vec3(point_z_up[0], point_z_up[1], 0.0)) - radius
@wp.func
def _sdf_vector_from_z_up(v: wp.vec3, up_axis: int):
if up_axis == int(Axis.X):
return wp.vec3(v[2], v[0], v[1])
if up_axis == int(Axis.Y):
return wp.vec3(v[0], v[2], v[1])
return v
@wp.func
def sdf_capsule_grad(point: wp.vec3, radius: float, half_height: float, up_axis: int = int(Axis.Y)):
"""Compute outward SDF gradient for ``sdf_capsule``.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radius [m]: Capsule radius.
half_height [m]: Half-height of the cylindrical section.
up_axis: Capsule long axis as ``int(newton.Axis.*)``.
Returns:
Unit-length outward gradient direction in local frame.
"""
_ = radius
eps = 1.0e-8
point_z_up = _sdf_point_to_z_up(point, up_axis)
grad_z_up = wp.vec3()
if point_z_up[2] > half_height:
v = wp.vec3(point_z_up[0], point_z_up[1], point_z_up[2] - half_height)
v_len = wp.length(v)
grad_z_up = wp.vec3(0.0, 0.0, 1.0)
if v_len > eps:
grad_z_up = v / v_len
elif point_z_up[2] < -half_height:
v = wp.vec3(point_z_up[0], point_z_up[1], point_z_up[2] + half_height)
v_len = wp.length(v)
grad_z_up = wp.vec3(0.0, 0.0, -1.0)
if v_len > eps:
grad_z_up = v / v_len
else:
v = wp.vec3(point_z_up[0], point_z_up[1], 0.0)
v_len = wp.length(v)
grad_z_up = wp.vec3(0.0, 0.0, 1.0)
if v_len > eps:
grad_z_up = v / v_len
return _sdf_vector_from_z_up(grad_z_up, up_axis)
@wp.func
def sdf_cylinder(
point: wp.vec3,
radius: float,
half_height: float,
up_axis: int = int(Axis.Y),
top_radius: float = -1.0,
):
"""Compute signed distance to ``Mesh.create_cylinder`` geometry.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radius [m]: Bottom radius.
half_height [m]: Half-height along the cylinder axis.
up_axis: Cylinder long axis as ``int(newton.Axis.*)``.
top_radius [m]: Top radius. Negative values use ``radius``.
Returns:
Signed distance [m], negative inside, zero on surface, positive outside.
"""
point_z_up = _sdf_point_to_z_up(point, up_axis)
if top_radius < 0.0 or wp.abs(top_radius - radius) <= 1.0e-6:
dx = wp.length(wp.vec3(point_z_up[0], point_z_up[1], 0.0)) - radius
dy = wp.abs(point_z_up[2]) - half_height
return wp.min(wp.max(dx, dy), 0.0) + wp.length(wp.vec2(wp.max(dx, 0.0), wp.max(dy, 0.0)))
return _sdf_capped_cone_z(radius, top_radius, half_height, point_z_up)
@wp.func
def sdf_cylinder_grad(
point: wp.vec3,
radius: float,
half_height: float,
up_axis: int = int(Axis.Y),
top_radius: float = -1.0,
):
"""Compute outward SDF gradient for ``sdf_cylinder``.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radius [m]: Bottom radius.
half_height [m]: Half-height along the cylinder axis.
up_axis: Cylinder long axis as ``int(newton.Axis.*)``.
top_radius [m]: Top radius. Negative values use ``radius``.
Returns:
Unit-length outward gradient direction in local frame.
"""
eps = 1.0e-8
point_z_up = _sdf_point_to_z_up(point, up_axis)
if top_radius >= 0.0 and wp.abs(top_radius - radius) > 1.0e-6:
# Use finite-difference gradient of the tapered capped-cone SDF.
fd_eps = 1.0e-4
dx = _sdf_capped_cone_z(
radius,
top_radius,
half_height,
point_z_up + wp.vec3(fd_eps, 0.0, 0.0),
) - _sdf_capped_cone_z(
radius,
top_radius,
half_height,
point_z_up - wp.vec3(fd_eps, 0.0, 0.0),
)
dy = _sdf_capped_cone_z(
radius,
top_radius,
half_height,
point_z_up + wp.vec3(0.0, fd_eps, 0.0),
) - _sdf_capped_cone_z(
radius,
top_radius,
half_height,
point_z_up - wp.vec3(0.0, fd_eps, 0.0),
)
dz = _sdf_capped_cone_z(
radius,
top_radius,
half_height,
point_z_up + wp.vec3(0.0, 0.0, fd_eps),
) - _sdf_capped_cone_z(
radius,
top_radius,
half_height,
point_z_up - wp.vec3(0.0, 0.0, fd_eps),
)
grad_z_up = wp.vec3(dx, dy, dz)
grad_len = wp.length(grad_z_up)
if grad_len > eps:
grad_z_up = grad_z_up / grad_len
else:
grad_z_up = wp.vec3(0.0, 0.0, 1.0)
return _sdf_vector_from_z_up(grad_z_up, up_axis)
v = wp.vec3(point_z_up[0], point_z_up[1], 0.0)
v_len = wp.length(v)
radial = wp.vec3(0.0, 0.0, 1.0)
if v_len > eps:
radial = v / v_len
axial = wp.vec3(0.0, 0.0, wp.sign(point_z_up[2]))
dx = v_len - radius
dy = wp.abs(point_z_up[2]) - half_height
grad_z_up = wp.vec3()
if dx > 0.0 and dy > 0.0:
g = radial * dx + axial * dy
g_len = wp.length(g)
if g_len > eps:
grad_z_up = g / g_len
else:
grad_z_up = radial
elif dx > dy:
grad_z_up = radial
else:
grad_z_up = axial
return _sdf_vector_from_z_up(grad_z_up, up_axis)
@wp.func
def sdf_ellipsoid(point: wp.vec3, radii: wp.vec3):
"""Compute approximate signed distance to an ellipsoid.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radii [m]: Ellipsoid radii along XYZ, shape [3], float.
Returns:
Approximate signed distance [m], negative inside, positive outside.
"""
# Approximate SDF for ellipsoid with radii (rx, ry, rz)
# Using the approximation: k0 * (k0 - 1) / k1
eps = 1.0e-8
r = wp.vec3(
wp.max(wp.abs(radii[0]), eps),
wp.max(wp.abs(radii[1]), eps),
wp.max(wp.abs(radii[2]), eps),
)
inv_r = wp.cw_div(wp.vec3(1.0, 1.0, 1.0), r)
inv_r2 = wp.cw_mul(inv_r, inv_r)
q0 = wp.cw_mul(point, inv_r) # p / r
q1 = wp.cw_mul(point, inv_r2) # p / r^2
k0 = wp.length(q0)
k1 = wp.length(q1)
if k1 > eps:
return k0 * (k0 - 1.0) / k1
# Deep inside / near center fallback
return -wp.min(wp.min(r[0], r[1]), r[2])
@wp.func
def sdf_ellipsoid_grad(point: wp.vec3, radii: wp.vec3):
"""Compute approximate outward SDF gradient for ``sdf_ellipsoid``.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radii [m]: Ellipsoid radii along XYZ, shape [3], float.
Returns:
Unit-length approximate outward gradient direction.
"""
# Gradient of the ellipsoid SDF approximation
# grad(d) ≈ normalize((k0 / k1) * (p / r^2))
eps = 1.0e-8
r = wp.vec3(
wp.max(wp.abs(radii[0]), eps),
wp.max(wp.abs(radii[1]), eps),
wp.max(wp.abs(radii[2]), eps),
)
inv_r = wp.cw_div(wp.vec3(1.0, 1.0, 1.0), r)
inv_r2 = wp.cw_mul(inv_r, inv_r)
q0 = wp.cw_mul(point, inv_r) # p / r
q1 = wp.cw_mul(point, inv_r2) # p / r^2
k0 = wp.length(q0)
k1 = wp.length(q1)
if k1 < eps:
return wp.vec3(0.0, 0.0, 1.0)
# Analytic gradient of the approximation
grad = q1 * (k0 / k1)
grad_len = wp.length(grad)
if grad_len > eps:
return grad / grad_len
return wp.vec3(0.0, 0.0, 1.0)
@wp.func
def sdf_cone(point: wp.vec3, radius: float, half_height: float, up_axis: int = int(Axis.Y)):
"""Compute signed distance to a cone for ``Mesh.create_cone`` geometry.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radius [m]: Cone base radius.
half_height [m]: Half-height from center to apex/base.
up_axis: Cone long axis as ``int(newton.Axis.*)``.
Returns:
Signed distance [m], negative inside, zero on surface, positive outside.
"""
point_z_up = _sdf_point_to_z_up(point, up_axis)
return _sdf_capped_cone_z(radius, 0.0, half_height, point_z_up)
@wp.func
def sdf_cone_grad(point: wp.vec3, radius: float, half_height: float, up_axis: int = int(Axis.Y)):
"""Compute outward SDF gradient for ``sdf_cone``.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
radius [m]: Cone base radius.
half_height [m]: Half-height from center to apex/base.
up_axis: Cone long axis as ``int(newton.Axis.*)``.
Returns:
Unit-length outward gradient direction in local frame.
"""
point_z_up = _sdf_point_to_z_up(point, up_axis)
if half_height <= 0.0:
return _sdf_vector_from_z_up(wp.vec3(0.0, 0.0, wp.sign(point_z_up[2])), up_axis)
# Gradient for cone with apex at +half_height and base at -half_height
r = wp.length(wp.vec3(point_z_up[0], point_z_up[1], 0.0))
dx = r - radius * (half_height - point_z_up[2]) / (2.0 * half_height)
dy = wp.abs(point_z_up[2]) - half_height
grad_z_up = wp.vec3()
if dx > dy:
# Closest to lateral surface
if r > 0.0:
radial_dir = wp.vec3(point_z_up[0], point_z_up[1], 0.0) / r
# Normal to cone surface
grad_z_up = wp.normalize(radial_dir + wp.vec3(0.0, 0.0, radius / (2.0 * half_height)))
else:
grad_z_up = wp.vec3(0.0, 0.0, 1.0)
else:
# Closest to cap
grad_z_up = wp.vec3(0.0, 0.0, wp.sign(point_z_up[2]))
return _sdf_vector_from_z_up(grad_z_up, up_axis)
@wp.func
def sdf_plane(point: wp.vec3, width: float, length: float):
"""Compute signed distance to a finite quad in the XY plane.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
width [m]: Half-extent along X.
length [m]: Half-extent along Y.
Returns:
Distance [m]. For finite extents (``width > 0`` and ``length > 0``), this
is a Chebyshev (L∞) distance approximation to the quad sheet (not exact
Euclidean distance). The exact Euclidean distance would be
``sqrt(max(|x|-width, 0)^2 + max(|y|-length, 0)^2 + z^2)``.
Otherwise, for ``width <= 0`` or ``length <= 0``, it reduces to the
signed distance of the infinite plane (``point.z``).
"""
# SDF for a quad in the xy plane
if width > 0.0 and length > 0.0:
d = wp.max(wp.abs(point[0]) - width, wp.abs(point[1]) - length)
return wp.max(d, wp.abs(point[2]))
return point[2]
@wp.func
def sdf_plane_grad(point: wp.vec3, width: float, length: float):
"""Compute a simple upward normal for ``sdf_plane``.
Args:
point [m]: Query point in the mesh local frame, shape [3], float.
width [m]: Half-extent along X.
length [m]: Half-extent along Y.
Returns:
Upward unit normal in local frame.
"""
_ = (width, length, point)
return wp.vec3(0.0, 0.0, 1.0)
@wp.func
def closest_point_plane(width: float, length: float, point: wp.vec3):
# projects the point onto the quad in the xy plane (if width and length > 0.0, otherwise the plane is infinite)
if width > 0.0:
x = wp.clamp(point[0], -width, width)
else:
x = point[0]
if length > 0.0:
y = wp.clamp(point[1], -length, length)
else:
y = point[1]
return wp.vec3(x, y, 0.0)
@wp.func
def closest_point_line_segment(a: wp.vec3, b: wp.vec3, point: wp.vec3):
ab = b - a
ap = point - a
t = wp.dot(ap, ab) / wp.dot(ab, ab)
t = wp.clamp(t, 0.0, 1.0)
return a + t * ab
@wp.func
def closest_point_box(upper: wp.vec3, point: wp.vec3):
# closest point to box surface
x = wp.clamp(point[0], -upper[0], upper[0])
y = wp.clamp(point[1], -upper[1], upper[1])
z = wp.clamp(point[2], -upper[2], upper[2])
if wp.abs(point[0]) <= upper[0] and wp.abs(point[1]) <= upper[1] and wp.abs(point[2]) <= upper[2]:
# the point is inside, find closest face
sx = wp.abs(wp.abs(point[0]) - upper[0])
sy = wp.abs(wp.abs(point[1]) - upper[1])
sz = wp.abs(wp.abs(point[2]) - upper[2])
# return closest point on closest side, handle corner cases
if (sx < sy and sx < sz) or (sy == 0.0 and sz == 0.0):
x = wp.sign(point[0]) * upper[0]
elif (sy < sx and sy < sz) or (sx == 0.0 and sz == 0.0):
y = wp.sign(point[1]) * upper[1]
else:
z = wp.sign(point[2]) * upper[2]
return wp.vec3(x, y, z)
@wp.func
def get_box_vertex(point_id: int, upper: wp.vec3):
# box vertex numbering:
# 6---7
# |\ |\ y
# | 2-+-3 |
# 4-+-5 | z \|
# \| \| o---x
# 0---1
# get the vertex of the box given its ID (0-7)
sign_x = float(point_id % 2) * 2.0 - 1.0
sign_y = float((point_id // 2) % 2) * 2.0 - 1.0
sign_z = float((point_id // 4) % 2) * 2.0 - 1.0
return wp.vec3(sign_x * upper[0], sign_y * upper[1], sign_z * upper[2])
@wp.func
def get_box_edge(edge_id: int, upper: wp.vec3):
# get the edge of the box given its ID (0-11)
if edge_id < 4:
# edges along x: 0-1, 2-3, 4-5, 6-7
i = edge_id * 2
j = i + 1
return wp.spatial_vector(get_box_vertex(i, upper), get_box_vertex(j, upper))
elif edge_id < 8:
# edges along y: 0-2, 1-3, 4-6, 5-7
edge_id -= 4
i = edge_id % 2 + edge_id // 2 * 4
j = i + 2
return wp.spatial_vector(get_box_vertex(i, upper), get_box_vertex(j, upper))
# edges along z: 0-4, 1-5, 2-6, 3-7
edge_id -= 8
i = edge_id
j = i + 4
return wp.spatial_vector(get_box_vertex(i, upper), get_box_vertex(j, upper))
@wp.func
def get_plane_edge(edge_id: int, plane_width: float, plane_length: float):
# get the edge of the plane given its ID (0-3)
p0x = (2.0 * float(edge_id % 2) - 1.0) * plane_width
p0y = (2.0 * float(edge_id // 2) - 1.0) * plane_length
if edge_id == 0 or edge_id == 3:
p1x = p0x
p1y = -p0y
else:
p1x = -p0x
p1y = p0y
return wp.spatial_vector(wp.vec3(p0x, p0y, 0.0), wp.vec3(p1x, p1y, 0.0))
@wp.func
def closest_edge_coordinate_box(upper: wp.vec3, edge_a: wp.vec3, edge_b: wp.vec3, max_iter: int):
# find point on edge closest to box, return its barycentric edge coordinate
# Golden-section search
a = float(0.0)
b = float(1.0)
h = b - a
invphi = 0.61803398875 # 1 / phi
invphi2 = 0.38196601125 # 1 / phi^2
c = a + invphi2 * h
d = a + invphi * h
query = (1.0 - c) * edge_a + c * edge_b
yc = sdf_box(query, upper[0], upper[1], upper[2])
query = (1.0 - d) * edge_a + d * edge_b
yd = sdf_box(query, upper[0], upper[1], upper[2])
for _k in range(max_iter):
if yc < yd: # yc > yd to find the maximum
b = d
d = c
yd = yc
h = invphi * h
c = a + invphi2 * h
query = (1.0 - c) * edge_a + c * edge_b
yc = sdf_box(query, upper[0], upper[1], upper[2])
else:
a = c
c = d
yc = yd
h = invphi * h
d = a + invphi * h
query = (1.0 - d) * edge_a + d * edge_b
yd = sdf_box(query, upper[0], upper[1], upper[2])
if yc < yd:
return 0.5 * (a + d)
return 0.5 * (c + b)
@wp.func
def closest_edge_coordinate_plane(
plane_width: float,
plane_length: float,
edge_a: wp.vec3,
edge_b: wp.vec3,
max_iter: int,
):
# find point on edge closest to plane, return its barycentric edge coordinate
# Golden-section search
a = float(0.0)
b = float(1.0)
h = b - a
invphi = 0.61803398875 # 1 / phi
invphi2 = 0.38196601125 # 1 / phi^2
c = a + invphi2 * h
d = a + invphi * h
query = (1.0 - c) * edge_a + c * edge_b
yc = sdf_plane(query, plane_width, plane_length)
query = (1.0 - d) * edge_a + d * edge_b
yd = sdf_plane(query, plane_width, plane_length)
for _k in range(max_iter):
if yc < yd: # yc > yd to find the maximum
b = d
d = c
yd = yc
h = invphi * h
c = a + invphi2 * h
query = (1.0 - c) * edge_a + c * edge_b
yc = sdf_plane(query, plane_width, plane_length)
else:
a = c
c = d
yc = yd
h = invphi * h
d = a + invphi * h
query = (1.0 - d) * edge_a + d * edge_b
yd = sdf_plane(query, plane_width, plane_length)
if yc < yd:
return 0.5 * (a + d)
return 0.5 * (c + b)
@wp.func
def closest_edge_coordinate_capsule(radius: float, half_height: float, edge_a: wp.vec3, edge_b: wp.vec3, max_iter: int):
# find point on edge closest to capsule, return its barycentric edge coordinate
# Golden-section search
a = float(0.0)
b = float(1.0)
h = b - a
invphi = 0.61803398875 # 1 / phi
invphi2 = 0.38196601125 # 1 / phi^2
c = a + invphi2 * h
d = a + invphi * h
query = (1.0 - c) * edge_a + c * edge_b
yc = sdf_capsule(query, radius, half_height, int(Axis.Z))
query = (1.0 - d) * edge_a + d * edge_b
yd = sdf_capsule(query, radius, half_height, int(Axis.Z))
for _k in range(max_iter):
if yc < yd: # yc > yd to find the maximum
b = d
d = c
yd = yc
h = invphi * h
c = a + invphi2 * h
query = (1.0 - c) * edge_a + c * edge_b
yc = sdf_capsule(query, radius, half_height, int(Axis.Z))
else:
a = c
c = d
yc = yd
h = invphi * h
d = a + invphi * h
query = (1.0 - d) * edge_a + d * edge_b
yd = sdf_capsule(query, radius, half_height, int(Axis.Z))
if yc < yd:
return 0.5 * (a + d)
return 0.5 * (c + b)
@wp.func
def closest_edge_coordinate_cylinder(
radius: float, half_height: float, edge_a: wp.vec3, edge_b: wp.vec3, max_iter: int
):
# find point on edge closest to cylinder, return its barycentric edge coordinate
# Golden-section search
a = float(0.0)
b = float(1.0)
h = b - a
invphi = 0.61803398875 # 1 / phi
invphi2 = 0.38196601125 # 1 / phi^2
c = a + invphi2 * h
d = a + invphi * h
query = (1.0 - c) * edge_a + c * edge_b
yc = sdf_cylinder(query, radius, half_height, int(Axis.Z))
query = (1.0 - d) * edge_a + d * edge_b
yd = sdf_cylinder(query, radius, half_height, int(Axis.Z))
for _k in range(max_iter):
if yc < yd: # yc > yd to find the maximum
b = d
d = c
yd = yc
h = invphi * h
c = a + invphi2 * h
query = (1.0 - c) * edge_a + c * edge_b
yc = sdf_cylinder(query, radius, half_height, int(Axis.Z))
else:
a = c
c = d
yc = yd
h = invphi * h
d = a + invphi * h
query = (1.0 - d) * edge_a + d * edge_b
yd = sdf_cylinder(query, radius, half_height, int(Axis.Z))
if yc < yd:
return 0.5 * (a + d)
return 0.5 * (c + b)
@wp.func
def mesh_sdf(mesh: wp.uint64, point: wp.vec3, max_dist: float):
face_index = int(0)
face_u = float(0.0)
face_v = float(0.0)
sign = float(0.0)
res = wp.mesh_query_point_sign_normal(mesh, point, max_dist, sign, face_index, face_u, face_v)
if res:
closest = wp.mesh_eval_position(mesh, face_index, face_u, face_v)
return wp.length(point - closest) * sign
return max_dist
@wp.func
def sdf_mesh(mesh: wp.uint64, point: wp.vec3, max_dist: float):
"""Compute signed distance to a triangle mesh.
Args:
mesh: Warp mesh ID (``mesh.id``).
point [m]: Query point in mesh local frame, shape [3], float.
max_dist [m]: Maximum query distance.
Returns:
Signed distance [m], negative inside, zero on surface, positive outside.
"""
return mesh_sdf(mesh, point, max_dist)
@wp.func
def closest_point_mesh(mesh: wp.uint64, point: wp.vec3, max_dist: float):
face_index = int(0)
face_u = float(0.0)
face_v = float(0.0)
sign = float(0.0)
res = wp.mesh_query_point_sign_normal(mesh, point, max_dist, sign, face_index, face_u, face_v)
if res:
return wp.mesh_eval_position(mesh, face_index, face_u, face_v)
# return arbitrary point from mesh
return wp.mesh_eval_position(mesh, 0, 0.0, 0.0)
@wp.func
def closest_edge_coordinate_mesh(mesh: wp.uint64, edge_a: wp.vec3, edge_b: wp.vec3, max_iter: int, max_dist: float):
# find point on edge closest to mesh, return its barycentric edge coordinate
# Golden-section search
a = float(0.0)
b = float(1.0)
h = b - a
invphi = 0.61803398875 # 1 / phi
invphi2 = 0.38196601125 # 1 / phi^2
c = a + invphi2 * h
d = a + invphi * h
query = (1.0 - c) * edge_a + c * edge_b
yc = mesh_sdf(mesh, query, max_dist)
query = (1.0 - d) * edge_a + d * edge_b
yd = mesh_sdf(mesh, query, max_dist)
for _k in range(max_iter):
if yc < yd: # yc > yd to find the maximum
b = d
d = c
yd = yc
h = invphi * h
c = a + invphi2 * h
query = (1.0 - c) * edge_a + c * edge_b
yc = mesh_sdf(mesh, query, max_dist)
else:
a = c
c = d
yc = yd
h = invphi * h
d = a + invphi * h
query = (1.0 - d) * edge_a + d * edge_b
yd = mesh_sdf(mesh, query, max_dist)
if yc < yd:
return 0.5 * (a + d)
return 0.5 * (c + b)
@wp.func
def volume_grad(volume: wp.uint64, p: wp.vec3):
eps = 0.05 # TODO make this a parameter
q = wp.volume_world_to_index(volume, p)
# compute gradient of the SDF using finite differences
dx = wp.volume_sample_f(volume, q + wp.vec3(eps, 0.0, 0.0), wp.Volume.LINEAR) - wp.volume_sample_f(
volume, q - wp.vec3(eps, 0.0, 0.0), wp.Volume.LINEAR
)
dy = wp.volume_sample_f(volume, q + wp.vec3(0.0, eps, 0.0), wp.Volume.LINEAR) - wp.volume_sample_f(
volume, q - wp.vec3(0.0, eps, 0.0), wp.Volume.LINEAR
)
dz = wp.volume_sample_f(volume, q + wp.vec3(0.0, 0.0, eps), wp.Volume.LINEAR) - wp.volume_sample_f(
volume, q - wp.vec3(0.0, 0.0, eps), wp.Volume.LINEAR
)
return wp.normalize(wp.vec3(dx, dy, dz))
@wp.func
def counter_increment(counter: wp.array[int], counter_index: int, tids: wp.array[int], tid: int, index_limit: int = -1):
"""
Increment the counter but only if it is smaller than index_limit, remember which thread received which counter value.
This allows the counter increment function to be used in differentiable computations where the backward pass will
be able to leverage the thread-local counter values.
If ``index_limit`` is less than zero, the counter is incremented without any limit.
Args:
counter: The counter array.
counter_index: The index of the counter to increment.
tids: The array to store the thread-local counter values.
tid: The thread index.
index_limit: The limit of the counter (optional, default is -1).
"""
count = wp.atomic_add(counter, counter_index, 1)
if count < index_limit or index_limit < 0:
tids[tid] = count
return count
tids[tid] = -1
return -1
@wp.func_replay(counter_increment)
def counter_increment_replay(
counter: wp.array[int], counter_index: int, tids: wp.array[int], tid: int, index_limit: int
):
return tids[tid]
@wp.kernel
def create_soft_contacts(
particle_q: wp.array[wp.vec3],
particle_radius: wp.array[float],
particle_flags: wp.array[wp.int32],
particle_world: wp.array[int], # World indices for particles
body_q: wp.array[wp.transform],
shape_transform: wp.array[wp.transform],
shape_body: wp.array[int],
shape_type: wp.array[int],
shape_scale: wp.array[wp.vec3],
shape_source_ptr: wp.array[wp.uint64],
shape_world: wp.array[int], # World indices for shapes
margin: float,
soft_contact_max: int,
shape_count: int,
shape_flags: wp.array[wp.int32],
shape_heightfield_index: wp.array[wp.int32],
heightfield_data: wp.array[HeightfieldData],
heightfield_elevations: wp.array[wp.float32],
# outputs
soft_contact_count: wp.array[int],
soft_contact_particle: wp.array[int],
soft_contact_shape: wp.array[int],
soft_contact_body_pos: wp.array[wp.vec3],
soft_contact_body_vel: wp.array[wp.vec3],
soft_contact_normal: wp.array[wp.vec3],
soft_contact_tids: wp.array[int],
):
tid = wp.tid()
particle_index, shape_index = tid // shape_count, tid % shape_count
if (particle_flags[particle_index] & ParticleFlags.ACTIVE) == 0:
return
if (shape_flags[shape_index] & ShapeFlags.COLLIDE_PARTICLES) == 0:
return
# Check world indices
particle_world_id = particle_world[particle_index]
shape_world_id = shape_world[shape_index]
# Skip collision between different worlds (unless one is global)
if particle_world_id != -1 and shape_world_id != -1 and particle_world_id != shape_world_id:
return
rigid_index = shape_body[shape_index]
px = particle_q[particle_index]
radius = particle_radius[particle_index]
X_wb = wp.transform_identity()
if rigid_index >= 0:
X_wb = body_q[rigid_index]
X_bs = shape_transform[shape_index]
X_ws = wp.transform_multiply(X_wb, X_bs)
X_sw = wp.transform_inverse(X_ws)
# transform particle position to shape local space
x_local = wp.transform_point(X_sw, px)
# geo description
geo_type = shape_type[shape_index]
geo_scale = shape_scale[shape_index]
# evaluate shape sdf
d = 1.0e6
n = wp.vec3()
v = wp.vec3()
if geo_type == GeoType.SPHERE:
d = sdf_sphere(x_local, geo_scale[0])
n = sdf_sphere_grad(x_local, geo_scale[0])
if geo_type == GeoType.BOX:
d = sdf_box(x_local, geo_scale[0], geo_scale[1], geo_scale[2])
n = sdf_box_grad(x_local, geo_scale[0], geo_scale[1], geo_scale[2])
if geo_type == GeoType.CAPSULE:
d = sdf_capsule(x_local, geo_scale[0], geo_scale[1], int(Axis.Z))
n = sdf_capsule_grad(x_local, geo_scale[0], geo_scale[1], int(Axis.Z))
if geo_type == GeoType.CYLINDER:
d = sdf_cylinder(x_local, geo_scale[0], geo_scale[1], int(Axis.Z))
n = sdf_cylinder_grad(x_local, geo_scale[0], geo_scale[1], int(Axis.Z))
if geo_type == GeoType.CONE:
d = sdf_cone(x_local, geo_scale[0], geo_scale[1], int(Axis.Z))
n = sdf_cone_grad(x_local, geo_scale[0], geo_scale[1], int(Axis.Z))
if geo_type == GeoType.ELLIPSOID:
d = sdf_ellipsoid(x_local, geo_scale)
n = sdf_ellipsoid_grad(x_local, geo_scale)
if geo_type == GeoType.MESH or geo_type == GeoType.CONVEX_MESH:
mesh = shape_source_ptr[shape_index]
face_index = int(0)
face_u = float(0.0)
face_v = float(0.0)
sign = float(0.0)
min_scale = wp.min(geo_scale)
if wp.mesh_query_point_sign_normal(
mesh, wp.cw_div(x_local, geo_scale), margin + radius / min_scale, sign, face_index, face_u, face_v
):
shape_p = wp.mesh_eval_position(mesh, face_index, face_u, face_v)
shape_v = wp.mesh_eval_velocity(mesh, face_index, face_u, face_v)
shape_p = wp.cw_mul(shape_p, geo_scale)
shape_v = wp.cw_mul(shape_v, geo_scale)
delta = x_local - shape_p
d = wp.length(delta) * sign
n = wp.normalize(delta) * sign
v = shape_v
if geo_type == GeoType.PLANE:
d = sdf_plane(x_local, geo_scale[0] * 0.5, geo_scale[1] * 0.5)
n = wp.vec3(0.0, 0.0, 1.0)
if geo_type == GeoType.HFIELD:
hfd = heightfield_data[shape_heightfield_index[shape_index]]
d, n = sample_sdf_grad_heightfield(hfd, heightfield_elevations, x_local)
if d < margin + radius:
index = counter_increment(soft_contact_count, 0, soft_contact_tids, tid)
if index < soft_contact_max:
# compute contact point in body local space
body_pos = wp.transform_point(X_bs, x_local - n * d)
body_vel = wp.transform_vector(X_bs, v)
world_normal = wp.transform_vector(X_ws, n)
soft_contact_shape[index] = shape_index
soft_contact_body_pos[index] = body_pos
soft_contact_body_vel[index] = body_vel
soft_contact_particle[index] = particle_index
soft_contact_normal[index] = world_normal
# --------------------------------------
# region Triangle collision detection
# types of triangle's closest point to a point
TRI_CONTACT_FEATURE_VERTEX_A = wp.constant(0)
TRI_CONTACT_FEATURE_VERTEX_B = wp.constant(1)
TRI_CONTACT_FEATURE_VERTEX_C = wp.constant(2)
TRI_CONTACT_FEATURE_EDGE_AB = wp.constant(3)
TRI_CONTACT_FEATURE_EDGE_AC = wp.constant(4)
TRI_CONTACT_FEATURE_EDGE_BC = wp.constant(5)
TRI_CONTACT_FEATURE_FACE_INTERIOR = wp.constant(6)
# constants used to access TriMeshCollisionDetector.resize_flags
VERTEX_COLLISION_BUFFER_OVERFLOW_INDEX = wp.constant(0)
TRI_COLLISION_BUFFER_OVERFLOW_INDEX = wp.constant(1)
EDGE_COLLISION_BUFFER_OVERFLOW_INDEX = wp.constant(2)
TRI_TRI_COLLISION_BUFFER_OVERFLOW_INDEX = wp.constant(3)
@wp.func
def compute_tri_aabb(
v1: wp.vec3,
v2: wp.vec3,
v3: wp.vec3,
):
lower = wp.min(wp.min(v1, v2), v3)
upper = wp.max(wp.max(v1, v2), v3)
return lower, upper
@wp.kernel
def compute_tri_aabbs(
pos: wp.array[wp.vec3],
tri_indices: wp.array2d[wp.int32],
lower_bounds: wp.array[wp.vec3],
upper_bounds: wp.array[wp.vec3],
):
t_id = wp.tid()
v1 = pos[tri_indices[t_id, 0]]
v2 = pos[tri_indices[t_id, 1]]
v3 = pos[tri_indices[t_id, 2]]
lower, upper = compute_tri_aabb(v1, v2, v3)
lower_bounds[t_id] = lower
upper_bounds[t_id] = upper
@wp.kernel
def compute_edge_aabbs(
pos: wp.array[wp.vec3],
edge_indices: wp.array2d[wp.int32],
lower_bounds: wp.array[wp.vec3],
upper_bounds: wp.array[wp.vec3],
):
e_id = wp.tid()
v1 = pos[edge_indices[e_id, 2]]
v2 = pos[edge_indices[e_id, 3]]
lower_bounds[e_id] = wp.min(v1, v2)
upper_bounds[e_id] = wp.max(v1, v2)
@wp.func
def tri_is_neighbor(a_1: wp.int32, a_2: wp.int32, a_3: wp.int32, b_1: wp.int32, b_2: wp.int32, b_3: wp.int32):
tri_is_neighbor = (
a_1 == b_1
or a_1 == b_2
or a_1 == b_3
or a_2 == b_1
or a_2 == b_2
or a_2 == b_3
or a_3 == b_1
or a_3 == b_2
or a_3 == b_3
)
return tri_is_neighbor
@wp.func
def vertex_adjacent_to_triangle(v: wp.int32, a: wp.int32, b: wp.int32, c: wp.int32):
return v == a or v == b or v == c
@wp.kernel
def init_triangle_collision_data_kernel(
query_radius: float,
# outputs
triangle_colliding_vertices_count: wp.array[wp.int32],
triangle_colliding_vertices_min_dist: wp.array[float],
resize_flags: wp.array[wp.int32],
):
tri_index = wp.tid()
triangle_colliding_vertices_count[tri_index] = 0
triangle_colliding_vertices_min_dist[tri_index] = query_radius
if tri_index == 0:
for i in range(4):
resize_flags[i] = 0
@wp.kernel
def vertex_triangle_collision_detection_kernel(
max_query_radius: float,
min_query_radius: float,
bvh_id: wp.uint64,
pos: wp.array[wp.vec3],
tri_indices: wp.array2d[wp.int32],
vertex_colliding_triangles_offsets: wp.array[wp.int32],
vertex_colliding_triangles_buffer_sizes: wp.array[wp.int32],
triangle_colliding_vertices_offsets: wp.array[wp.int32],
triangle_colliding_vertices_buffer_sizes: wp.array[wp.int32],
vertex_triangle_filtering_list: wp.array[wp.int32],
vertex_triangle_filtering_list_offsets: wp.array[wp.int32],
min_distance_filtering_ref_pos: wp.array[wp.vec3],
# outputs
vertex_colliding_triangles: wp.array[wp.int32],
vertex_colliding_triangles_count: wp.array[wp.int32],
vertex_colliding_triangles_min_dist: wp.array[float],
triangle_colliding_vertices: wp.array[wp.int32],
triangle_colliding_vertices_count: wp.array[wp.int32],
triangle_colliding_vertices_min_dist: wp.array[float],
resize_flags: wp.array[wp.int32],
):
"""
This function applies discrete collision detection between vertices and triangles. It uses pre-allocated spaces to
record the collision data. This collision detector works both ways, i.e., it records vertices' colliding triangles to
`vertex_colliding_triangles`, and records each triangles colliding vertices to `triangle_colliding_vertices`.
This function assumes that all the vertices are on triangles, and can be indexed from the pos argument.
Note:
The collision data buffer is pre-allocated and cannot be changed during collision detection, therefore, the space
may not be enough. If the space is not enough to record all the collision information, the function will set a
certain element in resized_flag to be true. The user can reallocate the buffer based on vertex_colliding_triangles_count
and vertex_colliding_triangles_count.
Args:
bvh_id (int): the bvh id you want to collide with
max_query_radius (float): the upper bound of collision distance.
min_query_radius (float): the lower bound of collision distance. This distance is evaluated based on min_distance_filtering_ref_pos
pos (array): positions of all the vertices that make up triangles
vertex_colliding_triangles_offsets (array): where each vertex' collision buffer starts
vertex_colliding_triangles_buffer_sizes (array): size of each vertex' collision buffer, will be modified if resizing is needed
vertex_colliding_triangles_min_dist (array): each vertex' min distance to all (non-neighbor) triangles
triangle_colliding_vertices_offsets (array): where each triangle's collision buffer starts
triangle_colliding_vertices_buffer_sizes (array): size of each triangle's collision buffer, will be modified if resizing is needed
min_distance_filtering_ref_pos (array): the position that minimal collision distance evaluation uses.
vertex_colliding_triangles (array): flattened buffer of vertices' collision triangles
vertex_colliding_triangles_count (array): number of triangles each vertex collides with
triangle_colliding_vertices (array): positions of all the triangles' collision vertices, every two elements
records the vertex index and a triangle index it collides to
triangle_colliding_vertices_count (array): number of triangles each vertex collides with
triangle_colliding_vertices_min_dist (array): each triangle's min distance to all (non-self) vertices
resized_flag (array): size == 3, (vertex_buffer_resize_required, triangle_buffer_resize_required, edge_buffer_resize_required)
"""
v_index = wp.tid()
v = pos[v_index]
vertex_buffer_offset = vertex_colliding_triangles_offsets[v_index]
vertex_buffer_size = vertex_colliding_triangles_offsets[v_index + 1] - vertex_buffer_offset
lower = wp.vec3(v[0] - max_query_radius, v[1] - max_query_radius, v[2] - max_query_radius)
upper = wp.vec3(v[0] + max_query_radius, v[1] + max_query_radius, v[2] + max_query_radius)
query = wp.bvh_query_aabb(bvh_id, lower, upper)
tri_index = wp.int32(0)
vertex_num_collisions = wp.int32(0)
min_dis_to_tris = max_query_radius
while wp.bvh_query_next(query, tri_index):
t1 = tri_indices[tri_index, 0]
t2 = tri_indices[tri_index, 1]
t3 = tri_indices[tri_index, 2]
if vertex_adjacent_to_triangle(v_index, t1, t2, t3):
continue
if vertex_triangle_filtering_list:
fl_start = vertex_triangle_filtering_list_offsets[v_index]
fl_end = vertex_triangle_filtering_list_offsets[v_index + 1] # start of next vertex slice (end exclusive)
if fl_end > fl_start:
# Optional fast-fail using first/last elements (remember end is exclusive)
first_val = vertex_triangle_filtering_list[fl_start]
last_val = vertex_triangle_filtering_list[fl_end - 1]
if (tri_index >= first_val) and (tri_index <= last_val):
idx = binary_search(vertex_triangle_filtering_list, tri_index, fl_start, fl_end)
# `idx` is the first index > tri_index within [fl_start, fl_end)
if idx > fl_start and vertex_triangle_filtering_list[idx - 1] == tri_index:
continue
u1 = pos[t1]
u2 = pos[t2]
u3 = pos[t3]
closest_p, _bary, _feature_type = triangle_closest_point(u1, u2, u3, v)
dist = wp.length(closest_p - v)
if min_distance_filtering_ref_pos and min_query_radius > 0.0:
closest_p_ref, _, __ = triangle_closest_point(
min_distance_filtering_ref_pos[t1],
min_distance_filtering_ref_pos[t2],
min_distance_filtering_ref_pos[t3],
min_distance_filtering_ref_pos[v_index],
)
dist_ref = wp.length(closest_p_ref - min_distance_filtering_ref_pos[v_index])
if dist_ref < min_query_radius:
continue
if dist < max_query_radius:
# record v-f collision to vertex
min_dis_to_tris = wp.min(min_dis_to_tris, dist)
if vertex_num_collisions < vertex_buffer_size:
vertex_colliding_triangles[2 * (vertex_buffer_offset + vertex_num_collisions)] = v_index
vertex_colliding_triangles[2 * (vertex_buffer_offset + vertex_num_collisions) + 1] = tri_index
else:
resize_flags[VERTEX_COLLISION_BUFFER_OVERFLOW_INDEX] = 1
vertex_num_collisions = vertex_num_collisions + 1
if triangle_colliding_vertices:
wp.atomic_min(triangle_colliding_vertices_min_dist, tri_index, dist)
tri_buffer_size = triangle_colliding_vertices_buffer_sizes[tri_index]
tri_num_collisions = wp.atomic_add(triangle_colliding_vertices_count, tri_index, 1)
if tri_num_collisions < tri_buffer_size:
tri_buffer_offset = triangle_colliding_vertices_offsets[tri_index]
# record v-f collision to triangle
triangle_colliding_vertices[tri_buffer_offset + tri_num_collisions] = v_index
else:
resize_flags[TRI_COLLISION_BUFFER_OVERFLOW_INDEX] = 1
vertex_colliding_triangles_count[v_index] = vertex_num_collisions
vertex_colliding_triangles_min_dist[v_index] = min_dis_to_tris
@wp.kernel
def edge_colliding_edges_detection_kernel(
max_query_radius: float,
min_query_radius: float,
bvh_id: wp.uint64,
pos: wp.array[wp.vec3],
edge_indices: wp.array2d[wp.int32],
edge_colliding_edges_offsets: wp.array[wp.int32],
edge_colliding_edges_buffer_sizes: wp.array[wp.int32],
edge_edge_parallel_epsilon: float,
edge_filtering_list: wp.array[wp.int32],
edge_filtering_list_offsets: wp.array[wp.int32],
min_distance_filtering_ref_pos: wp.array[wp.vec3],
# outputs
edge_colliding_edges: wp.array[wp.int32],
edge_colliding_edges_count: wp.array[wp.int32],
edge_colliding_edges_min_dist: wp.array[float],
resize_flags: wp.array[wp.int32],
):
"""
bvh_id (int): the bvh id you want to do collision detection on
max_query_radius (float): the upper bound of collision distance.
min_query_radius (float): the lower bound of collision distance. This distance is evaluated based on min_distance_filtering_ref_pos
pos (array): positions of all the vertices that make up edges
edge_colliding_triangles (array): flattened buffer of edges' collision edges
edge_colliding_edges_count (array): number of edges each edge collides
edge_colliding_triangles_offsets (array): where each edge's collision buffer starts
edge_colliding_triangles_buffer_size (array): size of each edge's collision buffer, will be modified if resizing is needed
edge_min_dis_to_triangles (array): each vertex' min distance to all (non-neighbor) triangles
resized_flag (array): size == 3, (vertex_buffer_resize_required, triangle_buffer_resize_required, edge_buffer_resize_required)
"""
e_index = wp.tid()
e0_v0 = edge_indices[e_index, 2]
e0_v1 = edge_indices[e_index, 3]
e0_v0_pos = pos[e0_v0]
e0_v1_pos = pos[e0_v1]
lower = wp.min(e0_v0_pos, e0_v1_pos)
upper = wp.max(e0_v0_pos, e0_v1_pos)
lower = wp.vec3(lower[0] - max_query_radius, lower[1] - max_query_radius, lower[2] - max_query_radius)
upper = wp.vec3(upper[0] + max_query_radius, upper[1] + max_query_radius, upper[2] + max_query_radius)
query = wp.bvh_query_aabb(bvh_id, lower, upper)
colliding_edge_index = wp.int32(0)
edge_num_collisions = wp.int32(0)
min_dis_to_edges = max_query_radius
while wp.bvh_query_next(query, colliding_edge_index):
e1_v0 = edge_indices[colliding_edge_index, 2]
e1_v1 = edge_indices[colliding_edge_index, 3]
if e0_v0 == e1_v0 or e0_v0 == e1_v1 or e0_v1 == e1_v0 or e0_v1 == e1_v1:
continue
if edge_filtering_list:
fl_start = edge_filtering_list_offsets[e_index]
fl_end = edge_filtering_list_offsets[e_index + 1] # start of next vertex slice (end exclusive)
if fl_end > fl_start:
# Optional fast-fail using first/last elements (remember end is exclusive)
first_val = edge_filtering_list[fl_start]
last_val = edge_filtering_list[fl_end - 1]
if (colliding_edge_index >= first_val) and (colliding_edge_index <= last_val):
idx = binary_search(edge_filtering_list, colliding_edge_index, fl_start, fl_end)
if idx > fl_start and edge_filtering_list[idx - 1] == colliding_edge_index:
continue
# else: key is out of range, cannot be present -> skip_this remains False
e1_v0_pos = pos[e1_v0]
e1_v1_pos = pos[e1_v1]
std = wp.closest_point_edge_edge(e0_v0_pos, e0_v1_pos, e1_v0_pos, e1_v1_pos, edge_edge_parallel_epsilon)
dist = std[2]
if min_distance_filtering_ref_pos and min_query_radius > 0.0:
e0_v0_pos_ref, e0_v1_pos_ref, e1_v0_pos_ref, e1_v1_pos_ref = (
min_distance_filtering_ref_pos[e0_v0],
min_distance_filtering_ref_pos[e0_v1],
min_distance_filtering_ref_pos[e1_v0],
min_distance_filtering_ref_pos[e1_v1],
)
std_ref = wp.closest_point_edge_edge(
e0_v0_pos_ref, e0_v1_pos_ref, e1_v0_pos_ref, e1_v1_pos_ref, edge_edge_parallel_epsilon
)
dist_ref = std_ref[2]
if dist_ref < min_query_radius:
continue
if dist < max_query_radius:
edge_buffer_offset = edge_colliding_edges_offsets[e_index]
edge_buffer_size = edge_colliding_edges_offsets[e_index + 1] - edge_buffer_offset
# record e-e collision to e0, and leave e1; e1 will detect this collision from its own thread
min_dis_to_edges = wp.min(min_dis_to_edges, dist)
if edge_num_collisions < edge_buffer_size:
edge_colliding_edges[2 * (edge_buffer_offset + edge_num_collisions)] = e_index
edge_colliding_edges[2 * (edge_buffer_offset + edge_num_collisions) + 1] = colliding_edge_index
else:
resize_flags[EDGE_COLLISION_BUFFER_OVERFLOW_INDEX] = 1
edge_num_collisions = edge_num_collisions + 1
edge_colliding_edges_count[e_index] = edge_num_collisions
edge_colliding_edges_min_dist[e_index] = min_dis_to_edges
@wp.kernel
def triangle_triangle_collision_detection_kernel(
bvh_id: wp.uint64,
pos: wp.array[wp.vec3],
tri_indices: wp.array2d[wp.int32],
triangle_intersecting_triangles_offsets: wp.array[wp.int32],
# outputs
triangle_intersecting_triangles: wp.array[wp.int32],
triangle_intersecting_triangles_count: wp.array[wp.int32],
resize_flags: wp.array[wp.int32],
):
tri_index = wp.tid()
t1_v1 = tri_indices[tri_index, 0]
t1_v2 = tri_indices[tri_index, 1]
t1_v3 = tri_indices[tri_index, 2]
v1 = pos[t1_v1]
v2 = pos[t1_v2]
v3 = pos[t1_v3]
lower, upper = compute_tri_aabb(v1, v2, v3)
buffer_offset = triangle_intersecting_triangles_offsets[tri_index]
buffer_size = triangle_intersecting_triangles_offsets[tri_index + 1] - buffer_offset
query = wp.bvh_query_aabb(bvh_id, lower, upper)
tri_index_2 = wp.int32(0)
intersection_count = wp.int32(0)
while wp.bvh_query_next(query, tri_index_2):
t2_v1 = tri_indices[tri_index_2, 0]
t2_v2 = tri_indices[tri_index_2, 1]
t2_v3 = tri_indices[tri_index_2, 2]
# filter out intersection test with neighbor triangles
if (
vertex_adjacent_to_triangle(t1_v1, t2_v1, t2_v2, t2_v3)
or vertex_adjacent_to_triangle(t1_v2, t2_v1, t2_v2, t2_v3)
or vertex_adjacent_to_triangle(t1_v3, t2_v1, t2_v2, t2_v3)
):
continue
u1 = pos[t2_v1]
u2 = pos[t2_v2]
u3 = pos[t2_v3]
if wp.intersect_tri_tri(v1, v2, v3, u1, u2, u3):
if intersection_count < buffer_size:
triangle_intersecting_triangles[buffer_offset + intersection_count] = tri_index_2
else:
resize_flags[TRI_TRI_COLLISION_BUFFER_OVERFLOW_INDEX] = 1
intersection_count = intersection_count + 1
triangle_intersecting_triangles_count[tri_index] = intersection_count
# endregion
================================================
FILE: newton/_src/geometry/mpr.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
# This code is based on the MPR implementation from Jitter Physics 2
# Original: https://github.com/notgiven688/jitterphysics2
# Copyright (c) Thorben Linneweber (MIT License)
# The code has been translated from C# to Python and modified for use in Newton.
#
# Jitter Physics 2's MPR implementation is itself based on XenoCollide.
# The XenoCollide license (zlib) is preserved in the function docstrings below
# as required by the zlib license terms.
"""
Minkowski Portal Refinement (MPR) collision detection algorithm.
This module implements the MPR algorithm for detecting collisions between convex shapes
and computing signed distance and contact information. MPR is an alternative to the
GJK+EPA approach that can be more efficient for penetrating contacts.
The algorithm works by:
1. Constructing an initial portal (triangle) in Minkowski space that contains the origin
2. Iteratively refining the portal by moving it closer to the origin
3. Computing signed distance and contact points once the origin is enclosed
Key features:
- Works directly with penetrating contacts (no need for EPA as a separate step)
- More numerically stable than EPA for deep penetrations
- Returns collision normal, signed distance, and witness points
The implementation uses support mapping to query shape geometry, making it applicable
to any convex shape that provides a support function.
"""
from typing import Any
import warp as wp
@wp.struct
class Vert:
"""Vertex structure for MPR algorithm containing points on both shapes."""
B: wp.vec3 # Point on shape B
BtoA: wp.vec3 # Vector from B to A
@wp.func
def vert_a(vert: Vert) -> wp.vec3:
"""Get point A by reconstructing from B and BtoA."""
return vert.B + vert.BtoA
def create_support_map_function(support_func: Any):
"""
Factory function to create support mapping functions for MPR algorithm.
This function creates specialized support mapping functions that work in Minkowski
space (A - B) and handle coordinate transformations between local and world space.
Args:
support_func: Support mapping function for individual shapes that takes
(geometry, direction, data_provider) and returns a support point
Returns:
Tuple of three functions:
- support_map_b: Support mapping for shape B with world space transformation
- minkowski_support: Support mapping for Minkowski difference A - B
- geometric_center: Computes geometric center of Minkowski difference
"""
# Support mapping functions (these replace the MinkowskiDiff struct methods)
@wp.func
def support_map_b(
geom_b: Any,
direction: wp.vec3,
orientation_b: wp.quat,
position_b: wp.vec3,
data_provider: Any,
) -> wp.vec3:
"""
Support mapping for shape B with transformation.
Args:
geom_b: Shape B geometry data
direction: Support direction in world space
orientation_b: Orientation of shape B
position_b: Position of shape B
data_provider: Support mapping data provider
Returns:
Support point in world space
"""
# Transform direction to local space of shape B
tmp = wp.quat_rotate_inv(orientation_b, direction)
# Get support point in local space
result = support_func(geom_b, tmp, data_provider)
# Transform result to world space
result = wp.quat_rotate(orientation_b, result)
result = result + position_b
return result
@wp.func
def minkowski_support(
geom_a: Any,
geom_b: Any,
direction: wp.vec3,
orientation_b: wp.quat,
position_b: wp.vec3,
extend: float,
data_provider: Any,
) -> Vert:
"""
Compute support point on Minkowski difference A - B.
Args:
geom_a: Shape A geometry data
geom_b: Shape B geometry data
direction: Support direction
orientation_b: Orientation of shape B
position_b: Position of shape B
extend: Combined margin extension [m]
data_provider: Support mapping data provider
Returns:
Vert containing support points
"""
v = Vert()
# Support point on A in positive direction
point_a = support_func(geom_a, direction, data_provider)
# Support point on B in negative direction
tmp_direction = -direction
v.B = support_map_b(geom_b, tmp_direction, orientation_b, position_b, data_provider)
# Apply contact offset extension (skip normalize when extend is zero)
if extend != 0.0:
d = wp.normalize(direction) * extend * 0.5
point_a = point_a + d
v.B = v.B - d
# Store BtoA vector
v.BtoA = point_a - v.B
return v
@wp.func
def geometric_center(
geom_a: Any,
geom_b: Any,
orientation_b: wp.quat,
position_b: wp.vec3,
data_provider: Any,
) -> Vert:
"""
Compute geometric center of Minkowski difference.
Args:
geom_a: Shape A geometry data
geom_b: Shape B geometry data
orientation_b: Orientation of shape B
position_b: Position of shape B
data_provider: Support mapping data provider
Returns:
Vert containing geometric centers of both shapes
"""
center = Vert()
# Both shape centers are at their local origins, so in the relative frame:
# center_A = vec3(0), center_B = position_b
center.B = position_b
center.BtoA = -position_b
return center
return support_map_b, minkowski_support, geometric_center
def create_solve_mpr(support_func: Any, _support_funcs: Any = None):
"""
Factory function to create MPR solver with specific support and center functions.
Args:
support_func: Support mapping function for shapes.
_support_funcs: Pre-built support functions tuple from
:func:`create_support_map_function`. When provided, these are reused
instead of creating new ones, allowing multiple solvers to share
compiled support code.
Returns:
``solve_mpr`` wrapper function. The core function is available as
``solve_mpr.core`` for callers that want to handle the relative-frame
transform themselves (e.g. fused MPR+GJK).
"""
if _support_funcs is not None:
_support_map_b, minkowski_support, geometric_center = _support_funcs
else:
_support_map_b, minkowski_support, geometric_center = create_support_map_function(support_func)
@wp.func
def solve_mpr_core(
geom_a: Any,
geom_b: Any,
orientation_b: wp.quat,
position_b: wp.vec3,
extend: float,
data_provider: Any,
MAX_ITER: int = 30,
COLLIDE_EPSILON: float = 1e-5,
) -> tuple[bool, wp.vec3, wp.vec3, wp.vec3, float]:
"""
Core MPR algorithm implementation.
XenoCollide is available under the zlib license:
XenoCollide Collision Detection and Physics Library
Copyright (c) 2007-2014 Gary Snethen http://xenocollide.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising
from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must
not claim that you wrote the original software. If you use this
software in a product, an acknowledgment in the product documentation
would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must
not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
The XenoCollide implementation below is altered and not identical to the
original. The license is kept untouched.
"""
NUMERIC_EPSILON = 1e-16
# Initialize variables
penetration = float(0.0)
point_a = wp.vec3(0.0, 0.0, 0.0)
point_b = wp.vec3(0.0, 0.0, 0.0)
normal = wp.vec3(0.0, 0.0, 0.0)
# Get geometric center
v0 = geometric_center(geom_a, geom_b, orientation_b, position_b, data_provider)
normal = v0.BtoA
if (
wp.abs(normal[0]) < NUMERIC_EPSILON
and wp.abs(normal[1]) < NUMERIC_EPSILON
and wp.abs(normal[2]) < NUMERIC_EPSILON
):
# Any direction is fine - add small perturbation
v0.BtoA = wp.vec3(1e-05, 0.0, 0.0)
normal = -v0.BtoA
# First support point
v1 = minkowski_support(geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider)
point_a = vert_a(v1)
point_b = v1.B
if wp.dot(v1.BtoA, normal) <= 0.0:
return False, point_a, point_b, normal, penetration
normal = wp.cross(v1.BtoA, v0.BtoA)
if wp.length_sq(normal) < NUMERIC_EPSILON * NUMERIC_EPSILON:
normal = v1.BtoA - v0.BtoA
normal = wp.normalize(normal)
temp1 = v1.BtoA
penetration = wp.dot(temp1, normal)
return True, point_a, point_b, normal, penetration
# Second support point
v2 = minkowski_support(geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider)
if wp.dot(v2.BtoA, normal) <= 0.0:
return False, point_a, point_b, normal, penetration
# Determine whether origin is on + or - side of plane
temp1 = v1.BtoA - v0.BtoA
temp2 = v2.BtoA - v0.BtoA
normal = wp.cross(temp1, temp2)
dist = wp.dot(normal, v0.BtoA)
# If the origin is on the - side of the plane, reverse the direction
if dist > 0.0:
# Swap v1 and v2
tmp_b = v1.B
tmp_btoa = v1.BtoA
v1.B = v2.B
v1.BtoA = v2.BtoA
v2.B = tmp_b
v2.BtoA = tmp_btoa
normal = -normal
phase1 = int(0)
phase2 = int(0)
hit = bool(False)
# Phase One: Identify a portal
v3 = Vert()
while True:
if phase1 > MAX_ITER:
return False, point_a, point_b, normal, penetration
phase1 += 1
v3 = minkowski_support(geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider)
if wp.dot(v3.BtoA, normal) <= 0.0:
return False, point_a, point_b, normal, penetration
# If origin is outside (v1.V(),v0.V(),v3.V()), then eliminate v2.V() and loop
temp1 = wp.cross(v1.BtoA, v3.BtoA)
if wp.dot(temp1, v0.BtoA) < 0.0:
v2 = v3
temp1 = v1.BtoA - v0.BtoA
temp2 = v3.BtoA - v0.BtoA
normal = wp.cross(temp1, temp2)
continue
# If origin is outside (v3.V(),v0.V(),v2.V()), then eliminate v1.V() and loop
temp1 = wp.cross(v3.BtoA, v2.BtoA)
if wp.dot(temp1, v0.BtoA) < 0.0:
v1 = v3
temp1 = v3.BtoA - v0.BtoA
temp2 = v2.BtoA - v0.BtoA
normal = wp.cross(temp1, temp2)
continue
break
# Phase Two: Refine the portal
v4 = Vert()
while True:
phase2 += 1
# Compute normal of the wedge face
temp1 = v2.BtoA - v1.BtoA
temp2 = v3.BtoA - v1.BtoA
normal = wp.cross(temp1, temp2)
normal_sq = wp.length_sq(normal)
# Can this happen??? Can it be handled more cleanly?
if normal_sq < NUMERIC_EPSILON * NUMERIC_EPSILON:
return False, point_a, point_b, normal, penetration
if not hit:
# Compute distance from origin to wedge face
d = wp.dot(normal, v1.BtoA)
# If the origin is inside the wedge, we have a hit
hit = d >= 0.0
v4 = minkowski_support(geom_a, geom_b, normal, orientation_b, position_b, extend, data_provider)
temp3 = v4.BtoA - v3.BtoA
delta = wp.dot(temp3, normal)
penetration = wp.dot(v4.BtoA, normal)
# If the origin is on the surface of the wedge, return a hit
if (
delta * delta <= COLLIDE_EPSILON * COLLIDE_EPSILON * normal_sq
or penetration <= 0.0
or phase2 > MAX_ITER
):
if hit:
inv_normal = 1.0 / wp.sqrt(normal_sq)
penetration *= inv_normal
normal = normal * inv_normal
# Barycentric interpolation to get witness points
temp3 = wp.cross(v1.BtoA, temp1)
gamma = wp.dot(temp3, normal) * inv_normal
temp3 = wp.cross(temp2, v1.BtoA)
beta = wp.dot(temp3, normal) * inv_normal
alpha = 1.0 - gamma - beta
point_a = alpha * vert_a(v1) + beta * vert_a(v2) + gamma * vert_a(v3)
point_b = alpha * v1.B + beta * v2.B + gamma * v3.B
return hit, point_a, point_b, normal, penetration
# Determine what region of the wedge the origin is in
temp1 = wp.cross(v4.BtoA, v0.BtoA)
dot = wp.dot(temp1, v1.BtoA)
if dot >= 0.0:
# Origin is outside of (v4.V(),v0.V(),v1.V())
dot = wp.dot(temp1, v2.BtoA)
if dot >= 0.0:
v1 = v4
else:
v3 = v4
else:
# Origin is outside of (v4.V(),v0.V(),v2.V())
dot = wp.dot(temp1, v3.BtoA)
if dot >= 0.0:
v2 = v4
else:
v1 = v4
@wp.func
def solve_mpr(
geom_a: Any,
geom_b: Any,
orientation_a: wp.quat,
orientation_b: wp.quat,
position_a: wp.vec3,
position_b: wp.vec3,
combined_margin: float,
data_provider: Any,
MAX_ITER: int = 30,
COLLIDE_EPSILON: float = 1e-5,
) -> tuple[bool, float, wp.vec3, wp.vec3]:
"""
Solve MPR (Minkowski Portal Refinement) for collision detection.
Args:
geom_a: Shape A geometry data
geom_b: Shape B geometry data
orientation_a: Orientation of shape A
orientation_b: Orientation of shape B
position_a: Position of shape A
position_b: Position of shape B
combined_margin: Sum of margin extensions for both shapes [m]
data_provider: Support mapping data provider
MAX_ITER: Maximum number of iterations for MPR algorithm
COLLIDE_EPSILON: Small number for numerical comparisons
Returns:
Tuple of:
collision detected (bool): True if shapes are colliding
signed_distance (float): Signed distance (negative indicates overlap)
contact point center (wp.vec3): Midpoint between witness points in world space
normal (wp.vec3): Contact normal from A to B in world space
"""
# Transform shape B to local space of shape A
relative_orientation_b = wp.quat_inverse(orientation_a) * orientation_b
relative_position_b = wp.quat_rotate_inv(orientation_a, position_b - position_a)
# Call the core MPR algorithm
result = solve_mpr_core(
geom_a,
geom_b,
relative_orientation_b,
relative_position_b,
combined_margin,
data_provider,
MAX_ITER,
COLLIDE_EPSILON,
)
collision, point_a, point_b, normal, penetration = result
point = 0.5 * (point_a + point_b)
# Transform results back to world space
point = wp.quat_rotate(orientation_a, point) + position_a
normal = wp.quat_rotate(orientation_a, normal)
# Convert to Newton signed distance convention (negative = overlap, positive = separation)
signed_distance = -penetration
return collision, signed_distance, point, normal
solve_mpr.core = solve_mpr_core
return solve_mpr
================================================
FILE: newton/_src/geometry/multicontact.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
# This code is based on the multi-contact manifold generation from Jitter Physics 2
# Original: https://github.com/notgiven688/jitterphysics2
# Copyright (c) Thorben Linneweber (MIT License)
# The code has been translated from C# to Python and modified for use in Newton.
"""
Multi-contact manifold generation for collision detection.
This module implements contact manifold generation algorithms for computing
multiple contact points between colliding shapes. It includes polygon clipping
and contact point selection algorithms.
"""
from typing import Any
import warp as wp
from newton._src.math import orthonormal_basis
from .contact_data import ContactData
from .mpr import create_support_map_function
# Constants
EPS = 0.00001
# The tilt angle defines how much the search direction gets tilted while searching for
# points on the contact manifold.
TILT_ANGLE_RAD = wp.static(2.0 * wp.pi / 180.0)
SIN_TILT_ANGLE = wp.static(wp.sin(TILT_ANGLE_RAD))
COS_TILT_ANGLE = wp.static(wp.cos(TILT_ANGLE_RAD))
COS_DEEPEST_CONTACT_THRESHOLD_ANGLE = wp.static(wp.cos(0.1 * wp.pi / 180.0))
@wp.func
def should_include_deepest_contact(normal_dot: float) -> bool:
return normal_dot < COS_DEEPEST_CONTACT_THRESHOLD_ANGLE
@wp.func
def excess_normal_deviation(dir_a: wp.vec3, dir_b: wp.vec3) -> bool:
"""
Check if the angle between two direction vectors exceeds the tilt angle threshold.
This is used to detect when contact polygon normals deviate too much from the
collision normal, indicating that the contact manifold may be unreliable.
Args:
dir_a: First direction vector.
dir_b: Second direction vector.
Returns:
True if the angle between the vectors exceeds TILT_ANGLE_RAD (2 degrees).
"""
dot = wp.abs(wp.dot(dir_a, dir_b))
return dot < COS_TILT_ANGLE
@wp.func
def signed_area(a: wp.vec2, b: wp.vec2, query_point: wp.vec2) -> float:
"""
Calculates twice the signed area for the triangle (a, b, query_point).
The result's sign indicates the triangle's orientation and is a robust way
to check which side of a line a point is on.
Args:
a: The first vertex of the triangle and the start of the line segment.
b: The second vertex of the triangle and the end of the line segment.
query_point: The third vertex of the triangle, the point to test against the line a-b.
Returns:
The result's sign determines the orientation of the points:
- Positive (> 0): The points are in a counter-clockwise (CCW) order.
This means query_point is to the "left" of the directed line from a to b.
- Negative (< 0): The points are in a clockwise (CW) order.
This means query_point is to the "right" of the directed line from a to b.
- Zero (== 0): The points are collinear; query_point lies on the infinite line defined by a and b.
"""
# It returns twice the signed area of the triangle
return (b[0] - a[0]) * (query_point[1] - a[1]) - (b[1] - a[1]) * (query_point[0] - a[0])
@wp.func
def ray_plane_intersection(
ray_origin: wp.vec3, ray_direction: wp.vec3, plane_d: float, plane_normal: wp.vec3
) -> wp.vec3:
"""
Compute intersection of a ray with a plane.
The plane is defined by the equation: dot(point, plane_normal) + plane_d = 0
where plane_d = -dot(point_on_plane, plane_normal).
Args:
ray_origin: Starting point of the ray.
ray_direction: Direction vector of the ray.
plane_d: Plane distance parameter (negative dot product of any point on plane with normal).
plane_normal: Normal vector of the plane.
Returns:
Intersection point of the ray with the plane.
"""
denom = wp.dot(ray_direction, plane_normal)
# Avoid division by zero; if denom is near zero, return origin unchanged
if wp.abs(denom) < 1.0e-12:
return ray_origin
# Plane equation: dot(point, normal) + d = 0
# Solve for t: dot(ray_origin + t*ray_direction, normal) + d = 0
# t = -(dot(ray_origin, normal) + d) / dot(ray_direction, normal)
t = -(wp.dot(ray_origin, plane_normal) + plane_d) / denom
return ray_origin + ray_direction * t
@wp.struct
class BodyProjector:
"""
Plane projector for back-projecting contact points onto shape surfaces.
The plane is defined by the equation: dot(point, normal) + plane_d = 0
where plane_d = -dot(point_on_plane, normal) for any point on the plane.
This representation uses a single float instead of storing a full point_on_plane vector,
saving 8 bytes per projector (2 floats on typical architectures with alignment).
"""
plane_d: float
normal: wp.vec3
@wp.struct
class IncrementalPlaneTracker:
reference_point: wp.vec3
previous_point: wp.vec3
normal: wp.vec3
largest_area_sq: float
@wp.func
def update_incremental_plane_tracker(
tracker: IncrementalPlaneTracker,
current_point: wp.vec3,
current_point_id: int,
) -> IncrementalPlaneTracker:
"""
Update the incremental plane tracker with a new point.
"""
if current_point_id == 0:
tracker.reference_point = current_point
tracker.largest_area_sq = 0.0
elif current_point_id == 1:
tracker.previous_point = current_point
else:
edge1 = tracker.previous_point - tracker.reference_point
edge2 = current_point - tracker.reference_point
cross = wp.cross(edge1, edge2)
area_sq = wp.dot(cross, cross)
if area_sq > tracker.largest_area_sq:
tracker.largest_area_sq = area_sq
tracker.normal = cross
tracker.previous_point = current_point
return tracker
@wp.func
def compute_line_segment_projector_normal(
segment_dir: wp.vec3,
reference_normal: wp.vec3,
) -> wp.vec3:
"""
Compute a normal for a line segment projector that is perpendicular to the segment
and lies in the plane defined by the segment and the reference normal.
Args:
segment_dir: Direction vector of the line segment.
reference_normal: Normal from the other body to use as reference.
Returns:
Normalized normal vector for the line segment projector.
"""
right = wp.cross(segment_dir, reference_normal)
normal = wp.cross(right, segment_dir)
length = wp.length(normal)
return normal / length if length > 1.0e-12 else reference_normal
@wp.func
def create_body_projectors(
plane_tracker_a: IncrementalPlaneTracker,
anchor_point_a: wp.vec3,
plane_tracker_b: IncrementalPlaneTracker,
anchor_point_b: wp.vec3,
contact_normal: wp.vec3,
) -> tuple[BodyProjector, BodyProjector]:
projector_a = BodyProjector()
projector_b = BodyProjector()
if plane_tracker_a.largest_area_sq == 0.0 and plane_tracker_b.largest_area_sq == 0.0:
# Both are line segments - compute normals using contact_normal as reference
dir_a = plane_tracker_a.previous_point - plane_tracker_a.reference_point
dir_b = plane_tracker_b.previous_point - plane_tracker_b.reference_point
point_on_plane_a = 0.5 * (plane_tracker_a.reference_point + plane_tracker_a.previous_point)
projector_a.normal = compute_line_segment_projector_normal(dir_a, contact_normal)
projector_a.plane_d = -wp.dot(point_on_plane_a, projector_a.normal)
point_on_plane_b = 0.5 * (plane_tracker_b.reference_point + plane_tracker_b.previous_point)
projector_b.normal = compute_line_segment_projector_normal(dir_b, contact_normal)
projector_b.plane_d = -wp.dot(point_on_plane_b, projector_b.normal)
return projector_a, projector_b
if plane_tracker_a.largest_area_sq > 0.0:
len_n = wp.sqrt(wp.max(1.0e-12, plane_tracker_a.largest_area_sq))
projector_a.normal = plane_tracker_a.normal / len_n
projector_a.plane_d = -wp.dot(anchor_point_a, projector_a.normal)
if plane_tracker_b.largest_area_sq > 0.0:
len_n = wp.sqrt(wp.max(1.0e-12, plane_tracker_b.largest_area_sq))
projector_b.normal = plane_tracker_b.normal / len_n
projector_b.plane_d = -wp.dot(anchor_point_b, projector_b.normal)
if plane_tracker_a.largest_area_sq == 0.0:
dir = plane_tracker_a.previous_point - plane_tracker_a.reference_point
point_on_plane_a = 0.5 * (plane_tracker_a.reference_point + plane_tracker_a.previous_point)
projector_a.normal = compute_line_segment_projector_normal(dir, projector_b.normal)
projector_a.plane_d = -wp.dot(point_on_plane_a, projector_a.normal)
if plane_tracker_b.largest_area_sq == 0.0:
dir = plane_tracker_b.previous_point - plane_tracker_b.reference_point
point_on_plane_b = 0.5 * (plane_tracker_b.reference_point + plane_tracker_b.previous_point)
projector_b.normal = compute_line_segment_projector_normal(dir, projector_a.normal)
projector_b.plane_d = -wp.dot(point_on_plane_b, projector_b.normal)
return projector_a, projector_b
@wp.func
def body_projector_project(
proj: BodyProjector,
input: wp.vec3,
contact_normal: wp.vec3,
) -> wp.vec3:
"""
Project a point back onto the original shape surface using a plane projector.
This function casts a ray from the input point along the contact normal and
finds where it intersects the projector's plane.
Args:
proj: Body projector defining the projection plane.
input: Point to project (typically in contact plane space).
contact_normal: Direction to cast the ray (typically the collision normal).
Returns:
Projected point on the shape's surface in world space.
"""
# Only plane projection is supported
return ray_plane_intersection(input, contact_normal, proj.plane_d, proj.normal)
@wp.func
def intersection_point(trim_seg_start: wp.vec2, trim_seg_end: wp.vec2, a: wp.vec2, b: wp.vec2) -> wp.vec2:
"""
Calculate the intersection point between a line segment and a polygon edge.
It is known that a and b lie on different sides of the trim segment.
Args:
trim_seg_start: Start point of the trimming segment.
trim_seg_end: End point of the trimming segment.
a: First point of the polygon edge.
b: Second point of the polygon edge.
Returns:
The intersection point as a vec2.
"""
# Since a and b are on opposite sides, their signed areas have opposite signs
# We can optimize: abs(signed_a) + abs(signed_b) = abs(signed_a - signed_b)
signed_a = signed_area(trim_seg_start, trim_seg_end, a)
signed_b = signed_area(trim_seg_start, trim_seg_end, b)
interp_ab = wp.abs(signed_a) / wp.abs(signed_a - signed_b)
# Interpolate between a and b
return (1.0 - interp_ab) * a + interp_ab * b
@wp.func
def insert_vec2(arr: wp.array[wp.vec2], arr_count: int, index: int, element: wp.vec2):
"""
Insert an element into an array at the specified index, shifting elements to the right.
Args:
arr: Array to insert into.
arr_count: Current number of elements in the array.
index: Index at which to insert the element.
element: Element to insert.
"""
i = arr_count
while i > index:
arr[i] = arr[i - 1]
i -= 1
arr[index] = element
@wp.func
def trim_in_place(
trim_seg_start: wp.vec2,
trim_seg_end: wp.vec2,
loop: wp.array[wp.vec2],
loop_count: int,
) -> int:
"""
Trim a polygon in place using a line segment (Sutherland-Hodgman clip).
All points are in 2D contact plane space.
Args:
trim_seg_start: Start point of the trimming segment.
trim_seg_end: End point of the trimming segment.
loop: Array of loop vertices (2D).
loop_count: Number of vertices in the loop.
Returns:
New number of vertices in the trimmed loop.
"""
if loop_count < 3:
return loop_count
intersection_a = wp.vec2(0.0, 0.0)
change_a = int(-1)
intersection_b = wp.vec2(0.0, 0.0)
change_b = int(-1)
keep = bool(False)
# Check first vertex
prev_outside = bool(signed_area(trim_seg_start, trim_seg_end, loop[0]) <= 0.0)
for i in range(loop_count):
next_idx = (i + 1) % loop_count
outside = signed_area(trim_seg_start, trim_seg_end, loop[next_idx]) <= 0.0
if outside != prev_outside:
intersection = intersection_point(trim_seg_start, trim_seg_end, loop[i], loop[next_idx])
if change_a < 0:
change_a = i
keep = not prev_outside
intersection_a = intersection
else:
change_b = i
intersection_b = intersection
prev_outside = outside
if change_a >= 0 and change_b >= 0:
loop_indexer = int(-1)
new_loop_count = int(loop_count)
i = int(0)
while i < loop_count:
# If the current vertex is on the side to be kept, copy it.
if keep:
loop_indexer += 1
loop[loop_indexer] = loop[i]
# If the current edge intersects the trim line, add the intersection point.
if i == change_a or i == change_b:
pt = intersection_a if i == change_a else intersection_b
# Handle special case: insertion needed when loop_indexer == i and not keep.
if loop_indexer == i and not keep:
loop_indexer += 1
insert_vec2(loop, new_loop_count, loop_indexer, pt)
new_loop_count += 1
i += 1
change_b += 1
loop_count += 1
else:
loop_indexer += 1
loop[loop_indexer] = pt
keep = not keep
i += 1
new_loop_count = loop_indexer + 1
elif prev_outside:
new_loop_count = 0
else:
new_loop_count = loop_count
return new_loop_count
@wp.func
def trim_all_in_place(
trim_poly: wp.array[wp.vec2],
trim_poly_count: int,
loop: wp.array[wp.vec2],
loop_count: int,
) -> int:
"""
Trim a polygon using all edges of another polygon (Sutherland-Hodgman clipping).
Both polygons (trim_poly and loop) are in 2D contact plane space and they are both convex.
Args:
trim_poly: Array of vertices defining the trimming polygon (2D).
trim_poly_count: Number of vertices in the trimming polygon.
loop: Array of vertices in the loop to be trimmed (2D).
loop_count: Number of vertices in the loop.
Returns:
New number of vertices in the trimmed loop.
"""
if trim_poly_count <= 1:
return wp.min(1, loop_count) # There is no trim polygon
move_distance = float(1e-5)
if trim_poly_count == 2:
# Convert line segment to thin rectangle
p0 = trim_poly[0]
p1 = trim_poly[1]
dir_x = p1[0] - p0[0]
dir_y = p1[1] - p0[1]
dir_len = wp.sqrt(dir_x * dir_x + dir_y * dir_y)
if dir_len > 1e-10:
perp_x = -dir_y / dir_len
perp_y = dir_x / dir_len
offset_x = perp_x * move_distance
offset_y = perp_y * move_distance
trim_poly[0] = wp.vec2(p0[0] - offset_x, p0[1] - offset_y)
trim_poly[1] = wp.vec2(p1[0] - offset_x, p1[1] - offset_y)
trim_poly[2] = wp.vec2(p1[0] + offset_x, p1[1] + offset_y)
trim_poly[3] = wp.vec2(p0[0] + offset_x, p0[1] + offset_y)
trim_poly_count = 4
else:
return wp.min(1, loop_count)
if loop_count == 2:
# Convert line segment to thin rectangle
p0 = loop[0]
p1 = loop[1]
dir_x = p1[0] - p0[0]
dir_y = p1[1] - p0[1]
dir_len = wp.sqrt(dir_x * dir_x + dir_y * dir_y)
if dir_len > 1e-10:
perp_x = -dir_y / dir_len
perp_y = dir_x / dir_len
offset_x = perp_x * move_distance
offset_y = perp_y * move_distance
loop[0] = wp.vec2(p0[0] - offset_x, p0[1] - offset_y)
loop[1] = wp.vec2(p1[0] - offset_x, p1[1] - offset_y)
loop[2] = wp.vec2(p1[0] + offset_x, p1[1] + offset_y)
loop[3] = wp.vec2(p0[0] + offset_x, p0[1] + offset_y)
loop_count = 4
else:
return wp.min(1, loop_count)
current_loop_count = loop_count
trim_poly_0 = trim_poly[0] # This allows to do more memory aliasing
for i in range(trim_poly_count):
trim_seg_start = trim_poly[i]
trim_seg_end = trim_poly_0 if i == trim_poly_count - 1 else trim_poly[i + 1]
current_loop_count = trim_in_place(trim_seg_start, trim_seg_end, loop, current_loop_count)
return current_loop_count
@wp.func
def approx_max_quadrilateral_area_with_calipers(hull: wp.array[wp.vec2], hull_count: int) -> wp.vec4i:
"""
Finds an approximate maximum area quadrilateral inside a convex hull in O(n) time
using the Rotating Calipers algorithm to find the hull's diameter.
Args:
hull: Array of hull vertices (2D).
hull_count: Number of vertices in the hull.
Returns:
vec4i containing (p1, p2, p3, p4) where p1, p2, p3, p4 are the indices
of the quadrilateral vertices that form the maximum area quadrilateral.
"""
n = hull_count
# --- Step 1: Find the hull's diameter using Rotating Calipers in O(n) ---
p1 = int(0)
p3 = int(1)
hp1 = hull[p1]
hp3 = hull[p3]
diff = wp.vec2(hp1[0] - hp3[0], hp1[1] - hp3[1])
max_dist_sq = diff[0] * diff[0] + diff[1] * diff[1]
# Relative epsilon for tie-breaking: only update if new value is at least (1 + epsilon) times better
# This is scale-invariant and avoids catastrophic cancellation in floating-point comparisons
# Important for objects with circular geometry to ensure consistent point selection
tie_epsilon_rel = 1.0e-3
# Start with point j opposite point i=0
j = int(1)
for i in range(n):
# For the current point i, find its antipodal point j by advancing j
# while the area of the triangle formed by the edge (i, i+1) and point j increases.
# This is equivalent to finding the point j furthest from the edge (i, i+1).
hull_i = hull[i]
hull_i_plus_1 = hull[(i + 1) % n]
while True:
hull_j = hull[j]
hull_j_plus_1 = hull[(j + 1) % n]
area_j_plus_1 = signed_area(hull_i, hull_i_plus_1, hull_j_plus_1)
area_j = signed_area(hull_i, hull_i_plus_1, hull_j)
if area_j_plus_1 > area_j:
j = (j + 1) % n
else:
break
# Now, (i, j) is an antipodal pair. Check its distance (2D)
hi = hull[i]
hj = hull[j]
d1 = wp.vec2(hi[0] - hj[0], hi[1] - hj[1])
dist_sq_1 = d1[0] * d1[0] + d1[1] * d1[1]
# Use relative tie-breaking: only update if new distance is meaningfully larger
if dist_sq_1 > max_dist_sq * (1.0 + tie_epsilon_rel):
max_dist_sq = dist_sq_1
p1 = i
p3 = j
# The next point, (i+1, j), is also an antipodal pair. Check its distance too (2D)
hip1 = hull[(i + 1) % n]
d2 = wp.vec2(hip1[0] - hj[0], hip1[1] - hj[1])
dist_sq_2 = d2[0] * d2[0] + d2[1] * d2[1]
# Use relative tie-breaking: only update if new distance is meaningfully larger
if dist_sq_2 > max_dist_sq * (1.0 + tie_epsilon_rel):
max_dist_sq = dist_sq_2
p1 = (i + 1) % n
p3 = j
# --- Step 2: Find points p2 and p4 furthest from the diameter (p1, p3) ---
p2 = int(0)
p4 = int(0)
max_area_1 = float(0.0)
max_area_2 = float(0.0)
hull_p1 = hull[p1]
hull_p3 = hull[p3]
for i in range(n):
# Use the signed area to determine which side of the line the point is on.
hull_i = hull[i]
area = signed_area(hull_p1, hull_p3, hull_i)
# Use relative tie-breaking: only update if new area is meaningfully larger
if area > max_area_1 * (1.0 + tie_epsilon_rel):
max_area_1 = area
p2 = i
elif -area > max_area_2 * (1.0 + tie_epsilon_rel): # Check the other side
max_area_2 = -area
p4 = i
return wp.vec4i(p1, p2, p3, p4)
@wp.func
def remove_zero_length_edges(loop: wp.array[wp.vec2], loop_count: int, eps: float) -> int:
"""
Remove zero-length edges from a polygon loop.
Args:
loop: Array of loop vertices (2D).
loop_count: Number of vertices in the loop.
eps: Epsilon threshold for considering edges as zero-length.
Returns:
New number of vertices in the cleaned loop.
"""
if loop_count < 2:
return 0
write_idx = int(0)
for read_idx in range(1, loop_count):
diff = loop[read_idx] - loop[write_idx]
if wp.length_sq(diff) > eps:
write_idx += 1
loop[write_idx] = loop[read_idx]
# Handle loop closure
if write_idx > 0:
diff = loop[write_idx] - loop[0]
if wp.length_sq(diff) < eps:
new_loop_count = write_idx
else:
new_loop_count = write_idx + 1
else:
new_loop_count = write_idx + 1
if new_loop_count < 2:
new_loop_count = 0
return new_loop_count
@wp.func
def add_avoid_duplicates_vec2(arr: wp.array[wp.vec2], arr_count: int, vec: wp.vec2, eps: float) -> tuple[int, bool]:
"""
Add a vector to an array, avoiding duplicates.
Args:
arr: Array to add to.
arr_count: Current number of elements in the array.
vec: Vector to add.
eps: Epsilon threshold for duplicate detection.
Returns:
Tuple of (new_count, was_added) where was_added is True if point was added
"""
# Check for duplicates. If the new vertex 'vec' is too close to the first or last existing vertex, ignore it.
# This is a simple reduction step to avoid redundant points.
if arr_count > 0:
if wp.length_sq(arr[0] - vec) < eps:
return arr_count, False
if arr_count > 1:
if wp.length_sq(arr[arr_count - 1] - vec) < eps:
return arr_count, False
arr[arr_count] = vec
return arr_count + 1, True
@wp.func_native("""
return (uint64_t)a.data;
""")
def get_ptr(a: wp.array[wp.vec2]) -> wp.uint64: ...
def create_build_manifold(support_func: Any, writer_func: Any, post_process_contact: Any, _support_funcs: Any = None):
"""
Factory function to create manifold generation functions with a specific support mapping function.
This factory creates two related functions for multi-contact manifold generation:
- build_manifold_core: The core implementation that uses preallocated buffers
- build_manifold: The main entry point that handles buffer allocation and result extraction
Args:
support_func: Support mapping function for shapes that takes
(geometry, direction, data_provider) and returns a support point
writer_func: Function to write contact data (signature: (ContactData, writer_data) -> None)
post_process_contact: Function to post-process contact data
Returns:
build_manifold function that generates up to 5 contact points between two shapes
using perturbed support mapping and polygon clipping.
"""
if _support_funcs is not None:
_support_map_b = _support_funcs[0]
else:
_support_map_b = create_support_map_function(support_func)[0]
@wp.func
def extract_4_point_contact_manifolds(
m_a: wp.array[wp.vec2],
m_a_count: int,
m_b: wp.array[wp.vec2],
m_b_count: int,
normal_local: wp.vec3,
cross_vector_1: wp.vec3,
cross_vector_2: wp.vec3,
center_local: wp.vec3,
projector_a: BodyProjector,
projector_b: BodyProjector,
orientation_a: wp.quat,
position_a_world: wp.vec3,
normal_world: wp.vec3,
writer_data: Any,
contact_template: Any,
geom_a: Any,
geom_b: Any,
position_a: wp.vec3,
position_b: wp.vec3,
quaternion_a: wp.quat,
quaternion_b: wp.quat,
) -> tuple[int, float]:
"""
Extract up to 4 contact points from two convex contact polygons and write them immediately.
All intermediate work (clipping, projectors) operates in shape A's local frame.
Final contact points are transformed to world space before writing.
Args:
m_a: Contact polygon vertices for shape A (2D contact plane space, up to 5 points).
m_a_count: Number of vertices in polygon A.
m_b: Contact polygon vertices for shape B (2D contact plane space, up to 5 points, space for 10).
m_b_count: Number of vertices in polygon B.
normal_local: Collision normal in A-local frame.
cross_vector_1: First tangent vector in A-local frame.
cross_vector_2: Second tangent vector in A-local frame.
center_local: Center point for back-projection in A-local frame.
projector_a: Body projector for shape A (in A-local frame).
projector_b: Body projector for shape B (in A-local frame).
orientation_a: World orientation of shape A (for final transform).
position_a_world: World position of shape A (for final transform).
normal_world: Contact normal in world space (for output).
writer_data: Data structure for contact writer.
contact_template: Pre-packed ContactData with static fields.
geom_a: Geometry data for shape A.
geom_b: Geometry data for shape B.
position_a: World position of shape A (for post_process_contact).
position_b: World position of shape B (for post_process_contact).
quaternion_a: Orientation of shape A (for post_process_contact).
quaternion_b: Orientation of shape B (for post_process_contact).
Returns:
Tuple of (loop_count, normal_dot) where:
- loop_count: Number of valid contact points written (0-4)
- normal_dot: Absolute dot product of polygon normals
"""
normal_dot = wp.abs(wp.dot(projector_a.normal, projector_b.normal))
loop_count = trim_all_in_place(m_a, m_a_count, m_b, m_b_count)
loop_count = remove_zero_length_edges(m_b, loop_count, EPS)
if loop_count > 1:
result = wp.vec4i()
if loop_count > 4:
result = approx_max_quadrilateral_area_with_calipers(m_b, loop_count)
loop_count = 4
else:
result = wp.vec4i(0, 1, 2, 3)
for i in range(loop_count):
ia = int(result[i])
# Back-project from 2D to 3D in A-local frame
p_local = m_b[ia].x * cross_vector_1 + m_b[ia].y * cross_vector_2 + center_local
a = body_projector_project(projector_a, p_local, normal_local)
b = body_projector_project(projector_b, p_local, normal_local)
contact_point_local = 0.5 * (a + b)
signed_distance = wp.dot(b - a, normal_local)
# Transform from A-local to world space
contact_point_world = wp.quat_rotate(orientation_a, contact_point_local) + position_a_world
contact_data = contact_template
contact_data.contact_point_center = contact_point_world
contact_data.contact_normal_a_to_b = normal_world
contact_data.contact_distance = signed_distance
contact_data = post_process_contact(
contact_data, geom_a, position_a, quaternion_a, geom_b, position_b, quaternion_b
)
writer_func(contact_data, writer_data, -1)
else:
normal_dot = 0.0
loop_count = 0
return loop_count, normal_dot
@wp.func
def build_manifold(
geom_a: Any,
geom_b: Any,
orientation_a: wp.quat,
position_a_world: wp.vec3,
relative_orientation_b: wp.quat,
relative_position_b: wp.vec3,
p_a: wp.vec3,
p_b: wp.vec3,
normal: wp.vec3,
data_provider: Any,
writer_data: Any,
contact_template: ContactData,
) -> int:
"""
Build a contact manifold between two convex shapes and write contacts directly.
All intermediate work operates in shape A's local frame to avoid redundant
quaternion transforms. Final contact points are transformed to world space
before writing.
Args:
geom_a: Geometry data for the first shape.
geom_b: Geometry data for the second shape.
orientation_a: World orientation of shape A (for final world-space transform).
position_a_world: World position of shape A (for final world-space transform).
relative_orientation_b: Orientation of B relative to A.
relative_position_b: Position of B relative to A (in A-local frame).
p_a: Anchor contact point on shape A in A-local frame (from GJK/MPR).
p_b: Anchor contact point on shape B in A-local frame (from GJK/MPR).
normal: Contact normal in A-local frame pointing from A to B.
data_provider: Support mapping data provider for shape queries.
writer_data: Data structure for contact writer.
contact_template: Pre-packed ContactData with static fields.
Returns:
Number of valid contact points written (0-5).
"""
# Precomputed cos/sin for 5 evenly spaced pentagonal angles (0, 72, 144, 216, 288 deg).
PENT_COS_0 = float(1.0)
PENT_SIN_0 = float(0.0)
PENT_COS_1 = wp.static(wp.cos(2.0 * wp.pi / 5.0))
PENT_SIN_1 = wp.static(wp.sin(2.0 * wp.pi / 5.0))
PENT_COS_2 = wp.static(wp.cos(4.0 * wp.pi / 5.0))
PENT_SIN_2 = wp.static(wp.sin(4.0 * wp.pi / 5.0))
PENT_COS_3 = wp.static(wp.cos(6.0 * wp.pi / 5.0))
PENT_SIN_3 = wp.static(wp.sin(6.0 * wp.pi / 5.0))
PENT_COS_4 = wp.static(wp.cos(8.0 * wp.pi / 5.0))
PENT_SIN_4 = wp.static(wp.sin(8.0 * wp.pi / 5.0))
a_count = int(0)
b_count = int(0)
# Orthonormal basis from the collision normal (in A-local frame).
tangent_a, tangent_b = orthonormal_basis(normal)
plane_tracker_a = IncrementalPlaneTracker()
plane_tracker_b = IncrementalPlaneTracker()
center = 0.5 * (p_a + p_b)
# Allocate buffers: 5 for A, up to 10 for B (5 + clipping headroom)
b_buffer = wp.zeros(shape=(10,), dtype=wp.vec2f)
a_buffer = wp.array(ptr=get_ptr(b_buffer) + wp.uint64(5 * 8), shape=(5,), dtype=wp.vec2f)
# --- Step 1: Find Contact Polygons using Perturbed Support Mapping ---
# Shape A: support_func returns points in A-local frame directly, no quat_rotate needed.
# Shape B: pre-transform basis to B-local, then transform results back to A-local.
local_normal_b = wp.quat_rotate_inv(relative_orientation_b, -normal)
local_ta_b = wp.quat_rotate_inv(relative_orientation_b, -tangent_a)
local_tb_b = wp.quat_rotate_inv(relative_orientation_b, -tangent_b)
for e in range(5):
c = PENT_COS_0
s = PENT_SIN_0
if e == 1:
c = PENT_COS_1
s = PENT_SIN_1
elif e == 2:
c = PENT_COS_2
s = PENT_SIN_2
elif e == 3:
c = PENT_COS_3
s = PENT_SIN_3
elif e == 4:
c = PENT_COS_4
s = PENT_SIN_4
cos_tilt = COS_TILT_ANGLE
c_sin = c * SIN_TILT_ANGLE
s_sin = s * SIN_TILT_ANGLE
# Shape A: direction and result both in A-local frame, zero quaternion ops.
dir_a = normal * cos_tilt + c_sin * tangent_a + s_sin * tangent_b
pt_a_3d = support_func(geom_a, dir_a, data_provider)
projected_a = pt_a_3d - center
pt_a_2d = wp.vec2(wp.dot(tangent_a, projected_a), wp.dot(tangent_b, projected_a))
a_count, was_added_a = add_avoid_duplicates_vec2(a_buffer, a_count, pt_a_2d, EPS)
if was_added_a:
plane_tracker_a = update_incremental_plane_tracker(plane_tracker_a, pt_a_3d, a_count - 1)
# Shape B: direction in B-local, result transformed to A-local.
local_dir_b = local_normal_b * cos_tilt + c_sin * local_ta_b + s_sin * local_tb_b
pt_b_local = support_func(geom_b, local_dir_b, data_provider)
pt_b_3d = wp.quat_rotate(relative_orientation_b, pt_b_local) + relative_position_b
projected_b = pt_b_3d - center
pt_b_2d = wp.vec2(wp.dot(tangent_a, projected_b), wp.dot(tangent_b, projected_b))
b_count, was_added_b = add_avoid_duplicates_vec2(b_buffer, b_count, pt_b_2d, EPS)
if was_added_b:
plane_tracker_b = update_incremental_plane_tracker(plane_tracker_b, pt_b_3d, b_count - 1)
# World-space normal (computed once for all output contacts)
normal_world = wp.quat_rotate(orientation_a, normal)
# World-space positions/orientations for post_process_contact
position_a_ws = position_a_world
position_b_ws = wp.quat_rotate(orientation_a, relative_position_b) + position_a_world
quaternion_a_ws = orientation_a
quaternion_b_ws = orientation_a * relative_orientation_b
if a_count < 2 or b_count < 2:
count_out = 0
normal_dot = 0.0
else:
projector_a, projector_b = create_body_projectors(plane_tracker_a, p_a, plane_tracker_b, p_b, normal)
if excess_normal_deviation(normal, projector_a.normal) or excess_normal_deviation(
normal, projector_b.normal
):
count_out = 0
normal_dot = 0.0
else:
num_manifold_points, normal_dot = extract_4_point_contact_manifolds(
a_buffer,
a_count,
b_buffer,
b_count,
normal,
tangent_a,
tangent_b,
center,
projector_a,
projector_b,
orientation_a,
position_a_world,
normal_world,
writer_data,
contact_template,
geom_a,
geom_b,
position_a_ws,
position_b_ws,
quaternion_a_ws,
quaternion_b_ws,
)
count_out = wp.min(num_manifold_points, 4)
if should_include_deepest_contact(normal_dot) or count_out == 0:
deepest_center_local = 0.5 * (p_a + p_b)
deepest_signed_distance = wp.dot(p_b - p_a, normal)
deepest_center_world = wp.quat_rotate(orientation_a, deepest_center_local) + position_a_world
contact_data = contact_template
contact_data.contact_point_center = deepest_center_world
contact_data.contact_normal_a_to_b = normal_world
contact_data.contact_distance = deepest_signed_distance
contact_data = post_process_contact(
contact_data, geom_a, position_a_ws, quaternion_a_ws, geom_b, position_b_ws, quaternion_b_ws
)
writer_func(contact_data, writer_data, -1)
count_out += 1
return count_out
return build_manifold
================================================
FILE: newton/_src/geometry/narrow_phase.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warnings
from typing import Any
import warp as wp
from ..core.types import MAXVAL, Devicelike
from ..geometry.collision_core import (
ENABLE_TILE_BVH_QUERY,
check_infinite_plane_bsphere_overlap,
compute_bounding_sphere_from_aabb,
compute_tight_aabb_from_support,
create_compute_gjk_mpr_contacts,
create_find_contacts,
get_triangle_shape_from_mesh,
mesh_vs_convex_midphase,
post_process_minkowski_only,
)
from ..geometry.collision_primitive import (
collide_capsule_capsule,
collide_plane_box,
collide_plane_capsule,
collide_plane_cylinder,
collide_plane_ellipsoid,
collide_plane_sphere,
collide_sphere_box,
collide_sphere_capsule,
collide_sphere_cylinder,
collide_sphere_sphere,
)
from ..geometry.contact_data import SHAPE_PAIR_HFIELD_BIT, ContactData, contact_passes_gap_check
from ..geometry.contact_reduction_global import (
GlobalContactReducer,
create_export_reduced_contacts_kernel,
mesh_triangle_contacts_to_reducer_kernel,
reduce_buffered_contacts_kernel,
write_contact_to_reducer,
)
from ..geometry.flags import ShapeFlags
from ..geometry.sdf_contact import (
compute_block_counts_from_weights,
compute_mesh_mesh_block_offsets_scan,
create_narrow_phase_process_mesh_mesh_contacts_kernel,
)
from ..geometry.sdf_hydroelastic import HydroelasticSDF
from ..geometry.sdf_texture import TextureSDFData
from ..geometry.support_function import (
SupportMapDataProvider,
extract_shape_data,
support_map_lean,
)
from ..geometry.types import GeoType
from ..utils.heightfield import (
HeightfieldData,
get_triangle_shape_from_heightfield,
heightfield_vs_convex_midphase,
)
@wp.struct
class ContactWriterData:
contact_max: int
contact_count: wp.array[int]
contact_pair: wp.array[wp.vec2i]
contact_position: wp.array[wp.vec3]
contact_normal: wp.array[wp.vec3]
contact_penetration: wp.array[float]
contact_tangent: wp.array[wp.vec3]
@wp.func
def write_contact_simple(
contact_data: ContactData,
writer_data: ContactWriterData,
output_index: int,
):
"""
Write a contact to the output arrays using the simplified API format.
Args:
contact_data: ContactData struct containing contact information
writer_data: ContactWriterData struct containing output arrays
output_index: If -1, use atomic_add to get the next available index if contact distance is less than gap_sum. If >= 0, use this index directly and skip gap check.
"""
total_separation_needed = (
contact_data.radius_eff_a + contact_data.radius_eff_b + contact_data.margin_a + contact_data.margin_b
)
contact_normal_a_to_b = wp.normalize(contact_data.contact_normal_a_to_b)
a_contact_world = contact_data.contact_point_center - contact_normal_a_to_b * (
0.5 * contact_data.contact_distance + contact_data.radius_eff_a
)
b_contact_world = contact_data.contact_point_center + contact_normal_a_to_b * (
0.5 * contact_data.contact_distance + contact_data.radius_eff_b
)
diff = b_contact_world - a_contact_world
distance = wp.dot(diff, contact_normal_a_to_b)
d = distance - total_separation_needed
if output_index < 0:
if d >= contact_data.gap_sum:
return
index = wp.atomic_add(writer_data.contact_count, 0, 1)
else:
index = output_index
if index >= writer_data.contact_max:
return
writer_data.contact_pair[index] = wp.vec2i(contact_data.shape_a, contact_data.shape_b)
writer_data.contact_position[index] = contact_data.contact_point_center
writer_data.contact_normal[index] = contact_normal_a_to_b
writer_data.contact_penetration[index] = d
if writer_data.contact_tangent.shape[0] > 0:
world_x = wp.vec3(1.0, 0.0, 0.0)
normal = contact_normal_a_to_b
if wp.abs(wp.dot(normal, world_x)) > 0.99:
world_x = wp.vec3(0.0, 1.0, 0.0)
writer_data.contact_tangent[index] = wp.normalize(world_x - wp.dot(world_x, normal) * normal)
def create_narrow_phase_primitive_kernel(writer_func: Any):
"""
Create a kernel for fast analytical collision detection of primitive shapes.
This kernel handles lightweight primitive pairs (sphere-sphere, sphere-capsule,
capsule-capsule, plane-sphere, plane-capsule) using direct analytical formulas
instead of iterative GJK/MPR. Remaining pairs are routed to specialized buffers
for mesh handling or to the GJK/MPR kernel for complex convex pairs.
Args:
writer_func: Contact writer function (e.g., write_contact_simple)
Returns:
A warp kernel for primitive collision detection
"""
@wp.kernel(enable_backward=False, module="unique")
def narrow_phase_primitive_kernel(
candidate_pair: wp.array[wp.vec2i],
candidate_pair_count: wp.array[int],
shape_types: wp.array[int],
shape_data: wp.array[wp.vec4],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
shape_gap: wp.array[float],
shape_flags: wp.array[wp.int32],
writer_data: Any,
total_num_threads: int,
# Output: pairs that need GJK/MPR processing
gjk_candidate_pairs: wp.array[wp.vec2i],
gjk_candidate_pairs_count: wp.array[int],
# Output: mesh collision pairs (for mesh processing)
shape_pairs_mesh: wp.array[wp.vec2i],
shape_pairs_mesh_count: wp.array[int],
# Output: mesh-plane collision pairs
shape_pairs_mesh_plane: wp.array[wp.vec2i],
shape_pairs_mesh_plane_cumsum: wp.array[int],
shape_pairs_mesh_plane_count: wp.array[int],
mesh_plane_vertex_total_count: wp.array[int],
# Output: mesh-mesh collision pairs
shape_pairs_mesh_mesh: wp.array[wp.vec2i],
shape_pairs_mesh_mesh_count: wp.array[int],
# Output: sdf-sdf hydroelastic collision pairs
shape_pairs_sdf_sdf: wp.array[wp.vec2i],
shape_pairs_sdf_sdf_count: wp.array[int],
):
"""
Fast narrow phase kernel for primitive shape collisions.
Handles sphere-sphere, sphere-capsule, capsule-capsule, plane-sphere, and
plane-capsule collisions analytically. Routes mesh pairs and complex convex
pairs to specialized processing pipelines.
"""
tid = wp.tid()
num_work_items = wp.min(candidate_pair.shape[0], candidate_pair_count[0])
# Early exit if no work
if num_work_items == 0:
return
for t in range(tid, num_work_items, total_num_threads):
# Get shape pair
pair = candidate_pair[t]
shape_a = pair[0]
shape_b = pair[1]
# Safety: ignore self-collision and invalid pairs
if shape_a == shape_b or shape_a < 0 or shape_b < 0:
continue
# Get shape types
type_a = shape_types[shape_a]
type_b = shape_types[shape_b]
# Sort shapes by type to ensure consistent collision handling order
if type_a > type_b:
shape_a, shape_b = shape_b, shape_a
type_a, type_b = type_b, type_a
# Check if both shapes are hydroelastic - route to SDF-SDF pipeline
is_hydro_a = (shape_flags[shape_a] & ShapeFlags.HYDROELASTIC) != 0
is_hydro_b = (shape_flags[shape_b] & ShapeFlags.HYDROELASTIC) != 0
if is_hydro_a and is_hydro_b and shape_pairs_sdf_sdf:
idx = wp.atomic_add(shape_pairs_sdf_sdf_count, 0, 1)
if idx < shape_pairs_sdf_sdf.shape[0]:
shape_pairs_sdf_sdf[idx] = wp.vec2i(shape_a, shape_b)
continue
# Get shape data
data_a = shape_data[shape_a]
data_b = shape_data[shape_b]
scale_a = wp.vec3(data_a[0], data_a[1], data_a[2])
scale_b = wp.vec3(data_b[0], data_b[1], data_b[2])
margin_offset_a = data_a[3]
margin_offset_b = data_b[3]
# Get transforms
X_a = shape_transform[shape_a]
X_b = shape_transform[shape_b]
pos_a = wp.transform_get_translation(X_a)
pos_b = wp.transform_get_translation(X_b)
quat_a = wp.transform_get_rotation(X_a)
quat_b = wp.transform_get_rotation(X_b)
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
gap_sum = gap_a + gap_b
# =====================================================================
# Route heightfield pairs.
# Heightfield-vs-mesh and heightfield-vs-heightfield go through the
# mesh-mesh SDF kernel (on-the-fly triangle + SDF evaluation).
# Other heightfield combinations (convex, plane) use the dedicated
# heightfield midphase with GJK/MPR per cell.
# =====================================================================
is_hfield_a = type_a == GeoType.HFIELD
is_hfield_b = type_b == GeoType.HFIELD
if is_hfield_a or is_hfield_b:
is_mesh_like_a = type_a == GeoType.MESH or is_hfield_a
is_mesh_like_b = type_b == GeoType.MESH or is_hfield_b
if is_mesh_like_a and is_mesh_like_b:
# Heightfield-vs-heightfield is unsupported in this path.
if is_hfield_a and is_hfield_b:
continue
# Normalize order so heightfield (if present) is always pair[0],
# and mark pair[0] with a high-bit flag consumed by the SDF kernel.
if is_hfield_b:
encoded_a = shape_b | SHAPE_PAIR_HFIELD_BIT
encoded_b = shape_a
elif is_hfield_a:
encoded_a = shape_a | SHAPE_PAIR_HFIELD_BIT
encoded_b = shape_b
else:
encoded_a = shape_a
encoded_b = shape_b
idx = wp.atomic_add(shape_pairs_mesh_mesh_count, 0, 1)
if idx < shape_pairs_mesh_mesh.shape[0]:
shape_pairs_mesh_mesh[idx] = wp.vec2i(encoded_a, encoded_b)
continue
# All other heightfield pairs: route through mesh midphase + GJK/MPR.
# Normalize so the heightfield is always pair[0].
if is_hfield_a:
hf_pair = wp.vec2i(shape_a, shape_b)
else:
hf_pair = wp.vec2i(shape_b, shape_a)
idx = wp.atomic_add(shape_pairs_mesh_count, 0, 1)
if idx < shape_pairs_mesh.shape[0]:
shape_pairs_mesh[idx] = hf_pair
continue
# =====================================================================
# Route mesh pairs to specialized buffers
# =====================================================================
is_mesh_a = type_a == GeoType.MESH
is_mesh_b = type_b == GeoType.MESH
is_plane_a = type_a == GeoType.PLANE
is_infinite_plane_a = is_plane_a and (scale_a[0] == 0.0 and scale_a[1] == 0.0)
# Mesh-mesh collision
if is_mesh_a and is_mesh_b:
idx = wp.atomic_add(shape_pairs_mesh_mesh_count, 0, 1)
if idx < shape_pairs_mesh_mesh.shape[0]:
shape_pairs_mesh_mesh[idx] = wp.vec2i(shape_a, shape_b)
continue
# Mesh-plane collision (infinite plane only)
if is_infinite_plane_a and is_mesh_b:
mesh_id = shape_source[shape_b]
if mesh_id != wp.uint64(0):
mesh_obj = wp.mesh_get(mesh_id)
vertex_count = mesh_obj.points.shape[0]
mesh_plane_idx = wp.atomic_add(shape_pairs_mesh_plane_count, 0, 1)
if mesh_plane_idx < shape_pairs_mesh_plane.shape[0]:
# Store (mesh, plane)
shape_pairs_mesh_plane[mesh_plane_idx] = wp.vec2i(shape_b, shape_a)
cumulative_count_before = wp.atomic_add(mesh_plane_vertex_total_count, 0, vertex_count)
shape_pairs_mesh_plane_cumsum[mesh_plane_idx] = cumulative_count_before + vertex_count
continue
# Mesh-convex collision
if is_mesh_a or is_mesh_b:
idx = wp.atomic_add(shape_pairs_mesh_count, 0, 1)
if idx < shape_pairs_mesh.shape[0]:
shape_pairs_mesh[idx] = wp.vec2i(shape_a, shape_b)
continue
# =====================================================================
# Handle lightweight primitives analytically
# =====================================================================
is_sphere_a = type_a == GeoType.SPHERE
is_sphere_b = type_b == GeoType.SPHERE
is_capsule_a = type_a == GeoType.CAPSULE
is_capsule_b = type_b == GeoType.CAPSULE
is_ellipsoid_b = type_b == GeoType.ELLIPSOID
is_cylinder_b = type_b == GeoType.CYLINDER
is_box_b = type_b == GeoType.BOX
# Compute effective radii for spheres and capsules
# (radius that can be represented as Minkowski sum with a sphere)
radius_eff_a = float(0.0)
radius_eff_b = float(0.0)
if is_sphere_a or is_capsule_a:
radius_eff_a = scale_a[0]
if is_sphere_b or is_capsule_b:
radius_eff_b = scale_b[0]
# Initialize contact result storage (up to 4 contacts).
# Distances default to MAXVAL so unused slots are automatically
# excluded by the unified num_contacts count after the if/elif chain.
contact_dist_0 = float(MAXVAL)
contact_dist_1 = float(MAXVAL)
contact_dist_2 = float(MAXVAL)
contact_dist_3 = float(MAXVAL)
contact_pos_0 = wp.vec3()
contact_pos_1 = wp.vec3()
contact_pos_2 = wp.vec3()
contact_pos_3 = wp.vec3()
contact_normal = wp.vec3()
# -----------------------------------------------------------------
# Plane-Sphere collision (type_a=PLANE=0, type_b=SPHERE=2)
# -----------------------------------------------------------------
if is_plane_a and is_sphere_b:
plane_normal = wp.quat_rotate(quat_a, wp.vec3(0.0, 0.0, 1.0))
sphere_radius = scale_b[0]
contact_dist_0, contact_pos_0 = collide_plane_sphere(plane_normal, pos_a, pos_b, sphere_radius)
contact_normal = plane_normal
# -----------------------------------------------------------------
# Plane-Ellipsoid collision (type_a=PLANE=0, type_b=ELLIPSOID=4)
# Produces 1 contact
# -----------------------------------------------------------------
elif is_plane_a and is_ellipsoid_b:
plane_normal = wp.quat_rotate(quat_a, wp.vec3(0.0, 0.0, 1.0))
ellipsoid_rot = wp.quat_to_matrix(quat_b)
ellipsoid_size = scale_b
contact_dist_0, contact_pos_0, contact_normal = collide_plane_ellipsoid(
plane_normal, pos_a, pos_b, ellipsoid_rot, ellipsoid_size
)
# -----------------------------------------------------------------
# Plane-Box collision (type_a=PLANE=0, type_b=BOX=6)
# Produces up to 4 contacts
# -----------------------------------------------------------------
elif is_plane_a and is_box_b:
plane_normal = wp.quat_rotate(quat_a, wp.vec3(0.0, 0.0, 1.0))
box_rot = wp.quat_to_matrix(quat_b)
box_size = scale_b
dists4_box, positions4_box, contact_normal = collide_plane_box(
plane_normal, pos_a, pos_b, box_rot, box_size, gap_sum
)
contact_dist_0 = dists4_box[0]
contact_dist_1 = dists4_box[1]
contact_dist_2 = dists4_box[2]
contact_dist_3 = dists4_box[3]
contact_pos_0 = wp.vec3(positions4_box[0, 0], positions4_box[0, 1], positions4_box[0, 2])
contact_pos_1 = wp.vec3(positions4_box[1, 0], positions4_box[1, 1], positions4_box[1, 2])
contact_pos_2 = wp.vec3(positions4_box[2, 0], positions4_box[2, 1], positions4_box[2, 2])
contact_pos_3 = wp.vec3(positions4_box[3, 0], positions4_box[3, 1], positions4_box[3, 2])
# -----------------------------------------------------------------
# Sphere-Sphere collision (type_a=SPHERE=2, type_b=SPHERE=2)
# -----------------------------------------------------------------
elif is_sphere_a and is_sphere_b:
radius_a = scale_a[0]
radius_b = scale_b[0]
contact_dist_0, contact_pos_0, contact_normal = collide_sphere_sphere(pos_a, radius_a, pos_b, radius_b)
# -----------------------------------------------------------------
# Plane-Capsule collision (type_a=PLANE=0, type_b=CAPSULE=3)
# Produces 2 contacts (both share same normal)
# -----------------------------------------------------------------
elif is_plane_a and is_capsule_b:
plane_normal = wp.quat_rotate(quat_a, wp.vec3(0.0, 0.0, 1.0))
capsule_axis = wp.quat_rotate(quat_b, wp.vec3(0.0, 0.0, 1.0))
capsule_radius = scale_b[0]
capsule_half_length = scale_b[1]
dists, positions, _frame = collide_plane_capsule(
plane_normal, pos_a, pos_b, capsule_axis, capsule_radius, capsule_half_length
)
contact_dist_0 = dists[0]
contact_dist_1 = dists[1]
contact_pos_0 = wp.vec3(positions[0, 0], positions[0, 1], positions[0, 2])
contact_pos_1 = wp.vec3(positions[1, 0], positions[1, 1], positions[1, 2])
contact_normal = plane_normal
# -----------------------------------------------------------------
# Plane-Cylinder collision (type_a=PLANE=0, type_b=CYLINDER=5)
# Produces up to 4 contacts
# -----------------------------------------------------------------
elif is_plane_a and is_cylinder_b:
plane_normal = wp.quat_rotate(quat_a, wp.vec3(0.0, 0.0, 1.0))
cylinder_axis = wp.quat_rotate(quat_b, wp.vec3(0.0, 0.0, 1.0))
cylinder_radius = scale_b[0]
cylinder_half_height = scale_b[1]
dists4, positions4, contact_normal = collide_plane_cylinder(
plane_normal, pos_a, pos_b, cylinder_axis, cylinder_radius, cylinder_half_height
)
contact_dist_0 = dists4[0]
contact_dist_1 = dists4[1]
contact_dist_2 = dists4[2]
contact_dist_3 = dists4[3]
contact_pos_0 = wp.vec3(positions4[0, 0], positions4[0, 1], positions4[0, 2])
contact_pos_1 = wp.vec3(positions4[1, 0], positions4[1, 1], positions4[1, 2])
contact_pos_2 = wp.vec3(positions4[2, 0], positions4[2, 1], positions4[2, 2])
contact_pos_3 = wp.vec3(positions4[3, 0], positions4[3, 1], positions4[3, 2])
# -----------------------------------------------------------------
# Sphere-Capsule collision (type_a=SPHERE=2, type_b=CAPSULE=3)
# -----------------------------------------------------------------
elif is_sphere_a and is_capsule_b:
sphere_radius = scale_a[0]
capsule_axis = wp.quat_rotate(quat_b, wp.vec3(0.0, 0.0, 1.0))
capsule_radius = scale_b[0]
capsule_half_length = scale_b[1]
contact_dist_0, contact_pos_0, contact_normal = collide_sphere_capsule(
pos_a, sphere_radius, pos_b, capsule_axis, capsule_radius, capsule_half_length
)
# -----------------------------------------------------------------
# Capsule-Capsule collision (type_a=CAPSULE=3, type_b=CAPSULE=3)
# Produces 1 contact (non-parallel) or 2 contacts (parallel axes)
# -----------------------------------------------------------------
elif is_capsule_a and is_capsule_b:
axis_a = wp.quat_rotate(quat_a, wp.vec3(0.0, 0.0, 1.0))
axis_b = wp.quat_rotate(quat_b, wp.vec3(0.0, 0.0, 1.0))
radius_a = scale_a[0]
half_length_a = scale_a[1]
radius_b = scale_b[0]
half_length_b = scale_b[1]
dists, positions, contact_normal = collide_capsule_capsule(
pos_a, axis_a, radius_a, half_length_a, pos_b, axis_b, radius_b, half_length_b
)
contact_dist_0 = dists[0]
contact_pos_0 = wp.vec3(positions[0, 0], positions[0, 1], positions[0, 2])
contact_dist_1 = dists[1]
contact_pos_1 = wp.vec3(positions[1, 0], positions[1, 1], positions[1, 2])
# -----------------------------------------------------------------
# Sphere-Cylinder collision (type_a=SPHERE=2, type_b=CYLINDER=5)
# -----------------------------------------------------------------
elif is_sphere_a and is_cylinder_b:
sphere_radius = scale_a[0]
cylinder_axis = wp.quat_rotate(quat_b, wp.vec3(0.0, 0.0, 1.0))
cylinder_radius = scale_b[0]
cylinder_half_height = scale_b[1]
contact_dist_0, contact_pos_0, contact_normal = collide_sphere_cylinder(
pos_a, sphere_radius, pos_b, cylinder_axis, cylinder_radius, cylinder_half_height
)
# -----------------------------------------------------------------
# Sphere-Box collision (type_a=SPHERE=2, type_b=BOX=6)
# -----------------------------------------------------------------
elif is_sphere_a and is_box_b:
sphere_radius = scale_a[0]
box_rot = wp.quat_to_matrix(quat_b)
box_size = scale_b
contact_dist_0, contact_pos_0, contact_normal = collide_sphere_box(
pos_a, sphere_radius, pos_b, box_rot, box_size
)
# =====================================================================
# Write all contacts (single write block for 0 to 4 contacts)
# =====================================================================
num_contacts = (
int(contact_dist_0 < MAXVAL)
+ int(contact_dist_1 < MAXVAL)
+ int(contact_dist_2 < MAXVAL)
+ int(contact_dist_3 < MAXVAL)
)
if num_contacts > 0:
# Prepare contact data (shared fields for both contacts)
contact_data = ContactData()
contact_data.contact_normal_a_to_b = contact_normal
contact_data.radius_eff_a = radius_eff_a
contact_data.radius_eff_b = radius_eff_b
contact_data.margin_a = margin_offset_a
contact_data.margin_b = margin_offset_b
contact_data.shape_a = shape_a
contact_data.shape_b = shape_b
contact_data.gap_sum = gap_sum
# Check margin for all possible contacts
contact_0_valid = False
if contact_dist_0 < MAXVAL:
contact_data.contact_point_center = contact_pos_0
contact_data.contact_distance = contact_dist_0
contact_0_valid = contact_passes_gap_check(contact_data)
contact_1_valid = False
if contact_dist_1 < MAXVAL:
contact_data.contact_point_center = contact_pos_1
contact_data.contact_distance = contact_dist_1
contact_1_valid = contact_passes_gap_check(contact_data)
contact_2_valid = False
if contact_dist_2 < MAXVAL:
contact_data.contact_point_center = contact_pos_2
contact_data.contact_distance = contact_dist_2
contact_2_valid = contact_passes_gap_check(contact_data)
contact_3_valid = False
if contact_dist_3 < MAXVAL:
contact_data.contact_point_center = contact_pos_3
contact_data.contact_distance = contact_dist_3
contact_3_valid = contact_passes_gap_check(contact_data)
# Count valid contacts and allocate consecutive indices
num_valid = int(contact_0_valid) + int(contact_1_valid) + int(contact_2_valid) + int(contact_3_valid)
if num_valid > 0:
base_index = wp.atomic_add(writer_data.contact_count, 0, num_valid)
# Do not invoke the writer callback for overflowing batches.
# This keeps user-provided writers safe while still preserving
# overflow visibility via contact_count > contact_max.
if base_index + num_valid > writer_data.contact_max:
continue
# Write first contact if valid
if contact_0_valid:
contact_data.contact_point_center = contact_pos_0
contact_data.contact_distance = contact_dist_0
writer_func(contact_data, writer_data, base_index)
base_index += 1
# Write second contact if valid
if contact_1_valid:
contact_data.contact_point_center = contact_pos_1
contact_data.contact_distance = contact_dist_1
writer_func(contact_data, writer_data, base_index)
base_index += 1
# Write third contact if valid
if contact_2_valid:
contact_data.contact_point_center = contact_pos_2
contact_data.contact_distance = contact_dist_2
writer_func(contact_data, writer_data, base_index)
base_index += 1
# Write fourth contact if valid
if contact_3_valid:
contact_data.contact_point_center = contact_pos_3
contact_data.contact_distance = contact_dist_3
writer_func(contact_data, writer_data, base_index)
continue
# =====================================================================
# Route remaining pairs to GJK/MPR kernel
# =====================================================================
idx = wp.atomic_add(gjk_candidate_pairs_count, 0, 1)
if idx < gjk_candidate_pairs.shape[0]:
gjk_candidate_pairs[idx] = wp.vec2i(shape_a, shape_b)
return narrow_phase_primitive_kernel
def create_narrow_phase_kernel_gjk_mpr(
external_aabb: bool, writer_func: Any, support_func: Any = None, post_process_contact: Any = None
):
"""
Create a GJK/MPR narrow phase kernel for complex convex shape collisions.
This kernel is called AFTER the primitive kernel has already:
- Sorted pairs by type (type_a <= type_b)
- Routed mesh pairs to specialized buffers
- Routed hydroelastic pairs to SDF-SDF buffer
- Handled primitive collisions analytically
The remaining pairs are complex convex-convex (plane-box, plane-cylinder,
plane-cone, box-box, cylinder-cylinder, etc.) that need GJK/MPR.
"""
@wp.kernel(enable_backward=False, module="unique")
def narrow_phase_kernel_gjk_mpr(
candidate_pair: wp.array[wp.vec2i],
candidate_pair_count: wp.array[int],
shape_types: wp.array[int],
shape_data: wp.array[wp.vec4],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
shape_gap: wp.array[float],
shape_collision_radius: wp.array[float],
shape_aabb_lower: wp.array[wp.vec3],
shape_aabb_upper: wp.array[wp.vec3],
writer_data: Any,
total_num_threads: int,
):
"""
GJK/MPR collision detection for complex convex pairs.
Pairs arrive pre-sorted (type_a <= type_b) and pre-filtered
(no meshes, no hydroelastic, no simple primitives).
"""
tid = wp.tid()
num_work_items = wp.min(candidate_pair.shape[0], candidate_pair_count[0])
# Early exit if no work (fast path for primitive-only scenes)
if num_work_items == 0:
return
for t in range(tid, num_work_items, total_num_threads):
# Get shape pair (already sorted by primitive kernel)
pair = candidate_pair[t]
shape_a = pair[0]
shape_b = pair[1]
# Safety checks
if shape_a == shape_b or shape_a < 0 or shape_b < 0:
continue
# Get shape types (already sorted: type_a <= type_b)
type_a = shape_types[shape_a]
type_b = shape_types[shape_b]
# Extract shape data
pos_a, quat_a, shape_data_a, scale_a, margin_offset_a = extract_shape_data(
shape_a, shape_transform, shape_types, shape_data, shape_source
)
pos_b, quat_b, shape_data_b, scale_b, margin_offset_b = extract_shape_data(
shape_b, shape_transform, shape_types, shape_data, shape_source
)
# Check for infinite planes
is_infinite_plane_a = (type_a == GeoType.PLANE) and (scale_a[0] == 0.0 and scale_a[1] == 0.0)
is_infinite_plane_b = (type_b == GeoType.PLANE) and (scale_b[0] == 0.0 and scale_b[1] == 0.0)
# Early exit: both infinite planes can't collide
if is_infinite_plane_a and is_infinite_plane_b:
continue
# Bounding sphere check is only needed for infinite plane pairs.
# For non-plane pairs with external AABBs, SAP already verified AABB overlap.
bsphere_radius_a = float(0.0)
bsphere_radius_b = float(0.0)
has_infinite_plane = is_infinite_plane_a or is_infinite_plane_b
if has_infinite_plane:
# Compute or fetch AABBs for bounding sphere overlap check
if wp.static(external_aabb):
aabb_a_lower = shape_aabb_lower[shape_a]
aabb_a_upper = shape_aabb_upper[shape_a]
aabb_b_lower = shape_aabb_lower[shape_b]
aabb_b_upper = shape_aabb_upper[shape_b]
if wp.static(not external_aabb):
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
gap_vec_a = wp.vec3(gap_a, gap_a, gap_a)
gap_vec_b = wp.vec3(gap_b, gap_b, gap_b)
# Shape A AABB
if is_infinite_plane_a:
radius_a = shape_collision_radius[shape_a]
half_extents_a = wp.vec3(radius_a, radius_a, radius_a)
aabb_a_lower = pos_a - half_extents_a - gap_vec_a
aabb_a_upper = pos_a + half_extents_a + gap_vec_a
else:
data_provider = SupportMapDataProvider()
aabb_a_lower, aabb_a_upper = compute_tight_aabb_from_support(
shape_data_a, quat_a, pos_a, data_provider
)
aabb_a_lower = aabb_a_lower - gap_vec_a
aabb_a_upper = aabb_a_upper + gap_vec_a
# Shape B AABB
if is_infinite_plane_b:
radius_b = shape_collision_radius[shape_b]
half_extents_b = wp.vec3(radius_b, radius_b, radius_b)
aabb_b_lower = pos_b - half_extents_b - gap_vec_b
aabb_b_upper = pos_b + half_extents_b + gap_vec_b
else:
data_provider = SupportMapDataProvider()
aabb_b_lower, aabb_b_upper = compute_tight_aabb_from_support(
shape_data_b, quat_b, pos_b, data_provider
)
aabb_b_lower = aabb_b_lower - gap_vec_b
aabb_b_upper = aabb_b_upper + gap_vec_b
# Compute bounding spheres and check for overlap (early rejection)
bsphere_center_a, bsphere_radius_a = compute_bounding_sphere_from_aabb(aabb_a_lower, aabb_a_upper)
bsphere_center_b, bsphere_radius_b = compute_bounding_sphere_from_aabb(aabb_b_lower, aabb_b_upper)
if not check_infinite_plane_bsphere_overlap(
shape_data_a,
shape_data_b,
pos_a,
pos_b,
quat_a,
quat_b,
bsphere_center_a,
bsphere_center_b,
bsphere_radius_a,
bsphere_radius_b,
):
continue
# Compute pairwise gap sum for contact detection
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
gap_sum = gap_a + gap_b
# Find and write contacts using GJK/MPR
wp.static(
create_find_contacts(writer_func, support_func=support_func, post_process_contact=post_process_contact)
)(
pos_a,
pos_b,
quat_a,
quat_b,
shape_data_a,
shape_data_b,
is_infinite_plane_a,
is_infinite_plane_b,
bsphere_radius_a,
bsphere_radius_b,
gap_sum,
shape_a,
shape_b,
margin_offset_a,
margin_offset_b,
writer_data,
)
return narrow_phase_kernel_gjk_mpr
@wp.kernel(enable_backward=False)
def narrow_phase_find_mesh_triangle_overlaps_kernel(
shape_types: wp.array[int],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
shape_gap: wp.array[float], # Per-shape contact gaps
shape_data: wp.array[wp.vec4], # Shape data (scale xyz, margin w)
shape_collision_radius: wp.array[float],
shape_heightfield_index: wp.array[wp.int32],
heightfield_data: wp.array[HeightfieldData],
shape_pairs_mesh: wp.array[wp.vec2i],
shape_pairs_mesh_count: wp.array[int],
total_num_threads: int,
# outputs
triangle_pairs: wp.array[wp.vec3i], # (shape_a, shape_b, triangle_idx)
triangle_pairs_count: wp.array[int],
):
"""Find triangles that overlap with a convex shape for mesh and heightfield pairs.
For mesh pairs, uses a tiled BVH query. For heightfield pairs, projects the
convex shape's bounding sphere onto the heightfield grid and emits triangle
pairs for each overlapping cell.
Outputs triples of ``(mesh_or_hfield_shape, other_shape, triangle_idx)``.
"""
tid, j = wp.tid()
num_mesh_pairs = shape_pairs_mesh_count[0]
# Strided loop over mesh pairs
for i in range(tid, num_mesh_pairs, total_num_threads):
pair = shape_pairs_mesh[i]
shape_a = pair[0]
shape_b = pair[1]
type_a = shape_types[shape_a]
type_b = shape_types[shape_b]
# -----------------------------------------------------------------
# Heightfield-vs-convex midphase (grid cell lookup)
# Pairs are normalized so the heightfield is always shape_a.
# -----------------------------------------------------------------
if type_a == GeoType.HFIELD:
# Only run on j==0; the j dimension is for tiled BVH queries (mesh only).
if j != 0:
continue
hfd = heightfield_data[shape_heightfield_index[shape_a]]
heightfield_vs_convex_midphase(
shape_a,
shape_b,
hfd,
shape_transform,
shape_collision_radius,
shape_gap,
triangle_pairs,
triangle_pairs_count,
)
continue
# -----------------------------------------------------------------
# Mesh-vs-convex midphase (BVH query)
# -----------------------------------------------------------------
mesh_shape = -1
non_mesh_shape = -1
if type_a == GeoType.MESH and type_b != GeoType.MESH:
mesh_shape = shape_a
non_mesh_shape = shape_b
elif type_b == GeoType.MESH and type_a != GeoType.MESH:
mesh_shape = shape_b
non_mesh_shape = shape_a
else:
# Mesh-mesh collision not supported in this path
continue
# Get mesh BVH ID and mesh transform
mesh_id = shape_source[mesh_shape]
if mesh_id == wp.uint64(0):
continue
# Get mesh world transform
X_mesh_ws = shape_transform[mesh_shape]
# Get non-mesh shape world transform
X_ws = shape_transform[non_mesh_shape]
# Use per-shape contact gaps for consistent pairwise thresholding.
gap_non_mesh = shape_gap[non_mesh_shape]
gap_mesh = shape_gap[mesh_shape]
gap_sum = gap_non_mesh + gap_mesh
# Call mesh_vs_convex_midphase with the shape_data and pair gap sum.
mesh_vs_convex_midphase(
j,
mesh_shape,
non_mesh_shape,
X_mesh_ws,
X_ws,
mesh_id,
shape_types,
shape_data,
shape_source,
gap_sum,
triangle_pairs,
triangle_pairs_count,
)
def create_narrow_phase_process_mesh_triangle_contacts_kernel(writer_func: Any):
@wp.kernel(enable_backward=False, module="unique")
def narrow_phase_process_mesh_triangle_contacts_kernel(
shape_types: wp.array[int],
shape_data: wp.array[wp.vec4],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
shape_gap: wp.array[float], # Per-shape contact gaps
shape_heightfield_index: wp.array[wp.int32],
heightfield_data: wp.array[HeightfieldData],
heightfield_elevations: wp.array[wp.float32],
triangle_pairs: wp.array[wp.vec3i],
triangle_pairs_count: wp.array[int],
writer_data: Any,
total_num_threads: int,
):
"""
Process triangle pairs to generate contacts using GJK/MPR.
"""
tid = wp.tid()
num_triangle_pairs = triangle_pairs_count[0]
for i in range(tid, num_triangle_pairs, total_num_threads):
if i >= triangle_pairs.shape[0]:
break
triple = triangle_pairs[i]
shape_a = triple[0]
shape_b = triple[1]
tri_idx = triple[2]
type_a = shape_types[shape_a]
if type_a == GeoType.HFIELD:
# Heightfield triangle
hfd = heightfield_data[shape_heightfield_index[shape_a]]
X_ws_a = shape_transform[shape_a]
shape_data_a, v0_world = get_triangle_shape_from_heightfield(
hfd, heightfield_elevations, X_ws_a, tri_idx
)
else:
# Mesh triangle
mesh_id_a = shape_source[shape_a]
scale_data_a = shape_data[shape_a]
mesh_scale_a = wp.vec3(scale_data_a[0], scale_data_a[1], scale_data_a[2])
X_ws_a = shape_transform[shape_a]
shape_data_a, v0_world = get_triangle_shape_from_mesh(mesh_id_a, mesh_scale_a, X_ws_a, tri_idx)
# Extract shape B data
pos_b, quat_b, shape_data_b, _scale_b, margin_offset_b = extract_shape_data(
shape_b,
shape_transform,
shape_types,
shape_data,
shape_source,
)
# Set pos_a to be vertex A (origin of triangle in local frame)
pos_a = v0_world
quat_a = wp.quat_identity() # Triangle has no orientation, use identity
# Extract margin offset for shape A (signed distance padding)
margin_offset_a = shape_data[shape_a][3]
# Sum per-shape contact gaps for consistent pairwise thresholding
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
gap_sum = gap_a + gap_b
# Compute and write contacts using GJK/MPR with standard post-processing
wp.static(create_compute_gjk_mpr_contacts(writer_func))(
shape_data_a,
shape_data_b,
quat_a,
quat_b,
pos_a,
pos_b,
gap_sum,
shape_a,
shape_b,
margin_offset_a,
margin_offset_b,
writer_data,
)
return narrow_phase_process_mesh_triangle_contacts_kernel
@wp.kernel(enable_backward=False)
def compute_mesh_plane_vert_counts(
shape_pairs_mesh_plane: wp.array[wp.vec2i],
shape_pairs_mesh_plane_count: wp.array[int],
shape_source: wp.array[wp.uint64],
vert_counts: wp.array[wp.int32],
):
"""Compute per-pair vertex counts in parallel for mesh-plane pairs.
Slots beyond ``pair_count`` are zeroed for correct ``array_scan`` results.
"""
i = wp.tid()
pair_count = wp.min(shape_pairs_mesh_plane_count[0], shape_pairs_mesh_plane.shape[0])
if i >= pair_count:
vert_counts[i] = 0
return
pair = shape_pairs_mesh_plane[i]
mesh_shape = pair[0]
mesh_id = shape_source[mesh_shape]
pair_verts = int(0)
if mesh_id != wp.uint64(0):
pair_verts = wp.mesh_get(mesh_id).points.shape[0]
vert_counts[i] = wp.int32(pair_verts)
def compute_mesh_plane_block_offsets_scan(
shape_pairs_mesh_plane: wp.array,
shape_pairs_mesh_plane_count: wp.array,
shape_source: wp.array,
target_blocks: int,
block_offsets: wp.array,
block_counts: wp.array,
weight_prefix_sums: wp.array,
device: str | None = None,
record_tape: bool = True,
):
"""Compute mesh-plane block offsets using parallel kernels and array_scan."""
n = block_counts.shape[0]
# Step 1: compute per-pair vertex counts in parallel
wp.launch(
kernel=compute_mesh_plane_vert_counts,
dim=n,
inputs=[
shape_pairs_mesh_plane,
shape_pairs_mesh_plane_count,
shape_source,
block_counts, # reuse as temp storage for vert counts
],
device=device,
record_tape=record_tape,
)
# Step 2: inclusive scan to get total
wp.utils.array_scan(block_counts, weight_prefix_sums, inclusive=True)
# Step 3: compute per-pair block counts using adaptive threshold
wp.launch(
kernel=compute_block_counts_from_weights,
dim=n,
inputs=[
weight_prefix_sums,
block_counts, # still holds vert counts
shape_pairs_mesh_plane_count,
shape_pairs_mesh_plane.shape[0],
target_blocks,
block_offsets, # reuse as temp for block counts
],
device=device,
record_tape=record_tape,
)
# Step 4: exclusive scan of block counts → block_offsets
wp.utils.array_scan(block_offsets, block_offsets, inclusive=False)
def create_narrow_phase_process_mesh_plane_contacts_kernel(
writer_func: Any,
reduce_contacts: bool = False,
):
"""
Create a mesh-plane collision kernel.
Args:
writer_func: Contact writer function (e.g., write_contact_simple)
reduce_contacts: If True, return multi-block load-balanced variant for global reduction.
Returns:
A warp kernel that processes mesh-plane collisions
"""
@wp.kernel(enable_backward=False, module="unique")
def narrow_phase_process_mesh_plane_contacts_kernel(
shape_data: wp.array[wp.vec4],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
shape_gap: wp.array[float],
_shape_collision_aabb_lower: wp.array[wp.vec3], # Unused but kept for API compatibility
_shape_collision_aabb_upper: wp.array[wp.vec3], # Unused but kept for API compatibility
_shape_voxel_resolution: wp.array[wp.vec3i], # Unused but kept for API compatibility
shape_pairs_mesh_plane: wp.array[wp.vec2i],
shape_pairs_mesh_plane_count: wp.array[int],
writer_data: Any,
total_num_blocks: int,
):
"""
Process mesh-plane collisions without contact reduction.
Each thread processes vertices in a strided manner and writes contacts directly.
"""
tid = wp.tid()
pair_count = shape_pairs_mesh_plane_count[0]
# Iterate over all mesh-plane pairs
for pair_idx in range(pair_count):
pair = shape_pairs_mesh_plane[pair_idx]
mesh_shape = pair[0]
plane_shape = pair[1]
# Get mesh
mesh_id = shape_source[mesh_shape]
if mesh_id == wp.uint64(0):
continue
mesh_obj = wp.mesh_get(mesh_id)
num_vertices = mesh_obj.points.shape[0]
# Get mesh world transform
X_mesh_ws = shape_transform[mesh_shape]
# Get plane world transform
X_plane_ws = shape_transform[plane_shape]
X_plane_sw = wp.transform_inverse(X_plane_ws)
# Get plane normal in world space (plane normal is along local +Z, pointing upward)
plane_normal = wp.transform_vector(X_plane_ws, wp.vec3(0.0, 0.0, 1.0))
# Get mesh scale
scale_data = shape_data[mesh_shape]
mesh_scale = wp.vec3(scale_data[0], scale_data[1], scale_data[2])
# Extract per-shape margin offsets (stored in shape_data.w)
margin_offset_mesh = shape_data[mesh_shape][3]
margin_offset_plane = shape_data[plane_shape][3]
total_margin_offset = margin_offset_mesh + margin_offset_plane
# Use per-shape contact gap for contact detection threshold
gap_mesh = shape_gap[mesh_shape]
gap_plane = shape_gap[plane_shape]
gap_sum = gap_mesh + gap_plane
# Strided loop over vertices across all threads in the launch
total_num_threads = total_num_blocks * wp.block_dim()
for vertex_idx in range(tid, num_vertices, total_num_threads):
# Get vertex position in mesh local space and transform to world space
vertex_local = wp.cw_mul(mesh_obj.points[vertex_idx], mesh_scale)
vertex_world = wp.transform_point(X_mesh_ws, vertex_local)
# Project vertex onto plane to get closest point
vertex_in_plane_space = wp.transform_point(X_plane_sw, vertex_world)
point_on_plane_local = wp.vec3(vertex_in_plane_space[0], vertex_in_plane_space[1], 0.0)
point_on_plane = wp.transform_point(X_plane_ws, point_on_plane_local)
# Compute distance
diff = vertex_world - point_on_plane
distance = wp.dot(diff, plane_normal)
# Check if this vertex generates a contact
if distance < gap_sum + total_margin_offset:
# Contact position is the midpoint
contact_pos = (vertex_world + point_on_plane) * 0.5
# Normal points from mesh to plane (negate plane normal since plane normal points up/away from plane)
contact_normal = -plane_normal
# Create contact data - contacts are already in world space
contact_data = ContactData()
contact_data.contact_point_center = contact_pos
contact_data.contact_normal_a_to_b = contact_normal
contact_data.contact_distance = distance
contact_data.radius_eff_a = 0.0
contact_data.radius_eff_b = 0.0
contact_data.margin_a = margin_offset_mesh
contact_data.margin_b = margin_offset_plane
contact_data.shape_a = mesh_shape
contact_data.shape_b = plane_shape
contact_data.gap_sum = gap_sum
if writer_data.contact_count[0] < writer_data.contact_max:
writer_func(contact_data, writer_data, -1)
# Return early if contact reduction is disabled
if not reduce_contacts:
return narrow_phase_process_mesh_plane_contacts_kernel
@wp.kernel(enable_backward=False, module="unique")
def narrow_phase_process_mesh_plane_contacts_reduce_kernel(
shape_data: wp.array[wp.vec4],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
shape_gap: wp.array[float],
_shape_collision_aabb_lower: wp.array[wp.vec3],
_shape_collision_aabb_upper: wp.array[wp.vec3],
_shape_voxel_resolution: wp.array[wp.vec3i],
shape_pairs_mesh_plane: wp.array[wp.vec2i],
shape_pairs_mesh_plane_count: wp.array[int],
block_offsets: wp.array[wp.int32],
writer_data: Any,
total_num_blocks: int,
):
"""Process mesh-plane collisions with dynamic load balancing.
Multiple GPU blocks can be assigned to the same mesh-plane pair
based on vertex count. Contacts are written directly to the
global contact reducer buffer via ``writer_func``; reduction into
the hashtable happens in a separate pass. This avoids per-block
shared-memory reduction and unifies the contact reduction path
with the one used for mesh-mesh contacts.
"""
block_id, t = wp.tid()
pair_count = wp.min(shape_pairs_mesh_plane_count[0], shape_pairs_mesh_plane.shape[0])
total_combos = block_offsets[pair_count]
# Grid stride loop over (pair, sub-block) combos for multi-block load balancing.
for combo_idx in range(block_id, total_combos, total_num_blocks):
# Binary search block_offsets to find the pair for this block
lo = int(0)
hi = int(pair_count)
while lo < hi:
mid = (lo + hi) // 2
if block_offsets[mid + 1] <= combo_idx:
lo = mid + 1
else:
hi = mid
pair_idx = int(lo)
pair_block_start = block_offsets[pair_idx]
block_in_pair = combo_idx - pair_block_start
blocks_for_pair = block_offsets[pair_idx + 1] - pair_block_start
# Get the mesh-plane pair
pair = shape_pairs_mesh_plane[pair_idx]
mesh_shape = pair[0]
plane_shape = pair[1]
# Get mesh
mesh_id = shape_source[mesh_shape]
if mesh_id == wp.uint64(0):
continue
mesh_obj = wp.mesh_get(mesh_id)
num_vertices = mesh_obj.points.shape[0]
# Compute vertex range for this sub-block
chunk_size = (num_vertices + blocks_for_pair - 1) // blocks_for_pair
vert_start = block_in_pair * chunk_size
vert_end = wp.min(vert_start + chunk_size, num_vertices)
# Get mesh world transform
X_mesh_ws = shape_transform[mesh_shape]
# Get plane world transform
X_plane_ws = shape_transform[plane_shape]
X_plane_sw = wp.transform_inverse(X_plane_ws)
# Get plane normal in world space (plane normal is along local +Z)
plane_normal = wp.transform_vector(X_plane_ws, wp.vec3(0.0, 0.0, 1.0))
# Get mesh scale
scale_data = shape_data[mesh_shape]
mesh_scale = wp.vec3(scale_data[0], scale_data[1], scale_data[2])
# Extract per-shape margin offsets (stored in shape_data.w)
margin_offset_mesh = shape_data[mesh_shape][3]
margin_offset_plane = shape_data[plane_shape][3]
total_margin_offset = margin_offset_mesh + margin_offset_plane
# Use per-shape contact gap for contact detection threshold
gap_mesh = shape_gap[mesh_shape]
gap_plane = shape_gap[plane_shape]
gap_sum = gap_mesh + gap_plane
# Process this block's chunk of vertices — write contacts directly
# to the global reducer buffer (no per-block shared memory reduction).
chunk_len = vert_end - vert_start
num_iterations = (chunk_len + wp.block_dim() - 1) // wp.block_dim()
for i in range(num_iterations):
vertex_idx = vert_start + i * wp.block_dim() + t
if vertex_idx < vert_end:
# Get vertex position in mesh local space and transform to world space
vertex_local = wp.cw_mul(mesh_obj.points[vertex_idx], mesh_scale)
vertex_world = wp.transform_point(X_mesh_ws, vertex_local)
# Project vertex onto plane to get closest point
vertex_in_plane_space = wp.transform_point(X_plane_sw, vertex_world)
point_on_plane_local = wp.vec3(vertex_in_plane_space[0], vertex_in_plane_space[1], 0.0)
point_on_plane = wp.transform_point(X_plane_ws, point_on_plane_local)
# Compute distance
diff = vertex_world - point_on_plane
distance = wp.dot(diff, plane_normal)
# Check if this vertex generates a contact
if distance < gap_sum + total_margin_offset:
# Contact position is the midpoint
contact_pos = (vertex_world + point_on_plane) * 0.5
# Normal points from mesh to plane
contact_normal = -plane_normal
contact_data = ContactData()
contact_data.contact_point_center = contact_pos
contact_data.contact_normal_a_to_b = contact_normal
contact_data.contact_distance = distance
contact_data.radius_eff_a = 0.0
contact_data.radius_eff_b = 0.0
contact_data.margin_a = margin_offset_mesh
contact_data.margin_b = margin_offset_plane
contact_data.shape_a = mesh_shape
contact_data.shape_b = plane_shape
contact_data.gap_sum = gap_sum
writer_func(contact_data, writer_data, -1)
return narrow_phase_process_mesh_plane_contacts_reduce_kernel
# =============================================================================
# Verification kernel
# =============================================================================
@wp.kernel(enable_backward=False)
def verify_narrow_phase_buffers(
broad_phase_count: wp.array[int],
max_broad_phase: int,
gjk_count: wp.array[int],
max_gjk: int,
mesh_count: wp.array[int],
max_mesh: int,
triangle_count: wp.array[int],
max_triangle: int,
mesh_plane_count: wp.array[int],
max_mesh_plane: int,
mesh_mesh_count: wp.array[int],
max_mesh_mesh: int,
sdf_sdf_count: wp.array[int],
max_sdf_sdf: int,
contact_count: wp.array[int],
max_contacts: int,
):
"""Check for buffer overflows in the collision pipeline."""
if broad_phase_count[0] > max_broad_phase:
wp.printf(
"Warning: Broad phase pair buffer overflowed %d > %d.\n",
broad_phase_count[0],
max_broad_phase,
)
if gjk_count[0] > max_gjk:
wp.printf(
"Warning: GJK candidate pair buffer overflowed %d > %d.\n",
gjk_count[0],
max_gjk,
)
if mesh_count:
if mesh_count[0] > max_mesh:
wp.printf(
"Warning: Mesh-convex shape pair buffer overflowed %d > %d.\n",
mesh_count[0],
max_mesh,
)
if triangle_count:
if triangle_count[0] > max_triangle:
wp.printf(
"Warning: Triangle pair buffer overflowed %d > %d.\n",
triangle_count[0],
max_triangle,
)
if mesh_plane_count:
if mesh_plane_count[0] > max_mesh_plane:
wp.printf(
"Warning: Mesh-plane shape pair buffer overflowed %d > %d.\n",
mesh_plane_count[0],
max_mesh_plane,
)
if mesh_mesh_count:
if mesh_mesh_count[0] > max_mesh_mesh:
wp.printf(
"Warning: Mesh-mesh shape pair buffer overflowed %d > %d.\n",
mesh_mesh_count[0],
max_mesh_mesh,
)
if sdf_sdf_count:
if sdf_sdf_count[0] > max_sdf_sdf:
wp.printf(
"Warning: SDF-SDF shape pair buffer overflowed %d > %d.\n",
sdf_sdf_count[0],
max_sdf_sdf,
)
if contact_count[0] > max_contacts:
wp.printf(
"Warning: Contact buffer overflowed %d > %d.\n",
contact_count[0],
max_contacts,
)
class NarrowPhase:
"""Resolve broad-phase shape pairs into simulation contacts.
This class orchestrates the narrow-phase collision pipeline by launching the
appropriate Warp kernels for primitive, mesh, heightfield, and SDF shape
pairs. It owns the intermediate counters and pair buffers used while
processing candidate pairs, then writes final contacts through a configurable
contact writer function.
"""
def __init__(
self,
*,
max_candidate_pairs: int,
max_triangle_pairs: int = 1000000,
reduce_contacts: bool = True,
device: Devicelike | None = None,
shape_aabb_lower: wp.array[wp.vec3] | None = None,
shape_aabb_upper: wp.array[wp.vec3] | None = None,
shape_voxel_resolution: wp.array[wp.vec3i] | None = None,
contact_writer_warp_func: Any | None = None,
hydroelastic_sdf: HydroelasticSDF | None = None,
has_meshes: bool = True,
has_heightfields: bool = False,
use_lean_gjk_mpr: bool = False,
) -> None:
"""
Initialize NarrowPhase with pre-allocated buffers.
Args:
max_candidate_pairs: Maximum number of candidate pairs from broad phase
max_triangle_pairs: Maximum number of triangle pairs for mesh and
heightfield collisions (conservative estimate).
reduce_contacts: Whether to reduce contacts for mesh-mesh and mesh-plane collisions.
When True, uses shared memory contact reduction to select representative contacts.
This improves performance and stability for meshes with many vertices. Defaults to True.
device: Device to allocate buffers on
shape_aabb_lower: Optional external AABB lower bounds array (if provided, AABBs won't be computed internally)
shape_aabb_upper: Optional external AABB upper bounds array (if provided, AABBs won't be computed internally)
shape_voxel_resolution: Optional per-shape voxel resolution array used for mesh/SDF and
hydroelastic contact processing.
contact_writer_warp_func: Optional custom contact writer function (first arg: ContactData, second arg: custom struct type)
hydroelastic_sdf: Optional SDF hydroelastic instance. Set is_hydroelastic=True on shapes to enable hydroelastic collisions.
has_meshes: Whether the scene contains any mesh shapes (GeoType.MESH). When False, mesh-related
kernel launches are skipped, improving performance for scenes with only primitive shapes.
Defaults to True for safety. Set to False when constructing from a model with no meshes.
has_heightfields: Whether the scene contains any heightfield shapes (GeoType.HFIELD). When True,
heightfield collision buffers and kernels are allocated. Defaults to False.
"""
self.max_candidate_pairs = max_candidate_pairs
self.max_triangle_pairs = max_triangle_pairs
self.device = device
self.reduce_contacts = reduce_contacts
self.has_meshes = has_meshes
self.has_heightfields = has_heightfields
# Warn when running on CPU with meshes: mesh-mesh SDF contacts require CUDA
is_gpu_device = wp.get_device(device).is_cuda
if has_meshes and not is_gpu_device:
warnings.warn(
"NarrowPhase running on CPU: mesh-mesh contacts will be skipped "
"(SDF-based mesh-mesh collision requires CUDA). "
"Use a CUDA device for full mesh-mesh contact support.",
stacklevel=2,
)
# Contact reduction requires GPU and meshes
if reduce_contacts and not (is_gpu_device and has_meshes):
self.reduce_contacts = False
# Determine if we're using external AABBs
self.external_aabb = shape_aabb_lower is not None and shape_aabb_upper is not None
if self.external_aabb:
# Use provided AABB arrays
self.shape_aabb_lower = shape_aabb_lower
self.shape_aabb_upper = shape_aabb_upper
else:
# Create empty AABB arrays (won't be used)
with wp.ScopedDevice(device):
self.shape_aabb_lower = wp.zeros(0, dtype=wp.vec3, device=device)
self.shape_aabb_upper = wp.zeros(0, dtype=wp.vec3, device=device)
self.shape_voxel_resolution = shape_voxel_resolution
# Determine the writer function
if contact_writer_warp_func is None:
writer_func = write_contact_simple
else:
writer_func = contact_writer_warp_func
self.tile_size_mesh_convex = 128
self.tile_size_mesh_mesh = 256
self.tile_size_mesh_plane = 512
self.block_dim = 128
# Create the appropriate kernel variants
# Primitive kernel handles lightweight primitives and routes remaining pairs
self.primitive_kernel = create_narrow_phase_primitive_kernel(writer_func)
# GJK/MPR kernel handles remaining convex-convex pairs
if use_lean_gjk_mpr:
# Use lean support function (CONVEX_MESH, BOX, SPHERE only) and lean post-processing
# (skip axial shape rolling stabilization) to reduce GPU i-cache pressure
self.narrow_phase_kernel = create_narrow_phase_kernel_gjk_mpr(
self.external_aabb,
writer_func,
support_func=support_map_lean,
post_process_contact=post_process_minkowski_only,
)
else:
self.narrow_phase_kernel = create_narrow_phase_kernel_gjk_mpr(self.external_aabb, writer_func)
# Create triangle contacts kernel when meshes or heightfields are present
if has_meshes or has_heightfields:
self.mesh_triangle_contacts_kernel = create_narrow_phase_process_mesh_triangle_contacts_kernel(writer_func)
else:
self.mesh_triangle_contacts_kernel = None
# Create mesh-specific kernels only when has_meshes=True
if has_meshes:
# Create mesh-plane kernel.
# When reducing, use multi-block load balancing and write contacts to the
# global reducer buffer (same path as mesh-mesh and mesh-triangle).
if self.reduce_contacts:
self.mesh_plane_contacts_kernel = create_narrow_phase_process_mesh_plane_contacts_kernel(
write_contact_to_reducer,
reduce_contacts=True,
)
else:
self.mesh_plane_contacts_kernel = create_narrow_phase_process_mesh_plane_contacts_kernel(
writer_func,
)
# Only create mesh-mesh SDF kernel on CUDA (uses __shared__ memory via func_native)
if is_gpu_device:
if self.reduce_contacts:
self.mesh_mesh_contacts_kernel = create_narrow_phase_process_mesh_mesh_contacts_kernel(
write_contact_to_reducer,
enable_heightfields=has_heightfields,
reduce_contacts=True,
)
else:
self.mesh_mesh_contacts_kernel = create_narrow_phase_process_mesh_mesh_contacts_kernel(
writer_func,
enable_heightfields=has_heightfields,
)
else:
self.mesh_mesh_contacts_kernel = None
else:
self.mesh_plane_contacts_kernel = None
self.mesh_mesh_contacts_kernel = None
# Create global contact reduction kernels for mesh-triangle contacts (only if has_meshes and reduce_contacts)
if self.reduce_contacts and has_meshes:
# Global contact reducer uses hardcoded BETA_THRESHOLD (0.1mm) same as shared-memory reduction
# Slot layout: NUM_SPATIAL_DIRECTIONS spatial + 1 max-depth = VALUES_PER_KEY slots per key
self.export_reduced_contacts_kernel = create_export_reduced_contacts_kernel(writer_func)
# Global contact reducer for all mesh contact types
self.global_contact_reducer = GlobalContactReducer(max_triangle_pairs, device=device)
else:
self.export_reduced_contacts_kernel = None
self.global_contact_reducer = None
self.hydroelastic_sdf = hydroelastic_sdf
# Pre-allocate all intermediate buffers.
# Counters live in one consolidated array for efficient zeroing.
with wp.ScopedDevice(device):
has_mesh_like = has_meshes or has_heightfields
n = 0 # counter index
gjk_idx = n
n += 1
sdf_sdf_idx = n
n += 1
mesh_like_idx = n if has_mesh_like else None
n += 2 if has_mesh_like else 0 # mesh_like pairs, triangle pairs
mesh_only_idx = n if has_meshes else None
n += 3 if has_meshes else 0 # mesh_plane, mesh_plane_vtx, mesh_mesh
c = wp.zeros(n, dtype=wp.int32, device=device)
self._counter_array = c
self.gjk_candidate_pairs_count = c[gjk_idx : gjk_idx + 1]
self.shape_pairs_sdf_sdf_count = c[sdf_sdf_idx : sdf_sdf_idx + 1]
self.shape_pairs_mesh_count = c[mesh_like_idx : mesh_like_idx + 1] if has_mesh_like else None
self.triangle_pairs_count = c[mesh_like_idx + 1 : mesh_like_idx + 2] if has_mesh_like else None
self.shape_pairs_mesh_plane_count = c[mesh_only_idx : mesh_only_idx + 1] if has_meshes else None
self.mesh_plane_vertex_total_count = c[mesh_only_idx + 1 : mesh_only_idx + 2] if has_meshes else None
self.shape_pairs_mesh_mesh_count = c[mesh_only_idx + 2 : mesh_only_idx + 3] if has_meshes else None
# Pair and work buffers
self.gjk_candidate_pairs = wp.zeros(max_candidate_pairs, dtype=wp.vec2i, device=device)
self.shape_pairs_mesh = (
wp.zeros(max_candidate_pairs, dtype=wp.vec2i, device=device) if has_mesh_like else None
)
self.triangle_pairs = (
wp.zeros(max_triangle_pairs, dtype=wp.vec3i, device=device) if has_meshes or has_heightfields else None
)
self.shape_pairs_mesh_plane = (
wp.zeros(max_candidate_pairs, dtype=wp.vec2i, device=device) if has_meshes else None
)
self.shape_pairs_mesh_plane_cumsum = (
wp.zeros(max_candidate_pairs, dtype=wp.int32, device=device) if has_meshes else None
)
self.shape_pairs_mesh_mesh = (
wp.zeros(max_candidate_pairs, dtype=wp.vec2i, device=device) if has_meshes else None
)
self.empty_tangent = None
if hydroelastic_sdf is not None:
self.shape_pairs_sdf_sdf = wp.zeros(hydroelastic_sdf.max_num_shape_pairs, dtype=wp.vec2i, device=device)
else:
# Empty arrays for when hydroelastic is disabled
self.shape_pairs_sdf_sdf = None
# Fixed thread count for kernel launches
# Use a reasonable minimum for GPU occupancy (256 blocks = 32K threads)
# but scale with expected workload to avoid massive overprovisioning.
# 256 blocks provides good occupancy on most GPUs (2-4 blocks per SM).
# Query GPU properties to compute appropriate thread limits
device_obj = wp.get_device(device)
if device_obj.is_cuda:
# Use 4 blocks per SM as a reasonable upper bound for occupancy
# This balances parallelism with resource utilization
max_blocks_limit = device_obj.sm_count * 4
else:
# CPU fallback: use a conservative limit
max_blocks_limit = 256
candidate_blocks = (max_candidate_pairs + self.block_dim - 1) // self.block_dim
min_blocks = 256 # 32K threads minimum for reasonable GPU occupancy on CUDA
num_blocks = max(min_blocks, min(candidate_blocks, max_blocks_limit))
self.total_num_threads = self.block_dim * num_blocks
self.num_tile_blocks = num_blocks
# Dynamic block allocation for mesh-mesh and mesh-plane contacts
if device_obj.is_cuda and self.reduce_contacts:
target_blocks = device_obj.sm_count * 4
n = max_candidate_pairs + 1
# Mesh-mesh
self.num_mesh_mesh_blocks = target_blocks
self.mesh_mesh_target_blocks = target_blocks
self.mesh_mesh_block_offsets = wp.zeros(n, dtype=wp.int32, device=device)
self.mesh_mesh_block_counts = wp.zeros(n, dtype=wp.int32, device=device)
self.mesh_mesh_weight_prefix_sums = wp.zeros(n, dtype=wp.int32, device=device)
# Mesh-plane
self.num_mesh_plane_blocks = target_blocks
self.mesh_plane_target_blocks = target_blocks
self.mesh_plane_block_offsets = wp.zeros(n, dtype=wp.int32, device=device)
self.mesh_plane_block_counts = wp.zeros(n, dtype=wp.int32, device=device)
self.mesh_plane_weight_prefix_sums = wp.zeros(n, dtype=wp.int32, device=device)
else:
self.num_mesh_mesh_blocks = self.num_tile_blocks
self.mesh_mesh_target_blocks = self.num_tile_blocks
self.mesh_mesh_block_offsets = None
self.mesh_mesh_block_counts = None
self.mesh_mesh_weight_prefix_sums = None
self.num_mesh_plane_blocks = self.num_tile_blocks
self.mesh_plane_target_blocks = self.num_tile_blocks
self.mesh_plane_block_offsets = None
self.mesh_plane_block_counts = None
self.mesh_plane_weight_prefix_sums = None
def launch_custom_write(
self,
*,
candidate_pair: wp.array[wp.vec2i], # Maybe colliding pairs
candidate_pair_count: wp.array[wp.int32], # Size one array
shape_types: wp.array[wp.int32], # All shape types, pairs index into it
shape_data: wp.array[wp.vec4], # Shape data (scale xyz, margin w)
shape_transform: wp.array[wp.transform], # In world space
shape_source: wp.array[wp.uint64], # The index into the source array, type define by shape_types
shape_sdf_index: wp.array[wp.int32], # Per-shape index into texture_sdf_data (-1 for none)
shape_gap: wp.array[wp.float32], # per-shape contact gap (detection threshold)
shape_collision_radius: wp.array[wp.float32], # per-shape collision radius for AABB fallback
shape_flags: wp.array[wp.int32], # per-shape flags (includes ShapeFlags.HYDROELASTIC)
shape_collision_aabb_lower: wp.array[wp.vec3], # Local-space AABB lower bounds
shape_collision_aabb_upper: wp.array[wp.vec3], # Local-space AABB upper bounds
shape_voxel_resolution: wp.array[wp.vec3i], # Voxel grid resolution per shape
texture_sdf_data: wp.array[TextureSDFData] | None = None, # Compact texture SDF data table
shape_heightfield_index: wp.array[wp.int32] | None = None,
heightfield_data: wp.array[HeightfieldData] | None = None,
heightfield_elevations: wp.array[wp.float32] | None = None,
writer_data: Any,
device: Devicelike | None = None, # Device to launch on
) -> None:
"""
Launch narrow phase collision detection with a custom contact writer struct.
All internal kernel launches use ``record_tape=False`` so that calls
are safe inside a :class:`warp.Tape` context.
Args:
candidate_pair: Array of potentially colliding shape pairs from broad phase
candidate_pair_count: Single-element array containing the number of candidate pairs
shape_types: Array of geometry types for all shapes
shape_data: Array of vec4 containing scale (xyz) and margin (w) for each shape
shape_transform: Array of world-space transforms for each shape
shape_source: Array of source pointers (mesh IDs, etc.) for each shape
shape_sdf_index: Per-shape SDF table index (-1 for shapes without SDF)
texture_sdf_data: Compact array of TextureSDFData structs
shape_gap: Array of per-shape contact gaps (detection threshold) for each shape
shape_collision_radius: Array of collision radii for each shape (for AABB fallback for planes/meshes)
shape_flags: Array of shape flags for each shape (includes ShapeFlags.HYDROELASTIC)
shape_collision_aabb_lower: Local-space AABB lower bounds for each shape (for voxel binning)
shape_collision_aabb_upper: Local-space AABB upper bounds for each shape (for voxel binning)
shape_voxel_resolution: Voxel grid resolution for each shape (for voxel binning)
writer_data: Custom struct instance for contact writing (type must match the custom writer function)
device: Device to launch on
"""
if device is None:
device = self.device if self.device is not None else candidate_pair.device
# Clear all counters with a single kernel launch (consolidated counter array)
self._counter_array.zero_()
# Stage 1: Launch primitive kernel for fast analytical collisions
# This handles sphere-sphere, sphere-capsule, capsule-capsule, plane-sphere, plane-capsule
# and routes remaining pairs to gjk_candidate_pairs and mesh buffers
wp.launch(
kernel=self.primitive_kernel,
dim=self.total_num_threads,
inputs=[
candidate_pair,
candidate_pair_count,
shape_types,
shape_data,
shape_transform,
shape_source,
shape_gap,
shape_flags,
writer_data,
self.total_num_threads,
],
outputs=[
self.gjk_candidate_pairs,
self.gjk_candidate_pairs_count,
self.shape_pairs_mesh,
self.shape_pairs_mesh_count,
self.shape_pairs_mesh_plane,
self.shape_pairs_mesh_plane_cumsum,
self.shape_pairs_mesh_plane_count,
self.mesh_plane_vertex_total_count,
self.shape_pairs_mesh_mesh,
self.shape_pairs_mesh_mesh_count,
self.shape_pairs_sdf_sdf,
self.shape_pairs_sdf_sdf_count,
],
device=device,
block_dim=self.block_dim,
record_tape=False,
)
# Stage 2: Launch GJK/MPR kernel for remaining convex pairs
# These are pairs that couldn't be handled analytically (box, cylinder, cone, convex hull, etc.)
# All routing has been done by the primitive kernel, so this kernel just does GJK/MPR.
wp.launch(
kernel=self.narrow_phase_kernel,
dim=self.total_num_threads,
inputs=[
self.gjk_candidate_pairs,
self.gjk_candidate_pairs_count,
shape_types,
shape_data,
shape_transform,
shape_source,
shape_gap,
shape_collision_radius,
self.shape_aabb_lower,
self.shape_aabb_upper,
writer_data,
self.total_num_threads,
],
device=device,
block_dim=self.block_dim,
record_tape=False,
)
# Skip mesh/heightfield kernels when no meshes or heightfields are present
if self.has_meshes or self.has_heightfields:
# Launch mesh-plane contact processing kernel (meshes only)
if self.has_meshes and not self.reduce_contacts:
wp.launch(
kernel=self.mesh_plane_contacts_kernel,
dim=self.total_num_threads,
inputs=[
shape_data,
shape_transform,
shape_source,
shape_gap,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
self.shape_pairs_mesh_plane,
self.shape_pairs_mesh_plane_count,
writer_data,
self.num_tile_blocks,
],
device=device,
block_dim=self.block_dim,
record_tape=False,
)
# Launch midphase: finds overlapping triangles for both mesh and heightfield pairs
second_dim = self.tile_size_mesh_convex if ENABLE_TILE_BVH_QUERY else 1
wp.launch(
kernel=narrow_phase_find_mesh_triangle_overlaps_kernel,
dim=[self.num_tile_blocks, second_dim],
inputs=[
shape_types,
shape_transform,
shape_source,
shape_gap,
shape_data,
shape_collision_radius,
shape_heightfield_index,
heightfield_data,
self.shape_pairs_mesh,
self.shape_pairs_mesh_count,
self.num_tile_blocks,
],
outputs=[
self.triangle_pairs,
self.triangle_pairs_count,
],
device=device,
block_dim=self.tile_size_mesh_convex,
record_tape=False,
)
# Launch contact processing for triangle pairs
if self.reduce_contacts:
# Unified global reduction for all mesh contact types.
assert self.global_contact_reducer is not None
self.global_contact_reducer.clear_active()
reducer_data = self.global_contact_reducer.get_data_struct()
# Mesh-plane contacts → global reducer (meshes only)
if self.has_meshes:
compute_mesh_plane_block_offsets_scan(
shape_pairs_mesh_plane=self.shape_pairs_mesh_plane,
shape_pairs_mesh_plane_count=self.shape_pairs_mesh_plane_count,
shape_source=shape_source,
target_blocks=self.mesh_plane_target_blocks,
block_offsets=self.mesh_plane_block_offsets,
block_counts=self.mesh_plane_block_counts,
weight_prefix_sums=self.mesh_plane_weight_prefix_sums,
device=device,
record_tape=False,
)
wp.launch_tiled(
kernel=self.mesh_plane_contacts_kernel,
dim=(self.num_mesh_plane_blocks,),
inputs=[
shape_data,
shape_transform,
shape_source,
shape_gap,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
self.shape_pairs_mesh_plane,
self.shape_pairs_mesh_plane_count,
self.mesh_plane_block_offsets,
reducer_data,
self.num_mesh_plane_blocks,
],
device=device,
block_dim=self.tile_size_mesh_plane,
record_tape=False,
)
# Mesh/heightfield-triangle contacts → same global reducer
wp.launch(
kernel=mesh_triangle_contacts_to_reducer_kernel,
dim=self.total_num_threads,
inputs=[
shape_types,
shape_data,
shape_transform,
shape_source,
shape_gap,
shape_heightfield_index,
heightfield_data,
heightfield_elevations,
self.triangle_pairs,
self.triangle_pairs_count,
reducer_data,
self.total_num_threads,
],
device=device,
block_dim=self.block_dim,
record_tape=False,
)
else:
# Direct contact processing without reduction
wp.launch(
kernel=self.mesh_triangle_contacts_kernel,
dim=self.total_num_threads,
inputs=[
shape_types,
shape_data,
shape_transform,
shape_source,
shape_gap,
shape_heightfield_index,
heightfield_data,
heightfield_elevations,
self.triangle_pairs,
self.triangle_pairs_count,
writer_data,
self.total_num_threads,
],
device=device,
block_dim=self.block_dim,
record_tape=False,
)
# Register mesh-plane/mesh-triangle contacts in hashtable BEFORE mesh-mesh.
# Mesh-mesh does inline hashtable registration in its kernel.
if self.reduce_contacts:
wp.launch(
kernel=reduce_buffered_contacts_kernel,
dim=self.total_num_threads,
inputs=[
reducer_data,
shape_transform,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
self.total_num_threads,
],
device=device,
block_dim=self.block_dim,
record_tape=False,
)
# Launch mesh-mesh contact processing kernel on CUDA.
# The kernel uses texture SDF for fast sampling, with BVH fallback via shape_sdf_index,
# as well as on-the-fly heightfield evaluation via heightfield_data.
if texture_sdf_data is None:
texture_sdf_data = wp.zeros(0, dtype=TextureSDFData, device=device)
if wp.get_device(device).is_cuda and self.mesh_mesh_contacts_kernel is not None:
if self.reduce_contacts and self.mesh_mesh_block_offsets is not None:
# Mesh-mesh contacts → buffer + inline hashtable registration
compute_mesh_mesh_block_offsets_scan(
shape_pairs_mesh_mesh=self.shape_pairs_mesh_mesh,
shape_pairs_mesh_mesh_count=self.shape_pairs_mesh_mesh_count,
shape_source=shape_source,
shape_heightfield_index=shape_heightfield_index,
heightfield_data=heightfield_data,
target_blocks=self.mesh_mesh_target_blocks,
block_offsets=self.mesh_mesh_block_offsets,
block_counts=self.mesh_mesh_block_counts,
weight_prefix_sums=self.mesh_mesh_weight_prefix_sums,
device=device,
record_tape=False,
)
wp.launch_tiled(
kernel=self.mesh_mesh_contacts_kernel,
dim=(self.num_mesh_mesh_blocks,),
inputs=[
shape_data,
shape_transform,
shape_source,
texture_sdf_data,
shape_sdf_index,
shape_gap,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
self.shape_pairs_mesh_mesh,
self.shape_pairs_mesh_mesh_count,
shape_heightfield_index,
heightfield_data,
heightfield_elevations,
self.mesh_mesh_block_offsets,
reducer_data,
self.num_mesh_mesh_blocks,
],
device=device,
block_dim=self.tile_size_mesh_mesh,
record_tape=False,
)
else:
# Non-reduce fallback: direct contact write, no dynamic allocation
wp.launch_tiled(
kernel=self.mesh_mesh_contacts_kernel,
dim=(self.num_tile_blocks,),
inputs=[
shape_data,
shape_transform,
shape_source,
texture_sdf_data,
shape_sdf_index,
shape_gap,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
self.shape_pairs_mesh_mesh,
self.shape_pairs_mesh_mesh_count,
shape_heightfield_index,
heightfield_data,
heightfield_elevations,
writer_data,
self.num_tile_blocks,
],
device=device,
block_dim=self.tile_size_mesh_mesh,
record_tape=False,
)
# Export reduced contacts from hashtable
if self.reduce_contacts:
# Zero exported_flags for cross-entry deduplication
self.global_contact_reducer.exported_flags.zero_()
wp.launch(
kernel=self.export_reduced_contacts_kernel,
dim=self.total_num_threads,
inputs=[
self.global_contact_reducer.hashtable.keys,
self.global_contact_reducer.ht_values,
self.global_contact_reducer.hashtable.active_slots,
self.global_contact_reducer.position_depth,
self.global_contact_reducer.normal,
self.global_contact_reducer.shape_pairs,
self.global_contact_reducer.exported_flags,
shape_types,
shape_data,
shape_gap,
writer_data,
self.total_num_threads,
],
device=device,
block_dim=self.block_dim,
record_tape=False,
)
if self.hydroelastic_sdf is not None:
self.hydroelastic_sdf.launch(
texture_sdf_data,
shape_sdf_index,
shape_transform,
shape_gap,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
self.shape_pairs_sdf_sdf,
self.shape_pairs_sdf_sdf_count,
writer_data,
)
# Verify no collision pipeline buffers overflowed
wp.launch(
kernel=verify_narrow_phase_buffers,
dim=[1],
inputs=[
candidate_pair_count,
candidate_pair.shape[0],
self.gjk_candidate_pairs_count,
self.gjk_candidate_pairs.shape[0],
self.shape_pairs_mesh_count,
self.shape_pairs_mesh.shape[0] if self.shape_pairs_mesh is not None else 0,
self.triangle_pairs_count,
self.triangle_pairs.shape[0] if self.triangle_pairs is not None else 0,
self.shape_pairs_mesh_plane_count,
self.shape_pairs_mesh_plane.shape[0] if self.shape_pairs_mesh_plane is not None else 0,
self.shape_pairs_mesh_mesh_count,
self.shape_pairs_mesh_mesh.shape[0] if self.shape_pairs_mesh_mesh is not None else 0,
self.shape_pairs_sdf_sdf_count,
self.shape_pairs_sdf_sdf.shape[0] if self.shape_pairs_sdf_sdf is not None else 0,
writer_data.contact_count,
writer_data.contact_max,
],
device=device,
record_tape=False,
)
def launch(
self,
*,
candidate_pair: wp.array[wp.vec2i], # Maybe colliding pairs
candidate_pair_count: wp.array[wp.int32], # Size one array
shape_types: wp.array[wp.int32], # All shape types, pairs index into it
shape_data: wp.array[wp.vec4], # Shape data (scale xyz, margin w)
shape_transform: wp.array[wp.transform], # In world space
shape_source: wp.array[wp.uint64], # The index into the source array, type define by shape_types
shape_sdf_index: wp.array[wp.int32] | None = None, # Per-shape index into texture_sdf_data (-1 for none)
texture_sdf_data: wp.array[TextureSDFData] | None = None, # Compact texture SDF data table
shape_gap: wp.array[wp.float32], # per-shape contact gap (detection threshold)
shape_collision_radius: wp.array[wp.float32], # per-shape collision radius for AABB fallback
shape_flags: wp.array[wp.int32], # per-shape flags (includes ShapeFlags.HYDROELASTIC)
shape_collision_aabb_lower: wp.array[wp.vec3] | None = None, # Local-space AABB lower bounds
shape_collision_aabb_upper: wp.array[wp.vec3] | None = None, # Local-space AABB upper bounds
shape_voxel_resolution: wp.array[wp.vec3i], # Voxel grid resolution per shape
# Outputs
contact_pair: wp.array[wp.vec2i],
contact_position: wp.array[wp.vec3],
contact_normal: wp.array[
wp.vec3
], # Pointing from pairId.x to pairId.y, represents z axis of local contact frame
contact_penetration: wp.array[float], # negative if bodies overlap
contact_count: wp.array[int], # Number of active contacts after narrow
contact_tangent: wp.array[wp.vec3] | None = None, # Represents x axis of local contact frame (None to disable)
device: Devicelike | None = None, # Device to launch on
**kwargs: Any,
) -> None:
"""
Launch narrow phase collision detection on candidate pairs from broad phase.
Args:
candidate_pair: Array of potentially colliding shape pairs from broad phase
candidate_pair_count: Single-element array containing the number of candidate pairs
shape_types: Array of geometry types for all shapes
shape_data: Array of vec4 containing scale (xyz) and margin (w) for each shape
shape_transform: Array of world-space transforms for each shape
shape_source: Array of source pointers (mesh IDs, etc.) for each shape
shape_sdf_index: Per-shape SDF table index (-1 for shapes without SDF)
texture_sdf_data: Compact array of TextureSDFData structs
shape_gap: Array of per-shape contact gaps (detection threshold) for each shape
shape_collision_radius: Array of collision radii for each shape (for AABB fallback for planes/meshes)
shape_collision_aabb_lower: Local-space AABB lower bounds for each shape (for voxel binning)
shape_collision_aabb_upper: Local-space AABB upper bounds for each shape (for voxel binning)
shape_voxel_resolution: Voxel grid resolution for each shape (for voxel binning)
contact_pair: Output array for contact shape pairs
contact_position: Output array for contact positions (center point)
contact_normal: Output array for contact normals
contact_penetration: Output array for penetration depths
contact_tangent: Output array for contact tangents, or None to disable tangent computation
contact_count: Output array (single element) for contact count
device: Device to launch on
"""
if device is None:
device = self.device if self.device is not None else candidate_pair.device
# Backward compatibility for older call sites/tests that still pass
# shape_local_aabb_lower/upper.
shape_local_aabb_lower = kwargs.pop("shape_local_aabb_lower", None)
shape_local_aabb_upper = kwargs.pop("shape_local_aabb_upper", None)
if kwargs:
unknown_keys = sorted(kwargs.keys())
if len(unknown_keys) == 1:
raise TypeError(f"NarrowPhase.launch() got an unexpected keyword argument '{unknown_keys[0]}'")
unknown = ", ".join(unknown_keys)
raise TypeError(f"NarrowPhase.launch() got unexpected keyword arguments: {unknown}")
if shape_collision_aabb_lower is None:
shape_collision_aabb_lower = shape_local_aabb_lower
if shape_collision_aabb_upper is None:
shape_collision_aabb_upper = shape_local_aabb_upper
if shape_collision_aabb_lower is None or shape_collision_aabb_upper is None:
raise TypeError(
"NarrowPhase.launch() missing required AABB bounds: provide either "
"shape_collision_aabb_lower/shape_collision_aabb_upper or "
"shape_local_aabb_lower/shape_local_aabb_upper"
)
if shape_sdf_index is None:
shape_sdf_index = wp.full(shape_types.shape[0], -1, dtype=wp.int32, device=device)
contact_max = contact_pair.shape[0]
# Handle optional tangent array - use empty array if None
if contact_tangent is None:
contact_tangent = self.empty_tangent
# Clear external contact count (internal counters are cleared in launch_custom_write)
contact_count.zero_()
# Create ContactWriterData struct
writer_data = ContactWriterData()
writer_data.contact_max = contact_max
writer_data.contact_count = contact_count
writer_data.contact_pair = contact_pair
writer_data.contact_position = contact_position
writer_data.contact_normal = contact_normal
writer_data.contact_penetration = contact_penetration
writer_data.contact_tangent = contact_tangent
# Delegate to launch_custom_write
self.launch_custom_write(
candidate_pair=candidate_pair,
candidate_pair_count=candidate_pair_count,
shape_types=shape_types,
shape_data=shape_data,
shape_transform=shape_transform,
shape_source=shape_source,
shape_sdf_index=shape_sdf_index,
texture_sdf_data=texture_sdf_data,
shape_gap=shape_gap,
shape_collision_radius=shape_collision_radius,
shape_flags=shape_flags,
shape_collision_aabb_lower=shape_collision_aabb_lower,
shape_collision_aabb_upper=shape_collision_aabb_upper,
shape_voxel_resolution=shape_voxel_resolution,
writer_data=writer_data,
device=device,
)
================================================
FILE: newton/_src/geometry/raycast.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
# Some ray intersection functions are adapted from https://iquilezles.org/articles/intersectors/
import warp as wp
from .types import (
GeoType,
)
# A small constant to avoid division by zero and other numerical issues
MINVAL = 1e-15
@wp.func
def _spinlock_acquire(lock: wp.array[wp.int32]):
# Try to acquire the lock by setting it to 1 if it's 0
while wp.atomic_cas(lock, 0, 0, 1) == 1:
pass
@wp.func
def _spinlock_release(lock: wp.array[wp.int32]):
# Release the lock by setting it back to 0
wp.atomic_exch(lock, 0, 0)
@wp.func
def ray_intersect_sphere(geom_to_world: wp.transform, ray_origin: wp.vec3, ray_direction: wp.vec3, r: float):
"""Computes ray-sphere intersection.
Args:
geom_to_world: The world transform of the sphere.
ray_origin: The origin of the ray in world space.
ray_direction: The direction of the ray in world space.
r: The radius of the sphere.
Returns:
The distance along the ray to the closest intersection point, or -1.0 if there is no intersection.
"""
t_hit = -1.0
# transform ray to local frame
world_to_geom = wp.transform_inverse(geom_to_world)
ray_origin_local = wp.transform_point(world_to_geom, ray_origin)
ray_direction_local = wp.transform_vector(world_to_geom, ray_direction)
d_len_sq = wp.dot(ray_direction_local, ray_direction_local)
if d_len_sq < MINVAL:
return -1.0
inv_d_len = 1.0 / wp.sqrt(d_len_sq)
d_local_norm = ray_direction_local * inv_d_len
oc = ray_origin_local
b = wp.dot(oc, d_local_norm)
c = wp.dot(oc, oc) - r * r
delta = b * b - c
if delta >= 0.0:
sqrt_delta = wp.sqrt(delta)
t1 = -b - sqrt_delta
if t1 >= 0.0:
t_hit = t1 * inv_d_len
else:
t2 = -b + sqrt_delta
if t2 >= 0.0:
t_hit = t2 * inv_d_len
return t_hit
@wp.func
def ray_intersect_particle_sphere(ray_origin: wp.vec3, ray_direction: wp.vec3, center: wp.vec3, radius: float):
"""Compute the closest hit along a (unit-length) ray against a sphere defined directly in world space.
Args:
ray_origin: The origin of the ray in world space.
ray_direction: The direction of the ray in world space (should be normalized).
center: The center of the sphere in world space.
radius: The radius of the sphere.
Returns:
The distance along the ray to the closest intersection point, or -1.0 if there is no intersection.
"""
oc = ray_origin - center
proj = wp.dot(ray_direction, oc)
c = wp.dot(oc, oc) - radius * radius
disc = proj * proj - c
if disc < 0.0:
return -1.0
sqrt_disc = wp.sqrt(disc)
t_hit = -proj - sqrt_disc
if t_hit < 0.0:
# hit behind ray origin, try other root
t_hit = -proj + sqrt_disc
if t_hit < 0.0:
return -1.0
return t_hit
@wp.func
def ray_intersect_ellipsoid(
geom_to_world: wp.transform, ray_origin: wp.vec3, ray_direction: wp.vec3, semi_axes: wp.vec3
):
"""Computes ray-ellipsoid intersection.
The ellipsoid is defined by semi-axes (a, b, c) along the local X, Y, Z axes respectively.
Based on Inigo Quilez's ellipsoid intersection algorithm.
Args:
geom_to_world: The world transform of the ellipsoid.
ray_origin: The origin of the ray in world space.
ray_direction: The direction of the ray in world space.
semi_axes: The semi-axes (a, b, c) of the ellipsoid.
Returns:
The distance along the ray to the closest intersection point, or -1.0 if there is no intersection.
"""
# Transform ray to local frame
world_to_geom = wp.transform_inverse(geom_to_world)
ro = wp.transform_point(world_to_geom, ray_origin)
rd = wp.transform_vector(world_to_geom, ray_direction)
# Reject degenerate rays (matching sphere/capsule pattern)
d_len_sq = wp.dot(rd, rd)
if d_len_sq < MINVAL:
return -1.0
ra = semi_axes
# Ensure semi-axes are valid
if ra[0] < MINVAL or ra[1] < MINVAL or ra[2] < MINVAL:
return -1.0
# Scale by inverse semi-axes (transforms ellipsoid to unit sphere)
ocn = wp.cw_div(ro, ra)
rdn = wp.cw_div(rd, ra)
a = wp.dot(rdn, rdn)
b = wp.dot(ocn, rdn)
c = wp.dot(ocn, ocn)
h = b * b - a * (c - 1.0)
if h < 0.0:
return -1.0 # No intersection
h = wp.sqrt(h)
# Two intersection points: (-b - h) / a and (-b + h) / a
t1 = (-b - h) / a
t2 = (-b + h) / a
# Return nearest positive intersection
if t1 >= 0.0:
return t1
if t2 >= 0.0:
return t2
return -1.0
@wp.func
def ray_intersect_box(geom_to_world: wp.transform, ray_origin: wp.vec3, ray_direction: wp.vec3, size: wp.vec3):
"""Computes ray-box intersection.
Args:
geom_to_world: The world transform of the box.
ray_origin: The origin of the ray in world space.
ray_direction: The direction of the ray in world space.
size: The half-extents of the box.
Returns:
The distance along the ray to the closest intersection point, or -1.0 if there is no intersection.
"""
# transform ray to local frame
world_to_geom = wp.transform_inverse(geom_to_world)
ray_origin_local = wp.transform_point(world_to_geom, ray_origin)
ray_direction_local = wp.transform_vector(world_to_geom, ray_direction)
t_hit = -1.0
t_near = -1.0e10
t_far = 1.0e10
hit = 1
for i in range(3):
if wp.abs(ray_direction_local[i]) < MINVAL:
if ray_origin_local[i] < -size[i] or ray_origin_local[i] > size[i]:
hit = 0
else:
inv_d_i = 1.0 / ray_direction_local[i]
t1 = (-size[i] - ray_origin_local[i]) * inv_d_i
t2 = (size[i] - ray_origin_local[i]) * inv_d_i
if t1 > t2:
temp = t1
t1 = t2
t2 = temp
t_near = wp.max(t_near, t1)
t_far = wp.min(t_far, t2)
if hit == 1 and t_near <= t_far and t_far >= 0.0:
if t_near >= 0.0:
t_hit = t_near
else:
t_hit = t_far
return t_hit
@wp.func
def ray_intersect_capsule(geom_to_world: wp.transform, ray_origin: wp.vec3, ray_direction: wp.vec3, r: float, h: float):
"""Computes ray-capsule intersection.
Args:
geom_to_world: The world transform of the capsule.
ray_origin: The origin of the ray in world space.
ray_direction: The direction of the ray in world space.
r: The radius of the capsule.
h: The half-height of the capsule's cylindrical part.
Returns:
The distance along the ray to the closest intersection point, or -1.0 if there is no intersection.
"""
t_hit = -1.0
# transform ray to local frame
world_to_geom = wp.transform_inverse(geom_to_world)
ray_origin_local = wp.transform_point(world_to_geom, ray_origin)
ray_direction_local = wp.transform_vector(world_to_geom, ray_direction)
d_len_sq = wp.dot(ray_direction_local, ray_direction_local)
if d_len_sq < MINVAL:
return -1.0
inv_d_len = 1.0 / wp.sqrt(d_len_sq)
d_local_norm = ray_direction_local * inv_d_len
min_t = 1.0e10
# Intersection with cylinder body
a_cyl = d_local_norm[0] * d_local_norm[0] + d_local_norm[1] * d_local_norm[1]
if a_cyl > MINVAL:
b_cyl = 2.0 * (ray_origin_local[0] * d_local_norm[0] + ray_origin_local[1] * d_local_norm[1])
c_cyl = ray_origin_local[0] * ray_origin_local[0] + ray_origin_local[1] * ray_origin_local[1] - r * r
delta_cyl = b_cyl * b_cyl - 4.0 * a_cyl * c_cyl
if delta_cyl >= 0.0:
sqrt_delta_cyl = wp.sqrt(delta_cyl)
t1 = (-b_cyl - sqrt_delta_cyl) / (2.0 * a_cyl)
if t1 >= 0.0:
z = ray_origin_local[2] + t1 * d_local_norm[2]
if wp.abs(z) <= h:
min_t = wp.min(min_t, t1)
t2 = (-b_cyl + sqrt_delta_cyl) / (2.0 * a_cyl)
if t2 >= 0.0:
z = ray_origin_local[2] + t2 * d_local_norm[2]
if wp.abs(z) <= h:
min_t = wp.min(min_t, t2)
# Intersection with sphere caps
# Top cap
oc_top = ray_origin_local - wp.vec3(0.0, 0.0, h)
b_top = wp.dot(oc_top, d_local_norm)
c_top = wp.dot(oc_top, oc_top) - r * r
delta_top = b_top * b_top - c_top
if delta_top >= 0.0:
sqrt_delta_top = wp.sqrt(delta_top)
t1_top = -b_top - sqrt_delta_top
if t1_top >= 0.0:
if (ray_origin_local[2] + t1_top * d_local_norm[2]) >= h:
min_t = wp.min(min_t, t1_top)
t2_top = -b_top + sqrt_delta_top
if t2_top >= 0.0:
if (ray_origin_local[2] + t2_top * d_local_norm[2]) >= h:
min_t = wp.min(min_t, t2_top)
# Bottom cap
oc_bot = ray_origin_local - wp.vec3(0.0, 0.0, -h)
b_bot = wp.dot(oc_bot, d_local_norm)
c_bot = wp.dot(oc_bot, oc_bot) - r * r
delta_bot = b_bot * b_bot - c_bot
if delta_bot >= 0.0:
sqrt_delta_bot = wp.sqrt(delta_bot)
t1_bot = -b_bot - sqrt_delta_bot
if t1_bot >= 0.0:
if (ray_origin_local[2] + t1_bot * d_local_norm[2]) <= -h:
min_t = wp.min(min_t, t1_bot)
t2_bot = -b_bot + sqrt_delta_bot
if t2_bot >= 0.0:
if (ray_origin_local[2] + t2_bot * d_local_norm[2]) <= -h:
min_t = wp.min(min_t, t2_bot)
if min_t < 1.0e9:
t_hit = min_t * inv_d_len
return t_hit
@wp.func
def ray_intersect_cylinder(
geom_to_world: wp.transform, ray_origin: wp.vec3, ray_direction: wp.vec3, r: float, h: float
):
"""Computes ray-cylinder intersection.
Args:
geom_to_world: The world transform of the cylinder.
ray_origin: The origin of the ray in world space.
ray_direction: The direction of the ray in world space.
r: The radius of the cylinder.
h: The half-height of the cylinder.
Returns:
The distance along the ray to the closest intersection point, or -1.0 if there is no intersection.
"""
# transform ray to local frame
world_to_geom = wp.transform_inverse(geom_to_world)
ray_origin_local = wp.transform_point(world_to_geom, ray_origin)
ray_direction_local = wp.transform_vector(world_to_geom, ray_direction)
t_hit = -1.0
min_t = 1.0e10
# Intersection with cylinder body
a_cyl = ray_direction_local[0] * ray_direction_local[0] + ray_direction_local[1] * ray_direction_local[1]
if a_cyl > MINVAL:
b_cyl = 2.0 * (ray_origin_local[0] * ray_direction_local[0] + ray_origin_local[1] * ray_direction_local[1])
c_cyl = ray_origin_local[0] * ray_origin_local[0] + ray_origin_local[1] * ray_origin_local[1] - r * r
delta_cyl = b_cyl * b_cyl - 4.0 * a_cyl * c_cyl
if delta_cyl >= 0.0:
sqrt_delta_cyl = wp.sqrt(delta_cyl)
inv_2a = 1.0 / (2.0 * a_cyl)
t1 = (-b_cyl - sqrt_delta_cyl) * inv_2a
if t1 >= 0.0:
z = ray_origin_local[2] + t1 * ray_direction_local[2]
if wp.abs(z) <= h:
min_t = wp.min(min_t, t1)
t2 = (-b_cyl + sqrt_delta_cyl) * inv_2a
if t2 >= 0.0:
z = ray_origin_local[2] + t2 * ray_direction_local[2]
if wp.abs(z) <= h:
min_t = wp.min(min_t, t2)
# Intersection with caps
if wp.abs(ray_direction_local[2]) > MINVAL:
inv_d_z = 1.0 / ray_direction_local[2]
# Top cap
t_top = (h - ray_origin_local[2]) * inv_d_z
if t_top >= 0.0:
x = ray_origin_local[0] + t_top * ray_direction_local[0]
y = ray_origin_local[1] + t_top * ray_direction_local[1]
if x * x + y * y <= r * r:
min_t = wp.min(min_t, t_top)
# Bottom cap
t_bot = (-h - ray_origin_local[2]) * inv_d_z
if t_bot >= 0.0:
x = ray_origin_local[0] + t_bot * ray_direction_local[0]
y = ray_origin_local[1] + t_bot * ray_direction_local[1]
if x * x + y * y <= r * r:
min_t = wp.min(min_t, t_bot)
if min_t < 1.0e9:
t_hit = min_t
return t_hit
@wp.func
def ray_intersect_cone(
geom_to_world: wp.transform, ray_origin: wp.vec3, ray_direction: wp.vec3, radius: float, half_height: float
):
"""Computes ray-cone intersection.
The cone is oriented along the Z-axis with the tip at +half_height and base at -half_height.
Args:
geom_to_world: The world transform of the cone.
ray_origin: The origin of the ray in world space.
ray_direction: The direction of the ray in world space.
radius: The radius of the cone's base.
half_height: Half the height of the cone (distance from center to tip/base).
Returns:
The distance along the ray to the closest intersection point, or -1.0 if there is no intersection.
"""
world_to_geom = wp.transform_inverse(geom_to_world)
ray_origin_local = wp.transform_point(world_to_geom, ray_origin)
ray_direction_local = wp.transform_vector(world_to_geom, ray_direction)
if wp.abs(half_height) < MINVAL:
return -1.0
if radius <= 0.0:
return -1.0
# pa = tip (cone extremes), pb = base center, ra = 0 (tip radius), rb = radius (base radius)
ro = ray_origin_local
rd = ray_direction_local
# Check conventions.rst, section "Newton Collision Primitives"
pa = wp.vec3(0.0, 0.0, half_height) # tip at +half_height
pb = wp.vec3(0.0, 0.0, -half_height) # base center at -half_height
ra = 0.0 # radius at tip
rb = radius # radius at base
ba = pb - pa
oa = ro - pa
ob = ro - pb
m0 = wp.dot(ba, ba)
m1 = wp.dot(oa, ba)
m2 = wp.dot(rd, ba)
m3 = wp.dot(rd, oa)
m5 = wp.dot(oa, oa)
m9 = wp.dot(ob, ba)
# caps
if m1 < 0.0:
temp = oa * m2 - rd * m1
if wp.dot(temp, temp) < (ra * ra * m2 * m2):
if wp.abs(m2) > MINVAL:
return -m1 / m2
elif m9 > 0.0:
if wp.abs(m2) > MINVAL:
t = -m9 / m2
temp_ob = ob + rd * t
if wp.dot(temp_ob, temp_ob) < (rb * rb):
return t
# body
rr = ra - rb
hy = m0 + rr * rr
k2 = m0 * m0 - m2 * m2 * hy
k1 = m0 * m0 * m3 - m1 * m2 * hy + m0 * ra * (rr * m2 * 1.0)
k0 = m0 * m0 * m5 - m1 * m1 * hy + m0 * ra * (rr * m1 * 2.0 - m0 * ra)
h = k1 * k1 - k2 * k0
if h < 0.0:
return -1.0 # no intersection
if wp.abs(k2) < MINVAL:
return -1.0 # degenerate case
t = (-k1 - wp.sqrt(h)) / k2
y = m1 + t * m2
if y < 0.0 or y > m0:
return -1.0 # no intersection
return t
@wp.func
def ray_intersect_mesh(
geom_to_world: wp.transform, ray_origin: wp.vec3, ray_direction: wp.vec3, size: wp.vec3, mesh_id: wp.uint64
):
"""Computes ray-mesh intersection using Warp's built-in mesh query.
Args:
geom_to_world: The world transform of the mesh.
ray_origin: The origin of the ray in world space.
ray_direction: The direction of the ray in world space.
size: The 3D scale of the mesh.
mesh_id: The Warp mesh ID for raycasting.
Returns:
The parameter t relative to ray direction magnitude to the closest intersection point, or -1.0 if there is no intersection.
"""
t_hit = -1.0
if mesh_id == wp.uint64(0):
return t_hit
# Transform ray to local frame
world_to_geom = wp.transform_inverse(geom_to_world)
ray_origin_local = wp.transform_point(world_to_geom, ray_origin)
ray_direction_local = wp.transform_vector(world_to_geom, ray_direction)
# Apply scale transformation with per-component clamping to MINVAL
safe_size = wp.vec3(
size[0] if wp.abs(size[0]) > MINVAL else wp.sign(size[0]) * MINVAL,
size[1] if wp.abs(size[1]) > MINVAL else wp.sign(size[1]) * MINVAL,
size[2] if wp.abs(size[2]) > MINVAL else wp.sign(size[2]) * MINVAL,
)
scaled_origin = wp.cw_div(ray_origin_local, safe_size)
scaled_direction = wp.cw_div(ray_direction_local, safe_size)
scaled_dir_length = wp.length(scaled_direction)
if scaled_dir_length < MINVAL:
return t_hit
normalized_direction = scaled_direction / scaled_dir_length
t = float(0.0)
u = float(0.0)
v = float(0.0)
sign = float(0.0)
normal = wp.vec3()
face_index = int(0)
max_t = 1.0e6
if wp.mesh_query_ray(mesh_id, scaled_origin, normalized_direction, max_t, t, u, v, sign, normal, face_index):
if t >= 0.0:
original_dir_length = wp.length(ray_direction_local)
if original_dir_length > MINVAL:
# Convert from distance along normalized scaled direction
# to parameter t relative to original ray direction magnitude
t_hit = t / scaled_dir_length
return t_hit
@wp.func
def ray_intersect_geom(
geom_to_world: wp.transform,
size: wp.vec3,
geomtype: int,
ray_origin: wp.vec3,
ray_direction: wp.vec3,
mesh_id: wp.uint64,
):
"""
Computes the intersection of a ray with a geometry.
Args:
geom_to_world: The world-to-shape transform.
size: The size of the geometry.
geomtype: The type of the geometry.
ray_origin: The origin of the ray.
ray_direction: The direction of the ray.
mesh_id: The Warp mesh ID for mesh geometries.
Returns:
The distance along the ray to the closest intersection point, or -1.0 if there is no intersection.
"""
t_hit = -1.0
if geomtype == GeoType.SPHERE:
r = size[0]
t_hit = ray_intersect_sphere(geom_to_world, ray_origin, ray_direction, r)
elif geomtype == GeoType.BOX:
t_hit = ray_intersect_box(geom_to_world, ray_origin, ray_direction, size)
elif geomtype == GeoType.CAPSULE:
r = size[0]
h = size[1]
t_hit = ray_intersect_capsule(geom_to_world, ray_origin, ray_direction, r, h)
elif geomtype == GeoType.CYLINDER:
r = size[0]
h = size[1]
t_hit = ray_intersect_cylinder(geom_to_world, ray_origin, ray_direction, r, h)
elif geomtype == GeoType.CONE:
r = size[0]
h = size[1]
t_hit = ray_intersect_cone(geom_to_world, ray_origin, ray_direction, r, h)
elif geomtype == GeoType.ELLIPSOID:
t_hit = ray_intersect_ellipsoid(geom_to_world, ray_origin, ray_direction, size)
elif geomtype == GeoType.MESH or geomtype == GeoType.CONVEX_MESH:
t_hit = ray_intersect_mesh(geom_to_world, ray_origin, ray_direction, size, mesh_id)
return t_hit
@wp.kernel
def raycast_kernel(
# Model
body_q: wp.array[wp.transform],
shape_body: wp.array[int],
shape_transform: wp.array[wp.transform],
geom_type: wp.array[int],
geom_size: wp.array[wp.vec3],
shape_source_ptr: wp.array[wp.uint64],
# Ray
ray_origin: wp.vec3,
ray_direction: wp.vec3,
# Lock helper
lock: wp.array[wp.int32],
# Output
min_dist: wp.array[float],
min_index: wp.array[int],
min_body_index: wp.array[int],
# Optional: world offsets for multi-world picking
shape_world: wp.array[int],
world_offsets: wp.array[wp.vec3],
):
"""
Computes the intersection of a ray with all geometries in the scene.
Args:
body_q: Array of body transforms.
shape_body: Maps shape index to body index.
shape_transform: Array of local shape transforms.
geom_type: Array of geometry types for each geometry.
geom_size: Array of sizes for each geometry.
shape_source_ptr: Array of mesh IDs for mesh geometries (wp.uint64).
ray_origin: The origin of the ray.
ray_direction: The direction of the ray.
lock: Lock array used for synchronization. Expected to be initialized to 0.
min_dist: A single-element array to store the minimum intersection distance. Expected to be initialized to a large value like 1e10.
min_index: A single-element array to store the index of the closest geometry. Expected to be initialized to -1.
min_body_index: A single-element array to store the body index of the closest geometry. Expected to be initialized to -1.
shape_world: Optional array mapping shape index to world index. Can be empty to disable world offsets.
world_offsets: Optional array of world offsets. Can be empty to disable world offsets.
"""
shape_idx = wp.tid()
# compute shape transform
b = shape_body[shape_idx]
X_wb = wp.transform_identity()
if b >= 0:
X_wb = body_q[b]
X_bs = shape_transform[shape_idx]
geom_to_world = wp.mul(X_wb, X_bs)
# Apply world offset if available (for multi-world picking)
if shape_world.shape[0] > 0 and world_offsets.shape[0] > 0:
world_idx = shape_world[shape_idx]
if world_idx >= 0 and world_idx < world_offsets.shape[0]:
offset = world_offsets[world_idx]
geom_to_world = wp.transform(geom_to_world.p + offset, geom_to_world.q)
geomtype = geom_type[shape_idx]
# Get mesh ID for mesh-like geometries
if geomtype == GeoType.MESH or geomtype == GeoType.CONVEX_MESH:
mesh_id = shape_source_ptr[shape_idx]
else:
mesh_id = wp.uint64(0)
t = ray_intersect_geom(geom_to_world, geom_size[shape_idx], geomtype, ray_origin, ray_direction, mesh_id)
if t >= 0.0 and t < min_dist[0]:
_spinlock_acquire(lock)
# Still use an atomic inside the spinlock to get a volatile read
old_min = wp.atomic_min(min_dist, 0, t)
if t <= old_min:
min_index[0] = shape_idx
min_body_index[0] = b
_spinlock_release(lock)
@wp.func
def ray_for_pixel(
camera_position: wp.vec3,
camera_direction: wp.vec3,
camera_up: wp.vec3,
camera_right: wp.vec3,
fov_scale: float,
camera_aspect_ratio: float,
resolution: wp.vec2,
pixel_x: int,
pixel_y: int,
):
"""
Generate a ray for a given pixel in a perspective camera.
Args:
camera_position: Camera position in world space
camera_direction: Camera forward direction (normalized)
camera_up: Camera up direction (normalized)
camera_right: Camera right direction (normalized)
camera_fov: Vertical field of view in radians
camera_aspect_ratio: Width/height aspect ratio
camera_near_clip: Near clipping plane distance
resolution: Image resolution as (width, height)
pixel_x: Pixel x coordinate (0 to width-1)
pixel_y: Pixel y coordinate (0 to height-1)
Returns:
Tuple of (ray_origin, ray_direction) in world space. With the direction normalized.
"""
width = resolution[0]
height = resolution[1]
# Convert to normalized coordinates [-1, 1] with (0,0) at center
ndc_x = (2.0 * float(pixel_x) + 1.0) / width - 1.0
ndc_y = 1.0 - (2.0 * float(pixel_y) + 1.0) / height # Flip Y axis
# Apply field of view and aspect ratio
cam_x = ndc_x * fov_scale * camera_aspect_ratio
cam_y = ndc_y * fov_scale
cam_z = 1.0 # Forward is negative Z in camera space (camera_direction already looks at -Z)
ray_dir_camera = wp.vec3(cam_x, cam_y, cam_z)
# Transform ray direction from camera to world space
ray_direction_world = (
camera_right * ray_dir_camera[0] + camera_up * ray_dir_camera[1] + camera_direction * ray_dir_camera[2]
)
ray_direction_world = wp.normalize(ray_direction_world)
return camera_position, ray_direction_world
@wp.kernel
def sensor_raycast_kernel(
# Model
body_q: wp.array[wp.transform],
shape_body: wp.array[int],
shape_transform: wp.array[wp.transform],
geom_type: wp.array[int],
geom_size: wp.array[wp.vec3],
shape_source_ptr: wp.array[wp.uint64],
# Camera parameters
camera_position: wp.vec3,
camera_direction: wp.vec3,
camera_up: wp.vec3,
camera_right: wp.vec3,
fov_scale: float,
camera_aspect_ratio: float,
resolution: wp.vec2,
# Output (per-pixel results)
hit_distances: wp.array2d[float],
):
"""
Raycast sensor kernel that casts rays for each pixel in an image.
Each thread processes one pixel, generating a ray and finding the closest intersection.
Args:
body_q: Array of body transforms
shape_body: Maps shape index to body index
shape_transform: Array of local shape transforms
geom_type: Array of geometry types for each geometry
geom_size: Array of sizes for each geometry
shape_source_ptr: Array of mesh IDs for mesh geometries
camera_position: Camera position in world space
camera_direction: Camera forward direction (normalized)
camera_up: Camera up direction (normalized)
camera_right: Camera right direction (normalized)
fov_scale: Scale factor for field of view, computed as tan(fov_radians/2) where fov_radians is the vertical field of view angle in radians
camera_aspect_ratio: Width/height aspect ratio
resolution: Image resolution as (width, height)
hit_distances: Output array of hit distances per pixel
"""
pixel_x, pixel_y, shape_idx = wp.tid()
# Skip if out of bounds
if pixel_x >= resolution[0] or pixel_y >= resolution[1]:
return
# Generate ray for this pixel
ray_origin, ray_direction = ray_for_pixel(
camera_position,
camera_direction,
camera_up,
camera_right,
fov_scale,
camera_aspect_ratio,
resolution,
pixel_x,
pixel_y,
)
# compute shape transform
b = shape_body[shape_idx]
X_wb = wp.transform_identity()
if b >= 0:
X_wb = body_q[b]
X_bs = shape_transform[shape_idx]
geom_to_world = wp.mul(X_wb, X_bs)
geomtype = geom_type[shape_idx]
# Get mesh ID for mesh-like geometries
if geomtype == GeoType.MESH or geomtype == GeoType.CONVEX_MESH:
mesh_id = shape_source_ptr[shape_idx]
else:
mesh_id = wp.uint64(0)
t = ray_intersect_geom(geom_to_world, geom_size[shape_idx], geomtype, ray_origin, ray_direction, mesh_id)
if t >= 0.0:
wp.atomic_min(hit_distances, pixel_y, pixel_x, t)
@wp.kernel
def sensor_raycast_particles_kernel(
grid: wp.uint64,
particle_positions: wp.array[wp.vec3],
particle_radius: wp.array[float],
search_radius: float,
march_step: float,
max_steps: wp.int32,
camera_position: wp.vec3,
camera_direction: wp.vec3,
camera_up: wp.vec3,
camera_right: wp.vec3,
fov_scale: float,
camera_aspect_ratio: float,
resolution: wp.vec2,
max_distance: float,
hit_distances: wp.array2d[float],
):
"""March rays against particles stored in a hash grid and record the nearest hit if found before max_distance.
Args:
grid: The hash grid containing the particles.
particle_positions: Array of particle positions.
particle_radius: Array of particle radii.
search_radius: The radius around each sample point to search for nearby particles.
march_step: The step size for ray marching.
max_steps: Maximum number of ray-march iterations allowed for a pixel.
camera_position: Camera position in world space.
camera_direction: Camera forward direction (normalized); rays travel along this vector.
camera_up: Camera up direction (normalized).
camera_right: Camera right direction (normalized).
fov_scale: Scale factor for field of view, computed as tan(fov_radians/2) where fov_radians is the vertical field of view angle in radians.
camera_aspect_ratio: Width/height aspect ratio.
resolution: Image resolution as (width, height).
max_distance: Maximum distance to march along the ray.
hit_distances: Output array of hit distances per pixel.
"""
pixel_x, pixel_y = wp.tid()
if pixel_x >= resolution[0] or pixel_y >= resolution[1]:
return
ray_origin, ray_direction = ray_for_pixel(
camera_position,
camera_direction,
camera_up,
camera_right,
fov_scale,
camera_aspect_ratio,
resolution,
pixel_x,
pixel_y,
)
best = hit_distances[pixel_y, pixel_x]
if best < 0.0:
best = max_distance
search_radius_local = search_radius
step = march_step
s = wp.int32(0)
t = float(0.0)
while s < max_steps and t <= max_distance and t <= best:
sample_pos = ray_origin + ray_direction * t
query = wp.hash_grid_query(grid, sample_pos, search_radius_local)
candidate = int(0)
while wp.hash_grid_query_next(query, candidate):
# Intersect ray with particle sphere
radius = particle_radius[candidate]
if radius <= 0.0:
continue
center = particle_positions[candidate]
t_hit = ray_intersect_particle_sphere(ray_origin, ray_direction, center, radius)
if t_hit < 0.0:
continue
if t_hit > max_distance:
continue
if t_hit < best:
hit_distances[pixel_y, pixel_x] = t_hit
best = t_hit
s += 1
t += step
================================================
FILE: newton/_src/geometry/remesh.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Point cloud extraction and surface reconstruction for mesh repair.
This module provides GPU-accelerated utilities to extract dense point clouds with
reliable surface normals from triangle meshes. The extraction uses multi-view
orthographic raycasting from camera directions distributed on an icosphere, with
online voxel-based downsampling for memory efficiency. Optional secondary "cavity
cameras" improve coverage of deep cavities and occluded regions.
The point cloud can then be used to reconstruct a clean, watertight mesh using
Poisson surface reconstruction.
Key features:
- GPU-accelerated raycasting using Warp
- Online downsampling via sparse voxel hash grid (Morton-encoded keys)
- Random camera roll and randomized processing order to reduce sampling bias
- Optional cavity cameras for improved coverage of occluded regions
- Probability-scaled selection of cavity candidates (favors deeper hits)
- Consistent outward-facing normals
Requirements:
- Point cloud extraction (PointCloudExtractor): Only requires Warp (included with Newton)
- Surface reconstruction (SurfaceReconstructor): Requires Open3D (`pip install open3d`)
This is useful for repairing meshes with:
- Inconsistent or flipped triangle winding
- Missing or incorrect vertex normals
- Non-manifold geometry
- Holes or self-intersections
- Deep cavities that are hard to capture from external viewpoints
Example:
Remesh a problematic mesh to get a clean, watertight version::
import numpy as np
from ..geometry.remesh import PointCloudExtractor, SurfaceReconstructor
# Load your mesh (vertices: Nx3, indices: Mx3 or flattened)
vertices = np.array(...) # your mesh vertices
indices = np.array(...) # your mesh triangle indices
# Step 1: Extract point cloud with reliable normals
# edge_segments controls view count: views = 20 * n^2
# cavity_cameras adds secondary cameras for deep cavities
extractor = PointCloudExtractor(
edge_segments=4, # 320 views
resolution=1000, # 1000x1000 rays per view
cavity_cameras=100, # 100 secondary hemisphere cameras
)
points, normals = extractor.extract(vertices, indices)
print(f"Extracted {len(points)} points")
# Step 2: Reconstruct clean mesh using Poisson reconstruction
reconstructor = SurfaceReconstructor(
depth=10,
simplify_tolerance=1e-7, # fraction of mesh diagonal
)
clean_mesh = reconstructor.reconstruct(points, normals)
print(f"Reconstructed {len(clean_mesh.indices) // 3} triangles")
# Use the clean mesh
new_vertices = clean_mesh.vertices # (N, 3) float32
new_indices = clean_mesh.indices # (M,) int32, flattened
"""
import math
import warnings
import numpy as np
import warp as wp
from ..geometry.hashtable import HashTable, hashtable_find_or_insert
from ..geometry.types import Mesh
# -----------------------------------------------------------------------------
# Morton encoding for sparse voxel grid (21 bits per axis = 63 bits total)
# -----------------------------------------------------------------------------
# Offset to handle negative coordinates: shift by 2^20 so range [-2^20, 2^20) maps to [0, 2^21)
VOXEL_COORD_OFFSET = wp.constant(wp.int32(1 << 20)) # 1,048,576
VOXEL_COORD_MASK = wp.constant(wp.uint64(0x1FFFFF)) # 21 bits = 2,097,151
@wp.func
def _split_by_3(x: wp.uint64) -> wp.uint64:
"""Spread 21-bit integer into 63 bits with 2 zeros between each bit (for Morton encoding)."""
# x = ---- ---- ---- ---- ---- ---- ---x xxxx xxxx xxxx xxxx xxxx (21 bits)
x = x & wp.uint64(0x1FFFFF) # Mask to 21 bits
# Spread bits apart using magic numbers (interleave with zeros)
x = (x | (x << wp.uint64(32))) & wp.uint64(0x1F00000000FFFF)
x = (x | (x << wp.uint64(16))) & wp.uint64(0x1F0000FF0000FF)
x = (x | (x << wp.uint64(8))) & wp.uint64(0x100F00F00F00F00F)
x = (x | (x << wp.uint64(4))) & wp.uint64(0x10C30C30C30C30C3)
x = (x | (x << wp.uint64(2))) & wp.uint64(0x1249249249249249)
return x
@wp.func
def morton_encode_3d(cx: wp.int32, cy: wp.int32, cz: wp.int32) -> wp.uint64:
"""Encode 3 signed integers into a 63-bit Morton code.
Each coordinate is shifted by VOXEL_COORD_OFFSET to handle negatives,
then the 21-bit values are interleaved: z takes bits 2,5,8,..., y takes 1,4,7,..., x takes 0,3,6,...
"""
# Shift to unsigned range
ux = wp.uint64(cx + VOXEL_COORD_OFFSET) & VOXEL_COORD_MASK
uy = wp.uint64(cy + VOXEL_COORD_OFFSET) & VOXEL_COORD_MASK
uz = wp.uint64(cz + VOXEL_COORD_OFFSET) & VOXEL_COORD_MASK
# Interleave bits
return _split_by_3(ux) | (_split_by_3(uy) << wp.uint64(1)) | (_split_by_3(uz) << wp.uint64(2))
@wp.func
def compute_voxel_key(
point: wp.vec3,
inv_voxel_size: wp.float32,
) -> wp.uint64:
"""Compute Morton-encoded voxel key for a point."""
# Quantize to integer voxel coordinates
cx = wp.int32(wp.floor(point[0] * inv_voxel_size))
cy = wp.int32(wp.floor(point[1] * inv_voxel_size))
cz = wp.int32(wp.floor(point[2] * inv_voxel_size))
return morton_encode_3d(cx, cy, cz)
# -----------------------------------------------------------------------------
# Random number generation (LCG-based, suitable for GPU)
# -----------------------------------------------------------------------------
# LCG constants (same as glibc)
_LCG_A = wp.constant(wp.uint32(1103515245))
_LCG_C = wp.constant(wp.uint32(12345))
@wp.func
def rand_init(seed: wp.uint32, thread_id: wp.uint32) -> wp.uint32:
"""Initialize random state from a seed and thread ID.
Combines seed with thread_id using XOR and applies one LCG step
to ensure different threads have different starting states.
"""
state = seed ^ thread_id
# Apply one LCG step to mix the bits
return state * _LCG_A + _LCG_C
@wp.func
def rand_next(state: wp.uint32) -> wp.uint32:
"""Advance the random state and return the new state."""
return state * _LCG_A + _LCG_C
@wp.func
def rand_float(state: wp.uint32) -> float:
"""Convert random state to a float in [0, 1]."""
# Use upper 31 bits (better quality in LCG)
return wp.float32(state & wp.uint32(0x7FFFFFFF)) / wp.float32(0x7FFFFFFF)
@wp.func
def rand_next_float(state: wp.uint32) -> tuple[wp.uint32, float]:
"""Advance state and return (new_state, random_float).
Use this when you need multiple random numbers in sequence.
"""
new_state = state * _LCG_A + _LCG_C
rand_val = wp.float32(new_state & wp.uint32(0x7FFFFFFF)) / wp.float32(0x7FFFFFFF)
return new_state, rand_val
# -----------------------------------------------------------------------------
# VoxelHashGrid - sparse voxel grid with online accumulation
# -----------------------------------------------------------------------------
@wp.kernel
def _accumulate_point_kernel(
point: wp.vec3,
normal: wp.vec3,
inv_voxel_size: wp.float32,
# Hash table arrays
keys: wp.array[wp.uint64],
active_slots: wp.array[wp.int32],
# Accumulator arrays
sum_positions_x: wp.array[wp.float32],
sum_positions_y: wp.array[wp.float32],
sum_positions_z: wp.array[wp.float32],
sum_normals_x: wp.array[wp.float32],
sum_normals_y: wp.array[wp.float32],
sum_normals_z: wp.array[wp.float32],
counts: wp.array[wp.int32],
):
"""Accumulate a single point into the voxel grid (for testing)."""
key = compute_voxel_key(point, inv_voxel_size)
idx = hashtable_find_or_insert(key, keys, active_slots)
if idx >= 0:
old_count = wp.atomic_add(counts, idx, 1)
# Only store position on first hit
if old_count == 0:
sum_positions_x[idx] = point[0]
sum_positions_y[idx] = point[1]
sum_positions_z[idx] = point[2]
# Always accumulate normals
wp.atomic_add(sum_normals_x, idx, normal[0])
wp.atomic_add(sum_normals_y, idx, normal[1])
wp.atomic_add(sum_normals_z, idx, normal[2])
@wp.kernel
def _finalize_voxels_kernel(
active_slots: wp.array[wp.int32],
num_active: wp.int32,
# Accumulator arrays (input)
sum_positions_x: wp.array[wp.float32],
sum_positions_y: wp.array[wp.float32],
sum_positions_z: wp.array[wp.float32],
sum_normals_x: wp.array[wp.float32],
sum_normals_y: wp.array[wp.float32],
sum_normals_z: wp.array[wp.float32],
counts: wp.array[wp.int32],
# Output arrays
out_points: wp.array[wp.vec3],
out_normals: wp.array[wp.vec3],
):
"""Finalize voxel averages and write to output arrays."""
tid = wp.tid()
if tid >= num_active:
return
idx = active_slots[tid]
count = counts[idx]
if count <= 0:
return
# Position: use the stored position directly (first hit, no averaging)
# This avoids position drift artifacts (bumps) from averaging hits
# at slightly different depths from different ray angles
pos = wp.vec3(
sum_positions_x[idx],
sum_positions_y[idx],
sum_positions_z[idx],
)
# Normal: average and normalize (averaging normals is good for smoothness)
avg_normal = wp.vec3(
sum_normals_x[idx],
sum_normals_y[idx],
sum_normals_z[idx],
)
normal_len = wp.length(avg_normal)
if normal_len > 1e-8:
avg_normal = avg_normal / normal_len
else:
avg_normal = wp.vec3(0.0, 1.0, 0.0) # Fallback
out_points[tid] = pos
out_normals[tid] = avg_normal
class VoxelHashGrid:
"""Sparse voxel grid with online accumulation of positions and normals.
Uses a GPU hash table to map voxel coordinates (Morton-encoded) to
accumulator slots. Points and normals are accumulated using atomic
operations, allowing fully parallel insertion from multiple threads.
This is useful for voxel-based downsampling of point clouds directly
on the GPU without intermediate storage.
Args:
capacity: Maximum number of unique voxels. Rounded up to power of two.
voxel_size: Size of each cubic voxel.
device: Warp device for computation.
Example:
>>> grid = VoxelHashGrid(capacity=1_000_000, voxel_size=0.01)
>>> # Accumulate points (typically done in a kernel)
>>> # ...
>>> points, normals, count = grid.finalize()
"""
def __init__(
self,
capacity: int,
voxel_size: float,
device: str | None = None,
):
if voxel_size <= 0:
raise ValueError(f"voxel_size must be positive, got {voxel_size}")
self.voxel_size = voxel_size
self.inv_voxel_size = 1.0 / voxel_size
self.device = device
# Hash table for voxel keys
self._hashtable = HashTable(capacity, device=device)
self.capacity = self._hashtable.capacity
# Accumulator arrays (separate x/y/z for atomic_add compatibility)
self.sum_positions_x = wp.zeros(self.capacity, dtype=wp.float32, device=device)
self.sum_positions_y = wp.zeros(self.capacity, dtype=wp.float32, device=device)
self.sum_positions_z = wp.zeros(self.capacity, dtype=wp.float32, device=device)
self.sum_normals_x = wp.zeros(self.capacity, dtype=wp.float32, device=device)
self.sum_normals_y = wp.zeros(self.capacity, dtype=wp.float32, device=device)
self.sum_normals_z = wp.zeros(self.capacity, dtype=wp.float32, device=device)
self.counts = wp.zeros(self.capacity, dtype=wp.int32, device=device)
# Max confidence per voxel (for two-pass best-hit selection)
self.max_confidences = wp.zeros(self.capacity, dtype=wp.float32, device=device)
@property
def keys(self) -> wp.array:
"""Hash table keys array (for use in kernels)."""
return self._hashtable.keys
@property
def active_slots(self) -> wp.array:
"""Active slots tracking array (for use in kernels)."""
return self._hashtable.active_slots
def clear(self):
"""Clear all voxels and reset accumulators."""
self._hashtable.clear()
self.sum_positions_x.zero_()
self.sum_positions_y.zero_()
self.sum_positions_z.zero_()
self.sum_normals_x.zero_()
self.sum_normals_y.zero_()
self.sum_normals_z.zero_()
self.counts.zero_()
self.max_confidences.zero_()
def get_num_voxels(self) -> int:
"""Get the current number of occupied voxels."""
return int(self._hashtable.active_slots.numpy()[self.capacity])
def finalize(self) -> tuple[np.ndarray, np.ndarray, int]:
"""Finalize accumulation and return averaged points and normals.
Computes the average position and normalized normal for each occupied
voxel and returns the results as numpy arrays.
Returns:
Tuple of (points, normals, num_points) where:
- points: (N, 3) float32 array of averaged positions
- normals: (N, 3) float32 array of normalized normals
- num_points: number of occupied voxels
"""
num_active = self.get_num_voxels()
if num_active == 0:
return (
np.zeros((0, 3), dtype=np.float32),
np.zeros((0, 3), dtype=np.float32),
0,
)
# Allocate output buffers
out_points = wp.zeros(num_active, dtype=wp.vec3, device=self.device)
out_normals = wp.zeros(num_active, dtype=wp.vec3, device=self.device)
# Launch finalization kernel
wp.launch(
_finalize_voxels_kernel,
dim=num_active,
inputs=[
self.active_slots,
num_active,
self.sum_positions_x,
self.sum_positions_y,
self.sum_positions_z,
self.sum_normals_x,
self.sum_normals_y,
self.sum_normals_z,
self.counts,
out_points,
out_normals,
],
device=self.device,
)
wp.synchronize()
return (
out_points.numpy(),
out_normals.numpy(),
num_active,
)
def compute_bounding_sphere(vertices: np.ndarray) -> tuple[np.ndarray, float]:
"""Compute a bounding sphere for a set of vertices.
Uses Ritter's algorithm for a reasonable approximation.
Args:
vertices: (N, 3) array of vertex positions.
Returns:
Tuple of (center, radius) where center is (3,) array.
Raises:
ValueError: If vertices array is empty.
"""
if len(vertices) == 0:
raise ValueError("Cannot compute bounding sphere for empty vertex array")
# Start with axis-aligned bounding box center
min_pt = np.min(vertices, axis=0)
max_pt = np.max(vertices, axis=0)
center = (min_pt + max_pt) / 2.0
# Compute radius as max distance from center
distances = np.linalg.norm(vertices - center, axis=1)
radius = float(np.max(distances))
# Handle single-vertex case: use small positive radius
if radius == 0.0:
radius = 1e-6
return center, radius
def create_icosahedron_directions(edge_segments: int = 2) -> np.ndarray:
"""Create camera directions from subdivided icosahedron face centers.
An icosahedron has 20 faces. Each face is subdivided into n^2 smaller
triangles where n is the number of segments per edge. This gives
fine-grained control over the number of directions.
Args:
edge_segments: Number of segments per triangle edge (n >= 1).
Total faces = 20 * n^2. Examples:
- n=1: 20 faces (original icosahedron)
- n=2: 80 faces
- n=3: 180 faces
- n=4: 320 faces
- n=5: 500 faces
Returns:
(N, 3) array of unit direction vectors, one per face.
"""
if edge_segments < 1:
raise ValueError(f"edge_segments must be >= 1, got {edge_segments}")
n = edge_segments
# Golden ratio
phi = (1.0 + np.sqrt(5.0)) / 2.0
# Icosahedron vertices (normalized)
ico_verts = np.array(
[
[-1, phi, 0],
[1, phi, 0],
[-1, -phi, 0],
[1, -phi, 0],
[0, -1, phi],
[0, 1, phi],
[0, -1, -phi],
[0, 1, -phi],
[phi, 0, -1],
[phi, 0, 1],
[-phi, 0, -1],
[-phi, 0, 1],
],
dtype=np.float64,
)
ico_verts = ico_verts / np.linalg.norm(ico_verts, axis=1, keepdims=True)
# Icosahedron faces (20 triangles)
ico_faces = np.array(
[
[0, 11, 5],
[0, 5, 1],
[0, 1, 7],
[0, 7, 10],
[0, 10, 11],
[1, 5, 9],
[5, 11, 4],
[11, 10, 2],
[10, 7, 6],
[7, 1, 8],
[3, 9, 4],
[3, 4, 2],
[3, 2, 6],
[3, 6, 8],
[3, 8, 9],
[4, 9, 5],
[2, 4, 11],
[6, 2, 10],
[8, 6, 7],
[9, 8, 1],
],
dtype=np.int32,
)
# Get vertices for all 20 faces: (20, 3, 3) - 20 faces, 3 vertices each, 3 coords
v0_all = ico_verts[ico_faces[:, 0]] # (20, 3)
v1_all = ico_verts[ico_faces[:, 1]] # (20, 3)
v2_all = ico_verts[ico_faces[:, 2]] # (20, 3)
if n == 1:
# No subdivision needed - just return face centers
centers = (v0_all + v1_all + v2_all) / 3.0
centers = centers / np.linalg.norm(centers, axis=1, keepdims=True)
return centers.astype(np.float32)
# Subdivide each face into n^2 triangles using barycentric coordinates
# Total sub-faces = 20 * n^2
# Pre-compute barycentric coordinates for all sub-triangle centers
# For upward triangles: centers at (i + 1/3, j + 1/3) in barycentric grid coords
# For downward triangles: centers at (i + 2/3, j + 2/3) in barycentric grid coords
# Generate all upward triangle barycentric centers
# Upward triangles exist for i in [0, n-1], j in [0, n-i-1]
up_coords = []
for i in range(n):
for j in range(n - i):
# Center of triangle with vertices at (i,j), (i+1,j), (i,j+1)
# Barycentric center: ((i + i+1 + i)/3, (j + j + j+1)/3) = (i + 1/3, j + 1/3)
bi = (i + (i + 1) + i) / 3.0
bj = (j + j + (j + 1)) / 3.0
up_coords.append((bi, bj))
# Generate all downward triangle barycentric centers
down_coords = []
for i in range(n):
for j in range(n - i - 1):
# Center of triangle with vertices at (i+1,j), (i+1,j+1), (i,j+1)
bi = ((i + 1) + (i + 1) + i) / 3.0
bj = (j + (j + 1) + (j + 1)) / 3.0
down_coords.append((bi, bj))
# Combine all sub-triangle centers
all_bary = np.array(up_coords + down_coords, dtype=np.float64) # (n^2, 2)
# Convert barycentric (i, j) to weights (w0, w1, w2) where w0 + w1 + w2 = 1
# p = w0*v0 + w1*v1 + w2*v2 where w0 = (n - i - j)/n, w1 = j/n, w2 = i/n
# Wait, need to be careful: barycentric coords (i, j, k) where k = n - i - j
# p = (i*v0 + j*v1 + k*v2) / n
# So w0 = i/n (weight for v0), w1 = j/n (weight for v1), w2 = k/n = (n-i-j)/n (weight for v2)
bi = all_bary[:, 0] # (num_subtris,)
bj = all_bary[:, 1] # (num_subtris,)
bk = n - bi - bj
w0 = bi / n # (num_subtris,)
w1 = bj / n
w2 = bk / n
# Compute centers for all 20 faces x num_subtris sub-triangles
# Result shape: (20, num_subtris, 3)
# centers[f, s] = w0[s]*v0_all[f] + w1[s]*v1_all[f] + w2[s]*v2_all[f]
# Use broadcasting: (20, 1, 3) * (1, num_subtris, 1) -> (20, num_subtris, 3)
centers = (
v0_all[:, np.newaxis, :] * w0[np.newaxis, :, np.newaxis]
+ v1_all[:, np.newaxis, :] * w1[np.newaxis, :, np.newaxis]
+ v2_all[:, np.newaxis, :] * w2[np.newaxis, :, np.newaxis]
) # (20, num_subtris, 3)
# Normalize to unit sphere
centers = centers / np.linalg.norm(centers, axis=2, keepdims=True)
# Reshape to (20 * num_subtris, 3)
centers = centers.reshape(-1, 3)
return centers.astype(np.float32)
def compute_hemisphere_edge_segments(target_rays: int) -> int:
"""Compute the icosahedron edge segments to get approximately target_rays hemisphere directions.
The number of hemisphere directions is approximately half of the full sphere directions:
- n edge segments gives 20 * n^2 full sphere directions
- Hemisphere has ~10 * n^2 directions
We solve: 10 * n^2 >= target_rays => n >= sqrt(target_rays / 10)
Args:
target_rays: Target number of hemisphere directions.
Returns:
Edge segments value that gives at least target_rays hemisphere directions.
"""
# Direct formula: n = ceil(sqrt(target_rays / 10))
n = max(1, math.ceil(math.sqrt(target_rays / 10.0)))
return n
def create_hemisphere_directions(target_rays: int) -> np.ndarray:
"""Create hemisphere directions from a subdivided icosahedron.
Generates approximately target_rays directions distributed over a hemisphere
(local Z > 0). These can be rotated to align with any surface normal.
Args:
target_rays: Target number of hemisphere directions. The actual count
will be the smallest icosahedron subdivision that meets or exceeds this.
Returns:
(N, 3) array of unit direction vectors in the upper hemisphere (z > 0).
"""
# Find edge segments that gives enough rays
edge_segments = compute_hemisphere_edge_segments(target_rays)
# Generate full sphere directions
all_directions = create_icosahedron_directions(edge_segments)
# Filter to upper hemisphere (z > 0)
hemisphere_directions = all_directions[all_directions[:, 2] > 0]
return hemisphere_directions
def compute_camera_basis(direction: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""Compute orthonormal camera basis vectors from a view direction.
Args:
direction: Unit direction vector the camera is looking along.
Returns:
Tuple of (right, up) unit vectors forming an orthonormal basis with direction.
Raises:
ValueError: If direction vector has zero or near-zero length.
"""
norm = np.linalg.norm(direction)
if norm < 1e-10:
raise ValueError("Direction vector has zero or near-zero length")
direction = direction / norm
# Choose an arbitrary up vector that's not parallel to direction
if abs(direction[1]) < 0.9:
world_up = np.array([0.0, 1.0, 0.0], dtype=np.float32)
else:
world_up = np.array([0.0, 0.0, 1.0], dtype=np.float32)
right = np.cross(world_up, direction)
right = right / np.linalg.norm(right)
up = np.cross(direction, right)
up = up / np.linalg.norm(up)
return right, up
@wp.kernel
def raycast_orthographic_kernel(
# Mesh
mesh_id: wp.uint64,
# Camera parameters
cam_origin: wp.vec3,
cam_dir: wp.vec3,
cam_right: wp.vec3,
cam_up: wp.vec3,
pixel_size: wp.float32,
resolution: wp.int32,
max_ray_dist: wp.float32,
# Voxel hash grid parameters
inv_voxel_size: wp.float32,
# Hash table arrays
keys: wp.array[wp.uint64],
active_slots: wp.array[wp.int32],
# Accumulator arrays
sum_positions_x: wp.array[wp.float32],
sum_positions_y: wp.array[wp.float32],
sum_positions_z: wp.array[wp.float32],
sum_normals_x: wp.array[wp.float32],
sum_normals_y: wp.array[wp.float32],
sum_normals_z: wp.array[wp.float32],
counts: wp.array[wp.int32],
max_confidences: wp.array[wp.float32],
# Two-pass mode: 0 = confidence pass, 1 = position pass
pass_mode: wp.int32,
# Cavity camera candidate buffers (optional - pass empty arrays to disable)
cavity_origins: wp.array[wp.vec3],
cavity_directions: wp.array[wp.vec3],
cavity_hit_distances: wp.array[wp.float32],
cavity_count: wp.array[wp.int32], # Single-element array for atomic counter
max_cavity_candidates: wp.int32,
camera_offset: wp.float32,
cavity_prob_scale: wp.float32, # Scale factor to control acceptance rate
random_seed: wp.uint32,
):
"""Raycast kernel for orthographic projection with two-pass best-hit selection.
Uses a two-pass approach for highest quality point cloud extraction:
- Pass 0 (confidence): Find max confidence per voxel (confidence = |dot(ray, normal)|)
- Pass 1 (position): Only write position/normal if confidence matches max
This ensures we keep the most perpendicular hit per voxel, which has the most
accurate position (least depth error from ray angle).
Normals are flipped to always point toward the camera (outward from surface).
Additionally, hits with larger distances (deeper in cavities) are probabilistically
selected as cavity camera candidates.
"""
px, py = wp.tid()
if px >= resolution or py >= resolution:
return
# Compute ray origin on the image plane
# Center the grid around the camera origin
half_res = wp.float32(resolution) * 0.5
offset_x = (wp.float32(px) - half_res + 0.5) * pixel_size
offset_y = (wp.float32(py) - half_res + 0.5) * pixel_size
ray_origin = cam_origin + cam_right * offset_x + cam_up * offset_y
ray_direction = cam_dir
# Query mesh intersection
query = wp.mesh_query_ray(mesh_id, ray_origin, ray_direction, max_ray_dist)
if query.result:
# Compute hit point
hit_point = ray_origin + ray_direction * query.t
# Get surface normal - ensure it points toward camera (opposite to ray direction)
normal = query.normal
if wp.dot(normal, ray_direction) > 0.0:
normal = -normal
normal = wp.normalize(normal)
# Confidence = how perpendicular the ray is to the surface
# Higher confidence = more accurate position (less depth error)
confidence = wp.abs(wp.dot(ray_direction, normal))
# Get or create voxel slot
key = compute_voxel_key(hit_point, inv_voxel_size)
idx = hashtable_find_or_insert(key, keys, active_slots)
if idx >= 0:
if pass_mode == 0:
# Pass 0: Confidence pass - find max confidence per voxel
wp.atomic_max(max_confidences, idx, confidence)
else:
# Pass 1: Position pass - only write if we have the best confidence
# Use small epsilon for floating point comparison
max_conf = max_confidences[idx]
if confidence >= max_conf - 1.0e-6:
# We're the best (or tied) - write position and accumulate normal
sum_positions_x[idx] = hit_point[0]
sum_positions_y[idx] = hit_point[1]
sum_positions_z[idx] = hit_point[2]
# Accumulate normals (averaging is good for smoothness)
wp.atomic_add(sum_normals_x, idx, normal[0])
wp.atomic_add(sum_normals_y, idx, normal[1])
wp.atomic_add(sum_normals_z, idx, normal[2])
wp.atomic_add(counts, idx, 1)
# Probabilistically select this hit as a cavity camera candidate (only in pass 1)
# Higher hit distance = higher probability of selection
if pass_mode == 1 and max_cavity_candidates > 0:
# Generate random number
thread_id = wp.uint32(px * resolution + py)
rand_state = rand_init(random_seed, thread_id)
rand_val = rand_float(rand_state)
# Acceptance probability: scaled by hit distance to favor deeper hits
# cavity_prob_scale controls overall rate to match expected ray count
accept_prob = (query.t / max_ray_dist) * cavity_prob_scale
if rand_val < accept_prob:
# Atomically claim a slot
slot = wp.atomic_add(cavity_count, 0, 1)
if slot < max_cavity_candidates:
# Origin: offset back along ray direction (guaranteed outside mesh)
cavity_origin = hit_point - ray_direction * camera_offset
# Direction: use surface normal (points into cavity perpendicular to surface)
cavity_dir = normal
# cavity_dir = -ray_direction # Alternative: use ray direction
cavity_origins[slot] = cavity_origin
cavity_directions[slot] = cavity_dir
cavity_hit_distances[slot] = query.t
@wp.kernel
def raycast_hemisphere_kernel(
# Mesh
mesh_id: wp.uint64,
# Camera parameters
cam_origin: wp.vec3,
cam_right: wp.vec3,
cam_up: wp.vec3,
cam_forward: wp.vec3,
min_ray_dist: wp.float32,
max_ray_dist: wp.float32,
# Hemisphere directions (local frame, z > 0)
hemisphere_dirs: wp.array[wp.vec3],
num_directions: wp.int32,
# Voxel hash grid parameters
inv_voxel_size: wp.float32,
# Hash table arrays
keys: wp.array[wp.uint64],
active_slots: wp.array[wp.int32],
# Accumulator arrays
sum_positions_x: wp.array[wp.float32],
sum_positions_y: wp.array[wp.float32],
sum_positions_z: wp.array[wp.float32],
sum_normals_x: wp.array[wp.float32],
sum_normals_y: wp.array[wp.float32],
sum_normals_z: wp.array[wp.float32],
counts: wp.array[wp.int32],
max_confidences: wp.array[wp.float32],
# Two-pass mode: 0 = confidence pass, 1 = position pass
pass_mode: wp.int32,
):
"""Raycast kernel for hemisphere projection from a cavity camera (two-pass).
Uses a two-pass approach for highest quality point cloud extraction:
- Pass 0 (confidence): Find max confidence per voxel
- Pass 1 (position): Only write position/normal if confidence matches max
The camera origin and forward direction come from cavity candidates collected
during primary raycasting.
"""
tid = wp.tid()
if tid >= num_directions:
return
# Get local hemisphere direction (z > 0 in local frame)
local_dir = hemisphere_dirs[tid]
# Transform to world space: local (x, y, z) -> world (right, up, forward)
# local z (forward) maps to cam_forward (the surface normal)
# local x maps to cam_right
# local y maps to cam_up
world_dir = cam_right * local_dir[0] + cam_up * local_dir[1] + cam_forward * local_dir[2]
world_dir = wp.normalize(world_dir)
# Query mesh intersection
query = wp.mesh_query_ray(mesh_id, cam_origin, world_dir, max_ray_dist)
if query.result and query.t > min_ray_dist:
# Compute hit point
hit_point = cam_origin + world_dir * query.t
# Get surface normal - ensure it points toward camera (opposite to ray direction)
normal = query.normal
if wp.dot(normal, world_dir) > 0.0:
normal = -normal
normal = wp.normalize(normal)
# Confidence = how perpendicular the ray is to the surface
confidence = wp.abs(wp.dot(world_dir, normal))
# Get or create voxel slot
key = compute_voxel_key(hit_point, inv_voxel_size)
idx = hashtable_find_or_insert(key, keys, active_slots)
if idx >= 0:
if pass_mode == 0:
# Pass 0: Confidence pass - find max confidence per voxel
wp.atomic_max(max_confidences, idx, confidence)
else:
# Pass 1: Position pass - only write if we have the best confidence
max_conf = max_confidences[idx]
if confidence >= max_conf - 1.0e-6:
# We're the best (or tied) - write position and accumulate normal
sum_positions_x[idx] = hit_point[0]
sum_positions_y[idx] = hit_point[1]
sum_positions_z[idx] = hit_point[2]
wp.atomic_add(sum_normals_x, idx, normal[0])
wp.atomic_add(sum_normals_y, idx, normal[1])
wp.atomic_add(sum_normals_z, idx, normal[2])
wp.atomic_add(counts, idx, 1)
class PointCloudExtractor:
"""Extract dense point clouds with normals from triangle meshes.
Uses multi-view orthographic raycasting from directions distributed on
an icosphere to capture the complete surface of a mesh. Normals are
guaranteed to be consistent (always pointing outward toward the camera).
Points are accumulated directly into a sparse voxel hash grid during
raycasting, providing built-in downsampling and dramatically reducing
memory usage compared to storing all ray hits.
Optionally, secondary "cavity cameras" can shoot hemisphere rays to improve
coverage of deep cavities and occluded regions. During primary raycasting,
hits with large ray distances are probabilistically collected as cavity camera
candidates (acceptance probability scales with hit distance and is auto-tuned
to match buffer capacity). The camera origin is offset back along the ray
direction to guarantee it's outside the mesh. Primary cameras are processed
in randomized order to ensure even distribution of cavity candidates.
Args:
edge_segments: Number of segments per icosahedron edge for camera directions.
Total views = 20 * n^2. Examples:
- n=1: 20 views
- n=2: 80 views
- n=3: 180 views
- n=4: 320 views
- n=5: 500 views
Higher values provide better coverage with finer control than recursive
subdivision.
resolution: Pixel resolution of the orthographic camera (resolution x resolution).
Must be between 1 and 10000. Also determines the number of rays per cavity
camera (~resolution^2 hemisphere directions).
voxel_size: Size of voxels for point accumulation. If None (default), automatically
computed as 0.1% of the mesh bounding sphere radius. Smaller values give
denser point clouds but require more memory.
max_voxels: Maximum number of unique voxels (hash table capacity). If None (default),
automatically estimated based on voxel_size and mesh extent to keep hash table
load factor around 50%. Set explicitly if you know your requirements.
device: Warp device to use for computation.
seed: Random seed for reproducibility. Controls camera roll angles, camera
processing order, and cavity candidate selection. Set to None for
non-deterministic behavior.
cavity_cameras: Number of secondary hemisphere cameras for improved cavity
coverage. Set to 0 (default) to disable. Camera positions are collected
during primary raycasting from hits with large ray distances (deep in
cavities). Each camera shoots ~resolution^2 rays in a hemisphere pattern,
with position and direction guaranteed to be outside the mesh surface.
Note:
Memory usage is dominated by the voxel hash grid, which scales with
``max_voxels`` (~32 bytes per voxel slot), not with ``resolution^2 * num_views``.
This makes high-resolution extraction practical even on memory-constrained systems.
Example:
>>> extractor = PointCloudExtractor(edge_segments=4, resolution=1000)
>>> points, normals = extractor.extract(vertices, indices)
>>> print(f"Extracted {len(points)} points with normals")
>>> # With cavity cameras for better coverage of occluded regions
>>> extractor = PointCloudExtractor(edge_segments=4, resolution=500, cavity_cameras=100)
>>> points, normals = extractor.extract(vertices, indices)
"""
def __init__(
self,
edge_segments: int = 2,
resolution: int = 1000,
voxel_size: float | None = None,
max_voxels: int | None = None,
device: str | None = None,
seed: int | None = 42,
cavity_cameras: int = 0,
):
# Validate parameters
if edge_segments < 1:
raise ValueError(f"edge_segments must be >= 1, got {edge_segments}")
if resolution < 1 or resolution > 10000:
raise ValueError(f"resolution must be between 1 and 10000 (inclusive), got {resolution}")
if voxel_size is not None and voxel_size <= 0:
raise ValueError(f"voxel_size must be positive, got {voxel_size}")
if max_voxels is not None and max_voxels < 1:
raise ValueError(f"max_voxels must be >= 1, got {max_voxels}")
if cavity_cameras < 0:
raise ValueError(f"cavity_cameras must be >= 0, got {cavity_cameras}")
self.edge_segments = edge_segments
self.resolution = resolution
self.voxel_size = voxel_size # None means auto-compute
self.max_voxels = max_voxels # None means auto-compute
self.device = device if device is not None else wp.get_device()
self.seed = seed
self.cavity_cameras = cavity_cameras
# Pre-compute camera directions for primary pass
self.directions = create_icosahedron_directions(edge_segments)
self.num_views = len(self.directions)
# Pre-compute hemisphere directions for cavity cameras
if cavity_cameras > 0:
target_rays = resolution * resolution
self.hemisphere_directions = create_hemisphere_directions(target_rays)
self.num_hemisphere_dirs = len(self.hemisphere_directions)
else:
self.hemisphere_directions = None
self.num_hemisphere_dirs = 0
def extract(
self,
vertices: np.ndarray,
indices: np.ndarray,
padding_factor: float = 1.1,
) -> tuple[np.ndarray, np.ndarray]:
"""Extract point cloud from a triangle mesh.
Performs multi-view orthographic raycasting with online voxel-based accumulation:
1. Primary pass: Rays from icosphere-distributed cameras (processed in random
order) with random roll per camera. Hits with large ray distances are
probabilistically collected as cavity camera candidates, with acceptance
probability auto-scaled based on expected hit count and buffer capacity.
2. Secondary pass (if cavity_cameras > 0): Hemisphere rays from sampled cavity
camera candidates (weighted by hit distance to favor deeper cavities).
Camera positions are offset back along the original ray direction to
guarantee they're outside the mesh.
Points are accumulated into a sparse voxel hash grid, automatically averaging
multiple hits per voxel. This provides built-in downsampling with minimal memory.
Args:
vertices: (N, 3) array of vertex positions.
indices: (M,) or (M/3, 3) array of triangle indices.
padding_factor: Multiplier for bounding sphere radius to ensure
rays start outside the mesh.
Returns:
Tuple of (points, normals) where:
- points: (N, 3) float32 array of world-space intersection points
- normals: (N, 3) float32 array of world-space surface normals
Raises:
ValueError: If vertices or indices are empty, or indices are invalid.
"""
# Ensure correct shapes
vertices = np.asarray(vertices, dtype=np.float32)
indices = np.asarray(indices, dtype=np.int32).flatten()
# Validate inputs
if len(vertices) == 0:
raise ValueError("Vertices array cannot be empty")
if len(indices) == 0:
raise ValueError("Indices array cannot be empty")
if len(indices) % 3 != 0:
raise ValueError(f"Indices length must be a multiple of 3, got {len(indices)}")
if np.any(indices < 0) or np.any(indices >= len(vertices)):
raise ValueError(
f"Indices must be in range [0, {len(vertices)}), got range [{indices.min()}, {indices.max()}]"
)
# Compute bounding sphere in original space
center, radius = compute_bounding_sphere(vertices)
# Normalize mesh to unit sphere centered at origin
# This ensures voxel coordinates are always in a predictable range (~±1000)
# regardless of input mesh scale, preventing hash coordinate overflow
if radius > 0:
normalized_vertices = (vertices - center) / radius
else:
# Degenerate case: single point or zero-size mesh
normalized_vertices = vertices - center
# In normalized space, the mesh fits in unit sphere (radius=1)
# All parameters are now in normalized space
normalized_radius = 1.0 if radius > 0 else 1e-6
padded_radius = normalized_radius * padding_factor
# Compute pixel size to cover the bounding sphere diameter
pixel_size = (2.0 * padded_radius) / self.resolution
# Maximum ray distance (diameter of bounding sphere with padding)
max_ray_dist = 2.0 * padded_radius * 1.5
# Voxel size in normalized space
# Auto: 0.001 gives ~1000 voxels across the diameter, well within ±1M Morton range
if self.voxel_size is None:
voxel_size = 0.0005 # Fixed in normalized space
else:
# User specified voxel_size is in original space, convert to normalized
voxel_size = self.voxel_size / radius if radius > 0 else self.voxel_size
# Compute max_voxels (auto or user-specified)
if self.max_voxels is None:
# In normalized space, radius=1, so surface voxels ≈ 4π / voxel_size²
# Use 4x for hash table load factor (~25%)
# Cap at 16M to avoid excessive memory for small voxels
estimated_surface_voxels = 4.0 * np.pi / (voxel_size**2)
max_voxels = min(1 << 26, max(1 << 20, int(estimated_surface_voxels * 4)))
else:
max_voxels = self.max_voxels
# Create sparse voxel hash grid for accumulation (in normalized space)
voxel_grid = VoxelHashGrid(
capacity=max_voxels,
voxel_size=voxel_size,
device=self.device,
)
# Create Warp mesh from normalized vertices
wp_vertices = wp.array(normalized_vertices, dtype=wp.vec3, device=self.device)
wp_indices = wp.array(indices, dtype=wp.int32, device=self.device)
mesh = wp.Mesh(points=wp_vertices, indices=wp_indices)
# Create random generator for camera roll angles
rng = np.random.default_rng(self.seed)
# Pre-compute all camera bases and random rotations (vectorized)
# directions is (num_views, 3)
directions = self.directions
# Compute camera bases for all directions at once
# Choose world_up based on direction[1] magnitude
world_ups = np.where(
np.abs(directions[:, 1:2]) < 0.9,
np.array([[0.0, 1.0, 0.0]]),
np.array([[0.0, 0.0, 1.0]]),
) # (num_views, 3)
# right = cross(world_up, direction), then normalize
rights = np.cross(world_ups, directions)
rights /= np.linalg.norm(rights, axis=1, keepdims=True)
# up = cross(direction, right)
ups = np.cross(directions, rights)
# Pre-generate all random roll angles and apply rotation
thetas = rng.uniform(0, 2 * np.pi, size=self.num_views)
cos_thetas = np.cos(thetas)[:, np.newaxis] # (num_views, 1)
sin_thetas = np.sin(thetas)[:, np.newaxis]
# Rotated: right' = cos*right + sin*up, up' = cos*up - sin*right
rights_rot = cos_thetas * rights + sin_thetas * ups
ups_rot = cos_thetas * ups - sin_thetas * rights
# Camera origins in normalized space (mesh is centered at origin)
# Cameras are placed at distance padded_radius from origin along each direction
cam_origins = -directions * padded_radius # Origin is at (0,0,0) in normalized space
# Camera offset for cavity candidates (0.1% of normalized radius = 0.001)
camera_offset = 0.001
# Allocate cavity camera candidate buffers if needed
if self.cavity_cameras > 0:
# Large buffer to collect candidates - at least 100K or 100x requested cameras
max_cavity_candidates = max(100_000, self.cavity_cameras * 100)
cavity_origins = wp.zeros(max_cavity_candidates, dtype=wp.vec3, device=self.device)
cavity_directions = wp.zeros(max_cavity_candidates, dtype=wp.vec3, device=self.device)
cavity_hit_distances = wp.zeros(max_cavity_candidates, dtype=wp.float32, device=self.device)
cavity_count = wp.zeros(1, dtype=wp.int32, device=self.device)
# Calculate probability scale to target ~2x buffer size candidates
# Total rays = num_views * resolution^2, assume ~50% hit rate
total_expected_hits = self.num_views * self.resolution * self.resolution * 0.5
# Average hit distance ratio is ~0.5, so base acceptance would be 0.5
# We want: 0.5 * prob_scale * total_hits ≈ 2 * max_cavity_candidates
# prob_scale = 4 * max_cavity_candidates / total_hits
cavity_prob_scale = float(4.0 * max_cavity_candidates / max(total_expected_hits, 1.0))
# Clamp to reasonable range
cavity_prob_scale = min(1.0, max(1e-6, cavity_prob_scale))
else:
# Empty arrays when cavity cameras disabled
max_cavity_candidates = 0
cavity_origins = wp.empty(0, dtype=wp.vec3, device=self.device)
cavity_directions = wp.empty(0, dtype=wp.vec3, device=self.device)
cavity_hit_distances = wp.empty(0, dtype=wp.float32, device=self.device)
cavity_count = wp.zeros(1, dtype=wp.int32, device=self.device)
cavity_prob_scale = 0.0
# Randomize camera order to get even distribution of cavity candidates
# (prevents later cameras from always overflowing the buffer)
camera_order = rng.permutation(self.num_views)
# Two-pass approach for best-hit selection:
# Pass 0: Find max confidence (|dot(ray, normal)|) per voxel across all cameras
# Pass 1: Only write position/normal for hits that match max confidence
# Helper function to run all cameras with given pass mode
def run_primary_cameras(pass_mode: int):
for i in camera_order:
direction = directions[i]
right = rights_rot[i]
up = ups_rot[i]
cam_origin = cam_origins[i]
# Different random seed per view for cavity candidate selection
random_seed = rng.integers(0, 2**31, dtype=np.uint32)
wp.launch(
kernel=raycast_orthographic_kernel,
dim=(self.resolution, self.resolution),
inputs=[
mesh.id,
wp.vec3(cam_origin[0], cam_origin[1], cam_origin[2]),
wp.vec3(direction[0], direction[1], direction[2]),
wp.vec3(right[0], right[1], right[2]),
wp.vec3(up[0], up[1], up[2]),
float(pixel_size),
self.resolution,
float(max_ray_dist),
float(voxel_grid.inv_voxel_size),
voxel_grid.keys,
voxel_grid.active_slots,
voxel_grid.sum_positions_x,
voxel_grid.sum_positions_y,
voxel_grid.sum_positions_z,
voxel_grid.sum_normals_x,
voxel_grid.sum_normals_y,
voxel_grid.sum_normals_z,
voxel_grid.counts,
voxel_grid.max_confidences,
pass_mode,
cavity_origins,
cavity_directions,
cavity_hit_distances,
cavity_count,
max_cavity_candidates,
float(camera_offset),
float(cavity_prob_scale),
int(random_seed),
],
device=self.device,
)
# Pass 0: Find max confidence per voxel
run_primary_cameras(pass_mode=0)
# Pass 1: Write positions for best-confidence hits
run_primary_cameras(pass_mode=1)
# Check hash table load factor and warn if too high
num_voxels_after_primary = voxel_grid.get_num_voxels()
load_factor = num_voxels_after_primary / voxel_grid.capacity
if load_factor > 0.7:
warnings.warn(
f"Voxel hash table is {load_factor:.0%} full ({num_voxels_after_primary}/{voxel_grid.capacity}). "
f"This may cause slowdowns. Consider increasing max_voxels or using a larger voxel_size.",
stacklevel=2,
)
# Secondary pass: cavity cameras for improved coverage of occluded regions
if self.cavity_cameras > 0:
wp.synchronize()
# Get the number of cavity candidates that were attempted to write
total_attempts = int(cavity_count.numpy()[0])
# Clamp to buffer size (some may have been dropped due to overflow)
num_candidates = min(total_attempts, max_cavity_candidates)
# Report buffer status
if total_attempts > max_cavity_candidates:
overflow_count = total_attempts - max_cavity_candidates
print(
f"Cavity candidates: {num_candidates:,} collected, "
f"{overflow_count:,} dropped (buffer overflow, not critical)"
)
elif num_candidates > 0:
print(f"Cavity candidates: {num_candidates:,} collected")
if num_candidates > 0:
# Prepare hemisphere directions on GPU
wp_hemisphere_dirs = wp.array(self.hemisphere_directions, dtype=wp.vec3, device=self.device)
# Minimum ray distance to avoid self-occlusion
min_ray_dist = camera_offset * 2.0
# Get cavity candidate data from GPU
origins_np = cavity_origins.numpy()[:num_candidates]
directions_np = cavity_directions.numpy()[:num_candidates]
hit_dists_np = cavity_hit_distances.numpy()[:num_candidates]
# Sample cavity cameras with weighted random choice
# Weight by hit distance to favor deeper cavities
weights = hit_dists_np.copy()
weights_sum = weights.sum()
if weights_sum > 0:
weights /= weights_sum
else:
weights = np.ones(num_candidates) / num_candidates
# Sample up to cavity_cameras, but no more than available candidates
num_to_sample = min(self.cavity_cameras, num_candidates)
sample_indices = rng.choice(num_candidates, size=num_to_sample, p=weights, replace=True)
# Pre-generate all random roll angles
thetas = rng.uniform(0, 2 * np.pi, size=num_to_sample)
# Pre-compute camera bases for all sampled cavity cameras
cavity_cam_data = []
for i in range(num_to_sample):
sample_idx = sample_indices[i]
cam_origin = origins_np[sample_idx]
cam_forward = directions_np[sample_idx] # Already points into mesh
# Compute camera basis (cam_forward is the forward direction)
right, up = compute_camera_basis(cam_forward)
# Apply random roll around forward direction
theta = thetas[i]
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
right_rot = cos_theta * right + sin_theta * up
up_rot = cos_theta * up - sin_theta * right
right, up = right_rot, up_rot
cavity_cam_data.append((cam_origin, right, up, cam_forward))
# Helper function to run all cavity cameras with given pass mode
def run_cavity_cameras(pass_mode: int):
for cam_origin, right, up, cam_forward in cavity_cam_data:
wp.launch(
kernel=raycast_hemisphere_kernel,
dim=self.num_hemisphere_dirs,
inputs=[
mesh.id,
wp.vec3(cam_origin[0], cam_origin[1], cam_origin[2]),
wp.vec3(right[0], right[1], right[2]),
wp.vec3(up[0], up[1], up[2]),
wp.vec3(cam_forward[0], cam_forward[1], cam_forward[2]),
float(min_ray_dist),
float(max_ray_dist),
wp_hemisphere_dirs,
self.num_hemisphere_dirs,
float(voxel_grid.inv_voxel_size),
voxel_grid.keys,
voxel_grid.active_slots,
voxel_grid.sum_positions_x,
voxel_grid.sum_positions_y,
voxel_grid.sum_positions_z,
voxel_grid.sum_normals_x,
voxel_grid.sum_normals_y,
voxel_grid.sum_normals_z,
voxel_grid.counts,
voxel_grid.max_confidences,
pass_mode,
],
device=self.device,
)
# Two-pass for cavity cameras as well
run_cavity_cameras(pass_mode=0) # Confidence pass
run_cavity_cameras(pass_mode=1) # Position pass
# Finalize voxel grid to get averaged points and normals
wp.synchronize()
final_num_voxels = voxel_grid.get_num_voxels()
final_load_factor = final_num_voxels / voxel_grid.capacity
print(
f"Voxel grid: {final_num_voxels:,} voxels, "
f"{final_load_factor:.1%} load factor "
f"({final_num_voxels:,}/{voxel_grid.capacity:,})"
)
points_np, normals_np, _num_points = voxel_grid.finalize()
# Transform points back from normalized space to original space
# normalized = (original - center) / radius
# original = normalized * radius + center
if radius > 0:
points_np = points_np * radius + center
else:
points_np = points_np + center
# Normals are unit vectors, no transformation needed
return points_np, normals_np
class SurfaceReconstructor:
"""Reconstruct triangle meshes from point clouds using Poisson reconstruction.
Uses Open3D's implementation of Screened Poisson Surface Reconstruction.
Note:
When used with PointCloudExtractor, the point cloud is already downsampled
via the built-in voxel hash grid accumulation. No additional downsampling
is needed.
Args:
depth: Octree depth for Poisson reconstruction (higher = more detail, slower).
Default is 10, which provides good detail.
scale: Scale factor for the reconstruction bounding box. Default 1.1.
linear_fit: Use linear interpolation for iso-surface extraction. Default False.
density_threshold_quantile: Quantile for removing low-density vertices
(boundary artifacts). Default 0.01 removes bottom 1%.
simplify_ratio: Target ratio to reduce triangle count (e.g., 0.1 = keep 10%).
If None, no simplification is performed. Uses quadric decimation which
preserves shape well and removes unnecessary triangles in flat areas.
target_triangles: Target number of triangles after simplification.
Overrides simplify_ratio if both are set.
simplify_tolerance: Maximum geometric error allowed during simplification,
as a fraction of the mesh bounding box diagonal (e.g., 0.0000001 = 0.00001% of diagonal).
Only coplanar/nearly-coplanar triangles within this tolerance are merged.
The mesh keeps all triangles it needs to stay within tolerance.
This is the recommended option for quality-preserving simplification.
Overrides simplify_ratio and target_triangles if set.
fast_simplification: If True (default), use pyfqmr for fast mesh simplification.
If False, use Open3D's simplify_quadric_decimation which may produce
slightly higher quality results but is significantly slower.
n_threads: Number of threads for Poisson reconstruction. Defaults to ``1``
to avoid an `Open3D bug `_.
Set to ``-1`` for automatic.
Example:
>>> extractor = PointCloudExtractor(edge_segments=4, resolution=1000)
>>> points, normals = extractor.extract(vertices, indices)
>>> reconstructor = SurfaceReconstructor(depth=10, simplify_tolerance=1e-7)
>>> mesh = reconstructor.reconstruct(points, normals)
>>> print(f"Reconstructed {len(mesh.indices) // 3} triangles")
"""
def __init__(
self,
depth: int = 10,
scale: float = 1.1,
linear_fit: bool = False,
density_threshold_quantile: float = 0.0,
simplify_ratio: float | None = None,
target_triangles: int | None = None,
simplify_tolerance: float | None = None,
fast_simplification: bool = True,
n_threads: int = 1,
):
# Validate parameters
if depth < 1:
raise ValueError(f"depth must be >= 1, got {depth}")
if scale <= 0:
raise ValueError(f"scale must be > 0, got {scale}")
if not (0.0 <= density_threshold_quantile <= 1.0):
raise ValueError(f"density_threshold_quantile must be in [0, 1], got {density_threshold_quantile}")
if simplify_ratio is not None and (simplify_ratio <= 0 or simplify_ratio > 1):
raise ValueError(f"simplify_ratio must be in (0, 1], got {simplify_ratio}")
if target_triangles is not None and target_triangles < 1:
raise ValueError(f"target_triangles must be >= 1, got {target_triangles}")
if simplify_tolerance is not None and simplify_tolerance < 0:
raise ValueError(f"simplify_tolerance must be >= 0, got {simplify_tolerance}")
self.depth = depth
self.scale = scale
self.linear_fit = linear_fit
self.density_threshold_quantile = density_threshold_quantile
self.simplify_ratio = simplify_ratio
self.target_triangles = target_triangles
self.simplify_tolerance = simplify_tolerance
self.fast_simplification = fast_simplification
self.n_threads = n_threads
def reconstruct(
self,
points: np.ndarray,
normals: np.ndarray,
verbose: bool = True,
) -> Mesh:
"""Reconstruct a triangle mesh from a point cloud.
Args:
points: (N, 3) array of point positions.
normals: (N, 3) array of surface normals (should be unit length).
verbose: Print progress information.
Returns:
Mesh containing vertices and triangle indices.
"""
import open3d as o3d # lazy import, open3d is optional
points = np.asarray(points, dtype=np.float32)
normals = np.asarray(normals, dtype=np.float32)
# Validate inputs
if len(points) == 0:
raise ValueError("Cannot reconstruct from empty point cloud")
# Create Open3D point cloud
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points.astype(np.float64))
pcd.normals = o3d.utility.Vector3dVector(normals.astype(np.float64))
# Run Poisson reconstruction
if verbose:
print(f"Running Poisson reconstruction (depth={self.depth})...")
mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(
pcd,
depth=self.depth,
scale=self.scale,
linear_fit=self.linear_fit,
n_threads=self.n_threads,
)
# Remove low-density vertices (boundary artifacts)
if self.density_threshold_quantile > 0:
densities = np.asarray(densities)
threshold = np.quantile(densities, self.density_threshold_quantile)
vertices_to_remove = densities < threshold
mesh.remove_vertices_by_mask(vertices_to_remove)
num_triangles_before = len(mesh.triangles)
if verbose:
print(f"Reconstructed mesh: {len(mesh.vertices)} vertices, {num_triangles_before} triangles")
# Simplify mesh if requested
needs_simplification = (
self.simplify_tolerance is not None or self.target_triangles is not None or self.simplify_ratio is not None
)
if needs_simplification:
if self.fast_simplification:
# Use pyfqmr (fast quadric mesh reduction)
vertices, faces = self._simplify_pyfqmr(mesh, num_triangles_before, verbose)
else:
# Use Open3D (slower but potentially higher quality)
vertices, faces = self._simplify_open3d(mesh, num_triangles_before, verbose)
else:
vertices = np.asarray(mesh.vertices, dtype=np.float32)
faces = np.asarray(mesh.triangles, dtype=np.int32)
# Convert to output format
indices = faces.flatten().astype(np.int32)
if verbose and needs_simplification:
num_triangles_after = len(faces)
if num_triangles_before > 0:
reduction = 100 * (1 - num_triangles_after / num_triangles_before)
print(
f"Simplified mesh: {len(vertices)} vertices, {num_triangles_after} triangles ({reduction:.1f}% reduction)"
)
else:
print(f"Simplified mesh: {len(vertices)} vertices, {num_triangles_after} triangles")
return Mesh(vertices=vertices, indices=indices, compute_inertia=False)
def _simplify_pyfqmr(self, mesh, num_triangles_before: int, verbose: bool) -> tuple[np.ndarray, np.ndarray]:
"""Simplify mesh using pyfqmr (fast)."""
from pyfqmr import Simplify # lazy import
vertices = np.asarray(mesh.vertices, dtype=np.float64)
faces = np.asarray(mesh.triangles, dtype=np.int32)
mesh_simplifier = Simplify()
mesh_simplifier.setMesh(vertices, faces)
if self.simplify_tolerance is not None:
# Error-based: use lossless mode with epsilon threshold
# Scale tolerance by mesh bounding box diagonal to make it scale-independent
min_coords = vertices.min(axis=0)
max_coords = vertices.max(axis=0)
diagonal = np.linalg.norm(max_coords - min_coords)
absolute_tolerance = self.simplify_tolerance * diagonal
if verbose:
print(
f"Simplifying mesh with pyfqmr (tolerance={self.simplify_tolerance} = {absolute_tolerance:.6f} absolute, diagonal={diagonal:.4f})..."
)
mesh_simplifier.simplify_mesh_lossless(epsilon=absolute_tolerance, verbose=False)
elif self.target_triangles is not None:
target = self.target_triangles
if verbose:
print(f"Simplifying mesh with pyfqmr to {target} triangles...")
mesh_simplifier.simplify_mesh(target_count=target, verbose=False)
elif self.simplify_ratio is not None:
target = int(num_triangles_before * self.simplify_ratio)
if verbose:
print(f"Simplifying mesh with pyfqmr to {self.simplify_ratio:.1%} ({target} triangles)...")
mesh_simplifier.simplify_mesh(target_count=target, verbose=False)
vertices, faces, _ = mesh_simplifier.getMesh()
return np.asarray(vertices, dtype=np.float32), faces
def _simplify_open3d(self, mesh, num_triangles_before: int, verbose: bool) -> tuple[np.ndarray, np.ndarray]:
"""Simplify mesh using Open3D (higher quality, slower)."""
if self.simplify_tolerance is not None:
# Error-based: aggressively target 1 triangle, but stop when error exceeds tolerance
bbox = mesh.get_axis_aligned_bounding_box()
diagonal = np.linalg.norm(bbox.get_max_bound() - bbox.get_min_bound())
# Open3D QEM uses squared distances, so square the tolerance
absolute_tolerance = (self.simplify_tolerance * diagonal) ** 2
if verbose:
print(
f"Simplifying mesh with Open3D (tolerance={self.simplify_tolerance} = {self.simplify_tolerance * diagonal:.6f} absolute, diagonal={diagonal:.4f})..."
)
mesh = mesh.simplify_quadric_decimation(
target_number_of_triangles=1,
maximum_error=absolute_tolerance,
)
elif self.target_triangles is not None:
target = self.target_triangles
if verbose:
print(f"Simplifying mesh with Open3D to {target} triangles...")
mesh = mesh.simplify_quadric_decimation(target_number_of_triangles=target)
elif self.simplify_ratio is not None:
target = int(num_triangles_before * self.simplify_ratio)
if verbose:
print(f"Simplifying mesh with Open3D to {self.simplify_ratio:.1%} ({target} triangles)...")
mesh = mesh.simplify_quadric_decimation(target_number_of_triangles=target)
vertices = np.asarray(mesh.vertices, dtype=np.float32)
faces = np.asarray(mesh.triangles, dtype=np.int32)
return vertices, faces
def extract_largest_island(
vertices: np.ndarray,
indices: np.ndarray,
verbose: bool = False,
) -> tuple[np.ndarray, np.ndarray]:
"""Extract the largest connected component (island) from a mesh.
Uses edge-based connectivity: triangles sharing an edge are considered connected.
Optimized using NumPy vectorization and SciPy sparse connected components.
This is useful as post-processing after Poisson reconstruction, which can
sometimes create small floating fragments in areas with sparse point coverage.
Args:
vertices: Vertex positions, shape (N, 3).
indices: Triangle indices, shape (M*3,) or (M, 3).
verbose: Print progress information. Default is False.
Returns:
Tuple of (new_vertices, new_indices) containing only the largest island.
Indices are returned as a flattened array (M*3,).
"""
from scipy import sparse
# Ensure indices are flattened
indices = np.asarray(indices).flatten()
num_triangles = len(indices) // 3
if num_triangles == 0:
return vertices, indices
# Reshape indices to (num_triangles, 3)
triangles = indices.reshape(-1, 3)
# Build edges array: each triangle contributes 3 edges
# Edge format: (min_vertex, max_vertex) for consistent ordering
# Shape: (num_triangles * 3, 2)
v0, v1, v2 = triangles[:, 0], triangles[:, 1], triangles[:, 2]
edges = np.stack(
[
np.stack([np.minimum(v0, v1), np.maximum(v0, v1)], axis=1),
np.stack([np.minimum(v1, v2), np.maximum(v1, v2)], axis=1),
np.stack([np.minimum(v2, v0), np.maximum(v2, v0)], axis=1),
],
axis=1,
).reshape(-1, 2) # Shape: (num_triangles * 3, 2)
# Triangle index for each edge
tri_indices = np.repeat(np.arange(num_triangles), 3)
# Encode edges as single integers for fast grouping
# Use a large multiplier to avoid collisions
max_vertex = int(vertices.shape[0])
edge_keys = edges[:, 0].astype(np.int64) * max_vertex + edges[:, 1].astype(np.int64)
# Sort edges to group identical edges together
sort_idx = np.argsort(edge_keys)
sorted_keys = edge_keys[sort_idx]
sorted_tris = tri_indices[sort_idx]
# Find where consecutive edges are the same (shared edges)
same_as_next = sorted_keys[:-1] == sorted_keys[1:]
# For each shared edge, connect the two triangles
# Get pairs of triangles that share an edge
tri_a = sorted_tris[:-1][same_as_next]
tri_b = sorted_tris[1:][same_as_next]
# Build sparse adjacency matrix for triangles
# Each shared edge creates a connection between two triangles
if len(tri_a) > 0:
# Create symmetric adjacency matrix
row = np.concatenate([tri_a, tri_b])
col = np.concatenate([tri_b, tri_a])
data = np.ones(len(row), dtype=np.int8)
adjacency = sparse.csr_matrix((data, (row, col)), shape=(num_triangles, num_triangles))
else:
# No shared edges - each triangle is its own component
adjacency = sparse.csr_matrix((num_triangles, num_triangles), dtype=np.int8)
# Find connected components using SciPy (highly optimized C code)
num_components, labels = sparse.csgraph.connected_components(adjacency, directed=False)
# If only one component, return as-is
if num_components == 1:
if verbose:
print("Island filtering: 1 component (mesh is fully connected)")
return vertices, indices
# Count triangles per component
component_sizes = np.bincount(labels)
largest_component = np.argmax(component_sizes)
largest_size = component_sizes[largest_component]
# Get mask of triangles to keep
keep_mask = labels == largest_component
keep_triangles = triangles[keep_mask]
# Find unique vertices used by kept triangles
used_vertices = np.unique(keep_triangles.flatten())
# Create vertex remapping
vertex_remap = np.full(len(vertices), -1, dtype=np.int32)
vertex_remap[used_vertices] = np.arange(len(used_vertices), dtype=np.int32)
# Remap triangle indices
new_indices = vertex_remap[keep_triangles.flatten()]
new_vertices = vertices[used_vertices]
if verbose:
print(f"Island filtering: {num_components} components found")
print(f" Kept largest: {largest_size} triangles ({largest_size * 100.0 / num_triangles:.1f}%)")
print(f" Removed: {num_triangles - largest_size} triangles from {num_components - 1} smaller islands")
return new_vertices, new_indices
def remesh_poisson(
vertices,
faces,
# Point cloud extraction parameters
edge_segments: int = 2,
resolution: int = 1000,
voxel_size: float | None = None,
cavity_cameras: int = 0,
# Surface reconstruction parameters
depth: int = 10,
density_threshold_quantile: float = 0.0,
simplify_tolerance: float | None = 1e-7,
simplify_ratio: float | None = None,
target_triangles: int | None = None,
fast_simplification: bool = True,
n_threads: int = 1,
# Post-processing parameters
keep_largest_island: bool = True,
# Control parameters
device: str | None = None,
seed: int | None = 42,
verbose: bool = False,
):
"""Remesh a 3D triangular surface mesh using Poisson surface reconstruction.
This function extracts a dense point cloud from the input mesh using GPU-accelerated
multi-view raycasting, then reconstructs a clean, watertight mesh using Screened
Poisson Surface Reconstruction.
This is useful for repairing meshes with:
- Inconsistent or flipped triangle winding
- Missing or incorrect vertex normals
- Non-manifold geometry
- Holes or self-intersections
Args:
vertices: A numpy array of shape (N, 3) containing the vertex positions.
faces: A numpy array of shape (M, 3) containing the vertex indices of the faces.
edge_segments: Number of segments per icosahedron edge for camera directions.
Total views = 20 * n^2. Higher values provide better surface coverage.
Default is 2 (80 views).
resolution: Pixel resolution of the orthographic camera (resolution x resolution).
Higher values capture finer details. Default is 1000.
voxel_size: Size of voxels for point accumulation. If None (default), automatically
computed based on mesh size.
cavity_cameras: Number of secondary hemisphere cameras for improved cavity
coverage. Set to 0 (default) to disable.
depth: Octree depth for Poisson reconstruction (higher = more detail, slower).
Default is 10.
density_threshold_quantile: Quantile for removing low-density vertices
(boundary artifacts). Default 0.0 keeps all vertices.
simplify_tolerance: Maximum geometric error allowed during simplification,
as a fraction of the mesh bounding box diagonal. Default is 1e-7.
Set to None to disable simplification.
simplify_ratio: Target ratio to reduce triangle count (e.g., 0.1 = keep 10%).
Only used if simplify_tolerance is None.
target_triangles: Target number of triangles after simplification.
Only used if simplify_tolerance and simplify_ratio are None.
fast_simplification: If True (default), use pyfqmr for fast mesh simplification.
If False, use Open3D (slower but potentially higher quality).
n_threads: Number of threads for Poisson reconstruction. Defaults to ``1``
to avoid an `Open3D bug `_.
Set to ``-1`` for automatic.
keep_largest_island: If True (default), keep only the largest connected component
after reconstruction. This removes small floating fragments that can occur
in areas with sparse point coverage.
device: Warp device for GPU computation. Default uses the default Warp device.
seed: Random seed for reproducibility. Default is 42.
verbose: Print progress information. Default is False.
Returns:
A tuple (vertices, faces) containing the remeshed mesh:
- vertices: A numpy array of shape (K, 3) with vertex positions.
- faces: A numpy array of shape (L, 3) with triangle indices.
Example:
>>> import numpy as np
>>> from ..geometry.remesh import remesh_poisson
>>> # Remesh with default settings
>>> new_verts, new_faces = remesh_poisson(vertices, faces)
>>> # Remesh with higher quality (more views, finer resolution)
>>> new_verts, new_faces = remesh_poisson(vertices, faces, edge_segments=4, resolution=2000)
"""
# Extract point cloud
extractor = PointCloudExtractor(
edge_segments=edge_segments,
resolution=resolution,
voxel_size=voxel_size,
device=device,
seed=seed,
cavity_cameras=cavity_cameras,
)
points, normals = extractor.extract(vertices, faces.flatten())
if verbose:
print(f"Extracted {len(points)} points")
# Reconstruct mesh
reconstructor = SurfaceReconstructor(
depth=depth,
density_threshold_quantile=density_threshold_quantile,
simplify_tolerance=simplify_tolerance,
simplify_ratio=simplify_ratio,
target_triangles=target_triangles,
fast_simplification=fast_simplification,
n_threads=n_threads,
)
mesh = reconstructor.reconstruct(points, normals, verbose=verbose)
# Get vertices and faces from reconstructed mesh
new_vertices = mesh.vertices
new_faces = mesh.indices.reshape(-1, 3)
# Post-processing: keep only the largest connected component
if keep_largest_island:
new_vertices, new_indices = extract_largest_island(new_vertices, new_faces, verbose=verbose)
new_faces = new_indices.reshape(-1, 3)
return new_vertices, new_faces
================================================
FILE: newton/_src/geometry/sdf_contact.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from typing import Any
import warp as wp
from ..geometry.contact_data import SHAPE_PAIR_HFIELD_BIT, SHAPE_PAIR_INDEX_MASK, ContactData
from ..geometry.sdf_texture import TextureSDFData, texture_sample_sdf, texture_sample_sdf_grad
from ..geometry.types import GeoType
from ..utils.heightfield import HeightfieldData, sample_sdf_grad_heightfield, sample_sdf_heightfield
# Handle both direct execution and module import
from .contact_reduction import (
create_shared_memory_pointer_block_dim_mul_func,
get_shared_memory_pointer_block_dim_plus_2_ints,
synchronize,
)
from .contact_reduction_global import GlobalContactReducerData, export_and_reduce_contact_centered
# Shared memory for caching triangle vertices that pass midphase culling.
# block_dim triangles x 3 vertices x 3 floats = 9 int32s per thread.
_get_shared_memory_vertex_cache = create_shared_memory_pointer_block_dim_mul_func(9)
@wp.func
def scale_sdf_result_to_world(
distance: float,
gradient: wp.vec3,
sdf_scale: wp.vec3,
inv_sdf_scale: wp.vec3,
min_sdf_scale: float,
) -> tuple[float, wp.vec3]:
"""
Convert SDF distance and gradient from unscaled space to scaled space.
Args:
distance: Signed distance in unscaled SDF local space
gradient: Gradient direction in unscaled SDF local space
sdf_scale: The SDF shape's scale vector
inv_sdf_scale: Precomputed 1.0 / sdf_scale
min_sdf_scale: Precomputed min(sdf_scale) for distance scaling
Returns:
Tuple of (scaled_distance, scaled_gradient)
"""
# Use min scale for conservative distance (won't miss contacts)
scaled_distance = distance * min_sdf_scale
# Gradient: apply inverse scale and renormalize
scaled_grad = wp.cw_mul(gradient, inv_sdf_scale)
grad_len = wp.length(scaled_grad)
if grad_len > 0.0:
scaled_grad = scaled_grad / grad_len
else:
scaled_grad = gradient
return scaled_distance, scaled_grad
@wp.func
def sample_sdf_using_mesh(
mesh_id: wp.uint64,
world_pos: wp.vec3,
max_dist: float = 1000.0,
) -> float:
"""
Sample signed distance to mesh surface using mesh query.
Uses wp.mesh_query_point_sign_normal to find the closest point on the mesh
and compute the signed distance. This is compatible with the return type of
sample_sdf_extrapolated.
Args:
mesh_id: The mesh ID (from wp.Mesh.id)
world_pos: Query position in mesh local coordinates
max_dist: Maximum distance to search for closest point
Returns:
The signed distance value (negative inside, positive outside)
"""
face_index = int(0)
face_u = float(0.0)
face_v = float(0.0)
sign = float(0.0)
res = wp.mesh_query_point_sign_normal(mesh_id, world_pos, max_dist, sign, face_index, face_u, face_v)
if res:
closest = wp.mesh_eval_position(mesh_id, face_index, face_u, face_v)
return wp.length(world_pos - closest) * sign
return max_dist
@wp.func
def sample_sdf_grad_using_mesh(
mesh_id: wp.uint64,
world_pos: wp.vec3,
max_dist: float = 1000.0,
) -> tuple[float, wp.vec3]:
"""
Sample signed distance and gradient to mesh surface using mesh query.
Uses wp.mesh_query_point_sign_normal to find the closest point on the mesh
and compute both the signed distance and the gradient direction. This is
compatible with the return type of sample_sdf_grad_extrapolated.
The gradient points in the direction of increasing distance (away from the surface
when outside, toward the surface when inside).
Args:
mesh_id: The mesh ID (from wp.Mesh.id)
world_pos: Query position in mesh local coordinates
max_dist: Maximum distance to search for closest point
Returns:
Tuple of (distance, gradient) where:
- distance: Signed distance value (negative inside, positive outside)
- gradient: Normalized direction of increasing distance
"""
face_index = int(0)
face_u = float(0.0)
face_v = float(0.0)
sign = float(0.0)
gradient = wp.vec3(0.0, 0.0, 0.0)
res = wp.mesh_query_point_sign_normal(mesh_id, world_pos, max_dist, sign, face_index, face_u, face_v)
if res:
closest = wp.mesh_eval_position(mesh_id, face_index, face_u, face_v)
diff = world_pos - closest
dist = wp.length(diff)
if dist > 0.0:
# Gradient points from surface toward query point, scaled by sign
# When outside (sign > 0): gradient points away from surface (correct for SDF)
# When inside (sign < 0): gradient points toward surface (correct for SDF)
gradient = (diff / dist) * sign
else:
# Point is exactly on surface - use face normal
# Get the face normal from the mesh
mesh = wp.mesh_get(mesh_id)
i0 = mesh.indices[face_index * 3 + 0]
i1 = mesh.indices[face_index * 3 + 1]
i2 = mesh.indices[face_index * 3 + 2]
v0 = mesh.points[i0]
v1 = mesh.points[i1]
v2 = mesh.points[i2]
face_normal = wp.normalize(wp.cross(v1 - v0, v2 - v0))
gradient = face_normal * sign
return dist * sign, gradient
# No hit found - return max distance with arbitrary gradient
return max_dist, wp.vec3(0.0, 0.0, 1.0)
@wp.func
def closest_pt_point_bary_triangle(c: wp.vec3) -> wp.vec3:
"""
Find the closest point to `c` on the standard barycentric triangle.
This function projects a barycentric coordinate point onto the valid barycentric
triangle defined by vertices (1,0,0), (0,1,0), (0,0,1) in barycentric space.
The valid region is where all coordinates are non-negative and sum to 1.
This is a specialized version of the general closest-point-on-triangle algorithm
optimized for the barycentric simplex.
Args:
c: Input barycentric coordinates (may be outside valid triangle region)
Returns:
The closest valid barycentric coordinates. All components will be >= 0
and sum to 1.0.
Note:
This is used in optimization algorithms that work in barycentric space,
where gradient descent may produce invalid coordinates that need projection.
"""
third = 1.0 / 3.0 # constexpr
c = c - wp.vec3(third * (c[0] + c[1] + c[2] - 1.0))
# two negative: return positive vertex
if c[1] < 0.0 and c[2] < 0.0:
return wp.vec3(1.0, 0.0, 0.0)
if c[0] < 0.0 and c[2] < 0.0:
return wp.vec3(0.0, 1.0, 0.0)
if c[0] < 0.0 and c[1] < 0.0:
return wp.vec3(0.0, 0.0, 1.0)
# one negative: return projection onto line if it is on the edge, or the largest vertex otherwise
if c[0] < 0.0:
d = c[0] * 0.5
y = c[1] + d
z = c[2] + d
if y > 1.0:
return wp.vec3(0.0, 1.0, 0.0)
if z > 1.0:
return wp.vec3(0.0, 0.0, 1.0)
return wp.vec3(0.0, y, z)
if c[1] < 0.0:
d = c[1] * 0.5
x = c[0] + d
z = c[2] + d
if x > 1.0:
return wp.vec3(1.0, 0.0, 0.0)
if z > 1.0:
return wp.vec3(0.0, 0.0, 1.0)
return wp.vec3(x, 0.0, z)
if c[2] < 0.0:
d = c[2] * 0.5
x = c[0] + d
y = c[1] + d
if x > 1.0:
return wp.vec3(1.0, 0.0, 0.0)
if y > 1.0:
return wp.vec3(0.0, 1.0, 0.0)
return wp.vec3(x, y, 0.0)
return c
@wp.func
def get_triangle_from_mesh(
mesh_id: wp.uint64,
mesh_scale: wp.vec3,
X_mesh_ws: wp.transform,
tri_idx: int,
) -> tuple[wp.vec3, wp.vec3, wp.vec3]:
"""
Extract a triangle from a mesh and transform it to world space.
This function retrieves a specific triangle from a mesh by its index,
applies scaling and transformation, and returns the three vertices
in world space coordinates.
Args:
mesh_id: The mesh ID (use wp.mesh_get to retrieve the mesh object)
mesh_scale: Scale to apply to mesh vertices (component-wise)
X_mesh_ws: Mesh world-space transform (position and rotation)
tri_idx: Triangle index in the mesh (0-based)
Returns:
Tuple of (v0_world, v1_world, v2_world) - the three triangle vertices
in world space after applying scale and transform.
Note:
The mesh indices array stores triangle vertex indices as a flat array:
[tri0_v0, tri0_v1, tri0_v2, tri1_v0, tri1_v1, tri1_v2, ...]
"""
mesh = wp.mesh_get(mesh_id)
# Extract triangle vertices from mesh (indices are stored as flat array: i0, i1, i2, i0, i1, i2, ...)
idx0 = mesh.indices[tri_idx * 3 + 0]
idx1 = mesh.indices[tri_idx * 3 + 1]
idx2 = mesh.indices[tri_idx * 3 + 2]
# Get vertex positions in mesh local space (with scale applied)
v0_local = wp.cw_mul(mesh.points[idx0], mesh_scale)
v1_local = wp.cw_mul(mesh.points[idx1], mesh_scale)
v2_local = wp.cw_mul(mesh.points[idx2], mesh_scale)
# Transform vertices to world space
v0_world = wp.transform_point(X_mesh_ws, v0_local)
v1_world = wp.transform_point(X_mesh_ws, v1_local)
v2_world = wp.transform_point(X_mesh_ws, v2_local)
return v0_world, v1_world, v2_world
@wp.func
def get_bounding_sphere(v0: wp.vec3, v1: wp.vec3, v2: wp.vec3) -> tuple[wp.vec3, float]:
"""
Compute a conservative bounding sphere for a triangle.
This uses the triangle centroid as the sphere center and the maximum
distance from the centroid to any vertex as the radius. This is a
conservative (potentially larger than optimal) but fast bounding sphere.
Args:
v0, v1, v2: Triangle vertices in world space
Returns:
Tuple of (center, radius) where:
- center: The centroid of the triangle
- radius: The maximum distance from centroid to any vertex
Note:
This is not the minimal bounding sphere, but it's fast to compute
and adequate for broad-phase culling.
"""
center = (v0 + v1 + v2) * (1.0 / 3.0)
radius = wp.max(wp.max(wp.length_sq(v0 - center), wp.length_sq(v1 - center)), wp.length_sq(v2 - center))
return center, wp.sqrt(radius)
@wp.func
def add_to_shared_buffer_atomic(
thread_id: int,
add_triangle: bool,
tri_idx: int,
buffer: wp.array[wp.int32],
v0: wp.vec3,
v1: wp.vec3,
v2: wp.vec3,
vertex_cache: wp.array[wp.vec3],
vertex_cache_offset: int,
):
"""Add a triangle index to a shared memory buffer using atomic operations.
Also caches the triangle's pre-computed vertices in ``vertex_cache`` so
that downstream consumers can read them from shared memory instead of
re-fetching from global memory.
Buffer layout:
- [0 .. block_dim-1]: Triangle indices
- [block_dim]: Current count of triangles in buffer
- [block_dim+1]: Progress counter (triangles processed so far)
Args:
thread_id: The calling thread's index within the thread block
add_triangle: Whether this thread wants to add a triangle
tri_idx: The triangle index to add (only used if add_triangle is True)
buffer: Shared memory buffer for triangle indices
v0: First vertex in unscaled SDF space (stored only if add_triangle is True)
v1: Second vertex in unscaled SDF space
v2: Third vertex in unscaled SDF space
vertex_cache: Shared memory array (double-buffered staging), dtype=vec3
vertex_cache_offset: Base offset into ``vertex_cache`` for the active staging buffer.
"""
capacity = wp.block_dim()
idx = -1
if add_triangle:
idx = wp.atomic_add(buffer, capacity, 1)
if idx < capacity:
buffer[idx] = tri_idx
base = vertex_cache_offset + idx * 3
vertex_cache[base] = v0
vertex_cache[base + 1] = v1
vertex_cache[base + 2] = v2
if thread_id == 0:
buffer[capacity + 1] += capacity
synchronize() # SYNC 1: All atomic writes and progress update complete
if thread_id == 0 and buffer[capacity] > capacity:
buffer[capacity] = capacity
if add_triangle and idx >= capacity:
wp.atomic_min(buffer, capacity + 1, tri_idx)
synchronize() # SYNC 2: All corrections complete, buffer consistent
@wp.func
def get_triangle_from_heightfield(
hfd: HeightfieldData,
elevation_data: wp.array[wp.float32],
mesh_scale: wp.vec3,
X_ws: wp.transform,
tri_idx: int,
) -> tuple[wp.vec3, wp.vec3, wp.vec3]:
"""Extract a triangle from a heightfield by linear triangle index.
Each grid cell produces 2 triangles. ``tri_idx`` encodes
``(row, col, sub_tri)`` as ``row * (ncol-1) * 2 + col * 2 + sub_tri``.
Returns ``(v0_world, v1_world, v2_world)`` matching :func:`get_triangle_from_mesh`.
"""
cells_per_row = hfd.ncol - 1
cell_idx = tri_idx // 2
tri_sub = tri_idx - cell_idx * 2
row = cell_idx // cells_per_row
col = cell_idx - row * cells_per_row
dx = 2.0 * hfd.hx / wp.float32(hfd.ncol - 1)
dy = 2.0 * hfd.hy / wp.float32(hfd.nrow - 1)
z_range = hfd.max_z - hfd.min_z
x0 = -hfd.hx + wp.float32(col) * dx
x1 = x0 + dx
y0 = -hfd.hy + wp.float32(row) * dy
y1 = y0 + dy
base = hfd.data_offset
h00 = elevation_data[base + row * hfd.ncol + col]
h10 = elevation_data[base + row * hfd.ncol + (col + 1)]
h01 = elevation_data[base + (row + 1) * hfd.ncol + col]
h11 = elevation_data[base + (row + 1) * hfd.ncol + (col + 1)]
p00 = wp.vec3(x0, y0, hfd.min_z + h00 * z_range)
p10 = wp.vec3(x1, y0, hfd.min_z + h10 * z_range)
p01 = wp.vec3(x0, y1, hfd.min_z + h01 * z_range)
p11 = wp.vec3(x1, y1, hfd.min_z + h11 * z_range)
if tri_sub == 0:
v0_local = p00
v1_local = p10
v2_local = p11
else:
v0_local = p00
v1_local = p11
v2_local = p01
v0_local = wp.cw_mul(v0_local, mesh_scale)
v1_local = wp.cw_mul(v1_local, mesh_scale)
v2_local = wp.cw_mul(v2_local, mesh_scale)
v0_world = wp.transform_point(X_ws, v0_local)
v1_world = wp.transform_point(X_ws, v1_local)
v2_world = wp.transform_point(X_ws, v2_local)
return v0_world, v1_world, v2_world
@wp.func
def get_triangle_count(shape_type: int, mesh_id: wp.uint64, hfd: HeightfieldData) -> int:
"""Return the number of triangles for a mesh or heightfield shape."""
if shape_type == GeoType.HFIELD:
if hfd.nrow <= 1 or hfd.ncol <= 1:
return 0
return 2 * (hfd.nrow - 1) * (hfd.ncol - 1)
return wp.mesh_get(mesh_id).indices.shape[0] // 3
def _create_sdf_contact_funcs(enable_heightfields: bool):
"""Generate SDF contact functions with heightfield branches eliminated at compile time.
When ``enable_heightfields`` is False, ``wp.static`` strips all heightfield code
paths from the generated functions, reducing register pressure and instruction
cache footprint — especially in the 16-iteration gradient descent loop of
``do_triangle_sdf_collision``.
Args:
enable_heightfields: When False, all heightfield code paths are compiled out.
Returns:
Tuple of ``(prefetch_triangle_vertices, find_interesting_triangles,
do_triangle_sdf_collision)``.
"""
@wp.func
def do_triangle_sdf_collision_func(
texture_sdf: TextureSDFData,
sdf_mesh_id: wp.uint64,
v0: wp.vec3,
v1: wp.vec3,
v2: wp.vec3,
use_bvh_for_sdf: bool,
sdf_is_heightfield: bool,
hfd_sdf: HeightfieldData,
elevation_data: wp.array[wp.float32],
) -> tuple[float, wp.vec3, wp.vec3]:
"""Compute the deepest contact between a triangle and an SDF volume.
Uses gradient descent in barycentric coordinates to find the point on the
triangle with the minimum signed distance to the SDF. The optimization
starts from the centroid or whichever vertex has the smallest initial
distance.
Returns:
Tuple of (distance, contact_point, contact_direction).
"""
third = 1.0 / 3.0
center = (v0 + v1 + v2) * third
p = center
if wp.static(enable_heightfields):
if sdf_is_heightfield:
dist = sample_sdf_heightfield(hfd_sdf, elevation_data, p)
d0 = sample_sdf_heightfield(hfd_sdf, elevation_data, v0)
d1 = sample_sdf_heightfield(hfd_sdf, elevation_data, v1)
d2 = sample_sdf_heightfield(hfd_sdf, elevation_data, v2)
elif use_bvh_for_sdf:
dist = sample_sdf_using_mesh(sdf_mesh_id, p)
d0 = sample_sdf_using_mesh(sdf_mesh_id, v0)
d1 = sample_sdf_using_mesh(sdf_mesh_id, v1)
d2 = sample_sdf_using_mesh(sdf_mesh_id, v2)
else:
dist = texture_sample_sdf(texture_sdf, p)
d0 = texture_sample_sdf(texture_sdf, v0)
d1 = texture_sample_sdf(texture_sdf, v1)
d2 = texture_sample_sdf(texture_sdf, v2)
else:
if use_bvh_for_sdf:
dist = sample_sdf_using_mesh(sdf_mesh_id, p)
d0 = sample_sdf_using_mesh(sdf_mesh_id, v0)
d1 = sample_sdf_using_mesh(sdf_mesh_id, v1)
d2 = sample_sdf_using_mesh(sdf_mesh_id, v2)
else:
dist = texture_sample_sdf(texture_sdf, p)
d0 = texture_sample_sdf(texture_sdf, v0)
d1 = texture_sample_sdf(texture_sdf, v1)
d2 = texture_sample_sdf(texture_sdf, v2)
if d0 < d1 and d0 < d2 and d0 < dist:
p = v0
uvw = wp.vec3(1.0, 0.0, 0.0)
elif d1 < d2 and d1 < dist:
p = v1
uvw = wp.vec3(0.0, 1.0, 0.0)
elif d2 < dist:
p = v2
uvw = wp.vec3(0.0, 0.0, 1.0)
else:
uvw = wp.vec3(third, third, third)
difference = wp.sqrt(
wp.max(
wp.length_sq(v0 - p),
wp.max(wp.length_sq(v1 - p), wp.length_sq(v2 - p)),
)
)
difference = wp.max(difference, 1e-8)
# Relaxed from 1e-3 to 3e-3: the tighter tolerance required more
# iterations that pushed float32 precision limits, hurting convergence
# without measurably improving contact quality.
tolerance_sq = 3e-3 * 3e-3
sdf_gradient = wp.vec3(0.0, 0.0, 0.0)
step = 1.0 / (2.0 * difference)
for _iter in range(16):
if wp.static(enable_heightfields):
if sdf_is_heightfield:
_, sdf_gradient = sample_sdf_grad_heightfield(hfd_sdf, elevation_data, p)
elif use_bvh_for_sdf:
_, sdf_gradient = sample_sdf_grad_using_mesh(sdf_mesh_id, p)
else:
_, sdf_gradient = texture_sample_sdf_grad(texture_sdf, p)
else:
if use_bvh_for_sdf:
_, sdf_gradient = sample_sdf_grad_using_mesh(sdf_mesh_id, p)
else:
_, sdf_gradient = texture_sample_sdf_grad(texture_sdf, p)
grad_len = wp.length(sdf_gradient)
if grad_len == 0.0:
# Arbitrary non-axis-aligned unit vector to break symmetry
sdf_gradient = wp.vec3(0.571846586, 0.705545099, 0.418566116)
grad_len = 1.0
sdf_gradient = sdf_gradient / grad_len
dfdu = wp.dot(sdf_gradient, v0 - p)
dfdv = wp.dot(sdf_gradient, v1 - p)
dfdw = wp.dot(sdf_gradient, v2 - p)
new_uvw = wp.vec3(uvw[0] - step * dfdu, uvw[1] - step * dfdv, uvw[2] - step * dfdw)
step = step * 0.8
new_uvw = closest_pt_point_bary_triangle(new_uvw)
p = v0 * new_uvw[0] + v1 * new_uvw[1] + v2 * new_uvw[2]
if wp.length_sq(uvw - new_uvw) < tolerance_sq:
break
uvw = new_uvw
if wp.static(enable_heightfields):
if sdf_is_heightfield:
dist, sdf_gradient = sample_sdf_grad_heightfield(hfd_sdf, elevation_data, p)
elif use_bvh_for_sdf:
dist, sdf_gradient = sample_sdf_grad_using_mesh(sdf_mesh_id, p)
else:
dist, sdf_gradient = texture_sample_sdf_grad(texture_sdf, p)
else:
if use_bvh_for_sdf:
dist, sdf_gradient = sample_sdf_grad_using_mesh(sdf_mesh_id, p)
else:
dist, sdf_gradient = texture_sample_sdf_grad(texture_sdf, p)
return dist, p, sdf_gradient
@wp.func
def find_interesting_triangles_func(
thread_id: int,
mesh_scale: wp.vec3,
mesh_to_sdf_transform: wp.transform,
mesh_id: wp.uint64,
texture_sdf: TextureSDFData,
sdf_mesh_id: wp.uint64,
buffer: wp.array[wp.int32],
contact_distance: float,
use_bvh_for_sdf: bool,
inv_sdf_scale: wp.vec3,
tri_end: int,
tri_shape_type: int,
sdf_shape_type: int,
hfd_tri: HeightfieldData,
hfd_sdf: HeightfieldData,
elevation_data: wp.array[wp.float32],
vertex_cache: wp.array[wp.vec3],
):
"""Midphase triangle culling for mesh-SDF collision.
Determines which triangles are close enough to the SDF to potentially generate
contacts. Triangles are transformed to unscaled SDF space before testing.
Vertices of accepted triangles are cached in ``vertex_cache`` so the consumer
can read them from shared memory instead of re-fetching from global memory.
Uses a two-level culling strategy:
1. **AABB early-out (pure ALU, no memory access):** If the triangle's bounding
sphere is farther from the SDF volume's AABB than ``contact_distance``, the
triangle is discarded immediately.
2. **SDF sample:** For triangles that survive the AABB test, sample the SDF at
the bounding-sphere center to get a tighter distance estimate.
Buffer layout: [0..block_dim-1] = triangle indices, [block_dim] = count,
[block_dim+1] = progress.
Args:
tri_end: Maximum triangle index (exclusive).
vertex_cache: Shared memory array of size block_dim*3, dtype=vec3.
"""
capacity = wp.block_dim()
if wp.static(enable_heightfields):
sdf_is_heightfield = sdf_shape_type == GeoType.HFIELD
else:
sdf_is_heightfield = False
sdf_aabb_lower = texture_sdf.sdf_box_lower
sdf_aabb_upper = texture_sdf.sdf_box_upper
synchronize() # Ensure buffer state is consistent before starting
while buffer[capacity + 1] < tri_end and buffer[capacity] < capacity:
base_tri_idx = buffer[capacity + 1]
tri_idx = base_tri_idx + thread_id
add_triangle = False
v0 = wp.vec3(0.0, 0.0, 0.0)
v1 = wp.vec3(0.0, 0.0, 0.0)
v2 = wp.vec3(0.0, 0.0, 0.0)
if tri_idx < tri_end:
if wp.static(enable_heightfields):
if tri_shape_type == GeoType.HFIELD:
v0_scaled, v1_scaled, v2_scaled = get_triangle_from_heightfield(
hfd_tri, elevation_data, mesh_scale, mesh_to_sdf_transform, tri_idx
)
else:
v0_scaled, v1_scaled, v2_scaled = get_triangle_from_mesh(
mesh_id, mesh_scale, mesh_to_sdf_transform, tri_idx
)
else:
v0_scaled, v1_scaled, v2_scaled = get_triangle_from_mesh(
mesh_id, mesh_scale, mesh_to_sdf_transform, tri_idx
)
v0 = wp.cw_mul(v0_scaled, inv_sdf_scale)
v1 = wp.cw_mul(v1_scaled, inv_sdf_scale)
v2 = wp.cw_mul(v2_scaled, inv_sdf_scale)
bounding_sphere_center, bounding_sphere_radius = get_bounding_sphere(v0, v1, v2)
threshold = bounding_sphere_radius + contact_distance
if sdf_is_heightfield:
sdf_dist = sample_sdf_heightfield(hfd_sdf, elevation_data, bounding_sphere_center)
add_triangle = sdf_dist <= threshold
elif use_bvh_for_sdf:
sdf_dist = sample_sdf_using_mesh(sdf_mesh_id, bounding_sphere_center, 1.01 * threshold)
add_triangle = sdf_dist <= threshold
else:
culling_radius = threshold
clamped = wp.min(wp.max(bounding_sphere_center, sdf_aabb_lower), sdf_aabb_upper)
aabb_dist_sq = wp.length_sq(bounding_sphere_center - clamped)
if aabb_dist_sq > culling_radius * culling_radius:
add_triangle = False
else:
sdf_dist = texture_sample_sdf(texture_sdf, bounding_sphere_center)
add_triangle = sdf_dist <= culling_radius
synchronize() # Ensure all threads have read base_tri_idx before any writes
add_to_shared_buffer_atomic(
thread_id,
add_triangle,
tri_idx,
buffer,
v0,
v1,
v2,
vertex_cache,
0,
)
# add_to_shared_buffer_atomic ends with sync, buffer is consistent for next while check
synchronize() # Final sync before returning
return find_interesting_triangles_func, do_triangle_sdf_collision_func
@wp.kernel(enable_backward=False)
def compute_mesh_mesh_tri_counts(
shape_pairs_mesh_mesh: wp.array[wp.vec2i],
shape_pairs_mesh_mesh_count: wp.array[int],
shape_source: wp.array[wp.uint64],
shape_heightfield_index: wp.array[wp.int32],
heightfield_data: wp.array[HeightfieldData],
tri_counts: wp.array[wp.int32],
):
"""Compute per-pair triangle counts for mesh-mesh (or heightfield-mesh) pairs.
Sums the triangle counts of both shapes in each pair — each shape may be
a triangle mesh or a heightfield. Each thread handles one slot in the
``tri_counts`` array. Slots beyond ``pair_count`` are zeroed so that a
subsequent ``array_scan`` over the full array produces correct prefix sums.
"""
i = wp.tid()
pair_count = wp.min(shape_pairs_mesh_mesh_count[0], shape_pairs_mesh_mesh.shape[0])
if i >= pair_count:
tri_counts[i] = 0
return
pair_encoded = shape_pairs_mesh_mesh[i]
has_hfield = (pair_encoded[0] & SHAPE_PAIR_HFIELD_BIT) != 0
pair = wp.vec2i(pair_encoded[0] & SHAPE_PAIR_INDEX_MASK, pair_encoded[1])
pair_tris = int(0)
for mode in range(2):
is_hfield = has_hfield and mode == 0
shape_idx = pair[mode]
if is_hfield:
hfd = heightfield_data[shape_heightfield_index[shape_idx]]
pair_tris += get_triangle_count(GeoType.HFIELD, wp.uint64(0), hfd)
else:
mesh_id = shape_source[shape_idx]
if mesh_id != wp.uint64(0):
pair_tris += wp.mesh_get(mesh_id).indices.shape[0] // 3
tri_counts[i] = wp.int32(pair_tris)
@wp.kernel(enable_backward=False)
def compute_block_counts_from_weights(
weight_prefix_sums: wp.array[wp.int32],
weights: wp.array[wp.int32],
pair_count_arr: wp.array[int],
max_pairs: int,
target_blocks: int,
block_counts: wp.array[wp.int32],
):
"""Convert per-pair weights to block counts using adaptive load balancing.
Reads the total weight from the inclusive prefix sum to compute the
adaptive ``weight_per_block`` threshold, then assigns each pair a
block count proportional to its weight. Slots beyond ``pair_count``
are zeroed for a subsequent exclusive ``array_scan``.
"""
i = wp.tid()
pair_count = wp.min(pair_count_arr[0], max_pairs)
if i >= pair_count:
block_counts[i] = 0
return
# Read total from inclusive prefix sum
total_weight = weight_prefix_sums[pair_count - 1]
weight_per_block = int(total_weight)
if target_blocks > 0 and total_weight > 0:
weight_per_block = wp.max(256, total_weight // target_blocks)
w = int(weights[i])
if weight_per_block > 0:
blocks = wp.max(1, (w + weight_per_block - 1) // weight_per_block)
else:
blocks = 1
block_counts[i] = wp.int32(blocks)
def compute_mesh_mesh_block_offsets_scan(
shape_pairs_mesh_mesh: wp.array,
shape_pairs_mesh_mesh_count: wp.array,
shape_source: wp.array,
shape_heightfield_index: wp.array,
heightfield_data: wp.array,
target_blocks: int,
block_offsets: wp.array,
block_counts: wp.array,
weight_prefix_sums: wp.array,
device: str | None = None,
record_tape: bool = True,
):
"""Compute mesh-mesh block offsets using parallel kernels and array_scan.
Runs a four-stage parallel pipeline: per-pair triangle counts →
inclusive scan → adaptive block counts → exclusive scan into
``block_offsets``.
"""
n = block_counts.shape[0]
# Step 1: compute per-pair triangle counts in parallel
wp.launch(
kernel=compute_mesh_mesh_tri_counts,
dim=n,
inputs=[
shape_pairs_mesh_mesh,
shape_pairs_mesh_mesh_count,
shape_source,
shape_heightfield_index,
heightfield_data,
block_counts, # reuse as temp storage for tri counts
],
device=device,
record_tape=record_tape,
)
# Step 2: inclusive scan to get total in last element
wp.utils.array_scan(block_counts, weight_prefix_sums, inclusive=True)
# Step 3: compute per-pair block counts using adaptive threshold
wp.launch(
kernel=compute_block_counts_from_weights,
dim=n,
inputs=[
weight_prefix_sums,
block_counts, # still holds tri counts
shape_pairs_mesh_mesh_count,
shape_pairs_mesh_mesh.shape[0],
target_blocks,
block_offsets, # reuse as temp for block counts
],
device=device,
record_tape=record_tape,
)
# Step 4: exclusive scan of block counts → block_offsets
wp.utils.array_scan(block_offsets, block_offsets, inclusive=False)
def create_narrow_phase_process_mesh_mesh_contacts_kernel(
writer_func: Any,
enable_heightfields: bool = True,
reduce_contacts: bool = False,
):
find_interesting_triangles, do_triangle_sdf_collision = _create_sdf_contact_funcs(enable_heightfields)
@wp.kernel(enable_backward=False, module="unique")
def mesh_sdf_collision_kernel(
shape_data: wp.array[wp.vec4],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
texture_sdf_table: wp.array[TextureSDFData],
shape_sdf_index: wp.array[wp.int32],
shape_gap: wp.array[float],
_shape_collision_aabb_lower: wp.array[wp.vec3],
_shape_collision_aabb_upper: wp.array[wp.vec3],
_shape_voxel_resolution: wp.array[wp.vec3i],
shape_pairs_mesh_mesh: wp.array[wp.vec2i],
shape_pairs_mesh_mesh_count: wp.array[int],
shape_heightfield_index: wp.array[wp.int32],
heightfield_data: wp.array[HeightfieldData],
heightfield_elevations: wp.array[wp.float32],
writer_data: Any,
total_num_blocks: int,
):
"""Process mesh-mesh and mesh-heightfield collisions using SDF-based detection."""
block_id, t = wp.tid()
pair_count = wp.min(shape_pairs_mesh_mesh_count[0], shape_pairs_mesh_mesh.shape[0])
# Strided loop over pairs
for pair_idx in range(block_id, pair_count, total_num_blocks):
pair_encoded = shape_pairs_mesh_mesh[pair_idx]
if wp.static(enable_heightfields):
has_hfield = (pair_encoded[0] & SHAPE_PAIR_HFIELD_BIT) != 0
pair = wp.vec2i(pair_encoded[0] & SHAPE_PAIR_INDEX_MASK, pair_encoded[1])
else:
has_hfield = False
pair = pair_encoded
gap_sum = shape_gap[pair[0]] + shape_gap[pair[1]]
for mode in range(2):
tri_shape = pair[mode]
sdf_shape = pair[1 - mode]
if wp.static(enable_heightfields):
tri_is_hfield = has_hfield and mode == 0
sdf_is_hfield = has_hfield and mode == 1
else:
tri_is_hfield = False
sdf_is_hfield = False
tri_type = GeoType.HFIELD if tri_is_hfield else GeoType.MESH
sdf_type = GeoType.HFIELD if sdf_is_hfield else GeoType.MESH
mesh_id_tri = shape_source[tri_shape]
mesh_id_sdf = shape_source[sdf_shape]
# Skip invalid sources (heightfields use HeightfieldData instead of mesh id)
if not tri_is_hfield and mesh_id_tri == wp.uint64(0):
continue
if not sdf_is_hfield and mesh_id_sdf == wp.uint64(0):
continue
hfd_tri = HeightfieldData()
hfd_sdf = HeightfieldData()
if wp.static(enable_heightfields):
if tri_is_hfield:
hfd_tri = heightfield_data[shape_heightfield_index[tri_shape]]
if sdf_is_hfield:
hfd_sdf = heightfield_data[shape_heightfield_index[sdf_shape]]
# SDF availability: heightfields always use on-the-fly evaluation
use_bvh_for_sdf = False
if not sdf_is_hfield:
sdf_idx = shape_sdf_index[sdf_shape]
use_bvh_for_sdf = sdf_idx < 0 or sdf_idx >= texture_sdf_table.shape[0]
if not use_bvh_for_sdf:
use_bvh_for_sdf = texture_sdf_table[sdf_idx].coarse_texture.width == 0
scale_data_tri = shape_data[tri_shape]
scale_data_sdf = shape_data[sdf_shape]
mesh_scale_tri = wp.vec3(scale_data_tri[0], scale_data_tri[1], scale_data_tri[2])
mesh_scale_sdf = wp.vec3(scale_data_sdf[0], scale_data_sdf[1], scale_data_sdf[2])
X_tri_ws = shape_transform[tri_shape]
X_sdf_ws = shape_transform[sdf_shape]
# Determine sdf_scale for the SDF query.
# Heightfields always use scale=identity, since SDF is directly sampled
# from elevation grid. For texture SDF, override to identity when scale
# is already baked. For BVH fallback, use the shape scale.
texture_sdf = TextureSDFData()
if sdf_is_hfield:
sdf_scale = wp.vec3(1.0, 1.0, 1.0)
else:
sdf_scale = mesh_scale_sdf
if not use_bvh_for_sdf:
texture_sdf = texture_sdf_table[sdf_idx]
if texture_sdf.scale_baked:
sdf_scale = wp.vec3(1.0, 1.0, 1.0)
X_mesh_to_sdf = wp.transform_multiply(wp.transform_inverse(X_sdf_ws), X_tri_ws)
triangle_mesh_margin = scale_data_tri[3]
sdf_mesh_margin = scale_data_sdf[3]
sdf_scale_safe = wp.vec3(
wp.max(sdf_scale[0], 1e-10),
wp.max(sdf_scale[1], 1e-10),
wp.max(sdf_scale[2], 1e-10),
)
inv_sdf_scale = wp.cw_div(wp.vec3(1.0, 1.0, 1.0), sdf_scale_safe)
min_sdf_scale = wp.min(wp.min(sdf_scale_safe[0], sdf_scale_safe[1]), sdf_scale_safe[2])
contact_threshold = gap_sum + triangle_mesh_margin + sdf_mesh_margin
contact_threshold_unscaled = contact_threshold / min_sdf_scale
tri_capacity = wp.block_dim()
selected_triangles = wp.array(
ptr=get_shared_memory_pointer_block_dim_plus_2_ints(),
shape=(wp.block_dim() + 2,),
dtype=wp.int32,
)
vertex_cache = wp.array(
ptr=_get_shared_memory_vertex_cache(),
shape=(wp.block_dim() * 3,),
dtype=wp.vec3,
)
if t == 0:
selected_triangles[tri_capacity] = 0
selected_triangles[tri_capacity + 1] = 0
synchronize()
num_tris = get_triangle_count(tri_type, mesh_id_tri, hfd_tri)
while selected_triangles[tri_capacity + 1] < num_tris:
find_interesting_triangles(
t,
mesh_scale_tri,
X_mesh_to_sdf,
mesh_id_tri,
texture_sdf,
mesh_id_sdf,
selected_triangles,
contact_threshold_unscaled,
use_bvh_for_sdf,
inv_sdf_scale,
num_tris,
tri_type,
sdf_type,
hfd_tri,
hfd_sdf,
heightfield_elevations,
vertex_cache,
)
has_triangle = t < selected_triangles[tri_capacity]
synchronize()
if has_triangle:
v0 = vertex_cache[t * 3]
v1 = vertex_cache[t * 3 + 1]
v2 = vertex_cache[t * 3 + 2]
dist_unscaled, point_unscaled, direction_unscaled = do_triangle_sdf_collision(
texture_sdf,
mesh_id_sdf,
v0,
v1,
v2,
use_bvh_for_sdf,
sdf_is_hfield,
hfd_sdf,
heightfield_elevations,
)
dist, direction = scale_sdf_result_to_world(
dist_unscaled, direction_unscaled, sdf_scale, inv_sdf_scale, min_sdf_scale
)
point = wp.cw_mul(point_unscaled, sdf_scale)
if dist < contact_threshold:
point_world = wp.transform_point(X_sdf_ws, point)
direction_world = wp.transform_vector(X_sdf_ws, direction)
direction_len = wp.length(direction_world)
if direction_len > 0.0:
direction_world = direction_world / direction_len
else:
fallback_dir = point_world - wp.transform_get_translation(X_sdf_ws)
fallback_len = wp.length(fallback_dir)
if fallback_len > 0.0:
direction_world = fallback_dir / fallback_len
else:
direction_world = wp.vec3(0.0, 1.0, 0.0)
contact_normal = -direction_world if mode == 0 else direction_world
contact_data = ContactData()
contact_data.contact_point_center = point_world
contact_data.contact_normal_a_to_b = contact_normal
contact_data.contact_distance = dist
contact_data.radius_eff_a = 0.0
contact_data.radius_eff_b = 0.0
contact_data.margin_a = shape_data[pair[0]][3]
contact_data.margin_b = shape_data[pair[1]][3]
contact_data.shape_a = pair[0]
contact_data.shape_b = pair[1]
contact_data.gap_sum = gap_sum
writer_func(contact_data, writer_data, -1)
synchronize()
if t == 0:
selected_triangles[tri_capacity] = 0
synchronize()
# Return early if contact reduction is disabled
if not reduce_contacts:
return mesh_sdf_collision_kernel
# =========================================================================
# Global reduction variant: uses hashtable instead of shared-memory reduction.
# Same block_offsets load balancing and shared-memory triangle selection,
# but contacts are written directly to global buffer + hashtable.
# =========================================================================
@wp.kernel(enable_backward=False, module="unique")
def mesh_sdf_collision_global_reduce_kernel(
shape_data: wp.array[wp.vec4],
shape_transform: wp.array[wp.transform],
shape_source: wp.array[wp.uint64],
texture_sdf_table: wp.array[TextureSDFData],
shape_sdf_index: wp.array[wp.int32],
shape_gap: wp.array[float],
shape_collision_aabb_lower: wp.array[wp.vec3],
shape_collision_aabb_upper: wp.array[wp.vec3],
shape_voxel_resolution: wp.array[wp.vec3i],
shape_pairs_mesh_mesh: wp.array[wp.vec2i],
shape_pairs_mesh_mesh_count: wp.array[int],
shape_heightfield_index: wp.array[wp.int32],
heightfield_data: wp.array[HeightfieldData],
heightfield_elevations: wp.array[wp.float32],
block_offsets: wp.array[wp.int32],
reducer_data: GlobalContactReducerData,
total_num_blocks: int,
):
"""Process mesh-mesh collisions with global hashtable contact reduction.
Same load balancing and triangle selection as the thread-block reduce kernel,
but contacts are written directly to the global buffer and registered in the
hashtable inline, matching thread-block reduction contact quality:
- Midpoint-centered position for spatial extreme projection
- Fixed beta threshold (0.0001 m)
- Tri-shape AABB for voxel computation (alternates per mode)
"""
block_id, t = wp.tid()
pair_count = wp.min(shape_pairs_mesh_mesh_count[0], shape_pairs_mesh_mesh.shape[0])
total_combos = block_offsets[pair_count]
for combo_idx in range(block_id, total_combos, total_num_blocks):
lo = int(0)
hi = int(pair_count)
while lo < hi:
mid = (lo + hi) // 2
if block_offsets[mid + 1] <= combo_idx:
lo = mid + 1
else:
hi = mid
pair_idx = int(lo)
pair_block_start = block_offsets[pair_idx]
block_in_pair = combo_idx - pair_block_start
blocks_for_pair = block_offsets[pair_idx + 1] - pair_block_start
pair_encoded = shape_pairs_mesh_mesh[pair_idx]
if wp.static(enable_heightfields):
has_hfield = (pair_encoded[0] & SHAPE_PAIR_HFIELD_BIT) != 0
pair = wp.vec2i(pair_encoded[0] & SHAPE_PAIR_INDEX_MASK, pair_encoded[1])
else:
has_hfield = False
pair = pair_encoded
gap_sum = shape_gap[pair[0]] + shape_gap[pair[1]]
for mode in range(2):
tri_shape = pair[mode]
sdf_shape = pair[1 - mode]
if wp.static(enable_heightfields):
tri_is_hfield = has_hfield and mode == 0
sdf_is_hfield = has_hfield and mode == 1
else:
tri_is_hfield = False
sdf_is_hfield = False
tri_type = GeoType.HFIELD if tri_is_hfield else GeoType.MESH
sdf_type = GeoType.HFIELD if sdf_is_hfield else GeoType.MESH
mesh_id_tri = shape_source[tri_shape]
mesh_id_sdf = shape_source[sdf_shape]
if not tri_is_hfield and mesh_id_tri == wp.uint64(0):
continue
if not sdf_is_hfield and mesh_id_sdf == wp.uint64(0):
continue
hfd_tri = HeightfieldData()
hfd_sdf = HeightfieldData()
if wp.static(enable_heightfields):
if tri_is_hfield:
hfd_tri = heightfield_data[shape_heightfield_index[tri_shape]]
if sdf_is_hfield:
hfd_sdf = heightfield_data[shape_heightfield_index[sdf_shape]]
use_bvh_for_sdf = False
if not sdf_is_hfield:
sdf_idx = shape_sdf_index[sdf_shape]
use_bvh_for_sdf = sdf_idx < 0 or sdf_idx >= texture_sdf_table.shape[0]
if not use_bvh_for_sdf:
use_bvh_for_sdf = texture_sdf_table[sdf_idx].coarse_texture.width == 0
scale_data_tri = shape_data[tri_shape]
scale_data_sdf = shape_data[sdf_shape]
mesh_scale_tri = wp.vec3(scale_data_tri[0], scale_data_tri[1], scale_data_tri[2])
mesh_scale_sdf = wp.vec3(scale_data_sdf[0], scale_data_sdf[1], scale_data_sdf[2])
X_tri_ws = shape_transform[tri_shape]
X_sdf_ws = shape_transform[sdf_shape]
X_ws_tri = wp.transform_inverse(X_tri_ws)
aabb_lower_tri = shape_collision_aabb_lower[tri_shape]
aabb_upper_tri = shape_collision_aabb_upper[tri_shape]
voxel_res_tri = shape_voxel_resolution[tri_shape]
texture_sdf = TextureSDFData()
if sdf_is_hfield:
sdf_scale = wp.vec3(1.0, 1.0, 1.0)
else:
sdf_scale = mesh_scale_sdf
if not use_bvh_for_sdf:
texture_sdf = texture_sdf_table[sdf_idx]
if texture_sdf.scale_baked:
sdf_scale = wp.vec3(1.0, 1.0, 1.0)
X_mesh_to_sdf = wp.transform_multiply(wp.transform_inverse(X_sdf_ws), X_tri_ws)
triangle_mesh_margin = scale_data_tri[3]
sdf_mesh_margin = scale_data_sdf[3]
midpoint = (wp.transform_get_translation(X_tri_ws) + wp.transform_get_translation(X_sdf_ws)) * 0.5
sdf_scale_safe = wp.vec3(
wp.max(sdf_scale[0], 1e-10),
wp.max(sdf_scale[1], 1e-10),
wp.max(sdf_scale[2], 1e-10),
)
inv_sdf_scale = wp.cw_div(wp.vec3(1.0, 1.0, 1.0), sdf_scale_safe)
min_sdf_scale = wp.min(wp.min(sdf_scale_safe[0], sdf_scale_safe[1]), sdf_scale_safe[2])
contact_threshold = gap_sum + triangle_mesh_margin + sdf_mesh_margin
contact_threshold_unscaled = contact_threshold / min_sdf_scale
tri_capacity = wp.block_dim()
selected_triangles = wp.array(
ptr=get_shared_memory_pointer_block_dim_plus_2_ints(),
shape=(wp.block_dim() + 2,),
dtype=wp.int32,
)
vertex_cache = wp.array(
ptr=_get_shared_memory_vertex_cache(),
shape=(wp.block_dim() * 3,),
dtype=wp.vec3,
)
num_tris = get_triangle_count(tri_type, mesh_id_tri, hfd_tri)
chunk_size = (num_tris + blocks_for_pair - 1) // blocks_for_pair
tri_start = block_in_pair * chunk_size
tri_end = wp.min(tri_start + chunk_size, num_tris)
if t == 0:
selected_triangles[tri_capacity] = 0
selected_triangles[tri_capacity + 1] = tri_start
synchronize()
while selected_triangles[tri_capacity + 1] < tri_end:
find_interesting_triangles(
t,
mesh_scale_tri,
X_mesh_to_sdf,
mesh_id_tri,
texture_sdf,
mesh_id_sdf,
selected_triangles,
contact_threshold_unscaled,
use_bvh_for_sdf,
inv_sdf_scale,
tri_end,
tri_type,
sdf_type,
hfd_tri,
hfd_sdf,
heightfield_elevations,
vertex_cache,
)
has_triangle = t < selected_triangles[tri_capacity]
synchronize()
if has_triangle:
v0 = vertex_cache[t * 3]
v1 = vertex_cache[t * 3 + 1]
v2 = vertex_cache[t * 3 + 2]
dist_unscaled, point_unscaled, direction_unscaled = do_triangle_sdf_collision(
texture_sdf,
mesh_id_sdf,
v0,
v1,
v2,
use_bvh_for_sdf,
sdf_is_hfield,
hfd_sdf,
heightfield_elevations,
)
dist, direction = scale_sdf_result_to_world(
dist_unscaled, direction_unscaled, sdf_scale, inv_sdf_scale, min_sdf_scale
)
point = wp.cw_mul(point_unscaled, sdf_scale)
if dist < contact_threshold:
point_world = wp.transform_point(X_sdf_ws, point)
direction_world = wp.transform_vector(X_sdf_ws, direction)
direction_len = wp.length(direction_world)
if direction_len > 0.0:
direction_world = direction_world / direction_len
else:
fallback_dir = point_world - wp.transform_get_translation(X_sdf_ws)
fallback_len = wp.length(fallback_dir)
if fallback_len > 0.0:
direction_world = fallback_dir / fallback_len
else:
direction_world = wp.vec3(0.0, 1.0, 0.0)
contact_normal = -direction_world if mode == 0 else direction_world
export_and_reduce_contact_centered(
pair[0],
pair[1],
point_world,
contact_normal,
dist,
point_world - midpoint,
X_ws_tri,
aabb_lower_tri,
aabb_upper_tri,
voxel_res_tri,
reducer_data,
)
synchronize()
if t == 0:
selected_triangles[tri_capacity] = 0
synchronize()
return mesh_sdf_collision_global_reduce_kernel
================================================
FILE: newton/_src/geometry/sdf_hydroelastic.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""SDF-based hydroelastic contact generation.
This module implements hydroelastic contact modeling between shapes represented
by Signed Distance Fields (SDFs). Hydroelastic contacts model compliant surfaces
where contact force is distributed over a contact patch rather than point contacts.
**Pipeline Overview:**
1. **Broadphase**: OBB intersection tests between SDF shape pairs
2. **Octree Refinement**: Hierarchical subdivision (8x8x8 → 4x4x4 → 2x2x2 → voxels)
to find iso-voxels where the zero-isosurface between SDFs exists
3. **Marching Cubes**: Extract contact surface triangles from iso-voxels
4. **Contact Generation**: Generate contacts at triangle centroids with force
proportional to penetration depth and surface area
5. **Contact Reduction**: Reduce contacts via ``HydroelasticContactReduction``
**Usage:**
Configure shapes with ``ShapeConfig(is_hydroelastic=True, kh=1e9)`` and
pass :class:`HydroelasticSDF.Config` to the collision pipeline.
See Also:
:class:`HydroelasticSDF.Config`: Configuration options for this module.
:class:`HydroelasticContactReduction`: Contact reduction for hydroelastic contacts.
"""
from __future__ import annotations
import warnings
from dataclasses import dataclass
from typing import Any
import numpy as np
import warp as wp
from newton._src.core.types import MAXVAL, Devicelike
from ..sim.builder import ShapeFlags
from ..sim.model import Model
from .collision_core import sat_box_intersection
from .contact_data import ContactData
from .contact_reduction import get_slot
from .contact_reduction_global import (
GlobalContactReducerData,
decode_oct,
encode_oct,
make_contact_key,
)
from .contact_reduction_hydroelastic import (
HydroelasticContactReduction,
HydroelasticReductionConfig,
export_hydroelastic_contact_to_buffer,
)
from .hashtable import hashtable_find_or_insert
from .sdf_mc import get_mc_tables, get_triangle_fraction
from .sdf_texture import TextureSDFData, texture_sample_sdf, texture_sample_sdf_at_voxel
from .utils import scan_with_total
vec8f = wp.types.vector(length=8, dtype=wp.float32)
PRE_PRUNE_MAX_PENETRATING = 2
@wp.kernel(enable_backward=False)
def map_shape_texture_sdf_data_kernel(
sdf_data: wp.array[TextureSDFData],
shape_sdf_index: wp.array[wp.int32],
out_shape_sdf_data: wp.array[TextureSDFData],
):
"""Map compact texture SDF table entries to per-shape TextureSDFData."""
shape_idx = wp.tid()
sdf_idx = shape_sdf_index[shape_idx]
if sdf_idx < 0:
out_shape_sdf_data[shape_idx].sdf_box_lower = wp.vec3(0.0, 0.0, 0.0)
out_shape_sdf_data[shape_idx].sdf_box_upper = wp.vec3(0.0, 0.0, 0.0)
out_shape_sdf_data[shape_idx].voxel_size = wp.vec3(0.0, 0.0, 0.0)
out_shape_sdf_data[shape_idx].voxel_radius = 0.0
out_shape_sdf_data[shape_idx].scale_baked = False
else:
out_shape_sdf_data[shape_idx] = sdf_data[sdf_idx]
@wp.func
def int_to_vec3f(x: wp.int32, y: wp.int32, z: wp.int32):
return wp.vec3f(float(x), float(y), float(z))
@wp.func
def get_effective_stiffness(k_a: wp.float32, k_b: wp.float32) -> wp.float32:
"""Compute effective stiffness for two materials in series."""
denom = k_a + k_b
if denom <= 0.0:
return 0.0
return (k_a * k_b) / denom
@wp.func
def mc_calc_face_texture(
flat_edge_verts_table: wp.array[wp.vec2ub],
corner_offsets_table: wp.array[wp.vec3ub],
tri_range_start: wp.int32,
corner_vals: vec8f,
corner_sdf_vals: vec8f,
sdf_a: TextureSDFData,
x_id: wp.int32,
y_id: wp.int32,
z_id: wp.int32,
) -> tuple[float, wp.vec3, wp.vec3, float, wp.mat33f]:
"""Extract a triangle face from a marching cubes voxel using texture SDF.
Vertex positions are returned in the SDF's local coordinate space.
A tiny thickness (1e-4 x voxel_radius) biases the signed-distance depth
just enough to classify touching-surface vertices as penetrating. The
resulting phantom force is negligible (< 0.1 % of typical contact forces)
but prevents zero-area contacts at exactly-touching surfaces.
"""
thickness = sdf_a.voxel_radius * 1.0e-4
face_verts = wp.mat33f()
vert_depths = wp.vec3f()
num_inside = wp.int32(0)
for vi in range(3):
edge_verts = wp.vec2i(flat_edge_verts_table[tri_range_start + vi])
v_idx_from = edge_verts[0]
v_idx_to = edge_verts[1]
val_0 = wp.float32(corner_vals[v_idx_from])
val_1 = wp.float32(corner_vals[v_idx_to])
p_0 = wp.vec3f(corner_offsets_table[v_idx_from])
p_1 = wp.vec3f(corner_offsets_table[v_idx_to])
val_diff = wp.float32(val_1 - val_0)
if wp.abs(val_diff) < 1e-8:
t = float(0.5)
else:
t = (0.0 - val_0) / val_diff
p = p_0 + t * (p_1 - p_0)
vol_idx = p + int_to_vec3f(x_id, y_id, z_id)
local_pos = sdf_a.sdf_box_lower + wp.cw_mul(vol_idx, sdf_a.voxel_size)
face_verts[vi] = local_pos
# Interpolate SDF depth from cached corner values (avoids texture lookup)
sdf_from = wp.float32(corner_sdf_vals[v_idx_from])
sdf_to = wp.float32(corner_sdf_vals[v_idx_to])
depth = sdf_from + t * (sdf_to - sdf_from) - thickness
vert_depths[vi] = depth
if depth < 0.0:
num_inside += 1
n = wp.cross(face_verts[1] - face_verts[0], face_verts[2] - face_verts[0])
normal = wp.normalize(n)
area = wp.length(n) / 2.0
center = (face_verts[0] + face_verts[1] + face_verts[2]) / 3.0
pen_depth = (vert_depths[0] + vert_depths[1] + vert_depths[2]) / 3.0
area *= get_triangle_fraction(vert_depths, num_inside)
return area, normal, center, pen_depth, face_verts
class HydroelasticSDF:
"""Hydroelastic contact generation with SDF-based collision detection.
This class implements hydroelastic contact modeling between shapes represented
by Signed Distance Fields (SDFs). It uses an octree-based broadphase to identify
potentially colliding regions, then applies marching cubes to extract the
zero-isosurface where both SDFs intersect. Contact points are generated at
triangle centroids on this isosurface, with contact forces proportional to
penetration depth and represented area.
The collision pipeline consists of:
1. Broadphase: Identifies overlapping OBBs of SDF between shape pairs
2. Octree refinement: Hierarchically subdivides blocks to find iso-voxels
3. Marching cubes: Extracts contact surface triangles from iso-voxels
4. Contact generation: Computes contact points, normals, depths, and areas
5. Optional contact reduction: Bins and reduces contacts per shape pair
Args:
num_shape_pairs: Maximum number of hydroelastic shape pairs to process.
total_num_tiles: Total number of SDF blocks across all hydroelastic shapes.
max_num_blocks_per_shape: Maximum block count for any single shape.
shape_sdf_block_coords: Block coordinates for each shape's SDF representation.
shape_sdf_shape2blocks: Mapping from shape index to (start, end) block range.
shape_material_kh: Hydroelastic stiffness coefficient for each shape.
n_shapes: Total number of shapes in the simulation.
config: Configuration options controlling buffer sizes, contact reduction,
and other behavior. Defaults to :class:`HydroelasticSDF.Config`.
device: Warp device for GPU computation.
writer_func: Callback for writing decoded contact data.
Note:
Instances are typically created internally by the collision pipeline
(via :meth:`~newton.Model.collide`) rather than constructed directly.
The pipeline automatically extracts the required SDF data and shape
information from the simulation :class:`~newton.Model`.
Contact IDs are packed into 32-bit integers using 9 bits per voxel axis coordinate.
For SDF grids larger than 512 voxels per axis, contact ID collisions may occur,
which can affect contact matching accuracy for warm-starting physics solvers.
See Also:
:class:`HydroelasticSDF.Config`: Configuration options for this class.
"""
@dataclass
class Config:
"""Controls properties of SDF hydroelastic collision handling."""
reduce_contacts: bool = True
"""Whether to reduce contacts to a smaller representative set per shape pair.
When False, all generated contacts are passed through without reduction."""
pre_prune_contacts: bool = True
"""Whether to perform local-first face compaction during generation.
This mode avoids global hashtable traffic in the hot generation loop and
writes a smaller contact set to the buffer before the normal reduce pass.
Only active when ``reduce_contacts`` is True."""
buffer_fraction: float = 1.0
"""Fraction of worst-case hydroelastic buffer allocations. Range: (0, 1].
This scales pre-allocated broadphase, iso-refinement, and face-contact
buffers before applying stage multipliers. Lower values reduce memory
usage and may cause overflows in dense scenes. Overflows are bounds-safe
and emit warnings; increase this value when warnings appear.
"""
buffer_mult_broad: int = 1
"""Multiplier for the preallocated broadphase buffer that stores overlapping
block pairs. Increase only if a broadphase overflow warning is issued."""
buffer_mult_iso: int = 1
"""Multiplier for preallocated iso-surface extraction buffers used during
hierarchical octree refinement (subblocks and voxels). Increase only if an iso buffer overflow warning is issued."""
buffer_mult_contact: int = 1
"""Multiplier for the preallocated face contact buffer that stores contact
positions, normals, depths, and areas. Increase only if a face contact overflow warning is issued."""
contact_buffer_fraction: float = 0.5
"""Fraction of the face contact buffer to allocate when ``reduce_contacts`` is True.
The reduce kernel selects winners from whatever fits in the buffer, so a smaller
buffer trades off coverage for memory savings.
Range: (0, 1]. Only applied when ``reduce_contacts`` is enabled; ignored otherwise."""
grid_size: int = 256 * 8 * 128
"""Grid size for contact handling. Can be tuned for performance."""
output_contact_surface: bool = False
"""Whether to output hydroelastic contact surface vertices for visualization."""
normal_matching: bool = True
"""Whether to rotate reduced contact normals so their weighted sum aligns with
the aggregate force direction. Only active when reduce_contacts is True."""
anchor_contact: bool = False
"""Whether to add an anchor contact at the center of pressure for each normal bin.
The anchor contact helps preserve moment balance. Only active when reduce_contacts is True."""
moment_matching: bool = False
"""Whether to adjust per-contact friction scales so that the maximum
friction moment per normal bin is preserved between reduced and
unreduced contacts. Automatically enables ``anchor_contact``.
Only active when reduce_contacts is True."""
margin_contact_area: float = 1e-2
"""Contact area used for non-penetrating contacts at the margin."""
@dataclass
class ContactSurfaceData:
"""
Data container for hydroelastic contact surface visualization.
Contains the vertex arrays and metadata needed for rendering
the contact surface triangles from hydroelastic collision detection.
"""
contact_surface_point: wp.array[wp.vec3f]
"""World-space positions of contact surface triangle vertices (3 per face)."""
contact_surface_depth: wp.array[wp.float32]
"""Penetration depth at each face centroid."""
contact_surface_shape_pair: wp.array[wp.vec2i]
"""Shape pair indices (shape_a, shape_b) for each face."""
face_contact_count: wp.array[wp.int32]
"""Array containing the number of face contacts."""
max_num_face_contacts: int
"""Maximum number of face contacts (buffer size)."""
def __init__(
self,
num_shape_pairs: int,
total_num_tiles: int,
max_num_blocks_per_shape: int,
shape_sdf_block_coords: wp.array[wp.vec3us],
shape_sdf_shape2blocks: wp.array[wp.vec2i],
shape_material_kh: wp.array[wp.float32],
n_shapes: int,
config: HydroelasticSDF.Config | None = None,
device: Devicelike | None = None,
writer_func: Any = None,
) -> None:
if config is None:
config = HydroelasticSDF.Config()
self.config = config
if device is None:
device = wp.get_device()
self.device = device
# keep local references for model arrays
self.shape_sdf_block_coords = shape_sdf_block_coords
self.shape_sdf_shape2blocks = shape_sdf_shape2blocks
self.shape_material_kh = shape_material_kh
self.n_shapes = n_shapes
self.max_num_shape_pairs = num_shape_pairs
self.total_num_tiles = total_num_tiles
self.max_num_blocks_per_shape = max_num_blocks_per_shape
frac = float(self.config.buffer_fraction)
if frac <= 0.0 or frac > 1.0:
raise ValueError(f"HydroelasticSDF.Config.buffer_fraction must be in (0, 1], got {frac}")
contact_frac = float(self.config.contact_buffer_fraction)
if contact_frac <= 0.0 or contact_frac > 1.0:
raise ValueError(f"HydroelasticSDF.Config.contact_buffer_fraction must be in (0, 1], got {contact_frac}")
mult = max(int(self.config.buffer_mult_iso * self.total_num_tiles * frac), 64)
self.max_num_blocks_broad = max(
int(self.max_num_shape_pairs * self.max_num_blocks_per_shape * self.config.buffer_mult_broad * frac),
64,
)
# Output buffer sizes for each octree level (subblocks 8x8x8 -> 4x4x4 -> 2x2x2 -> voxels)
# The voxel-level multiplier (48x) is sized for texture-backed SDFs.
self.iso_max_dims = (int(2 * mult), int(2 * mult), int(16 * mult), int(48 * mult))
self.max_num_iso_voxels = self.iso_max_dims[3]
# Input buffer sizes for each octree level
self.input_sizes = (self.max_num_blocks_broad, *self.iso_max_dims[:3])
with wp.ScopedDevice(device):
self.num_shape_pairs_array = wp.full((1,), self.max_num_shape_pairs, dtype=wp.int32)
# Allocate buffers for octree traversal (broadphase + 4 refinement levels)
self.iso_buffer_counts = [wp.zeros((1,), dtype=wp.int32) for _ in range(5)]
# Scratch buffers are per-level to avoid scanning the worst-case
# size at all refinement levels during graph-captured execution.
self.iso_buffer_prefix_scratch = [wp.zeros(level_input, dtype=wp.int32) for level_input in self.input_sizes]
self.iso_buffer_num_scratch = [wp.zeros(level_input, dtype=wp.int32) for level_input in self.input_sizes]
self.iso_subblock_idx_scratch = [wp.zeros(level_input, dtype=wp.uint8) for level_input in self.input_sizes]
self.iso_buffer_coords = [wp.empty((self.max_num_blocks_broad,), dtype=wp.vec3us)] + [
wp.empty((self.iso_max_dims[i],), dtype=wp.vec3us) for i in range(4)
]
self.iso_buffer_shape_pairs = [wp.empty((self.max_num_blocks_broad,), dtype=wp.vec2i)] + [
wp.empty((self.iso_max_dims[i],), dtype=wp.vec2i) for i in range(4)
]
# Aliases for commonly accessed final buffers
self.block_broad_collide_count = self.iso_buffer_counts[0]
self.iso_voxel_count = self.iso_buffer_counts[4]
self.iso_voxel_coords = self.iso_buffer_coords[4]
self.iso_voxel_shape_pair = self.iso_buffer_shape_pairs[4]
# Broadphase buffers
self.block_start_prefix = wp.zeros((self.max_num_shape_pairs,), dtype=wp.int32)
self.num_blocks_per_pair = wp.zeros((self.max_num_shape_pairs,), dtype=wp.int32)
self.block_broad_idx = wp.empty((self.max_num_blocks_broad,), dtype=wp.int32)
self.block_broad_collide_coords = self.iso_buffer_coords[0]
self.block_broad_collide_shape_pair = self.iso_buffer_shape_pairs[0]
# Face contacts written directly to GlobalContactReducer (no intermediate buffers)
# When pre-pruning is active, far fewer contacts reach the buffer so we
# scale down by contact_buffer_fraction to save memory.
face_contact_budget = config.buffer_mult_contact * self.max_num_iso_voxels
if config.reduce_contacts and config.pre_prune_contacts:
face_contact_budget = face_contact_budget * config.contact_buffer_fraction
self.max_num_face_contacts = max(int(face_contact_budget), 64)
if self.config.output_contact_surface:
# stores the point and depth of the contact surface vertex
self.iso_vertex_point = wp.empty((3 * self.max_num_face_contacts,), dtype=wp.vec3f)
self.iso_vertex_depth = wp.empty((self.max_num_face_contacts,), dtype=wp.float32)
self.iso_vertex_shape_pair = wp.empty((self.max_num_face_contacts,), dtype=wp.vec2i)
else:
self.iso_vertex_point = wp.empty((0,), dtype=wp.vec3f)
self.iso_vertex_depth = wp.empty((0,), dtype=wp.float32)
self.iso_vertex_shape_pair = wp.empty((0,), dtype=wp.vec2i)
self.mc_tables = get_mc_tables(device)
# Placeholder empty arrays for kernel parameters unused in no-prune mode
self._empty_vec3 = wp.empty((0,), dtype=wp.vec3, device=device)
self._empty_vec3i = wp.empty((0,), dtype=wp.vec3i, device=device)
# Pre-allocate per-shape SDF data buffer used in launch() so that
# no wp.empty() call occurs during CUDA graph capture (#1616).
self._shape_sdf_data = wp.empty(n_shapes, dtype=TextureSDFData, device=device)
self.generate_contacts_kernel = get_generate_contacts_kernel(
output_vertices=self.config.output_contact_surface,
pre_prune=self.config.reduce_contacts and self.config.pre_prune_contacts,
)
if self.config.reduce_contacts:
# Use HydroelasticContactReduction for efficient hashtable-based contact reduction
# The reducer uses spatial extremes + max-depth per normal bin + voxel-based slots
reduction_config = HydroelasticReductionConfig(
normal_matching=self.config.normal_matching,
anchor_contact=self.config.anchor_contact,
moment_matching=self.config.moment_matching,
margin_contact_area=self.config.margin_contact_area,
)
self.contact_reduction = HydroelasticContactReduction(
capacity=self.max_num_face_contacts,
device=device,
writer_func=writer_func,
config=reduction_config,
)
self.decode_contacts_kernel = None
else:
# No reduction - create a simple reducer for buffer storage and decode kernel
self.contact_reduction = HydroelasticContactReduction(
capacity=self.max_num_face_contacts,
device=device,
writer_func=writer_func,
config=HydroelasticReductionConfig(margin_contact_area=self.config.margin_contact_area),
)
self.decode_contacts_kernel = get_decode_contacts_kernel(
self.config.margin_contact_area,
writer_func,
)
self.grid_size = min(self.config.grid_size, self.max_num_face_contacts)
self._host_warning_poll_interval = 120
self._launch_counter = 0
@classmethod
def _from_model(
cls, model: Model, config: HydroelasticSDF.Config | None = None, writer_func: Any = None
) -> HydroelasticSDF | None:
"""Create HydroelasticSDF from a model.
Args:
model: The simulation model.
config: Optional configuration for hydroelastic collision handling.
writer_func: Optional writer function for decoding contacts.
Returns:
HydroelasticSDF instance, or None if no hydroelastic shape pairs exist.
"""
shape_flags = model.shape_flags.numpy()
# Check if any shapes have hydroelastic flag
has_hydroelastic = any((flags & ShapeFlags.HYDROELASTIC) for flags in shape_flags)
if not has_hydroelastic:
return None
shape_pairs = model.shape_contact_pairs.numpy()
num_hydroelastic_pairs = 0
for shape_a, shape_b in shape_pairs:
if (shape_flags[shape_a] & ShapeFlags.HYDROELASTIC) and (shape_flags[shape_b] & ShapeFlags.HYDROELASTIC):
num_hydroelastic_pairs += 1
if num_hydroelastic_pairs == 0:
return None
shape_sdf_index = model.shape_sdf_index.numpy()
sdf_index2blocks = model.sdf_index2blocks.numpy()
texture_sdf_data = model.texture_sdf_data.numpy()
shape_scale = model.shape_scale.numpy()
# Get indices of shapes that can collide and are hydroelastic
hydroelastic_indices = [
i
for i in range(model.shape_count)
if (shape_flags[i] & ShapeFlags.COLLIDE_SHAPES) and (shape_flags[i] & ShapeFlags.HYDROELASTIC)
]
for idx in hydroelastic_indices:
sdf_idx = shape_sdf_index[idx]
if sdf_idx < 0:
raise ValueError(f"Hydroelastic shape {idx} requires SDF data but has no attached/generated SDF.")
if not texture_sdf_data[sdf_idx]["scale_baked"]:
sx, sy, sz = shape_scale[idx]
if not (np.isclose(sx, 1.0) and np.isclose(sy, 1.0) and np.isclose(sz, 1.0)):
raise ValueError(
f"Hydroelastic shape {idx} uses non-unit scale but its SDF is not scale-baked. "
"Build a scale-baked SDF for hydroelastic use."
)
# Count total tiles and max blocks per shape for hydroelastic shapes
total_num_tiles = 0
max_num_blocks_per_shape = 0
shape_sdf_shape2blocks = np.zeros((model.shape_count, 2), dtype=np.int32)
for shape_idx in range(model.shape_count):
sdf_idx = shape_sdf_index[shape_idx]
if sdf_idx >= 0:
shape_sdf_shape2blocks[shape_idx] = sdf_index2blocks[sdf_idx]
for idx in hydroelastic_indices:
start_block, end_block = shape_sdf_shape2blocks[idx]
num_blocks = end_block - start_block
total_num_tiles += num_blocks
max_num_blocks_per_shape = max(max_num_blocks_per_shape, num_blocks)
return cls(
num_shape_pairs=num_hydroelastic_pairs,
total_num_tiles=total_num_tiles,
max_num_blocks_per_shape=max_num_blocks_per_shape,
shape_sdf_block_coords=model.sdf_block_coords,
shape_sdf_shape2blocks=wp.array(shape_sdf_shape2blocks, dtype=wp.vec2i, device=model.device),
shape_material_kh=model.shape_material_kh,
n_shapes=model.shape_count,
config=config,
device=model.device,
writer_func=writer_func,
)
def get_contact_surface(self) -> ContactSurfaceData | None:
"""Get hydroelastic :class:`ContactSurfaceData` for visualization.
Returns:
A :class:`ContactSurfaceData` instance containing vertex arrays and metadata for rendering,
or None if :attr:`~newton.geometry.HydroelasticSDF.Config.output_contact_surface` is False.
"""
if not self.config.output_contact_surface:
return None
return self.ContactSurfaceData(
contact_surface_point=self.iso_vertex_point,
contact_surface_depth=self.iso_vertex_depth,
contact_surface_shape_pair=self.iso_vertex_shape_pair,
face_contact_count=self.contact_reduction.contact_count,
max_num_face_contacts=self.max_num_face_contacts,
)
def launch(
self,
texture_sdf_data: wp.array[TextureSDFData],
shape_sdf_index: wp.array[wp.int32],
shape_transform: wp.array[wp.transform],
shape_gap: wp.array[wp.float32],
shape_collision_aabb_lower: wp.array[wp.vec3],
shape_collision_aabb_upper: wp.array[wp.vec3],
shape_voxel_resolution: wp.array[wp.vec3i],
shape_pairs_sdf_sdf: wp.array[wp.vec2i],
shape_pairs_sdf_sdf_count: wp.array[wp.int32],
writer_data: Any,
) -> None:
"""Run the full hydroelastic collision pipeline.
All internal kernel launches use ``record_tape=False`` so that this
method is safe to call inside a :class:`warp.Tape` context.
Args:
texture_sdf_data: Compact texture SDF table.
shape_sdf_index: Per-shape SDF index into texture_sdf_data.
shape_transform: World transforms for each shape.
shape_gap: Per-shape contact gap (detection threshold) for each shape.
shape_collision_aabb_lower: Per-shape collision AABB lower bounds.
shape_collision_aabb_upper: Per-shape collision AABB upper bounds.
shape_voxel_resolution: Per-shape voxel grid resolution.
shape_pairs_sdf_sdf: Pairs of shape indices to check for collision.
shape_pairs_sdf_sdf_count: Number of valid shape pairs.
writer_data: Contact data writer for output.
"""
shape_sdf_data = self._shape_sdf_data
wp.launch(
kernel=map_shape_texture_sdf_data_kernel,
dim=shape_sdf_index.shape[0],
inputs=[texture_sdf_data, shape_sdf_index],
outputs=[shape_sdf_data],
device=self.device,
record_tape=False,
)
self._broadphase_sdfs(
shape_sdf_data,
shape_transform,
shape_pairs_sdf_sdf,
shape_pairs_sdf_sdf_count,
)
self._find_iso_voxels(shape_sdf_data, shape_transform, shape_gap)
self._generate_contacts(shape_sdf_data, shape_transform, shape_gap)
if self.config.reduce_contacts:
self._reduce_decode_contacts(
shape_transform,
shape_collision_aabb_lower,
shape_collision_aabb_upper,
shape_voxel_resolution,
shape_gap,
writer_data,
)
else:
self._decode_contacts(
shape_transform,
shape_gap,
writer_data,
)
wp.launch(
kernel=verify_collision_step,
dim=[1],
inputs=[
self.block_broad_collide_count,
self.max_num_blocks_broad,
self.iso_buffer_counts[1],
self.iso_max_dims[0],
self.iso_buffer_counts[2],
self.iso_max_dims[1],
self.iso_buffer_counts[3],
self.iso_max_dims[2],
self.iso_voxel_count,
self.max_num_iso_voxels,
self.contact_reduction.contact_count,
self.max_num_face_contacts,
writer_data.contact_count,
writer_data.contact_max,
self.contact_reduction.reducer.ht_insert_failures,
],
device=self.device,
record_tape=False,
)
# Poll infrequently to avoid per-step host sync overhead while still surfacing
# dropped-contact conditions outside stdout-captured environments.
self._launch_counter += 1
if self._launch_counter % self._host_warning_poll_interval == 0:
hashtable_failures = int(self.contact_reduction.reducer.ht_insert_failures.numpy()[0])
if hashtable_failures > 0:
warnings.warn(
"Hydroelastic reduction dropped contacts due to hashtable insert "
f"failures ({hashtable_failures}). Increase rigid_contact_max "
"and/or HydroelasticSDF.Config.buffer_fraction.",
RuntimeWarning,
stacklevel=2,
)
def _broadphase_sdfs(
self,
shape_sdf_data: wp.array[TextureSDFData],
shape_transform: wp.array[wp.transform],
shape_pairs_sdf_sdf: wp.array[wp.vec2i],
shape_pairs_sdf_sdf_count: wp.array[wp.int32],
) -> None:
# Test collisions between OBB of SDFs
self.num_blocks_per_pair.zero_()
wp.launch(
kernel=broadphase_collision_pairs_count,
dim=[self.max_num_shape_pairs],
inputs=[
shape_transform,
shape_sdf_data,
shape_pairs_sdf_sdf,
shape_pairs_sdf_sdf_count,
self.shape_sdf_shape2blocks,
],
outputs=[
self.num_blocks_per_pair,
],
device=self.device,
record_tape=False,
)
scan_with_total(
self.num_blocks_per_pair,
self.block_start_prefix,
self.num_shape_pairs_array,
self.block_broad_collide_count,
)
wp.launch(
kernel=broadphase_collision_pairs_scatter,
dim=[self.max_num_shape_pairs],
inputs=[
self.num_blocks_per_pair,
shape_sdf_data,
self.block_start_prefix,
shape_pairs_sdf_sdf,
shape_pairs_sdf_sdf_count,
self.shape_sdf_shape2blocks,
self.max_num_blocks_broad,
],
outputs=[
self.block_broad_collide_shape_pair,
self.block_broad_idx,
],
device=self.device,
record_tape=False,
)
wp.launch(
kernel=broadphase_get_block_coords,
dim=[self.grid_size],
inputs=[
self.grid_size,
self.block_broad_collide_count,
self.block_broad_idx,
self.shape_sdf_block_coords,
self.max_num_blocks_broad,
],
outputs=[
self.block_broad_collide_coords,
],
device=self.device,
record_tape=False,
)
def _find_iso_voxels(
self,
shape_sdf_data: wp.array[TextureSDFData],
shape_transform: wp.array[wp.transform],
shape_gap: wp.array[wp.float32],
) -> None:
# Find voxels which contain the isosurface between the shapes using octree-like pruning.
# We do this by computing the difference between sdfs at the voxel/subblock center and comparing it to the voxel/subblock radius.
# The check is first performed for subblocks of size (8 x 8 x 8), then (4 x 4 x 4), then (2 x 2 x 2), and finally for each voxel.
for i, (subblock_size, n_blocks) in enumerate([(8, 1), (4, 2), (2, 2), (1, 2)]):
wp.launch(
kernel=count_iso_voxels_block,
dim=[self.grid_size],
inputs=[
self.grid_size,
self.iso_buffer_counts[i],
shape_sdf_data,
shape_transform,
self.shape_material_kh,
self.iso_buffer_coords[i],
self.iso_buffer_shape_pairs[i],
shape_gap,
subblock_size,
n_blocks,
self.input_sizes[i],
],
outputs=[
self.iso_buffer_num_scratch[i],
self.iso_subblock_idx_scratch[i],
],
device=self.device,
record_tape=False,
)
scan_with_total(
self.iso_buffer_num_scratch[i],
self.iso_buffer_prefix_scratch[i],
self.iso_buffer_counts[i],
self.iso_buffer_counts[i + 1],
)
wp.launch(
kernel=scatter_iso_subblock,
dim=[self.grid_size],
inputs=[
self.grid_size,
self.iso_buffer_counts[i],
self.iso_buffer_prefix_scratch[i],
self.iso_subblock_idx_scratch[i],
self.iso_buffer_shape_pairs[i],
self.iso_buffer_coords[i],
subblock_size,
self.input_sizes[i],
self.iso_max_dims[i],
],
outputs=[
self.iso_buffer_coords[i + 1],
self.iso_buffer_shape_pairs[i + 1],
],
device=self.device,
record_tape=False,
)
def _generate_contacts(
self,
shape_sdf_data: wp.array[TextureSDFData],
shape_transform: wp.array[wp.transform],
shape_gap: wp.array[wp.float32],
shape_local_aabb_lower: wp.array | None = None,
shape_local_aabb_upper: wp.array | None = None,
shape_voxel_resolution: wp.array | None = None,
) -> None:
"""Generate marching cubes contacts and write directly to the contact buffer.
Single pass: compute cube state and immediately write faces to reducer buffer.
When pre-pruning is active the extra AABB/voxel-resolution arrays must be
provided so the kernel can populate the hashtable and gate buffer writes.
"""
self.contact_reduction.clear()
reducer_data = self.contact_reduction.get_data_struct()
# Placeholder arrays for the pre-prune parameters when not used
if shape_local_aabb_lower is None:
shape_local_aabb_lower = self._empty_vec3
if shape_local_aabb_upper is None:
shape_local_aabb_upper = self._empty_vec3
if shape_voxel_resolution is None:
shape_voxel_resolution = self._empty_vec3i
wp.launch(
kernel=self.generate_contacts_kernel,
dim=[self.grid_size],
inputs=[
self.grid_size,
self.iso_voxel_count,
shape_sdf_data,
shape_transform,
self.shape_material_kh,
self.iso_voxel_coords,
self.iso_voxel_shape_pair,
self.mc_tables[0],
self.mc_tables[4],
self.mc_tables[3],
shape_gap,
self.max_num_iso_voxels,
reducer_data,
shape_local_aabb_lower,
shape_local_aabb_upper,
shape_voxel_resolution,
],
outputs=[
self.iso_vertex_point,
self.iso_vertex_depth,
self.iso_vertex_shape_pair,
],
device=self.device,
record_tape=False,
)
def _decode_contacts(
self,
shape_transform: wp.array[wp.transform],
shape_gap: wp.array[wp.float32],
writer_data: Any,
) -> None:
"""Decode hydroelastic contacts without reduction.
Contacts are already in the buffer (written by _generate_contacts).
This method exports all contacts directly without any reduction.
"""
wp.launch(
kernel=self.decode_contacts_kernel,
dim=[self.grid_size],
inputs=[
self.grid_size,
self.contact_reduction.contact_count,
self.shape_material_kh,
shape_transform,
shape_gap,
self.contact_reduction.reducer.position_depth,
self.contact_reduction.reducer.normal,
self.contact_reduction.reducer.shape_pairs,
self.contact_reduction.reducer.contact_area,
self.max_num_face_contacts,
],
outputs=[writer_data],
device=self.device,
record_tape=False,
)
def _reduce_decode_contacts(
self,
shape_transform: wp.array[wp.transform],
shape_collision_aabb_lower: wp.array[wp.vec3],
shape_collision_aabb_upper: wp.array[wp.vec3],
shape_voxel_resolution: wp.array[wp.vec3i],
shape_gap: wp.array[wp.float32],
writer_data: Any,
) -> None:
"""Reduce buffered contacts and export the winners.
Runs the reduction kernel to populate the hashtable (spatial extremes,
max-depth, voxel bins) and accumulate aggregates, then exports the
winning contacts via the writer function.
"""
self.contact_reduction.reduce(
shape_material_k_hydro=self.shape_material_kh,
shape_transform=shape_transform,
shape_collision_aabb_lower=shape_collision_aabb_lower,
shape_collision_aabb_upper=shape_collision_aabb_upper,
shape_voxel_resolution=shape_voxel_resolution,
grid_size=self.grid_size,
)
self.contact_reduction.export(
shape_gap=shape_gap,
shape_transform=shape_transform,
writer_data=writer_data,
grid_size=self.grid_size,
)
@wp.kernel(enable_backward=False)
def broadphase_collision_pairs_count(
shape_transform: wp.array[wp.transform],
shape_sdf_data: wp.array[TextureSDFData],
shape_pairs_sdf_sdf: wp.array[wp.vec2i],
shape_pairs_sdf_sdf_count: wp.array[wp.int32],
shape2blocks: wp.array[wp.vec2i],
# outputs
thread_num_blocks: wp.array[wp.int32],
):
tid = wp.tid()
if tid >= shape_pairs_sdf_sdf_count[0]:
return
pair = shape_pairs_sdf_sdf[tid]
shape_a = pair[0]
shape_b = pair[1]
sdf_a = shape_sdf_data[shape_a]
sdf_b = shape_sdf_data[shape_b]
half_extents_a = 0.5 * (sdf_a.sdf_box_upper - sdf_a.sdf_box_lower)
half_extents_b = 0.5 * (sdf_b.sdf_box_upper - sdf_b.sdf_box_lower)
center_offset_a = 0.5 * (sdf_a.sdf_box_lower + sdf_a.sdf_box_upper)
center_offset_b = 0.5 * (sdf_b.sdf_box_lower + sdf_b.sdf_box_upper)
does_collide = wp.bool(False)
world_transform_a = shape_transform[shape_a]
world_transform_b = shape_transform[shape_b]
# Apply center offset to transforms (since SAT assumes centered boxes)
centered_transform_a = wp.transform_multiply(world_transform_a, wp.transform(center_offset_a, wp.quat_identity()))
centered_transform_b = wp.transform_multiply(world_transform_b, wp.transform(center_offset_b, wp.quat_identity()))
does_collide = sat_box_intersection(centered_transform_a, half_extents_a, centered_transform_b, half_extents_b)
# Sort shapes so shape with smaller voxel size is shape_b (must match scatter kernel)
voxel_radius_a = shape_sdf_data[shape_a].voxel_radius
voxel_radius_b = shape_sdf_data[shape_b].voxel_radius
if voxel_radius_b > voxel_radius_a:
shape_b, shape_a = shape_a, shape_b
shape_b_idx = shape2blocks[shape_b]
block_start, block_end = shape_b_idx[0], shape_b_idx[1]
num_blocks = block_end - block_start
if does_collide:
thread_num_blocks[tid] = num_blocks
else:
thread_num_blocks[tid] = 0
@wp.kernel(enable_backward=False)
def broadphase_collision_pairs_scatter(
thread_num_blocks: wp.array[wp.int32],
shape_sdf_data: wp.array[TextureSDFData],
block_start_prefix: wp.array[wp.int32],
shape_pairs_sdf_sdf: wp.array[wp.vec2i],
shape_pairs_sdf_sdf_count: wp.array[wp.int32],
shape2blocks: wp.array[wp.vec2i],
max_num_blocks_broad: int,
# outputs
block_broad_collide_shape_pair: wp.array[wp.vec2i],
block_broad_idx: wp.array[wp.int32],
):
tid = wp.tid()
if tid >= shape_pairs_sdf_sdf_count[0]:
return
num_blocks = thread_num_blocks[tid]
if num_blocks == 0:
return
pair = shape_pairs_sdf_sdf[tid]
shape_a = pair[0]
shape_b = pair[1]
# sort shapes such that the shape with the smaller voxel size is in second place
# NOTE: Confirm that this is OK to do for downstream code
voxel_radius_a = shape_sdf_data[shape_a].voxel_radius
voxel_radius_b = shape_sdf_data[shape_b].voxel_radius
if voxel_radius_b > voxel_radius_a:
shape_b, shape_a = shape_a, shape_b
shape_b_idx = shape2blocks[shape_b]
shape_b_block_start = shape_b_idx[0]
block_start = block_start_prefix[tid]
remaining = max_num_blocks_broad - block_start
if remaining <= 0:
return
num_blocks = wp.min(num_blocks, remaining)
pair = wp.vec2i(shape_a, shape_b)
for i in range(num_blocks):
block_broad_collide_shape_pair[block_start + i] = pair
block_broad_idx[block_start + i] = shape_b_block_start + i
@wp.kernel(enable_backward=False)
def broadphase_get_block_coords(
grid_size: int,
block_count: wp.array[wp.int32],
block_broad_idx: wp.array[wp.int32],
block_coords: wp.array[wp.vec3us],
max_num_blocks_broad: int,
# outputs
block_broad_collide_coords: wp.array[wp.vec3us],
):
offset = wp.tid()
num_blocks = wp.min(block_count[0], max_num_blocks_broad)
for tid in range(offset, num_blocks, grid_size):
block_idx = block_broad_idx[tid]
block_broad_collide_coords[tid] = block_coords[block_idx]
@wp.func
def encode_coords_8(x: wp.int32, y: wp.int32, z: wp.int32) -> wp.uint8:
# Encode 3D coordinates in range [0, 1] per axis into a single 8-bit integer
return wp.uint8(1) << (wp.uint8(x) + wp.uint8(y) * wp.uint8(2) + wp.uint8(z) * wp.uint8(4))
@wp.func
def decode_coords_8(bit_pos: wp.uint8) -> wp.vec3ub:
# Decode bit position back to 3D coordinates
return wp.vec3ub(
bit_pos & wp.uint8(1), (bit_pos >> wp.uint8(1)) & wp.uint8(1), (bit_pos >> wp.uint8(2)) & wp.uint8(1)
)
@wp.func
def get_rel_stiffness(k_a: wp.float32, k_b: wp.float32) -> tuple[wp.float32, wp.float32]:
k_m_inv = 1.0 / wp.sqrt(k_a * k_b)
return k_a * k_m_inv, k_b * k_m_inv
@wp.func
def sdf_diff_sdf(
sdfA_data: TextureSDFData,
sdfB_data: TextureSDFData,
transfA: wp.transform,
transfB: wp.transform,
k_eff_a: wp.float32,
k_eff_b: wp.float32,
x_id: wp.int32,
y_id: wp.int32,
z_id: wp.int32,
) -> tuple[wp.float32, wp.float32, wp.float32, wp.bool]:
"""Compute signed distance difference between two SDFs at a voxel position.
SDF A is queried via texture SDF using voxel coordinates converted to local space.
SDF B is queried via texture SDF after transforming through world space.
"""
local_pos_a = sdfA_data.sdf_box_lower + wp.cw_mul(
wp.vec3(float(x_id), float(y_id), float(z_id)), sdfA_data.voxel_size
)
pointA_world = wp.transform_point(transfA, local_pos_a)
pointB = wp.transform_point(wp.transform_inverse(transfB), pointA_world)
valA = texture_sample_sdf_at_voxel(sdfA_data, x_id, y_id, z_id)
valB = texture_sample_sdf(sdfB_data, pointB)
is_valid = not (wp.isnan(valA) or wp.isnan(valB))
if valA < 0 and valB < 0:
diff = k_eff_a * valA - k_eff_b * valB
else:
diff = valA - valB
return diff, valA, valB, is_valid
@wp.func
def sdf_diff_sdf(
sdfA_data: TextureSDFData,
sdfB_data: TextureSDFData,
transfA: wp.transform,
transfB: wp.transform,
k_eff_a: wp.float32,
k_eff_b: wp.float32,
pos_a_local: wp.vec3,
) -> tuple[wp.float32, wp.float32, wp.float32, wp.bool]:
"""Compute signed distance difference between two SDFs at a local position.
SDF A is queried via texture SDF using fractional voxel coordinates converted to local space.
SDF B is queried via texture SDF after transforming through world space.
"""
local_pos_a = sdfA_data.sdf_box_lower + wp.cw_mul(pos_a_local, sdfA_data.voxel_size)
pointA_world = wp.transform_point(transfA, local_pos_a)
pointB = wp.transform_point(wp.transform_inverse(transfB), pointA_world)
valA = texture_sample_sdf(sdfA_data, local_pos_a)
valB = texture_sample_sdf(sdfB_data, pointB)
is_valid = not (wp.isnan(valA) or wp.isnan(valB))
if valA < 0 and valB < 0:
diff = k_eff_a * valA - k_eff_b * valB
else:
diff = valA - valB
return diff, valA, valB, is_valid
@wp.kernel(enable_backward=False)
def count_iso_voxels_block(
grid_size: int,
in_buffer_collide_count: wp.array[int],
shape_sdf_data: wp.array[TextureSDFData],
shape_transform: wp.array[wp.transform],
shape_material_kh: wp.array[float],
in_buffer_collide_coords: wp.array[wp.vec3us],
in_buffer_collide_shape_pair: wp.array[wp.vec2i],
shape_gap: wp.array[wp.float32],
subblock_size: int,
n_blocks: int,
max_input_buffer_size: int,
# outputs
iso_subblock_counts: wp.array[wp.int32],
iso_subblock_idx: wp.array[wp.uint8],
):
# checks if the isosurface between shapes a and b lies inside the subblock (iterating over subblocks of b).
# if so, write the subblock coordinates to the output.
offset = wp.tid()
num_items = wp.min(in_buffer_collide_count[0], max_input_buffer_size)
for tid in range(offset, num_items, grid_size):
pair = in_buffer_collide_shape_pair[tid]
shape_a = pair[0]
shape_b = pair[1]
sdf_data_a = shape_sdf_data[shape_a]
sdf_data_b = shape_sdf_data[shape_b]
X_ws_a = shape_transform[shape_a]
X_ws_b = shape_transform[shape_b]
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
voxel_radius = sdf_data_b.voxel_radius
r = float(subblock_size) * voxel_radius
k_a = shape_material_kh[shape_a]
k_b = shape_material_kh[shape_b]
k_eff_a, k_eff_b = get_rel_stiffness(k_a, k_b)
r_eff = r * (k_eff_a + k_eff_b)
# get global voxel coordinates
bc = in_buffer_collide_coords[tid]
X_b_to_a = wp.transform_multiply(wp.transform_inverse(X_ws_a), X_ws_b)
num_iso_subblocks = wp.int32(0)
subblock_idx = wp.uint8(0)
for x_local in range(n_blocks):
for y_local in range(n_blocks):
for z_local in range(n_blocks):
x_global = wp.vec3i(bc) + wp.vec3i(x_local, y_local, z_local) * subblock_size
# lookup distances at subblock center
x_center = wp.vec3f(x_global) + wp.vec3f(0.5 * float(subblock_size))
local_pos_b = sdf_data_b.sdf_box_lower + wp.cw_mul(x_center, sdf_data_b.voxel_size)
point_a = wp.transform_point(X_b_to_a, local_pos_b)
vb = texture_sample_sdf(sdf_data_b, local_pos_b)
va = texture_sample_sdf(sdf_data_a, point_a)
is_valid = not (wp.isnan(vb) or wp.isnan(va))
if vb < 0.0 and va < 0.0:
diff_val = k_eff_b * vb - k_eff_a * va
else:
diff_val = vb - va
# check if bounding sphere contains the isosurface and the distance is within contact gap
if wp.abs(diff_val) > r_eff or va > r + gap_a or vb > r + gap_b or not is_valid:
continue
num_iso_subblocks += 1
subblock_idx |= encode_coords_8(x_local, y_local, z_local)
iso_subblock_counts[tid] = num_iso_subblocks
iso_subblock_idx[tid] = subblock_idx
@wp.kernel(enable_backward=False)
def scatter_iso_subblock(
grid_size: int,
in_iso_subblock_count: wp.array[int],
in_iso_subblock_prefix: wp.array[int],
in_iso_subblock_idx: wp.array[wp.uint8],
in_iso_subblock_shape_pair: wp.array[wp.vec2i],
in_buffer_collide_coords: wp.array[wp.vec3us],
subblock_size: int,
max_input_buffer_size: int,
max_num_iso_subblocks: int,
# outputs
out_iso_subblock_coords: wp.array[wp.vec3us],
out_iso_subblock_shape_pair: wp.array[wp.vec2i],
):
offset = wp.tid()
num_items = wp.min(in_iso_subblock_count[0], max_input_buffer_size)
for tid in range(offset, num_items, grid_size):
write_idx = in_iso_subblock_prefix[tid]
subblock_idx = in_iso_subblock_idx[tid]
pair = in_iso_subblock_shape_pair[tid]
bc = in_buffer_collide_coords[tid]
if write_idx >= max_num_iso_subblocks:
continue
for i in range(8):
bit_pos = wp.uint8(i)
if (subblock_idx >> bit_pos) & wp.uint8(1) and not write_idx >= max_num_iso_subblocks:
local_coords = wp.vec3us(decode_coords_8(bit_pos))
global_coords = bc + local_coords * wp.uint16(subblock_size)
out_iso_subblock_coords[write_idx] = global_coords
out_iso_subblock_shape_pair[write_idx] = pair
write_idx += 1
@wp.func
def mc_iterate_voxel_vertices(
x_id: wp.int32,
y_id: wp.int32,
z_id: wp.int32,
corner_offsets_table: wp.array[wp.vec3ub],
sdf_data: TextureSDFData,
sdf_other_data: TextureSDFData,
X_ws: wp.transform,
X_ws_other: wp.transform,
k_eff: wp.float32,
k_eff_other: wp.float32,
gap_sum: wp.float32,
) -> tuple[wp.uint8, vec8f, vec8f, bool, bool]:
"""Iterate over the vertices of a voxel and return the cube index, corner values, and whether any vertices are inside the shape."""
cube_idx = wp.uint8(0)
any_verts_inside_gap = False
corner_vals = vec8f()
corner_sdf_vals = vec8f()
X_a_to_b = wp.transform_multiply(wp.transform_inverse(X_ws_other), X_ws)
for i in range(8):
corner_offset = wp.vec3i(corner_offsets_table[i])
x = x_id + corner_offset.x
y = y_id + corner_offset.y
z = z_id + corner_offset.z
local_pos_a = sdf_data.sdf_box_lower + wp.cw_mul(wp.vec3(float(x), float(y), float(z)), sdf_data.voxel_size)
point_b = wp.transform_point(X_a_to_b, local_pos_a)
valA = texture_sample_sdf_at_voxel(sdf_data, x, y, z)
valB = texture_sample_sdf(sdf_other_data, point_b)
is_valid = not (wp.isnan(valA) or wp.isnan(valB))
if not is_valid:
return wp.uint8(0), corner_vals, corner_sdf_vals, False, False
if valA < 0.0 and valB < 0.0:
v_diff = k_eff * valA - k_eff_other * valB
else:
v_diff = valA - valB
corner_vals[i] = v_diff
corner_sdf_vals[i] = valA
if v_diff < 0.0:
cube_idx |= wp.uint8(1) << wp.uint8(i)
if valA <= gap_sum:
any_verts_inside_gap = True
return cube_idx, corner_vals, corner_sdf_vals, any_verts_inside_gap, True
# =============================================================================
# Contact decode kernel (no reduction)
# =============================================================================
def get_decode_contacts_kernel(margin_contact_area: float = 1e-4, writer_func: Any = None):
"""Create a kernel that decodes hydroelastic contacts without reduction.
This kernel is used when reduce_contacts=False. It exports all generated
contacts directly to the writer without any spatial reduction.
Args:
margin_contact_area: Contact area used for non-penetrating contacts at the margin.
writer_func: Warp function for writing decoded contacts.
Returns:
A warp kernel that can be launched to decode all contacts.
"""
@wp.kernel(enable_backward=False)
def decode_contacts_kernel(
grid_size: int,
contact_count: wp.array[int],
shape_material_kh: wp.array[wp.float32],
shape_transform: wp.array[wp.transform],
shape_gap: wp.array[wp.float32],
position_depth: wp.array[wp.vec4],
normal: wp.array[wp.vec2], # Octahedral-encoded
shape_pairs: wp.array[wp.vec2i],
contact_area: wp.array[wp.float32],
max_num_face_contacts: int,
# outputs
writer_data: Any,
):
"""Decode all hydroelastic contacts without reduction.
Uses grid stride loop to process all contacts in the buffer.
"""
offset = wp.tid()
num_contacts = wp.min(contact_count[0], max_num_face_contacts)
# Calculate how many contacts this thread will process
my_contact_count = 0
if offset < num_contacts:
my_contact_count = (num_contacts - 1 - offset) // grid_size + 1
if my_contact_count == 0:
return
# Single atomic to reserve all slots for this thread (no rollback)
my_base_index = wp.atomic_add(writer_data.contact_count, 0, my_contact_count)
# Write contacts using reserved range
local_idx = int(0)
for tid in range(offset, num_contacts, grid_size):
output_index = my_base_index + local_idx
local_idx += 1
if output_index >= writer_data.contact_max:
continue
pair = shape_pairs[tid]
shape_a = pair[0]
shape_b = pair[1]
transform_b = shape_transform[shape_b]
pd = position_depth[tid]
pos = wp.vec3(pd[0], pd[1], pd[2])
depth = pd[3]
contact_normal = decode_oct(normal[tid])
normal_world = wp.transform_vector(transform_b, contact_normal)
pos_world = wp.transform_point(transform_b, pos)
# Sum per-shape gaps for pairwise contact detection threshold
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
gap_sum = gap_a + gap_b
k_a = shape_material_kh[shape_a]
k_b = shape_material_kh[shape_b]
k_eff = get_effective_stiffness(k_a, k_b)
area = contact_area[tid]
# Compute stiffness, use margin_contact_area for non-penetrating contacts
# Standard convention: depth < 0 = penetrating
if depth < 0.0:
c_stiffness = area * k_eff
else:
c_stiffness = wp.static(margin_contact_area) * k_eff
# Create ContactData for the writer function
# contact_distance = 2 * depth (depth is negative for penetrating)
contact_data = ContactData()
contact_data.contact_point_center = pos_world
contact_data.contact_normal_a_to_b = normal_world
contact_data.contact_distance = 2.0 * depth
contact_data.radius_eff_a = 0.0
contact_data.radius_eff_b = 0.0
contact_data.margin_a = 0.0
contact_data.margin_b = 0.0
contact_data.shape_a = shape_a
contact_data.shape_b = shape_b
contact_data.gap_sum = gap_sum
contact_data.contact_stiffness = c_stiffness
writer_func(contact_data, writer_data, output_index)
return decode_contacts_kernel
# =============================================================================
# Contact generation kernels
# =============================================================================
def get_generate_contacts_kernel(
output_vertices: bool,
pre_prune: bool = False,
):
"""Create kernel for hydroelastic contact generation.
This is a merged kernel that computes cube state and immediately writes
faces to the reducer buffer in a single pass, eliminating intermediate
storage for cube indices and corner values.
A separate ``reduce_hydroelastic_contacts_kernel`` then runs on the
buffer to populate the hashtable and select representative contacts.
When ``pre_prune`` is enabled, this kernel applies a local-first compaction
rule before writing contacts:
- keep top-K penetrating faces by area*|depth| (K=2)
- keep at most one non-penetrating fallback face (closest to penetration)
All penetrating faces always contribute to aggregate force terms (via
hashtable entries) regardless of whether they are later pruned from
buffer writes. This ensures aggregate stiffness/normal/anchor fidelity.
Args:
output_vertices: Whether to output contact surface vertices for visualization.
pre_prune: Whether to perform local-first face compaction.
Returns:
generate_contacts_kernel: Warp kernel for contact generation.
"""
@wp.kernel(enable_backward=False)
def generate_contacts_kernel(
grid_size: int,
iso_voxel_count: wp.array[wp.int32],
shape_sdf_data: wp.array[TextureSDFData],
shape_transform: wp.array[wp.transform],
shape_material_kh: wp.array[float],
iso_voxel_coords: wp.array[wp.vec3us],
iso_voxel_shape_pair: wp.array[wp.vec2i],
tri_range_table: wp.array[wp.int32],
flat_edge_verts_table: wp.array[wp.vec2ub],
corner_offsets_table: wp.array[wp.vec3ub],
shape_gap: wp.array[wp.float32],
max_num_iso_voxels: int,
reducer_data: GlobalContactReducerData,
# Unused — kept for signature compatibility with prior callers
_shape_local_aabb_lower: wp.array[wp.vec3],
_shape_local_aabb_upper: wp.array[wp.vec3],
_shape_voxel_resolution: wp.array[wp.vec3i],
# Outputs for visualization (optional)
iso_vertex_point: wp.array[wp.vec3f],
iso_vertex_depth: wp.array[wp.float32],
iso_vertex_shape_pair: wp.array[wp.vec2i],
):
"""Generate marching cubes contacts and write to GlobalContactReducer."""
offset = wp.tid()
num_voxels = wp.min(iso_voxel_count[0], max_num_iso_voxels)
for tid in range(offset, num_voxels, grid_size):
pair = iso_voxel_shape_pair[tid]
shape_a = pair[0]
shape_b = pair[1]
sdf_data_a = shape_sdf_data[shape_a]
sdf_data_b = shape_sdf_data[shape_b]
transform_a = shape_transform[shape_a]
transform_b = shape_transform[shape_b]
iso_coords = iso_voxel_coords[tid]
gap_a = shape_gap[shape_a]
gap_b = shape_gap[shape_b]
gap_sum = gap_a + gap_b
k_a = shape_material_kh[shape_a]
k_b = shape_material_kh[shape_b]
k_eff_a, k_eff_b = get_rel_stiffness(k_a, k_b)
x_id = wp.int32(iso_coords.x)
y_id = wp.int32(iso_coords.y)
z_id = wp.int32(iso_coords.z)
# Compute cube state (marching cubes lookup)
cube_idx, corner_vals, corner_sdf_vals, any_verts_inside, all_verts_valid = mc_iterate_voxel_vertices(
x_id,
y_id,
z_id,
corner_offsets_table,
sdf_data_b,
sdf_data_a,
transform_b,
transform_a,
k_eff_b,
k_eff_a,
gap_sum,
)
range_idx = wp.int32(cube_idx)
tri_range_start = tri_range_table[range_idx]
tri_range_end = tri_range_table[range_idx + 1]
num_verts = tri_range_end - tri_range_start
num_faces = num_verts // 3
if not any_verts_inside or not all_verts_valid:
num_faces = 0
if num_faces == 0:
continue
# Compute effective stiffness coefficient
k_eff = get_effective_stiffness(k_a, k_b)
X_ws_b = transform_b
# Generate faces and locally compact candidates before writing to the
# global contact buffer (reduces atomics and downstream reduction load).
best_pen0_valid = int(0)
best_pen0_score = float(-MAXVAL)
best_pen0_depth = float(0.0)
best_pen0_area = float(0.0)
best_pen0_normal = wp.vec3(0.0, 0.0, 1.0)
best_pen0_center = wp.vec3(0.0, 0.0, 0.0)
best_pen0_v0 = wp.vec3(0.0, 0.0, 0.0)
best_pen0_v1 = wp.vec3(0.0, 0.0, 0.0)
best_pen0_v2 = wp.vec3(0.0, 0.0, 0.0)
best_pen1_valid = int(0)
best_pen1_score = float(-MAXVAL)
best_pen1_depth = float(0.0)
best_pen1_area = float(0.0)
best_pen1_normal = wp.vec3(0.0, 0.0, 1.0)
best_pen1_center = wp.vec3(0.0, 0.0, 0.0)
best_pen1_v0 = wp.vec3(0.0, 0.0, 0.0)
best_pen1_v1 = wp.vec3(0.0, 0.0, 0.0)
best_pen1_v2 = wp.vec3(0.0, 0.0, 0.0)
best_nonpen_valid = int(0)
best_nonpen_depth = float(MAXVAL)
best_nonpen_area = float(0.0)
best_nonpen_normal = wp.vec3(0.0, 0.0, 1.0)
best_nonpen_center = wp.vec3(0.0, 0.0, 0.0)
best_nonpen_v0 = wp.vec3(0.0, 0.0, 0.0)
best_nonpen_v1 = wp.vec3(0.0, 0.0, 0.0)
best_nonpen_v2 = wp.vec3(0.0, 0.0, 0.0)
for fi in range(num_faces):
area, normal, face_center, pen_depth, face_verts = mc_calc_face_texture(
flat_edge_verts_table,
corner_offsets_table,
tri_range_start + 3 * fi,
corner_vals,
corner_sdf_vals,
sdf_data_b,
x_id,
y_id,
z_id,
)
# Accumulate stats per normal bin
if pen_depth < 0.0:
bin_id = get_slot(normal)
key = make_contact_key(shape_a, shape_b, bin_id)
entry_idx = hashtable_find_or_insert(key, reducer_data.ht_keys, reducer_data.ht_active_slots)
if entry_idx >= 0:
force_weight = area * (-pen_depth)
wp.atomic_add(reducer_data.agg_force, entry_idx, force_weight * normal)
wp.atomic_add(reducer_data.weighted_pos_sum, entry_idx, force_weight * face_center)
wp.atomic_add(reducer_data.weight_sum, entry_idx, force_weight)
reducer_data.entry_k_eff[entry_idx] = k_eff
else:
wp.atomic_add(reducer_data.ht_insert_failures, 0, 1)
if wp.static(not pre_prune):
contact_id = export_hydroelastic_contact_to_buffer(
shape_a,
shape_b,
face_center,
normal,
pen_depth,
area,
k_eff,
reducer_data,
)
if wp.static(output_vertices) and contact_id >= 0:
for vi in range(3):
iso_vertex_point[3 * contact_id + vi] = wp.transform_point(X_ws_b, face_verts[vi])
iso_vertex_depth[contact_id] = pen_depth
iso_vertex_shape_pair[contact_id] = pair
continue
# Local-first compaction: keep top-K penetrating faces by score.
if pen_depth < 0.0:
score = area * (-pen_depth)
if best_pen0_valid == 0 or score > best_pen0_score:
# Shift slot0 -> slot1
best_pen1_valid = best_pen0_valid
best_pen1_score = best_pen0_score
best_pen1_depth = best_pen0_depth
best_pen1_area = best_pen0_area
best_pen1_normal = best_pen0_normal
best_pen1_center = best_pen0_center
best_pen1_v0 = best_pen0_v0
best_pen1_v1 = best_pen0_v1
best_pen1_v2 = best_pen0_v2
best_pen0_valid = int(1)
best_pen0_score = score
best_pen0_depth = pen_depth
best_pen0_area = area
best_pen0_normal = normal
best_pen0_center = face_center
best_pen0_v0 = face_verts[0]
best_pen0_v1 = face_verts[1]
best_pen0_v2 = face_verts[2]
elif wp.static(PRE_PRUNE_MAX_PENETRATING > 1):
if best_pen1_valid == 0 or score > best_pen1_score:
best_pen1_valid = int(1)
best_pen1_score = score
best_pen1_depth = pen_depth
best_pen1_area = area
best_pen1_normal = normal
best_pen1_center = face_center
best_pen1_v0 = face_verts[0]
best_pen1_v1 = face_verts[1]
best_pen1_v2 = face_verts[2]
else:
# Defer non-penetrating contact and keep only the closest one.
if pen_depth < best_nonpen_depth:
best_nonpen_valid = int(1)
best_nonpen_depth = pen_depth
best_nonpen_area = area
best_nonpen_normal = normal
best_nonpen_center = face_center
best_nonpen_v0 = face_verts[0]
best_nonpen_v1 = face_verts[1]
best_nonpen_v2 = face_verts[2]
if wp.static(pre_prune):
# Batched reservation: one atomic for all kept contacts.
keep_count = int(0)
if best_pen0_valid == 1:
keep_count = keep_count + 1
if wp.static(PRE_PRUNE_MAX_PENETRATING > 1):
if best_pen1_valid == 1:
keep_count = keep_count + 1
if best_nonpen_valid == 1:
keep_count = keep_count + 1
if keep_count > 0:
base = wp.atomic_add(reducer_data.contact_count, 0, keep_count)
if base < reducer_data.capacity:
out_idx = base
if best_pen0_valid == 1 and out_idx < reducer_data.capacity:
reducer_data.position_depth[out_idx] = wp.vec4(
best_pen0_center[0], best_pen0_center[1], best_pen0_center[2], best_pen0_depth
)
reducer_data.normal[out_idx] = encode_oct(best_pen0_normal)
reducer_data.shape_pairs[out_idx] = wp.vec2i(shape_a, shape_b)
reducer_data.contact_area[out_idx] = best_pen0_area
if wp.static(output_vertices):
iso_vertex_point[3 * out_idx + 0] = wp.transform_point(X_ws_b, best_pen0_v0)
iso_vertex_point[3 * out_idx + 1] = wp.transform_point(X_ws_b, best_pen0_v1)
iso_vertex_point[3 * out_idx + 2] = wp.transform_point(X_ws_b, best_pen0_v2)
iso_vertex_depth[out_idx] = best_pen0_depth
iso_vertex_shape_pair[out_idx] = pair
out_idx = out_idx + 1
if wp.static(PRE_PRUNE_MAX_PENETRATING > 1):
if best_pen1_valid == 1 and out_idx < reducer_data.capacity:
reducer_data.position_depth[out_idx] = wp.vec4(
best_pen1_center[0], best_pen1_center[1], best_pen1_center[2], best_pen1_depth
)
reducer_data.normal[out_idx] = encode_oct(best_pen1_normal)
reducer_data.shape_pairs[out_idx] = wp.vec2i(shape_a, shape_b)
reducer_data.contact_area[out_idx] = best_pen1_area
if wp.static(output_vertices):
iso_vertex_point[3 * out_idx + 0] = wp.transform_point(X_ws_b, best_pen1_v0)
iso_vertex_point[3 * out_idx + 1] = wp.transform_point(X_ws_b, best_pen1_v1)
iso_vertex_point[3 * out_idx + 2] = wp.transform_point(X_ws_b, best_pen1_v2)
iso_vertex_depth[out_idx] = best_pen1_depth
iso_vertex_shape_pair[out_idx] = pair
out_idx = out_idx + 1
if best_nonpen_valid == 1 and out_idx < reducer_data.capacity:
reducer_data.position_depth[out_idx] = wp.vec4(
best_nonpen_center[0], best_nonpen_center[1], best_nonpen_center[2], best_nonpen_depth
)
reducer_data.normal[out_idx] = encode_oct(best_nonpen_normal)
reducer_data.shape_pairs[out_idx] = wp.vec2i(shape_a, shape_b)
reducer_data.contact_area[out_idx] = best_nonpen_area
if wp.static(output_vertices):
iso_vertex_point[3 * out_idx + 0] = wp.transform_point(X_ws_b, best_nonpen_v0)
iso_vertex_point[3 * out_idx + 1] = wp.transform_point(X_ws_b, best_nonpen_v1)
iso_vertex_point[3 * out_idx + 2] = wp.transform_point(X_ws_b, best_nonpen_v2)
iso_vertex_depth[out_idx] = best_nonpen_depth
iso_vertex_shape_pair[out_idx] = pair
return generate_contacts_kernel
# =============================================================================
# Verification kernel
# =============================================================================
@wp.kernel(enable_backward=False)
def verify_collision_step(
num_broad_collide: wp.array[int],
max_num_broad_collide: int,
num_iso_subblocks_0: wp.array[int],
max_num_iso_subblocks_0: int,
num_iso_subblocks_1: wp.array[int],
max_num_iso_subblocks_1: int,
num_iso_subblocks_2: wp.array[int],
max_num_iso_subblocks_2: int,
num_iso_voxels: wp.array[int],
max_num_iso_voxels: int,
face_contact_count: wp.array[int],
max_face_contact_count: int,
contact_count: wp.array[int],
max_contact_count: int,
ht_insert_failures: wp.array[int],
):
# Checks if any buffer overflowed in any stage of the collision pipeline.
has_overflow = False
if num_broad_collide[0] > max_num_broad_collide:
wp.printf(
" [hydroelastic] broad phase overflow: %d > %d. Increase buffer_fraction or buffer_mult_broad.\n",
num_broad_collide[0],
max_num_broad_collide,
)
has_overflow = True
if num_iso_subblocks_0[0] > max_num_iso_subblocks_0:
wp.printf(
" [hydroelastic] iso subblock L0 overflow: %d > %d. Increase buffer_fraction or buffer_mult_iso.\n",
num_iso_subblocks_0[0],
max_num_iso_subblocks_0,
)
has_overflow = True
if num_iso_subblocks_1[0] > max_num_iso_subblocks_1:
wp.printf(
" [hydroelastic] iso subblock L1 overflow: %d > %d. Increase buffer_fraction or buffer_mult_iso.\n",
num_iso_subblocks_1[0],
max_num_iso_subblocks_1,
)
has_overflow = True
if num_iso_subblocks_2[0] > max_num_iso_subblocks_2:
wp.printf(
" [hydroelastic] iso subblock L2 overflow: %d > %d. Increase buffer_fraction or buffer_mult_iso.\n",
num_iso_subblocks_2[0],
max_num_iso_subblocks_2,
)
has_overflow = True
if num_iso_voxels[0] > max_num_iso_voxels:
wp.printf(
" [hydroelastic] iso voxel overflow: %d > %d. Increase buffer_fraction or buffer_mult_iso.\n",
num_iso_voxels[0],
max_num_iso_voxels,
)
has_overflow = True
if face_contact_count[0] > max_face_contact_count:
wp.printf(
" [hydroelastic] face contact overflow: %d > %d. Increase buffer_fraction or buffer_mult_contact.\n",
face_contact_count[0],
max_face_contact_count,
)
has_overflow = True
if contact_count[0] > max_contact_count:
wp.printf(
" [hydroelastic] rigid contact output overflow: %d > %d. Increase rigid_contact_max.\n",
contact_count[0],
max_contact_count,
)
has_overflow = True
if ht_insert_failures[0] > 0:
wp.printf(
" [hydroelastic] reduction hashtable full: %d insert failures. "
"Increase rigid_contact_max and/or buffer_fraction.\n",
ht_insert_failures[0],
)
has_overflow = True
if has_overflow:
wp.printf(
"Warning: Hydroelastic buffers overflowed; some contacts may be dropped. "
"Increase HydroelasticSDF.Config.buffer_fraction and/or per-stage buffer multipliers.\n",
)
================================================
FILE: newton/_src/geometry/sdf_mc.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Marching Cubes utilities for SDF isosurface extraction.
Provides lookup tables and GPU functions for extracting triangular faces
from voxels that contain the zero-isosurface of an SDF. Used by hydroelastic
contact generation to find the contact surface between two colliding SDFs.
The marching cubes algorithm classifies each voxel by which of its 8 corners
are inside (negative SDF) vs outside (positive SDF), producing up to 5
triangles per voxel along the zero-crossing.
"""
import numpy as np
import warp as wp
from newton._src.core.types import MAXVAL
#: Corner values for a single voxel (8 corners)
vec8f = wp.types.vector(length=8, dtype=wp.float32)
def get_mc_tables(device):
"""Create marching cubes lookup tables on the specified device.
Returns:
Tuple of 5 warp arrays:
- tri_range_table: Start/end indices into triangle list per cube case (256 cases)
- tri_local_inds_table: Edge indices for each triangle vertex
- edge_to_verts_table: Corner vertex pairs for each of 12 edges
- corner_offsets_table: 3D offsets for 8 cube corners
- flat_edge_verts_table: Pre-flattened edge→vertex mapping for efficiency
"""
# 12 edges of a cube, each connecting two corner vertices
edge_to_verts = np.array(
[
[0, 1], # 0
[1, 2], # 1
[2, 3], # 2
[3, 0], # 3
[4, 5], # 4
[5, 6], # 5
[6, 7], # 6
[7, 4], # 7
[0, 4], # 8
[1, 5], # 9
[2, 6], # 10
[3, 7], # 11
]
)
tri_range_table = wp._src.marching_cubes._get_mc_case_to_tri_range_table(device)
tri_local_inds_table = wp._src.marching_cubes._get_mc_tri_local_inds_table(device)
corner_offsets_table = wp.array(wp._src.marching_cubes.mc_cube_corner_offsets, dtype=wp.vec3ub, device=device)
edge_to_verts_table = wp.array(edge_to_verts, dtype=wp.vec2ub, device=device)
# Create flattened table:
# Instead of tri_local_inds_table[i] -> edge_to_verts_table[edge_idx, 0/1],
# we directly map tri_local_inds_table[i] -> vec2i(v_from, v_to)
tri_local_inds_np = tri_local_inds_table.numpy()
flat_edge_verts = np.zeros((len(tri_local_inds_np), 2), dtype=np.uint8)
for i, edge_idx in enumerate(tri_local_inds_np):
flat_edge_verts[i, 0] = edge_to_verts[edge_idx, 0]
flat_edge_verts[i, 1] = edge_to_verts[edge_idx, 1]
flat_edge_verts_table = wp.array(flat_edge_verts, dtype=wp.vec2ub, device=device)
return (
tri_range_table,
tri_local_inds_table,
edge_to_verts_table,
corner_offsets_table,
flat_edge_verts_table,
)
@wp.func
def int_to_vec3f(x: wp.int32, y: wp.int32, z: wp.int32):
"""Convert integer voxel coordinates to float vector."""
return wp.vec3f(float(x), float(y), float(z))
@wp.func
def get_triangle_fraction(vert_depths: wp.vec3f, num_inside: wp.int32) -> wp.float32:
"""Compute the fraction of a triangle's area that lies inside the object.
Uses linear interpolation along edges to estimate where the zero-crossing
occurs, then computes the area ratio. Returns 1.0 if all vertices inside,
0.0 if all outside, or a proportional fraction for partial intersections.
"""
if num_inside == 3:
return 1.0
if num_inside == 0:
return 0.0
# Find the vertex with different inside/outside status
# With standard convention: negative depth = inside (penetrating)
idx = wp.int32(0)
if num_inside == 1:
# Find the one vertex that IS inside (negative depth)
if vert_depths[1] < 0.0:
idx = 1
elif vert_depths[2] < 0.0:
idx = 2
else: # num_inside == 2
# Find the one vertex that is NOT inside (non-negative depth)
if vert_depths[1] >= 0.0:
idx = 1
elif vert_depths[2] >= 0.0:
idx = 2
d0 = vert_depths[idx]
d1 = vert_depths[(idx + 1) % 3]
d2 = vert_depths[(idx + 2) % 3]
denom = (d0 - d1) * (d0 - d2)
eps = wp.float32(1e-8)
if wp.abs(denom) < eps:
if num_inside == 1:
return 0.0
else:
return 1.0
fraction = wp.clamp((d0 * d0) / denom, 0.0, 1.0)
if num_inside == 2:
return 1.0 - fraction
else:
return fraction
@wp.func
def mc_calc_face(
flat_edge_verts_table: wp.array[wp.vec2ub],
corner_offsets_table: wp.array[wp.vec3ub],
tri_range_start: wp.int32,
corner_vals: vec8f,
sdf_a: wp.uint64,
x_id: wp.int32,
y_id: wp.int32,
z_id: wp.int32,
isovalue: wp.float32 = 0.0,
) -> tuple[float, wp.vec3, wp.vec3, float, wp.mat33f]:
"""Extract a triangle face from a marching cubes voxel.
Interpolates vertex positions along cube edges where the SDF crosses zero,
then computes face properties for contact generation.
Returns:
Tuple of (area, normal, center, penetration_depth, vertices):
- area: Triangle area scaled by fraction inside the object
- normal: Outward-facing unit normal
- center: Triangle centroid in world space
- penetration_depth: Average SDF depth (negative = penetrating, standard convention)
- vertices: 3x3 matrix with vertex positions as rows
"""
face_verts = wp.mat33f()
vert_depths = wp.vec3f()
num_inside = wp.int32(0)
for vi in range(3):
edge_verts = wp.vec2i(flat_edge_verts_table[tri_range_start + vi])
v_idx_from = edge_verts[0]
v_idx_to = edge_verts[1]
val_0 = wp.float32(corner_vals[v_idx_from])
val_1 = wp.float32(corner_vals[v_idx_to])
p_0 = wp.vec3f(corner_offsets_table[v_idx_from])
p_1 = wp.vec3f(corner_offsets_table[v_idx_to])
val_diff = wp.float32(val_1 - val_0)
if wp.abs(val_diff) < 1e-8:
p = 0.5 * (p_0 + p_1)
else:
t = (isovalue - val_0) / val_diff
p = p_0 + t * (p_1 - p_0)
vol_idx = p + int_to_vec3f(x_id, y_id, z_id)
p_scaled = wp.volume_index_to_world(sdf_a, vol_idx)
face_verts[vi] = p_scaled
depth = wp.volume_sample_f(sdf_a, vol_idx, wp.Volume.LINEAR)
if depth >= wp.static(MAXVAL * 0.99) or wp.isnan(depth):
depth = 0.0
vert_depths[vi] = depth # Keep SDF convention: negative = inside/penetrating
if depth < 0.0:
num_inside += 1
n = wp.cross(face_verts[1] - face_verts[0], face_verts[2] - face_verts[0])
normal = wp.normalize(n)
area = wp.length(n) / 2.0
center = (face_verts[0] + face_verts[1] + face_verts[2]) / 3.0
pen_depth = (vert_depths[0] + vert_depths[1] + vert_depths[2]) / 3.0
area *= get_triangle_fraction(vert_depths, num_inside)
return area, normal, center, pen_depth, face_verts
================================================
FILE: newton/_src/geometry/sdf_texture.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Texture-based (tex3d) sparse SDF construction and sampling.
This module provides a GPU-accelerated sparse SDF implementation using 3D CUDA textures.
Construction mirrors the NanoVDB sparse-volume pattern in ``sdf_utils.py``:
1. Check subgrid occupancy by querying mesh SDF at subgrid centers
2. Build background/coarse SDF by querying mesh at subgrid corner positions
3. Populate only occupied subgrid textures by querying mesh at each texel
The format uses:
- A coarse 3D texture for background/far-field sampling
- A packed subgrid 3D texture for narrow-band high-resolution sampling
- An indirection array mapping coarse cells to subgrid blocks
Sampling uses analytical trilinear gradient computation from 8 corner texel reads,
providing exact accuracy with only 8 texture reads (vs 56 for finite differences).
"""
from __future__ import annotations
import numpy as np
import warp as wp
from .sdf_utils import get_distance_to_mesh
# Sentinel values for subgrid indirection slots.
# Plain int so wp.static() works in kernels; numpy casts on assignment.
SLOT_EMPTY = 0xFFFFFFFF # No subgrid data (empty/far-field cell)
SLOT_LINEAR = 0xFFFFFFFE # Subgrid demoted to coarse interpolation
# ============================================================================
# Texture SDF Data Structure
# ============================================================================
class QuantizationMode:
"""Quantization modes for subgrid SDF data."""
FLOAT32 = 4 # No quantization, full precision
UINT16 = 2 # 16-bit quantization
UINT8 = 1 # 8-bit quantization
@wp.struct
class TextureSDFData:
"""Sparse SDF stored in 3D CUDA textures with indirection array.
Uses a two-level structure:
- A coarse 3D texture for background/far-field sampling
- A packed subgrid 3D texture for narrow-band high-resolution sampling
- An indirection array mapping coarse cells to subgrid texture blocks
"""
# Textures and indirection
coarse_texture: wp.Texture3D
subgrid_texture: wp.Texture3D
subgrid_start_slots: wp.array3d[wp.uint32]
# Grid parameters
sdf_box_lower: wp.vec3
sdf_box_upper: wp.vec3
inv_sdf_dx: wp.vec3
subgrid_size: int
subgrid_size_f: float # float(subgrid_size) - avoids int->float conversion
subgrid_samples_f: float # float(subgrid_size + 1) - samples per subgrid dimension
fine_to_coarse: float
# Spatial metadata
voxel_size: wp.vec3
voxel_radius: wp.float32
# Quantization parameters for subgrid values
subgrids_min_sdf_value: float
subgrids_sdf_value_range: float # max - min
# Whether shape_scale was baked into the SDF
scale_baked: wp.bool
# ============================================================================
# Sparse SDF Construction Kernels
# ============================================================================
@wp.func
def _idx3d(x: int, y: int, z: int, size_x: int, size_y: int) -> int:
"""Convert 3D coordinates to linear index."""
return z * size_x * size_y + y * size_x + x
@wp.func
def _id_to_xyz(idx: int, size_x: int, size_y: int) -> wp.vec3i:
"""Convert linear index to 3D coordinates."""
z = idx // (size_x * size_y)
rem = idx - z * size_x * size_y
y = rem // size_x
x = rem - y * size_x
return wp.vec3i(x, y, z)
@wp.kernel
def _check_subgrid_occupied_kernel(
mesh: wp.uint64,
subgrid_centers: wp.array[wp.vec3],
threshold: wp.vec2f,
winding_threshold: float,
subgrid_required: wp.array[wp.int32],
):
"""Mark subgrids that overlap the narrow band by checking mesh SDF at center."""
tid = wp.tid()
sample_pos = subgrid_centers[tid]
signed_distance = get_distance_to_mesh(mesh, sample_pos, 10000.0, winding_threshold)
is_occupied = wp.bool(False)
if wp.sign(signed_distance) > 0.0:
is_occupied = signed_distance < threshold[1]
else:
is_occupied = signed_distance > threshold[0]
if is_occupied:
subgrid_required[tid] = 1
else:
subgrid_required[tid] = 0
@wp.kernel
def _check_subgrid_linearity_kernel(
mesh: wp.uint64,
background_sdf: wp.array[float],
subgrid_required: wp.array[wp.int32],
subgrid_is_linear: wp.array[wp.int32],
cells_per_subgrid: int,
min_corner: wp.vec3,
cell_size: wp.vec3,
winding_threshold: float,
num_subgrids_x: int,
num_subgrids_y: int,
bg_size_x: int,
bg_size_y: int,
bg_size_z: int,
error_threshold: float,
):
"""Demote occupied subgrids whose SDF is well-approximated by the coarse grid.
For each occupied subgrid, queries the mesh SDF at every fine-grid sample
and compares against the trilinearly interpolated coarse/background SDF.
Subgrids where the maximum absolute error is below *error_threshold* are
marked as linear (``subgrid_is_linear[tid] = 1``) and removed from
``subgrid_required`` so no subgrid texture data is allocated for them.
"""
tid = wp.tid()
if subgrid_required[tid] == 0:
return
coords = _id_to_xyz(tid, num_subgrids_x, num_subgrids_y)
block_x = coords[0]
block_y = coords[1]
block_z = coords[2]
s = 1.0 / float(cells_per_subgrid)
samples_per_dim = cells_per_subgrid + 1
max_abs_error = float(0.0)
for lz in range(samples_per_dim):
for ly in range(samples_per_dim):
for lx in range(samples_per_dim):
gx = block_x * cells_per_subgrid + lx
gy = block_y * cells_per_subgrid + ly
gz = block_z * cells_per_subgrid + lz
pos = min_corner + wp.vec3(
float(gx) * cell_size[0],
float(gy) * cell_size[1],
float(gz) * cell_size[2],
)
mesh_val = get_distance_to_mesh(mesh, pos, 10000.0, winding_threshold)
coarse_fx = float(block_x) + float(lx) * s
coarse_fy = float(block_y) + float(ly) * s
coarse_fz = float(block_z) + float(lz) * s
x0 = int(wp.floor(coarse_fx))
y0 = int(wp.floor(coarse_fy))
z0 = int(wp.floor(coarse_fz))
x0 = wp.clamp(x0, 0, bg_size_x - 2)
y0 = wp.clamp(y0, 0, bg_size_y - 2)
z0 = wp.clamp(z0, 0, bg_size_z - 2)
tx = wp.clamp(coarse_fx - float(x0), 0.0, 1.0)
ty = wp.clamp(coarse_fy - float(y0), 0.0, 1.0)
tz = wp.clamp(coarse_fz - float(z0), 0.0, 1.0)
v000 = background_sdf[_idx3d(x0, y0, z0, bg_size_x, bg_size_y)]
v100 = background_sdf[_idx3d(x0 + 1, y0, z0, bg_size_x, bg_size_y)]
v010 = background_sdf[_idx3d(x0, y0 + 1, z0, bg_size_x, bg_size_y)]
v110 = background_sdf[_idx3d(x0 + 1, y0 + 1, z0, bg_size_x, bg_size_y)]
v001 = background_sdf[_idx3d(x0, y0, z0 + 1, bg_size_x, bg_size_y)]
v101 = background_sdf[_idx3d(x0 + 1, y0, z0 + 1, bg_size_x, bg_size_y)]
v011 = background_sdf[_idx3d(x0, y0 + 1, z0 + 1, bg_size_x, bg_size_y)]
v111 = background_sdf[_idx3d(x0 + 1, y0 + 1, z0 + 1, bg_size_x, bg_size_y)]
c00 = v000 * (1.0 - tx) + v100 * tx
c10 = v010 * (1.0 - tx) + v110 * tx
c01 = v001 * (1.0 - tx) + v101 * tx
c11 = v011 * (1.0 - tx) + v111 * tx
c0 = c00 * (1.0 - ty) + c10 * ty
c1 = c01 * (1.0 - ty) + c11 * ty
coarse_val = c0 * (1.0 - tz) + c1 * tz
max_abs_error = wp.max(max_abs_error, wp.abs(mesh_val - coarse_val))
if max_abs_error < error_threshold:
subgrid_is_linear[tid] = 1
subgrid_required[tid] = 0
@wp.kernel
def _build_coarse_sdf_from_mesh_kernel(
mesh: wp.uint64,
background_sdf: wp.array[float],
min_corner: wp.vec3,
cell_size: wp.vec3,
cells_per_subgrid: int,
bg_size_x: int,
bg_size_y: int,
bg_size_z: int,
winding_threshold: float,
):
"""Populate background SDF by querying mesh at subgrid corner positions."""
tid = wp.tid()
total_bg = bg_size_x * bg_size_y * bg_size_z
if tid >= total_bg:
return
coords = _id_to_xyz(tid, bg_size_x, bg_size_y)
x_block = coords[0]
y_block = coords[1]
z_block = coords[2]
pos = min_corner + wp.vec3(
float(x_block * cells_per_subgrid) * cell_size[0],
float(y_block * cells_per_subgrid) * cell_size[1],
float(z_block * cells_per_subgrid) * cell_size[2],
)
background_sdf[tid] = get_distance_to_mesh(mesh, pos, 10000.0, winding_threshold)
@wp.kernel
def _populate_subgrid_texture_float32_kernel(
mesh: wp.uint64,
subgrid_required: wp.array[wp.int32],
subgrid_addresses: wp.array[wp.int32],
subgrid_start_slots: wp.array3d[wp.uint32],
subgrid_texture: wp.array[float],
cells_per_subgrid: int,
min_corner: wp.vec3,
cell_size: wp.vec3,
winding_threshold: float,
num_subgrids_x: int,
num_subgrids_y: int,
num_subgrids_z: int,
tex_blocks_per_dim: int,
tex_size: int,
):
"""Populate subgrid texture by querying mesh SDF (float32 version)."""
tid = wp.tid()
total_subgrids = num_subgrids_x * num_subgrids_y * num_subgrids_z
samples_per_dim = cells_per_subgrid + 1
samples_per_subgrid = samples_per_dim * samples_per_dim * samples_per_dim
subgrid_idx = tid // samples_per_subgrid
local_sample = tid - subgrid_idx * samples_per_subgrid
if subgrid_idx >= total_subgrids:
return
if subgrid_required[subgrid_idx] == 0:
return
subgrid_coords = _id_to_xyz(subgrid_idx, num_subgrids_x, num_subgrids_y)
block_x = subgrid_coords[0]
block_y = subgrid_coords[1]
block_z = subgrid_coords[2]
local_coords = _id_to_xyz(local_sample, samples_per_dim, samples_per_dim)
lx = local_coords[0]
ly = local_coords[1]
lz = local_coords[2]
gx = block_x * cells_per_subgrid + lx
gy = block_y * cells_per_subgrid + ly
gz = block_z * cells_per_subgrid + lz
pos = min_corner + wp.vec3(
float(gx) * cell_size[0],
float(gy) * cell_size[1],
float(gz) * cell_size[2],
)
sdf_val = get_distance_to_mesh(mesh, pos, 10000.0, winding_threshold)
address = subgrid_addresses[subgrid_idx]
if address < 0:
return
addr_coords = _id_to_xyz(address, tex_blocks_per_dim, tex_blocks_per_dim)
addr_x = addr_coords[0]
addr_y = addr_coords[1]
addr_z = addr_coords[2]
if local_sample == 0:
start_slot = wp.uint32(addr_x) | (wp.uint32(addr_y) << wp.uint32(10)) | (wp.uint32(addr_z) << wp.uint32(20))
subgrid_start_slots[block_x, block_y, block_z] = start_slot
tex_x = addr_x * samples_per_dim + lx
tex_y = addr_y * samples_per_dim + ly
tex_z = addr_z * samples_per_dim + lz
tex_idx = _idx3d(tex_x, tex_y, tex_z, tex_size, tex_size)
subgrid_texture[tex_idx] = sdf_val
@wp.kernel
def _populate_subgrid_texture_uint16_kernel(
mesh: wp.uint64,
subgrid_required: wp.array[wp.int32],
subgrid_addresses: wp.array[wp.int32],
subgrid_start_slots: wp.array3d[wp.uint32],
subgrid_texture: wp.array[wp.uint16],
cells_per_subgrid: int,
min_corner: wp.vec3,
cell_size: wp.vec3,
winding_threshold: float,
num_subgrids_x: int,
num_subgrids_y: int,
num_subgrids_z: int,
tex_blocks_per_dim: int,
tex_size: int,
sdf_min: float,
sdf_range_inv: float,
):
"""Populate subgrid texture by querying mesh SDF (uint16 quantized version)."""
tid = wp.tid()
total_subgrids = num_subgrids_x * num_subgrids_y * num_subgrids_z
samples_per_dim = cells_per_subgrid + 1
samples_per_subgrid = samples_per_dim * samples_per_dim * samples_per_dim
subgrid_idx = tid // samples_per_subgrid
local_sample = tid - subgrid_idx * samples_per_subgrid
if subgrid_idx >= total_subgrids:
return
if subgrid_required[subgrid_idx] == 0:
return
subgrid_coords = _id_to_xyz(subgrid_idx, num_subgrids_x, num_subgrids_y)
block_x = subgrid_coords[0]
block_y = subgrid_coords[1]
block_z = subgrid_coords[2]
local_coords = _id_to_xyz(local_sample, samples_per_dim, samples_per_dim)
lx = local_coords[0]
ly = local_coords[1]
lz = local_coords[2]
gx = block_x * cells_per_subgrid + lx
gy = block_y * cells_per_subgrid + ly
gz = block_z * cells_per_subgrid + lz
pos = min_corner + wp.vec3(
float(gx) * cell_size[0],
float(gy) * cell_size[1],
float(gz) * cell_size[2],
)
sdf_val = get_distance_to_mesh(mesh, pos, 10000.0, winding_threshold)
address = subgrid_addresses[subgrid_idx]
if address < 0:
return
addr_coords = _id_to_xyz(address, tex_blocks_per_dim, tex_blocks_per_dim)
addr_x = addr_coords[0]
addr_y = addr_coords[1]
addr_z = addr_coords[2]
if local_sample == 0:
start_slot = wp.uint32(addr_x) | (wp.uint32(addr_y) << wp.uint32(10)) | (wp.uint32(addr_z) << wp.uint32(20))
subgrid_start_slots[block_x, block_y, block_z] = start_slot
tex_x = addr_x * samples_per_dim + lx
tex_y = addr_y * samples_per_dim + ly
tex_z = addr_z * samples_per_dim + lz
v_normalized = wp.clamp((sdf_val - sdf_min) * sdf_range_inv, 0.0, 1.0)
quantized = wp.uint16(v_normalized * 65535.0)
tex_idx = _idx3d(tex_x, tex_y, tex_z, tex_size, tex_size)
subgrid_texture[tex_idx] = quantized
@wp.kernel
def _populate_subgrid_texture_uint8_kernel(
mesh: wp.uint64,
subgrid_required: wp.array[wp.int32],
subgrid_addresses: wp.array[wp.int32],
subgrid_start_slots: wp.array3d[wp.uint32],
subgrid_texture: wp.array[wp.uint8],
cells_per_subgrid: int,
min_corner: wp.vec3,
cell_size: wp.vec3,
winding_threshold: float,
num_subgrids_x: int,
num_subgrids_y: int,
num_subgrids_z: int,
tex_blocks_per_dim: int,
tex_size: int,
sdf_min: float,
sdf_range_inv: float,
):
"""Populate subgrid texture by querying mesh SDF (uint8 quantized version)."""
tid = wp.tid()
total_subgrids = num_subgrids_x * num_subgrids_y * num_subgrids_z
samples_per_dim = cells_per_subgrid + 1
samples_per_subgrid = samples_per_dim * samples_per_dim * samples_per_dim
subgrid_idx = tid // samples_per_subgrid
local_sample = tid - subgrid_idx * samples_per_subgrid
if subgrid_idx >= total_subgrids:
return
if subgrid_required[subgrid_idx] == 0:
return
subgrid_coords = _id_to_xyz(subgrid_idx, num_subgrids_x, num_subgrids_y)
block_x = subgrid_coords[0]
block_y = subgrid_coords[1]
block_z = subgrid_coords[2]
local_coords = _id_to_xyz(local_sample, samples_per_dim, samples_per_dim)
lx = local_coords[0]
ly = local_coords[1]
lz = local_coords[2]
gx = block_x * cells_per_subgrid + lx
gy = block_y * cells_per_subgrid + ly
gz = block_z * cells_per_subgrid + lz
pos = min_corner + wp.vec3(
float(gx) * cell_size[0],
float(gy) * cell_size[1],
float(gz) * cell_size[2],
)
sdf_val = get_distance_to_mesh(mesh, pos, 10000.0, winding_threshold)
address = subgrid_addresses[subgrid_idx]
if address < 0:
return
addr_coords = _id_to_xyz(address, tex_blocks_per_dim, tex_blocks_per_dim)
addr_x = addr_coords[0]
addr_y = addr_coords[1]
addr_z = addr_coords[2]
if local_sample == 0:
start_slot = wp.uint32(addr_x) | (wp.uint32(addr_y) << wp.uint32(10)) | (wp.uint32(addr_z) << wp.uint32(20))
subgrid_start_slots[block_x, block_y, block_z] = start_slot
tex_x = addr_x * samples_per_dim + lx
tex_y = addr_y * samples_per_dim + ly
tex_z = addr_z * samples_per_dim + lz
v_normalized = wp.clamp((sdf_val - sdf_min) * sdf_range_inv, 0.0, 1.0)
quantized = wp.uint8(v_normalized * 255.0)
tex_idx = _idx3d(tex_x, tex_y, tex_z, tex_size, tex_size)
subgrid_texture[tex_idx] = quantized
# ============================================================================
# Volume Sampling Kernel (for NanoVDB → texture conversion)
# ============================================================================
@wp.kernel
def _sample_volume_at_positions_kernel(
volume: wp.uint64,
positions: wp.array[wp.vec3],
out_values: wp.array[float],
):
"""Sample NanoVDB volume at world-space positions."""
tid = wp.tid()
pos = positions[tid]
idx = wp.volume_world_to_index(volume, pos)
out_values[tid] = wp.volume_sample_f(volume, idx, wp.Volume.LINEAR)
# ============================================================================
# Texture Sampling Functions (wp.func, used by collision kernels)
# ============================================================================
@wp.func
def apply_subgrid_start(start_slot: wp.uint32, local_f: wp.vec3, subgrid_samples_f: float) -> wp.vec3:
"""Apply subgrid block offset to local coordinates."""
block_x = float(start_slot & wp.uint32(0x3FF))
block_y = float((start_slot >> wp.uint32(10)) & wp.uint32(0x3FF))
block_z = float((start_slot >> wp.uint32(20)) & wp.uint32(0x3FF))
return wp.vec3(
local_f[0] + block_x * subgrid_samples_f,
local_f[1] + block_y * subgrid_samples_f,
local_f[2] + block_z * subgrid_samples_f,
)
@wp.func
def apply_subgrid_sdf_scale(raw_value: float, min_value: float, value_range: float) -> float:
"""Apply quantization scale to convert normalized [0,1] value back to SDF distance."""
return raw_value * value_range + min_value
vec8f = wp.types.vector(length=8, dtype=wp.float32)
@wp.func
def _read_cell_corners(
sdf: TextureSDFData,
f: wp.vec3,
) -> tuple[vec8f, float, float, float]:
"""Locate the fine-grid cell containing *f* and read 8 corner texel values.
Point-samples each corner at integer+0.5 coordinates (exact texel centres)
so the caller can perform full float32 trilinear interpolation, avoiding
the 8-bit fixed-point weight precision of CUDA hardware texture filtering.
Args:
sdf: texture SDF data.
f: query position in fine-grid coordinates
(``cw_mul(clamped - sdf_box_lower, inv_sdf_dx)``).
Returns:
``(corners, tx, ty, tz)`` where *corners* packs the 8 SDF values as
``[v000, v100, v010, v110, v001, v101, v011, v111]`` and
``(tx, ty, tz)`` are the fractional interpolation weights in [0, 1].
"""
coarse_x = sdf.coarse_texture.width - 1
coarse_y = sdf.coarse_texture.height - 1
coarse_z = sdf.coarse_texture.depth - 1
fine_verts_x = float(coarse_x) * sdf.subgrid_size_f
fine_verts_y = float(coarse_y) * sdf.subgrid_size_f
fine_verts_z = float(coarse_z) * sdf.subgrid_size_f
fx = wp.clamp(f[0], 0.0, fine_verts_x)
fy = wp.clamp(f[1], 0.0, fine_verts_y)
fz = wp.clamp(f[2], 0.0, fine_verts_z)
num_fine_cells_x = int(fine_verts_x)
num_fine_cells_y = int(fine_verts_y)
num_fine_cells_z = int(fine_verts_z)
ix = wp.clamp(int(wp.floor(fx)), 0, num_fine_cells_x - 1)
iy = wp.clamp(int(wp.floor(fy)), 0, num_fine_cells_y - 1)
iz = wp.clamp(int(wp.floor(fz)), 0, num_fine_cells_z - 1)
tx = fx - float(ix)
ty = fy - float(iy)
tz = fz - float(iz)
x_base = wp.clamp(int(float(ix) * sdf.fine_to_coarse), 0, coarse_x - 1)
y_base = wp.clamp(int(float(iy) * sdf.fine_to_coarse), 0, coarse_y - 1)
z_base = wp.clamp(int(float(iz) * sdf.fine_to_coarse), 0, coarse_z - 1)
start_slot = sdf.subgrid_start_slots[x_base, y_base, z_base]
v000 = float(0.0)
v100 = float(0.0)
v010 = float(0.0)
v110 = float(0.0)
v001 = float(0.0)
v101 = float(0.0)
v011 = float(0.0)
v111 = float(0.0)
if start_slot >= wp.static(SLOT_LINEAR):
cx = float(x_base)
cy = float(y_base)
cz = float(z_base)
coarse_f = wp.vec3(fx, fy, fz) * sdf.fine_to_coarse
tx = coarse_f[0] - cx
ty = coarse_f[1] - cy
tz = coarse_f[2] - cz
v000 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 0.5, cy + 0.5, cz + 0.5), dtype=float)
v100 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 1.5, cy + 0.5, cz + 0.5), dtype=float)
v010 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 0.5, cy + 1.5, cz + 0.5), dtype=float)
v110 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 1.5, cy + 1.5, cz + 0.5), dtype=float)
v001 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 0.5, cy + 0.5, cz + 1.5), dtype=float)
v101 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 1.5, cy + 0.5, cz + 1.5), dtype=float)
v011 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 0.5, cy + 1.5, cz + 1.5), dtype=float)
v111 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 1.5, cy + 1.5, cz + 1.5), dtype=float)
else:
block_x = float(start_slot & wp.uint32(0x3FF))
block_y = float((start_slot >> wp.uint32(10)) & wp.uint32(0x3FF))
block_z = float((start_slot >> wp.uint32(20)) & wp.uint32(0x3FF))
tex_ox = block_x * sdf.subgrid_samples_f
tex_oy = block_y * sdf.subgrid_samples_f
tex_oz = block_z * sdf.subgrid_samples_f
lx = float(ix) - float(x_base) * sdf.subgrid_size_f
ly = float(iy) - float(y_base) * sdf.subgrid_size_f
lz = float(iz) - float(z_base) * sdf.subgrid_size_f
ox = tex_ox + lx + 0.5
oy = tex_oy + ly + 0.5
oz = tex_oz + lz + 0.5
v000 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy, oz), dtype=float)
v100 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox + 1.0, oy, oz), dtype=float)
v010 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy + 1.0, oz), dtype=float)
v110 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox + 1.0, oy + 1.0, oz), dtype=float)
v001 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy, oz + 1.0), dtype=float)
v101 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox + 1.0, oy, oz + 1.0), dtype=float)
v011 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy + 1.0, oz + 1.0), dtype=float)
v111 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox + 1.0, oy + 1.0, oz + 1.0), dtype=float)
v000 = apply_subgrid_sdf_scale(v000, sdf.subgrids_min_sdf_value, sdf.subgrids_sdf_value_range)
v100 = apply_subgrid_sdf_scale(v100, sdf.subgrids_min_sdf_value, sdf.subgrids_sdf_value_range)
v010 = apply_subgrid_sdf_scale(v010, sdf.subgrids_min_sdf_value, sdf.subgrids_sdf_value_range)
v110 = apply_subgrid_sdf_scale(v110, sdf.subgrids_min_sdf_value, sdf.subgrids_sdf_value_range)
v001 = apply_subgrid_sdf_scale(v001, sdf.subgrids_min_sdf_value, sdf.subgrids_sdf_value_range)
v101 = apply_subgrid_sdf_scale(v101, sdf.subgrids_min_sdf_value, sdf.subgrids_sdf_value_range)
v011 = apply_subgrid_sdf_scale(v011, sdf.subgrids_min_sdf_value, sdf.subgrids_sdf_value_range)
v111 = apply_subgrid_sdf_scale(v111, sdf.subgrids_min_sdf_value, sdf.subgrids_sdf_value_range)
corners = vec8f(v000, v100, v010, v110, v001, v101, v011, v111)
return corners, tx, ty, tz
@wp.func
def _trilinear(corners: vec8f, tx: float, ty: float, tz: float) -> float:
"""Trilinear interpolation from 8 corner values and fractional weights."""
c00 = corners[0] + (corners[1] - corners[0]) * tx
c10 = corners[2] + (corners[3] - corners[2]) * tx
c01 = corners[4] + (corners[5] - corners[4]) * tx
c11 = corners[6] + (corners[7] - corners[6]) * tx
c0 = c00 + (c10 - c00) * ty
c1 = c01 + (c11 - c01) * ty
return c0 + (c1 - c0) * tz
@wp.func
def texture_sample_sdf_at_voxel(
sdf: TextureSDFData,
ix: int,
iy: int,
iz: int,
) -> float:
"""Sample SDF at an exact integer fine-grid vertex with a single texel read.
At integer grid coordinates the trilinear fractional weights are zero, so
only the corner-0 texel contributes. This replaces 8 texture reads with 1
for the common subgrid case, which is the dominant path in hydroelastic
marching-cubes corner evaluation.
For coarse (``SLOT_LINEAR``) cells the value must still be interpolated
from the coarse grid, so this falls back to :func:`texture_sample_sdf`.
Args:
sdf: texture SDF data
ix: fine-grid x index
iy: fine-grid y index
iz: fine-grid z index
Returns:
Signed distance value [m].
"""
coarse_x = sdf.coarse_texture.width - 1
coarse_y = sdf.coarse_texture.height - 1
coarse_z = sdf.coarse_texture.depth - 1
x_base = wp.clamp(int(float(ix) * sdf.fine_to_coarse), 0, coarse_x - 1)
y_base = wp.clamp(int(float(iy) * sdf.fine_to_coarse), 0, coarse_y - 1)
z_base = wp.clamp(int(float(iz) * sdf.fine_to_coarse), 0, coarse_z - 1)
start_slot = sdf.subgrid_start_slots[x_base, y_base, z_base]
if start_slot < wp.static(SLOT_LINEAR):
block_x = float(start_slot & wp.uint32(0x3FF))
block_y = float((start_slot >> wp.uint32(10)) & wp.uint32(0x3FF))
block_z = float((start_slot >> wp.uint32(20)) & wp.uint32(0x3FF))
lx = float(ix) - float(x_base) * sdf.subgrid_size_f
ly = float(iy) - float(y_base) * sdf.subgrid_size_f
lz = float(iz) - float(z_base) * sdf.subgrid_size_f
ox = block_x * sdf.subgrid_samples_f + lx + 0.5
oy = block_y * sdf.subgrid_samples_f + ly + 0.5
oz = block_z * sdf.subgrid_samples_f + lz + 0.5
raw = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy, oz), dtype=float)
return raw * sdf.subgrids_sdf_value_range + sdf.subgrids_min_sdf_value
local_pos = sdf.sdf_box_lower + wp.cw_mul(
wp.vec3(float(ix), float(iy), float(iz)),
sdf.voxel_size,
)
return texture_sample_sdf(sdf, local_pos)
@wp.func
def texture_sample_sdf(
sdf: TextureSDFData,
local_pos: wp.vec3,
) -> float:
"""Sample SDF value from texture with extrapolation for out-of-bounds points.
Uses manual float32 trilinear interpolation from 8 corner texel reads
to avoid CUDA hardware texture filtering precision issues (8-bit
fixed-point interpolation weights that cause jitter in contact forces).
Fuses cell lookup, texel reads, trilinear blend, and quantization
de-scale into a single pass for the value-only path.
Args:
sdf: texture SDF data
local_pos: query position in local SDF space [m]
Returns:
Signed distance value [m].
"""
clamped = wp.vec3(
wp.clamp(local_pos[0], sdf.sdf_box_lower[0], sdf.sdf_box_upper[0]),
wp.clamp(local_pos[1], sdf.sdf_box_lower[1], sdf.sdf_box_upper[1]),
wp.clamp(local_pos[2], sdf.sdf_box_lower[2], sdf.sdf_box_upper[2]),
)
diff_mag = wp.length(local_pos - clamped)
f = wp.cw_mul(clamped - sdf.sdf_box_lower, sdf.inv_sdf_dx)
coarse_x = sdf.coarse_texture.width - 1
coarse_y = sdf.coarse_texture.height - 1
coarse_z = sdf.coarse_texture.depth - 1
fine_verts_x = float(coarse_x) * sdf.subgrid_size_f
fine_verts_y = float(coarse_y) * sdf.subgrid_size_f
fine_verts_z = float(coarse_z) * sdf.subgrid_size_f
fx = wp.clamp(f[0], 0.0, fine_verts_x)
fy = wp.clamp(f[1], 0.0, fine_verts_y)
fz = wp.clamp(f[2], 0.0, fine_verts_z)
num_fine_cells_x = int(fine_verts_x)
num_fine_cells_y = int(fine_verts_y)
num_fine_cells_z = int(fine_verts_z)
ix = wp.clamp(int(wp.floor(fx)), 0, num_fine_cells_x - 1)
iy = wp.clamp(int(wp.floor(fy)), 0, num_fine_cells_y - 1)
iz = wp.clamp(int(wp.floor(fz)), 0, num_fine_cells_z - 1)
tx = fx - float(ix)
ty = fy - float(iy)
tz = fz - float(iz)
x_base = wp.clamp(int(float(ix) * sdf.fine_to_coarse), 0, coarse_x - 1)
y_base = wp.clamp(int(float(iy) * sdf.fine_to_coarse), 0, coarse_y - 1)
z_base = wp.clamp(int(float(iz) * sdf.fine_to_coarse), 0, coarse_z - 1)
start_slot = sdf.subgrid_start_slots[x_base, y_base, z_base]
v000 = float(0.0)
v100 = float(0.0)
v010 = float(0.0)
v110 = float(0.0)
v001 = float(0.0)
v101 = float(0.0)
v011 = float(0.0)
v111 = float(0.0)
needs_scale = False
if start_slot >= wp.static(SLOT_LINEAR):
cx = float(x_base)
cy = float(y_base)
cz = float(z_base)
coarse_f = wp.vec3(fx, fy, fz) * sdf.fine_to_coarse
tx = coarse_f[0] - cx
ty = coarse_f[1] - cy
tz = coarse_f[2] - cz
v000 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 0.5, cy + 0.5, cz + 0.5), dtype=float)
v100 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 1.5, cy + 0.5, cz + 0.5), dtype=float)
v010 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 0.5, cy + 1.5, cz + 0.5), dtype=float)
v110 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 1.5, cy + 1.5, cz + 0.5), dtype=float)
v001 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 0.5, cy + 0.5, cz + 1.5), dtype=float)
v101 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 1.5, cy + 0.5, cz + 1.5), dtype=float)
v011 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 0.5, cy + 1.5, cz + 1.5), dtype=float)
v111 = wp.texture_sample(sdf.coarse_texture, wp.vec3f(cx + 1.5, cy + 1.5, cz + 1.5), dtype=float)
else:
needs_scale = True
block_x = float(start_slot & wp.uint32(0x3FF))
block_y = float((start_slot >> wp.uint32(10)) & wp.uint32(0x3FF))
block_z = float((start_slot >> wp.uint32(20)) & wp.uint32(0x3FF))
tex_ox = block_x * sdf.subgrid_samples_f
tex_oy = block_y * sdf.subgrid_samples_f
tex_oz = block_z * sdf.subgrid_samples_f
lx = float(ix) - float(x_base) * sdf.subgrid_size_f
ly = float(iy) - float(y_base) * sdf.subgrid_size_f
lz = float(iz) - float(z_base) * sdf.subgrid_size_f
ox = tex_ox + lx + 0.5
oy = tex_oy + ly + 0.5
oz = tex_oz + lz + 0.5
v000 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy, oz), dtype=float)
v100 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox + 1.0, oy, oz), dtype=float)
v010 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy + 1.0, oz), dtype=float)
v110 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox + 1.0, oy + 1.0, oz), dtype=float)
v001 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy, oz + 1.0), dtype=float)
v101 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox + 1.0, oy, oz + 1.0), dtype=float)
v011 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox, oy + 1.0, oz + 1.0), dtype=float)
v111 = wp.texture_sample(sdf.subgrid_texture, wp.vec3f(ox + 1.0, oy + 1.0, oz + 1.0), dtype=float)
c00 = v000 + (v100 - v000) * tx
c10 = v010 + (v110 - v010) * tx
c01 = v001 + (v101 - v001) * tx
c11 = v011 + (v111 - v011) * tx
c0 = c00 + (c10 - c00) * ty
c1 = c01 + (c11 - c01) * ty
sdf_val = c0 + (c1 - c0) * tz
if needs_scale:
sdf_val = sdf_val * sdf.subgrids_sdf_value_range + sdf.subgrids_min_sdf_value
return sdf_val + diff_mag
@wp.func
def texture_sample_sdf_grad(
sdf: TextureSDFData,
local_pos: wp.vec3,
) -> tuple[float, wp.vec3]:
"""Sample SDF value and gradient using analytical trilinear from 8 corner texels.
Uses :func:`_read_cell_corners` for point-sampled texel reads and performs
both trilinear interpolation and analytical gradient computation in float32.
Args:
sdf: texture SDF data
local_pos: query position in local SDF space [m]
Returns:
Tuple of (distance [m], gradient [unitless]).
"""
clamped = wp.vec3(
wp.clamp(local_pos[0], sdf.sdf_box_lower[0], sdf.sdf_box_upper[0]),
wp.clamp(local_pos[1], sdf.sdf_box_lower[1], sdf.sdf_box_upper[1]),
wp.clamp(local_pos[2], sdf.sdf_box_lower[2], sdf.sdf_box_upper[2]),
)
diff = local_pos - clamped
diff_mag = wp.length(diff)
f = wp.cw_mul(clamped - sdf.sdf_box_lower, sdf.inv_sdf_dx)
corners, tx, ty, tz = _read_cell_corners(sdf, f)
sdf_val = _trilinear(corners, tx, ty, tz)
# Analytical gradient (partial derivatives of trilinear)
omtx = 1.0 - tx
omty = 1.0 - ty
omtz = 1.0 - tz
v000 = corners[0]
v100 = corners[1]
v010 = corners[2]
v110 = corners[3]
v001 = corners[4]
v101 = corners[5]
v011 = corners[6]
v111 = corners[7]
gx = omty * omtz * (v100 - v000) + ty * omtz * (v110 - v010) + omty * tz * (v101 - v001) + ty * tz * (v111 - v011)
gy = omtx * omtz * (v010 - v000) + tx * omtz * (v110 - v100) + omtx * tz * (v011 - v001) + tx * tz * (v111 - v101)
gz = omtx * omty * (v001 - v000) + tx * omty * (v101 - v100) + omtx * ty * (v011 - v010) + tx * ty * (v111 - v110)
grad = wp.cw_mul(wp.vec3(gx, gy, gz), sdf.inv_sdf_dx)
if diff_mag > 0.0:
sdf_val = sdf_val + diff_mag
grad = diff / diff_mag
return sdf_val, grad
# ============================================================================
# Host-side Construction Functions
# ============================================================================
def build_sparse_sdf_from_mesh(
mesh: wp.Mesh,
grid_size_x: int,
grid_size_y: int,
grid_size_z: int,
cell_size: np.ndarray,
min_corner: np.ndarray,
max_corner: np.ndarray,
subgrid_size: int = 8,
narrow_band_thickness: float = 0.1,
quantization_mode: int = QuantizationMode.UINT16,
winding_threshold: float = 0.5,
linearization_error_threshold: float | None = None,
device: str = "cuda",
) -> dict:
"""Build sparse SDF texture representation by querying mesh directly.
Mirrors the NanoVDB sparse-volume construction pattern: check subgrid
occupancy at centers, then populate only occupied subgrids.
Args:
mesh: Warp mesh with ``support_winding_number=True``.
grid_size_x: fine grid X dimension [sample].
grid_size_y: fine grid Y dimension [sample].
grid_size_z: fine grid Z dimension [sample].
cell_size: fine grid cell size per axis [m].
min_corner: lower corner of domain [m].
max_corner: upper corner of domain [m].
subgrid_size: cells per subgrid.
narrow_band_thickness: distance threshold for subgrids [m].
quantization_mode: :class:`QuantizationMode` value.
winding_threshold: winding number threshold for inside/outside.
linearization_error_threshold: maximum absolute SDF error [m] below
which an occupied subgrid is considered linear and its high-res
data is omitted. ``None`` auto-computes from domain extents,
``0.0`` disables the optimization.
device: Warp device string.
Returns:
Dictionary with all sparse SDF data.
"""
# Ceiling division ensures the subgrid grid fully covers the fine grid.
# Floor division can truncate the domain when the number of fine cells
# is not a multiple of subgrid_size, leaving narrow-band regions without
# subgrid coverage.
num_cells_x = grid_size_x - 1
num_cells_y = grid_size_y - 1
num_cells_z = grid_size_z - 1
w = (num_cells_x + subgrid_size - 1) // subgrid_size
h = (num_cells_y + subgrid_size - 1) // subgrid_size
d = (num_cells_z + subgrid_size - 1) // subgrid_size
total_subgrids = w * h * d
min_corner_wp = wp.vec3(float(min_corner[0]), float(min_corner[1]), float(min_corner[2]))
cell_size_wp = wp.vec3(float(cell_size[0]), float(cell_size[1]), float(cell_size[2]))
# Build background SDF (coarse grid) by querying mesh at subgrid corners
bg_size_x = w + 1
bg_size_y = h + 1
bg_size_z = d + 1
total_bg = bg_size_x * bg_size_y * bg_size_z
background_sdf = wp.zeros(total_bg, dtype=float, device=device)
wp.launch(
_build_coarse_sdf_from_mesh_kernel,
dim=total_bg,
inputs=[
mesh.id,
background_sdf,
min_corner_wp,
cell_size_wp,
subgrid_size,
bg_size_x,
bg_size_y,
bg_size_z,
winding_threshold,
],
device=device,
)
# Check subgrid occupancy by querying mesh SDF at subgrid centers
subgrid_centers = np.empty((total_subgrids, 3), dtype=np.float32)
for idx in range(total_subgrids):
bz = idx // (w * h)
rem = idx - bz * w * h
by = rem // w
bx = rem - by * w
subgrid_centers[idx, 0] = (bx * subgrid_size + subgrid_size * 0.5) * cell_size[0] + min_corner[0]
subgrid_centers[idx, 1] = (by * subgrid_size + subgrid_size * 0.5) * cell_size[1] + min_corner[1]
subgrid_centers[idx, 2] = (bz * subgrid_size + subgrid_size * 0.5) * cell_size[2] + min_corner[2]
subgrid_centers_gpu = wp.array(subgrid_centers, dtype=wp.vec3, device=device)
# Expand threshold by subgrid half-diagonal (same pattern as NanoVDB tiles)
half_subgrid = subgrid_size * 0.5 * cell_size
subgrid_radius = float(np.linalg.norm(half_subgrid))
threshold = wp.vec2f(-narrow_band_thickness - subgrid_radius, narrow_band_thickness + subgrid_radius)
subgrid_required = wp.zeros(total_subgrids, dtype=wp.int32, device=device)
wp.launch(
_check_subgrid_occupied_kernel,
dim=total_subgrids,
inputs=[mesh.id, subgrid_centers_gpu, threshold, winding_threshold, subgrid_required],
device=device,
)
wp.synchronize()
# Snapshot the full narrow-band occupancy before linearization demotes
# some subgrids. The hydroelastic broadphase needs block coordinates
# for ALL occupied subgrids (including linear ones) so the octree
# explores the full contact surface.
subgrid_occupied = subgrid_required.numpy().copy()
# Demote occupied subgrids whose SDF is well-approximated by the coarse
# grid (linear field). Demoted subgrids get 0xFFFFFFFE instead of a
# subgrid address, saving texture memory while preserving narrow-band
# membership information.
if linearization_error_threshold is None:
extents = max_corner - min_corner
linearization_error_threshold = float(1e-6 * np.linalg.norm(extents))
subgrid_is_linear = wp.zeros(total_subgrids, dtype=wp.int32, device=device)
if linearization_error_threshold > 0.0:
wp.launch(
_check_subgrid_linearity_kernel,
dim=total_subgrids,
inputs=[
mesh.id,
background_sdf,
subgrid_required,
subgrid_is_linear,
subgrid_size,
min_corner_wp,
cell_size_wp,
winding_threshold,
w,
h,
bg_size_x,
bg_size_y,
bg_size_z,
linearization_error_threshold,
],
device=device,
)
wp.synchronize()
# Exclusive scan to assign sequential addresses to required subgrids
subgrid_addresses = wp.zeros(total_subgrids, dtype=wp.int32, device=device)
wp._src.utils.array_scan(subgrid_required, subgrid_addresses, inclusive=False)
wp.synchronize()
required_np = subgrid_required.numpy()
num_required = int(np.sum(required_np))
# Conservative quantization bounds from narrow band range
global_sdf_min = -narrow_band_thickness - subgrid_radius
global_sdf_max = narrow_band_thickness + subgrid_radius
sdf_range = global_sdf_max - global_sdf_min
if sdf_range < 1e-10:
sdf_range = 1.0
if num_required == 0:
subgrid_start_slots = np.full((w, h, d), SLOT_EMPTY, dtype=np.uint32)
subgrid_texture_data = np.zeros((1, 1, 1), dtype=np.float32)
tex_size = 1
final_sdf_min = 0.0
final_sdf_range = 1.0
else:
cubic_root = num_required ** (1.0 / 3.0)
tex_blocks_per_dim = max(1, int(np.ceil(cubic_root)))
while tex_blocks_per_dim**3 < num_required:
tex_blocks_per_dim += 1
samples_per_dim = subgrid_size + 1
tex_size = tex_blocks_per_dim * samples_per_dim
subgrid_start_slots = np.full((w, h, d), SLOT_EMPTY, dtype=np.uint32)
subgrid_start_slots_gpu = wp.array(subgrid_start_slots, dtype=wp.uint32, device=device)
total_tex_samples = tex_size * tex_size * tex_size
samples_per_subgrid = samples_per_dim**3
total_work = total_subgrids * samples_per_subgrid
sdf_range_inv = 1.0 / sdf_range
if quantization_mode == QuantizationMode.FLOAT32:
subgrid_texture_gpu = wp.zeros(total_tex_samples, dtype=float, device=device)
wp.launch(
_populate_subgrid_texture_float32_kernel,
dim=total_work,
inputs=[
mesh.id,
subgrid_required,
subgrid_addresses,
subgrid_start_slots_gpu,
subgrid_texture_gpu,
subgrid_size,
min_corner_wp,
cell_size_wp,
winding_threshold,
w,
h,
d,
tex_blocks_per_dim,
tex_size,
],
device=device,
)
final_sdf_min = 0.0
final_sdf_range = 1.0
subgrid_texture_data = subgrid_texture_gpu.numpy().reshape((tex_size, tex_size, tex_size))
elif quantization_mode == QuantizationMode.UINT16:
subgrid_texture_gpu = wp.zeros(total_tex_samples, dtype=wp.uint16, device=device)
wp.launch(
_populate_subgrid_texture_uint16_kernel,
dim=total_work,
inputs=[
mesh.id,
subgrid_required,
subgrid_addresses,
subgrid_start_slots_gpu,
subgrid_texture_gpu,
subgrid_size,
min_corner_wp,
cell_size_wp,
winding_threshold,
w,
h,
d,
tex_blocks_per_dim,
tex_size,
global_sdf_min,
sdf_range_inv,
],
device=device,
)
final_sdf_min = global_sdf_min
final_sdf_range = sdf_range
subgrid_texture_data = subgrid_texture_gpu.numpy().reshape((tex_size, tex_size, tex_size))
elif quantization_mode == QuantizationMode.UINT8:
subgrid_texture_gpu = wp.zeros(total_tex_samples, dtype=wp.uint8, device=device)
wp.launch(
_populate_subgrid_texture_uint8_kernel,
dim=total_work,
inputs=[
mesh.id,
subgrid_required,
subgrid_addresses,
subgrid_start_slots_gpu,
subgrid_texture_gpu,
subgrid_size,
min_corner_wp,
cell_size_wp,
winding_threshold,
w,
h,
d,
tex_blocks_per_dim,
tex_size,
global_sdf_min,
sdf_range_inv,
],
device=device,
)
final_sdf_min = global_sdf_min
final_sdf_range = sdf_range
subgrid_texture_data = subgrid_texture_gpu.numpy().reshape((tex_size, tex_size, tex_size))
else:
raise ValueError(f"Unknown quantization mode: {quantization_mode}")
wp.synchronize()
subgrid_start_slots = subgrid_start_slots_gpu.numpy()
# Write SLOT_LINEAR for subgrids that overlap the narrow band but were
# demoted because their SDF is well-approximated by the coarse grid.
is_linear_np = subgrid_is_linear.numpy()
if np.any(is_linear_np):
for idx in range(total_subgrids):
if is_linear_np[idx]:
bz = idx // (w * h)
rem = idx - bz * w * h
by = rem // w
bx = rem - by * w
subgrid_start_slots[bx, by, bz] = SLOT_LINEAR
background_sdf_np = background_sdf.numpy().reshape((bg_size_z, bg_size_y, bg_size_x))
# Padded max covers the full ceiling-divided subgrid grid.
padded_max = min_corner + np.array([w, h, d], dtype=float) * subgrid_size * cell_size
return {
"coarse_sdf": background_sdf_np.astype(np.float32),
"subgrid_data": subgrid_texture_data,
"subgrid_start_slots": subgrid_start_slots,
"coarse_dims": (w, h, d),
"subgrid_tex_size": tex_size,
"num_subgrids": num_required,
"min_extents": min_corner,
"max_extents": padded_max,
"cell_size": cell_size,
"subgrid_size": subgrid_size,
"quantization_mode": quantization_mode,
"subgrids_min_sdf_value": final_sdf_min,
"subgrids_sdf_value_range": final_sdf_range,
"subgrid_required": required_np,
"subgrid_occupied": subgrid_occupied,
}
def create_sparse_sdf_textures(
sparse_data: dict,
device: str = "cuda",
) -> tuple[TextureSDFData, wp.Texture3D, wp.Texture3D]:
"""Create TextureSDFData struct with GPU textures from sparse data.
Args:
sparse_data: dictionary from :func:`build_sparse_sdf_from_mesh`.
device: Warp device string.
Returns:
Tuple of ``(texture_sdf, coarse_texture, subgrid_texture)``.
Caller must keep texture references alive to prevent GC.
"""
coarse_tex = wp.Texture3D(
sparse_data["coarse_sdf"],
filter_mode=wp.TextureFilterMode.CLOSEST,
address_mode=wp.TextureAddressMode.CLAMP,
normalized_coords=False,
device=device,
)
subgrid_tex = wp.Texture3D(
sparse_data["subgrid_data"],
filter_mode=wp.TextureFilterMode.CLOSEST,
address_mode=wp.TextureAddressMode.CLAMP,
normalized_coords=False,
device=device,
)
subgrid_slots = wp.array(sparse_data["subgrid_start_slots"], dtype=wp.uint32, device=device)
cell_size = sparse_data["cell_size"]
min_ext = sparse_data["min_extents"]
max_ext = sparse_data["max_extents"]
sdf_params = TextureSDFData()
sdf_params.coarse_texture = coarse_tex
sdf_params.subgrid_texture = subgrid_tex
sdf_params.subgrid_start_slots = subgrid_slots
sdf_params.sdf_box_lower = wp.vec3(float(min_ext[0]), float(min_ext[1]), float(min_ext[2]))
sdf_params.sdf_box_upper = wp.vec3(float(max_ext[0]), float(max_ext[1]), float(max_ext[2]))
sdf_params.inv_sdf_dx = wp.vec3(1.0 / float(cell_size[0]), 1.0 / float(cell_size[1]), 1.0 / float(cell_size[2]))
sdf_params.subgrid_size = sparse_data["subgrid_size"]
sdf_params.subgrid_size_f = float(sparse_data["subgrid_size"])
sdf_params.subgrid_samples_f = float(sparse_data["subgrid_size"] + 1)
sdf_params.fine_to_coarse = 1.0 / sparse_data["subgrid_size"]
sdf_params.voxel_size = wp.vec3(float(cell_size[0]), float(cell_size[1]), float(cell_size[2]))
sdf_params.voxel_radius = float(0.5 * np.linalg.norm(cell_size))
sdf_params.subgrids_min_sdf_value = sparse_data["subgrids_min_sdf_value"]
sdf_params.subgrids_sdf_value_range = sparse_data["subgrids_sdf_value_range"]
sdf_params.scale_baked = False
return sdf_params, coarse_tex, subgrid_tex
def create_texture_sdf_from_mesh(
mesh: wp.Mesh,
*,
margin: float = 0.05,
narrow_band_range: tuple[float, float] = (-0.1, 0.1),
max_resolution: int = 64,
subgrid_size: int = 8,
quantization_mode: int = QuantizationMode.UINT16,
winding_threshold: float = 0.5,
scale_baked: bool = False,
device: str | None = None,
) -> tuple[TextureSDFData, wp.Texture3D, wp.Texture3D, list]:
"""Create texture SDF from a Warp mesh.
This is the main entry point for texture SDF construction. It mirrors the
parameters of :func:`~newton._src.geometry.sdf_utils._compute_sdf_from_shape_impl`.
Args:
mesh: Warp mesh with ``support_winding_number=True``.
margin: extra AABB padding [m].
narrow_band_range: signed narrow-band distance range [m] as ``(inner, outer)``.
max_resolution: maximum grid dimension [voxel].
subgrid_size: cells per subgrid.
quantization_mode: :class:`QuantizationMode` value.
winding_threshold: winding number threshold for inside/outside classification.
scale_baked: whether shape scale was baked into the mesh vertices.
device: Warp device string. ``None`` uses the mesh's device.
Returns:
Tuple of ``(texture_sdf, coarse_texture, subgrid_texture, block_coords)``.
Caller must keep texture references alive to prevent GC.
``block_coords`` is a list of ``wp.vec3us`` block coordinates for
hydroelastic broadphase.
"""
if device is None:
device = str(mesh.device)
points_np = mesh.points.numpy()
mesh_min = np.min(points_np, axis=0)
mesh_max = np.max(points_np, axis=0)
min_ext = mesh_min - margin
max_ext = mesh_max + margin
# Compute grid dimensions (same math as the former build_dense_sdf)
ext = max_ext - min_ext
max_ext_scalar = np.max(ext)
if max_ext_scalar < 1e-10:
return create_empty_texture_sdf_data(), None, None, []
cell_size_scalar = max_ext_scalar / max_resolution
dims = np.ceil(ext / cell_size_scalar).astype(int) + 1
grid_x, grid_y, grid_z = int(dims[0]), int(dims[1]), int(dims[2])
cell_size = ext / (dims - 1)
narrow_band_thickness = max(abs(narrow_band_range[0]), abs(narrow_band_range[1]))
sparse_data = build_sparse_sdf_from_mesh(
mesh,
grid_x,
grid_y,
grid_z,
cell_size,
min_ext,
max_ext,
subgrid_size=subgrid_size,
narrow_band_thickness=narrow_band_thickness,
quantization_mode=quantization_mode,
winding_threshold=winding_threshold,
device=device,
)
sdf_params, coarse_tex, subgrid_tex = create_sparse_sdf_textures(sparse_data, device)
sdf_params.scale_baked = scale_baked
# Dilate the non-linear subgrid set by one ring of occupied neighbors so
# the hydroelastic broadphase explores flat box faces (which are linear
# but adjacent to non-linear edges/corners).
block_coords = block_coords_from_subgrid_required(
sparse_data["subgrid_required"],
sparse_data["coarse_dims"],
sparse_data["subgrid_size"],
subgrid_occupied=sparse_data["subgrid_occupied"],
)
return sdf_params, coarse_tex, subgrid_tex, block_coords
def create_texture_sdf_from_volume(
sparse_volume: wp.Volume,
coarse_volume: wp.Volume,
*,
min_ext: np.ndarray,
max_ext: np.ndarray,
voxel_size: np.ndarray,
narrow_band_range: tuple[float, float] = (-0.1, 0.1),
subgrid_size: int = 8,
scale_baked: bool = False,
linearization_error_threshold: float | None = None,
device: str = "cuda",
) -> tuple[TextureSDFData, wp.Texture3D, wp.Texture3D]:
"""Create texture SDF from existing NanoVDB sparse and coarse volumes.
Samples the NanoVDB volumes at each texel position to build the texture SDF.
This is used during construction for primitive shapes that already have NanoVDB
volumes but need texture SDFs for the collision pipeline.
Args:
sparse_volume: NanoVDB sparse volume with SDF values.
coarse_volume: NanoVDB coarse (background) volume with SDF values.
min_ext: lower corner of the SDF domain [m].
max_ext: upper corner of the SDF domain [m].
voxel_size: fine grid cell size per axis [m].
narrow_band_range: signed narrow-band distance range [m] as ``(inner, outer)``.
subgrid_size: cells per subgrid.
scale_baked: whether shape scale was baked into the SDF.
linearization_error_threshold: maximum absolute SDF error [m] below
which an occupied subgrid is considered linear and its high-res
data is omitted. ``None`` auto-computes from domain extents,
``0.0`` disables the optimization.
device: Warp device string.
Returns:
Tuple of ``(texture_sdf, coarse_texture, subgrid_texture)``.
Caller must keep texture references alive to prevent GC.
"""
ext = max_ext - min_ext
# Compute fine grid dimensions from extents and voxel size.
# Use ceiling division so the coarse grid fully covers the NanoVDB domain.
cells_per_axis = np.round(ext / voxel_size).astype(int)
w = int((cells_per_axis[0] + subgrid_size - 1) // subgrid_size)
h = int((cells_per_axis[1] + subgrid_size - 1) // subgrid_size)
d = int((cells_per_axis[2] + subgrid_size - 1) // subgrid_size)
total_subgrids = w * h * d
# Padded grid covers w*subgrid_size cells (+ 1 vertex) per axis.
# Keep cell_size = voxel_size so voxel indices map 1:1.
cell_size = voxel_size.copy()
padded_max = min_ext + np.array([w, h, d], dtype=float) * subgrid_size * cell_size
# Build background/coarse SDF by sampling coarse volume at subgrid corners
bg_size_x = w + 1
bg_size_y = h + 1
bg_size_z = d + 1
total_bg = bg_size_x * bg_size_y * bg_size_z
# Sample coarse grid from the coarse NanoVDB volume using a GPU kernel
bg_positions = np.zeros((total_bg, 3), dtype=np.float32)
for idx in range(total_bg):
z_block = idx // (bg_size_x * bg_size_y)
rem = idx - z_block * bg_size_x * bg_size_y
y_block = rem // bg_size_x
x_block = rem - y_block * bg_size_x
bg_positions[idx] = min_ext + np.array(
[
float(x_block * subgrid_size) * cell_size[0],
float(y_block * subgrid_size) * cell_size[1],
float(z_block * subgrid_size) * cell_size[2],
]
)
bg_positions_gpu = wp.array(bg_positions, dtype=wp.vec3, device=device)
bg_sdf_gpu = wp.zeros(total_bg, dtype=float, device=device)
wp.launch(
_sample_volume_at_positions_kernel,
dim=total_bg,
inputs=[coarse_volume.id, bg_positions_gpu, bg_sdf_gpu],
device=device,
)
# Check subgrid occupancy by sampling sparse volume at subgrid centers
narrow_band_thickness = max(abs(narrow_band_range[0]), abs(narrow_band_range[1]))
half_subgrid = subgrid_size * 0.5 * cell_size
subgrid_radius = float(np.linalg.norm(half_subgrid))
subgrid_centers = np.empty((total_subgrids, 3), dtype=np.float32)
for idx in range(total_subgrids):
bz = idx // (w * h)
rem = idx - bz * w * h
by = rem // w
bx = rem - by * w
subgrid_centers[idx, 0] = (bx * subgrid_size + subgrid_size * 0.5) * cell_size[0] + min_ext[0]
subgrid_centers[idx, 1] = (by * subgrid_size + subgrid_size * 0.5) * cell_size[1] + min_ext[1]
subgrid_centers[idx, 2] = (bz * subgrid_size + subgrid_size * 0.5) * cell_size[2] + min_ext[2]
center_positions_gpu = wp.array(subgrid_centers, dtype=wp.vec3, device=device)
center_sdf_gpu = wp.zeros(total_subgrids, dtype=float, device=device)
wp.launch(
_sample_volume_at_positions_kernel,
dim=total_subgrids,
inputs=[sparse_volume.id, center_positions_gpu, center_sdf_gpu],
device=device,
)
wp.synchronize()
center_sdf_np = center_sdf_gpu.numpy()
threshold_inner = -narrow_band_thickness - subgrid_radius
threshold_outer = narrow_band_thickness + subgrid_radius
subgrid_required = np.zeros(total_subgrids, dtype=np.int32)
for idx in range(total_subgrids):
val = center_sdf_np[idx]
if val > 0:
subgrid_required[idx] = 1 if val < threshold_outer else 0
else:
subgrid_required[idx] = 1 if val > threshold_inner else 0
# Snapshot occupancy before linearization (see GPU path comment).
subgrid_occupied = subgrid_required.copy()
# Demote occupied subgrids whose SDF is well-approximated by the coarse
# grid (linear field).
if linearization_error_threshold is None:
linearization_error_threshold = float(1e-6 * np.linalg.norm(ext))
subgrid_is_linear = np.zeros(total_subgrids, dtype=np.int32)
if linearization_error_threshold > 0.0:
bg_sdf_np = bg_sdf_gpu.numpy()
samples_per_dim_lin = subgrid_size + 1
s_inv = 1.0 / float(subgrid_size)
occupied_indices = np.nonzero(subgrid_required)[0]
if len(occupied_indices) > 0:
all_positions = []
for idx in occupied_indices:
bz = idx // (w * h)
rem = idx - bz * w * h
by = rem // w
bx = rem - by * w
for lz in range(samples_per_dim_lin):
for ly in range(samples_per_dim_lin):
for lx in range(samples_per_dim_lin):
gx = bx * subgrid_size + lx
gy = by * subgrid_size + ly
gz = bz * subgrid_size + lz
pos = min_ext + np.array(
[
float(gx) * cell_size[0],
float(gy) * cell_size[1],
float(gz) * cell_size[2],
]
)
all_positions.append(pos)
all_positions_gpu = wp.array(np.array(all_positions, dtype=np.float32), dtype=wp.vec3, device=device)
all_sdf_gpu = wp.zeros(len(all_positions), dtype=float, device=device)
wp.launch(
_sample_volume_at_positions_kernel,
dim=len(all_positions),
inputs=[sparse_volume.id, all_positions_gpu, all_sdf_gpu],
device=device,
)
wp.synchronize()
all_sdf_np = all_sdf_gpu.numpy()
samples_per_subgrid = samples_per_dim_lin**3
for i, idx in enumerate(occupied_indices):
bz_i = idx // (w * h)
rem_i = idx - bz_i * w * h
by_i = rem_i // w
bx_i = rem_i - by_i * w
max_err = 0.0
base = i * samples_per_subgrid
for lz in range(samples_per_dim_lin):
for ly in range(samples_per_dim_lin):
for lx in range(samples_per_dim_lin):
local_idx = lz * samples_per_dim_lin * samples_per_dim_lin + ly * samples_per_dim_lin + lx
vol_val = all_sdf_np[base + local_idx]
cfx = float(bx_i) + float(lx) * s_inv
cfy = float(by_i) + float(ly) * s_inv
cfz = float(bz_i) + float(lz) * s_inv
x0 = max(0, min(int(np.floor(cfx)), bg_size_x - 2))
y0 = max(0, min(int(np.floor(cfy)), bg_size_y - 2))
z0 = max(0, min(int(np.floor(cfz)), bg_size_z - 2))
tx = np.clip(cfx - float(x0), 0.0, 1.0)
ty = np.clip(cfy - float(y0), 0.0, 1.0)
tz = np.clip(cfz - float(z0), 0.0, 1.0)
def _bg(xi, yi, zi):
return float(bg_sdf_np[zi * bg_size_x * bg_size_y + yi * bg_size_x + xi])
c00 = _bg(x0, y0, z0) * (1.0 - tx) + _bg(x0 + 1, y0, z0) * tx
c10 = _bg(x0, y0 + 1, z0) * (1.0 - tx) + _bg(x0 + 1, y0 + 1, z0) * tx
c01 = _bg(x0, y0, z0 + 1) * (1.0 - tx) + _bg(x0 + 1, y0, z0 + 1) * tx
c11 = _bg(x0, y0 + 1, z0 + 1) * (1.0 - tx) + _bg(x0 + 1, y0 + 1, z0 + 1) * tx
c0 = c00 * (1.0 - ty) + c10 * ty
c1 = c01 * (1.0 - ty) + c11 * ty
coarse_val = c0 * (1.0 - tz) + c1 * tz
max_err = max(max_err, abs(vol_val - coarse_val))
if max_err < linearization_error_threshold:
subgrid_is_linear[idx] = 1
subgrid_required[idx] = 0
num_required = int(np.sum(subgrid_required))
# Conservative quantization bounds from narrow band range
global_sdf_min = threshold_inner
global_sdf_max = threshold_outer
sdf_range = global_sdf_max - global_sdf_min
if sdf_range < 1e-10:
sdf_range = 1.0
if num_required == 0:
subgrid_start_slots = np.full((w, h, d), SLOT_EMPTY, dtype=np.uint32)
subgrid_texture_data = np.zeros((1, 1, 1), dtype=np.float32)
tex_size = 1
else:
cubic_root = num_required ** (1.0 / 3.0)
tex_blocks_per_dim = max(1, int(np.ceil(cubic_root)))
while tex_blocks_per_dim**3 < num_required:
tex_blocks_per_dim += 1
samples_per_dim = subgrid_size + 1
tex_size = tex_blocks_per_dim * samples_per_dim
# Assign sequential addresses to required subgrids
subgrid_start_slots = np.full((w, h, d), SLOT_EMPTY, dtype=np.uint32)
address = 0
for idx in range(total_subgrids):
if subgrid_required[idx]:
addr_z = address // (tex_blocks_per_dim * tex_blocks_per_dim)
addr_rem = address - addr_z * tex_blocks_per_dim * tex_blocks_per_dim
addr_y = addr_rem // tex_blocks_per_dim
addr_x = addr_rem - addr_y * tex_blocks_per_dim
bz = idx // (w * h)
rem = idx - bz * w * h
by = rem // w
bx = rem - by * w
subgrid_start_slots[bx, by, bz] = int(addr_x) | (int(addr_y) << 10) | (int(addr_z) << 20)
address += 1
# Build positions array for all subgrid texels, then sample volume
total_texel_work = num_required * samples_per_dim**3
texel_positions = np.empty((total_texel_work, 3), dtype=np.float32)
texel_tex_indices = np.empty(total_texel_work, dtype=np.int32)
work_idx = 0
subgrid_texture_data = np.zeros((tex_size, tex_size, tex_size), dtype=np.float32)
for sg_idx in range(total_subgrids):
if not subgrid_required[sg_idx]:
continue
sg_z = sg_idx // (w * h)
sg_rem = sg_idx - sg_z * w * h
sg_y = sg_rem // w
sg_x = sg_rem - sg_y * w
slot = subgrid_start_slots[sg_x, sg_y, sg_z]
addr_x = int(slot & 0x3FF)
addr_y = int((slot >> 10) & 0x3FF)
addr_z = int((slot >> 20) & 0x3FF)
for lz in range(samples_per_dim):
for ly in range(samples_per_dim):
for lx in range(samples_per_dim):
gx = sg_x * subgrid_size + lx
gy = sg_y * subgrid_size + ly
gz = sg_z * subgrid_size + lz
pos = min_ext + np.array(
[
float(gx) * cell_size[0],
float(gy) * cell_size[1],
float(gz) * cell_size[2],
]
)
tex_x = addr_x * samples_per_dim + lx
tex_y = addr_y * samples_per_dim + ly
tex_z = addr_z * samples_per_dim + lz
texel_positions[work_idx] = pos
texel_tex_indices[work_idx] = tex_z * tex_size * tex_size + tex_y * tex_size + tex_x
work_idx += 1
# Sample all texel positions from the sparse volume on GPU
texel_positions_gpu = wp.array(texel_positions, dtype=wp.vec3, device=device)
texel_sdf_gpu = wp.zeros(total_texel_work, dtype=float, device=device)
wp.launch(
_sample_volume_at_positions_kernel,
dim=total_texel_work,
inputs=[sparse_volume.id, texel_positions_gpu, texel_sdf_gpu],
device=device,
)
wp.synchronize()
texel_sdf_np = texel_sdf_gpu.numpy()
# Replace background/corrupted values from sparse volume with
# coarse volume samples. The NanoVDB sparse volume uses 1e18 as
# background for unallocated tiles; linear interpolation near tile
# boundaries blends this background into texels, corrupting them.
bg_threshold = threshold_outer * 2.0
outlier_mask = (texel_sdf_np > bg_threshold) | (texel_sdf_np < -bg_threshold)
if np.any(outlier_mask):
outlier_positions = texel_positions[outlier_mask]
outlier_gpu = wp.array(outlier_positions, dtype=wp.vec3, device=device)
outlier_sdf_gpu = wp.zeros(len(outlier_positions), dtype=float, device=device)
wp.launch(
_sample_volume_at_positions_kernel,
dim=len(outlier_positions),
inputs=[coarse_volume.id, outlier_gpu, outlier_sdf_gpu],
device=device,
)
wp.synchronize()
texel_sdf_np[outlier_mask] = outlier_sdf_gpu.numpy()
flat_texture = subgrid_texture_data.ravel()
for i in range(total_texel_work):
flat_texture[texel_tex_indices[i]] = texel_sdf_np[i]
subgrid_texture_data = flat_texture.reshape((tex_size, tex_size, tex_size))
wp.synchronize()
# Write SLOT_LINEAR for subgrids that overlap the narrow band but were
# demoted because their SDF is well-approximated by the coarse grid.
if np.any(subgrid_is_linear):
for idx in range(total_subgrids):
if subgrid_is_linear[idx]:
bz = idx // (w * h)
rem = idx - bz * w * h
by = rem // w
bx = rem - by * w
subgrid_start_slots[bx, by, bz] = SLOT_LINEAR
background_sdf_np = bg_sdf_gpu.numpy().reshape((bg_size_z, bg_size_y, bg_size_x))
sparse_data = {
"coarse_sdf": background_sdf_np.astype(np.float32),
"subgrid_data": subgrid_texture_data,
"subgrid_start_slots": subgrid_start_slots,
"coarse_dims": (w, h, d),
"subgrid_tex_size": tex_size,
"num_subgrids": num_required,
"min_extents": min_ext,
"max_extents": padded_max,
"cell_size": cell_size,
"subgrid_size": subgrid_size,
"quantization_mode": QuantizationMode.FLOAT32,
"subgrids_min_sdf_value": 0.0,
"subgrids_sdf_value_range": 1.0,
"subgrid_required": subgrid_required,
"subgrid_occupied": subgrid_occupied,
}
sdf_params, coarse_tex, subgrid_tex = create_sparse_sdf_textures(sparse_data, device)
sdf_params.scale_baked = scale_baked
return sdf_params, coarse_tex, subgrid_tex
def block_coords_from_subgrid_required(
subgrid_required: np.ndarray,
coarse_dims: tuple[int, int, int],
subgrid_size: int,
subgrid_occupied: np.ndarray | None = None,
) -> list:
"""Derive SDF block coordinates from texture subgrid occupancy.
This converts the texture subgrid occupancy array into voxel-space block
coordinates compatible with the hydroelastic broadphase pipeline.
When *subgrid_occupied* is supplied (the pre-linearization narrow-band
mask), all occupied subgrids are included — matching the behavior of
NanoVDB active tiles. This ensures full contact surface coverage for
flat faces that were demoted to linear interpolation.
Args:
subgrid_required: 1D array of occupancy flags for non-linear subgrids.
coarse_dims: tuple (w, h, d) number of subgrids per axis.
subgrid_size: cells per subgrid.
subgrid_occupied: optional 1D array of full narrow-band occupancy
(before linearization). When provided, all occupied subgrids
are included in the output.
Returns:
List of ``wp.vec3us`` block coordinates for selected subgrids.
"""
w, h, _d = coarse_dims
include = subgrid_occupied if subgrid_occupied is not None else subgrid_required
coords = []
for idx in range(len(include)):
if include[idx]:
bz = idx // (w * h)
rem = idx - bz * w * h
by = rem // w
bx = rem - by * w
coords.append(wp.vec3us(bx * subgrid_size, by * subgrid_size, bz * subgrid_size))
return coords
def create_empty_texture_sdf_data() -> TextureSDFData:
"""Return an empty TextureSDFData struct for shapes without texture SDF.
An empty struct has ``coarse_texture.width == 0``, which collision kernels
use to detect the absence of a texture SDF and fall back to BVH.
Returns:
A zeroed-out :class:`TextureSDFData` struct.
"""
sdf = TextureSDFData()
sdf.subgrid_size = 0
sdf.subgrid_size_f = 0.0
sdf.subgrid_samples_f = 0.0
sdf.fine_to_coarse = 0.0
sdf.inv_sdf_dx = wp.vec3(0.0, 0.0, 0.0)
sdf.sdf_box_lower = wp.vec3(0.0, 0.0, 0.0)
sdf.sdf_box_upper = wp.vec3(0.0, 0.0, 0.0)
sdf.voxel_size = wp.vec3(0.0, 0.0, 0.0)
sdf.voxel_radius = 0.0
sdf.subgrids_min_sdf_value = 0.0
sdf.subgrids_sdf_value_range = 1.0
sdf.scale_baked = False
return sdf
# ============================================================================
# Isomesh extraction from texture SDF (marching cubes)
# ============================================================================
@wp.kernel(enable_backward=False)
def _count_isomesh_faces_texture_kernel(
sdf_array: wp.array[TextureSDFData],
active_coarse_cells: wp.array[wp.vec3i],
subgrid_size: int,
tri_range_table: wp.array[wp.int32],
corner_offsets_table: wp.array[wp.vec3ub],
isovalue: wp.float32,
face_count: wp.array[int],
):
cell_idx, local_x, local_y, local_z = wp.tid()
sdf = sdf_array[0]
coarse = active_coarse_cells[cell_idx]
x_id = coarse[0] * subgrid_size + local_x
y_id = coarse[1] * subgrid_size + local_y
z_id = coarse[2] * subgrid_size + local_z
cube_idx = wp.int32(0)
for i in range(8):
co = wp.vec3i(corner_offsets_table[i])
v = texture_sample_sdf_at_voxel(sdf, x_id + co.x, y_id + co.y, z_id + co.z)
if wp.isnan(v):
return
if v < isovalue:
cube_idx |= 1 << i
tri_start = tri_range_table[cube_idx]
tri_end = tri_range_table[cube_idx + 1]
num_faces = (tri_end - tri_start) // 3
if num_faces > 0:
wp.atomic_add(face_count, 0, num_faces)
@wp.kernel(enable_backward=False)
def _generate_isomesh_texture_kernel(
sdf_array: wp.array[TextureSDFData],
active_coarse_cells: wp.array[wp.vec3i],
subgrid_size: int,
tri_range_table: wp.array[wp.int32],
flat_edge_verts_table: wp.array[wp.vec2ub],
corner_offsets_table: wp.array[wp.vec3ub],
isovalue: wp.float32,
face_count: wp.array[int],
vertices: wp.array[wp.vec3],
):
cell_idx, local_x, local_y, local_z = wp.tid()
sdf = sdf_array[0]
coarse = active_coarse_cells[cell_idx]
x_id = coarse[0] * subgrid_size + local_x
y_id = coarse[1] * subgrid_size + local_y
z_id = coarse[2] * subgrid_size + local_z
cube_idx = wp.int32(0)
corner_vals = vec8f()
for i in range(8):
co = wp.vec3i(corner_offsets_table[i])
v = texture_sample_sdf_at_voxel(sdf, x_id + co.x, y_id + co.y, z_id + co.z)
if wp.isnan(v):
return
corner_vals[i] = v
if v < isovalue:
cube_idx |= 1 << i
tri_start = tri_range_table[cube_idx]
tri_end = tri_range_table[cube_idx + 1]
num_verts = tri_end - tri_start
num_faces = num_verts // 3
if num_faces == 0:
return
out_idx = wp.atomic_add(face_count, 0, num_faces)
for fi in range(5):
if fi >= num_faces:
return
for vi in range(3):
edge_verts = wp.vec2i(flat_edge_verts_table[tri_start + 3 * fi + vi])
v_from = edge_verts[0]
v_to = edge_verts[1]
val_0 = wp.float32(corner_vals[v_from])
val_1 = wp.float32(corner_vals[v_to])
p_0 = wp.vec3f(corner_offsets_table[v_from])
p_1 = wp.vec3f(corner_offsets_table[v_to])
val_diff = val_1 - val_0
if wp.abs(val_diff) < 1e-8:
p = 0.5 * (p_0 + p_1)
else:
t = (isovalue - val_0) / val_diff
p = p_0 + t * (p_1 - p_0)
vol_idx = p + wp.vec3(float(x_id), float(y_id), float(z_id))
local_pos = sdf.sdf_box_lower + wp.cw_mul(vol_idx, sdf.voxel_size)
vertices[3 * out_idx + 3 * fi + vi] = local_pos
def compute_isomesh_from_texture_sdf(
tex_data_array: wp.array,
sdf_idx: int,
subgrid_start_slots: wp.array,
coarse_dims: tuple[int, int, int],
device=None,
isovalue: float = 0.0,
) -> Mesh | None:
"""Extract an isosurface mesh from a texture SDF via marching cubes.
Iterates over coarse cells that have subgrids (the narrow-band region
where the surface lives) and runs marching cubes on their fine voxels.
Args:
tex_data_array: Warp array of :class:`TextureSDFData` structs.
sdf_idx: Index into *tex_data_array* to extract.
subgrid_start_slots: The 3D ``subgrid_start_slots`` array for this SDF
entry (used to determine which coarse cells are active).
coarse_dims: ``(cx, cy, cz)`` number of coarse cells per axis.
device: Warp device.
isovalue: Surface level to extract [m]. ``0.0`` gives the
zero-isosurface; positive values extract an outward offset surface.
Returns:
:class:`~newton.Mesh` with the isosurface, or ``None`` if empty.
"""
from .sdf_mc import get_mc_tables # noqa: PLC0415
from .types import Mesh # noqa: PLC0415
if device is None:
device = wp.get_device()
if subgrid_start_slots is None:
return None
tex_np = tex_data_array.numpy()
entry = tex_np[sdf_idx]
subgrid_size = int(entry["subgrid_size"])
if subgrid_size == 0:
return None
cx, cy, cz = coarse_dims
single = tex_data_array[sdf_idx : sdf_idx + 1]
slots_np = subgrid_start_slots.numpy()
active_cells = []
for ix in range(cx):
for iy in range(cy):
for iz in range(cz):
if slots_np[ix, iy, iz] != SLOT_EMPTY:
active_cells.append((ix, iy, iz))
if not active_cells:
return None
active_coarse_cells = wp.array(active_cells, dtype=wp.vec3i, device=device)
num_active = len(active_cells)
mc_tables = get_mc_tables(device)
tri_range_table = mc_tables[0]
flat_edge_verts_table = mc_tables[4]
corner_offsets_table = mc_tables[3]
face_count = wp.zeros((1,), dtype=int, device=device)
wp.launch(
_count_isomesh_faces_texture_kernel,
dim=(num_active, subgrid_size, subgrid_size, subgrid_size),
inputs=[single, active_coarse_cells, subgrid_size, tri_range_table, corner_offsets_table, float(isovalue)],
outputs=[face_count],
device=device,
)
num_faces = int(face_count.numpy()[0])
if num_faces == 0:
return None
max_verts = 3 * num_faces
verts = wp.empty((max_verts,), dtype=wp.vec3, device=device)
face_count.zero_()
wp.launch(
_generate_isomesh_texture_kernel,
dim=(num_active, subgrid_size, subgrid_size, subgrid_size),
inputs=[
single,
active_coarse_cells,
subgrid_size,
tri_range_table,
flat_edge_verts_table,
corner_offsets_table,
float(isovalue),
],
outputs=[face_count, verts],
device=device,
)
verts_np = verts.numpy()
faces_np = np.arange(3 * num_faces).reshape(-1, 3)
faces_np = faces_np[:, ::-1]
return Mesh(verts_np, faces_np)
================================================
FILE: newton/_src/geometry/sdf_utils.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import logging
from collections.abc import Sequence
from typing import TYPE_CHECKING
import numpy as np
import warp as wp
from ..core.types import MAXVAL, Axis, Devicelike
from .kernels import sdf_box, sdf_capsule, sdf_cone, sdf_cylinder, sdf_ellipsoid, sdf_sphere
from .sdf_mc import get_mc_tables, int_to_vec3f, mc_calc_face, vec8f
from .types import GeoType, Mesh
logger = logging.getLogger(__name__)
if TYPE_CHECKING:
from .sdf_texture import TextureSDFData
@wp.struct
class SDFData:
"""Encapsulates all data needed for SDF-based collision detection.
Contains both sparse (narrow band) and coarse (background) SDF volumes
with the same spatial extents but different resolutions.
"""
# Sparse (narrow band) SDF - high resolution near surface
sparse_sdf_ptr: wp.uint64
sparse_voxel_size: wp.vec3
sparse_voxel_radius: wp.float32
# Coarse (background) SDF - 8x8x8 covering entire volume
coarse_sdf_ptr: wp.uint64
coarse_voxel_size: wp.vec3
# Shared extents (same for both volumes)
center: wp.vec3
half_extents: wp.vec3
# Background value used for unallocated voxels in the sparse SDF
background_value: wp.float32
# Whether shape_scale was baked into the SDF
scale_baked: wp.bool
@wp.func
def sample_sdf_extrapolated(
sdf_data: SDFData,
sdf_pos: wp.vec3,
) -> float:
"""Sample NanoVDB SDF with extrapolation for points outside the narrow band or extent.
Handles three cases:
1. Point in narrow band: returns sparse grid value directly.
2. Point inside extent but outside narrow band: returns coarse grid value.
3. Point outside extent: projects to boundary, returns value at boundary + distance to boundary.
Args:
sdf_data: SDFData struct containing sparse/coarse volumes and extent info.
sdf_pos: Query position in the SDF's local coordinate space [m].
Returns:
The signed distance value [m], extrapolated if necessary.
"""
lower = sdf_data.center - sdf_data.half_extents
upper = sdf_data.center + sdf_data.half_extents
inside_extent = (
sdf_pos[0] >= lower[0]
and sdf_pos[0] <= upper[0]
and sdf_pos[1] >= lower[1]
and sdf_pos[1] <= upper[1]
and sdf_pos[2] >= lower[2]
and sdf_pos[2] <= upper[2]
)
if inside_extent:
sparse_idx = wp.volume_world_to_index(sdf_data.sparse_sdf_ptr, sdf_pos)
sparse_dist = wp.volume_sample_f(sdf_data.sparse_sdf_ptr, sparse_idx, wp.Volume.LINEAR)
if sparse_dist >= sdf_data.background_value * 0.99 or wp.isnan(sparse_dist):
coarse_idx = wp.volume_world_to_index(sdf_data.coarse_sdf_ptr, sdf_pos)
return wp.volume_sample_f(sdf_data.coarse_sdf_ptr, coarse_idx, wp.Volume.LINEAR)
else:
return sparse_dist
else:
eps = 1e-2 * sdf_data.sparse_voxel_size
clamped_pos = wp.min(wp.max(sdf_pos, lower + eps), upper - eps)
dist_to_boundary = wp.length(sdf_pos - clamped_pos)
coarse_idx = wp.volume_world_to_index(sdf_data.coarse_sdf_ptr, clamped_pos)
boundary_dist = wp.volume_sample_f(sdf_data.coarse_sdf_ptr, coarse_idx, wp.Volume.LINEAR)
return boundary_dist + dist_to_boundary
@wp.func
def sample_sdf_grad_extrapolated(
sdf_data: SDFData,
sdf_pos: wp.vec3,
) -> tuple[float, wp.vec3]:
"""Sample NanoVDB SDF with gradient, with extrapolation for points outside narrow band or extent.
Handles three cases:
1. Point in narrow band: returns sparse grid value and gradient directly.
2. Point inside extent but outside narrow band: returns coarse grid value and gradient.
3. Point outside extent: returns extrapolated distance and direction toward boundary.
Args:
sdf_data: SDFData struct containing sparse/coarse volumes and extent info.
sdf_pos: Query position in the SDF's local coordinate space [m].
Returns:
Tuple of (distance [m], gradient [unitless]) where gradient points toward increasing distance.
"""
lower = sdf_data.center - sdf_data.half_extents
upper = sdf_data.center + sdf_data.half_extents
gradient = wp.vec3(0.0, 0.0, 0.0)
inside_extent = (
sdf_pos[0] >= lower[0]
and sdf_pos[0] <= upper[0]
and sdf_pos[1] >= lower[1]
and sdf_pos[1] <= upper[1]
and sdf_pos[2] >= lower[2]
and sdf_pos[2] <= upper[2]
)
if inside_extent:
sparse_idx = wp.volume_world_to_index(sdf_data.sparse_sdf_ptr, sdf_pos)
sparse_dist = wp.volume_sample_grad_f(sdf_data.sparse_sdf_ptr, sparse_idx, wp.Volume.LINEAR, gradient)
if sparse_dist >= sdf_data.background_value * 0.99 or wp.isnan(sparse_dist):
coarse_idx = wp.volume_world_to_index(sdf_data.coarse_sdf_ptr, sdf_pos)
coarse_dist = wp.volume_sample_grad_f(sdf_data.coarse_sdf_ptr, coarse_idx, wp.Volume.LINEAR, gradient)
return coarse_dist, gradient
else:
return sparse_dist, gradient
else:
eps = 1e-2 * sdf_data.sparse_voxel_size
clamped_pos = wp.min(wp.max(sdf_pos, lower + eps), upper - eps)
diff = sdf_pos - clamped_pos
dist_to_boundary = wp.length(diff)
coarse_idx = wp.volume_world_to_index(sdf_data.coarse_sdf_ptr, clamped_pos)
boundary_dist = wp.volume_sample_f(sdf_data.coarse_sdf_ptr, coarse_idx, wp.Volume.LINEAR)
extrapolated_dist = boundary_dist + dist_to_boundary
if dist_to_boundary > 0.0:
gradient = diff / dist_to_boundary
else:
wp.volume_sample_grad_f(sdf_data.coarse_sdf_ptr, coarse_idx, wp.Volume.LINEAR, gradient)
return extrapolated_dist, gradient
class SDF:
"""Opaque SDF container owning kernel payload and runtime references."""
def __init__(
self,
*,
data: SDFData,
sparse_volume: wp.Volume | None = None,
coarse_volume: wp.Volume | None = None,
block_coords: np.ndarray | Sequence[wp.vec3us] | None = None,
texture_block_coords: Sequence[wp.vec3us] | None = None,
texture_data: "TextureSDFData | None" = None,
shape_margin: float = 0.0,
_coarse_texture: wp.Texture3D | None = None,
_subgrid_texture: wp.Texture3D | None = None,
_internal: bool = False,
):
if not _internal:
raise RuntimeError(
"SDF objects are created via mesh.build_sdf(), SDF.create_from_mesh(), or SDF.create_from_data()."
)
self.data = data
self.sparse_volume = sparse_volume
self.coarse_volume = coarse_volume
self.block_coords = block_coords
self.texture_block_coords = texture_block_coords
self.texture_data = texture_data
self.shape_margin = shape_margin
# Keep texture references alive to prevent GC
self._coarse_texture = _coarse_texture
self._subgrid_texture = _subgrid_texture
def to_kernel_data(self) -> SDFData:
"""Return kernel-facing SDF payload."""
return self.data
def to_texture_kernel_data(self) -> "TextureSDFData | None":
"""Return texture SDF kernel payload, or ``None`` if unavailable."""
return self.texture_data
def is_empty(self) -> bool:
"""Return True when this SDF has no sparse/coarse payload."""
return int(self.data.sparse_sdf_ptr) == 0 and int(self.data.coarse_sdf_ptr) == 0
def validate(self) -> None:
"""Validate consistency of kernel pointers and owned volumes."""
if int(self.data.sparse_sdf_ptr) == 0 and self.sparse_volume is not None:
raise ValueError("SDFData sparse pointer is empty but sparse_volume is set.")
if int(self.data.coarse_sdf_ptr) == 0 and self.coarse_volume is not None:
raise ValueError("SDFData coarse pointer is empty but coarse_volume is set.")
def extract_isomesh(self, isovalue: float = 0.0, device: "Devicelike | None" = None) -> "Mesh | None":
"""Extract an isosurface mesh at the requested isovalue.
Prefers the texture SDF path when available (avoids NanoVDB volume
indirection); falls back to the NanoVDB sparse volume.
Args:
isovalue: Surface level to extract [m]. ``0.0`` gives the
original surface; positive values give an outward offset.
device: CUDA device. When ``None`` uses the current device.
Returns:
:class:`Mesh` or ``None`` when the SDF has no data or the
isovalue falls outside the stored narrow band.
"""
if self.texture_data is not None and self._coarse_texture is not None:
from .sdf_texture import TextureSDFData, compute_isomesh_from_texture_sdf # noqa: PLC0415
with wp.ScopedDevice(device):
tex_arr = wp.array([self.texture_data], dtype=TextureSDFData, device=device)
ct = self._coarse_texture
coarse_dims = (ct.width - 1, ct.height - 1, ct.depth - 1)
slots = self.texture_data.subgrid_start_slots
result = compute_isomesh_from_texture_sdf(
tex_arr, 0, slots, coarse_dims, device=device, isovalue=isovalue
)
if result is not None:
return result
if self.sparse_volume is not None:
# The sparse volume has shape_margin already baked in (subtracted
# from every SDF value), so its zero-isosurface sits at
# shape_margin from the base geometry. Compensate so callers get
# the surface at the requested isovalue from the base geometry.
corrected_isovalue = isovalue - self.shape_margin if self.shape_margin else isovalue
return compute_isomesh(self.sparse_volume, isovalue=corrected_isovalue, device=device)
return None
def __copy__(self) -> "SDF":
"""Return self; SDF runtime handles are immutable and shared."""
return self
def __deepcopy__(self, memo: dict[int, object]) -> "SDF":
"""Keep deep-copy stable by reusing this instance.
`wp.Volume` instances inside SDF are ctypes-backed and not picklable.
Treating SDF as an immutable runtime handle keeps builder deepcopy usable.
"""
memo[id(self)] = self
return self
@staticmethod
def create_from_points(
points: np.ndarray | Sequence[Sequence[float]],
indices: np.ndarray | Sequence[int],
*,
device: Devicelike | None = None,
narrow_band_range: tuple[float, float] = (-0.1, 0.1),
target_voxel_size: float | None = None,
max_resolution: int | None = None,
margin: float = 0.05,
shape_margin: float = 0.0,
scale: tuple[float, float, float] | None = None,
) -> "SDF":
"""Create an SDF from triangle mesh points and indices.
Args:
points: Vertex positions [m], shape ``(N, 3)``.
indices: Triangle vertex indices [index], flattened or shape ``(M, 3)``.
device: CUDA device for SDF allocation. When ``None``, uses the
current :class:`wp.ScopedDevice` or the Warp default device.
narrow_band_range: Signed narrow-band distance range [m] as ``(inner, outer)``.
target_voxel_size: Target sparse-grid voxel size [m]. If provided, takes
precedence over ``max_resolution``.
max_resolution: Maximum sparse-grid dimension [voxel]. Used when
``target_voxel_size`` is not provided.
margin: Extra AABB padding [m] added before discretization.
shape_margin: Shape margin offset [m] to subtract from SDF values.
scale: Scale factors ``(sx, sy, sz)`` to bake into the SDF.
Returns:
A validated :class:`SDF` runtime handle with sparse/coarse volumes.
"""
mesh = Mesh(points, indices, compute_inertia=False)
return SDF.create_from_mesh(
mesh,
device=device,
narrow_band_range=narrow_band_range,
target_voxel_size=target_voxel_size,
max_resolution=max_resolution,
margin=margin,
shape_margin=shape_margin,
scale=scale,
)
@staticmethod
def create_from_mesh(
mesh: Mesh,
*,
device: Devicelike | None = None,
narrow_band_range: tuple[float, float] = (-0.1, 0.1),
target_voxel_size: float | None = None,
max_resolution: int | None = None,
margin: float = 0.05,
shape_margin: float = 0.0,
scale: tuple[float, float, float] | None = None,
texture_format: str = "uint16",
) -> "SDF":
"""Create an SDF from a mesh in local mesh coordinates.
Args:
mesh: Source mesh geometry.
device: CUDA device for SDF allocation. When ``None``, uses the
current :class:`wp.ScopedDevice` or the Warp default device.
narrow_band_range: Signed narrow-band distance range [m] as
``(inner, outer)``.
target_voxel_size: Target sparse-grid voxel size [m]. If provided,
takes precedence over ``max_resolution``.
max_resolution: Maximum sparse-grid dimension [voxel]. Used when
``target_voxel_size`` is not provided.
margin: Extra AABB padding [m] added before discretization.
shape_margin: Shape margin offset [m] to subtract from SDF values.
When non-zero, the SDF surface is effectively shrunk inward by
this amount. Useful for modeling compliant layers in hydroelastic
collision. Defaults to ``0.0``.
scale: Scale factors ``(sx, sy, sz)`` [unitless] to bake into the
SDF. When provided, mesh vertices are scaled before SDF
generation and ``scale_baked`` is set to ``True`` in the
resulting SDF. Required for hydroelastic collision with
non-unit shape scale. Defaults to ``None`` (no scale baking;
scale applied at runtime).
texture_format: Subgrid texture storage format. ``"uint16"``
(default) uses 16-bit normalized textures for half the memory
of ``"float32"`` with negligible precision loss. ``"uint8"``
uses 8-bit textures for minimum memory.
Returns:
A validated :class:`SDF` runtime handle with sparse/coarse volumes.
"""
effective_max_resolution = 64 if max_resolution is None and target_voxel_size is None else max_resolution
bake_scale = scale is not None
effective_scale = scale if scale is not None else (1.0, 1.0, 1.0)
sdf_data, sparse_volume, coarse_volume, block_coords = _compute_sdf_from_shape_impl(
shape_type=GeoType.MESH,
shape_geo=mesh,
shape_scale=effective_scale,
shape_margin=shape_margin,
narrow_band_distance=narrow_band_range,
margin=margin,
target_voxel_size=target_voxel_size,
max_resolution=effective_max_resolution if effective_max_resolution is not None else 64,
bake_scale=bake_scale,
device=device,
)
# Build texture SDF alongside NanoVDB
texture_data = None
coarse_texture = None
subgrid_texture = None
tex_block_coords = None
if wp.is_cuda_available():
from .sdf_texture import QuantizationMode, create_texture_sdf_from_mesh # noqa: PLC0415
_tex_fmt_map = {
"float32": QuantizationMode.FLOAT32,
"uint16": QuantizationMode.UINT16,
"uint8": QuantizationMode.UINT8,
}
if texture_format not in _tex_fmt_map:
raise ValueError(f"Unknown texture_format {texture_format!r}. Expected one of {list(_tex_fmt_map)}.")
qmode = _tex_fmt_map[texture_format]
with wp.ScopedDevice(device):
verts = mesh.vertices * np.array(effective_scale)[None, :]
pos = wp.array(verts, dtype=wp.vec3)
indices = wp.array(mesh.indices, dtype=wp.int32)
tex_mesh = wp.Mesh(points=pos, indices=indices, support_winding_number=True)
signed_volume = compute_mesh_signed_volume(pos, indices)
winding_threshold = 0.5 if signed_volume >= 0.0 else -0.5
res = effective_max_resolution if effective_max_resolution is not None else 64
texture_data, coarse_texture, subgrid_texture, tex_block_coords = create_texture_sdf_from_mesh(
tex_mesh,
margin=margin,
narrow_band_range=narrow_band_range,
max_resolution=res,
quantization_mode=qmode,
winding_threshold=winding_threshold,
scale_baked=bake_scale,
)
wp.synchronize()
sdf = SDF(
data=sdf_data,
sparse_volume=sparse_volume,
coarse_volume=coarse_volume,
block_coords=block_coords,
texture_block_coords=tex_block_coords,
texture_data=texture_data,
shape_margin=shape_margin,
_coarse_texture=coarse_texture,
_subgrid_texture=subgrid_texture,
_internal=True,
)
sdf.validate()
return sdf
@staticmethod
def create_from_data(
*,
sparse_volume: wp.Volume | None = None,
coarse_volume: wp.Volume | None = None,
block_coords: np.ndarray | Sequence[wp.vec3us] | None = None,
center: Sequence[float] | None = None,
half_extents: Sequence[float] | None = None,
background_value: float = MAXVAL,
scale_baked: bool = False,
shape_margin: float = 0.0,
texture_data: "TextureSDFData | None" = None,
) -> "SDF":
"""Create an SDF from precomputed runtime resources."""
sdf_data = create_empty_sdf_data()
if sparse_volume is not None:
sdf_data.sparse_sdf_ptr = sparse_volume.id
sparse_voxel_size = np.asarray(sparse_volume.get_voxel_size(), dtype=np.float32)
sdf_data.sparse_voxel_size = wp.vec3(sparse_voxel_size)
sdf_data.sparse_voxel_radius = 0.5 * float(np.linalg.norm(sparse_voxel_size))
if coarse_volume is not None:
sdf_data.coarse_sdf_ptr = coarse_volume.id
coarse_voxel_size = np.asarray(coarse_volume.get_voxel_size(), dtype=np.float32)
sdf_data.coarse_voxel_size = wp.vec3(coarse_voxel_size)
sdf_data.center = wp.vec3(center) if center is not None else wp.vec3(0.0, 0.0, 0.0)
sdf_data.half_extents = wp.vec3(half_extents) if half_extents is not None else wp.vec3(0.0, 0.0, 0.0)
sdf_data.background_value = background_value
sdf_data.scale_baked = scale_baked
sdf = SDF(
data=sdf_data,
sparse_volume=sparse_volume,
coarse_volume=coarse_volume,
block_coords=block_coords,
shape_margin=shape_margin,
texture_data=texture_data,
_internal=True,
)
sdf.validate()
return sdf
# Default background value for unallocated voxels in sparse SDF.
# Using MAXVAL ensures trilinear interpolation with unallocated voxels produces values >= MAXVAL * 0.99,
# allowing detection of unallocated voxels without triggering verify_fp false positives.
SDF_BACKGROUND_VALUE = MAXVAL
def create_empty_sdf_data() -> SDFData:
"""Create an empty SDFData struct for shapes that don't need SDF collision.
Returns:
An SDFData struct with zeroed pointers and extents.
"""
sdf_data = SDFData()
sdf_data.sparse_sdf_ptr = wp.uint64(0)
sdf_data.sparse_voxel_size = wp.vec3(0.0, 0.0, 0.0)
sdf_data.sparse_voxel_radius = 0.0
sdf_data.coarse_sdf_ptr = wp.uint64(0)
sdf_data.coarse_voxel_size = wp.vec3(0.0, 0.0, 0.0)
sdf_data.center = wp.vec3(0.0, 0.0, 0.0)
sdf_data.half_extents = wp.vec3(0.0, 0.0, 0.0)
sdf_data.background_value = SDF_BACKGROUND_VALUE
sdf_data.scale_baked = False
return sdf_data
@wp.kernel
def compute_mesh_signed_volume_kernel(
points: wp.array[wp.vec3],
indices: wp.array[wp.int32],
volume_sum: wp.array[wp.float32],
):
"""Compute signed volume contribution from each triangle."""
tri_idx = wp.tid()
v0 = points[indices[tri_idx * 3 + 0]]
v1 = points[indices[tri_idx * 3 + 1]]
v2 = points[indices[tri_idx * 3 + 2]]
wp.atomic_add(volume_sum, 0, wp.dot(v0, wp.cross(v1, v2)) / 6.0)
def compute_mesh_signed_volume(points: wp.array, indices: wp.array) -> float:
"""Compute signed volume of a mesh on GPU. Positive = correct winding, negative = inverted."""
num_tris = indices.shape[0] // 3
volume_sum = wp.zeros(1, dtype=wp.float32)
wp.launch(compute_mesh_signed_volume_kernel, dim=num_tris, inputs=[points, indices, volume_sum])
return float(volume_sum.numpy()[0])
@wp.func
def get_distance_to_mesh(mesh: wp.uint64, point: wp.vec3, max_dist: wp.float32, winding_threshold: wp.float32):
res = wp.mesh_query_point_sign_winding_number(mesh, point, max_dist, 2.0, winding_threshold)
if res.result:
closest = wp.mesh_eval_position(mesh, res.face, res.u, res.v)
vec_to_surface = closest - point
sign = res.sign
# For inverted meshes (threshold < 0), the winding > threshold comparison
# gives inverted signs, so we flip them back
if winding_threshold < 0.0:
sign = -sign
return sign * wp.length(vec_to_surface)
return max_dist
@wp.kernel
def sdf_from_mesh_kernel(
mesh: wp.uint64,
sdf: wp.uint64,
tile_points: wp.array[wp.vec3i],
shape_margin: wp.float32,
winding_threshold: wp.float32,
):
"""
Populate SDF grid from triangle mesh.
Only processes specified tiles. Launch with dim=(num_tiles, 8, 8, 8).
"""
tile_idx, local_x, local_y, local_z = wp.tid()
# Get the tile origin and compute global voxel coordinates
tile_origin = tile_points[tile_idx]
x_id = tile_origin[0] + local_x
y_id = tile_origin[1] + local_y
z_id = tile_origin[2] + local_z
sample_pos = wp.volume_index_to_world(sdf, int_to_vec3f(x_id, y_id, z_id))
signed_distance = get_distance_to_mesh(mesh, sample_pos, 10000.0, winding_threshold)
signed_distance -= shape_margin
wp.volume_store(sdf, x_id, y_id, z_id, signed_distance)
@wp.kernel(enable_backward=False)
def sdf_from_primitive_kernel(
shape_type: wp.int32,
shape_scale: wp.vec3,
sdf: wp.uint64,
tile_points: wp.array[wp.vec3i],
shape_margin: wp.float32,
):
"""
Populate SDF grid from primitive shape.
Only processes specified tiles. Launch with dim=(num_tiles, 8, 8, 8).
"""
tile_idx, local_x, local_y, local_z = wp.tid()
tile_origin = tile_points[tile_idx]
x_id = tile_origin[0] + local_x
y_id = tile_origin[1] + local_y
z_id = tile_origin[2] + local_z
sample_pos = wp.volume_index_to_world(sdf, int_to_vec3f(x_id, y_id, z_id))
signed_distance = float(1.0e6)
if shape_type == GeoType.SPHERE:
signed_distance = sdf_sphere(sample_pos, shape_scale[0])
elif shape_type == GeoType.BOX:
signed_distance = sdf_box(sample_pos, shape_scale[0], shape_scale[1], shape_scale[2])
elif shape_type == GeoType.CAPSULE:
signed_distance = sdf_capsule(sample_pos, shape_scale[0], shape_scale[1], int(Axis.Z))
elif shape_type == GeoType.CYLINDER:
signed_distance = sdf_cylinder(sample_pos, shape_scale[0], shape_scale[1], int(Axis.Z))
elif shape_type == GeoType.ELLIPSOID:
signed_distance = sdf_ellipsoid(sample_pos, shape_scale)
elif shape_type == GeoType.CONE:
signed_distance = sdf_cone(sample_pos, shape_scale[0], shape_scale[1], int(Axis.Z))
signed_distance -= shape_margin
wp.volume_store(sdf, x_id, y_id, z_id, signed_distance)
@wp.kernel
def check_tile_occupied_mesh_kernel(
mesh: wp.uint64,
tile_points: wp.array[wp.vec3f],
threshold: wp.vec2f,
winding_threshold: wp.float32,
tile_occupied: wp.array[bool],
):
tid = wp.tid()
sample_pos = tile_points[tid]
signed_distance = get_distance_to_mesh(mesh, sample_pos, 10000.0, winding_threshold)
is_occupied = wp.bool(False)
if wp.sign(signed_distance) > 0.0:
is_occupied = signed_distance < threshold[1]
else:
is_occupied = signed_distance > threshold[0]
tile_occupied[tid] = is_occupied
@wp.kernel(enable_backward=False)
def check_tile_occupied_primitive_kernel(
shape_type: wp.int32,
shape_scale: wp.vec3,
tile_points: wp.array[wp.vec3f],
threshold: wp.vec2f,
tile_occupied: wp.array[bool],
):
tid = wp.tid()
sample_pos = tile_points[tid]
signed_distance = float(1.0e6)
if shape_type == GeoType.SPHERE:
signed_distance = sdf_sphere(sample_pos, shape_scale[0])
elif shape_type == GeoType.BOX:
signed_distance = sdf_box(sample_pos, shape_scale[0], shape_scale[1], shape_scale[2])
elif shape_type == GeoType.CAPSULE:
signed_distance = sdf_capsule(sample_pos, shape_scale[0], shape_scale[1], int(Axis.Z))
elif shape_type == GeoType.CYLINDER:
signed_distance = sdf_cylinder(sample_pos, shape_scale[0], shape_scale[1], int(Axis.Z))
elif shape_type == GeoType.ELLIPSOID:
signed_distance = sdf_ellipsoid(sample_pos, shape_scale)
elif shape_type == GeoType.CONE:
signed_distance = sdf_cone(sample_pos, shape_scale[0], shape_scale[1], int(Axis.Z))
is_occupied = wp.bool(False)
if wp.sign(signed_distance) > 0.0:
is_occupied = signed_distance < threshold[1]
else:
is_occupied = signed_distance > threshold[0]
tile_occupied[tid] = is_occupied
def get_primitive_extents(shape_type: int, shape_scale: Sequence[float]) -> tuple[list[float], list[float]]:
"""Get the bounding box extents for a primitive shape.
Args:
shape_type: Type of the primitive shape (from GeoType).
shape_scale: Scale factors for the shape.
Returns:
Tuple of (min_ext, max_ext) as lists of [x, y, z] coordinates.
Raises:
NotImplementedError: If shape_type is not a supported primitive.
"""
if shape_type == GeoType.SPHERE:
min_ext = [-shape_scale[0], -shape_scale[0], -shape_scale[0]]
max_ext = [shape_scale[0], shape_scale[0], shape_scale[0]]
elif shape_type == GeoType.BOX:
min_ext = [-shape_scale[0], -shape_scale[1], -shape_scale[2]]
max_ext = [shape_scale[0], shape_scale[1], shape_scale[2]]
elif shape_type == GeoType.CAPSULE:
min_ext = [-shape_scale[0], -shape_scale[0], -shape_scale[1] - shape_scale[0]]
max_ext = [shape_scale[0], shape_scale[0], shape_scale[1] + shape_scale[0]]
elif shape_type == GeoType.CYLINDER:
min_ext = [-shape_scale[0], -shape_scale[0], -shape_scale[1]]
max_ext = [shape_scale[0], shape_scale[0], shape_scale[1]]
elif shape_type == GeoType.ELLIPSOID:
min_ext = [-shape_scale[0], -shape_scale[1], -shape_scale[2]]
max_ext = [shape_scale[0], shape_scale[1], shape_scale[2]]
elif shape_type == GeoType.CONE:
min_ext = [-shape_scale[0], -shape_scale[0], -shape_scale[1]]
max_ext = [shape_scale[0], shape_scale[0], shape_scale[1]]
else:
raise NotImplementedError(f"Extents not implemented for shape type: {shape_type}")
return min_ext, max_ext
def _compute_sdf_from_shape_impl(
shape_type: int,
shape_geo: Mesh | None = None,
shape_scale: Sequence[float] = (1.0, 1.0, 1.0),
shape_margin: float = 0.0,
narrow_band_distance: Sequence[float] = (-0.1, 0.1),
margin: float = 0.05,
target_voxel_size: float | None = None,
max_resolution: int = 64,
bake_scale: bool = False,
verbose: bool = False,
device: Devicelike | None = None,
) -> tuple[SDFData, wp.Volume | None, wp.Volume | None, Sequence[wp.vec3us]]:
"""Compute sparse and coarse SDF volumes for a shape.
The SDF is computed in the mesh's unscaled local space. Scale is intentionally
NOT a parameter - the collision system handles scaling at runtime, ensuring
the SDF and mesh BVH stay consistent and allowing dynamic scale changes.
Args:
shape_type: Type of the shape.
shape_geo: Optional source geometry. Required for mesh shapes.
shape_scale: Scale factors for the mesh. Applied before SDF generation if bake_scale is True.
shape_margin: Margin offset to subtract from SDF values.
narrow_band_distance: Tuple of (inner, outer) distances for narrow band.
margin: Margin to add to bounding box. Must be > 0.
target_voxel_size: Target voxel size for sparse SDF grid. If None, computed as max_extent/max_resolution.
max_resolution: Maximum dimension for sparse SDF grid when target_voxel_size is None. Must be divisible by 8.
bake_scale: If True, bake shape_scale into the SDF. If False, use (1,1,1) scale.
verbose: Print debug info.
device: CUDA device for all GPU allocations. When ``None``, uses the
current :class:`wp.ScopedDevice` or the Warp default device.
Returns:
Tuple of (sdf_data, sparse_volume, coarse_volume, block_coords) where:
- sdf_data: SDFData struct with pointers and extents
- sparse_volume: wp.Volume object for sparse SDF (keep alive for reference counting)
- coarse_volume: wp.Volume object for coarse SDF (keep alive for reference counting)
- block_coords: List of wp.vec3us tile coordinates for allocated blocks in the sparse volume
Raises:
RuntimeError: If CUDA is not available.
"""
if not wp.is_cuda_available():
raise RuntimeError("compute_sdf_from_shape requires CUDA but no CUDA device is available")
if shape_type == GeoType.PLANE or shape_type == GeoType.HFIELD:
# SDF collisions are not supported for Plane or HField shapes, falling back to mesh collisions
return create_empty_sdf_data(), None, None, []
with wp.ScopedDevice(device):
assert isinstance(narrow_band_distance, Sequence), "narrow_band_distance must be a tuple of two floats"
assert len(narrow_band_distance) == 2, "narrow_band_distance must be a tuple of two floats"
assert narrow_band_distance[0] < 0.0 < narrow_band_distance[1], (
"narrow_band_distance[0] must be less than 0.0 and narrow_band_distance[1] must be greater than 0.0"
)
assert margin > 0, "margin must be > 0"
# Determine effective scale based on bake_scale flag
effective_scale = tuple(shape_scale) if bake_scale else (1.0, 1.0, 1.0)
offset = margin + shape_margin
if shape_type == GeoType.MESH:
if shape_geo is None:
raise ValueError("shape_geo must be provided for GeoType.MESH.")
verts = shape_geo.vertices * np.array(effective_scale)[None, :]
pos = wp.array(verts, dtype=wp.vec3)
indices = wp.array(shape_geo.indices, dtype=wp.int32)
mesh = wp.Mesh(points=pos, indices=indices, support_winding_number=True)
m_id = mesh.id
# Compute winding threshold based on mesh volume sign
# Positive volume = correct winding (threshold 0.5), negative = inverted (threshold -0.5)
signed_volume = compute_mesh_signed_volume(pos, indices)
winding_threshold = 0.5 if signed_volume >= 0.0 else -0.5
if verbose and signed_volume < 0:
print("Mesh has inverted winding (negative volume), using threshold -0.5")
min_ext = np.min(verts, axis=0).tolist()
max_ext = np.max(verts, axis=0).tolist()
else:
min_ext, max_ext = get_primitive_extents(shape_type, effective_scale)
min_ext = np.array(min_ext) - offset
max_ext = np.array(max_ext) + offset
ext = max_ext - min_ext
# Compute center and half_extents for oriented bounding box collision detection
center = (min_ext + max_ext) * 0.5
half_extents = (max_ext - min_ext) * 0.5
# Calculate uniform voxel size based on the longest dimension
max_extent = np.max(ext)
# If target_voxel_size not specified, compute from max_resolution
if target_voxel_size is None:
# Warp volumes are allocated in tiles of 8 voxels
assert max_resolution % 8 == 0, "max_resolution must be divisible by 8 for SDF volume allocation"
# we store coords as uint16
assert max_resolution < 1 << 16, f"max_resolution must be less than {1 << 16}"
target_voxel_size = max_extent / max_resolution
voxel_size_max_ext = target_voxel_size
grid_tile_nums = (ext / voxel_size_max_ext).astype(int) // 8
grid_tile_nums = np.maximum(grid_tile_nums, 1)
grid_dims = grid_tile_nums * 8
actual_voxel_size = ext / (grid_dims - 1)
if verbose:
print(
f"Extent: {ext}, Grid dims: {grid_dims}, voxel size: {actual_voxel_size} target_voxel_size: {target_voxel_size}"
)
tile_max = np.around((max_ext - min_ext) / actual_voxel_size).astype(np.int32) // 8
tiles = np.array(
[[i, j, k] for i in range(tile_max[0] + 1) for j in range(tile_max[1] + 1) for k in range(tile_max[2] + 1)],
dtype=np.int32,
)
tile_points = tiles * 8
tile_center_points_world = (tile_points + 4) * actual_voxel_size + min_ext
tile_center_points_world = wp.array(tile_center_points_world, dtype=wp.vec3f)
tile_occupied = wp.zeros(len(tile_points), dtype=bool)
# for each tile point, check if it should be marked as occupied
tile_radius = np.linalg.norm(4 * actual_voxel_size)
threshold = wp.vec2f(narrow_band_distance[0] - tile_radius, narrow_band_distance[1] + tile_radius)
if shape_type == GeoType.MESH:
wp.launch(
check_tile_occupied_mesh_kernel,
dim=(len(tile_points)),
inputs=[m_id, tile_center_points_world, threshold, winding_threshold],
outputs=[tile_occupied],
)
else:
wp.launch(
check_tile_occupied_primitive_kernel,
dim=(len(tile_points)),
inputs=[shape_type, effective_scale, tile_center_points_world, threshold],
outputs=[tile_occupied],
)
if verbose:
print("Occupancy: ", tile_occupied.numpy().sum() / len(tile_points))
tile_points = tile_points[tile_occupied.numpy()]
tile_points_wp = wp.array(tile_points, dtype=wp.vec3i)
sparse_volume = wp.Volume.allocate_by_tiles(
tile_points=tile_points_wp,
voxel_size=wp.vec3(actual_voxel_size),
translation=wp.vec3(min_ext),
bg_value=SDF_BACKGROUND_VALUE,
)
# populate the sparse volume with the sdf values
# Only process allocated tiles (num_tiles x 8x8x8)
num_allocated_tiles = len(tile_points)
if shape_type == GeoType.MESH:
wp.launch(
sdf_from_mesh_kernel,
dim=(num_allocated_tiles, 8, 8, 8),
inputs=[m_id, sparse_volume.id, tile_points_wp, shape_margin, winding_threshold],
)
else:
wp.launch(
sdf_from_primitive_kernel,
dim=(num_allocated_tiles, 8, 8, 8),
inputs=[shape_type, effective_scale, sparse_volume.id, tile_points_wp, shape_margin],
)
tiles = sparse_volume.get_tiles().numpy()
block_coords = [wp.vec3us(t_coords) for t_coords in tiles]
# Create coarse background SDF (8x8x8 voxels = one tile) with same extents
coarse_dims = 8
coarse_voxel_size = ext / (coarse_dims - 1)
coarse_tile_points = np.array([[0, 0, 0]], dtype=np.int32)
coarse_tile_points_wp = wp.array(coarse_tile_points, dtype=wp.vec3i)
coarse_volume = wp.Volume.allocate_by_tiles(
tile_points=coarse_tile_points_wp,
voxel_size=wp.vec3(coarse_voxel_size),
translation=wp.vec3(min_ext),
bg_value=SDF_BACKGROUND_VALUE,
)
# Populate the coarse volume with SDF values (single tile)
if shape_type == GeoType.MESH:
wp.launch(
sdf_from_mesh_kernel,
dim=(1, 8, 8, 8),
inputs=[m_id, coarse_volume.id, coarse_tile_points_wp, shape_margin, winding_threshold],
)
else:
wp.launch(
sdf_from_primitive_kernel,
dim=(1, 8, 8, 8),
inputs=[shape_type, effective_scale, coarse_volume.id, coarse_tile_points_wp, shape_margin],
)
if shape_type == GeoType.MESH:
# Synchronize to ensure all kernels reading from the temporary wp.Mesh
# (created above for SDF construction) have completed before it goes
# out of scope. Without this, wp.Mesh.__del__ can free the BVH / winding-
# number data while an asynchronous kernel is still reading it, corrupting
# the CUDA context on some driver/GPU combinations (#1616).
wp.synchronize()
if verbose:
print(f"Coarse SDF: dims={coarse_dims}x{coarse_dims}x{coarse_dims}, voxel size: {coarse_voxel_size}")
# Create and populate SDFData struct
sdf_data = SDFData()
sdf_data.sparse_sdf_ptr = sparse_volume.id
sdf_data.sparse_voxel_size = wp.vec3(actual_voxel_size)
sdf_data.sparse_voxel_radius = 0.5 * float(np.linalg.norm(actual_voxel_size))
sdf_data.coarse_sdf_ptr = coarse_volume.id
sdf_data.coarse_voxel_size = wp.vec3(coarse_voxel_size)
sdf_data.center = wp.vec3(center)
sdf_data.half_extents = wp.vec3(half_extents)
sdf_data.background_value = SDF_BACKGROUND_VALUE
sdf_data.scale_baked = bake_scale
return sdf_data, sparse_volume, coarse_volume, block_coords
def compute_sdf_from_shape(
shape_type: int,
shape_geo: Mesh | None = None,
shape_scale: Sequence[float] = (1.0, 1.0, 1.0),
shape_margin: float = 0.0,
narrow_band_distance: Sequence[float] = (-0.1, 0.1),
margin: float = 0.05,
target_voxel_size: float | None = None,
max_resolution: int = 64,
bake_scale: bool = False,
verbose: bool = False,
device: Devicelike | None = None,
) -> tuple[SDFData, wp.Volume | None, wp.Volume | None, Sequence[wp.vec3us]]:
"""Compute sparse and coarse SDF volumes for a shape.
Mesh shape dispatches through :meth:`SDF.create_from_mesh` to keep that path canonical.
Args:
shape_type: Geometry type identifier from :class:`GeoType`.
shape_geo: Source mesh geometry when ``shape_type`` is ``GeoType.MESH``.
shape_scale: Shape scale [unitless].
shape_margin: Shape margin offset [m] subtracted from sampled SDF.
narrow_band_distance: Signed narrow-band distance range [m] as ``(inner, outer)``.
margin: Extra AABB padding [m] added before discretization.
target_voxel_size: Target sparse-grid voxel size [m]. If provided, takes
precedence over ``max_resolution``.
max_resolution: Maximum sparse-grid dimension [voxel] when
``target_voxel_size`` is not provided.
bake_scale: If ``True``, bake ``shape_scale`` into generated SDF data.
verbose: If ``True``, print debug information during SDF construction.
device: CUDA device for SDF allocation. When ``None``, uses the
current :class:`wp.ScopedDevice` or the Warp default device.
Returns:
Tuple ``(sdf_data, sparse_volume, coarse_volume, block_coords)``.
"""
if shape_type == GeoType.MESH:
if shape_geo is None:
raise ValueError("shape_geo must be provided for GeoType.MESH.")
# Canonical mesh path: use SDF.create_from_mesh for all mesh SDF generation.
sdf = SDF.create_from_mesh(
shape_geo,
device=device,
narrow_band_range=tuple(narrow_band_distance),
target_voxel_size=target_voxel_size,
max_resolution=max_resolution,
margin=margin,
shape_margin=shape_margin,
scale=tuple(shape_scale) if bake_scale else None,
)
return sdf.to_kernel_data(), sdf.sparse_volume, sdf.coarse_volume, (sdf.block_coords or [])
return _compute_sdf_from_shape_impl(
shape_type=shape_type,
shape_geo=shape_geo,
shape_scale=shape_scale,
shape_margin=shape_margin,
narrow_band_distance=narrow_band_distance,
margin=margin,
target_voxel_size=target_voxel_size,
max_resolution=max_resolution,
bake_scale=bake_scale,
verbose=verbose,
device=device,
)
def compute_isomesh(volume: wp.Volume, isovalue: float = 0.0, device: Devicelike | None = None) -> Mesh | None:
"""Compute an isosurface mesh from a sparse SDF volume.
Uses a two-pass approach to minimize memory allocation:
1. First pass: count actual triangles produced
2. Allocate exact memory needed
3. Second pass: generate vertices
Args:
volume: The SDF volume.
isovalue: Surface level to extract [m]. ``0.0`` gives the
zero-isosurface; positive values extract an outward offset surface.
device: CUDA device for GPU allocations. When ``None``, uses the
current :class:`wp.ScopedDevice` or the Warp default device.
Returns:
Mesh object containing the isosurface mesh.
"""
if device is not None:
device = wp.get_device(device)
else:
device = wp.get_device()
mc_tables = get_mc_tables(device)
tile_points = volume.get_tiles()
tile_points_wp = wp.array(tile_points, dtype=wp.vec3i, device=device)
num_tiles = tile_points.shape[0]
if num_tiles == 0:
return None
face_count = wp.zeros((1,), dtype=int, device=device)
wp.launch(
count_isomesh_faces_kernel,
dim=(num_tiles, 8, 8, 8),
inputs=[volume.id, tile_points_wp, mc_tables[0], mc_tables[3], float(isovalue)],
outputs=[face_count],
device=device,
)
num_faces = int(face_count.numpy()[0])
if num_faces == 0:
return None
max_verts = 3 * num_faces
verts = wp.empty((max_verts,), dtype=wp.vec3, device=device)
face_normals = wp.empty((num_faces,), dtype=wp.vec3, device=device)
face_count.zero_()
wp.launch(
generate_isomesh_kernel,
dim=(num_tiles, 8, 8, 8),
inputs=[volume.id, tile_points_wp, mc_tables[0], mc_tables[4], mc_tables[3], float(isovalue)],
outputs=[face_count, verts, face_normals],
device=device,
)
verts_np = verts.numpy()
faces_np = np.arange(3 * num_faces).reshape(-1, 3)
faces_np = faces_np[:, ::-1]
return Mesh(verts_np, faces_np)
def compute_offset_mesh(
shape_type: int,
shape_geo: Mesh | None = None,
shape_scale: Sequence[float] = (1.0, 1.0, 1.0),
offset: float = 0.0,
max_resolution: int = 48,
device: Devicelike | None = None,
) -> Mesh | None:
"""Compute the offset (Minkowski-inflated) isosurface mesh of a shape.
For primitive shapes with analytical SDFs (sphere, box, capsule, cylinder,
ellipsoid, cone) this evaluates the SDF directly on a dense grid, avoiding
NanoVDB volume construction. For mesh / convex-mesh shapes with a
pre-built :class:`SDF` (via :meth:`Mesh.build_sdf`), the existing volume
or texture SDF is reused. Only when no pre-built SDF is available does
this fall back to constructing a temporary NanoVDB volume.
Args:
shape_type: Geometry type identifier from :class:`GeoType`.
shape_geo: Source mesh geometry when *shape_type* is
:attr:`GeoType.MESH` or :attr:`GeoType.CONVEX_MESH`.
shape_scale: Shape scale factors [unitless].
offset: Outward surface offset [m]. Use ``0`` for the original surface.
max_resolution: Maximum grid dimension [voxels].
device: CUDA device for GPU allocations.
Returns:
A :class:`Mesh` representing the offset isosurface, or ``None`` when
the shape type is unsupported (plane, heightfield) or the resulting
mesh would be empty.
"""
if shape_type in (GeoType.PLANE, GeoType.HFIELD):
return None
if shape_type in _ANALYTICAL_SDF_TYPES:
return compute_offset_mesh_analytical(
shape_type=shape_type,
shape_scale=shape_scale,
offset=offset,
max_resolution=max_resolution,
device=device,
)
# Reuse existing SDF on the mesh when available (avoids building a
# NanoVDB volume from scratch). This assumes the stored SDF was built
# with shape_margin=0 (default) so that extracting at isovalue=offset
# yields the correct inflated surface. The fallback path below uses
# bake_scale=True, so we skip the shortcut when scale hasn't been baked
# AND the caller requests non-unit scale — otherwise the extracted
# vertices would be in unscaled mesh-local space.
if shape_geo is not None:
existing_sdf = getattr(shape_geo, "sdf", None)
if existing_sdf is not None:
scale_ok = existing_sdf.data.scale_baked or all(abs(s - 1.0) < 1e-6 for s in shape_scale)
if scale_ok:
result = existing_sdf.extract_isomesh(isovalue=offset, device=device)
if result is not None:
return result
if shape_type not in (GeoType.MESH, GeoType.CONVEX_MESH):
raise ValueError(
f"compute_offset_mesh: unsupported shape type {shape_type} "
f"without an analytical SDF or a pre-built SDF on the geometry."
)
if shape_geo is None:
raise ValueError("shape_geo must be provided for mesh/convex-mesh offset meshing.")
padding = max(abs(offset) * 0.5, 0.02)
narrow_band = (-abs(offset) - padding, abs(offset) + padding)
margin = max(padding, 0.05)
_sdf_data, sparse_volume, _coarse_volume, _block_coords = _compute_sdf_from_shape_impl(
shape_type=GeoType.MESH,
shape_geo=shape_geo,
shape_scale=shape_scale,
shape_margin=offset,
narrow_band_distance=narrow_band,
margin=margin,
max_resolution=max_resolution,
bake_scale=True,
device=device,
)
if sparse_volume is None:
return None
return compute_isomesh(sparse_volume, device=device)
@wp.kernel(enable_backward=False)
def count_isomesh_faces_kernel(
sdf: wp.uint64,
tile_points: wp.array[wp.vec3i],
tri_range_table: wp.array[wp.int32],
corner_offsets_table: wp.array[wp.vec3ub],
isovalue: wp.float32,
face_count: wp.array[int],
):
"""Count isosurface faces without generating vertices (first pass of two-pass approach).
Only processes specified tiles. Launch with dim=(num_tiles, 8, 8, 8).
"""
tile_idx, local_x, local_y, local_z = wp.tid()
tile_origin = tile_points[tile_idx]
x_id = tile_origin[0] + local_x
y_id = tile_origin[1] + local_y
z_id = tile_origin[2] + local_z
cube_idx = wp.int32(0)
for i in range(8):
corner_offset = wp.vec3i(corner_offsets_table[i])
x = x_id + corner_offset.x
y = y_id + corner_offset.y
z = z_id + corner_offset.z
v = wp.volume_lookup_f(sdf, x, y, z)
if v >= wp.static(MAXVAL * 0.99):
return
if v < isovalue:
cube_idx |= 1 << i
# look up the tri range for the cube index
tri_range_start = tri_range_table[cube_idx]
tri_range_end = tri_range_table[cube_idx + 1]
num_verts = tri_range_end - tri_range_start
num_faces = num_verts // 3
if num_faces > 0:
wp.atomic_add(face_count, 0, num_faces)
@wp.kernel(enable_backward=False)
def generate_isomesh_kernel(
sdf: wp.uint64,
tile_points: wp.array[wp.vec3i],
tri_range_table: wp.array[wp.int32],
flat_edge_verts_table: wp.array[wp.vec2ub],
corner_offsets_table: wp.array[wp.vec3ub],
isovalue: wp.float32,
face_count: wp.array[int],
vertices: wp.array[wp.vec3],
face_normals: wp.array[wp.vec3],
):
"""Generate isosurface mesh vertices and normals using marching cubes.
Only processes specified tiles. Launch with dim=(num_tiles, 8, 8, 8).
"""
tile_idx, local_x, local_y, local_z = wp.tid()
tile_origin = tile_points[tile_idx]
x_id = tile_origin[0] + local_x
y_id = tile_origin[1] + local_y
z_id = tile_origin[2] + local_z
cube_idx = wp.int32(0)
corner_vals = vec8f()
for i in range(8):
corner_offset = wp.vec3i(corner_offsets_table[i])
x = x_id + corner_offset.x
y = y_id + corner_offset.y
z = z_id + corner_offset.z
v = wp.volume_lookup_f(sdf, x, y, z)
if v >= wp.static(MAXVAL * 0.99):
return
corner_vals[i] = v
if v < isovalue:
cube_idx |= 1 << i
tri_range_start = tri_range_table[cube_idx]
tri_range_end = tri_range_table[cube_idx + 1]
num_verts = tri_range_end - tri_range_start
num_faces = num_verts // 3
out_idx_faces = wp.atomic_add(face_count, 0, num_faces)
if num_verts == 0:
return
for fi in range(5):
if fi >= num_faces:
return
_area, normal, _face_center, _pen_depth, face_verts = mc_calc_face(
flat_edge_verts_table,
corner_offsets_table,
tri_range_start + 3 * fi,
corner_vals,
sdf,
x_id,
y_id,
z_id,
isovalue,
)
vertices[3 * out_idx_faces + 3 * fi + 0] = wp.vec3(face_verts[0])
vertices[3 * out_idx_faces + 3 * fi + 1] = wp.vec3(face_verts[1])
vertices[3 * out_idx_faces + 3 * fi + 2] = wp.vec3(face_verts[2])
face_normals[out_idx_faces + fi] = normal
# ---------------------------------------------------------------------------
# Dense-grid analytical marching cubes for primitive shapes
# ---------------------------------------------------------------------------
# These kernels skip NanoVDB volume construction entirely and evaluate the
# analytical SDF on a flat dense grid, which is significantly faster for
# primitives (sphere, box, capsule, cylinder, ellipsoid, cone).
# ---------------------------------------------------------------------------
_ANALYTICAL_SDF_TYPES = frozenset(
{
GeoType.SPHERE,
GeoType.BOX,
GeoType.CAPSULE,
GeoType.CYLINDER,
GeoType.ELLIPSOID,
GeoType.CONE,
}
)
@wp.kernel(enable_backward=False)
def _populate_dense_sdf_kernel(
shape_type: wp.int32,
shape_scale: wp.vec3,
origin: wp.vec3,
voxel_size: wp.vec3,
ny: wp.int32,
nz: wp.int32,
shape_offset: wp.float32,
sdf_values: wp.array[wp.float32],
):
"""Evaluate analytical SDF at every point of a dense regular grid."""
x, y, z = wp.tid()
pos = wp.vec3(
origin[0] + float(x) * voxel_size[0],
origin[1] + float(y) * voxel_size[1],
origin[2] + float(z) * voxel_size[2],
)
d = float(1.0e6)
if shape_type == GeoType.SPHERE:
d = sdf_sphere(pos, shape_scale[0])
elif shape_type == GeoType.BOX:
d = sdf_box(pos, shape_scale[0], shape_scale[1], shape_scale[2])
elif shape_type == GeoType.CAPSULE:
d = sdf_capsule(pos, shape_scale[0], shape_scale[1], int(Axis.Z))
elif shape_type == GeoType.CYLINDER:
d = sdf_cylinder(pos, shape_scale[0], shape_scale[1], int(Axis.Z))
elif shape_type == GeoType.ELLIPSOID:
d = sdf_ellipsoid(pos, shape_scale)
elif shape_type == GeoType.CONE:
d = sdf_cone(pos, shape_scale[0], shape_scale[1], int(Axis.Z))
sdf_values[x * ny * nz + y * nz + z] = d - shape_offset
@wp.kernel(enable_backward=False)
def _count_dense_mc_faces_kernel(
sdf_values: wp.array[wp.float32],
ny: wp.int32,
nz: wp.int32,
tri_range_table: wp.array[wp.int32],
corner_offsets_table: wp.array[wp.vec3ub],
face_count: wp.array[int],
):
"""Count marching-cubes faces on a dense SDF grid (first MC pass)."""
x, y, z = wp.tid()
cube_idx = wp.int32(0)
for i in range(8):
co = wp.vec3i(corner_offsets_table[i])
v = sdf_values[(x + co[0]) * ny * nz + (y + co[1]) * nz + (z + co[2])]
if v < 0.0:
cube_idx |= 1 << i
tri_start = tri_range_table[cube_idx]
tri_end = tri_range_table[cube_idx + 1]
num_faces = (tri_end - tri_start) // 3
if num_faces > 0:
wp.atomic_add(face_count, 0, num_faces)
@wp.kernel(enable_backward=False)
def _generate_dense_mc_kernel(
sdf_values: wp.array[wp.float32],
ny: wp.int32,
nz: wp.int32,
origin: wp.vec3,
voxel_size: wp.vec3,
tri_range_table: wp.array[wp.int32],
flat_edge_verts_table: wp.array[wp.vec2ub],
corner_offsets_table: wp.array[wp.vec3ub],
face_count: wp.array[int],
vertices: wp.array[wp.vec3],
face_normals: wp.array[wp.vec3],
):
"""Generate marching-cubes vertices on a dense SDF grid (second MC pass)."""
x, y, z = wp.tid()
cube_idx = wp.int32(0)
corner_vals = vec8f()
for i in range(8):
co = wp.vec3i(corner_offsets_table[i])
v = sdf_values[(x + co[0]) * ny * nz + (y + co[1]) * nz + (z + co[2])]
corner_vals[i] = v
if v < 0.0:
cube_idx |= 1 << i
tri_start = tri_range_table[cube_idx]
tri_end = tri_range_table[cube_idx + 1]
num_verts = tri_end - tri_start
num_faces = num_verts // 3
out_idx = wp.atomic_add(face_count, 0, num_faces)
if num_verts == 0:
return
base = wp.vec3(float(x), float(y), float(z))
for fi in range(5):
if fi >= num_faces:
return
face_verts = wp.mat33f()
for vi in range(3):
ev = wp.vec2i(flat_edge_verts_table[tri_start + 3 * fi + vi])
val_0 = wp.float32(corner_vals[ev[0]])
val_1 = wp.float32(corner_vals[ev[1]])
p_0 = wp.vec3f(corner_offsets_table[ev[0]])
p_1 = wp.vec3f(corner_offsets_table[ev[1]])
val_diff = val_1 - val_0
if wp.abs(val_diff) < 1e-8:
p = 0.5 * (p_0 + p_1)
else:
t = (0.0 - val_0) / val_diff
p = p_0 + t * (p_1 - p_0)
local = base + p
face_verts[vi] = wp.vec3(
origin[0] + local[0] * voxel_size[0],
origin[1] + local[1] * voxel_size[1],
origin[2] + local[2] * voxel_size[2],
)
n = wp.cross(face_verts[1] - face_verts[0], face_verts[2] - face_verts[0])
normal = wp.normalize(n)
vertices[3 * out_idx + 3 * fi + 0] = wp.vec3(face_verts[0])
vertices[3 * out_idx + 3 * fi + 1] = wp.vec3(face_verts[1])
vertices[3 * out_idx + 3 * fi + 2] = wp.vec3(face_verts[2])
face_normals[out_idx + fi] = normal
def compute_offset_mesh_analytical(
shape_type: int,
shape_scale: Sequence[float] = (1.0, 1.0, 1.0),
offset: float = 0.0,
max_resolution: int = 48,
device: Devicelike | None = None,
) -> Mesh | None:
"""Compute the offset isosurface mesh for a primitive via direct analytical SDF evaluation.
Unlike :func:`compute_offset_mesh` this skips NanoVDB volume construction
and evaluates the analytical SDF on a dense regular grid before running
marching cubes. This is faster for primitive shapes.
Args:
shape_type: Geometry type identifier from :class:`GeoType`. Must be a
primitive with an analytical SDF (sphere, box, capsule, cylinder,
ellipsoid, or cone).
shape_scale: Shape scale factors [unitless].
offset: Outward surface offset [m]. Use ``0`` for the original surface.
max_resolution: Maximum grid dimension [voxels].
device: CUDA device for GPU allocations.
Returns:
A :class:`Mesh` representing the offset isosurface, or ``None`` when
the shape type is unsupported or the mesh would be empty.
"""
if shape_type not in _ANALYTICAL_SDF_TYPES:
return None
if device is None:
cur = wp.get_device()
device = cur if cur.is_cuda else "cuda:0"
with wp.ScopedDevice(device):
min_ext_list, max_ext_list = get_primitive_extents(shape_type, shape_scale)
padding = max(abs(offset) * 0.5, 0.02)
total_expansion = max(abs(offset) + padding, 0.05)
min_ext = np.array(min_ext_list, dtype=np.float64) - total_expansion
max_ext = np.array(max_ext_list, dtype=np.float64) + total_expansion
ext = max_ext - min_ext
# Adaptively increase resolution when the expansion dominates the
# shape extents (e.g. a 1 mm sphere with 0.05 m expansion). This
# ensures at least ~4 voxels span the smallest shape dimension while
# capping total memory via a voxel budget (default ~4M voxels ≈ 16 MB
# of float32) so thin/flat shapes don't cause OOM.
max_voxel_budget = 4_000_000
shape_ext = np.array(max_ext_list, dtype=np.float64) - np.array(min_ext_list, dtype=np.float64)
min_shape_dim = float(np.min(shape_ext))
if min_shape_dim > 0.0:
effective_resolution = max(max_resolution, int(np.ceil(float(np.max(ext)) / min_shape_dim * 4)))
else:
effective_resolution = max_resolution
max_extent = float(np.max(ext))
voxel_target = max_extent / effective_resolution
grid_dims = np.maximum(np.round(ext / voxel_target).astype(int), 2)
total_voxels = int(np.prod(grid_dims))
if total_voxels > max_voxel_budget:
scale_factor = (max_voxel_budget / total_voxels) ** (1.0 / 3.0)
grid_dims = np.maximum(np.round(grid_dims * scale_factor).astype(int), 2)
logger.warning(
"compute_offset_mesh_analytical: clamped grid from %d voxels to %dx%dx%d (%d voxels) "
"for shape type %d with scale %s. Visualization may be lower-fidelity for this shape.",
total_voxels,
grid_dims[0],
grid_dims[1],
grid_dims[2],
int(np.prod(grid_dims)),
shape_type,
shape_scale,
)
actual_voxel_size = ext / (grid_dims - 1)
nx, ny, nz = int(grid_dims[0]), int(grid_dims[1]), int(grid_dims[2])
sdf_values = wp.empty((nx * ny * nz,), dtype=wp.float32, device=device)
wp.launch(
_populate_dense_sdf_kernel,
dim=(nx, ny, nz),
inputs=[
int(shape_type),
wp.vec3(float(shape_scale[0]), float(shape_scale[1]), float(shape_scale[2])),
wp.vec3(float(min_ext[0]), float(min_ext[1]), float(min_ext[2])),
wp.vec3(float(actual_voxel_size[0]), float(actual_voxel_size[1]), float(actual_voxel_size[2])),
ny,
nz,
float(offset),
],
outputs=[sdf_values],
device=device,
)
mc_tables = get_mc_tables(device)
face_count = wp.zeros((1,), dtype=int, device=device)
wp.launch(
_count_dense_mc_faces_kernel,
dim=(nx - 1, ny - 1, nz - 1),
inputs=[sdf_values, ny, nz, mc_tables[0], mc_tables[3]],
outputs=[face_count],
device=device,
)
num_faces = int(face_count.numpy()[0])
if num_faces == 0:
logger.warning(
"compute_offset_mesh_analytical: marching cubes produced no faces for shape type %d "
"with scale %s and offset %.4g (grid %dx%dx%d). "
"The shape may be too small for the grid resolution.",
shape_type,
shape_scale,
offset,
nx,
ny,
nz,
)
return None
verts = wp.empty((3 * num_faces,), dtype=wp.vec3, device=device)
face_normals_out = wp.empty((num_faces,), dtype=wp.vec3, device=device)
face_count.zero_()
wp.launch(
_generate_dense_mc_kernel,
dim=(nx - 1, ny - 1, nz - 1),
inputs=[
sdf_values,
ny,
nz,
wp.vec3(float(min_ext[0]), float(min_ext[1]), float(min_ext[2])),
wp.vec3(float(actual_voxel_size[0]), float(actual_voxel_size[1]), float(actual_voxel_size[2])),
mc_tables[0],
mc_tables[4],
mc_tables[3],
],
outputs=[face_count, verts, face_normals_out],
device=device,
)
verts_np = verts.numpy()
faces_np = np.arange(3 * num_faces).reshape(-1, 3)
faces_np = faces_np[:, ::-1]
return Mesh(verts_np, faces_np)
================================================
FILE: newton/_src/geometry/simplex_solver.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
# This code is based on the GJK/simplex solver implementation from Jitter Physics 2
# Original: https://github.com/notgiven688/jitterphysics2
# Copyright (c) Thorben Linneweber (MIT License)
# The code has been translated from C# to Python and modified for use in Newton.
"""
Gilbert-Johnson-Keerthi (GJK) algorithm with simplex solver for collision detection.
This module implements the GJK distance algorithm, which computes the minimum distance
between two convex shapes. GJK operates on the Minkowski difference of the shapes and
iteratively builds a simplex (1-4 vertices) that either contains the origin (indicating
collision) or gets progressively closer to it (for distance computation).
The algorithm works by:
1. Building a simplex in Minkowski space using support mapping
2. Finding the point on the simplex closest to the origin
3. Computing a new search direction toward the origin
4. Iterating until convergence or collision detection
Key features:
- Distance computation between separated shapes
- Collision detection when shapes overlap (returns signed_distance = 0)
- Barycentric coordinates (stored as vec4) for witness points
- Numerically stable simplex reduction using Johnson's distance subalgorithm
The implementation uses support mapping to query shape geometry, making it applicable
to any convex shape that provides a support function.
"""
from typing import Any
import warp as wp
from .mpr import Vert, create_support_map_function
EPSILON = 1e-8
Mat83f = wp.types.matrix(shape=(8, 3), dtype=wp.float32)
def create_solve_closest_distance(support_func: Any, _support_funcs: Any = None):
"""
Factory function to create GJK distance solver with specific support and center functions.
Storage Convention for Simplex Vertices (Mat83f):
------------------------------------------------
The simplex stores up to 4 vertices in a flat array where each vertex uses 2 consecutive vec3 slots:
- v[2*i] stores B (point on shape B)
- v[2*i + 1] stores BtoA (vector from B to A, i.e., the Minkowski difference A - B)
This storage scheme allows:
- Direct access to the Minkowski difference (BtoA) which is used in most GJK operations
- Efficient reconstruction of point A when needed: A = B + BtoA
- Reduced function call overhead compared to wrapping field access in functions
Args:
support_func: Support mapping function for shapes.
_support_funcs: Pre-built support functions tuple from
:func:`create_support_map_function`. When provided, these are reused
instead of creating new ones, allowing multiple solvers to share
compiled support code.
Returns:
``solve_closest_distance`` wrapper function. The core function is
available as ``solve_closest_distance.core`` for callers that want to
handle the relative-frame transform themselves.
"""
if _support_funcs is not None:
_support_map_b, minkowski_support, geometric_center = _support_funcs
else:
_support_map_b, minkowski_support, geometric_center = create_support_map_function(support_func)
@wp.func
def simplex_get_vertex(v: Mat83f, i: int) -> Vert:
"""
Get vertex by index from the simplex.
Storage convention:
- v[2*i] stores B (point on shape B)
- v[2*i + 1] stores BtoA (vector from B to A)
"""
result = Vert()
result.B = v[2 * i]
result.BtoA = v[2 * i + 1]
return result
@wp.func
def closest_segment(
v: Mat83f,
i0: int,
i1: int,
) -> tuple[wp.vec3, wp.vec4, wp.uint32]:
"""Find closest point on line segment."""
# Get Minkowski difference vectors (BtoA) directly
a = v[2 * i0 + 1]
b = v[2 * i1 + 1]
edge = b - a
vsq = wp.length_sq(edge)
degenerate = vsq < EPSILON
# Guard division by zero in degenerate cases
denom = vsq
if degenerate:
denom = EPSILON
t = -wp.dot(a, edge) / denom
lambda0 = 1.0 - t
lambda1 = t
mask = (wp.uint32(1) << wp.uint32(i0)) | (wp.uint32(1) << wp.uint32(i1))
bc = wp.vec4(0.0, 0.0, 0.0, 0.0)
if lambda0 < 0.0 or degenerate:
mask = wp.uint32(1) << wp.uint32(i1)
lambda0 = 0.0
lambda1 = 1.0
elif lambda1 < 0.0:
mask = wp.uint32(1) << wp.uint32(i0)
lambda0 = 1.0
lambda1 = 0.0
bc[i0] = lambda0
bc[i1] = lambda1
return lambda0 * a + lambda1 * b, bc, mask
@wp.func
def closest_triangle(
v: Mat83f,
i0: int,
i1: int,
i2: int,
) -> tuple[wp.vec3, wp.vec4, wp.uint32]:
"""Find closest point on triangle."""
# Get Minkowski difference vectors (BtoA) directly
a = v[2 * i0 + 1]
b = v[2 * i1 + 1]
c = v[2 * i2 + 1]
u = a - b
w = a - c
normal = wp.cross(u, w)
t = wp.length_sq(normal)
degenerate = t < EPSILON
# Guard division by zero in degenerate cases
denom = t
if degenerate:
denom = EPSILON
it = 1.0 / denom
c1 = wp.cross(u, a)
c2 = wp.cross(a, w)
lambda2 = wp.dot(c1, normal) * it
lambda1 = wp.dot(c2, normal) * it
lambda0 = 1.0 - lambda2 - lambda1
best_distance = 1e30 # Large value
closest_pt = wp.vec3(0.0, 0.0, 0.0)
bc = wp.vec4(0.0, 0.0, 0.0, 0.0)
mask = wp.uint32(0)
# Check if we need to fall back to edges
if lambda0 < 0.0 or degenerate:
closest, bc_tmp, m = closest_segment(v, i1, i2)
dist = wp.length_sq(closest)
if dist < best_distance:
bc = bc_tmp
mask = m
best_distance = dist
closest_pt = closest
if lambda1 < 0.0 or degenerate:
closest, bc_tmp, m = closest_segment(v, i0, i2)
dist = wp.length_sq(closest)
if dist < best_distance:
bc = bc_tmp
mask = m
best_distance = dist
closest_pt = closest
if lambda2 < 0.0 or degenerate:
closest, bc_tmp, m = closest_segment(v, i0, i1)
dist = wp.length_sq(closest)
if dist < best_distance:
bc = bc_tmp
mask = m
closest_pt = closest
if mask != wp.uint32(0):
return closest_pt, bc, mask
bc[i0] = lambda0
bc[i1] = lambda1
bc[i2] = lambda2
mask = (wp.uint32(1) << wp.uint32(i0)) | (wp.uint32(1) << wp.uint32(i1)) | (wp.uint32(1) << wp.uint32(i2))
return lambda0 * a + lambda1 * b + lambda2 * c, bc, mask
@wp.func
def determinant(a: wp.vec3, b: wp.vec3, c: wp.vec3, d: wp.vec3) -> float:
"""Compute determinant for tetrahedron volume."""
return wp.dot(b - a, wp.cross(c - a, d - a))
@wp.func
def closest_tetrahedron(
v: Mat83f,
) -> tuple[wp.vec3, wp.vec4, wp.uint32]:
"""Find closest point on tetrahedron."""
# Get Minkowski difference vectors (BtoA) directly
v0 = v[2 * 0 + 1]
v1 = v[2 * 1 + 1]
v2 = v[2 * 2 + 1]
v3 = v[2 * 3 + 1]
det_t = determinant(v0, v1, v2, v3)
degenerate = wp.abs(det_t) < EPSILON
# Guard division by zero in degenerate cases
denom = det_t
if degenerate:
denom = EPSILON
inverse_det_t = 1.0 / denom
zero = wp.vec3(0.0, 0.0, 0.0)
lambda0 = determinant(zero, v1, v2, v3) * inverse_det_t
lambda1 = determinant(v0, zero, v2, v3) * inverse_det_t
lambda2 = determinant(v0, v1, zero, v3) * inverse_det_t
lambda3 = 1.0 - lambda0 - lambda1 - lambda2
best_distance = 1e30 # Large value
closest_pt = wp.vec3(0.0, 0.0, 0.0)
bc = wp.vec4(0.0, 0.0, 0.0, 0.0)
mask = wp.uint32(0)
# Check faces
if lambda0 < 0.0 or degenerate:
closest, bc_tmp, m = closest_triangle(v, 1, 2, 3)
dist = wp.length_sq(closest)
if dist < best_distance:
bc = bc_tmp
mask = m
best_distance = dist
closest_pt = closest
if lambda1 < 0.0 or degenerate:
closest, bc_tmp, m = closest_triangle(v, 0, 2, 3)
dist = wp.length_sq(closest)
if dist < best_distance:
bc = bc_tmp
mask = m
best_distance = dist
closest_pt = closest
if lambda2 < 0.0 or degenerate:
closest, bc_tmp, m = closest_triangle(v, 0, 1, 3)
dist = wp.length_sq(closest)
if dist < best_distance:
bc = bc_tmp
mask = m
best_distance = dist
closest_pt = closest
if lambda3 < 0.0 or degenerate:
closest, bc_tmp, m = closest_triangle(v, 0, 1, 2)
dist = wp.length_sq(closest)
if dist < best_distance:
bc = bc_tmp
mask = m
closest_pt = closest
if mask != wp.uint32(0):
return closest_pt, bc, mask
bc[0] = lambda0
bc[1] = lambda1
bc[2] = lambda2
bc[3] = lambda3
mask = wp.uint32(15) # 0b1111
return zero, bc, mask
@wp.func
def simplex_get_closest(v: Mat83f, barycentric: wp.vec4, usage_mask: wp.uint32) -> tuple[wp.vec3, wp.vec3]:
"""Get closest points on both shapes."""
point_a = wp.vec3(0.0, 0.0, 0.0)
point_b = wp.vec3(0.0, 0.0, 0.0)
for i in range(4):
if (usage_mask & (wp.uint32(1) << wp.uint32(i))) == wp.uint32(0):
continue
vertex = simplex_get_vertex(v, i)
bc_val = barycentric[i]
# Reconstruct point A from B and BtoA
point_a = point_a + bc_val * (vertex.B + vertex.BtoA)
point_b = point_b + bc_val * vertex.B
return point_a, point_b
@wp.func
def solve_closest_distance_core(
geom_a: Any,
geom_b: Any,
orientation_b: wp.quat,
position_b: wp.vec3,
extend: float,
data_provider: Any,
MAX_ITER: int = 30,
COLLIDE_EPSILON: float = 1e-4,
) -> tuple[bool, wp.vec3, wp.vec3, wp.vec3, float]:
"""
Core GJK distance algorithm implementation.
This function computes the minimum distance between two convex shapes using the
GJK algorithm. It builds a simplex iteratively using support mapping and finds
the point on the simplex closest to the origin in Minkowski space.
Assumes that shape A is located at the origin (position zero) and not rotated.
Shape B is transformed relative to shape A using the provided orientation and position.
Args:
geom_a: Shape A geometry data (in local frame at origin)
geom_b: Shape B geometry data
orientation_b: Orientation of shape B relative to shape A
position_b: Position of shape B relative to shape A
extend: Contact offset extension (sum of contact offsets)
data_provider: Support mapping data provider
MAX_ITER: Maximum number of GJK iterations (default: 30)
COLLIDE_EPSILON: Convergence threshold for distance computation (default: 1e-4)
Returns:
Tuple of:
separated (bool): True if shapes are separated, False if overlapping
point_a (wp.vec3): Witness point on shape A (in A's local frame)
point_b (wp.vec3): Witness point on shape B (in A's local frame)
normal (wp.vec3): Contact normal from A to B (in A's local frame)
distance (float): Minimum distance between shapes (0 if overlapping)
"""
# Initialize variables
distance = float(0.0)
point_a = wp.vec3(0.0, 0.0, 0.0)
point_b = wp.vec3(0.0, 0.0, 0.0)
normal = wp.vec3(0.0, 0.0, 0.0)
# Initialize simplex state
simplex_v = Mat83f()
simplex_barycentric = wp.vec4(0.0, 0.0, 0.0, 0.0)
simplex_usage_mask = wp.uint32(0)
iter_count = int(MAX_ITER)
# Get geometric center
center = geometric_center(geom_a, geom_b, orientation_b, position_b, data_provider)
# Use BtoA directly (Minkowski difference)
v = center.BtoA
dist_sq = wp.length_sq(v)
last_search_dir = wp.vec3(1.0, 0.0, 0.0)
while iter_count > 0:
iter_count -= 1
if dist_sq < COLLIDE_EPSILON * COLLIDE_EPSILON:
# Shapes are overlapping
distance = 0.0
normal = wp.vec3(0.0, 0.0, 0.0)
point_a, point_b = simplex_get_closest(simplex_v, simplex_barycentric, simplex_usage_mask)
return False, point_a, point_b, normal, distance
search_dir = -v
# Track last search direction for robust normal fallback
last_search_dir = search_dir
# Get support point in search direction
w = minkowski_support(geom_a, geom_b, search_dir, orientation_b, position_b, extend, data_provider)
# Check for convergence using Frank-Wolfe duality gap
# Use BtoA directly (Minkowski difference)
w_v = w.BtoA
delta_dist = wp.dot(v, v - w_v)
if delta_dist < COLLIDE_EPSILON * wp.sqrt(dist_sq):
break
# Check for duplicate vertex (numerical stalling)
is_duplicate = bool(False)
for i in range(4):
if (simplex_usage_mask & (wp.uint32(1) << wp.uint32(i))) != wp.uint32(0):
# Compare BtoA vectors directly
if wp.length_sq(simplex_v[2 * i + 1] - w_v) < COLLIDE_EPSILON * COLLIDE_EPSILON:
is_duplicate = bool(True)
break
if is_duplicate:
break
# Inline simplex_add_vertex
# Count used vertices and find free slot
use_count = 0
free_slot = 0
indices = wp.vec4i(0)
for i in range(4):
if (simplex_usage_mask & (wp.uint32(1) << wp.uint32(i))) != wp.uint32(0):
indices[use_count] = i
use_count += 1
else:
free_slot = i
indices[use_count] = free_slot
use_count += 1
# Set vertex in simplex using new storage convention: B, then BtoA
simplex_v[2 * free_slot] = w.B
simplex_v[2 * free_slot + 1] = w.BtoA
closest = wp.vec3(0.0, 0.0, 0.0)
success = True
if use_count == 1:
i0 = indices[0]
# Get BtoA directly (Minkowski difference)
closest = simplex_v[2 * i0 + 1]
simplex_usage_mask = wp.uint32(1) << wp.uint32(i0)
simplex_barycentric[i0] = 1.0
elif use_count == 2:
i0 = indices[0]
i1 = indices[1]
closest, bc, mask = closest_segment(simplex_v, i0, i1)
simplex_barycentric = bc
simplex_usage_mask = mask
elif use_count == 3:
i0 = indices[0]
i1 = indices[1]
i2 = indices[2]
closest, bc, mask = closest_triangle(simplex_v, i0, i1, i2)
simplex_barycentric = bc
simplex_usage_mask = mask
elif use_count == 4:
closest, bc, mask = closest_tetrahedron(simplex_v)
simplex_barycentric = bc
simplex_usage_mask = mask
# If mask == 15 (0b1111), origin is inside tetrahedron (overlap)
# Return False to indicate overlap detection
inside_tetrahedron = mask == wp.uint32(15)
success = not inside_tetrahedron
else:
success = False
new_v = closest
if not success:
# Shapes are overlapping
distance = 0.0
normal = wp.vec3(0.0, 0.0, 0.0)
point_a, point_b = simplex_get_closest(simplex_v, simplex_barycentric, simplex_usage_mask)
return False, point_a, point_b, normal, distance
v = new_v
dist_sq = wp.length_sq(v)
distance = wp.sqrt(dist_sq)
# Compute closest points first
point_a, point_b = simplex_get_closest(simplex_v, simplex_barycentric, simplex_usage_mask)
# Prefer A->B vector if reliable; otherwise fall back to -v or last search dir
delta = point_b - point_a
delta_len_sq = wp.length_sq(delta)
if delta_len_sq > EPSILON * EPSILON:
normal = delta * (1.0 / wp.sqrt(delta_len_sq))
elif distance > COLLIDE_EPSILON:
# Separated but delta is tiny: use -v
normal = v * (-1.0 / distance)
else:
# Overlap/near-contact: use last_search_dir, then stable axis
nsq = wp.length_sq(last_search_dir)
if nsq > 0.0:
normal = last_search_dir * (1.0 / wp.sqrt(nsq))
else:
normal = wp.vec3(1.0, 0.0, 0.0)
return True, point_a, point_b, normal, distance
@wp.func
def solve_closest_distance(
geom_a: Any,
geom_b: Any,
orientation_a: wp.quat,
orientation_b: wp.quat,
position_a: wp.vec3,
position_b: wp.vec3,
combined_margin: float,
data_provider: Any,
MAX_ITER: int = 30,
COLLIDE_EPSILON: float = 1e-4,
) -> tuple[bool, float, wp.vec3, wp.vec3]:
"""
Solve GJK distance computation between two shapes.
Args:
geom_a: Shape A geometry data
geom_b: Shape B geometry data
orientation_a: Orientation of shape A
orientation_b: Orientation of shape B
position_a: Position of shape A
position_b: Position of shape B
combined_margin: Sum of margin extensions for both shapes [m]
data_provider: Support mapping data provider
MAX_ITER: Maximum number of iterations for GJK algorithm
COLLIDE_EPSILON: Small number for numerical comparisons
Returns:
Tuple of (collision, distance, contact point center, normal)
"""
# Transform into reference frame of body A
relative_orientation_b = wp.quat_inverse(orientation_a) * orientation_b
relative_position_b = wp.quat_rotate_inv(orientation_a, position_b - position_a)
# Perform distance test
result = solve_closest_distance_core(
geom_a,
geom_b,
relative_orientation_b,
relative_position_b,
combined_margin,
data_provider,
MAX_ITER,
COLLIDE_EPSILON,
)
separated, point_a, point_b, normal, distance = result
point = 0.5 * (point_a + point_b)
# Transform results back to world space
point = wp.quat_rotate(orientation_a, point) + position_a
normal = wp.quat_rotate(orientation_a, normal)
# Align semantics with MPR: return collision flag
collision = not separated
return collision, distance, point, normal
solve_closest_distance.core = solve_closest_distance_core
return solve_closest_distance
================================================
FILE: newton/_src/geometry/support_function.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Support mapping functions for collision detection primitives.
This module implements support mapping (also called support functions) for various
geometric primitives. A support mapping finds the furthest point of a shape in a
given direction, which is a fundamental operation for collision detection algorithms
like GJK, MPR, and EPA.
The support mapping operates in the shape's local coordinate frame and returns the
support point (furthest point in the given direction).
Supported primitives:
- Box (axis-aligned rectangular prism)
- Sphere
- Capsule (cylinder with hemispherical caps)
- Ellipsoid
- Cylinder
- Cone
- Plane (finite rectangular plane)
- Convex hull (arbitrary convex mesh)
- Triangle
The module also provides utilities for packing mesh pointers into vectors and
defining generic shape data structures that work across all primitive types.
"""
import enum
import warp as wp
from .types import GeoType
# Is not allowed to share values with GeoType
class GeoTypeEx(enum.IntEnum):
TRIANGLE = 1000
@wp.struct
class SupportMapDataProvider:
"""
Placeholder for data access needed by support mapping (e.g., mesh buffers).
Extend with fields as required by your shapes.
Not needed for Newton but can be helpful for projects like MuJoCo Warp where
the convex hull data is stored in warp arrays that would bloat the GenericShapeData struct.
"""
pass
@wp.func
def pack_mesh_ptr(ptr: wp.uint64) -> wp.vec3:
"""Pack a 64-bit pointer into 3 floats using 22 bits per component"""
# Extract 22-bit chunks from the pointer
chunk1 = float(ptr & wp.uint64(0x3FFFFF)) # bits 0-21
chunk2 = float((ptr >> wp.uint64(22)) & wp.uint64(0x3FFFFF)) # bits 22-43
chunk3 = float((ptr >> wp.uint64(44)) & wp.uint64(0xFFFFF)) # bits 44-63 (20 bits)
return wp.vec3(chunk1, chunk2, chunk3)
@wp.func
def unpack_mesh_ptr(arr: wp.vec3) -> wp.uint64:
"""Unpack 3 floats back into a 64-bit pointer"""
# Convert floats back to integers and combine
chunk1 = wp.uint64(arr[0]) & wp.uint64(0x3FFFFF)
chunk2 = (wp.uint64(arr[1]) & wp.uint64(0x3FFFFF)) << wp.uint64(22)
chunk3 = (wp.uint64(arr[2]) & wp.uint64(0xFFFFF)) << wp.uint64(44)
return chunk1 | chunk2 | chunk3
@wp.struct
class GenericShapeData:
"""
Minimal shape descriptor for support mapping.
Fields:
- shape_type: matches values from GeoType
- scale: parameter encoding per primitive
- BOX: half-extents (x, y, z)
- SPHERE: radius in x
- CAPSULE: radius in x, half-height in y (axis +Z)
- ELLIPSOID: semi-axes (x, y, z)
- CYLINDER: radius in x, half-height in y (axis +Z)
- CONE: radius in x, half-height in y (axis +Z, apex at +Z)
- PLANE: half-width in x, half-length in y (lies in XY plane at z=0, normal along +Z)
- TRIANGLE: vertex B-A stored in scale, vertex C-A stored in auxiliary
"""
shape_type: int
scale: wp.vec3
auxiliary: wp.vec3
@wp.func
def support_map(geom: GenericShapeData, direction: wp.vec3, data_provider: SupportMapDataProvider) -> wp.vec3:
"""
Return the support point of a primitive in its local frame.
Conventions for `geom.scale` and `geom.auxiliary`:
- BOX: half-extents in x/y/z
- SPHERE: radius in x component
- CAPSULE: radius in x, half-height in y (axis along +Z)
- ELLIPSOID: semi-axes in x/y/z
- CYLINDER: radius in x, half-height in y (axis along +Z)
- CONE: radius in x, half-height in y (axis along +Z, apex at +Z)
- PLANE: half-width in x, half-length in y (lies in XY plane at z=0, normal along +Z)
- CONVEX_MESH: scale contains mesh scale, auxiliary contains packed mesh pointer
- TRIANGLE: scale contains vector B-A, auxiliary contains vector C-A (relative to vertex A at origin)
"""
eps = 1.0e-12
result = wp.vec3(0.0, 0.0, 0.0)
if geom.shape_type == GeoType.CONVEX_MESH:
# Convex hull support: find the furthest point in the direction
mesh_ptr = unpack_mesh_ptr(geom.auxiliary)
mesh = wp.mesh_get(mesh_ptr)
mesh_scale = geom.scale
num_verts = mesh.points.shape[0]
# Pre-scale direction: dot(scale*v, d) == dot(v, scale*d)
# This moves the per-vertex cw_mul out of the loop (only 1 at the end)
scaled_dir = wp.cw_mul(direction, mesh_scale)
max_dot = float(-1.0e10)
best_idx = int(0)
for i in range(num_verts):
dot_val = wp.dot(mesh.points[i], scaled_dir)
if dot_val > max_dot:
max_dot = dot_val
best_idx = i
result = wp.cw_mul(mesh.points[best_idx], mesh_scale)
elif geom.shape_type == GeoTypeEx.TRIANGLE:
# Triangle vertices: a at origin, b at scale, c at auxiliary
tri_a = wp.vec3(0.0, 0.0, 0.0)
tri_b = geom.scale
tri_c = geom.auxiliary
# Compute dot products with direction for each vertex
dot_a = wp.dot(tri_a, direction)
dot_b = wp.dot(tri_b, direction)
dot_c = wp.dot(tri_c, direction)
# Find the vertex with maximum dot product (furthest in the direction)
if dot_a >= dot_b and dot_a >= dot_c:
result = tri_a
elif dot_b >= dot_c:
result = tri_b
else:
result = tri_c
elif geom.shape_type == GeoType.BOX:
sx = 1.0 if direction[0] >= 0.0 else -1.0
sy = 1.0 if direction[1] >= 0.0 else -1.0
sz = 1.0 if direction[2] >= 0.0 else -1.0
result = wp.vec3(sx * geom.scale[0], sy * geom.scale[1], sz * geom.scale[2])
elif geom.shape_type == GeoType.SPHERE:
radius = geom.scale[0]
dir_len_sq = wp.length_sq(direction)
if dir_len_sq > eps:
n = wp.normalize(direction)
else:
n = wp.vec3(1.0, 0.0, 0.0)
result = n * radius
elif geom.shape_type == GeoType.CAPSULE:
radius = geom.scale[0]
half_height = geom.scale[1]
# Capsule = segment + sphere (adapted from C# code to Z-axis convention)
# Sphere part: support in normalized direction
dir_len_sq = wp.length_sq(direction)
if dir_len_sq > eps:
n = wp.normalize(direction)
else:
n = wp.vec3(1.0, 0.0, 0.0)
result = n * radius
# Segment endpoints are at (0, 0, +half_height) and (0, 0, -half_height)
# Use sign of Z-component to pick the correct endpoint
if direction[2] >= 0.0:
result = result + wp.vec3(0.0, 0.0, half_height)
else:
result = result + wp.vec3(0.0, 0.0, -half_height)
elif geom.shape_type == GeoType.ELLIPSOID:
# Ellipsoid support for semi-axes a, b, c in direction d:
# p* = (a^2 dx, b^2 dy, c^2 dz) / sqrt((a dx)^2 + (b dy)^2 + (c dz)^2)
a = geom.scale[0]
b = geom.scale[1]
c = geom.scale[2]
dir_len_sq = wp.length_sq(direction)
if dir_len_sq > eps:
adx = a * direction[0]
bdy = b * direction[1]
cdz = c * direction[2]
denom_sq = adx * adx + bdy * bdy + cdz * cdz
if denom_sq > eps:
denom = wp.sqrt(denom_sq)
result = wp.vec3(
(a * a) * direction[0] / denom, (b * b) * direction[1] / denom, (c * c) * direction[2] / denom
)
else:
result = wp.vec3(a, 0.0, 0.0)
else:
result = wp.vec3(a, 0.0, 0.0)
elif geom.shape_type == GeoType.CYLINDER:
radius = geom.scale[0]
half_height = geom.scale[1]
# Cylinder support: project direction to XY plane for lateral surface
dir_xy = wp.vec3(direction[0], direction[1], 0.0)
dir_xy_len_sq = wp.length_sq(dir_xy)
if dir_xy_len_sq > eps:
n_xy = wp.normalize(dir_xy)
lateral_point = wp.vec3(n_xy[0] * radius, n_xy[1] * radius, 0.0)
else:
lateral_point = wp.vec3(radius, 0.0, 0.0)
# Choose between top cap, bottom cap, or lateral surface
if direction[2] > 0.0:
result = wp.vec3(lateral_point[0], lateral_point[1], half_height)
elif direction[2] < 0.0:
result = wp.vec3(lateral_point[0], lateral_point[1], -half_height)
else:
result = lateral_point
elif geom.shape_type == GeoType.CONE:
radius = geom.scale[0]
half_height = geom.scale[1]
# Cone support: apex at +Z, base disk at z=-half_height.
# Using slope k = radius / (2*half_height), the optimal support is:
# apex if dz >= k * ||d_xy||, otherwise base rim in d_xy direction.
apex = wp.vec3(0.0, 0.0, half_height)
dir_xy = wp.vec3(direction[0], direction[1], 0.0)
dir_xy_len = wp.length(dir_xy)
k = radius / (2.0 * half_height) if half_height > eps else 0.0
if dir_xy_len <= eps:
# Purely vertical direction
if direction[2] >= 0.0:
result = apex
else:
result = wp.vec3(radius, 0.0, -half_height)
else:
if direction[2] >= k * dir_xy_len:
result = apex
else:
n_xy = dir_xy / dir_xy_len
result = wp.vec3(n_xy[0] * radius, n_xy[1] * radius, -half_height)
elif geom.shape_type == GeoType.PLANE:
# Finite plane support: rectangular plane in XY, extents in scale[0] (half-width X) and scale[1] (half-length Y)
# The plane lies at z=0 with normal along +Z
half_width = geom.scale[0]
half_length = geom.scale[1]
# Clamp the direction to the plane boundaries
sx = 1.0 if direction[0] >= 0.0 else -1.0
sy = 1.0 if direction[1] >= 0.0 else -1.0
# The support point is at the corner in the XY plane (z=0)
result = wp.vec3(sx * half_width, sy * half_length, 0.0)
else:
# Unhandled type: return origin
result = wp.vec3(0.0, 0.0, 0.0)
return result
@wp.func
def support_map_lean(geom: GenericShapeData, direction: wp.vec3, data_provider: SupportMapDataProvider) -> wp.vec3:
"""
Lean support function for common shape types only: CONVEX_MESH, BOX, SPHERE.
This is a specialized version of support_map with reduced code size to improve
GPU instruction cache utilization. It omits support for CAPSULE, ELLIPSOID,
CYLINDER, CONE, PLANE, and TRIANGLE shapes.
"""
result = wp.vec3(0.0, 0.0, 0.0)
if geom.shape_type == GeoType.CONVEX_MESH:
mesh_ptr = unpack_mesh_ptr(geom.auxiliary)
mesh = wp.mesh_get(mesh_ptr)
scaled_dir = wp.cw_mul(direction, geom.scale)
max_dot = float(-1.0e10)
best_idx = int(0)
for i in range(mesh.points.shape[0]):
dot_val = wp.dot(mesh.points[i], scaled_dir)
if dot_val > max_dot:
max_dot = dot_val
best_idx = i
result = wp.cw_mul(mesh.points[best_idx], geom.scale)
elif geom.shape_type == GeoType.BOX:
sx = 1.0 if direction[0] >= 0.0 else -1.0
sy = 1.0 if direction[1] >= 0.0 else -1.0
sz = 1.0 if direction[2] >= 0.0 else -1.0
result = wp.vec3(sx * geom.scale[0], sy * geom.scale[1], sz * geom.scale[2])
elif geom.shape_type == GeoType.SPHERE:
radius = geom.scale[0]
dir_len_sq = wp.length_sq(direction)
if dir_len_sq > 1.0e-12:
n = wp.normalize(direction)
else:
n = wp.vec3(1.0, 0.0, 0.0)
result = n * radius
return result
@wp.func
def extract_shape_data(
shape_idx: int,
shape_transform: wp.array[wp.transform],
shape_types: wp.array[int],
shape_data: wp.array[wp.vec4], # scale (xyz), margin_offset (w) or other data
shape_source: wp.array[wp.uint64],
):
"""
Extract shape data from the narrow phase API arrays.
Args:
shape_idx: Index of the shape
shape_transform: World space transforms (already computed)
shape_types: Shape types
shape_data: Shape data (vec4 - scale xyz, margin_offset w)
shape_source: Source pointers (mesh IDs etc.)
Returns:
tuple: (position, orientation, shape_data, scale, margin_offset)
"""
# Get shape's world transform (already in world space)
X_ws = shape_transform[shape_idx]
position = wp.transform_get_translation(X_ws)
orientation = wp.transform_get_rotation(X_ws)
# Extract scale and margin offset from shape_data.
# shape_data stores scale in xyz and margin offset in w.
data = shape_data[shape_idx]
scale = wp.vec3(data[0], data[1], data[2])
margin_offset = data[3]
# Create generic shape data
result = GenericShapeData()
result.shape_type = shape_types[shape_idx]
result.scale = scale
result.auxiliary = wp.vec3(0.0, 0.0, 0.0)
# For CONVEX_MESH, pack the mesh pointer into auxiliary
if shape_types[shape_idx] == GeoType.CONVEX_MESH:
result.auxiliary = pack_mesh_ptr(shape_source[shape_idx])
return position, orientation, result, scale, margin_offset
================================================
FILE: newton/_src/geometry/terrain_generator.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Compact procedural terrain generator for Newton physics examples.
Provides various terrain generation functions that output Newton-compatible triangle meshes.
Supports creating grids of terrain blocks with different procedural patterns.
"""
from typing import Any
import numpy as np
# ============================================================================
# Helper Functions
# ============================================================================
def _create_box(
size: tuple[float, float, float], position: tuple[float, float, float] | None = None
) -> tuple[np.ndarray, np.ndarray]:
"""Create a box mesh as (vertices, faces) from dimensions and position.
Each face has its own vertices to ensure sharp edges with per-face normals.
Args:
size: (width, depth, height) dimensions of the box
position: (x, y, z) center position of the box. If None, centered at origin.
Returns:
Tuple of (vertices, faces) where vertices is (24, 3) float32 array
and faces is (12, 3) int32 array (will be flattened in caller)
"""
w, d, h = size
half_w, half_d, half_h = w / 2, d / 2, h / 2
# Create separate vertices for each face to get sharp edges
# Each face gets 4 vertices (one per corner)
# Order: bottom, top, front, back, left, right faces
# Bottom face (z = -half_h, normal pointing down)
bottom_vertices = np.array(
[
[-half_w, -half_d, -half_h], # 0: front-left
[half_w, -half_d, -half_h], # 1: front-right
[half_w, half_d, -half_h], # 2: back-right
[-half_w, half_d, -half_h], # 3: back-left
],
dtype=np.float32,
)
# Top face (z = half_h, normal pointing up)
top_vertices = np.array(
[
[-half_w, -half_d, half_h], # 4: front-left
[half_w, -half_d, half_h], # 5: front-right
[half_w, half_d, half_h], # 6: back-right
[-half_w, half_d, half_h], # 7: back-left
],
dtype=np.float32,
)
# Front face (y = -half_d, normal pointing forward)
front_vertices = np.array(
[
[-half_w, -half_d, -half_h], # 8: bottom-left
[half_w, -half_d, -half_h], # 9: bottom-right
[half_w, -half_d, half_h], # 10: top-right
[-half_w, -half_d, half_h], # 11: top-left
],
dtype=np.float32,
)
# Back face (y = half_d, normal pointing backward)
back_vertices = np.array(
[
[half_w, half_d, -half_h], # 12: bottom-right
[-half_w, half_d, -half_h], # 13: bottom-left
[-half_w, half_d, half_h], # 14: top-left
[half_w, half_d, half_h], # 15: top-right
],
dtype=np.float32,
)
# Left face (x = -half_w, normal pointing left)
left_vertices = np.array(
[
[-half_w, half_d, -half_h], # 16: back-bottom
[-half_w, -half_d, -half_h], # 17: front-bottom
[-half_w, -half_d, half_h], # 18: front-top
[-half_w, half_d, half_h], # 19: back-top
],
dtype=np.float32,
)
# Right face (x = half_w, normal pointing right)
right_vertices = np.array(
[
[half_w, -half_d, -half_h], # 20: front-bottom
[half_w, half_d, -half_h], # 21: back-bottom
[half_w, half_d, half_h], # 22: back-top
[half_w, -half_d, half_h], # 23: front-top
],
dtype=np.float32,
)
# Combine all vertices
vertices = np.vstack(
[
bottom_vertices,
top_vertices,
front_vertices,
back_vertices,
left_vertices,
right_vertices,
]
)
# Translate to position if provided
if position is not None:
vertices += np.array(position, dtype=np.float32)
# Define faces (12 triangles for a box)
# Each face is two triangles, counter-clockwise when viewed from outside
# Vertex indices: bottom (0-3), top (4-7), front (8-11), back (12-15), left (16-19), right (20-23)
faces = np.array(
[
# Bottom face (z = -half_h)
[0, 2, 1],
[0, 3, 2],
# Top face (z = half_h)
[4, 5, 6],
[4, 6, 7],
# Front face (y = -half_d)
[8, 9, 10],
[8, 10, 11],
# Back face (y = half_d)
[12, 13, 14],
[12, 14, 15],
# Left face (x = -half_w)
[16, 17, 18],
[16, 18, 19],
# Right face (x = half_w)
[20, 21, 22],
[20, 22, 23],
],
dtype=np.int32,
)
return vertices, faces
# ============================================================================
# Primitive Terrain Functions
# ============================================================================
def _flat_terrain(size: tuple[float, float], height: float = 0.0) -> tuple[np.ndarray, np.ndarray]:
"""Generate a flat plane terrain.
Args:
size: (width, height) size of the terrain plane in meters
height: Z-coordinate height of the terrain plane
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices
"""
x0 = [size[0], size[1], height]
x1 = [size[0], 0.0, height]
x2 = [0.0, size[1], height]
x3 = [0.0, 0.0, height]
vertices = np.array([x0, x1, x2, x3], dtype=np.float32)
faces = np.array([[1, 0, 2], [2, 3, 1]], dtype=np.int32)
return vertices, faces.flatten()
def _pyramid_stairs_terrain(
size: tuple[float, float], step_width: float = 0.5, step_height: float = 0.1, platform_width: float = 1.0
) -> tuple[np.ndarray, np.ndarray]:
"""Generate pyramid stairs terrain with steps converging to center platform.
Args:
size: (width, height) size of the terrain in meters
step_width: Width of each step ring
step_height: Height increment for each step
platform_width: Width of the center platform
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices
"""
meshes = []
center = [size[0] / 2, size[1] / 2, 0.0]
num_steps_x = int((size[0] - platform_width) / (2 * step_width))
num_steps_y = int((size[1] - platform_width) / (2 * step_width))
num_steps = min(num_steps_x, num_steps_y)
# Add ground plane
ground_pos = (center[0], center[1], -step_height / 2)
meshes.append(_create_box((size[0], size[1], step_height), ground_pos))
# Create concentric rectangular steps (including final ring around platform)
for k in range(num_steps + 1):
box_size = (size[0] - 2 * k * step_width, size[1] - 2 * k * step_width)
box_z = center[2] + (k + 1) * step_height / 2.0
box_offset = (k + 0.5) * step_width
box_height = (k + 1) * step_height
# Skip if this would be smaller than the platform
if box_size[0] <= platform_width or box_size[1] <= platform_width:
continue
# Top/bottom/left/right boxes
for dx, dy, sx, sy in [
(0, size[1] / 2 - box_offset, box_size[0], step_width), # top
(0, -size[1] / 2 + box_offset, box_size[0], step_width), # bottom
(size[0] / 2 - box_offset, 0, step_width, box_size[1] - 2 * step_width), # right
(-size[0] / 2 + box_offset, 0, step_width, box_size[1] - 2 * step_width), # left
]:
pos = (center[0] + dx, center[1] + dy, box_z)
meshes.append(_create_box((sx, sy, box_height), pos))
# Center platform (two steps higher than the last step ring)
platform_height = (num_steps + 2) * step_height
box_dims = (platform_width, platform_width, platform_height)
box_pos = (center[0], center[1], center[2] + platform_height / 2)
meshes.append(_create_box(box_dims, box_pos))
return _combine_meshes(meshes)
def _random_grid_terrain(
size: tuple[float, float],
grid_width: float = 0.5,
grid_height_range: tuple[float, float] = (-0.15, 0.15),
platform_width: float | None = None,
seed: int | None = None,
) -> tuple[np.ndarray, np.ndarray]:
"""Generate terrain with randomized height grid cells.
Args:
size: (width, height) size of the terrain in meters
grid_width: Width of each grid cell
grid_height_range: (min_height, max_height) range for random height variation
platform_width: Unused parameter (kept for API compatibility)
seed: Random seed for reproducibility
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices
"""
rng = np.random.default_rng(seed)
num_boxes_x = int(size[0] / grid_width)
num_boxes_y = int(size[1] / grid_width)
# Template box for a grid cell
template_vertices, template_faces = _create_box((grid_width, grid_width, 1.0))
# Create grid with random heights
all_vertices = []
all_faces = []
vertex_count = 0
for ix in range(num_boxes_x):
for it in range(num_boxes_y):
# Position grid cells starting from (0, 0) with proper alignment
x = ix * grid_width + grid_width / 2
y = it * grid_width + grid_width / 2
h_noise = rng.uniform(*grid_height_range)
# Offset vertices (box is centered at origin)
v = template_vertices.copy()
v[:, 0] += x
v[:, 1] += y
v[:, 2] -= 0.5
# Raise top face vertices (indices 4-7) by random height
v[4:8, 2] += h_noise
all_vertices.append(v)
all_faces.append(template_faces + vertex_count)
vertex_count += 24 # Each box has 24 vertices (4 per face, 6 faces)
vertices = np.vstack(all_vertices).astype(np.float32)
faces = np.vstack(all_faces).astype(np.int32)
return vertices, faces.flatten()
def _wave_terrain(
size: tuple[float, float], wave_amplitude: float = 0.3, wave_frequency: float = 2.0, resolution: int = 50
) -> tuple[np.ndarray, np.ndarray]:
"""Generate 2D sine wave terrain with zero boundaries.
Args:
size: (width, height) size of the terrain in meters
wave_amplitude: Amplitude of the sine wave
wave_frequency: Frequency of the sine wave
resolution: Number of grid points per dimension
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices
"""
x = np.linspace(0, size[0], resolution)
y = np.linspace(0, size[1], resolution)
X, Y = np.meshgrid(x, y)
# Create 2D sine pattern that is naturally zero at all boundaries
# sin(n*pi*x/L) is zero at x=0 and x=L for integer n
Z = wave_amplitude * np.sin(wave_frequency * np.pi * X / size[0]) * np.sin(wave_frequency * np.pi * Y / size[1])
# Create vertices and faces
vertices = np.column_stack([X.ravel(), Y.ravel(), Z.ravel()]).astype(np.float32)
faces = []
for i in range(resolution - 1):
for j in range(resolution - 1):
v0 = i * resolution + j
v1 = i * resolution + (j + 1)
v2 = (i + 1) * resolution + j
v3 = (i + 1) * resolution + (j + 1)
# Counter-clockwise winding for upward-facing triangles
faces.append([v0, v1, v2])
faces.append([v2, v1, v3])
return vertices, np.array(faces, dtype=np.int32).flatten()
def _box_terrain(
size: tuple[float, float], box_height: float = 0.5, platform_width: float = 1.5
) -> tuple[np.ndarray, np.ndarray]:
"""Generate terrain with a raised box platform in center.
Args:
size: (width, height) size of the terrain in meters
box_height: Height of the raised platform
platform_width: Width of the raised platform
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices
"""
meshes = []
# Ground plane
ground_pos = (size[0] / 2, size[1] / 2, -0.5)
meshes.append(_create_box((size[0], size[1], 1.0), ground_pos))
# Raised platform
platform_pos = (size[0] / 2, size[1] / 2, box_height / 2 - 0.5)
meshes.append(_create_box((platform_width, platform_width, 1.0 + box_height), platform_pos))
return _combine_meshes(meshes)
def _gap_terrain(
size: tuple[float, float], gap_width: float = 0.8, platform_width: float = 1.2
) -> tuple[np.ndarray, np.ndarray]:
"""Generate terrain with a gap around the center platform.
Args:
size: (width, height) size of the terrain in meters
gap_width: Width of the gap around the platform
platform_width: Width of the center platform
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices
"""
meshes = []
center = (size[0] / 2, size[1] / 2, -0.5)
# Outer border
thickness_x = (size[0] - platform_width - 2 * gap_width) / 2
thickness_y = (size[1] - platform_width - 2 * gap_width) / 2
for dx, dy, sx, sy in [
(0, (size[1] - thickness_y) / 2, size[0], thickness_y), # top
(0, -(size[1] - thickness_y) / 2, size[0], thickness_y), # bottom
((size[0] - thickness_x) / 2, 0, thickness_x, platform_width + 2 * gap_width), # right
(-(size[0] - thickness_x) / 2, 0, thickness_x, platform_width + 2 * gap_width), # left
]:
pos = (center[0] + dx, center[1] + dy, center[2])
meshes.append(_create_box((sx, sy, 1.0), pos))
# Center platform
meshes.append(_create_box((platform_width, platform_width, 1.0), center))
return _combine_meshes(meshes)
# ============================================================================
# Terrain Grid Generator
# ============================================================================
def _heightfield_terrain(
size: tuple[float, float],
heightfield: np.ndarray | None = None,
center_x: float | None = None,
center_y: float | None = None,
ground_z: float = 0.0,
) -> tuple[np.ndarray, np.ndarray]:
"""Generate terrain from a custom heightfield array.
This is a wrapper around heightfield_to_mesh that fits the terrain_funcs signature.
Args:
size: (width, height) size of the terrain block in meters
heightfield: (grid_size, grid_size) array of Z heights. If None, creates flat terrain.
center_x: Center X coordinate. If None, defaults to size[0]/2 to align with other terrain types.
center_y: Center Y coordinate. If None, defaults to size[1]/2 to align with other terrain types.
ground_z: Z coordinate of bottom surface (default: 0.0)
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices (flattened)
"""
if heightfield is None:
# Default to flat terrain if no heightfield provided
return _flat_terrain(size)
# Default center to size/2 to align with other terrain types (which span [0, size])
if center_x is None:
center_x = size[0] / 2
if center_y is None:
center_y = size[1] / 2
vertices, indices = create_mesh_heightfield(
heightfield=heightfield,
extent_x=size[0],
extent_y=size[1],
center_x=center_x,
center_y=center_y,
ground_z=ground_z,
)
return vertices, indices
def create_mesh_terrain(
grid_size: tuple[int, int] = (4, 4),
block_size: tuple[float, float] = (5.0, 5.0),
terrain_types: list[str] | str | object | None = None,
terrain_params: dict[str, dict[str, Any]] | None = None,
seed: int | None = None,
) -> tuple[np.ndarray, np.ndarray]:
"""Generate a grid of procedural terrain blocks.
This is the main public API function for generating terrain grids.
Args:
grid_size: (rows, cols) number of terrain blocks
block_size: (width, height) size of each terrain block in meters
terrain_types: List of terrain type names, single terrain type string,
or callable function (any object with __call__). If None, uses all types.
Available types: 'flat', 'pyramid_stairs', 'random_grid', 'wave', 'box', 'gap', 'heightfield'
terrain_params: Dictionary mapping terrain types to their parameter dicts.
For 'heightfield' type, params should include 'heightfield' (np.ndarray)
seed: Random seed for reproducibility
Returns:
tuple of (vertices, indices) where:
- vertices: (N, 3) float32 array of vertex positions
- indices: (M,) int32 array of triangle indices (flattened)
"""
# Default terrain types
if terrain_types is None:
terrain_types = ["flat", "pyramid_stairs", "random_grid", "wave", "box", "gap"]
terrain_funcs = {
"flat": _flat_terrain,
"pyramid_stairs": _pyramid_stairs_terrain,
"random_grid": _random_grid_terrain,
"wave": _wave_terrain,
"box": _box_terrain,
"gap": _gap_terrain,
"heightfield": _heightfield_terrain,
}
if terrain_params is None:
terrain_params = {}
# Create RNG for deterministic terrain generation
rng = np.random.default_rng(seed) if seed is not None else None
all_vertices = []
all_indices = []
vertex_offset = 0
rows, cols = grid_size
for row in range(rows):
for col in range(cols):
# Select terrain type (cycle or random)
if isinstance(terrain_types, list):
terrain_idx = (row * cols + col) % len(terrain_types)
terrain_name = terrain_types[terrain_idx]
else:
terrain_name = terrain_types
# Get terrain function
if callable(terrain_name):
terrain_func = terrain_name
else:
terrain_func = terrain_funcs[terrain_name]
# Get parameters for this terrain type
params = terrain_params.get(terrain_name, {})
# Forward seed to stochastic terrain functions if not already provided
if rng is not None and terrain_func is _random_grid_terrain and "seed" not in params:
params = dict(params)
params["seed"] = int(rng.integers(0, 2**32))
# Generate terrain block
vertices, indices = terrain_func(block_size, **params)
# Offset to grid position
offset_x = col * block_size[0]
offset_y = row * block_size[1]
vertices[:, 0] += offset_x
vertices[:, 1] += offset_y
# Accumulate geometry
all_vertices.append(vertices)
all_indices.append(indices + vertex_offset)
vertex_offset += len(vertices)
# Combine all blocks
vertices = np.vstack(all_vertices).astype(np.float32)
indices = np.concatenate(all_indices).astype(np.int32)
return vertices, indices
# ============================================================================
# Helper Functions
# ============================================================================
def _combine_meshes(meshes: list[tuple[np.ndarray, np.ndarray]]) -> tuple[np.ndarray, np.ndarray]:
"""Combine multiple (vertices, faces) tuples into a single mesh.
Args:
meshes: List of (vertices, faces) tuples to combine
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices (flattened)
"""
if len(meshes) == 1:
vertices, faces = meshes[0]
return vertices.astype(np.float32), faces.flatten().astype(np.int32)
all_vertices = []
all_faces = []
vertex_offset = 0
for vertices, faces in meshes:
all_vertices.append(vertices)
all_faces.append(faces + vertex_offset)
vertex_offset += len(vertices)
combined_vertices = np.vstack(all_vertices).astype(np.float32)
combined_faces = np.vstack(all_faces).astype(np.int32)
return combined_vertices, combined_faces.flatten()
def _to_newton_mesh(vertices: np.ndarray, indices: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""Convert terrain geometry to Newton mesh format.
This is a convenience function that ensures proper dtypes.
Args:
vertices: (N, 3) array of vertex positions
indices: (M,) array of triangle indices (flattened)
Returns:
tuple of (vertices, indices) with proper dtypes for Newton (float32 and int32)
"""
return vertices.astype(np.float32), indices.astype(np.int32)
def create_mesh_heightfield(
heightfield: np.ndarray,
extent_x: float,
extent_y: float,
center_x: float = 0.0,
center_y: float = 0.0,
ground_z: float = 0.0,
) -> tuple[np.ndarray, np.ndarray]:
"""Convert a 2D heightfield array to a watertight triangle mesh.
Creates a closed mesh with top surface (from heightfield), bottom surface (at ground_z),
and side walls connecting them. Each grid cell becomes two triangles.
Args:
heightfield: (grid_size_x, grid_size_y) array of Z heights for top surface
extent_x: Physical size in X direction (meters)
extent_y: Physical size in Y direction (meters)
center_x: Center X coordinate (default: 0.0)
center_y: Center Y coordinate (default: 0.0)
ground_z: Z coordinate of bottom surface (default: 0.0)
Returns:
tuple of (vertices, indices) where vertices is (N, 3) float32 array
and indices is (M,) int32 array of triangle indices (flattened)
Raises:
ValueError: If heightfield is not a 2D array
ValueError: If heightfield dimensions are smaller than 2x2
ValueError: If extent_x or extent_y are not positive
"""
# Validate heightfield dimensions
if heightfield.ndim != 2:
raise ValueError(f"heightfield must be 2D array, got shape {heightfield.shape}")
grid_size_x, grid_size_y = heightfield.shape
# Validate minimum grid size
if grid_size_x < 2 or grid_size_y < 2:
raise ValueError(f"heightfield must be at least 2x2, got shape ({grid_size_x}, {grid_size_y})")
# Validate extent values
if extent_x <= 0:
raise ValueError(f"extent_x must be positive, got {extent_x}")
if extent_y <= 0:
raise ValueError(f"extent_y must be positive, got {extent_y}")
# Create grid coordinates
x = np.linspace(-extent_x / 2, extent_x / 2, grid_size_x) + center_x
y = np.linspace(-extent_y / 2, extent_y / 2, grid_size_y) + center_y
X, Y = np.meshgrid(x, y, indexing="ij")
# Top and bottom surface vertices
top_vertices = np.column_stack([X.ravel(), Y.ravel(), heightfield.ravel()]).astype(np.float32)
bottom_z = np.full_like(heightfield, ground_z)
bottom_vertices = np.column_stack([X.ravel(), Y.ravel(), bottom_z.ravel()]).astype(np.float32)
vertices = np.vstack([top_vertices, bottom_vertices])
# Generate quad indices for all grid cells
i_indices = np.arange(grid_size_x - 1)
j_indices = np.arange(grid_size_y - 1)
ii, jj = np.meshgrid(i_indices, j_indices, indexing="ij")
ii, jj = ii.ravel(), jj.ravel()
v0 = ii * grid_size_y + jj
v1 = ii * grid_size_y + (jj + 1)
v2 = (ii + 1) * grid_size_y + jj
v3 = (ii + 1) * grid_size_y + (jj + 1)
# Top surface faces (counter-clockwise)
top_faces = np.column_stack([np.column_stack([v0, v2, v1]), np.column_stack([v1, v2, v3])]).reshape(-1, 3)
# Bottom surface faces (clockwise)
num_top_vertices = len(top_vertices)
bottom_faces = np.column_stack(
[
np.column_stack([num_top_vertices + v0, num_top_vertices + v1, num_top_vertices + v2]),
np.column_stack([num_top_vertices + v1, num_top_vertices + v3, num_top_vertices + v2]),
]
).reshape(-1, 3)
# Side wall faces (4 edges)
side_faces_list = []
i_edge = np.arange(grid_size_x - 1)
j_edge = np.arange(grid_size_y - 1)
# Front edge (j=0)
t0, t1 = i_edge * grid_size_y, (i_edge + 1) * grid_size_y
b0, b1 = num_top_vertices + t0, num_top_vertices + t1
side_faces_list.append(
np.column_stack([np.column_stack([t0, b0, t1]), np.column_stack([t1, b0, b1])]).reshape(-1, 3)
)
# Back edge (j=grid_size_y-1)
t0 = i_edge * grid_size_y + (grid_size_y - 1)
t1 = (i_edge + 1) * grid_size_y + (grid_size_y - 1)
b0, b1 = num_top_vertices + t0, num_top_vertices + t1
side_faces_list.append(
np.column_stack([np.column_stack([t0, t1, b0]), np.column_stack([t1, b1, b0])]).reshape(-1, 3)
)
# Left edge (i=0)
t0, t1 = j_edge, j_edge + 1
b0, b1 = num_top_vertices + t0, num_top_vertices + t1
side_faces_list.append(
np.column_stack([np.column_stack([t0, t1, b0]), np.column_stack([t1, b1, b0])]).reshape(-1, 3)
)
# Right edge (i=grid_size_x-1)
t0 = (grid_size_x - 1) * grid_size_y + j_edge
t1 = (grid_size_x - 1) * grid_size_y + (j_edge + 1)
b0, b1 = num_top_vertices + t0, num_top_vertices + t1
side_faces_list.append(
np.column_stack([np.column_stack([t0, b0, t1]), np.column_stack([t1, b0, b1])]).reshape(-1, 3)
)
# Combine all faces
all_faces = np.vstack([top_faces, bottom_faces, *side_faces_list])
indices = all_faces.astype(np.int32).flatten()
return vertices, indices
================================================
FILE: newton/_src/geometry/types.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import enum
import os
import warnings
from collections.abc import Sequence
from typing import TYPE_CHECKING
import numpy as np
import warp as wp
from ..core.types import Axis, Devicelike, Vec2, Vec3, override
from ..utils.texture import compute_texture_hash
if TYPE_CHECKING:
from ..sim.model import Model
from .sdf_utils import SDF
def _normalize_texture_input(texture: str | os.PathLike[str] | np.ndarray | None) -> str | np.ndarray | None:
"""Normalize texture input for lazy storage.
String paths and PathLike objects are stored as strings (no decoding).
Arrays are normalized to contiguous arrays.
Decoding of paths is deferred until the viewer requests the image data.
"""
if texture is None:
return None
if isinstance(texture, os.PathLike):
return os.fspath(texture)
if isinstance(texture, str):
return texture
# Array input: make it contiguous
return np.ascontiguousarray(np.asarray(texture))
class GeoType(enum.IntEnum):
"""
Enumeration of geometric shape types supported in Newton.
Each member represents a different primitive or mesh-based geometry
that can be used for collision, rendering, or simulation.
"""
NONE = 0
"""No geometry (placeholder)."""
PLANE = 1
"""Plane."""
HFIELD = 2
"""Height field (terrain)."""
SPHERE = 3
"""Sphere."""
CAPSULE = 4
"""Capsule (cylinder with hemispherical ends)."""
ELLIPSOID = 5
"""Ellipsoid."""
CYLINDER = 6
"""Cylinder."""
BOX = 7
"""Axis-aligned box."""
MESH = 8
"""Triangle mesh."""
CONE = 9
"""Cone."""
CONVEX_MESH = 10
"""Convex hull."""
GAUSSIAN = 11
"""Gaussian splat."""
class Mesh:
"""
Represents a triangle mesh for collision and simulation.
This class encapsulates a triangle mesh, including its geometry, physical properties,
and utility methods for simulation. Meshes are typically used for collision detection,
visualization, and inertia computation in physics simulation.
Attributes:
mass [kg]: Mesh mass in local coordinates, computed with density 1.0 when
``compute_inertia`` is ``True``.
com [m]: Mesh center of mass in local coordinates.
inertia [kg*m^2]: Mesh inertia tensor about :attr:`com` in local coordinates.
Example:
Load a mesh from an OBJ file using OpenMesh and create a Newton Mesh:
.. code-block:: python
import numpy as np
import newton
import openmesh
m = openmesh.read_trimesh("mesh.obj")
mesh_points = np.array(m.points())
mesh_indices = np.array(m.face_vertex_indices(), dtype=np.int32).flatten()
mesh = newton.Mesh(mesh_points, mesh_indices)
"""
MAX_HULL_VERTICES = 64
"""Default maximum vertex count for convex hull approximation."""
def __init__(
self,
vertices: Sequence[Vec3] | np.ndarray,
indices: Sequence[int] | np.ndarray,
normals: Sequence[Vec3] | np.ndarray | None = None,
uvs: Sequence[Vec2] | np.ndarray | None = None,
compute_inertia: bool = True,
is_solid: bool = True,
maxhullvert: int | None = None,
color: Vec3 | None = None,
roughness: float | None = None,
metallic: float | None = None,
texture: str | np.ndarray | None = None,
*,
sdf: "SDF | None" = None,
):
"""
Construct a Mesh object from a triangle mesh.
The mesh's center of mass and inertia tensor are automatically calculated
using a density of 1.0 if ``compute_inertia`` is True. This computation is only valid
if the mesh is closed (two-manifold).
Args:
vertices: List or array of mesh vertices, shape (N, 3).
indices: Flattened list or array of triangle indices (3 per triangle).
normals: Optional per-vertex normals, shape (N, 3).
uvs: Optional per-vertex UVs, shape (N, 2).
compute_inertia: If True, compute mass, inertia tensor, and center of mass (default: True).
is_solid: If True, mesh is assumed solid for inertia computation (default: True).
maxhullvert: Max vertices for convex hull approximation (default: :attr:`~newton.Mesh.MAX_HULL_VERTICES`).
color: Optional per-mesh base color (values in [0, 1]).
roughness: Optional mesh roughness in [0, 1].
metallic: Optional mesh metallic in [0, 1].
texture: Optional texture path/URL or image data (H, W, C).
sdf: Optional prebuilt SDF object owned by this mesh.
"""
from .inertia import compute_inertia_mesh # noqa: PLC0415
self._vertices = np.array(vertices, dtype=np.float32).reshape(-1, 3)
self._indices = np.array(indices, dtype=np.int32).flatten()
self._normals = np.array(normals, dtype=np.float32).reshape(-1, 3) if normals is not None else None
self._uvs = np.array(uvs, dtype=np.float32).reshape(-1, 2) if uvs is not None else None
self._color: Vec3 | None = None
self.color = color
# Store texture lazily: strings/paths are kept as-is, arrays are normalized
self._texture = _normalize_texture_input(texture)
self._roughness = roughness
self._metallic = metallic
self.is_solid = is_solid
self.has_inertia = compute_inertia
self.mesh = None
if maxhullvert is None:
maxhullvert = Mesh.MAX_HULL_VERTICES
self.maxhullvert = maxhullvert
self._cached_hash = None
self._texture_hash = None
self.sdf = sdf
if compute_inertia:
self.mass, self.com, self.inertia, _ = compute_inertia_mesh(1.0, vertices, indices, is_solid=is_solid)
else:
self.inertia = wp.mat33(np.eye(3))
self.mass = 1.0
self.com = wp.vec3()
@staticmethod
def create_sphere(
radius: float = 1.0,
*,
num_latitudes: int = 32,
num_longitudes: int = 32,
reverse_winding: bool = False,
compute_normals: bool = True,
compute_uvs: bool = True,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a UV sphere mesh.
Args:
radius [m]: Sphere radius.
num_latitudes: Number of latitude subdivisions.
num_longitudes: Number of longitude subdivisions.
reverse_winding: If ``True``, reverse triangle winding order.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
A sphere mesh.
"""
from ..utils.mesh import create_mesh_sphere # noqa: PLC0415
positions, indices, normals, uvs = create_mesh_sphere(
radius,
num_latitudes=num_latitudes,
num_longitudes=num_longitudes,
reverse_winding=reverse_winding,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
return Mesh(
vertices=positions,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
)
@staticmethod
def create_ellipsoid(
rx: float = 1.0,
ry: float = 1.0,
rz: float = 1.0,
*,
num_latitudes: int = 32,
num_longitudes: int = 32,
reverse_winding: bool = False,
compute_normals: bool = True,
compute_uvs: bool = True,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a UV ellipsoid mesh.
Args:
rx [m]: Semi-axis length along X.
ry [m]: Semi-axis length along Y.
rz [m]: Semi-axis length along Z.
num_latitudes: Number of latitude subdivisions.
num_longitudes: Number of longitude subdivisions.
reverse_winding: If ``True``, reverse triangle winding order.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
An ellipsoid mesh.
"""
from ..utils.mesh import create_mesh_ellipsoid # noqa: PLC0415
positions, indices, normals, uvs = create_mesh_ellipsoid(
rx,
ry,
rz,
num_latitudes=num_latitudes,
num_longitudes=num_longitudes,
reverse_winding=reverse_winding,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
return Mesh(
vertices=positions,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
)
@staticmethod
def create_capsule(
radius: float,
half_height: float,
*,
up_axis: Axis = Axis.Y,
segments: int = 32,
compute_normals: bool = True,
compute_uvs: bool = True,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a capsule mesh.
Args:
radius [m]: Radius of the capsule hemispheres and cylindrical body.
half_height [m]: Half-height of the cylindrical section.
up_axis: Long axis as a ``newton.Axis`` value.
segments: Tessellation resolution for both caps and body.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
A capsule mesh.
"""
from ..utils.mesh import create_mesh_capsule # noqa: PLC0415
positions, indices, normals, uvs = create_mesh_capsule(
radius,
half_height,
up_axis=int(up_axis),
segments=segments,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
return Mesh(
vertices=positions,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
)
@staticmethod
def create_cylinder(
radius: float,
half_height: float,
*,
up_axis: Axis = Axis.Y,
segments: int = 32,
top_radius: float | None = None,
compute_normals: bool = True,
compute_uvs: bool = True,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a cylinder or truncated cone mesh.
Args:
radius [m]: Bottom radius.
half_height [m]: Half-height along the cylinder axis.
up_axis: Long axis as a ``newton.Axis`` value.
segments: Circumferential tessellation resolution.
top_radius [m]: Optional top radius. If ``None``, equals ``radius``.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
A cylinder or truncated-cone mesh.
"""
from ..utils.mesh import create_mesh_cylinder # noqa: PLC0415
positions, indices, normals, uvs = create_mesh_cylinder(
radius,
half_height,
up_axis=int(up_axis),
segments=segments,
top_radius=top_radius,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
return Mesh(
vertices=positions,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
)
@staticmethod
def create_cone(
radius: float,
half_height: float,
*,
up_axis: Axis = Axis.Y,
segments: int = 32,
compute_normals: bool = True,
compute_uvs: bool = True,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a cone mesh.
Args:
radius [m]: Base radius.
half_height [m]: Half-height from center to apex/base.
up_axis: Long axis as a ``newton.Axis`` value.
segments: Circumferential tessellation resolution.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
A cone mesh.
"""
from ..utils.mesh import create_mesh_cone # noqa: PLC0415
positions, indices, normals, uvs = create_mesh_cone(
radius,
half_height,
up_axis=int(up_axis),
segments=segments,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
return Mesh(
vertices=positions,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
)
@staticmethod
def create_arrow(
base_radius: float,
base_height: float,
*,
cap_radius: float | None = None,
cap_height: float | None = None,
up_axis: Axis = Axis.Y,
segments: int = 32,
compute_normals: bool = True,
compute_uvs: bool = True,
compute_inertia: bool = True,
) -> "Mesh":
"""Create an arrow mesh (cylinder shaft + cone head).
Args:
base_radius [m]: Shaft radius.
base_height [m]: Shaft full height (not half-height).
cap_radius [m]: Optional arrowhead base radius. If ``None``, uses
``base_radius * 1.8``.
cap_height [m]: Optional arrowhead full height (not half-height).
If ``None``, uses ``base_height * 0.18``.
up_axis: Long axis as a ``newton.Axis`` value.
segments: Circumferential tessellation resolution.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
An arrow mesh.
"""
from ..utils.mesh import create_mesh_arrow # noqa: PLC0415
positions, indices, normals, uvs = create_mesh_arrow(
base_radius,
base_height,
cap_radius=cap_radius,
cap_height=cap_height,
up_axis=int(up_axis),
segments=segments,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
return Mesh(
vertices=positions,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
)
@staticmethod
def create_box(
hx: float,
hy: float | None = None,
hz: float | None = None,
*,
duplicate_vertices: bool = True,
compute_normals: bool = True,
compute_uvs: bool = True,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a box mesh from half-extents.
Args:
hx [m]: Half-extent along X.
hy [m]: Half-extent along Y. If ``None``, uses ``hx``.
hz [m]: Half-extent along Z. If ``None``, uses ``hx``.
duplicate_vertices: If ``True``, duplicate vertices per face.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
A box mesh.
"""
from ..utils.mesh import create_mesh_box # noqa: PLC0415
if hy is None:
hy = hx
if hz is None:
hz = hx
positions, indices, normals, uvs = create_mesh_box(
float(hx),
float(hy),
float(hz),
duplicate_vertices=duplicate_vertices,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
return Mesh(
vertices=positions,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
)
@staticmethod
def create_plane(
width: float,
length: float,
*,
compute_normals: bool = True,
compute_uvs: bool = True,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a rectangular plane mesh.
The plane lies in the XY plane and faces +Z (normals point along +Z).
Args:
width [m]: Plane width along X.
length [m]: Plane length along Y.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
A plane mesh.
"""
from ..utils.mesh import create_mesh_plane # noqa: PLC0415
positions, indices, normals, uvs = create_mesh_plane(
width,
length,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
return Mesh(
vertices=positions,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
)
@staticmethod
def create_terrain(
grid_size: tuple[int, int] = (4, 4),
block_size: tuple[float, float] = (5.0, 5.0),
terrain_types: list[str] | str | object | None = None,
terrain_params: dict | None = None,
seed: int | None = None,
*,
compute_normals: bool = False,
compute_uvs: bool = False,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a procedural terrain mesh from terrain blocks.
Args:
grid_size: Terrain grid size as ``(rows, cols)``.
block_size [m]: Terrain block dimensions as ``(width, length)``.
terrain_types: Terrain type name(s) or callable generator(s).
terrain_params: Optional per-terrain parameter dictionary.
seed: Optional random seed for deterministic terrain generation.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
A terrain mesh.
"""
from .terrain_generator import create_mesh_terrain # noqa: PLC0415
vertices, indices = create_mesh_terrain(
grid_size=grid_size,
block_size=block_size,
terrain_types=terrain_types,
terrain_params=terrain_params,
seed=seed,
)
normals = None
uvs = None
if compute_normals:
from ..utils.mesh import compute_vertex_normals # noqa: PLC0415
normals = compute_vertex_normals(vertices, indices).astype(np.float32)
if compute_uvs:
total_x = grid_size[1] * block_size[0]
total_y = grid_size[0] * block_size[1]
uvs = np.column_stack(
[
vertices[:, 0] / total_x if total_x > 0 else np.zeros(len(vertices)),
vertices[:, 1] / total_y if total_y > 0 else np.zeros(len(vertices)),
]
).astype(np.float32)
return Mesh(vertices, indices, normals=normals, uvs=uvs, compute_inertia=compute_inertia)
@staticmethod
def create_heightfield(
heightfield: np.ndarray,
extent_x: float,
extent_y: float,
center_x: float = 0.0,
center_y: float = 0.0,
ground_z: float = 0.0,
*,
compute_normals: bool = False,
compute_uvs: bool = False,
compute_inertia: bool = True,
) -> "Mesh":
"""Create a watertight mesh from a 2D heightfield.
Args:
heightfield: Height samples as a 2D array using ij-indexing where
``heightfield[i, j]`` maps to ``(x_i, y_j)`` (i = X, j = Y).
extent_x [m]: Total extent along X.
extent_y [m]: Total extent along Y.
center_x [m]: Heightfield center position along X.
center_y [m]: Heightfield center position along Y.
ground_z [m]: Bottom surface Z value for watertight side walls.
compute_normals: If ``True``, generate per-vertex normals.
compute_uvs: If ``True``, generate per-vertex UV coordinates.
compute_inertia: If ``True``, compute mesh mass properties.
Returns:
A heightfield mesh.
"""
from .terrain_generator import create_mesh_heightfield # noqa: PLC0415
vertices, indices = create_mesh_heightfield(
heightfield=heightfield,
extent_x=extent_x,
extent_y=extent_y,
center_x=center_x,
center_y=center_y,
ground_z=ground_z,
)
normals = None
uvs = None
if compute_normals:
from ..utils.mesh import compute_vertex_normals # noqa: PLC0415
normals = compute_vertex_normals(vertices, indices).astype(np.float32)
if compute_uvs:
num_top = len(vertices) // 2
u = (vertices[:, 0] - (center_x - extent_x / 2)) / extent_x
v = (vertices[:, 1] - (center_y - extent_y / 2)) / extent_y
uvs = np.column_stack([u, v]).astype(np.float32)
uvs[:num_top] = np.clip(uvs[:num_top], 0.0, 1.0)
return Mesh(vertices, indices, normals=normals, uvs=uvs, compute_inertia=compute_inertia)
def copy(
self,
vertices: Sequence[Vec3] | np.ndarray | None = None,
indices: Sequence[int] | np.ndarray | None = None,
recompute_inertia: bool = False,
):
"""
Create a copy of this mesh, optionally with new vertices or indices.
Args:
vertices: New vertices to use (default: current vertices).
indices: New indices to use (default: current indices).
recompute_inertia: If True, recompute inertia properties (default: False).
Returns:
A new Mesh object with the specified properties.
"""
if vertices is None:
vertices = self.vertices.copy()
if indices is None:
indices = self.indices.copy()
m = Mesh(
vertices,
indices,
compute_inertia=recompute_inertia,
is_solid=self.is_solid,
maxhullvert=self.maxhullvert,
normals=self.normals.copy() if self.normals is not None else None,
uvs=self.uvs.copy() if self.uvs is not None else None,
color=self.color,
texture=self._texture
if isinstance(self._texture, str)
else (self._texture.copy() if self._texture is not None else None),
roughness=self._roughness,
metallic=self._metallic,
)
if not recompute_inertia:
m.inertia = self.inertia
m.mass = self.mass
m.com = self.com
m.has_inertia = self.has_inertia
m.sdf = self.sdf
return m
def build_sdf(
self,
*,
device: Devicelike | None = None,
narrow_band_range: tuple[float, float] | None = None,
target_voxel_size: float | None = None,
max_resolution: int | None = None,
margin: float | None = None,
shape_margin: float = 0.0,
scale: tuple[float, float, float] | None = None,
texture_format: str = "uint16",
) -> "SDF":
"""Build and attach an SDF for this mesh.
Args:
device: CUDA device for SDF allocation. When ``None``, uses the
current :class:`wp.ScopedDevice` or the Warp default device.
narrow_band_range: Signed narrow-band distance range [m] as
``(inner, outer)``. Uses ``(-0.1, 0.1)`` when not provided.
target_voxel_size: Target sparse-grid voxel size [m]. If provided,
takes precedence over ``max_resolution``.
max_resolution: Maximum sparse-grid dimension [voxel] along the longest
AABB axis, used when ``target_voxel_size`` is not provided. Must be
divisible by 8.
margin: Extra AABB padding [m] added before discretization. Uses
``0.05`` when not provided.
shape_margin: Shape margin offset [m] to subtract from SDF values.
When non-zero, the SDF surface is effectively shrunk inward by
this amount. Useful for modeling compliant layers in hydroelastic
collision. Defaults to ``0.0``.
scale: Scale factors ``(sx, sy, sz)`` to bake into the SDF. When
provided, the mesh vertices are scaled before SDF generation
and ``scale_baked`` is set to ``True`` in the resulting SDF.
Required for hydroelastic collision with non-unit shape scale.
Defaults to ``None`` (no scale baking, scale applied at runtime).
texture_format: Subgrid texture storage format for the SDF.
``"uint16"`` (default) stores subgrid voxels in 16-bit
normalized textures (half the memory of float32).
``"float32"`` stores full-precision values. ``"uint8"`` uses
8-bit textures for minimum memory at lower precision.
Returns:
The attached :class:`SDF` instance.
Raises:
RuntimeError: If this mesh already has an SDF attached.
"""
if self.sdf is not None:
raise RuntimeError("Mesh already has an SDF. Call clear_sdf() before rebuilding.")
_valid_tex_fmts = ("float32", "uint16", "uint8")
if texture_format not in _valid_tex_fmts:
raise ValueError(f"Unknown texture_format {texture_format!r}. Expected one of {list(_valid_tex_fmts)}.")
from .sdf_utils import SDF # noqa: PLC0415
self.sdf = SDF.create_from_mesh(
self,
device=device,
narrow_band_range=narrow_band_range if narrow_band_range is not None else (-0.1, 0.1),
target_voxel_size=target_voxel_size,
max_resolution=max_resolution,
margin=margin if margin is not None else 0.05,
shape_margin=shape_margin,
scale=scale,
texture_format=texture_format,
)
return self.sdf
def clear_sdf(self) -> None:
"""Detach and release the currently attached SDF.
Returns:
``None``.
"""
self.sdf = None
@property
def vertices(self):
return self._vertices
@vertices.setter
def vertices(self, value):
self._vertices = np.array(value, dtype=np.float32).reshape(-1, 3)
self._cached_hash = None
@property
def indices(self):
return self._indices
@indices.setter
def indices(self, value):
self._indices = np.array(value, dtype=np.int32).flatten()
self._cached_hash = None
@property
def normals(self):
return self._normals
@property
def uvs(self):
return self._uvs
@property
def color(self) -> Vec3 | None:
"""Optional display RGB color with values in [0, 1]."""
return self._color
@color.setter
def color(self, value: Vec3 | None):
self._color = value
@property
def texture(self) -> str | np.ndarray | None:
"""Optional texture as a file path or a normalized RGBA array."""
return self._texture
@texture.setter
def texture(self, value: str | np.ndarray | None):
# Store texture lazily: strings/paths are kept as-is, arrays are normalized
self._texture = _normalize_texture_input(value)
self._texture_hash = None
self._cached_hash = None
@property
def texture_hash(self) -> int:
"""Content-based hash of the assigned texture.
Returns a stable integer hash derived from the texture data.
The value is lazily computed and cached until :attr:`~newton.Mesh.texture`
is reassigned.
"""
return self._compute_texture_hash()
def _compute_texture_hash(self) -> int:
if self._texture_hash is None:
self._texture_hash = compute_texture_hash(self._texture)
return self._texture_hash
@property
def roughness(self) -> float | None:
return self._roughness
@roughness.setter
def roughness(self, value: float | None):
self._roughness = value
self._cached_hash = None
@property
def metallic(self) -> float | None:
return self._metallic
@metallic.setter
def metallic(self, value: float | None):
self._metallic = value
self._cached_hash = None
# construct simulation ready buffers from points
def finalize(self, device: Devicelike = None, requires_grad: bool = False) -> wp.uint64:
"""
Construct a simulation-ready Warp Mesh object from the mesh data and return its ID.
Args:
device: Device on which to allocate mesh buffers.
requires_grad: If True, mesh points and velocities are allocated with gradient tracking.
Returns:
The ID of the simulation-ready Warp Mesh.
"""
with wp.ScopedDevice(device):
pos = wp.array(self.vertices, requires_grad=requires_grad, dtype=wp.vec3)
vel = wp.zeros_like(pos)
indices = wp.array(self.indices, dtype=wp.int32)
self.mesh = wp.Mesh(points=pos, velocities=vel, indices=indices)
return self.mesh.id
def compute_convex_hull(self, replace: bool = False) -> "Mesh":
"""
Compute and return the convex hull of this mesh.
Args:
replace: If True, replace this mesh's vertices/indices with the convex hull (in-place).
If False, return a new Mesh for the convex hull.
Returns:
The convex hull mesh (either new or self, depending on `replace`).
"""
from .utils import remesh_convex_hull # noqa: PLC0415
hull_vertices, hull_faces = remesh_convex_hull(self.vertices, maxhullvert=self.maxhullvert)
if replace:
self.vertices = hull_vertices
self.indices = hull_faces
return self
else:
# create a new mesh for the convex hull
hull_mesh = Mesh(hull_vertices, hull_faces, compute_inertia=False)
hull_mesh.maxhullvert = self.maxhullvert # preserve maxhullvert setting
hull_mesh.is_solid = self.is_solid
hull_mesh.has_inertia = self.has_inertia
hull_mesh.mass = self.mass
hull_mesh.com = self.com
hull_mesh.inertia = self.inertia
return hull_mesh
@override
def __hash__(self) -> int:
"""
Compute a hash of the mesh data for use in caching.
The hash considers the mesh vertices, indices, and whether the mesh is solid.
Uses a cached hash if available, otherwise computes and caches the hash.
Returns:
The hash value for the mesh.
"""
if self._cached_hash is None:
self._cached_hash = hash(
(
tuple(np.array(self.vertices).flatten()),
tuple(np.array(self.indices).flatten()),
self.is_solid,
self._compute_texture_hash(),
self._roughness,
self._metallic,
)
)
return self._cached_hash
# ---- Factory methods ---------------------------------------------------
@staticmethod
def create_from_usd(prim, **kwargs) -> "Mesh":
"""Load a Mesh from a USD prim with the ``UsdGeom.Mesh`` schema.
This is a convenience wrapper around :func:`newton.usd.get_mesh`.
See that function for full documentation.
Args:
prim: The USD prim to load the mesh from.
**kwargs: Additional arguments passed to :func:`newton.usd.get_mesh`
(e.g. ``load_normals``, ``load_uvs``).
Returns:
Mesh: A new Mesh instance.
"""
from ..usd.utils import get_mesh # noqa: PLC0415
result = get_mesh(prim, **kwargs)
if isinstance(result, tuple):
return result[0]
return result
@staticmethod
def create_from_file(filename: str, method: str | None = None, **kwargs) -> "Mesh":
"""Load a Mesh from a 3D model file.
Supports common surface mesh formats including OBJ, PLY, STL, and
other formats supported by trimesh, meshio, openmesh, or pcu.
Args:
filename: Path to the mesh file.
method: Loading backend to use (``"trimesh"``, ``"meshio"``,
``"pcu"``, ``"openmesh"``). If ``None``, each backend is
tried in order until one succeeds.
**kwargs: Additional arguments passed to the :class:`Mesh`
constructor (e.g. ``compute_inertia``, ``is_solid``).
Returns:
Mesh: A new Mesh instance.
"""
if not os.path.exists(filename):
raise FileNotFoundError(f"File not found: {filename}")
from .utils import load_mesh # noqa: PLC0415
mesh_points, mesh_indices = load_mesh(filename, method=method)
return Mesh(vertices=mesh_points, indices=mesh_indices, **kwargs)
class TetMesh:
"""Represents a tetrahedral mesh for volumetric deformable simulation.
Stores vertex positions (surface + interior nodes), tetrahedral element
connectivity, and an optional surface triangle mesh. If no surface mesh
is provided, it is automatically computed from the open (unshared) faces
of the tetrahedra.
Optionally carries per-element material arrays and a density value loaded
from file. These are used as defaults by builder methods and can be
overridden at instantiation time.
Example:
Create a TetMesh from raw arrays:
.. code-block:: python
import numpy as np
import newton
vertices = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]], dtype=np.float32)
tet_indices = np.array([0, 1, 2, 3], dtype=np.int32)
tet_mesh = newton.TetMesh(vertices, tet_indices)
"""
_RESERVED_ATTR_KEYS = frozenset({"vertices", "tet_indices", "k_mu", "k_lambda", "k_damp", "density"})
def __init__(
self,
vertices: Sequence[Vec3] | np.ndarray,
tet_indices: Sequence[int] | np.ndarray,
k_mu: np.ndarray | float | None = None,
k_lambda: np.ndarray | float | None = None,
k_damp: np.ndarray | float | None = None,
density: float | None = None,
custom_attributes: (
"dict[str, np.ndarray] | dict[str, tuple[np.ndarray, Model.AttributeFrequency]] | None"
) = None,
):
"""Construct a TetMesh from vertex positions and tet connectivity.
Args:
vertices: Vertex positions [m], shape (N, 3).
tet_indices: Tetrahedral element indices, flattened (4 per tet).
k_mu: First elastic Lame parameter [Pa]. Scalar (uniform) or
per-element array of shape (tet_count,).
k_lambda: Second elastic Lame parameter [Pa]. Scalar (uniform) or
per-element array of shape (tet_count,).
k_damp: Rayleigh damping coefficient [-] (dimensionless). Scalar
(uniform) or per-element array of shape (tet_count,).
density: Uniform density [kg/m^3] for mass computation.
custom_attributes: Dictionary of named custom arrays with their
:class:`~newton.Model.AttributeFrequency`. Each value can be
either a bare array (frequency auto-inferred from length) or a
``(array, frequency)`` tuple.
"""
self._vertices = np.array(vertices, dtype=np.float32).reshape(-1, 3)
self._tet_indices = np.array(tet_indices, dtype=np.int32).flatten()
if len(self._tet_indices) % 4 != 0:
raise ValueError(f"tet_indices length must be a multiple of 4, got {len(self._tet_indices)}.")
vertex_count = len(self._vertices)
if len(self._tet_indices) > 0:
idx_min = int(self._tet_indices.min())
idx_max = int(self._tet_indices.max())
if idx_min < 0:
raise ValueError(f"tet_indices contains negative index {idx_min}.")
if idx_max >= vertex_count:
raise ValueError(f"tet_indices contains index {idx_max} which exceeds vertex count {vertex_count}.")
tet_count = len(self._tet_indices) // 4
self._k_mu = self._broadcast_material(k_mu, tet_count, "k_mu")
self._k_lambda = self._broadcast_material(k_lambda, tet_count, "k_lambda")
self._k_damp = self._broadcast_material(k_damp, tet_count, "k_damp")
self._density = density
# Compute surface triangles from boundary faces (before custom attrs so tri_count is available)
self._surface_tri_indices = self._compute_surface_triangles()
tri_count = len(self._surface_tri_indices) // 3
self.custom_attributes: dict[str, tuple[np.ndarray, int]] = {}
for k, v in (custom_attributes or {}).items():
if k in self._RESERVED_ATTR_KEYS:
raise ValueError(
f"Custom attribute name '{k}' is reserved. Reserved names: {sorted(self._RESERVED_ATTR_KEYS)}"
)
if isinstance(v, tuple):
arr, freq = v
self.custom_attributes[k] = (np.asarray(arr), freq)
else:
arr = np.asarray(v)
freq = self._infer_frequency(arr, vertex_count, tet_count, tri_count, k)
self.custom_attributes[k] = (arr, freq)
self._cached_hash: int | None = None
@staticmethod
def _broadcast_material(value: np.ndarray | float | None, tet_count: int, name: str) -> np.ndarray | None:
if value is None:
return None
arr = np.asarray(value, dtype=np.float32)
if arr.ndim == 0:
return np.full(tet_count, arr.item(), dtype=np.float32)
arr = arr.flatten()
if len(arr) == 1:
return np.full(tet_count, arr[0], dtype=np.float32)
if len(arr) != tet_count:
raise ValueError(f"{name} array length ({len(arr)}) does not match tet count ({tet_count}).")
return arr
@staticmethod
def _infer_frequency(
arr: np.ndarray, vertex_count: int, tet_count: int, tri_count: int, name: str
) -> "Model.AttributeFrequency":
"""Infer :class:`~newton.Model.AttributeFrequency` from array length.
Args:
arr: The attribute array.
vertex_count: Number of vertices in the mesh.
tet_count: Number of tetrahedra in the mesh.
tri_count: Number of surface triangles in the mesh.
name: Attribute name (for error messages).
Returns:
The inferred frequency.
Raises:
ValueError: If the array length is ambiguous (matches multiple
counts) or matches none of the known counts.
"""
from ..sim.model import Model # noqa: PLC0415
first_dim = arr.shape[0] if arr.ndim >= 1 else 1
counts = {"vertex_count": vertex_count, "tet_count": tet_count, "tri_count": tri_count}
matches = [label for label, c in counts.items() if first_dim == c and c > 0]
if len(matches) > 1:
raise ValueError(
f"Cannot infer frequency for custom attribute '{name}': array length {first_dim} matches "
f"{', '.join(matches)}. Pass an explicit (array, frequency) tuple instead."
)
if first_dim == vertex_count and vertex_count > 0:
return Model.AttributeFrequency.PARTICLE
if first_dim == tet_count and tet_count > 0:
return Model.AttributeFrequency.TETRAHEDRON
if first_dim == tri_count and tri_count > 0:
return Model.AttributeFrequency.TRIANGLE
raise ValueError(
f"Cannot infer frequency for custom attribute '{name}': array length {first_dim} matches none of "
f"vertex_count ({vertex_count}), tet_count ({tet_count}), tri_count ({tri_count}). "
f"Pass an explicit (array, frequency) tuple instead."
)
@staticmethod
def compute_surface_triangles(tet_indices: np.ndarray) -> np.ndarray:
"""Extract boundary triangles from tetrahedral element indices.
Finds faces that belong to exactly one tetrahedron (boundary faces)
using a vectorized approach.
Args:
tet_indices: Flattened tetrahedral element indices (4 per tet).
Returns:
Flattened boundary triangle indices, 3 per triangle, int32.
"""
tet_indices = np.asarray(tet_indices, dtype=np.int32).flatten()
tets = tet_indices.reshape(-1, 4)
n = len(tets)
if n == 0:
return np.array([], dtype=np.int32)
# Each tet contributes 4 faces with specific winding order:
# face 0: (v0, v2, v1)
# face 1: (v1, v2, v3)
# face 2: (v0, v1, v3)
# face 3: (v0, v3, v2)
# fmt: off
face_idx = np.array([
[0, 2, 1],
[1, 2, 3],
[0, 1, 3],
[0, 3, 2],
])
# fmt: on
# Build all faces: shape (4*n, 3) with original winding
all_faces = tets[:, face_idx].reshape(-1, 3)
# Sort vertex indices per face to create canonical keys
sorted_faces = np.sort(all_faces, axis=1)
# Find unique sorted faces and their counts
_, inverse, counts = np.unique(sorted_faces, axis=0, return_inverse=True, return_counts=True)
# Boundary faces appear exactly once
boundary_mask = counts[inverse] == 1
return all_faces[boundary_mask].astype(np.int32).flatten()
def _compute_surface_triangles(self) -> np.ndarray:
return TetMesh.compute_surface_triangles(self._tet_indices)
# ---- Properties --------------------------------------------------------
@property
def vertices(self) -> np.ndarray:
"""Vertex positions [m], shape (N, 3), float32."""
return self._vertices
@property
def tet_indices(self) -> np.ndarray:
"""Tetrahedral element indices, flattened, 4 per tet."""
return self._tet_indices
@property
def tet_count(self) -> int:
"""Number of tetrahedral elements."""
return len(self._tet_indices) // 4
@property
def vertex_count(self) -> int:
"""Number of vertices."""
return len(self._vertices)
@property
def surface_tri_indices(self) -> np.ndarray:
"""Surface triangle indices (open faces), flattened, 3 per tri.
Automatically computed from tet connectivity at construction time
by extracting boundary faces (faces belonging to exactly one tet).
"""
return self._surface_tri_indices
@property
def k_mu(self) -> np.ndarray | None:
"""Per-element first Lame parameter [Pa], shape (tet_count,) or None."""
return self._k_mu
@property
def k_lambda(self) -> np.ndarray | None:
"""Per-element second Lame parameter [Pa], shape (tet_count,) or None."""
return self._k_lambda
@property
def k_damp(self) -> np.ndarray | None:
"""Per-element Rayleigh damping coefficient [-], shape (tet_count,) or None."""
return self._k_damp
@property
def density(self) -> float | None:
"""Uniform density [kg/m^3] or None."""
return self._density
# ---- Factory methods ---------------------------------------------------
@staticmethod
def create_from_usd(prim) -> "TetMesh":
"""Load a tetrahedral mesh from a USD prim with the ``UsdGeom.TetMesh`` schema.
Reads vertex positions from the ``points`` attribute and tetrahedral
connectivity from ``tetVertexIndices``. If a physics material is bound
to the prim (via ``material:binding:physics``) and contains
``youngsModulus``, ``poissonsRatio``, or ``density`` attributes
(under the ``omniphysics:`` or ``physxDeformableBody:`` namespaces),
those values are read and converted to Lame parameters (``k_mu``,
``k_lambda``) and density on the returned TetMesh. Material properties
are set to ``None`` if not present.
Example:
.. code-block:: python
from pxr import Usd
import newton
import newton.usd
usd_stage = Usd.Stage.Open("tetmesh.usda")
tetmesh = newton.usd.get_tetmesh(usd_stage.GetPrimAtPath("/MyTetMesh"))
# tetmesh.vertices -- np.ndarray, shape (N, 3)
# tetmesh.tet_indices -- np.ndarray, flattened (4 per tet)
Args:
prim: The USD prim to load the tetrahedral mesh from.
Returns:
TetMesh: A :class:`newton.TetMesh` with vertex positions and tet connectivity.
"""
from ..usd.utils import get_tetmesh # noqa: PLC0415
return get_tetmesh(prim)
@staticmethod
def create_from_file(filename: str) -> "TetMesh":
"""Load a TetMesh from a volumetric mesh file.
Supports ``.vtk``, ``.msh``, ``.vtu``, and other formats with
tetrahedral cells via meshio. Also supports ``.npz`` files saved
by :meth:`TetMesh.save` (numpy only, no extra dependencies).
Args:
filename: Path to the volumetric mesh file.
Returns:
TetMesh: A new TetMesh instance.
"""
if not os.path.exists(filename):
raise FileNotFoundError(f"File not found: {filename}")
ext = os.path.splitext(filename)[1].lower()
if ext == ".npz":
data = np.load(filename)
kwargs = {}
for key in ("k_mu", "k_lambda", "k_damp"):
if key in data:
kwargs[key] = data[key]
if "density" in data:
kwargs["density"] = float(data["density"])
known_keys = {
"vertices",
"tet_indices",
"k_mu",
"k_lambda",
"k_damp",
"density",
"__custom_names__",
"__custom_freqs__",
}
freq_map: dict[str, int] = {}
if "__custom_names__" in data and "__custom_freqs__" in data:
from ..sim.model import Model as _Model # noqa: PLC0415
names = data["__custom_names__"]
freqs = data["__custom_freqs__"]
for n, f in zip(names, freqs, strict=True):
freq_map[str(n)] = int(f)
custom: dict[str, np.ndarray | tuple] = {}
for k in data.files:
if k not in known_keys:
arr = np.asarray(data[k])
if k in freq_map:
from ..sim.model import Model as _Model # noqa: PLC0415
custom[k] = (arr, _Model.AttributeFrequency(freq_map[k]))
else:
custom[k] = arr
if custom:
kwargs["custom_attributes"] = custom
return TetMesh(
vertices=data["vertices"],
tet_indices=data["tet_indices"],
**kwargs,
)
import meshio
m = meshio.read(filename)
# Find tetrahedral cells
tet_indices = None
tet_cell_idx = None
for i, cell_block in enumerate(m.cells):
if cell_block.type == "tetra":
tet_indices = np.array(cell_block.data, dtype=np.int32).flatten()
tet_cell_idx = i
break
if tet_indices is None:
raise ValueError(f"No tetrahedral cells found in '{filename}'.")
vertices = np.array(m.points, dtype=np.float32)
# Read material arrays from cell data
kwargs: dict = {}
material_keys = {"k_mu", "k_lambda", "k_damp", "density"}
if m.cell_data and tet_cell_idx is not None:
for key in material_keys:
if key in m.cell_data:
arr = np.asarray(m.cell_data[key][tet_cell_idx], dtype=np.float32)
if key == "density":
if arr.size > 1 and not np.allclose(arr, arr[0]):
raise ValueError(
f"Non-uniform per-element density found in '{filename}'. "
f"TetMesh only supports a single uniform density value."
)
kwargs["density"] = float(arr[0])
else:
kwargs[key] = arr
# Read custom attributes from cell data and point data
from ..sim.model import Model as _Model # noqa: PLC0415
custom: dict[str, tuple[np.ndarray, _Model.AttributeFrequency]] = {}
if m.cell_data and tet_cell_idx is not None:
for key, arrays in m.cell_data.items():
if key not in material_keys:
custom[key] = (np.asarray(arrays[tet_cell_idx]), _Model.AttributeFrequency.TETRAHEDRON)
if m.point_data:
for key, arr in m.point_data.items():
custom[key] = (np.asarray(arr), _Model.AttributeFrequency.PARTICLE)
if custom:
kwargs["custom_attributes"] = custom
return TetMesh(vertices=vertices, tet_indices=tet_indices, **kwargs)
def save(self, filename: str):
"""Save the TetMesh to a file.
For ``.npz``, saves all arrays via :func:`numpy.savez` (no extra
dependencies). For other formats (``.vtk``, ``.msh``, ``.vtu``,
etc.), uses meshio.
Args:
filename: Path to write the file to.
"""
ext = os.path.splitext(filename)[1].lower()
if ext == ".npz":
save_dict = {
"vertices": self._vertices,
"tet_indices": self._tet_indices,
}
if self._k_mu is not None:
save_dict["k_mu"] = self._k_mu
if self._k_lambda is not None:
save_dict["k_lambda"] = self._k_lambda
if self._k_damp is not None:
save_dict["k_damp"] = self._k_damp
if self._density is not None:
save_dict["density"] = np.array(self._density)
custom_names = []
custom_freqs = []
for k, (arr, freq) in self.custom_attributes.items():
save_dict[k] = arr
custom_names.append(k)
custom_freqs.append(int(freq))
if custom_names:
save_dict["__custom_names__"] = np.array(custom_names)
save_dict["__custom_freqs__"] = np.array(custom_freqs, dtype=np.int32)
np.savez(filename, **save_dict)
return
import meshio
cells = [("tetra", self._tet_indices.reshape(-1, 4))]
cell_data: dict[str, list[np.ndarray]] = {}
point_data: dict[str, np.ndarray] = {}
# Save material arrays as cell data
for name, arr in [("k_mu", self._k_mu), ("k_lambda", self._k_lambda), ("k_damp", self._k_damp)]:
if arr is not None:
cell_data[name] = [arr]
if self._density is not None:
cell_data["density"] = [np.full(self.tet_count, self._density, dtype=np.float32)]
# Save custom attributes as point or cell data based on frequency
from ..sim.model import Model as _Model # noqa: PLC0415
for name, (arr, freq) in self.custom_attributes.items():
if freq == _Model.AttributeFrequency.TETRAHEDRON:
cell_data[name] = [arr]
elif freq == _Model.AttributeFrequency.PARTICLE:
point_data[name] = arr
else:
warnings.warn(
f"Custom attribute '{name}' with frequency {freq} cannot be saved to meshio format "
f"(only PARTICLE and TETRAHEDRON are supported). Skipping.",
stacklevel=2,
)
mesh = meshio.Mesh(
points=self._vertices,
cells=cells,
cell_data=cell_data if cell_data else {},
point_data=point_data if point_data else {},
)
mesh.write(filename)
def __eq__(self, other):
if not isinstance(other, TetMesh):
return NotImplemented
return np.array_equal(self._vertices, other._vertices) and np.array_equal(self._tet_indices, other._tet_indices)
def __hash__(self):
if self._cached_hash is None:
self._cached_hash = hash((self._vertices.tobytes(), self._tet_indices.tobytes()))
return self._cached_hash
class Heightfield:
"""
Represents a heightfield (2D elevation grid) for terrain and large static surfaces.
Heightfields are efficient representations of terrain using a 2D grid of elevation values.
They are always static (zero mass, zero inertia) and more memory-efficient than equivalent
triangle meshes.
The elevation data is always normalized to [0, 1] internally. World-space heights are
computed as: ``z = min_z + data[r, c] * (max_z - min_z)``.
Example:
Create a heightfield from raw elevation data (auto-normalizes):
.. code-block:: python
import numpy as np
import newton
nrow, ncol = 10, 10
elevation = np.random.rand(nrow, ncol).astype(np.float32) * 5.0 # 0-5 meters
hfield = newton.Heightfield(
data=elevation,
nrow=nrow,
ncol=ncol,
hx=5.0, # half-extent X (field spans [-5, +5] meters)
hy=5.0, # half-extent Y
)
# min_z and max_z are auto-derived from the data (0.0 and 5.0)
Create with explicit height range:
.. code-block:: python
hfield = newton.Heightfield(
data=normalized_data, # any values, will be normalized
nrow=nrow,
ncol=ncol,
hx=5.0,
hy=5.0,
min_z=-1.0,
max_z=3.0,
)
"""
def __init__(
self,
data: Sequence[Sequence[float]] | np.ndarray,
nrow: int,
ncol: int,
hx: float = 1.0,
hy: float = 1.0,
min_z: float | None = None,
max_z: float | None = None,
):
"""
Construct a Heightfield object from a 2D elevation grid.
The input data is normalized to [0, 1]. If ``min_z`` and ``max_z`` are not provided,
they are derived from the data's minimum and maximum values.
Args:
data: 2D array of elevation values, shape (nrow, ncol). Any numeric values are
accepted and will be normalized to [0, 1] internally.
nrow: Number of rows in the heightfield grid.
ncol: Number of columns in the heightfield grid.
hx: Half-extent in X direction. The heightfield spans [-hx, +hx].
hy: Half-extent in Y direction. The heightfield spans [-hy, +hy].
min_z: World-space Z value corresponding to data minimum. Must be provided
together with ``max_z``, or both omitted to auto-derive from data.
max_z: World-space Z value corresponding to data maximum. Must be provided
together with ``min_z``, or both omitted to auto-derive from data.
"""
if nrow < 2 or ncol < 2:
raise ValueError(f"Heightfield requires nrow >= 2 and ncol >= 2, got nrow={nrow}, ncol={ncol}")
if (min_z is None) != (max_z is None):
raise ValueError("min_z and max_z must both be provided or both omitted")
raw = np.array(data, dtype=np.float32).reshape(nrow, ncol)
d_min, d_max = float(raw.min()), float(raw.max())
# Normalize data to [0, 1]
if d_max > d_min:
self._data = (raw - d_min) / (d_max - d_min)
else:
self._data = np.zeros_like(raw)
self.nrow = nrow
self.ncol = ncol
self.hx = hx
self.hy = hy
self.min_z = d_min if min_z is None else float(min_z)
self.max_z = d_max if max_z is None else float(max_z)
self.is_solid = True
self.has_inertia = False
self._cached_hash = None
# Heightfields are always static
self.inertia = wp.mat33()
self.mass = 0.0
self.com = wp.vec3()
@property
def data(self):
"""Get the normalized [0, 1] elevation data as a 2D numpy array."""
return self._data
@data.setter
def data(self, value):
"""Set the elevation data from a 2D array. Data is normalized to [0, 1]."""
raw = np.array(value, dtype=np.float32).reshape(self.nrow, self.ncol)
d_min, d_max = float(raw.min()), float(raw.max())
if d_max > d_min:
self._data = (raw - d_min) / (d_max - d_min)
else:
self._data = np.zeros_like(raw)
self.min_z = d_min
self.max_z = d_max
self._cached_hash = None
@override
def __hash__(self) -> int:
"""
Compute a hash of the heightfield data for use in caching.
Returns:
The hash value for the heightfield.
"""
if self._cached_hash is None:
self._cached_hash = hash(
(
tuple(self._data.flatten()),
self.nrow,
self.ncol,
self.hx,
self.hy,
self.min_z,
self.max_z,
)
)
return self._cached_hash
class Gaussian:
"""Represents a Gaussian splat asset for rendering and rigid body attachment.
A Gaussian splat is a collection of oriented, scaled 3D Gaussians with
appearance data (color via spherical harmonics or flat RGB). Gaussian
objects can be attached to rigid bodies as a shape type (``GeoType.GAUSSIAN``)
for rendering, with collision handled by an optional proxy geometry.
Example:
Load a Gaussian splat from a ``.ply`` file and inspect it:
.. code-block:: python
import newton
gaussian = newton.Gaussian.create_from_ply("object.ply")
print(gaussian.count, gaussian.sh_degree)
"""
@wp.struct
class Data:
num_points: wp.int32
transforms: wp.array[wp.transformf]
scales: wp.array[wp.vec3f]
opacities: wp.array[wp.float32]
sh_coeffs: wp.array2d[wp.float32]
bvh_id: wp.uint64
min_response: wp.float32
def __init__(
self,
positions: np.ndarray,
rotations: np.ndarray | None = None,
scales: np.ndarray | None = None,
opacities: np.ndarray | None = None,
sh_coeffs: np.ndarray | None = None,
sh_degree: int | None = None,
min_response: float = 0.1,
):
"""Construct a Gaussian splat asset from arrays.
Args:
positions: Gaussian centers in local space [m], shape ``(N, 3)``, float.
rotations: Quaternion orientations ``(x, y, z, w)``, shape ``(N, 4)``, float.
If ``None``, defaults to identity quaternions.
scales: Per-axis scales (linear), shape ``(N, 3)``, float.
If ``None``, defaults to ones.
opacities: Opacity values ``[0, 1]``, shape ``(N,)``, float.
If ``None``, defaults to ones (fully opaque).
sh_coeffs: Spherical harmonic coefficients, shape ``(N, C)``, float.
The number of coefficients *C* determines the SH degree
(``C = 3`` -> degree 0, ``C = 12`` -> degree 1, etc.).
sh_degree: Spherical harmonic degree.
min_response: Minimum response required for alpha testing.
"""
self._positions = np.ascontiguousarray(np.asarray(positions, dtype=np.float32).reshape(-1, 3))
n = self._positions.shape[0]
if rotations is not None:
self._rotations = np.ascontiguousarray(np.asarray(rotations, dtype=np.float32).reshape(n, 4))
else:
self._rotations = np.tile(np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), (n, 1))
if scales is not None:
self._scales = np.ascontiguousarray(np.asarray(scales, dtype=np.float32).reshape(n, 3))
else:
self._scales = np.ones((n, 3), dtype=np.float32)
if opacities is not None:
self._opacities = np.ascontiguousarray(np.asarray(opacities, dtype=np.float32).reshape(n))
else:
self._opacities = np.ones(n, dtype=np.float32)
if sh_coeffs is not None:
self._sh_coeffs = np.ascontiguousarray(np.asarray(sh_coeffs, dtype=np.float32).reshape(n, -1))
else:
self._sh_coeffs = np.ones((n, 3), dtype=np.float32)
if sh_degree is not None:
self._sh_degree = sh_degree
else:
self._sh_degree = self._find_sh_degree()
if not np.isfinite(min_response) or not (0.0 < min_response < 1.0):
raise ValueError("min_response must be finite and in (0, 1)")
self._min_response = float(min_response)
self._cached_hash = None
self._positions.setflags(write=False)
self._rotations.setflags(write=False)
self._scales.setflags(write=False)
self._opacities.setflags(write=False)
self._sh_coeffs.setflags(write=False)
# GPU arrays populated by finalize()
self.warp_bvh: wp.Bvh = None
self.warp_data: Gaussian.Data = None
# Inertia: Gaussians are render-only so they contribute no mass
self.has_inertia = False
self.mass = 0.0
self.com = wp.vec3()
self.I = wp.mat33()
self.is_solid = False
# ---- Properties ----------------------------------------------------------
@property
def count(self) -> int:
"""Number of Gaussians in this asset."""
return self._positions.shape[0]
@property
def positions(self) -> np.ndarray:
"""Gaussian centers in local space [m], shape ``(N, 3)``, float."""
return self._positions
@property
def rotations(self) -> np.ndarray:
"""Quaternion orientations ``(x, y, z, w)``, shape ``(N, 4)``, float."""
return self._rotations
@property
def scales(self) -> np.ndarray:
"""Per-axis linear scales, shape ``(N, 3)``, float."""
return self._scales
@property
def opacities(self) -> np.ndarray:
"""Opacity values ``[0, 1]``, shape ``(N,)``, float."""
return self._opacities
@property
def sh_coeffs(self) -> np.ndarray | None:
"""Spherical harmonic coefficients, shape ``(N, C)``, float."""
return self._sh_coeffs
@property
def sh_degree(self) -> int:
"""Spherical harmonics degree (0-3), int"""
return self._sh_degree
@property
def min_response(self) -> float:
"""Min response, float."""
return self._min_response
def _find_sh_degree(self) -> int:
"""Spherical harmonics degree (0-3), inferred from *sh_coeffs* shape."""
c = self._sh_coeffs.shape[1]
# SH bands: degree 0 -> 1*3=3, degree 1 -> 4*3=12,
# degree 2 -> 9*3=27, degree 3 -> 16*3=48
for deg, num in ((3, 48), (2, 27), (1, 12), (0, 3)):
if c >= num:
return deg
return 0
# ---- Finalize (GPU upload) -----------------------------------------------
def finalize(self, device: Devicelike = None) -> Data:
"""Upload Gaussian data to the GPU as Warp arrays.
Args:
device: Device on which to allocate buffers.
Returns:
Gaussian.Data struct containing the Warp arrays.
"""
from ..sensors.warp_raytrace.gaussians import compute_gaussian_bvh_bounds # noqa: PLC0415
with wp.ScopedDevice(device):
self.warp_data = Gaussian.Data()
self.warp_data.transforms = wp.array(
np.append(self._positions, self._rotations, axis=1), dtype=wp.transformf
)
self.warp_data.scales = wp.array(self._scales, dtype=wp.vec3f)
self.warp_data.opacities = wp.array(self._opacities, dtype=wp.float32)
self.warp_data.sh_coeffs = wp.array(self._sh_coeffs, dtype=wp.float32)
self.warp_data.min_response = self.min_response
self.warp_data.num_points = self.warp_data.transforms.shape[0]
lowers = wp.zeros(self.count, dtype=wp.vec3f)
uppers = wp.zeros(self.count, dtype=wp.vec3f)
wp.launch(
kernel=compute_gaussian_bvh_bounds,
dim=self.count,
inputs=[self.warp_data, lowers, uppers],
)
self.warp_bvh = wp.Bvh(lowers, uppers)
self.warp_data.bvh_id = self.warp_bvh.id
return self.warp_data
# ---- Factory methods -----------------------------------------------------
@staticmethod
def create_from_ply(filename: str, min_response: float = 0.1) -> "Gaussian":
"""Load Gaussian splat data from a ``.ply`` file (standard 3DGS format).
Reads positions (``x/y/z``), rotations (``rot_0..3``), scales
(``scale_0..2``, stored as log-scale), opacities (logit-space),
and SH coefficients (``f_dc_*``, ``f_rest_*``). Converts log-scale
and logit-opacity to linear values.
Args:
filename: Path to a ``.ply`` file in standard 3DGS format.
min_response: Min response (default = 0.1).
Returns:
A new :class:`Gaussian` instance.
"""
import open3d as o3d
pcd = o3d.t.io.read_point_cloud(filename)
point_attrs = {name: np.asarray(tensor.numpy()) for name, tensor in pcd.point.items()}
positions = point_attrs.get("positions")
if positions is None:
raise ValueError("PLY Gaussian point cloud is missing required 'positions' attribute")
positions = np.ascontiguousarray(np.asarray(positions, dtype=np.float32).reshape(-1, 3))
def _get_point_attr(name: str, width: int | None = None) -> np.ndarray | None:
values = point_attrs.get(name)
if values is None:
return None
values = np.asarray(values, dtype=np.float32)
if width is None:
return np.ascontiguousarray(values.reshape(-1))
return np.ascontiguousarray(values.reshape(-1, width))
def _require_point_attr(name: str, message: str) -> np.ndarray:
values = _get_point_attr(name)
if values is None:
raise ValueError(message)
return values
# Rotations (quaternion w,x,y,z)
if "rot_0" in point_attrs:
missing_rotation = "PLY Gaussian point cloud is missing one or more rotation attributes"
rot_0 = _require_point_attr("rot_0", missing_rotation)
rot_1 = _require_point_attr("rot_1", missing_rotation)
rot_2 = _require_point_attr("rot_2", missing_rotation)
rot_3 = _require_point_attr("rot_3", missing_rotation)
rotations = np.stack([rot_1, rot_2, rot_3, rot_0], axis=1).astype(np.float32)
rotations /= np.maximum(np.linalg.norm(rotations, axis=1, keepdims=True), 1e-12)
else:
rotations = None
# Scales (stored as log-scale in standard 3DGS)
if "scale_0" in point_attrs:
missing_scale = "PLY Gaussian point cloud is missing one or more scale attributes"
scale_0 = _require_point_attr("scale_0", missing_scale)
scale_1 = _require_point_attr("scale_1", missing_scale)
scale_2 = _require_point_attr("scale_2", missing_scale)
log_scales = np.stack([scale_0, scale_1, scale_2], axis=1).astype(np.float32)
scales = np.exp(log_scales)
else:
scales = None
# Opacities (stored in logit-space in standard 3DGS)
if "opacity" in point_attrs:
logit_opacities = _get_point_attr("opacity")
opacities = 1.0 / (1.0 + np.exp(-logit_opacities))
else:
opacities = None
# Spherical harmonic coefficients
sh_dc_names = [f"f_dc_{i}" for i in range(3)]
has_sh_dc = all(name in point_attrs for name in sh_dc_names)
sh_coeffs = None
if has_sh_dc:
sh_dc = np.stack(
[
_require_point_attr(name, "PLY Gaussian point cloud is missing SH DC attributes")
for name in sh_dc_names
],
axis=1,
).astype(np.float32)
rest_names = []
i = 0
while f"f_rest_{i}" in point_attrs:
rest_names.append(f"f_rest_{i}")
i += 1
if rest_names:
sh_rest = np.stack(
[
_require_point_attr(name, "PLY Gaussian point cloud is missing SH rest attributes")
for name in rest_names
],
axis=1,
).astype(np.float32)
sh_coeffs = np.concatenate([sh_dc, sh_rest], axis=1)
else:
sh_coeffs = sh_dc
return Gaussian(
positions=positions,
rotations=rotations,
scales=scales,
opacities=opacities,
sh_coeffs=sh_coeffs,
min_response=min_response,
)
@staticmethod
def create_from_usd(prim, min_response: float = 0.1) -> "Gaussian":
"""Load Gaussian splat data from a USD prim.
Reads positions from attributes: `positions`, `orientations`, `scales`, `opacities` and `radiance:sphericalHarmonicsCoefficients`.
Args:
prim: A USD prim containing Gaussian splat data.
min_response: Min response (default = 0.1).
Returns:
A new :class:`Gaussian` instance.
"""
from ..usd.utils import get_gaussian # noqa: PLC0415
return get_gaussian(prim, min_response=min_response)
# ---- Utility -------------------------------------------------------------
def compute_aabb(self) -> tuple[np.ndarray, np.ndarray]:
"""Compute axis-aligned bounding box of Gaussian centers.
Returns:
Tuple of ``(lower, upper)`` as ``(3,)`` arrays [m].
"""
lower = self._positions.min(axis=0)
upper = self._positions.max(axis=0)
return lower, upper
def compute_proxy_mesh(self, method: str = "convex_hull") -> "Mesh":
"""Generate a proxy collision :class:`Mesh` from Gaussian positions.
Args:
method: ``"convex_hull"`` (default) or ``"alphashape"`` or ``"points"``.
Returns:
A :class:`Mesh` for use as collision proxy.
"""
if method == "convex_hull":
from .utils import remesh_convex_hull # noqa: PLC0415
hull_verts, hull_faces = remesh_convex_hull(self._positions)
return Mesh(hull_verts, hull_faces, compute_inertia=True)
elif method == "alphashape":
from .utils import remesh_alphashape # noqa: PLC0415
hull_verts, hull_faces = remesh_alphashape(self._positions)
return Mesh(hull_verts, hull_faces, compute_inertia=True)
elif method == "points":
return self.compute_points_mesh()
raise ValueError(
f"Unsupported proxy mesh method: {method!r}. Supported: 'convex_hull', 'alphashape', 'points'."
)
def compute_points_mesh(self) -> "Mesh":
from ..utils.mesh import create_mesh_box # noqa: PLC0415
mesh_points, mesh_indices, _normals, _uvs = create_mesh_box(
1.0,
1.0,
1.0,
duplicate_vertices=False,
compute_normals=False,
compute_uvs=False,
)
points = (
(self.positions[: self.count][:, None] + self.scales[: self.count][:, None] * mesh_points)
.flatten()
.reshape(-1, 3)
)
offsets = mesh_points.shape[0] * np.arange(self.count)
indices = (offsets[:, None] + mesh_indices).flatten()
return Mesh(vertices=points, indices=indices)
@override
def __hash__(self) -> int:
if self._cached_hash is None:
self._cached_hash = hash(
(
self._positions.data.tobytes(),
self._rotations.data.tobytes(),
self._scales.data.tobytes(),
self._opacities.data.tobytes(),
self._sh_coeffs.data.tobytes(),
float(self._min_response),
)
)
return self._cached_hash
@override
def __eq__(self, other: object) -> bool:
if not isinstance(other, Gaussian):
return NotImplemented
return (
np.array_equal(self._positions, other._positions)
and np.array_equal(self._rotations, other._rotations)
and np.array_equal(self._scales, other._scales)
and np.array_equal(self._opacities, other._opacities)
and np.array_equal(self._sh_coeffs, other._sh_coeffs)
and self._min_response == other._min_response
)
================================================
FILE: newton/_src/geometry/utils.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import contextlib
import os
import warnings
from collections import defaultdict
from typing import Any, Literal
import numpy as np
import warp as wp
from ..core.types import Vec3
from .inertia import compute_inertia_mesh
from .types import (
GeoType,
Heightfield,
Mesh,
)
# Warp kernel for inertia-based OBB computation
@wp.kernel(enable_backward=False)
def compute_obb_candidates(
vertices: wp.array[wp.vec3],
base_quat: wp.quat,
volumes: wp.array2d[float],
transforms: wp.array2d[wp.transform],
extents: wp.array2d[wp.vec3],
):
"""Compute OBB candidates for different rotations around principal axes."""
angle_idx, axis_idx = wp.tid()
num_angles_per_axis = volumes.shape[0]
# Compute rotation angle around one of the principal axes (X=0, Y=1, Z=2)
angle = float(angle_idx) * (2.0 * wp.pi) / float(num_angles_per_axis)
# Select the standard basis vector for the current axis
local_axis = wp.vec3(0.0, 0.0, 0.0)
local_axis[axis_idx] = 1.0
# Create incremental rotation around principal axis
incremental_quat = wp.quat_from_axis_angle(local_axis, angle)
# Compose rotations: first rotate into principal frame, then apply incremental rotation
quat = base_quat * incremental_quat
# Initialize bounds
min_bounds = wp.vec3(1e10, 1e10, 1e10)
max_bounds = wp.vec3(-1e10, -1e10, -1e10)
# Compute bounds for all vertices
num_verts = vertices.shape[0]
for i in range(num_verts):
rotated = wp.quat_rotate(quat, vertices[i])
min_bounds = wp.min(min_bounds, rotated)
max_bounds = wp.max(max_bounds, rotated)
# Compute extents and volume
box_extents = (max_bounds - min_bounds) * 0.5
volume = box_extents[0] * box_extents[1] * box_extents[2]
# Compute center in rotated space and transform back
center = (max_bounds + min_bounds) * 0.5
world_center = wp.quat_rotate_inv(quat, center)
# Store results
volumes[angle_idx, axis_idx] = volume
extents[angle_idx, axis_idx] = box_extents
transforms[angle_idx, axis_idx] = wp.transform(world_center, wp.quat_inverse(quat))
def compute_shape_radius(geo_type: int, scale: Vec3, src: Mesh | Heightfield | None) -> float:
"""
Calculates the radius of a sphere that encloses the shape, used for broadphase collision detection.
"""
if geo_type == GeoType.SPHERE:
return scale[0]
elif geo_type == GeoType.BOX:
return np.linalg.norm(scale)
elif geo_type == GeoType.CAPSULE or geo_type == GeoType.CYLINDER or geo_type == GeoType.CONE:
return scale[0] + scale[1]
elif geo_type == GeoType.ELLIPSOID:
# Bounding sphere radius is the largest semi-axis
return max(scale[0], scale[1], scale[2])
elif geo_type == GeoType.MESH or geo_type == GeoType.CONVEX_MESH:
vmax = np.max(np.abs(src.vertices), axis=0) * np.max(scale)
return np.linalg.norm(vmax)
elif geo_type == GeoType.PLANE:
if scale[0] > 0.0 and scale[1] > 0.0:
# finite plane
return np.linalg.norm(scale)
else:
return 1.0e6
elif geo_type == GeoType.HFIELD:
# Heightfield bounding sphere centered at the shape origin.
# X/Y are symmetric ([-hx, +hx], [-hy, +hy]), but Z spans [min_z, max_z]
# which may not be symmetric around 0.
if src is not None:
half_x = src.hx * scale[0]
half_y = src.hy * scale[1]
max_abs_z = max(abs(src.min_z), abs(src.max_z)) * scale[2]
return np.sqrt(half_x**2 + half_y**2 + max_abs_z**2)
else:
return np.linalg.norm(scale)
elif geo_type == GeoType.GAUSSIAN:
if src is not None:
lower, upper = src.compute_aabb()
scale_arr = np.abs(np.asarray(scale, dtype=np.float32))
vmax = np.maximum(np.abs(lower), np.abs(upper)) * scale_arr
if hasattr(src, "scales") and len(src.scales) > 0:
vmax = vmax + np.max(np.abs(src.scales), axis=0) * scale_arr
return float(np.linalg.norm(vmax))
return 10.0
else:
return 10.0
def compute_aabb(vertices: np.ndarray) -> tuple[Vec3, Vec3]:
"""Compute the axis-aligned bounding box of a set of vertices."""
min_coords = np.min(vertices, axis=0)
max_coords = np.max(vertices, axis=0)
return min_coords, max_coords
def compute_inertia_box_mesh(
vertices: np.ndarray,
indices: np.ndarray,
is_solid: bool = True,
) -> tuple[wp.vec3, wp.vec3, wp.quat]:
"""Compute the equivalent inertia box of a triangular mesh.
The equivalent inertia box is the box whose inertia tensor matches that of
the mesh. Unlike a bounding box it does **not** necessarily enclose the
geometry — it characterises the mass distribution.
The half-sizes are derived from the principal inertia eigenvalues
(*I₀*, *I₁*, *I₂*) and volume *V* of the mesh:
.. math::
h_i = \\tfrac{1}{2}\\sqrt{\\frac{6\\,(I_j + I_k - I_i)}{V}}
where *(i, j, k)* is a cyclic permutation of *(0, 1, 2)*.
Args:
vertices: Vertex positions, shape ``(N, 3)``.
indices: Triangle indices (flattened or ``(M, 3)``).
is_solid: If ``True`` treat the mesh as solid; otherwise as a thin
shell (see :func:`compute_inertia_mesh`).
Returns:
Tuple of ``(center, half_extents, rotation)`` where *center* is the
center of mass, *half_extents* are the box half-sizes along the
principal axes (not necessarily sorted), and *rotation* is the
quaternion rotating from the principal-axis frame to the mesh frame.
"""
_mass, com, inertia_tensor, volume = compute_inertia_mesh(
density=1.0,
vertices=vertices.tolist() if isinstance(vertices, np.ndarray) else vertices,
indices=np.asarray(indices).flatten().tolist(),
is_solid=is_solid,
)
if volume < 1e-12:
return wp.vec3(0.0, 0.0, 0.0), wp.vec3(0.0, 0.0, 0.0), wp.quat_identity()
inertia = np.array(inertia_tensor).reshape(3, 3)
eigvals, eigvecs = np.linalg.eigh(inertia)
# Sort eigenvalues (and eigenvectors) in ascending order.
order = np.argsort(eigvals)
eigvals = eigvals[order]
eigvecs = eigvecs[:, order]
# Ensure right-handed frame.
if np.linalg.det(eigvecs) < 0:
eigvecs[:, 0] = -eigvecs[:, 0]
# Derive equivalent box half-sizes from principal inertia eigenvalues.
half_extents = np.zeros(3)
for i in range(3):
j, k = (i + 1) % 3, (i + 2) % 3
arg = 6.0 * (eigvals[j] + eigvals[k] - eigvals[i]) / volume
half_extents[i] = 0.5 * np.sqrt(max(arg, 0.0))
# Convert the eigenvector matrix (columns = principal axes in mesh frame)
# to a quaternion.
rotation = wp.quat_from_matrix(wp.mat33(*eigvecs.T.flatten().tolist()))
return wp.vec3(*np.array(com)), wp.vec3(*half_extents), rotation
def compute_pca_obb(vertices: np.ndarray) -> tuple[wp.transform, wp.vec3]:
"""Compute the oriented bounding box of a set of vertices.
Args:
vertices: A numpy array of shape (N, 3) containing the vertex positions.
Returns:
A tuple containing:
- transform: The transform of the oriented bounding box
- extents: The half-extents of the box along its principal axes
"""
if len(vertices) == 0:
return wp.transform_identity(), wp.vec3(0.0, 0.0, 0.0)
if len(vertices) == 1:
return wp.transform(wp.vec3(vertices[0]), wp.quat_identity()), wp.vec3(0.0, 0.0, 0.0)
# Center the vertices
center = np.mean(vertices, axis=0)
centered_vertices = vertices - center
# Compute covariance matrix with handling for degenerate cases
if len(vertices) < 3:
# For 2 points, create a line-aligned OBB
direction = centered_vertices[1] if len(vertices) > 1 else np.array([1, 0, 0])
direction = direction / np.linalg.norm(direction) if np.linalg.norm(direction) > 1e-6 else np.array([1, 0, 0])
# Create orthogonal basis
if abs(direction[0]) < 0.9:
perpendicular = np.cross(direction, [1, 0, 0])
else:
perpendicular = np.cross(direction, [0, 1, 0])
perpendicular = perpendicular / np.linalg.norm(perpendicular)
third = np.cross(direction, perpendicular)
eigenvectors = np.column_stack([direction, perpendicular, third])
else:
cov_matrix = np.cov(centered_vertices.T)
eigenvalues, eigenvectors = np.linalg.eigh(cov_matrix)
# Sort by eigenvalues in descending order
sorted_indices = np.argsort(eigenvalues)[::-1]
eigenvalues = eigenvalues[sorted_indices]
eigenvectors = eigenvectors[:, sorted_indices]
# Ensure right-handed coordinate system
if np.linalg.det(eigenvectors) < 0:
eigenvectors[:, 2] *= -1
# Project vertices onto principal axes
projected = centered_vertices @ eigenvectors
# Compute extents
min_coords = np.min(projected, axis=0)
max_coords = np.max(projected, axis=0)
extents = (max_coords - min_coords) / 2.0
# Calculate the center in the projected coordinate system
center_offset = (max_coords + min_coords) / 2.0
# Transform the center offset back to the original coordinate system
center = center + center_offset @ eigenvectors.T
# Convert rotation matrix to quaternion
# The rotation matrix should transform from the original coordinate system to the principal axes
# eigenvectors is the rotation matrix from original to principal axes
rotation_matrix = eigenvectors
# Convert to quaternion using Warp's quat_from_matrix function
# First convert numpy array to Warp matrix
orientation = wp.quat_from_matrix(wp.mat33(rotation_matrix))
return wp.transform(wp.vec3(center), orientation), wp.vec3(extents)
def compute_inertia_obb(
vertices: np.ndarray,
num_angle_steps: int = 360,
) -> tuple[wp.transform, wp.vec3]:
"""
Compute oriented bounding box using inertia-based principal axes.
This method provides more stable results than PCA for symmetric objects:
1. Computes convex hull of the input vertices
2. Computes inertia tensor of the hull and extracts principal axes
3. Uses Warp kernels to test rotations around each principal axis
4. Returns the OBB with minimum volume
Args:
vertices: Array of shape (N, 3) containing the vertex positions
num_angle_steps: Number of angle steps to test per axis (default: 360)
Returns:
Tuple of (transform, extents)
"""
if len(vertices) == 0:
return wp.transform_identity(), wp.vec3(0.0, 0.0, 0.0)
if len(vertices) == 1:
return wp.transform(wp.vec3(vertices[0]), wp.quat_identity()), wp.vec3(0.0, 0.0, 0.0)
# Step 1: Compute convex hull
hull_vertices, hull_faces = remesh_convex_hull(vertices, maxhullvert=0) # 0 = no limit
hull_indices = hull_faces.flatten()
# Step 2: Compute mesh inertia
_mass, com, inertia_tensor, _volume = compute_inertia_mesh(
density=1.0, # Unit density
vertices=hull_vertices.tolist(),
indices=hull_indices.tolist(),
is_solid=True,
)
# Adjust vertices to be centered at COM
center = np.array(com)
centered_vertices = hull_vertices - center
# Convert inertia tensor to numpy array for diagonalization
inertia = np.array(inertia_tensor).reshape(3, 3)
# Get principal axes by diagonalizing inertia tensor
eigenvalues, eigenvectors = np.linalg.eigh(inertia)
# Sort by eigenvalues in ascending order (largest inertia = smallest dimension)
# This helps with consistent ordering
sorted_indices = np.argsort(eigenvalues)
eigenvectors = eigenvectors[:, sorted_indices]
# Ensure no reflection in the transformation
if np.linalg.det(eigenvectors) < 0:
eigenvectors[:, 2] *= -1
principal_axes = eigenvectors
# Convert principal axes rotation matrix to quaternion
# The principal_axes matrix transforms from world to principal frame
base_quat = wp.quat_from_matrix(wp.mat33(principal_axes.T.flatten()))
# Step 3: Warp kernel search
# Allocate 2D arrays: (num_angle_steps, 3 axes)
vertices_wp = wp.array(centered_vertices, dtype=wp.vec3)
volumes = wp.zeros((num_angle_steps, 3), dtype=float)
transforms = wp.zeros((num_angle_steps, 3), dtype=wp.transform)
extents = wp.zeros((num_angle_steps, 3), dtype=wp.vec3)
# Launch kernel with 2D dimensions
wp.launch(
compute_obb_candidates,
dim=(num_angle_steps, 3),
inputs=[vertices_wp, base_quat, volumes, transforms, extents],
)
# Find minimum volume
volumes_host = volumes.numpy()
best_idx = np.unravel_index(np.argmin(volumes_host), volumes_host.shape)
# Get results
best_transform = transforms.numpy()[best_idx]
best_extents = extents.numpy()[best_idx]
# Adjust transform to account for original center
best_transform[0:3] += center
return wp.transform(*best_transform), wp.vec3(*best_extents)
def load_mesh(filename: str, method: str | None = None):
"""
Loads a 3D triangular surface mesh from a file.
Args:
filename (str): The path to the 3D model file (obj, and other formats supported by the different methods) to load.
method (str): The method to use for loading the mesh (default None). Can be either `"trimesh"`, `"meshio"`, `"pcu"`, or `"openmesh"`. If None, every method is tried and the first successful mesh import where the number of vertices is greater than 0 is returned.
Returns:
Tuple of (mesh_points, mesh_indices), where mesh_points is a Nx3 numpy array of vertex positions (float32),
and mesh_indices is a Mx3 numpy array of vertex indices (int32) for the triangular faces.
"""
if not os.path.exists(filename):
raise FileNotFoundError(f"File not found: {filename}")
def load_mesh_with_method(method):
if method == "meshio":
import meshio
m = meshio.read(filename)
mesh_points = np.array(m.points)
mesh_indices = np.array(m.cells[0].data, dtype=np.int32)
elif method == "openmesh":
import openmesh
m = openmesh.read_trimesh(filename)
mesh_points = np.array(m.points())
mesh_indices = np.array(m.face_vertex_indices(), dtype=np.int32)
elif method == "pcu":
import point_cloud_utils as pcu
mesh_points, mesh_indices = pcu.load_mesh_vf(filename)
mesh_indices = mesh_indices.flatten()
else:
import trimesh
m = trimesh.load(filename)
if hasattr(m, "geometry"):
# multiple meshes are contained in a scene; combine to one mesh
mesh_points = []
mesh_indices = []
index_offset = 0
for geom in m.geometry.values():
vertices = np.array(geom.vertices, dtype=np.float32)
faces = np.array(geom.faces.flatten(), dtype=np.int32)
mesh_points.append(vertices)
mesh_indices.append(faces + index_offset)
index_offset += len(vertices)
mesh_points = np.concatenate(mesh_points, axis=0)
mesh_indices = np.concatenate(mesh_indices)
else:
# a single mesh
mesh_points = np.array(m.vertices, dtype=np.float32)
mesh_indices = np.array(m.faces.flatten(), dtype=np.int32)
return mesh_points, mesh_indices
if method is None:
methods = ["trimesh", "meshio", "pcu", "openmesh"]
for method in methods:
try:
mesh = load_mesh_with_method(method)
if mesh is not None and len(mesh[0]) > 0:
return mesh
except Exception:
pass
raise ValueError(f"Failed to load mesh using any of the methods: {methods}")
else:
mesh = load_mesh_with_method(method)
if mesh is None or len(mesh[0]) == 0:
raise ValueError(f"Failed to load mesh using method {method}")
return mesh
def visualize_meshes(
meshes: list[tuple[list, list]], num_cols=0, num_rows=0, titles=None, scale_axes=True, show_plot=True
):
"""Render meshes in a grid with matplotlib."""
import matplotlib.pyplot as plt
if titles is None:
titles = []
num_cols = min(num_cols, len(meshes))
num_rows = min(num_rows, len(meshes))
if num_cols and not num_rows:
num_rows = int(np.ceil(len(meshes) / num_cols))
elif num_rows and not num_cols:
num_cols = int(np.ceil(len(meshes) / num_rows))
else:
num_cols = len(meshes)
num_rows = 1
vertices = [np.array(v).reshape((-1, 3)) for v, _ in meshes]
faces = [np.array(f, dtype=np.int32).reshape((-1, 3)) for _, f in meshes]
if scale_axes:
ranges = np.array([v.max(axis=0) - v.min(axis=0) for v in vertices])
max_range = ranges.max()
mid_points = np.array([v.max(axis=0) + v.min(axis=0) for v in vertices]) * 0.5
fig = plt.figure(figsize=(12, 6))
for i, (vertices, faces) in enumerate(meshes):
ax = fig.add_subplot(num_rows, num_cols, i + 1, projection="3d")
if i < len(titles):
ax.set_title(titles[i])
ax.plot_trisurf(vertices[:, 0], vertices[:, 1], vertices[:, 2], triangles=faces, edgecolor="k")
if scale_axes:
mid = mid_points[i]
ax.set_xlim(mid[0] - max_range, mid[0] + max_range)
ax.set_ylim(mid[1] - max_range, mid[1] + max_range)
ax.set_zlim(mid[2] - max_range, mid[2] + max_range)
if show_plot:
plt.show()
return fig
@contextlib.contextmanager
def silence_stdio():
"""
Redirect *both* Python-level and C-level stdout/stderr to os.devnull
for the duration of the with-block.
"""
devnull = open(os.devnull, "w")
# Duplicate the real fds so we can restore them later
old_stdout_fd = os.dup(1)
old_stderr_fd = os.dup(2)
try:
# Point fds 1 and 2 at /dev/null
os.dup2(devnull.fileno(), 1)
os.dup2(devnull.fileno(), 2)
# Also patch the Python objects that wrap those fds
with contextlib.redirect_stdout(devnull), contextlib.redirect_stderr(devnull):
yield
finally:
# Restore original fds
os.dup2(old_stdout_fd, 1)
os.dup2(old_stderr_fd, 2)
os.close(old_stdout_fd)
os.close(old_stderr_fd)
devnull.close()
def remesh_ftetwild(
vertices: np.ndarray,
faces: np.ndarray,
optimize: bool = False,
edge_length_fac: float = 0.05,
verbose: bool = False,
):
"""Remesh a 3D triangular surface mesh using "Fast Tetrahedral Meshing in the Wild" (fTetWild).
This is useful for improving the quality of the mesh, and for ensuring that the mesh is
watertight. This function first tetrahedralizes the mesh, then extracts the surface mesh.
The resulting mesh is guaranteed to be watertight and may have a different topology than the
input mesh.
Uses pytetwild, a Python wrapper for fTetWild, to perform the remeshing.
See https://github.com/pyvista/pytetwild.
Args:
vertices: A numpy array of shape (N, 3) containing the vertex positions.
faces: A numpy array of shape (M, 3) containing the vertex indices of the faces.
optimize: Whether to optimize the mesh quality during remeshing.
edge_length_fac: The target edge length of the tetrahedral element as a fraction of the bounding box diagonal.
Returns:
A tuple (vertices, faces) containing the remeshed mesh. Returns the original vertices and faces
if the remeshing fails.
"""
from pytetwild import tetrahedralize
def tet_fn(v, f):
return tetrahedralize(v, f, optimize=optimize, edge_length_fac=edge_length_fac)
if verbose:
tet_vertices, tet_indices = tet_fn(vertices, faces)
else:
# Suppress stdout and stderr during tetrahedralize
with silence_stdio():
tet_vertices, tet_indices = tet_fn(vertices, faces)
def face_indices(tet):
face1 = (tet[0], tet[2], tet[1])
face2 = (tet[1], tet[2], tet[3])
face3 = (tet[0], tet[1], tet[3])
face4 = (tet[0], tet[3], tet[2])
return (
(face1, tuple(sorted(face1))),
(face2, tuple(sorted(face2))),
(face3, tuple(sorted(face3))),
(face4, tuple(sorted(face4))),
)
# determine surface faces
elements_per_face = defaultdict(set)
unique_faces = {}
for e, tet in enumerate(tet_indices):
for face, key in face_indices(tet):
elements_per_face[key].add(e)
unique_faces[key] = face
surface_faces = [face for key, face in unique_faces.items() if len(elements_per_face[key]) == 1]
new_vertices = np.array(tet_vertices)
new_faces = np.array(surface_faces, dtype=np.int32)
if len(new_vertices) == 0 or len(new_faces) == 0:
warnings.warn(
"Remeshing failed, the optimized mesh has no vertices or faces; return previous mesh.", stacklevel=2
)
return vertices, faces
return new_vertices, new_faces
def remesh_alphashape(vertices: np.ndarray, alpha: float = 3.0):
"""Remesh a 3D triangular surface mesh using the alpha shape algorithm.
Args:
vertices: A numpy array of shape (N, 3) containing the vertex positions.
faces: A numpy array of shape (M, 3) containing the vertex indices of the faces (not needed).
alpha: The alpha shape parameter.
Returns:
A tuple (vertices, faces) containing the remeshed mesh.
"""
import alphashape
with silence_stdio():
alpha_shape = alphashape.alphashape(vertices, alpha)
return np.array(alpha_shape.vertices), np.array(alpha_shape.faces, dtype=np.int32)
def remesh_quadratic(
vertices: np.ndarray,
faces: np.ndarray,
target_reduction: float = 0.5,
target_count: int | None = None,
**kwargs: Any,
):
"""Remesh a 3D triangular surface mesh using fast quadratic mesh simplification.
https://github.com/pyvista/fast-simplification
Args:
vertices: A numpy array of shape (N, 3) containing the vertex positions.
faces: A numpy array of shape (M, 3) containing the vertex indices of the faces.
target_reduction: The target reduction factor for the number of faces (0.0 to 1.0).
**kwargs: Additional keyword arguments for the remeshing algorithm.
Returns:
A tuple (vertices, faces) containing the remeshed mesh.
"""
from fast_simplification import simplify
return simplify(vertices, faces, target_reduction=target_reduction, target_count=target_count, **kwargs)
def remesh_convex_hull(vertices: np.ndarray, maxhullvert: int = 0):
"""Compute the convex hull of a set of 3D points and return the vertices and faces of the convex hull mesh.
Uses ``scipy.spatial.ConvexHull`` to compute the convex hull.
Args:
vertices: A numpy array of shape (N, 3) containing the vertex positions.
maxhullvert: The maximum number of vertices for the convex hull. If 0, no limit is applied.
Returns:
A tuple (verts, faces) where:
- verts: A numpy array of shape (M, 3) containing the vertex positions of the convex hull.
- faces: A numpy array of shape (K, 3) containing the vertex indices of the triangular faces of the convex hull.
"""
from scipy.spatial import ConvexHull
qhull_options = "Qt"
if maxhullvert > 0:
# qhull "TA" actually means "number of vertices added after the initial simplex"
# from mujoco's user_mesh.cc
qhull_options += f" TA{maxhullvert - 4}"
hull = ConvexHull(vertices, qhull_options=qhull_options)
verts = hull.points.copy().astype(np.float32)
faces = hull.simplices.astype(np.int32)
# fix winding order of faces
centre = verts.mean(0)
for i, tri in enumerate(faces):
a, b, c = verts[tri]
normal = np.cross(b - a, c - a)
if np.dot(normal, a - centre) < 0:
faces[i] = tri[[0, 2, 1]]
# trim vertices to only those that are used in the faces
unique_verts = np.unique(faces.flatten())
verts = verts[unique_verts]
# update face indices to use the new vertex indices
mapping = {v: i for i, v in enumerate(unique_verts)}
faces = np.array([mapping[v] for v in faces.flatten()], dtype=np.int32).reshape(faces.shape)
return verts, faces
RemeshingMethod = Literal["ftetwild", "alphashape", "quadratic", "convex_hull", "poisson"]
def remesh(
vertices: np.ndarray,
faces: np.ndarray,
method: RemeshingMethod = "quadratic",
visualize: bool = False,
**remeshing_kwargs: Any,
) -> tuple[np.ndarray, np.ndarray]:
"""
Remeshes a 3D triangular surface mesh using the specified method.
Args:
vertices: A numpy array of shape (N, 3) containing the vertex positions.
faces: A numpy array of shape (M, 3) containing the vertex indices of the faces.
method: The remeshing method to use. One of "ftetwild", "quadratic", "convex_hull",
"alphashape", or "poisson".
visualize: Whether to render the input and output meshes using matplotlib.
**remeshing_kwargs: Additional keyword arguments passed to the remeshing function.
Returns:
A tuple (vertices, faces) containing the remeshed mesh.
"""
if method == "ftetwild":
new_vertices, new_faces = remesh_ftetwild(vertices, faces, **remeshing_kwargs)
elif method == "alphashape":
new_vertices, new_faces = remesh_alphashape(vertices, **remeshing_kwargs)
elif method == "quadratic":
new_vertices, new_faces = remesh_quadratic(vertices, faces, **remeshing_kwargs)
elif method == "convex_hull":
new_vertices, new_faces = remesh_convex_hull(vertices, **remeshing_kwargs)
elif method == "poisson":
from newton._src.geometry.remesh import remesh_poisson # noqa: PLC0415
new_vertices, new_faces = remesh_poisson(vertices, faces, **remeshing_kwargs)
else:
raise ValueError(f"Unknown remeshing method: {method}")
if visualize:
# side-by-side visualization of the input and output meshes
visualize_meshes(
[(vertices, faces), (new_vertices, new_faces)],
titles=[
f"Original ({len(vertices)} verts, {len(faces)} faces)",
f"Remeshed ({len(new_vertices)} verts, {len(new_faces)} faces)",
],
)
return new_vertices, new_faces
def remesh_mesh(
mesh: Mesh,
method: RemeshingMethod = "quadratic",
recompute_inertia: bool = False,
inplace: bool = False,
**remeshing_kwargs: Any,
) -> Mesh:
"""
Remeshes a Mesh object using the specified remeshing method.
Args:
mesh: The mesh to be remeshed.
method: The remeshing method to use.
One of "ftetwild", "quadratic", "convex_hull", "alphashape", or "poisson".
Defaults to "quadratic".
recompute_inertia: If True, recompute the mass, center of mass,
and inertia tensor of the mesh after remeshing. Defaults to False.
inplace: If True, modify the mesh in place. If False,
return a new mesh instance with the remeshed geometry. Defaults to False.
**remeshing_kwargs: Additional keyword arguments passed to the remeshing function.
Returns:
Mesh: The remeshed mesh. If `inplace` is True, returns the modified input mesh.
"""
if method == "convex_hull":
remeshing_kwargs["maxhullvert"] = mesh.maxhullvert
vertices, indices = remesh(mesh.vertices, mesh.indices.reshape(-1, 3), method=method, **remeshing_kwargs)
if inplace:
mesh.vertices = vertices
mesh.indices = indices.flatten()
if recompute_inertia:
mesh.mass, mesh.com, mesh.inertia, _ = compute_inertia_mesh(1.0, vertices, indices, is_solid=mesh.is_solid)
else:
return mesh.copy(vertices=vertices, indices=indices, recompute_inertia=recompute_inertia)
return mesh
def transform_points(points: np.ndarray, transform: wp.transform, scale: Vec3 | None = None) -> np.ndarray:
if scale is not None:
points = points * np.array(scale, dtype=np.float32)
return points @ np.array(wp.quat_to_matrix(transform.q)).reshape(3, 3) + transform.p
@wp.kernel(enable_backward=False)
def get_total_kernel(
counts: wp.array[int],
prefix_sums: wp.array[int],
num_elements: wp.array[int],
max_elements: int,
total: wp.array[int],
):
"""
Get the total of an array of counts and prefix sums.
"""
if num_elements[0] <= 0 or max_elements <= 0:
total[0] = 0
return
# Clip to array bounds to avoid out-of-bounds access
n = wp.min(num_elements[0], max_elements)
final_idx = n - 1
total[0] = prefix_sums[final_idx] + counts[final_idx]
def scan_with_total(
counts: wp.array[int],
prefix_sums: wp.array[int],
num_elements: wp.array[int],
total: wp.array[int],
):
"""
Computes an exclusive prefix sum and total of a counts array.
Args:
counts: Input array of per-element counts.
prefix_sums: Output array for exclusive prefix sums (same size as counts).
num_elements: Single-element array containing the number of valid elements in counts.
total: Single-element output array that will contain the sum of all counts.
"""
wp.utils.array_scan(counts, prefix_sums, inclusive=False)
wp.launch(
get_total_kernel,
dim=[1],
inputs=[counts, prefix_sums, num_elements, counts.shape[0], total],
device=counts.device,
record_tape=False,
)
__all__ = ["compute_shape_radius", "load_mesh", "visualize_meshes"]
================================================
FILE: newton/_src/math/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from typing import Any
import warp as wp
from .spatial import (
quat_between_axes,
quat_between_vectors_robust,
quat_decompose,
quat_velocity,
transform_twist,
transform_wrench,
velocity_at_point,
)
@wp.func
def boltzmann(a: float, b: float, alpha: float):
"""
Compute the Boltzmann-weighted average of two values.
This function returns a smooth interpolation between `a` and `b` using a Boltzmann (softmax-like) weighting,
controlled by the parameter `alpha`. As `alpha` increases, the result approaches `max(a, b)`;
as `alpha` decreases, the result approaches the mean of `a` and `b`.
Args:
a: The first value.
b: The second value.
alpha: The sharpness parameter. Higher values make the function more "max-like".
Returns:
float: The Boltzmann-weighted average of `a` and `b`.
"""
e1 = wp.exp(alpha * a)
e2 = wp.exp(alpha * b)
return (a * e1 + b * e2) / (e1 + e2)
@wp.func
def smooth_max(a: float, b: float, eps: float):
"""
Compute a smooth approximation of the maximum of two values.
This function returns a value close to `max(a, b)`, but is differentiable everywhere.
The `eps` parameter controls the smoothness: larger values make the transition smoother.
Args:
a: The first value.
b: The second value.
eps: Smoothing parameter (should be small and positive).
Returns:
float: A smooth approximation of `max(a, b)`.
"""
d = a - b
return 0.5 * (a + b + wp.sqrt(d * d + eps))
@wp.func
def smooth_min(a: float, b: float, eps: float):
"""
Compute a smooth approximation of the minimum of two values.
This function returns a value close to `min(a, b)`, but is differentiable everywhere.
The `eps` parameter controls the smoothness: larger values make the transition smoother.
Args:
a: The first value.
b: The second value.
eps: Smoothing parameter (should be small and positive).
Returns:
float: A smooth approximation of `min(a, b)`.
"""
d = a - b
return 0.5 * (a + b - wp.sqrt(d * d + eps))
@wp.func
def leaky_max(a: float, b: float):
"""
Compute a numerically stable, differentiable approximation of `max(a, b)`.
This is equivalent to `smooth_max(a, b, 1e-5)`.
Args:
a: The first value.
b: The second value.
Returns:
float: A smooth, "leaky" maximum of `a` and `b`.
"""
return smooth_max(a, b, 1e-5)
@wp.func
def leaky_min(a: float, b: float):
"""
Compute a numerically stable, differentiable approximation of `min(a, b)`.
This is equivalent to `smooth_min(a, b, 1e-5)`.
Args:
a: The first value.
b: The second value.
Returns:
float: A smooth, "leaky" minimum of `a` and `b`.
"""
return smooth_min(a, b, 1e-5)
@wp.func
def vec_min(a: wp.vec3, b: wp.vec3):
"""
Compute the elementwise minimum of two 3D vectors.
Args:
a: The first vector.
b: The second vector.
Returns:
wp.vec3: The elementwise minimum.
"""
return wp.vec3(wp.min(a[0], b[0]), wp.min(a[1], b[1]), wp.min(a[2], b[2]))
@wp.func
def vec_max(a: wp.vec3, b: wp.vec3):
"""
Compute the elementwise maximum of two 3D vectors.
Args:
a: The first vector.
b: The second vector.
Returns:
wp.vec3: The elementwise maximum.
"""
return wp.vec3(wp.max(a[0], b[0]), wp.max(a[1], b[1]), wp.max(a[2], b[2]))
@wp.func
def vec_leaky_min(a: wp.vec3, b: wp.vec3):
"""
Compute the elementwise "leaky" minimum of two 3D vectors.
This uses `leaky_min` for each component.
Args:
a: The first vector.
b: The second vector.
Returns:
wp.vec3: The elementwise leaky minimum.
"""
return wp.vec3(leaky_min(a[0], b[0]), leaky_min(a[1], b[1]), leaky_min(a[2], b[2]))
@wp.func
def vec_leaky_max(a: wp.vec3, b: wp.vec3):
"""
Compute the elementwise "leaky" maximum of two 3D vectors.
This uses `leaky_max` for each component.
Args:
a: The first vector.
b: The second vector.
Returns:
wp.vec3: The elementwise leaky maximum.
"""
return wp.vec3(leaky_max(a[0], b[0]), leaky_max(a[1], b[1]), leaky_max(a[2], b[2]))
@wp.func
def vec_abs(a: wp.vec3):
"""
Compute the elementwise absolute value of a 3D vector.
Args:
a: The input vector.
Returns:
wp.vec3: The elementwise absolute value.
"""
return wp.vec3(wp.abs(a[0]), wp.abs(a[1]), wp.abs(a[2]))
@wp.func
def vec_allclose(a: Any, b: Any, rtol: float = 1e-5, atol: float = 1e-8) -> bool:
"""Check whether two Warp vectors are element-wise equal within a tolerance.
Uses the same criterion as NumPy's ``allclose``:
``abs(a[i] - b[i]) <= atol + rtol * abs(b[i])`` for every element.
Args:
a: First vector.
b: Second vector.
rtol: Relative tolerance.
atol: Absolute tolerance.
Returns:
bool: ``True`` if all elements satisfy the tolerance, ``False`` otherwise.
"""
for i in range(wp.static(len(a))):
if wp.abs(a[i] - b[i]) > atol + rtol * wp.abs(b[i]):
return False
return True
@wp.func
def vec_inside_limits(a: Any, lower: Any, upper: Any) -> bool:
"""Check whether every element of a vector lies within the given bounds.
Returns ``True`` when ``lower[i] <= a[i] <= upper[i]`` for all elements.
Args:
a: Vector to test.
lower: Element-wise lower bounds (inclusive).
upper: Element-wise upper bounds (inclusive).
Returns:
bool: ``True`` if all elements are within bounds, ``False`` otherwise.
"""
for i in range(wp.static(len(a))):
if a[i] < lower[i] or a[i] > upper[i]:
return False
return True
@wp.func
def orthonormal_basis(n: wp.vec3):
r"""Build an orthonormal basis from a normal vector.
Given a (typically unit-length) normal vector ``n``, this returns two
tangent vectors ``b1`` and ``b2`` such that:
.. math::
b_1 \cdot n = 0,\quad b_2 \cdot n = 0,\quad
b_1 \cdot b_2 = 0,\quad \|b_1\|=\|b_2\|=1.
Args:
n: Normal vector (assumed to be close to unit length).
Returns:
Tuple[wp.vec3, wp.vec3]: Orthonormal tangent vectors ``(b1, b2)``.
"""
b1 = wp.vec3()
b2 = wp.vec3()
if n[2] < 0.0:
a = 1.0 / (1.0 - n[2])
b = n[0] * n[1] * a
b1[0] = 1.0 - n[0] * n[0] * a
b1[1] = -b
b1[2] = n[0]
b2[0] = b
b2[1] = n[1] * n[1] * a - 1.0
b2[2] = -n[1]
else:
a = 1.0 / (1.0 + n[2])
b = -n[0] * n[1] * a
b1[0] = 1.0 - n[0] * n[0] * a
b1[1] = b
b1[2] = -n[0]
b2[0] = b
b2[1] = 1.0 - n[1] * n[1] * a
b2[2] = -n[1]
return b1, b2
EPSILON = 1e-15
@wp.func
def safe_div(x: Any, y: Any, eps: float = EPSILON) -> Any:
"""Safe division that returns ``x / y``, falling back to ``x / eps`` when ``y`` is zero.
Args:
x: Numerator.
y: Denominator.
eps: Small positive fallback used in place of ``y`` when ``y == 0``.
Returns:
The quotient ``x / y``, or ``x / eps`` if ``y`` is zero.
"""
return x / wp.where(y != 0.0, y, eps)
@wp.func
def normalize_with_norm(x: Any):
"""Normalize a vector and return both the unit vector and the original norm.
If the input has zero length it is returned unchanged with a norm of ``0.0``.
Args:
x: Input vector.
Returns:
Tuple[vector, float]: ``(normalized_x, norm)`` where ``normalized_x`` is the
unit-length direction and ``norm`` is ``wp.length(x)``.
"""
norm = wp.length(x)
if norm == 0.0:
return x, 0.0
return x / norm, norm
__all__ = [
"boltzmann",
"leaky_max",
"leaky_min",
"normalize_with_norm",
"orthonormal_basis",
"quat_between_axes",
"quat_between_vectors_robust",
"quat_decompose",
"quat_velocity",
"safe_div",
"smooth_max",
"smooth_min",
"transform_twist",
"transform_wrench",
"vec_abs",
"vec_allclose",
"vec_inside_limits",
"vec_leaky_max",
"vec_leaky_min",
"vec_max",
"vec_min",
"velocity_at_point",
]
================================================
FILE: newton/_src/math/spatial.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ..core.types import Axis, AxisType
@wp.func
def quat_between_vectors_robust(from_vec: wp.vec3, to_vec: wp.vec3, eps: float = 1.0e-8) -> wp.quat:
"""Robustly compute the quaternion that rotates ``from_vec`` to ``to_vec``.
This is a safer version of :obj:`warp.quat_between_vectors() ` that handles the
anti-parallel (180-degree) singularity by selecting a deterministic axis
orthogonal to ``from_vec``.
Args:
from_vec: Source vector (assumed normalized).
to_vec: Target vector (assumed normalized).
eps: Tolerance for parallel/anti-parallel checks.
Returns:
wp.quat: Rotation quaternion q such that q * from_vec = to_vec.
"""
d = wp.dot(from_vec, to_vec)
if d >= 1.0 - eps:
return wp.quat_identity()
if d <= -1.0 + eps:
# Deterministic axis orthogonal to from_vec.
# Prefer cross with X, fallback to Y if nearly parallel.
helper = wp.vec3(1.0, 0.0, 0.0)
if wp.abs(from_vec[0]) >= 0.9:
helper = wp.vec3(0.0, 1.0, 0.0)
axis = wp.cross(from_vec, helper)
axis_len = wp.length(axis)
if axis_len <= eps:
axis = wp.cross(from_vec, wp.vec3(0.0, 0.0, 1.0))
axis_len = wp.length(axis)
# Final fallback: if axis is still degenerate, pick an arbitrary axis.
if axis_len <= eps:
return wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), wp.pi)
axis = axis / axis_len
return wp.quat_from_axis_angle(axis, wp.pi)
return wp.quat_between_vectors(from_vec, to_vec)
@wp.func
def velocity_at_point(qd: wp.spatial_vector, r: wp.vec3) -> wp.vec3:
"""Evaluate the linear velocity of an offset point on a rigid body.
In Newton, spatial twist vectors are stored as
:math:`q_d = (v, \\omega)`, where :math:`v` is linear velocity and
:math:`\\omega` is angular velocity. Warp's
:func:`warp.velocity_at_point() ` uses :math:`(\\omega, v)` ordering, so this
wrapper converts the layout before calling Warp.
The kinematic relation is:
.. math::
v_p = v + \\omega \\times r
where :math:`r` is the point position relative to the twist origin.
Args:
qd: Spatial twist in Newton layout ``(linear, angular)``.
r: Point offset from the frame origin [m].
Returns:
wp.vec3: Linear velocity of the offset point [m/s].
"""
qd_wp = wp.spatial_vector(wp.spatial_bottom(qd), wp.spatial_top(qd))
return wp.velocity_at_point(qd_wp, r)
@wp.func
def transform_twist(t: wp.transform, x: wp.spatial_vector) -> wp.spatial_vector:
"""Transform a spatial twist between coordinate frames.
This applies Warp's twist transform while preserving Newton's spatial
layout. Newton stores twists as :math:`x = (v, \\omega)` (linear,
angular), while Warp's low-level helper expects :math:`(\\omega, v)`.
For rigid transform :math:`t = (R, p)` from source to destination:
.. math::
\\omega' = R\\omega,\\quad v' = Rv + p \\times \\omega'
Args:
t: Rigid transform from source frame to destination frame.
x: Spatial twist in Newton layout ``(linear, angular)``.
Returns:
wp.spatial_vector: Transformed twist in Newton layout
``(linear, angular)``.
"""
x_wp = wp.spatial_vector(wp.spatial_bottom(x), wp.spatial_top(x))
y_wp = wp.transform_twist(t, x_wp)
return wp.spatial_vector(wp.spatial_bottom(y_wp), wp.spatial_top(y_wp))
@wp.func
def transform_wrench(t: wp.transform, x: wp.spatial_vector) -> wp.spatial_vector:
"""Transform a spatial wrench between coordinate frames.
This applies Warp's wrench transform while preserving Newton's spatial
layout. Newton stores wrenches as :math:`x = (f, \\tau)` (force, torque),
while Warp expects :math:`(\\tau, f)`.
For rigid transform :math:`t = (R, p)` from source to destination:
.. math::
f' = Rf,\\quad \\tau' = R\\tau + p \\times f'
Args:
t: Rigid transform from source frame to destination frame.
x: Spatial wrench in Newton layout ``(force, torque)``.
Returns:
wp.spatial_vector: Transformed wrench in Newton layout
``(force, torque)``.
"""
x_wp = wp.spatial_vector(wp.spatial_bottom(x), wp.spatial_top(x))
y_wp = wp.transform_wrench(t, x_wp)
return wp.spatial_vector(wp.spatial_bottom(y_wp), wp.spatial_top(y_wp))
@wp.func
def _wrap_angle_pm_pi(theta: float) -> float:
"""Wrap an angle to the principal interval :math:`[-\\pi, \\pi)`.
Args:
theta: Input angle [rad].
Returns:
float: Wrapped angle [rad] in :math:`[-\\pi, \\pi)`.
"""
two_pi = 2.0 * wp.pi
wrapped = wp.mod(theta + wp.pi, two_pi)
if wrapped < 0.0:
wrapped += two_pi
return wrapped - wp.pi
@wp.func
def quat_decompose(q: wp.quat) -> wp.vec3:
"""Decompose a quaternion into wrapped XYZ Euler coordinates.
This wrapper calls :func:`warp.quat_to_euler() ` with the Newton convention
:math:`(i, j, k) = (2, 1, 0)`, then wraps each angle to the principal
branch. Wrapping avoids equivalent representations that differ by
:math:`2\\pi`,
which improves stability when reconstructing joint coordinates.
.. math::
e = \\operatorname{quat\\_to\\_euler}(q, 2, 1, 0),\\quad
e_i \\leftarrow \\operatorname{wrap}_{[-\\pi,\\pi)}(e_i)
Args:
q: Input quaternion in Warp layout ``(x, y, z, w)``.
Returns:
wp.vec3: Wrapped Euler coordinates ``(x, y, z)`` [rad].
"""
angles = wp.quat_to_euler(q, 2, 1, 0)
return wp.vec3(
_wrap_angle_pm_pi(angles[0]),
_wrap_angle_pm_pi(angles[1]),
_wrap_angle_pm_pi(angles[2]),
)
@wp.func
def quat_velocity(q_now: wp.quat, q_prev: wp.quat, dt: float) -> wp.vec3:
"""Approximate angular velocity from successive world quaternions (world frame).
Uses right-trivialized mapping via
:math:`\\Delta q = q_{\\text{now}} q_{\\text{prev}}^{-1}`.
.. math::
\\Delta q = q_{now} q_{prev}^{-1},\\quad
\\omega \\approx \\hat{u}(\\Delta q)\\,\\frac{\\theta(\\Delta q)}{\\Delta t}
Args:
q_now: Current orientation in world frame.
q_prev: Previous orientation in world frame.
dt: Time step [s].
Returns:
Angular velocity :math:`\\omega` in world frame [rad/s].
"""
# Normalize inputs
q1 = wp.normalize(q_now)
q0 = wp.normalize(q_prev)
# Enforce shortest-arc by aligning quaternion hemisphere
if wp.dot(q1, q0) < 0.0:
q0 = wp.quat(-q0[0], -q0[1], -q0[2], -q0[3])
# dq = q1 * conj(q0)
dq = wp.normalize(wp.mul(q1, wp.quat_inverse(q0)))
axis, angle = wp.quat_to_axis_angle(dq)
return axis * (angle / dt)
__axis_rotations = {}
def quat_between_axes(*axes: AxisType) -> wp.quat:
"""Compute the rotation between a sequence of axes.
This function returns a quaternion that represents the cumulative rotation
through a sequence of axes. For example, for axes (a, b, c), it computes
the rotation from a to c by composing the rotation from a to b and b to c.
Args:
axes: A sequence of axes, e.g., ('x', 'y', 'z').
Returns:
The total rotation quaternion.
"""
q = wp.quat_identity()
for i in range(len(axes) - 1):
src = Axis.from_any(axes[i])
dst = Axis.from_any(axes[i + 1])
if (src.value, dst.value) in __axis_rotations:
dq = __axis_rotations[(src.value, dst.value)]
else:
dq = wp.quat_between_vectors(src.to_vec3(), dst.to_vec3())
__axis_rotations[(src.value, dst.value)] = dq
q *= dq
return q
__all__ = [
"quat_between_axes",
"quat_between_vectors_robust",
"quat_decompose",
"quat_velocity",
"transform_twist",
"transform_wrench",
"velocity_at_point",
]
================================================
FILE: newton/_src/sensors/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
================================================
FILE: newton/_src/sensors/sensor_contact.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warnings
from enum import Enum
from typing import Literal
import numpy as np
import warp as wp
from ..sim import Contacts, Model, State
from ..utils.selection import match_labels
# Object type constants used in the sensing-object transform kernel.
_OBJ_TYPE_TOTAL = 0
_OBJ_TYPE_SHAPE = 1
_OBJ_TYPE_BODY = 2
@wp.kernel(enable_backward=False)
def compute_sensing_obj_transforms_kernel(
indices: wp.array[wp.int32],
obj_types: wp.array[wp.int32],
shape_body: wp.array[wp.int32],
shape_transform: wp.array[wp.transform],
body_q: wp.array[wp.transform],
# output
transforms: wp.array[wp.transform],
):
tid = wp.tid()
idx = indices[tid]
obj_type = obj_types[tid]
if obj_type == wp.static(_OBJ_TYPE_BODY):
transforms[tid] = body_q[idx]
elif obj_type == wp.static(_OBJ_TYPE_SHAPE):
body_idx = shape_body[idx]
if body_idx >= 0:
transforms[tid] = wp.transform_multiply(body_q[body_idx], shape_transform[idx])
else:
transforms[tid] = shape_transform[idx]
@wp.kernel(enable_backward=False)
def accumulate_contact_forces_kernel(
num_contacts: wp.array[wp.int32],
contact_shape0: wp.array[wp.int32],
contact_shape1: wp.array[wp.int32],
contact_force: wp.array[wp.spatial_vector],
contact_normal: wp.array[wp.vec3],
sensing_shape_to_row: wp.array[wp.int32],
counterpart_shape_to_col: wp.array[wp.int32],
# output
force_matrix: wp.array2d[wp.vec3],
total_force: wp.array[wp.vec3],
force_matrix_friction: wp.array2d[wp.vec3],
total_force_friction: wp.array[wp.vec3],
):
"""Accumulate per-contact forces and friction into total and/or per-counterpart arrays. Parallelizes over contacts."""
con_idx = wp.tid()
if con_idx >= num_contacts[0]:
return
shape0 = contact_shape0[con_idx]
shape1 = contact_shape1[con_idx]
assert shape0 >= 0 and shape1 >= 0
force = wp.spatial_top(contact_force[con_idx])
# Decompose into normal and friction (tangential) components
n = contact_normal[con_idx]
len_sq = wp.dot(n, n)
if wp.abs(len_sq - 1.0) > 1.0e-4:
n = wp.normalize(n)
friction = force - wp.dot(force, n) * n
row0 = sensing_shape_to_row[shape0]
row1 = sensing_shape_to_row[shape1]
# total force and friction
if total_force:
assert total_force_friction
if row0 >= 0:
wp.atomic_add(total_force, row0, force)
wp.atomic_add(total_force_friction, row0, friction)
if row1 >= 0:
wp.atomic_add(total_force, row1, -force)
wp.atomic_add(total_force_friction, row1, -friction)
# per-counterpart forces and friction
if force_matrix:
assert force_matrix_friction
col0 = counterpart_shape_to_col[shape0]
col1 = counterpart_shape_to_col[shape1]
if row0 >= 0 and col1 >= 0:
wp.atomic_add(force_matrix, row0, col1, force)
wp.atomic_add(force_matrix_friction, row0, col1, friction)
if row1 >= 0 and col0 >= 0:
wp.atomic_add(force_matrix, row1, col0, -force)
wp.atomic_add(force_matrix_friction, row1, col0, -friction)
@wp.kernel(enable_backward=False)
def expand_body_to_shape_kernel(
body_to_row: wp.array[wp.int32],
body_to_col: wp.array[wp.int32],
shape_body: wp.array[wp.int32],
# output
shape_to_row: wp.array[wp.int32],
shape_to_col: wp.array[wp.int32],
):
"""Expand body-indexed maps to shape-indexed arrays. Parallelizes over shapes."""
tid = wp.tid()
body = shape_body[tid]
if body_to_row:
row = -1
if body >= 0:
row = body_to_row[body]
shape_to_row[tid] = row
if body_to_col:
col = -1
if body >= 0:
col = body_to_col[body]
shape_to_col[tid] = col
def _check_index_bounds(indices: list[int], count: int, param_name: str, entity_name: str) -> None:
"""Raise IndexError if any index is out of range [0, count)."""
for idx in indices:
if idx < 0 or idx >= count:
raise IndexError(f"{param_name} contains index {idx}, but model only has {count} {entity_name}")
def _split_globals(indices: list[int], local_start: int, tail_global_start: int):
"""Partition sorted shape/body indices into (globals, locals) based on world boundaries."""
head = 0
while head < len(indices) and indices[head] < local_start:
head += 1
tail = len(indices)
while tail > head and indices[tail - 1] >= tail_global_start:
tail -= 1
return indices[:head] + indices[tail:], indices[head:tail]
def _normalize_world_start(ws: list[int], world_count: int) -> list[int]:
"""Remap all-global entities into one implicit world when no ``add_world()`` calls were made."""
n = ws[-1] # total entity count
has_no_local_entities = ws[0] == ws[-2]
if has_no_local_entities:
assert world_count <= 1, (
f"No local entities but world_count={world_count}"
) # internal invariant from ModelBuilder
return [0, n, n]
return ws
def _ensure_sorted_unique(indices: list[int], param_name: str) -> list[int]:
"""Return *indices* in strictly increasing order; duplicates are not allowed.
Raises:
ValueError: If *indices* contains duplicate values.
"""
for i in range(1, len(indices)):
if indices[i] == indices[i - 1]:
raise ValueError(f"{param_name} contains duplicate index {indices[i]}")
if indices[i] < indices[i - 1]:
return _ensure_sorted_unique(sorted(indices), param_name)
return indices
def _assign_counterpart_columns(
c_globals: list[int],
c_locals: list[int],
counterpart_world_start: list[int],
world_count: int,
n_entities: int,
) -> tuple[np.ndarray, int, list[list[int]]]:
"""Build counterpart-to-column mapping and per-world counterpart lists.
Returns:
col_map: Array mapping each entity index to its column, or -1 if not a counterpart.
max_cols: Maximum column count across all worlds.
counterparts_by_world: Per-world list of counterpart indices (globals + locals).
"""
col_map = np.full(n_entities, -1, dtype=np.int32)
for col, idx in enumerate(c_globals):
col_map[idx] = col
n_global_cols = len(c_globals)
counterparts_by_world: list[list[int]] = []
max_cols = n_global_cols
n_locals = len(c_locals)
i = 0 # cursor into c_locals
for w in range(world_count):
local_col = n_global_cols
cur_world_locals: list[int] = []
world_end = counterpart_world_start[w + 1]
while i < n_locals and c_locals[i] < world_end:
col_map[c_locals[i]] = local_col
cur_world_locals.append(c_locals[i])
local_col += 1
i += 1
max_cols = max(max_cols, local_col)
counterparts_by_world.append(c_globals + cur_world_locals)
return col_map, max_cols, counterparts_by_world
class SensorContact:
"""Measures contact forces on a set of **sensing objects** (bodies or shapes).
In its simplest form the sensor reports :attr:`total_force` — the total contact force on each sensing object.
Optionally, specify **counterparts** to get a per-counterpart breakdown in :attr:`force_matrix`.
:attr:`total_force` and :attr:`force_matrix` are each nullable: ``total_force`` is ``None`` when
``measure_total=False``; ``force_matrix`` is ``None`` when no counterparts are specified. Their friction
counterparts :attr:`total_force_friction` and :attr:`force_matrix_friction` follow the same rules.
.. rubric:: Multi-world behavior
When the model contains multiple worlds, counterpart mappings are resolved per-world. The collision pipeline and
solver are expected to produce only within-world contacts, so cross-world force accumulation does not arise in
practice. Global counterparts (e.g. ground plane) contribute to every world they contact.
In single-world models where no ``add_world()`` call was made (all entities are global / ``world=-1``), the sensor
treats the entire model as one implicit world and all entities are valid sensing objects.
When counterparts are specified, the force matrix has shape ``(sum_of_sensors_across_worlds, max_counterparts)``
where ``max_counterparts`` is the maximum counterpart count of any single world. Row order matches
:attr:`sensing_obj_idx`. Columns beyond a world's own counterpart count are zero-padded.
:attr:`sensing_obj_idx` and :attr:`counterpart_indices` are flat lists that describe the structure of the output
arrays.
.. rubric:: Terms
- **Sensing object** -- body or shape carrying a contact sensor.
- **Counterpart** -- the other body or shape in a contact interaction.
.. rubric:: Construction and update order
``SensorContact`` requests the ``force`` extended attribute from the model at init, so a :class:`~newton.Contacts`
object created afterwards (via :meth:`Model.contacts() ` or directly) will include it
automatically.
:meth:`update` reads from ``contacts.force``. Call ``solver.update_contacts(contacts)`` before
``sensor.update()`` so that contact forces are current.
Parameters that select bodies or shapes accept label patterns -- see :ref:`label-matching`.
Example:
Measure total contact force on a sphere resting on the ground:
.. testcode::
import warp as wp
import newton
from newton.sensors import SensorContact
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0, 0, 0.1), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.1, label="ball")
model = builder.finalize()
sensor = SensorContact(model, sensing_obj_shapes="ball")
solver = newton.solvers.SolverMuJoCo(model)
state = model.state()
contacts = model.contacts()
solver.step(state, state, None, None, dt=1.0 / 60.0)
solver.update_contacts(contacts)
sensor.update(state, contacts)
force = sensor.total_force.numpy() # (n_sensing_objs, 3)
Raises:
ValueError: If the configuration of sensing/counterpart objects is invalid.
"""
class ObjectType(Enum):
"""Deprecated. Type tag for entries in legacy :attr:`~newton.sensors.SensorContact.sensing_objs` and :attr:`~newton.sensors.SensorContact.counterparts` properties."""
TOTAL = _OBJ_TYPE_TOTAL
"""Total force entry."""
SHAPE = _OBJ_TYPE_SHAPE
"""Individual shape."""
BODY = _OBJ_TYPE_BODY
"""Individual body."""
sensing_obj_type: Literal["body", "shape"]
"""Whether :attr:`sensing_obj_idx` contains body indices (``"body"``) or shape indices (``"shape"``)."""
sensing_obj_idx: list[int]
"""Body or shape index per sensing object, matching the row of output arrays. For ``list[int]`` inputs the caller's
order is preserved; for string patterns the order follows ascending body/shape index."""
counterpart_type: Literal["body", "shape"] | None
"""Whether :attr:`counterpart_indices` contains body indices (``"body"``) or shape indices (``"shape"``).
``None`` when no counterparts are specified."""
counterpart_indices: list[list[int]]
"""Counterpart body or shape indices per sensing object. ``counterpart_indices[i]`` lists the counterparts for row ``i``. Global counterparts appear first, followed by per-world locals in ascending index order."""
total_force: wp.array[wp.vec3] | None
"""Total contact force [N] per sensing object, shape ``(n_sensing_objs,)``, dtype :class:`vec3`.
``None`` when ``measure_total=False``."""
force_matrix: wp.array2d[wp.vec3] | None
"""Per-counterpart contact forces [N], shape ``(n_sensing_objs, max_counterparts)``, dtype :class:`vec3`.
Entry ``[i, j]`` is the force on sensing object ``i`` from counterpart ``counterpart_indices[i][j]``, in world
frame. ``None`` when no counterparts are specified."""
total_force_friction: wp.array[wp.vec3] | None
"""Total friction (tangential) contact force [N] per sensing object, shape ``(n_sensing_objs,)``,
dtype :class:`vec3`. ``None`` when ``measure_total=False``."""
force_matrix_friction: wp.array2d[wp.vec3] | None
"""Per-counterpart friction (tangential) contact forces [N], shape ``(n_sensing_objs, max_counterparts)``,
dtype :class:`vec3`. Entry ``[i, j]`` is the friction force on sensing object ``i`` from counterpart
``counterpart_indices[i][j]``, in world frame. ``None`` when no counterparts are specified."""
sensing_obj_transforms: wp.array[wp.transform]
"""World-frame transforms of sensing objects [m, unitless quaternion],
shape ``(n_sensing_objs,)``, dtype :class:`transform`."""
def __init__(
self,
model: Model,
*,
sensing_obj_bodies: str | list[str] | list[int] | None = None,
sensing_obj_shapes: str | list[str] | list[int] | None = None,
counterpart_bodies: str | list[str] | list[int] | None = None,
counterpart_shapes: str | list[str] | list[int] | None = None,
measure_total: bool = True,
verbose: bool | None = None,
request_contact_attributes: bool = True,
# deprecated
include_total: bool | None = None,
):
"""Initialize the SensorContact.
Exactly one of ``sensing_obj_bodies`` or ``sensing_obj_shapes`` must be specified to define the sensing
objects. At most one of ``counterpart_bodies`` or ``counterpart_shapes`` may be specified. If neither is
specified, only :attr:`total_force` and :attr:`total_force_friction` are available (no per-counterpart breakdown).
Args:
model: The simulation model providing shape/body definitions and world layout.
sensing_obj_bodies: List of body indices, single pattern to match
against body labels, or list of patterns where any one matches.
sensing_obj_shapes: List of shape indices, single pattern to match
against shape labels, or list of patterns where any one matches.
counterpart_bodies: List of body indices, single pattern to match
against body labels, or list of patterns where any one matches.
counterpart_shapes: List of shape indices, single pattern to match
against shape labels, or list of patterns where any one matches.
measure_total: If True (default), :attr:`total_force` and :attr:`total_force_friction` are allocated.
If False, both are None.
verbose: If True, print details. If None, uses ``wp.config.verbose``.
request_contact_attributes: If True (default), transparently request the extended contact attribute
``force`` from the model.
include_total: Deprecated. Use ``measure_total`` instead.
"""
if include_total is not None:
warnings.warn(
"SensorContact: 'include_total' is deprecated, use 'measure_total' instead.",
DeprecationWarning,
stacklevel=2,
)
measure_total = include_total
if (sensing_obj_bodies is None) == (sensing_obj_shapes is None):
raise ValueError("Exactly one of `sensing_obj_bodies` and `sensing_obj_shapes` must be specified")
if (counterpart_bodies is not None) and (counterpart_shapes is not None):
raise ValueError("At most one of `counterpart_bodies` and `counterpart_shapes` may be specified.")
self.device = model.device
self.verbose = verbose if verbose is not None else wp.config.verbose
# request contact force attribute
if request_contact_attributes:
model.request_contact_attributes("force")
if sensing_obj_bodies is not None:
s_bodies = match_labels(model.body_label, sensing_obj_bodies)
_check_index_bounds(s_bodies, len(model.body_label), "sensing_obj_bodies", "bodies")
s_shapes = []
else:
s_bodies = []
s_shapes = match_labels(model.shape_label, sensing_obj_shapes)
_check_index_bounds(s_shapes, len(model.shape_label), "sensing_obj_shapes", "shapes")
using_counterparts = True
if counterpart_bodies is not None:
c_bodies = match_labels(model.body_label, counterpart_bodies)
_check_index_bounds(c_bodies, len(model.body_label), "counterpart_bodies", "bodies")
c_shapes = []
elif counterpart_shapes is not None:
c_bodies = []
c_shapes = match_labels(model.shape_label, counterpart_shapes)
_check_index_bounds(c_shapes, len(model.shape_label), "counterpart_shapes", "shapes")
else:
c_shapes = []
c_bodies = []
using_counterparts = False
world_count = model.world_count
# Determine whether sensing objects and counterparts are body-level or shape-level.
sensing_is_body = sensing_obj_bodies is not None
counterpart_is_body = counterpart_bodies is not None
sensing_indices = s_bodies if sensing_is_body else s_shapes
counterpart_indices = c_bodies if counterpart_is_body else c_shapes
sensing_world_start = _normalize_world_start(
(model.body_world_start if sensing_is_body else model.shape_world_start).list(), world_count
)
counterpart_world_start = _normalize_world_start(
(model.body_world_start if counterpart_is_body else model.shape_world_start).list(), world_count
)
sensing_indices_ordered = list(sensing_indices) # preserve user's original order
sensing_indices = _ensure_sorted_unique(
sensing_indices, "sensing_obj_bodies" if sensing_is_body else "sensing_obj_shapes"
)
counterpart_indices = _ensure_sorted_unique(
counterpart_indices, "counterpart_bodies" if counterpart_is_body else "counterpart_shapes"
)
if not sensing_indices:
raise ValueError(
f"No {'bodies' if sensing_is_body else 'shapes'} matched the sensing object pattern(s). "
"Check that the labels exist in the model."
)
if using_counterparts and not counterpart_indices:
raise ValueError(
f"No {'bodies' if counterpart_is_body else 'shapes'} matched the counterpart pattern(s). "
"Check that the labels exist in the model."
)
s_globals, _ = _split_globals(sensing_indices, sensing_world_start[0], sensing_world_start[world_count])
if s_globals:
raise ValueError(f"Global bodies/shapes (world=-1) cannot be sensing objects. Global indices: {s_globals}")
# Assign rows to sensing objects
n_entities_s = len(model.body_label) if sensing_is_body else model.shape_count
sensing_to_row = np.full(n_entities_s, -1, dtype=np.int32)
sensing_to_row[sensing_indices_ordered] = np.arange(len(sensing_indices_ordered), dtype=np.int32)
# Assign columns to counterparts: first global, then local
c_globals, c_locals = _split_globals(
counterpart_indices, counterpart_world_start[0], counterpart_world_start[world_count]
)
n_entities_c = len(model.body_label) if counterpart_is_body else model.shape_count
counterpart_to_col, max_readings, counterparts_by_world = _assign_counterpart_columns(
c_globals, c_locals, counterpart_world_start, world_count, n_entities_c
)
if not measure_total and max_readings == 0:
raise ValueError(
"Sensor configured with measure_total=False and no counterparts — "
"at least one output (total_force or force_matrix) must be enabled."
)
n_rows = len(sensing_indices)
# --- Build Warp arrays ---
n_shapes = model.shape_count
body_to_row = None
body_to_col = None
if sensing_is_body:
body_to_row = wp.array(sensing_to_row, dtype=wp.int32, device=self.device)
self._sensing_shape_to_row = wp.full(n_shapes, -1, dtype=wp.int32, device=self.device)
else:
self._sensing_shape_to_row = wp.array(sensing_to_row, dtype=wp.int32, device=self.device)
if counterpart_is_body:
body_to_col = wp.array(counterpart_to_col, dtype=wp.int32, device=self.device)
self._counterpart_shape_to_col = wp.full(n_shapes, -1, dtype=wp.int32, device=self.device)
else:
self._counterpart_shape_to_col = wp.array(counterpart_to_col, dtype=wp.int32, device=self.device)
if sensing_is_body or counterpart_is_body:
wp.launch(
expand_body_to_shape_kernel,
dim=n_shapes,
inputs=[
body_to_row if sensing_is_body else None,
body_to_col if counterpart_is_body else None,
model.shape_body,
],
outputs=[
self._sensing_shape_to_row,
self._counterpart_shape_to_col,
],
device=self.device,
)
total_cols = int(measure_total) + max_readings
self._net_force = wp.zeros((n_rows, total_cols), dtype=wp.vec3, device=self.device)
self._net_friction_force = wp.zeros((n_rows, total_cols), dtype=wp.vec3, device=self.device)
if measure_total:
self.total_force = self._net_force[:, 0]
self.total_force_friction = self._net_friction_force[:, 0]
else:
self.total_force = None
self.total_force_friction = None
if max_readings > 0:
self.force_matrix = self._net_force[:, int(measure_total) :]
self.force_matrix_friction = self._net_friction_force[:, int(measure_total) :]
else:
self.force_matrix = None
self.force_matrix_friction = None
self.sensing_obj_type = "body" if sensing_is_body else "shape"
self.counterpart_type = "body" if counterpart_is_body else ("shape" if counterpart_indices else None)
self.sensing_obj_idx = sensing_indices_ordered
# Map each sensing object to its world's counterpart list.
world_starts = np.array(sensing_world_start[:world_count])
worlds = np.searchsorted(world_starts, sensing_indices_ordered, side="right") - 1
self.counterpart_indices = [counterparts_by_world[w] for w in worlds]
if self.verbose:
print("SensorContact initialized:")
print(f" Sensing objects: {n_rows} ({self.sensing_obj_type}s)")
print(
f" Counterpart columns: {max_readings}"
+ (f" ({self.counterpart_type}s)" if self.counterpart_type else "")
)
print(
f" total_force: {'yes' if measure_total else 'no'}, "
f"force_matrix: {'yes' if max_readings > 0 else 'no'}"
)
self._model = model
self._sensing_obj_indices = wp.array(sensing_indices_ordered, dtype=wp.int32, device=self.device)
obj_type = _OBJ_TYPE_BODY if sensing_is_body else _OBJ_TYPE_SHAPE
self._sensing_obj_types = wp.full(n_rows, obj_type, dtype=wp.int32, device=self.device)
self.sensing_obj_transforms = wp.zeros(n_rows, dtype=wp.transform, device=self.device)
self._init_deprecated_shims(measure_total, world_count, worlds)
def _init_deprecated_shims(self, measure_total: bool, world_count: int, sensing_obj_worlds: np.ndarray):
"""Store data needed by deprecated backward-compatible properties.
The properties themselves are computed lazily on first access and cached.
"""
self._measure_total = measure_total
self._world_count = world_count
self._sensing_obj_worlds = sensing_obj_worlds
@property
def shape(self) -> tuple[int, int]:
"""Deprecated. Dimensions of :attr:`net_force`."""
warnings.warn(
"SensorContact.shape is deprecated. Use total_force.shape / force_matrix.shape instead.",
DeprecationWarning,
stacklevel=2,
)
return tuple(self._net_force.shape)
@property
def sensing_objs(self) -> list[list[tuple[int, ObjectType]]]:
"""Deprecated. Use :attr:`sensing_obj_idx` and :attr:`sensing_obj_type` instead."""
warnings.warn(
"SensorContact.sensing_objs is deprecated. Use 'sensing_obj_idx' and 'sensing_obj_type' instead.",
DeprecationWarning,
stacklevel=2,
)
if hasattr(self, "_deprecated_sensing_objs"):
return self._deprecated_sensing_objs
obj_type = self.ObjectType.BODY if self.sensing_obj_type == "body" else self.ObjectType.SHAPE
result: list[list[tuple[int, SensorContact.ObjectType]]] = [[] for _ in range(self._world_count)]
for i, idx in enumerate(self.sensing_obj_idx):
result[int(self._sensing_obj_worlds[i])].append((idx, obj_type))
self._deprecated_sensing_objs = result
return result
@property
def counterparts(self) -> list[list[tuple[int, ObjectType]]]:
"""Deprecated. Use :attr:`counterpart_indices` and :attr:`counterpart_type` instead."""
warnings.warn(
"SensorContact.counterparts is deprecated. Use 'counterpart_indices' and 'counterpart_type' instead.",
DeprecationWarning,
stacklevel=2,
)
if hasattr(self, "_deprecated_counterparts"):
return self._deprecated_counterparts
cp_type = (
self.ObjectType.BODY
if self.counterpart_type == "body"
else self.ObjectType.SHAPE
if self.counterpart_type == "shape"
else None
)
result: list[list[tuple[int, SensorContact.ObjectType]]] = [[] for _ in range(self._world_count)]
seen_worlds: set[int] = set()
for i in range(len(self.sensing_obj_idx)):
w = int(self._sensing_obj_worlds[i])
if w in seen_worlds:
continue
seen_worlds.add(w)
entries: list[tuple[int, SensorContact.ObjectType]] = []
if self._measure_total:
entries.append((-1, self.ObjectType.TOTAL))
if cp_type is not None:
for idx in self.counterpart_indices[i]:
entries.append((idx, cp_type))
result[w] = entries
self._deprecated_counterparts = result
return result
@property
def reading_indices(self) -> list[list[list[int]]]:
"""Deprecated. Active counterpart indices per sensing object, per world."""
warnings.warn(
"SensorContact.reading_indices is deprecated. Use 'counterpart_indices' instead.",
DeprecationWarning,
stacklevel=2,
)
if hasattr(self, "_deprecated_reading_indices"):
return self._deprecated_reading_indices
result: list[list[list[int]]] = [[] for _ in range(self._world_count)]
for i in range(len(self.sensing_obj_idx)):
w = int(self._sensing_obj_worlds[i])
n_active = int(self._measure_total) + len(self.counterpart_indices[i])
result[w].append(list(range(n_active)))
self._deprecated_reading_indices = result
return result
def update(self, state: State | None, contacts: Contacts):
"""Update the contact sensor readings based on the provided state and contacts.
Computes world-frame transforms for all sensing objects and evaluates contact forces and their friction
(tangential) components (total and/or per-counterpart, depending on sensor configuration).
Args:
state: The simulation state providing body transforms, or None to skip
the transform update.
contacts: The contact data to evaluate.
Raises:
ValueError: If ``contacts.force`` is None.
ValueError: If ``contacts.device`` does not match the sensor's device.
"""
# update sensing object transforms
n = len(self._sensing_obj_indices)
if n > 0 and state is not None and state.body_q is not None:
wp.launch(
compute_sensing_obj_transforms_kernel,
dim=n,
inputs=[
self._sensing_obj_indices,
self._sensing_obj_types,
self._model.shape_body,
self._model.shape_transform,
state.body_q,
],
outputs=[self.sensing_obj_transforms],
device=self.device,
)
if contacts.force is None:
raise ValueError(
"SensorContact requires a ``Contacts`` object with ``force`` allocated. "
"Create ``SensorContact`` before ``Contacts`` for automatically requesting it."
)
if contacts.device != self.device:
raise ValueError(f"Contacts device ({contacts.device}) does not match sensor device ({self.device}).")
self._eval_forces(contacts)
@property
def net_force(self) -> wp.array2d:
"""Deprecated. Use :attr:`total_force` and :attr:`force_matrix` instead."""
warnings.warn(
"SensorContact.net_force is deprecated. Use 'total_force' for total forces "
"and 'force_matrix' for per-counterpart forces.",
DeprecationWarning,
stacklevel=2,
)
return self._net_force
def _eval_forces(self, contacts: Contacts):
"""Zero and recompute force and friction arrays from the given contacts."""
self._net_force.zero_()
self._net_friction_force.zero_()
wp.launch(
accumulate_contact_forces_kernel,
dim=contacts.rigid_contact_max,
inputs=[
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.force,
contacts.rigid_contact_normal,
self._sensing_shape_to_row,
self._counterpart_shape_to_col,
],
outputs=[
self.force_matrix,
self.total_force,
self.force_matrix_friction,
self.total_force_friction,
],
device=self.device,
)
================================================
FILE: newton/_src/sensors/sensor_frame_transform.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Frame Transform Sensor - measures transforms relative to sites."""
import warp as wp
from ..geometry import ShapeFlags
from ..sim.model import Model
from ..sim.state import State
from ..utils.selection import match_labels
@wp.kernel
def compute_shape_transforms_kernel(
shapes: wp.array[int],
shape_body: wp.array[int],
shape_transform: wp.array[wp.transform],
body_q: wp.array[wp.transform],
# output
world_transforms: wp.array[wp.transform],
):
"""Compute world transforms for a list of shape indices.
Args:
shapes: Array of shape indices
shape_body: Model's shape_body array (body parent of each shape)
shape_transform: Model's shape_transform array (local transforms)
body_q: State's body_q array (body world transforms)
world_transforms: Output array for computed world transforms
"""
tid = wp.tid()
shape_idx = shapes[tid]
body_idx = shape_body[shape_idx]
if body_idx >= 0:
# Shape attached to a body
X_wb = body_q[body_idx]
X_bs = shape_transform[shape_idx]
world_transforms[shape_idx] = wp.transform_multiply(X_wb, X_bs)
else:
# Static shape in world frame
world_transforms[shape_idx] = shape_transform[shape_idx]
@wp.kernel
def compute_relative_transforms_kernel(
all_shape_transforms: wp.array[wp.transform],
shapes: wp.array[int],
reference_sites: wp.array[int],
# output
relative_transforms: wp.array[wp.transform],
):
"""Compute relative transforms expressing object poses in reference frame coordinates.
Args:
all_shape_transforms: Array of world transforms for all shapes (indexed by shape index)
shapes: Indices of target shapes
reference_sites: Indices of reference sites
relative_transforms: Output array of relative transforms
Computes X_ro = X_wr^{-1} * X_wo for each pair, where:
- X_wo is the world transform of the object shape (object to world)
- X_wr is the world transform of the reference site (reference to world)
- X_ro is the transform from object to reference (expresses object pose in reference frame)
"""
tid = wp.tid()
shape_idx = shapes[tid]
ref_idx = reference_sites[tid]
X_wo = all_shape_transforms[shape_idx]
X_wr = all_shape_transforms[ref_idx]
# Compute relative transform: express object pose in reference frame coordinates
X_ro = wp.transform_multiply(wp.transform_inverse(X_wr), X_wo)
relative_transforms[tid] = X_ro
class SensorFrameTransform:
"""Sensor that measures transforms of shapes/sites relative to reference sites.
This sensor computes the relative transform from each reference site to each
target shape: ``X_ro = inverse(X_wr) * X_wo``, where *X_wo* is the world
transform of the target, *X_wr* is the world transform of the reference site,
and *X_ro* expresses the target's pose in the reference frame.
**Objects** (``shapes``) can be any shape index, including both regular shapes
and sites. **Reference frames** (``reference_sites``) must be sites (validated
at initialization). A single reference site broadcasts to all shapes;
otherwise the counts must match 1:1.
Attributes:
transforms: Relative transforms [m, unitless quaternion], shape
``(N,)`` (updated after each call to :meth:`update`).
The ``shapes`` and ``reference_sites`` parameters accept label patterns -- see :ref:`label-matching`.
Example:
Measure a shape's pose relative to a reference site:
.. testcode::
import warp as wp
import newton
from newton.sensors import SensorFrameTransform
builder = newton.ModelBuilder()
body = builder.add_body(xform=wp.transform((0, 0, 1), wp.quat_identity()))
builder.add_shape_box(body, hx=0.1, hy=0.1, hz=0.1, label="box")
builder.add_site(body, label="ref")
model = builder.finalize()
sensor = SensorFrameTransform(model, shapes="box", reference_sites="ref")
state = model.state()
sensor.update(state)
transforms = sensor.transforms.numpy() # shape: (N, 7)
"""
def __init__(
self,
model: Model,
shapes: str | list[str] | list[int],
reference_sites: str | list[str] | list[int],
*,
verbose: bool | None = None,
):
"""Initialize the SensorFrameTransform.
Args:
model: The model to measure.
shapes: List of shape indices, single pattern to match against shape
labels, or list of patterns where any one matches.
reference_sites: List of site indices, single pattern to match against
site labels, or list of patterns where any one matches. Must expand
to one site or the same number as ``shapes``.
verbose: If True, print details. If None, uses ``wp.config.verbose``.
Raises:
ValueError: If arguments are invalid or no labels match.
"""
self.model = model
self.verbose = verbose if verbose is not None else wp.config.verbose
# Resolve label patterns to indices
original_shapes = shapes
shapes = match_labels(model.shape_label, shapes)
original_reference_sites = reference_sites
reference_sites = match_labels(model.shape_label, reference_sites)
# Validate shape indices
if not shapes:
if isinstance(original_shapes, list) and len(original_shapes) == 0:
raise ValueError("'shapes' must not be empty")
raise ValueError(f"No shapes matched the given pattern {original_shapes!r}")
if any(idx < 0 or idx >= model.shape_count for idx in shapes):
raise ValueError(f"Invalid shape indices. Must be in range [0, {model.shape_count})")
# Validate reference site indices
if not reference_sites:
if isinstance(original_reference_sites, list) and len(original_reference_sites) == 0:
raise ValueError("'reference_sites' must not be empty")
raise ValueError(f"No reference sites matched the given pattern {original_reference_sites!r}")
if any(idx < 0 or idx >= model.shape_count for idx in reference_sites):
raise ValueError(f"Invalid reference site indices. Must be in range [0, {model.shape_count})")
# Verify that reference indices are actually sites
shape_flags = model.shape_flags.numpy()
for idx in reference_sites:
if not (shape_flags[idx] & ShapeFlags.SITE):
raise ValueError(f"Reference index {idx} (label: {model.shape_label[idx]}) is not a site")
# Handle reference site matching
if len(reference_sites) == 1:
# Single reference site for all shapes
reference_sites_matched = reference_sites * len(shapes)
elif len(reference_sites) == len(shapes):
reference_sites_matched = list(reference_sites)
else:
raise ValueError(
f"Number of reference sites ({len(reference_sites)}) must match "
f"number of shapes ({len(shapes)}) or be 1"
)
# Build list of unique shape indices that need transforms computed
all_indices = set(shapes) | set(reference_sites_matched)
self._unique_shape_indices = sorted(all_indices)
# Allocate transform array for all shapes (indexed by shape index)
# Only the shapes we care about will be computed, rest stay zero
self._all_shape_transforms = wp.zeros(
model.shape_count,
dtype=wp.transform,
device=model.device,
)
# Allocate output array
self.transforms = wp.zeros(
len(shapes),
dtype=wp.transform,
device=model.device,
)
# Convert indices to warp arrays (done once at init)
self._unique_indices_arr = wp.array(self._unique_shape_indices, dtype=int, device=model.device)
self._shape_indices_arr = wp.array(shapes, dtype=int, device=model.device)
self._reference_indices_arr = wp.array(reference_sites_matched, dtype=int, device=model.device)
if self.verbose:
print("SensorFrameTransform initialized:")
print(f" Shapes: {len(shapes)}")
print(f" Reference sites: {len(set(reference_sites_matched))} unique")
print(
f" Unique shapes to compute: {len(self._unique_shape_indices)} (optimized from {len(shapes) + len(reference_sites_matched)})"
)
def update(self, state: State):
"""Update sensor measurements based on current state.
Reads ``state.body_q`` to compute world-frame shape transforms.
Args:
state: The current state. Reads ``body_q``, which is updated by a solver step or :func:`~newton.eval_fk`.
"""
# Compute world transforms for all unique shapes directly into the all_shape_transforms array
wp.launch(
compute_shape_transforms_kernel,
dim=len(self._unique_shape_indices),
inputs=[self._unique_indices_arr, self.model.shape_body, self.model.shape_transform, state.body_q],
outputs=[self._all_shape_transforms],
device=self.model.device,
)
# Compute relative transforms by indexing directly into all_shape_transforms
wp.launch(
compute_relative_transforms_kernel,
dim=len(self._shape_indices_arr),
inputs=[self._all_shape_transforms, self._shape_indices_arr, self._reference_indices_arr],
outputs=[self.transforms],
device=self.model.device,
)
================================================
FILE: newton/_src/sensors/sensor_imu.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""IMU Sensor - measures accelerations and angular velocities at sensor sites."""
import warp as wp
from ..geometry.flags import ShapeFlags
from ..sim.model import Model
from ..sim.state import State
from ..utils.selection import match_labels
@wp.kernel
def compute_sensor_imu_kernel(
gravity: wp.array[wp.vec3],
body_world: wp.array[wp.int32],
body_com: wp.array[wp.vec3],
shape_body: wp.array[int],
shape_transform: wp.array[wp.transform],
sensor_sites: wp.array[int],
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_qdd: wp.array[wp.spatial_vector],
# output
accelerometer: wp.array[wp.vec3],
gyroscope: wp.array[wp.vec3],
):
"""Compute accelerations and angular velocities at sensor sites."""
sensor_idx = wp.tid()
if sensor_idx >= len(sensor_sites):
return
site_idx = sensor_sites[sensor_idx]
body_idx = shape_body[site_idx]
site_transform = shape_transform[site_idx]
if body_idx < 0:
accelerometer[sensor_idx] = wp.quat_rotate_inv(site_transform.q, -gravity[0])
gyroscope[sensor_idx] = wp.vec3(0.0)
return
world_idx = body_world[body_idx]
world_g = gravity[wp.max(world_idx, 0)]
body_acc = body_qdd[body_idx]
body_quat = body_q[body_idx].q
r = wp.quat_rotate(body_quat, site_transform.p - body_com[body_idx])
vel_ang = wp.spatial_bottom(body_qd[body_idx])
acc_lin = (
wp.spatial_top(body_acc)
- world_g
+ wp.cross(wp.spatial_bottom(body_acc), r)
+ wp.cross(vel_ang, wp.cross(vel_ang, r))
)
q = body_quat * site_transform.q
accelerometer[sensor_idx] = wp.quat_rotate_inv(q, acc_lin)
gyroscope[sensor_idx] = wp.quat_rotate_inv(q, vel_ang)
class SensorIMU:
"""Inertial Measurement Unit sensor.
Measures linear acceleration (specific force) and angular velocity at the
given sites. Each site defines an IMU frame; outputs are expressed in that
frame.
This sensor requires the extended state attribute ``body_qdd``. By default,
constructing the sensor requests ``body_qdd`` from the model so that
subsequent ``model.state()`` calls allocate it automatically. The solver
must also support computing ``body_qdd``
(e.g. :class:`~newton.solvers.SolverMuJoCo`).
The ``sites`` parameter accepts label patterns -- see :ref:`label-matching`.
Example:
.. testcode::
import warp as wp
import newton
from newton.sensors import SensorIMU
builder = newton.ModelBuilder()
builder.add_ground_plane()
body = builder.add_body(xform=wp.transform((0, 0, 1), wp.quat_identity()))
builder.add_shape_sphere(body, radius=0.1)
builder.add_site(body, label="imu_0")
model = builder.finalize()
imu = SensorIMU(model, sites="imu_*")
solver = newton.solvers.SolverMuJoCo(model)
state = model.state()
# after solver step
solver.step(state, state, None, None, dt=1.0 / 60.0)
imu.update(state)
acc = imu.accelerometer.numpy()
gyro = imu.gyroscope.numpy()
"""
accelerometer: wp.array[wp.vec3]
"""Linear acceleration readings [m/s²] in sensor frame, shape ``(n_sensors,)``."""
gyroscope: wp.array[wp.vec3]
"""Angular velocity readings [rad/s] in sensor frame, shape ``(n_sensors,)``."""
def __init__(
self,
model: Model,
sites: str | list[str] | list[int],
*,
verbose: bool | None = None,
request_state_attributes: bool = True,
):
"""Initialize SensorIMU.
Transparently requests the extended state attribute ``body_qdd`` from the model, which is required for acceleration
data.
Args:
model: The model to use.
sites: List of site indices, single pattern to match against site
labels, or list of patterns where any one matches.
verbose: If True, print details. If None, uses ``wp.config.verbose``.
request_state_attributes: If True (default), transparently request the extended state attribute ``body_qdd`` from the model.
If False, ``model`` is not modified and the attribute must be requested elsewhere before calling ``model.state()``.
Raises:
ValueError: If no labels match or invalid sites are passed.
"""
self.model = model
self.verbose = verbose if verbose is not None else wp.config.verbose
original_sites = sites
sites = match_labels(model.shape_label, sites)
if not sites:
if isinstance(original_sites, list) and len(original_sites) == 0:
raise ValueError("'sites' must not be empty")
raise ValueError(f"No sites matched the given pattern {original_sites!r}")
# request acceleration state attribute
if request_state_attributes:
self.model.request_state_attributes("body_qdd")
self._validate_sensor_sites(sites)
self.sensor_sites_arr = wp.array(sites, dtype=int, device=model.device)
self.n_sensors: int = len(sites)
self.accelerometer = wp.zeros(self.n_sensors, dtype=wp.vec3, device=model.device)
self.gyroscope = wp.zeros(self.n_sensors, dtype=wp.vec3, device=model.device)
if self.verbose:
print("SensorIMU initialized:")
print(f" Sites: {len(set(sites))}")
# TODO: body per site
def _validate_sensor_sites(self, sensor_sites: list[int]):
"""Validate the sensor sites."""
shape_flags = self.model.shape_flags.numpy()
for site_idx in sensor_sites:
if site_idx < 0 or site_idx >= self.model.shape_count:
raise ValueError(f"sensor site index {site_idx} is out of range")
if not (shape_flags[site_idx] & ShapeFlags.SITE):
raise ValueError(f"sensor site index {site_idx} is not a site")
def update(self, state: State):
"""Update the IMU sensor.
Args:
state: The state to update the sensor from.
"""
if state.body_qdd is None:
raise ValueError("SensorIMU requires a State with body_qdd allocated. Create SensorIMU before State.")
wp.launch(
compute_sensor_imu_kernel,
dim=self.n_sensors,
inputs=[
self.model.gravity,
self.model.body_world,
self.model.body_com,
self.model.shape_body,
self.model.shape_transform,
self.sensor_sites_arr,
state.body_q,
state.body_qd,
state.body_qdd,
],
outputs=[self.accelerometer, self.gyroscope],
device=self.model.device,
)
================================================
FILE: newton/_src/sensors/sensor_raycast.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import math
import warnings
import numpy as np
import warp as wp
from ..geometry.raycast import sensor_raycast_kernel, sensor_raycast_particles_kernel
from ..sim import Model, State
@wp.kernel
def clamp_no_hits_kernel(depth_image: wp.array[float], max_dist: float):
"""Kernel to replace max_distance values with -1.0 to indicate no intersection."""
tid = wp.tid()
if depth_image[tid] >= max_dist:
depth_image[tid] = -1.0
INT32_MAX = (1 << 31) - 1
# Upper bound on work per pixel when ray-marching particles
MAX_PARTICLE_RAY_MARCH_STEPS = 1 << 20
class SensorRaycast:
"""Raycast-based depth sensor for generating depth images.
The SensorRaycast simulates a depth camera by casting rays from a virtual camera through each pixel
in an image. For each pixel, it finds the closest intersection with the scene geometry and records
the distance as a depth value.
The sensor supports perspective cameras with configurable field of view, aspect ratio, and resolution.
The resulting depth image has the same resolution as specified, with depth values representing the
distance from the camera to the closest surface along each ray.
.. rubric:: Camera Coordinate System
The camera uses a right-handed coordinate system where:
- The forward direction (camera_direction) is the direction the camera is looking
- The up direction (camera_up) defines the camera's vertical orientation
- The right direction (camera_right) is computed as the cross product of forward and up
.. rubric:: Depth Values
- Positive depth values: Distance to the closest surface
- Negative depth values (-1.0): No intersection found (ray missed all geometry)
Attributes:
device: The device (CPU/GPU) where computations are performed
camera_position: 3D position of the camera in world space
camera_direction: Forward direction vector (normalized)
camera_up: Up direction vector (normalized)
camera_right: Right direction vector (normalized)
fov_radians: Vertical field of view in radians
aspect_ratio: Width/height aspect ratio
width: Image width in pixels
height: Image height in pixels
depth_image: 2D depth image array (height, width)
"""
def __init__(
self,
model: Model,
camera_position: tuple[float, float, float] | np.ndarray,
camera_direction: tuple[float, float, float] | np.ndarray,
camera_up: tuple[float, float, float] | np.ndarray,
fov_radians: float,
width: int,
height: int,
*,
max_distance: float = 1000.0,
):
"""Initialize a SensorRaycast.
Args:
model: The Newton model containing the geometry to raycast against
camera_position: 3D position of the camera in world space
camera_direction: Forward direction of the camera (will be normalized)
camera_up: Up direction of the camera (will be normalized)
fov_radians: Vertical field of view in radians
width: Image width in pixels
height: Image height in pixels
max_distance: Maximum ray distance; rays beyond this return no hit
"""
self.model = model
self.device = model.device
self.width = width
self.height = height
self.fov_radians = fov_radians
self.aspect_ratio = float(width) / float(height)
self.max_distance = max_distance
# Set initial camera parameters
self.camera_position = np.array(camera_position, dtype=np.float32)
camera_dir = np.array(camera_direction, dtype=np.float32)
camera_up = np.array(camera_up, dtype=np.float32)
# Pre-compute field of view scale
self.fov_scale = math.tan(fov_radians * 0.5)
# Create depth image buffer
self._depth_buffer = wp.zeros((height, width), dtype=float, device=self.device)
self.depth_image = self._depth_buffer
self._resolution = wp.vec2(float(width), float(height))
# Compute camera basis vectors and warp vectors
self._compute_camera_basis(camera_dir, camera_up)
# Lazily constructed structure for particle queries
self._particle_grid: wp.HashGrid | None = None
self._particle_step_warning_emitted = False
def _compute_camera_basis(self, direction: np.ndarray, up: np.ndarray):
"""Compute orthonormal camera basis vectors and update warp vectors.
Args:
direction: Camera direction vector (will be normalized)
up: Camera up vector (will be normalized)
"""
# Normalize direction vectors
self.camera_direction = direction / np.linalg.norm(direction)
self.camera_up = up / np.linalg.norm(up)
# Compute right vector as cross product of forward and up
self.camera_right = np.cross(self.camera_direction, self.camera_up)
right_norm = np.linalg.norm(self.camera_right)
if right_norm < 1e-8:
# Camera direction and up are parallel, use a different up vector
if abs(self.camera_direction[2]) < 0.9:
temp_up = np.array([0.0, 0.0, 1.0], dtype=np.float32)
else:
temp_up = np.array([0.0, 1.0, 0.0], dtype=np.float32)
self.camera_right = np.cross(self.camera_direction, temp_up)
right_norm = np.linalg.norm(self.camera_right)
self.camera_right = self.camera_right / right_norm
# Recompute up vector to ensure orthogonality
self.camera_up = np.cross(self.camera_right, self.camera_direction)
self.camera_up = self.camera_up / np.linalg.norm(self.camera_up)
def update(
self,
state: State,
*,
include_particles: bool = False,
particle_march_step: float | None = None,
):
"""Update the raycast sensor to generate a depth image.
Casts rays from the camera through each pixel and records the distance to the closest
intersection with the scene geometry. When ``include_particles`` is enabled (not enabled by default),
particles stored in the simulation state are also considered.
Args:
state: The current state of the simulation containing body poses
include_particles: Whether to test ray intersections against particles present in ``state``
particle_march_step: Optional stride used when marching along each ray during particle queries.
Defaults to half of the maximum particle radius when particles are available.
"""
if include_particles and particle_march_step is not None and particle_march_step <= 0.0:
raise ValueError("particle_march_step must be positive when provided.")
# Reset depth buffer to maximum distance
self._depth_buffer.fill_(self.max_distance)
num_shapes = len(self.model.shape_body)
if (include_particles and self._does_state_have_particles(state)) or num_shapes != 0:
camera_position = wp.vec3(*self.camera_position)
camera_direction = wp.vec3(*self.camera_direction)
camera_up = wp.vec3(*self.camera_up)
camera_right = wp.vec3(*self.camera_right)
# Launch raycast kernel for each pixel-shape combination
# We use 3D launch with dimensions (width, height, num_shapes)
if num_shapes > 0:
wp.launch(
kernel=sensor_raycast_kernel,
dim=(self.width, self.height, num_shapes),
inputs=[
# Model data
state.body_q,
self.model.shape_body,
self.model.shape_transform,
self.model.shape_type,
self.model.shape_scale,
self.model.shape_source_ptr,
# Camera parameters
camera_position,
camera_direction,
camera_up,
camera_right,
self.fov_scale,
self.aspect_ratio,
self._resolution,
],
outputs=[self._depth_buffer],
device=self.device,
)
if include_particles and self._does_state_have_particles(state):
self._raycast_particles(
state=state,
camera_position=camera_position,
camera_direction=camera_direction,
camera_up=camera_up,
camera_right=camera_right,
march_step=particle_march_step,
)
# Set pixels that still have max_distance to -1.0 to indicate no hit
self._clamp_no_hits()
def _get_particle_grid(self) -> wp.HashGrid:
"""Return a hash grid for particle queries, constructing it lazily."""
if self._particle_grid is None:
with wp.ScopedDevice(self.device):
self._particle_grid = wp.HashGrid(128, 128, 128)
return self._particle_grid
def _raycast_particles(
self,
state: State,
camera_position: wp.vec3,
camera_direction: wp.vec3,
camera_up: wp.vec3,
camera_right: wp.vec3,
march_step: float | None,
) -> None:
"""Intersect rays with particles using a spatial hash grid."""
particle_positions = state.particle_q
particle_count = state.particle_count
particle_radius = self.model.particle_radius
max_radius = float(self.model.particle_max_radius)
search_radius = max_radius + 1.0e-6
step = march_step if march_step is not None else 0.5 * search_radius
max_steps, truncated, requested_steps = self._compute_particle_march_steps(step)
if truncated and not self._particle_step_warning_emitted:
requested_msg = "infinite" if not math.isfinite(requested_steps) else f"{requested_steps:,}"
max_allowed = min(INT32_MAX, MAX_PARTICLE_RAY_MARCH_STEPS)
warnings.warn(
f"Particle ray marching limited to {max_allowed:,} steps (requested {requested_msg}). "
"Increase particle_march_step or reduce max_distance for full coverage.",
RuntimeWarning,
stacklevel=2,
)
self._particle_step_warning_emitted = True
grid = self._get_particle_grid()
with wp.ScopedDevice(self.device):
grid.reserve(particle_count)
grid.build(particle_positions, radius=search_radius)
wp.launch(
kernel=sensor_raycast_particles_kernel,
dim=(self.width, self.height),
inputs=[
grid.id,
particle_positions,
particle_radius,
float(search_radius),
float(step),
int(max_steps),
camera_position,
camera_direction,
camera_up,
camera_right,
float(self.fov_scale),
float(self.aspect_ratio),
self._resolution,
float(self.max_distance),
],
outputs=[self._depth_buffer],
device=self.device,
)
def _does_state_have_particles(self, state: State) -> bool:
"""Check if the given state has particles available for raycasting."""
particle_positions = state.particle_q
if particle_positions is None or state.particle_count == 0:
return False
if self.model.particle_radius is None or self.model.particle_max_radius <= 0.0:
raise ValueError("Model must have valid particle radius to raycast when particles are present.")
return True
def _compute_particle_march_steps(self, step: float) -> tuple[int, bool, float]:
"""Return (steps, truncated, requested_steps) safeguarding loop counters and runtime."""
if step <= 0.0:
raise ValueError("particle march step must be positive.")
ratio = float(self.max_distance) / float(step)
if ratio <= 0.0:
return 1, False, 1.0
max_allowed_steps = min(INT32_MAX, MAX_PARTICLE_RAY_MARCH_STEPS)
if not math.isfinite(ratio):
return max_allowed_steps, True, math.inf
requested_steps = math.floor(ratio) + 1
if requested_steps > max_allowed_steps:
return max_allowed_steps, True, requested_steps
return int(requested_steps), False, int(requested_steps)
def _clamp_no_hits(self):
"""Replace max_distance values with -1.0 to indicate no intersection."""
# Flatten the depth buffer for linear indexing
flattened_buffer = self._depth_buffer.flatten()
wp.launch(
kernel=clamp_no_hits_kernel,
dim=self.height * self.width,
inputs=[flattened_buffer, self.max_distance],
device=self.device,
)
def get_depth_image(self) -> wp.array2d:
"""Get the depth image as a 2D array.
Returns:
2D depth image array with shape (height, width). Values are:
- Positive: Distance to closest surface
- -1.0: No intersection found
"""
return self.depth_image
def get_depth_image_numpy(self) -> np.ndarray:
"""Get the depth image as a numpy array.
Returns:
Numpy array with shape (height, width) containing depth values.
Values are the same as get_depth_image() but as a numpy array.
"""
return self.depth_image.numpy()
def update_camera_pose(
self,
position: tuple[float, float, float] | np.ndarray | None = None,
direction: tuple[float, float, float] | np.ndarray | None = None,
up: tuple[float, float, float] | np.ndarray | None = None,
):
"""Update the camera pose parameters.
Args:
position: New camera position (if provided)
direction: New camera direction (if provided, will be normalized)
up: New camera up vector (if provided, will be normalized)
"""
if position is not None:
self.camera_position = np.array(position, dtype=np.float32)
if direction is not None or up is not None:
# Use current values if not provided
camera_dir = np.array(direction, dtype=np.float32) if direction is not None else self.camera_direction
camera_up = np.array(up, dtype=np.float32) if up is not None else self.camera_up
# Recompute camera basis using shared method
self._compute_camera_basis(camera_dir, camera_up)
def update_camera_parameters(
self,
fov_radians: float | None = None,
width: int | None = None,
height: int | None = None,
max_distance: float | None = None,
):
"""Update camera intrinsic parameters.
Args:
fov_radians: New vertical field of view in radians
width: New image width in pixels
height: New image height in pixels
max_distance: New maximum ray distance
"""
recreate_buffer = False
if width is not None and width != self.width:
self.width = width
recreate_buffer = True
if height is not None and height != self.height:
self.height = height
recreate_buffer = True
if fov_radians is not None:
self.fov_radians = fov_radians
self.fov_scale = math.tan(fov_radians * 0.5)
if max_distance is not None:
self.max_distance = max_distance
if recreate_buffer:
self.aspect_ratio = float(self.width) / float(self.height)
self._resolution = wp.vec2(float(self.width), float(self.height))
self._depth_buffer = wp.zeros((self.height, self.width), dtype=float, device=self.device)
self.depth_image = self._depth_buffer
def point_camera_at(
self,
target: tuple[float, float, float] | np.ndarray,
position: tuple[float, float, float] | np.ndarray | None = None,
up: tuple[float, float, float] | np.ndarray | None = None,
):
"""Point the camera at a specific target location.
Args:
target: 3D point to look at
position: New camera position (if provided)
up: Up vector for camera orientation (default: [0, 0, 1])
"""
if position is not None:
self.camera_position = np.array(position, dtype=np.float32)
if up is None:
up = np.array([0.0, 0.0, 1.0], dtype=np.float32)
target = np.array(target, dtype=np.float32)
direction = target - self.camera_position
self.update_camera_pose(
position=self.camera_position,
direction=direction,
up=up,
)
================================================
FILE: newton/_src/sensors/sensor_tiled_camera.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warnings
from dataclasses import dataclass
import numpy as np
import warp as wp
from ..sim import Model, State
from .warp_raytrace import (
ClearData,
GaussianRenderMode,
RenderConfig,
RenderContext,
RenderLightType,
RenderOrder,
Utils,
)
class _SensorTiledCameraMeta(type):
@property
def RenderContext(cls) -> type[RenderContext]:
warnings.warn(
"Access to SensorTiledCamera.RenderContext is deprecated.",
DeprecationWarning,
stacklevel=2,
)
return RenderContext
class SensorTiledCamera(metaclass=_SensorTiledCameraMeta):
"""Warp-based tiled camera sensor for raytraced rendering across multiple worlds.
Renders up to five image channels per (world, camera) pair:
- **color** -- RGBA shaded image (``uint32``).
- **depth** -- ray-hit distance [m] (``float32``); negative means no hit.
- **normal** -- surface normal at hit point (``vec3f``).
- **albedo** -- unshaded surface color (``uint32``).
- **shape_index** -- shape id per pixel (``uint32``).
All output arrays have shape ``(world_count, camera_count, height, width)``. Use the ``flatten_*`` helpers to
rearrange them into tiled RGBA buffers for display, with one tile per (world, camera) pair laid out in a grid.
Shapes without the ``VISIBLE`` flag are excluded.
Example:
::
sensor = SensorTiledCamera(model)
rays = sensor.utils.compute_pinhole_camera_rays(width, height, fov)
color = sensor.utils.create_color_image_output(width, height)
# each step
sensor.update(state, camera_transforms, rays, color_image=color)
See :class:`RenderConfig` for optional rendering settings and :attr:`ClearData` / :attr:`DEFAULT_CLEAR_DATA` /
:attr:`GRAY_CLEAR_DATA` for image-clear presets.
"""
RenderLightType = RenderLightType
RenderOrder = RenderOrder
GaussianRenderMode = GaussianRenderMode
RenderConfig = RenderConfig
ClearData = ClearData
Utils = Utils
DEFAULT_CLEAR_DATA = ClearData()
GRAY_CLEAR_DATA = ClearData(clear_color=0xFF666666, clear_albedo=0xFF000000)
@dataclass
class Config:
"""Sensor configuration.
.. deprecated::
Use :class:`RenderConfig` and ``SensorTiledCamera.utils.*`` instead.
"""
checkerboard_texture: bool = False
""".. deprecated:: Use ``SensorTiledCamera.utils.assign_checkerboard_material_to_all_shapes()`` instead."""
default_light: bool = False
""".. deprecated:: Use ``SensorTiledCamera.utils.create_default_light()`` instead."""
default_light_shadows: bool = False
""".. deprecated:: Use ``SensorTiledCamera.utils.create_default_light(enable_shadows=True)`` instead."""
enable_ambient_lighting: bool = True
""".. deprecated:: Use ``render_config.enable_ambient_lighting`` instead."""
colors_per_world: bool = False
""".. deprecated:: Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``)."""
colors_per_shape: bool = False
""".. deprecated:: Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``)."""
backface_culling: bool = True
""".. deprecated:: Use ``render_config.enable_backface_culling`` instead."""
enable_textures: bool = False
""".. deprecated:: Use ``render_config.enable_textures`` instead."""
enable_particles: bool = True
""".. deprecated:: Use ``render_config.enable_particles`` instead."""
def __init__(self, model: Model, *, config: Config | RenderConfig | None = None, load_textures: bool = True):
"""Initialize the tiled camera sensor from a simulation model.
Builds the internal :class:`RenderContext`, loads shape geometry (and
optionally textures) from *model*, and exposes :attr:`utils` for
creating output buffers, computing rays, and assigning materials.
Args:
model: Simulation model whose shapes will be rendered.
config: Rendering configuration. Pass a :class:`RenderConfig` to
control raytrace settings directly, or ``None`` to use
defaults. The legacy :class:`Config` dataclass is still
accepted but deprecated.
load_textures: Load texture data from the model. Set to ``False``
to skip texture loading when textures are not needed.
"""
self.model = model
render_config = config
if render_config is None:
render_config = RenderConfig()
elif isinstance(config, SensorTiledCamera.Config):
warnings.warn(
"SensorTiledCamera.Config is deprecated, use SensorTiledCamera.RenderConfig and SensorTiledCamera.utils.* functions instead.",
category=DeprecationWarning,
stacklevel=2,
)
render_config = RenderConfig()
render_config.enable_ambient_lighting = config.enable_ambient_lighting
render_config.enable_backface_culling = config.backface_culling
render_config.enable_textures = config.enable_textures
render_config.enable_particles = config.enable_particles
self.__render_context = RenderContext(
world_count=self.model.world_count,
config=render_config,
device=self.model.device,
)
self.__render_context.init_from_model(self.model, load_textures)
if isinstance(config, SensorTiledCamera.Config):
if config.checkerboard_texture:
self.utils.assign_checkerboard_material_to_all_shapes()
if config.default_light:
self.utils.create_default_light(config.default_light_shadows)
if config.colors_per_world:
self.utils.assign_random_colors_per_world()
elif config.colors_per_shape:
self.utils.assign_random_colors_per_shape()
def sync_transforms(self, state: State):
"""Synchronize shape transforms from the simulation state.
:meth:`update` calls this automatically when *state* is not None.
Args:
state: The current simulation state containing body transforms.
"""
self.__render_context.update(self.model, state)
def update(
self,
state: State | None,
camera_transforms: wp.array2d[wp.transformf],
camera_rays: wp.array4d[wp.vec3f],
*,
color_image: wp.array4d[wp.uint32] | None = None,
depth_image: wp.array4d[wp.float32] | None = None,
shape_index_image: wp.array4d[wp.uint32] | None = None,
normal_image: wp.array4d[wp.vec3f] | None = None,
albedo_image: wp.array4d[wp.uint32] | None = None,
refit_bvh: bool = True,
clear_data: ClearData | None = DEFAULT_CLEAR_DATA,
):
"""Render output images for all worlds and cameras.
Each output array has shape ``(world_count, camera_count, height, width)`` where element
``[world_id, camera_id, y, x]`` corresponds to the ray in ``camera_rays[camera_id, y, x]``. Each output
channel is optional -- pass None to skip that channel's rendering entirely.
Args:
state: Simulation state with body transforms. If not None, calls :meth:`sync_transforms` first.
camera_transforms: Camera-to-world transforms, shape ``(camera_count, world_count)``.
camera_rays: Camera-space rays from :meth:`compute_pinhole_camera_rays`, shape
``(camera_count, height, width, 2)``.
color_image: Output for RGBA color. None to skip.
depth_image: Output for ray-hit distance [m]. None to skip.
shape_index_image: Output for per-pixel shape id. None to skip.
normal_image: Output for surface normals. None to skip.
albedo_image: Output for unshaded surface color. None to skip.
refit_bvh: Refit the BVH before rendering.
clear_data: Values to clear output buffers with.
See :attr:`DEFAULT_CLEAR_DATA`, :attr:`GRAY_CLEAR_DATA`.
"""
if state is not None:
self.sync_transforms(state)
self.__render_context.render(
camera_transforms,
camera_rays,
color_image,
depth_image,
shape_index_image,
normal_image,
albedo_image,
refit_bvh=refit_bvh,
clear_data=clear_data,
)
def compute_pinhole_camera_rays(
self, width: int, height: int, camera_fovs: float | list[float] | np.ndarray | wp.array[wp.float32]
) -> wp.array4d[wp.vec3f]:
"""Compute camera-space ray directions for pinhole cameras.
Generates rays in camera space (origin at the camera center, direction normalized) for each pixel based on the
vertical field of view.
.. deprecated::
Use ``SensorTiledCamera.utils.compute_pinhole_camera_rays`` instead.
Args:
width: Image width [px].
height: Image height [px].
camera_fovs: Vertical FOV angles [rad], shape ``(camera_count,)``.
Returns:
camera_rays: Shape ``(camera_count, height, width, 2)``, dtype ``vec3f``.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.compute_pinhole_camera_rays is deprecated, use SensorTiledCamera.utils.compute_pinhole_camera_rays instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.__render_context.utils.compute_pinhole_camera_rays(width, height, camera_fovs)
def flatten_color_image_to_rgba(
self,
image: wp.array4d[wp.uint32],
out_buffer: wp.array3d[wp.uint8] | None = None,
worlds_per_row: int | None = None,
):
"""Flatten rendered color image to a tiled RGBA buffer.
Arranges ``(world_count * camera_count)`` tiles in a grid. Each tile shows one camera's view of one world.
.. deprecated::
Use ``SensorTiledCamera.utils.flatten_color_image_to_rgba`` instead.
Args:
image: Color output from :meth:`update`, shape ``(world_count, camera_count, height, width)``.
out_buffer: Pre-allocated RGBA buffer. If None, allocates a new one.
worlds_per_row: Tiles per row in the grid. If None, picks a square-ish layout.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.flatten_color_image_to_rgba is deprecated, use SensorTiledCamera.utils.flatten_color_image_to_rgba instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.utils.flatten_color_image_to_rgba(image, out_buffer, worlds_per_row)
def flatten_normal_image_to_rgba(
self,
image: wp.array4d[wp.vec3f],
out_buffer: wp.array3d[wp.uint8] | None = None,
worlds_per_row: int | None = None,
):
"""Flatten rendered normal image to a tiled RGBA buffer.
Arranges ``(world_count * camera_count)`` tiles in a grid. Each tile shows one camera's view of one world.
.. deprecated::
Use ``SensorTiledCamera.utils.flatten_normal_image_to_rgba`` instead.
Args:
image: Normal output from :meth:`update`, shape ``(world_count, camera_count, height, width)``.
out_buffer: Pre-allocated RGBA buffer. If None, allocates a new one.
worlds_per_row: Tiles per row in the grid. If None, picks a square-ish layout.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.flatten_normal_image_to_rgba is deprecated, use SensorTiledCamera.utils.flatten_normal_image_to_rgba instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.utils.flatten_normal_image_to_rgba(image, out_buffer, worlds_per_row)
def flatten_depth_image_to_rgba(
self,
image: wp.array4d[wp.float32],
out_buffer: wp.array3d[wp.uint8] | None = None,
worlds_per_row: int | None = None,
depth_range: wp.array[wp.float32] | None = None,
):
"""Flatten rendered depth image to a tiled RGBA buffer.
Encodes depth as grayscale: inverts values (closer = brighter) and normalizes to the ``[50, 255]``
range. Background pixels (no hit) remain black.
.. deprecated::
Use ``SensorTiledCamera.utils.flatten_depth_image_to_rgba`` instead.
Args:
image: Depth output from :meth:`update`, shape ``(world_count, camera_count, height, width)``.
out_buffer: Pre-allocated RGBA buffer. If None, allocates a new one.
worlds_per_row: Tiles per row in the grid. If None, picks a square-ish layout.
depth_range: Depth range to normalize to, shape ``(2,)`` ``[near, far]``. If None, computes from *image*.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.flatten_depth_image_to_rgba is deprecated, use SensorTiledCamera.utils.flatten_depth_image_to_rgba instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.utils.flatten_depth_image_to_rgba(image, out_buffer, worlds_per_row, depth_range)
def assign_random_colors_per_world(self, seed: int = 100):
"""Assign each world a random color, applied to all its shapes.
.. deprecated::
Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``).
Args:
seed: Random seed.
"""
warnings.warn(
"``SensorTiledCamera.assign_random_colors_per_world`` is deprecated. Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``).",
category=DeprecationWarning,
stacklevel=2,
)
self.utils.assign_random_colors_per_world(seed)
def assign_random_colors_per_shape(self, seed: int = 100):
"""Assign a random color to each shape.
.. deprecated::
Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``).
Args:
seed: Random seed.
"""
warnings.warn(
"``SensorTiledCamera.assign_random_colors_per_shape`` is deprecated. Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``).",
category=DeprecationWarning,
stacklevel=2,
)
self.utils.assign_random_colors_per_shape(seed)
def create_default_light(self, enable_shadows: bool = True):
"""Create a default directional light oriented at ``(-1, 1, -1)``.
.. deprecated::
Use ``SensorTiledCamera.utils.create_default_light`` instead.
Args:
enable_shadows: Enable shadow casting for this light.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.create_default_light is deprecated, use SensorTiledCamera.utils.create_default_light instead.",
category=DeprecationWarning,
stacklevel=2,
)
self.utils.create_default_light(enable_shadows)
def assign_checkerboard_material_to_all_shapes(self, resolution: int = 64, checker_size: int = 32):
"""Assign a gray checkerboard texture material to all shapes.
Creates a gray checkerboard pattern texture and applies it to all shapes
in the scene.
.. deprecated::
Use ``SensorTiledCamera.utils.assign_checkerboard_material_to_all_shapes`` instead.
Args:
resolution: Texture resolution in pixels (square texture).
checker_size: Size of each checkerboard square in pixels.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.assign_checkerboard_material_to_all_shapes is deprecated, use SensorTiledCamera.utils.assign_checkerboard_material_to_all_shapes instead.",
category=DeprecationWarning,
stacklevel=2,
)
self.utils.assign_checkerboard_material_to_all_shapes(resolution, checker_size)
def create_color_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create a color output array for :meth:`update`.
.. deprecated::
Use ``SensorTiledCamera.utils.create_color_image_output`` instead.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``uint32``.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.create_color_image_output is deprecated, use SensorTiledCamera.utils.create_color_image_output instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.utils.create_color_image_output(width, height, camera_count)
def create_depth_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.float32]:
"""Create a depth output array for :meth:`update`.
.. deprecated::
Use ``SensorTiledCamera.utils.create_depth_image_output`` instead.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``float32``.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.create_depth_image_output is deprecated, use SensorTiledCamera.utils.create_depth_image_output instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.utils.create_depth_image_output(width, height, camera_count)
def create_shape_index_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create a shape-index output array for :meth:`update`.
.. deprecated::
Use ``SensorTiledCamera.utils.create_shape_index_image_output`` instead.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``uint32``.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.create_shape_index_image_output is deprecated, use SensorTiledCamera.utils.create_shape_index_image_output instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.utils.create_shape_index_image_output(width, height, camera_count)
def create_normal_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.vec3f]:
"""Create a normal output array for :meth:`update`.
.. deprecated::
Use ``SensorTiledCamera.utils.create_normal_image_output`` instead.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``vec3f``.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.create_normal_image_output is deprecated, use SensorTiledCamera.utils.create_normal_image_output instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.utils.create_normal_image_output(width, height, camera_count)
def create_albedo_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create an albedo output array for :meth:`update`.
.. deprecated::
Use ``SensorTiledCamera.utils.create_albedo_image_output`` instead.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``uint32``.
"""
warnings.warn(
"Deprecated: SensorTiledCamera.create_albedo_image_output is deprecated, use SensorTiledCamera.utils.create_albedo_image_output instead.",
category=DeprecationWarning,
stacklevel=2,
)
return self.utils.create_albedo_image_output(width, height, camera_count)
@property
def render_context(self) -> RenderContext:
"""Internal Warp raytracing context used by :meth:`update` and buffer helpers.
.. deprecated::
Direct access is deprecated and will be removed. Prefer this
class's public methods, or :attr:`render_config` for
:class:`RenderConfig` access.
Returns:
The shared :class:`RenderContext` instance.
"""
warnings.warn(
"Direct access to SensorTiledCamera.render_context is deprecated and will be removed in a future release.",
category=DeprecationWarning,
stacklevel=2,
)
return self.__render_context
@property
def render_config(self) -> RenderConfig:
"""Low-level raytrace settings on the internal :class:`RenderContext`.
Populated at construction from :class:`Config` and from fixed defaults
(for example global world and shadow flags on the context). Attributes may
be modified to change behavior for subsequent :meth:`update` calls.
Returns:
The live :class:`RenderConfig` instance (same object as
``render_context.config`` without triggering deprecation warnings).
"""
return self.__render_context.config
@property
def utils(self) -> Utils:
"""Utility helpers for creating output buffers, computing rays, and assigning materials/lights."""
return self.__render_context.utils
================================================
FILE: newton/_src/sensors/warp_raytrace/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .render_context import RenderContext
from .types import ClearData, GaussianRenderMode, MeshData, RenderConfig, RenderLightType, RenderOrder, TextureData
from .utils import Utils
__all__ = [
"ClearData",
"GaussianRenderMode",
"MeshData",
"RenderConfig",
"RenderContext",
"RenderLightType",
"RenderOrder",
"TextureData",
"Utils",
]
================================================
FILE: newton/_src/sensors/warp_raytrace/bvh.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...core import MAXVAL
from ...geometry import GeoType
@wp.func
def compute_shape_bounds(
transform: wp.transformf, scale: wp.vec3f, shape_min_bounds: wp.vec3f, shape_max_bounds: wp.vec3f
) -> tuple[wp.vec3f, wp.vec3f]:
shape_min_bounds = wp.cw_mul(shape_min_bounds, scale)
shape_max_bounds = wp.cw_mul(shape_max_bounds, scale)
min_bound = wp.vec3f(MAXVAL)
max_bound = wp.vec3f(-MAXVAL)
corner_1 = wp.transform_point(transform, wp.vec3f(shape_min_bounds[0], shape_min_bounds[1], shape_min_bounds[2]))
min_bound = wp.min(min_bound, corner_1)
max_bound = wp.max(max_bound, corner_1)
corner_2 = wp.transform_point(transform, wp.vec3f(shape_max_bounds[0], shape_min_bounds[1], shape_min_bounds[2]))
min_bound = wp.min(min_bound, corner_2)
max_bound = wp.max(max_bound, corner_2)
corner_3 = wp.transform_point(transform, wp.vec3f(shape_max_bounds[0], shape_max_bounds[1], shape_min_bounds[2]))
min_bound = wp.min(min_bound, corner_3)
max_bound = wp.max(max_bound, corner_3)
corner_4 = wp.transform_point(transform, wp.vec3f(shape_min_bounds[0], shape_max_bounds[1], shape_min_bounds[2]))
min_bound = wp.min(min_bound, corner_4)
max_bound = wp.max(max_bound, corner_4)
corner_5 = wp.transform_point(transform, wp.vec3f(shape_min_bounds[0], shape_min_bounds[1], shape_max_bounds[2]))
min_bound = wp.min(min_bound, corner_5)
max_bound = wp.max(max_bound, corner_5)
corner_6 = wp.transform_point(transform, wp.vec3f(shape_max_bounds[0], shape_min_bounds[1], shape_max_bounds[2]))
min_bound = wp.min(min_bound, corner_6)
max_bound = wp.max(max_bound, corner_6)
corner_7 = wp.transform_point(transform, wp.vec3f(shape_min_bounds[0], shape_max_bounds[1], shape_max_bounds[2]))
min_bound = wp.min(min_bound, corner_7)
max_bound = wp.max(max_bound, corner_7)
corner_8 = wp.transform_point(transform, wp.vec3f(shape_max_bounds[0], shape_max_bounds[1], shape_max_bounds[2]))
min_bound = wp.min(min_bound, corner_8)
max_bound = wp.max(max_bound, corner_8)
return min_bound, max_bound
@wp.func
def compute_box_bounds(transform: wp.transformf, size: wp.vec3f) -> tuple[wp.vec3f, wp.vec3f]:
min_bound = wp.vec3f(MAXVAL)
max_bound = wp.vec3f(-MAXVAL)
for x in range(2):
for y in range(2):
for z in range(2):
local_corner = wp.vec3f(
size[0] * (2.0 * wp.float32(x) - 1.0),
size[1] * (2.0 * wp.float32(y) - 1.0),
size[2] * (2.0 * wp.float32(z) - 1.0),
)
world_corner = wp.transform_point(transform, local_corner)
min_bound = wp.min(min_bound, world_corner)
max_bound = wp.max(max_bound, world_corner)
return min_bound, max_bound
@wp.func
def compute_sphere_bounds(pos: wp.vec3f, radius: wp.float32) -> tuple[wp.vec3f, wp.vec3f]:
return pos - wp.vec3f(radius), pos + wp.vec3f(radius)
@wp.func
def compute_capsule_bounds(transform: wp.transformf, size: wp.vec3f) -> tuple[wp.vec3f, wp.vec3f]:
radius = size[0]
half_length = size[1]
extent = wp.vec3f(radius, radius, half_length + radius)
return compute_box_bounds(transform, extent)
@wp.func
def compute_cylinder_bounds(transform: wp.transformf, size: wp.vec3f) -> tuple[wp.vec3f, wp.vec3f]:
radius = size[0]
half_length = size[1]
extent = wp.vec3f(radius, radius, half_length)
return compute_box_bounds(transform, extent)
@wp.func
def compute_cone_bounds(transform: wp.transformf, size: wp.vec3f) -> tuple[wp.vec3f, wp.vec3f]:
extent = wp.vec3f(size[0], size[0], size[1])
return compute_box_bounds(transform, extent)
@wp.func
def compute_plane_bounds(transform: wp.transformf, size: wp.vec3f) -> tuple[wp.vec3f, wp.vec3f]:
# If plane size is non-positive, treat as infinite plane and use a large default extent
size_scale = wp.max(size[0], size[1]) * 2.0
if size[0] <= 0.0 or size[1] <= 0.0:
size_scale = 1000.0
min_bound = wp.vec3f(MAXVAL)
max_bound = wp.vec3f(-MAXVAL)
for x in range(2):
for y in range(2):
local_corner = wp.vec3f(
size_scale * (2.0 * wp.float32(x) - 1.0),
size_scale * (2.0 * wp.float32(y) - 1.0),
0.0,
)
world_corner = wp.transform_point(transform, local_corner)
min_bound = wp.min(min_bound, world_corner)
max_bound = wp.max(max_bound, world_corner)
extent = wp.vec3f(0.1)
return min_bound - extent, max_bound + extent
@wp.func
def compute_ellipsoid_bounds(transform: wp.transformf, size: wp.vec3f) -> tuple[wp.vec3f, wp.vec3f]:
extent = wp.vec3f(wp.abs(size[0]), wp.abs(size[1]), wp.abs(size[2]))
return compute_box_bounds(transform, extent)
@wp.kernel(enable_backward=False)
def compute_shape_bvh_bounds(
shape_count_enabled: wp.int32,
world_count: wp.int32,
shape_world_index: wp.array[wp.int32],
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_bounds: wp.array2d[wp.vec3f],
out_bvh_lowers: wp.array[wp.vec3f],
out_bvh_uppers: wp.array[wp.vec3f],
out_bvh_groups: wp.array[wp.int32],
):
tid = wp.tid()
bvh_index_local = tid % shape_count_enabled
if bvh_index_local >= shape_count_enabled:
return
shape_index = shape_enabled[bvh_index_local]
world_index = shape_world_index[shape_index]
if world_index < 0:
world_index = world_count + world_index
if world_index >= world_count:
return
transform = shape_transforms[shape_index]
size = shape_sizes[shape_index]
geom_type = shape_types[shape_index]
lower = wp.vec3f()
upper = wp.vec3f()
if geom_type == GeoType.SPHERE:
lower, upper = compute_sphere_bounds(wp.transform_get_translation(transform), size[0])
elif geom_type == GeoType.CAPSULE:
lower, upper = compute_capsule_bounds(transform, size)
elif geom_type == GeoType.CYLINDER:
lower, upper = compute_cylinder_bounds(transform, size)
elif geom_type == GeoType.CONE:
lower, upper = compute_cone_bounds(transform, size)
elif geom_type == GeoType.PLANE:
lower, upper = compute_plane_bounds(transform, size)
elif geom_type == GeoType.ELLIPSOID:
lower, upper = compute_ellipsoid_bounds(transform, size)
elif geom_type == GeoType.BOX:
lower, upper = compute_box_bounds(transform, size)
elif geom_type == GeoType.MESH or geom_type == GeoType.GAUSSIAN:
min_bounds = shape_bounds[shape_index, 0]
max_bounds = shape_bounds[shape_index, 1]
lower, upper = compute_shape_bounds(transform, size, min_bounds, max_bounds)
out_bvh_lowers[bvh_index_local] = lower
out_bvh_uppers[bvh_index_local] = upper
out_bvh_groups[bvh_index_local] = world_index
@wp.kernel(enable_backward=False)
def compute_particle_bvh_bounds(
num_particles: wp.int32,
world_count: wp.int32,
particle_world_index: wp.array[wp.int32],
particle_position: wp.array[wp.vec3f],
particle_radius: wp.array[wp.float32],
out_bvh_lowers: wp.array[wp.vec3f],
out_bvh_uppers: wp.array[wp.vec3f],
out_bvh_groups: wp.array[wp.int32],
):
tid = wp.tid()
bvh_index_local = tid % num_particles
if bvh_index_local >= num_particles:
return
particle_index = bvh_index_local
world_index = particle_world_index[particle_index]
if world_index < 0:
world_index = world_count + world_index
if world_index >= world_count:
return
lower, upper = compute_sphere_bounds(particle_position[particle_index], particle_radius[particle_index])
out_bvh_lowers[bvh_index_local] = lower
out_bvh_uppers[bvh_index_local] = upper
out_bvh_groups[bvh_index_local] = world_index
@wp.kernel(enable_backward=False)
def compute_bvh_group_roots(bvh_id: wp.uint64, out_bvh_group_roots: wp.array[wp.int32]):
tid = wp.tid()
out_bvh_group_roots[tid] = wp.bvh_get_group_root(bvh_id, tid)
================================================
FILE: newton/_src/sensors/warp_raytrace/gaussians.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from typing import TYPE_CHECKING
import warp as wp
from ...geometry import Gaussian
from ...math import safe_div
from . import bvh
from .ray_intersect import GeomHit, map_ray_to_local_scaled
from .types import GaussianRenderMode
if TYPE_CHECKING:
from .render_context import RenderContext
SH_C0 = wp.float32(0.28209479177387814)
@wp.func
def compute_gaussian_bounds(gaussians_data: Gaussian.Data, tid: wp.int32) -> tuple[wp.vec3f, wp.vec3f]:
transform = gaussians_data.transforms[tid]
scale = gaussians_data.scales[tid]
mod = gaussians_data.min_response / wp.max(gaussians_data.opacities[tid], wp.float32(1e-6))
min_response = wp.clamp(mod, wp.float32(1e-6), wp.float32(0.97))
ks = wp.sqrt(wp.log(min_response) / wp.float32(-0.5))
scale = wp.vec3f(scale[0] * ks, scale[1] * ks, scale[2] * ks)
return bvh.compute_ellipsoid_bounds(transform, scale)
@wp.kernel
def compute_gaussian_bvh_bounds(
gaussians_data: Gaussian.Data,
lowers: wp.array[wp.vec3f],
uppers: wp.array[wp.vec3f],
):
tid = wp.tid()
lower, upper = compute_gaussian_bounds(gaussians_data, tid)
lowers[tid] = lower
uppers[tid] = upper
@wp.func
def canonical_ray_hit_distance(ray_origin: wp.vec3f, ray_direction: wp.vec3f) -> wp.float32:
numerator = -wp.dot(ray_origin, ray_direction)
return safe_div(numerator, wp.dot(ray_direction, ray_direction))
@wp.func
def canonical_ray_min_squared_distance(ray_origin: wp.vec3f, ray_direction: wp.vec3f) -> wp.float32:
gcrod = wp.cross(ray_direction, ray_origin)
return wp.dot(gcrod, gcrod)
@wp.func
def canonical_ray_max_kernel_response(ray_origin: wp.vec3f, ray_direction: wp.vec3f) -> wp.float32:
return wp.exp(-0.5 * canonical_ray_min_squared_distance(ray_origin, ray_direction))
@wp.func
def ray_gsplat_hit_response(
transform: wp.transformf,
scale: wp.vec3f,
opacity: wp.float32,
min_response: wp.float32,
ray_origin_world: wp.vec3f,
ray_direction_world: wp.vec3f,
max_distance: wp.float32,
) -> tuple[wp.float32, wp.float32]:
ray_origin_local, ray_direction_local = map_ray_to_local_scaled(
transform, scale, ray_origin_world, ray_direction_world
)
hit_distance = canonical_ray_hit_distance(ray_origin_local, ray_direction_local)
if hit_distance > 0.0 and hit_distance < max_distance:
max_response = canonical_ray_max_kernel_response(ray_origin_local, wp.normalize(ray_direction_local))
alpha = wp.min(wp.float32(1.0), max_response * opacity)
if max_response > min_response and alpha > wp.static(1.0 / 255.0):
return alpha, hit_distance
return 0.0, -1.0
def create_shade_function(config: RenderContext.Config, state: RenderContext.State) -> wp.Function:
@wp.func
def shade(
transform: wp.transformf,
scale: wp.vec3f,
ray_origin: wp.vec3f,
ray_direction: wp.vec3f,
gaussian_data: Gaussian.Data,
max_distance: wp.float32,
) -> tuple[GeomHit, wp.vec3f]:
result_hit = GeomHit()
result_hit.hit = False
result_hit.normal = wp.vec3f(0.0)
result_hit.distance = max_distance
result_color = wp.vec3f(0.0)
ray_origin_local, ray_direction_local = map_ray_to_local_scaled(transform, scale, ray_origin, ray_direction)
hit_index = wp.int32(0)
min_distance = wp.float32(0.0)
ray_transmittance = wp.float32(1.0)
hit_distances = wp.vector(max_distance, length=wp.static(config.gaussians_max_num_hits), dtype=wp.float32)
hit_indices = wp.vector(-1, length=wp.static(config.gaussians_max_num_hits), dtype=wp.int32)
hit_alphas = wp.vector(0.0, length=wp.static(config.gaussians_max_num_hits), dtype=wp.float32)
while ray_transmittance > 0.003:
num_hits = wp.int32(0)
for i in range(wp.static(config.gaussians_max_num_hits)):
hit_distances[i] = max_distance - min_distance
query = wp.bvh_query_ray(
gaussian_data.bvh_id, ray_origin_local + ray_direction_local * min_distance, ray_direction_local
)
while wp.bvh_query_next(query, hit_index, hit_distances[-1]):
hit_alpha, hit_distance = ray_gsplat_hit_response(
gaussian_data.transforms[hit_index],
gaussian_data.scales[hit_index],
gaussian_data.opacities[hit_index],
gaussian_data.min_response,
ray_origin_local,
ray_direction_local,
hit_distances[-1],
)
if hit_distance > -1:
if num_hits < wp.static(config.gaussians_max_num_hits):
num_hits += 1
for h in range(num_hits):
if hit_distance < hit_distances[h]:
for hh in range(num_hits - 1, h, -1):
hit_distances[hh] = hit_distances[hh - 1]
hit_indices[hh] = hit_indices[hh - 1]
hit_alphas[hh] = hit_alphas[hh - 1]
hit_distances[h] = hit_distance
hit_indices[h] = hit_index
hit_alphas[h] = hit_alpha
break
if num_hits == 0:
break
for hit in range(num_hits):
hit_index = hit_indices[hit]
color = SH_C0 * wp.vec3f(
gaussian_data.sh_coeffs[hit_index][0],
gaussian_data.sh_coeffs[hit_index][1],
gaussian_data.sh_coeffs[hit_index][2],
) + wp.vec3f(0.5)
opacity = hit_alphas[hit]
result_color += color * opacity * ray_transmittance
ray_transmittance *= 1.0 - opacity
if ray_transmittance < wp.static(config.gaussians_min_transmittance):
result_hit.distance = wp.min(hit_distances[hit], result_hit.distance)
min_distance = hit_distances[-1] + wp.float32(1e-06)
if wp.static(config.gaussians_mode) == GaussianRenderMode.FAST:
break
if ray_transmittance < wp.static(config.gaussians_min_transmittance):
result_hit.hit = True
result_color /= 1.0 - ray_transmittance
return result_hit, result_color
return shade
================================================
FILE: newton/_src/sensors/warp_raytrace/lighting.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from typing import TYPE_CHECKING
import warp as wp
from ...core import MAXVAL
from . import raytrace
if TYPE_CHECKING:
from .render_context import RenderContext
def create_compute_lighting_function(config: RenderContext.Config, state: RenderContext.State) -> wp.Function:
raytrace_first_hit = raytrace.create_first_hit_function(config, state)
@wp.func
def compute_lighting(
world_index: wp.int32,
bvh_shapes_size: wp.int32,
bvh_shapes_id: wp.uint64,
bvh_shapes_group_roots: wp.array[wp.int32],
bvh_particles_size: wp.int32,
bvh_particles_id: wp.uint64,
bvh_particles_group_roots: wp.array[wp.int32],
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_source_ptr: wp.array[wp.uint64],
light_active: wp.bool,
light_type: wp.int32,
light_cast_shadow: wp.bool,
light_position: wp.vec3f,
light_orientation: wp.vec3f,
particles_position: wp.array[wp.vec3f],
particles_radius: wp.array[wp.float32],
triangle_mesh_id: wp.uint64,
normal: wp.vec3f,
hit_point: wp.vec3f,
) -> wp.float32:
light_contribution = wp.float32(0.0)
if not light_active:
return light_contribution
L = wp.vec3f(0.0, 0.0, 0.0)
dist_to_light = wp.float32(MAXVAL)
attenuation = wp.float32(1.0)
if light_type == 1: # directional light
L = wp.normalize(-light_orientation)
else:
to_light = light_position - hit_point
dist_to_light = wp.length(to_light)
L = wp.normalize(to_light)
attenuation = 1.0 / (1.0 + 0.02 * dist_to_light * dist_to_light)
if light_type == 0: # spot light
spot_dir = wp.normalize(light_orientation)
cos_theta = wp.dot(-L, spot_dir)
inner = 0.95
outer = 0.85
spot_factor = wp.min(1.0, wp.max(0.0, (cos_theta - outer) / (inner - outer)))
attenuation = attenuation * spot_factor
ndotl = wp.max(0.0, wp.dot(normal, L))
if ndotl == 0.0:
return light_contribution
visible = wp.float32(1.0)
shadow_min_visibility = wp.float32(0.3) # reduce shadow darkness (0: full black, 1: no shadow)
if wp.static(config.enable_shadows) and light_cast_shadow:
# Nudge the origin slightly along the surface normal to avoid
# self-intersection when casting shadow rays
eps = 1.0e-4
shadow_origin = hit_point + normal * eps
# Distance-limited shadows: cap by dist_to_light (for non-directional)
max_t = wp.max(wp.float32(1.0e-4), wp.float32(dist_to_light - 1.0e-3))
if light_type == 1: # directional light
max_t = wp.float32(1.0e8)
shadow_hit = raytrace_first_hit(
bvh_shapes_size,
bvh_shapes_id,
bvh_shapes_group_roots,
bvh_particles_size,
bvh_particles_id,
bvh_particles_group_roots,
world_index,
shape_enabled,
shape_types,
shape_sizes,
shape_transforms,
shape_source_ptr,
particles_position,
particles_radius,
triangle_mesh_id,
shadow_origin,
L,
max_t,
)
if shadow_hit:
visible = shadow_min_visibility
return ndotl * attenuation * visible
return compute_lighting
================================================
FILE: newton/_src/sensors/warp_raytrace/ray_intersect.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...geometry import raycast
from .types import MeshData
EPSILON = 1e-6
@wp.struct
class GeomHit:
hit: wp.bool
distance: wp.float32
normal: wp.vec3f
@wp.func
def safe_div_vec3(x: wp.vec3f, y: wp.vec3f) -> wp.vec3f:
return wp.vec3f(
x[0] / wp.where(y[0] != 0.0, y[0], EPSILON),
x[1] / wp.where(y[1] != 0.0, y[1], EPSILON),
x[2] / wp.where(y[2] != 0.0, y[2], EPSILON),
)
@wp.func
def map_ray_to_local(
transform: wp.transformf, ray_origin: wp.vec3f, ray_direction: wp.vec3f
) -> tuple[wp.vec3f, wp.vec3f]:
"""Maps ray to local shape frame coordinates.
Args:
transform: transform of shape frame
ray_origin: starting point of ray in world coordinates
ray_direction: direction of ray in world coordinates
Returns:
3D point and 3D direction in local shape frame
"""
inv_transform = wp.transform_inverse(transform)
ray_origin_local = wp.transform_point(inv_transform, ray_origin)
ray_direction_local = wp.transform_vector(inv_transform, ray_direction)
return ray_origin_local, ray_direction_local
@wp.func
def map_ray_to_local_scaled(
transform: wp.transformf, scale: wp.vec3f, ray_origin: wp.vec3f, ray_direction: wp.vec3f
) -> tuple[wp.vec3f, wp.vec3f]:
ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin, ray_direction)
inv_size = safe_div_vec3(wp.vec3f(1.0), scale)
return wp.cw_mul(ray_origin_local, inv_size), wp.cw_mul(ray_direction_local, inv_size)
@wp.func
def ray_intersect_plane(
transform: wp.transformf,
size: wp.vec3f,
enable_backface_culling: wp.bool,
ray_origin: wp.vec3f,
ray_direction: wp.vec3f,
) -> wp.float32:
"""Returns the distance at which a ray intersects with a plane."""
# map to local frame
ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin, ray_direction)
# ray parallel to plane: no intersection
if wp.abs(ray_direction_local[2]) < EPSILON:
return -1.0
# z-vec not pointing towards front face: reject
if enable_backface_culling and ray_direction_local[2] > -EPSILON:
return -1.0
# intersection with plane
t_hit = -ray_origin_local[2] / ray_direction_local[2]
if t_hit < 0.0:
return -1.0
p = wp.vec2f(
ray_origin_local[0] + t_hit * ray_direction_local[0], ray_origin_local[1] + t_hit * ray_direction_local[1]
)
# accept only within rendered rectangle
if (size[0] <= 0.0 or wp.abs(p[0]) <= size[0]) and (size[1] <= 0.0 or wp.abs(p[1]) <= size[1]):
return t_hit
return -1.0
@wp.func
def ray_intersect_plane_with_normal(
transform: wp.transformf,
size: wp.vec3f,
enable_backface_culling: wp.bool,
ray_origin: wp.vec3f,
ray_direction: wp.vec3f,
) -> GeomHit:
"""Returns distance and normal at which a ray intersects with a plane."""
geom_hit = GeomHit()
geom_hit.distance = ray_intersect_plane(transform, size, enable_backface_culling, ray_origin, ray_direction)
geom_hit.hit = geom_hit.distance > -1
if geom_hit.hit:
geom_hit.normal = wp.transform_vector(transform, wp.vec3f(0.0, 0.0, 1.0))
geom_hit.normal = wp.normalize(geom_hit.normal)
return geom_hit
@wp.func
def ray_intersect_sphere_with_normal(
transform: wp.transformf, size: wp.vec3f, ray_origin: wp.vec3f, ray_direction: wp.vec3f
) -> GeomHit:
"""Returns distance and normal at which a ray intersects with a sphere."""
geom_hit = GeomHit()
geom_hit.distance = raycast.ray_intersect_sphere(transform, ray_origin, ray_direction, size[0])
geom_hit.hit = geom_hit.distance > -1
if geom_hit.hit:
geom_hit.normal = wp.normalize(
ray_origin + geom_hit.distance * ray_direction - wp.transform_get_translation(transform)
)
return geom_hit
@wp.func
def ray_intersect_particle_sphere_with_normal(
ray_origin: wp.vec3f, ray_direction: wp.vec3f, center: wp.vec3f, radius: wp.float32
) -> GeomHit:
"""Returns distance and normal at which a ray intersects with a particle sphere."""
geom_hit = GeomHit()
geom_hit.distance = raycast.ray_intersect_particle_sphere(ray_origin, ray_direction, center, radius)
geom_hit.hit = geom_hit.distance > -1
if geom_hit.hit:
geom_hit.normal = wp.normalize(ray_origin + geom_hit.distance * ray_direction - center)
return geom_hit
@wp.func
def ray_intersect_capsule_with_normal(
transform: wp.transformf, size: wp.vec3f, ray_origin: wp.vec3f, ray_direction: wp.vec3f
) -> GeomHit:
"""Returns distance and normal at which a ray intersects with a capsule."""
geom_hit = GeomHit()
geom_hit.distance = raycast.ray_intersect_capsule(transform, ray_origin, ray_direction, size[0], size[1])
geom_hit.hit = geom_hit.distance > -1
if geom_hit.hit:
ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin, ray_direction)
hit_local = ray_origin_local + geom_hit.distance * ray_direction_local
z_clamped = wp.min(size[1], wp.max(-size[1], hit_local[2]))
axis_point = wp.vec3f(0.0, 0.0, z_clamped)
normal_local = wp.normalize(hit_local - axis_point)
geom_hit.normal = wp.transform_vector(transform, normal_local)
geom_hit.normal = wp.normalize(geom_hit.normal)
return geom_hit
@wp.func
def ray_intersect_ellipsoid_with_normal(
transform: wp.transformf, size: wp.vec3f, ray_origin: wp.vec3f, ray_direction: wp.vec3f
) -> GeomHit:
"""Returns the distance and normal at which a ray intersects with an ellipsoid."""
geom_hit = GeomHit()
geom_hit.distance = raycast.ray_intersect_ellipsoid(transform, ray_origin, ray_direction, size)
geom_hit.hit = geom_hit.distance > -1
if geom_hit.hit:
ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin, ray_direction)
hit_local = ray_origin_local + geom_hit.distance * ray_direction_local
inv_size = safe_div_vec3(wp.vec3f(1.0), size)
inv_size_sq = wp.cw_mul(inv_size, inv_size)
geom_hit.normal = wp.cw_mul(hit_local, inv_size_sq)
geom_hit.normal = wp.transform_vector(transform, geom_hit.normal)
geom_hit.normal = wp.normalize(geom_hit.normal)
return geom_hit
@wp.func
def ray_intersect_cylinder_with_normal(
transform: wp.transformf, size: wp.vec3f, ray_origin: wp.vec3f, ray_direction: wp.vec3f
) -> GeomHit:
"""Returns distance and normal at which a ray intersects with a cylinder."""
geom_hit = GeomHit()
geom_hit.distance = raycast.ray_intersect_cylinder(transform, ray_origin, ray_direction, size[0], size[1])
geom_hit.hit = geom_hit.distance > -1
if geom_hit.hit:
ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin, ray_direction)
hit_local = ray_origin_local + geom_hit.distance * ray_direction_local
z_clamped = wp.min(size[1], wp.max(-size[1], hit_local[2]))
if z_clamped >= (size[1] - EPSILON) or z_clamped <= (-size[1] + EPSILON):
geom_hit.normal = wp.vec3f(0.0, 0.0, z_clamped)
else:
geom_hit.normal = wp.normalize(hit_local - wp.vec3f(0.0, 0.0, z_clamped))
geom_hit.normal = wp.transform_vector(transform, geom_hit.normal)
geom_hit.normal = wp.normalize(geom_hit.normal)
return geom_hit
@wp.func
def ray_intersect_cone_with_normal(
transform: wp.transformf, size: wp.vec3f, ray_origin: wp.vec3f, ray_direction: wp.vec3f
) -> GeomHit:
"""Returns distance and normal at which a ray intersects with a cone."""
geom_hit = GeomHit()
geom_hit.distance = raycast.ray_intersect_cone(transform, ray_origin, ray_direction, size[0], size[1])
geom_hit.hit = geom_hit.distance > -1
if geom_hit.hit:
ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin, ray_direction)
hit_local = ray_origin_local + geom_hit.distance * ray_direction_local
half_height = size[1]
radius = size[0]
if wp.abs(hit_local[2] - half_height) <= EPSILON:
normal_local = wp.vec3f(0.0, 0.0, 1.0)
elif wp.abs(hit_local[2] + half_height) <= EPSILON:
normal_local = wp.vec3f(0.0, 0.0, -1.0)
else:
radial_sq = hit_local[0] * hit_local[0] + hit_local[1] * hit_local[1]
radial = wp.sqrt(radial_sq)
if radial <= EPSILON:
normal_local = wp.vec3f(0.0, 0.0, 1.0)
else:
denom = wp.max(2.0 * wp.abs(half_height), EPSILON)
slope = radius / denom
normal_local = wp.vec3f(hit_local[0], hit_local[1], slope * radial)
normal_local = wp.normalize(normal_local)
geom_hit.normal = wp.transform_vector(transform, normal_local)
geom_hit.normal = wp.normalize(geom_hit.normal)
return geom_hit
_IFACE = wp.types.matrix((3, 2), dtype=wp.int32)(1, 2, 0, 2, 0, 1)
@wp.func
def ray_intersect_box_with_normal(
transform: wp.transformf, size: wp.vec3f, ray_origin: wp.vec3f, ray_direction: wp.vec3f
) -> GeomHit:
"""Returns distance and normal at which a ray intersects with a box."""
geom_hit = GeomHit()
geom_hit.distance = raycast.ray_intersect_box(transform, ray_origin, ray_direction, size)
geom_hit.hit = geom_hit.distance > -1
if geom_hit.hit:
geom_hit.normal = wp.vec3f(0.0)
ray_origin_local, ray_direction_local = map_ray_to_local(transform, ray_origin, ray_direction)
for i in range(3):
if wp.abs(ray_direction_local[i]) > EPSILON:
for side in range(-1, 2, 2):
sol = (wp.float32(side) * size[i] - ray_origin_local[i]) / ray_direction_local[i]
if sol >= 0.0:
id0 = _IFACE[i][0]
id1 = _IFACE[i][1]
p0 = ray_origin_local[id0] + sol * ray_direction_local[id0]
p1 = ray_origin_local[id1] + sol * ray_direction_local[id1]
if (wp.abs(p0) <= size[id0]) and (wp.abs(p1) <= size[id1]):
if sol >= 0.0 and wp.abs(sol - geom_hit.distance) < EPSILON:
geom_hit.normal[i] = -1.0 if side < 0 else 1.0
geom_hit.normal = wp.transform_vector(transform, geom_hit.normal)
geom_hit.normal = wp.normalize(geom_hit.normal)
return geom_hit
return geom_hit
@wp.func
def ray_intersect_mesh_no_transform(
mesh_id: wp.uint64,
ray_origin: wp.vec3f,
ray_direction: wp.vec3f,
enable_backface_culling: wp.bool,
max_t: wp.float32,
) -> tuple[GeomHit, wp.float32, wp.float32, wp.int32]:
"""Returns intersection information at which a ray intersects with a mesh.
Requires wp.Mesh be constructed and their ids to be passed"""
geom_hit = GeomHit()
geom_hit.hit = False
query = wp.mesh_query_ray(mesh_id, ray_origin, ray_direction, max_t)
if query.result:
if not enable_backface_culling or wp.dot(ray_direction, query.normal) < 0.0:
geom_hit.hit = True
geom_hit.distance = query.t
geom_hit.normal = wp.normalize(query.normal)
return geom_hit, query.u, query.v, query.face
return geom_hit, 0.0, 0.0, -1
@wp.func
def ray_intersect_mesh_with_normal(
transform: wp.transformf,
scale: wp.vec3f,
ray_origin: wp.vec3f,
ray_direction: wp.vec3f,
mesh_id: wp.uint64,
shape_mesh_data_id: wp.int32,
mesh_data: wp.array[MeshData],
enable_backface_culling: wp.bool,
max_t: wp.float32,
) -> tuple[GeomHit, wp.float32, wp.float32, wp.int32]:
"""Returns intersection information at which a ray intersects with a mesh."""
geom_hit = GeomHit()
geom_hit.hit = False
ray_origin_local, ray_direction_local = map_ray_to_local_scaled(transform, scale, ray_origin, ray_direction)
query = wp.mesh_query_ray(mesh_id, ray_origin_local, ray_direction_local, max_t)
if query.result:
if not enable_backface_culling or wp.dot(ray_direction_local, query.normal) < 0.0:
geom_hit.hit = True
geom_hit.distance = query.t
geom_hit.normal = query.normal
if shape_mesh_data_id > -1:
normals = mesh_data[shape_mesh_data_id].normals
if normals.shape[0] > 0:
n0 = wp.mesh_get_index(mesh_id, query.face * 3 + 0)
n1 = wp.mesh_get_index(mesh_id, query.face * 3 + 1)
n2 = wp.mesh_get_index(mesh_id, query.face * 3 + 2)
geom_hit.normal = (
normals[n0] * query.u + normals[n1] * query.v + normals[n2] * (1.0 - query.u - query.v)
)
geom_hit.normal = wp.transform_vector(transform, safe_div_vec3(geom_hit.normal, scale))
geom_hit.normal = wp.normalize(geom_hit.normal)
return geom_hit, query.u, query.v, query.face
return geom_hit, 0.0, 0.0, -1
@wp.func
def ray_intersect_mesh(
transform: wp.transformf,
scale: wp.vec3f,
ray_origin: wp.vec3f,
ray_direction: wp.vec3f,
mesh_id: wp.uint64,
enable_backface_culling: wp.bool,
max_t: wp.float32,
) -> wp.float32:
"""Returns intersection distance at which a ray intersects with a mesh."""
ray_origin_local, ray_direction_local = map_ray_to_local_scaled(transform, scale, ray_origin, ray_direction)
query = wp.mesh_query_ray(mesh_id, ray_origin_local, ray_direction_local, max_t)
if query.result:
if not enable_backface_culling or wp.dot(ray_direction_local, query.normal) < 0.0:
return query.t
return -1.0
================================================
FILE: newton/_src/sensors/warp_raytrace/raytrace.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from typing import TYPE_CHECKING
import warp as wp
from ...geometry import Gaussian, GeoType, raycast
from . import gaussians, ray_intersect
from .types import MeshData
if TYPE_CHECKING:
from .render_context import RenderContext
NO_HIT_SHAPE_ID = wp.uint32(0xFFFFFFFF)
MAX_SHAPE_ID = wp.uint32(0xFFFFFFF0)
TRIANGLE_MESH_SHAPE_ID = wp.uint32(0xFFFFFFFD)
PARTICLES_SHAPE_ID = wp.uint32(0xFFFFFFFE)
@wp.struct
class ClosestHit:
distance: wp.float32
normal: wp.vec3f
shape_index: wp.uint32
bary_u: wp.float32
bary_v: wp.float32
face_idx: wp.int32
color: wp.vec3f
@wp.func
def get_group_roots(group_roots: wp.array[wp.int32], world_index: wp.int32, want_global_world: wp.int32) -> wp.int32:
if want_global_world != 0:
return group_roots[group_roots.shape[0] - 1]
return group_roots[world_index]
def create_closest_hit_function(config: RenderContext.Config, state: RenderContext.State) -> wp.Function:
shade_gaussians = gaussians.create_shade_function(config, state)
@wp.func
def closest_hit_shape(
closest_hit: ClosestHit,
bvh_shapes_size: wp.int32,
bvh_shapes_id: wp.uint64,
bvh_shapes_group_roots: wp.array[wp.int32],
world_index: wp.int32,
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_source_ptr: wp.array[wp.uint64],
shape_mesh_data_ids: wp.array[wp.int32],
mesh_data: wp.array[MeshData],
gaussians_data: wp.array[Gaussian.Data],
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
) -> ClosestHit:
if bvh_shapes_size:
for i in range(wp.static(2 if config.enable_global_world else 1)):
group_root = get_group_roots(bvh_shapes_group_roots, world_index, i)
if group_root < 0:
continue
gaussians_hit = wp.vector(length=wp.static(state.num_gaussians), dtype=wp.uint32)
num_gaussians_hit = wp.int32(0)
query = wp.bvh_query_ray(bvh_shapes_id, ray_origin_world, ray_dir_world, group_root)
shape_index = wp.int32(0)
while wp.bvh_query_next(query, shape_index, closest_hit.distance):
si = shape_enabled[shape_index]
geom_hit = ray_intersect.GeomHit()
geom_hit.hit = False
hit_u = wp.float32(0.0)
hit_v = wp.float32(0.0)
hit_face_id = wp.int32(-1)
hit_color = wp.vec3f(0.0)
shape_type = shape_types[si]
if shape_type == GeoType.MESH:
geom_hit, hit_u, hit_v, hit_face_id = ray_intersect.ray_intersect_mesh_with_normal(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
shape_source_ptr[si],
shape_mesh_data_ids[si],
mesh_data,
wp.static(config.enable_backface_culling),
closest_hit.distance,
)
elif shape_type == GeoType.PLANE:
geom_hit = ray_intersect.ray_intersect_plane_with_normal(
shape_transforms[si],
shape_sizes[si],
wp.static(config.enable_backface_culling),
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.SPHERE:
geom_hit = ray_intersect.ray_intersect_sphere_with_normal(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.ELLIPSOID:
geom_hit = ray_intersect.ray_intersect_ellipsoid_with_normal(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.CAPSULE:
geom_hit = ray_intersect.ray_intersect_capsule_with_normal(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.CYLINDER:
geom_hit = ray_intersect.ray_intersect_cylinder_with_normal(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.CONE:
geom_hit = ray_intersect.ray_intersect_cone_with_normal(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.BOX:
geom_hit = ray_intersect.ray_intersect_box_with_normal(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.GAUSSIAN:
if num_gaussians_hit < wp.static(state.num_gaussians):
gaussians_hit[num_gaussians_hit] = si
num_gaussians_hit += 1
# gaussian_id = shape_source_ptr[si]
# geom_hit, hit_color = shade_gaussians(
# shape_transforms[si],
# shape_sizes[si],
# ray_origin_world,
# ray_dir_world,
# gaussians_data[gaussian_id],
# closest_hit.distance
# )
if geom_hit.hit and geom_hit.distance < closest_hit.distance:
closest_hit.distance = geom_hit.distance
closest_hit.normal = geom_hit.normal
closest_hit.shape_index = si
closest_hit.bary_u = hit_u
closest_hit.bary_v = hit_v
closest_hit.face_idx = hit_face_id
closest_hit.color = hit_color
# Temporary workaround. Warp BVH queries share some stack data,
# which breaks nested wp.bvh_query_ray calls.
# Once it is fixed in Warp, remove this code block and put
# the commented out block above back in.
# Although, this workaround may actually be a performance improvement
# since it only renders gaussians if they are not blocked by other
# objects.
if num_gaussians_hit > 0:
for gi in range(num_gaussians_hit):
si = gaussians_hit[gi]
gaussian_id = shape_source_ptr[si]
geom_hit, hit_color = shade_gaussians(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
gaussians_data[gaussian_id],
closest_hit.distance,
)
if geom_hit.hit and geom_hit.distance < closest_hit.distance:
closest_hit.distance = geom_hit.distance
closest_hit.normal = geom_hit.normal
closest_hit.shape_index = si
closest_hit.bary_u = hit_u
closest_hit.bary_v = hit_v
closest_hit.face_idx = hit_face_id
closest_hit.color = hit_color
return closest_hit
@wp.func
def closest_hit_particles(
closest_hit: ClosestHit,
bvh_particles_size: wp.int32,
bvh_particles_id: wp.uint64,
bvh_particles_group_roots: wp.array[wp.int32],
world_index: wp.int32,
particles_position: wp.array[wp.vec3f],
particles_radius: wp.array[wp.float32],
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
) -> ClosestHit:
if bvh_particles_size:
for i in range(wp.static(2 if config.enable_global_world else 1)):
group_root = get_group_roots(bvh_particles_group_roots, world_index, i)
if group_root < 0:
continue
query = wp.bvh_query_ray(bvh_particles_id, ray_origin_world, ray_dir_world, group_root)
si = wp.int32(0)
while wp.bvh_query_next(query, si, closest_hit.distance):
geom_hit = ray_intersect.ray_intersect_particle_sphere_with_normal(
ray_origin_world,
ray_dir_world,
particles_position[si],
particles_radius[si],
)
if geom_hit.hit and geom_hit.distance < closest_hit.distance:
closest_hit.distance = geom_hit.distance
closest_hit.normal = geom_hit.normal
closest_hit.shape_index = PARTICLES_SHAPE_ID
return closest_hit
@wp.func
def closest_hit_triangle_mesh(
closest_hit: ClosestHit,
triangle_mesh_id: wp.uint64,
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
) -> ClosestHit:
if triangle_mesh_id:
geom_hit, bary_u, bary_v, face_idx = ray_intersect.ray_intersect_mesh_no_transform(
triangle_mesh_id,
ray_origin_world,
ray_dir_world,
wp.static(config.enable_backface_culling),
closest_hit.distance,
)
if geom_hit.hit:
closest_hit.distance = geom_hit.distance
closest_hit.normal = geom_hit.normal
closest_hit.shape_index = TRIANGLE_MESH_SHAPE_ID
closest_hit.bary_u = bary_u
closest_hit.bary_v = bary_v
closest_hit.face_idx = face_idx
return closest_hit
@wp.func
def closest_hit(
bvh_shapes_size: wp.int32,
bvh_shapes_id: wp.uint64,
bvh_shapes_group_roots: wp.array[wp.int32],
bvh_particles_size: wp.int32,
bvh_particles_id: wp.uint64,
bvh_particles_group_roots: wp.array[wp.int32],
world_index: wp.int32,
max_distance: wp.float32,
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_source_ptr: wp.array[wp.uint64],
shape_mesh_data_ids: wp.array[wp.int32],
mesh_data: wp.array[MeshData],
particles_position: wp.array[wp.vec3f],
particles_radius: wp.array[wp.float32],
triangle_mesh_id: wp.uint64,
gaussians_data: wp.array[Gaussian.Data],
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
) -> ClosestHit:
closest_hit = ClosestHit()
closest_hit.distance = max_distance
closest_hit.shape_index = NO_HIT_SHAPE_ID
closest_hit.color = wp.vec3f(0.0)
closest_hit = closest_hit_triangle_mesh(closest_hit, triangle_mesh_id, ray_origin_world, ray_dir_world)
closest_hit = closest_hit_shape(
closest_hit,
bvh_shapes_size,
bvh_shapes_id,
bvh_shapes_group_roots,
world_index,
shape_enabled,
shape_types,
shape_sizes,
shape_transforms,
shape_source_ptr,
shape_mesh_data_ids,
mesh_data,
gaussians_data,
ray_origin_world,
ray_dir_world,
)
if wp.static(config.enable_particles):
closest_hit = closest_hit_particles(
closest_hit,
bvh_particles_size,
bvh_particles_id,
bvh_particles_group_roots,
world_index,
particles_position,
particles_radius,
ray_origin_world,
ray_dir_world,
)
return closest_hit
return closest_hit
def create_closest_hit_depth_only_function(config: RenderContext.Config, state: RenderContext.State) -> wp.Function:
shade_gaussians = gaussians.create_shade_function(config, state)
@wp.func
def closest_hit_shape_depth_only(
closest_hit: ClosestHit,
bvh_shapes_size: wp.int32,
bvh_shapes_id: wp.uint64,
bvh_shapes_group_roots: wp.array[wp.int32],
world_index: wp.int32,
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_source_ptr: wp.array[wp.uint64],
shape_mesh_data_ids: wp.array[wp.int32],
mesh_data: wp.array[MeshData],
gaussians_data: wp.array[Gaussian.Data],
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
) -> ClosestHit:
if bvh_shapes_size:
for i in range(wp.static(2 if config.enable_global_world else 1)):
group_root = get_group_roots(bvh_shapes_group_roots, world_index, i)
if group_root < 0:
continue
gaussians_hit = wp.vector(length=wp.static(state.num_gaussians), dtype=wp.uint32)
num_gaussians_hit = wp.int32(0)
query = wp.bvh_query_ray(bvh_shapes_id, ray_origin_world, ray_dir_world, group_root)
shape_index = wp.int32(0)
while wp.bvh_query_next(query, shape_index, closest_hit.distance):
si = shape_enabled[shape_index]
hit_dist = -1.0
shape_type = shape_types[si]
if shape_type == GeoType.MESH:
hit_dist = ray_intersect.ray_intersect_mesh(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
shape_source_ptr[si],
wp.static(config.enable_backface_culling),
closest_hit.distance,
)
elif shape_type == GeoType.PLANE:
hit_dist = ray_intersect.ray_intersect_plane(
shape_transforms[si],
shape_sizes[si],
wp.static(config.enable_backface_culling),
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.SPHERE:
hit_dist = raycast.ray_intersect_sphere(
shape_transforms[si], ray_origin_world, ray_dir_world, shape_sizes[si][0]
)
elif shape_type == GeoType.ELLIPSOID:
hit_dist = raycast.ray_intersect_ellipsoid(
shape_transforms[si], ray_origin_world, ray_dir_world, shape_sizes[si]
)
elif shape_type == GeoType.CAPSULE:
hit_dist = raycast.ray_intersect_capsule(
shape_transforms[si],
ray_origin_world,
ray_dir_world,
shape_sizes[si][0],
shape_sizes[si][1],
)
elif shape_type == GeoType.CYLINDER:
hit_dist = raycast.ray_intersect_cylinder(
shape_transforms[si],
ray_origin_world,
ray_dir_world,
shape_sizes[si][0],
shape_sizes[si][1],
)
elif shape_type == GeoType.CONE:
hit_dist = raycast.ray_intersect_cone(
shape_transforms[si],
ray_origin_world,
ray_dir_world,
shape_sizes[si][0],
shape_sizes[si][1],
)
elif shape_type == GeoType.BOX:
hit_dist = raycast.ray_intersect_box(
shape_transforms[si], ray_origin_world, ray_dir_world, shape_sizes[si]
)
elif shape_type == GeoType.GAUSSIAN:
if num_gaussians_hit < wp.static(state.num_gaussians):
gaussians_hit[num_gaussians_hit] = si
num_gaussians_hit += 1
if hit_dist > -1.0 and hit_dist < closest_hit.distance:
closest_hit.distance = hit_dist
closest_hit.shape_index = si
if num_gaussians_hit > 0:
for gi in range(num_gaussians_hit):
si = gaussians_hit[gi]
gaussian_id = shape_source_ptr[si]
geom_hit, _ = shade_gaussians(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
gaussians_data[gaussian_id],
closest_hit.distance,
)
if geom_hit.hit and geom_hit.distance < closest_hit.distance:
closest_hit.distance = geom_hit.distance
closest_hit.shape_index = si
return closest_hit
@wp.func
def closest_hit_particles_depth_only(
closest_hit: ClosestHit,
bvh_particles_size: wp.int32,
bvh_particles_id: wp.uint64,
bvh_particles_group_roots: wp.array[wp.int32],
world_index: wp.int32,
particles_position: wp.array[wp.vec3f],
particles_radius: wp.array[wp.float32],
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
) -> ClosestHit:
if bvh_particles_size:
for i in range(wp.static(2 if config.enable_global_world else 1)):
group_root = get_group_roots(bvh_particles_group_roots, world_index, i)
if group_root < 0:
continue
query = wp.bvh_query_ray(bvh_particles_id, ray_origin_world, ray_dir_world, group_root)
si = wp.int32(0)
while wp.bvh_query_next(query, si, closest_hit.distance):
hit_dist = raycast.ray_intersect_particle_sphere(
ray_origin_world,
ray_dir_world,
particles_position[si],
particles_radius[si],
)
if hit_dist > -1.0 and hit_dist < closest_hit.distance:
closest_hit.distance = hit_dist
closest_hit.shape_index = PARTICLES_SHAPE_ID
return closest_hit
@wp.func
def closest_hit_triangle_mesh_depth_only(
closest_hit: ClosestHit,
triangle_mesh_id: wp.uint64,
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
) -> ClosestHit:
if triangle_mesh_id:
geom_hit, _bary_u, _bary_v, _face_idx = ray_intersect.ray_intersect_mesh_no_transform(
triangle_mesh_id,
ray_origin_world,
ray_dir_world,
wp.static(config.enable_backface_culling),
closest_hit.distance,
)
if geom_hit.hit:
closest_hit.distance = geom_hit.distance
closest_hit.shape_index = TRIANGLE_MESH_SHAPE_ID
return closest_hit
@wp.func
def closest_hit_depth_only(
bvh_shapes_size: wp.int32,
bvh_shapes_id: wp.uint64,
bvh_shapes_group_roots: wp.array[wp.int32],
bvh_particles_size: wp.int32,
bvh_particles_id: wp.uint64,
bvh_particles_group_roots: wp.array[wp.int32],
world_index: wp.int32,
max_distance: wp.float32,
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_source_ptr: wp.array[wp.uint64],
shape_mesh_data_ids: wp.array[wp.int32],
mesh_data: wp.array[MeshData],
particles_position: wp.array[wp.vec3f],
particles_radius: wp.array[wp.float32],
triangle_mesh_id: wp.uint64,
gaussians_data: wp.array[Gaussian.Data],
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
) -> ClosestHit:
closest_hit = ClosestHit()
closest_hit.distance = max_distance
closest_hit.shape_index = NO_HIT_SHAPE_ID
closest_hit = closest_hit_triangle_mesh_depth_only(
closest_hit, triangle_mesh_id, ray_origin_world, ray_dir_world
)
closest_hit = closest_hit_shape_depth_only(
closest_hit,
bvh_shapes_size,
bvh_shapes_id,
bvh_shapes_group_roots,
world_index,
shape_enabled,
shape_types,
shape_sizes,
shape_transforms,
shape_source_ptr,
shape_mesh_data_ids,
mesh_data,
gaussians_data,
ray_origin_world,
ray_dir_world,
)
if wp.static(config.enable_particles):
closest_hit = closest_hit_particles_depth_only(
closest_hit,
bvh_particles_size,
bvh_particles_id,
bvh_particles_group_roots,
world_index,
particles_position,
particles_radius,
ray_origin_world,
ray_dir_world,
)
return closest_hit
return closest_hit_depth_only
def create_first_hit_function(config: RenderContext.Config, state: RenderContext.State) -> wp.Function:
@wp.func
def first_hit_shape(
bvh_shapes_size: wp.int32,
bvh_shapes_id: wp.uint64,
bvh_shapes_group_roots: wp.array[wp.int32],
world_index: wp.int32,
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_source_ptr: wp.array[wp.uint64],
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
max_dist: wp.float32,
) -> wp.bool:
if bvh_shapes_size:
for i in range(wp.static(2 if config.enable_global_world else 1)):
group_root = get_group_roots(bvh_shapes_group_roots, world_index, i)
if group_root < 0:
continue
query = wp.bvh_query_ray(bvh_shapes_id, ray_origin_world, ray_dir_world, group_root)
shape_index = wp.int32(0)
while wp.bvh_query_next(query, shape_index, max_dist):
si = shape_enabled[shape_index]
hit_dist = wp.float32(-1)
shape_type = shape_types[si]
if shape_type == GeoType.MESH:
hit_dist = ray_intersect.ray_intersect_mesh(
shape_transforms[si],
shape_sizes[si],
ray_origin_world,
ray_dir_world,
shape_source_ptr[si],
False,
max_dist,
)
elif shape_type == GeoType.PLANE:
hit_dist = ray_intersect.ray_intersect_plane(
shape_transforms[si],
shape_sizes[si],
wp.static(config.enable_backface_culling),
ray_origin_world,
ray_dir_world,
)
elif shape_type == GeoType.SPHERE:
hit_dist = raycast.ray_intersect_sphere(
shape_transforms[si], ray_origin_world, ray_dir_world, shape_sizes[si][0]
)
elif shape_type == GeoType.ELLIPSOID:
hit_dist = raycast.ray_intersect_ellipsoid(
shape_transforms[si], ray_origin_world, ray_dir_world, shape_sizes[si]
)
elif shape_type == GeoType.CAPSULE:
hit_dist = raycast.ray_intersect_capsule(
shape_transforms[si],
ray_origin_world,
ray_dir_world,
shape_sizes[si][0],
shape_sizes[si][1],
)
elif shape_type == GeoType.CYLINDER:
hit_dist = raycast.ray_intersect_cylinder(
shape_transforms[si],
ray_origin_world,
ray_dir_world,
shape_sizes[si][0],
shape_sizes[si][1],
)
elif shape_type == GeoType.CONE:
hit_dist = raycast.ray_intersect_cone(
shape_transforms[si],
ray_origin_world,
ray_dir_world,
shape_sizes[si][0],
shape_sizes[si][1],
)
elif shape_type == GeoType.BOX:
hit_dist = raycast.ray_intersect_box(
shape_transforms[si], ray_origin_world, ray_dir_world, shape_sizes[si]
)
if hit_dist > -1 and hit_dist < max_dist:
return True
return False
@wp.func
def first_hit_particles(
bvh_particles_size: wp.int32,
bvh_particles_id: wp.uint64,
bvh_particles_group_roots: wp.array[wp.int32],
world_index: wp.int32,
particles_position: wp.array[wp.vec3f],
particles_radius: wp.array[wp.float32],
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
max_dist: wp.float32,
) -> wp.bool:
if bvh_particles_size:
for i in range(wp.static(2 if config.enable_global_world else 1)):
group_root = get_group_roots(bvh_particles_group_roots, world_index, i)
if group_root < 0:
continue
query = wp.bvh_query_ray(bvh_particles_id, ray_origin_world, ray_dir_world, group_root)
si = wp.int32(0)
while wp.bvh_query_next(query, si, max_dist):
hit_dist = raycast.ray_intersect_particle_sphere(
ray_origin_world,
ray_dir_world,
particles_position[si],
particles_radius[si],
)
if hit_dist > -1.0 and hit_dist < max_dist:
return True
return False
@wp.func
def first_hit_triangle_mesh(
triangle_mesh_id: wp.uint64,
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
max_dist: wp.float32,
) -> wp.bool:
if triangle_mesh_id:
geom_hit, _bary_u, _bary_v, _face_idx = ray_intersect.ray_intersect_mesh_no_transform(
triangle_mesh_id, ray_origin_world, ray_dir_world, wp.static(config.enable_backface_culling), max_dist
)
return geom_hit.hit
return False
@wp.func
def first_hit(
bvh_shapes_size: wp.int32,
bvh_shapes_id: wp.uint64,
bvh_shapes_group_roots: wp.array[wp.int32],
bvh_particles_size: wp.int32,
bvh_particles_id: wp.uint64,
bvh_particles_group_roots: wp.array[wp.int32],
world_index: wp.int32,
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_source_ptr: wp.array[wp.uint64],
particles_position: wp.array[wp.vec3f],
particles_radius: wp.array[wp.float32],
triangle_mesh_id: wp.uint64,
ray_origin_world: wp.vec3f,
ray_dir_world: wp.vec3f,
max_distance: wp.float32,
) -> wp.bool:
if first_hit_triangle_mesh(triangle_mesh_id, ray_origin_world, ray_dir_world, max_distance):
return True
if first_hit_shape(
bvh_shapes_size,
bvh_shapes_id,
bvh_shapes_group_roots,
world_index,
shape_enabled,
shape_types,
shape_sizes,
shape_transforms,
shape_source_ptr,
ray_origin_world,
ray_dir_world,
max_distance,
):
return True
if wp.static(config.enable_particles):
if first_hit_particles(
bvh_particles_size,
bvh_particles_id,
bvh_particles_group_roots,
world_index,
particles_position,
particles_radius,
ray_origin_world,
ray_dir_world,
max_distance,
):
return True
return False
return first_hit
================================================
FILE: newton/_src/sensors/warp_raytrace/render.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from typing import TYPE_CHECKING
import warp as wp
from ...geometry import Gaussian, GeoType
from . import lighting, raytrace, textures, tiling
from .types import MeshData, RenderOrder, TextureData
if TYPE_CHECKING:
from .render_context import RenderContext
def create_kernel(
config: RenderContext.Config, state: RenderContext.State, clear_data: RenderContext.ClearData
) -> wp.kernel:
compute_lighting = lighting.create_compute_lighting_function(config, state)
if state.render_color or state.render_normal:
raytrace_closest_hit = raytrace.create_closest_hit_function(config, state)
else:
raytrace_closest_hit = raytrace.create_closest_hit_depth_only_function(config, state)
@wp.kernel(enable_backward=False)
def render_megakernel(
# Model and Config
world_count: wp.int32,
camera_count: wp.int32,
light_count: wp.int32,
img_width: wp.int32,
img_height: wp.int32,
# Camera
camera_rays: wp.array4d[wp.vec3f],
camera_transforms: wp.array2d[wp.transformf],
# Shapes BVH
bvh_shapes_size: wp.int32,
bvh_shapes_id: wp.uint64,
bvh_shapes_group_roots: wp.array[wp.int32],
# Shapes
shape_enabled: wp.array[wp.uint32],
shape_types: wp.array[wp.int32],
shape_sizes: wp.array[wp.vec3f],
shape_colors: wp.array[wp.vec3f],
shape_transforms: wp.array[wp.transformf],
shape_source_ptr: wp.array[wp.uint64],
shape_texture_ids: wp.array[wp.int32],
shape_mesh_data_ids: wp.array[wp.int32],
# Particle BVH
bvh_particles_size: wp.int32,
bvh_particles_id: wp.uint64,
bvh_particles_group_roots: wp.array[wp.int32],
# Particles
particles_position: wp.array[wp.vec3f],
particles_radius: wp.array[wp.float32],
# Triangle Mesh:
triangle_mesh_id: wp.uint64,
# Meshes
mesh_data: wp.array[MeshData],
# Gaussians
gaussians_data: wp.array[Gaussian.Data],
# Textures
texture_data: wp.array[TextureData],
# Lights
light_active: wp.array[wp.bool],
light_type: wp.array[wp.int32],
light_cast_shadow: wp.array[wp.bool],
light_positions: wp.array[wp.vec3f],
light_orientations: wp.array[wp.vec3f],
# Outputs
out_color: wp.array[wp.uint32],
out_depth: wp.array[wp.float32],
out_shape_index: wp.array[wp.uint32],
out_normal: wp.array[wp.vec3f],
out_albedo: wp.array[wp.uint32],
):
tid = wp.tid()
if wp.static(config.render_order == RenderOrder.PIXEL_PRIORITY):
world_index, camera_index, py, px = tiling.tid_to_coord_pixel_priority(
tid, world_count, camera_count, img_width
)
elif wp.static(config.render_order == RenderOrder.VIEW_PRIORITY):
world_index, camera_index, py, px = tiling.tid_to_coord_view_priority(
tid, camera_count, img_width, img_height
)
elif wp.static(config.render_order == RenderOrder.TILED):
world_index, camera_index, py, px = tiling.tid_to_coord_tiled(
tid, camera_count, img_width, img_height, wp.static(config.tile_width), wp.static(config.tile_height)
)
else:
return
if px >= img_width or py >= img_height:
return
pixels_per_camera = img_width * img_height
pixels_per_world = camera_count * pixels_per_camera
out_index = world_index * pixels_per_world + camera_index * pixels_per_camera + py * img_width + px
camera_transform = camera_transforms[camera_index, world_index]
ray_origin_world = wp.transform_point(camera_transform, camera_rays[camera_index, py, px, 0])
ray_dir_world = wp.transform_vector(camera_transform, camera_rays[camera_index, py, px, 1])
closest_hit = raytrace_closest_hit(
bvh_shapes_size,
bvh_shapes_id,
bvh_shapes_group_roots,
bvh_particles_size,
bvh_particles_id,
bvh_particles_group_roots,
world_index,
wp.static(config.max_distance),
shape_enabled,
shape_types,
shape_sizes,
shape_transforms,
shape_source_ptr,
shape_mesh_data_ids,
mesh_data,
particles_position,
particles_radius,
triangle_mesh_id,
gaussians_data,
ray_origin_world,
ray_dir_world,
)
if closest_hit.shape_index == raytrace.NO_HIT_SHAPE_ID:
if wp.static(state.render_color):
out_color[out_index] = wp.uint32(wp.static(clear_data.clear_color))
if wp.static(state.render_albedo):
out_albedo[out_index] = wp.uint32(wp.static(clear_data.clear_albedo))
if wp.static(state.render_depth):
out_depth[out_index] = wp.float32(wp.static(clear_data.clear_depth))
if wp.static(state.render_normal):
out_normal[out_index] = wp.vec3f(
wp.static(clear_data.clear_normal[0]),
wp.static(clear_data.clear_normal[1]),
wp.static(clear_data.clear_normal[2]),
)
if wp.static(state.render_shape_index):
out_shape_index[out_index] = wp.uint32(wp.static(clear_data.clear_shape_index))
return
if wp.static(state.render_depth):
out_depth[out_index] = closest_hit.distance
if wp.static(state.render_normal):
out_normal[out_index] = closest_hit.normal
if wp.static(state.render_shape_index):
out_shape_index[out_index] = closest_hit.shape_index
if not wp.static(state.render_color) and not wp.static(state.render_albedo):
return
is_gaussian = wp.bool(False)
if closest_hit.shape_index < raytrace.MAX_SHAPE_ID:
if shape_types[closest_hit.shape_index] == GeoType.GAUSSIAN:
is_gaussian = wp.bool(True)
albedo_color = wp.vec3f(0.0)
if not is_gaussian:
hit_point = ray_origin_world + ray_dir_world * closest_hit.distance
albedo_color = wp.vec3f(1.0)
if closest_hit.shape_index < raytrace.MAX_SHAPE_ID:
albedo_color = shape_colors[closest_hit.shape_index]
if wp.static(config.enable_textures) and closest_hit.shape_index < raytrace.MAX_SHAPE_ID:
texture_index = shape_texture_ids[closest_hit.shape_index]
if texture_index > -1:
tex_color = textures.sample_texture(
shape_types[closest_hit.shape_index],
shape_transforms[closest_hit.shape_index],
texture_data,
texture_index,
shape_source_ptr[closest_hit.shape_index],
mesh_data,
shape_mesh_data_ids[closest_hit.shape_index],
hit_point,
closest_hit.bary_u,
closest_hit.bary_v,
closest_hit.face_idx,
)
albedo_color = wp.cw_mul(albedo_color, tex_color)
if wp.static(state.render_albedo):
out_albedo[out_index] = tiling.pack_rgba_to_uint32(albedo_color, 1.0)
if not wp.static(state.render_color):
return
shaded_color = closest_hit.color
if not is_gaussian:
if wp.static(config.enable_ambient_lighting):
up = wp.vec3f(0.0, 0.0, 1.0)
len_n = wp.length(closest_hit.normal)
n = closest_hit.normal if len_n > 0.0 else up
n = wp.normalize(n)
hemispheric = 0.5 * (wp.dot(n, up) + 1.0)
sky = wp.vec3f(0.4, 0.4, 0.45)
ground = wp.vec3f(0.1, 0.1, 0.12)
ambient_color = sky * hemispheric + ground * (1.0 - hemispheric)
ambient_intensity = 0.5
shaded_color = wp.cw_mul(albedo_color, ambient_color * ambient_intensity)
# Apply lighting and shadows
for light_index in range(light_count):
light_contribution = compute_lighting(
world_index,
bvh_shapes_size,
bvh_shapes_id,
bvh_shapes_group_roots,
bvh_particles_size,
bvh_particles_id,
bvh_particles_group_roots,
shape_enabled,
shape_types,
shape_sizes,
shape_transforms,
shape_source_ptr,
light_active[light_index],
light_type[light_index],
light_cast_shadow[light_index],
light_positions[light_index],
light_orientations[light_index],
particles_position,
particles_radius,
triangle_mesh_id,
closest_hit.normal,
hit_point,
)
shaded_color = shaded_color + albedo_color * light_contribution
out_color[out_index] = tiling.pack_rgba_to_uint32(shaded_color, 1.0)
return render_megakernel
================================================
FILE: newton/_src/sensors/warp_raytrace/render_context.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warnings
from collections.abc import Callable
from dataclasses import dataclass
import numpy as np
import warp as wp
from ...core import MAXVAL
from ...geometry import Gaussian, GeoType, Mesh, ShapeFlags
from ...sim import Model, State
from ...utils import load_texture, normalize_texture
from .bvh import (
compute_bvh_group_roots,
compute_particle_bvh_bounds,
compute_shape_bvh_bounds,
)
from .gaussians import compute_gaussian_bounds
from .render import create_kernel
from .types import ClearData, MeshData, RenderConfig, RenderOrder, TextureData
from .utils import Utils
@wp.kernel(enable_backward=False)
def compute_shape_bounds(
in_shape_type: wp.array[wp.int32],
in_shape_ptr: wp.array[wp.uint64],
in_gaussians: wp.array[Gaussian.Data],
out_bounds: wp.array2d[wp.vec3f],
):
tid = wp.tid()
min_point = wp.vec3(MAXVAL)
max_point = wp.vec3(-MAXVAL)
if in_shape_type[tid] == GeoType.MESH:
mesh = wp.mesh_get(in_shape_ptr[tid])
for i in range(mesh.points.shape[0]):
min_point = wp.min(min_point, mesh.points[i])
max_point = wp.max(max_point, mesh.points[i])
elif in_shape_type[tid] == GeoType.GAUSSIAN:
gaussian_id = in_shape_ptr[tid]
for i in range(in_gaussians[gaussian_id].num_points):
lower, upper = compute_gaussian_bounds(in_gaussians[gaussian_id], i)
min_point = wp.min(min_point, lower)
max_point = wp.max(max_point, upper)
out_bounds[tid, 0] = min_point
out_bounds[tid, 1] = max_point
@wp.kernel(enable_backward=False)
def convert_newton_transform(
in_body_transforms: wp.array[wp.transform],
in_shape_body: wp.array[wp.int32],
in_transform: wp.array[wp.transformf],
in_scale: wp.array[wp.vec3f],
out_transforms: wp.array[wp.transformf],
out_sizes: wp.array[wp.vec3f],
):
tid = wp.tid()
body = in_shape_body[tid]
body_transform = wp.transform_identity()
if body >= 0:
body_transform = in_body_transforms[body]
out_transforms[tid] = wp.mul(body_transform, in_transform[tid])
out_sizes[tid] = in_scale[tid]
@wp.func
def is_supported_shape_type(shape_type: wp.int32) -> wp.bool:
if shape_type == GeoType.BOX:
return True
if shape_type == GeoType.CAPSULE:
return True
if shape_type == GeoType.CYLINDER:
return True
if shape_type == GeoType.ELLIPSOID:
return True
if shape_type == GeoType.PLANE:
return True
if shape_type == GeoType.SPHERE:
return True
if shape_type == GeoType.CONE:
return True
if shape_type == GeoType.MESH:
return True
if shape_type == GeoType.GAUSSIAN:
return True
wp.printf("Unsupported shape geom type: %d\n", shape_type)
return False
@wp.kernel(enable_backward=False)
def compute_enabled_shapes(
shape_type: wp.array[wp.int32],
shape_flags: wp.array[wp.int32],
out_shape_enabled: wp.array[wp.uint32],
out_shape_enabled_count: wp.array[wp.int32],
):
tid = wp.tid()
if not bool(shape_flags[tid] & ShapeFlags.VISIBLE):
return
if not is_supported_shape_type(shape_type[tid]):
return
index = wp.atomic_add(out_shape_enabled_count, 0, 1)
out_shape_enabled[index] = wp.uint32(tid)
class RenderContext:
Config = RenderConfig
ClearData = ClearData
@dataclass(unsafe_hash=True)
class State:
"""Mutable flags tracking which render outputs are active."""
num_gaussians: int = 0
render_color: bool = False
render_depth: bool = False
render_shape_index: bool = False
render_normal: bool = False
render_albedo: bool = False
DEFAULT_CLEAR_DATA = ClearData()
def __init__(self, world_count: int = 1, config: Config | None = None, device: str | None = None):
"""Create a new render context.
Args:
world_count: Number of simulation worlds to render.
config: Render configuration. If ``None``, uses default
:class:`Config` settings.
device: Warp device string (e.g. ``"cuda:0"``). If ``None``,
the default Warp device is used.
"""
self.device: str | None = device
self.utils = Utils(self)
self.config = config if config else RenderContext.Config()
self.state = RenderContext.State()
self.kernel_cache: dict[int, wp.Kernel] = {}
self.world_count: int = world_count
self.bvh_shapes: wp.Bvh | None = None
self.bvh_shapes_group_roots: wp.array[wp.int32] | None = None
self.bvh_particles: wp.Bvh | None = None
self.bvh_particles_group_roots: wp.array[wp.int32] | None = None
self.triangle_mesh: wp.Mesh | None = None
self.shape_count_enabled: int = 0
self.shape_count_total: int = 0
self.__triangle_points: wp.array[wp.vec3f] | None = None
self.__triangle_indices: wp.array[wp.int32] | None = None
self.__particles_position: wp.array[wp.vec3f] | None = None
self.__particles_radius: wp.array[wp.float32] | None = None
self.__particles_world_index: wp.array[wp.int32] | None = None
self.__gaussians_data: wp.array[Gaussian.Data] | None = None
self.shape_enabled: wp.array[wp.uint32] | None = None
self.shape_types: wp.array[wp.int32] | None = None
self.shape_sizes: wp.array[wp.vec3f] | None = None
self.shape_transforms: wp.array[wp.transformf] | None = None
self.shape_colors: wp.array[wp.vec3f] | None = None
self.shape_world_index: wp.array[wp.int32] | None = None
self.shape_source_ptr: wp.array[wp.uint64] | None = None
self.shape_bounds: wp.array2d[wp.vec3f] | None = None
self.shape_texture_ids: wp.array[wp.int32] | None = None
self.shape_mesh_data_ids: wp.array[wp.int32] | None = None
self.mesh_data: wp.array[MeshData] | None = None
self.texture_data: wp.array[TextureData] | None = None
self.lights_active: wp.array[wp.bool] | None = None
self.lights_type: wp.array[wp.int32] | None = None
self.lights_cast_shadow: wp.array[wp.bool] | None = None
self.lights_position: wp.array[wp.vec3f] | None = None
self.lights_orientation: wp.array[wp.vec3f] | None = None
def init_from_model(self, model: Model, load_textures: bool = True):
"""Initialize render context state from a Newton simulation model.
Populates shape, particle, triangle, and texture data from *model*.
Call once after construction or when the model topology changes.
Args:
model: Newton simulation model providing shapes and particles.
load_textures: Load mesh textures from disk. Set False for
checkerboard or custom texture workflows.
"""
self.world_count = model.world_count
self.bvh_shapes = None
self.bvh_shapes_group_roots = None
self.bvh_particles = None
self.bvh_particles_group_roots = None
self.triangle_mesh = None
self.__triangle_points = None
self.__triangle_indices = None
self.__particles_position = None
self.__particles_radius = None
self.__particles_world_index = None
self.shape_source_ptr = model.shape_source_ptr
self.shape_bounds = wp.empty((model.shape_count, 2), dtype=wp.vec3f, ndim=2, device=self.device)
if model.particle_q is not None and model.particle_q.shape[0]:
self.particles_position = model.particle_q
self.particles_radius = model.particle_radius
self.particles_world_index = model.particle_world
if model.tri_indices is not None and model.tri_indices.shape[0]:
self.triangle_points = model.particle_q
self.triangle_indices = model.tri_indices.flatten()
self.config.enable_particles = False
self.shape_enabled = wp.empty(model.shape_count, dtype=wp.uint32, device=self.device)
self.shape_types = model.shape_type
self.shape_sizes = wp.empty(model.shape_count, dtype=wp.vec3f, device=self.device)
self.shape_transforms = wp.empty(model.shape_count, dtype=wp.transformf, device=self.device)
self.shape_colors = model.shape_color
self.shape_world_index = model.shape_world
self.gaussians_data = model.gaussians_data
self.__load_texture_and_mesh_data(model, load_textures)
num_enabled_shapes = wp.zeros(1, dtype=wp.int32, device=self.device)
wp.launch(
kernel=compute_enabled_shapes,
dim=model.shape_count,
inputs=[
model.shape_type,
model.shape_flags,
self.shape_enabled,
num_enabled_shapes,
],
device=self.device,
)
self.shape_count_total = model.shape_count
self.shape_count_enabled = int(num_enabled_shapes.numpy()[0])
self.__compute_shape_bounds()
def update(self, model: Model, state: State):
"""Synchronize transforms and particle positions from simulation state.
Args:
model: Newton simulation model (for shape metadata).
state: Current simulation state with body transforms and
particle positions.
"""
if self.has_shapes:
wp.launch(
kernel=convert_newton_transform,
dim=model.shape_count,
inputs=[
state.body_q,
model.shape_body,
model.shape_transform,
model.shape_scale,
self.shape_transforms,
self.shape_sizes,
],
device=self.device,
)
if self.has_triangle_mesh:
self.triangle_points = state.particle_q
if self.has_particles:
self.particles_position = state.particle_q
def refit_bvh(self):
"""Rebuild or refit the BVH acceleration structures.
Updates shape, particle, and triangle-mesh BVHs so that
subsequent :meth:`render` calls use current geometry positions.
"""
self.bvh_shapes, self.bvh_shapes_group_roots = self.__update_bvh(
self.bvh_shapes, self.bvh_shapes_group_roots, self.shape_count_enabled, self.__compute_bvh_bounds_shapes
)
self.bvh_particles, self.bvh_particles_group_roots = self.__update_bvh(
self.bvh_particles,
self.bvh_particles_group_roots,
self.particle_count_total,
self.__compute_bvh_bounds_particles,
)
if self.has_triangle_mesh:
if self.triangle_mesh is None:
self.triangle_mesh = wp.Mesh(self.triangle_points, self.triangle_indices, device=self.device)
else:
self.triangle_mesh.refit()
def render(
self,
camera_transforms: wp.array2d[wp.transformf],
camera_rays: wp.array4d[wp.vec3f],
color_image: wp.array4d[wp.uint32] | None = None,
depth_image: wp.array4d[wp.float32] | None = None,
shape_index_image: wp.array4d[wp.uint32] | None = None,
normal_image: wp.array4d[wp.vec3f] | None = None,
albedo_image: wp.array4d[wp.uint32] | None = None,
refit_bvh: bool = True,
clear_data: RenderContext.ClearData | None = DEFAULT_CLEAR_DATA,
):
"""Raytrace the scene into the provided output images.
At least one output image must be supplied. All non-``None``
output arrays must have shape
``(world_count, camera_count, height, width)``.
Args:
camera_transforms: Per-camera transforms, shape
``(camera_count, world_count)``.
camera_rays: Ray origins and directions, shape
``(camera_count, height, width, 2)``.
color_image: Output RGBA color buffer (packed ``uint32``).
depth_image: Output depth buffer [m].
shape_index_image: Output shape-index buffer.
normal_image: Output world-space surface normals.
albedo_image: Output albedo buffer (packed ``uint32``).
refit_bvh: If ``True``, call :meth:`refit_bvh` before
rendering.
clear_data: Values used to clear output images before
rendering. Pass ``None`` to use :attr:`DEFAULT_CLEAR_DATA`.
"""
if self.has_shapes or self.has_particles or self.has_triangle_mesh or self.has_gaussians:
if refit_bvh:
self.refit_bvh()
width = camera_rays.shape[2]
height = camera_rays.shape[1]
camera_count = camera_rays.shape[0]
if clear_data is None:
clear_data = RenderContext.DEFAULT_CLEAR_DATA
self.state.render_color = color_image is not None
self.state.render_depth = depth_image is not None
self.state.render_shape_index = shape_index_image is not None
self.state.render_normal = normal_image is not None
self.state.render_albedo = albedo_image is not None
assert camera_transforms.shape == (camera_count, self.world_count), (
f"camera_transforms size must match {camera_count} x {self.world_count}"
)
assert camera_rays.shape == (camera_count, height, width, 2), (
f"camera_rays size must match {camera_count} x {height} x {width} x 2"
)
if color_image is not None:
assert color_image.shape == (self.world_count, camera_count, height, width), (
f"color_image size must match {self.world_count} x {camera_count} x {height} x {width}"
)
if depth_image is not None:
assert depth_image.shape == (self.world_count, camera_count, height, width), (
f"depth_image size must match {self.world_count} x {camera_count} x {height} x {width}"
)
if shape_index_image is not None:
assert shape_index_image.shape == (self.world_count, camera_count, height, width), (
f"shape_index_image size must match {self.world_count} x {camera_count} x {height} x {width}"
)
if normal_image is not None:
assert normal_image.shape == (self.world_count, camera_count, height, width), (
f"normal_image size must match {self.world_count} x {camera_count} x {height} x {width}"
)
if albedo_image is not None:
assert albedo_image.shape == (self.world_count, camera_count, height, width), (
f"albedo_image size must match {self.world_count} x {camera_count} x {height} x {width}"
)
if self.config.render_order == RenderOrder.TILED:
assert width % self.config.tile_width == 0, "render width must be a multiple of tile_width"
assert height % self.config.tile_height == 0, "render height must be a multiple of tile_height"
# Reshaping output images to one dimension, slightly improves performance in the Kernel.
if color_image is not None:
color_image = color_image.reshape(self.world_count * camera_count * width * height)
if depth_image is not None:
depth_image = depth_image.reshape(self.world_count * camera_count * width * height)
if shape_index_image is not None:
shape_index_image = shape_index_image.reshape(self.world_count * camera_count * width * height)
if normal_image is not None:
normal_image = normal_image.reshape(self.world_count * camera_count * width * height)
if albedo_image is not None:
albedo_image = albedo_image.reshape(self.world_count * camera_count * width * height)
kernel_cache_key = hash((self.config, self.state, clear_data))
render_kernel = self.kernel_cache.get(kernel_cache_key)
if render_kernel is None:
render_kernel = create_kernel(self.config, self.state, clear_data)
self.kernel_cache[kernel_cache_key] = render_kernel
wp.launch(
kernel=render_kernel,
dim=(self.world_count * camera_count * width * height),
inputs=[
# Model and config
self.world_count,
camera_count,
self.light_count,
width,
height,
# Camera
camera_rays,
camera_transforms,
# Shape BVH
self.shape_count_enabled,
self.bvh_shapes.id if self.bvh_shapes else 0,
self.bvh_shapes_group_roots,
# Shapes
self.shape_enabled,
self.shape_types,
self.shape_sizes,
self.shape_colors,
self.shape_transforms,
self.shape_source_ptr,
self.shape_texture_ids,
self.shape_mesh_data_ids,
# Particle BVH
self.particle_count_total,
self.bvh_particles.id if self.bvh_particles else 0,
self.bvh_particles_group_roots,
# Particles
self.particles_position,
self.particles_radius,
# Triangle Mesh
self.triangle_mesh.id if self.triangle_mesh is not None else 0,
# Meshes
self.mesh_data,
# Gaussians
self.gaussians_data,
# Textures
self.texture_data,
# Lights
self.lights_active,
self.lights_type,
self.lights_cast_shadow,
self.lights_position,
self.lights_orientation,
# Outputs
color_image,
depth_image,
shape_index_image,
normal_image,
albedo_image,
],
device=self.device,
)
@property
def world_count_total(self) -> int:
if self.config.enable_global_world:
return self.world_count + 1
return self.world_count
@property
def particle_count_total(self) -> int:
if self.particles_position is not None:
return self.particles_position.shape[0]
return 0
@property
def light_count(self) -> int:
if self.lights_active is not None:
return self.lights_active.shape[0]
return 0
@property
def gaussians_count_total(self) -> int:
if self.gaussians_data is not None:
return self.gaussians_data.shape[0]
return 0
@property
def has_shapes(self) -> bool:
return self.shape_count_enabled > 0
@property
def has_particles(self) -> bool:
return self.particles_position is not None
@property
def has_triangle_mesh(self) -> bool:
return self.triangle_points is not None
@property
def has_gaussians(self) -> bool:
return self.gaussians_data is not None
@property
def triangle_points(self) -> wp.array[wp.vec3f]:
return self.__triangle_points
@triangle_points.setter
def triangle_points(self, triangle_points: wp.array[wp.vec3f]):
if self.__triangle_points is None or self.__triangle_points.ptr != triangle_points.ptr:
self.triangle_mesh = None
self.__triangle_points = triangle_points
@property
def triangle_indices(self) -> wp.array[wp.int32]:
return self.__triangle_indices
@triangle_indices.setter
def triangle_indices(self, triangle_indices: wp.array[wp.int32]):
if self.__triangle_indices is None or self.__triangle_indices.ptr != triangle_indices.ptr:
self.triangle_mesh = None
self.__triangle_indices = triangle_indices
@property
def particles_position(self) -> wp.array[wp.vec3f]:
return self.__particles_position
@particles_position.setter
def particles_position(self, particles_position: wp.array[wp.vec3f]):
if self.__particles_position is None or self.__particles_position.ptr != particles_position.ptr:
self.bvh_particles = None
self.__particles_position = particles_position
@property
def particles_radius(self) -> wp.array[wp.float32]:
return self.__particles_radius
@particles_radius.setter
def particles_radius(self, particles_radius: wp.array[wp.float32]):
if self.__particles_radius is None or self.__particles_radius.ptr != particles_radius.ptr:
self.bvh_particles = None
self.__particles_radius = particles_radius
@property
def particles_world_index(self) -> wp.array[wp.int32]:
return self.__particles_world_index
@particles_world_index.setter
def particles_world_index(self, particles_world_index: wp.array[wp.int32]):
if self.__particles_world_index is None or self.__particles_world_index.ptr != particles_world_index.ptr:
self.bvh_particles = None
self.__particles_world_index = particles_world_index
@property
def gaussians_data(self) -> wp.array[Gaussian.Data]:
return self.__gaussians_data
@gaussians_data.setter
def gaussians_data(self, gaussians_data: wp.array[Gaussian.Data]):
self.__gaussians_data = gaussians_data
if gaussians_data is None:
self.state.num_gaussians = 0
else:
self.state.num_gaussians = gaussians_data.shape[0]
def __update_bvh(
self,
bvh: wp.Bvh,
group_roots: wp.array[wp.int32],
size: int,
bounds_callback: Callable[[wp.array[wp.vec3f], wp.array[wp.vec3f], wp.array[wp.int32]], None],
):
"""Build a new BVH or refit an existing one.
If *bvh* is ``None`` a new :class:`wp.Bvh` is constructed and
group roots are computed; otherwise the existing BVH is refit
in-place.
Args:
bvh: Existing BVH to refit, or ``None`` to build a new one.
group_roots: Existing group-root array, or ``None``.
size: Number of primitives (shapes or particles).
bounds_callback: Callback that fills lower/upper/group
arrays for the BVH primitives.
Returns:
Tuple of ``(bvh, group_roots)``.
"""
if size:
lowers = bvh.lowers if bvh is not None else wp.zeros(size, dtype=wp.vec3f, device=self.device)
uppers = bvh.uppers if bvh is not None else wp.zeros(size, dtype=wp.vec3f, device=self.device)
groups = bvh.groups if bvh is not None else wp.zeros(size, dtype=wp.int32, device=self.device)
bounds_callback(lowers, uppers, groups)
if bvh is None:
bvh = wp.Bvh(lowers, uppers, groups=groups)
group_roots = wp.zeros((self.world_count_total), dtype=wp.int32, device=self.device)
wp.launch(
kernel=compute_bvh_group_roots,
dim=self.world_count_total,
inputs=[bvh.id, group_roots],
device=self.device,
)
else:
bvh.refit()
return bvh, group_roots
def __compute_bvh_bounds_shapes(
self, lowers: wp.array[wp.vec3f], uppers: wp.array[wp.vec3f], groups: wp.array[wp.int32]
):
"""Compute axis-aligned bounding boxes for enabled shapes."""
wp.launch(
kernel=compute_shape_bvh_bounds,
dim=self.shape_count_enabled,
inputs=[
self.shape_count_enabled,
self.world_count_total,
self.shape_world_index,
self.shape_enabled,
self.shape_types,
self.shape_sizes,
self.shape_transforms,
self.shape_bounds,
lowers,
uppers,
groups,
],
device=self.device,
)
def __compute_bvh_bounds_particles(
self, lowers: wp.array[wp.vec3f], uppers: wp.array[wp.vec3f], groups: wp.array[wp.int32]
):
"""Compute axis-aligned bounding boxes for particles."""
wp.launch(
kernel=compute_particle_bvh_bounds,
dim=self.particle_count_total,
inputs=[
self.particle_count_total,
self.world_count_total,
self.particles_world_index,
self.particles_position,
self.particles_radius,
lowers,
uppers,
groups,
],
device=self.device,
)
def __compute_shape_bounds(self):
"""Compute per-shape local-space bounding boxes for meshes and Gaussians."""
wp.launch(
kernel=compute_shape_bounds,
dim=self.shape_source_ptr.size,
inputs=[
self.shape_types,
self.shape_source_ptr,
self.gaussians_data,
self.shape_bounds,
],
device=self.device,
)
def __load_texture_and_mesh_data(self, model: Model, load_textures: bool):
"""Load mesh UV/normal data and textures from *model*.
Populates :attr:`mesh_data`, :attr:`texture_data`, and the
per-shape texture/mesh-data index arrays. Textures and mesh
data are deduplicated by hash/identity.
Args:
model: Newton simulation model containing shape sources.
load_textures: If ``True``, load image textures from disk;
otherwise assign ``-1`` texture IDs to all shapes.
"""
self.__mesh_data = []
self.__texture_data = []
texture_hashes = {}
mesh_hashes = {}
mesh_data_ids = []
texture_data_ids = []
for shape in model.shape_source:
if isinstance(shape, Mesh):
if shape.texture is not None and load_textures:
if shape.texture_hash not in texture_hashes:
pixels = load_texture(shape.texture)
if pixels is None:
raise ValueError(f"Failed to load texture: {shape.texture}")
# Normalize texture to ensure a consistent channel layout and dtype
pixels = normalize_texture(pixels, require_channels=True)
if pixels.dtype != np.uint8:
pixels = pixels.astype(np.uint8, copy=False)
texture_hashes[shape.texture_hash] = len(self.__texture_data)
data = TextureData()
data.texture = wp.Texture2D(
pixels,
filter_mode=wp.TextureFilterMode.LINEAR,
address_mode=wp.TextureAddressMode.WRAP,
normalized_coords=True,
dtype=wp.uint8,
num_channels=4,
device=self.device,
)
data.repeat = wp.vec2f(1.0, 1.0)
self.__texture_data.append(data)
texture_data_ids.append(texture_hashes[shape.texture_hash])
else:
texture_data_ids.append(-1)
if shape.uvs is not None or shape.normals is not None:
if shape not in mesh_hashes:
mesh_hashes[shape] = len(self.__mesh_data)
data = MeshData()
if shape.uvs is not None:
data.uvs = wp.array(shape.uvs, dtype=wp.vec2f, device=self.device)
if shape.normals is not None:
data.normals = wp.array(shape.normals, dtype=wp.vec3f, device=self.device)
self.__mesh_data.append(data)
mesh_data_ids.append(mesh_hashes[shape])
else:
mesh_data_ids.append(-1)
else:
texture_data_ids.append(-1)
mesh_data_ids.append(-1)
self.texture_data = wp.array(self.__texture_data, dtype=TextureData, device=self.device)
self.shape_texture_ids = wp.array(texture_data_ids, dtype=wp.int32, device=self.device)
self.mesh_data = wp.array(self.__mesh_data, dtype=MeshData, device=self.device)
self.shape_mesh_data_ids = wp.array(mesh_data_ids, dtype=wp.int32, device=self.device)
def create_color_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create an output array for color rendering.
.. deprecated::
Use :meth:`SensorTiledCamera.utils.create_color_image_output`.
"""
warnings.warn(
"RenderContext.create_color_image_output is deprecated, use SensorTiledCamera.utils.create_color_image_output instead.",
DeprecationWarning,
stacklevel=2,
)
return self.utils.create_color_image_output(width, height, camera_count)
def create_depth_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.float32]:
"""Create an output array for depth rendering.
.. deprecated::
Use :meth:`SensorTiledCamera.utils.create_depth_image_output`.
"""
warnings.warn(
"RenderContext.create_depth_image_output is deprecated, use SensorTiledCamera.utils.create_depth_image_output instead.",
DeprecationWarning,
stacklevel=2,
)
return self.utils.create_depth_image_output(width, height, camera_count)
def create_shape_index_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create an output array for shape-index rendering.
.. deprecated::
Use :meth:`SensorTiledCamera.utils.create_shape_index_image_output`.
"""
warnings.warn(
"RenderContext.create_shape_index_image_output is deprecated, use SensorTiledCamera.utils.create_shape_index_image_output instead.",
DeprecationWarning,
stacklevel=2,
)
return self.utils.create_shape_index_image_output(width, height, camera_count)
def create_normal_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.vec3f]:
"""Create an output array for surface-normal rendering.
.. deprecated::
Use :meth:`SensorTiledCamera.utils.create_normal_image_output`.
"""
warnings.warn(
"RenderContext.create_normal_image_output is deprecated, use SensorTiledCamera.utils.create_normal_image_output instead.",
DeprecationWarning,
stacklevel=2,
)
return self.utils.create_normal_image_output(width, height, camera_count)
def create_albedo_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create an output array for albedo rendering.
.. deprecated::
Use :meth:`SensorTiledCamera.utils.create_albedo_image_output`.
"""
warnings.warn(
"RenderContext.create_albedo_image_output is deprecated, use SensorTiledCamera.utils.create_albedo_image_output instead.",
DeprecationWarning,
stacklevel=2,
)
return self.utils.create_albedo_image_output(width, height, camera_count)
================================================
FILE: newton/_src/sensors/warp_raytrace/textures.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...geometry import GeoType
from .types import MeshData, TextureData
@wp.func
def flip_v(uv: wp.vec2f) -> wp.vec2f:
return wp.vec2f(uv[0], 1.0 - uv[1])
@wp.func
def sample_texture_2d(uv: wp.vec2f, texture_data: TextureData) -> wp.vec3f:
color = wp.texture_sample(texture_data.texture, uv, dtype=wp.vec4f)
return wp.vec3f(color[0], color[1], color[2])
@wp.func
def sample_texture_plane(
hit_point: wp.vec3f,
shape_transform: wp.transformf,
texture_data: TextureData,
) -> wp.vec3f:
inv_transform = wp.transform_inverse(shape_transform)
local = wp.transform_point(inv_transform, hit_point)
uv = wp.vec2f(local[0], local[1])
return sample_texture_2d(flip_v(wp.cw_mul(uv, texture_data.repeat)), texture_data)
@wp.func
def sample_texture_mesh(
bary_u: wp.float32,
bary_v: wp.float32,
face_id: wp.int32,
mesh_id: wp.uint64,
mesh_data: MeshData,
texture_data: TextureData,
) -> wp.vec3f:
bary_w = 1.0 - bary_u - bary_v
uv0 = wp.mesh_get_index(mesh_id, face_id * 3 + 0)
uv1 = wp.mesh_get_index(mesh_id, face_id * 3 + 1)
uv2 = wp.mesh_get_index(mesh_id, face_id * 3 + 2)
uv = mesh_data.uvs[uv0] * bary_u + mesh_data.uvs[uv1] * bary_v + mesh_data.uvs[uv2] * bary_w
return sample_texture_2d(flip_v(wp.cw_mul(uv, texture_data.repeat)), texture_data)
@wp.func
def sample_texture(
shape_type: wp.int32,
shape_transform: wp.transformf,
texture_data: wp.array[TextureData],
texture_index: wp.int32,
mesh_id: wp.uint64,
mesh_data: wp.array[MeshData],
mesh_data_index: wp.int32,
hit_point: wp.vec3f,
bary_u: wp.float32,
bary_v: wp.float32,
face_id: wp.int32,
) -> wp.vec3f:
DEFAULT_RETURN = wp.vec3f(1.0, 1.0, 1.0)
if texture_index == -1:
return DEFAULT_RETURN
if shape_type == GeoType.PLANE:
return sample_texture_plane(hit_point, shape_transform, texture_data[texture_index])
if shape_type == GeoType.MESH:
if face_id < 0 or mesh_data_index < 0:
return DEFAULT_RETURN
if mesh_data[mesh_data_index].uvs.shape[0] == 0:
return DEFAULT_RETURN
return sample_texture_mesh(
bary_u, bary_v, face_id, mesh_id, mesh_data[mesh_data_index], texture_data[texture_index]
)
return DEFAULT_RETURN
================================================
FILE: newton/_src/sensors/warp_raytrace/tiling.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
@wp.func
def tid_to_coord_tiled(
tid: wp.int32,
camera_count: wp.int32,
width: wp.int32,
height: wp.int32,
tile_width: wp.int32,
tile_height: wp.int32,
):
num_pixels_per_view = width * height
num_pixels_per_tile = tile_width * tile_height
num_tiles_per_row = width // tile_width
pixel_idx = tid % num_pixels_per_view
view_idx = tid // num_pixels_per_view
world_index = view_idx // camera_count
camera_index = view_idx % camera_count
tile_idx = pixel_idx // num_pixels_per_tile
tile_pixel_idx = pixel_idx % num_pixels_per_tile
tile_y = tile_idx // num_tiles_per_row
tile_x = tile_idx % num_tiles_per_row
py = tile_y * tile_height + tile_pixel_idx // tile_width
px = tile_x * tile_width + tile_pixel_idx % tile_width
return world_index, camera_index, py, px
@wp.func
def tid_to_coord_pixel_priority(tid: wp.int32, world_count: wp.int32, camera_count: wp.int32, width: wp.int32):
num_views_per_pixel = world_count * camera_count
pixel_idx = tid // num_views_per_pixel
view_idx = tid % num_views_per_pixel
world_index = view_idx % world_count
camera_index = view_idx // world_count
py = pixel_idx // width
px = pixel_idx % width
return world_index, camera_index, py, px
@wp.func
def tid_to_coord_view_priority(tid: wp.int32, camera_count: wp.int32, width: wp.int32, height: wp.int32):
num_pixels_per_view = width * height
pixel_idx = tid % num_pixels_per_view
view_idx = tid // num_pixels_per_view
world_index = view_idx // camera_count
camera_index = view_idx % camera_count
py = pixel_idx // width
px = pixel_idx % width
return world_index, camera_index, py, px
@wp.func
def pack_rgba_to_uint32(rgb: wp.vec3f, alpha: wp.float32) -> wp.uint32:
"""Pack RGBA values into a single uint32 for efficient memory access."""
return (
(wp.clamp(wp.uint32(alpha * 255.0), wp.uint32(0), wp.uint32(255)) << wp.uint32(24))
| (wp.clamp(wp.uint32(rgb[2] * 255.0), wp.uint32(0), wp.uint32(255)) << wp.uint32(16))
| (wp.clamp(wp.uint32(rgb[1] * 255.0), wp.uint32(0), wp.uint32(255)) << wp.uint32(8))
| wp.clamp(wp.uint32(rgb[0] * 255.0), wp.uint32(0), wp.uint32(255))
)
================================================
FILE: newton/_src/sensors/warp_raytrace/types.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import enum
from dataclasses import dataclass
import warp as wp
class RenderLightType(enum.IntEnum):
"""Light types supported by the Warp raytracer."""
SPOTLIGHT = 0
"""Spotlight."""
DIRECTIONAL = 1
"""Directional Light."""
class RenderOrder(enum.IntEnum):
"""Render Order"""
PIXEL_PRIORITY = 0
"""Render the same pixel of every view before continuing to the next one"""
VIEW_PRIORITY = 1
"""Render all pixels of a whole view before continuing to the next one"""
TILED = 2
"""Render pixels in tiles, defined by tile_width x tile_height"""
class GaussianRenderMode(enum.IntEnum):
"""Gaussian Render Mode"""
FAST = 0
"""Fast Render Mode"""
QUALITY = 1
"""Quality Render Mode, collect hits until minimum transmittance is reached"""
@dataclass(unsafe_hash=True)
class RenderConfig:
"""Raytrace render settings shared across all worlds."""
enable_global_world: bool = True
"""Include shapes that belong to no specific world."""
enable_textures: bool = False
"""Enable texture-mapped rendering for meshes."""
enable_shadows: bool = False
"""Enable shadow rays for directional lights."""
enable_ambient_lighting: bool = True
"""Enable ambient lighting for the scene."""
enable_particles: bool = True
"""Enable particle rendering."""
enable_backface_culling: bool = True
"""Cull back-facing triangles."""
render_order: int = RenderOrder.PIXEL_PRIORITY
"""Render traversal order (see :class:`RenderOrder`)."""
tile_width: int = 16
"""Tile width [px] for ``RenderOrder.TILED`` traversal."""
tile_height: int = 8
"""Tile height [px] for ``RenderOrder.TILED`` traversal."""
max_distance: float = 1000.0
"""Maximum ray distance [m]."""
gaussians_mode: int = GaussianRenderMode.FAST
"""Gaussian splatting render mode (see :class:`GaussianRenderMode`)."""
gaussians_min_transmittance: float = 0.49
"""Minimum transmittance before early-out during Gaussian rendering."""
gaussians_max_num_hits: int = 20
"""Maximum Gaussian hits accumulated per ray."""
@dataclass(unsafe_hash=True)
class ClearData:
"""Default values written to output images before rendering."""
clear_color: int = 0
clear_depth: float = 0.0
clear_shape_index: int = 0xFFFFFFFF
clear_normal: tuple[float, float, float] = (0.0, 0.0, 0.0)
clear_albedo: int = 0
@wp.struct
class MeshData:
"""Per-mesh auxiliary vertex data for texture mapping and smooth shading.
Attributes:
uvs: Per-vertex UV coordinates, shape ``[vertex_count, 2]``, dtype ``vec2f``.
normals: Per-vertex normals for smooth shading, shape ``[vertex_count, 3]``, dtype ``vec3f``.
"""
uvs: wp.array[wp.vec2f]
normals: wp.array[wp.vec3f]
@wp.struct
class TextureData:
"""Texture image data for surface shading during raytracing.
Uses a hardware-accelerated ``wp.Texture2D`` with bilinear filtering.
Attributes:
texture: 2D Texture as ``wp.Texture2D``.
repeat: UV tiling factors along U and V axes.
"""
texture: wp.Texture2D
repeat: wp.vec2f
================================================
FILE: newton/_src/sensors/warp_raytrace/utils.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import math
import warnings
from typing import TYPE_CHECKING
import numpy as np
import warp as wp
from ...core import MAXVAL
from .types import RenderLightType, TextureData
if TYPE_CHECKING:
from .render_context import RenderContext
@wp.kernel(enable_backward=False)
def compute_pinhole_camera_rays(
width: int,
height: int,
camera_fovs: wp.array[wp.float32],
out_rays: wp.array4d[wp.vec3f],
):
camera_index, py, px = wp.tid()
aspect_ratio = float(width) / float(height)
u = (float(px) + 0.5) / float(width) - 0.5
v = (float(py) + 0.5) / float(height) - 0.5
h = wp.tan(camera_fovs[camera_index] / 2.0)
ray_direction_camera_space = wp.vec3f(u * 2.0 * h * aspect_ratio, -v * 2.0 * h, -1.0)
out_rays[camera_index, py, px, 0] = wp.vec3f(0.0)
out_rays[camera_index, py, px, 1] = wp.normalize(ray_direction_camera_space)
@wp.kernel(enable_backward=False)
def flatten_color_image(
color_image: wp.array4d[wp.uint32],
buffer: wp.array3d[wp.uint8],
width: wp.int32,
height: wp.int32,
camera_count: wp.int32,
worlds_per_row: wp.int32,
):
world_id, camera_id, y, x = wp.tid()
view_id = world_id * camera_count + camera_id
row = view_id // worlds_per_row
col = view_id % worlds_per_row
px = col * width + x
py = row * height + y
color = color_image[world_id, camera_id, y, x]
buffer[py, px, 0] = wp.uint8((color >> wp.uint32(0)) & wp.uint32(0xFF))
buffer[py, px, 1] = wp.uint8((color >> wp.uint32(8)) & wp.uint32(0xFF))
buffer[py, px, 2] = wp.uint8((color >> wp.uint32(16)) & wp.uint32(0xFF))
buffer[py, px, 3] = wp.uint8((color >> wp.uint32(24)) & wp.uint32(0xFF))
@wp.kernel(enable_backward=False)
def flatten_normal_image(
normal_image: wp.array4d[wp.vec3f],
buffer: wp.array3d[wp.uint8],
width: wp.int32,
height: wp.int32,
camera_count: wp.int32,
worlds_per_row: wp.int32,
):
world_id, camera_id, y, x = wp.tid()
view_id = world_id * camera_count + camera_id
row = view_id // worlds_per_row
col = view_id % worlds_per_row
px = col * width + x
py = row * height + y
normal = normal_image[world_id, camera_id, y, x] * 0.5 + wp.vec3f(0.5)
buffer[py, px, 0] = wp.uint8(normal[0] * 255.0)
buffer[py, px, 1] = wp.uint8(normal[1] * 255.0)
buffer[py, px, 2] = wp.uint8(normal[2] * 255.0)
buffer[py, px, 3] = wp.uint8(255)
@wp.kernel(enable_backward=False)
def find_depth_range(depth_image: wp.array4d[wp.float32], depth_range: wp.array[wp.float32]):
world_id, camera_id, y, x = wp.tid()
depth = depth_image[world_id, camera_id, y, x]
if depth > 0:
wp.atomic_min(depth_range, 0, depth)
wp.atomic_max(depth_range, 1, depth)
@wp.kernel(enable_backward=False)
def flatten_depth_image(
depth_image: wp.array4d[wp.float32],
buffer: wp.array3d[wp.uint8],
depth_range: wp.array[wp.float32],
width: wp.int32,
height: wp.int32,
camera_count: wp.int32,
worlds_per_row: wp.int32,
):
world_id, camera_id, y, x = wp.tid()
view_id = world_id * camera_count + camera_id
row = view_id // worlds_per_row
col = view_id % worlds_per_row
px = col * width + x
py = row * height + y
value = wp.uint8(0)
depth = depth_image[world_id, camera_id, y, x]
if depth > 0:
denom = wp.max(depth_range[1] - depth_range[0], 1e-6)
value = wp.uint8(255.0 - ((depth - depth_range[0]) / denom) * 205.0)
buffer[py, px, 0] = value
buffer[py, px, 1] = value
buffer[py, px, 2] = value
buffer[py, px, 3] = value
@wp.kernel(enable_backward=False)
def convert_ray_depth_to_forward_depth_kernel(
depth_image: wp.array4d[wp.float32],
camera_rays: wp.array4d[wp.vec3f],
camera_transforms: wp.array2d[wp.transformf],
out_depth: wp.array4d[wp.float32],
):
world_index, camera_index, py, px = wp.tid()
ray_depth = depth_image[world_index, camera_index, py, px]
camera_transform = camera_transforms[camera_index, world_index]
camera_ray = camera_rays[camera_index, py, px, 1]
ray_dir_world = wp.transform_vector(camera_transform, camera_ray)
cam_forward_world = wp.normalize(wp.transform_vector(camera_transform, wp.vec3f(0.0, 0.0, -1.0)))
out_depth[world_index, camera_index, py, px] = ray_depth * wp.dot(ray_dir_world, cam_forward_world)
class Utils:
"""Utility functions for the RenderContext."""
def __init__(self, render_context: RenderContext):
self.__render_context = render_context
def create_color_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create a color output array for :meth:`~SensorTiledCamera.update`.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``uint32``.
"""
return wp.zeros(
(self.__render_context.world_count, camera_count, height, width),
dtype=wp.uint32,
device=self.__render_context.device,
)
def create_depth_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.float32]:
"""Create a depth output array for :meth:`~SensorTiledCamera.update`.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``float32``.
"""
return wp.zeros(
(self.__render_context.world_count, camera_count, height, width),
dtype=wp.float32,
device=self.__render_context.device,
)
def create_shape_index_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create a shape-index output array for :meth:`~SensorTiledCamera.update`.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``uint32``.
"""
return wp.zeros(
(self.__render_context.world_count, camera_count, height, width),
dtype=wp.uint32,
device=self.__render_context.device,
)
def create_normal_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.vec3f]:
"""Create a normal output array for :meth:`~SensorTiledCamera.update`.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``vec3f``.
"""
return wp.zeros(
(self.__render_context.world_count, camera_count, height, width),
dtype=wp.vec3f,
device=self.__render_context.device,
)
def create_albedo_image_output(self, width: int, height: int, camera_count: int = 1) -> wp.array4d[wp.uint32]:
"""Create an albedo output array for :meth:`~SensorTiledCamera.update`.
Args:
width: Image width [px].
height: Image height [px].
camera_count: Number of cameras.
Returns:
Array of shape ``(world_count, camera_count, height, width)``, dtype ``uint32``.
"""
return wp.zeros(
(self.__render_context.world_count, camera_count, height, width),
dtype=wp.uint32,
device=self.__render_context.device,
)
def compute_pinhole_camera_rays(
self, width: int, height: int, camera_fovs: float | list[float] | np.ndarray | wp.array[wp.float32]
) -> wp.array4d[wp.vec3f]:
"""Compute camera-space ray directions for pinhole cameras.
Generates rays in camera space (origin at the camera center, direction normalized) for each pixel based on the
vertical field of view.
Args:
width: Image width [px].
height: Image height [px].
camera_fovs: Vertical FOV angles [rad], shape ``(camera_count,)``.
Returns:
camera_rays: Shape ``(camera_count, height, width, 2)``, dtype ``vec3f``.
"""
if isinstance(camera_fovs, float):
camera_fovs = wp.array([camera_fovs], dtype=wp.float32, device=self.__render_context.device)
elif isinstance(camera_fovs, list):
camera_fovs = wp.array(camera_fovs, dtype=wp.float32, device=self.__render_context.device)
elif isinstance(camera_fovs, np.ndarray):
camera_fovs = wp.array(camera_fovs, dtype=wp.float32, device=self.__render_context.device)
camera_count = camera_fovs.size
camera_rays = wp.empty((camera_count, height, width, 2), dtype=wp.vec3f, device=self.__render_context.device)
wp.launch(
kernel=compute_pinhole_camera_rays,
dim=(camera_count, height, width),
inputs=[
width,
height,
camera_fovs,
camera_rays,
],
device=self.__render_context.device,
)
return camera_rays
def convert_ray_depth_to_forward_depth(
self,
depth_image: wp.array4d[wp.float32],
camera_transforms: wp.array2d[wp.transformf],
camera_rays: wp.array4d[wp.vec3f],
out_depth: wp.array4d[wp.float32] | None = None,
) -> wp.array4d[wp.float32]:
"""Convert ray-distance depth to forward (planar) depth.
Projects each pixel's hit distance along its ray onto the camera's
forward axis, producing depth measured perpendicular to the image
plane. The forward axis is derived from each camera transform by
transforming camera-space ``(0, 0, -1)`` into world space.
Args:
depth_image: Ray-distance depth [m] from :meth:`update`, shape
``(world_count, camera_count, height, width)``.
camera_transforms: World-space camera transforms, shape
``(camera_count, world_count)``.
camera_rays: Camera-space rays from
:meth:`compute_pinhole_camera_rays`, shape
``(camera_count, height, width, 2)``.
out_depth: Output forward-depth array [m] with the same shape as
*depth_image*. If ``None``, allocates a new one.
Returns:
Forward (planar) depth array, same shape as *depth_image* [m].
"""
world_count = depth_image.shape[0]
camera_count = depth_image.shape[1]
height = depth_image.shape[2]
width = depth_image.shape[3]
if out_depth is None:
out_depth = wp.empty_like(depth_image, device=self.__render_context.device)
wp.launch(
kernel=convert_ray_depth_to_forward_depth_kernel,
dim=(world_count, camera_count, height, width),
inputs=[
depth_image,
camera_rays,
camera_transforms,
out_depth,
],
device=self.__render_context.device,
)
return out_depth
def flatten_color_image_to_rgba(
self,
image: wp.array4d[wp.uint32],
out_buffer: wp.array3d[wp.uint8] | None = None,
worlds_per_row: int | None = None,
) -> wp.array3d[wp.uint8]:
"""Flatten rendered color image to a tiled RGBA buffer.
Arranges ``(world_count * camera_count)`` tiles in a grid. Each tile shows one camera's view of one world.
Args:
image: Color output from :meth:`~SensorTiledCamera.update`, shape ``(world_count, camera_count, height, width)``.
out_buffer: Pre-allocated RGBA buffer. If None, allocates a new one.
worlds_per_row: Tiles per row in the grid. If None, picks a square-ish layout.
"""
camera_count = image.shape[1]
height = image.shape[2]
width = image.shape[3]
out_buffer, worlds_per_row = self.__reshape_buffer_for_flatten(
width, height, camera_count, out_buffer, worlds_per_row
)
wp.launch(
flatten_color_image,
(
self.__render_context.world_count,
camera_count,
height,
width,
),
[
image,
out_buffer,
width,
height,
camera_count,
worlds_per_row,
],
device=self.__render_context.device,
)
return out_buffer
def flatten_normal_image_to_rgba(
self,
image: wp.array4d[wp.vec3f],
out_buffer: wp.array3d[wp.uint8] | None = None,
worlds_per_row: int | None = None,
) -> wp.array3d[wp.uint8]:
"""Flatten rendered normal image to a tiled RGBA buffer.
Arranges ``(world_count * camera_count)`` tiles in a grid. Each tile shows one camera's view of one world.
Args:
image: Normal output from :meth:`~SensorTiledCamera.update`, shape ``(world_count, camera_count, height, width)``.
out_buffer: Pre-allocated RGBA buffer. If None, allocates a new one.
worlds_per_row: Tiles per row in the grid. If None, picks a square-ish layout.
"""
camera_count = image.shape[1]
height = image.shape[2]
width = image.shape[3]
out_buffer, worlds_per_row = self.__reshape_buffer_for_flatten(
width, height, camera_count, out_buffer, worlds_per_row
)
wp.launch(
flatten_normal_image,
(
self.__render_context.world_count,
camera_count,
height,
width,
),
[
image,
out_buffer,
width,
height,
camera_count,
worlds_per_row,
],
device=self.__render_context.device,
)
return out_buffer
def flatten_depth_image_to_rgba(
self,
image: wp.array4d[wp.float32],
out_buffer: wp.array3d[wp.uint8] | None = None,
worlds_per_row: int | None = None,
depth_range: wp.array[wp.float32] | None = None,
) -> wp.array3d[wp.uint8]:
"""Flatten rendered depth image to a tiled RGBA buffer.
Encodes depth as grayscale: inverts values (closer = brighter) and normalizes to the ``[50, 255]``
range. Background pixels (no hit) remain black.
Args:
image: Depth output from :meth:`~SensorTiledCamera.update`, shape ``(world_count, camera_count, height, width)``.
out_buffer: Pre-allocated RGBA buffer. If None, allocates a new one.
worlds_per_row: Tiles per row in the grid. If None, picks a square-ish layout.
depth_range: Depth range to normalize to, shape ``(2,)`` ``[near, far]``. If None, computes from *image*.
"""
camera_count = image.shape[1]
height = image.shape[2]
width = image.shape[3]
out_buffer, worlds_per_row = self.__reshape_buffer_for_flatten(
width, height, camera_count, out_buffer, worlds_per_row
)
if depth_range is None:
depth_range = wp.array([MAXVAL, 0.0], dtype=wp.float32, device=self.__render_context.device)
wp.launch(find_depth_range, image.shape, [image, depth_range], device=self.__render_context.device)
wp.launch(
flatten_depth_image,
(
self.__render_context.world_count,
camera_count,
height,
width,
),
[
image,
out_buffer,
depth_range,
width,
height,
camera_count,
worlds_per_row,
],
device=self.__render_context.device,
)
return out_buffer
def assign_random_colors_per_world(self, seed: int = 100):
"""Assign each world a random color, applied to all its shapes.
.. deprecated::
Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``).
Args:
seed: Random seed.
"""
warnings.warn(
"``SensorTiledCamera.utils.assign_random_colors_per_world`` is deprecated. Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``).",
category=DeprecationWarning,
stacklevel=2,
)
if not self.__render_context.shape_count_total:
return
colors = np.random.default_rng(seed).random((self.__render_context.shape_count_total, 3)) * 0.5 + 0.5
self.__render_context.shape_colors = wp.array(
colors[self.__render_context.shape_world_index.numpy() % len(colors)],
dtype=wp.vec3f,
device=self.__render_context.device,
)
def assign_random_colors_per_shape(self, seed: int = 100):
"""Assign a random color to each shape.
.. deprecated::
Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``).
Args:
seed: Random seed.
"""
warnings.warn(
"``SensorTiledCamera.utils.assign_random_colors_per_shape`` is deprecated. Use shape colors instead (e.g. ``builder.add_shape_cylinder(..., color=(r, g, b))``).",
category=DeprecationWarning,
stacklevel=2,
)
colors = np.random.default_rng(seed).random((self.__render_context.shape_count_total, 3)) * 0.5 + 0.5
self.__render_context.shape_colors = wp.array(colors, dtype=wp.vec3f, device=self.__render_context.device)
def create_default_light(self, enable_shadows: bool = True, direction: wp.vec3f | None = None):
"""Create a default directional light oriented at ``(-1, 1, -1)``.
Args:
enable_shadows: Enable shadow casting for this light.
direction: Normalized light direction. If ``None``, defaults to
(normalized ``(-1, 1, -1)``).
"""
self.__render_context.config.enable_shadows = enable_shadows
self.__render_context.lights_active = wp.array([True], dtype=wp.bool, device=self.__render_context.device)
self.__render_context.lights_type = wp.array(
[RenderLightType.DIRECTIONAL], dtype=wp.int32, device=self.__render_context.device
)
self.__render_context.lights_cast_shadow = wp.array([True], dtype=wp.bool, device=self.__render_context.device)
self.__render_context.lights_position = wp.array(
[wp.vec3f(0.0)], dtype=wp.vec3f, device=self.__render_context.device
)
self.__render_context.lights_orientation = wp.array(
[direction if direction is not None else wp.vec3f(-0.57735026, 0.57735026, -0.57735026)],
dtype=wp.vec3f,
device=self.__render_context.device,
)
def assign_checkerboard_material_to_all_shapes(self, resolution: int = 64, checker_size: int = 32):
"""Assign a gray checkerboard texture material to all shapes.
Creates a gray checkerboard pattern texture and applies it to all shapes
in the scene.
Args:
resolution: Texture resolution in pixels (square texture).
checker_size: Size of each checkerboard square in pixels.
"""
checkerboard = (
(np.arange(resolution) // checker_size)[:, None] + (np.arange(resolution) // checker_size)
) % 2 == 0
pixels = np.where(checkerboard, 0xFF808080, 0xFFBFBFBF).astype(np.uint32)
texture_ids = np.full(self.__render_context.shape_count_total, fill_value=0, dtype=np.int32)
self.__checkerboard_data = TextureData()
self.__checkerboard_data.texture = wp.Texture2D(
pixels.view(np.uint8).reshape(resolution, resolution, 4),
filter_mode=wp.TextureFilterMode.CLOSEST,
address_mode=wp.TextureAddressMode.WRAP,
normalized_coords=True,
dtype=wp.uint8,
num_channels=4,
device=self.__render_context.device,
)
self.__checkerboard_data.repeat = wp.vec2f(1.0, 1.0)
self.__render_context.config.enable_textures = True
self.__render_context.texture_data = wp.array(
[self.__checkerboard_data], dtype=TextureData, device=self.__render_context.device
)
self.__render_context.shape_texture_ids = wp.array(
texture_ids, dtype=wp.int32, device=self.__render_context.device
)
def __reshape_buffer_for_flatten(
self,
width: int,
height: int,
camera_count: int,
out_buffer: wp.array | None = None,
worlds_per_row: int | None = None,
) -> wp.array():
world_and_camera_count = self.__render_context.world_count * camera_count
if not worlds_per_row:
worlds_per_row = math.ceil(math.sqrt(world_and_camera_count))
worlds_per_col = math.ceil(world_and_camera_count / worlds_per_row)
if out_buffer is None:
return wp.empty(
(
worlds_per_col * height,
worlds_per_row * width,
4,
),
dtype=wp.uint8,
device=self.__render_context.device,
), worlds_per_row
return out_buffer.reshape((worlds_per_col * height, worlds_per_row * width, 4)), worlds_per_row
================================================
FILE: newton/_src/sim/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .articulation import eval_fk, eval_ik, eval_jacobian, eval_mass_matrix
from .builder import ModelBuilder
from .collide import CollisionPipeline
from .contacts import Contacts
from .control import Control
from .enums import (
BodyFlags,
EqType,
JointTargetMode,
JointType,
)
from .model import Model
from .state import State
__all__ = [
"BodyFlags",
"CollisionPipeline",
"Contacts",
"Control",
"EqType",
"JointTargetMode",
"JointType",
"Model",
"ModelBuilder",
"State",
"eval_fk",
"eval_ik",
"eval_jacobian",
"eval_mass_matrix",
]
================================================
FILE: newton/_src/sim/articulation.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warp as wp
from ..math import quat_decompose, transform_twist, velocity_at_point
from .enums import BodyFlags, JointType
from .model import Model
from .state import State
@wp.func
def compute_2d_rotational_dofs(
axis_0: wp.vec3,
axis_1: wp.vec3,
q0: float,
q1: float,
qd0: float,
qd1: float,
):
"""
Computes the rotation quaternion and 3D angular velocity given the joint axes, coordinates and velocities.
"""
q_off = wp.quat_from_matrix(wp.matrix_from_cols(axis_0, axis_1, wp.cross(axis_0, axis_1)))
# body local axes
local_0 = wp.quat_rotate(q_off, wp.vec3(1.0, 0.0, 0.0))
local_1 = wp.quat_rotate(q_off, wp.vec3(0.0, 1.0, 0.0))
axis_0 = local_0
q_0 = wp.quat_from_axis_angle(axis_0, q0)
axis_1 = wp.quat_rotate(q_0, local_1)
q_1 = wp.quat_from_axis_angle(axis_1, q1)
rot = q_1 * q_0
vel = axis_0 * qd0 + axis_1 * qd1
return rot, vel
@wp.func
def invert_2d_rotational_dofs(
axis_0: wp.vec3,
axis_1: wp.vec3,
q_p: wp.quat,
q_c: wp.quat,
w_err: wp.vec3,
):
"""
Computes generalized joint position and velocity coordinates for a 2D rotational joint given the joint axes, relative orientations and angular velocity differences between the two bodies the joint connects.
"""
q_off = wp.quat_from_matrix(wp.matrix_from_cols(axis_0, axis_1, wp.cross(axis_0, axis_1)))
q_pc = wp.quat_inverse(q_off) * wp.quat_inverse(q_p) * q_c * q_off
# decompose to a compound rotation each axis
angles = quat_decompose(q_pc)
# find rotation axes
local_0 = wp.quat_rotate(q_off, wp.vec3(1.0, 0.0, 0.0))
local_1 = wp.quat_rotate(q_off, wp.vec3(0.0, 1.0, 0.0))
local_2 = wp.quat_rotate(q_off, wp.vec3(0.0, 0.0, 1.0))
axis_0 = local_0
q_0 = wp.quat_from_axis_angle(axis_0, angles[0])
axis_1 = wp.quat_rotate(q_0, local_1)
q_1 = wp.quat_from_axis_angle(axis_1, angles[1])
axis_2 = wp.quat_rotate(q_1 * q_0, local_2)
# convert angular velocity to local space
w_err_p = wp.quat_rotate_inv(q_p, w_err)
# given joint axes and angular velocity error, solve for joint velocities
c12 = wp.cross(axis_1, axis_2)
c02 = wp.cross(axis_0, axis_2)
vel = wp.vec2(wp.dot(w_err_p, c12) / wp.dot(axis_0, c12), wp.dot(w_err_p, c02) / wp.dot(axis_1, c02))
return wp.vec2(angles[0], angles[1]), vel
@wp.func
def compute_3d_rotational_dofs(
axis_0: wp.vec3,
axis_1: wp.vec3,
axis_2: wp.vec3,
q0: float,
q1: float,
q2: float,
qd0: float,
qd1: float,
qd2: float,
):
"""
Computes the rotation quaternion and 3D angular velocity given the joint axes, coordinates and velocities.
"""
q_off = wp.quat_from_matrix(wp.matrix_from_cols(axis_0, axis_1, axis_2))
# body local axes
local_0 = wp.quat_rotate(q_off, wp.vec3(1.0, 0.0, 0.0))
local_1 = wp.quat_rotate(q_off, wp.vec3(0.0, 1.0, 0.0))
local_2 = wp.quat_rotate(q_off, wp.vec3(0.0, 0.0, 1.0))
# reconstruct rotation axes
axis_0 = local_0
q_0 = wp.quat_from_axis_angle(axis_0, q0)
axis_1 = wp.quat_rotate(q_0, local_1)
q_1 = wp.quat_from_axis_angle(axis_1, q1)
axis_2 = wp.quat_rotate(q_1 * q_0, local_2)
q_2 = wp.quat_from_axis_angle(axis_2, q2)
rot = q_2 * q_1 * q_0
vel = axis_0 * qd0 + axis_1 * qd1 + axis_2 * qd2
return rot, vel
@wp.func
def invert_3d_rotational_dofs(
axis_0: wp.vec3, axis_1: wp.vec3, axis_2: wp.vec3, q_p: wp.quat, q_c: wp.quat, w_err: wp.vec3
):
"""
Computes generalized joint position and velocity coordinates for a 3D rotational joint given the joint axes, relative orientations and angular velocity differences between the two bodies the joint connects.
"""
q_off = wp.quat_from_matrix(wp.matrix_from_cols(axis_0, axis_1, axis_2))
q_pc = wp.quat_inverse(q_off) * wp.quat_inverse(q_p) * q_c * q_off
# decompose to a compound rotation each axis
angles = quat_decompose(q_pc)
# find rotation axes
local_0 = wp.quat_rotate(q_off, wp.vec3(1.0, 0.0, 0.0))
local_1 = wp.quat_rotate(q_off, wp.vec3(0.0, 1.0, 0.0))
local_2 = wp.quat_rotate(q_off, wp.vec3(0.0, 0.0, 1.0))
axis_0 = local_0
q_0 = wp.quat_from_axis_angle(axis_0, angles[0])
axis_1 = wp.quat_rotate(q_0, local_1)
q_1 = wp.quat_from_axis_angle(axis_1, angles[1])
axis_2 = wp.quat_rotate(q_1 * q_0, local_2)
# convert angular velocity to local space
w_err_p = wp.quat_rotate_inv(q_p, w_err)
# given joint axes and angular velocity error, solve for joint velocities
c12 = wp.cross(axis_1, axis_2)
c02 = wp.cross(axis_0, axis_2)
c01 = wp.cross(axis_0, axis_1)
velocities = wp.vec3(
wp.dot(w_err_p, c12) / wp.dot(axis_0, c12),
wp.dot(w_err_p, c02) / wp.dot(axis_1, c02),
wp.dot(w_err_p, c01) / wp.dot(axis_2, c01),
)
return angles, velocities
@wp.func
def eval_single_articulation_fk(
joint_start: int,
joint_end: int,
joint_articulation: wp.array[int],
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_com: wp.array[wp.vec3],
body_flags: wp.array[wp.int32],
body_flag_filter: int,
# outputs
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
):
for i in range(joint_start, joint_end):
articulation = joint_articulation[i]
if articulation == -1:
continue
parent = joint_parent[i]
child = joint_child[i]
# compute transform across the joint
type = joint_type[i]
X_pj = joint_X_p[i]
X_cj = joint_X_c[i]
q_start = joint_q_start[i]
qd_start = joint_qd_start[i]
lin_axis_count = joint_dof_dim[i, 0]
ang_axis_count = joint_dof_dim[i, 1]
X_j = wp.transform_identity()
v_j = wp.spatial_vector(wp.vec3(), wp.vec3())
if type == JointType.PRISMATIC:
axis = joint_axis[qd_start]
q = joint_q[q_start]
qd = joint_qd[qd_start]
X_j = wp.transform(axis * q, wp.quat_identity())
v_j = wp.spatial_vector(axis * qd, wp.vec3())
if type == JointType.REVOLUTE:
axis = joint_axis[qd_start]
q = joint_q[q_start]
qd = joint_qd[qd_start]
X_j = wp.transform(wp.vec3(), wp.quat_from_axis_angle(axis, q))
v_j = wp.spatial_vector(wp.vec3(), axis * qd)
if type == JointType.BALL:
r = wp.quat(joint_q[q_start + 0], joint_q[q_start + 1], joint_q[q_start + 2], joint_q[q_start + 3])
w = wp.vec3(joint_qd[qd_start + 0], joint_qd[qd_start + 1], joint_qd[qd_start + 2])
X_j = wp.transform(wp.vec3(), r)
v_j = wp.spatial_vector(wp.vec3(), w)
if type == JointType.FREE or type == JointType.DISTANCE:
t = wp.transform(
wp.vec3(joint_q[q_start + 0], joint_q[q_start + 1], joint_q[q_start + 2]),
wp.quat(joint_q[q_start + 3], joint_q[q_start + 4], joint_q[q_start + 5], joint_q[q_start + 6]),
)
v = wp.spatial_vector(
wp.vec3(joint_qd[qd_start + 0], joint_qd[qd_start + 1], joint_qd[qd_start + 2]),
wp.vec3(joint_qd[qd_start + 3], joint_qd[qd_start + 4], joint_qd[qd_start + 5]),
)
X_j = t
v_j = v
if type == JointType.D6:
pos = wp.vec3(0.0)
rot = wp.quat_identity()
vel_v = wp.vec3(0.0)
vel_w = wp.vec3(0.0)
# unroll for loop to ensure joint actions remain differentiable
# (since differentiating through a for loop that updates a local variable is not supported)
if lin_axis_count > 0:
axis = joint_axis[qd_start + 0]
pos += axis * joint_q[q_start + 0]
vel_v += axis * joint_qd[qd_start + 0]
if lin_axis_count > 1:
axis = joint_axis[qd_start + 1]
pos += axis * joint_q[q_start + 1]
vel_v += axis * joint_qd[qd_start + 1]
if lin_axis_count > 2:
axis = joint_axis[qd_start + 2]
pos += axis * joint_q[q_start + 2]
vel_v += axis * joint_qd[qd_start + 2]
iq = q_start + lin_axis_count
iqd = qd_start + lin_axis_count
if ang_axis_count == 1:
axis = joint_axis[iqd]
rot = wp.quat_from_axis_angle(axis, joint_q[iq])
vel_w = joint_qd[iqd] * axis
if ang_axis_count == 2:
rot, vel_w = compute_2d_rotational_dofs(
joint_axis[iqd + 0],
joint_axis[iqd + 1],
joint_q[iq + 0],
joint_q[iq + 1],
joint_qd[iqd + 0],
joint_qd[iqd + 1],
)
if ang_axis_count == 3:
rot, vel_w = compute_3d_rotational_dofs(
joint_axis[iqd + 0],
joint_axis[iqd + 1],
joint_axis[iqd + 2],
joint_q[iq + 0],
joint_q[iq + 1],
joint_q[iq + 2],
joint_qd[iqd + 0],
joint_qd[iqd + 1],
joint_qd[iqd + 2],
)
X_j = wp.transform(pos, rot)
v_j = wp.spatial_vector(vel_v, vel_w)
# transform from world to parent joint anchor frame
X_wpj = X_pj
if parent >= 0:
X_wp = body_q[parent]
X_wpj = X_wp * X_wpj
# transform from world to joint anchor frame at child body
X_wcj = X_wpj * X_j
# transform from world to child body frame
X_wc = X_wcj * wp.transform_inverse(X_cj)
# Velocity must be evaluated at the actual child-body origin. For translated
# joints, sampling parent motion only at the fixed
# parent anchor misses the transport term from the current joint displacement.
v_parent_origin = wp.vec3()
w_parent = wp.vec3()
if parent >= 0:
v_wp = body_qd[parent]
w_parent = wp.spatial_bottom(v_wp)
v_parent_origin = velocity_at_point(
v_wp, wp.transform_get_translation(X_wc) - wp.transform_get_translation(X_wp)
)
# Transform joint motion into world space. The linear part of v_j is defined
# at the child joint anchor; if the child body origin is offset from that
# anchor, transport the joint angular motion to the body origin.
linear_joint_anchor = wp.transform_vector(X_wpj, wp.spatial_top(v_j))
angular_joint_world = wp.transform_vector(X_wpj, wp.spatial_bottom(v_j))
child_origin_offset_world = wp.transform_get_translation(X_wc) - wp.transform_get_translation(X_wcj)
linear_joint_origin = linear_joint_anchor + wp.cross(angular_joint_world, child_origin_offset_world)
v_wc = wp.spatial_vector(v_parent_origin + linear_joint_origin, w_parent + angular_joint_world)
if (body_flags[child] & body_flag_filter) != 0:
body_q[child] = X_wc
body_qd[child] = v_wc
@wp.kernel
def eval_articulation_fk(
articulation_start: wp.array[int],
articulation_count: int, # total number of articulations
articulation_mask: wp.array[
bool
], # used to enable / disable FK for an articulation, if None then treat all as enabled
articulation_indices: wp.array[int], # can be None, articulation indices to process
joint_articulation: wp.array[int],
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_com: wp.array[wp.vec3],
body_flags: wp.array[wp.int32],
body_flag_filter: int,
# outputs
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
):
tid = wp.tid()
# Determine which articulation to process
if articulation_indices:
# Using indices - get actual articulation ID from array
articulation_id = articulation_indices[tid]
else:
# No indices - articulation ID is just the thread index
articulation_id = tid
# Bounds check
if articulation_id < 0 or articulation_id >= articulation_count:
return # Invalid articulation index
# early out if disabling FK for this articulation
if articulation_mask:
if not articulation_mask[articulation_id]:
return
joint_start = articulation_start[articulation_id]
joint_end = articulation_start[articulation_id + 1]
eval_single_articulation_fk(
joint_start,
joint_end,
joint_articulation,
joint_q,
joint_qd,
joint_q_start,
joint_qd_start,
joint_type,
joint_parent,
joint_child,
joint_X_p,
joint_X_c,
joint_axis,
joint_dof_dim,
body_com,
body_flags,
body_flag_filter,
# outputs
body_q,
body_qd,
)
def eval_fk(
model: Model,
joint_q: wp.array[float],
joint_qd: wp.array[float],
state: State | Model | object,
mask: wp.array[bool] | None = None,
indices: wp.array[int] | None = None,
body_flag_filter: int = BodyFlags.ALL,
):
"""
Evaluates the model's forward kinematics given the joint coordinates and updates the state's body information (:attr:`State.body_q` and :attr:`State.body_qd`).
Args:
model: The model to evaluate.
joint_q: Generalized joint position coordinates, shape [joint_coord_count], float
joint_qd: Generalized joint velocity coordinates, shape [joint_dof_count], float
state: The state-like target to update (e.g., :class:`State` or :class:`Model`).
mask: The mask to use to enable / disable FK for an articulation. If None then treat all as enabled, shape [articulation_count], bool
indices: Integer indices of articulations to update. If None, updates all articulations.
Cannot be used together with mask parameter.
body_flag_filter: Body flag filter controlling which bodies are written to in ``state.body_q`` and
``state.body_qd``. Default updates both dynamic and kinematic bodies. Bodies that do not
match the filter retain their existing values; they are not zeroed or invalidated.
"""
# Validate inputs
if mask is not None and indices is not None:
raise ValueError("Cannot specify both mask and indices parameters")
# Determine launch dimensions
if indices is not None:
num_articulations = len(indices)
else:
num_articulations = model.articulation_count
wp.launch(
kernel=eval_articulation_fk,
dim=num_articulations,
inputs=[
model.articulation_start,
model.articulation_count,
mask,
indices,
model.joint_articulation,
joint_q,
joint_qd,
model.joint_q_start,
model.joint_qd_start,
model.joint_type,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_axis,
model.joint_dof_dim,
model.body_com,
model.body_flags,
body_flag_filter,
],
outputs=[
state.body_q,
state.body_qd,
],
device=model.device,
)
@wp.kernel
def compute_shape_world_transforms(
shape_transform: wp.array[wp.transform],
shape_body: wp.array[int],
body_q: wp.array[wp.transform],
# outputs
shape_world_transform: wp.array[wp.transform],
):
"""Compute world-space transforms for shapes by concatenating local shape
transforms with body transforms.
Args:
shape_transform: Local shape transforms in body frame,
shape [shape_count, 7]
shape_body: Body index for each shape, shape [shape_count]
body_q: Body transforms in world frame, shape [body_count, 7]
shape_world_transform: Output world transforms for shapes,
shape [shape_count, 7]
"""
shape_idx = wp.tid()
# Get the local shape transform
X_bs = shape_transform[shape_idx]
# Get the body index for this shape
body_idx = shape_body[shape_idx]
# If shape is attached to a body (body_idx >= 0), concatenate transforms
if body_idx >= 0:
# Get the body transform in world space
X_wb = body_q[body_idx]
# Concatenate: world_transform = body_transform * shape_transform
X_ws = wp.transform_multiply(X_wb, X_bs)
shape_world_transform[shape_idx] = X_ws
else:
# Shape is not attached to a body (static shape), use local
# transform as world transform
shape_world_transform[shape_idx] = X_bs
@wp.func
def reconstruct_angular_q_qd(q_pc: wp.quat, w_err: wp.vec3, X_wp: wp.transform, axis: wp.vec3):
"""
Reconstructs the angular joint coordinates and velocities given the relative rotation and angular velocity
between a parent and child body.
Args:
q_pc (quat): The relative rotation between the parent and child body.
w_err (vec3): The angular velocity between the parent and child body.
X_wp (transform): The transform from the parent body frame to the joint parent anchor frame.
axis (vec3): The joint axis in the joint parent anchor frame.
Returns:
q (float): The joint position coordinate.
qd (float): The joint velocity coordinate.
"""
axis_p = wp.transform_vector(X_wp, axis)
twist = wp.quat_twist(axis, q_pc)
q = wp.acos(twist[3]) * 2.0 * wp.sign(wp.dot(axis, wp.vec3(twist[0], twist[1], twist[2])))
qd = wp.dot(w_err, axis_p)
return q, qd
@wp.kernel
def eval_articulation_ik(
articulation_start: wp.array[int],
articulation_count: int, # total number of articulations
articulation_mask: wp.array[bool], # can be None, mask to filter articulations
articulation_indices: wp.array[int], # can be None, articulation indices to process
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
body_flags: wp.array[wp.int32],
body_flag_filter: int,
joint_q: wp.array[float],
joint_qd: wp.array[float],
):
art_idx, joint_offset = wp.tid() # articulation index and joint offset within articulation
# Determine which articulation to process
if articulation_indices:
articulation_id = articulation_indices[art_idx]
else:
articulation_id = art_idx
# Bounds check
if articulation_id < 0 or articulation_id >= articulation_count:
return # Invalid articulation index
# early out if disabling IK for this articulation
if articulation_mask:
if not articulation_mask[articulation_id]:
return
# Get joint range for this articulation
joint_start = articulation_start[articulation_id]
joint_end = articulation_start[articulation_id + 1]
# Check if this thread has a valid joint to process
joint_idx = joint_start + joint_offset
if joint_idx >= joint_end:
return # This thread has no joint (padding thread)
parent = joint_parent[joint_idx]
child = joint_child[joint_idx]
if (body_flags[child] & body_flag_filter) == 0:
return
X_pj = joint_X_p[joint_idx]
X_cj = joint_X_c[joint_idx]
w_p = wp.vec3()
v_p = wp.vec3()
v_wp = wp.spatial_vector()
# parent anchor frame in world space
X_wpj = X_pj
if parent >= 0:
X_wp = body_q[parent]
X_wpj = X_wp * X_pj
v_wp = body_qd[parent]
w_p = wp.spatial_bottom(v_wp)
v_p = velocity_at_point(v_wp, wp.transform_get_translation(X_wpj) - wp.transform_get_translation(X_wp))
# child transform and moment arm
X_wc = body_q[child]
X_wcj = X_wc * X_cj
v_wc = body_qd[child]
w_c = wp.spatial_bottom(v_wc)
v_c = velocity_at_point(v_wc, wp.transform_get_translation(X_wcj) - wp.transform_get_translation(X_wc))
# joint properties
type = joint_type[joint_idx]
# compute position and orientation differences between anchor frames
x_p = wp.transform_get_translation(X_wpj)
x_c = wp.transform_get_translation(X_wcj)
q_p = wp.transform_get_rotation(X_wpj)
q_c = wp.transform_get_rotation(X_wcj)
x_err = x_c - x_p
v_err = v_c - v_p
w_err = w_c - w_p
q_start = joint_q_start[joint_idx]
qd_start = joint_qd_start[joint_idx]
lin_axis_count = joint_dof_dim[joint_idx, 0]
ang_axis_count = joint_dof_dim[joint_idx, 1]
if type == JointType.PRISMATIC:
axis = joint_axis[qd_start]
# world space joint axis
axis_p = wp.quat_rotate(q_p, axis)
# evaluate joint coordinates
q = wp.dot(x_err, axis_p)
qd = wp.dot(v_err, axis_p)
joint_q[q_start] = q
joint_qd[qd_start] = qd
return
if type == JointType.REVOLUTE:
axis = joint_axis[qd_start]
q_pc = wp.quat_inverse(q_p) * q_c
q, qd = reconstruct_angular_q_qd(q_pc, w_err, X_wpj, axis)
joint_q[q_start] = q
joint_qd[qd_start] = qd
return
if type == JointType.BALL:
q_pc = wp.quat_inverse(q_p) * q_c
joint_q[q_start + 0] = q_pc[0]
joint_q[q_start + 1] = q_pc[1]
joint_q[q_start + 2] = q_pc[2]
joint_q[q_start + 3] = q_pc[3]
ang_vel = wp.transform_vector(wp.transform_inverse(X_wpj), w_err)
joint_qd[qd_start + 0] = ang_vel[0]
joint_qd[qd_start + 1] = ang_vel[1]
joint_qd[qd_start + 2] = ang_vel[2]
return
if type == JointType.FIXED:
return
if type == JointType.FREE or type == JointType.DISTANCE:
q_pc = wp.quat_inverse(q_p) * q_c
x_err_c = wp.quat_rotate_inv(q_p, x_err)
v_err_c = wp.quat_rotate_inv(q_p, v_err)
w_err_c = wp.quat_rotate_inv(q_p, w_err)
joint_q[q_start + 0] = x_err_c[0]
joint_q[q_start + 1] = x_err_c[1]
joint_q[q_start + 2] = x_err_c[2]
joint_q[q_start + 3] = q_pc[0]
joint_q[q_start + 4] = q_pc[1]
joint_q[q_start + 5] = q_pc[2]
joint_q[q_start + 6] = q_pc[3]
joint_qd[qd_start + 0] = v_err_c[0]
joint_qd[qd_start + 1] = v_err_c[1]
joint_qd[qd_start + 2] = v_err_c[2]
joint_qd[qd_start + 3] = w_err_c[0]
joint_qd[qd_start + 4] = w_err_c[1]
joint_qd[qd_start + 5] = w_err_c[2]
return
if type == JointType.D6:
x_err_c = wp.quat_rotate_inv(q_p, x_err)
v_err_c = wp.quat_rotate_inv(q_p, v_err)
if lin_axis_count > 0:
axis = joint_axis[qd_start + 0]
joint_q[q_start + 0] = wp.dot(x_err_c, axis)
joint_qd[qd_start + 0] = wp.dot(v_err_c, axis)
if lin_axis_count > 1:
axis = joint_axis[qd_start + 1]
joint_q[q_start + 1] = wp.dot(x_err_c, axis)
joint_qd[qd_start + 1] = wp.dot(v_err_c, axis)
if lin_axis_count > 2:
axis = joint_axis[qd_start + 2]
joint_q[q_start + 2] = wp.dot(x_err_c, axis)
joint_qd[qd_start + 2] = wp.dot(v_err_c, axis)
if ang_axis_count == 1:
axis = joint_axis[qd_start]
q_pc = wp.quat_inverse(q_p) * q_c
q, qd = reconstruct_angular_q_qd(q_pc, w_err, X_wpj, joint_axis[qd_start + lin_axis_count])
joint_q[q_start + lin_axis_count] = q
joint_qd[qd_start + lin_axis_count] = qd
if ang_axis_count == 2:
axis_0 = joint_axis[qd_start + lin_axis_count + 0]
axis_1 = joint_axis[qd_start + lin_axis_count + 1]
qs2, qds2 = invert_2d_rotational_dofs(axis_0, axis_1, q_p, q_c, w_err)
joint_q[q_start + lin_axis_count + 0] = qs2[0]
joint_q[q_start + lin_axis_count + 1] = qs2[1]
joint_qd[qd_start + lin_axis_count + 0] = qds2[0]
joint_qd[qd_start + lin_axis_count + 1] = qds2[1]
if ang_axis_count == 3:
axis_0 = joint_axis[qd_start + lin_axis_count + 0]
axis_1 = joint_axis[qd_start + lin_axis_count + 1]
axis_2 = joint_axis[qd_start + lin_axis_count + 2]
qs3, qds3 = invert_3d_rotational_dofs(axis_0, axis_1, axis_2, q_p, q_c, w_err)
joint_q[q_start + lin_axis_count + 0] = qs3[0]
joint_q[q_start + lin_axis_count + 1] = qs3[1]
joint_q[q_start + lin_axis_count + 2] = qs3[2]
joint_qd[qd_start + lin_axis_count + 0] = qds3[0]
joint_qd[qd_start + lin_axis_count + 1] = qds3[1]
joint_qd[qd_start + lin_axis_count + 2] = qds3[2]
return
# given maximal coordinate model computes ik (closest point projection)
def eval_ik(
model: Model,
state: State | Model | object,
joint_q: wp.array[float],
joint_qd: wp.array[float],
mask: wp.array[bool] | None = None,
indices: wp.array[int] | None = None,
body_flag_filter: int = BodyFlags.ALL,
):
"""
Evaluates the model's inverse kinematics given the state's body information (:attr:`State.body_q` and :attr:`State.body_qd`) and updates the generalized joint coordinates `joint_q` and `joint_qd`.
Args:
model: The model to evaluate.
state: The state-like object with the body's maximal coordinates (positions :attr:`State.body_q` and velocities :attr:`State.body_qd`) to use.
joint_q: Generalized joint position coordinates, shape [joint_coord_count], float
joint_qd: Generalized joint velocity coordinates, shape [joint_dof_count], float
mask: Boolean mask indicating which articulations to update. If None, updates all (or those specified by indices).
indices: Integer indices of articulations to update. If None, updates all articulations.
body_flag_filter: Body flag filter controlling which joints are written based on each joint's child
body flag. Default updates joints for both dynamic and kinematic child bodies. Entries that
do not match the filter retain their existing values in ``joint_q`` and ``joint_qd``.
Note:
The mask and indices parameters are mutually exclusive. If both are provided, a ValueError is raised.
"""
# Check for mutually exclusive parameters
if mask is not None and indices is not None:
raise ValueError("mask and indices parameters are mutually exclusive - please use only one")
# Determine launch dimensions
if indices is not None:
num_articulations = len(indices)
else:
num_articulations = model.articulation_count
# Always use 2D launch for joint-level parallelism
wp.launch(
kernel=eval_articulation_ik,
dim=(num_articulations, model.max_joints_per_articulation),
inputs=[
model.articulation_start,
model.articulation_count,
mask,
indices,
state.body_q,
state.body_qd,
model.body_com,
model.joint_type,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_axis,
model.joint_dof_dim,
model.joint_q_start,
model.joint_qd_start,
model.body_flags,
body_flag_filter,
],
outputs=[joint_q, joint_qd],
device=model.device,
)
@wp.func
def jcalc_motion_subspace(
type: int,
joint_axis: wp.array[wp.vec3],
lin_axis_count: int,
ang_axis_count: int,
X_sc: wp.transform,
qd_start: int,
# outputs
joint_S_s: wp.array[wp.spatial_vector],
):
"""Compute motion subspace (joint Jacobian columns) for a joint.
This populates joint_S_s with the motion subspace vectors for each DoF,
which represent how each joint coordinate affects the spatial velocity.
Note:
CABLE joints are not currently supported. CABLE joints have complex,
configuration-dependent motion subspaces (dynamic stretch direction and
isotropic angular DOF) and are primarily designed for VBD solver.
If encountered, their Jacobian columns will remain zero.
"""
if type == JointType.PRISMATIC:
axis = joint_axis[qd_start]
S_s = transform_twist(X_sc, wp.spatial_vector(axis, wp.vec3()))
joint_S_s[qd_start] = S_s
elif type == JointType.REVOLUTE:
axis = joint_axis[qd_start]
S_s = transform_twist(X_sc, wp.spatial_vector(wp.vec3(), axis))
joint_S_s[qd_start] = S_s
elif type == JointType.D6:
if lin_axis_count > 0:
axis = joint_axis[qd_start + 0]
S_s = transform_twist(X_sc, wp.spatial_vector(axis, wp.vec3()))
joint_S_s[qd_start + 0] = S_s
if lin_axis_count > 1:
axis = joint_axis[qd_start + 1]
S_s = transform_twist(X_sc, wp.spatial_vector(axis, wp.vec3()))
joint_S_s[qd_start + 1] = S_s
if lin_axis_count > 2:
axis = joint_axis[qd_start + 2]
S_s = transform_twist(X_sc, wp.spatial_vector(axis, wp.vec3()))
joint_S_s[qd_start + 2] = S_s
if ang_axis_count > 0:
axis = joint_axis[qd_start + lin_axis_count + 0]
S_s = transform_twist(X_sc, wp.spatial_vector(wp.vec3(), axis))
joint_S_s[qd_start + lin_axis_count + 0] = S_s
if ang_axis_count > 1:
axis = joint_axis[qd_start + lin_axis_count + 1]
S_s = transform_twist(X_sc, wp.spatial_vector(wp.vec3(), axis))
joint_S_s[qd_start + lin_axis_count + 1] = S_s
if ang_axis_count > 2:
axis = joint_axis[qd_start + lin_axis_count + 2]
S_s = transform_twist(X_sc, wp.spatial_vector(wp.vec3(), axis))
joint_S_s[qd_start + lin_axis_count + 2] = S_s
elif type == JointType.BALL:
S_0 = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 1.0, 0.0, 0.0))
S_1 = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 1.0, 0.0))
S_2 = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 1.0))
joint_S_s[qd_start + 0] = S_0
joint_S_s[qd_start + 1] = S_1
joint_S_s[qd_start + 2] = S_2
elif type == JointType.FREE or type == JointType.DISTANCE:
joint_S_s[qd_start + 0] = transform_twist(X_sc, wp.spatial_vector(1.0, 0.0, 0.0, 0.0, 0.0, 0.0))
joint_S_s[qd_start + 1] = transform_twist(X_sc, wp.spatial_vector(0.0, 1.0, 0.0, 0.0, 0.0, 0.0))
joint_S_s[qd_start + 2] = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 1.0, 0.0, 0.0, 0.0))
joint_S_s[qd_start + 3] = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 1.0, 0.0, 0.0))
joint_S_s[qd_start + 4] = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 1.0, 0.0))
joint_S_s[qd_start + 5] = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 1.0))
@wp.kernel
def eval_articulation_jacobian(
articulation_start: wp.array[int],
articulation_count: int,
articulation_mask: wp.array[bool],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_ancestor: wp.array[int],
joint_qd_start: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_q: wp.array[wp.transform],
# outputs
J: wp.array3d[float],
joint_S_s: wp.array[wp.spatial_vector],
):
"""Compute spatial Jacobian for articulations.
The Jacobian J maps joint velocities to spatial velocities of each link.
Output shape: (articulation_count, max_links*6, max_dofs)
"""
art_idx = wp.tid()
if art_idx >= articulation_count:
return
if articulation_mask:
if not articulation_mask[art_idx]:
return
joint_start = articulation_start[art_idx]
joint_end = articulation_start[art_idx + 1]
joint_count = joint_end - joint_start
articulation_dof_start = joint_qd_start[joint_start]
# First pass: compute body transforms and motion subspaces
for i in range(joint_count):
j = joint_start + i
parent = joint_parent[j]
type = joint_type[j]
X_pj = joint_X_p[j]
# parent anchor frame in world space
X_wpj = X_pj
if parent >= 0:
X_wp = body_q[parent]
X_wpj = X_wp * X_pj
qd_start = joint_qd_start[j]
lin_axis_count = joint_dof_dim[j, 0]
ang_axis_count = joint_dof_dim[j, 1]
# compute motion subspace in world frame
jcalc_motion_subspace(
type,
joint_axis,
lin_axis_count,
ang_axis_count,
X_wpj,
qd_start,
joint_S_s,
)
# Second pass: build Jacobian by walking kinematic chain
for i in range(joint_count):
row_start = i * 6
j = joint_start + i
while j != -1:
joint_dof_start = joint_qd_start[j]
joint_dof_end = joint_qd_start[j + 1]
joint_dof_count = joint_dof_end - joint_dof_start
# Fill out each row of the Jacobian walking up the tree
for dof in range(joint_dof_count):
col = (joint_dof_start - articulation_dof_start) + dof
S = joint_S_s[joint_dof_start + dof]
for k in range(6):
J[art_idx, row_start + k, col] = S[k]
j = joint_ancestor[j]
def eval_jacobian(
model: Model,
state: State,
J: wp.array | None = None,
joint_S_s: wp.array | None = None,
mask: wp.array | None = None,
) -> wp.array | None:
"""Evaluate spatial Jacobian for articulations.
Computes the spatial Jacobian J that maps joint velocities to spatial
velocities of each link in world frame. The Jacobian is computed for
each articulation in the model.
Args:
model: The model containing articulation definitions.
state: The state containing body transforms (body_q).
J: Optional output array for the Jacobian, shape (articulation_count, max_links*6, max_dofs).
If None, allocates internally.
joint_S_s: Optional pre-allocated temp array for motion subspaces,
shape (joint_dof_count,), dtype wp.spatial_vector.
If None, allocates internally.
mask: Optional boolean mask to select which articulations to compute.
Shape [articulation_count]. If None, computes for all articulations.
Returns:
The Jacobian array J, or None if the model has no articulations.
"""
if model.articulation_count == 0:
return None
# Allocate output if not provided
if J is None:
max_links = model.max_joints_per_articulation
max_dofs = model.max_dofs_per_articulation
J = wp.empty(
(model.articulation_count, max_links * 6, max_dofs),
dtype=float,
device=model.device,
)
# Zero the output buffer
J.zero_()
# Allocate temp if not provided
if joint_S_s is None:
joint_S_s = wp.zeros(
model.joint_dof_count,
dtype=wp.spatial_vector,
device=model.device,
)
wp.launch(
kernel=eval_articulation_jacobian,
dim=model.articulation_count,
inputs=[
model.articulation_start,
model.articulation_count,
mask,
model.joint_type,
model.joint_parent,
model.joint_ancestor,
model.joint_qd_start,
model.joint_X_p,
model.joint_axis,
model.joint_dof_dim,
state.body_q,
],
outputs=[J, joint_S_s],
device=model.device,
)
return J
@wp.func
def transform_spatial_inertia(t: wp.transform, I: wp.spatial_matrix):
"""Transform a spatial inertia tensor to a new coordinate frame.
Note: This is duplicated from featherstone/kernels.py to avoid circular imports.
"""
t_inv = wp.transform_inverse(t)
q = wp.transform_get_rotation(t_inv)
p = wp.transform_get_translation(t_inv)
r1 = wp.quat_rotate(q, wp.vec3(1.0, 0.0, 0.0))
r2 = wp.quat_rotate(q, wp.vec3(0.0, 1.0, 0.0))
r3 = wp.quat_rotate(q, wp.vec3(0.0, 0.0, 1.0))
R = wp.matrix_from_cols(r1, r2, r3)
S = wp.skew(p) @ R
# fmt: off
T = wp.spatial_matrix(
R[0, 0], R[0, 1], R[0, 2], S[0, 0], S[0, 1], S[0, 2],
R[1, 0], R[1, 1], R[1, 2], S[1, 0], S[1, 1], S[1, 2],
R[2, 0], R[2, 1], R[2, 2], S[2, 0], S[2, 1], S[2, 2],
0.0, 0.0, 0.0, R[0, 0], R[0, 1], R[0, 2],
0.0, 0.0, 0.0, R[1, 0], R[1, 1], R[1, 2],
0.0, 0.0, 0.0, R[2, 0], R[2, 1], R[2, 2],
)
# fmt: on
return wp.mul(wp.mul(wp.transpose(T), I), T)
@wp.kernel
def compute_body_spatial_inertia(
body_inertia: wp.array[wp.mat33],
body_mass: wp.array[float],
body_com: wp.array[wp.vec3],
body_q: wp.array[wp.transform],
# outputs
body_I_s: wp.array[wp.spatial_matrix],
):
"""Compute spatial inertia for each body in world frame."""
tid = wp.tid()
I_local = body_inertia[tid]
m = body_mass[tid]
com = body_com[tid]
X_wb = body_q[tid]
# Build spatial inertia in body COM frame
# fmt: off
I_m = wp.spatial_matrix(
m, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, m, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, m, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, I_local[0, 0], I_local[0, 1], I_local[0, 2],
0.0, 0.0, 0.0, I_local[1, 0], I_local[1, 1], I_local[1, 2],
0.0, 0.0, 0.0, I_local[2, 0], I_local[2, 1], I_local[2, 2],
)
# fmt: on
# Transform from COM frame to world frame
X_com = wp.transform(com, wp.quat_identity())
X_sm = X_wb * X_com
I_s = transform_spatial_inertia(X_sm, I_m)
body_I_s[tid] = I_s
@wp.kernel
def eval_articulation_mass_matrix(
articulation_start: wp.array[int],
articulation_count: int,
articulation_mask: wp.array[bool],
joint_child: wp.array[int],
joint_qd_start: wp.array[int],
body_I_s: wp.array[wp.spatial_matrix],
J: wp.array3d[float],
# outputs
H: wp.array3d[float],
):
"""Compute generalized mass matrix H = J^T * M * J.
The mass matrix H relates joint accelerations to joint forces/torques.
Output shape: (articulation_count, max_dofs, max_dofs)
"""
art_idx = wp.tid()
if art_idx >= articulation_count:
return
if articulation_mask:
if not articulation_mask[art_idx]:
return
joint_start = articulation_start[art_idx]
joint_end = articulation_start[art_idx + 1]
joint_count = joint_end - joint_start
articulation_dof_start = joint_qd_start[joint_start]
articulation_dof_end = joint_qd_start[joint_end]
articulation_dof_count = articulation_dof_end - articulation_dof_start
# H = J^T * M * J
# M is block diagonal with 6x6 spatial inertia blocks
# We compute this as: for each link i, H += J_i^T * I_i * J_i
for link_idx in range(joint_count):
j = joint_start + link_idx
child = joint_child[j]
I_s = body_I_s[child]
row_start = link_idx * 6
# Compute contribution from this link: H += J_i^T * I_i * J_i
for dof_i in range(articulation_dof_count):
for dof_j in range(articulation_dof_count):
sum_val = float(0.0)
# J_i^T * I_i * J_j (for the 6 rows of this link)
for k in range(6):
for l in range(6):
J_ik = J[art_idx, row_start + k, dof_i]
J_jl = J[art_idx, row_start + l, dof_j]
sum_val += J_ik * I_s[k, l] * J_jl
H[art_idx, dof_i, dof_j] = H[art_idx, dof_i, dof_j] + sum_val
def eval_mass_matrix(
model: Model,
state: State,
H: wp.array | None = None,
J: wp.array | None = None,
body_I_s: wp.array | None = None,
joint_S_s: wp.array | None = None,
mask: wp.array | None = None,
) -> wp.array | None:
"""Evaluate generalized mass matrix for articulations.
Computes the generalized mass matrix H = J^T * M * J, where J is the spatial
Jacobian and M is the block-diagonal spatial mass matrix. The mass matrix
relates joint accelerations to joint forces/torques.
Args:
model: The model containing articulation definitions.
state: The state containing body transforms (body_q).
H: Optional output array for mass matrix, shape (articulation_count, max_dofs, max_dofs).
If None, allocates internally.
J: Optional pre-computed Jacobian. If None, computes internally.
Shape (articulation_count, max_links*6, max_dofs).
body_I_s: Optional pre-allocated temp array for spatial inertias,
shape (body_count,), dtype wp.spatial_matrix. If None, allocates internally.
joint_S_s: Optional pre-allocated temp array for motion subspaces (only used if J is None),
shape (joint_dof_count,), dtype wp.spatial_vector. If None, allocates internally.
mask: Optional boolean mask to select which articulations to compute.
Shape [articulation_count]. If None, computes for all articulations.
Returns:
The mass matrix array H, or None if the model has no articulations.
"""
if model.articulation_count == 0:
return None
# Allocate output if not provided
if H is None:
max_dofs = model.max_dofs_per_articulation
H = wp.empty(
(model.articulation_count, max_dofs, max_dofs),
dtype=float,
device=model.device,
)
# Zero the output buffer
H.zero_()
# Allocate or use provided body_I_s
if body_I_s is None:
body_I_s = wp.zeros(
model.body_count,
dtype=wp.spatial_matrix,
device=model.device,
)
# Compute spatial inertias in world frame
wp.launch(
kernel=compute_body_spatial_inertia,
dim=model.body_count,
inputs=[
model.body_inertia,
model.body_mass,
model.body_com,
state.body_q,
],
outputs=[body_I_s],
device=model.device,
)
# Compute Jacobian if not provided
if J is None:
max_links = model.max_joints_per_articulation
max_dofs = model.max_dofs_per_articulation
J = wp.zeros(
(model.articulation_count, max_links * 6, max_dofs),
dtype=float,
device=model.device,
)
eval_jacobian(model, state, J, joint_S_s=joint_S_s, mask=mask)
wp.launch(
kernel=eval_articulation_mass_matrix,
dim=model.articulation_count,
inputs=[
model.articulation_start,
model.articulation_count,
mask,
model.joint_child,
model.joint_qd_start,
body_I_s,
J,
],
outputs=[H],
device=model.device,
)
return H
================================================
FILE: newton/_src/sim/builder.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""A module for building Newton models."""
from __future__ import annotations
import copy
import ctypes
import inspect
import math
import warnings
from collections import Counter, deque
from collections.abc import Callable, Iterable, Sequence
from dataclasses import dataclass, replace
from typing import TYPE_CHECKING, Any, Literal
import numpy as np
import warp as wp
from ..core.types import (
MAXVAL,
Axis,
AxisType,
Devicelike,
Mat22,
Mat33,
Quat,
Transform,
Vec3,
Vec4,
Vec6,
axis_to_vec3,
flag_to_int,
)
from ..geometry import (
Gaussian,
GeoType,
Mesh,
ParticleFlags,
ShapeFlags,
compute_inertia_shape,
compute_shape_radius,
transform_inertia,
)
from ..geometry.inertia import validate_and_correct_inertia_kernel, verify_and_correct_inertia
from ..geometry.types import Heightfield
from ..geometry.utils import RemeshingMethod, compute_inertia_obb, remesh_mesh
from ..math import quat_between_vectors_robust
from ..usd.schema_resolver import SchemaResolver
from ..utils import compute_world_offsets
from ..utils.mesh import MeshAdjacency
from .enums import (
BodyFlags,
EqType,
JointTargetMode,
JointType,
)
from .graph_coloring import (
ColoringAlgorithm,
color_graph,
color_rigid_bodies,
combine_independent_particle_coloring,
construct_particle_graph,
)
from .model import Model
if TYPE_CHECKING:
from newton_actuators import Actuator
from pxr import Usd
from ..geometry.types import TetMesh
UsdStage = Usd.Stage
else:
UsdStage = Any
class ModelBuilder:
"""A helper class for building simulation models at runtime.
Use the ModelBuilder to construct a simulation scene. The ModelBuilder
represents the scene using standard Python data structures like lists,
which are convenient but unsuitable for efficient simulation.
Call :meth:`finalize ` to construct a simulation-ready Model.
Example
-------
.. testcode::
import newton
from newton.solvers import SolverXPBD
builder = newton.ModelBuilder()
# anchor point (zero mass)
builder.add_particle((0, 1.0, 0.0), (0.0, 0.0, 0.0), 0.0)
# build chain
for i in range(1, 10):
builder.add_particle((i, 1.0, 0.0), (0.0, 0.0, 0.0), 1.0)
builder.add_spring(i - 1, i, 1.0e3, 0.0, 0)
# create model
model = builder.finalize()
state_0, state_1 = model.state(), model.state()
control = model.control()
solver = SolverXPBD(model)
contacts = model.contacts()
for i in range(10):
state_0.clear_forces()
model.collide(state_0, contacts)
solver.step(state_0, state_1, control, contacts, dt=1.0 / 60.0)
state_0, state_1 = state_1, state_0
World Grouping
--------------------
ModelBuilder supports world grouping to organize entities for multi-world simulations.
Each entity (particle, body, shape, joint, articulation) has an associated world index:
- Index -1: Global entities shared across all worlds (e.g., ground plane)
- Index 0, 1, 2, ...: World-specific entities
See :doc:`Worlds ` for a full overview of world semantics,
layout, and supported workflows.
There are two supported workflows for assigning world indices:
1. **Using begin_world()/end_world()**: Entities added outside any world
context, before the first :meth:`begin_world` or after the matching
:meth:`end_world`, are assigned to the global world (index ``-1``).
:class:`ModelBuilder` manages :attr:`current_world` while a world context is
active::
builder = ModelBuilder()
builder.add_ground_plane() # global (world -1)
builder.begin_world(label="robot_0")
builder.add_body(...) # world 0
builder.end_world()
2. **Using add_world()/replicate()**: All entities from the sub-builder are
assigned to a new world::
robot = ModelBuilder()
robot.add_body(...) # World assignments here will be overridden
main = ModelBuilder()
main.add_world(robot) # All robot entities -> world 0
main.replicate(robot, world_count=2) # Add more worlds from the same source
:attr:`current_world` is builder-managed, read-only state. Use
:meth:`begin_world`, :meth:`end_world`, :meth:`add_world`, or
:meth:`replicate` to manage world assignment.
Note:
It is strongly recommended to use the ModelBuilder to construct a simulation rather
than creating your own Model object directly, however it is possible to do so if
desired.
"""
_DEFAULT_GROUND_PLANE_COLOR = (0.125, 0.125, 0.15)
_SHAPE_COLOR_PALETTE = (
# Paul Tol - Bright 9
(68, 119, 170), # blue
(102, 204, 238), # cyan
(34, 136, 51), # green
(204, 187, 68), # yellow
(238, 102, 119), # red
(170, 51, 119), # magenta
(238, 153, 51), # orange
(0, 153, 136), # teal
)
_BODY_ARMATURE_ARG_DEPRECATION_MESSAGE = (
"ModelBuilder.add_link(..., armature=...) and ModelBuilder.add_body(..., armature=...) "
"are deprecated and will be removed in a future release. "
"Add any isotropic artificial inertia directly to 'inertia' instead."
)
_DEFAULT_BODY_ARMATURE_DEPRECATION_MESSAGE = (
"ModelBuilder.default_body_armature is deprecated and will be removed in a future release. "
"Add any isotropic artificial inertia directly to 'inertia' instead."
)
@staticmethod
def _shape_palette_color(index: int) -> tuple[float, float, float]:
color = ModelBuilder._SHAPE_COLOR_PALETTE[index % len(ModelBuilder._SHAPE_COLOR_PALETTE)]
return (color[0] / 255.0, color[1] / 255.0, color[2] / 255.0)
@dataclass
class ActuatorEntry:
"""Stores accumulated indices and arguments for one actuator type + scalar params combo.
Each element in input_indices/output_indices represents one actuator.
For single-input actuators: [[idx1], [idx2], ...] → flattened to 1D array
For multi-input actuators: [[idx1, idx2], [idx3, idx4], ...] → 2D array
"""
input_indices: list[list[int]] # Per-actuator input indices
output_indices: list[list[int]] # Per-actuator output indices
args: list[dict[str, Any]] # Per-actuator array params (scalar params in dict key)
@dataclass
class ShapeConfig:
"""
Represents the properties of a collision shape used in simulation.
"""
density: float = 1000.0
"""The density of the shape material."""
ke: float = 2.5e3
"""The contact elastic stiffness. Used by SemiImplicit, Featherstone, MuJoCo."""
kd: float = 100.0
"""The contact damping coefficient. Used by SemiImplicit, Featherstone, MuJoCo."""
kf: float = 1000.0
"""The friction damping coefficient. Used by SemiImplicit, Featherstone."""
ka: float = 0.0
"""The contact adhesion distance. Used by SemiImplicit, Featherstone."""
mu: float = 1.0
"""The coefficient of friction. Used by all solvers."""
restitution: float = 0.0
"""The coefficient of restitution. Used by XPBD. To take effect, enable restitution in solver constructor via ``enable_restitution=True``."""
mu_torsional: float = 0.005
"""The coefficient of torsional friction (resistance to spinning at contact point). Used by XPBD, MuJoCo."""
mu_rolling: float = 0.0001
"""The coefficient of rolling friction (resistance to rolling motion). Used by XPBD, MuJoCo."""
margin: float = 0.0
"""Outward offset from the shape's surface [m] for collision detection.
Extends the effective collision surface outward by this amount. When two shapes collide,
their margins are summed (margin_a + margin_b) to determine the total separation [m].
This value is also used when computing inertia for hollow shapes (``is_solid=False``)."""
gap: float | None = None
"""Additional contact detection gap [m]. If None, uses builder.rigid_gap as default.
Broad phase uses (margin + gap) [m] for AABB expansion and pair filtering."""
is_solid: bool = True
"""Indicates whether the shape is solid or hollow. Defaults to True."""
collision_group: int = 1
"""The collision group ID for the shape. Defaults to 1 (default group). Set to 0 to disable collisions for this shape."""
collision_filter_parent: bool = True
"""Whether to inherit collision filtering from the parent. Defaults to True."""
has_shape_collision: bool = True
"""Whether the shape can collide with other shapes. Defaults to True."""
has_particle_collision: bool = True
"""Whether the shape can collide with particles. Defaults to True."""
is_visible: bool = True
"""Indicates whether the shape is visible in the simulation. Defaults to True."""
is_site: bool = False
"""Indicates whether the shape is a site (non-colliding reference point). Directly setting this to True will NOT enforce site invariants. Use `mark_as_site()` or set via the `flags` property to ensure invariants. Defaults to False."""
sdf_narrow_band_range: tuple[float, float] = (-0.1, 0.1)
"""The narrow band distance range (inner, outer) for primitive SDF computation."""
sdf_target_voxel_size: float | None = None
"""Target voxel size for sparse SDF grid.
If provided, enables primitive SDF generation and takes precedence over
sdf_max_resolution. Requires GPU since wp.Volume only supports CUDA."""
sdf_max_resolution: int | None = None
"""Maximum dimension for sparse SDF grid (must be divisible by 8).
If provided (and sdf_target_voxel_size is None), enables primitive SDF
generation. Requires GPU since wp.Volume only supports CUDA."""
sdf_texture_format: str = "uint16"
"""Subgrid texture storage format for the SDF. ``"uint16"``
(default) stores subgrid voxels as 16-bit normalized textures (half
the memory of ``"float32"``). ``"float32"`` stores full-precision
values. ``"uint8"`` uses 8-bit textures for minimum memory."""
is_hydroelastic: bool = False
"""Whether the shape collides using SDF-based hydroelastics. For hydroelastic collisions, both participating shapes must have is_hydroelastic set to True. Defaults to False.
.. note::
Hydroelastic collision handling only works with volumetric shapes and in particular will not work for shapes like flat meshes or cloth.
This flag will be automatically set to False for planes and heightfields in :meth:`ModelBuilder.add_shape`.
"""
kh: float = 1.0e10
"""Contact stiffness coefficient for hydroelastic collisions. Used by MuJoCo, Featherstone, SemiImplicit when is_hydroelastic is True.
.. note::
For MuJoCo, stiffness values will internally be scaled by masses.
Users should choose kh to match their desired force-to-penetration ratio.
"""
def configure_sdf(
self,
*,
max_resolution: int | None = None,
target_voxel_size: float | None = None,
is_hydroelastic: bool = False,
kh: float = 1.0e10,
texture_format: str | None = None,
) -> None:
"""Enable SDF-based collision for this shape.
Sets SDF and hydroelastic options in one place. Call this when the shape
should use SDF mesh-mesh collision and optionally hydroelastic contacts.
Args:
max_resolution: Maximum dimension for sparse SDF grid (must be divisible by 8).
If provided, enables SDF-based mesh-mesh collision and clears any
previous target_voxel_size setting.
target_voxel_size: Target voxel size for sparse SDF grid. If provided, enables
SDF generation and clears any previous max_resolution setting.
is_hydroelastic: Whether to use SDF-based hydroelastic contacts. Both shapes
in a pair must have this enabled.
kh: Contact stiffness coefficient for hydroelastic collisions.
texture_format: Subgrid texture storage format. ``"uint16"``
(default) uses 16-bit normalized textures. ``"float32"``
uses full-precision. ``"uint8"`` uses 8-bit textures.
Raises:
ValueError: If both max_resolution and target_voxel_size are provided.
"""
if max_resolution is not None and target_voxel_size is not None:
raise ValueError("configure_sdf accepts either max_resolution or target_voxel_size, not both.")
if max_resolution is not None:
self.sdf_max_resolution = max_resolution
self.sdf_target_voxel_size = None
if target_voxel_size is not None:
self.sdf_target_voxel_size = target_voxel_size
self.sdf_max_resolution = None
self.is_hydroelastic = is_hydroelastic
self.kh = kh
if texture_format is not None:
self.sdf_texture_format = texture_format
def validate(self, shape_type: int | None = None) -> None:
"""Validate ShapeConfig parameters.
Args:
shape_type: Optional shape geometry type used for context-specific
validation.
"""
_valid_tex_fmts = ("float32", "uint16", "uint8")
if self.sdf_texture_format not in _valid_tex_fmts:
raise ValueError(
f"Unknown sdf_texture_format {self.sdf_texture_format!r}. Expected one of {list(_valid_tex_fmts)}."
)
if self.sdf_max_resolution is not None and self.sdf_target_voxel_size is not None:
raise ValueError("Set only one of sdf_max_resolution or sdf_target_voxel_size, not both.")
if self.sdf_max_resolution is not None and self.sdf_max_resolution % 8 != 0:
raise ValueError(
f"sdf_max_resolution must be divisible by 8 (got {self.sdf_max_resolution}). "
"This is required because SDF volumes are allocated in 8x8x8 tiles."
)
hydroelastic_supported = shape_type not in (GeoType.PLANE, GeoType.HFIELD)
hydroelastic_requires_configured_sdf = shape_type in (
GeoType.SPHERE,
GeoType.BOX,
GeoType.CAPSULE,
GeoType.CYLINDER,
GeoType.ELLIPSOID,
GeoType.CONE,
)
if (
self.is_hydroelastic
and hydroelastic_supported
and hydroelastic_requires_configured_sdf
and self.has_shape_collision
and self.sdf_max_resolution is None
and self.sdf_target_voxel_size is None
):
raise ValueError(
"Hydroelastic shapes require an SDF. Set either sdf_max_resolution or sdf_target_voxel_size."
)
def mark_as_site(self) -> None:
"""Marks this shape as a site and enforces all site invariants.
Sets:
- is_site = True
- has_shape_collision = False
- has_particle_collision = False
- density = 0.0
- collision_group = 0
"""
self.is_site = True
self.has_shape_collision = False
self.has_particle_collision = False
self.density = 0.0
self.collision_group = 0
@property
def flags(self) -> int:
"""Returns the flags for the shape."""
shape_flags = ShapeFlags.VISIBLE if self.is_visible else 0
shape_flags |= ShapeFlags.COLLIDE_SHAPES if self.has_shape_collision else 0
shape_flags |= ShapeFlags.COLLIDE_PARTICLES if self.has_particle_collision else 0
shape_flags |= ShapeFlags.SITE if self.is_site else 0
shape_flags |= ShapeFlags.HYDROELASTIC if self.is_hydroelastic else 0
return shape_flags
@flags.setter
def flags(self, value: int):
"""Sets the flags for the shape."""
self.is_visible = bool(value & ShapeFlags.VISIBLE)
self.is_hydroelastic = bool(value & ShapeFlags.HYDROELASTIC)
# Check if SITE flag is being set
is_site_flag = bool(value & ShapeFlags.SITE)
if is_site_flag:
# Use mark_as_site() to enforce invariants
self.mark_as_site()
# Collision flags will be cleared by mark_as_site()
else:
# SITE flag is being cleared - restore non-site defaults
defaults = self.__class__()
self.is_site = False
self.density = defaults.density
self.collision_group = defaults.collision_group
self.has_shape_collision = bool(value & ShapeFlags.COLLIDE_SHAPES)
self.has_particle_collision = bool(value & ShapeFlags.COLLIDE_PARTICLES)
def copy(self) -> ModelBuilder.ShapeConfig:
return copy.copy(self)
class JointDofConfig:
"""
Describes a joint axis (a single degree of freedom) that can have limits and be driven towards a target.
"""
def __init__(
self,
axis: AxisType | Vec3 = Axis.X,
limit_lower: float = -MAXVAL,
limit_upper: float = MAXVAL,
limit_ke: float = 1e4,
limit_kd: float = 1e1,
target_pos: float = 0.0,
target_vel: float = 0.0,
target_ke: float = 0.0,
target_kd: float = 0.0,
armature: float = 0.0,
effort_limit: float = 1e6,
velocity_limit: float = 1e6,
friction: float = 0.0,
actuator_mode: JointTargetMode | None = None,
):
self.axis = wp.normalize(axis_to_vec3(axis))
"""The 3D joint axis in the joint parent anchor frame."""
self.limit_lower = limit_lower
"""The lower position limit of the joint axis. Defaults to -MAXVAL (unlimited)."""
self.limit_upper = limit_upper
"""The upper position limit of the joint axis. Defaults to MAXVAL (unlimited)."""
self.limit_ke = limit_ke
"""The elastic stiffness of the joint axis limits. Defaults to 1e4."""
self.limit_kd = limit_kd
"""The damping stiffness of the joint axis limits. Defaults to 1e1."""
self.target_pos = target_pos
"""The target position of the joint axis.
If the initial `target_pos` is outside the limits,
it defaults to the midpoint of `limit_lower` and `limit_upper`. Otherwise, defaults to 0.0."""
self.target_vel = target_vel
"""The target velocity of the joint axis."""
self.target_ke = target_ke
"""The proportional gain of the target drive PD controller. Defaults to 0.0."""
self.target_kd = target_kd
"""The derivative gain of the target drive PD controller. Defaults to 0.0."""
self.armature = armature
"""Artificial inertia added around the joint axis [kg·m² or kg]. Defaults to 0."""
self.effort_limit = effort_limit
"""Maximum effort (force or torque) the joint axis can exert. Defaults to 1e6."""
self.velocity_limit = velocity_limit
"""Maximum velocity the joint axis can achieve. Defaults to 1e6."""
self.friction = friction
"""Friction coefficient for the joint axis. Defaults to 0.0."""
self.actuator_mode = actuator_mode
"""Actuator mode for this DOF. Determines which actuators are installed (see :class:`JointTargetMode`).
If None, the mode is inferred from gains and targets."""
if self.target_pos > self.limit_upper or self.target_pos < self.limit_lower:
self.target_pos = 0.5 * (self.limit_lower + self.limit_upper)
@classmethod
def create_unlimited(cls, axis: AxisType | Vec3) -> ModelBuilder.JointDofConfig:
"""Creates a JointDofConfig with no limits."""
return ModelBuilder.JointDofConfig(
axis=axis,
limit_lower=-MAXVAL,
limit_upper=MAXVAL,
target_pos=0.0,
target_vel=0.0,
target_ke=0.0,
target_kd=0.0,
armature=0.0,
limit_ke=0.0,
limit_kd=0.0,
)
@dataclass
class CustomAttribute:
"""
Represents a custom attribute definition for the ModelBuilder.
This is used to define custom attributes that are not part of the standard ModelBuilder API.
Custom attributes can be defined for the :class:`~newton.Model`, :class:`~newton.State`, :class:`~newton.Control`, or :class:`~newton.Contacts` objects, depending on the :class:`Model.AttributeAssignment` category.
Custom attributes must be declared before use via the :meth:`newton.ModelBuilder.add_custom_attribute` method.
See :ref:`custom_attributes` for more information.
"""
name: str
"""Variable name to expose on the Model. Must be a valid Python identifier."""
dtype: type
"""Warp dtype (e.g., wp.float32, wp.int32, wp.bool, wp.vec3) that is compatible with Warp arrays,
or ``str`` for string attributes that remain as Python lists."""
frequency: Model.AttributeFrequency | str
"""Frequency category that determines how the attribute is indexed in the Model.
Can be either:
- A :class:`Model.AttributeFrequency` enum value for built-in frequencies (BODY, SHAPE, JOINT, etc.)
Uses dict-based storage where keys are entity indices, allowing sparse assignment.
- A string for custom frequencies using the full frequency key (e.g., ``"mujoco:pair"``).
Uses list-based storage for sequential data appended via :meth:`~newton.ModelBuilder.add_custom_values`. All attributes
sharing the same custom frequency must have the same count, validated by
:meth:`finalize `."""
assignment: Model.AttributeAssignment = Model.AttributeAssignment.MODEL
"""Assignment category (see :class:`Model.AttributeAssignment`), defaults to :attr:`Model.AttributeAssignment.MODEL`"""
namespace: str | None = None
"""Namespace for the attribute. If None, the attribute is added directly to the assigned object without a namespace."""
references: str | None = None
"""For attributes containing entity indices, specifies how values are transformed during add_builder/add_world/replicate merging.
Built-in entity types (values are offset by entity count):
- ``"body"``, ``"shape"``, ``"joint"``, ``"joint_dof"``, ``"joint_coord"``, ``"articulation"``, ``"equality_constraint"``,
``"constraint_mimic"``, ``"particle"``, ``"edge"``, ``"triangle"``, ``"tetrahedron"``, ``"spring"``
Special handling:
- ``"world"``: Values are replaced with the builder-managed
:attr:`ModelBuilder.current_world` context (not offset)
Custom frequencies (values are offset by that frequency's count):
- Any custom frequency string, e.g., ``"mujoco:pair"``
"""
default: Any = None
"""Default value for the attribute. If None, the default value is determined based on the dtype."""
values: dict[int, Any] | list[Any] | None = None
"""Storage for specific values (overrides).
For enum frequencies (BODY, SHAPE, etc.): dict[int, Any] mapping entity indices to values.
For string frequencies ("mujoco:pair", etc.): list[Any] for sequential custom data.
If None, the attribute is not initialized with any values. Values can be assigned in subsequent
``ModelBuilder.add_*(..., custom_attributes={...})`` method calls for specific entities after
the CustomAttribute has been added through the :meth:`ModelBuilder.add_custom_attribute` method."""
usd_attribute_name: str | None = None
"""Name of the USD attribute to read values from during USD parsing.
- If ``None`` (default), the name is derived automatically as ``"newton:"``
where ```` is ``":"`` or just ``""`` if no namespace is set.
- If set to ``"*"``, the :attr:`usd_value_transformer` is called for every prim matching
the attribute's frequency, regardless of which USD attributes exist on the prim. The transformer
receives ``None`` as the value argument. This is useful for computing attribute values from
arbitrary prim data rather than reading a specific USD attribute.
A :attr:`usd_value_transformer` **must** be provided when using ``"*"``; otherwise,
:meth:`~newton.ModelBuilder.add_custom_attribute` raises a :class:`ValueError`.
"""
mjcf_attribute_name: str | None = None
"""Name of the attribute in the MJCF definition. If None, the attribute name is used."""
urdf_attribute_name: str | None = None
"""Name of the attribute in the URDF definition. If None, the attribute name is used."""
usd_value_transformer: Callable[[Any, dict[str, Any]], Any] | None = None
"""Transformer function that converts a USD attribute value to a valid Warp dtype. If undefined, the generic converter from :func:`newton.usd.convert_warp_value` is used. Receives a context dict with the following keys:
- ``"prim"``: The USD prim to query.
- ``"attr"``: The :class:`~newton.ModelBuilder.CustomAttribute` object to get the value for."""
mjcf_value_transformer: Callable[[str, dict[str, Any] | None], Any] | None = None
"""Transformer function that converts a MJCF attribute value string to a valid Warp dtype. If undefined, the generic converter from :func:`newton.utils.parse_warp_value_from_string` is used. Receives an optional context dict with parsing-time information (e.g., use_degrees, joint_type)."""
urdf_value_transformer: Callable[[str, dict[str, Any] | None], Any] | None = None
"""Transformer function that converts a URDF attribute value string to a valid Warp dtype. If undefined, the generic converter from :func:`newton.utils.parse_warp_value_from_string` is used. Receives an optional context dict with parsing-time information."""
def __post_init__(self):
"""Initialize default values and validate dtype compatibility."""
# Allow str dtype for string attributes (stored as Python lists, not warp arrays)
if self.dtype is not str:
# ensure dtype is a valid Warp dtype
try:
_size = wp.types.type_size_in_bytes(self.dtype)
except TypeError as e:
raise ValueError(f"Invalid dtype: {self.dtype}. Must be a valid Warp dtype or str.") from e
# Set dtype-specific default value if none was provided
if self.default is None:
self.default = self._default_for_dtype(self.dtype)
# Initialize values with correct container type based on frequency
if self.values is None:
self.values = self._create_empty_values_container()
if self.usd_attribute_name is None:
self.usd_attribute_name = f"newton:{self.key}"
if self.mjcf_attribute_name is None:
self.mjcf_attribute_name = self.name
if self.urdf_attribute_name is None:
self.urdf_attribute_name = self.name
@staticmethod
def _default_for_dtype(dtype: object) -> Any:
"""Get default value for dtype when not specified."""
# string type gets empty string
if dtype is str:
return ""
# quaternions get identity quaternion
if wp.types.type_is_quaternion(dtype):
return wp.quat_identity(dtype._wp_scalar_type_)
if dtype is wp.bool or dtype is bool:
return False
# vectors, matrices, scalars
return dtype(0)
@property
def key(self) -> str:
"""Return the full name of the attribute, formatted as "namespace:name" or "name" if no namespace is specified."""
return f"{self.namespace}:{self.name}" if self.namespace else self.name
@property
def is_custom_frequency(self) -> bool:
"""Check if this attribute uses a custom (string) frequency.
Returns:
True if the frequency is a string (custom frequency), False if it's a
Model.AttributeFrequency enum (built-in frequency like BODY, SHAPE, etc.).
"""
return isinstance(self.frequency, str)
def _create_empty_values_container(self) -> list | dict:
"""Create appropriate empty container based on frequency type."""
return [] if self.is_custom_frequency else {}
def _get_values_count(self) -> int:
"""Get current count of values in this attribute."""
if self.values is None:
return 0
return len(self.values)
def build_array(
self, count: int, device: Devicelike | None = None, requires_grad: bool = False
) -> wp.array | list:
"""Build wp.array (or list for string dtype) from count, dtype, default and overrides.
For string dtype, returns a Python list[str] instead of a Warp array.
"""
if self.values is None or len(self.values) == 0:
# No values provided, use default for all
arr = [self.default] * count
elif self.is_custom_frequency:
# Custom frequency: vals is a list, replace None with defaults and pad/truncate as needed
arr = [val if val is not None else self.default for val in self.values]
arr = arr + [self.default] * max(0, count - len(arr))
arr = arr[:count] # Truncate if needed
else:
# Enum frequency: vals is a dict, use get() to fill gaps with defaults
arr = [self.values.get(i, self.default) for i in range(count)]
# String dtype: return as Python list instead of warp array
if self.dtype is str:
return arr
return wp.array(arr, dtype=self.dtype, requires_grad=requires_grad, device=device)
@dataclass
class CustomFrequency:
"""
Represents a custom frequency definition for the ModelBuilder.
Custom frequencies allow defining entity types beyond the built-in ones (BODY, SHAPE, JOINT, etc.).
They must be registered via :meth:`ModelBuilder.add_custom_frequency` before any custom attributes
using them can be added.
The optional ``usd_prim_filter`` callback enables automatic USD parsing for this frequency.
When provided, :meth:`ModelBuilder.add_usd` will call this function for each prim in the USD
stage to determine whether custom attribute values with this frequency should be extracted from it.
See :ref:`custom_attributes` for more information on custom frequencies.
Example:
.. code-block:: python
# Define a custom frequency for MuJoCo actuators with USD parsing support
def is_actuator_prim(prim: Usd.Prim, context: dict[str, Any]) -> bool:
return prim.GetTypeName() == "MjcActuator"
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="actuator",
namespace="mujoco",
usd_prim_filter=is_actuator_prim,
)
)
"""
name: str
"""The name of the custom frequency (e.g., ``"actuator"``, ``"pair"``)."""
namespace: str | None = None
"""Namespace for the custom frequency. If provided, the frequency key becomes ``"namespace:name"``.
If None, the custom frequency is registered without a namespace."""
usd_prim_filter: Callable[[Usd.Prim, dict[str, Any]], bool] | None = None
"""Select which USD prims are used for this frequency.
Called by :meth:`newton.ModelBuilder.add_usd` for each visited prim with:
- ``prim``: current ``Usd.Prim``
- ``context``: callback context dictionary with ``prim``, ``result``,
and ``builder``
Return ``True`` to parse this prim for the frequency, or ``False`` to skip it.
If this is ``None``, the frequency is not parsed automatically from USD.
Example:
.. code-block:: python
def is_actuator_prim(prim: Usd.Prim, context: dict[str, Any]) -> bool:
return prim.GetTypeName() == "MjcActuator"
"""
usd_entry_expander: Callable[[Usd.Prim, dict[str, Any]], Iterable[dict[str, Any]]] | None = None
"""Build row entries for a matching USD prim.
Called by :meth:`newton.ModelBuilder.add_usd` after :attr:`usd_prim_filter`
returns ``True``. Return an iterable of dictionaries; each dictionary is one
row passed to :meth:`newton.ModelBuilder.add_custom_values`.
Use this when one prim should produce multiple rows. Missing keys in a row are
filled with ``None`` so defaults still apply. Returning an empty iterable adds
no rows.
See also:
When this callback is set, :meth:`newton.ModelBuilder.add_usd` does not run
default per-attribute extraction for this frequency on matched prims
(``usd_attribute_name`` / ``usd_value_transformer``).
Example:
.. code-block:: python
def expand_tendon_rows(prim: Usd.Prim, context: dict[str, Any]) -> Iterable[dict[str, Any]]:
for joint_path in prim.GetCustomDataByKey("joint_paths") or []:
yield {"joint": joint_path, "stiffness": prim.GetCustomDataByKey("stiffness")}
"""
def __post_init__(self):
"""Validate frequency naming and callback relationships."""
if not self.name or ":" in self.name:
raise ValueError(f"name must be non-empty and colon-free, got '{self.name}'")
if self.namespace is not None and (not self.namespace or ":" in self.namespace):
raise ValueError(f"namespace must be non-empty and colon-free, got '{self.namespace}'")
if self.usd_entry_expander is not None and self.usd_prim_filter is None:
raise ValueError("usd_entry_expander requires usd_prim_filter")
@property
def key(self) -> str:
"""The key of the custom frequency (e.g., ``"mujoco:actuator"`` or ``"pair"``)."""
return f"{self.namespace}:{self.name}" if self.namespace else self.name
def __init__(self, up_axis: AxisType = Axis.Z, gravity: float = -9.81):
"""
Initializes a new ModelBuilder instance for constructing simulation models.
Args:
up_axis: The axis to use as the "up" direction in the simulation.
Defaults to Axis.Z.
gravity: The magnitude of gravity to apply along the up axis.
Defaults to -9.81.
"""
self.world_count: int = 0
"""Number of worlds accumulated for :attr:`Model.world_count`."""
# region defaults
self.default_shape_cfg = ModelBuilder.ShapeConfig()
"""Default shape configuration used when shape-creation methods are called with ``cfg=None``.
Update this object before adding shapes to set default contact/material properties."""
self.default_joint_cfg = ModelBuilder.JointDofConfig()
"""Default joint DoF configuration used when joint DoF configuration is omitted."""
self.default_particle_radius = 0.1
"""Default particle radius used when particle radius is not provided explicitly."""
self.default_tri_ke = 100.0
"""Default triangle elastic stiffness for cloth/soft-triangle constraints."""
self.default_tri_ka = 100.0
"""Default triangle area stiffness for cloth/soft-triangle constraints."""
self.default_tri_kd = 10.0
"""Default triangle damping for cloth/soft-triangle constraints."""
self.default_tri_drag = 0.0
"""Default aerodynamic drag coefficient for triangle elements."""
self.default_tri_lift = 0.0
"""Default aerodynamic lift coefficient for triangle elements."""
self.default_spring_ke = 100.0
"""Default spring elastic stiffness for distance constraints."""
self.default_spring_kd = 0.0
"""Default spring damping for distance constraints."""
self.default_edge_ke = 100.0
"""Default edge-bending elastic stiffness."""
self.default_edge_kd = 0.0
"""Default edge-bending damping."""
self.default_tet_k_mu = 1.0e3
"""Default first Lame parameter [Pa] for tetrahedral elements."""
self.default_tet_k_lambda = 1.0e3
"""Default second Lame parameter [Pa] for tetrahedral elements."""
self.default_tet_k_damp = 0.0
"""Default damping stiffness for tetrahedral elements."""
self.default_tet_density = 1.0
"""Default density [kg/m^3] for tetrahedral soft bodies."""
self._default_body_armature = 0.0
# endregion
# region compiler settings (similar to MuJoCo)
self.balance_inertia: bool = True
"""Whether to automatically correct rigid body inertia tensors that violate the triangle inequality.
When True, adds a scalar multiple of the identity matrix to preserve rotation structure while
ensuring physical validity (I1 + I2 >= I3 for principal moments). Default: True."""
self.bound_mass: float | None = None
"""Minimum allowed mass value for rigid bodies [kg]. If set, any body mass below this
value will be clamped to this minimum. Set to None to disable mass clamping.
Default: None."""
self.bound_inertia: float | None = None
"""Minimum allowed eigenvalue for rigid body inertia tensors [kg*m^2]. If set, ensures
all principal moments of inertia are at least this value. Set to None to disable inertia
eigenvalue clamping. Default: None."""
self.validate_inertia_detailed: bool = False
"""Whether to use detailed (slower) inertia validation that provides per-body warnings.
When False, uses a fast GPU kernel that reports only the total number of corrected bodies.
When True, uses a CPU implementation that reports specific issues for each body.
Both modes produce semantically identical corrected values on the returned
:class:`Model`. Neither mode modifies the builder's internal state — corrected
values live only on the Model.
Default: False."""
# endregion
# particles
self.particle_q: list[Vec3] = []
"""Particle positions [m] accumulated for :attr:`Model.particle_q`."""
self.particle_qd: list[Vec3] = []
"""Particle velocities [m/s] accumulated for :attr:`Model.particle_qd`."""
self.particle_mass: list[float] = []
"""Particle masses [kg] accumulated for :attr:`Model.particle_mass`."""
self.particle_radius: list[float] = []
"""Particle radii [m] accumulated for :attr:`Model.particle_radius`."""
self.particle_flags: list[int | ParticleFlags] = []
"""Particle flags accumulated for :attr:`Model.particle_flags`."""
self.particle_max_velocity: float = 1e5
"""Maximum particle velocity [m/s] propagated to :attr:`Model.particle_max_velocity`."""
self.particle_color_groups: list[Any] = []
"""Particle color groups accumulated for :attr:`Model.particle_color_groups`."""
self.particle_world: list[int] = []
"""World indices accumulated for :attr:`Model.particle_world`."""
# shapes (each shape has an entry in these arrays)
self.shape_label: list[str] = []
"""Shape labels accumulated for :attr:`Model.shape_label`."""
self.shape_transform: list[Transform] = []
"""Shape-to-body transforms accumulated for :attr:`Model.shape_transform`."""
self.shape_body: list[int] = []
"""Body indices accumulated for :attr:`Model.shape_body`."""
self.shape_flags: list[int] = []
"""Shape flags accumulated for :attr:`Model.shape_flags`."""
self.shape_type: list[int] = []
"""Geometry type ids accumulated for :attr:`Model.shape_type`."""
self.shape_scale: list[Vec3] = []
"""Shape scales accumulated for :attr:`Model.shape_scale`."""
self.shape_source: list[Any] = []
"""Source geometry objects accumulated for :attr:`Model.shape_source`."""
self.shape_color: list[Vec3] = []
"""Resolved display colors accumulated for :attr:`Model.shape_color`."""
self.shape_is_solid: list[bool] = []
"""Solid-vs-hollow flags accumulated for :attr:`Model.shape_is_solid`."""
self.shape_margin: list[float] = []
"""Shape margins [m] accumulated for :attr:`Model.shape_margin`."""
self.shape_material_ke: list[float] = []
"""Contact stiffness values [N/m] accumulated for :attr:`Model.shape_material_ke`."""
self.shape_material_kd: list[float] = []
"""Contact damping values accumulated for :attr:`Model.shape_material_kd`."""
self.shape_material_kf: list[float] = []
"""Friction stiffness values accumulated for :attr:`Model.shape_material_kf`."""
self.shape_material_ka: list[float] = []
"""Adhesion distances [m] accumulated for :attr:`Model.shape_material_ka`."""
self.shape_material_mu: list[float] = []
"""Friction coefficients accumulated for :attr:`Model.shape_material_mu`."""
self.shape_material_restitution: list[float] = []
"""Restitution coefficients accumulated for :attr:`Model.shape_material_restitution`."""
self.shape_material_mu_torsional: list[float] = []
"""Torsional friction coefficients accumulated for :attr:`Model.shape_material_mu_torsional`."""
self.shape_material_mu_rolling: list[float] = []
"""Rolling friction coefficients accumulated for :attr:`Model.shape_material_mu_rolling`."""
self.shape_material_kh: list[float] = []
"""Hydroelastic stiffness values accumulated for :attr:`Model.shape_material_kh`."""
self.shape_gap: list[float] = []
"""Contact gaps [m] accumulated for :attr:`Model.shape_gap`."""
self.shape_collision_group: list[int] = []
"""Collision groups accumulated for :attr:`Model.shape_collision_group`."""
self.shape_collision_radius: list[float] = []
"""Broadphase collision radii [m] accumulated for :attr:`Model.shape_collision_radius`."""
self.shape_world: list[int] = []
"""World indices accumulated for :attr:`Model.shape_world`."""
self.shape_sdf_narrow_band_range: list[tuple[float, float]] = []
"""Per-shape SDF narrow-band ranges retained until :meth:`finalize ` generates
SDF data."""
self.shape_sdf_target_voxel_size: list[float | None] = []
"""Per-shape target SDF voxel sizes retained until :meth:`finalize `."""
self.shape_sdf_max_resolution: list[int | None] = []
"""Per-shape SDF maximum resolutions retained until :meth:`finalize `."""
self.shape_sdf_texture_format: list[str] = []
"""Per-shape SDF texture format retained until :meth:`finalize `."""
# Mesh SDF storage (texture SDF arrays created at finalize)
# filtering to ignore certain collision pairs
self.shape_collision_filter_pairs: list[tuple[int, int]] = []
"""Shape collision filter pairs accumulated for :attr:`Model.shape_collision_filter_pairs`."""
self._requested_contact_attributes: set[str] = set()
"""Optional contact attributes requested via :meth:`request_contact_attributes`."""
self._requested_state_attributes: set[str] = set()
"""Optional state attributes requested via :meth:`request_state_attributes`."""
# springs
self.spring_indices: list[int] = []
"""Spring particle index pairs accumulated for :attr:`Model.spring_indices`."""
self.spring_rest_length: list[float] = []
"""Spring rest lengths [m] accumulated for :attr:`Model.spring_rest_length`."""
self.spring_stiffness: list[float] = []
"""Spring stiffness values [N/m] accumulated for :attr:`Model.spring_stiffness`."""
self.spring_damping: list[float] = []
"""Spring damping values accumulated for :attr:`Model.spring_damping`."""
self.spring_control: list[float] = []
"""Spring control activations accumulated for :attr:`Model.spring_control`."""
# triangles
self.tri_indices: list[tuple[int, int, int]] = []
"""Triangle connectivity accumulated for :attr:`Model.tri_indices`."""
self.tri_poses: list[Mat22] = []
"""Triangle rest-pose 2x2 matrices accumulated for :attr:`Model.tri_poses`."""
self.tri_activations: list[float] = []
"""Triangle activations accumulated for :attr:`Model.tri_activations`."""
self.tri_materials: list[tuple[float, float, float, float, float]] = []
"""Triangle material rows accumulated for :attr:`Model.tri_materials`."""
self.tri_areas: list[float] = []
"""Triangle rest areas [m^2] accumulated for :attr:`Model.tri_areas`."""
# edges (bending)
self.edge_indices: list[tuple[int, int, int, int]] = []
"""Bending-edge connectivity accumulated for :attr:`Model.edge_indices`."""
self.edge_rest_angle: list[float] = []
"""Edge rest angles [rad] accumulated for :attr:`Model.edge_rest_angle`."""
self.edge_rest_length: list[float] = []
"""Edge rest lengths [m] accumulated for :attr:`Model.edge_rest_length`."""
self.edge_bending_properties: list[tuple[float, float]] = []
"""Bending stiffness/damping rows accumulated for :attr:`Model.edge_bending_properties`."""
# tetrahedra
self.tet_indices: list[tuple[int, int, int, int]] = []
"""Tetrahedral connectivity accumulated for :attr:`Model.tet_indices`."""
self.tet_poses: list[Mat33] = []
"""Tetrahedral rest-pose 3x3 matrices accumulated for :attr:`Model.tet_poses`."""
self.tet_activations: list[float] = []
"""Tetrahedral activations accumulated for :attr:`Model.tet_activations`."""
self.tet_materials: list[tuple[float, float, float]] = []
"""Tetrahedral material rows accumulated for :attr:`Model.tet_materials`."""
# muscles
self.muscle_start: list[int] = []
"""Muscle waypoint start indices accumulated for :attr:`Model.muscle_start`."""
self.muscle_params: list[tuple[float, float, float, float, float]] = []
"""Muscle parameter rows accumulated for :attr:`Model.muscle_params`."""
self.muscle_activations: list[float] = []
"""Muscle activations accumulated for :attr:`Model.muscle_activations`."""
self.muscle_bodies: list[int] = []
"""Muscle waypoint body indices accumulated for :attr:`Model.muscle_bodies`."""
self.muscle_points: list[Vec3] = []
"""Muscle waypoint local offsets accumulated for :attr:`Model.muscle_points`."""
# rigid bodies
self.body_mass: list[float] = []
"""Body masses [kg] accumulated for :attr:`Model.body_mass`."""
self.body_inertia: list[Mat33] = []
"""Body inertia tensors accumulated for :attr:`Model.body_inertia`."""
self.body_inv_mass: list[float] = []
"""Inverse body masses accumulated for :attr:`Model.body_inv_mass`."""
self.body_inv_inertia: list[Mat33] = []
"""Inverse body inertia tensors accumulated for :attr:`Model.body_inv_inertia`."""
self.body_com: list[Vec3] = []
"""Body centers of mass [m] accumulated for :attr:`Model.body_com`."""
self.body_q: list[Transform] = []
"""Body poses accumulated for :attr:`Model.body_q`."""
self.body_qd: list[Vec6] = []
"""Body spatial velocities accumulated for :attr:`Model.body_qd`."""
self.body_label: list[str] = []
"""Body labels accumulated for :attr:`Model.body_label`."""
self.body_lock_inertia: list[bool] = []
"""Per-body inertia-lock flags retained while composing bodies in the builder."""
self.body_flags: list[int] = []
"""Body flags accumulated for :attr:`Model.body_flags`."""
self.body_shapes: dict[int, list[int]] = {-1: []}
"""Mapping from body index to attached shape indices, finalized into :attr:`Model.body_shapes`."""
self.body_world: list[int] = []
"""World indices accumulated for :attr:`Model.body_world`."""
self.body_color_groups: list[Any] = []
"""Rigid-body color groups accumulated for :attr:`Model.body_color_groups`."""
# rigid joints
self.joint_parent: list[int] = []
"""Parent body indices accumulated for :attr:`Model.joint_parent`."""
self.joint_parents: dict[int, list[int]] = {}
"""Mapping from child body index to parent body indices used while composing articulations."""
self.joint_children: dict[int, list[int]] = {}
"""Mapping from parent body index to child body indices used while composing articulations."""
self.joint_child: list[int] = []
"""Child body indices accumulated for :attr:`Model.joint_child`."""
self.joint_axis: list[Vec3] = []
"""Joint axes accumulated for :attr:`Model.joint_axis`."""
self.joint_X_p: list[Transform] = []
"""Parent-frame joint transforms accumulated for :attr:`Model.joint_X_p`."""
self.joint_X_c: list[Transform] = []
"""Child-frame joint transforms accumulated for :attr:`Model.joint_X_c`."""
self.joint_q: list[float] = []
"""Joint coordinates accumulated for :attr:`Model.joint_q`."""
self.joint_qd: list[float] = []
"""Joint velocities accumulated for :attr:`Model.joint_qd`."""
self.joint_cts: list[float] = []
"""Per-joint constraint placeholders used to derive finalized joint-constraint counts."""
self.joint_f: list[float] = []
"""Joint forces accumulated for :attr:`Model.joint_f`."""
self.joint_act: list[float] = []
"""Joint actuation inputs accumulated for :attr:`Model.joint_act`."""
self.joint_type: list[int] = []
"""Joint type ids accumulated for :attr:`Model.joint_type`."""
self.joint_label: list[str] = []
"""Joint labels accumulated for :attr:`Model.joint_label`."""
self.joint_armature: list[float] = []
"""Joint armature values accumulated for :attr:`Model.joint_armature`."""
self.joint_target_mode: list[int] = []
"""Joint target modes accumulated for :attr:`Model.joint_target_mode`."""
self.joint_target_ke: list[float] = []
"""Joint target stiffness values accumulated for :attr:`Model.joint_target_ke`."""
self.joint_target_kd: list[float] = []
"""Joint target damping values accumulated for :attr:`Model.joint_target_kd`."""
self.joint_limit_lower: list[float] = []
"""Lower joint limits accumulated for :attr:`Model.joint_limit_lower`."""
self.joint_limit_upper: list[float] = []
"""Upper joint limits accumulated for :attr:`Model.joint_limit_upper`."""
self.joint_limit_ke: list[float] = []
"""Joint limit stiffness values accumulated for :attr:`Model.joint_limit_ke`."""
self.joint_limit_kd: list[float] = []
"""Joint limit damping values accumulated for :attr:`Model.joint_limit_kd`."""
self.joint_target_pos: list[float] = []
"""Joint position targets accumulated for :attr:`Model.joint_target_pos`."""
self.joint_target_vel: list[float] = []
"""Joint velocity targets accumulated for :attr:`Model.joint_target_vel`."""
self.joint_effort_limit: list[float] = []
"""Joint effort limits accumulated for :attr:`Model.joint_effort_limit`."""
self.joint_velocity_limit: list[float] = []
"""Joint velocity limits accumulated for :attr:`Model.joint_velocity_limit`."""
self.joint_friction: list[float] = []
"""Joint friction values accumulated for :attr:`Model.joint_friction`."""
self.joint_twist_lower: list[float] = []
"""Lower twist limits accumulated for :attr:`Model.joint_twist_lower`."""
self.joint_twist_upper: list[float] = []
"""Upper twist limits accumulated for :attr:`Model.joint_twist_upper`."""
self.joint_enabled: list[bool] = []
"""Joint enabled flags accumulated for :attr:`Model.joint_enabled`."""
self.joint_q_start: list[int] = []
"""Joint coordinate start indices accumulated for :attr:`Model.joint_q_start`."""
self.joint_qd_start: list[int] = []
"""Joint DoF start indices accumulated for :attr:`Model.joint_qd_start`."""
self.joint_cts_start: list[int] = []
"""Joint-constraint start indices retained while building per-joint constraint data."""
self.joint_dof_dim: list[tuple[int, int]] = []
"""Per-joint linear/angular DoF dimensions accumulated for :attr:`Model.joint_dof_dim`."""
self.joint_world: list[int] = []
"""World indices accumulated for :attr:`Model.joint_world`."""
self.joint_articulation: list[int] = []
"""Articulation indices accumulated for :attr:`Model.joint_articulation`."""
self.articulation_start: list[int] = []
"""Articulation start indices accumulated for :attr:`Model.articulation_start`."""
self.articulation_label: list[str] = []
"""Articulation labels accumulated for :attr:`Model.articulation_label`."""
self.articulation_world: list[int] = []
"""World indices accumulated for :attr:`Model.articulation_world`."""
self.joint_dof_count: int = 0
"""Total joint DoF count propagated to :attr:`Model.joint_dof_count`."""
self.joint_coord_count: int = 0
"""Total joint coordinate count propagated to :attr:`Model.joint_coord_count`."""
self.joint_constraint_count: int = 0
"""Total joint constraint count propagated to :attr:`Model.joint_constraint_count`."""
self._current_world: int = -1
"""Internal world context backing the read-only :attr:`current_world` property."""
self.up_axis: Axis = Axis.from_any(up_axis)
"""Up axis used when expanding scalar gravity into per-world gravity vectors."""
self.gravity: float = gravity
"""Gravity acceleration [m/s^2] applied along :attr:`up_axis` for newly added worlds."""
self.world_gravity: list[Vec3] = []
"""Per-world gravity vectors retained until :meth:`finalize ` populates
:attr:`Model.gravity`."""
self.rigid_gap: float = 0.1
"""Default rigid contact gap [m] applied when adding a shape whose
``ModelBuilder.ShapeConfig.gap`` is ``None``. The resolved per-shape values are later
propagated to :attr:`Model.shape_gap`."""
self.num_rigid_contacts_per_world: int | None = None
"""Optional per-world rigid-contact allocation budget used to set :attr:`Model.rigid_contact_max`."""
# equality constraints
self.equality_constraint_type: list[EqType] = []
"""Equality constraint types accumulated for :attr:`Model.equality_constraint_type`."""
self.equality_constraint_body1: list[int] = []
"""First body indices accumulated for :attr:`Model.equality_constraint_body1`."""
self.equality_constraint_body2: list[int] = []
"""Second body indices accumulated for :attr:`Model.equality_constraint_body2`."""
self.equality_constraint_anchor: list[Vec3] = []
"""Equality constraint anchors accumulated for :attr:`Model.equality_constraint_anchor`."""
self.equality_constraint_relpose: list[Transform] = []
"""Relative poses accumulated for :attr:`Model.equality_constraint_relpose`."""
self.equality_constraint_torquescale: list[float] = []
"""Torque scales accumulated for :attr:`Model.equality_constraint_torquescale`."""
self.equality_constraint_joint1: list[int] = []
"""First joint indices accumulated for :attr:`Model.equality_constraint_joint1`."""
self.equality_constraint_joint2: list[int] = []
"""Second joint indices accumulated for :attr:`Model.equality_constraint_joint2`."""
self.equality_constraint_polycoef: list[list[float]] = []
"""Polynomial coefficient rows accumulated for :attr:`Model.equality_constraint_polycoef`."""
self.equality_constraint_label: list[str] = []
"""Equality constraint labels accumulated for :attr:`Model.equality_constraint_label`."""
self.equality_constraint_enabled: list[bool] = []
"""Equality constraint enabled flags accumulated for :attr:`Model.equality_constraint_enabled`."""
self.equality_constraint_world: list[int] = []
"""World indices accumulated for :attr:`Model.equality_constraint_world`."""
# mimic constraints
self.constraint_mimic_joint0: list[int] = []
"""Follower joint indices accumulated for :attr:`Model.constraint_mimic_joint0`."""
self.constraint_mimic_joint1: list[int] = []
"""Leader joint indices accumulated for :attr:`Model.constraint_mimic_joint1`."""
self.constraint_mimic_coef0: list[float] = []
"""Offset coefficients accumulated for :attr:`Model.constraint_mimic_coef0`."""
self.constraint_mimic_coef1: list[float] = []
"""Scale coefficients accumulated for :attr:`Model.constraint_mimic_coef1`."""
self.constraint_mimic_enabled: list[bool] = []
"""Enabled flags accumulated for :attr:`Model.constraint_mimic_enabled`."""
self.constraint_mimic_label: list[str] = []
"""Mimic constraint labels accumulated for :attr:`Model.constraint_mimic_label`."""
self.constraint_mimic_world: list[int] = []
"""World indices accumulated for :attr:`Model.constraint_mimic_world`."""
# per-world entity start indices
self.particle_world_start: list[int] = []
"""Per-world particle starts accumulated for :attr:`Model.particle_world_start`."""
self.body_world_start: list[int] = []
"""Per-world body starts accumulated for :attr:`Model.body_world_start`."""
self.shape_world_start: list[int] = []
"""Per-world shape starts accumulated for :attr:`Model.shape_world_start`."""
self.joint_world_start: list[int] = []
"""Per-world joint starts accumulated for :attr:`Model.joint_world_start`."""
self.articulation_world_start: list[int] = []
"""Per-world articulation starts accumulated for :attr:`Model.articulation_world_start`."""
self.equality_constraint_world_start: list[int] = []
"""Per-world equality-constraint starts accumulated for :attr:`Model.equality_constraint_world_start`."""
self.joint_dof_world_start: list[int] = []
"""Per-world joint DoF starts accumulated for :attr:`Model.joint_dof_world_start`."""
self.joint_coord_world_start: list[int] = []
"""Per-world joint-coordinate starts accumulated for :attr:`Model.joint_coord_world_start`."""
self.joint_constraint_world_start: list[int] = []
"""Per-world joint-constraint starts accumulated for :attr:`Model.joint_constraint_world_start`."""
# Custom attributes (user-defined per-frequency arrays)
self.custom_attributes: dict[str, ModelBuilder.CustomAttribute] = {}
"""Registered custom attributes to materialize during :meth:`finalize `."""
# Registered custom frequencies (must be registered before adding attributes with that frequency)
self.custom_frequencies: dict[str, ModelBuilder.CustomFrequency] = {}
"""Registered custom string frequencies keyed by ``namespace:name`` or bare name."""
# Incrementally maintained counts for custom string frequencies
self._custom_frequency_counts: dict[str, int] = {}
"""Running counts for custom string frequencies used to size custom attribute arrays."""
# Actuator entries (accumulated during add_actuator calls)
# Key is (actuator_class, scalar_params_tuple) to separate instances with different scalar params
self.actuator_entries: dict[tuple[type[Actuator], tuple[Any, ...]], ModelBuilder.ActuatorEntry] = {}
"""Actuator entry groups accumulated from :meth:`add_actuator`, keyed by class and scalar parameters."""
def add_shape_collision_filter_pair(self, shape_a: int, shape_b: int) -> None:
"""Add a collision filter pair in canonical order.
Args:
shape_a: First shape index
shape_b: Second shape index
"""
self.shape_collision_filter_pairs.append((min(shape_a, shape_b), max(shape_a, shape_b)))
def add_custom_attribute(self, attribute: CustomAttribute) -> None:
"""
Define a custom per-entity attribute to be added to the Model.
See :ref:`custom_attributes` for more information.
For attributes with custom string frequencies (not enum frequencies like BODY, SHAPE, etc.),
the frequency must be registered first via :meth:`add_custom_frequency`. This ensures
explicit declaration of custom entity types and enables USD parsing support.
Args:
attribute: The custom attribute to add.
Raises:
ValueError: If the attribute key already exists with incompatible specification,
if the attribute uses a custom string frequency that hasn't been registered,
or if ``usd_attribute_name`` is ``"*"`` without a ``usd_value_transformer``.
Example:
.. doctest::
builder = newton.ModelBuilder()
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="my_attribute",
frequency=newton.Model.AttributeFrequency.BODY,
dtype=wp.float32,
default=20.0,
assignment=newton.Model.AttributeAssignment.MODEL,
namespace="my_namespace",
)
)
builder.add_body(custom_attributes={"my_namespace:my_attribute": 30.0})
builder.add_body() # we leave out the custom_attributes, so the attribute will use the default value 20.0
model = builder.finalize()
# the model has now a Model.AttributeNamespace object with the name "my_namespace"
# and an attribute "my_attribute" that is a wp.array of shape (body_count, 1)
# with the default value 20.0
assert np.allclose(model.my_namespace.my_attribute.numpy(), [30.0, 20.0])
"""
key = attribute.key
existing = self.custom_attributes.get(key)
if existing:
# validate that specification matches exactly
if (
existing.frequency != attribute.frequency
or existing.dtype != attribute.dtype
or existing.assignment != attribute.assignment
or existing.namespace != attribute.namespace
or existing.references != attribute.references
):
raise ValueError(f"Custom attribute '{key}' already exists with incompatible spec")
return
# Validate that custom frequencies are registered before use
if attribute.is_custom_frequency:
freq_key = attribute.frequency
if freq_key not in self.custom_frequencies:
raise ValueError(
f"Custom frequency '{freq_key}' is not registered. "
f"Please register it first using add_custom_frequency() before adding attributes with this frequency."
)
# Validate that wildcard USD attributes have a transformer
if attribute.usd_attribute_name == "*" and attribute.usd_value_transformer is None:
raise ValueError(
f"Custom attribute '{key}' uses usd_attribute_name='*' but no usd_value_transformer is provided. "
f"A wildcard USD attribute requires a usd_value_transformer to compute values from prim data."
)
self.custom_attributes[key] = attribute
def add_custom_frequency(self, frequency: CustomFrequency) -> None:
"""
Register a custom frequency for the builder.
Custom frequencies must be registered before adding any custom attributes that use them.
This enables explicit declaration of custom entity types and optionally provides USD
parsing support via the ``usd_prim_filter`` callback.
This method is idempotent: registering the same frequency multiple times is silently
ignored (useful when loading multiple files that all register the same frequencies).
Args:
frequency: A :class:`CustomFrequency` object with full configuration.
Example:
.. code-block:: python
# Full registration with USD parsing support
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="actuator",
namespace="mujoco",
usd_prim_filter=is_actuator_prim,
)
)
"""
freq_obj = frequency
freq_key = freq_obj.key
if freq_key in self.custom_frequencies:
existing = self.custom_frequencies[freq_key]
if (
existing.usd_prim_filter is not freq_obj.usd_prim_filter
or existing.usd_entry_expander is not freq_obj.usd_entry_expander
):
raise ValueError(f"Custom frequency '{freq_key}' is already registered with different callbacks.")
# Already registered with equivalent callbacks - silently skip
return
self.custom_frequencies[freq_key] = freq_obj
if freq_key not in self._custom_frequency_counts:
self._custom_frequency_counts[freq_key] = 0
def has_custom_attribute(self, key: str) -> bool:
"""Check if a custom attribute is defined."""
return key in self.custom_attributes
def get_custom_attributes_by_frequency(
self, frequencies: Sequence[Model.AttributeFrequency | str]
) -> list[CustomAttribute]:
"""
Get custom attributes by frequency.
This is useful for processing custom attributes for different kinds of simulation objects.
For example, you can get all the custom attributes for bodies, shapes, joints, etc.
Args:
frequencies: The frequencies to get custom attributes for.
Returns:
Custom attributes matching the requested frequencies.
"""
return [attr for attr in self.custom_attributes.values() if attr.frequency in frequencies]
def get_custom_frequency_keys(self) -> set[str]:
"""Return set of custom frequency keys (string frequencies) defined in this builder."""
return set(self._custom_frequency_counts.keys())
def add_custom_values(self, **kwargs: Any) -> dict[str, int]:
"""Append values to custom attributes with custom string frequencies.
Each keyword argument specifies an attribute key and the value to append. Values are
stored in a list and appended sequentially for robust indexing. Only works with
attributes that have a custom string frequency (not built-in enum frequencies).
This is useful for custom entity types that aren't built into the model,
such as user-defined groupings or solver-specific data.
Args:
**kwargs: Mapping of attribute keys to values. Keys should be the full
attribute key (e.g., ``"mujoco:pair_geom1"`` or just ``"my_attr"`` if no namespace).
Returns:
A mapping from attribute keys to the index where each value was added.
If all attributes had the same count before the call, all indices will be equal.
Raises:
AttributeError: If an attribute key is not defined.
TypeError: If an attribute has an enum frequency (must have custom frequency).
Example:
.. code-block:: python
builder.add_custom_values(
**{
"mujoco:pair_geom1": 0,
"mujoco:pair_geom2": 1,
"mujoco:pair_world": builder.current_world,
}
)
# Returns: {'mujoco:pair_geom1': 0, 'mujoco:pair_geom2': 0, 'mujoco:pair_world': 0}
"""
indices: dict[str, int] = {}
frequency_indices: dict[str, int] = {} # Track indices assigned per frequency in this call
for key, value in kwargs.items():
attr = self.custom_attributes.get(key)
if attr is None:
raise AttributeError(
f"Custom attribute '{key}' is not defined. Please declare it first using add_custom_attribute()."
)
if not attr.is_custom_frequency:
raise TypeError(
f"Custom attribute '{key}' has frequency={attr.frequency}, "
f"but add_custom_values() only works with custom frequency attributes."
)
# Ensure attr.values is initialized
if attr.values is None:
attr.values = []
freq_key = attr.frequency
assert isinstance(freq_key, str), f"Custom frequency '{freq_key}' is not a string"
# Determine index for this frequency (same index for all attrs with same frequency in this call)
if freq_key not in frequency_indices:
# First attribute with this frequency - use authoritative counter
current_count = self._custom_frequency_counts.get(freq_key, 0)
frequency_indices[freq_key] = current_count
# Update authoritative counter for this frequency
self._custom_frequency_counts[freq_key] = current_count + 1
idx = frequency_indices[freq_key]
# Ensure attr.values has length at least idx+1, padding with None as needed
while len(attr.values) <= idx:
attr.values.append(None)
# Assign value at the correct index
attr.values[idx] = value
indices[key] = idx
return indices
def add_custom_values_batch(self, entries: Sequence[dict[str, Any]]) -> list[dict[str, int]]:
"""Append multiple custom-frequency rows in a single call.
Args:
entries: Sequence of rows where each row maps custom attribute keys to values.
Each row is forwarded to :meth:`add_custom_values`.
Returns:
Index maps returned by :meth:`add_custom_values` for each row.
"""
out: list[dict[str, int]] = []
for row in entries:
out.append(self.add_custom_values(**row))
return out
def _process_custom_attributes(
self,
entity_index: int | list[int],
custom_attrs: dict[str, Any],
expected_frequency: Model.AttributeFrequency,
) -> None:
"""Process custom attributes from kwargs and assign them to an entity.
This method validates that custom attributes exist with the correct frequency,
then assigns values to the specific entity. The assignment is inferred from the
attribute definition.
Attribute names can optionally include a namespace prefix in the format "namespace:attr_name".
If no namespace prefix is provided, the attribute is assumed to be in the default namespace (None).
Args:
entity_index: Index of the entity (body, shape, joint, etc.). Can be a single index or a list of indices.
custom_attrs: Dictionary of custom attribute names to values.
Keys can be "attr_name" or "namespace:attr_name". Values can be a single value or a list of values.
expected_frequency: Expected frequency for these attributes.
"""
for attr_key, value in custom_attrs.items():
# Parse namespace prefix if present (format: "namespace:attr_name" or "attr_name")
full_key = attr_key
# Ensure the custom attribute is defined
custom_attr = self.custom_attributes.get(full_key)
if custom_attr is None:
raise AttributeError(
f"Custom attribute '{full_key}' is not defined. "
f"Please declare it first using add_custom_attribute()."
)
# Validate frequency matches
if custom_attr.frequency != expected_frequency:
raise ValueError(
f"Custom attribute '{full_key}' has frequency {custom_attr.frequency}, "
f"but expected {expected_frequency} for this entity type"
)
# Set the value for this specific entity
if custom_attr.values is None:
custom_attr.values = {}
# Fill in the value(s)
if isinstance(entity_index, list):
value_is_sequence = isinstance(value, (list, tuple))
if isinstance(value, np.ndarray):
value_is_sequence = value.ndim != 0
if value_is_sequence:
if len(value) != len(entity_index):
raise ValueError(f"Expected {len(entity_index)} values, got {len(value)}")
custom_attr.values.update(zip(entity_index, value, strict=False))
else:
custom_attr.values.update((idx, value) for idx in entity_index)
else:
custom_attr.values[entity_index] = value
def _process_joint_custom_attributes(
self,
joint_index: int,
custom_attrs: dict[str, Any],
) -> None:
"""Process custom attributes from kwargs for joints, supporting multiple frequencies.
Joint attributes are processed based on their declared frequency:
- JOINT frequency: Single value per joint
- JOINT_DOF frequency: List or dict of values for each DOF
- JOINT_COORD frequency: List or dict of values for each coordinate
For DOF and COORD attributes, values can be:
- A list with length matching the joint's DOF/coordinate count (all DOFs get values)
- A dict mapping DOF/coord indices to values (only specified indices get values, rest use defaults)
- A single scalar value, which is broadcast (replicated) to all DOFs/coordinates of the joint
For joints with zero DOFs (e.g., fixed joints), JOINT_DOF attributes are silently skipped
regardless of the value passed.
When using dict format, unspecified indices will be filled with the attribute's default value by
:meth:`finalize `.
Args:
joint_index: Index of the joint
custom_attrs: Dictionary of custom attribute names to values
"""
def apply_indexed_values(
*,
value: Any,
attr_key: str,
expected_frequency: Model.AttributeFrequency,
index_start: int,
index_count: int,
index_label: str,
count_label: str,
length_error_template: str,
) -> None:
# For joints with zero DOFs/coords (e.g., fixed joints), there is nothing to assign.
if index_count == 0:
return
if isinstance(value, dict):
for offset, offset_value in value.items():
if not isinstance(offset, int):
raise TypeError(
f"{expected_frequency.name} attribute '{attr_key}' dict keys must be integers "
f"({index_label} indices), got {type(offset)}"
)
if offset < 0 or offset >= index_count:
raise ValueError(
f"{expected_frequency.name} attribute '{attr_key}' has invalid {index_label} index "
f"{offset} (joint has {index_count} {count_label})"
)
self._process_custom_attributes(
entity_index=index_start + offset,
custom_attrs={attr_key: offset_value},
expected_frequency=expected_frequency,
)
return
value_sanitized = value
if isinstance(value_sanitized, np.ndarray):
if value_sanitized.ndim == 0:
value_sanitized = value_sanitized.item()
else:
value_sanitized = value_sanitized.tolist()
if not isinstance(value_sanitized, (list, tuple)):
# Broadcast a single scalar value to all DOFs/coords of the joint.
value_sanitized = [value_sanitized] * index_count
actual = len(value_sanitized)
if actual != index_count:
raise ValueError(length_error_template.format(attr_key=attr_key, actual=actual, expected=index_count))
for i, indexed_value in enumerate(value_sanitized):
self._process_custom_attributes(
entity_index=index_start + i,
custom_attrs={attr_key: indexed_value},
expected_frequency=expected_frequency,
)
for attr_key, value in custom_attrs.items():
# Look up the attribute to determine its frequency
custom_attr = self.custom_attributes.get(attr_key)
if custom_attr is None:
raise AttributeError(
f"Custom attribute '{attr_key}' is not defined. "
f"Please declare it first using add_custom_attribute()."
)
# Process based on declared frequency
if custom_attr.frequency == Model.AttributeFrequency.JOINT:
# Single value per joint
self._process_custom_attributes(
entity_index=joint_index,
custom_attrs={attr_key: value},
expected_frequency=Model.AttributeFrequency.JOINT,
)
elif custom_attr.frequency in (
Model.AttributeFrequency.JOINT_DOF,
Model.AttributeFrequency.JOINT_COORD,
Model.AttributeFrequency.JOINT_CONSTRAINT,
):
freq = custom_attr.frequency
freq_config = {
Model.AttributeFrequency.JOINT_DOF: (
self.joint_qd_start,
self.joint_dof_count,
"DOF",
"DOFs",
),
Model.AttributeFrequency.JOINT_COORD: (
self.joint_q_start,
self.joint_coord_count,
"coord",
"coordinates",
),
Model.AttributeFrequency.JOINT_CONSTRAINT: (
self.joint_cts_start,
self.joint_constraint_count,
"constraint",
"constraints",
),
}
start_array, total_count, index_label, count_label = freq_config[freq]
index_start = start_array[joint_index]
if joint_index + 1 < len(start_array):
index_end = start_array[joint_index + 1]
else:
index_end = total_count
apply_indexed_values(
value=value,
attr_key=attr_key,
expected_frequency=freq,
index_start=index_start,
index_count=index_end - index_start,
index_label=index_label,
count_label=count_label,
length_error_template=(
f"{freq.name} attribute '{{attr_key}}' has {{actual}} values "
f"but joint has {{expected}} {count_label}"
),
)
else:
raise ValueError(
f"Custom attribute '{attr_key}' has unsupported frequency {custom_attr.frequency} for joints"
)
def add_actuator(
self,
actuator_class: type[Actuator],
input_indices: list[int],
output_indices: list[int] | None = None,
**kwargs: Any,
) -> None:
"""Add an external actuator, independent of any ``UsdPhysics`` joint drives.
External actuators (e.g. :class:`newton_actuators.ActuatorPD`) apply
forces computed outside the physics engine. Multiple calls with the same
*actuator_class* and identical scalar parameters are accumulated into one
entry; the actuator instance is created during :meth:`finalize `.
Args:
actuator_class: The actuator class (must derive from
:class:`newton_actuators.Actuator`).
input_indices: DOF indices this actuator reads from. Length 1 for single-input,
length > 1 for multi-input actuators.
output_indices: DOF indices for writing output. Defaults to *input_indices*.
**kwargs: Actuator parameters (e.g., ``kp``, ``kd``, ``max_force``).
"""
if output_indices is None:
output_indices = input_indices.copy()
resolved = actuator_class.resolve_arguments(kwargs)
# Extract scalar params to form the entry key
scalar_param_names = getattr(actuator_class, "SCALAR_PARAMS", set())
scalar_key = tuple(sorted((k, resolved[k]) for k in scalar_param_names if k in resolved))
# Key is (class, scalar_params) so different scalar values create separate entries
entry_key = (actuator_class, scalar_key)
entry = self.actuator_entries.setdefault(
entry_key,
ModelBuilder.ActuatorEntry(input_indices=[], output_indices=[], args=[]),
)
# Filter out scalar params from args (they're already in the key)
array_params = {k: v for k, v in resolved.items() if k not in scalar_param_names}
# Validate dimension consistency before appending
if entry.input_indices:
expected_input_dim = len(entry.input_indices[0])
if len(input_indices) != expected_input_dim:
raise ValueError(
f"Input indices dimension mismatch for {actuator_class.__name__}: "
f"expected {expected_input_dim}, got {len(input_indices)}. "
f"All actuators of the same type must have the same number of inputs."
)
if entry.output_indices:
expected_output_dim = len(entry.output_indices[0])
if len(output_indices) != expected_output_dim:
raise ValueError(
f"Output indices dimension mismatch for {actuator_class.__name__}: "
f"expected {expected_output_dim}, got {len(output_indices)}. "
f"All actuators of the same type must have the same number of outputs."
)
# Each call adds one actuator with its input/output indices
entry.input_indices.append(input_indices)
entry.output_indices.append(output_indices)
entry.args.append(array_params)
def _stack_args_to_arrays(
self,
args_list: list[dict[str, Any]],
device: Devicelike | None = None,
requires_grad: bool = False,
) -> dict[str, wp.array]:
"""Convert list of per-index arg dicts into dict of warp arrays.
Args:
args_list: List of dicts, one per index. Each dict has same keys.
device: Device for warp arrays.
requires_grad: Whether the arrays require gradients.
Returns:
Mapping from parameter names to warp arrays.
"""
if not args_list:
return {}
result = {}
for key in args_list[0].keys():
values = [args[key] for args in args_list]
result[key] = wp.array(values, dtype=wp.float32, device=device, requires_grad=requires_grad)
return result
@staticmethod
def _build_index_array(indices: list[list[int]], device: Devicelike) -> wp.array:
"""Build a warp array from nested index lists.
If all inner lists have length 1, creates a 1D array (N,).
Otherwise, creates a 2D array (N, M) where M is the inner list length.
Args:
indices: Nested list of indices, one inner list per actuator.
device: Device for the warp array.
Returns:
Array with shape ``(N,)`` or ``(N, M)``.
Raises:
ValueError: If inner lists have inconsistent lengths (for 2D case).
"""
if not indices:
return wp.array([], dtype=wp.uint32, device=device)
inner_lengths = [len(idx_list) for idx_list in indices]
max_len = max(inner_lengths)
if max_len == 1:
# All single-input: flatten to 1D
flat = [idx_list[0] for idx_list in indices]
return wp.array(flat, dtype=wp.uint32, device=device)
else:
# Multi-input: create 2D array
if not all(length == max_len for length in inner_lengths):
raise ValueError(
f"All actuators must have the same number of inputs for 2D indexing. Got lengths: {inner_lengths}"
)
return wp.array(indices, dtype=wp.uint32, device=device)
@property
def default_site_cfg(self) -> ShapeConfig:
"""Returns a ShapeConfig configured for sites (non-colliding reference points).
This config has all site invariants enforced:
- is_site = True
- has_shape_collision = False
- has_particle_collision = False
- density = 0.0
- collision_group = 0
Returns:
A new configuration suitable for creating sites.
"""
cfg = self.ShapeConfig()
cfg.mark_as_site()
return cfg
@property
def up_vector(self) -> tuple[float, float, float]:
"""
Returns the 3D unit vector corresponding to the current up axis (read-only).
This property computes the up direction as a 3D vector based on the value of :attr:`up_axis`.
For example, if ``up_axis`` is ``Axis.Z``, this returns ``(0, 0, 1)``.
Returns:
The 3D up vector corresponding to the current up axis.
"""
return self.up_axis.to_vector()
@up_vector.setter
def up_vector(self, _):
raise AttributeError(
"The 'up_vector' property is read-only and cannot be set. Instead, use 'up_axis' to set the up axis."
)
@property
def current_world(self) -> int:
"""Returns the builder-managed world context for subsequently added entities.
A value of ``-1`` means newly added entities are global. Use
:meth:`begin_world`, :meth:`end_world`, :meth:`add_world`, or
:meth:`replicate` to manage world assignment.
Returns:
The current world index for newly added entities.
"""
return self._current_world
@current_world.setter
def current_world(self, _):
message = (
"The 'current_world' property is read-only and cannot be set. "
+ "Use 'begin_world()', 'end_world()', 'add_world()', or "
+ "'replicate()' to manage worlds."
)
raise AttributeError(message)
# region counts
@property
def shape_count(self):
"""
The number of shapes in the model.
"""
return len(self.shape_type)
@property
def body_count(self):
"""
The number of rigid bodies in the model.
"""
return len(self.body_q)
@property
def joint_count(self):
"""
The number of joints in the model.
"""
return len(self.joint_type)
@property
def particle_count(self):
"""
The number of particles in the model.
"""
return len(self.particle_q)
@property
def tri_count(self):
"""
The number of triangles in the model.
"""
return len(self.tri_poses)
@property
def tet_count(self):
"""
The number of tetrahedra in the model.
"""
return len(self.tet_poses)
@property
def edge_count(self):
"""
The number of edges (for bending) in the model.
"""
return len(self.edge_rest_angle)
@property
def spring_count(self):
"""
The number of springs in the model.
"""
return len(self.spring_rest_length)
@property
def muscle_count(self):
"""
The number of muscles in the model.
"""
return len(self.muscle_start)
@property
def articulation_count(self):
"""
The number of articulations in the model.
"""
return len(self.articulation_start)
# endregion
def replicate(
self,
builder: ModelBuilder,
world_count: int,
spacing: tuple[float, float, float] = (0.0, 0.0, 0.0),
):
"""
Replicates the given builder multiple times, offsetting each copy according to the supplied spacing.
This method is useful for creating multiple instances of a sub-model (e.g., robots, scenes)
arranged in a regular grid or along a line. Each copy is offset in space by a multiple of the
specified spacing vector, and all entities from each copy are assigned to a new world.
Note:
For visual separation of worlds, it is recommended to use the viewer's
`set_world_offsets()` method instead of physical spacing. This improves numerical
stability by keeping all worlds at the origin in the physics simulation.
Args:
builder: The builder to replicate. All entities from this builder will be copied.
world_count: The number of worlds to create.
spacing: The spacing between each copy along each axis.
For example, (5.0, 5.0, 0.0) arranges copies in a 2D grid in the XY plane.
Defaults to (0.0, 0.0, 0.0).
"""
offsets = compute_world_offsets(world_count, spacing, self.up_axis)
xform = wp.transform_identity()
for i in range(world_count):
xform[:3] = offsets[i]
self.add_world(builder, xform=xform)
def add_articulation(
self, joints: list[int], label: str | None = None, custom_attributes: dict[str, Any] | None = None
):
"""
Adds an articulation to the model from a list of joint indices.
The articulation is a set of joints that must be contiguous and monotonically increasing.
Some functions, such as forward kinematics :func:`newton.eval_fk`, are parallelized over articulations.
Args:
joints: List of joint indices to include in the articulation. Must be contiguous and monotonic.
label: The label of the articulation. If None, a default label will be created.
custom_attributes: Dictionary of custom attribute values for ARTICULATION frequency attributes.
Raises:
ValueError: If joints are not contiguous, not monotonic, or belong to different worlds.
Example:
.. code-block:: python
link1 = builder.add_link(...)
link2 = builder.add_link(...)
link3 = builder.add_link(...)
joint1 = builder.add_joint_revolute(parent=-1, child=link1)
joint2 = builder.add_joint_revolute(parent=link1, child=link2)
joint3 = builder.add_joint_revolute(parent=link2, child=link3)
# Create articulation from the joints
builder.add_articulation([joint1, joint2, joint3])
"""
if not joints:
raise ValueError("Cannot create an articulation with no joints")
# Sort joints to ensure we can validate them properly
sorted_joints = sorted(joints)
# Validate joints are monotonically increasing (no duplicates)
if sorted_joints != joints:
raise ValueError(
f"Joints must be provided in monotonically increasing order. Got {joints}, expected {sorted_joints}"
)
# Validate joints are contiguous
for i in range(1, len(sorted_joints)):
if sorted_joints[i] != sorted_joints[i - 1] + 1:
raise ValueError(
f"Joints must be contiguous. Got indices {sorted_joints}, but there is a gap between "
f"{sorted_joints[i - 1]} and {sorted_joints[i]}. Create all joints for an articulation "
f"before creating joints for another articulation."
)
# Validate all joints exist and don't already belong to an articulation
for joint_idx in joints:
if joint_idx < 0 or joint_idx >= len(self.joint_type):
raise ValueError(
f"Joint index {joint_idx} is out of range. Valid range is 0 to {len(self.joint_type) - 1}"
)
if self.joint_articulation[joint_idx] >= 0:
existing_art = self.joint_articulation[joint_idx]
raise ValueError(
f"Joint {joint_idx} ('{self.joint_label[joint_idx]}') already belongs to articulation {existing_art} "
f"('{self.articulation_label[existing_art]}'). Each joint can only belong to one articulation."
)
# Validate all joints belong to the same world (current world)
for joint_idx in joints:
if joint_idx < len(self.joint_world) and self.joint_world[joint_idx] != self.current_world:
raise ValueError(
f"Joint {joint_idx} belongs to world {self.joint_world[joint_idx]}, but current world is "
f"{self.current_world}. All joints in an articulation must belong to the same world."
)
# Basic tree structure validation (check for cycles, single parent)
# Build a simple tree structure check - each child should have only one parent in this articulation
child_to_parent = {}
for joint_idx in joints:
child = self.joint_child[joint_idx]
parent = self.joint_parent[joint_idx]
if child in child_to_parent and child_to_parent[child] != parent:
raise ValueError(
f"Body {child} has multiple parents in this articulation: {child_to_parent[child]} and {parent}. "
f"This creates an invalid tree structure. Loop-closing joints must not be part of an articulation."
)
child_to_parent[child] = parent
# Validate that only root bodies (parent == -1) can be kinematic
self._validate_kinematic_articulation_joints(joints)
# Store the articulation using the first joint's index as the start
articulation_idx = self.articulation_count
self.articulation_start.append(sorted_joints[0])
self.articulation_label.append(label or f"articulation_{articulation_idx}")
self.articulation_world.append(self.current_world)
# Mark all joints as belonging to this articulation
for joint_idx in joints:
self.joint_articulation[joint_idx] = articulation_idx
# Process custom attributes for this articulation
if custom_attributes:
self._process_custom_attributes(
entity_index=articulation_idx,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.ARTICULATION,
)
# region importers
def add_urdf(
self,
source: str,
*,
xform: Transform | None = None,
floating: bool | None = None,
base_joint: dict | None = None,
parent_body: int = -1,
scale: float = 1.0,
hide_visuals: bool = False,
parse_visuals_as_colliders: bool = False,
up_axis: AxisType = Axis.Z,
force_show_colliders: bool = False,
enable_self_collisions: bool = True,
ignore_inertial_definitions: bool = False,
joint_ordering: Literal["bfs", "dfs"] | None = "dfs",
bodies_follow_joint_ordering: bool = True,
collapse_fixed_joints: bool = False,
mesh_maxhullvert: int | None = None,
force_position_velocity_actuation: bool = False,
override_root_xform: bool = False,
):
"""
Parses a URDF file and adds the bodies and joints to the given ModelBuilder.
Args:
source: The filename of the URDF file to parse, or the URDF XML string content.
xform: The transform to apply to the root body. If None, the transform is set to identity.
override_root_xform: If ``True``, the articulation root's world-space
transform is replaced by ``xform`` instead of being composed with it,
preserving only the internal structure (relative body positions). Useful
for cloning articulations at explicit positions. When a ``base_joint`` is
specified, ``xform`` is applied as the full parent transform (including
rotation) rather than splitting position/rotation. Not intended for
sources containing multiple articulations, as all roots would be placed
at the same ``xform``. Defaults to ``False``.
floating: Controls the base joint type for the root body.
- ``None`` (default): Uses format-specific default (creates a FIXED joint for URDF).
- ``True``: Creates a FREE joint with 6 DOF (3 translation + 3 rotation). Only valid when
``parent_body == -1`` since FREE joints must connect to world frame.
- ``False``: Creates a FIXED joint (0 DOF).
Cannot be specified together with ``base_joint``.
base_joint: Custom joint specification for connecting the root body to the world
(or to ``parent_body`` if specified). This parameter enables hierarchical composition with
custom mobility. Dictionary with joint parameters as accepted by
:meth:`ModelBuilder.add_joint` (e.g., joint type, axes, limits, stiffness).
Cannot be specified together with ``floating``.
parent_body: Parent body index for hierarchical composition. If specified, attaches the
imported root body to this existing body, making them part of the same kinematic articulation.
The connection type is determined by ``floating`` or ``base_joint``. If ``-1`` (default),
the root connects to the world frame. **Restriction**: Only the most recently added
articulation can be used as parent; attempting to attach to an older articulation will raise
a ``ValueError``.
.. note::
Valid combinations of ``floating``, ``base_joint``, and ``parent_body``:
.. list-table::
:header-rows: 1
:widths: 15 15 15 55
* - floating
- base_joint
- parent_body
- Result
* - ``None``
- ``None``
- ``-1``
- Format default (URDF: FIXED joint)
* - ``True``
- ``None``
- ``-1``
- FREE joint to world (6 DOF)
* - ``False``
- ``None``
- ``-1``
- FIXED joint to world (0 DOF)
* - ``None``
- ``{dict}``
- ``-1``
- Custom joint to world (e.g., D6)
* - ``False``
- ``None``
- ``body_idx``
- FIXED joint to parent body
* - ``None``
- ``{dict}``
- ``body_idx``
- Custom joint to parent body (e.g., D6)
* - *explicitly set*
- *explicitly set*
- *any*
- ❌ Error: mutually exclusive (cannot specify both)
* - ``True``
- ``None``
- ``body_idx``
- ❌ Error: FREE joints require world frame
scale: The scaling factor to apply to the imported mechanism.
hide_visuals: If True, hide visual shapes.
parse_visuals_as_colliders: If True, the geometry defined under the `` tags is used for collision handling instead of the `` geometries.
up_axis: The up axis of the URDF. This is used to transform the URDF to the builder's up axis. It also determines the up axis of capsules and cylinders in the URDF. The default is Z.
force_show_colliders: If True, the collision shapes are always shown, even if there are visual shapes.
enable_self_collisions: If True, self-collisions are enabled.
ignore_inertial_definitions: If True, the inertial parameters defined in the URDF are ignored and the inertia is calculated from the shape geometry.
joint_ordering: The ordering of the joints in the simulation. Can be either "bfs" or "dfs" for breadth-first or depth-first search, or ``None`` to keep joints in the order in which they appear in the URDF. Default is "dfs".
bodies_follow_joint_ordering: If True, the bodies are added to the builder in the same order as the joints (parent then child body). Otherwise, bodies are added in the order they appear in the URDF. Default is True.
collapse_fixed_joints: If True, fixed joints are removed and the respective bodies are merged.
mesh_maxhullvert: Maximum vertices for convex hull approximation of meshes.
force_position_velocity_actuation: If True and both position (stiffness) and velocity
(damping) gains are non-zero, joints use :attr:`~newton.JointTargetMode.POSITION_VELOCITY` actuation mode.
If False (default), actuator modes are inferred per joint via :func:`newton.JointTargetMode.from_gains`:
:attr:`~newton.JointTargetMode.POSITION` if stiffness > 0, :attr:`~newton.JointTargetMode.VELOCITY` if only
damping > 0, :attr:`~newton.JointTargetMode.EFFORT` if a drive is present but both gains are zero
(direct torque control), or :attr:`~newton.JointTargetMode.NONE` if no drive/actuation is applied.
"""
from ..utils.import_urdf import parse_urdf # noqa: PLC0415
return parse_urdf(
self,
source,
xform=xform,
floating=floating,
base_joint=base_joint,
parent_body=parent_body,
scale=scale,
hide_visuals=hide_visuals,
parse_visuals_as_colliders=parse_visuals_as_colliders,
up_axis=up_axis,
force_show_colliders=force_show_colliders,
enable_self_collisions=enable_self_collisions,
ignore_inertial_definitions=ignore_inertial_definitions,
joint_ordering=joint_ordering,
bodies_follow_joint_ordering=bodies_follow_joint_ordering,
collapse_fixed_joints=collapse_fixed_joints,
mesh_maxhullvert=mesh_maxhullvert,
force_position_velocity_actuation=force_position_velocity_actuation,
override_root_xform=override_root_xform,
)
def add_usd(
self,
source: str | UsdStage,
*,
xform: Transform | None = None,
floating: bool | None = None,
base_joint: dict | None = None,
parent_body: int = -1,
only_load_enabled_rigid_bodies: bool = False,
only_load_enabled_joints: bool = True,
joint_drive_gains_scaling: float = 1.0,
verbose: bool = False,
ignore_paths: list[str] | None = None,
collapse_fixed_joints: bool = False,
enable_self_collisions: bool = True,
apply_up_axis_from_stage: bool = False,
root_path: str = "/",
joint_ordering: Literal["bfs", "dfs"] | None = "dfs",
bodies_follow_joint_ordering: bool = True,
skip_mesh_approximation: bool = False,
load_sites: bool = True,
load_visual_shapes: bool = True,
hide_collision_shapes: bool = False,
force_show_colliders: bool = False,
parse_mujoco_options: bool = True,
mesh_maxhullvert: int | None = None,
schema_resolvers: list[SchemaResolver] | None = None,
force_position_velocity_actuation: bool = False,
override_root_xform: bool = False,
) -> dict[str, Any]:
"""Parses a Universal Scene Description (USD) stage containing UsdPhysics schema definitions for rigid-body articulations and adds the bodies, shapes and joints to the given ModelBuilder.
The USD description has to be either a path (file name or URL), or an existing USD stage instance that implements the `Stage `_ interface.
See :ref:`usd_parsing` for more information.
Args:
source: The file path to the USD file, or an existing USD stage instance.
xform: The transform to apply to the entire scene.
override_root_xform: If ``True``, the articulation root's world-space
transform is replaced by ``xform`` instead of being composed with it,
preserving only the internal structure (relative body positions). Useful
for cloning articulations at explicit positions. Not intended for sources
containing multiple articulations, as all roots would be placed at the
same ``xform``. Defaults to ``False``.
floating: Controls the base joint type for the root body (bodies not connected as
a child to any joint).
- ``None`` (default): Uses format-specific default (creates a FREE joint for USD bodies without joints).
- ``True``: Creates a FREE joint with 6 DOF (3 translation + 3 rotation). Only valid when
``parent_body == -1`` since FREE joints must connect to world frame.
- ``False``: Creates a FIXED joint (0 DOF).
Cannot be specified together with ``base_joint``.
base_joint: Custom joint specification for connecting the root body to the world
(or to ``parent_body`` if specified). This parameter enables hierarchical composition with
custom mobility. Dictionary with joint parameters as accepted by
:meth:`ModelBuilder.add_joint` (e.g., joint type, axes, limits, stiffness).
Cannot be specified together with ``floating``.
parent_body: Parent body index for hierarchical composition. If specified, attaches the
imported root body to this existing body, making them part of the same kinematic articulation.
The connection type is determined by ``floating`` or ``base_joint``. If ``-1`` (default),
the root connects to the world frame. **Restriction**: Only the most recently added
articulation can be used as parent; attempting to attach to an older articulation will raise
a ``ValueError``.
.. note::
Valid combinations of ``floating``, ``base_joint``, and ``parent_body``:
.. list-table::
:header-rows: 1
:widths: 15 15 15 55
* - floating
- base_joint
- parent_body
- Result
* - ``None``
- ``None``
- ``-1``
- Format default (USD: FREE joint for bodies without joints)
* - ``True``
- ``None``
- ``-1``
- FREE joint to world (6 DOF)
* - ``False``
- ``None``
- ``-1``
- FIXED joint to world (0 DOF)
* - ``None``
- ``{dict}``
- ``-1``
- Custom joint to world (e.g., D6)
* - ``False``
- ``None``
- ``body_idx``
- FIXED joint to parent body
* - ``None``
- ``{dict}``
- ``body_idx``
- Custom joint to parent body (e.g., D6)
* - *explicitly set*
- *explicitly set*
- *any*
- ❌ Error: mutually exclusive (cannot specify both)
* - ``True``
- ``None``
- ``body_idx``
- ❌ Error: FREE joints require world frame
only_load_enabled_rigid_bodies: If True, only rigid bodies which do not have `physics:rigidBodyEnabled` set to False are loaded.
only_load_enabled_joints: If True, only joints which do not have `physics:jointEnabled` set to False are loaded.
joint_drive_gains_scaling: The default scaling of the PD control gains (stiffness and damping), if not set in the PhysicsScene with as "newton:joint_drive_gains_scaling".
verbose: If True, print additional information about the parsed USD file. Default is False.
ignore_paths: A list of regular expressions matching prim paths to ignore.
collapse_fixed_joints: If True, fixed joints are removed and the respective bodies are merged. Only considered if not set on the PhysicsScene as "newton:collapse_fixed_joints".
enable_self_collisions: Default for whether self-collisions are enabled for all shapes within an articulation. Resolved via the schema resolver from ``newton:selfCollisionEnabled`` (NewtonArticulationRootAPI) or ``physxArticulation:enabledSelfCollisions``; if neither is authored, this value takes precedence.
apply_up_axis_from_stage: If True, the up axis of the stage will be used to set :attr:`newton.ModelBuilder.up_axis`. Otherwise, the stage will be rotated such that its up axis aligns with the builder's up axis. Default is False.
root_path: The USD path to import, defaults to "/".
joint_ordering: The ordering of the joints in the simulation. Can be either "bfs" or "dfs" for breadth-first or depth-first search, or ``None`` to keep joints in the order in which they appear in the USD. Default is "dfs".
bodies_follow_joint_ordering: If True, the bodies are added to the builder in the same order as the joints (parent then child body). Otherwise, bodies are added in the order they appear in the USD. Default is True.
skip_mesh_approximation: If True, mesh approximation is skipped. Otherwise, meshes are approximated according to the ``physics:approximation`` attribute defined on the UsdPhysicsMeshCollisionAPI (if it is defined). Default is False.
load_sites: If True, sites (prims with MjcSiteAPI) are loaded as non-colliding reference points. If False, sites are ignored. Default is True.
load_visual_shapes: If True, non-physics visual geometry is loaded. If False, visual-only shapes are ignored (sites are still controlled by ``load_sites``). Default is True.
hide_collision_shapes: If True, collision shapes on bodies that already
have visual-only geometry are hidden unconditionally, regardless of
whether the collider has authored PBR material data. Collision
shapes on bodies without visual-only geometry remain visible as a
rendering fallback. Default is False.
force_show_colliders: If True, collision shapes get the VISIBLE flag
regardless of whether visual shapes exist on the same body. Note that
``hide_collision_shapes=True`` still suppresses the VISIBLE flag for
colliders on bodies with visual-only geometry. Default is False.
parse_mujoco_options: Whether MuJoCo solver options from the PhysicsScene should be parsed. If False, solver options are not loaded and custom attributes retain their default values. Default is True.
mesh_maxhullvert: Maximum vertices for convex hull approximation of meshes. Note that an authored ``newton:maxHullVertices`` attribute on any shape with a ``NewtonMeshCollisionAPI`` will take priority over this value.
schema_resolvers: Resolver instances in priority order. Default is to only parse Newton-specific attributes.
Schema resolvers collect per-prim "solver-specific" attributes, see :ref:`schema_resolvers` for more information.
These include namespaced attributes such as ``newton:*``, ``physx*``
(e.g., ``physxScene:*``, ``physxRigidBody:*``, ``physxSDFMeshCollision:*``), and ``mjc:*`` that
are authored in the USD but not strictly required to build the simulation. This is useful for
inspection, experimentation, or custom pipelines that read these values via
``result["schema_attrs"]`` returned from ``parse_usd()``.
.. note::
Using the ``schema_resolvers`` argument is an experimental feature that may be removed or changed significantly in the future.
force_position_velocity_actuation: If True and both stiffness (kp) and damping (kd)
are non-zero, joints use :attr:`~newton.JointTargetMode.POSITION_VELOCITY` actuation mode.
If False (default), actuator modes are inferred per joint via :func:`newton.JointTargetMode.from_gains`:
:attr:`~newton.JointTargetMode.POSITION` if stiffness > 0, :attr:`~newton.JointTargetMode.VELOCITY` if only
damping > 0, :attr:`~newton.JointTargetMode.EFFORT` if a drive is present but both gains are zero
(direct torque control), or :attr:`~newton.JointTargetMode.NONE` if no drive/actuation is applied.
Returns:
The returned mapping has the following entries:
.. list-table::
:widths: 25 75
* - ``"fps"``
- USD stage frames per second
* - ``"duration"``
- Difference between end time code and start time code of the USD stage
* - ``"up_axis"``
- :class:`Axis` representing the stage's up axis ("X", "Y", or "Z")
* - ``"path_body_map"``
- Mapping from prim path (str) of a rigid body prim (e.g. that implements the PhysicsRigidBodyAPI) to the respective body index in :class:`~newton.ModelBuilder`
* - ``"path_joint_map"``
- Mapping from prim path (str) of a joint prim (e.g. that implements the PhysicsJointAPI) to the respective joint index in :class:`~newton.ModelBuilder`
* - ``"path_shape_map"``
- Mapping from prim path (str) of the UsdGeom to the respective shape index in :class:`~newton.ModelBuilder`
* - ``"path_shape_scale"``
- Mapping from prim path (str) of the UsdGeom to its respective 3D world scale
* - ``"mass_unit"``
- The stage's Kilograms Per Unit (KGPU) definition (1.0 by default)
* - ``"linear_unit"``
- The stage's Meters Per Unit (MPU) definition (1.0 by default)
* - ``"scene_attributes"``
- Dictionary of all attributes applied to the PhysicsScene prim
* - ``"collapse_results"``
- Dictionary returned by :meth:`newton.ModelBuilder.collapse_fixed_joints` if ``collapse_fixed_joints`` is True, otherwise None.
* - ``"physics_dt"``
- The resolved physics scene time step (float or None)
* - ``"schema_attrs"``
- Dictionary of collected per-prim schema attributes (dict)
* - ``"max_solver_iterations"``
- The resolved maximum solver iterations (int or None)
* - ``"path_body_relative_transform"``
- Mapping from prim path to relative transform for bodies merged via ``collapse_fixed_joints``
* - ``"path_original_body_map"``
- Mapping from prim path to original body index before ``collapse_fixed_joints``
* - ``"actuator_count"``
- Number of external actuators parsed from the USD stage
"""
from ..utils.import_usd import parse_usd # noqa: PLC0415
return parse_usd(
self,
source,
xform=xform,
floating=floating,
base_joint=base_joint,
parent_body=parent_body,
only_load_enabled_rigid_bodies=only_load_enabled_rigid_bodies,
only_load_enabled_joints=only_load_enabled_joints,
joint_drive_gains_scaling=joint_drive_gains_scaling,
verbose=verbose,
ignore_paths=ignore_paths,
collapse_fixed_joints=collapse_fixed_joints,
enable_self_collisions=enable_self_collisions,
apply_up_axis_from_stage=apply_up_axis_from_stage,
root_path=root_path,
joint_ordering=joint_ordering,
bodies_follow_joint_ordering=bodies_follow_joint_ordering,
skip_mesh_approximation=skip_mesh_approximation,
load_sites=load_sites,
load_visual_shapes=load_visual_shapes,
hide_collision_shapes=hide_collision_shapes,
force_show_colliders=force_show_colliders,
parse_mujoco_options=parse_mujoco_options,
mesh_maxhullvert=mesh_maxhullvert,
schema_resolvers=schema_resolvers,
force_position_velocity_actuation=force_position_velocity_actuation,
override_root_xform=override_root_xform,
)
def add_mjcf(
self,
source: str,
*,
xform: Transform | None = None,
floating: bool | None = None,
base_joint: dict | None = None,
parent_body: int = -1,
armature_scale: float = 1.0,
scale: float = 1.0,
hide_visuals: bool = False,
parse_visuals_as_colliders: bool = False,
parse_meshes: bool = True,
parse_sites: bool = True,
parse_visuals: bool = True,
parse_mujoco_options: bool = True,
up_axis: AxisType = Axis.Z,
ignore_names: Sequence[str] = (),
ignore_classes: Sequence[str] = (),
visual_classes: Sequence[str] = ("visual",),
collider_classes: Sequence[str] = ("collision",),
no_class_as_colliders: bool = True,
force_show_colliders: bool = False,
enable_self_collisions: bool = True,
ignore_inertial_definitions: bool = False,
collapse_fixed_joints: bool = False,
verbose: bool = False,
skip_equality_constraints: bool = False,
convert_3d_hinge_to_ball_joints: bool = False,
mesh_maxhullvert: int | None = None,
ctrl_direct: bool = False,
path_resolver: Callable[[str | None, str], str] | None = None,
override_root_xform: bool = False,
):
"""
Parses MuJoCo XML (MJCF) file and adds the bodies and joints to the given ModelBuilder.
MuJoCo-specific custom attributes are registered on the builder automatically.
Args:
source: The filename of the MuJoCo file to parse, or the MJCF XML string content.
xform: The transform to apply to the imported mechanism.
override_root_xform: If ``True``, the articulation root's world-space
transform is replaced by ``xform`` instead of being composed with it,
preserving only the internal structure (relative body positions). Useful
for cloning articulations at explicit positions. Not intended for sources
containing multiple articulations, as all roots would be placed at the
same ``xform``. Defaults to ``False``.
floating: Controls the base joint type for the root body.
- ``None`` (default): Uses format-specific default (honors ```` tags in MJCF,
otherwise creates a FIXED joint).
- ``True``: Creates a FREE joint with 6 DOF (3 translation + 3 rotation). Only valid when
``parent_body == -1`` since FREE joints must connect to world frame.
- ``False``: Creates a FIXED joint (0 DOF).
Cannot be specified together with ``base_joint``.
base_joint: Custom joint specification for connecting the root body to the world
(or to ``parent_body`` if specified). This parameter enables hierarchical composition with
custom mobility. Dictionary with joint parameters as accepted by
:meth:`ModelBuilder.add_joint` (e.g., joint type, axes, limits, stiffness).
Cannot be specified together with ``floating``.
parent_body: Parent body index for hierarchical composition. If specified, attaches the
imported root body to this existing body, making them part of the same kinematic articulation.
The connection type is determined by ``floating`` or ``base_joint``. If ``-1`` (default),
the root connects to the world frame. **Restriction**: Only the most recently added
articulation can be used as parent; attempting to attach to an older articulation will raise
a ``ValueError``.
.. note::
Valid combinations of ``floating``, ``base_joint``, and ``parent_body``:
.. list-table::
:header-rows: 1
:widths: 15 15 15 55
* - floating
- base_joint
- parent_body
- Result
* - ``None``
- ``None``
- ``-1``
- Format default (MJCF: honors ````, else FIXED)
* - ``True``
- ``None``
- ``-1``
- FREE joint to world (6 DOF)
* - ``False``
- ``None``
- ``-1``
- FIXED joint to world (0 DOF)
* - ``None``
- ``{dict}``
- ``-1``
- Custom joint to world (e.g., D6)
* - ``False``
- ``None``
- ``body_idx``
- FIXED joint to parent body
* - ``None``
- ``{dict}``
- ``body_idx``
- Custom joint to parent body (e.g., D6)
* - *explicitly set*
- *explicitly set*
- *any*
- ❌ Error: mutually exclusive (cannot specify both)
* - ``True``
- ``None``
- ``body_idx``
- ❌ Error: FREE joints require world frame
armature_scale: Scaling factor to apply to the MJCF-defined joint armature values.
scale: The scaling factor to apply to the imported mechanism.
hide_visuals: If True, hide visual shapes after loading them (affects visibility, not loading).
parse_visuals_as_colliders: If True, the geometry defined under the `visual_classes` tags is used for collision handling instead of the `collider_classes` geometries.
parse_meshes: Whether geometries of type `"mesh"` should be parsed. If False, geometries of type `"mesh"` are ignored.
parse_sites: Whether sites (non-colliding reference points) should be parsed. If False, sites are ignored.
parse_visuals: Whether visual geometries (non-collision shapes) should be loaded. If False, visual shapes are not loaded (different from `hide_visuals` which loads but hides them). Default is True.
parse_mujoco_options: Whether solver options from the MJCF `` tag should be parsed. If False, solver options are not loaded and custom attributes retain their default values. Default is True.
up_axis: The up axis of the MuJoCo scene. The default is Z up.
ignore_names: A list of regular expressions. Bodies and joints with a name matching one of the regular expressions will be ignored.
ignore_classes: A list of regular expressions. Bodies and joints with a class matching one of the regular expressions will be ignored.
visual_classes: A list of regular expressions. Visual geometries with a class matching one of the regular expressions will be parsed.
collider_classes: A list of regular expressions. Collision geometries with a class matching one of the regular expressions will be parsed.
no_class_as_colliders: If True, geometries without a class are parsed as collision geometries. If False, geometries without a class are parsed as visual geometries.
force_show_colliders: If True, the collision shapes are always shown, even if there are visual shapes.
enable_self_collisions: If True, self-collisions are enabled.
ignore_inertial_definitions: If True, the inertial parameters defined in the MJCF are ignored and the inertia is calculated from the shape geometry.
collapse_fixed_joints: If True, fixed joints are removed and the respective bodies are merged.
verbose: If True, print additional information about parsing the MJCF.
skip_equality_constraints: Whether tags should be parsed. If True, equality constraints are ignored.
convert_3d_hinge_to_ball_joints: If True, series of three hinge joints are converted to a single ball joint. Default is False.
mesh_maxhullvert: Maximum vertices for convex hull approximation of meshes.
ctrl_direct: If True, all actuators use :attr:`~newton.solvers.SolverMuJoCo.CtrlSource.CTRL_DIRECT` mode
where control comes directly from ``control.mujoco.ctrl`` (MuJoCo-native behavior).
See :ref:`custom_attributes` for details on custom attributes. If False (default), position/velocity
actuators use :attr:`~newton.solvers.SolverMuJoCo.CtrlSource.JOINT_TARGET` mode where control comes
from :attr:`newton.Control.joint_target_pos` and :attr:`newton.Control.joint_target_vel`.
path_resolver: Callback to resolve file paths. Takes (base_dir, file_path) and returns a resolved path. For elements, can return either a file path or XML content directly. For asset elements (mesh, texture, etc.), must return an absolute file path. The default resolver joins paths and returns absolute file paths.
"""
from ..solvers.mujoco.solver_mujoco import SolverMuJoCo # noqa: PLC0415
from ..utils.import_mjcf import parse_mjcf # noqa: PLC0415
SolverMuJoCo.register_custom_attributes(self)
return parse_mjcf(
self,
source,
xform=xform,
floating=floating,
base_joint=base_joint,
parent_body=parent_body,
armature_scale=armature_scale,
scale=scale,
hide_visuals=hide_visuals,
parse_visuals_as_colliders=parse_visuals_as_colliders,
parse_meshes=parse_meshes,
parse_sites=parse_sites,
parse_visuals=parse_visuals,
parse_mujoco_options=parse_mujoco_options,
up_axis=up_axis,
ignore_names=ignore_names,
ignore_classes=ignore_classes,
visual_classes=visual_classes,
collider_classes=collider_classes,
no_class_as_colliders=no_class_as_colliders,
force_show_colliders=force_show_colliders,
enable_self_collisions=enable_self_collisions,
ignore_inertial_definitions=ignore_inertial_definitions,
collapse_fixed_joints=collapse_fixed_joints,
verbose=verbose,
skip_equality_constraints=skip_equality_constraints,
convert_3d_hinge_to_ball_joints=convert_3d_hinge_to_ball_joints,
mesh_maxhullvert=mesh_maxhullvert,
ctrl_direct=ctrl_direct,
path_resolver=path_resolver,
override_root_xform=override_root_xform,
)
# endregion
# region World management methods
def begin_world(
self,
label: str | None = None,
attributes: dict[str, Any] | None = None,
gravity: Vec3 | None = None,
):
"""Begin a new world context for adding entities.
This method starts a new world scope where all subsequently added entities
(bodies, shapes, joints, particles, etc.) will be assigned to this world.
Use :meth:`end_world` to close the world context and return to the global scope.
**Important:** Worlds cannot be nested. You must call :meth:`end_world` before
calling :meth:`begin_world` again.
Args:
label: Optional unique identifier for this world. If None,
a default label "world_{index}" will be generated.
attributes: Optional custom attributes to associate
with this world for later use.
gravity: Optional gravity vector for this world. If None,
the world will use the builder's default gravity (computed from
``self.gravity`` and ``self.up_vector``).
Raises:
RuntimeError: If called when already inside a world context
(:attr:`current_world` is not ``-1``).
Example::
builder = ModelBuilder()
# Add global ground plane
builder.add_ground_plane() # Added to world -1 (global)
# Create world 0 with default gravity
builder.begin_world(label="robot_0")
builder.add_body(...) # Added to world 0
builder.add_shape_box(...) # Added to world 0
builder.end_world()
# Create world 1 with custom zero gravity
builder.begin_world(label="robot_1", gravity=(0.0, 0.0, 0.0))
builder.add_body(...) # Added to world 1
builder.add_shape_box(...) # Added to world 1
builder.end_world()
"""
if self.current_world != -1:
raise RuntimeError(
f"Cannot begin a new world: already in world context (current_world={self.current_world}). "
"Call end_world() first to close the current world context."
)
# Set the current world to the next available world index
self._current_world = self.world_count
self.world_count += 1
# Store world metadata if needed (for future use)
# Note: We might want to add world_label and world_attributes lists in __init__ if needed
# For now, we just track the world index
# Initialize this world's gravity
if gravity is not None:
self.world_gravity.append(gravity)
else:
up_vector = self.up_vector
self.world_gravity.append(
(up_vector[0] * self.gravity, up_vector[1] * self.gravity, up_vector[2] * self.gravity)
)
def end_world(self):
"""End the current world context and return to global scope.
After calling this method, subsequently added entities will be assigned
to the global world (-1) until :meth:`begin_world` is called again.
Raises:
RuntimeError: If called when not in a world context
(:attr:`current_world` is ``-1``).
Example::
builder = ModelBuilder()
builder.begin_world()
builder.add_body(...) # Added to current world
builder.end_world() # Return to global scope
builder.add_ground_plane() # Added to world -1 (global)
"""
if self.current_world == -1:
raise RuntimeError("Cannot end world: not currently in a world context (current_world is already -1).")
# Reset to global world
self._current_world = -1
def add_world(
self,
builder: ModelBuilder,
xform: Transform | None = None,
label_prefix: str | None = None,
):
"""Add a builder as a new world.
This is a convenience method that combines :meth:`begin_world`,
:meth:`add_builder`, and :meth:`end_world` into a single call.
It's the recommended way to add homogeneous worlds (multiple instances
of the same scene/robot).
Args:
builder: The builder containing entities to add as a new world.
xform: Optional transform to apply to all root bodies
in the builder. Useful for spacing out worlds visually.
label_prefix: Optional prefix prepended to all entity labels
from the source builder. Useful for distinguishing multiple instances
of the same model (e.g., ``"left_arm"`` vs ``"right_arm"``).
Raises:
RuntimeError: If called when already in a world context (via begin_world).
Example::
# Create a robot blueprint
robot = ModelBuilder()
robot.add_body(...)
robot.add_shape_box(...)
# Create main scene with multiple robot instances
scene = ModelBuilder()
scene.add_ground_plane() # Global ground plane
# Add multiple robot worlds
for i in range(3):
scene.add_world(robot) # Each robot is a separate world
"""
self.begin_world()
self.add_builder(builder, xform=xform, label_prefix=label_prefix)
self.end_world()
# endregion
def add_builder(
self,
builder: ModelBuilder,
xform: Transform | None = None,
label_prefix: str | None = None,
):
"""Copies the data from another `ModelBuilder` into this `ModelBuilder`.
All entities from the source builder are added to this builder's current world context
(the value of :attr:`current_world`). Any world assignments that existed in the source
builder are overwritten - all entities will be assigned to the active world context.
Use :meth:`begin_world`, :meth:`end_world`, :meth:`add_world`, or
:meth:`replicate` to manage world assignment. :attr:`current_world`
is read-only and should not be set directly.
Example::
main_builder = ModelBuilder()
sub_builder = ModelBuilder()
sub_builder.add_body(...)
sub_builder.add_shape_box(...)
# Adds all entities from sub_builder to main_builder's active
# world context (-1 by default)
main_builder.add_builder(sub_builder)
# With transform and label prefix
main_builder.add_builder(sub_builder, xform=wp.transform((1, 0, 0)), label_prefix="left")
Args:
builder: The model builder to copy data from.
xform: Optional offset transform applied to root bodies.
label_prefix: Optional prefix prepended to all entity labels
from the source builder. Labels are joined with ``/``
(e.g., ``"left/panda/base_link"``).
"""
if builder.up_axis != self.up_axis:
raise ValueError("Cannot add a builder with a different up axis.")
# Copy gravity from source builder
if self.current_world >= 0 and self.current_world < len(self.world_gravity):
# We're in a world context, update this world's gravity vector
builder_up = builder.up_vector
self.world_gravity[self.current_world] = (
builder_up[0] * builder.gravity,
builder_up[1] * builder.gravity,
builder_up[2] * builder.gravity,
)
elif self.current_world < 0:
# No world context (add_builder called directly), copy scalar gravity
self.gravity = builder.gravity
self._requested_contact_attributes.update(builder._requested_contact_attributes)
self._requested_state_attributes.update(builder._requested_state_attributes)
if xform is not None:
xform = wp.transform(*xform)
# explicitly resolve the transform multiplication function to avoid
# repeatedly resolving builtin overloads during shape transformation
transform_mul_cfunc = wp._src.context.runtime.core.wp_builtin_mul_transformf_transformf
# dispatches two transform multiplies to the native implementation
def transform_mul(a: wp.transform, b: wp.transform) -> wp.transform:
out = wp.transform.from_buffer(np.empty(7, dtype=np.float32))
transform_mul_cfunc(a, b, ctypes.byref(out))
return out
start_particle_idx = self.particle_count
start_body_idx = self.body_count
start_shape_idx = self.shape_count
start_joint_idx = self.joint_count
start_joint_dof_idx = self.joint_dof_count
start_joint_coord_idx = self.joint_coord_count
start_joint_constraint_idx = self.joint_constraint_count
start_articulation_idx = self.articulation_count
start_equality_constraint_idx = len(self.equality_constraint_type)
start_constraint_mimic_idx = len(self.constraint_mimic_joint0)
start_edge_idx = self.edge_count
start_triangle_idx = self.tri_count
start_tetrahedron_idx = self.tet_count
start_spring_idx = self.spring_count
if builder.particle_count:
self.particle_max_velocity = builder.particle_max_velocity
if xform is not None:
pos_offset = xform.p
else:
pos_offset = np.zeros(3)
self.particle_q.extend((np.array(builder.particle_q) + pos_offset).tolist())
# other particle attributes are added below
if builder.spring_count:
self.spring_indices.extend((np.array(builder.spring_indices, dtype=np.int32) + start_particle_idx).tolist())
if builder.edge_count:
# Update edge indices by adding offset, preserving -1 values
edge_indices = np.array(builder.edge_indices, dtype=np.int32)
mask = edge_indices != -1
edge_indices[mask] += start_particle_idx
self.edge_indices.extend(edge_indices.tolist())
if builder.tri_count:
self.tri_indices.extend((np.array(builder.tri_indices, dtype=np.int32) + start_particle_idx).tolist())
if builder.tet_count:
self.tet_indices.extend((np.array(builder.tet_indices, dtype=np.int32) + start_particle_idx).tolist())
builder_coloring_translated = [group + start_particle_idx for group in builder.particle_color_groups]
self.particle_color_groups = combine_independent_particle_coloring(
self.particle_color_groups, builder_coloring_translated
)
start_body_idx = self.body_count
start_shape_idx = self.shape_count
for s, b in enumerate(builder.shape_body):
if b > -1:
new_b = b + start_body_idx
self.shape_body.append(new_b)
self.shape_transform.append(builder.shape_transform[s])
else:
self.shape_body.append(-1)
# apply offset transform to root bodies
if xform is not None:
self.shape_transform.append(transform_mul(xform, builder.shape_transform[s]))
else:
self.shape_transform.append(builder.shape_transform[s])
for b, shapes in builder.body_shapes.items():
if b == -1:
self.body_shapes[-1].extend([s + start_shape_idx for s in shapes])
else:
self.body_shapes[b + start_body_idx] = [s + start_shape_idx for s in shapes]
if builder.joint_count:
start_q = len(self.joint_q)
start_X_p = len(self.joint_X_p)
self.joint_X_p.extend(builder.joint_X_p)
self.joint_q.extend(builder.joint_q)
if xform is not None:
for i in range(len(builder.joint_X_p)):
if builder.joint_type[i] == JointType.FREE:
qi = builder.joint_q_start[i]
xform_prev = wp.transform(*builder.joint_q[qi : qi + 7])
tf = transform_mul(xform, xform_prev)
qi += start_q
self.joint_q[qi : qi + 7] = tf
elif builder.joint_parent[i] == -1:
self.joint_X_p[start_X_p + i] = transform_mul(xform, builder.joint_X_p[i])
# offset the indices
self.articulation_start.extend([a + self.joint_count for a in builder.articulation_start])
new_parents = [p + start_body_idx if p != -1 else -1 for p in builder.joint_parent]
new_children = [c + start_body_idx for c in builder.joint_child]
self.joint_parent.extend(new_parents)
self.joint_child.extend(new_children)
# Update parent/child lookups
for p, c in zip(new_parents, new_children, strict=True):
if c not in self.joint_parents:
self.joint_parents[c] = [p]
else:
self.joint_parents[c].append(p)
if p not in self.joint_children:
self.joint_children[p] = [c]
elif c not in self.joint_children[p]:
self.joint_children[p].append(c)
self.joint_q_start.extend([c + self.joint_coord_count for c in builder.joint_q_start])
self.joint_qd_start.extend([c + self.joint_dof_count for c in builder.joint_qd_start])
self.joint_cts_start.extend([c + self.joint_constraint_count for c in builder.joint_cts_start])
if xform is not None:
for i in range(builder.body_count):
self.body_q.append(transform_mul(xform, builder.body_q[i]))
else:
self.body_q.extend(builder.body_q)
# Copy collision groups without modification
self.shape_collision_group.extend(builder.shape_collision_group)
# Copy collision filter pairs with offset
self.shape_collision_filter_pairs.extend(
[(i + start_shape_idx, j + start_shape_idx) for i, j in builder.shape_collision_filter_pairs]
)
# Handle world assignments
# For particles
if builder.particle_count > 0:
# Override all world indices with current world
particle_groups = [self.current_world] * builder.particle_count
self.particle_world.extend(particle_groups)
# For bodies
if builder.body_count > 0:
body_groups = [self.current_world] * builder.body_count
self.body_world.extend(body_groups)
# For shapes
if builder.shape_count > 0:
shape_worlds = [self.current_world] * builder.shape_count
self.shape_world.extend(shape_worlds)
# For joints
if builder.joint_count > 0:
s = [self.current_world] * builder.joint_count
self.joint_world.extend(s)
# Offset articulation indices for joints (-1 stays -1)
self.joint_articulation.extend(
[a + start_articulation_idx if a >= 0 else -1 for a in builder.joint_articulation]
)
# For articulations
if builder.articulation_count > 0:
articulation_groups = [self.current_world] * builder.articulation_count
self.articulation_world.extend(articulation_groups)
# For equality constraints
if len(builder.equality_constraint_type) > 0:
constraint_worlds = [self.current_world] * len(builder.equality_constraint_type)
self.equality_constraint_world.extend(constraint_worlds)
# Remap body and joint indices in equality constraints
self.equality_constraint_type.extend(builder.equality_constraint_type)
self.equality_constraint_body1.extend(
[b + start_body_idx if b != -1 else -1 for b in builder.equality_constraint_body1]
)
self.equality_constraint_body2.extend(
[b + start_body_idx if b != -1 else -1 for b in builder.equality_constraint_body2]
)
self.equality_constraint_anchor.extend(builder.equality_constraint_anchor)
self.equality_constraint_torquescale.extend(builder.equality_constraint_torquescale)
self.equality_constraint_relpose.extend(builder.equality_constraint_relpose)
self.equality_constraint_joint1.extend(
[j + start_joint_idx if j != -1 else -1 for j in builder.equality_constraint_joint1]
)
self.equality_constraint_joint2.extend(
[j + start_joint_idx if j != -1 else -1 for j in builder.equality_constraint_joint2]
)
self.equality_constraint_polycoef.extend(builder.equality_constraint_polycoef)
if label_prefix:
self.equality_constraint_label.extend(
f"{label_prefix}/{lbl}" if lbl else lbl for lbl in builder.equality_constraint_label
)
else:
self.equality_constraint_label.extend(builder.equality_constraint_label)
self.equality_constraint_enabled.extend(builder.equality_constraint_enabled)
# For mimic constraints
if len(builder.constraint_mimic_joint0) > 0:
constraint_worlds = [self.current_world] * len(builder.constraint_mimic_joint0)
self.constraint_mimic_world.extend(constraint_worlds)
# Remap joint indices in mimic constraints
self.constraint_mimic_joint0.extend(
[j + start_joint_idx if j != -1 else -1 for j in builder.constraint_mimic_joint0]
)
self.constraint_mimic_joint1.extend(
[j + start_joint_idx if j != -1 else -1 for j in builder.constraint_mimic_joint1]
)
self.constraint_mimic_coef0.extend(builder.constraint_mimic_coef0)
self.constraint_mimic_coef1.extend(builder.constraint_mimic_coef1)
self.constraint_mimic_enabled.extend(builder.constraint_mimic_enabled)
if label_prefix:
self.constraint_mimic_label.extend(
f"{label_prefix}/{lbl}" if lbl else lbl for lbl in builder.constraint_mimic_label
)
else:
self.constraint_mimic_label.extend(builder.constraint_mimic_label)
# Handle label attributes specially to support label_prefix
label_attrs = ["articulation_label", "body_label", "joint_label", "shape_label"]
for attr in label_attrs:
src = getattr(builder, attr)
dst = getattr(self, attr)
if label_prefix:
dst.extend(f"{label_prefix}/{lbl}" if lbl else lbl for lbl in src)
else:
dst.extend(src)
more_builder_attrs = [
"body_inertia",
"body_mass",
"body_inv_inertia",
"body_inv_mass",
"body_com",
"body_lock_inertia",
"body_flags",
"body_qd",
"joint_type",
"joint_enabled",
"joint_X_c",
"joint_armature",
"joint_axis",
"joint_dof_dim",
"joint_qd",
"joint_cts",
"joint_f",
"joint_act",
"joint_target_pos",
"joint_target_vel",
"joint_limit_lower",
"joint_limit_upper",
"joint_limit_ke",
"joint_limit_kd",
"joint_target_ke",
"joint_target_kd",
"joint_target_mode",
"joint_effort_limit",
"joint_velocity_limit",
"joint_friction",
"shape_flags",
"shape_type",
"shape_scale",
"shape_source",
"shape_color",
"shape_is_solid",
"shape_margin",
"shape_material_ke",
"shape_material_kd",
"shape_material_kf",
"shape_material_ka",
"shape_material_mu",
"shape_material_restitution",
"shape_material_mu_torsional",
"shape_material_mu_rolling",
"shape_material_kh",
"shape_collision_radius",
"shape_gap",
"shape_sdf_narrow_band_range",
"shape_sdf_max_resolution",
"shape_sdf_target_voxel_size",
"shape_sdf_texture_format",
"particle_qd",
"particle_mass",
"particle_radius",
"particle_flags",
"edge_rest_angle",
"edge_rest_length",
"edge_bending_properties",
"spring_rest_length",
"spring_stiffness",
"spring_damping",
"spring_control",
"tri_poses",
"tri_activations",
"tri_materials",
"tri_areas",
"tet_poses",
"tet_activations",
"tet_materials",
]
for attr in more_builder_attrs:
getattr(self, attr).extend(getattr(builder, attr))
self.joint_dof_count += builder.joint_dof_count
self.joint_coord_count += builder.joint_coord_count
self.joint_constraint_count += builder.joint_constraint_count
# Merge custom attributes from the sub-builder
# Shared offset map for both frequency and references
# Note: "world" is NOT included here - WORLD frequency is handled specially
entity_offsets = {
"body": start_body_idx,
"shape": start_shape_idx,
"joint": start_joint_idx,
"joint_dof": start_joint_dof_idx,
"joint_coord": start_joint_coord_idx,
"joint_constraint": start_joint_constraint_idx,
"articulation": start_articulation_idx,
"equality_constraint": start_equality_constraint_idx,
"constraint_mimic": start_constraint_mimic_idx,
"particle": start_particle_idx,
"edge": start_edge_idx,
"triangle": start_triangle_idx,
"tetrahedron": start_tetrahedron_idx,
"spring": start_spring_idx,
}
# Snapshot custom frequency counts BEFORE iteration (they get updated during merge)
custom_frequency_offsets = dict(self._custom_frequency_counts)
def get_offset(entity_or_key: str | None) -> int:
"""Get offset for an entity type or custom frequency."""
if entity_or_key is None:
return 0
if entity_or_key in entity_offsets:
return entity_offsets[entity_or_key]
if entity_or_key in custom_frequency_offsets:
return custom_frequency_offsets[entity_or_key]
if entity_or_key in builder._custom_frequency_counts:
return 0
raise ValueError(
f"Unknown references value '{entity_or_key}'. "
f"Valid values are: {list(entity_offsets.keys())} or custom frequencies."
)
for full_key, attr in builder.custom_attributes.items():
# Fast path: skip attributes with no values (avoids computing offsets/closures)
if not attr.values:
# Still need to declare empty attribute on first merge
if full_key not in self.custom_attributes:
freq_key = attr.frequency
mapped_values = [] if isinstance(freq_key, str) else {}
self.custom_attributes[full_key] = replace(attr, values=mapped_values)
continue
# Index offset based on frequency
freq_key = attr.frequency
if isinstance(freq_key, str):
# Custom frequency: offset by pre-merge count
index_offset = custom_frequency_offsets.get(freq_key, 0)
elif attr.frequency == Model.AttributeFrequency.ONCE:
index_offset = 0
elif attr.frequency == Model.AttributeFrequency.WORLD:
# WORLD frequency: indices are keyed by world index, not by offset
# When called via add_world(), current_world is the world being added
index_offset = 0 if self.current_world == -1 else self.current_world
else:
index_offset = get_offset(attr.frequency.name.lower())
# Value transformation based on references
use_current_world = attr.references == "world"
value_offset = 0 if use_current_world else get_offset(attr.references)
def transform_value(v, offset=value_offset, replace_with_world=use_current_world):
if replace_with_world:
return self.current_world
if offset == 0:
return v
# Handle integers, preserving negative sentinels (e.g., -1 means "invalid")
if isinstance(v, int):
return v + offset if v >= 0 else v
# Handle list/tuple explicitly, preserving negative sentinels in elements
if isinstance(v, (list, tuple)):
transformed = [x + offset if isinstance(x, int) and x >= 0 else x for x in v]
return type(v)(transformed)
# For other types (numpy, warp, etc.), try arithmetic offset
try:
return v + offset
except TypeError:
return v
# Declare the attribute if it doesn't exist in the main builder
merged = self.custom_attributes.get(full_key)
if merged is None:
if isinstance(freq_key, str):
# String frequency: copy list as-is (no offset for sequential data)
mapped_values = [transform_value(value) for value in attr.values]
else:
# Enum frequency: remap dict indices with offset
mapped_values = {index_offset + idx: transform_value(value) for idx, value in attr.values.items()}
self.custom_attributes[full_key] = replace(attr, values=mapped_values)
continue
# Prevent silent divergence if defaults differ
# Handle array/vector types by converting to comparable format
try:
defaults_match = merged.default == attr.default
# Handle array-like comparisons
if hasattr(defaults_match, "__iter__") and not isinstance(defaults_match, (str, bytes)):
defaults_match = all(defaults_match)
except (ValueError, TypeError):
# If comparison fails, assume they're different
defaults_match = False
if not defaults_match:
raise ValueError(
f"Custom attribute '{full_key}' default mismatch when merging builders: "
f"existing={merged.default}, incoming={attr.default}"
)
# Remap indices and copy values
if merged.values is None:
merged.values = [] if isinstance(freq_key, str) else {}
if isinstance(freq_key, str):
# String frequency: extend list with transformed values
new_values = [transform_value(value) for value in attr.values]
merged.values.extend(new_values)
else:
# Enum frequency: update dict with remapped indices
new_indices = {index_offset + idx: transform_value(value) for idx, value in attr.values.items()}
merged.values.update(new_indices)
# Carry over custom frequency registrations (including usd_prim_filter) from the source builder.
# This must happen before updating counts so that the destination builder has the full
# frequency metadata for USD parsing and future attribute additions.
for freq_key, freq_obj in builder.custom_frequencies.items():
if freq_key not in self.custom_frequencies:
self.custom_frequencies[freq_key] = freq_obj
# Update custom frequency counts once per unique frequency (not per attribute)
for freq_key, builder_count in builder._custom_frequency_counts.items():
offset = custom_frequency_offsets.get(freq_key, 0)
self._custom_frequency_counts[freq_key] = offset + builder_count
# Merge actuator entries from the sub-builder with offset DOF indices
for entry_key, sub_entry in builder.actuator_entries.items():
entry = self.actuator_entries.setdefault(
entry_key,
ModelBuilder.ActuatorEntry(input_indices=[], output_indices=[], args=[]),
)
# Offset indices by start_joint_dof_idx (each actuator's indices are a list)
for idx_list in sub_entry.input_indices:
entry.input_indices.append([idx + start_joint_dof_idx for idx in idx_list])
for idx_list in sub_entry.output_indices:
entry.output_indices.append([idx + start_joint_dof_idx for idx in idx_list])
entry.args.extend(sub_entry.args)
@staticmethod
def _coerce_mat33(value: Any) -> wp.mat33:
"""Coerce a mat33-like value into a wp.mat33 without triggering Warp row-vector constructor warnings."""
if wp.types.type_is_matrix(type(value)):
return value
if isinstance(value, (list, tuple)) and len(value) == 3:
rows = []
is_rows = True
for r in value:
if wp.types.type_is_vector(type(r)):
rows.append(wp.vec3(*r))
elif isinstance(r, (list, tuple, np.ndarray)) and len(r) == 3:
rows.append(wp.vec3(*r))
else:
is_rows = False
break
if is_rows:
return wp.matrix_from_rows(*rows)
if isinstance(value, np.ndarray) and value.shape == (3, 3):
return wp.mat33(*value.reshape(-1).tolist())
return wp.mat33(*value)
@staticmethod
def _external_warning_stacklevel() -> int:
frame = inspect.currentframe()
if frame is None:
return 2
frame = frame.f_back
stacklevel = 1
try:
while frame is not None and frame.f_code.co_filename == __file__:
frame = frame.f_back
stacklevel += 1
return stacklevel
finally:
del frame
@classmethod
def _warn_body_armature_arg_deprecated(cls) -> None:
warnings.warn(
cls._BODY_ARMATURE_ARG_DEPRECATION_MESSAGE,
DeprecationWarning,
stacklevel=cls._external_warning_stacklevel(),
)
@classmethod
def _warn_default_body_armature_deprecated(cls) -> None:
warnings.warn(
cls._DEFAULT_BODY_ARMATURE_DEPRECATION_MESSAGE,
DeprecationWarning,
stacklevel=cls._external_warning_stacklevel(),
)
@property
def default_body_armature(self) -> float:
"""Deprecated default body armature.
.. deprecated:: 1.1
Add any isotropic artificial inertia directly to ``inertia`` instead.
"""
self._warn_default_body_armature_deprecated()
return self._default_body_armature
@default_body_armature.setter
def default_body_armature(self, value: float) -> None:
self._warn_default_body_armature_deprecated()
self._default_body_armature = value
def add_link(
self,
xform: Transform | None = None,
armature: float | None = None,
com: Vec3 | None = None,
inertia: Mat33 | None = None,
mass: float = 0.0,
label: str | None = None,
lock_inertia: bool = False,
is_kinematic: bool = False,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a link (rigid body) to the model within an articulation.
This method creates a link without automatically adding a joint. To connect this link
to the articulation structure, you must explicitly call one of the joint methods
(e.g., :meth:`add_joint_revolute`, :meth:`add_joint_fixed`, etc.) after creating the link.
After calling this method and one of the joint methods, ensure that an articulation is created using :meth:`add_articulation`.
.. deprecated:: 1.1
The ``armature`` parameter is deprecated. Add any isotropic artificial
inertia directly to ``inertia`` instead.
Args:
xform: The location of the body in the world frame.
armature: Deprecated. Artificial inertia added to the body. If ``None``,
the deprecated default value from :attr:`default_body_armature` is used.
Add any isotropic artificial inertia directly to ``inertia`` instead.
com: The center of mass of the body w.r.t its origin. If None, the center of mass is assumed to be at the origin.
inertia: The 3x3 inertia tensor of the body (specified relative to the center of mass). If None, the inertia tensor is assumed to be zero.
mass: Mass of the body.
label: Label of the body (optional).
lock_inertia: If True, prevents subsequent shape additions from modifying this body's mass,
center of mass, or inertia. This does not affect merging behavior in
:meth:`collapse_fixed_joints`, which always accumulates mass and inertia across merged bodies.
is_kinematic: If True, the body is kinematic and does not respond to forces.
Only root bodies (bodies whose joint parent is ``-1``) may be kinematic.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
The index of the body in the model.
"""
if armature is not None and armature != 0.0:
self._warn_body_armature_arg_deprecated()
if xform is None:
xform = wp.transform()
else:
xform = wp.transform(*xform)
if com is None:
com = wp.vec3()
else:
com = axis_to_vec3(com)
if inertia is None:
inertia = wp.mat33()
else:
inertia = self._coerce_mat33(inertia)
body_id = len(self.body_mass)
# body data
if armature is None:
armature = self._default_body_armature
inertia = inertia + wp.mat33(np.eye(3, dtype=np.float32)) * armature
self.body_inertia.append(inertia)
self.body_mass.append(mass)
self.body_com.append(com)
self.body_lock_inertia.append(lock_inertia)
self.body_flags.append(int(BodyFlags.KINEMATIC) if is_kinematic else int(BodyFlags.DYNAMIC))
if mass > 0.0:
self.body_inv_mass.append(1.0 / mass)
else:
self.body_inv_mass.append(0.0)
if any(x for x in inertia):
self.body_inv_inertia.append(wp.inverse(inertia))
else:
self.body_inv_inertia.append(inertia)
self.body_q.append(xform)
self.body_qd.append(wp.spatial_vector())
self.body_label.append(label or f"body_{body_id}")
self.body_shapes[body_id] = []
self.body_world.append(self.current_world)
# Process custom attributes
if custom_attributes:
self._process_custom_attributes(
entity_index=body_id,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.BODY,
)
return body_id
def add_body(
self,
xform: Transform | None = None,
armature: float | None = None,
com: Vec3 | None = None,
inertia: Mat33 | None = None,
mass: float = 0.0,
label: str | None = None,
lock_inertia: bool = False,
is_kinematic: bool = False,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a stand-alone free-floating rigid body to the model.
This is a convenience method that creates a single-body articulation with a free joint,
allowing the body to move freely in 6 degrees of freedom. This is equivalent to calling:
1. :meth:`add_link` to create the body
2. :meth:`add_joint_free` to add a free joint connecting the body to the world
3. :meth:`add_articulation` to create a new articulation from the joint
For creating articulations with multiple linked bodies, use :meth:`add_link`,
the appropriate joint methods, and :meth:`add_articulation` directly.
.. deprecated:: 1.1
The ``armature`` parameter is deprecated. Add any isotropic artificial
inertia directly to ``inertia`` instead.
Args:
xform: The location of the body in the world frame.
armature: Deprecated. Artificial inertia added to the body. If ``None``,
the deprecated default value from :attr:`default_body_armature` is used.
Add any isotropic artificial inertia directly to ``inertia`` instead.
com: The center of mass of the body w.r.t its origin. If None, the center of mass is assumed to be at the origin.
inertia: The 3x3 inertia tensor of the body (specified relative to the center of mass). If None, the inertia tensor is assumed to be zero.
mass: Mass of the body.
label: Label of the body. When provided, the auto-created free joint and articulation
are assigned labels ``{label}_free_joint`` and ``{label}_articulation`` respectively.
lock_inertia: If True, prevents subsequent shape additions from modifying this body's mass,
center of mass, or inertia. This does not affect merging behavior in
:meth:`collapse_fixed_joints`, which always accumulates mass and inertia across merged bodies.
is_kinematic: If True, the body is kinematic and does not respond to forces.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
The index of the body in the model.
"""
body_id = self.add_link(
xform=xform,
armature=armature,
com=com,
inertia=inertia,
mass=mass,
label=label,
lock_inertia=lock_inertia,
is_kinematic=is_kinematic,
custom_attributes=custom_attributes,
)
# Add a free joint to make it float
joint_id = self.add_joint_free(
child=body_id,
label=f"{label}_free_joint" if label else None,
)
# Create an articulation from the joint
articulation_label = f"{label}_articulation" if label else None
self.add_articulation([joint_id], label=articulation_label)
return body_id
# region joints
def add_joint(
self,
joint_type: JointType,
parent: int,
child: int,
linear_axes: list[JointDofConfig] | None = None,
angular_axes: list[JointDofConfig] | None = None,
label: str | None = None,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""
Generic method to add any type of joint to this ModelBuilder.
Args:
joint_type: The type of joint to add (see :ref:`Joint types`).
parent: The index of the parent body (-1 is the world).
child: The index of the child body.
linear_axes: The linear axes (see :class:`JointDofConfig`) of the joint,
defined in the joint parent anchor frame.
angular_axes: The angular axes (see :class:`JointDofConfig`) of the joint,
defined in the joint parent anchor frame.
label: The label of the joint (optional).
parent_xform: The transform from the parent body frame to the joint parent anchor frame.
If None, the identity transform is used.
child_xform: The transform from the child body frame to the joint child anchor frame.
If None, the identity transform is used.
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled (not considered by :class:`SolverFeatherstone`).
custom_attributes: Dictionary of custom attribute keys (see :attr:`CustomAttribute.key`) to values. Note that custom attributes with frequency :attr:`Model.AttributeFrequency.JOINT_DOF` or :attr:`Model.AttributeFrequency.JOINT_COORD` can be provided as: (1) lists with length equal to the joint's DOF or coordinate count, (2) dicts mapping DOF/coordinate indices to values, or (3) a single scalar value that is broadcast to all DOFs/coordinates of the joint. For joints with zero DOFs (e.g., fixed joints), JOINT_DOF attributes are silently skipped. Custom attributes with frequency :attr:`Model.AttributeFrequency.JOINT` require a single value to be defined.
Returns:
The index of the added joint.
"""
if linear_axes is None:
linear_axes = []
if angular_axes is None:
angular_axes = []
if parent_xform is None:
parent_xform = wp.transform()
else:
parent_xform = wp.transform(*parent_xform)
if child_xform is None:
child_xform = wp.transform()
else:
child_xform = wp.transform(*child_xform)
# Validate that parent and child bodies belong to the current world
if parent != -1: # -1 means world/ground
if parent < 0 or parent >= len(self.body_world):
raise ValueError(f"Parent body index {parent} is out of range")
if self.body_world[parent] != self.current_world:
raise ValueError(
f"Cannot create joint: parent body {parent} belongs to world {self.body_world[parent]}, "
f"but current world is {self.current_world}"
)
if child < 0 or child >= len(self.body_world):
raise ValueError(f"Child body index {child} is out of range")
if self.body_world[child] != self.current_world:
raise ValueError(
f"Cannot create joint: child body {child} belongs to world {self.body_world[child]}, "
f"but current world is {self.current_world}"
)
self.joint_type.append(joint_type)
self.joint_parent.append(parent)
if child not in self.joint_parents:
self.joint_parents[child] = [parent]
else:
self.joint_parents[child].append(parent)
if parent not in self.joint_children:
self.joint_children[parent] = [child]
elif child not in self.joint_children[parent]:
self.joint_children[parent].append(child)
self.joint_child.append(child)
self.joint_X_p.append(parent_xform)
self.joint_X_c.append(child_xform)
self.joint_label.append(label or f"joint_{self.joint_count}")
self.joint_dof_dim.append((len(linear_axes), len(angular_axes)))
self.joint_enabled.append(enabled)
self.joint_world.append(self.current_world)
self.joint_articulation.append(-1)
def add_axis_dim(dim: ModelBuilder.JointDofConfig):
self.joint_axis.append(dim.axis)
self.joint_target_pos.append(dim.target_pos)
self.joint_target_vel.append(dim.target_vel)
# Use actuator_mode if explicitly set, otherwise infer from gains
if dim.actuator_mode is not None:
mode = int(dim.actuator_mode)
else:
# Infer has_drive from whether gains are non-zero: non-zero gains imply a drive exists.
# This ensures freejoints (gains=0) get NONE, while joints with gains get appropriate mode.
has_drive = dim.target_ke != 0.0 or dim.target_kd != 0.0
mode = int(JointTargetMode.from_gains(dim.target_ke, dim.target_kd, has_drive=has_drive))
# Store per-DOF actuator properties
self.joint_target_mode.append(mode)
self.joint_target_ke.append(dim.target_ke)
self.joint_target_kd.append(dim.target_kd)
self.joint_limit_ke.append(dim.limit_ke)
self.joint_limit_kd.append(dim.limit_kd)
self.joint_armature.append(dim.armature)
self.joint_effort_limit.append(dim.effort_limit)
self.joint_velocity_limit.append(dim.velocity_limit)
self.joint_friction.append(dim.friction)
if np.isfinite(dim.limit_lower):
self.joint_limit_lower.append(dim.limit_lower)
else:
self.joint_limit_lower.append(-MAXVAL)
if np.isfinite(dim.limit_upper):
self.joint_limit_upper.append(dim.limit_upper)
else:
self.joint_limit_upper.append(MAXVAL)
for dim in linear_axes:
add_axis_dim(dim)
for dim in angular_axes:
add_axis_dim(dim)
dof_count, coord_count = joint_type.dof_count(len(linear_axes) + len(angular_axes))
cts_count = joint_type.constraint_count(len(linear_axes) + len(angular_axes))
for _ in range(coord_count):
self.joint_q.append(0.0)
for _ in range(dof_count):
self.joint_qd.append(0.0)
self.joint_f.append(0.0)
self.joint_act.append(0.0)
for _ in range(cts_count):
self.joint_cts.append(0.0)
if joint_type == JointType.FREE or joint_type == JointType.DISTANCE or joint_type == JointType.BALL:
# ensure that a valid quaternion is used for the angular dofs
self.joint_q[-1] = 1.0
self.joint_q_start.append(self.joint_coord_count)
self.joint_qd_start.append(self.joint_dof_count)
self.joint_cts_start.append(self.joint_constraint_count)
self.joint_dof_count += dof_count
self.joint_coord_count += coord_count
self.joint_constraint_count += cts_count
if collision_filter_parent and parent > -1:
for child_shape in self.body_shapes[child]:
if not self.shape_flags[child_shape] & ShapeFlags.COLLIDE_SHAPES:
continue
for parent_shape in self.body_shapes[parent]:
if not self.shape_flags[parent_shape] & ShapeFlags.COLLIDE_SHAPES:
continue
self.add_shape_collision_filter_pair(parent_shape, child_shape)
joint_index = self.joint_count - 1
# Process custom attributes
if custom_attributes:
self._process_joint_custom_attributes(
joint_index=joint_index,
custom_attrs=custom_attributes,
)
return joint_index
def add_joint_revolute(
self,
parent: int,
child: int,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
axis: AxisType | Vec3 | JointDofConfig | None = None,
target_pos: float | None = None,
target_vel: float | None = None,
target_ke: float | None = None,
target_kd: float | None = None,
limit_lower: float | None = None,
limit_upper: float | None = None,
limit_ke: float | None = None,
limit_kd: float | None = None,
armature: float | None = None,
effort_limit: float | None = None,
velocity_limit: float | None = None,
friction: float | None = None,
actuator_mode: JointTargetMode | None = None,
label: str | None = None,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
**kwargs,
) -> int:
"""Adds a revolute (hinge) joint to the model. It has one degree of freedom.
Args:
parent: The index of the parent body.
child: The index of the child body.
parent_xform: The transform from the parent body frame to the joint parent anchor frame.
child_xform: The transform from the child body frame to the joint child anchor frame.
axis: The axis of rotation in the joint parent anchor frame, which is
the parent body's local frame transformed by `parent_xform`. It can be a :class:`JointDofConfig` object
whose settings will be used instead of the other arguments.
target_pos: The target position of the joint.
target_vel: The target velocity of the joint.
target_ke: The stiffness of the joint target.
target_kd: The damping of the joint target.
limit_lower: The lower limit of the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_lower`` is used.
limit_upper: The upper limit of the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_upper`` is used.
limit_ke: The stiffness of the joint limit. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_ke`` is used.
limit_kd: The damping of the joint limit. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_kd`` is used.
armature: Artificial inertia added around the joint axis. If None, the default value from ``ModelBuilder.default_joint_cfg.armature`` is used.
effort_limit: Maximum effort (force/torque) the joint axis can exert. If None, the default value from ``ModelBuilder.default_joint_cfg.effort_limit`` is used.
velocity_limit: Maximum velocity the joint axis can achieve. If None, the default value from ``ModelBuilder.default_joint_cfg.velocity_limit`` is used.
friction: Friction coefficient for the joint axis. If None, the default value from ``ModelBuilder.default_joint_cfg.friction`` is used.
label: The label of the joint.
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled.
custom_attributes: Dictionary of custom attribute values for JOINT, JOINT_DOF, or JOINT_COORD frequency attributes.
Returns:
The index of the added joint.
"""
if axis is None:
axis = self.default_joint_cfg.axis
if isinstance(axis, ModelBuilder.JointDofConfig):
ax = axis
else:
ax = ModelBuilder.JointDofConfig(
axis=axis,
limit_lower=limit_lower if limit_lower is not None else self.default_joint_cfg.limit_lower,
limit_upper=limit_upper if limit_upper is not None else self.default_joint_cfg.limit_upper,
target_pos=target_pos if target_pos is not None else self.default_joint_cfg.target_pos,
target_vel=target_vel if target_vel is not None else self.default_joint_cfg.target_vel,
target_ke=target_ke if target_ke is not None else self.default_joint_cfg.target_ke,
target_kd=target_kd if target_kd is not None else self.default_joint_cfg.target_kd,
limit_ke=limit_ke if limit_ke is not None else self.default_joint_cfg.limit_ke,
limit_kd=limit_kd if limit_kd is not None else self.default_joint_cfg.limit_kd,
armature=armature if armature is not None else self.default_joint_cfg.armature,
effort_limit=effort_limit if effort_limit is not None else self.default_joint_cfg.effort_limit,
velocity_limit=velocity_limit if velocity_limit is not None else self.default_joint_cfg.velocity_limit,
friction=friction if friction is not None else self.default_joint_cfg.friction,
actuator_mode=actuator_mode if actuator_mode is not None else self.default_joint_cfg.actuator_mode,
)
return self.add_joint(
JointType.REVOLUTE,
parent,
child,
parent_xform=parent_xform,
child_xform=child_xform,
angular_axes=[ax],
label=label,
collision_filter_parent=collision_filter_parent,
enabled=enabled,
custom_attributes=custom_attributes,
**kwargs,
)
def add_joint_prismatic(
self,
parent: int,
child: int,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
axis: AxisType | Vec3 | JointDofConfig = Axis.X,
target_pos: float | None = None,
target_vel: float | None = None,
target_ke: float | None = None,
target_kd: float | None = None,
limit_lower: float | None = None,
limit_upper: float | None = None,
limit_ke: float | None = None,
limit_kd: float | None = None,
armature: float | None = None,
effort_limit: float | None = None,
velocity_limit: float | None = None,
friction: float | None = None,
actuator_mode: JointTargetMode | None = None,
label: str | None = None,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a prismatic (sliding) joint to the model. It has one degree of freedom.
Args:
parent: The index of the parent body.
child: The index of the child body.
parent_xform: The transform from the parent body frame to the joint parent anchor frame.
child_xform: The transform from the child body frame to the joint child anchor frame.
axis: The axis of translation in the joint parent anchor frame, which is
the parent body's local frame transformed by `parent_xform`. It can be a :class:`JointDofConfig` object
whose settings will be used instead of the other arguments.
target_pos: The target position of the joint.
target_vel: The target velocity of the joint.
target_ke: The stiffness of the joint target.
target_kd: The damping of the joint target.
limit_lower: The lower limit of the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_lower`` is used.
limit_upper: The upper limit of the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_upper`` is used.
limit_ke: The stiffness of the joint limit. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_ke`` is used.
limit_kd: The damping of the joint limit. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_kd`` is used.
armature: Artificial inertia added around the joint axis. If None, the default value from ``ModelBuilder.default_joint_cfg.armature`` is used.
effort_limit: Maximum effort (force) the joint axis can exert. If None, the default value from ``ModelBuilder.default_joint_cfg.effort_limit`` is used.
velocity_limit: Maximum velocity the joint axis can achieve. If None, the default value from ``ModelBuilder.default_joint_cfg.velocity_limit`` is used.
friction: Friction coefficient for the joint axis. If None, the default value from ``ModelBuilder.default_joint_cfg.friction`` is used.
label: The label of the joint.
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled.
custom_attributes: Dictionary of custom attribute values for JOINT, JOINT_DOF, or JOINT_COORD frequency attributes.
Returns:
The index of the added joint.
"""
if axis is None:
axis = self.default_joint_cfg.axis
if isinstance(axis, ModelBuilder.JointDofConfig):
ax = axis
else:
ax = ModelBuilder.JointDofConfig(
axis=axis,
limit_lower=limit_lower if limit_lower is not None else self.default_joint_cfg.limit_lower,
limit_upper=limit_upper if limit_upper is not None else self.default_joint_cfg.limit_upper,
target_pos=target_pos if target_pos is not None else self.default_joint_cfg.target_pos,
target_vel=target_vel if target_vel is not None else self.default_joint_cfg.target_vel,
target_ke=target_ke if target_ke is not None else self.default_joint_cfg.target_ke,
target_kd=target_kd if target_kd is not None else self.default_joint_cfg.target_kd,
limit_ke=limit_ke if limit_ke is not None else self.default_joint_cfg.limit_ke,
limit_kd=limit_kd if limit_kd is not None else self.default_joint_cfg.limit_kd,
armature=armature if armature is not None else self.default_joint_cfg.armature,
effort_limit=effort_limit if effort_limit is not None else self.default_joint_cfg.effort_limit,
velocity_limit=velocity_limit if velocity_limit is not None else self.default_joint_cfg.velocity_limit,
friction=friction if friction is not None else self.default_joint_cfg.friction,
actuator_mode=actuator_mode if actuator_mode is not None else self.default_joint_cfg.actuator_mode,
)
return self.add_joint(
JointType.PRISMATIC,
parent,
child,
parent_xform=parent_xform,
child_xform=child_xform,
linear_axes=[ax],
label=label,
collision_filter_parent=collision_filter_parent,
enabled=enabled,
custom_attributes=custom_attributes,
)
def add_joint_ball(
self,
parent: int,
child: int,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
armature: float | None = None,
friction: float | None = None,
label: str | None = None,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
actuator_mode: JointTargetMode | None = None,
) -> int:
"""Adds a ball (spherical) joint to the model. Its position is defined by a 4D quaternion (xyzw) and its velocity is a 3D vector.
Args:
parent: The index of the parent body.
child: The index of the child body.
parent_xform: The transform from the parent body frame to the joint parent anchor frame.
child_xform: The transform from the child body frame to the joint child anchor frame.
armature: Artificial inertia added around the joint axes. If None, the default value from ``ModelBuilder.default_joint_cfg.armature`` is used.
friction: Friction coefficient for the joint axes. If None, the default value from ``ModelBuilder.default_joint_cfg.friction`` is used.
label: The label of the joint.
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled.
custom_attributes: Dictionary of custom attribute values for JOINT, JOINT_DOF, or JOINT_COORD frequency attributes.
actuator_mode: The actuator mode for this joint's DOFs. If None, defaults to NONE.
Returns:
The index of the added joint.
.. note:: Target position and velocity control for ball joints is currently only supported in :class:`newton.solvers.SolverMuJoCo`.
"""
if armature is None:
armature = self.default_joint_cfg.armature
if friction is None:
friction = self.default_joint_cfg.friction
x = ModelBuilder.JointDofConfig(
axis=Axis.X,
armature=armature,
friction=friction,
actuator_mode=actuator_mode,
)
y = ModelBuilder.JointDofConfig(
axis=Axis.Y,
armature=armature,
friction=friction,
actuator_mode=actuator_mode,
)
z = ModelBuilder.JointDofConfig(
axis=Axis.Z,
armature=armature,
friction=friction,
actuator_mode=actuator_mode,
)
return self.add_joint(
JointType.BALL,
parent,
child,
parent_xform=parent_xform,
child_xform=child_xform,
angular_axes=[x, y, z],
label=label,
collision_filter_parent=collision_filter_parent,
enabled=enabled,
custom_attributes=custom_attributes,
)
def add_joint_fixed(
self,
parent: int,
child: int,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
label: str | None = None,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a fixed (static) joint to the model. It has no degrees of freedom.
See :meth:`collapse_fixed_joints` for a helper function that removes these fixed joints and merges the connecting bodies to simplify the model and improve stability.
Args:
parent: The index of the parent body.
child: The index of the child body.
parent_xform: The transform of the joint in the parent body's local frame.
child_xform: The transform of the joint in the child body's local frame.
label: The label of the joint.
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled.
custom_attributes: Dictionary of custom attribute values for JOINT frequency attributes.
Returns:
The index of the added joint
"""
joint_index = self.add_joint(
JointType.FIXED,
parent,
child,
parent_xform=parent_xform,
child_xform=child_xform,
label=label,
collision_filter_parent=collision_filter_parent,
enabled=enabled,
)
# Process custom attributes (only JOINT frequency is valid for fixed joints)
if custom_attributes:
self._process_joint_custom_attributes(joint_index, custom_attributes)
return joint_index
def add_joint_free(
self,
child: int,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
parent: int = -1,
label: str | None = None,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a free joint to the model.
It has 7 positional degrees of freedom (first 3 linear and then 4 angular dimensions for the orientation quaternion in `xyzw` notation) and 6 velocity degrees of freedom (see :ref:`Twist conventions in Newton `).
The positional dofs are initialized by the child body's transform (see :attr:`body_q` and the ``xform`` argument to :meth:`add_body`).
Args:
child: The index of the child body.
parent_xform: The transform of the joint in the parent body's local frame.
child_xform: The transform of the joint in the child body's local frame.
parent: The index of the parent body (-1 by default to use the world frame, e.g. to make the child body and its children a floating-base mechanism).
label: The label of the joint.
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled.
custom_attributes: Dictionary of custom attribute values for JOINT, JOINT_DOF, or JOINT_COORD frequency attributes.
Returns:
The index of the added joint.
"""
joint_id = self.add_joint(
JointType.FREE,
parent,
child,
parent_xform=parent_xform,
child_xform=child_xform,
label=label,
collision_filter_parent=collision_filter_parent,
enabled=enabled,
linear_axes=[
ModelBuilder.JointDofConfig.create_unlimited(Axis.X),
ModelBuilder.JointDofConfig.create_unlimited(Axis.Y),
ModelBuilder.JointDofConfig.create_unlimited(Axis.Z),
],
angular_axes=[
ModelBuilder.JointDofConfig.create_unlimited(Axis.X),
ModelBuilder.JointDofConfig.create_unlimited(Axis.Y),
ModelBuilder.JointDofConfig.create_unlimited(Axis.Z),
],
custom_attributes=custom_attributes,
)
q_start = self.joint_q_start[joint_id]
# set the positional dofs to the child body's transform
self.joint_q[q_start : q_start + 7] = list(self.body_q[child])
return joint_id
def add_joint_distance(
self,
parent: int,
child: int,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
min_distance: float = -1.0,
max_distance: float = 1.0,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a distance joint to the model. The distance joint constraints the distance between the joint anchor points on the two bodies (see :ref:`FK-IK`) it connects to the interval [`min_distance`, `max_distance`].
It has 7 positional degrees of freedom (first 3 linear and then 4 angular dimensions for the orientation quaternion in `xyzw` notation) and 6 velocity degrees of freedom (first 3 linear and then 3 angular velocity dimensions).
Args:
parent: The index of the parent body.
child: The index of the child body.
parent_xform: The transform of the joint in the parent body's local frame.
child_xform: The transform of the joint in the child body's local frame.
min_distance: The minimum distance between the bodies (no limit if negative).
max_distance: The maximum distance between the bodies (no limit if negative).
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled.
custom_attributes: Dictionary of custom attribute values for JOINT, JOINT_DOF, or JOINT_COORD frequency attributes.
Returns:
The index of the added joint.
.. note:: Distance joints are currently only supported in :class:`newton.solvers.SolverXPBD`.
"""
ax = ModelBuilder.JointDofConfig(
axis=(1.0, 0.0, 0.0),
limit_lower=min_distance,
limit_upper=max_distance,
)
return self.add_joint(
JointType.DISTANCE,
parent,
child,
parent_xform=parent_xform,
child_xform=child_xform,
linear_axes=[
ax,
ModelBuilder.JointDofConfig.create_unlimited(Axis.Y),
ModelBuilder.JointDofConfig.create_unlimited(Axis.Z),
],
angular_axes=[
ModelBuilder.JointDofConfig.create_unlimited(Axis.X),
ModelBuilder.JointDofConfig.create_unlimited(Axis.Y),
ModelBuilder.JointDofConfig.create_unlimited(Axis.Z),
],
collision_filter_parent=collision_filter_parent,
enabled=enabled,
custom_attributes=custom_attributes,
)
def add_joint_d6(
self,
parent: int,
child: int,
linear_axes: Sequence[JointDofConfig] | None = None,
angular_axes: Sequence[JointDofConfig] | None = None,
label: str | None = None,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
**kwargs,
) -> int:
"""Adds a generic joint with custom linear and angular axes. The number of axes determines the number of degrees of freedom of the joint.
Args:
parent: The index of the parent body.
child: The index of the child body.
linear_axes: A list of linear axes.
angular_axes: A list of angular axes.
label: The label of the joint.
parent_xform: The transform from the parent body frame to the joint parent anchor frame.
child_xform: The transform from the child body frame to the joint child anchor frame.
armature: Artificial inertia added around the joint axes. If None, the default value from ``ModelBuilder.default_joint_cfg.armature`` is used.
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled.
custom_attributes: Dictionary of custom attribute values for JOINT, JOINT_DOF, or JOINT_COORD frequency attributes.
Returns:
The index of the added joint.
"""
if linear_axes is None:
linear_axes = []
if angular_axes is None:
angular_axes = []
return self.add_joint(
JointType.D6,
parent,
child,
parent_xform=parent_xform,
child_xform=child_xform,
linear_axes=list(linear_axes),
angular_axes=list(angular_axes),
label=label,
collision_filter_parent=collision_filter_parent,
enabled=enabled,
custom_attributes=custom_attributes,
**kwargs,
)
def add_joint_cable(
self,
parent: int,
child: int,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
stretch_stiffness: float | None = None,
stretch_damping: float | None = None,
bend_stiffness: float | None = None,
bend_damping: float | None = None,
label: str | None = None,
collision_filter_parent: bool = True,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
**kwargs,
) -> int:
"""Adds a cable joint to the model. It has two degrees of freedom: one linear (stretch)
that constrains the distance between the attachment points, and one angular (bend/twist)
that penalizes the relative rotation of the attachment frames.
.. note::
Cable joints are supported by :class:`newton.solvers.SolverVBD`, which uses an
AVBD backend for rigid bodies. For cable joints, the stretch and bend behavior
is defined by the parent/child attachment transforms; the joint axis stored in
:class:`JointDofConfig` is not currently used directly.
Args:
parent: The index of the parent body.
child: The index of the child body.
parent_xform: The transform from the parent body frame to the joint parent anchor frame; its
translation is the attachment point.
child_xform: The transform from the child body frame to the joint child anchor frame; its
translation is the attachment point.
stretch_stiffness: Cable stretch stiffness (stored as ``target_ke``) [N/m]. If None, defaults to 1.0e9.
stretch_damping: Cable stretch damping (stored as ``target_kd``). In :class:`newton.solvers.SolverVBD`
this is a dimensionless (Rayleigh-style) coefficient. If None,
defaults to 0.0.
bend_stiffness: Cable bend/twist stiffness (stored as ``target_ke``) [N*m] (torque per radian). If None,
defaults to 0.0.
bend_damping: Cable bend/twist damping (stored as ``target_kd``). In :class:`newton.solvers.SolverVBD`
this is a dimensionless (Rayleigh-style) coefficient. If None,
defaults to 0.0.
label: The label of the joint.
collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
enabled: Whether the joint is enabled.
custom_attributes: Dictionary of custom attribute values for JOINT, JOINT_DOF, or JOINT_COORD
frequency attributes.
Returns:
The index of the added joint.
"""
# Linear DOF (stretch)
se_ke = 1.0e9 if stretch_stiffness is None else stretch_stiffness
se_kd = 0.0 if stretch_damping is None else stretch_damping
ax_lin = ModelBuilder.JointDofConfig(target_ke=se_ke, target_kd=se_kd)
# Angular DOF (bend/twist)
bend_ke = 0.0 if bend_stiffness is None else bend_stiffness
bend_kd = 0.0 if bend_damping is None else bend_damping
ax_ang = ModelBuilder.JointDofConfig(target_ke=bend_ke, target_kd=bend_kd)
return self.add_joint(
JointType.CABLE,
parent,
child,
parent_xform=parent_xform,
child_xform=child_xform,
linear_axes=[ax_lin],
angular_axes=[ax_ang],
label=label,
collision_filter_parent=collision_filter_parent,
enabled=enabled,
custom_attributes=custom_attributes,
**kwargs,
)
def add_equality_constraint(
self,
constraint_type: EqType,
body1: int = -1,
body2: int = -1,
anchor: Vec3 | None = None,
torquescale: float | None = None,
relpose: Transform | None = None,
joint1: int = -1,
joint2: int = -1,
polycoef: list[float] | None = None,
label: str | None = None,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Generic method to add any type of equality constraint to this ModelBuilder.
Args:
constraint_type: Equality constraint type. Use ``EqType.CONNECT`` to
pin a point to another body or the world, ``EqType.WELD`` to
constrain relative pose, or ``EqType.JOINT`` to couple two joints.
body1: Index of the first body participating in the constraint (-1 for world)
body2: Index of the second body participating in the constraint (-1 for world)
anchor: Anchor point on body1
torquescale: Scales the angular residual for weld
relpose: Relative pose of body2 for weld. If None, the identity transform is used.
joint1: Index of the first joint for joint coupling
joint2: Index of the second joint for joint coupling
polycoef: Five polynomial coefficients for ``EqType.JOINT`` coupling
label: Optional constraint label
enabled: Whether constraint is active
custom_attributes: Custom attributes to set on the constraint
Returns:
Constraint index
"""
if anchor is None:
anchor_vec = wp.vec3()
else:
anchor_vec = axis_to_vec3(anchor)
if relpose is None:
relpose_tf = wp.transform_identity()
else:
relpose_tf = wp.transform(*relpose)
if torquescale is None:
torquescale_value = 1.0 if constraint_type == EqType.WELD else 0.0
else:
torquescale_value = float(torquescale)
self.equality_constraint_type.append(constraint_type)
self.equality_constraint_body1.append(body1)
self.equality_constraint_body2.append(body2)
self.equality_constraint_anchor.append(anchor_vec)
self.equality_constraint_torquescale.append(torquescale_value)
self.equality_constraint_relpose.append(relpose_tf)
self.equality_constraint_joint1.append(joint1)
self.equality_constraint_joint2.append(joint2)
self.equality_constraint_polycoef.append(polycoef or [0.0, 0.0, 0.0, 0.0, 0.0])
self.equality_constraint_label.append(label or "")
self.equality_constraint_enabled.append(enabled)
self.equality_constraint_world.append(self.current_world)
constraint_idx = len(self.equality_constraint_type) - 1
# Process custom attributes
if custom_attributes:
self._process_custom_attributes(
entity_index=constraint_idx,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.EQUALITY_CONSTRAINT,
)
return constraint_idx
def add_equality_constraint_connect(
self,
body1: int = -1,
body2: int = -1,
anchor: Vec3 | None = None,
label: str | None = None,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a connect equality constraint to the model.
This constraint connects two bodies at a point. It effectively defines a ball joint outside the kinematic tree.
Args:
body1: Index of the first body participating in the constraint (-1 for world)
body2: Index of the second body participating in the constraint (-1 for world)
anchor: Anchor point on body1
label: Optional constraint label
enabled: Whether constraint is active
custom_attributes: Custom attributes to set on the constraint
Returns:
Constraint index
"""
return self.add_equality_constraint(
constraint_type=EqType.CONNECT,
body1=body1,
body2=body2,
anchor=anchor,
label=label,
enabled=enabled,
custom_attributes=custom_attributes,
)
def add_equality_constraint_joint(
self,
joint1: int = -1,
joint2: int = -1,
polycoef: list[float] | None = None,
label: str | None = None,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a joint equality constraint to the model.
Constrains the position or angle of one joint to be a quartic polynomial of another joint. Only scalar joint types (prismatic and revolute) can be used.
Args:
joint1: Index of the first joint
joint2: Index of the second joint
polycoef: Polynomial coefficients for joint coupling
label: Optional constraint label
enabled: Whether constraint is active
custom_attributes: Custom attributes to set on the constraint
Returns:
Constraint index
"""
return self.add_equality_constraint(
constraint_type=EqType.JOINT,
joint1=joint1,
joint2=joint2,
polycoef=polycoef,
label=label,
enabled=enabled,
custom_attributes=custom_attributes,
)
def add_equality_constraint_weld(
self,
body1: int = -1,
body2: int = -1,
anchor: Vec3 | None = None,
torquescale: float | None = None,
relpose: Transform | None = None,
label: str | None = None,
enabled: bool = True,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a weld equality constraint to the model.
Attaches two bodies to each other, removing all relative degrees of freedom between them (softly).
Args:
body1: Index of the first body participating in the constraint (-1 for world)
body2: Index of the second body participating in the constraint (-1 for world)
anchor: Coordinates of the weld point relative to body2
torquescale: Scales the angular residual for weld
relpose: Relative pose of body2 relative to body1. If None, the identity transform is used
label: Optional constraint label
enabled: Whether constraint is active
custom_attributes: Custom attributes to set on the constraint
Returns:
Constraint index
"""
return self.add_equality_constraint(
constraint_type=EqType.WELD,
body1=body1,
body2=body2,
anchor=anchor,
torquescale=torquescale,
relpose=relpose,
custom_attributes=custom_attributes,
label=label,
enabled=enabled,
)
def add_constraint_mimic(
self,
joint0: int,
joint1: int,
coef0: float = 0.0,
coef1: float = 1.0,
enabled: bool = True,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a mimic constraint to the model.
A mimic constraint enforces that ``joint0 = coef0 + coef1 * joint1``,
following URDF mimic joint semantics. Both scalar (prismatic, revolute) and
multi-DOF joints are supported. For multi-DOF joints, the mimic behavior is
applied equally to all degrees of freedom.
Args:
joint0: Index of the follower joint (the one being constrained)
joint1: Index of the leader joint (the one being mimicked)
coef0: Offset added after scaling
coef1: Scale factor applied to joint1's position/angle
enabled: Whether constraint is active
label: Optional constraint label
custom_attributes: Custom attributes to set on the constraint
Returns:
Constraint index
"""
joint_count = self.joint_count
if joint0 < 0 or joint0 >= joint_count:
raise ValueError(f"Invalid follower joint index {joint0}; expected 0..{joint_count - 1}")
if joint1 < 0 or joint1 >= joint_count:
raise ValueError(f"Invalid leader joint index {joint1}; expected 0..{joint_count - 1}")
if self.joint_world[joint0] != self.current_world or self.joint_world[joint1] != self.current_world:
raise ValueError(
"Mimic constraint joints must belong to the current world. "
f"joint0_world={self.joint_world[joint0]}, joint1_world={self.joint_world[joint1]}, "
f"current_world={self.current_world}."
)
self.constraint_mimic_joint0.append(joint0)
self.constraint_mimic_joint1.append(joint1)
self.constraint_mimic_coef0.append(coef0)
self.constraint_mimic_coef1.append(coef1)
self.constraint_mimic_enabled.append(enabled)
self.constraint_mimic_label.append(label or "")
self.constraint_mimic_world.append(self.current_world)
constraint_idx = len(self.constraint_mimic_joint0) - 1
# Process custom attributes
if custom_attributes:
self._process_custom_attributes(
entity_index=constraint_idx,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.CONSTRAINT_MIMIC,
)
return constraint_idx
# endregion
def plot_articulation(
self,
show_body_labels: bool = True,
show_joint_labels: bool = True,
show_joint_types: bool = True,
plot_shapes: bool = True,
show_shape_labels: bool = True,
show_shape_types: bool = True,
show_legend: bool = True,
) -> None:
"""
Visualizes the model's articulation graph using matplotlib and networkx.
Uses the spring layout algorithm from networkx to arrange the nodes.
Bodies are shown as orange squares, shapes are shown as blue circles.
Args:
show_body_labels: Whether to show the body labels or indices
show_joint_labels: Whether to show the joint labels or indices
show_joint_types: Whether to show the joint types
plot_shapes: Whether to render the shapes connected to the rigid bodies
show_shape_labels: Whether to show the shape labels or indices
show_shape_types: Whether to show the shape geometry types
show_legend: Whether to show a legend
"""
import matplotlib.pyplot as plt
import networkx as nx
def joint_type_str(type):
if type == JointType.FREE:
return "free"
elif type == JointType.BALL:
return "ball"
elif type == JointType.PRISMATIC:
return "prismatic"
elif type == JointType.REVOLUTE:
return "revolute"
elif type == JointType.D6:
return "D6"
elif type == JointType.FIXED:
return "fixed"
elif type == JointType.DISTANCE:
return "distance"
elif type == JointType.CABLE:
return "cable"
return "unknown"
def shape_type_str(type):
if type == GeoType.SPHERE:
return "sphere"
if type == GeoType.BOX:
return "box"
if type == GeoType.CAPSULE:
return "capsule"
if type == GeoType.CYLINDER:
return "cylinder"
if type == GeoType.CONE:
return "cone"
if type == GeoType.MESH:
return "mesh"
if type == GeoType.PLANE:
return "plane"
if type == GeoType.CONVEX_MESH:
return "convex_hull"
if type == GeoType.NONE:
return "none"
return "unknown"
if show_body_labels:
vertices = ["world", *self.body_label]
else:
vertices = ["-1"] + [str(i) for i in range(self.body_count)]
if plot_shapes:
for i in range(self.shape_count):
shape_label = []
if show_shape_labels:
shape_label.append(self.shape_label[i])
if show_shape_types:
shape_label.append(f"({shape_type_str(self.shape_type[i])})")
vertices.append("\n".join(shape_label))
edges = []
edge_labels = []
edge_colors = []
for i in range(self.joint_count):
edge = (self.joint_child[i] + 1, self.joint_parent[i] + 1)
edges.append(edge)
if show_joint_labels:
joint_label = self.joint_label[i]
else:
joint_label = str(i)
if show_joint_types:
joint_label += f"\n({joint_type_str(self.joint_type[i])})"
edge_labels.append(joint_label)
art_id = self.joint_articulation[i]
if art_id == -1:
edge_colors.append("r")
else:
edge_colors.append("k")
if plot_shapes:
for i in range(self.shape_count):
edges.append((len(self.body_label) + i + 1, self.shape_body[i] + 1))
# plot graph
G = nx.DiGraph()
for i in range(len(vertices)):
G.add_node(i, label=vertices[i])
for i in range(len(edges)):
label = edge_labels[i] if i < len(edge_labels) else ""
G.add_edge(edges[i][0], edges[i][1], label=label)
pos = nx.spring_layout(G, iterations=250)
# pos = nx.kamada_kawai_layout(G)
nx.draw_networkx_edges(G, pos, node_size=100, edgelist=edges, edge_color=edge_colors, arrows=True)
# render body vertices
draw_args = {"node_size": 100}
bodies = nx.subgraph(G, list(range(self.body_count + 1)))
nx.draw_networkx_nodes(bodies, pos, node_color="orange", node_shape="s", **draw_args)
if plot_shapes:
# render shape vertices
shapes = nx.subgraph(G, list(range(self.body_count + 1, len(vertices))))
nx.draw_networkx_nodes(shapes, pos, node_color="skyblue", **draw_args)
nx.draw_networkx_edges(
G, pos, node_size=0, edgelist=edges[self.joint_count :], edge_color="gray", style="dashed"
)
edge_labels = nx.get_edge_attributes(G, "label")
nx.draw_networkx_edge_labels(
G, pos, edge_labels=edge_labels, font_size=6, bbox={"alpha": 0.6, "color": "w", "lw": 0}
)
# add node labels
nx.draw_networkx_labels(G, pos, dict(enumerate(vertices)), font_size=6)
if show_legend:
plt.plot([], [], "s", color="orange", label="body")
plt.plot([], [], "k->", label="joint (child -> parent)")
if plot_shapes:
plt.plot([], [], "o", color="skyblue", label="shape")
plt.plot([], [], "k--", label="shape-body connection")
plt.legend(loc="upper left", fontsize=6)
plt.show()
def collapse_fixed_joints(
self, verbose: bool = wp.config.verbose, joints_to_keep: list[str] | None = None
) -> dict[str, Any]:
"""Removes fixed joints from the model and merges the bodies they connect. This is useful for simplifying the model for faster and more stable simulation.
Args:
verbose: If True, print additional information about the collapsed joints. Defaults to the value of `wp.config.verbose`.
joints_to_keep: An optional list of joint labels to be excluded from the collapse process.
"""
body_data = {}
body_children = {-1: []}
visited = {}
merged_body_data = {}
for i in range(self.body_count):
body_lbl = self.body_label[i]
inertia_i = self._coerce_mat33(self.body_inertia[i])
body_data[i] = {
"shapes": self.body_shapes[i],
"q": self.body_q[i],
"qd": self.body_qd[i],
"mass": self.body_mass[i],
"inertia": inertia_i,
"inv_mass": self.body_inv_mass[i],
"inv_inertia": self.body_inv_inertia[i],
"com": axis_to_vec3(self.body_com[i]),
"lock_inertia": self.body_lock_inertia[i],
"flags": self.body_flags[i],
"label": body_lbl,
"original_id": i,
}
visited[i] = False
body_children[i] = []
joint_data = {}
for i in range(self.joint_count):
joint_lbl = self.joint_label[i]
parent = self.joint_parent[i]
child = self.joint_child[i]
body_children[parent].append(child)
q_start = self.joint_q_start[i]
qd_start = self.joint_qd_start[i]
cts_start = self.joint_cts_start[i]
if i < self.joint_count - 1:
q_dim = self.joint_q_start[i + 1] - q_start
qd_dim = self.joint_qd_start[i + 1] - qd_start
cts_dim = self.joint_cts_start[i + 1] - cts_start
else:
q_dim = len(self.joint_q) - q_start
qd_dim = len(self.joint_qd) - qd_start
cts_dim = len(self.joint_cts) - cts_start
data = {
"type": self.joint_type[i],
"q": self.joint_q[q_start : q_start + q_dim],
"qd": self.joint_qd[qd_start : qd_start + qd_dim],
"cts": self.joint_cts[cts_start : cts_start + cts_dim],
"armature": self.joint_armature[qd_start : qd_start + qd_dim],
"q_start": q_start,
"qd_start": qd_start,
"cts_start": cts_start,
"label": joint_lbl,
"parent_xform": wp.transform_expand(self.joint_X_p[i]),
"child_xform": wp.transform_expand(self.joint_X_c[i]),
"enabled": self.joint_enabled[i],
"axes": [],
"axis_dim": self.joint_dof_dim[i],
"parent": parent,
"child": child,
"original_id": i,
}
num_lin_axes, num_ang_axes = self.joint_dof_dim[i]
for j in range(qd_start, qd_start + num_lin_axes + num_ang_axes):
data["axes"].append(
{
"axis": self.joint_axis[j],
"actuator_mode": self.joint_target_mode[j],
"target_ke": self.joint_target_ke[j],
"target_kd": self.joint_target_kd[j],
"limit_ke": self.joint_limit_ke[j],
"limit_kd": self.joint_limit_kd[j],
"limit_lower": self.joint_limit_lower[j],
"limit_upper": self.joint_limit_upper[j],
"target_pos": self.joint_target_pos[j],
"target_vel": self.joint_target_vel[j],
"effort_limit": self.joint_effort_limit[j],
}
)
joint_data[(parent, child)] = data
# sort body children so we traverse the tree in the same order as the bodies are listed
for children in body_children.values():
children.sort(key=lambda x: body_data[x]["original_id"])
# Find bodies referenced in equality constraints that shouldn't be merged into world
bodies_in_constraints = set()
for i in range(len(self.equality_constraint_body1)):
body1 = self.equality_constraint_body1[i]
body2 = self.equality_constraint_body2[i]
if body1 >= 0:
bodies_in_constraints.add(body1)
if body2 >= 0:
bodies_in_constraints.add(body2)
retained_joints = []
retained_bodies = []
body_remap = {-1: -1}
body_merged_parent = {}
body_merged_transform = {}
# depth first search over the joint graph
def dfs(parent_body: int, child_body: int, incoming_xform: wp.transform, last_dynamic_body: int):
nonlocal visited
nonlocal retained_joints
nonlocal retained_bodies
nonlocal body_data
nonlocal joints_to_keep
joint = joint_data[(parent_body, child_body)]
# Don't merge fixed joints if the child body is referenced in an equality constraint
# and would be merged into world (last_dynamic_body == -1)
should_skip_merge = child_body in bodies_in_constraints and last_dynamic_body == -1
# Don't merge fixed joints listed in joints_to_keep list
if joints_to_keep is None:
joints_to_keep = []
joint_in_keep_list = joint["label"] in joints_to_keep
if should_skip_merge and joint["type"] == JointType.FIXED:
# Skip merging this fixed joint because the body is referenced in an equality constraint
if verbose:
parent_lbl = self.body_label[parent_body] if parent_body > -1 else "world"
child_lbl = self.body_label[child_body]
print(
f"Skipping collapse of fixed joint {joint['label']} between {parent_lbl} and {child_lbl}: "
f"{child_lbl} is referenced in an equality constraint and cannot be merged into world"
)
if joint_in_keep_list and joint["type"] == JointType.FIXED:
# Skip merging this joint if it is listed in the joints_to_keep list
parent_lbl = self.body_label[parent_body] if parent_body > -1 else "world"
child_lbl = self.body_label[child_body]
if verbose:
print(
f"Skipping collapse of joint {joint['label']} between {parent_lbl} and {child_lbl}: "
f"{child_lbl} is listed in joints_to_keep and this fixed joint will be preserved"
)
# Warn if the child_body of skipped joint has zero or negative mass
if body_data[child_body]["mass"] <= 0:
warnings.warn(
f"Skipped joint {joint['label']} has a child {child_lbl} with zero or negative mass ({body_data[child_body]['mass']}). "
f"This may cause unexpected behavior.",
UserWarning,
stacklevel=3,
)
if joint["type"] == JointType.FIXED and not should_skip_merge and not joint_in_keep_list:
joint_xform = joint["parent_xform"] * wp.transform_inverse(joint["child_xform"])
incoming_xform = incoming_xform * joint_xform
parent_lbl = self.body_label[parent_body] if parent_body > -1 else "world"
child_lbl = self.body_label[child_body]
last_dynamic_body_label = self.body_label[last_dynamic_body] if last_dynamic_body > -1 else "world"
if verbose:
print(
f"Remove fixed joint {joint['label']} between {parent_lbl} and {child_lbl}, "
f"merging {child_lbl} into {last_dynamic_body_label}"
)
child_id = body_data[child_body]["original_id"]
relative_xform = incoming_xform
merged_body_data[self.body_label[child_body]] = {
"relative_xform": relative_xform,
"parent_body": self.body_label[parent_body],
}
body_merged_parent[child_body] = last_dynamic_body
body_merged_transform[child_body] = incoming_xform
for shape in self.body_shapes[child_id]:
shape_tf = self.shape_transform[shape]
self.shape_transform[shape] = incoming_xform * shape_tf
if verbose:
print(
f" Shape {shape} moved to body {last_dynamic_body_label} with transform {self.shape_transform[shape]}"
)
if last_dynamic_body > -1:
self.shape_body[shape] = body_data[last_dynamic_body]["id"]
body_data[last_dynamic_body]["shapes"].append(shape)
else:
self.shape_body[shape] = -1
self.body_shapes[-1].append(shape)
if last_dynamic_body > -1:
source_m = body_data[last_dynamic_body]["mass"]
source_com = body_data[last_dynamic_body]["com"]
# add inertia to last_dynamic_body
m = body_data[child_body]["mass"]
com = wp.transform_point(incoming_xform, body_data[child_body]["com"])
inertia = body_data[child_body]["inertia"]
body_data[last_dynamic_body]["inertia"] += transform_inertia(
m, inertia, incoming_xform.p, incoming_xform.q
)
body_data[last_dynamic_body]["mass"] += m
body_data[last_dynamic_body]["com"] = (m * com + source_m * source_com) / (m + source_m)
# indicate to recompute inverse mass, inertia for this body
body_data[last_dynamic_body]["inv_mass"] = None
else:
joint["parent_xform"] = incoming_xform * joint["parent_xform"]
joint["parent"] = last_dynamic_body
last_dynamic_body = child_body
incoming_xform = wp.transform()
retained_joints.append(joint)
new_id = len(retained_bodies)
body_data[child_body]["id"] = new_id
retained_bodies.append(child_body)
for shape in body_data[child_body]["shapes"]:
self.shape_body[shape] = new_id
visited[parent_body] = True
if visited[child_body] or child_body not in body_children:
return
visited[child_body] = True
for child in body_children[child_body]:
if not visited[child]:
dfs(child_body, child, incoming_xform, last_dynamic_body)
elif (child_body, child) in joint_data:
# Loop-closing joint: child was already visited via another path.
# Retain the joint but don't re-process the child body.
loop_joint = joint_data[(child_body, child)]
if loop_joint["type"] != JointType.FIXED:
loop_joint["parent_xform"] = incoming_xform * loop_joint["parent_xform"]
loop_joint["parent"] = last_dynamic_body
if child in body_merged_parent:
# Child was merged into another body — remap child and adjust child_xform
merge_xform = body_merged_transform[child]
loop_joint["child_xform"] = merge_xform * loop_joint["child_xform"]
loop_joint["child"] = body_merged_parent[child]
retained_joints.append(loop_joint)
for body in body_children[-1]:
if not visited[body]:
dfs(-1, body, wp.transform(), -1)
# Handle disconnected subtrees: bodies not reachable from world.
# This happens when joints only connect bodies to each other (no joint
# has parent == -1) and free joints to world were not auto-inserted
# (e.g. when no PhysicsArticulationRootAPI exists but joints are present).
children_in_joints = {c for p, cs in body_children.items() if p >= 0 for c in cs}
for body_id in range(self.body_count):
if visited[body_id]:
continue
if body_id in children_in_joints:
# Not a root — will be visited when its parent root is processed.
continue
# This body is a root of a disconnected subtree (or an isolated body).
new_id = len(retained_bodies)
body_data[body_id]["id"] = new_id
retained_bodies.append(body_id)
for shape in body_data[body_id]["shapes"]:
self.shape_body[shape] = new_id
visited[body_id] = True
for child in body_children[body_id]:
if not visited[child]:
dfs(body_id, child, wp.transform(), body_id)
# repopulate the model
# save original body groups before clearing
original_body_group = self.body_world[:] if self.body_world else []
self.body_label.clear()
self.body_q.clear()
self.body_qd.clear()
self.body_mass.clear()
self.body_inertia.clear()
self.body_com.clear()
self.body_lock_inertia.clear()
self.body_flags.clear()
self.body_inv_mass.clear()
self.body_inv_inertia.clear()
self.body_world.clear() # Clear body groups
static_shapes = self.body_shapes[-1]
self.body_shapes.clear()
# restore static shapes
self.body_shapes[-1] = static_shapes
for i in retained_bodies:
body = body_data[i]
new_id = len(self.body_label)
body_remap[body["original_id"]] = new_id
self.body_label.append(body["label"])
self.body_q.append(body["q"])
self.body_qd.append(body["qd"])
m = body["mass"]
inertia = body["inertia"]
self.body_mass.append(m)
self.body_inertia.append(inertia)
self.body_com.append(body["com"])
self.body_lock_inertia.append(body["lock_inertia"])
self.body_flags.append(body["flags"])
if body["inv_mass"] is None:
# recompute inverse mass and inertia
if m > 0.0:
self.body_inv_mass.append(1.0 / m)
self.body_inv_inertia.append(wp.inverse(inertia))
else:
self.body_inv_mass.append(0.0)
self.body_inv_inertia.append(wp.mat33(0.0))
else:
self.body_inv_mass.append(body["inv_mass"])
self.body_inv_inertia.append(body["inv_inertia"])
self.body_shapes[new_id] = body["shapes"]
# Rebuild body group - use original group if it exists
if original_body_group and body["original_id"] < len(original_body_group):
self.body_world.append(original_body_group[body["original_id"]])
else:
# If no group was assigned, use default -1
self.body_world.append(-1)
# sort joints so they appear in the same order as before
retained_joints.sort(key=lambda x: x["original_id"])
joint_remap = {}
for i, joint in enumerate(retained_joints):
joint_remap[joint["original_id"]] = i
# update articulation_start
for i, old_i in enumerate(self.articulation_start):
start_i = old_i
while start_i not in joint_remap:
start_i += 1
if start_i >= self.joint_count:
break
self.articulation_start[i] = joint_remap.get(start_i, start_i)
# remove empty articulation starts, i.e. where the start and end are the same
self.articulation_start = list(set(self.articulation_start))
# save original joint worlds and articulations before clearing
original_ = self.joint_world[:] if self.joint_world else []
original_articulation = self.joint_articulation[:] if self.joint_articulation else []
self.joint_label.clear()
self.joint_type.clear()
self.joint_parent.clear()
self.joint_child.clear()
self.joint_q.clear()
self.joint_qd.clear()
self.joint_cts.clear()
self.joint_q_start.clear()
self.joint_qd_start.clear()
self.joint_cts_start.clear()
self.joint_enabled.clear()
self.joint_armature.clear()
self.joint_X_p.clear()
self.joint_X_c.clear()
self.joint_axis.clear()
self.joint_target_mode.clear()
self.joint_target_ke.clear()
self.joint_target_kd.clear()
self.joint_limit_lower.clear()
self.joint_limit_upper.clear()
self.joint_limit_ke.clear()
self.joint_effort_limit.clear()
self.joint_limit_kd.clear()
self.joint_dof_dim.clear()
self.joint_target_pos.clear()
self.joint_target_vel.clear()
self.joint_world.clear()
self.joint_articulation.clear()
for joint in retained_joints:
self.joint_label.append(joint["label"])
self.joint_type.append(joint["type"])
self.joint_parent.append(body_remap[joint["parent"]])
self.joint_child.append(body_remap[joint["child"]])
self.joint_q_start.append(len(self.joint_q))
self.joint_qd_start.append(len(self.joint_qd))
self.joint_cts_start.append(len(self.joint_cts))
self.joint_q.extend(joint["q"])
self.joint_qd.extend(joint["qd"])
self.joint_cts.extend(joint["cts"])
self.joint_armature.extend(joint["armature"])
self.joint_enabled.append(joint["enabled"])
self.joint_X_p.append(joint["parent_xform"])
self.joint_X_c.append(joint["child_xform"])
self.joint_dof_dim.append(joint["axis_dim"])
# Rebuild joint world - use original world if it exists
if original_ and joint["original_id"] < len(original_):
self.joint_world.append(original_[joint["original_id"]])
else:
# If no world was assigned, use default -1
self.joint_world.append(-1)
# Rebuild joint articulation assignment
if original_articulation and joint["original_id"] < len(original_articulation):
self.joint_articulation.append(original_articulation[joint["original_id"]])
else:
self.joint_articulation.append(-1)
for axis in joint["axes"]:
self.joint_axis.append(axis["axis"])
self.joint_target_mode.append(axis["actuator_mode"])
self.joint_target_ke.append(axis["target_ke"])
self.joint_target_kd.append(axis["target_kd"])
self.joint_limit_lower.append(axis["limit_lower"])
self.joint_limit_upper.append(axis["limit_upper"])
self.joint_limit_ke.append(axis["limit_ke"])
self.joint_limit_kd.append(axis["limit_kd"])
self.joint_target_pos.append(axis["target_pos"])
self.joint_target_vel.append(axis["target_vel"])
self.joint_effort_limit.append(axis["effort_limit"])
# Update DOF and coordinate counts to match the rebuilt arrays
self.joint_dof_count = len(self.joint_qd)
self.joint_coord_count = len(self.joint_q)
# Trim per-DOF arrays that were not cleared/rebuilt above
for attr_name in ("joint_velocity_limit", "joint_friction"):
arr = getattr(self, attr_name)
if len(arr) > self.joint_dof_count:
setattr(self, attr_name, arr[: self.joint_dof_count])
# Reset the constraint count based on the retained joints
self.joint_constraint_count = len(self.joint_cts)
# Remap equality constraint body/joint indices and transform anchors for merged bodies
for i in range(len(self.equality_constraint_body1)):
old_body1 = self.equality_constraint_body1[i]
old_body2 = self.equality_constraint_body2[i]
body1_was_merged = False
body2_was_merged = False
if old_body1 in body_remap:
self.equality_constraint_body1[i] = body_remap[old_body1]
elif old_body1 in body_merged_parent:
self.equality_constraint_body1[i] = body_remap[body_merged_parent[old_body1]]
body1_was_merged = True
if old_body2 in body_remap:
self.equality_constraint_body2[i] = body_remap[old_body2]
elif old_body2 in body_merged_parent:
self.equality_constraint_body2[i] = body_remap[body_merged_parent[old_body2]]
body2_was_merged = True
constraint_type = self.equality_constraint_type[i]
# Transform anchor/relpose from merged body's frame to parent body's frame
if body1_was_merged:
merge_xform = body_merged_transform[old_body1]
if constraint_type == EqType.CONNECT:
anchor = axis_to_vec3(self.equality_constraint_anchor[i])
self.equality_constraint_anchor[i] = wp.transform_point(merge_xform, anchor)
if constraint_type == EqType.WELD:
relpose = self.equality_constraint_relpose[i]
self.equality_constraint_relpose[i] = merge_xform * relpose
if body2_was_merged and constraint_type == EqType.WELD:
merge_xform = body_merged_transform[old_body2]
anchor = axis_to_vec3(self.equality_constraint_anchor[i])
relpose = self.equality_constraint_relpose[i]
self.equality_constraint_anchor[i] = wp.transform_point(merge_xform, anchor)
self.equality_constraint_relpose[i] = relpose * wp.transform_inverse(merge_xform)
old_joint1 = self.equality_constraint_joint1[i]
old_joint2 = self.equality_constraint_joint2[i]
if old_joint1 in joint_remap:
self.equality_constraint_joint1[i] = joint_remap[old_joint1]
elif old_joint1 != -1:
if verbose:
print(f"Warning: Equality constraint references removed joint {old_joint1}, disabling constraint")
self.equality_constraint_enabled[i] = False
if old_joint2 in joint_remap:
self.equality_constraint_joint2[i] = joint_remap[old_joint2]
elif old_joint2 != -1:
if verbose:
print(f"Warning: Equality constraint references removed joint {old_joint2}, disabling constraint")
self.equality_constraint_enabled[i] = False
# Remap mimic constraint joint indices
for i in range(len(self.constraint_mimic_joint0)):
old_joint0 = self.constraint_mimic_joint0[i]
old_joint1 = self.constraint_mimic_joint1[i]
if old_joint0 in joint_remap:
self.constraint_mimic_joint0[i] = joint_remap[old_joint0]
elif old_joint0 != -1:
if verbose:
print(f"Warning: Mimic constraint references removed joint {old_joint0}, disabling constraint")
self.constraint_mimic_enabled[i] = False
if old_joint1 in joint_remap:
self.constraint_mimic_joint1[i] = joint_remap[old_joint1]
elif old_joint1 != -1:
if verbose:
print(f"Warning: Mimic constraint references removed joint {old_joint1}, disabling constraint")
self.constraint_mimic_enabled[i] = False
# Rebuild parent/child lookups
self.joint_parents.clear()
self.joint_children.clear()
for p, c in zip(self.joint_parent, self.joint_child, strict=True):
if c not in self.joint_parents:
self.joint_parents[c] = [p]
else:
self.joint_parents[c].append(p)
if p not in self.joint_children:
self.joint_children[p] = [c]
elif c not in self.joint_children[p]:
self.joint_children[p].append(c)
return {
"body_remap": body_remap,
"joint_remap": joint_remap,
"body_merged_parent": body_merged_parent,
"body_merged_transform": body_merged_transform,
# TODO clean up this data
"merged_body_data": merged_body_data,
}
# muscles
def add_muscle(
self, bodies: list[int], positions: list[Vec3], f0: float, lm: float, lt: float, lmax: float, pen: float
) -> int:
"""Adds a muscle-tendon activation unit.
Args:
bodies: A list of body indices for each waypoint
positions: A list of positions of each waypoint in the body's local frame
f0: Force scaling
lm: Muscle length
lt: Tendon length
lmax: Maximally efficient muscle length
pen: Penalty factor
Returns:
The index of the muscle in the model
.. note:: The simulation support for muscles is in progress and not yet fully functional.
"""
n = len(bodies)
self.muscle_start.append(len(self.muscle_bodies))
self.muscle_params.append((f0, lm, lt, lmax, pen))
self.muscle_activations.append(0.0)
for i in range(n):
self.muscle_bodies.append(bodies[i])
self.muscle_points.append(positions[i])
# return the index of the muscle
return len(self.muscle_start) - 1
# region shapes
def add_shape(
self,
*,
body: int,
type: int,
xform: Transform | None = None,
cfg: ShapeConfig | None = None,
scale: Vec3 | None = None,
src: Mesh | Gaussian | Heightfield | Any | None = None,
is_static: bool = False,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a generic collision shape to the model.
This is the base method for adding shapes; prefer using specific helpers like :meth:`add_shape_sphere` where possible.
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body (e.g., static world geometry).
type: The geometry type of the shape (e.g., `GeoType.BOX`, `GeoType.SPHERE`).
xform: The transform of the shape in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
cfg: The configuration for the shape's physical and collision properties. If `None`, :attr:`default_shape_cfg` is used. Defaults to `None`.
scale: The scale of the geometry. The interpretation depends on the shape type. Defaults to `(1.0, 1.0, 1.0)` if `None`.
src: The source geometry data, e.g., a :class:`Mesh` object for `GeoType.MESH`. Defaults to `None`.
is_static: If `True`, the shape will have zero mass, and its density property in `cfg` will be effectively ignored for mass calculation. Typically used for fixed, non-movable collision geometry. Defaults to `False`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color. Mesh-backed shapes fall back to :attr:`~newton.Mesh.color`.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated (e.g., "shape_N"). Defaults to `None`.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
The index of the newly added shape.
"""
if xform is None:
xform = wp.transform()
else:
xform = wp.transform(*xform)
if cfg is None:
cfg = self.default_shape_cfg
cfg.validate(shape_type=type)
if type == GeoType.MESH:
if (
cfg.sdf_max_resolution is not None
or cfg.sdf_target_voxel_size is not None
or cfg.sdf_narrow_band_range != (-0.1, 0.1)
or cfg.sdf_texture_format != "uint16"
):
raise ValueError(
"Mesh shapes do not use cfg.sdf_* for SDF generation. "
"Build and attach an SDF on the mesh via mesh.build_sdf()."
)
if cfg.is_hydroelastic and (src is None or getattr(src, "sdf", None) is None):
raise ValueError(
"Hydroelastic mesh shapes require mesh.sdf. "
"Call mesh.build_sdf() before add_shape_mesh(..., cfg=...)."
)
if scale is None:
scale = (1.0, 1.0, 1.0)
# Validate site invariants
if cfg.is_site:
shape_label = label or f"shape_{self.shape_count}"
# Sites must not have collision enabled
if cfg.has_shape_collision or cfg.has_particle_collision:
raise ValueError(
f"Site shape '{shape_label}' cannot have collision enabled. "
f"Sites must be non-colliding reference points. "
f"has_shape_collision={cfg.has_shape_collision}, "
f"has_particle_collision={cfg.has_particle_collision}"
)
# Sites must have zero density (no mass contribution)
if cfg.density != 0.0:
raise ValueError(
f"Site shape '{shape_label}' must have zero density. "
f"Sites do not contribute to body mass. "
f"Got density={cfg.density}"
)
# Sites must have collision group 0 (no collision filtering)
if cfg.collision_group != 0:
raise ValueError(
f"Site shape '{shape_label}' must have collision_group=0. "
f"Sites do not participate in collision detection. "
f"Got collision_group={cfg.collision_group}"
)
self.shape_body.append(body)
shape = self.shape_count
if cfg.has_shape_collision:
# no contacts between shapes of the same body
for same_body_shape in self.body_shapes[body]:
self.add_shape_collision_filter_pair(same_body_shape, shape)
self.body_shapes[body].append(shape)
self.shape_label.append(label or f"shape_{shape}")
self.shape_transform.append(xform)
# Get flags and clear HYDROELASTIC for unsupported shape types (PLANE, HFIELD)
shape_flags = cfg.flags
if (shape_flags & ShapeFlags.HYDROELASTIC) and (type == GeoType.PLANE or type == GeoType.HFIELD):
shape_flags &= (
~ShapeFlags.HYDROELASTIC
) # Falling back to mesh/primitive collisions for plane and hfield shapes
resolved_color = color
if resolved_color is None and src is not None:
resolved_color = getattr(src, "color", None)
if resolved_color is None:
resolved_color = ModelBuilder._shape_palette_color(shape)
self.shape_flags.append(shape_flags)
self.shape_type.append(type)
self.shape_scale.append((float(scale[0]), float(scale[1]), float(scale[2])))
self.shape_source.append(src)
self.shape_color.append(resolved_color)
self.shape_margin.append(cfg.margin)
self.shape_is_solid.append(cfg.is_solid)
self.shape_material_ke.append(cfg.ke)
self.shape_material_kd.append(cfg.kd)
self.shape_material_kf.append(cfg.kf)
self.shape_material_ka.append(cfg.ka)
self.shape_material_mu.append(cfg.mu)
self.shape_material_restitution.append(cfg.restitution)
self.shape_material_mu_torsional.append(cfg.mu_torsional)
self.shape_material_mu_rolling.append(cfg.mu_rolling)
self.shape_material_kh.append(cfg.kh)
self.shape_gap.append(cfg.gap if cfg.gap is not None else self.rigid_gap)
self.shape_collision_group.append(cfg.collision_group)
self.shape_collision_radius.append(compute_shape_radius(type, scale, src))
self.shape_world.append(self.current_world)
self.shape_sdf_narrow_band_range.append(cfg.sdf_narrow_band_range)
self.shape_sdf_target_voxel_size.append(cfg.sdf_target_voxel_size)
self.shape_sdf_max_resolution.append(cfg.sdf_max_resolution)
self.shape_sdf_texture_format.append(cfg.sdf_texture_format)
if cfg.has_shape_collision and cfg.collision_filter_parent and body > -1 and body in self.joint_parents:
for parent_body in self.joint_parents[body]:
if parent_body > -1:
for parent_shape in self.body_shapes[parent_body]:
self.add_shape_collision_filter_pair(parent_shape, shape)
if cfg.has_shape_collision and cfg.collision_filter_parent and body > -1 and body in self.joint_children:
for child_body in self.joint_children[body]:
for child_shape in self.body_shapes[child_body]:
self.add_shape_collision_filter_pair(shape, child_shape)
if not is_static and cfg.density > 0.0 and body >= 0 and not self.body_lock_inertia[body]:
(m, c, inertia) = compute_inertia_shape(type, scale, src, cfg.density, cfg.is_solid, cfg.margin)
com_body = wp.transform_point(xform, c)
self._update_body_mass(body, m, inertia, com_body, xform.q)
# Process custom attributes
if custom_attributes:
self._process_custom_attributes(
entity_index=shape,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.SHAPE,
)
return shape
def add_shape_plane(
self,
plane: Vec4 | None = (0.0, 0.0, 1.0, 0.0),
xform: Transform | None = None,
width: float = 10.0,
length: float = 10.0,
body: int = -1,
cfg: ShapeConfig | None = None,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""
Adds a plane collision shape to the model.
If `xform` is provided, it directly defines the plane's position and orientation. The plane's collision normal
is assumed to be along the local Z-axis of this `xform`.
If `xform` is `None`, it will be derived from the `plane` equation `a*x + b*y + c*z + d = 0`.
Plane shapes added via this method are always static (massless).
Args:
plane: The plane equation `(a, b, c, d)`. If `xform` is `None`, this defines the plane.
The normal is `(a,b,c)`. If `(a,b,c)` is unit-length, `d` is the negative signed offset from the
origin along that normal, so `(0.0, 0.0, 1.0, -h)` defines the plane `z = h`. Defaults to
`(0.0, 0.0, 1.0, 0.0)` (an XY ground plane at Z=0) if `xform` is also `None`.
xform: The transform of the plane in the world or parent body's frame. If `None`, transform is derived from `plane`. Defaults to `None`.
width: The visual/collision extent of the plane along its local X-axis. If `0.0`, considered infinite for collision. Defaults to `10.0`.
length: The visual/collision extent of the plane along its local Y-axis. If `0.0`, considered infinite for collision. Defaults to `10.0`.
body: The index of the parent body this shape belongs to. Use -1 for world-static planes. Defaults to `-1`.
cfg: The configuration for the shape's physical and collision properties. If `None`, :attr:`default_shape_cfg` is used. Defaults to `None`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the per-shape display color.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute values for SHAPE frequency attributes.
Returns:
The index of the newly added shape.
"""
if xform is None:
assert plane is not None, "Either xform or plane must be provided"
# compute position and rotation from plane equation
# For plane equation ax + by + cz + d = 0, the closest point to the origin is
# -(d/||n||) * (n/||n||), so the signed offset along the normalized normal is -d/||n||.
normal = np.array(plane[:3])
norm = np.linalg.norm(normal)
normal /= norm
d_normalized = plane[3] / norm
pos = -d_normalized * normal
# compute rotation from local +Z axis to plane normal
rot = wp.quat_between_vectors(wp.vec3(0.0, 0.0, 1.0), wp.vec3(*normal))
xform = wp.transform(pos, rot)
if cfg is None:
cfg = self.default_shape_cfg
scale = wp.vec3(width, length, 0.0)
return self.add_shape(
body=body,
type=GeoType.PLANE,
xform=xform,
cfg=cfg,
scale=scale,
is_static=True,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_ground_plane(
self,
height: float = 0.0,
cfg: ShapeConfig | None = None,
color: Vec3 | None = _DEFAULT_GROUND_PLANE_COLOR,
label: str | None = None,
) -> int:
"""Adds a ground plane collision shape to the model.
Args:
height: The vertical offset of the ground plane along the up-vector axis. Positive values raise the plane, negative values lower it. Defaults to `0.0`.
cfg: The configuration for the shape's physical and collision properties. If `None`, :attr:`default_shape_cfg` is used. Defaults to `None`.
color: Optional display RGB color with values in [0, 1]. Defaults to the ground plane color ``(0.125, 0.125, 0.15)``. Pass ``None`` to use the per-shape palette color instead.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
Returns:
The index of the newly added shape.
"""
return self.add_shape_plane(
plane=(*self.up_vector, -height),
width=0.0,
length=0.0,
cfg=cfg,
label=label or "ground_plane",
color=color,
)
def add_shape_sphere(
self,
body: int,
xform: Transform | None = None,
radius: float = 1.0,
cfg: ShapeConfig | None = None,
as_site: bool = False,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a sphere collision shape or site to a body.
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body.
xform: The transform of the sphere in the parent body's local frame. The sphere is centered at this transform's position. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
radius: The radius of the sphere. Defaults to `1.0`.
cfg: The configuration for the shape's properties. If `None`, uses :attr:`default_shape_cfg` (or :attr:`default_site_cfg` when `as_site=True`). If `as_site=True` and `cfg` is provided, a copy is made and site invariants are enforced via `mark_as_site()`. Defaults to `None`.
as_site: If `True`, creates a site (non-colliding reference point) instead of a collision shape. Defaults to `False`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
The index of the newly added shape or site.
"""
if cfg is None:
cfg = self.default_site_cfg if as_site else self.default_shape_cfg
elif as_site:
cfg = cfg.copy()
cfg.mark_as_site()
scale: Vec3 = wp.vec3(radius, 0.0, 0.0)
return self.add_shape(
body=body,
type=GeoType.SPHERE,
xform=xform,
cfg=cfg,
scale=scale,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_shape_ellipsoid(
self,
body: int,
xform: Transform | None = None,
rx: float = 1.0,
ry: float = 0.75,
rz: float = 0.5,
cfg: ShapeConfig | None = None,
as_site: bool = False,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
**kwargs,
) -> int:
"""Adds an ellipsoid collision shape or site to a body.
The ellipsoid is centered at its local origin as defined by `xform`, with semi-axes
`rx`, `ry`, `rz` along the local X, Y, Z axes respectively.
Note:
Ellipsoid collision is handled by the GJK/MPR collision pipeline,
which provides accurate collision detection for all convex shape pairs.
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body.
xform: The transform of the ellipsoid in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
rx: The semi-axis of the ellipsoid along its local X-axis [m]. Defaults to `1.0`.
ry: The semi-axis of the ellipsoid along its local Y-axis [m]. Defaults to `0.75`.
rz: The semi-axis of the ellipsoid along its local Z-axis [m]. Defaults to `0.5`.
cfg: The configuration for the shape's properties. If `None`, uses :attr:`default_shape_cfg` (or :attr:`default_site_cfg` when `as_site=True`). If `as_site=True` and `cfg` is provided, a copy is made and site invariants are enforced via `mark_as_site()`. Defaults to `None`.
as_site: If `True`, creates a site (non-colliding reference point) instead of a collision shape. Defaults to `False`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
The index of the newly added shape or site.
Example:
Create an ellipsoid with different semi-axes:
.. doctest::
builder = newton.ModelBuilder()
body = builder.add_body()
# Add an ellipsoid with semi-axes 1.0, 0.5, 0.25
builder.add_shape_ellipsoid(
body=body,
rx=1.0, # X semi-axis
ry=0.5, # Y semi-axis
rz=0.25, # Z semi-axis
)
# A sphere is a special case where rx = ry = rz
builder.add_shape_ellipsoid(body=body, rx=0.5, ry=0.5, rz=0.5)
"""
# Backward compat: accept deprecated a, b, c parameter names
_deprecated_map = {"a": ("rx", rx, 1.0), "b": ("ry", ry, 0.75), "c": ("rz", rz, 0.5)}
for old_name, (new_name, new_val, default) in _deprecated_map.items():
if old_name in kwargs:
if new_val != default:
raise TypeError(f"Cannot specify both '{old_name}' and '{new_name}'")
warnings.warn(
f"Parameter '{old_name}' is deprecated, use '{new_name}' instead.",
DeprecationWarning,
stacklevel=2,
)
if "a" in kwargs:
rx = kwargs.pop("a")
if "b" in kwargs:
ry = kwargs.pop("b")
if "c" in kwargs:
rz = kwargs.pop("c")
if kwargs:
raise TypeError(f"Unexpected keyword arguments: {set(kwargs)}")
if cfg is None:
cfg = self.default_site_cfg if as_site else self.default_shape_cfg
elif as_site:
cfg = cfg.copy()
cfg.mark_as_site()
scale = wp.vec3(rx, ry, rz)
return self.add_shape(
body=body,
type=GeoType.ELLIPSOID,
xform=xform,
cfg=cfg,
scale=scale,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_shape_box(
self,
body: int,
xform: Transform | None = None,
hx: float = 0.5,
hy: float = 0.5,
hz: float = 0.5,
cfg: ShapeConfig | None = None,
as_site: bool = False,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a box collision shape or site to a body.
The box is centered at its local origin as defined by `xform`.
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body.
xform: The transform of the box in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
hx: The half-extent of the box along its local X-axis. Defaults to `0.5`.
hy: The half-extent of the box along its local Y-axis. Defaults to `0.5`.
hz: The half-extent of the box along its local Z-axis. Defaults to `0.5`.
cfg: The configuration for the shape's properties. If `None`, uses :attr:`default_shape_cfg` (or :attr:`default_site_cfg` when `as_site=True`). If `as_site=True` and `cfg` is provided, a copy is made and site invariants are enforced via `mark_as_site()`. Defaults to `None`.
as_site: If `True`, creates a site (non-colliding reference point) instead of a collision shape. Defaults to `False`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
The index of the newly added shape or site.
"""
if cfg is None:
cfg = self.default_site_cfg if as_site else self.default_shape_cfg
elif as_site:
cfg = cfg.copy()
cfg.mark_as_site()
scale = wp.vec3(hx, hy, hz)
return self.add_shape(
body=body,
type=GeoType.BOX,
xform=xform,
cfg=cfg,
scale=scale,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_shape_capsule(
self,
body: int,
xform: Transform | None = None,
radius: float = 1.0,
half_height: float = 0.5,
cfg: ShapeConfig | None = None,
as_site: bool = False,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a capsule collision shape or site to a body.
The capsule is centered at its local origin as defined by `xform`. Its length extends along the Z-axis.
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body.
xform: The transform of the capsule in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
radius: The radius of the capsule's hemispherical ends and its cylindrical segment. Defaults to `1.0`.
half_height: The half-length of the capsule's central cylindrical segment (excluding the hemispherical ends). Defaults to `0.5`.
cfg: The configuration for the shape's properties. If `None`, uses :attr:`default_shape_cfg` (or :attr:`default_site_cfg` when `as_site=True`). If `as_site=True` and `cfg` is provided, a copy is made and site invariants are enforced via `mark_as_site()`. Defaults to `None`.
as_site: If `True`, creates a site (non-colliding reference point) instead of a collision shape. Defaults to `False`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
The index of the newly added shape or site.
"""
if cfg is None:
cfg = self.default_site_cfg if as_site else self.default_shape_cfg
elif as_site:
cfg = cfg.copy()
cfg.mark_as_site()
if xform is None:
xform = wp.transform()
else:
xform = wp.transform(*xform)
scale = wp.vec3(radius, half_height, 0.0)
return self.add_shape(
body=body,
type=GeoType.CAPSULE,
xform=xform,
cfg=cfg,
scale=scale,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_shape_cylinder(
self,
body: int,
xform: Transform | None = None,
radius: float = 1.0,
half_height: float = 0.5,
cfg: ShapeConfig | None = None,
as_site: bool = False,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a cylinder collision shape or site to a body.
The cylinder is centered at its local origin as defined by `xform`. Its length extends along the Z-axis.
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body.
xform: The transform of the cylinder in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
radius: The radius of the cylinder. Defaults to `1.0`.
half_height: The half-length of the cylinder along the Z-axis. Defaults to `0.5`.
cfg: The configuration for the shape's properties. If `None`, uses :attr:`default_shape_cfg` (or :attr:`default_site_cfg` when `as_site=True`). If `as_site=True` and `cfg` is provided, a copy is made and site invariants are enforced via `mark_as_site()`. Defaults to `None`.
as_site: If `True`, creates a site (non-colliding reference point) instead of a collision shape. Defaults to `False`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute values for SHAPE frequency attributes.
Returns:
The index of the newly added shape or site.
"""
if cfg is None:
cfg = self.default_site_cfg if as_site else self.default_shape_cfg
elif as_site:
cfg = cfg.copy()
cfg.mark_as_site()
if xform is None:
xform = wp.transform()
else:
xform = wp.transform(*xform)
scale = wp.vec3(radius, half_height, 0.0)
return self.add_shape(
body=body,
type=GeoType.CYLINDER,
xform=xform,
cfg=cfg,
scale=scale,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_shape_cone(
self,
body: int,
xform: Transform | None = None,
radius: float = 1.0,
half_height: float = 0.5,
cfg: ShapeConfig | None = None,
as_site: bool = False,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a cone collision shape to a body.
The cone's origin is at its geometric center, with the base at -half_height and apex at +half_height along the Z-axis.
The center of mass is located at -half_height/2 from the origin (1/4 of the total height from the base toward the apex).
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body.
xform: The transform of the cone in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
radius: The radius of the cone's base. Defaults to `1.0`.
half_height: The half-height of the cone (distance from the geometric center to either the base or apex). The total height is 2*half_height. Defaults to `0.5`.
cfg: The configuration for the shape's physical and collision properties. If `None`, :attr:`default_shape_cfg` is used. Defaults to `None`.
as_site: If `True`, creates a site (non-colliding reference point) instead of a collision shape. Defaults to `False`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute values for SHAPE frequency attributes.
Returns:
The index of the newly added shape.
"""
if cfg is None:
cfg = self.default_site_cfg if as_site else self.default_shape_cfg
elif as_site:
cfg = cfg.copy()
cfg.mark_as_site()
if xform is None:
xform = wp.transform()
else:
xform = wp.transform(*xform)
scale = wp.vec3(radius, half_height, 0.0)
return self.add_shape(
body=body,
type=GeoType.CONE,
xform=xform,
cfg=cfg,
scale=scale,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_shape_mesh(
self,
body: int,
xform: Transform | None = None,
mesh: Mesh | None = None,
scale: Vec3 | None = None,
cfg: ShapeConfig | None = None,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a triangle mesh collision shape to a body.
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body.
xform: The transform of the mesh in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
mesh: The :class:`Mesh` object containing the vertex and triangle data. Defaults to `None`.
scale: The scale of the mesh. Defaults to `None`, in which case the scale is `(1.0, 1.0, 1.0)`.
cfg: The configuration for the shape's physical and collision properties. If `None`, :attr:`default_shape_cfg` is used. Defaults to `None`.
color: Optional display RGB color with values in [0, 1]. If `None`, falls back to :attr:`~newton.Mesh.color` when available.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute values for SHAPE frequency attributes.
Returns:
The index of the newly added shape.
"""
if cfg is None:
cfg = self.default_shape_cfg
return self.add_shape(
body=body,
type=GeoType.MESH,
xform=xform,
cfg=cfg,
scale=scale,
src=mesh,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_shape_convex_hull(
self,
body: int,
xform: Transform | None = None,
mesh: Mesh | None = None,
scale: Vec3 | None = None,
cfg: ShapeConfig | None = None,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a convex hull collision shape to a body.
Args:
body: The index of the parent body this shape belongs to. Use -1 for shapes not attached to any specific body.
xform: The transform of the convex hull in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
mesh: The :class:`Mesh` object containing the vertex data for the convex hull. Defaults to `None`.
scale: The scale of the convex hull. Defaults to `None`, in which case the scale is `(1.0, 1.0, 1.0)`.
cfg: The configuration for the shape's physical and collision properties. If `None`, :attr:`default_shape_cfg` is used. Defaults to `None`.
color: Optional display RGB color with values in [0, 1]. If `None`, falls back to :attr:`~newton.Mesh.color` when available.
label: An optional unique label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute values for SHAPE frequency attributes.
Returns:
The index of the newly added shape.
"""
if cfg is None:
cfg = self.default_shape_cfg
return self.add_shape(
body=body,
type=GeoType.CONVEX_MESH,
xform=xform,
cfg=cfg,
scale=scale,
src=mesh,
label=label,
color=color,
custom_attributes=custom_attributes,
)
def add_shape_heightfield(
self,
xform: Transform | None = None,
heightfield: Heightfield | None = None,
scale: Vec3 | None = None,
cfg: ShapeConfig | None = None,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a heightfield (2D elevation grid) collision shape to the model.
Heightfields are efficient representations of terrain using a 2D grid of elevation values.
They are always static (attached to the world body) and more memory-efficient than
equivalent triangle meshes.
Args:
xform: The transform of the heightfield in world frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
heightfield: The :class:`Heightfield` object containing the elevation grid data. Defaults to `None`.
scale: The scale of the heightfield. Defaults to `None`, in which case the scale is `(1.0, 1.0, 1.0)`.
cfg: The configuration for the shape's physical and collision properties. If `None`, :attr:`default_shape_cfg` is used. Defaults to `None`.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color.
label: An optional label for identifying the shape. If `None`, a default label is automatically generated. Defaults to `None`.
custom_attributes: Dictionary of custom attribute values for SHAPE frequency attributes.
Returns:
The index of the newly added shape.
"""
if heightfield is None:
raise ValueError("add_shape_heightfield() requires a Heightfield instance.")
if cfg is None:
cfg = self.default_shape_cfg
return self.add_shape(
body=-1,
type=GeoType.HFIELD,
xform=xform,
cfg=cfg,
scale=scale,
src=heightfield,
is_static=True,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_shape_gaussian(
self,
body: int,
xform: Transform | None = None,
gaussian: Gaussian | None = None,
scale: Vec3 | None = None,
cfg: ShapeConfig | None = None,
collision_proxy: str | Mesh | None = None,
color: Vec3 | None = None,
label: str | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a Gaussian splat shape to a body.
The Gaussian is attached as a ``GeoType.GAUSSIAN`` shape for rendering.
Collision is handled separately via *collision_proxy*.
Args:
body: The index of the parent body this shape belongs to.
Use ``-1`` for static world geometry.
xform: Transform in parent body's local frame. Defaults to identity.
gaussian: The :class:`Gaussian` splat asset.
scale: 3D scale applied to Gaussian positions. Defaults to ``(1, 1, 1)``.
cfg: Shape configuration. If ``None``, uses :attr:`default_shape_cfg`
with ``has_shape_collision=False`` (Gaussians are render-only by
default).
collision_proxy: Collision strategy. Options:
- ``None``: no collision (render-only).
- ``"convex_hull"``: auto-generate convex hull from Gaussian positions.
- A :class:`Mesh` instance: use the provided mesh as collision proxy.
color: Optional display RGB color with values in [0, 1]. If `None`, uses the default per-shape display color.
label: Optional unique label for identifying the shape.
custom_attributes: Dictionary of custom attribute values for SHAPE
frequency attributes.
Returns:
The index of the Gaussian shape.
"""
# Backward compat: detect Gaussian passed as second positional arg (old API
# had signature add_shape_gaussian(body, gaussian, xform=...)).
if isinstance(xform, Gaussian):
warnings.warn(
"Passing 'gaussian' as the second positional argument is deprecated. "
"Use add_shape_gaussian(body, xform=..., gaussian=...) instead.",
DeprecationWarning,
stacklevel=2,
)
if gaussian is not None:
raise TypeError("Cannot pass 'gaussian' both as positional and keyword argument.")
gaussian = xform
xform = None
if gaussian is None:
raise TypeError("'gaussian' is required when adding a Gaussian shape.")
if cfg is None:
cfg = self.default_shape_cfg.copy()
else:
cfg = cfg.copy()
# Gaussian shape is render-only; collisions are represented by optional proxy geometry.
proxy_cfg_base = cfg.copy()
cfg.has_shape_collision = False
cfg.has_particle_collision = False
cfg.density = 0.0
# Optionally add a collision proxy alongside the Gaussian shape
if collision_proxy is not None:
if isinstance(collision_proxy, str):
proxy_mesh = gaussian.compute_proxy_mesh(method=collision_proxy)
elif isinstance(collision_proxy, Mesh):
proxy_mesh = collision_proxy
else:
raise TypeError(f"collision_proxy must be None, a string, or a Mesh, got {type(collision_proxy)}")
proxy_cfg = proxy_cfg_base.copy()
proxy_cfg.is_visible = False
proxy_cfg.has_shape_collision = True
self.add_shape_convex_hull(
body=body,
xform=xform,
mesh=proxy_mesh,
scale=scale,
cfg=proxy_cfg,
label=f"{label or 'gaussian'}_collision_proxy",
)
return self.add_shape(
body=body,
type=GeoType.GAUSSIAN,
xform=xform,
cfg=cfg,
scale=scale,
src=gaussian,
is_static=True,
label=label,
custom_attributes=custom_attributes,
color=color,
)
def add_site(
self,
body: int,
xform: Transform | None = None,
type: int = GeoType.SPHERE,
scale: Vec3 = (0.01, 0.01, 0.01),
label: str | None = None,
visible: bool = False,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a site (non-colliding reference point) to a body.
Sites are abstract markers that don't participate in physics simulation or collision detection.
They are useful for:
- Sensor attachment points (IMU, camera, etc.)
- Frame of reference definitions
- Debugging and visualization markers
- Spatial tendon attachment points (when exported to MuJoCo)
Args:
body: The index of the parent body this site belongs to. Use -1 for sites not attached to any specific body (for sites defined a at static world position).
xform: The transform of the site in the parent body's local frame. If `None`, the identity transform `wp.transform()` is used. Defaults to `None`.
type: The geometry type for visualization (e.g., `GeoType.SPHERE`, `GeoType.BOX`). Defaults to `GeoType.SPHERE`.
scale: The scale/size of the site for visualization. Defaults to `(0.01, 0.01, 0.01)`.
label: An optional unique label for identifying the site. If `None`, a default label is automatically generated. Defaults to `None`.
visible: If True, the site will be visible for debugging. If False (default), the site is hidden.
custom_attributes: Dictionary of custom attribute names to values.
Returns:
The index of the newly added site (which is stored as a shape internally).
Example:
Add an IMU sensor site to a robot torso::
body = builder.add_body()
imu_site = builder.add_site(
body,
xform=wp.transform((0.0, 0.0, 0.1), wp.quat_identity()),
label="imu_sensor",
visible=True, # Show for debugging
)
"""
# Create config for non-colliding site
cfg = self.default_site_cfg.copy()
cfg.is_visible = visible
return self.add_shape(
body=body,
type=type,
xform=xform,
cfg=cfg,
scale=scale,
label=label,
custom_attributes=custom_attributes,
)
def approximate_meshes(
self,
method: Literal["coacd", "vhacd", "bounding_sphere", "bounding_box"] | RemeshingMethod = "convex_hull",
shape_indices: list[int] | None = None,
raise_on_failure: bool = False,
keep_visual_shapes: bool = False,
**remeshing_kwargs: dict[str, Any],
) -> set[int]:
"""Approximates the mesh shapes of the model.
The following methods are supported:
+------------------------+-------------------------------------------------------------------------------+
| Method | Description |
+========================+===============================================================================+
| ``"coacd"`` | Convex decomposition using `CoACD `_ |
+------------------------+-------------------------------------------------------------------------------+
| ``"vhacd"`` | Convex decomposition using `V-HACD `_ |
+------------------------+-------------------------------------------------------------------------------+
| ``"bounding_sphere"`` | Approximate the mesh with a sphere |
+------------------------+-------------------------------------------------------------------------------+
| ``"bounding_box"`` | Approximate the mesh with an oriented bounding box |
+------------------------+-------------------------------------------------------------------------------+
| ``"convex_hull"`` | Approximate the mesh with a convex hull (default) |
+------------------------+-------------------------------------------------------------------------------+
| ```` | Any remeshing method supported by :func:`newton.utils.remesh_mesh` |
+------------------------+-------------------------------------------------------------------------------+
.. note::
The ``coacd`` and ``vhacd`` methods require additional dependencies (``coacd`` or ``trimesh`` and ``vhacdx`` respectively) to be installed.
The convex hull approximation requires ``scipy`` to be installed.
The ``raise_on_failure`` parameter controls the behavior when the remeshing fails:
- If `True`, an exception is raised when the remeshing fails.
- If `False`, a warning is logged, and the method falls back to the next available method in the order of preference:
- If convex decomposition via CoACD or V-HACD fails or dependencies are not available, the method will fall back to using the ``convex_hull`` method.
- If convex hull approximation fails, it will fall back to the ``bounding_box`` method.
Args:
method: The method to use for approximating the mesh shapes.
shape_indices: The indices of the shapes to simplify. If `None`, all mesh shapes that have the :attr:`ShapeFlags.COLLIDE_SHAPES` flag set are simplified.
raise_on_failure: If `True`, raises an exception if the remeshing fails. If `False`, it will log a warning and continue with the fallback method.
**remeshing_kwargs: Additional keyword arguments passed to the remeshing function.
Returns:
Indices of the shapes that were successfully remeshed.
"""
remeshing_methods = [*RemeshingMethod.__args__, "coacd", "vhacd", "bounding_sphere", "bounding_box"]
if method not in remeshing_methods:
raise ValueError(
f"Unsupported remeshing method: {method}. Supported methods are: {', '.join(remeshing_methods)}."
)
def get_shape_custom_attributes(shape: int) -> dict[str, Any] | None:
custom_attributes = {
full_key: custom_attr.values[shape]
for full_key, custom_attr in self.custom_attributes.items()
if custom_attr.frequency == Model.AttributeFrequency.SHAPE
and isinstance(custom_attr.values, dict)
and shape in custom_attr.values
}
return custom_attributes or None
if shape_indices is None:
shape_indices = [
i
for i, stype in enumerate(self.shape_type)
if stype == GeoType.MESH and self.shape_flags[i] & ShapeFlags.COLLIDE_SHAPES
]
if keep_visual_shapes:
# if keeping visual shapes, first copy input shapes, mark the copies as visual-only,
# and mark the originals as non-visible.
# in the rare event that approximation fails, we end up with two identical shapes,
# one collision-only, one visual-only, but this simplifies the logic below.
for shape in shape_indices:
if not (self.shape_flags[shape] & ShapeFlags.VISIBLE):
continue
body = self.shape_body[shape]
xform = self.shape_transform[shape]
color = self.shape_color[shape]
custom_attributes = get_shape_custom_attributes(shape)
cfg = ModelBuilder.ShapeConfig(
density=0.0, # do not add extra mass / inertia
margin=self.shape_margin[shape],
is_solid=self.shape_is_solid[shape],
has_shape_collision=False,
has_particle_collision=False,
is_visible=True,
)
self.add_shape_mesh(
body=body,
xform=xform,
cfg=cfg,
mesh=self.shape_source[shape],
color=color,
label=f"{self.shape_label[shape]}_visual",
scale=self.shape_scale[shape],
custom_attributes=custom_attributes,
)
# disable visibility of the original shape
self.shape_flags[shape] &= ~ShapeFlags.VISIBLE
# keep track of remeshed shapes to handle fallbacks
remeshed_shapes = set()
if method == "coacd" or method == "vhacd":
try:
if method == "coacd":
# convex decomposition using CoACD
import coacd
else:
# convex decomposition using V-HACD
import trimesh
decompositions = {}
for shape in shape_indices:
mesh: Mesh = self.shape_source[shape]
scale = self.shape_scale[shape]
hash_m = hash(mesh)
if hash_m in decompositions:
decomposition = decompositions[hash_m]
else:
if method == "coacd":
cmesh = coacd.Mesh(mesh.vertices, mesh.indices.reshape(-1, 3))
coacd_settings = {
"threshold": 0.5,
"mcts_nodes": 20,
"mcts_iterations": 5,
"mcts_max_depth": 1,
"merge": False,
"max_convex_hull": mesh.maxhullvert,
}
coacd_settings.update(remeshing_kwargs)
decomposition = coacd.run_coacd(cmesh, **coacd_settings)
else:
tmesh = trimesh.Trimesh(mesh.vertices, mesh.indices.reshape(-1, 3))
vhacd_settings = {
"maxNumVerticesPerCH": mesh.maxhullvert,
}
vhacd_settings.update(remeshing_kwargs)
decomposition = trimesh.decomposition.convex_decomposition(tmesh, **vhacd_settings)
decomposition = [(d["vertices"], d["faces"]) for d in decomposition]
decompositions[hash_m] = decomposition
if len(decomposition) == 0:
continue
# note we need to copy the mesh to avoid modifying the original mesh
self.shape_source[shape] = self.shape_source[shape].copy(
vertices=decomposition[0][0], indices=decomposition[0][1]
)
# mark as convex mesh type
self.shape_type[shape] = GeoType.CONVEX_MESH
if len(decomposition) > 1:
body = self.shape_body[shape]
xform = self.shape_transform[shape]
color = self.shape_color[shape]
custom_attributes = get_shape_custom_attributes(shape)
cfg = ModelBuilder.ShapeConfig(
density=0.0, # do not add extra mass / inertia
ke=self.shape_material_ke[shape],
kd=self.shape_material_kd[shape],
kf=self.shape_material_kf[shape],
ka=self.shape_material_ka[shape],
mu=self.shape_material_mu[shape],
restitution=self.shape_material_restitution[shape],
mu_torsional=self.shape_material_mu_torsional[shape],
mu_rolling=self.shape_material_mu_rolling[shape],
kh=self.shape_material_kh[shape],
margin=self.shape_margin[shape],
is_solid=self.shape_is_solid[shape],
collision_group=self.shape_collision_group[shape],
collision_filter_parent=self.default_shape_cfg.collision_filter_parent,
)
cfg.flags = self.shape_flags[shape]
for i in range(1, len(decomposition)):
# add additional convex parts as convex meshes
self.add_shape_convex_hull(
body=body,
xform=xform,
mesh=Mesh(decomposition[i][0], decomposition[i][1]),
scale=scale,
cfg=cfg,
color=color,
label=f"{self.shape_label[shape]}_convex_{i}",
custom_attributes=custom_attributes,
)
remeshed_shapes.add(shape)
except Exception as e:
if raise_on_failure:
raise RuntimeError(f"Remeshing with method '{method}' failed.") from e
else:
warnings.warn(
f"Remeshing with method '{method}' failed: {e}. Falling back to convex_hull.", stacklevel=2
)
method = "convex_hull"
if method in RemeshingMethod.__args__:
# remeshing of the individual meshes
remeshed = {}
for shape in shape_indices:
if shape in remeshed_shapes:
# already remeshed with coacd or vhacd
continue
mesh: Mesh = self.shape_source[shape]
hash_m = hash(mesh)
rmesh = remeshed.get(hash_m, None)
if rmesh is None:
try:
rmesh = remesh_mesh(mesh, method=method, inplace=False, **remeshing_kwargs)
remeshed[hash_m] = rmesh
except Exception as e:
if raise_on_failure:
raise RuntimeError(f"Remeshing with method '{method}' failed for shape {shape}.") from e
else:
warnings.warn(
f"Remeshing with method '{method}' failed for shape {shape}: {e}. Falling back to bounding_box.",
stacklevel=2,
)
continue
# note we need to copy the mesh to avoid modifying the original mesh
self.shape_source[shape] = self.shape_source[shape].copy(vertices=rmesh.vertices, indices=rmesh.indices)
# mark convex_hull result as convex mesh type for efficient collision detection
if method == "convex_hull":
self.shape_type[shape] = GeoType.CONVEX_MESH
remeshed_shapes.add(shape)
if method == "bounding_box":
for shape in shape_indices:
if shape in remeshed_shapes:
continue
mesh: Mesh = self.shape_source[shape]
scale = self.shape_scale[shape]
vertices = mesh.vertices * np.array([*scale])
tf, scale = compute_inertia_obb(vertices)
self.shape_type[shape] = GeoType.BOX
self.shape_source[shape] = None
self.shape_scale[shape] = scale
shape_tf = self.shape_transform[shape]
self.shape_transform[shape] = shape_tf * tf
remeshed_shapes.add(shape)
elif method == "bounding_sphere":
for shape in shape_indices:
if shape in remeshed_shapes:
continue
mesh: Mesh = self.shape_source[shape]
scale = self.shape_scale[shape]
scale_array = np.asarray(scale, dtype=np.float32)
vertices = np.asarray(mesh.vertices, dtype=np.float32) * scale_array
center = np.mean(vertices, axis=0, dtype=np.float32)
radius = float(np.max(np.linalg.norm(vertices - center, axis=1).astype(np.float32, copy=False)))
self.shape_type[shape] = GeoType.SPHERE
self.shape_source[shape] = None
self.shape_scale[shape] = (radius, 0.0, 0.0)
tf = wp.transform(center, wp.quat_identity())
shape_tf = self.shape_transform[shape]
self.shape_transform[shape] = shape_tf * tf
remeshed_shapes.add(shape)
# Hide approximated primitives on bodies that have other visible shapes.
# Primitives (box, sphere) can't carry visual materials, so they should
# not be visible when the body already has dedicated visual geometry.
visible_count_per_body = Counter(
self.shape_body[i] for i in range(len(self.shape_body)) if self.shape_flags[i] & ShapeFlags.VISIBLE
)
for shape in remeshed_shapes:
if self.shape_type[shape] in (GeoType.MESH, GeoType.CONVEX_MESH):
continue
if not (self.shape_flags[shape] & ShapeFlags.VISIBLE):
continue
body = self.shape_body[shape]
if visible_count_per_body.get(body, 0) > 1:
self.shape_flags[shape] &= ~ShapeFlags.VISIBLE
visible_count_per_body[body] -= 1
return remeshed_shapes
def add_rod(
self,
positions: list[Vec3],
quaternions: list[Quat] | None = None,
radius: float = 0.1,
cfg: ShapeConfig | None = None,
stretch_stiffness: float | None = None,
stretch_damping: float | None = None,
bend_stiffness: float | None = None,
bend_damping: float | None = None,
closed: bool = False,
label: str | None = None,
wrap_in_articulation: bool = True,
) -> tuple[list[int], list[int]]:
"""Adds a rod composed of capsule bodies connected by cable joints.
Constructs a chain of capsule bodies from the given centerline points and orientations.
Each segment is a capsule aligned by the corresponding quaternion, and adjacent capsules
are connected by cable joints providing one linear (stretch) and one angular (bend/twist)
degree of freedom.
Args:
positions: Centerline node positions (segment endpoints) in world space. These are the
tip/end points of the capsules, with one extra point so that for ``N`` segments there
are ``N+1`` positions.
quaternions: Optional per-segment (per-edge) orientations in world space. If provided,
must have ``len(positions) - 1`` elements and each quaternion should align the capsule's
local +Z with the segment direction ``positions[i+1] - positions[i]``. If None,
orientations are computed automatically to align +Z with each segment direction.
radius: Capsule radius.
cfg: Shape configuration for the capsules. If None, :attr:`default_shape_cfg` is used.
stretch_stiffness: Stretch stiffness for the cable joints. For rods, this is treated as a
material-like axial/shear stiffness (commonly interpreted as EA)
with units [N] and is internally converted to an effective point stiffness [N/m] by dividing by
segment length. If None, defaults to 1.0e9.
stretch_damping: Stretch damping for the cable joints (applied per-joint; not length-normalized). If None,
defaults to 0.0.
bend_stiffness: Bend/twist stiffness for the cable joints. For rods, this is treated as a
material-like bending/twist stiffness (e.g., EI) with units [N*m^2] and is internally converted to
an effective per-joint stiffness [N*m] (torque per radian) by dividing by segment length. If None,
defaults to 0.0.
bend_damping: Bend/twist damping for the cable joints (applied per-joint; not length-normalized). If None,
defaults to 0.0.
closed: If True, connects the last segment back to the first to form a closed loop. If False,
creates an open chain. Note: rods require at least 2 segments.
label: Optional label prefix for bodies, shapes, and joints.
wrap_in_articulation: If True, the created joints are automatically wrapped into a single
articulation. Defaults to True to ensure valid simulation models.
Returns:
A pair ``(body_indices, joint_indices)``. For an open chain,
``len(joint_indices) == num_segments - 1``; for a closed loop, ``len(joint_indices) == num_segments``.
Articulations:
By default (``wrap_in_articulation=True``), the created joints are wrapped into a single
articulation, which avoids orphan joints during :meth:`finalize `.
If ``wrap_in_articulation=False``, this method will return the created joint indices but will
not wrap them; callers must place them into one or more articulations (via :meth:`add_articulation`)
before calling :meth:`finalize `.
Raises:
ValueError: If ``positions`` and ``quaternions`` lengths are incompatible.
ValueError: If the rod has fewer than 2 segments.
Note:
- Bend defaults are 0.0 (no bending resistance unless specified). Stretch defaults to a high
stiffness (1.0e9), which keeps neighboring capsules closely coupled (approximately inextensible).
- Internally, stretch and bend stiffnesses are pre-scaled by dividing by segment length so solver kernels
do not need per-segment length normalization.
- Damping values are passed through as provided (per joint) and are not length-normalized.
- Each segment is implemented as a capsule primitive. The segment's body transform is
placed at the start point ``positions[i]`` with a local center-of-mass offset of
``(0, 0, half_height)`` so that the COM lies at the segment midpoint. The capsule shape
is added with a local transform of ``(0, 0, half_height)`` so it spans from the start to
the end along local +Z.
"""
if cfg is None:
cfg = self.default_shape_cfg
# Stretch defaults: high stiffness to keep neighboring capsules tightly coupled
stretch_stiffness = 1.0e9 if stretch_stiffness is None else stretch_stiffness
stretch_damping = 0.0 if stretch_damping is None else stretch_damping
# Bend defaults: 0.0 (users must explicitly set for bending resistance)
bend_stiffness = 0.0 if bend_stiffness is None else bend_stiffness
bend_damping = 0.0 if bend_damping is None else bend_damping
# Input validation
if stretch_stiffness < 0.0 or bend_stiffness < 0.0:
raise ValueError("add_rod: stretch_stiffness and bend_stiffness must be >= 0")
num_segments = len(positions) - 1
if num_segments < 1:
raise ValueError("add_rod: positions must contain at least 2 points")
# Coerce all input positions to wp.vec3 so arithmetic (p1 - p0), wp.length, wp.normalize
# always operate on Warp vector types even if the caller passed tuples/lists.
positions_wp: list[wp.vec3] = [axis_to_vec3(p) for p in positions]
if quaternions is not None and len(quaternions) != num_segments:
raise ValueError(
f"add_rod: quaternions must have {num_segments} elements for {num_segments} segments, "
f"got {len(quaternions)} quaternions"
)
if num_segments < 2:
# A "rod" in this API is defined as multiple capsules coupled by cable joints.
# If you want a single capsule, create a body + capsule shape directly.
raise ValueError(
f"add_rod: requires at least 2 segments (got {num_segments}); "
"for a single capsule, create a body and add a capsule shape instead."
)
# Build linear graph edges: (0, 1), (1, 2), ..., (N-1, N)
# Note: positions has N+1 elements for N segments.
edges = [(i, i + 1) for i in range(num_segments)]
# Delegate to add_rod_graph to create bodies and internal joints.
# We use wrap_in_articulation=False and let add_rod manage articulation wrapping so that:
# - open chains are wrapped into a single articulation (tree), and
# - closed loops add one extra "loop joint" after wrapping, which must not be part of an articulation.
link_bodies, link_joints = self.add_rod_graph(
node_positions=positions_wp,
edges=edges,
radius=radius,
cfg=cfg,
stretch_stiffness=stretch_stiffness,
stretch_damping=stretch_damping,
bend_stiffness=bend_stiffness,
bend_damping=bend_damping,
label=label,
wrap_in_articulation=False,
quaternions=quaternions,
)
# Wrap all joints into an articulation if requested.
if wrap_in_articulation and link_joints:
rod_art_label = f"{label}_articulation" if label else None
self.add_articulation(link_joints, label=rod_art_label)
# For closed loops, add one extra loop-closing cable joint that is intentionally
# *not* part of an articulation (articulations must be trees/forests).
if closed:
if not wrap_in_articulation:
warnings.warn(
"add_rod: wrap_in_articulation=False requires the caller to wrap joints via add_articulation() "
"before finalize; closed=True also adds a loop-closing joint that must remain outside any "
"articulation.",
UserWarning,
stacklevel=2,
)
if link_bodies:
first_body = link_bodies[0]
last_body = link_bodies[-1]
# Connect the end of the last segment to the start of the first segment.
L_last = float(wp.length(positions_wp[-1] - positions_wp[-2]))
min_segment_length = 1.0e-9
if L_last <= min_segment_length:
L_last = min_segment_length
parent_xform = wp.transform(wp.vec3(0.0, 0.0, L_last), wp.quat_identity())
child_xform = wp.transform(wp.vec3(0.0, 0.0, 0.0), wp.quat_identity())
# Normalize stiffness by segment length, consistent with add_rod_graph().
stretch_ke_eff = stretch_stiffness / L_last
bend_ke_eff = bend_stiffness / L_last
loop_joint_label = f"{label}_cable_{len(link_joints) + 1}" if label else None
j_loop = self.add_joint_cable(
parent=last_body,
child=first_body,
parent_xform=parent_xform,
child_xform=child_xform,
bend_stiffness=bend_ke_eff,
bend_damping=bend_damping,
stretch_stiffness=stretch_ke_eff,
stretch_damping=stretch_damping,
label=loop_joint_label,
collision_filter_parent=True,
enabled=True,
)
link_joints.append(j_loop)
return link_bodies, link_joints
def add_rod_graph(
self,
node_positions: list[Vec3],
edges: list[tuple[int, int]],
radius: float = 0.1,
cfg: ShapeConfig | None = None,
stretch_stiffness: float | None = None,
stretch_damping: float | None = None,
bend_stiffness: float | None = None,
bend_damping: float | None = None,
label: str | None = None,
wrap_in_articulation: bool = True,
quaternions: list[Quat] | None = None,
junction_collision_filter: bool = True,
) -> tuple[list[int], list[int]]:
"""Adds a rod/cable *graph* (supports junctions) from nodes + edges.
This is a generalization of :meth:`add_rod` to support branching/junction topologies.
Representation:
- Each *edge* becomes a capsule rigid body spanning from ``node_positions[u]`` to
``node_positions[v]`` (body frame is placed at the start node ``u`` and local +Z points
toward ``v``).
- Cable joints are created between edge-bodies that share a node, using a spanning-tree
traversal so that each body has a single parent when wrapped into an articulation.
Notes:
- If ``wrap_in_articulation=True`` (default), joints are created as a forest (one
articulation per connected component). This keeps the joint graph articulation-safe
(tree/forest), avoiding cycles at junctions.
- Cycles in the edge adjacency graph are *not* explicitly closed with extra joints when
``wrap_in_articulation=True`` (cycles would violate articulation tree constraints). If
you need closed loops, build them explicitly without articulation wrapping.
- If ``wrap_in_articulation=False``, joints are created directly at each node to connect
all incident edges. This can preserve rings/loops, but does not produce an articulation
tree (edges may effectively have multiple "parents" in the joint graph).
Args:
node_positions: Junction node positions in world space.
edges: List of (u, v) node index pairs defining rod segments. Each edge creates one
capsule body oriented so its local +Z points from node ``u`` to node ``v``.
radius: Capsule radius.
cfg: Shape configuration for the capsules. If None, :attr:`default_shape_cfg` is used.
stretch_stiffness: Material-like axial stiffness (EA) [N], normalized by edge length
into an effective joint stiffness [N/m]. Defaults to 1.0e9.
stretch_damping: Stretch damping (per joint). Defaults to 0.0.
bend_stiffness: Material-like bend/twist stiffness (EI) [N*m^2], normalized by edge
length into an effective joint stiffness [N*m]. Defaults to 0.0.
bend_damping: Bend/twist damping (per joint). Defaults to 0.0.
label: Optional label prefix for bodies, shapes, joints, and articulations.
wrap_in_articulation: If True, wraps the generated joint forest into one articulation
per connected component.
quaternions: Optional per-edge orientations in world space. If provided, must have
``len(edges)`` elements and each quaternion must align the capsule's local +Z with
the corresponding edge direction ``node_positions[v] - node_positions[u]``. If
None, orientations are computed automatically to align +Z with each edge direction.
junction_collision_filter: If True, adds collision filters between *non-jointed* segment
bodies that are incident to a junction node (degree >= 3). This prevents immediate
self-collision impulses at welded junctions, even though the joint set is a spanning
tree (so not all incident body pairs are directly jointed).
Returns:
A pair ``(body_indices, joint_indices)`` where bodies correspond to
edges in the same order as ``edges``.
"""
if cfg is None:
cfg = self.default_shape_cfg
# Stretch defaults: high stiffness to keep neighboring capsules tightly coupled
stretch_stiffness = 1.0e9 if stretch_stiffness is None else stretch_stiffness
stretch_damping = 0.0 if stretch_damping is None else stretch_damping
# Bend defaults: 0.0 (users must explicitly set for bending resistance)
bend_stiffness = 0.0 if bend_stiffness is None else bend_stiffness
bend_damping = 0.0 if bend_damping is None else bend_damping
if stretch_stiffness < 0.0 or bend_stiffness < 0.0:
raise ValueError("add_rod_graph: stretch_stiffness and bend_stiffness must be >= 0")
if len(node_positions) < 2:
raise ValueError("add_rod_graph: node_positions must contain at least 2 nodes")
if len(edges) < 1:
raise ValueError("add_rod_graph: edges must contain at least 1 edge")
num_nodes = len(node_positions)
num_edges = len(edges)
if quaternions is not None and len(quaternions) != num_edges:
raise ValueError(
f"add_rod_graph: quaternions must have {num_edges} elements for {num_edges} edges, "
f"got {len(quaternions)} quaternions"
)
# Guard against near-zero lengths: edge length is used to normalize stiffness (EA/L, EI/L).
min_segment_length = 1.0e-9
# Coerce all input node positions to wp.vec3 so arithmetic (p1 - p0), wp.length, wp.normalize
# always operate on Warp vector types even if the caller passed tuples/lists.
node_positions_wp: list[wp.vec3] = [axis_to_vec3(p) for p in node_positions]
# Build per-node incidence for spanning-tree traversal.
node_incidence: list[list[int]] = [[] for _ in range(num_nodes)]
# Per-edge data
edge_u: list[int] = []
edge_v: list[int] = []
edge_len: list[float] = []
edge_bodies: list[int] = []
# Create all edge bodies first.
for e_idx, (u, v) in enumerate(edges):
if u < 0 or u >= num_nodes or v < 0 or v >= num_nodes:
raise ValueError(
f"add_rod_graph: edge {e_idx} has invalid node indices ({u}, {v}) for {num_nodes} nodes"
)
if u == v:
raise ValueError(f"add_rod_graph: edge {e_idx} connects a node to itself ({u} -> {v})")
p0 = node_positions_wp[u]
p1 = node_positions_wp[v]
seg_vec = p1 - p0
seg_length = float(wp.length(seg_vec))
if seg_length <= min_segment_length:
raise ValueError(
f"add_rod_graph: edge {e_idx} has a too-small length (length={seg_length:.3e}); "
f"segment length must be > {min_segment_length:.1e}"
)
if quaternions is None:
seg_dir = wp.normalize(seg_vec)
q = quat_between_vectors_robust(wp.vec3(0.0, 0.0, 1.0), seg_dir)
else:
q = quaternions[e_idx]
# Local +Z must align with the segment direction.
seg_dir = wp.normalize(seg_vec)
local_z_world = wp.quat_rotate(q, wp.vec3(0.0, 0.0, 1.0))
alignment = wp.dot(seg_dir, local_z_world)
if alignment < 0.999:
raise ValueError(
"add_rod_graph: quaternion at edge index "
f"{e_idx} does not align capsule +Z with edge direction (node_positions[v] - node_positions[u]); "
"quaternions must be world-space and constructed so that local +Z maps to the "
"edge direction node_positions[v] - node_positions[u]."
)
half_height = 0.5 * seg_length
# Position body at start node, with COM offset to segment center
body_q = wp.transform(p0, q)
com_offset = wp.vec3(0.0, 0.0, half_height)
body_label = f"{label}_edge_body_{e_idx}" if label else None
shape_label = f"{label}_edge_capsule_{e_idx}" if label else None
body_id = self.add_link(xform=body_q, com=com_offset, label=body_label)
# Place capsule so it spans from start to end along +Z
capsule_xform = wp.transform(wp.vec3(0.0, 0.0, half_height), wp.quat_identity())
self.add_shape_capsule(
body_id,
xform=capsule_xform,
radius=radius,
half_height=half_height,
cfg=cfg,
label=shape_label,
)
edge_u.append(u)
edge_v.append(v)
edge_len.append(seg_length)
edge_bodies.append(body_id)
node_incidence[u].append(e_idx)
node_incidence[v].append(e_idx)
def _edge_anchor_xform(e_idx: int, node_idx: int) -> wp.transform:
# Body frame is at start node u; end node v is at local z = edge_len.
if node_idx == edge_u[e_idx]:
z = 0.0
elif node_idx == edge_v[e_idx]:
z = edge_len[e_idx]
else:
raise RuntimeError("add_rod_graph: internal error (node not incident to edge)")
return wp.transform(wp.vec3(0.0, 0.0, float(z)), wp.quat_identity())
joint_counter = 0
jointed_body_pairs: set[tuple[int, int]] = set()
def _remember_jointed_pair(parent_body: int, child_body: int) -> None:
# Canonical order so lookups are symmetric.
if parent_body <= child_body:
jointed_body_pairs.add((parent_body, child_body))
else:
jointed_body_pairs.add((child_body, parent_body))
def _build_joints_star() -> list[int]:
"""Builds joints by connecting incident edges directly at each node."""
nonlocal joint_counter
all_joints: list[int] = []
# No articulation constraints: connect incident edges directly at each node.
# This preserves cycles (rings/loops) but can create multi-parent relationships, which is
# fine when not wrapping into an articulation.
for node_idx in range(num_nodes):
inc = node_incidence[node_idx]
if len(inc) < 2:
continue
# Deterministic parent choice: use the first edge in incidence list.
# Since node_incidence is built by iterating edges in order (0, 1, 2...),
# this implicitly picks the edge with the lowest index as the parent.
parent_edge = inc[0]
parent_body = edge_bodies[parent_edge]
parent_xform = _edge_anchor_xform(parent_edge, node_idx)
for child_edge in inc[1:]:
child_body = edge_bodies[child_edge]
if parent_body == child_body:
raise RuntimeError("add_rod_graph: internal error (self-connection)")
child_xform = _edge_anchor_xform(child_edge, node_idx)
# Normalize stiffness by segment length, consistent with add_rod().
# Use a symmetric length so stiffness is traversal/order invariant.
L_parent = edge_len[parent_edge]
L_child = edge_len[child_edge]
L_sym = 0.5 * (L_parent + L_child)
stretch_ke_eff = stretch_stiffness / L_sym
bend_ke_eff = bend_stiffness / L_sym
joint_counter += 1
joint_label = f"{label}_cable_{joint_counter}" if label else None
j = self.add_joint_cable(
parent=parent_body,
child=child_body,
parent_xform=parent_xform,
child_xform=child_xform,
bend_stiffness=bend_ke_eff,
bend_damping=bend_damping,
stretch_stiffness=stretch_ke_eff,
stretch_damping=stretch_damping,
label=joint_label,
collision_filter_parent=True,
enabled=True,
)
all_joints.append(j)
_remember_jointed_pair(parent_body, child_body)
return all_joints
def _build_joints_forest() -> list[int]:
"""Builds joints using a spanning-forest traversal to ensure articulation-safe (tree) topology."""
nonlocal joint_counter
all_joints: list[int] = []
visited = [False] * num_edges
component_index = 0
for start_edge in range(num_edges):
if visited[start_edge]:
continue
# BFS over edges
queue: deque[int] = deque([start_edge])
visited[start_edge] = True
component_joints: list[int] = []
component_edges: list[int] = []
while queue:
parent_edge = queue.popleft()
component_edges.append(parent_edge)
parent_body = edge_bodies[parent_edge]
for shared_node in (edge_u[parent_edge], edge_v[parent_edge]):
for child_edge in node_incidence[shared_node]:
if child_edge == parent_edge or visited[child_edge]:
continue
child_body = edge_bodies[child_edge]
if parent_body == child_body:
raise RuntimeError("add_rod_graph: internal error (self-connection)")
# Anchors at the shared node on each edge body
parent_xform = _edge_anchor_xform(parent_edge, shared_node)
child_xform = _edge_anchor_xform(child_edge, shared_node)
# Normalize stiffness by segment length, consistent with add_rod().
# Use a symmetric length so stiffness is traversal/order invariant.
L_parent = edge_len[parent_edge]
L_child = edge_len[child_edge]
L_sym = 0.5 * (L_parent + L_child)
stretch_ke_eff = stretch_stiffness / L_sym
bend_ke_eff = bend_stiffness / L_sym
joint_counter += 1
joint_label = f"{label}_cable_{joint_counter}" if label else None
j = self.add_joint_cable(
parent=parent_body,
child=child_body,
parent_xform=parent_xform,
child_xform=child_xform,
bend_stiffness=bend_ke_eff,
bend_damping=bend_damping,
stretch_stiffness=stretch_ke_eff,
stretch_damping=stretch_damping,
label=joint_label,
collision_filter_parent=True,
enabled=True,
)
component_joints.append(j)
all_joints.append(j)
_remember_jointed_pair(parent_body, child_body)
visited[child_edge] = True
queue.append(child_edge)
# If the original node-edge graph contains a cycle, we cannot "close" it with extra
# joints while keeping an articulation tree. Warn so callers don't assume rings/loops
# are preserved under `wrap_in_articulation=True`.
if component_edges:
component_nodes: set[int] = set()
for e_idx in component_edges:
component_nodes.add(edge_u[e_idx])
component_nodes.add(edge_v[e_idx])
# Undirected graph cycle condition: E > V - 1 (for any connected component).
if len(component_edges) > max(0, len(component_nodes) - 1):
warnings.warn(
"add_rod_graph: detected a cycle (closed loop) in the edge graph. "
"With wrap_in_articulation=True, joints are built as a tree/forest, so "
"cycles are not closed. Use wrap_in_articulation=False and add explicit "
"closure constraints if you need a ring/loop.",
UserWarning,
stacklevel=2,
)
# Wrap the connected component into an articulation.
if component_joints:
if label:
art_label = (
f"{label}_articulation_{component_index}"
if component_index > 0
else f"{label}_articulation"
)
else:
art_label = None
self.add_articulation(component_joints, label=art_label)
component_index += 1
return all_joints
if not wrap_in_articulation:
all_joints = _build_joints_star()
else:
all_joints = _build_joints_forest()
if junction_collision_filter:
# Filter collisions among *non-jointed* sibling bodies incident to each junction node
# (degree >= 3). Jointed parent/child pairs are already filtered by
# add_joint_cable(collision_filter_parent=True).
for inc in node_incidence:
if len(inc) < 3:
continue
bodies_set = {edge_bodies[e_idx] for e_idx in inc}
if len(bodies_set) < 2:
continue
bodies = sorted(bodies_set)
for i in range(len(bodies)):
for j in range(i + 1, len(bodies)):
bi = bodies[i]
bj = bodies[j]
if (bi, bj) in jointed_body_pairs:
# Already filtered by add_joint_cable(collision_filter_parent=True).
continue
for si in self.body_shapes.get(bi, []):
for sj in self.body_shapes.get(bj, []):
self.add_shape_collision_filter_pair(int(si), int(sj))
return edge_bodies, all_joints
# endregion
# particles
def add_particle(
self,
pos: Vec3,
vel: Vec3,
mass: float,
radius: float | None = None,
flags: int = ParticleFlags.ACTIVE,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a single particle to the model.
Args:
pos: The initial position of the particle.
vel: The initial velocity of the particle.
mass: The mass of the particle.
radius: The radius of the particle used in collision handling. If None, the radius is set to the default value (:attr:`default_particle_radius`).
flags: The flags that control the dynamical behavior of the particle, see :class:`newton.ParticleFlags`.
custom_attributes: Dictionary of custom attribute names to values.
Note:
Set the mass equal to zero to create a 'kinematic' particle that is not subject to dynamics.
Returns:
The index of the particle in the system.
"""
self.particle_q.append(pos)
self.particle_qd.append(vel)
self.particle_mass.append(mass)
if radius is None:
radius = self.default_particle_radius
self.particle_radius.append(radius)
self.particle_flags.append(flags)
self.particle_world.append(self.current_world)
particle_id = self.particle_count - 1
# Process custom attributes
if custom_attributes:
self._process_custom_attributes(
entity_index=particle_id,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.PARTICLE,
)
return particle_id
def add_particles(
self,
pos: list[Vec3],
vel: list[Vec3],
mass: list[float],
radius: list[float] | None = None,
flags: list[int] | None = None,
custom_attributes: dict[str, Any] | None = None,
):
"""Adds a group of particles to the model.
Args:
pos: The initial positions of the particles.
vel: The initial velocities of the particles.
mass: The mass of the particles.
radius: The radius of the particles used in collision handling. If None, the radius is set to the default value (:attr:`default_particle_radius`).
flags: The flags that control the dynamical behavior of the particles, see :class:`newton.ParticleFlags`.
custom_attributes: Dictionary of custom attribute names to lists of values (one value for each particle).
Note:
Set the mass equal to zero to create a 'kinematic' particle that is not subject to dynamics.
"""
particle_start = self.particle_count
particle_count = len(pos)
self.particle_q.extend(pos)
self.particle_qd.extend(vel)
self.particle_mass.extend(mass)
if radius is None:
radius = [self.default_particle_radius] * particle_count
if flags is None:
flags = [ParticleFlags.ACTIVE] * particle_count
self.particle_radius.extend(radius)
self.particle_flags.extend(flags)
# Maintain world assignment for bulk particle creation
self.particle_world.extend([self.current_world] * particle_count)
# Process custom attributes
if custom_attributes and particle_count:
particle_indices = list(range(particle_start, particle_start + particle_count))
self._process_custom_attributes(
entity_index=particle_indices,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.PARTICLE,
)
def add_spring(
self,
i: int,
j: int,
ke: float,
kd: float,
control: float,
custom_attributes: dict[str, Any] | None = None,
):
"""Adds a spring between two particles in the system
Args:
i: The index of the first particle
j: The index of the second particle
ke: The elastic stiffness of the spring
kd: The damping stiffness of the spring
control: The actuation level of the spring
custom_attributes: Dictionary of custom attribute names to values.
Note:
The spring is created with a rest-length based on the distance
between the particles in their initial configuration.
"""
self.spring_indices.append(i)
self.spring_indices.append(j)
self.spring_stiffness.append(ke)
self.spring_damping.append(kd)
self.spring_control.append(control)
# compute rest length
p = np.asarray(self.particle_q[i], dtype=np.float32)
q = np.asarray(self.particle_q[j], dtype=np.float32)
delta = np.subtract(p, q)
l = np.sqrt(np.dot(delta, delta))
self.spring_rest_length.append(l)
# Process custom attributes
if custom_attributes:
spring_index = len(self.spring_rest_length) - 1
self._process_custom_attributes(
entity_index=spring_index,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.SPRING,
)
def add_triangle(
self,
i: int,
j: int,
k: int,
tri_ke: float | None = None,
tri_ka: float | None = None,
tri_kd: float | None = None,
tri_drag: float | None = None,
tri_lift: float | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> float:
"""Adds a triangular FEM element between three particles in the system.
Triangles are modeled as viscoelastic elements with elastic stiffness and damping
parameters specified on the model. See :attr:`~newton.Model.tri_materials`.
Args:
i: The index of the first particle.
j: The index of the second particle.
k: The index of the third particle.
tri_ke: The elastic stiffness of the triangle. If None, the default value (:attr:`default_tri_ke`) is used.
tri_ka: The area stiffness of the triangle. If None, the default value (:attr:`default_tri_ka`) is used.
tri_kd: The damping stiffness of the triangle. If None, the default value (:attr:`default_tri_kd`) is used.
tri_drag: The drag coefficient of the triangle. If None, the default value (:attr:`default_tri_drag`) is used.
tri_lift: The lift coefficient of the triangle. If None, the default value (:attr:`default_tri_lift`) is used.
custom_attributes: Dictionary of custom attribute names to values.
Return:
The area of the triangle
Note:
The triangle is created with a rest-length based on the distance
between the particles in their initial configuration.
"""
tri_ke = tri_ke if tri_ke is not None else self.default_tri_ke
tri_ka = tri_ka if tri_ka is not None else self.default_tri_ka
tri_kd = tri_kd if tri_kd is not None else self.default_tri_kd
tri_drag = tri_drag if tri_drag is not None else self.default_tri_drag
tri_lift = tri_lift if tri_lift is not None else self.default_tri_lift
# compute basis for 2D rest pose
p = self.particle_q[i]
q = self.particle_q[j]
r = self.particle_q[k]
qp = q - p
rp = r - p
# construct basis aligned with the triangle
n = wp.normalize(wp.cross(qp, rp))
e1 = wp.normalize(qp)
e2 = wp.normalize(wp.cross(n, e1))
R = np.array((e1, e2))
M = np.array((qp, rp))
D = R @ M.T
area = np.linalg.det(D) / 2.0
if area <= 0.0:
print("inverted or degenerate triangle element")
return 0.0
else:
inv_D = np.linalg.inv(D)
self.tri_indices.append((i, j, k))
self.tri_poses.append(inv_D.tolist())
self.tri_activations.append(0.0)
self.tri_materials.append((tri_ke, tri_ka, tri_kd, tri_drag, tri_lift))
self.tri_areas.append(area)
# Process custom attributes
if custom_attributes:
tri_index = len(self.tri_indices) - 1
self._process_custom_attributes(
entity_index=tri_index,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.TRIANGLE,
)
return area
def add_triangles(
self,
i: list[int] | np.ndarray,
j: list[int] | np.ndarray,
k: list[int] | np.ndarray,
tri_ke: list[float] | None = None,
tri_ka: list[float] | None = None,
tri_kd: list[float] | None = None,
tri_drag: list[float] | None = None,
tri_lift: list[float] | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> list[float]:
"""Adds triangular FEM elements between groups of three particles in the system.
Triangles are modeled as viscoelastic elements with elastic stiffness and damping
Parameters specified on the model. See model.tri_ke, model.tri_kd.
Args:
i: The indices of the first particle
j: The indices of the second particle
k: The indices of the third particle
tri_ke: The elastic stiffness of the triangles. If None, the default value (:attr:`default_tri_ke`) is used.
tri_ka: The area stiffness of the triangles. If None, the default value (:attr:`default_tri_ka`) is used.
tri_kd: The damping stiffness of the triangles. If None, the default value (:attr:`default_tri_kd`) is used.
tri_drag: The drag coefficient of the triangles. If None, the default value (:attr:`default_tri_drag`) is used.
tri_lift: The lift coefficient of the triangles. If None, the default value (:attr:`default_tri_lift`) is used.
custom_attributes: Dictionary of custom attribute names to values.
Return:
The areas of the triangles
Note:
A triangle is created with a rest-length based on the distance
between the particles in their initial configuration.
"""
# compute basis for 2D rest pose
q_ = np.asarray(self.particle_q)
p = q_[i]
q = q_[j]
r = q_[k]
qp = q - p
rp = r - p
def normalized(a):
l = np.linalg.norm(a, axis=-1, keepdims=True)
l[l == 0] = 1.0
return a / l
n = normalized(np.cross(qp, rp))
e1 = normalized(qp)
e2 = normalized(np.cross(n, e1))
R = np.concatenate((e1[..., None], e2[..., None]), axis=-1)
M = np.concatenate((qp[..., None], rp[..., None]), axis=-1)
D = np.matmul(R.transpose(0, 2, 1), M)
areas = np.linalg.det(D) / 2.0
areas[areas < 0.0] = 0.0
valid_inds = (areas > 0.0).nonzero()[0]
if len(valid_inds) < len(areas):
print("inverted or degenerate triangle elements")
D[areas == 0.0] = np.eye(2)[None, ...]
inv_D = np.linalg.inv(D)
i_ = np.asarray(i)
j_ = np.asarray(j)
k_ = np.asarray(k)
inds = np.concatenate((i_[valid_inds, None], j_[valid_inds, None], k_[valid_inds, None]), axis=-1)
tri_start = len(self.tri_indices)
self.tri_indices.extend(inds.tolist())
self.tri_poses.extend(inv_D[valid_inds].tolist())
self.tri_activations.extend([0.0] * len(valid_inds))
def init_if_none(arr, defaultValue):
if arr is None:
return [defaultValue] * len(areas)
return arr
tri_ke = init_if_none(tri_ke, self.default_tri_ke)
tri_ka = init_if_none(tri_ka, self.default_tri_ka)
tri_kd = init_if_none(tri_kd, self.default_tri_kd)
tri_drag = init_if_none(tri_drag, self.default_tri_drag)
tri_lift = init_if_none(tri_lift, self.default_tri_lift)
self.tri_materials.extend(
zip(
np.array(tri_ke)[valid_inds],
np.array(tri_ka)[valid_inds],
np.array(tri_kd)[valid_inds],
np.array(tri_drag)[valid_inds],
np.array(tri_lift)[valid_inds],
strict=False,
)
)
areas = areas.tolist()
self.tri_areas.extend(areas)
# Process custom attributes
if custom_attributes and len(valid_inds) > 0:
tri_indices = list(range(tri_start, tri_start + len(valid_inds)))
self._process_custom_attributes(
entity_index=tri_indices,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.TRIANGLE,
)
return areas
def add_tetrahedron(
self,
i: int,
j: int,
k: int,
l: int,
k_mu: float = 1.0e3,
k_lambda: float = 1.0e3,
k_damp: float = 0.0,
custom_attributes: dict[str, Any] | None = None,
) -> float:
"""Adds a tetrahedral FEM element between four particles in the system.
Tetrahedra are modeled as viscoelastic elements with a NeoHookean energy
density based on [Smith et al. 2018].
Args:
i: The index of the first particle
j: The index of the second particle
k: The index of the third particle
l: The index of the fourth particle
k_mu: The first elastic Lame parameter
k_lambda: The second elastic Lame parameter
k_damp: The element's damping stiffness
custom_attributes: Dictionary of custom attribute names to values.
Return:
The volume of the tetrahedron
Note:
The tetrahedron is created with a rest-pose based on the particle's initial configuration
"""
# compute basis for 2D rest pose
p = np.array(self.particle_q[i])
q = np.array(self.particle_q[j])
r = np.array(self.particle_q[k])
s = np.array(self.particle_q[l])
qp = q - p
rp = r - p
sp = s - p
Dm = np.array((qp, rp, sp)).T
volume = np.linalg.det(Dm) / 6.0
if volume <= 0.0:
print("inverted tetrahedral element")
else:
inv_Dm = np.linalg.inv(Dm)
self.tet_indices.append((i, j, k, l))
self.tet_poses.append(inv_Dm.tolist())
self.tet_activations.append(0.0)
self.tet_materials.append((k_mu, k_lambda, k_damp))
# Process custom attributes
if custom_attributes:
tet_index = len(self.tet_indices) - 1
self._process_custom_attributes(
entity_index=tet_index,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.TETRAHEDRON,
)
return volume
def add_edge(
self,
i: int,
j: int,
k: int,
l: int,
rest: float | None = None,
edge_ke: float | None = None,
edge_kd: float | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> int:
"""Adds a bending edge element between two adjacent triangles in the cloth mesh, defined by four vertices.
The bending energy model follows the discrete shell formulation from [Grinspun et al. 2003].
The bending stiffness is controlled by the `edge_ke` parameter, and the bending damping by the `edge_kd` parameter.
Args:
i: The index of the first particle, i.e., opposite vertex 0
j: The index of the second particle, i.e., opposite vertex 1
k: The index of the third particle, i.e., vertex 0
l: The index of the fourth particle, i.e., vertex 1
rest: The rest angle across the edge in radians, if not specified it will be computed
edge_ke: The bending stiffness coefficient
edge_kd: The bending damping coefficient
custom_attributes: Dictionary of custom attribute names to values.
Return:
The index of the edge.
Note:
The edge lies between the particles indexed by 'k' and 'l' parameters with the opposing
vertices indexed by 'i' and 'j'. This defines two connected triangles with counterclockwise
winding: (i, k, l), (j, l, k).
"""
edge_ke = edge_ke if edge_ke is not None else self.default_edge_ke
edge_kd = edge_kd if edge_kd is not None else self.default_edge_kd
# compute rest angle
x3 = self.particle_q[k]
x4 = self.particle_q[l]
if rest is None:
rest = 0.0
if i != -1 and j != -1:
x1 = self.particle_q[i]
x2 = self.particle_q[j]
n1 = wp.normalize(wp.cross(x3 - x1, x4 - x1))
n2 = wp.normalize(wp.cross(x4 - x2, x3 - x2))
e = wp.normalize(x4 - x3)
cos_theta = np.clip(np.dot(n1, n2), -1.0, 1.0)
sin_theta = np.dot(np.cross(n1, n2), e)
rest = math.atan2(sin_theta, cos_theta)
self.edge_indices.append((i, j, k, l))
self.edge_rest_angle.append(rest)
self.edge_rest_length.append(wp.length(x4 - x3))
self.edge_bending_properties.append((edge_ke, edge_kd))
edge_index = len(self.edge_indices) - 1
# Process custom attributes
if custom_attributes:
self._process_custom_attributes(
entity_index=edge_index,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.EDGE,
)
return edge_index
def add_edges(
self,
i: list[int],
j: list[int],
k: list[int],
l: list[int],
rest: list[float] | None = None,
edge_ke: list[float] | None = None,
edge_kd: list[float] | None = None,
custom_attributes: dict[str, Any] | None = None,
) -> None:
"""Adds bending edge elements between two adjacent triangles in the cloth mesh, defined by four vertices.
The bending energy model follows the discrete shell formulation from [Grinspun et al. 2003].
The bending stiffness is controlled by the `edge_ke` parameter, and the bending damping by the `edge_kd` parameter.
Args:
i: The indices of the first particles, i.e., opposite vertex 0
j: The indices of the second particles, i.e., opposite vertex 1
k: The indices of the third particles, i.e., vertex 0
l: The indices of the fourth particles, i.e., vertex 1
rest: The rest angles across the edges in radians, if not specified they will be computed
edge_ke: The bending stiffness coefficients
edge_kd: The bending damping coefficients
custom_attributes: Dictionary of custom attribute names to values.
Note:
The edge lies between the particles indexed by 'k' and 'l' parameters with the opposing
vertices indexed by 'i' and 'j'. This defines two connected triangles with counterclockwise
winding: (i, k, l), (j, l, k).
"""
# Convert inputs to numpy arrays
i_ = np.asarray(i)
j_ = np.asarray(j)
k_ = np.asarray(k)
l_ = np.asarray(l)
# Cache particle positions as numpy array
particle_q_ = np.asarray(self.particle_q)
x3 = particle_q_[k_]
x4 = particle_q_[l_]
x4_minus_x3 = x4 - x3
if rest is None:
rest = np.zeros_like(i_, dtype=float)
valid_mask = (i_ != -1) & (j_ != -1)
# compute rest angle
x1_valid = particle_q_[i_[valid_mask]]
x2_valid = particle_q_[j_[valid_mask]]
x3_valid = particle_q_[k_[valid_mask]]
x4_valid = particle_q_[l_[valid_mask]]
def normalized(a):
l = np.linalg.norm(a, axis=-1, keepdims=True)
l[l == 0] = 1.0
return a / l
n1 = normalized(np.cross(x3_valid - x1_valid, x4_valid - x1_valid))
n2 = normalized(np.cross(x4_valid - x2_valid, x3_valid - x2_valid))
e = normalized(x4_valid - x3_valid)
def dot(a, b):
return (a * b).sum(axis=-1)
cos_theta = np.clip(dot(n1, n2), -1.0, 1.0)
sin_theta = dot(np.cross(n1, n2), e)
rest[valid_mask] = np.arctan2(sin_theta, cos_theta)
rest = rest.tolist()
inds = np.concatenate((i_[:, None], j_[:, None], k_[:, None], l_[:, None]), axis=-1)
edge_start = len(self.edge_indices)
self.edge_indices.extend(inds.tolist())
self.edge_rest_angle.extend(rest)
self.edge_rest_length.extend(np.linalg.norm(x4_minus_x3, axis=1).tolist())
def init_if_none(arr, defaultValue):
if arr is None:
return [defaultValue] * len(i)
return arr
edge_ke = init_if_none(edge_ke, self.default_edge_ke)
edge_kd = init_if_none(edge_kd, self.default_edge_kd)
self.edge_bending_properties.extend(zip(edge_ke, edge_kd, strict=False))
# Process custom attributes
if custom_attributes and len(i) > 0:
edge_indices = list(range(edge_start, edge_start + len(i)))
self._process_custom_attributes(
entity_index=edge_indices,
custom_attrs=custom_attributes,
expected_frequency=Model.AttributeFrequency.EDGE,
)
def add_cloth_grid(
self,
pos: Vec3,
rot: Quat,
vel: Vec3,
dim_x: int,
dim_y: int,
cell_x: float,
cell_y: float,
mass: float,
reverse_winding: bool = False,
fix_left: bool = False,
fix_right: bool = False,
fix_top: bool = False,
fix_bottom: bool = False,
tri_ke: float | None = None,
tri_ka: float | None = None,
tri_kd: float | None = None,
tri_drag: float | None = None,
tri_lift: float | None = None,
edge_ke: float | None = None,
edge_kd: float | None = None,
add_springs: bool = False,
spring_ke: float | None = None,
spring_kd: float | None = None,
particle_radius: float | None = None,
custom_attributes_particles: dict[str, Any] | None = None,
custom_attributes_edges: dict[str, Any] | None = None,
custom_attributes_triangles: dict[str, Any] | None = None,
):
"""Helper to create a regular planar cloth grid
Creates a rectangular grid of particles with FEM triangles and bending elements
automatically.
Args:
pos: The position of the cloth in world space
rot: The orientation of the cloth in world space
vel: The velocity of the cloth in world space
dim_x_: The number of rectangular cells along the x-axis
dim_y: The number of rectangular cells along the y-axis
cell_x: The width of each cell in the x-direction
cell_y: The width of each cell in the y-direction
mass: The mass of each particle
reverse_winding: Flip the winding of the mesh
fix_left: Make the left-most edge of particles kinematic (fixed in place)
fix_right: Make the right-most edge of particles kinematic
fix_top: Make the top-most edge of particles kinematic
fix_bottom: Make the bottom-most edge of particles kinematic
"""
def grid_index(x, y, dim_x):
return y * dim_x + x
indices, vertices = [], []
for y in range(0, dim_y + 1):
for x in range(0, dim_x + 1):
local_pos = wp.vec3(x * cell_x, y * cell_y, 0.0)
vertices.append(local_pos)
if x > 0 and y > 0:
v0 = grid_index(x - 1, y - 1, dim_x + 1)
v1 = grid_index(x, y - 1, dim_x + 1)
v2 = grid_index(x, y, dim_x + 1)
v3 = grid_index(x - 1, y, dim_x + 1)
if reverse_winding:
indices.extend([v0, v1, v2])
indices.extend([v0, v2, v3])
else:
indices.extend([v0, v1, v3])
indices.extend([v1, v2, v3])
start_vertex = len(self.particle_q)
total_mass = mass * (dim_x + 1) * (dim_x + 1)
total_area = cell_x * cell_y * dim_x * dim_y
density = total_mass / total_area
self.add_cloth_mesh(
pos=pos,
rot=rot,
scale=1.0,
vel=vel,
vertices=vertices,
indices=indices,
density=density,
tri_ke=tri_ke,
tri_ka=tri_ka,
tri_kd=tri_kd,
tri_drag=tri_drag,
tri_lift=tri_lift,
edge_ke=edge_ke,
edge_kd=edge_kd,
add_springs=add_springs,
spring_ke=spring_ke,
spring_kd=spring_kd,
particle_radius=particle_radius,
custom_attributes_particles=custom_attributes_particles,
custom_attributes_triangles=custom_attributes_triangles,
custom_attributes_edges=custom_attributes_edges,
)
vertex_id = 0
for y in range(dim_y + 1):
for x in range(dim_x + 1):
particle_mass = mass
particle_flag = ParticleFlags.ACTIVE
if (
(x == 0 and fix_left)
or (x == dim_x and fix_right)
or (y == 0 and fix_bottom)
or (y == dim_y and fix_top)
):
particle_flag = particle_flag & ~ParticleFlags.ACTIVE
particle_mass = 0.0
self.particle_flags[start_vertex + vertex_id] = particle_flag
self.particle_mass[start_vertex + vertex_id] = particle_mass
vertex_id = vertex_id + 1
def add_cloth_mesh(
self,
pos: Vec3,
rot: Quat,
scale: float,
vel: Vec3,
vertices: list[Vec3],
indices: list[int],
density: float,
tri_ke: float | None = None,
tri_ka: float | None = None,
tri_kd: float | None = None,
tri_drag: float | None = None,
tri_lift: float | None = None,
edge_ke: float | None = None,
edge_kd: float | None = None,
add_springs: bool = False,
spring_ke: float | None = None,
spring_kd: float | None = None,
particle_radius: float | None = None,
custom_attributes_particles: dict[str, Any] | None = None,
custom_attributes_edges: dict[str, Any] | None = None,
custom_attributes_triangles: dict[str, Any] | None = None,
custom_attributes_springs: dict[str, Any] | None = None,
) -> None:
"""Helper to create a cloth model from a regular triangle mesh
Creates one FEM triangle element and one bending element for every face
and edge in the input triangle mesh
Args:
pos: The position of the cloth in world space
rot: The orientation of the cloth in world space
vel: The velocity of the cloth in world space
vertices: A list of vertex positions
indices: A list of triangle indices, 3 entries per-face
density: The density per-area of the mesh
particle_radius: The particle_radius which controls particle based collisions.
custom_attributes_particles: Dictionary of custom attribute names to values for the particles.
custom_attributes_edges: Dictionary of custom attribute names to values for the edges.
custom_attributes_triangles: Dictionary of custom attribute names to values for the triangles.
custom_attributes_springs: Dictionary of custom attribute names to values for the springs.
Note:
The mesh should be two-manifold.
"""
tri_ke = tri_ke if tri_ke is not None else self.default_tri_ke
tri_ka = tri_ka if tri_ka is not None else self.default_tri_ka
tri_kd = tri_kd if tri_kd is not None else self.default_tri_kd
tri_drag = tri_drag if tri_drag is not None else self.default_tri_drag
tri_lift = tri_lift if tri_lift is not None else self.default_tri_lift
edge_ke = edge_ke if edge_ke is not None else self.default_edge_ke
edge_kd = edge_kd if edge_kd is not None else self.default_edge_kd
spring_ke = spring_ke if spring_ke is not None else self.default_spring_ke
spring_kd = spring_kd if spring_kd is not None else self.default_spring_kd
particle_radius = particle_radius if particle_radius is not None else self.default_particle_radius
num_verts = int(len(vertices))
num_tris = int(len(indices) / 3)
start_vertex = len(self.particle_q)
start_tri = len(self.tri_indices)
# particles
# for v in vertices:
# p = wp.quat_rotate(rot, v * scale) + pos
# self.add_particle(p, vel, 0.0, radius=particle_radius)
vertices_np = np.array(vertices) * scale
rot_mat_np = np.array(wp.quat_to_matrix(rot), dtype=np.float32).reshape(3, 3)
verts_3d_np = np.dot(vertices_np, rot_mat_np.T) + pos
self.add_particles(
verts_3d_np.tolist(),
[vel] * num_verts,
mass=[0.0] * num_verts,
radius=[particle_radius] * num_verts,
custom_attributes=custom_attributes_particles,
)
# triangles
inds = start_vertex + np.array(indices)
inds = inds.reshape(-1, 3)
areas = self.add_triangles(
inds[:, 0],
inds[:, 1],
inds[:, 2],
[tri_ke] * num_tris,
[tri_ka] * num_tris,
[tri_kd] * num_tris,
[tri_drag] * num_tris,
[tri_lift] * num_tris,
custom_attributes=custom_attributes_triangles,
)
for t in range(num_tris):
area = areas[t]
self.particle_mass[inds[t, 0]] += density * area / 3.0
self.particle_mass[inds[t, 1]] += density * area / 3.0
self.particle_mass[inds[t, 2]] += density * area / 3.0
end_tri = len(self.tri_indices)
adj = MeshAdjacency(self.tri_indices[start_tri:end_tri])
edge_indices = np.fromiter(
(x for e in adj.edges.values() for x in (e.o0, e.o1, e.v0, e.v1)),
int,
).reshape(-1, 4)
self.add_edges(
edge_indices[:, 0],
edge_indices[:, 1],
edge_indices[:, 2],
edge_indices[:, 3],
edge_ke=[edge_ke] * len(edge_indices),
edge_kd=[edge_kd] * len(edge_indices),
custom_attributes=custom_attributes_edges,
)
if add_springs:
spring_indices = set()
for i, j, k, l in edge_indices:
spring_indices.add((min(k, l), max(k, l)))
if i != -1:
spring_indices.add((min(i, k), max(i, k)))
spring_indices.add((min(i, l), max(i, l)))
if j != -1:
spring_indices.add((min(j, k), max(j, k)))
spring_indices.add((min(j, l), max(j, l)))
if i != -1 and j != -1:
spring_indices.add((min(i, j), max(i, j)))
for i, j in spring_indices:
self.add_spring(i, j, spring_ke, spring_kd, control=0.0, custom_attributes=custom_attributes_springs)
def add_particle_grid(
self,
pos: Vec3,
rot: Quat,
vel: Vec3,
dim_x: int,
dim_y: int,
dim_z: int,
cell_x: float,
cell_y: float,
cell_z: float,
mass: float,
jitter: float,
radius_mean: float | None = None,
radius_std: float = 0.0,
flags: list[int] | int | None = None,
custom_attributes: dict[str, Any] | None = None,
):
"""
Adds a regular 3D grid of particles to the model.
This helper function creates a grid of particles arranged in a rectangular lattice,
with optional random jitter and per-particle radius variation. The grid is defined
by its dimensions along each axis and the spacing between particles.
Args:
pos: The world-space position of the grid origin.
rot: The rotation to apply to the grid (as a quaternion).
vel: The initial velocity to assign to each particle.
dim_x: Number of particles along the X axis.
dim_y: Number of particles along the Y axis.
dim_z: Number of particles along the Z axis.
cell_x: Spacing between particles along the X axis.
cell_y: Spacing between particles along the Y axis.
cell_z: Spacing between particles along the Z axis.
mass: Mass to assign to each particle.
jitter: Maximum random offset to apply to each particle position.
radius_mean: Mean radius for particles. If None, uses the builder's default.
radius_std: Standard deviation for particle radii. If > 0, radii are sampled from a normal distribution.
flags: Flags to assign to each particle. If None, uses the builder's default.
custom_attributes: Dictionary of custom attribute names to values for the particles.
Returns:
Nothing. The builder is updated in place.
"""
# local grid
px = np.arange(dim_x) * cell_x
py = np.arange(dim_y) * cell_y
pz = np.arange(dim_z) * cell_z
points = np.stack(np.meshgrid(px, py, pz)).reshape(3, -1).T
# apply transform to points
rot_mat = wp.quat_to_matrix(rot)
points = points @ np.array(rot_mat).reshape(3, 3).T + np.array(pos)
velocity = np.broadcast_to(np.array(vel).reshape(1, 3), points.shape)
# add jitter
rng = np.random.default_rng(42 + len(self.particle_q))
points += (rng.random(points.shape) - 0.5) * jitter
if radius_mean is None:
radius_mean = self.default_particle_radius
radii = np.full(points.shape[0], fill_value=radius_mean)
if radius_std > 0.0:
radii += rng.standard_normal(radii.shape) * radius_std
masses = [mass] * points.shape[0]
if flags is not None:
flags = [flags] * points.shape[0]
# Broadcast scalar custom attribute values to all particles
num_particles = points.shape[0]
broadcast_custom_attrs = None
if custom_attributes:
broadcast_custom_attrs = {}
for key, value in custom_attributes.items():
# Check if value is a sequence (but not string/bytes) or numpy array
is_array = isinstance(value, np.ndarray)
is_sequence = isinstance(value, Sequence) and not isinstance(value, (str, bytes))
if is_array or is_sequence:
# Value is already a sequence/array - validate length
if len(value) != num_particles:
raise ValueError(
f"Custom attribute '{key}' has {len(value)} values but {num_particles} particles in grid"
)
broadcast_custom_attrs[key] = list(value) if is_array else value
else:
# Scalar value - broadcast to all particles
broadcast_custom_attrs[key] = [value] * num_particles
self.add_particles(
pos=points.tolist(),
vel=velocity.tolist(),
mass=masses,
radius=radii.tolist(),
flags=flags,
custom_attributes=broadcast_custom_attrs,
)
def add_soft_grid(
self,
pos: Vec3,
rot: Quat,
vel: Vec3,
dim_x: int,
dim_y: int,
dim_z: int,
cell_x: float,
cell_y: float,
cell_z: float,
density: float,
k_mu: float,
k_lambda: float,
k_damp: float,
fix_left: bool = False,
fix_right: bool = False,
fix_top: bool = False,
fix_bottom: bool = False,
tri_ke: float = 0.0,
tri_ka: float = 0.0,
tri_kd: float = 0.0,
tri_drag: float = 0.0,
tri_lift: float = 0.0,
add_surface_mesh_edges: bool = True,
edge_ke: float = 0.0,
edge_kd: float = 0.0,
particle_radius: float | None = None,
):
"""Helper to create a rectangular tetrahedral FEM grid
Creates a regular grid of FEM tetrahedra and surface triangles. Useful for example
to create beams and sheets. Each hexahedral cell is decomposed into 5
tetrahedral elements.
Args:
pos: The position of the solid in world space
rot: The orientation of the solid in world space
vel: The velocity of the solid in world space
dim_x_: The number of rectangular cells along the x-axis
dim_y: The number of rectangular cells along the y-axis
dim_z: The number of rectangular cells along the z-axis
cell_x: The width of each cell in the x-direction
cell_y: The width of each cell in the y-direction
cell_z: The width of each cell in the z-direction
density: The density of each particle
k_mu: The first elastic Lame parameter
k_lambda: The second elastic Lame parameter
k_damp: The damping stiffness
fix_left: Make the left-most edge of particles kinematic (fixed in place)
fix_right: Make the right-most edge of particles kinematic
fix_top: Make the top-most edge of particles kinematic
fix_bottom: Make the bottom-most edge of particles kinematic
tri_ke: Stiffness for surface mesh triangles. Defaults to 0.0.
tri_ka: Area stiffness for surface mesh triangles. Defaults to 0.0.
tri_kd: Damping for surface mesh triangles. Defaults to 0.0.
tri_drag: Drag coefficient for surface mesh triangles. Defaults to 0.0.
tri_lift: Lift coefficient for surface mesh triangles. Defaults to 0.0.
add_surface_mesh_edges: Whether to create zero-stiffness bending edges on the
generated surface mesh. These edges improve collision robustness for VBD solver. Defaults to True.
edge_ke: Bending edge stiffness used when ``add_surface_mesh_edges`` is True. Defaults to 0.0.
edge_kd: Bending edge damping used when ``add_surface_mesh_edges`` is True. Defaults to 0.0.
particle_radius: particle's contact radius (controls rigidbody-particle contact distance)
Note:
The generated surface triangles and optional edges are for collision purposes.
Their stiffness and damping values default to zero so they do not introduce additional
elastic forces. Set the triangle stiffness parameters above to non-zero values if you
want the surface to behave like a thin skin.
"""
start_vertex = len(self.particle_q)
mass = cell_x * cell_y * cell_z * density
for z in range(dim_z + 1):
for y in range(dim_y + 1):
for x in range(dim_x + 1):
v = wp.vec3(x * cell_x, y * cell_y, z * cell_z)
m = mass
if fix_left and x == 0:
m = 0.0
if fix_right and x == dim_x:
m = 0.0
if fix_top and y == dim_y:
m = 0.0
if fix_bottom and y == 0:
m = 0.0
p = wp.quat_rotate(rot, v) + pos
self.add_particle(p, vel, m, particle_radius)
# dict of open faces
faces = {}
def add_face(i: int, j: int, k: int):
key = tuple(sorted((i, j, k)))
if key not in faces:
faces[key] = (i, j, k)
else:
del faces[key]
def add_tet(i: int, j: int, k: int, l: int):
self.add_tetrahedron(i, j, k, l, k_mu, k_lambda, k_damp)
add_face(i, k, j)
add_face(j, k, l)
add_face(i, j, l)
add_face(i, l, k)
def grid_index(x, y, z):
return (dim_x + 1) * (dim_y + 1) * z + (dim_x + 1) * y + x
for z in range(dim_z):
for y in range(dim_y):
for x in range(dim_x):
v0 = grid_index(x, y, z) + start_vertex
v1 = grid_index(x + 1, y, z) + start_vertex
v2 = grid_index(x + 1, y, z + 1) + start_vertex
v3 = grid_index(x, y, z + 1) + start_vertex
v4 = grid_index(x, y + 1, z) + start_vertex
v5 = grid_index(x + 1, y + 1, z) + start_vertex
v6 = grid_index(x + 1, y + 1, z + 1) + start_vertex
v7 = grid_index(x, y + 1, z + 1) + start_vertex
if (x & 1) ^ (y & 1) ^ (z & 1):
add_tet(v0, v1, v4, v3)
add_tet(v2, v3, v6, v1)
add_tet(v5, v4, v1, v6)
add_tet(v7, v6, v3, v4)
add_tet(v4, v1, v6, v3)
else:
add_tet(v1, v2, v5, v0)
add_tet(v3, v0, v7, v2)
add_tet(v4, v7, v0, v5)
add_tet(v6, v5, v2, v7)
add_tet(v5, v2, v7, v0)
# add surface triangles
start_tri = len(self.tri_indices)
for _k, v in faces.items():
self.add_triangle(v[0], v[1], v[2], tri_ke, tri_ka, tri_kd, tri_drag, tri_lift)
end_tri = len(self.tri_indices)
if add_surface_mesh_edges:
# add surface mesh edges (for collision)
if end_tri > start_tri:
adj = MeshAdjacency(self.tri_indices[start_tri:end_tri])
edge_indices = np.fromiter(
(x for e in adj.edges.values() for x in (e.o0, e.o1, e.v0, e.v1)),
int,
).reshape(-1, 4)
if len(edge_indices) > 0:
# Add edges with specified stiffness/damping (for collision)
for o1, o2, v1, v2 in edge_indices:
self.add_edge(o1, o2, v1, v2, None, edge_ke, edge_kd)
def add_soft_mesh(
self,
pos: Vec3,
rot: Quat,
scale: float,
vel: Vec3,
mesh: TetMesh | None = None,
vertices: list[Vec3] | None = None,
indices: list[int] | None = None,
density: float | None = None,
k_mu: float | np.ndarray | None = None,
k_lambda: float | np.ndarray | None = None,
k_damp: float | np.ndarray | None = None,
tri_ke: float = 0.0,
tri_ka: float = 0.0,
tri_kd: float = 0.0,
tri_drag: float = 0.0,
tri_lift: float = 0.0,
add_surface_mesh_edges: bool = True,
edge_ke: float = 0.0,
edge_kd: float = 0.0,
particle_radius: float | None = None,
) -> None:
"""Helper to create a tetrahedral model from an input tetrahedral mesh.
Can be called with either a :class:`~newton.TetMesh` object or raw
``vertices``/``indices`` arrays. When both are provided, explicit
parameters override the values from the TetMesh.
Args:
pos: The position of the solid in world space.
rot: The orientation of the solid in world space.
scale: Uniform scale applied to vertex positions.
vel: The velocity of the solid in world space.
mesh: A :class:`~newton.TetMesh` object. When provided, its
vertices, indices, material arrays, density, and pre-computed
surface triangles are used directly.
vertices: A list of vertex positions, array of 3D points.
Required if ``mesh`` is not provided.
indices: A list of tetrahedron indices, 4 entries per-element,
flattened array. Required if ``mesh`` is not provided.
density: The density [kg/m^3] of the mesh. Overrides ``mesh.density``
if both are provided.
k_mu: The first elastic Lame parameter [Pa]. Scalar or per-element
array. Overrides ``mesh.k_mu`` if both are provided.
k_lambda: The second elastic Lame parameter [Pa]. Scalar or
per-element array. Overrides ``mesh.k_lambda`` if both are
provided.
k_damp: The damping stiffness. Scalar or per-element array.
Overrides ``mesh.k_damp`` if both are provided.
tri_ke: Stiffness for surface mesh triangles. Defaults to 0.0.
tri_ka: Area stiffness for surface mesh triangles. Defaults to 0.0.
tri_kd: Damping for surface mesh triangles. Defaults to 0.0.
tri_drag: Drag coefficient for surface mesh triangles. Defaults to 0.0.
tri_lift: Lift coefficient for surface mesh triangles. Defaults to 0.0.
add_surface_mesh_edges: Whether to create zero-stiffness bending edges on the
generated surface mesh. These edges improve collision robustness for VBD solver. Defaults to True.
edge_ke: Bending edge stiffness used when ``add_surface_mesh_edges`` is True. Defaults to 0.0.
edge_kd: Bending edge damping used when ``add_surface_mesh_edges`` is True. Defaults to 0.0.
particle_radius: particle's contact radius (controls rigidbody-particle contact distance).
Note:
**Parameter resolution order:** explicit argument > :class:`~newton.TetMesh`
attribute > builder default (:attr:`default_tet_density`,
:attr:`default_tet_k_mu`, :attr:`default_tet_k_lambda`,
:attr:`default_tet_k_damp`).
The generated surface triangles and optional edges are for collision purposes.
Their stiffness and damping values default to zero so they do not introduce additional
elastic forces. Set the stiffness parameters above to non-zero values if you
want the surface to behave like a thin skin.
"""
from ..geometry.types import TetMesh # noqa: PLC0415
# Resolve parameters: explicit args > mesh attributes > error
if mesh is not None:
if not isinstance(mesh, TetMesh):
raise TypeError(f"mesh must be a TetMesh, got {type(mesh).__name__}")
if vertices is None:
vertices = mesh.vertices
if indices is None:
indices = mesh.tet_indices
if density is None:
density = mesh.density
if k_mu is None:
k_mu = mesh.k_mu
if k_lambda is None:
k_lambda = mesh.k_lambda
if k_damp is None:
k_damp = mesh.k_damp
if vertices is None or indices is None:
raise ValueError("Either 'mesh' or both 'vertices' and 'indices' must be provided.")
if density is None:
density = self.default_tet_density
if k_mu is None:
k_mu = self.default_tet_k_mu
if k_lambda is None:
k_lambda = self.default_tet_k_lambda
if k_damp is None:
k_damp = self.default_tet_k_damp
num_tets = int(len(indices) / 4)
k_mu_arr = np.broadcast_to(np.asarray(k_mu, dtype=np.float32).flatten(), num_tets)
k_lambda_arr = np.broadcast_to(np.asarray(k_lambda, dtype=np.float32).flatten(), num_tets)
k_damp_arr = np.broadcast_to(np.asarray(k_damp, dtype=np.float32).flatten(), num_tets)
# Extract custom attributes grouped by frequency, validating against builder registry
particle_custom: dict[str, np.ndarray] = {}
tet_custom: dict[str, np.ndarray] = {}
tri_custom: dict[str, np.ndarray] = {}
if mesh is not None and mesh.custom_attributes:
for attr_name, (arr, freq) in mesh.custom_attributes.items():
registered = self.custom_attributes.get(attr_name)
if registered is None:
raise ValueError(
f"TetMesh custom attribute '{attr_name}' is not registered in ModelBuilder. "
f"Register it first via add_custom_attribute()."
)
if registered.frequency != freq:
raise ValueError(
f"Frequency mismatch for custom attribute '{attr_name}': TetMesh has "
f"{Model.AttributeFrequency(freq).name} but ModelBuilder expects "
f"{registered.frequency.name}."
)
if freq == Model.AttributeFrequency.PARTICLE:
particle_custom[attr_name] = arr
elif freq == Model.AttributeFrequency.TETRAHEDRON:
tet_custom[attr_name] = arr
elif freq == Model.AttributeFrequency.TRIANGLE:
tri_custom[attr_name] = arr
start_vertex = len(self.particle_q)
pos = wp.vec3(pos[0], pos[1], pos[2])
# add particles
for vi, v in enumerate(vertices):
p = wp.quat_rotate(rot, wp.vec3(v[0], v[1], v[2]) * scale) + pos
p_custom = {k: arr[vi] for k, arr in particle_custom.items()} if particle_custom else None
self.add_particle(p, vel, 0.0, particle_radius, custom_attributes=p_custom)
# add tetrahedra
for t in range(num_tets):
v0 = start_vertex + indices[t * 4 + 0]
v1 = start_vertex + indices[t * 4 + 1]
v2 = start_vertex + indices[t * 4 + 2]
v3 = start_vertex + indices[t * 4 + 3]
t_custom = {k: arr[t] for k, arr in tet_custom.items()} if tet_custom else None
volume = self.add_tetrahedron(
v0,
v1,
v2,
v3,
float(k_mu_arr[t]),
float(k_lambda_arr[t]),
float(k_damp_arr[t]),
custom_attributes=t_custom,
)
# distribute volume fraction to particles
if volume > 0.0:
self.particle_mass[v0] += density * volume / 4.0
self.particle_mass[v1] += density * volume / 4.0
self.particle_mass[v2] += density * volume / 4.0
self.particle_mass[v3] += density * volume / 4.0
# Compute surface triangles — reuse pre-computed result from TetMesh
# only when the caller did not override the indices.
if mesh is not None and indices is mesh.tet_indices and len(mesh.surface_tri_indices) > 0:
surface_tri_indices = mesh.surface_tri_indices
else:
surface_tri_indices = TetMesh.compute_surface_triangles(indices)
# add surface triangles
start_tri = len(self.tri_indices)
surf = surface_tri_indices.reshape(-1, 3)
for ti, tri in enumerate(surf):
tr_custom = {k: arr[ti] for k, arr in tri_custom.items()} if tri_custom else None
self.add_triangle(
start_vertex + int(tri[0]),
start_vertex + int(tri[1]),
start_vertex + int(tri[2]),
tri_ke,
tri_ka,
tri_kd,
tri_drag,
tri_lift,
custom_attributes=tr_custom,
)
end_tri = len(self.tri_indices)
if add_surface_mesh_edges:
# add surface mesh edges (for collision)
if end_tri > start_tri:
adj = MeshAdjacency(self.tri_indices[start_tri:end_tri])
edge_indices = np.fromiter(
(x for e in adj.edges.values() for x in (e.o0, e.o1, e.v0, e.v1)),
int,
).reshape(-1, 4)
if len(edge_indices) > 0:
# Add edges with specified stiffness/damping (for collision)
for o1, o2, v1, v2 in edge_indices:
self.add_edge(o1, o2, v1, v2, None, edge_ke, edge_kd)
# incrementally updates rigid body mass with additional mass and inertia expressed at a local to the body
def _update_body_mass(self, i: int, m: float, inertia: Mat33, p: Vec3, q: Quat):
if i == -1:
return
# find new COM
new_mass = self.body_mass[i] + m
if new_mass == 0.0: # no mass
return
new_com = (self.body_com[i] * self.body_mass[i] + p * m) / new_mass
# shift inertia to new COM
com_offset = new_com - self.body_com[i]
shape_offset = new_com - p
new_inertia = transform_inertia(
self.body_mass[i], self.body_inertia[i], com_offset, wp.quat_identity()
) + transform_inertia(m, inertia, shape_offset, q)
self.body_mass[i] = new_mass
self.body_inertia[i] = new_inertia
self.body_com[i] = new_com
if new_mass > 0.0:
self.body_inv_mass[i] = 1.0 / new_mass
else:
self.body_inv_mass[i] = 0.0
if any(x for x in new_inertia):
self.body_inv_inertia[i] = wp.inverse(new_inertia)
else:
self.body_inv_inertia[i] = new_inertia
def _validate_parent_body(self, parent_body: int, child: int) -> None:
"""
Validate that parent_body is a valid body index.
Args:
parent_body: The parent body index to validate (-1 for world is OK).
child: The child body index (to check for self-reference).
Raises:
ValueError: If validation fails.
"""
if parent_body == -1:
return # -1 is valid (world reference)
# Check bounds
if parent_body < -1:
raise ValueError(f"Invalid parent_body index: {parent_body}. Must be >= -1 (use -1 for world).")
if parent_body >= len(self.body_mass):
raise ValueError(
f"Invalid parent_body index: {parent_body}. "
f"Body index out of bounds (model has {len(self.body_mass)} bodies)."
)
# Check self-reference
if parent_body == child:
raise ValueError(f"Cannot attach body {child} to itself (parent_body == child).")
# Validate body has positive mass (optional warning)
if self.body_mass[parent_body] <= 0.0:
warnings.warn(
f"parent_body {parent_body} has zero or negative mass ({self.body_mass[parent_body]}). "
f"This may cause unexpected behavior.",
UserWarning,
stacklevel=3,
)
def _validate_kinematic_joint_attachment(self, child: int, parent: int) -> None:
"""Validate that kinematic bodies only attach to the world."""
if parent == -1 or not (int(self.body_flags[child]) & int(BodyFlags.KINEMATIC)):
return
child_label = self.body_label[child]
parent_label = self.body_label[parent]
raise ValueError(
f"Body {child} ('{child_label}') is kinematic but is attached to parent body {parent} "
f"('{parent_label}'). Only root bodies (whose joint parent is the world) can be kinematic."
)
def _validate_kinematic_articulation_joints(self, joint_indices: Iterable[int]) -> None:
"""Validate that all kinematic joints in an articulation are rooted at the world."""
for joint_idx in joint_indices:
self._validate_kinematic_joint_attachment(self.joint_child[joint_idx], self.joint_parent[joint_idx])
def _find_articulation_for_body(self, body_id: int) -> int | None:
"""
Find which articulation (if any) contains the given body.
A body "belongs to" the articulation where it appears as a child in a joint.
If a body is only a parent (e.g., root body of an articulation), it belongs
to the articulation of its child joints.
Args:
body_id: The body index to search for.
Returns:
The articulation index if found, or ``None`` if the body is not in any articulation.
Algorithm:
1. Priority 1: Find articulation where body is a child (most common case)
- A body can only be a child in ONE joint (tree structure)
- This uniquely identifies the body's home articulation
2. Priority 2: Find articulation where body is a parent (for root bodies)
- Root bodies are parents but not children
- If parent in multiple articulations, returns the first found
- This should be rare; most bodies are children in exactly one articulation
Note:
In valid tree structures, a body should be a child in at most one joint,
making this lookup deterministic. Bodies that are only parents (root bodies)
may appear in multiple articulations; in such cases, the first articulation
found is returned.
"""
# Priority 1: Check if body is a child in any joint
# A body should be a child in at most ONE joint (tree structure)
for joint_idx in range(len(self.joint_child)):
if self.joint_child[joint_idx] == body_id:
art_id = self.joint_articulation[joint_idx]
if art_id >= 0: # -1 means no articulation
return art_id # Body found as child - this is its home articulation
# Priority 2: If not found as child, check if body is a parent in any joint
# This handles root bodies that are parents but not children
parent_articulations = []
for joint_idx in range(len(self.joint_parent)):
if self.joint_parent[joint_idx] == body_id:
art_id = self.joint_articulation[joint_idx]
if art_id >= 0 and art_id not in parent_articulations:
parent_articulations.append(art_id)
# Use first articulation found, but warn if multiple (shouldn't happen in valid trees)
if parent_articulations:
result = parent_articulations[0]
if len(parent_articulations) > 1:
warnings.warn(
f"Body {body_id} is a parent in multiple articulations {parent_articulations}. "
f"Using articulation {result}. This may indicate an unusual model structure.",
UserWarning,
stacklevel=3,
)
return result
return None
@staticmethod
def _validate_base_joint_params(floating: bool | None, base_joint: dict | None, parent: int) -> None:
"""
Validate floating and base_joint parameter combinations.
This is a shared validation function used by all importers (MJCF, URDF, USD)
to ensure consistent parameter validation.
Args:
floating: The floating parameter value (True, False, or None).
base_joint: Dict with joint parameters (or None).
parent: The parent body index (-1 for world, >= 0 for a body).
Raises:
ValueError: If parameter combinations are invalid:
- Both floating and base_joint are specified (mutually exclusive)
- floating=True with parent != -1 (FREE joints require world frame)
- base_joint dict contains conflicting keys like 'parent', 'child', etc.
"""
if floating is not None and base_joint is not None:
raise ValueError(
f"Cannot specify both 'floating' and 'base_joint' parameters. "
f"These are mutually exclusive ways to control root attachment:\n"
f" - Use 'floating' for simple FREE/FIXED joints\n"
f" - Use 'base_joint' dict for custom joint parameters\n"
f"Current values: floating={floating}, base_joint={{dict}}"
)
if floating is True and parent != -1:
raise ValueError(
f"Cannot create FREE joint when parent_body={parent} (not world). "
f"FREE joints must connect to world frame (parent_body=-1).\n"
f"Did you mean:\n"
f" - Use floating=False to create FIXED joint to parent\n"
f" - Use base_joint dict with D6 joint parameters for 6-DOF mobility attached to parent"
)
# Validate base_joint dict doesn't contain conflicting keys
if base_joint is not None:
conflicting_keys = set(base_joint.keys()) & {"parent", "child", "parent_xform", "child_xform"}
if conflicting_keys:
raise ValueError(
f"base_joint dict cannot specify {conflicting_keys}. "
f"These parameters are automatically set based on parent_body and attachment:\n"
f" - 'parent' is set from parent_body parameter (currently {parent})\n"
f" - 'child' is set to the imported root body\n"
f" - 'parent_xform' and 'child_xform' are set from xform parameter\n"
f"Please remove {conflicting_keys} from the base_joint dict and use the "
f"parent_body argument instead."
)
def _check_sequential_composition(self, parent_body: int) -> int | None:
"""
Check if attaching to parent_body is sequential (most recent articulation).
Args:
parent_body: The parent body index to check.
Returns:
The parent articulation ID, or None if parent_body is world (-1) or not in an articulation.
Raises:
ValueError: If attempting to attach to a non-sequential articulation.
"""
if parent_body == -1:
return None
parent_articulation = self._find_articulation_for_body(parent_body)
if parent_articulation is None:
return None
num_articulations = len(self.articulation_start)
is_sequential = parent_articulation == num_articulations - 1
if is_sequential:
return parent_articulation
else:
body_name = self.body_label[parent_body] if parent_body < len(self.body_label) else f"#{parent_body}"
raise ValueError(
f"Cannot attach to parent_body {body_name} in articulation #{parent_articulation} "
f"(most recent is #{num_articulations - 1}). "
f"Attach to the most recently added articulation or build in order."
)
def _finalize_imported_articulation(
self,
joint_indices: list[int],
parent_body: int,
articulation_label: str | None = None,
custom_attributes: dict | None = None,
) -> None:
"""
Attach imported joints to parent articulation or create a new articulation.
This helper method encapsulates the common logic used by all importers (MJCF, URDF, USD)
for handling articulation creation after importing a model.
Args:
joint_indices: List of joint indices from the imported model.
parent_body: The parent body index (-1 for world, or a body index for hierarchical composition).
articulation_label: Optional label for the articulation (e.g., model name).
custom_attributes: Optional custom attributes for the articulation.
Note:
- If parent_body != -1 and it belongs to an articulation, the imported joints are added
to the parent's articulation (only works for sequential composition).
- If parent_body != -1 but is not in any articulation, raises ValueError.
- If parent_body == -1, a new articulation is created.
- If joint_indices is empty, does nothing.
Raises:
ValueError: If parent_body is specified but not part of any articulation.
"""
if not joint_indices:
return
if parent_body != -1:
# Check if attachment is sequential
parent_articulation = self._check_sequential_composition(parent_body=parent_body)
if parent_articulation is not None:
self._validate_kinematic_articulation_joints(joint_indices)
# Mark all new joints as belonging to the parent's articulation
for joint_idx in joint_indices:
self.joint_articulation[joint_idx] = parent_articulation
else:
# Parent body exists but is not in any articulation - this is an error
# because user explicitly specified parent_body but it can't be used
body_name = self.body_label[parent_body] if parent_body < len(self.body_label) else f"#{parent_body}"
raise ValueError(
f"Cannot attach to parent_body '{body_name}': body is not part of any articulation. "
f"Only bodies within articulations can be used as parent_body. "
f"To create an independent articulation, use parent_body=-1 (default)."
)
else:
# No parent_body specified, create a new articulation
self.add_articulation(
joints=joint_indices,
label=articulation_label,
custom_attributes=custom_attributes,
)
def _add_base_joint(
self,
child: int,
floating: bool | None = None,
base_joint: dict | None = None,
label: str | None = None,
parent_xform: Transform | None = None,
child_xform: Transform | None = None,
parent: int = -1,
) -> int:
"""
Internal helper for importers to create base joints.
This method is used by importers (URDF, MJCF, USD) to attach imported bodies
to the world or to a parent body with the appropriate joint type.
Args:
child: The body index to connect.
floating: If None (default), behavior depends on format-specific defaults.
If True, creates a FREE joint (only valid when ``parent == -1``).
If False, always creates a fixed joint.
base_joint: Dict with joint parameters passed to :meth:`add_joint`.
Cannot be specified together with ``floating``.
label: A unique label for the joint.
parent_xform: The transform of the joint in the parent frame.
If None, defaults to ``body_q[child]`` when parent is world (-1),
or identity when parent is another body.
child_xform: The transform of the joint in the child frame.
If None, defaults to identity transform.
parent: The index of the parent body. Use -1 (default) to connect to world.
Returns:
The index of the created joint.
Raises:
ValueError: If both ``floating`` and ``base_joint`` are specified,
or if ``floating=True`` with ``parent != -1``, or if parent body
is not part of any articulation.
"""
# Validate parameter combinations
self._validate_base_joint_params(floating, base_joint, parent)
self._validate_parent_body(parent, child)
# Validate that parent body is in an articulation (if not world)
if parent != -1:
parent_articulation = self._find_articulation_for_body(parent)
if parent_articulation is None:
body_name = self.body_label[parent] if parent < len(self.body_label) else f"#{parent}"
raise ValueError(
f"Cannot attach to parent_body '{body_name}': body is not part of any articulation. "
f"Only bodies within articulations can be used as parent_body. "
f"To create an independent articulation, use parent_body=-1 (default)."
)
# Determine transforms
if parent_xform is None:
parent_xform = self.body_q[child] if parent == -1 else wp.transform_identity()
if child_xform is None:
child_xform = wp.transform_identity()
# Create joint based on parameters
if base_joint is not None:
# Use custom joint parameters from dict
joint_params = base_joint.copy()
joint_params["parent"] = parent
joint_params["child"] = child
joint_params["parent_xform"] = parent_xform
joint_params["child_xform"] = child_xform
if "label" not in joint_params and label is not None:
joint_params["label"] = label
return self.add_joint(**joint_params)
elif floating is True or (floating is None and parent == -1):
# FREE joint (floating=True always requires parent==-1, validated above)
# Note: We don't pass parent_xform here because add_joint_free initializes joint_q from body_q[child]
# and the caller (e.g., URDF importer) will set the correct joint_q values afterward
return self.add_joint_free(child, label=label)
else:
# FIXED joint (floating=False or floating=None with parent body)
return self.add_joint_fixed(parent, child, parent_xform, child_xform, label=label)
def _add_base_joints_to_floating_bodies(
self,
new_bodies: Iterable[int] | None = None,
floating: bool | None = None,
base_joint: dict | None = None,
):
"""
Adds joints and single-joint articulations to every rigid body that is not a child in any joint
and has positive mass.
Args:
new_bodies: The set of body indices to consider for adding joints.
floating: If True or None (default), floating bodies receive a free joint.
If False, floating bodies receive a fixed joint.
base_joint: Dict with joint parameters passed to :meth:`add_joint`.
When specified, this takes precedence over the ``floating`` parameter.
Note:
- Bodies that are already a child in any joint will be skipped.
- Only bodies with strictly positive mass will receive a joint.
- Each joint is added to its own single-joint articulation.
- This is useful for ensuring that all floating (unconnected) bodies are properly articulated.
"""
if new_bodies is None:
return
# set(self.joint_child) is connected_bodies
floating_bodies = set(new_bodies) - set(self.joint_child)
for body_id in floating_bodies:
if self.body_mass[body_id] <= 0:
continue
joint = self._add_base_joint(body_id, floating=floating, base_joint=base_joint)
# Use body label as articulation label for single-body articulations
self.add_articulation([joint], label=self.body_label[body_id])
def request_contact_attributes(self, *attributes: str) -> None:
"""
Request that specific contact attributes be allocated when creating a Contacts object from the finalized Model.
Args:
*attributes: Variable number of attribute names (strings).
"""
# Local import to avoid adding more module-level dependencies in this large file.
from .contacts import Contacts # noqa: PLC0415
Contacts.validate_extended_attributes(attributes)
self._requested_contact_attributes.update(attributes)
def request_state_attributes(self, *attributes: str) -> None:
"""
Request that specific state attributes be allocated when creating a State object from the finalized Model.
See :ref:`extended_state_attributes` for details and usage.
Args:
*attributes: Variable number of attribute names (strings).
"""
# Local import to avoid adding more module-level dependencies in this large file.
from .state import State # noqa: PLC0415
State.validate_extended_attributes(attributes)
self._requested_state_attributes.update(attributes)
def set_coloring(self, particle_color_groups: Iterable[Iterable[int] | np.ndarray]) -> None:
"""
Sets coloring information with user-provided coloring.
Args:
particle_color_groups: A list of list or `np.array` with `dtype`=`int`. The length of the list is the number of colors
and each list or `np.array` contains the indices of vertices with this color.
"""
particle_color_groups = [
color_group if isinstance(color_group, np.ndarray) else np.array(color_group)
for color_group in particle_color_groups
]
self.particle_color_groups = particle_color_groups
def color(
self,
include_bending: bool = False,
balance_colors: bool = True,
target_max_min_color_ratio: float = 1.1,
coloring_algorithm: ColoringAlgorithm = ColoringAlgorithm.MCS,
) -> None:
"""
Runs coloring algorithm to generate coloring information.
This populates both :attr:`particle_color_groups` (for particles) and
:attr:`body_color_groups` (for rigid bodies) on the builder, which are
consumed by :class:`newton.solvers.SolverVBD`.
Call :meth:`color` (or :meth:`set_coloring`) before
:meth:`finalize ` when using
:class:`newton.solvers.SolverVBD`; :meth:`finalize ` does not
implicitly color the model.
Args:
include_bending: Whether to include bending edges in the coloring graph. Set to `True` if your
model contains bending edges (added via :meth:`add_edge`) that participate in bending constraints.
When enabled, the coloring graph includes connections between opposite vertices of each edge (o1-o2),
ensuring proper dependency handling for parallel bending computations. Leave as `False` if your model
has no bending edges or if bending edges should not affect the coloring.
balance_colors: Whether to apply the color balancing algorithm to balance the size of each color
target_max_min_color_ratio: the color balancing algorithm will stop when the ratio between the largest color and
the smallest color reaches this value
coloring_algorithm: Coloring algorithm to use. `ColoringAlgorithm.MCS` uses
maximum cardinality search (MCS), while `ColoringAlgorithm.GREEDY` uses
degree-ordered greedy coloring. The MCS algorithm typically generates 30% to
50% fewer colors compared to the ordered greedy algorithm, while maintaining
the same linear complexity. Although MCS has a constant overhead that makes
it about twice as slow as the greedy algorithm, it produces significantly
better coloring results. We recommend using MCS, especially if coloring is
only part of preprocessing.
Note:
References to the coloring algorithm:
MCS: Pereira, F. M. Q., & Palsberg, J. (2005, November). Register allocation via coloring of chordal graphs. In Asian Symposium on Programming Languages and Systems (pp. 315-329). Berlin, Heidelberg: Springer Berlin Heidelberg.
Ordered Greedy: Ton-That, Q. M., Kry, P. G., & Andrews, S. (2023). Parallel block Neo-Hookean XPBD using graph clustering. Computers & Graphics, 110, 1-10.
"""
if self.particle_count != 0:
tri_indices = np.array(self.tri_indices, dtype=np.int32) if self.tri_indices else None
tri_materials = np.array(self.tri_materials)
tet_indices = np.array(self.tet_indices, dtype=np.int32) if self.tet_indices else None
tet_materials = np.array(self.tet_materials)
bending_edge_indices = None
bending_edge_active_mask = None
if include_bending and self.edge_indices:
bending_edge_indices = np.array(self.edge_indices, dtype=np.int32)
bending_edge_props = np.array(self.edge_bending_properties)
# Active if either stiffness or damping is non-zero
bending_edge_active_mask = (bending_edge_props[:, 0] != 0.0) | (bending_edge_props[:, 1] != 0.0)
graph_edge_indices = construct_particle_graph(
tri_indices,
tri_materials[:, 0] * tri_materials[:, 1] if len(tri_materials) else None,
bending_edge_indices,
bending_edge_active_mask,
tet_indices,
tet_materials[:, 0] * tet_materials[:, 1] if len(tet_materials) else None,
)
if len(graph_edge_indices) > 0:
self.particle_color_groups = color_graph(
self.particle_count,
graph_edge_indices,
balance_colors,
target_max_min_color_ratio,
coloring_algorithm,
)
else:
# No edges to color - assign all particles to single color group
if len(self.particle_q) > 0:
self.particle_color_groups = [np.arange(len(self.particle_q), dtype=int)]
else:
self.particle_color_groups = []
# Also color rigid bodies based on joint connectivity
self.body_color_groups = color_rigid_bodies(
self.body_count,
self.joint_parent,
self.joint_child,
algorithm=coloring_algorithm,
balance_colors=balance_colors,
target_max_min_color_ratio=target_max_min_color_ratio,
)
def _validate_world_ordering(self):
"""Validate that world indices are monotonic, contiguous, and properly ordered.
This method checks:
1. World indices are monotonic (non-decreasing after first non-negative)
2. World indices are contiguous (no gaps in sequence)
3. Global entities (world -1) only appear at beginning or end of arrays
4. All world indices are in valid range [-1, world_count-1]
Raises:
ValueError: If any validation check fails.
"""
# List of all world arrays to validate
world_arrays = [
("particle_world", self.particle_world),
("body_world", self.body_world),
("shape_world", self.shape_world),
("joint_world", self.joint_world),
("articulation_world", self.articulation_world),
("equality_constraint_world", self.equality_constraint_world),
("constraint_mimic_world", self.constraint_mimic_world),
]
all_world_indices = set()
for array_name, world_array in world_arrays:
if not world_array:
continue
arr = np.array(world_array, dtype=np.int32)
# Check for invalid world indices (must be in range [-1, world_count-1])
max_valid = self.world_count - 1
invalid_indices = np.where((arr < -1) | (arr > max_valid))[0]
if len(invalid_indices) > 0:
invalid_values = arr[invalid_indices]
raise ValueError(
f"Invalid world index in {array_name}: found value(s) {invalid_values.tolist()} "
f"at indices {invalid_indices.tolist()}. Valid range is -1 to {max_valid} (world_count={self.world_count})."
)
# Check for global entity positioning (world -1)
# Find first and last occurrence of -1
negative_indices = np.where(arr == -1)[0]
if len(negative_indices) > 0:
# Check that all -1s form contiguous blocks at start and/or end
# Count -1s at the start
start_neg_count = 0
for i in range(len(arr)):
if arr[i] == -1:
start_neg_count += 1
else:
break
# Count -1s at the end (but only if they don't overlap with start)
end_neg_count = 0
if start_neg_count < len(arr): # There are non-negative values after the start block
for i in range(len(arr) - 1, -1, -1):
if arr[i] == -1:
end_neg_count += 1
else:
break
expected_neg_count = start_neg_count + end_neg_count
actual_neg_count = len(negative_indices)
if expected_neg_count != actual_neg_count:
# There are -1s in the middle
raise ValueError(
f"Invalid world ordering in {array_name}: global entities (world -1) "
f"must only appear at the beginning or end of the array, not in the middle. "
f"Found -1 values at indices: {negative_indices.tolist()}"
)
# Check monotonic ordering for non-negative values
non_neg_mask = arr >= 0
if np.any(non_neg_mask):
non_neg_values = arr[non_neg_mask]
# Check that non-negative values are monotonic (non-decreasing)
if not np.all(non_neg_values[1:] >= non_neg_values[:-1]):
# Find where the order breaks
for i in range(1, len(non_neg_values)):
if non_neg_values[i] < non_neg_values[i - 1]:
raise ValueError(
f"Invalid world ordering in {array_name}: world indices must be monotonic "
f"(non-decreasing). Found world {non_neg_values[i]} after world {non_neg_values[i - 1]}."
)
# Collect all non-negative world indices for contiguity check
all_world_indices.update(non_neg_values)
# Check contiguity: all world indices should form a sequence 0, 1, 2, ..., n-1
if all_world_indices:
world_list = sorted(all_world_indices)
expected = list(range(world_list[-1] + 1))
if world_list != expected:
missing = set(expected) - set(world_list)
raise ValueError(
f"World indices are not contiguous. Missing world(s): {sorted(missing)}. "
f"Found worlds: {world_list}. Worlds must form a continuous sequence starting from 0."
)
def _validate_joints(self):
"""Validate that all joints belong to an articulation, except for "loop joints".
Loop joints connect two bodies that are already reachable via articulated joints
(used to create kinematic loops, converted to equality constraints by MuJoCo solver).
Raises:
ValueError: If any validation check fails.
"""
if self.joint_count > 0:
# First, find all bodies reachable via articulated joints
articulated_bodies = set()
articulated_bodies.add(-1) # World is always reachable
for i, art in enumerate(self.joint_articulation):
if art >= 0: # Joint is in an articulation
parent = self.joint_parent[i]
child = self.joint_child[i]
articulated_bodies.add(parent)
articulated_bodies.add(child)
# Now check for true orphan joints: non-articulated joints whose child
# is NOT reachable via other articulated joints
orphan_joints = []
for i, art in enumerate(self.joint_articulation):
if art < 0: # Joint is not in an articulation
child = self.joint_child[i]
if child not in articulated_bodies:
# This is a true orphan - the child body has no articulated path
orphan_joints.append(i)
# else: this is a loop joint - child is already reachable, so it's allowed
if orphan_joints:
joint_labels = [self.joint_label[i] for i in orphan_joints[:5]] # Show first 5
raise ValueError(
f"Found {len(orphan_joints)} joint(s) not belonging to any articulation. "
f"Call add_articulation() for all joints. Orphan joints: {joint_labels}"
+ ("..." if len(orphan_joints) > 5 else "")
)
def _validate_shapes(self) -> bool:
"""Validate shape gaps for stable broad phase detection.
Margin is an outward offset from a shape's surface [m], while broad phase uses
``margin + gap`` [m] for expansion/filtering. For reliable detection, ``gap`` [m]
should be non-negative so effective expansion is not reduced below the shape
margin.
This check only considers shapes that participate in collisions (with the
`COLLIDE_SHAPES` or `COLLIDE_PARTICLES` flag).
Warns:
UserWarning: If any colliding shape has ``gap < 0``.
Returns:
Whether all colliding shapes have non-negative gaps.
"""
collision_flags_mask = ShapeFlags.COLLIDE_SHAPES | ShapeFlags.COLLIDE_PARTICLES
shapes_with_bad_gap = []
for i in range(self.shape_count):
# Skip shapes that don't participate in any collisions (e.g., sites, visual-only)
if not (self.shape_flags[i] & collision_flags_mask):
continue
margin = self.shape_margin[i]
gap = self.shape_gap[i]
if gap < 0.0:
shapes_with_bad_gap.append(
f"{self.shape_label[i] or f'shape_{i}'} (margin={margin:.6g}, gap={gap:.6g})"
)
if shapes_with_bad_gap:
example_shapes = shapes_with_bad_gap[:5]
warnings.warn(
f"Found {len(shapes_with_bad_gap)} shape(s) with gap < 0. "
f"This can cause missed collisions in broad phase because effective expansion uses margin + gap. "
f"Set gap >= 0 for each shape. "
f"Affected shapes: {example_shapes}" + ("..." if len(shapes_with_bad_gap) > 5 else ""),
stacklevel=2,
)
return len(shapes_with_bad_gap) == 0
def _validate_structure(self) -> None:
"""Validate structural invariants of the model.
This method performs consolidated validation of all structural constraints,
using vectorized numpy operations for efficiency:
- Body references: shape_body, joint_parent, joint_child, equality_constraint_body1/2
- Joint references: equality_constraint_joint1/2
- Self-referential joints: joint_parent[i] != joint_child[i]
- Start array monotonicity: joint_q_start, joint_qd_start, articulation_start
- Array length consistency: per-DOF and per-coord arrays
Raises:
ValueError: If any structural validation check fails.
"""
body_count = self.body_count
joint_count = self.joint_count
# Validate per-body flags: each body must be either dynamic or
# kinematic. Filter masks such as BodyFlags.ALL are not valid stored
# body states.
if len(self.body_flags) != body_count:
raise ValueError(f"Invalid body_flags length: expected {body_count} entries, got {len(self.body_flags)}.")
if body_count > 0:
body_flags = np.array(self.body_flags, dtype=np.int32)
valid_mask = (body_flags == int(BodyFlags.DYNAMIC)) | (body_flags == int(BodyFlags.KINEMATIC))
if not np.all(valid_mask):
idx = int(np.where(~valid_mask)[0][0])
body_label = self.body_label[idx] if idx < len(self.body_label) else f"body_{idx}"
raise ValueError(
f"Invalid body flag for body {idx} ('{body_label}'): got {int(body_flags[idx])}, "
f"but expected exactly one of BodyFlags.DYNAMIC or BodyFlags.KINEMATIC."
)
# Validate shape_body references: must be in [-1, body_count-1]
if self.shape_count > 0:
shape_body = np.array(self.shape_body, dtype=np.int32)
invalid_mask = (shape_body < -1) | (shape_body >= body_count)
if np.any(invalid_mask):
invalid_indices = np.where(invalid_mask)[0]
idx = invalid_indices[0]
shape_label = self.shape_label[idx] or f"shape_{idx}"
raise ValueError(
f"Invalid body reference in shape_body: shape {idx} ('{shape_label}') references body {shape_body[idx]}, "
f"but valid range is [-1, {body_count - 1}] (body_count={body_count})."
)
# Validate joint_parent references: must be in [-1, body_count-1]
if joint_count > 0:
joint_parent = np.array(self.joint_parent, dtype=np.int32)
invalid_mask = (joint_parent < -1) | (joint_parent >= body_count)
if np.any(invalid_mask):
invalid_indices = np.where(invalid_mask)[0]
idx = invalid_indices[0]
joint_label = self.joint_label[idx] or f"joint_{idx}"
raise ValueError(
f"Invalid body reference in joint_parent: joint {idx} ('{joint_label}') references parent body {joint_parent[idx]}, "
f"but valid range is [-1, {body_count - 1}] (body_count={body_count})."
)
# Validate joint_child references: must be in [0, body_count-1] (child cannot be world)
joint_child = np.array(self.joint_child, dtype=np.int32)
invalid_mask = (joint_child < 0) | (joint_child >= body_count)
if np.any(invalid_mask):
invalid_indices = np.where(invalid_mask)[0]
idx = invalid_indices[0]
joint_label = self.joint_label[idx] or f"joint_{idx}"
raise ValueError(
f"Invalid body reference in joint_child: joint {idx} ('{joint_label}') references child body {joint_child[idx]}, "
f"but valid range is [0, {body_count - 1}] (body_count={body_count}). Child cannot be the world (-1)."
)
# Validate self-referential joints: parent != child
self_ref_mask = joint_parent == joint_child
if np.any(self_ref_mask):
invalid_indices = np.where(self_ref_mask)[0]
idx = invalid_indices[0]
joint_label = self.joint_label[idx] or f"joint_{idx}"
raise ValueError(
f"Self-referential joint: joint {idx} ('{joint_label}') has parent and child both set to body {joint_parent[idx]}."
)
# Validate equality constraint body references
equality_count = len(self.equality_constraint_type)
if equality_count > 0:
eq_body1 = np.array(self.equality_constraint_body1, dtype=np.int32)
invalid_mask = (eq_body1 < -1) | (eq_body1 >= body_count)
if np.any(invalid_mask):
invalid_indices = np.where(invalid_mask)[0]
idx = invalid_indices[0]
eq_key = self.equality_constraint_label[idx] or f"equality_constraint_{idx}"
raise ValueError(
f"Invalid body reference in equality_constraint_body1: constraint {idx} ('{eq_key}') references body {eq_body1[idx]}, "
f"but valid range is [-1, {body_count - 1}] (body_count={body_count})."
)
eq_body2 = np.array(self.equality_constraint_body2, dtype=np.int32)
invalid_mask = (eq_body2 < -1) | (eq_body2 >= body_count)
if np.any(invalid_mask):
invalid_indices = np.where(invalid_mask)[0]
idx = invalid_indices[0]
eq_key = self.equality_constraint_label[idx] or f"equality_constraint_{idx}"
raise ValueError(
f"Invalid body reference in equality_constraint_body2: constraint {idx} ('{eq_key}') references body {eq_body2[idx]}, "
f"but valid range is [-1, {body_count - 1}] (body_count={body_count})."
)
# Validate equality constraint joint references
eq_joint1 = np.array(self.equality_constraint_joint1, dtype=np.int32)
invalid_mask = (eq_joint1 < -1) | (eq_joint1 >= joint_count)
if np.any(invalid_mask):
invalid_indices = np.where(invalid_mask)[0]
idx = invalid_indices[0]
eq_key = self.equality_constraint_label[idx] or f"equality_constraint_{idx}"
raise ValueError(
f"Invalid joint reference in equality_constraint_joint1: constraint {idx} ('{eq_key}') references joint {eq_joint1[idx]}, "
f"but valid range is [-1, {joint_count - 1}] (joint_count={joint_count})."
)
eq_joint2 = np.array(self.equality_constraint_joint2, dtype=np.int32)
invalid_mask = (eq_joint2 < -1) | (eq_joint2 >= joint_count)
if np.any(invalid_mask):
invalid_indices = np.where(invalid_mask)[0]
idx = invalid_indices[0]
eq_key = self.equality_constraint_label[idx] or f"equality_constraint_{idx}"
raise ValueError(
f"Invalid joint reference in equality_constraint_joint2: constraint {idx} ('{eq_key}') references joint {eq_joint2[idx]}, "
f"but valid range is [-1, {joint_count - 1}] (joint_count={joint_count})."
)
# Validate start array monotonicity
if joint_count > 0:
joint_q_start = np.array(self.joint_q_start, dtype=np.int32)
if len(joint_q_start) > 1:
diffs = np.diff(joint_q_start)
if np.any(diffs < 0):
idx = np.where(diffs < 0)[0][0]
raise ValueError(
f"joint_q_start is not monotonically increasing: "
f"joint_q_start[{idx}]={joint_q_start[idx]} > joint_q_start[{idx + 1}]={joint_q_start[idx + 1]}."
)
joint_qd_start = np.array(self.joint_qd_start, dtype=np.int32)
if len(joint_qd_start) > 1:
diffs = np.diff(joint_qd_start)
if np.any(diffs < 0):
idx = np.where(diffs < 0)[0][0]
raise ValueError(
f"joint_qd_start is not monotonically increasing: "
f"joint_qd_start[{idx}]={joint_qd_start[idx]} > joint_qd_start[{idx + 1}]={joint_qd_start[idx + 1]}."
)
articulation_count = self.articulation_count
if articulation_count > 0:
articulation_start = np.array(self.articulation_start, dtype=np.int32)
if len(articulation_start) > 1:
diffs = np.diff(articulation_start)
if np.any(diffs < 0):
idx = np.where(diffs < 0)[0][0]
raise ValueError(
f"articulation_start is not monotonically increasing: "
f"articulation_start[{idx}]={articulation_start[idx]} > articulation_start[{idx + 1}]={articulation_start[idx + 1]}."
)
# Validate array length consistency
if joint_count > 0:
# Per-DOF arrays should have length == joint_dof_count
dof_arrays = [
("joint_axis", self.joint_axis),
("joint_armature", self.joint_armature),
("joint_target_ke", self.joint_target_ke),
("joint_target_kd", self.joint_target_kd),
("joint_limit_lower", self.joint_limit_lower),
("joint_limit_upper", self.joint_limit_upper),
("joint_limit_ke", self.joint_limit_ke),
("joint_limit_kd", self.joint_limit_kd),
("joint_target_pos", self.joint_target_pos),
("joint_target_vel", self.joint_target_vel),
("joint_effort_limit", self.joint_effort_limit),
("joint_velocity_limit", self.joint_velocity_limit),
("joint_friction", self.joint_friction),
("joint_target_mode", self.joint_target_mode),
]
for name, arr in dof_arrays:
if len(arr) != self.joint_dof_count:
raise ValueError(
f"Array length mismatch: {name} has length {len(arr)}, "
f"but expected {self.joint_dof_count} (joint_dof_count)."
)
# Per-coord arrays should have length == joint_coord_count
coord_arrays = [
("joint_q", self.joint_q),
]
for name, arr in coord_arrays:
if len(arr) != self.joint_coord_count:
raise ValueError(
f"Array length mismatch: {name} has length {len(arr)}, "
f"but expected {self.joint_coord_count} (joint_coord_count)."
)
# Start arrays should have length == joint_count
start_arrays = [
("joint_q_start", self.joint_q_start),
("joint_qd_start", self.joint_qd_start),
]
for name, arr in start_arrays:
if len(arr) != joint_count:
raise ValueError(
f"Array length mismatch: {name} has length {len(arr)}, "
f"but expected {joint_count} (joint_count)."
)
def validate_joint_ordering(self) -> bool:
"""Validate that joints within articulations follow DFS topological ordering.
This check ensures that joints are ordered such that parent bodies are processed
before child bodies within each articulation. This ordering is required by some
solvers (e.g., MuJoCo) for correct kinematic computations.
This method is public and opt-in because the check has O(n log n) complexity
due to topological sorting. It is skipped by default in
:meth:`finalize `.
Warns:
UserWarning: If joints are not in DFS topological order.
Returns:
Whether joints are correctly ordered.
"""
from ..utils import topological_sort # noqa: PLC0415
if self.joint_count == 0:
return True
joint_parent = np.array(self.joint_parent, dtype=np.int32)
joint_child = np.array(self.joint_child, dtype=np.int32)
joint_articulation = np.array(self.joint_articulation, dtype=np.int32)
# Get unique articulations (excluding -1 which means not in any articulation)
articulation_ids = np.unique(joint_articulation)
articulation_ids = articulation_ids[articulation_ids >= 0]
all_ordered = True
for art_id in articulation_ids:
# Get joints in this articulation
art_joints = np.where(joint_articulation == art_id)[0]
if len(art_joints) <= 1:
continue
# Build joint list for topological sort
joints_simple = [(int(joint_parent[i]), int(joint_child[i])) for i in art_joints]
try:
joint_order = topological_sort(joints_simple, use_dfs=True, custom_indices=list(art_joints))
# Check if current order matches expected DFS order
if any(joint_order[i] != art_joints[i] for i in range(len(joints_simple))):
art_key = (
self.articulation_label[art_id]
if art_id < len(self.articulation_label)
else f"articulation_{art_id}"
)
warnings.warn(
f"Joints in articulation '{art_key}' (id={art_id}) are not in DFS topological order. "
f"This may cause issues with some solvers (e.g., MuJoCo). "
f"Current order: {list(art_joints)}, expected: {joint_order}.",
stacklevel=2,
)
all_ordered = False
except ValueError as e:
# Topological sort failed (e.g., cycle detected)
art_key = (
self.articulation_label[art_id]
if art_id < len(self.articulation_label)
else f"articulation_{art_id}"
)
warnings.warn(
f"Failed to validate joint ordering for articulation '{art_key}' (id={art_id}): {e}",
stacklevel=2,
)
all_ordered = False
return all_ordered
def _build_world_starts(self):
"""
Constructs the per-world entity start indices.
This method validates that the per-world start index lists for various entities
(particles, bodies, shapes, joints, articulations, equality constraints and joint
coordinates/DOFs/constraints) are cumulative and match the total counts of those
entities. Moreover, it appends the start of tail-end global entities and the
overall total counts to the end of each start index lists.
The format of the start index lists is as follows (where `*` can be `body`, `shape`, `joint`, etc.):
.. code-block:: python
world_*_start = [ start_world_0, start_world_1, ..., start_world_N , start_global_tail, total_count]
This allows retrieval of per-world counts using:
.. code-block:: python
global_*_count = start_world_0 + (total_count - start_global_tail)
world_*_count[w] = world_*_start[w + 1] - world_*_start[w]
e.g.
.. code-block:: python
body_world = [-1, -1, 0, 0, ..., 1, 1, ..., N - 1, N - 1, ..., -1, -1, -1, ...]
body_world_start = [2, 15, 25, ..., 50, 60, 72]
# world : -1 | 0 | 1 ... | N-1 | -1 | total
"""
# List of all world starts of entities
world_entity_start_arrays = [
(self.particle_world_start, self.particle_count, self.particle_world, "particle"),
(self.body_world_start, self.body_count, self.body_world, "body"),
(self.shape_world_start, self.shape_count, self.shape_world, "shape"),
(self.joint_world_start, self.joint_count, self.joint_world, "joint"),
(self.articulation_world_start, self.articulation_count, self.articulation_world, "articulation"),
(
self.equality_constraint_world_start,
len(self.equality_constraint_type),
self.equality_constraint_world,
"equality constraint",
),
]
def build_entity_start_array(
entity_count: int, entity_world: list[int], world_entity_start: list[int], name: str
):
# Ensure that entity_world has length equal to entity_count
if len(entity_world) != entity_count:
raise ValueError(
f"World array for {name}s has incorrect length: expected {entity_count}, found {len(entity_world)}."
)
# Initialize world_entity_start with zeros
world_entity_start.clear()
world_entity_start.extend([0] * (self.world_count + 2))
# Count global entities at the front of the entity_world array
front_global_entity_count = 0
for w in entity_world:
if w == -1:
front_global_entity_count += 1
else:
break
world_entity_start[0] = front_global_entity_count
# Compute per-world cumulative counts
entity_world_np = np.asarray(entity_world, dtype=np.int32)
world_counts = np.bincount(entity_world_np[entity_world_np >= 0], minlength=self.world_count)
for w in range(self.world_count):
world_entity_start[w + 1] = world_entity_start[w] + int(world_counts[w])
# Set the last element to the total entity counts over all worlds in the model
world_entity_start[-1] = entity_count
# Check that all world offset indices are cumulative and match counts
for world_start_array, total_count, entity_world_array, name in world_entity_start_arrays:
# First build the start lists by appending tail-end global and total entity counts
build_entity_start_array(total_count, entity_world_array, world_start_array, name)
# Ensure the world_start array has length world_count + 2 (for global entities at start/end)
expected_length = self.world_count + 2
if len(world_start_array) != expected_length:
raise ValueError(
f"World start indices for {name}s have incorrect length: "
f"expected {expected_length}, found {len(world_start_array)}."
)
# Ensure that per-world start indices are non-decreasing and compute sum of per-world counts
sum_of_counts = world_start_array[0]
for w in range(self.world_count + 1):
start_idx = world_start_array[w]
end_idx = world_start_array[w + 1]
count = end_idx - start_idx
if count < 0:
raise ValueError(
f"Invalid world start indices for {name}s: world {w} has negative count ({count}). "
f"Start index: {start_idx}, end index: {end_idx}."
)
sum_of_counts += count
# Ensure the sum of per-world counts equals the total count
if sum_of_counts != total_count:
raise ValueError(
f"Sum of per-world {name} counts does not equal total count: "
f"expected {total_count}, found {sum_of_counts}."
)
# Ensure that the last entry equals the total count
if world_start_array[-1] != total_count:
raise ValueError(
f"World start indices for {name}s do not match total count: "
f"expected final index {total_count}, found {world_start_array[-1]}."
)
# List of world starts of joints spaces, i.e. coords/DOFs/constraints
world_joint_space_start_arrays = [
(self.joint_dof_world_start, self.joint_qd_start, self.joint_dof_count, "joint DOF"),
(self.joint_coord_world_start, self.joint_q_start, self.joint_coord_count, "joint coordinate"),
(self.joint_constraint_world_start, self.joint_cts_start, self.joint_constraint_count, "joint constraint"),
]
def build_joint_space_start_array(
space_count: int, joint_space_start: list[int], world_space_start: list[int], name: str
):
# Ensure that joint_space_start has length equal to self.joint_count
if len(joint_space_start) != self.joint_count:
raise ValueError(
f"Joint start array for {name}s has incorrect length: "
f"expected {self.joint_count}, found {len(joint_space_start)}."
)
# Initialize world_space_start with zeros
world_space_start.clear()
world_space_start.extend([0] * (self.world_count + 2))
# Extend joint_space_start with total count to enable computing per-world counts
joint_space_start_ext = copy.copy(joint_space_start)
joint_space_start_ext.append(space_count)
# Count global entities at the front of the entity_world array
front_global_space_count = 0
for j, w in enumerate(self.joint_world):
if w == -1:
front_global_space_count += joint_space_start_ext[j + 1] - joint_space_start_ext[j]
else:
break
# Compute per-world cumulative joint space counts to initialize world_space_start
for j, w in enumerate(self.joint_world):
if w >= 0:
world_space_start[w + 1] += joint_space_start_ext[j + 1] - joint_space_start_ext[j]
# Convert per-world counts to cumulative start indices
world_space_start[0] += front_global_space_count
for w in range(self.world_count):
world_space_start[w + 1] += world_space_start[w]
# Add total (i.e. final) entity counts to the per-world start indices
world_space_start[-1] = space_count
# Check that all world offset indices are cumulative and match counts
for world_start_array, space_start_array, total_count, name in world_joint_space_start_arrays:
# First finalize the start array by appending tail-end global and total entity counts
build_joint_space_start_array(total_count, space_start_array, world_start_array, name)
# Ensure the world_start array has length world_count + 2 (for global entities at start/end)
expected_length = self.world_count + 2
if len(world_start_array) != expected_length:
raise ValueError(
f"World start indices for {name}s have incorrect length: "
f"expected {expected_length}, found {len(world_start_array)}."
)
# Ensure that per-world start indices are non-decreasing and compute sum of per-world counts
sum_of_counts = world_start_array[0]
for w in range(self.world_count + 1):
start_idx = world_start_array[w]
end_idx = world_start_array[w + 1]
count = end_idx - start_idx
if count < 0:
raise ValueError(
f"Invalid world start indices for {name}s: world {w} has negative count ({count}). "
f"Start index: {start_idx}, end index: {end_idx}."
)
sum_of_counts += count
# Ensure the sum of per-world counts equals the total count
if sum_of_counts != total_count:
raise ValueError(
f"Sum of per-world {name} counts does not equal total count: "
f"expected {total_count}, found {sum_of_counts}."
)
# Ensure that the last entry equals the total count
if world_start_array[-1] != total_count:
raise ValueError(
f"World start indices for {name}s do not match total count: "
f"expected final index {total_count}, found {world_start_array[-1]}."
)
def finalize(
self,
device: Devicelike | None = None,
*,
requires_grad: bool = False,
skip_all_validations: bool = False,
skip_validation_worlds: bool = False,
skip_validation_joints: bool = False,
skip_validation_shapes: bool = False,
skip_validation_structure: bool = False,
skip_validation_joint_ordering: bool = True,
) -> Model:
"""
Finalize the builder and create a concrete :class:`~newton.Model` for simulation.
This method transfers all simulation data from the builder to device memory,
returning a Model object ready for simulation. It should be called after all
elements (particles, bodies, shapes, joints, etc.) have been added to the builder.
Args:
device: The simulation device to use (e.g., 'cpu', 'cuda'). If None, uses the current Warp device.
requires_grad: If True, enables gradient computation for the model (for differentiable simulation).
skip_all_validations: If True, skips all validation checks. Use for maximum performance when
you are confident the model is valid. Default is False.
skip_validation_worlds: If True, skips validation of world ordering and contiguity. Default is False.
skip_validation_joints: If True, skips validation of joints belonging to an articulation. Default is False.
skip_validation_shapes: If True, skips validation of shapes having valid contact margins. Default is False.
skip_validation_structure: If True, skips validation of structural invariants (body/joint references,
array lengths, monotonicity). Default is False.
skip_validation_joint_ordering: If True, skips validation of DFS topological joint ordering within
articulations. Default is True (opt-in) because this check has O(n log n) complexity.
Returns:
A fully constructed Model object containing all simulation data on the specified device.
Notes:
- Performs validation and correction of rigid body inertia and mass properties.
- Closes all start-index arrays (e.g., for muscles, joints, articulations) with sentinel values.
- Sets up all arrays and properties required for simulation, including particles, bodies, shapes,
joints, springs, muscles, constraints, and collision/contact data.
"""
# ensure the world count is set correctly
self.world_count = max(1, self.world_count)
# validate world ordering and contiguity
if not skip_all_validations and not skip_validation_worlds:
self._validate_world_ordering()
# validate joints belong to an articulation
if not skip_all_validations and not skip_validation_joints:
self._validate_joints()
# validate shapes have valid contact margins
if not skip_all_validations and not skip_validation_shapes:
self._validate_shapes()
# validate structural invariants (body/joint references, array lengths)
if not skip_all_validations and not skip_validation_structure:
self._validate_structure()
# validate DFS topological joint ordering (opt-in, skipped by default)
if not skip_all_validations and not skip_validation_joint_ordering:
self.validate_joint_ordering()
# construct world starts by ensuring they are cumulative and appending
# tail-end global counts and sum total counts over the entire model.
# This method also performs relevant validation checks on the start.
self._build_world_starts()
# construct particle inv masses
ms = np.array(self.particle_mass, dtype=np.float32)
# static particles (with zero mass) have zero inverse mass
particle_inv_mass = np.divide(1.0, ms, out=np.zeros_like(ms), where=ms != 0.0)
with wp.ScopedDevice(device):
# -------------------------------------
# construct Model (non-time varying) data
m = Model(device)
m.request_contact_attributes(*self._requested_contact_attributes)
m.request_state_attributes(*self._requested_state_attributes)
m.requires_grad = requires_grad
m.world_count = self.world_count
# ---------------------
# particles
# state (initial)
m.particle_q = wp.array(self.particle_q, dtype=wp.vec3, requires_grad=requires_grad)
m.particle_qd = wp.array(self.particle_qd, dtype=wp.vec3, requires_grad=requires_grad)
m.particle_mass = wp.array(self.particle_mass, dtype=wp.float32, requires_grad=requires_grad)
m.particle_inv_mass = wp.array(particle_inv_mass, dtype=wp.float32, requires_grad=requires_grad)
m.particle_radius = wp.array(self.particle_radius, dtype=wp.float32, requires_grad=requires_grad)
m.particle_flags = wp.array([flag_to_int(f) for f in self.particle_flags], dtype=wp.int32)
m.particle_world = wp.array(self.particle_world, dtype=wp.int32)
m.particle_max_radius = np.max(self.particle_radius) if len(self.particle_radius) > 0 else 0.0
m.particle_max_velocity = self.particle_max_velocity
particle_colors = np.empty(self.particle_count, dtype=int)
for color in range(len(self.particle_color_groups)):
particle_colors[self.particle_color_groups[color]] = color
m.particle_colors = wp.array(particle_colors, dtype=int)
m.particle_color_groups = [wp.array(group, dtype=int) for group in self.particle_color_groups]
# hash-grid for particle interactions
if self.particle_count > 1 and m.particle_max_radius > 0.0:
m.particle_grid = wp.HashGrid(128, 128, 128)
else:
m.particle_grid = None
# ---------------------
# collision geometry
m.shape_label = self.shape_label
m.shape_transform = wp.array(self.shape_transform, dtype=wp.transform, requires_grad=requires_grad)
m.shape_body = wp.array(self.shape_body, dtype=wp.int32)
m.shape_flags = wp.array(self.shape_flags, dtype=wp.int32)
m.body_shapes = self.body_shapes
# build list of ids for geometry sources (meshes, sdfs)
geo_sources = []
finalized_geos = {} # do not duplicate geometry
gaussians = []
for geo in self.shape_source:
geo_hash = hash(geo) # avoid repeated hash computations
if geo and not isinstance(geo, Heightfield):
if geo_hash not in finalized_geos:
if isinstance(geo, Mesh):
finalized_geos[geo_hash] = geo.finalize(device=device)
elif isinstance(geo, Gaussian):
finalized_geos[geo_hash] = len(gaussians)
gaussians.append(geo.finalize(device=device))
else:
finalized_geos[geo_hash] = geo.finalize()
geo_sources.append(finalized_geos[geo_hash])
else:
# add null pointer
geo_sources.append(0)
m.shape_type = wp.array(self.shape_type, dtype=wp.int32)
m.shape_source_ptr = wp.array(geo_sources, dtype=wp.uint64)
m.gaussians_count = len(gaussians)
m.gaussians_data = wp.array(gaussians, dtype=Gaussian.Data)
m.shape_scale = wp.array(self.shape_scale, dtype=wp.vec3, requires_grad=requires_grad)
m.shape_is_solid = wp.array(self.shape_is_solid, dtype=wp.bool)
m.shape_margin = wp.array(self.shape_margin, dtype=wp.float32, requires_grad=requires_grad)
m.shape_collision_radius = wp.array(
self.shape_collision_radius, dtype=wp.float32, requires_grad=requires_grad
)
m.shape_world = wp.array(self.shape_world, dtype=wp.int32)
m.shape_source = self.shape_source # used for rendering
m.shape_color = wp.array(self.shape_color, dtype=wp.vec3)
m.shape_material_ke = wp.array(self.shape_material_ke, dtype=wp.float32, requires_grad=requires_grad)
m.shape_material_kd = wp.array(self.shape_material_kd, dtype=wp.float32, requires_grad=requires_grad)
m.shape_material_kf = wp.array(self.shape_material_kf, dtype=wp.float32, requires_grad=requires_grad)
m.shape_material_ka = wp.array(self.shape_material_ka, dtype=wp.float32, requires_grad=requires_grad)
m.shape_material_mu = wp.array(self.shape_material_mu, dtype=wp.float32, requires_grad=requires_grad)
m.shape_material_restitution = wp.array(
self.shape_material_restitution, dtype=wp.float32, requires_grad=requires_grad
)
m.shape_material_mu_torsional = wp.array(
self.shape_material_mu_torsional, dtype=wp.float32, requires_grad=requires_grad
)
m.shape_material_mu_rolling = wp.array(
self.shape_material_mu_rolling, dtype=wp.float32, requires_grad=requires_grad
)
m.shape_material_kh = wp.array(self.shape_material_kh, dtype=wp.float32, requires_grad=requires_grad)
m.shape_gap = wp.array(self.shape_gap, dtype=wp.float32, requires_grad=requires_grad)
m.shape_collision_filter_pairs = {
(min(s1, s2), max(s1, s2)) for s1, s2 in self.shape_collision_filter_pairs
}
m.shape_collision_group = wp.array(self.shape_collision_group, dtype=wp.int32)
# ---------------------
# Compute local AABBs and voxel resolutions for contact reduction
local_aabb_lower = []
local_aabb_upper = []
voxel_resolution = []
from ..geometry.contact_reduction import NUM_VOXEL_DEPTH_SLOTS # noqa: PLC0415
voxel_budget = NUM_VOXEL_DEPTH_SLOTS
# Cache per unique (shape_type, shape_params, margin) to avoid redundant AABB computation
# for instanced shapes (e.g., 256 robots sharing the same shape parameters)
shape_aabb_cache = {}
def compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget):
"""Compute voxel resolution from AABB with given budget."""
size = aabb_upper - aabb_lower
size = np.maximum(size, 1e-6) # Avoid division by zero
# Target voxel size for approximately cubic voxels
volume = size[0] * size[1] * size[2]
v = (volume / voxel_budget) ** (1.0 / 3.0)
v = max(v, 1e-6)
# Initial resolution
nx = max(1, round(size[0] / v))
ny = max(1, round(size[1] / v))
nz = max(1, round(size[2] / v))
# Reduce until under budget (reduce largest axis first for more cubic voxels)
while nx * ny * nz > voxel_budget:
if nx >= ny and nx >= nz and nx > 1:
nx -= 1
elif ny >= nz and ny > 1:
ny -= 1
elif nz > 1:
nz -= 1
else:
break
return nx, ny, nz
for _shape_idx, (shape_type, shape_src, shape_scale) in enumerate(
zip(self.shape_type, self.shape_source, self.shape_scale, strict=True)
):
# Create cache key based on shape type and parameters
if (shape_type == GeoType.MESH or shape_type == GeoType.CONVEX_MESH) and shape_src is not None:
cache_key = (shape_type, id(shape_src), tuple(shape_scale))
else:
cache_key = (shape_type, tuple(shape_scale))
# Check cache first
if cache_key in shape_aabb_cache:
aabb_lower, aabb_upper, nx, ny, nz = shape_aabb_cache[cache_key]
else:
# Compute AABB based on shape type
if shape_type == GeoType.MESH and shape_src is not None:
# Compute local AABB from mesh vertices
vertices = shape_src.vertices
lo = vertices.min(axis=0) * np.array(shape_scale)
hi = vertices.max(axis=0) * np.array(shape_scale)
aabb_lower = np.minimum(lo, hi)
aabb_upper = np.maximum(lo, hi)
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
elif shape_type == GeoType.CONVEX_MESH and shape_src is not None:
lo = shape_src.vertices.min(axis=0) * np.array(shape_scale)
hi = shape_src.vertices.max(axis=0) * np.array(shape_scale)
aabb_lower = np.minimum(lo, hi)
aabb_upper = np.maximum(lo, hi)
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
elif shape_type == GeoType.ELLIPSOID:
# Ellipsoid: shape_scale = (semi_axis_x, semi_axis_y, semi_axis_z)
sx, sy, sz = shape_scale
aabb_lower = np.array([-sx, -sy, -sz])
aabb_upper = np.array([sx, sy, sz])
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
elif shape_type == GeoType.BOX:
# Box: shape_scale = (hx, hy, hz) half-extents
hx, hy, hz = shape_scale
aabb_lower = np.array([-hx, -hy, -hz])
aabb_upper = np.array([hx, hy, hz])
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
elif shape_type == GeoType.SPHERE:
# Sphere: shape_scale = (radius, radius, radius)
r = shape_scale[0]
aabb_lower = np.array([-r, -r, -r])
aabb_upper = np.array([r, r, r])
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
elif shape_type == GeoType.CAPSULE:
# Capsule: shape_scale = (radius, half_height, radius)
# Capsule is along Z axis with hemispherical caps (matches SDF in kernels.py)
r, half_height, _ = shape_scale
aabb_lower = np.array([-r, -r, -half_height - r])
aabb_upper = np.array([r, r, half_height + r])
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
elif shape_type == GeoType.CYLINDER:
# Cylinder: shape_scale = (radius, half_height, radius)
# Cylinder is along Z axis (matches SDF in kernels.py)
r, half_height, _ = shape_scale
aabb_lower = np.array([-r, -r, -half_height])
aabb_upper = np.array([r, r, half_height])
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
elif shape_type == GeoType.CONE:
# Cone: shape_scale = (radius, half_height, radius)
# Cone is along Z axis (matches SDF in kernels.py)
r, half_height, _ = shape_scale
aabb_lower = np.array([-r, -r, -half_height])
aabb_upper = np.array([r, r, half_height])
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
elif shape_type == GeoType.HFIELD and shape_src is not None:
hx = abs(shape_src.hx * shape_scale[0])
hy = abs(shape_src.hy * shape_scale[1])
z_lo = shape_src.min_z * shape_scale[2]
z_hi = shape_src.max_z * shape_scale[2]
aabb_lower = np.array([-hx, -hy, min(z_lo, z_hi)])
aabb_upper = np.array([hx, hy, max(z_lo, z_hi)])
nx, ny, nz = compute_voxel_resolution_from_aabb(aabb_lower, aabb_upper, voxel_budget)
else:
# Other shapes (PLANE, etc.): use default unit cube with 1x1x1 voxel grid
aabb_lower = np.array([-1.0, -1.0, -1.0])
aabb_upper = np.array([1.0, 1.0, 1.0])
nx, ny, nz = 1, 1, 1
# Cache the result for reuse by identical shapes
shape_aabb_cache[cache_key] = (aabb_lower, aabb_upper, nx, ny, nz)
local_aabb_lower.append(aabb_lower)
local_aabb_upper.append(aabb_upper)
voxel_resolution.append([nx, ny, nz])
m.shape_collision_aabb_lower = wp.array(local_aabb_lower, dtype=wp.vec3, device=device)
m.shape_collision_aabb_upper = wp.array(local_aabb_upper, dtype=wp.vec3, device=device)
m._shape_voxel_resolution = wp.array(voxel_resolution, dtype=wp.vec3i, device=device)
# ---------------------
# Compute and compact texture SDF resources (shared table + per-shape index indirection)
from ..geometry.types import Mesh as NewtonMesh # noqa: PLC0415
def _create_primitive_mesh(stype: int, scale: Sequence[float] | None) -> NewtonMesh | None:
"""Create a watertight mesh from a primitive shape for texture SDF construction."""
from ..core.types import Axis # noqa: PLC0415
sx, sy, sz = scale if scale is not None else (1.0, 1.0, 1.0)
common_kw = {"compute_normals": False, "compute_uvs": False, "compute_inertia": False}
if stype == GeoType.BOX:
return NewtonMesh.create_box(sx, sy, sz, duplicate_vertices=False, **common_kw)
elif stype == GeoType.SPHERE:
return NewtonMesh.create_sphere(sx, **common_kw)
elif stype == GeoType.CAPSULE:
return NewtonMesh.create_capsule(sx, sy, up_axis=Axis.Z, **common_kw)
elif stype == GeoType.CYLINDER:
return NewtonMesh.create_cylinder(sx, sy, up_axis=Axis.Z, **common_kw)
elif stype == GeoType.CONE:
return NewtonMesh.create_cone(sx, sy, up_axis=Axis.Z, **common_kw)
elif stype == GeoType.ELLIPSOID:
return NewtonMesh.create_ellipsoid(sx, sy, sz, **common_kw)
return None
current_device = wp.get_device(device)
is_gpu = current_device.is_cuda
has_mesh_sdf = any(
stype == GeoType.MESH
and ssrc is not None
and sflags & ShapeFlags.COLLIDE_SHAPES
and getattr(ssrc, "sdf", None) is not None
for stype, ssrc, sflags in zip(self.shape_type, self.shape_source, self.shape_flags, strict=True)
)
has_hydroelastic_shapes = any(
(sflags & ShapeFlags.HYDROELASTIC) and (sflags & ShapeFlags.COLLIDE_SHAPES)
for sflags in self.shape_flags
)
if (has_mesh_sdf or has_hydroelastic_shapes) and not is_gpu:
raise ValueError(
"SDF collision paths require a CUDA-capable GPU device. "
"Texture SDFs (used for SDF collision) only support CUDA."
)
sdf_block_coords = []
sdf_index2blocks = []
from ..geometry.sdf_texture import ( # noqa: PLC0415
QuantizationMode,
TextureSDFData,
create_empty_texture_sdf_data,
create_texture_sdf_from_mesh,
)
_tex_fmt_map = {
"float32": QuantizationMode.FLOAT32,
"uint16": QuantizationMode.UINT16,
"uint8": QuantizationMode.UINT8,
}
compact_texture_sdf_data = []
compact_texture_sdf_coarse_textures = []
compact_texture_sdf_subgrid_textures = []
compact_texture_sdf_subgrid_start_slots = []
shape_sdf_index = [-1] * len(self.shape_type)
sdf_cache = {}
for i in range(len(self.shape_type)):
shape_type = self.shape_type[i]
shape_src = self.shape_source[i]
shape_flags = self.shape_flags[i]
shape_scale = self.shape_scale[i]
shape_gap = self.shape_gap[i]
sdf_narrow_band_range = self.shape_sdf_narrow_band_range[i]
sdf_target_voxel_size = self.shape_sdf_target_voxel_size[i]
sdf_max_resolution = self.shape_sdf_max_resolution[i]
sdf_tex_fmt = self.shape_sdf_texture_format[i]
is_hydroelastic = bool(shape_flags & ShapeFlags.HYDROELASTIC)
has_shape_collision = bool(shape_flags & ShapeFlags.COLLIDE_SHAPES)
block_coords = []
cache_key = None
mesh_sdf = None
if shape_type == GeoType.MESH and has_shape_collision and shape_src is not None:
mesh_sdf = getattr(shape_src, "sdf", None)
if mesh_sdf is not None:
cache_key = ("mesh_sdf", id(mesh_sdf))
if mesh_sdf.texture_block_coords is not None:
block_coords = list(mesh_sdf.texture_block_coords)
elif mesh_sdf.block_coords is not None:
block_coords = list(mesh_sdf.block_coords)
else:
block_coords = []
elif is_hydroelastic and has_shape_collision:
effective_max_resolution = sdf_max_resolution
if sdf_target_voxel_size is None and effective_max_resolution is None:
effective_max_resolution = 64
cache_key = (
"primitive_generated",
shape_type,
shape_gap,
tuple(sdf_narrow_band_range),
sdf_target_voxel_size,
effective_max_resolution,
tuple(shape_scale),
sdf_tex_fmt,
)
if cache_key is not None:
if cache_key in sdf_cache:
shape_sdf_index[i] = sdf_cache[cache_key]
else:
sdf_idx = len(compact_texture_sdf_data)
sdf_cache[cache_key] = sdf_idx
shape_sdf_index[i] = sdf_idx
tex_block_coords = None
if mesh_sdf is not None:
tex_data = mesh_sdf.to_texture_kernel_data()
if tex_data is not None:
compact_texture_sdf_data.append(tex_data)
compact_texture_sdf_coarse_textures.append(mesh_sdf._coarse_texture)
compact_texture_sdf_subgrid_textures.append(mesh_sdf._subgrid_texture)
compact_texture_sdf_subgrid_start_slots.append(tex_data.subgrid_start_slots)
if mesh_sdf.texture_block_coords is not None:
tex_block_coords = mesh_sdf.texture_block_coords
else:
compact_texture_sdf_data.append(create_empty_texture_sdf_data())
compact_texture_sdf_coarse_textures.append(None)
compact_texture_sdf_subgrid_textures.append(None)
compact_texture_sdf_subgrid_start_slots.append(None)
else:
prim_mesh = _create_primitive_mesh(shape_type, shape_scale)
if prim_mesh is not None:
prim_wp_mesh = wp.Mesh(
points=wp.array(prim_mesh.vertices, dtype=wp.vec3, device=device),
indices=wp.array(prim_mesh.indices.flatten(), dtype=wp.int32, device=device),
support_winding_number=True,
)
try:
tex_data, c_tex, s_tex, tex_bc = create_texture_sdf_from_mesh(
prim_wp_mesh,
margin=shape_gap,
narrow_band_range=tuple(sdf_narrow_band_range),
max_resolution=effective_max_resolution,
quantization_mode=_tex_fmt_map[sdf_tex_fmt],
scale_baked=True,
device=device,
)
except Exception as e:
warnings.warn(
f"Texture SDF construction failed for shape {i} "
f"(type={shape_type}): {e}. Falling back to BVH.",
stacklevel=2,
)
tex_data = create_empty_texture_sdf_data()
c_tex = None
s_tex = None
tex_bc = None
compact_texture_sdf_data.append(tex_data)
compact_texture_sdf_coarse_textures.append(c_tex)
compact_texture_sdf_subgrid_textures.append(s_tex)
compact_texture_sdf_subgrid_start_slots.append(
tex_data.subgrid_start_slots if c_tex is not None else None
)
tex_block_coords = tex_bc
else:
compact_texture_sdf_data.append(create_empty_texture_sdf_data())
compact_texture_sdf_coarse_textures.append(None)
compact_texture_sdf_subgrid_textures.append(None)
compact_texture_sdf_subgrid_start_slots.append(None)
final_block_coords = list(tex_block_coords) if tex_block_coords is not None else block_coords
block_start_idx = len(sdf_block_coords)
sdf_block_coords.extend(final_block_coords)
sdf_index2blocks.append([block_start_idx, len(sdf_block_coords)])
m.shape_sdf_index = wp.array(shape_sdf_index, dtype=wp.int32, device=device)
m.sdf_block_coords = wp.array(sdf_block_coords, dtype=wp.vec3us)
m.sdf_index2blocks = (
wp.array(sdf_index2blocks, dtype=wp.vec2i) if sdf_index2blocks else wp.array([], dtype=wp.vec2i)
)
m.texture_sdf_data = (
wp.array(compact_texture_sdf_data, dtype=TextureSDFData, device=device)
if compact_texture_sdf_data
else wp.array([], dtype=TextureSDFData, device=device)
)
m.texture_sdf_coarse_textures = compact_texture_sdf_coarse_textures
m.texture_sdf_subgrid_textures = compact_texture_sdf_subgrid_textures
m.texture_sdf_subgrid_start_slots = compact_texture_sdf_subgrid_start_slots
# ---------------------
# heightfield collision data
hfield_count = sum(1 for t in self.shape_type if t == GeoType.HFIELD)
has_heightfields = hfield_count > 0
if hfield_count > 1:
warnings.warn(
"Heightfield-vs-heightfield collision is not supported; "
"contacts between heightfield pairs will be skipped.",
stacklevel=2,
)
from ..utils.heightfield import HeightfieldData, create_empty_heightfield_data # noqa: PLC0415
compact_heightfield_data = []
elevation_chunks = []
shape_heightfield_index = [-1] * len(self.shape_type)
offset = 0
if has_heightfields:
for i in range(len(self.shape_type)):
if self.shape_type[i] == GeoType.HFIELD and self.shape_source[i] is not None:
hf = self.shape_source[i]
hd = HeightfieldData()
hd.data_offset = offset
hd.nrow = hf.nrow
hd.ncol = hf.ncol
hd.hx = hf.hx
hd.hy = hf.hy
hd.min_z = hf.min_z
hd.max_z = hf.max_z
shape_heightfield_index[i] = len(compact_heightfield_data)
compact_heightfield_data.append(hd)
elevation_chunks.append(hf.data.flatten())
offset += hf.nrow * hf.ncol
m.shape_heightfield_index = wp.array(
shape_heightfield_index if shape_heightfield_index else [-1],
dtype=wp.int32,
device=device,
)
m.heightfield_data = (
wp.array(compact_heightfield_data, dtype=HeightfieldData, device=device)
if compact_heightfield_data
else wp.array([create_empty_heightfield_data()], dtype=HeightfieldData, device=device)
)
m.heightfield_elevations = (
wp.array(np.concatenate(elevation_chunks), dtype=wp.float32, device=device)
if elevation_chunks
else wp.zeros(1, dtype=wp.float32, device=device)
)
# ---------------------
# springs
def _to_wp_array(data, dtype, requires_grad):
if len(data) == 0:
return None
return wp.array(data, dtype=dtype, requires_grad=requires_grad)
m.spring_indices = _to_wp_array(self.spring_indices, wp.int32, requires_grad=False)
m.spring_rest_length = _to_wp_array(self.spring_rest_length, wp.float32, requires_grad=requires_grad)
m.spring_stiffness = _to_wp_array(self.spring_stiffness, wp.float32, requires_grad=requires_grad)
m.spring_damping = _to_wp_array(self.spring_damping, wp.float32, requires_grad=requires_grad)
m.spring_control = _to_wp_array(self.spring_control, wp.float32, requires_grad=requires_grad)
# ---------------------
# triangles
m.tri_indices = _to_wp_array(self.tri_indices, wp.int32, requires_grad=False)
m.tri_poses = _to_wp_array(self.tri_poses, wp.mat22, requires_grad=requires_grad)
m.tri_activations = _to_wp_array(self.tri_activations, wp.float32, requires_grad=requires_grad)
m.tri_materials = _to_wp_array(self.tri_materials, wp.float32, requires_grad=requires_grad)
m.tri_areas = _to_wp_array(self.tri_areas, wp.float32, requires_grad=requires_grad)
# ---------------------
# edges
m.edge_indices = _to_wp_array(self.edge_indices, wp.int32, requires_grad=False)
m.edge_rest_angle = _to_wp_array(self.edge_rest_angle, wp.float32, requires_grad=requires_grad)
m.edge_rest_length = _to_wp_array(self.edge_rest_length, wp.float32, requires_grad=requires_grad)
m.edge_bending_properties = _to_wp_array(
self.edge_bending_properties, wp.float32, requires_grad=requires_grad
)
# ---------------------
# tetrahedra
m.tet_indices = _to_wp_array(self.tet_indices, wp.int32, requires_grad=False)
m.tet_poses = _to_wp_array(self.tet_poses, wp.mat33, requires_grad=requires_grad)
m.tet_activations = _to_wp_array(self.tet_activations, wp.float32, requires_grad=requires_grad)
m.tet_materials = _to_wp_array(self.tet_materials, wp.float32, requires_grad=requires_grad)
# -----------------------
# muscles
# close the muscle waypoint indices
muscle_start = copy.copy(self.muscle_start)
muscle_start.append(len(self.muscle_bodies))
m.muscle_start = wp.array(muscle_start, dtype=wp.int32)
m.muscle_params = wp.array(self.muscle_params, dtype=wp.float32, requires_grad=requires_grad)
m.muscle_bodies = wp.array(self.muscle_bodies, dtype=wp.int32)
m.muscle_points = wp.array(self.muscle_points, dtype=wp.vec3, requires_grad=requires_grad)
m.muscle_activations = wp.array(self.muscle_activations, dtype=wp.float32, requires_grad=requires_grad)
# --------------------------------------
# rigid bodies
# Apply inertia verification and correction
# This catches negative masses/inertias and other critical issues.
# Neither path mutates the builder — corrected values only appear
# on the returned Model so that finalize() is side-effect-free.
if len(self.body_mass) > 0:
if self.validate_inertia_detailed:
# Use detailed Python validation with per-body warnings.
# Build corrected copies without modifying builder lists.
corrected_mass = list(self.body_mass)
corrected_inertia = list(self.body_inertia)
corrected_inv_mass = list(self.body_inv_mass)
corrected_inv_inertia = list(self.body_inv_inertia)
for i in range(len(self.body_mass)):
mass = self.body_mass[i]
inertia = self.body_inertia[i]
body_label = self.body_label[i] if i < len(self.body_label) else f"body_{i}"
new_mass, new_inertia, was_corrected = verify_and_correct_inertia(
mass,
inertia,
self.balance_inertia,
self.bound_mass,
self.bound_inertia,
body_label,
)
if was_corrected:
corrected_mass[i] = new_mass
corrected_inertia[i] = new_inertia
if new_mass > 0.0:
corrected_inv_mass[i] = 1.0 / new_mass
else:
corrected_inv_mass[i] = 0.0
if any(x for x in new_inertia):
corrected_inv_inertia[i] = wp.inverse(new_inertia)
else:
corrected_inv_inertia[i] = new_inertia
# Create arrays from corrected copies
m.body_mass = wp.array(corrected_mass, dtype=wp.float32, requires_grad=requires_grad)
m.body_inv_mass = wp.array(corrected_inv_mass, dtype=wp.float32, requires_grad=requires_grad)
m.body_inertia = wp.array(corrected_inertia, dtype=wp.mat33, requires_grad=requires_grad)
m.body_inv_inertia = wp.array(corrected_inv_inertia, dtype=wp.mat33, requires_grad=requires_grad)
else:
# Use fast Warp kernel validation
body_mass_array = wp.array(self.body_mass, dtype=wp.float32, requires_grad=requires_grad)
body_inertia_array = wp.array(self.body_inertia, dtype=wp.mat33, requires_grad=requires_grad)
body_inv_mass_array = wp.array(self.body_inv_mass, dtype=wp.float32, requires_grad=requires_grad)
body_inv_inertia_array = wp.array(
self.body_inv_inertia, dtype=wp.mat33, requires_grad=requires_grad
)
correction_count = wp.zeros(1, dtype=wp.int32)
# Launch validation kernel (corrects arrays in-place on device)
wp.launch(
kernel=validate_and_correct_inertia_kernel,
dim=len(self.body_mass),
inputs=[
body_mass_array,
body_inertia_array,
body_inv_mass_array,
body_inv_inertia_array,
self.balance_inertia,
self.bound_mass if self.bound_mass is not None else 0.0,
self.bound_inertia if self.bound_inertia is not None else 0.0,
correction_count,
],
)
# Check if any corrections were made (single int transfer)
num_corrections = int(correction_count.numpy()[0])
if num_corrections > 0:
warnings.warn(
f"Inertia validation corrected {num_corrections} bodies. "
f"Set validate_inertia_detailed=True for detailed per-body warnings.",
stacklevel=2,
)
# Use the corrected arrays directly on the Model.
# Builder state is intentionally left unchanged — corrected
# values live only on the returned Model.
m.body_mass = body_mass_array
m.body_inv_mass = body_inv_mass_array
m.body_inertia = body_inertia_array
m.body_inv_inertia = body_inv_inertia_array
else:
# No bodies, create empty arrays
m.body_mass = wp.array(self.body_mass, dtype=wp.float32, requires_grad=requires_grad)
m.body_inv_mass = wp.array(self.body_inv_mass, dtype=wp.float32, requires_grad=requires_grad)
m.body_inertia = wp.array(self.body_inertia, dtype=wp.mat33, requires_grad=requires_grad)
m.body_inv_inertia = wp.array(self.body_inv_inertia, dtype=wp.mat33, requires_grad=requires_grad)
m.body_q = wp.array(self.body_q, dtype=wp.transform, requires_grad=requires_grad)
m.body_qd = wp.array(self.body_qd, dtype=wp.spatial_vector, requires_grad=requires_grad)
m.body_com = wp.array(self.body_com, dtype=wp.vec3, requires_grad=requires_grad)
m.body_label = self.body_label
m.body_flags = wp.array(self.body_flags, dtype=wp.int32)
m.body_world = wp.array(self.body_world, dtype=wp.int32)
# body colors
if self.body_color_groups:
body_colors = np.empty(self.body_count, dtype=int)
for color in range(len(self.body_color_groups)):
body_colors[self.body_color_groups[color]] = color
m.body_colors = wp.array(body_colors, dtype=int)
m.body_color_groups = [wp.array(group, dtype=int) for group in self.body_color_groups]
# joints
m.joint_type = wp.array(self.joint_type, dtype=wp.int32)
m.joint_parent = wp.array(self.joint_parent, dtype=wp.int32)
m.joint_child = wp.array(self.joint_child, dtype=wp.int32)
m.joint_X_p = wp.array(self.joint_X_p, dtype=wp.transform, requires_grad=requires_grad)
m.joint_X_c = wp.array(self.joint_X_c, dtype=wp.transform, requires_grad=requires_grad)
m.joint_dof_dim = wp.array(np.array(self.joint_dof_dim), dtype=wp.int32, ndim=2)
m.joint_axis = wp.array(self.joint_axis, dtype=wp.vec3, requires_grad=requires_grad)
m.joint_q = wp.array(self.joint_q, dtype=wp.float32, requires_grad=requires_grad)
m.joint_qd = wp.array(self.joint_qd, dtype=wp.float32, requires_grad=requires_grad)
m.joint_label = self.joint_label
m.joint_world = wp.array(self.joint_world, dtype=wp.int32)
# compute joint ancestors
child_to_joint = {}
for i, child in enumerate(self.joint_child):
child_to_joint[child] = i
parent_joint = []
for parent in self.joint_parent:
parent_joint.append(child_to_joint.get(parent, -1))
m.joint_ancestor = wp.array(parent_joint, dtype=wp.int32)
m.joint_articulation = wp.array(self.joint_articulation, dtype=wp.int32)
# dynamics properties
m.joint_armature = wp.array(self.joint_armature, dtype=wp.float32, requires_grad=requires_grad)
m.joint_target_mode = wp.array(self.joint_target_mode, dtype=wp.int32)
m.joint_target_ke = wp.array(self.joint_target_ke, dtype=wp.float32, requires_grad=requires_grad)
m.joint_target_kd = wp.array(self.joint_target_kd, dtype=wp.float32, requires_grad=requires_grad)
m.joint_target_pos = wp.array(self.joint_target_pos, dtype=wp.float32, requires_grad=requires_grad)
m.joint_target_vel = wp.array(self.joint_target_vel, dtype=wp.float32, requires_grad=requires_grad)
m.joint_f = wp.array(self.joint_f, dtype=wp.float32, requires_grad=requires_grad)
m.joint_act = wp.array(self.joint_act, dtype=wp.float32, requires_grad=requires_grad)
m.joint_effort_limit = wp.array(self.joint_effort_limit, dtype=wp.float32, requires_grad=requires_grad)
m.joint_velocity_limit = wp.array(self.joint_velocity_limit, dtype=wp.float32, requires_grad=requires_grad)
m.joint_friction = wp.array(self.joint_friction, dtype=wp.float32, requires_grad=requires_grad)
m.joint_limit_lower = wp.array(self.joint_limit_lower, dtype=wp.float32, requires_grad=requires_grad)
m.joint_limit_upper = wp.array(self.joint_limit_upper, dtype=wp.float32, requires_grad=requires_grad)
m.joint_limit_ke = wp.array(self.joint_limit_ke, dtype=wp.float32, requires_grad=requires_grad)
m.joint_limit_kd = wp.array(self.joint_limit_kd, dtype=wp.float32, requires_grad=requires_grad)
m.joint_enabled = wp.array(self.joint_enabled, dtype=wp.bool)
# 'close' the start index arrays with a sentinel value
joint_q_start = copy.copy(self.joint_q_start)
joint_q_start.append(self.joint_coord_count)
joint_qd_start = copy.copy(self.joint_qd_start)
joint_qd_start.append(self.joint_dof_count)
articulation_start = copy.copy(self.articulation_start)
articulation_start.append(self.joint_count)
# Compute max joints and dofs per articulation for IK/Jacobian kernel launches
max_joints_per_articulation = 0
max_dofs_per_articulation = 0
for art_idx in range(len(self.articulation_start)):
joint_start = articulation_start[art_idx]
joint_end = articulation_start[art_idx + 1]
num_joints = joint_end - joint_start
max_joints_per_articulation = max(max_joints_per_articulation, num_joints)
# Compute dofs for this articulation
dof_start = joint_qd_start[joint_start]
dof_end = joint_qd_start[joint_end]
num_dofs = dof_end - dof_start
max_dofs_per_articulation = max(max_dofs_per_articulation, num_dofs)
m.joint_q_start = wp.array(joint_q_start, dtype=wp.int32)
m.joint_qd_start = wp.array(joint_qd_start, dtype=wp.int32)
m.articulation_start = wp.array(articulation_start, dtype=wp.int32)
m.articulation_label = self.articulation_label
m.articulation_world = wp.array(self.articulation_world, dtype=wp.int32)
m.max_joints_per_articulation = max_joints_per_articulation
m.max_dofs_per_articulation = max_dofs_per_articulation
# ---------------------
# equality constraints
m.equality_constraint_type = wp.array(self.equality_constraint_type, dtype=wp.int32)
m.equality_constraint_body1 = wp.array(self.equality_constraint_body1, dtype=wp.int32)
m.equality_constraint_body2 = wp.array(self.equality_constraint_body2, dtype=wp.int32)
m.equality_constraint_anchor = wp.array(self.equality_constraint_anchor, dtype=wp.vec3)
m.equality_constraint_torquescale = wp.array(self.equality_constraint_torquescale, dtype=wp.float32)
m.equality_constraint_relpose = wp.array(
self.equality_constraint_relpose, dtype=wp.transform, requires_grad=requires_grad
)
m.equality_constraint_joint1 = wp.array(self.equality_constraint_joint1, dtype=wp.int32)
m.equality_constraint_joint2 = wp.array(self.equality_constraint_joint2, dtype=wp.int32)
m.equality_constraint_polycoef = wp.array(self.equality_constraint_polycoef, dtype=wp.float32)
m.equality_constraint_label = self.equality_constraint_label
m.equality_constraint_enabled = wp.array(self.equality_constraint_enabled, dtype=wp.bool)
m.equality_constraint_world = wp.array(self.equality_constraint_world, dtype=wp.int32)
# mimic constraints
m.constraint_mimic_joint0 = wp.array(self.constraint_mimic_joint0, dtype=wp.int32)
m.constraint_mimic_joint1 = wp.array(self.constraint_mimic_joint1, dtype=wp.int32)
m.constraint_mimic_coef0 = wp.array(self.constraint_mimic_coef0, dtype=wp.float32)
m.constraint_mimic_coef1 = wp.array(self.constraint_mimic_coef1, dtype=wp.float32)
m.constraint_mimic_enabled = wp.array(self.constraint_mimic_enabled, dtype=wp.bool)
m.constraint_mimic_label = self.constraint_mimic_label
m.constraint_mimic_world = wp.array(self.constraint_mimic_world, dtype=wp.int32)
# ---------------------
# per-world start indices
m.particle_world_start = wp.array(self.particle_world_start, dtype=wp.int32)
m.body_world_start = wp.array(self.body_world_start, dtype=wp.int32)
m.shape_world_start = wp.array(self.shape_world_start, dtype=wp.int32)
m.joint_world_start = wp.array(self.joint_world_start, dtype=wp.int32)
m.articulation_world_start = wp.array(self.articulation_world_start, dtype=wp.int32)
m.equality_constraint_world_start = wp.array(self.equality_constraint_world_start, dtype=wp.int32)
m.joint_dof_world_start = wp.array(self.joint_dof_world_start, dtype=wp.int32)
m.joint_coord_world_start = wp.array(self.joint_coord_world_start, dtype=wp.int32)
m.joint_constraint_world_start = wp.array(self.joint_constraint_world_start, dtype=wp.int32)
# ---------------------
# counts
m.joint_count = self.joint_count
m.joint_dof_count = self.joint_dof_count
m.joint_coord_count = self.joint_coord_count
m.joint_constraint_count = self.joint_constraint_count
m.particle_count = len(self.particle_q)
m.body_count = self.body_count
m.shape_count = len(self.shape_type)
m.tri_count = len(self.tri_poses)
m.tet_count = len(self.tet_poses)
m.edge_count = len(self.edge_rest_angle)
m.spring_count = len(self.spring_rest_length)
m.muscle_count = len(self.muscle_start)
m.articulation_count = len(self.articulation_start)
m.equality_constraint_count = len(self.equality_constraint_type)
m.constraint_mimic_count = len(self.constraint_mimic_joint0)
self.find_shape_contact_pairs(m)
# enable ground plane
m.up_axis = self.up_axis
# set gravity - create per-world gravity array for multi-world support
if self.world_gravity:
# Use per-world gravity from world_gravity list
gravity_vecs = self.world_gravity
else:
# Fallback: use scalar gravity for all worlds
gravity_vec = tuple(g * self.gravity for g in self.up_vector)
gravity_vecs = [gravity_vec] * self.world_count
m.gravity = wp.array(
gravity_vecs,
dtype=wp.vec3,
device=device,
requires_grad=requires_grad,
)
# Create actuators from accumulated entries
m.actuators = []
for (actuator_class, scalar_key), entry in self.actuator_entries.items():
input_indices = self._build_index_array(entry.input_indices, device)
output_indices = self._build_index_array(entry.output_indices, device)
param_arrays = self._stack_args_to_arrays(entry.args, device=device, requires_grad=requires_grad)
scalar_params = dict(scalar_key)
actuator = actuator_class(
input_indices=input_indices,
output_indices=output_indices,
**param_arrays,
**scalar_params,
)
m.actuators.append(actuator)
# Add custom attributes onto the model (with lazy evaluation)
# Early return if no custom attributes exist to avoid overhead
if not self.custom_attributes:
return m
# Resolve authoritative counts for custom frequencies
# Use incremental _custom_frequency_counts as primary source, with safety fallback
custom_frequency_counts: dict[str, int] = {}
frequency_max_lens: dict[str, int] = {} # Track max len(values) per frequency as fallback
# First pass: collect max len(values) per frequency as fallback
for _full_key, custom_attr in self.custom_attributes.items():
freq_key = custom_attr.frequency
if isinstance(freq_key, str):
attr_len = len(custom_attr.values) if custom_attr.values else 0
frequency_max_lens[freq_key] = max(frequency_max_lens.get(freq_key, 0), attr_len)
# Determine authoritative counts: prefer _custom_frequency_counts, fallback to max lens
for freq_key, max_len in frequency_max_lens.items():
if freq_key in self._custom_frequency_counts:
# Use authoritative incremental counter
custom_frequency_counts[freq_key] = self._custom_frequency_counts[freq_key]
else:
# Safety fallback: use max observed length
custom_frequency_counts[freq_key] = max_len
# Warn about MODEL attributes with fewer values than expected (non-MODEL
# attributes are filled at runtime via _add_custom_attributes).
for full_key, custom_attr in self.custom_attributes.items():
freq_key = custom_attr.frequency
if isinstance(freq_key, str) and custom_attr.assignment == Model.AttributeAssignment.MODEL:
attr_count = len(custom_attr.values) if custom_attr.values else 0
expected_count = custom_frequency_counts[freq_key]
if attr_count < expected_count:
warnings.warn(
f"Custom attribute '{full_key}' has {attr_count} values but frequency '{freq_key}' "
f"expects {expected_count}. Missing values will be filled with defaults.",
UserWarning,
stacklevel=2,
)
# Store custom frequency counts on the model for selection.py and other consumers
m.custom_frequency_counts = custom_frequency_counts
# Process custom attributes
for _full_key, custom_attr in self.custom_attributes.items():
freq_key = custom_attr.frequency
# determine count by frequency
if isinstance(freq_key, str):
# Custom frequency: count determined by validated frequency count
count = custom_frequency_counts.get(freq_key, 0)
elif freq_key == Model.AttributeFrequency.ONCE:
count = 1
elif freq_key == Model.AttributeFrequency.BODY:
count = m.body_count
elif freq_key == Model.AttributeFrequency.SHAPE:
count = m.shape_count
elif freq_key == Model.AttributeFrequency.JOINT:
count = m.joint_count
elif freq_key == Model.AttributeFrequency.JOINT_DOF:
count = m.joint_dof_count
elif freq_key == Model.AttributeFrequency.JOINT_COORD:
count = m.joint_coord_count
elif freq_key == Model.AttributeFrequency.JOINT_CONSTRAINT:
count = m.joint_constraint_count
elif freq_key == Model.AttributeFrequency.ARTICULATION:
count = m.articulation_count
elif freq_key == Model.AttributeFrequency.WORLD:
count = m.world_count
elif freq_key == Model.AttributeFrequency.EQUALITY_CONSTRAINT:
count = m.equality_constraint_count
elif freq_key == Model.AttributeFrequency.CONSTRAINT_MIMIC:
count = m.constraint_mimic_count
elif freq_key == Model.AttributeFrequency.PARTICLE:
count = m.particle_count
elif freq_key == Model.AttributeFrequency.EDGE:
count = m.edge_count
elif freq_key == Model.AttributeFrequency.TRIANGLE:
count = m.tri_count
elif freq_key == Model.AttributeFrequency.TETRAHEDRON:
count = m.tet_count
elif freq_key == Model.AttributeFrequency.SPRING:
count = m.spring_count
else:
continue
# Skip empty custom frequency attributes
if count == 0:
continue
result = custom_attr.build_array(count, device=device, requires_grad=requires_grad)
m.add_attribute(custom_attr.name, result, freq_key, custom_attr.assignment, custom_attr.namespace)
return m
def _test_group_pair(self, group_a: int, group_b: int) -> bool:
"""Test if two collision groups should interact.
This matches the exact logic from broad_phase_common.test_group_pair kernel function.
Args:
group_a: First collision group ID
group_b: Second collision group ID
Returns:
Whether the groups should collide.
"""
if group_a == 0 or group_b == 0:
return False
if group_a > 0:
return group_a == group_b or group_b < 0
if group_a < 0:
return group_a != group_b
return False
def _test_world_and_group_pair(
self, world_a: int, world_b: int, collision_group_a: int, collision_group_b: int
) -> bool:
"""Test if two entities should collide based on world indices and collision groups.
This matches the exact logic from broad_phase_common.test_world_and_group_pair kernel function.
Args:
world_a: World index of first entity
world_b: World index of second entity
collision_group_a: Collision group of first entity
collision_group_b: Collision group of second entity
Returns:
Whether the entities should collide.
"""
# Check world indices first
if world_a != -1 and world_b != -1 and world_a != world_b:
return False
# If same world or at least one is global (-1), check collision groups
return self._test_group_pair(collision_group_a, collision_group_b)
def find_shape_contact_pairs(self, model: Model):
"""
Identifies and stores all potential shape contact pairs for collision detection.
This method examines the collision groups and collision masks of all shapes in the model
to determine which pairs of shapes should be considered for contact generation. It respects
any user-specified collision filter pairs to avoid redundant or undesired contacts.
The resulting contact pairs are stored in the model as a 2D array of shape indices.
Uses the exact same filtering logic as the broad phase kernels (test_world_and_group_pair)
to ensure consistency between EXPLICIT mode (precomputed pairs) and NXN/SAP modes.
Args:
model: The simulation model to which the contact pairs will be assigned.
Side Effects:
- Sets `model.shape_contact_pairs` to a wp.array of shape pairs (wp.vec2i).
- Sets `model.shape_contact_pair_count` to the number of contact pairs found.
"""
filters: set[tuple[int, int]] = model.shape_collision_filter_pairs
contact_pairs: list[tuple[int, int]] = []
# Keep only colliding shapes (those with COLLIDE_SHAPES flag) and sort by world for optimization
colliding_indices = [i for i, flag in enumerate(self.shape_flags) if flag & ShapeFlags.COLLIDE_SHAPES]
sorted_indices = sorted(colliding_indices, key=lambda i: self.shape_world[i])
# Iterate over all pairs of colliding shapes
for i1 in range(len(sorted_indices)):
s1 = sorted_indices[i1]
world1 = self.shape_world[s1]
collision_group1 = self.shape_collision_group[s1]
for i2 in range(i1 + 1, len(sorted_indices)):
s2 = sorted_indices[i2]
world2 = self.shape_world[s2]
collision_group2 = self.shape_collision_group[s2]
# Early break optimization: if both shapes are in non-global worlds and different worlds,
# they can never collide. Since shapes are sorted by world, all remaining shapes will also
# be in different worlds, so we can break early.
if world1 != -1 and world2 != -1 and world1 != world2:
break
# Apply the exact same filtering logic as test_world_and_group_pair kernel
if not self._test_world_and_group_pair(world1, world2, collision_group1, collision_group2):
continue
if s1 > s2:
shape_a, shape_b = s2, s1
else:
shape_a, shape_b = s1, s2
# Skip if explicitly filtered
if (shape_a, shape_b) not in filters:
contact_pairs.append((shape_a, shape_b))
model.shape_contact_pairs = wp.array(np.array(contact_pairs), dtype=wp.vec2i, device=model.device)
model.shape_contact_pair_count = len(contact_pairs)
================================================
FILE: newton/_src/sim/collide.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from typing import Literal
import numpy as np
import warp as wp
from ..geometry.broad_phase_nxn import BroadPhaseAllPairs, BroadPhaseExplicit
from ..geometry.broad_phase_sap import BroadPhaseSAP
from ..geometry.collision_core import compute_tight_aabb_from_support
from ..geometry.contact_data import ContactData
from ..geometry.differentiable_contacts import launch_differentiable_contact_augment
from ..geometry.kernels import create_soft_contacts
from ..geometry.narrow_phase import NarrowPhase
from ..geometry.sdf_hydroelastic import HydroelasticSDF
from ..geometry.support_function import (
GenericShapeData,
SupportMapDataProvider,
pack_mesh_ptr,
)
from ..geometry.types import GeoType
from ..sim.contacts import Contacts
from ..sim.model import Model
from ..sim.state import State
@wp.struct
class ContactWriterData:
"""Contact writer data for collide write_contact function."""
contact_max: int
# Body information arrays (for transforming to body-local coordinates)
body_q: wp.array[wp.transform]
shape_body: wp.array[int]
shape_gap: wp.array[float]
# Output arrays
contact_count: wp.array[int]
out_shape0: wp.array[int]
out_shape1: wp.array[int]
out_point0: wp.array[wp.vec3]
out_point1: wp.array[wp.vec3]
out_offset0: wp.array[wp.vec3]
out_offset1: wp.array[wp.vec3]
out_normal: wp.array[wp.vec3]
out_margin0: wp.array[float]
out_margin1: wp.array[float]
out_tids: wp.array[int]
# Per-contact shape properties, empty arrays if not enabled.
# Zero-values indicate that no per-contact shape properties are set for this contact
out_stiffness: wp.array[float]
out_damping: wp.array[float]
out_friction: wp.array[float]
@wp.func
def write_contact(
contact_data: ContactData,
writer_data: ContactWriterData,
output_index: int,
):
"""
Write a contact to the output arrays using ContactData and ContactWriterData.
Args:
contact_data: ContactData struct containing contact information
writer_data: ContactWriterData struct containing body info and output arrays
output_index: If -1, use atomic_add to get the next available index if contact distance is less than margin. If >= 0, use this index directly and skip margin check.
"""
total_separation_needed = (
contact_data.radius_eff_a + contact_data.radius_eff_b + contact_data.margin_a + contact_data.margin_b
)
offset_mag_a = contact_data.radius_eff_a + contact_data.margin_a
offset_mag_b = contact_data.radius_eff_b + contact_data.margin_b
# Distance calculation matching box_plane_collision
contact_normal_a_to_b = wp.normalize(contact_data.contact_normal_a_to_b)
a_contact_world = contact_data.contact_point_center - contact_normal_a_to_b * (
0.5 * contact_data.contact_distance + contact_data.radius_eff_a
)
b_contact_world = contact_data.contact_point_center + contact_normal_a_to_b * (
0.5 * contact_data.contact_distance + contact_data.radius_eff_b
)
diff = b_contact_world - a_contact_world
distance = wp.dot(diff, contact_normal_a_to_b)
d = distance - total_separation_needed
# Use per-shape contact gaps (sum of both shapes)
gap_a = writer_data.shape_gap[contact_data.shape_a]
gap_b = writer_data.shape_gap[contact_data.shape_b]
contact_gap = gap_a + gap_b
index = output_index
if index < 0:
# compute index using atomic counter
if d > contact_gap:
return
index = wp.atomic_add(writer_data.contact_count, 0, 1)
if index >= writer_data.contact_max:
return
writer_data.out_shape0[index] = contact_data.shape_a
writer_data.out_shape1[index] = contact_data.shape_b
# Get body indices for the shapes
body0 = writer_data.shape_body[contact_data.shape_a]
body1 = writer_data.shape_body[contact_data.shape_b]
# Compute body inverse transforms
X_bw_a = wp.transform_identity() if body0 == -1 else wp.transform_inverse(writer_data.body_q[body0])
X_bw_b = wp.transform_identity() if body1 == -1 else wp.transform_inverse(writer_data.body_q[body1])
# Contact points are stored in body frames
writer_data.out_point0[index] = wp.transform_point(X_bw_a, a_contact_world)
writer_data.out_point1[index] = wp.transform_point(X_bw_b, b_contact_world)
contact_normal = contact_normal_a_to_b
# Offsets in body frames (offset0 points toward B, offset1 points toward A)
writer_data.out_offset0[index] = wp.transform_vector(X_bw_a, offset_mag_a * contact_normal)
writer_data.out_offset1[index] = wp.transform_vector(X_bw_b, -offset_mag_b * contact_normal)
writer_data.out_normal[index] = contact_normal
writer_data.out_margin0[index] = offset_mag_a
writer_data.out_margin1[index] = offset_mag_b
writer_data.out_tids[index] = 0 # tid not available in this context
# Write stiffness/damping/friction only if per-contact shape properties are enabled
if writer_data.out_stiffness.shape[0] > 0:
writer_data.out_stiffness[index] = contact_data.contact_stiffness
writer_data.out_damping[index] = contact_data.contact_damping
writer_data.out_friction[index] = contact_data.contact_friction_scale
@wp.kernel(enable_backward=False)
def compute_shape_aabbs(
body_q: wp.array[wp.transform],
shape_transform: wp.array[wp.transform],
shape_body: wp.array[int],
shape_type: wp.array[int],
shape_scale: wp.array[wp.vec3],
shape_collision_radius: wp.array[float],
shape_source_ptr: wp.array[wp.uint64],
shape_margin: wp.array[float],
shape_gap: wp.array[float],
shape_collision_aabb_lower: wp.array[wp.vec3],
shape_collision_aabb_upper: wp.array[wp.vec3],
# outputs
aabb_lower: wp.array[wp.vec3],
aabb_upper: wp.array[wp.vec3],
):
"""Compute axis-aligned bounding boxes for each shape in world space.
Uses support function for most shapes. Meshes and heightfields use the pre-computed
local AABB transformed to world frame. Infinite planes use bounding sphere fallback.
AABBs are enlarged by per-shape effective gap for contact detection.
Effective expansion is ``shape_margin + shape_gap``.
"""
shape_id = wp.tid()
rigid_id = shape_body[shape_id]
geo_type = shape_type[shape_id]
# Compute world transform
if rigid_id == -1:
X_ws = shape_transform[shape_id]
else:
X_ws = wp.transform_multiply(body_q[rigid_id], shape_transform[shape_id])
pos = wp.transform_get_translation(X_ws)
orientation = wp.transform_get_rotation(X_ws)
# Enlarge AABB by per-shape effective gap for contact detection
effective_gap = shape_margin[shape_id] + shape_gap[shape_id]
margin_vec = wp.vec3(effective_gap, effective_gap, effective_gap)
# Check if this is an infinite plane, mesh, or heightfield
scale = shape_scale[shape_id]
is_infinite_plane = (geo_type == GeoType.PLANE) and (scale[0] == 0.0 and scale[1] == 0.0)
is_mesh = geo_type == GeoType.MESH
is_hfield = geo_type == GeoType.HFIELD
if is_infinite_plane:
# Bounding sphere fallback for infinite planes
radius = shape_collision_radius[shape_id]
half_extents = wp.vec3(radius, radius, radius)
aabb_lower[shape_id] = pos - half_extents - margin_vec
aabb_upper[shape_id] = pos + half_extents + margin_vec
elif is_mesh or is_hfield:
# Tight local AABB transformed to world space.
# Scale is already baked into shape_collision_aabb by the builder,
# so we only need to handle the rotation here.
local_lo = shape_collision_aabb_lower[shape_id]
local_hi = shape_collision_aabb_upper[shape_id]
center = (local_lo + local_hi) * 0.5
half = (local_hi - local_lo) * 0.5
# Rotate center to world frame
world_center = wp.quat_rotate(orientation, center) + pos
# Rotated AABB half-extents via abs of rotation matrix columns
r0 = wp.quat_rotate(orientation, wp.vec3(1.0, 0.0, 0.0))
r1 = wp.quat_rotate(orientation, wp.vec3(0.0, 1.0, 0.0))
r2 = wp.quat_rotate(orientation, wp.vec3(0.0, 0.0, 1.0))
world_half = wp.vec3(
wp.abs(r0[0]) * half[0] + wp.abs(r1[0]) * half[1] + wp.abs(r2[0]) * half[2],
wp.abs(r0[1]) * half[0] + wp.abs(r1[1]) * half[1] + wp.abs(r2[1]) * half[2],
wp.abs(r0[2]) * half[0] + wp.abs(r1[2]) * half[1] + wp.abs(r2[2]) * half[2],
)
aabb_lower[shape_id] = world_center - world_half - margin_vec
aabb_upper[shape_id] = world_center + world_half + margin_vec
else:
# Use support function to compute tight AABB
# Create generic shape data
shape_data = GenericShapeData()
shape_data.shape_type = geo_type
shape_data.scale = scale
shape_data.auxiliary = wp.vec3(0.0, 0.0, 0.0)
# For CONVEX_MESH, pack the mesh pointer
if geo_type == GeoType.CONVEX_MESH:
shape_data.auxiliary = pack_mesh_ptr(shape_source_ptr[shape_id])
data_provider = SupportMapDataProvider()
# Compute tight AABB using helper function
aabb_min_world, aabb_max_world = compute_tight_aabb_from_support(shape_data, orientation, pos, data_provider)
aabb_lower[shape_id] = aabb_min_world - margin_vec
aabb_upper[shape_id] = aabb_max_world + margin_vec
@wp.kernel(enable_backward=False)
def prepare_geom_data_kernel(
shape_transform: wp.array[wp.transform],
shape_body: wp.array[int],
shape_type: wp.array[int],
shape_scale: wp.array[wp.vec3],
shape_margin: wp.array[float],
body_q: wp.array[wp.transform],
# Outputs
geom_data: wp.array[wp.vec4], # scale xyz, margin w
geom_transform: wp.array[wp.transform], # world space transform
):
"""Prepare geometry data arrays for NarrowPhase API."""
idx = wp.tid()
# Pack scale and margin into geom_data
scale = shape_scale[idx]
margin = shape_margin[idx]
geom_data[idx] = wp.vec4(scale[0], scale[1], scale[2], margin)
# Compute world space transform
body_idx = shape_body[idx]
if body_idx >= 0:
geom_transform[idx] = wp.transform_multiply(body_q[body_idx], shape_transform[idx])
else:
geom_transform[idx] = shape_transform[idx]
def _estimate_rigid_contact_max(model: Model) -> int:
"""
Estimate the maximum number of rigid contacts for the collision pipeline.
Uses a linear neighbor-budget estimate assuming each non-plane shape contacts
at most ``MAX_NEIGHBORS_PER_SHAPE`` others (spatial locality). The non-plane
term is additive across independent worlds so a single-pool computation is
correct. The plane term (each plane vs all non-planes in its world) would be
quadratic if computed globally, so it is evaluated per world when metadata is
available.
When precomputed contact pairs are available their count is used as an
alternative tighter bound (``min`` of heuristic and pair-based estimate).
Args:
model: The simulation model.
Returns:
Estimated maximum number of rigid contacts.
"""
if not hasattr(model, "shape_type") or model.shape_type is None:
return 1000 # Fallback
shape_types = model.shape_type.numpy()
# Primitive pairs (GJK/MPR) produce up to 5 manifold contacts.
# Mesh-involved pairs (SDF + contact reduction) typically retain ~40.
PRIMITIVE_CPP = 5
MESH_CPP = 40
MAX_NEIGHBORS_PER_SHAPE = 20
mesh_mask = (shape_types == int(GeoType.MESH)) | (shape_types == int(GeoType.HFIELD))
plane_mask = shape_types == int(GeoType.PLANE)
non_plane_mask = ~plane_mask
num_meshes = int(np.count_nonzero(mesh_mask))
num_non_planes = int(np.count_nonzero(non_plane_mask))
num_primitives = num_non_planes - num_meshes
num_planes = int(np.count_nonzero(plane_mask))
# Weighted contacts from non-plane shape types.
# Each shape's neighbor pairs are weighted by its type's contacts-per-pair.
# Divide by 2 to avoid double-counting pairs.
non_plane_contacts = (
num_primitives * MAX_NEIGHBORS_PER_SHAPE * PRIMITIVE_CPP + num_meshes * MAX_NEIGHBORS_PER_SHAPE * MESH_CPP
) // 2
# Weighted average contacts-per-pair based on the scene's shape mix.
avg_cpp = (
(num_primitives * PRIMITIVE_CPP + num_meshes * MESH_CPP) // max(num_non_planes, 1) if num_non_planes > 0 else 0
)
# Plane contacts: each plane contacts all non-plane shapes *in its world*.
# The naive global formula (num_planes * num_non_planes) is O(worlds²) when
# both counts grow with the number of worlds. Use per-world counts instead.
plane_contacts = 0
if num_planes > 0 and num_non_planes > 0:
has_world_info = (
hasattr(model, "shape_world")
and model.shape_world is not None
and hasattr(model, "world_count")
and model.world_count > 0
)
shape_world = model.shape_world.numpy() if has_world_info else None
if shape_world is not None and len(shape_world) == len(shape_types):
global_mask = shape_world == -1
local_mask = ~global_mask
n_worlds = model.world_count
global_planes = int(np.count_nonzero(global_mask & plane_mask))
global_non_planes = int(np.count_nonzero(global_mask & non_plane_mask))
local_plane_counts = np.bincount(shape_world[local_mask & plane_mask], minlength=n_worlds)[:n_worlds]
local_non_plane_counts = np.bincount(shape_world[local_mask & non_plane_mask], minlength=n_worlds)[
:n_worlds
]
per_world_planes = local_plane_counts + global_planes
per_world_non_planes = local_non_plane_counts + global_non_planes
# Global-global pairs appear in every world slice; keep one copy.
plane_pair_count = int(np.sum(per_world_planes * per_world_non_planes))
if n_worlds > 1:
plane_pair_count -= (n_worlds - 1) * global_planes * global_non_planes
plane_contacts = plane_pair_count * avg_cpp
else:
# Fallback: exact type-weighted sum (correct for single-world models).
plane_contacts = num_planes * (num_primitives * PRIMITIVE_CPP + num_meshes * MESH_CPP)
total_contacts = non_plane_contacts + plane_contacts
# When precomputed contact pairs are available, use as a tighter bound.
if hasattr(model, "shape_contact_pair_count") and model.shape_contact_pair_count > 0:
weighted_cpp = max(avg_cpp, PRIMITIVE_CPP)
pair_contacts = int(model.shape_contact_pair_count) * weighted_cpp
total_contacts = min(total_contacts, pair_contacts)
# Ensure minimum allocation
return max(1000, total_contacts)
BROAD_PHASE_MODES = ("nxn", "sap", "explicit")
def _normalize_broad_phase_mode(mode: str) -> str:
mode_str = str(mode).lower()
if mode_str not in BROAD_PHASE_MODES:
raise ValueError(f"Unsupported broad phase mode: {mode!r}")
return mode_str
def _infer_broad_phase_mode_from_instance(broad_phase: BroadPhaseAllPairs | BroadPhaseSAP | BroadPhaseExplicit) -> str:
if isinstance(broad_phase, BroadPhaseAllPairs):
return "nxn"
if isinstance(broad_phase, BroadPhaseSAP):
return "sap"
if isinstance(broad_phase, BroadPhaseExplicit):
return "explicit"
raise TypeError(
"broad_phase must be a BroadPhaseAllPairs, BroadPhaseSAP, or BroadPhaseExplicit instance "
f"(got {type(broad_phase)!r})"
)
class CollisionPipeline:
"""
Full-featured collision pipeline with GJK/MPR narrow phase and pluggable broad phase.
Key features:
- GJK/MPR algorithms for convex-convex collision detection
- Multiple broad phase options: NXN (all-pairs), SAP (sweep-and-prune), EXPLICIT (precomputed pairs)
- Mesh-mesh collision via SDF with contact reduction
- Optional hydroelastic contact model for compliant surfaces
For most users, construct with ``CollisionPipeline(model, ...)``.
.. note::
Differentiable rigid contacts (the ``rigid_contact_diff_*`` arrays when
``requires_grad`` is enabled) are **experimental**. The narrow phase stays
frozen and gradients are a tangent approximation; validate accuracy and
usefulness on your workflow before relying on them in optimization loops.
"""
def __init__(
self,
model: Model,
*,
reduce_contacts: bool = True,
rigid_contact_max: int | None = None,
max_triangle_pairs: int = 1000000,
shape_pairs_filtered: wp.array[wp.vec2i] | None = None,
soft_contact_max: int | None = None,
soft_contact_margin: float = 0.01,
requires_grad: bool | None = None,
broad_phase: Literal["nxn", "sap", "explicit"]
| BroadPhaseAllPairs
| BroadPhaseSAP
| BroadPhaseExplicit
| None = None,
narrow_phase: NarrowPhase | None = None,
sdf_hydroelastic_config: HydroelasticSDF.Config | None = None,
):
"""
Initialize the CollisionPipeline (expert API).
Args:
model: The simulation model.
reduce_contacts: Whether to reduce contacts for mesh-mesh collisions. Defaults to True.
rigid_contact_max: Maximum number of rigid contacts to allocate.
Resolution order:
- If provided, use this value.
- Else if ``model.rigid_contact_max > 0``, use the model value.
- Else estimate automatically from model shape and pair metadata.
max_triangle_pairs:
Maximum number of triangle pairs allocated by narrow phase
for mesh and heightfield collisions. Increase this when
scenes with large/complex meshes or heightfields report
triangle-pair overflow warnings.
soft_contact_max: Maximum number of soft contacts to allocate.
If None, computed as shape_count * particle_count.
soft_contact_margin: Margin for soft contact generation. Defaults to 0.01.
requires_grad: Whether to enable gradient computation. If None, uses model.requires_grad.
broad_phase:
Either a broad phase mode string ("explicit", "nxn", "sap") or
a prebuilt broad phase instance for expert usage.
narrow_phase: Optional prebuilt narrow phase instance. Must be
provided together with a broad phase instance for expert usage.
shape_pairs_filtered: Precomputed shape pairs for EXPLICIT mode.
When broad_phase is "explicit", uses model.shape_contact_pairs if not provided. For
"nxn"/"sap" modes, ignored.
sdf_hydroelastic_config: Configuration for
hydroelastic collision handling. Defaults to None.
.. note::
When ``requires_grad`` is true (explicitly or via ``model.requires_grad``),
rigid-contact autodiff via ``rigid_contact_diff_*`` is **experimental**;
see :meth:`collide`.
"""
mode_from_broad_phase: str | None = None
broad_phase_instance: BroadPhaseAllPairs | BroadPhaseSAP | BroadPhaseExplicit | None = None
if broad_phase is not None:
if isinstance(broad_phase, str):
mode_from_broad_phase = _normalize_broad_phase_mode(broad_phase)
else:
broad_phase_instance = broad_phase
shape_count = model.shape_count
particle_count = model.particle_count
device = model.device
# Resolve rigid contact capacity with explicit > model > estimated precedence.
if rigid_contact_max is None:
model_rigid_contact_max = int(getattr(model, "rigid_contact_max", 0) or 0)
if model_rigid_contact_max > 0:
rigid_contact_max = model_rigid_contact_max
else:
rigid_contact_max = _estimate_rigid_contact_max(model)
self._rigid_contact_max = rigid_contact_max
if max_triangle_pairs <= 0:
raise ValueError("max_triangle_pairs must be > 0")
# Keep model-level default in sync with the resolved pipeline capacity.
# This avoids divergence between model- and contacts-based users (e.g. VBD init).
model.rigid_contact_max = rigid_contact_max
if requires_grad is None:
requires_grad = model.requires_grad
shape_world = getattr(model, "shape_world", None)
shape_flags = getattr(model, "shape_flags", None)
with wp.ScopedDevice(device):
shape_aabb_lower = wp.zeros(shape_count, dtype=wp.vec3, device=device)
shape_aabb_upper = wp.zeros(shape_count, dtype=wp.vec3, device=device)
self.model = model
self.shape_count = shape_count
self.device = device
self.reduce_contacts = reduce_contacts
self.requires_grad = requires_grad
self.soft_contact_margin = soft_contact_margin
using_expert_components = broad_phase_instance is not None or narrow_phase is not None
if using_expert_components:
if broad_phase_instance is None or narrow_phase is None:
raise ValueError("Provide both broad_phase and narrow_phase for expert component construction")
if sdf_hydroelastic_config is not None:
raise ValueError("sdf_hydroelastic_config cannot be used when narrow_phase is provided")
inferred_mode = _infer_broad_phase_mode_from_instance(broad_phase_instance)
self.broad_phase_mode = inferred_mode
self.broad_phase = broad_phase_instance
if self.broad_phase_mode == "explicit":
if shape_pairs_filtered is None:
shape_pairs_filtered = getattr(model, "shape_contact_pairs", None)
if shape_pairs_filtered is None:
raise ValueError(
"shape_pairs_filtered must be provided for explicit broad phase "
"(or set model.shape_contact_pairs)"
)
self.shape_pairs_filtered = shape_pairs_filtered
self.shape_pairs_max = len(shape_pairs_filtered)
self.shape_pairs_excluded = None
self.shape_pairs_excluded_count = 0
else:
self.shape_pairs_filtered = None
self.shape_pairs_max = (shape_count * (shape_count - 1)) // 2
self.shape_pairs_excluded = self._build_excluded_pairs(model)
self.shape_pairs_excluded_count = (
self.shape_pairs_excluded.shape[0] if self.shape_pairs_excluded is not None else 0
)
if narrow_phase.max_candidate_pairs < self.shape_pairs_max:
raise ValueError(
"Provided narrow_phase.max_candidate_pairs is too small for this model and broad phase mode "
f"(required at least {self.shape_pairs_max}, got {narrow_phase.max_candidate_pairs})"
)
self.narrow_phase = narrow_phase
self.hydroelastic_sdf = self.narrow_phase.hydroelastic_sdf
else:
self.broad_phase_mode = mode_from_broad_phase if mode_from_broad_phase is not None else "explicit"
if self.broad_phase_mode == "explicit":
if shape_pairs_filtered is None:
shape_pairs_filtered = getattr(model, "shape_contact_pairs", None)
if shape_pairs_filtered is None:
raise ValueError(
"shape_pairs_filtered must be provided for broad_phase=EXPLICIT "
"(or set model.shape_contact_pairs)"
)
self.broad_phase = BroadPhaseExplicit()
self.shape_pairs_filtered = shape_pairs_filtered
self.shape_pairs_max = len(shape_pairs_filtered)
self.shape_pairs_excluded = None
self.shape_pairs_excluded_count = 0
elif self.broad_phase_mode == "nxn":
if shape_world is None:
raise ValueError("model.shape_world is required for broad_phase=NXN")
self.broad_phase = BroadPhaseAllPairs(shape_world, shape_flags=shape_flags, device=device)
self.shape_pairs_filtered = None
self.shape_pairs_max = (shape_count * (shape_count - 1)) // 2
self.shape_pairs_excluded = self._build_excluded_pairs(model)
self.shape_pairs_excluded_count = (
self.shape_pairs_excluded.shape[0] if self.shape_pairs_excluded is not None else 0
)
elif self.broad_phase_mode == "sap":
if shape_world is None:
raise ValueError("model.shape_world is required for broad_phase=SAP")
self.broad_phase = BroadPhaseSAP(shape_world, shape_flags=shape_flags, device=device)
self.shape_pairs_filtered = None
self.shape_pairs_max = (shape_count * (shape_count - 1)) // 2
self.shape_pairs_excluded = self._build_excluded_pairs(model)
self.shape_pairs_excluded_count = (
self.shape_pairs_excluded.shape[0] if self.shape_pairs_excluded is not None else 0
)
else:
raise ValueError(f"Unsupported broad phase mode: {self.broad_phase_mode}")
# Initialize SDF hydroelastic (returns None if no hydroelastic shape pairs in the model)
hydroelastic_sdf = HydroelasticSDF._from_model(
model,
config=sdf_hydroelastic_config,
writer_func=write_contact,
)
# Detect shape classes to optimize narrow-phase kernel launches.
# Keep mesh and heightfield flags independent: heightfield-only scenes
# should not trigger mesh-only kernel setup/launches.
has_meshes = False
has_heightfields = False
use_lean_gjk_mpr = False
if hasattr(model, "shape_type") and model.shape_type is not None:
shape_types = model.shape_type.numpy()
has_heightfields = bool((shape_types == int(GeoType.HFIELD)).any())
has_meshes = bool((shape_types == int(GeoType.MESH)).any())
# Use lean GJK/MPR kernel when scene has no capsules, ellipsoids,
# cylinders, or cones (which need full support function and axial
# rolling post-processing)
lean_unsupported = {
int(GeoType.CAPSULE),
int(GeoType.ELLIPSOID),
int(GeoType.CYLINDER),
int(GeoType.CONE),
}
use_lean_gjk_mpr = not bool(lean_unsupported & set(shape_types.tolist()))
# Initialize narrow phase with pre-allocated buffers
# max_triangle_pairs is a conservative estimate for mesh collision triangle pairs
# Pass write_contact as custom writer to write directly to final Contacts format
self.narrow_phase = NarrowPhase(
max_candidate_pairs=self.shape_pairs_max,
max_triangle_pairs=max_triangle_pairs,
reduce_contacts=self.reduce_contacts,
device=device,
shape_aabb_lower=shape_aabb_lower,
shape_aabb_upper=shape_aabb_upper,
contact_writer_warp_func=write_contact,
shape_voxel_resolution=model._shape_voxel_resolution,
hydroelastic_sdf=hydroelastic_sdf,
has_meshes=has_meshes,
has_heightfields=has_heightfields,
use_lean_gjk_mpr=use_lean_gjk_mpr,
)
self.hydroelastic_sdf = self.narrow_phase.hydroelastic_sdf
# Allocate buffers
with wp.ScopedDevice(device):
self.broad_phase_pair_count = wp.zeros(1, dtype=wp.int32, device=device)
self.broad_phase_shape_pairs = wp.zeros(self.shape_pairs_max, dtype=wp.vec2i, device=device)
self.geom_data = wp.zeros(shape_count, dtype=wp.vec4, device=device)
self.geom_transform = wp.zeros(shape_count, dtype=wp.transform, device=device)
if (
getattr(self.narrow_phase, "shape_aabb_lower", None) is None
or getattr(self.narrow_phase, "shape_aabb_upper", None) is None
):
raise ValueError("narrow_phase must expose shape_aabb_lower and shape_aabb_upper arrays")
if self.narrow_phase.shape_aabb_lower.shape[0] != shape_count:
raise ValueError(
"narrow_phase.shape_aabb_lower must have one entry per model shape "
f"(expected {shape_count}, got {self.narrow_phase.shape_aabb_lower.shape[0]})"
)
if self.narrow_phase.shape_aabb_upper.shape[0] != shape_count:
raise ValueError(
"narrow_phase.shape_aabb_upper must have one entry per model shape "
f"(expected {shape_count}, got {self.narrow_phase.shape_aabb_upper.shape[0]})"
)
if soft_contact_max is None:
soft_contact_max = shape_count * particle_count
self.soft_contact_margin = soft_contact_margin
self._soft_contact_max = soft_contact_max
self.requires_grad = requires_grad
@property
def rigid_contact_max(self) -> int:
"""Maximum rigid contact buffer capacity used by this pipeline."""
return self._rigid_contact_max
@property
def soft_contact_max(self) -> int:
"""Maximum soft contact buffer capacity used by this pipeline."""
return self._soft_contact_max
def contacts(self) -> Contacts:
"""
Allocate and return a new :class:`newton.Contacts` object for this pipeline.
The returned buffer uses this pipeline's ``requires_grad`` flag (resolved at
construction from the argument or ``model.requires_grad``).
Returns:
A newly allocated contacts buffer sized for this pipeline.
.. note::
If ``requires_grad`` is true, ``rigid_contact_diff_*`` arrays may be
allocated; rigid-contact differentiability is **experimental** (see
:meth:`collide`).
"""
contacts = Contacts(
self.rigid_contact_max,
self.soft_contact_max,
requires_grad=self.requires_grad,
device=self.model.device,
per_contact_shape_properties=self.narrow_phase.hydroelastic_sdf is not None,
requested_attributes=self.model.get_requested_contact_attributes(),
)
# attach custom attributes with assignment==CONTACT
self.model._add_custom_attributes(contacts, Model.AttributeAssignment.CONTACT, requires_grad=self.requires_grad)
return contacts
@staticmethod
def _build_excluded_pairs(model: Model) -> wp.array[wp.vec2i] | None:
if not hasattr(model, "shape_collision_filter_pairs"):
return None
filters = model.shape_collision_filter_pairs
if not filters:
return None
sorted_pairs = sorted(filters) # lexicographic (already canonical min,max)
return wp.array(
np.array(sorted_pairs),
dtype=wp.vec2i,
device=model.device,
)
def collide(
self,
state: State,
contacts: Contacts,
*,
soft_contact_margin: float | None = None,
):
"""Run the collision pipeline using NarrowPhase.
Safe to call inside a :class:`wp.Tape` context. The non-differentiable
broad-phase and narrow-phase kernels are launched with tape recording
hardcoded ``record_tape=False`` internally. The differentiable kernels
(soft-contact generation and rigid-contact augmentation) are recorded on
the tape so that gradients flow through ``state.body_q`` and
``state.particle_q``.
When ``requires_grad=True``, the differentiable rigid-contact arrays
(``contacts.rigid_contact_diff_*``) are populated by a lightweight
augmentation kernel that reconstructs world-space contact points from
the frozen narrow-phase output through the body transforms.
.. note::
This rigid-contact gradient path is **experimental**: usefulness and
numerical behaviour are still being assessed across real-world scenarios.
Args:
state: The current simulation state.
contacts: The contacts buffer to populate (will be cleared first).
soft_contact_margin: Margin for soft contact generation.
If ``None``, uses the value from construction.
"""
contacts.clear()
# TODO: validate contacts dimensions & compatibility
# Clear counters
self.broad_phase_pair_count.zero_()
model = self.model
# update any additional parameters
soft_contact_margin = soft_contact_margin if soft_contact_margin is not None else self.soft_contact_margin
# Rigid contact detection -- broad phase + narrow phase.
# These kernels hardcode record_tape=False internally so they are
# never captured on an active wp.Tape. The differentiable
# augmentation and soft-contact kernels that follow are tape-safe
# and recorded normally.
# Compute AABBs for all shapes (already expanded by per-shape effective gaps)
wp.launch(
kernel=compute_shape_aabbs,
dim=model.shape_count,
inputs=[
state.body_q,
model.shape_transform,
model.shape_body,
model.shape_type,
model.shape_scale,
model.shape_collision_radius,
model.shape_source_ptr,
model.shape_margin,
model.shape_gap,
model.shape_collision_aabb_lower,
model.shape_collision_aabb_upper,
],
outputs=[
self.narrow_phase.shape_aabb_lower,
self.narrow_phase.shape_aabb_upper,
],
device=self.device,
record_tape=False,
)
# Run broad phase (AABBs are already expanded by effective gaps, so pass None)
if isinstance(self.broad_phase, BroadPhaseAllPairs):
self.broad_phase.launch(
self.narrow_phase.shape_aabb_lower,
self.narrow_phase.shape_aabb_upper,
None, # AABBs are pre-expanded, no additional margin needed
model.shape_collision_group,
model.shape_world,
model.shape_count,
self.broad_phase_shape_pairs,
self.broad_phase_pair_count,
device=self.device,
filter_pairs=self.shape_pairs_excluded,
num_filter_pairs=self.shape_pairs_excluded_count,
)
elif isinstance(self.broad_phase, BroadPhaseSAP):
self.broad_phase.launch(
self.narrow_phase.shape_aabb_lower,
self.narrow_phase.shape_aabb_upper,
None, # AABBs are pre-expanded, no additional margin needed
model.shape_collision_group,
model.shape_world,
model.shape_count,
self.broad_phase_shape_pairs,
self.broad_phase_pair_count,
device=self.device,
filter_pairs=self.shape_pairs_excluded,
num_filter_pairs=self.shape_pairs_excluded_count,
)
else: # BroadPhaseExplicit
self.broad_phase.launch(
self.narrow_phase.shape_aabb_lower,
self.narrow_phase.shape_aabb_upper,
None, # AABBs are pre-expanded, no additional margin needed
self.shape_pairs_filtered,
len(self.shape_pairs_filtered),
self.broad_phase_shape_pairs,
self.broad_phase_pair_count,
device=self.device,
)
# Prepare geometry data arrays for NarrowPhase API
wp.launch(
kernel=prepare_geom_data_kernel,
dim=model.shape_count,
inputs=[
model.shape_transform,
model.shape_body,
model.shape_type,
model.shape_scale,
model.shape_margin,
state.body_q,
],
outputs=[
self.geom_data,
self.geom_transform,
],
device=self.device,
record_tape=False,
)
# Create ContactWriterData struct for custom contact writing
writer_data = ContactWriterData()
writer_data.contact_max = contacts.rigid_contact_max
writer_data.body_q = state.body_q
writer_data.shape_body = model.shape_body
writer_data.shape_gap = model.shape_gap
writer_data.contact_count = contacts.rigid_contact_count
writer_data.out_shape0 = contacts.rigid_contact_shape0
writer_data.out_shape1 = contacts.rigid_contact_shape1
writer_data.out_point0 = contacts.rigid_contact_point0
writer_data.out_point1 = contacts.rigid_contact_point1
writer_data.out_offset0 = contacts.rigid_contact_offset0
writer_data.out_offset1 = contacts.rigid_contact_offset1
writer_data.out_normal = contacts.rigid_contact_normal
writer_data.out_margin0 = contacts.rigid_contact_margin0
writer_data.out_margin1 = contacts.rigid_contact_margin1
writer_data.out_tids = contacts.rigid_contact_tids
writer_data.out_stiffness = contacts.rigid_contact_stiffness
writer_data.out_damping = contacts.rigid_contact_damping
writer_data.out_friction = contacts.rigid_contact_friction
# Run narrow phase with custom contact writer (writes directly to Contacts format)
self.narrow_phase.launch_custom_write(
candidate_pair=self.broad_phase_shape_pairs,
candidate_pair_count=self.broad_phase_pair_count,
shape_types=model.shape_type,
shape_data=self.geom_data,
shape_transform=self.geom_transform,
shape_source=model.shape_source_ptr,
shape_sdf_index=model.shape_sdf_index,
texture_sdf_data=model.texture_sdf_data,
shape_gap=model.shape_gap,
shape_collision_radius=model.shape_collision_radius,
shape_flags=model.shape_flags,
shape_collision_aabb_lower=model.shape_collision_aabb_lower,
shape_collision_aabb_upper=model.shape_collision_aabb_upper,
shape_voxel_resolution=self.narrow_phase.shape_voxel_resolution,
shape_heightfield_index=model.shape_heightfield_index,
heightfield_data=model.heightfield_data,
heightfield_elevations=model.heightfield_elevations,
writer_data=writer_data,
device=self.device,
)
# Differentiable contact augmentation: reconstruct world-space contact
# quantities through body_q so that gradients flow via wp.Tape.
if self.requires_grad and contacts.rigid_contact_diff_distance is not None:
launch_differentiable_contact_augment(
contacts=contacts,
body_q=state.body_q,
shape_body=model.shape_body,
device=self.device,
)
# Generate soft contacts for particles and shapes
particle_count = len(state.particle_q) if state.particle_q else 0
if state.particle_q and model.shape_count > 0:
wp.launch(
kernel=create_soft_contacts,
dim=particle_count * model.shape_count,
inputs=[
state.particle_q,
model.particle_radius,
model.particle_flags,
model.particle_world,
state.body_q,
model.shape_transform,
model.shape_body,
model.shape_type,
model.shape_scale,
model.shape_source_ptr,
model.shape_world,
soft_contact_margin,
self.soft_contact_max,
model.shape_count,
model.shape_flags,
model.shape_heightfield_index,
model.heightfield_data,
model.heightfield_elevations,
],
outputs=[
contacts.soft_contact_count,
contacts.soft_contact_particle,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_body_vel,
contacts.soft_contact_normal,
contacts.soft_contact_tids,
],
device=self.device,
)
================================================
FILE: newton/_src/sim/contacts.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warp as wp
from warp import DeviceLike as Devicelike
class Contacts:
"""
Stores contact information for rigid and soft body collisions, to be consumed by a solver.
This class manages buffers for contact data such as positions, normals, margins, and shape indices
for both rigid-rigid and soft-rigid contacts. The buffers are allocated on the specified device and can
optionally require gradients for differentiable simulation.
.. note::
This class is a temporary solution and its interface may change in the future.
"""
EXTENDED_ATTRIBUTES: frozenset[str] = frozenset(("force",))
"""
Names of optional extended contact attributes that are not allocated by default.
These can be requested via :meth:`newton.ModelBuilder.request_contact_attributes` or
:meth:`newton.Model.request_contact_attributes` before calling :meth:`newton.Model.contacts` or
:meth:`newton.CollisionPipeline.contacts`.
See :ref:`extended_contact_attributes` for details and usage.
"""
@classmethod
def validate_extended_attributes(cls, attributes: tuple[str, ...]) -> None:
"""Validate names passed to request_contact_attributes().
Only extended contact attributes listed in :attr:`EXTENDED_ATTRIBUTES` are accepted.
Args:
attributes: Tuple of attribute names to validate.
Raises:
ValueError: If any attribute name is not in :attr:`EXTENDED_ATTRIBUTES`.
"""
if not attributes:
return
invalid = sorted(set(attributes).difference(cls.EXTENDED_ATTRIBUTES))
if invalid:
allowed = ", ".join(sorted(cls.EXTENDED_ATTRIBUTES))
bad = ", ".join(invalid)
raise ValueError(f"Unknown extended contact attribute(s): {bad}. Allowed: {allowed}.")
def __init__(
self,
rigid_contact_max: int,
soft_contact_max: int,
requires_grad: bool = False,
device: Devicelike = None,
per_contact_shape_properties: bool = False,
clear_buffers: bool = False,
requested_attributes: set[str] | None = None,
):
"""
Initialize Contacts storage.
Args:
rigid_contact_max: Maximum number of rigid contacts
soft_contact_max: Maximum number of soft contacts
requires_grad: Whether contact arrays require gradients for differentiable
simulation. When ``True``, soft contact arrays (body_pos, body_vel, normal)
are allocated with gradients so that gradient-based optimization can flow
through particle-shape contacts, **and** additional differentiable rigid
contact arrays are allocated (``rigid_contact_diff_*``) that provide
first-order gradients of contact distance and world-space points with
respect to body poses.
device: Device to allocate buffers on
per_contact_shape_properties: Enable per-contact stiffness/damping/friction arrays
clear_buffers: If True, clear() will zero all contact buffers (slower but conservative).
If False (default), clear() only resets counts, relying on collision detection
to overwrite active contacts. This is much faster (86-90% fewer kernel launches)
and safe since solvers only read up to contact_count.
requested_attributes: Set of extended contact attribute names to allocate.
See :attr:`EXTENDED_ATTRIBUTES` for available options.
.. note::
The ``rigid_contact_diff_*`` arrays allocated when ``requires_grad=True`` are
**experimental**; see :meth:`newton.CollisionPipeline.collide`.
"""
self.per_contact_shape_properties = per_contact_shape_properties
self.clear_buffers = clear_buffers
with wp.ScopedDevice(device):
# Consolidated counter array to minimize kernel launches for zeroing
# Layout: [rigid_contact_count, soft_contact_count]
self._counter_array = wp.zeros(2, dtype=wp.int32)
# Create sliced views for individual counters (no additional allocation)
self.rigid_contact_count = self._counter_array[0:1]
# rigid contacts — never requires_grad (narrow phase has enable_backward=False)
self.rigid_contact_point_id = wp.zeros(rigid_contact_max, dtype=wp.int32)
self.rigid_contact_shape0 = wp.full(rigid_contact_max, -1, dtype=wp.int32)
self.rigid_contact_shape1 = wp.full(rigid_contact_max, -1, dtype=wp.int32)
self.rigid_contact_point0 = wp.zeros(rigid_contact_max, dtype=wp.vec3)
"""Body-frame contact point on shape 0 [m], shape (rigid_contact_max,), dtype :class:`vec3`."""
self.rigid_contact_point1 = wp.zeros(rigid_contact_max, dtype=wp.vec3)
"""Body-frame contact point on shape 1 [m], shape (rigid_contact_max,), dtype :class:`vec3`."""
self.rigid_contact_offset0 = wp.zeros(rigid_contact_max, dtype=wp.vec3)
"""Body-frame friction anchor offset for shape 0 [m], shape (rigid_contact_max,), dtype :class:`vec3`.
Equal to the contact normal scaled by ``effective_radius + margin`` and
expressed in shape 0's body frame. Combined with
``rigid_contact_point0`` to form a shifted friction anchor that accounts
for rotational effects of finite contact thickness in tangential friction
calculations."""
self.rigid_contact_offset1 = wp.zeros(rigid_contact_max, dtype=wp.vec3)
"""Body-frame friction anchor offset for shape 1 [m], shape (rigid_contact_max,), dtype :class:`vec3`.
Equal to the contact normal scaled by ``effective_radius + margin`` and
expressed in shape 1's body frame. Combined with
``rigid_contact_point1`` to form a shifted friction anchor that accounts
for rotational effects of finite contact thickness in tangential friction
calculations."""
self.rigid_contact_normal = wp.zeros(rigid_contact_max, dtype=wp.vec3)
"""Contact normal pointing from shape 0 toward shape 1 (A-to-B) [unitless], shape (rigid_contact_max,), dtype :class:`vec3`."""
self.rigid_contact_margin0 = wp.zeros(rigid_contact_max, dtype=wp.float32)
"""Surface thickness for shape 0: effective radius + margin [m], shape (rigid_contact_max,), dtype float."""
self.rigid_contact_margin1 = wp.zeros(rigid_contact_max, dtype=wp.float32)
"""Surface thickness for shape 1: effective radius + margin [m], shape (rigid_contact_max,), dtype float."""
self.rigid_contact_tids = wp.full(rigid_contact_max, -1, dtype=wp.int32)
# to be filled by the solver (currently unused)
self.rigid_contact_force = wp.zeros(rigid_contact_max, dtype=wp.vec3)
"""Contact force [N], shape (rigid_contact_max,), dtype :class:`vec3`."""
# Differentiable rigid contact arrays -- only allocated when requires_grad
# is True. Populated by the post-processing kernel in
# :mod:`newton._src.geometry.differentiable_contacts`.
if requires_grad:
self.rigid_contact_diff_distance = wp.zeros(rigid_contact_max, dtype=wp.float32, requires_grad=True)
"""Differentiable signed distance [m], shape (rigid_contact_max,), dtype float."""
self.rigid_contact_diff_normal = wp.zeros(rigid_contact_max, dtype=wp.vec3, requires_grad=False)
"""Contact normal (A-to-B, world frame) [unitless], shape (rigid_contact_max,), dtype :class:`vec3`."""
self.rigid_contact_diff_point0_world = wp.zeros(rigid_contact_max, dtype=wp.vec3, requires_grad=True)
"""World-space contact point on shape 0 [m], shape (rigid_contact_max,), dtype :class:`vec3`."""
self.rigid_contact_diff_point1_world = wp.zeros(rigid_contact_max, dtype=wp.vec3, requires_grad=True)
"""World-space contact point on shape 1 [m], shape (rigid_contact_max,), dtype :class:`vec3`."""
else:
self.rigid_contact_diff_distance = None
"""Differentiable signed distance [m], shape (rigid_contact_max,), dtype float."""
self.rigid_contact_diff_normal = None
"""Contact normal (A-to-B, world frame) [unitless], shape (rigid_contact_max,), dtype :class:`vec3`."""
self.rigid_contact_diff_point0_world = None
"""World-space contact point on shape 0 [m], shape (rigid_contact_max,), dtype :class:`vec3`."""
self.rigid_contact_diff_point1_world = None
"""World-space contact point on shape 1 [m], shape (rigid_contact_max,), dtype :class:`vec3`."""
# contact stiffness/damping/friction (only allocated if per_contact_shape_properties is enabled)
if self.per_contact_shape_properties:
self.rigid_contact_stiffness = wp.zeros(rigid_contact_max, dtype=wp.float32)
"""Per-contact stiffness [N/m], shape (rigid_contact_max,), dtype float."""
self.rigid_contact_damping = wp.zeros(rigid_contact_max, dtype=wp.float32)
"""Per-contact damping [N·s/m], shape (rigid_contact_max,), dtype float."""
self.rigid_contact_friction = wp.zeros(rigid_contact_max, dtype=wp.float32)
"""Per-contact friction coefficient [dimensionless], shape (rigid_contact_max,), dtype float."""
else:
self.rigid_contact_stiffness = None
"""Per-contact stiffness [N/m], shape (rigid_contact_max,), dtype float."""
self.rigid_contact_damping = None
"""Per-contact damping [N·s/m], shape (rigid_contact_max,), dtype float."""
self.rigid_contact_friction = None
"""Per-contact friction coefficient [dimensionless], shape (rigid_contact_max,), dtype float."""
# soft contacts — requires_grad flows through here for differentiable simulation
self.soft_contact_count = self._counter_array[1:2]
self.soft_contact_particle = wp.full(soft_contact_max, -1, dtype=int)
self.soft_contact_shape = wp.full(soft_contact_max, -1, dtype=int)
self.soft_contact_body_pos = wp.zeros(soft_contact_max, dtype=wp.vec3, requires_grad=requires_grad)
"""Contact position on body [m], shape (soft_contact_max,), dtype :class:`vec3`."""
self.soft_contact_body_vel = wp.zeros(soft_contact_max, dtype=wp.vec3, requires_grad=requires_grad)
"""Contact velocity on body [m/s], shape (soft_contact_max,), dtype :class:`vec3`."""
self.soft_contact_normal = wp.zeros(soft_contact_max, dtype=wp.vec3, requires_grad=requires_grad)
"""Contact normal direction [unitless], shape (soft_contact_max,), dtype :class:`vec3`."""
self.soft_contact_tids = wp.full(soft_contact_max, -1, dtype=int)
# Extended contact attributes (optional, allocated on demand)
self.force: wp.array | None = None
"""Contact forces (spatial) [N, N·m], shape (rigid_contact_max + soft_contact_max,), dtype :class:`spatial_vector`.
Force and torque exerted on body0 by body1, referenced to the center of mass (COM) of body0, and in world frame, where body0 and body1 are the bodies of shape0 and shape1.
First three entries: linear force [N]; last three entries: torque (moment) [N·m].
When both rigid and soft contacts are present, soft contact forces follow rigid contact forces.
This is an extended contact attribute; see :ref:`extended_contact_attributes` for more information.
"""
if requested_attributes and "force" in requested_attributes:
total_contacts = rigid_contact_max + soft_contact_max
self.force = wp.zeros(total_contacts, dtype=wp.spatial_vector, requires_grad=requires_grad)
self.requires_grad = requires_grad
self.rigid_contact_max = rigid_contact_max
self.soft_contact_max = soft_contact_max
def clear(self):
"""
Clear contact data, resetting counts and optionally clearing all buffers.
By default (clear_buffers=False), only resets contact counts. This is highly optimized,
requiring just 1 kernel launch. Collision detection overwrites all data up to the new
contact_count, and solvers only read up to count, so clearing stale data is unnecessary.
If clear_buffers=True (conservative mode), performs full buffer clearing with sentinel
values and zeros. This requires 7-10 kernel launches but may be useful for debugging.
"""
# Clear all counters with a single kernel launch (consolidated counter array)
self._counter_array.zero_()
if self.clear_buffers:
# Conservative path: clear all buffers (7-10 kernel launches)
# This is slower but may be useful for debugging or special cases
self.rigid_contact_shape0.fill_(-1)
self.rigid_contact_shape1.fill_(-1)
self.rigid_contact_tids.fill_(-1)
self.rigid_contact_force.zero_()
if self.force is not None:
self.force.zero_()
if self.rigid_contact_diff_distance is not None:
self.rigid_contact_diff_distance.zero_()
self.rigid_contact_diff_normal.zero_()
self.rigid_contact_diff_point0_world.zero_()
self.rigid_contact_diff_point1_world.zero_()
if self.per_contact_shape_properties:
self.rigid_contact_stiffness.zero_()
self.rigid_contact_damping.zero_()
self.rigid_contact_friction.zero_()
self.soft_contact_particle.fill_(-1)
self.soft_contact_shape.fill_(-1)
self.soft_contact_tids.fill_(-1)
# else: Optimized path (default) - only counter clear needed
# Collision detection overwrites all active contacts [0, contact_count)
# Solvers only read [0, contact_count), so stale data is never accessed
@property
def device(self):
"""
Returns the device on which the contact buffers are allocated.
"""
return self.rigid_contact_count.device
================================================
FILE: newton/_src/sim/control.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warp as wp
class Control:
"""Time-varying control data for a :class:`Model`.
Time-varying control data includes joint torques, control inputs, muscle activations,
and activation forces for triangle and tetrahedral elements.
The exact attributes depend on the contents of the model. Control objects
should generally be created using the :func:`newton.Model.control()` function.
"""
def __init__(self):
self.joint_f: wp.array | None = None
"""
Array of generalized joint forces [N or N·m, depending on joint type] with shape ``(joint_dof_count,)``
and type ``float``.
The degrees of freedom for free joints are included in this array and have the same
convention as the :attr:`newton.State.body_f` array where the 6D wrench is defined as
``(f_x, f_y, f_z, t_x, t_y, t_z)``, where ``f_x``, ``f_y``, and ``f_z`` are the components
of the force vector (linear) [N] and ``t_x``, ``t_y``, and ``t_z`` are the
components of the torque vector (angular) [N·m]. For free joints, the wrench is applied in world frame with the
body's center of mass (COM) as reference point.
.. note::
The Featherstone solver currently applies free-joint forces in the body-origin frame instead of the
center-of-mass frame, which can lead to unexpected rotation when applying linear force to a body with a non-zero COM offset.
"""
self.joint_target_pos: wp.array | None = None
"""Per-DOF position targets [m or rad, depending on joint type], shape ``(joint_dof_count,)``, type ``float`` (optional)."""
self.joint_target_vel: wp.array | None = None
"""Per-DOF velocity targets [m/s or rad/s, depending on joint type], shape ``(joint_dof_count,)``, type ``float`` (optional)."""
self.joint_act: wp.array | None = None
"""Per-DOF feedforward actuation input, shape ``(joint_dof_count,)``, type ``float`` (optional).
This is an additive feedforward term used by actuators (e.g. :class:`ActuatorPD`) in their control law
before PD/PID correction is applied.
"""
self.tri_activations: wp.array | None = None
"""Array of triangle element activations [dimensionless] with shape ``(tri_count,)`` and type ``float``."""
self.tet_activations: wp.array | None = None
"""Array of tetrahedral element activations [dimensionless] with shape ``(tet_count,)`` and type ``float``."""
self.muscle_activations: wp.array | None = None
"""
Array of muscle activations [dimensionless, 0 to 1] with shape ``(muscle_count,)`` and type ``float``.
.. note::
Support for muscle dynamics is not yet implemented.
"""
def clear(self) -> None:
"""Reset the control inputs to zero."""
if self.joint_f is not None:
self.joint_f.zero_()
if self.tri_activations is not None:
self.tri_activations.zero_()
if self.tet_activations is not None:
self.tet_activations.zero_()
if self.muscle_activations is not None:
self.muscle_activations.zero_()
if self.joint_target_pos is not None:
self.joint_target_pos.zero_()
if self.joint_target_vel is not None:
self.joint_target_vel.zero_()
if self.joint_act is not None:
self.joint_act.zero_()
self._clear_namespaced_arrays()
def _clear_namespaced_arrays(self) -> None:
"""Clear all wp.array attributes in namespaced containers (e.g., control.mujoco.ctrl)."""
from .model import Model # noqa: PLC0415
for attr in self.__dict__.values():
if isinstance(attr, Model.AttributeNamespace):
for value in attr.__dict__.values():
if isinstance(value, wp.array):
value.zero_()
================================================
FILE: newton/_src/sim/enums.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from enum import IntEnum
# Body flags
class BodyFlags(IntEnum):
"""
Per-body dynamic state flags.
Each body must store exactly one runtime state flag:
:attr:`DYNAMIC` or :attr:`KINEMATIC`. :attr:`ALL` is a convenience
filter mask for APIs such as :func:`newton.eval_fk` and is not a valid
stored body state.
"""
DYNAMIC = 1 << 0
"""Dynamic body that participates in simulation dynamics."""
KINEMATIC = 1 << 1
"""User-prescribed body that does not respond to applied forces."""
ALL = DYNAMIC | KINEMATIC
"""Filter bitmask selecting both dynamic and kinematic bodies."""
# Types of joints linking rigid bodies
class JointType(IntEnum):
"""
Enumeration of joint types supported in Newton.
"""
PRISMATIC = 0
"""Prismatic joint: allows translation along a single axis (1 DoF)."""
REVOLUTE = 1
"""Revolute joint: allows rotation about a single axis (1 DoF)."""
BALL = 2
"""Ball joint: allows rotation about all three axes (3 DoF, quaternion parameterization)."""
FIXED = 3
"""Fixed joint: locks all relative motion (0 DoF)."""
FREE = 4
"""Free joint: allows full 6-DoF motion (translation and rotation, 7 coordinates)."""
DISTANCE = 5
"""Distance joint: keeps two bodies at a distance within its joint limits (6 DoF, 7 coordinates)."""
D6 = 6
"""6-DoF joint: Generic joint with up to 3 translational and 3 rotational degrees of freedom."""
CABLE = 7
"""Cable joint: one linear (stretch) and one angular (isotropic bend/twist) DoF."""
def dof_count(self, num_axes: int) -> tuple[int, int]:
"""
Returns the number of degrees of freedom (DoF) in velocity and the number of coordinates
in position for this joint type.
Args:
num_axes (int): The number of axes for the joint.
Returns:
tuple[int, int]: A tuple (dof_count, coord_count) where:
- dof_count: Number of velocity degrees of freedom for the joint.
- coord_count: Number of position coordinates for the joint.
Notes:
- For PRISMATIC and REVOLUTE joints, both values are 1 (single axis).
- For BALL joints, dof_count is 3 (angular velocity), coord_count is 4 (quaternion).
- For FREE and DISTANCE joints, dof_count is 6 (3 translation + 3 rotation), coord_count is 7 (3 position + 4 quaternion).
- For FIXED joints, both values are 0.
"""
dof_count = num_axes
coord_count = num_axes
if self == JointType.BALL:
dof_count = 3
coord_count = 4
elif self == JointType.FREE or self == JointType.DISTANCE:
dof_count = 6
coord_count = 7
elif self == JointType.FIXED:
dof_count = 0
coord_count = 0
return dof_count, coord_count
def constraint_count(self, num_axes: int) -> int:
"""
Returns the number of velocity-level bilateral kinematic constraints for this joint type.
Args:
num_axes (int): The number of DoF axes for the joint.
Returns:
int: The number of bilateral kinematic constraints for the joint.
Notes:
- For PRISMATIC and REVOLUTE joints, this equals 5 (single DoF axis).
- For FREE and DISTANCE joints, `cts_count = 0` since it yields no constraints.
- For FIXED joints, `cts_count = 6` since it fully constrains the associated bodies.
"""
cts_count = 6 - num_axes
if self == JointType.BALL:
cts_count = 3
elif self == JointType.FREE or self == JointType.DISTANCE:
cts_count = 0
elif self == JointType.FIXED:
cts_count = 6
return cts_count
# (temporary) equality constraint types
class EqType(IntEnum):
"""
Enumeration of equality constraint types between bodies or joints.
Note:
This is a temporary solution and the interface may change in the future.
"""
CONNECT = 0
"""Constrains two bodies at a point (like a ball joint)."""
WELD = 1
"""Welds two bodies together (like a fixed joint)."""
JOINT = 2
"""Constrains the position or angle of one joint to be a quartic polynomial of another joint (like a prismatic or revolute joint)."""
class JointTargetMode(IntEnum):
"""
Enumeration of actuator modes for joint degrees of freedom.
This enum manages UsdPhysics compliance by specifying whether joint_target_pos/vel
inputs are active for a given DOF. It determines which actuators are installed when
using solvers that require explicit actuator definitions (e.g., MuJoCo solver).
Note:
MuJoCo general actuators (motor, general, etc.) are handled separately via
custom attributes with "mujoco:actuator" frequency and control.mujoco.ctrl,
not through this enum.
"""
NONE = 0
"""No actuators are installed for this DOF. The joint is passive/unactuated."""
POSITION = 1
"""Only a position actuator is installed for this DOF. Tracks joint_target_pos."""
VELOCITY = 2
"""Only a velocity actuator is installed for this DOF. Tracks joint_target_vel."""
POSITION_VELOCITY = 3
"""Both position and velocity actuators are installed. Tracks both joint_target_pos and joint_target_vel."""
EFFORT = 4
"""A drive is applied but no gains are configured. No MuJoCo actuator is created for this DOF.
The user is expected to supply force via joint_f."""
@staticmethod
def from_gains(
target_ke: float,
target_kd: float,
force_position_velocity: bool = False,
has_drive: bool = False,
) -> "JointTargetMode":
"""Infer actuator mode from position and velocity gains.
Args:
target_ke: Position gain (stiffness).
target_kd: Velocity gain (damping).
force_position_velocity: If True and both gains are non-zero,
forces POSITION_VELOCITY mode instead of just POSITION.
has_drive: If True, a drive/actuator is applied to the joint.
When True but both gains are 0, returns EFFORT mode.
When False, returns NONE regardless of gains.
Returns:
The inferred JointTargetMode based on which gains are non-zero:
- NONE: No drive applied
- EFFORT: Drive applied but both gains are 0 (direct torque control)
- POSITION: Only position gain is non-zero
- VELOCITY: Only velocity gain is non-zero
- POSITION_VELOCITY: Both gains non-zero (or forced)
"""
if not has_drive:
return JointTargetMode.NONE
if force_position_velocity and (target_ke != 0.0 and target_kd != 0.0):
return JointTargetMode.POSITION_VELOCITY
elif target_ke != 0.0:
return JointTargetMode.POSITION
elif target_kd != 0.0:
return JointTargetMode.VELOCITY
else:
return JointTargetMode.EFFORT
__all__ = [
"BodyFlags",
"EqType",
"JointTargetMode",
"JointType",
]
================================================
FILE: newton/_src/sim/graph_coloring.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warnings
from enum import Enum
from typing import Literal
import numpy as np
import warp as wp
class ColoringAlgorithm(Enum):
MCS = 0
GREEDY = 1
@wp.kernel
def validate_graph_coloring(edge_indices: wp.array2d[int], colors: wp.array[int]):
edge_idx = wp.tid()
e_v_1 = edge_indices[edge_idx, 0]
e_v_2 = edge_indices[edge_idx, 1]
wp.expect_neq(colors[e_v_1], colors[e_v_2])
@wp.kernel
def count_color_group_size(
colors: wp.array[int],
group_sizes: wp.array[int],
):
for particle_idx in range(colors.shape[0]):
particle_color = colors[particle_idx]
group_sizes[particle_color] = group_sizes[particle_color] + 1
@wp.kernel
def fill_color_groups(
colors: wp.array[int],
group_fill_count: wp.array[int],
group_offsets: wp.array[int],
# flattened color groups
color_groups_flatten: wp.array[int],
):
for particle_idx in range(colors.shape[0]):
particle_color = colors[particle_idx]
group_offset = group_offsets[particle_color]
group_idx = group_fill_count[particle_color]
color_groups_flatten[group_idx + group_offset] = wp.int32(particle_idx)
group_fill_count[particle_color] = group_idx + 1
def convert_to_color_groups(num_colors, particle_colors, return_wp_array=False, device="cpu") -> list[int]:
group_sizes = wp.zeros(shape=(num_colors,), dtype=int, device="cpu")
wp.launch(kernel=count_color_group_size, inputs=[particle_colors, group_sizes], device="cpu", dim=1)
group_sizes_np = group_sizes.numpy()
group_offsets_np = np.concatenate([np.array([0]), np.cumsum(group_sizes_np)])
group_offsets = wp.array(group_offsets_np, dtype=int, device="cpu")
group_fill_count = wp.zeros(shape=(num_colors,), dtype=int, device="cpu")
color_groups_flatten = wp.empty(shape=(group_sizes_np.sum(),), dtype=int, device="cpu")
wp.launch(
kernel=fill_color_groups,
inputs=[particle_colors, group_fill_count, group_offsets, color_groups_flatten],
device="cpu",
dim=1,
)
color_groups_flatten_np = color_groups_flatten.numpy()
color_groups = []
if return_wp_array:
for color_idx in range(num_colors):
color_groups.append(
wp.array(
color_groups_flatten_np[group_offsets_np[color_idx] : group_offsets_np[color_idx + 1]],
dtype=int,
device=device,
)
)
else:
for color_idx in range(num_colors):
color_groups.append(color_groups_flatten_np[group_offsets_np[color_idx] : group_offsets_np[color_idx + 1]])
return color_groups
def _canonicalize_edges_np(edges_np: np.ndarray) -> np.ndarray:
"""Sort edge endpoints and drop duplicate edges."""
if edges_np.size == 0:
return np.empty((0, 2), dtype=np.int32)
edges_sorted = np.sort(edges_np, axis=1)
edges_unique = np.unique(edges_sorted, axis=0)
return edges_unique.astype(np.int32)
def construct_tetmesh_graph_edges(tet_indices: np.array, tet_active_mask):
"""
Convert tet connectivity (n_tets x 4) into unique graph edges (u, v).
"""
if tet_indices is None:
edges_np = np.empty((0, 2), dtype=np.int32)
else:
if isinstance(tet_indices, wp.array):
tet_np = tet_indices.numpy()
else:
tet_np = np.asarray(tet_indices, dtype=np.int32)
if tet_active_mask is not None:
mask_arr = np.asarray(tet_active_mask, dtype=bool)
# Handle scalar mask (True means all active, False means none active)
if mask_arr.ndim == 0:
if not mask_arr:
tet_np = tet_np[:0] # Empty array
# else: all active, no filtering needed
else:
tet_np = tet_np[mask_arr]
if tet_np.size == 0:
edges_np = np.empty((0, 2), dtype=np.int32)
else:
v0 = tet_np[:, 0]
v1 = tet_np[:, 1]
v2 = tet_np[:, 2]
v3 = tet_np[:, 3]
edges_np = np.stack(
[
np.stack([v0, v1], axis=1),
np.stack([v0, v2], axis=1),
np.stack([v0, v3], axis=1),
np.stack([v1, v2], axis=1),
np.stack([v1, v3], axis=1),
np.stack([v2, v3], axis=1),
],
axis=0,
).reshape(-1, 2)
edges_np = _canonicalize_edges_np(edges_np)
return edges_np
def construct_trimesh_graph_edges(
tri_indices,
tri_active_mask=None,
bending_edge_indices=None,
bending_edge_active_mask=None,
return_wp_array=True,
):
"""
A function that generates vertex coloring for a trimesh, which is represented by the number of vertices and edges of the mesh.
It will convert the trimesh to a graph and then apply coloring.
It returns a list of `np.array` with `dtype`=`int`. The length of the list is the number of colors
and each `np.array` contains the indices of vertices with this color.
Args:
num_nodes: The number of the nodes in the graph
trimesh_edge_indices: A `wp.array` with of shape (number_edges, 4), each row is (o1, o2, v1, v2), see `sim.Model`'s definition of `edge_indices`.
include_bending_energy: whether to consider bending energy in the coloring process. If set to `True`, the generated
graph will contain all the edges connecting o1 and o2; otherwise, the graph will be equivalent to the trimesh.
"""
edges_np_list = []
# Primary triangle edges
if tri_indices is not None:
if isinstance(tri_indices, wp.array):
tri_indices = tri_indices.numpy()
if tri_indices.size > 0:
if tri_active_mask is not None:
mask_arr = np.asarray(tri_active_mask, dtype=bool)
# Handle scalar mask (True means all active, False means none active)
if mask_arr.ndim == 0:
if not mask_arr:
tri_indices = tri_indices[:0] # Empty array
# else: all active, no filtering needed
else:
tri_indices = tri_indices[mask_arr]
if tri_indices.size > 0:
v0 = tri_indices[:, 0]
v1 = tri_indices[:, 1]
v2 = tri_indices[:, 2]
tri_edges = np.stack(
[
np.stack([v0, v1], axis=1),
np.stack([v0, v2], axis=1),
np.stack([v1, v2], axis=1),
],
axis=0,
).reshape(-1, 2)
edges_np_list.append(tri_edges)
# Optional bending edges (hinges). Each row has four vertices; include all 2-combinations
# of the active hinge vertices, skipping any vertex indices that are negative.
if bending_edge_indices is not None:
bend_np = np.asarray(bending_edge_indices, dtype=np.int32)
if bend_np.size > 0:
if bending_edge_active_mask is not None:
mask_arr = np.asarray(bending_edge_active_mask, dtype=bool)
# Handle scalar mask (True means all active, False means none active)
if mask_arr.ndim == 0:
if not mask_arr:
bend_np = bend_np[:0] # Empty array
# else: all active, no filtering needed
else:
bend_np = bend_np[mask_arr]
if bend_np.size > 0:
v0 = bend_np[:, 0:1]
v1 = bend_np[:, 1:2]
v2 = bend_np[:, 2:3]
v3 = bend_np[:, 3:4]
pairs = np.concatenate(
[
np.concatenate([v0, v1], axis=1),
np.concatenate([v0, v2], axis=1),
np.concatenate([v0, v3], axis=1),
np.concatenate([v1, v2], axis=1),
np.concatenate([v1, v3], axis=1),
np.concatenate([v2, v3], axis=1),
],
axis=0,
)
valid = np.all(pairs >= 0, axis=1)
pairs = pairs[valid]
if pairs.size > 0:
edges_np_list.append(pairs)
if edges_np_list:
edges_np = np.concatenate(edges_np_list, axis=0)
else:
edges_np = np.empty((0, 2), dtype=np.int32)
edges = _canonicalize_edges_np(edges_np)
if return_wp_array:
edges = wp.array(edges, dtype=int, device="cpu")
return edges
def construct_particle_graph(
tri_graph_edges: np.array,
tri_active_mask: np.array,
bending_edge_indices: np.array,
bending_edge_active_mask: np.array,
tet_graph_edges_np: np.array,
tet_active_mask: np.array,
):
"""Construct unified particle graph edges from triangular and tetrahedral meshes.
Combines triangle mesh edges (including optional bending edges) with tetrahedral
mesh edges into a single unified graph for particle coloring. The resulting graph
represents all constraints between particles that must be considered during
parallel Gauss-Seidel iteration.
Args:
tri_graph_edges: Triangle mesh indices (N_tris x 3) or None.
tri_active_mask: Boolean mask indicating which triangles are active, or None.
bending_edge_indices: Bending edge indices (N_edges x 4) with structure [o1, o2, v1, v2] per row,
where o1, o2 are opposite vertices and v1, v2 are hinge edge vertices, or None.
bending_edge_active_mask: Boolean mask indicating which bending edges are active, or None.
tet_graph_edges_np: Tetrahedral mesh indices (N_tets x 4) or None.
tet_active_mask: Boolean mask indicating which tetrahedra are active, or None.
Returns:
wp.array: Canonicalized graph edges (N_edges x 2) as a warp array on CPU,
where each row [i, j] represents an edge with i < j.
"""
tri_graph_edges = construct_trimesh_graph_edges(
tri_graph_edges,
tri_active_mask,
bending_edge_indices,
bending_edge_active_mask,
return_wp_array=False,
)
tet_graph_edges = construct_tetmesh_graph_edges(tet_graph_edges_np, tet_active_mask)
merged_edges = _canonicalize_edges_np(np.vstack([tri_graph_edges, tet_graph_edges]).astype(np.int32))
graph_edge_indices = wp.array(merged_edges, dtype=int, device="cpu")
return graph_edge_indices
def color_graph(
num_nodes,
graph_edge_indices: wp.array2d[int],
balance_colors: bool = True,
target_max_min_color_ratio: float = 1.1,
algorithm: ColoringAlgorithm = ColoringAlgorithm.MCS,
) -> list[int]:
"""
A function that generates coloring for a graph, which is represented by the number of nodes and an array of edges.
It returns a list of `np.array` with `dtype`=`int`. The length of the list is the number of colors
and each `np.array` contains the indices of vertices with this color.
Args:
num_nodes: The number of the nodes in the graph
graph_edge_indices: A `wp.array` of shape (number_edges, 2)
balance_colors: Whether to apply the color balancing algorithm to balance the size of each color
target_max_min_color_ratio: the color balancing algorithm will stop when the ratio between the largest color and
the smallest color reaches this value
algorithm: Value should an enum type of ColoringAlgorithm, otherwise it will raise an error. ColoringAlgorithm.mcs means using the MCS coloring algorithm,
while ColoringAlgorithm.ordered_greedy means using the degree-ordered greedy algorithm. The MCS algorithm typically generates 30% to 50% fewer colors
compared to the ordered greedy algorithm, while maintaining the same linear complexity. Although MCS has a constant overhead that makes it about twice
as slow as the greedy algorithm, it produces significantly better coloring results. We recommend using MCS, especially if coloring is only part of the
preprocessing stage.e.
Note:
References to the coloring algorithm:
MCS: Pereira, F. M. Q., & Palsberg, J. (2005, November). Register allocation via coloring of chordal graphs. In Asian Symposium on Programming Languages and Systems (pp. 315-329). Berlin, Heidelberg: Springer Berlin Heidelberg.
Ordered Greedy: Ton-That, Q. M., Kry, P. G., & Andrews, S. (2023). Parallel block Neo-Hookean XPBD using graph clustering. Computers & Graphics, 110, 1-10.
"""
if num_nodes == 0:
return []
particle_colors = wp.empty(shape=(num_nodes), dtype=wp.int32, device="cpu")
if graph_edge_indices.ndim != 2:
raise ValueError(
f"graph_edge_indices must be a 2 dimensional array! The provided one is {graph_edge_indices.ndim} dimensional."
)
if graph_edge_indices.device.is_cpu:
indices = graph_edge_indices
else:
indices = wp.clone(graph_edge_indices, device="cpu")
num_colors = wp._src.context.runtime.core.wp_graph_coloring(
num_nodes,
indices.__ctype__(),
algorithm.value,
particle_colors.__ctype__(),
)
if balance_colors:
max_min_ratio = wp._src.context.runtime.core.wp_balance_coloring(
num_nodes,
indices.__ctype__(),
num_colors,
target_max_min_color_ratio,
particle_colors.__ctype__(),
)
if max_min_ratio > target_max_min_color_ratio and wp.config.verbose:
warnings.warn(
f"Color balancing terminated early: max/min ratio {max_min_ratio:.3f} "
f"exceeds target {target_max_min_color_ratio:.3f}. "
"The graph may not be further optimizable.",
stacklevel=2,
)
color_groups = convert_to_color_groups(num_colors, particle_colors, return_wp_array=False)
return color_groups
def plot_graph(
vertices,
edges,
edge_labels=None,
node_labels=None,
node_colors=None,
layout: Literal["spring", "kamada_kawai"] = "kamada_kawai",
):
"""
Plots a graph using matplotlib and networkx.
Args:
vertices: A numpy array of shape (N,) containing the vertex indices.
edges: A numpy array of shape (M, 2) containing the vertex indices of the edges.
edge_labels: A list of edge labels.
node_labels: A list of node labels.
node_colors: A list of node colors.
layout: The layout of the graph. Can be "spring" or "kamada_kawai".
"""
import matplotlib.pyplot as plt
import networkx as nx
if edge_labels is None:
edge_labels = []
G = nx.DiGraph()
name_to_index = {}
for i, name in enumerate(vertices):
G.add_node(i)
name_to_index[name] = i
g_edge_labels = {}
for i, (a, b) in enumerate(edges):
ai = a if isinstance(a, int) else name_to_index[a]
bi = b if isinstance(b, int) else name_to_index[b]
label = None
if i < len(edge_labels):
label = edge_labels[i]
g_edge_labels[(ai, bi)] = label
G.add_edge(ai, bi, label=label)
if layout == "spring":
pos = nx.spring_layout(G, k=3.5, iterations=200)
elif layout == "kamada_kawai":
pos = nx.kamada_kawai_layout(G)
else:
raise ValueError(f"Invalid layout: {layout}")
default_draw_args = {"alpha": 0.9, "edgecolors": "black", "linewidths": 0.5}
nx.draw_networkx_nodes(G, pos, node_color=node_colors, **default_draw_args)
nx.draw_networkx_labels(
G,
pos,
labels=dict(enumerate(node_labels if node_labels is not None else vertices)),
font_size=8,
bbox={"facecolor": "white", "alpha": 0.8, "edgecolor": "none", "pad": 0.5},
)
nx.draw_networkx_edges(G, pos, edgelist=G.edges(), arrows=True, edge_color="black", node_size=1000)
nx.draw_networkx_edge_labels(
G,
pos,
edge_labels=g_edge_labels,
font_color="darkslategray",
font_size=8,
)
plt.axis("off")
plt.show()
def combine_independent_particle_coloring(color_groups_1, color_groups_2) -> list[int]:
"""
A function that combines 2 independent coloring groups. Note that color_groups_1 and color_groups_2 must be from 2 independent
graphs so that there is no connection between them. This algorithm will sort color_groups_1 in ascending order and
sort color_groups_2 in descending order, and combine each group with the same index, this way we are always combining
the smaller group with the larger group.
Args:
color_groups_1: A list of `np.array` with `dtype`=`int`. The length of the list is the number of colors
and each `np.array` contains the indices of vertices with this color.
color_groups_2: A list of `np.array` with `dtype`=`int`. The length of the list is the number of colors
and each `np.array` contains the indices of vertices with this color.
"""
if len(color_groups_1) == 0:
return color_groups_2
if len(color_groups_2) == 0:
return color_groups_1
num_colors_after_combining = max(len(color_groups_1), len(color_groups_2))
color_groups_combined = []
# this made sure that the leftover groups are always the largest
if len(color_groups_1) < len(color_groups_2):
color_groups_1, color_groups_2 = color_groups_2, color_groups_1
# sort group 1 in ascending order
color_groups_1_sorted = sorted(color_groups_1, key=lambda group: len(group))
# sort group 1 in descending order
color_groups_2_sorted = sorted(color_groups_2, key=lambda group: -len(group))
# so that we are combining the smaller group with the larger group
# which will balance the load of each group
for i in range(num_colors_after_combining):
group_1 = color_groups_1_sorted[i] if i < len(color_groups_1) else None
group_2 = color_groups_2_sorted[i] if i < len(color_groups_2) else None
if group_1 is not None and group_2 is not None:
color_groups_combined.append(np.concatenate([group_1, group_2]))
elif group_1 is not None:
color_groups_combined.append(group_1)
else:
color_groups_combined.append(group_2)
return color_groups_combined
def color_rigid_bodies(
num_bodies: int,
joint_parent: list[int],
joint_child: list[int],
balance_colors: bool = True,
target_max_min_color_ratio: float = 1.1,
algorithm: ColoringAlgorithm = ColoringAlgorithm.MCS,
):
"""
Generate a graph coloring for rigid bodies from joint connectivity.
Bodies connected by a joint are treated as adjacent in the graph and cannot share
the same color. The result can be used to schedule per-color parallel processing
(e.g. in the VBD solver) without conflicts.
Returns a list of ``np.ndarray`` with ``dtype=int``. The list length is the number
of colors, and each array contains the body indices of that color. This mirrors the
return format of ``color_trimesh``/``color_graph``.
Args:
num_bodies: Number of bodies (graph nodes).
joint_parent: Parent body indices for each joint (use -1 for world).
joint_child: Child body indices for each joint.
balance_colors: Whether to balance color group sizes.
target_max_min_color_ratio: Stop balancing when max/min group size ratio reaches this value.
algorithm: Coloring algorithm to use.
"""
if num_bodies == 0:
return []
# Build edge list from joint connections
edge_list = []
if len(joint_parent) != len(joint_child):
raise ValueError(
f"joint_parent and joint_child must have the same length (got {len(joint_parent)} and {len(joint_child)})"
)
for parent, child in zip(joint_parent, joint_child, strict=True):
if parent != -1 and child != -1 and parent != child:
edge_list.append([parent, child])
if not edge_list:
# No joints between bodies, all can have same color
return [np.arange(num_bodies, dtype=int)]
# Convert to numpy array for processing
edge_indices = np.array(edge_list, dtype=int)
# Convert to warp array for the existing color_graph function
edge_indices_wp = wp.array(edge_indices, dtype=int, device="cpu")
# Use existing color_graph function
color_groups = color_graph(num_bodies, edge_indices_wp, balance_colors, target_max_min_color_ratio, algorithm)
return color_groups
================================================
FILE: newton/_src/sim/ik/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Inverse-kinematics submodule."""
from .ik_common import IKJacobianType
from .ik_lbfgs_optimizer import IKOptimizerLBFGS
from .ik_lm_optimizer import IKOptimizerLM
from .ik_objectives import IKObjective, IKObjectiveJointLimit, IKObjectivePosition, IKObjectiveRotation
from .ik_solver import IKOptimizer, IKSampler, IKSolver
__all__ = [
"IKJacobianType",
"IKObjective",
"IKObjectiveJointLimit",
"IKObjectivePosition",
"IKObjectiveRotation",
"IKOptimizer",
"IKOptimizerLBFGS",
"IKOptimizerLM",
"IKSampler",
"IKSolver",
]
================================================
FILE: newton/_src/sim/ik/ik_common.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Common enums and utility kernels shared across IK components."""
from __future__ import annotations
from enum import Enum
import warp as wp
from ..articulation import eval_single_articulation_fk
from ..enums import BodyFlags
class IKJacobianType(Enum):
"""
Specifies the backend used for Jacobian computation in inverse kinematics.
"""
AUTODIFF = "autodiff"
"""Use Warp's reverse-mode autodiff for every objective."""
ANALYTIC = "analytic"
"""Use analytic Jacobians for objectives that support them."""
MIXED = "mixed"
"""Use analytic Jacobians where available, otherwise use autodiff."""
@wp.kernel
def _eval_fk_articulation_batched(
articulation_start: wp.array[wp.int32],
joint_articulation: wp.array[int],
joint_q: wp.array2d[wp.float32],
joint_qd: wp.array2d[wp.float32],
joint_q_start: wp.array[wp.int32],
joint_qd_start: wp.array[wp.int32],
joint_type: wp.array[wp.int32],
joint_parent: wp.array[wp.int32],
joint_child: wp.array[wp.int32],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[wp.int32],
body_com: wp.array[wp.vec3],
body_flags: wp.array[wp.int32],
body_q: wp.array2d[wp.transform],
body_qd: wp.array2d[wp.spatial_vector],
):
problem_idx, articulation_idx = wp.tid()
joint_start = articulation_start[articulation_idx]
joint_end = articulation_start[articulation_idx + 1]
eval_single_articulation_fk(
joint_start,
joint_end,
joint_articulation,
joint_q[problem_idx],
joint_qd[problem_idx],
joint_q_start,
joint_qd_start,
joint_type,
joint_parent,
joint_child,
joint_X_p,
joint_X_c,
joint_axis,
joint_dof_dim,
body_com,
body_flags,
int(BodyFlags.ALL),
body_q[problem_idx],
body_qd[problem_idx],
)
def eval_fk_batched(model, joint_q, joint_qd, body_q, body_qd):
"""Compute batched forward kinematics for a set of articulations."""
n_problems = joint_q.shape[0]
wp.launch(
kernel=_eval_fk_articulation_batched,
dim=[n_problems, model.articulation_count],
inputs=[
model.articulation_start,
model.joint_articulation,
joint_q,
joint_qd,
model.joint_q_start,
model.joint_qd_start,
model.joint_type,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_axis,
model.joint_dof_dim,
model.body_com,
model.body_flags,
],
outputs=[body_q, body_qd],
device=model.device,
)
@wp.kernel
def fk_accum(
joint_parent: wp.array[wp.int32],
X_local: wp.array2d[wp.transform],
body_q: wp.array2d[wp.transform],
):
problem_idx, local_joint_idx = wp.tid()
Xw = X_local[problem_idx, local_joint_idx]
parent = joint_parent[local_joint_idx]
while parent >= 0:
Xw = X_local[problem_idx, parent] * Xw
parent = joint_parent[parent]
body_q[problem_idx, local_joint_idx] = Xw
@wp.kernel
def compute_costs(
residuals: wp.array2d[wp.float32],
num_residuals: int,
costs: wp.array[wp.float32],
):
problem_idx = wp.tid()
cost = float(0.0)
for i in range(num_residuals):
r = residuals[problem_idx, i]
cost += r * r
costs[problem_idx] = cost
================================================
FILE: newton/_src/sim/ik/ik_lbfgs_optimizer.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""L-BFGS optimizer backend for inverse kinematics."""
from __future__ import annotations
from collections.abc import Callable, Sequence
from dataclasses import dataclass
from typing import Any, ClassVar
import numpy as np
import warp as wp
from ..model import Model
from .ik_common import IKJacobianType, compute_costs, eval_fk_batched, fk_accum
from .ik_objectives import IKObjective
@wp.kernel
def _scale_negate(
src: wp.array2d[wp.float32], # (n_batch, n_dofs)
scale: float,
# outputs
dst: wp.array2d[wp.float32], # (n_batch, n_dofs)
):
row, dof_idx = wp.tid()
dst[row, dof_idx] = -scale * src[row, dof_idx]
@wp.kernel
def _fan_out_problem_idx(
batch_problem_idx: wp.array[wp.int32],
out_indices: wp.array2d[wp.int32],
):
row_idx, candidate_idx = wp.tid()
out_indices[row_idx, candidate_idx] = batch_problem_idx[row_idx]
@wp.kernel
def _generate_candidates_velocity(
joint_q: wp.array2d[wp.float32], # (n_batch, n_coords)
search_direction: wp.array2d[wp.float32], # (n_batch, n_dofs)
line_search_alphas: wp.array[wp.float32], # (n_steps)
# outputs
candidate_q: wp.array3d[wp.float32], # (n_batch, n_steps, n_coords)
candidate_dq: wp.array3d[wp.float32], # (n_batch, n_steps, n_dofs)
):
row, step_idx = wp.tid()
alpha = line_search_alphas[step_idx]
n_coords = joint_q.shape[1]
for coord in range(n_coords):
candidate_q[row, step_idx, coord] = joint_q[row, coord]
n_dofs = search_direction.shape[1]
for dof in range(n_dofs):
candidate_dq[row, step_idx, dof] = alpha * search_direction[row, dof]
@wp.kernel
def _apply_residual_mask(
residuals: wp.array2d[wp.float32],
mask: wp.array2d[wp.float32],
seeds_out: wp.array2d[wp.float32],
):
row, residual_idx = wp.tid()
seeds_out[row, residual_idx] = residuals[row, residual_idx] * mask[row, residual_idx]
@wp.kernel
def _accumulate_gradients(
base_grad: wp.array2d[wp.float32],
add_grad: wp.array2d[wp.float32],
):
row, dof_idx = wp.tid()
base_grad[row, dof_idx] += add_grad[row, dof_idx]
@dataclass(slots=True)
class BatchCtx:
joint_q: wp.array2d[wp.float32]
residuals: wp.array2d[wp.float32]
fk_body_q: wp.array2d[wp.transform]
problem_idx: wp.array[wp.int32]
# AUTODIFF and MIXED
fk_body_qd: wp.array2d[wp.spatial_vector] | None = None
dq_dof: wp.array2d[wp.float32] | None = None
joint_q_proposed: wp.array2d[wp.float32] | None = None
joint_qd: wp.array2d[wp.float32] | None = None
# ANALYTIC and MIXED
jacobian_out: wp.array3d[wp.float32] | None = None
motion_subspace: wp.array2d[wp.spatial_vector] | None = None
fk_qd_zero: wp.array2d[wp.float32] | None = None
fk_X_local: wp.array2d[wp.transform] | None = None
# MIXED-only helpers
gradient_tmp: wp.array2d[wp.float32] | None = None
autodiff_mask: wp.array2d[wp.float32] | None = None
autodiff_seed: wp.array2d[wp.float32] | None = None
class IKOptimizerLBFGS:
"""L-BFGS optimizer for batched inverse kinematics.
The optimizer maintains a limited-memory quasi-Newton approximation and
chooses step sizes with a parallel strong-Wolfe line search. It supports
the same Jacobian backends as :class:`~newton.ik.IKOptimizerLM`.
Args:
model: Shared articulation model.
n_batch: Number of evaluation rows solved in parallel. This is
typically ``n_problems * n_seeds`` after any sampling expansion.
objectives: Ordered IK objectives applied to every batch row.
jacobian_mode: Jacobian backend to use.
history_len: Number of ``(s, y)`` correction pairs retained in the
L-BFGS history.
h0_scale: Scalar used for the initial inverse-Hessian
approximation.
line_search_alphas: Candidate step sizes tested in parallel during
the line search.
wolfe_c1: Armijo sufficient-decrease constant.
wolfe_c2: Strong-Wolfe curvature constant.
problem_idx: Optional mapping from batch rows to base problem indices
for per-problem objective data.
"""
TILE_N_DOFS = None
TILE_N_RESIDUALS = None
TILE_HISTORY_LEN = None
TILE_N_LINE_STEPS = None
_cache: ClassVar[dict[tuple[int, int, int, int, str], type]] = {}
def __new__(
cls,
model: Model,
n_batch: int,
objectives: Sequence[IKObjective],
*a: Any,
**kw: Any,
) -> IKOptimizerLBFGS:
n_dofs = model.joint_dof_count
n_residuals = sum(o.residual_dim() for o in objectives)
history_len = kw.get("history_len", 10)
alphas = kw.get("line_search_alphas") or [0.1, 0.2, 0.5, 0.8, 1.0, 1.5, 2.0, 3.0]
n_line_search = len(alphas)
arch = model.device.arch
key = (n_dofs, n_residuals, history_len, n_line_search, arch)
spec_cls = cls._cache.get(key)
if spec_cls is None:
spec_cls = cls._build_specialized(key)
cls._cache[key] = spec_cls
return super().__new__(spec_cls)
def __init__(
self,
model: Model,
n_batch: int,
objectives: Sequence[IKObjective],
jacobian_mode: IKJacobianType = IKJacobianType.AUTODIFF,
history_len: int = 10,
h0_scale: float = 1.0,
line_search_alphas: Sequence[float] | None = None,
wolfe_c1: float = 1e-4,
wolfe_c2: float = 0.9,
*,
problem_idx: wp.array[wp.int32] | None = None,
) -> None:
if line_search_alphas is None:
line_search_alphas = [0.1, 0.2, 0.5, 0.8, 1.0, 1.5, 2.0, 3.0]
self.model = model
self.device = model.device
self.n_batch = n_batch
self.n_coords = model.joint_coord_count
self.n_dofs = model.joint_dof_count
self.n_residuals = sum(o.residual_dim() for o in objectives)
self.history_len = history_len
self.n_line_search = len(line_search_alphas)
self.h0_scale = h0_scale
self.wolfe_c1 = wolfe_c1
self.wolfe_c2 = wolfe_c2
self.objectives = objectives
self.jacobian_mode = jacobian_mode
if self.TILE_N_DOFS is not None:
assert self.n_dofs == self.TILE_N_DOFS
if self.TILE_N_RESIDUALS is not None:
assert self.n_residuals == self.TILE_N_RESIDUALS
if self.TILE_HISTORY_LEN is not None:
assert self.history_len == self.TILE_HISTORY_LEN
if self.TILE_N_LINE_STEPS is not None:
assert self.n_line_search == self.TILE_N_LINE_STEPS
grad = jacobian_mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED)
self.has_analytic_objective = any(o.supports_analytic() for o in objectives)
self.has_autodiff_objective = any(not o.supports_analytic() for o in objectives)
self._alloc_solver_buffers(grad)
self.problem_idx = problem_idx if problem_idx is not None else self.problem_idx_identity
self._alloc_line_search_buffers(grad, line_search_alphas)
self.tape = wp.Tape() if grad else None
self._build_residual_offsets()
if self.jacobian_mode != IKJacobianType.AUTODIFF:
self._alloc_line_search_analytic_buffers()
else:
self.candidate_jacobians = None
self.cand_joint_S_s = None
self.cand_X_local = None
if self.jacobian_mode == IKJacobianType.MIXED:
self._alloc_mixed_buffers()
else:
self.gradient_tmp = None
self.candidate_gradient_tmp = None
self.autodiff_residual_mask = None
self.autodiff_residual_seed = None
self.autodiff_residual_mask_candidates = None
self.candidate_autodiff_residual_grads = None
self._init_objectives()
self._init_cuda_streams()
def _alloc_solver_buffers(self, grad: bool) -> None:
device = self.device
model = self.model
self.qd_zero = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, device=device)
self.body_q = wp.zeros((self.n_batch, model.body_count), dtype=wp.transform, requires_grad=grad, device=device)
self.body_qd = (
wp.zeros((self.n_batch, model.body_count), dtype=wp.spatial_vector, device=device) if grad else None
)
self.joint_q_proposed = wp.zeros(
(self.n_batch, self.n_coords), dtype=wp.float32, requires_grad=grad, device=device
)
self.residuals = wp.zeros((self.n_batch, self.n_residuals), dtype=wp.float32, requires_grad=grad, device=device)
self.jacobian = wp.zeros((self.n_batch, self.n_residuals, self.n_dofs), dtype=wp.float32, device=device)
self.dq_dof = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, requires_grad=grad, device=device)
self.gradient = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, device=device)
self.gradient_prev = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, device=device)
self.search_direction = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, device=device)
self.last_step_dq = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, device=device)
self.s_history = wp.zeros((self.n_batch, self.history_len, self.n_dofs), dtype=wp.float32, device=device)
self.y_history = wp.zeros((self.n_batch, self.history_len, self.n_dofs), dtype=wp.float32, device=device)
self.rho_history = wp.zeros((self.n_batch, self.history_len), dtype=wp.float32, device=device)
self.alpha_history = wp.zeros((self.n_batch, self.history_len), dtype=wp.float32, device=device)
self.history_count = wp.zeros(self.n_batch, dtype=wp.int32, device=device)
self.history_start = wp.zeros(self.n_batch, dtype=wp.int32, device=device)
self.costs = wp.zeros(self.n_batch, dtype=wp.float32, device=device)
self.problem_idx_identity = wp.array(np.arange(self.n_batch, dtype=np.int32), dtype=wp.int32, device=device)
self.X_local = wp.zeros((self.n_batch, model.joint_count), dtype=wp.transform, device=device)
if self.jacobian_mode != IKJacobianType.AUTODIFF and self.has_analytic_objective:
self.joint_S_s = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.spatial_vector, device=device)
else:
self.joint_S_s = None
def _alloc_line_search_buffers(self, grad: bool, line_search_alphas: Sequence[float]) -> None:
device = self.device
model = self.model
self.line_search_alphas = wp.array(line_search_alphas, dtype=wp.float32, device=device)
self.candidate_q = wp.zeros((self.n_batch, self.n_line_search, self.n_coords), dtype=wp.float32, device=device)
self.candidate_residuals = wp.zeros(
(self.n_batch, self.n_line_search, self.n_residuals), dtype=wp.float32, device=device
)
if self.n_line_search > 0:
self.candidate_problem_idx = wp.zeros((self.n_batch, self.n_line_search), dtype=wp.int32, device=device)
wp.launch(
_fan_out_problem_idx,
dim=[self.n_batch, self.n_line_search],
inputs=[self.problem_idx],
outputs=[self.candidate_problem_idx],
device=device,
)
else:
self.candidate_problem_idx = None
self.candidate_costs = wp.zeros((self.n_batch, self.n_line_search), dtype=wp.float32, device=device)
self.best_step_idx = wp.zeros(self.n_batch, dtype=wp.int32, device=device)
self.initial_slope = wp.zeros(self.n_batch, dtype=wp.float32, device=device)
self.candidate_gradients = wp.zeros(
(self.n_batch, self.n_line_search, self.n_dofs), dtype=wp.float32, device=device
)
self.candidate_slopes = wp.zeros((self.n_batch, self.n_line_search), dtype=wp.float32, device=device)
body_count = model.body_count
self.cand_body_q = wp.zeros(
(self.n_batch, self.n_line_search, body_count),
dtype=wp.transform,
requires_grad=grad,
device=device,
)
if grad:
self.cand_body_qd = wp.zeros(
(self.n_batch, self.n_line_search, body_count),
dtype=wp.spatial_vector,
device=device,
)
else:
self.cand_body_qd = None
if self.n_line_search > 0:
self.cand_joint_q_proposed = wp.zeros(
(self.n_batch, self.n_line_search, self.n_coords),
dtype=wp.float32,
requires_grad=grad,
device=device,
)
self.cand_dq_dof = wp.zeros(
(self.n_batch, self.n_line_search, self.n_dofs),
dtype=wp.float32,
requires_grad=grad,
device=device,
)
self.cand_step_dq = wp.zeros(
(self.n_batch, self.n_line_search, self.n_dofs),
dtype=wp.float32,
device=device,
)
else:
self.cand_joint_q_proposed = None
self.cand_dq_dof = None
self.cand_step_dq = None
if self.n_line_search > 0:
self.cand_qd_zero = wp.zeros(
(self.n_batch, self.n_line_search, self.n_dofs),
dtype=wp.float32,
device=device,
)
else:
self.cand_qd_zero = None
def _alloc_line_search_analytic_buffers(self) -> None:
device = self.device
if self.n_line_search > 0:
self.candidate_jacobians = wp.zeros(
(self.n_batch, self.n_line_search, self.n_residuals, self.n_dofs),
dtype=wp.float32,
device=device,
)
else:
self.candidate_jacobians = None
if self.has_analytic_objective and self.n_line_search > 0:
self.cand_joint_S_s = wp.zeros(
(self.n_batch, self.n_line_search, self.n_dofs),
dtype=wp.spatial_vector,
device=device,
)
else:
self.cand_joint_S_s = None
if self.jacobian_mode == IKJacobianType.ANALYTIC and self.n_line_search > 0:
self.cand_X_local = wp.zeros(
(self.n_batch, self.n_line_search, self.model.joint_count),
dtype=wp.transform,
device=device,
)
else:
self.cand_X_local = None
def _alloc_mixed_buffers(self) -> None:
device = self.device
self.gradient_tmp = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, device=device)
if self.n_line_search > 0:
self.candidate_gradient_tmp = wp.zeros(
(self.n_batch, self.n_line_search, self.n_dofs),
dtype=wp.float32,
device=device,
)
else:
self.candidate_gradient_tmp = None
mask_row = np.ones(self.n_residuals, dtype=np.float32)
for obj, offset in zip(self.objectives, self.residual_offsets, strict=False):
width = obj.residual_dim()
if obj.supports_analytic():
mask_row[offset : offset + width] = 0.0
if self.n_batch > 0:
mask_matrix = np.tile(mask_row, (self.n_batch, 1))
else:
mask_matrix = np.zeros((0, self.n_residuals), dtype=np.float32)
self.autodiff_residual_mask = wp.array(mask_matrix, dtype=wp.float32, device=device)
self.autodiff_residual_seed = wp.zeros((self.n_batch, self.n_residuals), dtype=wp.float32, device=device)
if self.n_line_search > 0:
B = self.n_batch * self.n_line_search
if B > 0:
mask_candidates = np.tile(mask_row, (B, 1))
else:
mask_candidates = np.zeros((0, self.n_residuals), dtype=np.float32)
self.autodiff_residual_mask_candidates = wp.array(mask_candidates, dtype=wp.float32, device=device)
self.candidate_autodiff_residual_grads = wp.zeros(
(self.n_batch, self.n_line_search, self.n_residuals), dtype=wp.float32, device=device
)
else:
self.autodiff_residual_mask_candidates = None
self.candidate_autodiff_residual_grads = None
def _build_residual_offsets(self) -> None:
self.residual_offsets = []
off = 0
for obj in self.objectives:
self.residual_offsets.append(off)
off += obj.residual_dim()
def _ctx_solver(
self,
joint_q: wp.array2d[wp.float32],
*,
residuals: wp.array2d[wp.float32] | None = None,
) -> BatchCtx:
"""Build a context for operations on the solver batch."""
ctx = BatchCtx(
joint_q=joint_q,
residuals=residuals if residuals is not None else self.residuals,
fk_body_q=self.body_q,
problem_idx=self.problem_idx,
fk_body_qd=self.body_qd,
dq_dof=self.dq_dof,
joint_q_proposed=self.joint_q_proposed,
joint_qd=self.qd_zero,
jacobian_out=self.jacobian,
motion_subspace=self.joint_S_s,
fk_qd_zero=self.qd_zero,
fk_X_local=self.X_local,
gradient_tmp=self.gradient_tmp,
autodiff_mask=self.autodiff_residual_mask,
autodiff_seed=self.autodiff_residual_seed,
)
self._validate_ctx(
ctx,
label="solver",
require_autodiff=self.jacobian_mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED),
require_analytic=(
self.jacobian_mode == IKJacobianType.ANALYTIC
or (self.jacobian_mode == IKJacobianType.MIXED and self.has_analytic_objective)
),
require_fk_x_local=self.jacobian_mode == IKJacobianType.ANALYTIC,
)
return ctx
def _ctx_candidates(self) -> BatchCtx:
"""Build a context for the flattened line-search candidate batch."""
if self.n_line_search == 0:
raise RuntimeError("line-search context requested without candidate buffers")
P = self.n_batch
S = self.n_line_search
B = P * S
def _reshape2(arr):
return arr.reshape((B, arr.shape[-1]))
def _reshape3(arr):
return arr.reshape((B, arr.shape[-2], arr.shape[-1]))
cand_body_qd = getattr(self, "cand_body_qd", None)
cand_dq_dof = getattr(self, "cand_dq_dof", None)
cand_joint_q_proposed = getattr(self, "cand_joint_q_proposed", None)
cand_qd_zero = getattr(self, "cand_qd_zero", None)
candidate_jacobians = getattr(self, "candidate_jacobians", None)
cand_joint_S_s = getattr(self, "cand_joint_S_s", None)
cand_X_local = getattr(self, "cand_X_local", None)
candidate_gradient_tmp = getattr(self, "candidate_gradient_tmp", None)
autodiff_mask_candidates = getattr(self, "autodiff_residual_mask_candidates", None)
candidate_autodiff_residual_grads = getattr(self, "candidate_autodiff_residual_grads", None)
problem_idx_flat = self.candidate_problem_idx.flatten()
ctx = BatchCtx(
joint_q=_reshape2(self.candidate_q),
residuals=_reshape2(self.candidate_residuals),
fk_body_q=_reshape2(self.cand_body_q),
problem_idx=problem_idx_flat,
fk_body_qd=_reshape2(cand_body_qd) if cand_body_qd is not None else None,
dq_dof=_reshape2(cand_dq_dof) if cand_dq_dof is not None else None,
joint_q_proposed=_reshape2(cand_joint_q_proposed) if cand_joint_q_proposed is not None else None,
joint_qd=_reshape2(cand_qd_zero) if cand_qd_zero is not None else None,
jacobian_out=_reshape3(candidate_jacobians) if candidate_jacobians is not None else None,
motion_subspace=_reshape2(cand_joint_S_s) if cand_joint_S_s is not None else None,
fk_qd_zero=_reshape2(cand_qd_zero) if cand_qd_zero is not None else None,
fk_X_local=_reshape2(cand_X_local) if cand_X_local is not None else None,
gradient_tmp=_reshape2(candidate_gradient_tmp) if candidate_gradient_tmp is not None else None,
autodiff_mask=_reshape2(autodiff_mask_candidates) if autodiff_mask_candidates is not None else None,
autodiff_seed=_reshape2(candidate_autodiff_residual_grads)
if candidate_autodiff_residual_grads is not None
else None,
)
self._validate_ctx(
ctx,
label="candidates",
require_autodiff=self.jacobian_mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED),
require_analytic=(
self.jacobian_mode == IKJacobianType.ANALYTIC
or (self.jacobian_mode == IKJacobianType.MIXED and self.has_analytic_objective)
),
require_fk_x_local=self.jacobian_mode == IKJacobianType.ANALYTIC,
)
return ctx
def _validate_ctx(
self,
ctx: BatchCtx,
*,
label: str,
require_autodiff: bool,
require_analytic: bool,
require_fk_x_local: bool,
) -> None:
missing: list[str] = []
if ctx.joint_q is None:
missing.append("joint_q")
if ctx.residuals is None:
missing.append("residuals")
if ctx.fk_body_q is None:
missing.append("fk_body_q")
if ctx.problem_idx is None:
missing.append("problem_idx")
if require_autodiff:
if ctx.fk_body_qd is None:
missing.append("fk_body_qd")
if ctx.dq_dof is None:
missing.append("dq_dof")
if ctx.joint_q_proposed is None:
missing.append("joint_q_proposed")
if ctx.joint_qd is None:
missing.append("joint_qd")
if self.jacobian_mode == IKJacobianType.MIXED and ctx.autodiff_mask is None:
missing.append("autodiff_mask")
if self.jacobian_mode == IKJacobianType.MIXED and ctx.autodiff_seed is None:
missing.append("autodiff_seed")
if require_analytic:
if ctx.jacobian_out is None:
missing.append("jacobian_out")
if ctx.motion_subspace is None:
missing.append("motion_subspace")
if ctx.fk_qd_zero is None:
missing.append("fk_qd_zero")
if self.jacobian_mode == IKJacobianType.MIXED and self.has_analytic_objective and ctx.gradient_tmp is None:
missing.append("gradient_tmp")
if require_fk_x_local and ctx.fk_X_local is None:
missing.append("fk_X_local")
if missing:
joined = ", ".join(missing)
raise RuntimeError(f"{label} context missing required buffers: {joined}")
def _gradient_at(self, ctx: BatchCtx, out_grad: wp.array2d[wp.float32]) -> None:
mode = self.jacobian_mode
if mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED):
self._grad_autodiff(ctx, out_grad)
if mode == IKJacobianType.ANALYTIC:
self._grad_analytic(ctx, out_grad, accumulate=False)
elif mode == IKJacobianType.MIXED and self.has_analytic_objective:
self._grad_analytic(ctx, out_grad, accumulate=True)
def _grad_autodiff(self, ctx: BatchCtx, out_grad: wp.array2d[wp.float32]) -> None:
batch = ctx.joint_q.shape[0]
self.tape.reset()
self.tape.gradients = {}
ctx.dq_dof.zero_()
with self.tape:
self._integrate_dq(
ctx.joint_q,
dq_in=ctx.dq_dof,
joint_q_out=ctx.joint_q_proposed,
joint_qd_out=ctx.joint_qd,
)
res_ctx = BatchCtx(
joint_q=ctx.joint_q_proposed,
residuals=ctx.residuals,
fk_body_q=ctx.fk_body_q,
problem_idx=ctx.problem_idx,
fk_body_qd=ctx.fk_body_qd,
joint_qd=ctx.joint_qd,
)
self._residuals_autodiff(res_ctx)
residuals_2d = ctx.residuals
self.tape.outputs = [residuals_2d]
if ctx.autodiff_mask is not None and ctx.autodiff_seed is not None:
wp.launch(
_apply_residual_mask,
dim=[batch, self.n_residuals],
inputs=[ctx.residuals, ctx.autodiff_mask],
outputs=[ctx.autodiff_seed],
device=self.device,
)
seed = ctx.autodiff_seed
self.tape.backward(grads={residuals_2d: seed})
else:
self.tape.backward(grads={residuals_2d: residuals_2d})
wp.copy(out_grad, self.tape.gradients[ctx.dq_dof])
self.tape.zero()
def _grad_analytic(
self,
ctx: BatchCtx,
out_grad: wp.array2d[wp.float32],
*,
accumulate: bool,
) -> None:
if not accumulate:
self._residuals_analytic(ctx)
ctx.fk_qd_zero.zero_()
ctx.jacobian_out.zero_()
self._compute_motion_subspace(
body_q=ctx.fk_body_q,
joint_S_s_out=ctx.motion_subspace,
joint_qd_in=ctx.fk_qd_zero,
)
def _emit_jac(obj, off, body_q_view, q_view, model, J_view, S_view):
if obj.supports_analytic():
obj.compute_jacobian_analytic(body_q_view, q_view, model, J_view, S_view, off)
self._parallel_for_objectives(
_emit_jac,
ctx.fk_body_q,
ctx.joint_q,
self.model,
ctx.jacobian_out,
ctx.motion_subspace,
)
target = ctx.gradient_tmp if accumulate else out_grad
if target is None:
target = out_grad
elif accumulate:
target.zero_()
wp.launch_tiled(
self._compute_gradient_jtr_tiled,
dim=ctx.joint_q.shape[0],
inputs=[ctx.jacobian_out, ctx.residuals],
outputs=[target],
block_dim=self.TILE_THREADS,
device=self.device,
)
if accumulate and target is not out_grad:
wp.launch(
_accumulate_gradients,
dim=[ctx.joint_q.shape[0], self.n_dofs],
inputs=[out_grad, target],
device=self.device,
)
def _for_objectives_residuals(self, ctx: BatchCtx) -> None:
def _do(obj, offset, body_q_view, joint_q_view, model, output_residuals, base_idx_array):
obj.compute_residuals(
body_q_view,
joint_q_view,
model,
output_residuals,
offset,
problem_idx=base_idx_array,
)
self._parallel_for_objectives(
_do,
ctx.fk_body_q,
ctx.joint_q,
self.model,
ctx.residuals,
ctx.problem_idx,
)
def _residuals_autodiff(self, ctx: BatchCtx) -> None:
eval_fk_batched(
self.model,
ctx.joint_q,
ctx.joint_qd,
ctx.fk_body_q,
ctx.fk_body_qd,
)
ctx.residuals.zero_()
self._for_objectives_residuals(ctx)
def _residuals_analytic(self, ctx: BatchCtx) -> None:
self._fk_two_pass(
self.model,
ctx.joint_q,
ctx.fk_body_q,
ctx.fk_X_local,
ctx.joint_q.shape[0],
)
ctx.residuals.zero_()
self._for_objectives_residuals(ctx)
def _init_objectives(self) -> None:
"""Allocate any per-objective buffers that must live on ``self.device``."""
for obj, offset in zip(self.objectives, self.residual_offsets, strict=False):
obj.set_batch_layout(self.n_residuals, offset, self.n_batch)
obj.bind_device(self.device)
if self.jacobian_mode == IKJacobianType.MIXED:
mode = IKJacobianType.ANALYTIC if obj.supports_analytic() else IKJacobianType.AUTODIFF
else:
mode = self.jacobian_mode
obj.init_buffers(model=self.model, jacobian_mode=mode)
def _init_cuda_streams(self) -> None:
"""Allocate per-objective Warp streams and sync events."""
self.objective_streams = []
self.sync_events = []
if self.device.is_cuda:
for _ in range(len(self.objectives)):
stream = wp.Stream(self.device)
event = wp.Event(self.device)
self.objective_streams.append(stream)
self.sync_events.append(event)
else:
self.objective_streams = [None] * len(self.objectives)
self.sync_events = [None] * len(self.objectives)
def _parallel_for_objectives(self, fn: Callable[..., None], *extra: Any) -> None:
"""Run across objectives on parallel CUDA streams."""
if self.device.is_cuda:
main = wp.get_stream(self.device)
init_evt = main.record_event()
for obj, offset, obj_stream, sync_event in zip(
self.objectives, self.residual_offsets, self.objective_streams, self.sync_events, strict=False
):
obj_stream.wait_event(init_evt)
with wp.ScopedStream(obj_stream):
fn(obj, offset, *extra)
obj_stream.record_event(sync_event)
for sync_event in self.sync_events:
main.wait_event(sync_event)
else:
for obj, offset in zip(self.objectives, self.residual_offsets, strict=False):
fn(obj, offset, *extra)
def step(
self,
joint_q_in: wp.array2d[wp.float32],
joint_q_out: wp.array2d[wp.float32],
iterations: int = 50,
) -> None:
"""Run several L-BFGS iterations on a batch of joint configurations.
Args:
joint_q_in: Input joint coordinates, shape [n_batch, joint_coord_count].
joint_q_out: Output buffer for the optimized coordinates, shape
[n_batch, joint_coord_count]. It may alias ``joint_q_in`` for
in-place updates.
iterations: Number of L-BFGS iterations to execute.
"""
if joint_q_in.shape != (self.n_batch, self.n_coords):
raise ValueError("joint_q_in has incompatible shape")
if joint_q_out.shape != (self.n_batch, self.n_coords):
raise ValueError("joint_q_out has incompatible shape")
if joint_q_in.ptr != joint_q_out.ptr:
wp.copy(joint_q_out, joint_q_in)
joint_q = joint_q_out
for i in range(iterations):
self._step(joint_q, iteration=i)
def reset(self) -> None:
"""Clear L-BFGS history and cached line-search state."""
self.history_count.zero_()
self.history_start.zero_()
self.s_history.zero_()
self.y_history.zero_()
self.rho_history.zero_()
self.alpha_history.zero_()
self.gradient.zero_()
self.gradient_prev.zero_()
self.search_direction.zero_()
self.last_step_dq.zero_()
self.best_step_idx.zero_()
self.costs.zero_()
if self.cand_step_dq is not None:
self.cand_step_dq.zero_()
def compute_costs(self, joint_q: wp.array2d[wp.float32]) -> wp.array[wp.float32]:
"""Evaluate squared residual costs for a batch of joint configurations.
Args:
joint_q: Joint coordinates to evaluate, shape [n_batch, joint_coord_count].
Returns:
Costs for each batch row, shape [n_batch].
"""
self._compute_residuals(joint_q)
wp.launch(
compute_costs,
dim=self.n_batch,
inputs=[self.residuals, self.n_residuals],
outputs=[self.costs],
device=self.device,
)
return self.costs
def _compute_residuals(
self,
joint_q: wp.array2d[wp.float32],
residuals_out: wp.array2d[wp.float32] | None = None,
) -> wp.array2d[wp.float32]:
residuals = residuals_out if residuals_out is not None else self.residuals
ctx = self._ctx_solver(joint_q, residuals=residuals)
if self.jacobian_mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED):
self._residuals_autodiff(ctx)
else:
self._residuals_analytic(ctx)
return ctx.residuals
def _compute_motion_subspace(
self,
*,
body_q: wp.array2d[wp.transform],
joint_S_s_out: wp.array2d[wp.spatial_vector],
joint_qd_in: wp.array2d[wp.float32],
) -> None:
n_joints = self.model.joint_count
batch = body_q.shape[0]
wp.launch(
self._compute_motion_subspace_2d,
dim=[batch, n_joints],
inputs=[
self.model.joint_type,
self.model.joint_parent,
self.model.joint_qd_start,
joint_qd_in,
self.model.joint_axis,
self.model.joint_dof_dim,
body_q,
self.model.joint_X_p,
],
outputs=[
joint_S_s_out,
],
device=self.device,
)
def _integrate_dq(
self,
joint_q: wp.array2d[wp.float32],
*,
dq_in: wp.array2d[wp.float32],
joint_q_out: wp.array2d[wp.float32],
joint_qd_out: wp.array2d[wp.float32],
step_size: float = 1.0,
) -> None:
batch = joint_q.shape[0]
wp.launch(
self._integrate_dq_dof,
dim=[batch, self.model.joint_count],
inputs=[
self.model.joint_type,
self.model.joint_q_start,
self.model.joint_qd_start,
self.model.joint_dof_dim,
joint_q,
dq_in,
joint_qd_out,
step_size,
],
outputs=[
joint_q_out,
joint_qd_out,
],
device=self.device,
)
joint_qd_out.zero_()
def _step(self, joint_q: wp.array2d[wp.float32], iteration: int = 0) -> None:
"""Execute one L-BFGS iteration."""
self.compute_costs(joint_q)
ctx = self._ctx_solver(joint_q)
self._gradient_at(ctx, self.gradient)
if iteration == 0:
wp.copy(self.gradient_prev, self.gradient)
wp.launch(
_scale_negate,
dim=[self.n_batch, self.n_dofs],
inputs=[self.gradient, 1e-2],
outputs=[self.last_step_dq],
device=self.device,
)
self._integrate_dq(
joint_q,
dq_in=self.last_step_dq,
joint_q_out=self.joint_q_proposed,
joint_qd_out=self.qd_zero,
step_size=1.0,
)
wp.copy(joint_q, self.joint_q_proposed)
return
self._update_history()
self._compute_search_direction()
self._compute_initial_slope()
wp.copy(self.gradient_prev, self.gradient)
self._line_search(joint_q)
self._line_search_select_best(joint_q)
def _compute_initial_slope(self) -> None:
"""Compute and store dot(gradient, search_direction) for the current state."""
wp.launch_tiled(
self._compute_slope_tiled,
dim=[self.n_batch],
inputs=[self.gradient, self.search_direction],
outputs=[self.initial_slope],
block_dim=self.TILE_THREADS,
device=self.device,
)
def _compute_search_direction(self) -> None:
"""Compute L-BFGS search direction using two-loop recursion."""
wp.launch_tiled(
self._compute_search_direction_tiled,
dim=[self.n_batch],
inputs=[
self.gradient,
self.s_history,
self.y_history,
self.rho_history,
self.alpha_history,
self.history_count,
self.history_start,
self.h0_scale,
],
outputs=[
self.search_direction,
],
block_dim=self.TILE_THREADS,
device=self.device,
)
def _update_history(self) -> None:
"""Update L-BFGS history with new s_k and y_k pairs."""
# if self.device.is_cuda:
wp.launch_tiled(
self._update_history_tiled,
dim=[self.n_batch],
inputs=[
self.last_step_dq,
self.gradient,
self.gradient_prev,
self.history_len,
],
outputs=[
self.s_history,
self.y_history,
self.rho_history,
self.history_count,
self.history_start,
],
block_dim=self.TILE_THREADS,
device=self.device,
)
def _line_search(self, joint_q: wp.array2d[wp.float32]) -> None:
"""
Generate candidate configurations and compute their costs and gradients
to check the Wolfe conditions.
"""
P = self.n_batch
S = self.n_line_search
B = P * S
if S == 0:
return
wp.launch(
_generate_candidates_velocity,
dim=[P, S],
inputs=[joint_q, self.search_direction, self.line_search_alphas],
outputs=[self.candidate_q, self.cand_dq_dof],
device=self.device,
)
if self.cand_step_dq is not None:
wp.copy(self.cand_step_dq, self.cand_dq_dof)
cand_ctx = self._ctx_candidates()
self._integrate_dq(
cand_ctx.joint_q,
dq_in=cand_ctx.dq_dof,
joint_q_out=cand_ctx.joint_q_proposed,
joint_qd_out=cand_ctx.joint_qd,
step_size=1.0,
)
wp.copy(cand_ctx.joint_q, cand_ctx.joint_q_proposed)
n_candidates = self.n_batch * self.n_line_search
candidate_gradients_flat = self.candidate_gradients.reshape((n_candidates, -1))
# NOTE: _gradient_at also computes residuals (needed for costs)
self._gradient_at(cand_ctx, candidate_gradients_flat)
wp.launch(
compute_costs,
dim=B,
inputs=[cand_ctx.residuals, self.n_residuals],
outputs=[self.candidate_costs.flatten()],
device=self.device,
)
wp.launch_tiled(
self._compute_slope_candidates_tiled,
dim=[P, S],
inputs=[
self.candidate_gradients,
self.search_direction,
],
outputs=[
self.candidate_slopes,
],
block_dim=self.TILE_THREADS,
device=self.device,
)
def _line_search_select_best(self, joint_q: wp.array2d[wp.float32]) -> None:
"""Select the best step size based on Wolfe conditions and update joint_q."""
if self.n_line_search == 0:
return
wp.copy(self.joint_q_proposed, joint_q)
wp.launch_tiled(
self._select_best_step_tiled,
dim=[self.n_batch],
inputs=[
self.candidate_costs,
self.cand_step_dq,
self.costs,
self.initial_slope,
self.candidate_slopes,
self.line_search_alphas,
self.wolfe_c1,
self.wolfe_c2,
],
outputs=[
self.best_step_idx,
self.last_step_dq,
],
block_dim=self.TILE_THREADS,
device=self.device,
)
self._integrate_dq(
self.joint_q_proposed,
dq_in=self.last_step_dq,
joint_q_out=joint_q,
joint_qd_out=self.qd_zero,
step_size=1.0,
)
@classmethod
def _build_specialized(cls, key: tuple[int, int, int, int, str]) -> type[IKOptimizerLBFGS]:
"""Build a specialized IKOptimizerLBFGS subclass with tiled kernels for given dimensions."""
C, R, M_HIST, N_LINE_SEARCH, _ARCH = key
def _compute_slope_template(
# inputs
gradient: wp.array2d[wp.float32], # (n_batch, n_dofs)
search_direction: wp.array2d[wp.float32], # (n_batch, n_dofs)
# outputs
slope_out: wp.array[wp.float32], # (n_batch,)
):
row = wp.tid()
DOF = _Specialized.TILE_N_DOFS
g = wp.tile_load(gradient[row], shape=(DOF,))
p = wp.tile_load(search_direction[row], shape=(DOF,))
slope = wp.tile_sum(wp.tile_map(wp.mul, g, p))
slope_out[row] = slope[0]
def _compute_slope_candidates_template(
# inputs
candidate_gradient: wp.array3d[wp.float32], # (n_batch, n_line_steps, n_dofs)
search_direction: wp.array2d[wp.float32], # (n_batch, n_dofs)
# outputs
slope_out: wp.array2d[wp.float32], # (n_batch, n_line_steps)
):
row, step_idx = wp.tid()
DOF = _Specialized.TILE_N_DOFS
g = wp.tile_load(candidate_gradient[row, step_idx], shape=(DOF,))
p = wp.tile_load(search_direction[row], shape=(DOF,))
slope = wp.tile_sum(wp.tile_map(wp.mul, g, p))
slope_out[row, step_idx] = slope[0]
def _compute_gradient_jtr_template(
# inputs
jacobian: wp.array3d[wp.float32], # (n_batch, n_residuals, n_dofs)
residuals: wp.array2d[wp.float32], # (n_batch, n_residuals)
# outputs
gradient: wp.array2d[wp.float32], # (n_batch, n_dofs)
):
row = wp.tid()
RES = _Specialized.TILE_N_RESIDUALS
DOF = _Specialized.TILE_N_DOFS
J = wp.tile_load(jacobian[row], shape=(RES, DOF))
r = wp.tile_load(residuals[row], shape=(RES,))
Jt = wp.tile_transpose(J)
r_2d = wp.tile_reshape(r, shape=(RES, 1))
grad_2d = wp.tile_matmul(Jt, r_2d)
grad_1d = wp.tile_reshape(grad_2d, shape=(DOF,))
wp.tile_store(gradient[row], grad_1d)
def _compute_search_direction_template(
# inputs
gradient: wp.array2d[wp.float32], # (n_batch, n_dofs)
s_history: wp.array3d[wp.float32], # (n_batch, history_len, n_dofs)
y_history: wp.array3d[wp.float32], # (n_batch, history_len, n_dofs)
rho_history: wp.array2d[wp.float32], # (n_batch, history_len)
alpha_history: wp.array2d[wp.float32], # (n_batch, history_len)
history_count: wp.array[wp.int32], # (n_batch)
history_start: wp.array[wp.int32], # (n_batch)
h0_scale: float, # scalar
# outputs
search_direction: wp.array2d[wp.float32], # (n_batch, n_dofs)
):
row = wp.tid()
DOF = _Specialized.TILE_N_DOFS
M_HIST = _Specialized.TILE_HISTORY_LEN
q = wp.tile_load(gradient[row], shape=(DOF,), storage="shared")
count = history_count[row]
start = history_start[row]
# First loop: backward through history
for i in range(count):
idx = (start + count - 1 - i) % M_HIST
s_i = wp.tile_load(s_history[row, idx], shape=(DOF,), storage="shared")
rho_i = rho_history[row, idx]
s_dot_q = wp.tile_sum(wp.tile_map(wp.mul, s_i, q))
alpha_i = rho_i * s_dot_q[0]
alpha_history[row, idx] = alpha_i
y_i = wp.tile_load(y_history[row, idx], shape=(DOF,), storage="shared")
for j in range(DOF):
q[j] = q[j] - alpha_i * y_i[j]
# Apply initial Hessian approximation in-place
for j in range(DOF):
q[j] = h0_scale * q[j]
# Second loop: forward through history
for i in range(count):
idx = (start + i) % M_HIST
y_i = wp.tile_load(y_history[row, idx], shape=(DOF,), storage="shared")
s_i = wp.tile_load(s_history[row, idx], shape=(DOF,), storage="shared")
rho_i = rho_history[row, idx]
alpha_i = alpha_history[row, idx]
y_dot_q = wp.tile_sum(wp.tile_map(wp.mul, y_i, q))
beta = rho_i * y_dot_q[0]
diff = alpha_i - beta
for j in range(DOF):
q[j] = q[j] + diff * s_i[j]
# Store negative gradient (descent direction)
for j in range(DOF):
q[j] = -q[j]
wp.tile_store(search_direction[row], q)
def _update_history_template(
# inputs
last_step: wp.array2d[wp.float32], # (n_batch, n_dofs)
gradient: wp.array2d[wp.float32], # (n_batch, n_dofs)
gradient_prev: wp.array2d[wp.float32], # (n_batch, n_dofs)
history_len: int,
# outputs
s_history: wp.array3d[wp.float32],
y_history: wp.array3d[wp.float32],
rho_history: wp.array2d[wp.float32],
history_count: wp.array[wp.int32],
history_start: wp.array[wp.int32],
):
row = wp.tid()
DOF = _Specialized.TILE_N_DOFS
s_k = wp.tile_load(last_step[row], shape=(DOF,))
g_curr = wp.tile_load(gradient[row], shape=(DOF,))
g_prev = wp.tile_load(gradient_prev[row], shape=(DOF,))
y_k = wp.tile_map(wp.sub, g_curr, g_prev)
y_dot_s_tile = wp.tile_sum(wp.tile_map(wp.mul, y_k, s_k))
y_dot_s = y_dot_s_tile[0]
# Check curvature condition to ensure Hessian approximation is positive definite
if y_dot_s > 1e-8:
rho_k = 1.0 / y_dot_s
count = history_count[row]
start = history_start[row]
write_idx = (start + count) % history_len
if count < history_len:
history_count[row] = count + 1
else:
history_start[row] = (start + 1) % history_len
wp.tile_store(s_history[row, write_idx], s_k)
wp.tile_store(y_history[row, write_idx], y_k)
rho_history[row, write_idx] = rho_k
def _select_best_step_template(
# inputs
candidate_costs: wp.array2d[wp.float32], # (n_batch, n_line_steps)
candidate_step: wp.array3d[wp.float32], # (n_batch, n_line_steps, n_dofs)
cost_initial: wp.array[wp.float32], # (n_batch)
slope_initial: wp.array[wp.float32], # (n_batch)
candidate_slopes: wp.array2d[wp.float32], # (n_batch, n_line_steps)
line_search_alphas: wp.array[wp.float32], # (n_line_steps)
wolfe_c1: float, # scalar
wolfe_c2: float, # scalar
# outputs
best_step_idx_out: wp.array[wp.int32], # (n_batch)
last_step_out: wp.array2d[wp.float32], # (n_batch, n_dofs)
):
row = wp.tid()
N_STEPS = _Specialized.TILE_N_LINE_STEPS
DOF = _Specialized.TILE_N_DOFS
cost_k = cost_initial[row]
slope_k = slope_initial[row]
best_idx = int(-1)
# Search backwards for the largest step size satisfying Wolfe conditions
for i in range(N_STEPS - 1, -1, -1):
cost_new = candidate_costs[row, i]
alpha = line_search_alphas[i]
# Armijo (Sufficient Decrease) Condition
armijo_ok = cost_new <= cost_k + wolfe_c1 * alpha * slope_k
# Strong Curvature Condition
slope_new = candidate_slopes[row, i]
curvature_ok = wp.abs(slope_new) <= wolfe_c2 * wp.abs(slope_k)
if armijo_ok and curvature_ok:
best_idx = i
break
# Fallback: If no step satisfies Wolfe, choose the one with the minimum cost.
if best_idx == -1:
costs = wp.tile_load(candidate_costs[row], shape=(N_STEPS,), storage="shared")
argmin_tile = wp.tile_argmin(costs)
best_idx = argmin_tile[0]
accept_idx = best_idx
if best_idx >= 0:
cost_best = candidate_costs[row, best_idx]
if cost_best >= cost_k:
accept_idx = -1
best_step_idx_out[row] = accept_idx
if accept_idx >= 0:
best_step_vec = wp.tile_load(candidate_step[row, accept_idx], shape=(DOF,), storage="shared")
wp.tile_store(last_step_out[row], best_step_vec)
else:
zero_vec = wp.tile_zeros(shape=(DOF,), dtype=wp.float32)
wp.tile_store(last_step_out[row], zero_vec)
_compute_slope_template.__name__ = f"_compute_slope_tiled_{C}"
_compute_slope_template.__qualname__ = f"_compute_slope_tiled_{C}"
_compute_slope_tiled = wp.kernel(enable_backward=False, module="unique")(_compute_slope_template)
_compute_slope_candidates_template.__name__ = f"_compute_slope_candidates_tiled_{C}_{N_LINE_SEARCH}"
_compute_slope_candidates_template.__qualname__ = f"_compute_slope_candidates_tiled_{C}_{N_LINE_SEARCH}"
_compute_slope_candidates_tiled = wp.kernel(enable_backward=False, module="unique")(
_compute_slope_candidates_template
)
_compute_gradient_jtr_template.__name__ = f"_compute_gradient_jtr_tiled_{C}_{R}"
_compute_gradient_jtr_template.__qualname__ = f"_compute_gradient_jtr_tiled_{C}_{R}"
_compute_gradient_jtr_tiled = wp.kernel(enable_backward=False, module="unique")(_compute_gradient_jtr_template)
_compute_search_direction_template.__name__ = f"_compute_search_direction_tiled_{C}_{M_HIST}"
_compute_search_direction_template.__qualname__ = f"_compute_search_direction_tiled_{C}_{M_HIST}"
_compute_search_direction_tiled = wp.kernel(enable_backward=False, module="unique")(
_compute_search_direction_template
)
_update_history_template.__name__ = f"_update_history_tiled_{C}_{M_HIST}"
_update_history_template.__qualname__ = f"_update_history_tiled_{C}_{M_HIST}"
_update_history_tiled = wp.kernel(enable_backward=False, module="unique")(_update_history_template)
_select_best_step_template.__name__ = f"_select_best_step_tiled_{C}_{N_LINE_SEARCH}"
_select_best_step_template.__qualname__ = f"_select_best_step_tiled_{C}_{N_LINE_SEARCH}"
_select_best_step_tiled = wp.kernel(enable_backward=False, module="unique")(_select_best_step_template)
# late-import jcalc_motion, jcalc_transform to avoid circular import error
from ...solvers.featherstone.kernels import ( # noqa: PLC0415
jcalc_integrate,
jcalc_motion,
jcalc_transform,
)
@wp.kernel
def _integrate_dq_dof(
# model-wide
joint_type: wp.array[wp.int32], # (n_joints)
joint_q_start: wp.array[wp.int32], # (n_joints + 1)
joint_qd_start: wp.array[wp.int32], # (n_joints + 1)
joint_dof_dim: wp.array2d[wp.int32], # (n_joints, 2) → (lin, ang)
# per-row
joint_q_curr: wp.array2d[wp.float32], # (n_batch, n_coords)
joint_qd_curr: wp.array2d[wp.float32], # (n_batch, n_dofs) (typically all-zero)
dq_dof: wp.array2d[wp.float32], # (n_batch, n_dofs) ← update direction (q̇)
dt: float, # step scale (usually 1.0)
# outputs
joint_q_out: wp.array2d[wp.float32], # (n_batch, n_coords)
joint_qd_out: wp.array2d[wp.float32], # (n_batch, n_dofs)
):
"""
Integrate the candidate update ``dq_dof`` (interpreted as a
joint-space velocity times ``dt``) into a new configuration.
q_out = integrate(q_curr, dq_dof)
One thread handles one joint of one batch row. All joint types
supported by ``jcalc_integrate`` (revolute, prismatic, ball,
free, D6, ...) work out of the box.
"""
row, joint_idx = wp.tid()
# Static joint metadata
t = joint_type[joint_idx]
coord_start = joint_q_start[joint_idx]
dof_start = joint_qd_start[joint_idx]
lin_axes = joint_dof_dim[joint_idx, 0]
ang_axes = joint_dof_dim[joint_idx, 1]
# Views into the current batch row
q_row = joint_q_curr[row]
qd_row = joint_qd_curr[row] # typically zero
delta_row = dq_dof[row] # update vector
q_out_row = joint_q_out[row]
qd_out_row = joint_qd_out[row]
# Treat `delta_row` as acceleration with dt=1:
# qd_new = 0 + delta (qd ← delta)
# q_new = q + qd_new * dt (q ← q + delta)
jcalc_integrate(
t,
q_row,
qd_row,
delta_row, # passed as joint_qdd
coord_start,
dof_start,
lin_axes,
ang_axes,
dt,
q_out_row,
qd_out_row,
)
@wp.kernel(module="unique")
def _compute_motion_subspace_2d(
joint_type: wp.array[wp.int32], # (n_joints)
joint_parent: wp.array[wp.int32], # (n_joints)
joint_qd_start: wp.array[wp.int32], # (n_joints + 1)
joint_qd: wp.array2d[wp.float32], # (n_batch, n_joint_dof_count)
joint_axis: wp.array[wp.vec3], # (n_joint_dof_count)
joint_dof_dim: wp.array2d[wp.int32], # (n_joints, 2)
body_q: wp.array2d[wp.transform], # (n_batch, n_bodies)
joint_X_p: wp.array[wp.transform], # (n_joints)
# outputs
joint_S_s: wp.array2d[wp.spatial_vector], # (n_batch, n_joint_dof_count)
):
row, joint_idx = wp.tid()
type = joint_type[joint_idx]
parent = joint_parent[joint_idx]
qd_start = joint_qd_start[joint_idx]
X_pj = joint_X_p[joint_idx]
X_wpj = X_pj
if parent >= 0:
X_wpj = body_q[row, parent] * X_pj
lin_axis_count = joint_dof_dim[joint_idx, 0]
ang_axis_count = joint_dof_dim[joint_idx, 1]
joint_qd_1d = joint_qd[row]
S_s_out = joint_S_s[row]
jcalc_motion(
type,
joint_axis,
lin_axis_count,
ang_axis_count,
X_wpj,
joint_qd_1d,
qd_start,
S_s_out,
)
@wp.kernel(module="unique")
def _fk_local(
joint_type: wp.array[wp.int32], # (n_joints)
joint_q: wp.array2d[wp.float32], # (n_batch, n_coords)
joint_q_start: wp.array[wp.int32], # (n_joints + 1)
joint_qd_start: wp.array[wp.int32], # (n_joints + 1)
joint_axis: wp.array[wp.vec3], # (n_axes)
joint_dof_dim: wp.array2d[wp.int32], # (n_joints, 2) → (lin, ang)
joint_X_p: wp.array[wp.transform], # (n_joints)
joint_X_c: wp.array[wp.transform], # (n_joints)
# outputs
X_local_out: wp.array2d[wp.transform], # (n_batch, n_joints)
):
row, local_joint_idx = wp.tid()
t = joint_type[local_joint_idx]
q_start = joint_q_start[local_joint_idx]
axis_start = joint_qd_start[local_joint_idx]
lin_axes = joint_dof_dim[local_joint_idx, 0]
ang_axes = joint_dof_dim[local_joint_idx, 1]
X_j = jcalc_transform(
t,
joint_axis,
axis_start,
lin_axes,
ang_axes,
joint_q[row], # 1-D row slice
q_start,
)
X_rel = joint_X_p[local_joint_idx] * X_j * wp.transform_inverse(joint_X_c[local_joint_idx])
X_local_out[row, local_joint_idx] = X_rel
def _fk_two_pass(model, joint_q, body_q, X_local, n_batch):
"""Compute forward kinematics using two-pass algorithm.
Args:
model: newton.Model instance
joint_q: 2D array [n_batch, joint_coord_count]
body_q: 2D array [n_batch, body_count] (output)
X_local: 2D array [n_batch, joint_count] (workspace)
n_batch: Number of rows to process
"""
wp.launch(
_fk_local,
dim=[n_batch, model.joint_count],
inputs=[
model.joint_type,
joint_q,
model.joint_q_start,
model.joint_qd_start,
model.joint_axis,
model.joint_dof_dim,
model.joint_X_p,
model.joint_X_c,
],
outputs=[
X_local,
],
device=model.device,
)
wp.launch(
fk_accum,
dim=[n_batch, model.joint_count],
inputs=[
model.joint_parent,
X_local,
],
outputs=[
body_q,
],
device=model.device,
)
class _Specialized(IKOptimizerLBFGS):
TILE_N_DOFS = wp.constant(C)
TILE_N_RESIDUALS = wp.constant(R)
TILE_HISTORY_LEN = wp.constant(M_HIST)
TILE_N_LINE_STEPS = wp.constant(N_LINE_SEARCH)
TILE_THREADS = wp.constant(32)
_Specialized.__name__ = f"LBFGS_Wolfe_{C}x{R}x{M_HIST}x{N_LINE_SEARCH}"
_Specialized._compute_gradient_jtr_tiled = staticmethod(_compute_gradient_jtr_tiled)
_Specialized._compute_slope_tiled = staticmethod(_compute_slope_tiled)
_Specialized._compute_slope_candidates_tiled = staticmethod(_compute_slope_candidates_tiled)
_Specialized._compute_search_direction_tiled = staticmethod(_compute_search_direction_tiled)
_Specialized._update_history_tiled = staticmethod(_update_history_tiled)
_Specialized._select_best_step_tiled = staticmethod(_select_best_step_tiled)
_Specialized._integrate_dq_dof = staticmethod(_integrate_dq_dof)
_Specialized._compute_motion_subspace_2d = staticmethod(_compute_motion_subspace_2d)
_Specialized._fk_two_pass = staticmethod(_fk_two_pass)
return _Specialized
================================================
FILE: newton/_src/sim/ik/ik_lm_optimizer.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Levenberg-Marquardt optimizer backend for inverse kinematics."""
from __future__ import annotations
from collections.abc import Callable, Sequence
from dataclasses import dataclass
from typing import Any, ClassVar
import numpy as np
import warp as wp
from ..model import Model
from .ik_common import IKJacobianType, compute_costs, eval_fk_batched, fk_accum
from .ik_objectives import IKObjective
@dataclass(slots=True)
class BatchCtx:
joint_q: wp.array2d[wp.float32]
residuals: wp.array2d[wp.float32]
fk_body_q: wp.array2d[wp.transform]
problem_idx: wp.array[wp.int32]
# AUTODIFF and MIXED
fk_body_qd: wp.array2d[wp.spatial_vector] | None = None
dq_dof: wp.array2d[wp.float32] | None = None
joint_q_proposed: wp.array2d[wp.float32] | None = None
joint_qd: wp.array2d[wp.float32] | None = None
# ANALYTIC and MIXED
jacobian_out: wp.array3d[wp.float32] | None = None
motion_subspace: wp.array2d[wp.spatial_vector] | None = None
fk_qd_zero: wp.array2d[wp.float32] | None = None
fk_X_local: wp.array2d[wp.transform] | None = None
@wp.kernel
def _accept_reject(
cost_curr: wp.array[wp.float32],
cost_prop: wp.array[wp.float32],
pred_red: wp.array[wp.float32],
rho_min: float,
accept: wp.array[wp.int32],
):
row = wp.tid()
rho = (cost_curr[row] - cost_prop[row]) / (pred_red[row] + 1.0e-8)
accept[row] = wp.int32(1) if rho >= rho_min else wp.int32(0)
@wp.kernel
def _update_lm_state(
joint_q_proposed: wp.array2d[wp.float32],
residuals_proposed: wp.array2d[wp.float32],
costs_proposed: wp.array[wp.float32],
accept_flags: wp.array[wp.int32],
n_coords: int,
num_residuals: int,
lambda_factor: float,
lambda_min: float,
lambda_max: float,
joint_q_current: wp.array2d[wp.float32],
residuals_current: wp.array2d[wp.float32],
costs: wp.array[wp.float32],
lambda_values: wp.array[wp.float32],
):
row = wp.tid()
if accept_flags[row] == 1:
for i in range(n_coords):
joint_q_current[row, i] = joint_q_proposed[row, i]
for i in range(num_residuals):
residuals_current[row, i] = residuals_proposed[row, i]
costs[row] = costs_proposed[row]
lambda_values[row] = lambda_values[row] / lambda_factor
else:
new_lambda = lambda_values[row] * lambda_factor
lambda_values[row] = wp.clamp(new_lambda, lambda_min, lambda_max)
class IKOptimizerLM:
"""Levenberg-Marquardt optimizer for batched inverse kinematics.
The optimizer solves a batch of independent IK problems that share a
single articulation model and objective list. Jacobians can be evaluated
with ``IKJacobianType.AUTODIFF``, ``IKJacobianType.ANALYTIC``, or
``IKJacobianType.MIXED``.
Args:
model: Shared articulation model.
n_batch: Number of evaluation rows solved in parallel. This is
typically ``n_problems * n_seeds`` after any sampling expansion.
objectives: Ordered IK objectives applied to every batch row.
lambda_initial: Initial LM damping factor for each batch row.
jacobian_mode: Jacobian backend to use.
lambda_factor: Factor used to increase or decrease the damping term
after each trial step.
lambda_min: Minimum allowed damping value.
lambda_max: Maximum allowed damping value.
rho_min: Minimum ratio of actual to predicted decrease required to
accept a step.
problem_idx: Optional mapping from batch rows to base problem indices
for per-problem objective data.
"""
TILE_N_DOFS = None
TILE_N_RESIDUALS = None
_cache: ClassVar[dict[tuple[int, int, str], type]] = {}
def __new__(
cls,
model: Model,
n_batch: int,
objectives: Sequence[IKObjective],
*a: Any,
**kw: Any,
) -> IKOptimizerLM:
n_dofs = model.joint_dof_count
n_residuals = sum(o.residual_dim() for o in objectives)
arch = model.device.arch
key = (n_dofs, n_residuals, arch)
spec_cls = cls._cache.get(key)
if spec_cls is None:
spec_cls = cls._build_specialized(key)
cls._cache[key] = spec_cls
return super().__new__(spec_cls)
def __init__(
self,
model: Model,
n_batch: int,
objectives: Sequence[IKObjective],
lambda_initial: float = 0.1,
jacobian_mode: IKJacobianType = IKJacobianType.AUTODIFF,
lambda_factor: float = 2.0,
lambda_min: float = 1e-5,
lambda_max: float = 1e10,
rho_min: float = 1e-3,
*,
problem_idx: wp.array[wp.int32] | None = None,
) -> None:
self.model = model
self.device = model.device
self.n_batch = n_batch
self.n_coords = model.joint_coord_count
self.n_dofs = model.joint_dof_count
self.n_residuals = sum(o.residual_dim() for o in objectives)
self.objectives = objectives
self.jacobian_mode = jacobian_mode
self.has_analytic_objective = any(o.supports_analytic() for o in objectives)
self.has_autodiff_objective = any(not o.supports_analytic() for o in objectives)
self.lambda_initial = lambda_initial
self.lambda_factor = lambda_factor
self.lambda_min = lambda_min
self.lambda_max = lambda_max
self.rho_min = rho_min
if self.TILE_N_DOFS is not None:
assert self.n_dofs == self.TILE_N_DOFS
if self.TILE_N_RESIDUALS is not None:
assert self.n_residuals == self.TILE_N_RESIDUALS
grad = jacobian_mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED)
self._alloc_solver_buffers(grad)
self.problem_idx = problem_idx if problem_idx is not None else self.problem_idx_identity
self.tape = wp.Tape() if grad else None
self._build_residual_offsets()
self._init_objectives()
self._init_cuda_streams()
def _init_objectives(self) -> None:
"""Allocate any per-objective buffers that must live on ``self.device``."""
for obj, offset in zip(self.objectives, self.residual_offsets, strict=False):
obj.set_batch_layout(self.n_residuals, offset, self.n_batch)
obj.bind_device(self.device)
if self.jacobian_mode == IKJacobianType.MIXED:
mode = IKJacobianType.ANALYTIC if obj.supports_analytic() else IKJacobianType.AUTODIFF
else:
mode = self.jacobian_mode
obj.init_buffers(model=self.model, jacobian_mode=mode)
def _init_cuda_streams(self) -> None:
"""Allocate per-objective Warp streams and sync events."""
self.objective_streams = []
self.sync_events = []
if self.device.is_cuda:
for _ in range(len(self.objectives)):
stream = wp.Stream(self.device)
event = wp.Event(self.device)
self.objective_streams.append(stream)
self.sync_events.append(event)
else:
self.objective_streams = [None] * len(self.objectives)
self.sync_events = [None] * len(self.objectives)
def _parallel_for_objectives(self, fn: Callable[..., None], *extra: Any) -> None:
"""Run across objectives on parallel CUDA streams."""
if self.device.is_cuda:
main = wp.get_stream(self.device)
init_evt = main.record_event()
for obj, offset, obj_stream, sync_event in zip(
self.objectives, self.residual_offsets, self.objective_streams, self.sync_events, strict=False
):
obj_stream.wait_event(init_evt)
with wp.ScopedStream(obj_stream):
fn(obj, offset, *extra)
obj_stream.record_event(sync_event)
for sync_event in self.sync_events:
main.wait_event(sync_event)
else:
for obj, offset in zip(self.objectives, self.residual_offsets, strict=False):
fn(obj, offset, *extra)
def _alloc_solver_buffers(self, grad: bool) -> None:
device = self.device
model = self.model
self.qd_zero = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, device=device)
self.body_q = wp.zeros((self.n_batch, model.body_count), dtype=wp.transform, requires_grad=grad, device=device)
self.body_qd = (
wp.zeros((self.n_batch, model.body_count), dtype=wp.spatial_vector, device=device) if grad else None
)
self.residuals = wp.zeros((self.n_batch, self.n_residuals), dtype=wp.float32, requires_grad=grad, device=device)
self.residuals_proposed = wp.zeros(
(self.n_batch, self.n_residuals), dtype=wp.float32, requires_grad=grad, device=device
)
self.residuals_3d = wp.zeros((self.n_batch, self.n_residuals, 1), dtype=wp.float32, device=device)
self.jacobian = wp.zeros((self.n_batch, self.n_residuals, self.n_dofs), dtype=wp.float32, device=device)
self.dq_dof = wp.zeros((self.n_batch, self.n_dofs), dtype=wp.float32, requires_grad=grad, device=device)
self.joint_q_proposed = wp.zeros(
(self.n_batch, self.n_coords), dtype=wp.float32, requires_grad=grad, device=device
)
self.costs = wp.zeros(self.n_batch, dtype=wp.float32, device=device)
self.costs_proposed = wp.zeros(self.n_batch, dtype=wp.float32, device=device)
self.lambda_values = wp.zeros(self.n_batch, dtype=wp.float32, device=device)
self.accept_flags = wp.zeros(self.n_batch, dtype=wp.int32, device=device)
self.pred_reduction = wp.zeros(self.n_batch, dtype=wp.float32, device=device)
self.problem_idx_identity = wp.array(np.arange(self.n_batch, dtype=np.int32), dtype=wp.int32, device=device)
self.X_local = wp.zeros((self.n_batch, model.joint_count), dtype=wp.transform, device=device)
self.joint_S_s = (
wp.zeros((self.n_batch, self.n_dofs), dtype=wp.spatial_vector, device=device)
if self.jacobian_mode != IKJacobianType.AUTODIFF and self.has_analytic_objective
else None
)
def _build_residual_offsets(self) -> None:
offsets: list[int] = []
offset = 0
for obj in self.objectives:
offsets.append(offset)
offset += obj.residual_dim()
self.residual_offsets = offsets
def _ctx_solver(
self,
joint_q: wp.array2d[wp.float32],
*,
residuals: wp.array2d[wp.float32] | None = None,
jacobian: wp.array3d[wp.float32] | None = None,
) -> BatchCtx:
ctx = BatchCtx(
joint_q=joint_q,
residuals=residuals if residuals is not None else self.residuals,
fk_body_q=self.body_q,
problem_idx=self.problem_idx,
fk_body_qd=getattr(self, "body_qd", None),
dq_dof=self.dq_dof,
joint_q_proposed=self.joint_q_proposed,
joint_qd=self.qd_zero,
jacobian_out=jacobian if jacobian is not None else self.jacobian,
motion_subspace=getattr(self, "joint_S_s", None),
fk_qd_zero=self.qd_zero,
fk_X_local=self.X_local,
)
self._validate_ctx_for_mode(ctx)
return ctx
def _validate_ctx_for_mode(self, ctx: BatchCtx) -> None:
missing: list[str] = []
for name in ("joint_q", "residuals", "fk_body_q", "problem_idx"):
if getattr(ctx, name) is None:
missing.append(name)
mode = self.jacobian_mode
if mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED):
for name in ("fk_body_qd", "dq_dof", "joint_q_proposed", "joint_qd"):
if getattr(ctx, name) is None:
missing.append(name)
needs_analytic = mode == IKJacobianType.ANALYTIC or (
mode == IKJacobianType.MIXED and self.has_analytic_objective
)
if needs_analytic:
for name in ("jacobian_out", "motion_subspace", "fk_qd_zero"):
if getattr(ctx, name) is None:
missing.append(name)
if ctx.fk_X_local is None:
missing.append("fk_X_local")
if missing:
raise RuntimeError(f"solver context missing: {', '.join(missing)}")
def _for_objectives_residuals(self, ctx: BatchCtx) -> None:
def _do(obj, offset, body_q_view, joint_q_view, model, output_residuals, problem_idx_array):
obj.compute_residuals(
body_q_view,
joint_q_view,
model,
output_residuals,
offset,
problem_idx=problem_idx_array,
)
self._parallel_for_objectives(
_do,
ctx.fk_body_q,
ctx.joint_q,
self.model,
ctx.residuals,
ctx.problem_idx,
)
def _residuals_autodiff(self, ctx: BatchCtx) -> None:
eval_fk_batched(
self.model,
ctx.joint_q,
ctx.joint_qd,
ctx.fk_body_q,
ctx.fk_body_qd,
)
ctx.residuals.zero_()
self._for_objectives_residuals(ctx)
def _residuals_analytic(self, ctx: BatchCtx) -> None:
self._fk_two_pass(
self.model,
ctx.joint_q,
ctx.fk_body_q,
ctx.fk_X_local,
ctx.joint_q.shape[0],
)
ctx.residuals.zero_()
self._for_objectives_residuals(ctx)
def _jacobian_at(self, ctx: BatchCtx) -> wp.array3d[wp.float32]:
mode = self.jacobian_mode
if mode == IKJacobianType.AUTODIFF:
self._jacobian_autodiff(ctx)
return ctx.jacobian_out
if mode == IKJacobianType.ANALYTIC:
self._jacobian_analytic(ctx, accumulate=False)
return ctx.jacobian_out
# MIXED mode
if self.has_autodiff_objective:
self._jacobian_autodiff(ctx)
else:
ctx.jacobian_out.zero_()
if self.has_analytic_objective:
self._jacobian_analytic(ctx, accumulate=self.has_autodiff_objective)
return ctx.jacobian_out
def _jacobian_autodiff(self, ctx: BatchCtx) -> None:
if self.tape is None:
raise RuntimeError("Autodiff Jacobian requested but tape is not initialized")
ctx.jacobian_out.zero_()
self.tape.reset()
self.tape.gradients = {}
ctx.dq_dof.zero_()
with self.tape:
self._integrate_dq(
ctx.joint_q,
dq_in=ctx.dq_dof,
joint_q_out=ctx.joint_q_proposed,
joint_qd_out=ctx.joint_qd,
)
res_ctx = BatchCtx(
joint_q=ctx.joint_q_proposed,
residuals=ctx.residuals,
fk_body_q=ctx.fk_body_q,
problem_idx=ctx.problem_idx,
fk_body_qd=ctx.fk_body_qd,
joint_qd=ctx.joint_qd,
)
self._residuals_autodiff(res_ctx)
residuals_flat = ctx.residuals.flatten()
self.tape.outputs = [residuals_flat]
for obj, offset in zip(self.objectives, self.residual_offsets, strict=False):
if self.jacobian_mode == IKJacobianType.MIXED and obj.supports_analytic():
continue
obj.compute_jacobian_autodiff(self.tape, self.model, ctx.jacobian_out, offset, ctx.dq_dof)
self.tape.zero()
def _jacobian_analytic(self, ctx: BatchCtx, *, accumulate: bool) -> None:
if not accumulate:
ctx.jacobian_out.zero_()
ctx.fk_qd_zero.zero_()
self._compute_motion_subspace(
body_q=ctx.fk_body_q,
joint_S_s_out=ctx.motion_subspace,
joint_qd_in=ctx.fk_qd_zero,
)
def _emit(obj, off, body_q_view, joint_q_view, model, jac_view, motion_subspace_view):
if obj.supports_analytic():
obj.compute_jacobian_analytic(body_q_view, joint_q_view, model, jac_view, motion_subspace_view, off)
elif not accumulate:
raise ValueError(f"Objective {type(obj).__name__} does not support analytic Jacobian")
self._parallel_for_objectives(
_emit,
ctx.fk_body_q,
ctx.joint_q,
self.model,
ctx.jacobian_out,
ctx.motion_subspace,
)
def step(
self,
joint_q_in: wp.array2d[wp.float32],
joint_q_out: wp.array2d[wp.float32],
iterations: int = 10,
step_size: float = 1.0,
) -> None:
"""Run several LM iterations on a batch of joint configurations.
Args:
joint_q_in: Input joint coordinates, shape [n_batch, joint_coord_count].
joint_q_out: Output buffer for the optimized coordinates, shape
[n_batch, joint_coord_count]. It may alias ``joint_q_in`` for
in-place updates.
iterations: Number of LM iterations to execute.
step_size: Scalar applied to each computed update before
integration.
"""
if joint_q_in.shape != (self.n_batch, self.n_coords):
raise ValueError("joint_q_in has incompatible shape")
if joint_q_out.shape != (self.n_batch, self.n_coords):
raise ValueError("joint_q_out has incompatible shape")
if joint_q_in.ptr != joint_q_out.ptr:
wp.copy(joint_q_out, joint_q_in)
joint_q = joint_q_out
self.lambda_values.fill_(self.lambda_initial)
for i in range(iterations):
self._step(joint_q, step_size=step_size, iteration=i)
def _compute_residuals(
self,
joint_q: wp.array2d[wp.float32],
output_residuals: wp.array2d[wp.float32] | None = None,
) -> wp.array2d[wp.float32]:
buffer = output_residuals or self.residuals
ctx = self._ctx_solver(joint_q, residuals=buffer)
if self.jacobian_mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED):
self._residuals_autodiff(ctx)
else:
self._residuals_analytic(ctx)
return ctx.residuals
def _compute_motion_subspace(
self,
*,
body_q: wp.array2d[wp.transform],
joint_S_s_out: wp.array2d[wp.spatial_vector],
joint_qd_in: wp.array2d[wp.float32],
) -> None:
n_joints = self.model.joint_count
batch = body_q.shape[0]
wp.launch(
self._compute_motion_subspace_2d,
dim=[batch, n_joints],
inputs=[
self.model.joint_type,
self.model.joint_parent,
self.model.joint_qd_start,
joint_qd_in,
self.model.joint_axis,
self.model.joint_dof_dim,
body_q,
self.model.joint_X_p,
],
outputs=[
joint_S_s_out,
],
device=self.device,
)
def _integrate_dq(
self,
joint_q: wp.array2d[wp.float32],
*,
dq_in: wp.array2d[wp.float32],
joint_q_out: wp.array2d[wp.float32],
joint_qd_out: wp.array2d[wp.float32],
step_size: float = 1.0,
) -> None:
batch = joint_q.shape[0]
wp.launch(
self._integrate_dq_dof,
dim=[batch, self.model.joint_count],
inputs=[
self.model.joint_type,
self.model.joint_q_start,
self.model.joint_qd_start,
self.model.joint_dof_dim,
joint_q,
dq_in,
joint_qd_out,
step_size,
],
outputs=[
joint_q_out,
joint_qd_out,
],
device=self.device,
)
joint_qd_out.zero_()
def _step(
self,
joint_q: wp.array2d[wp.float32],
step_size: float = 1.0,
iteration: int = 0,
) -> None:
"""Execute one Levenberg-Marquardt iteration with adaptive damping."""
ctx_curr = self._ctx_solver(joint_q)
if iteration == 0:
if self.jacobian_mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED):
self._residuals_autodiff(ctx_curr)
else:
self._residuals_analytic(ctx_curr)
wp.launch(
compute_costs,
dim=self.n_batch,
inputs=[ctx_curr.residuals, self.n_residuals],
outputs=[self.costs],
device=self.device,
)
self._jacobian_at(ctx_curr)
residuals_flat = ctx_curr.residuals.flatten()
residuals_3d_flat = self.residuals_3d.flatten()
wp.copy(residuals_3d_flat, residuals_flat)
self.dq_dof.zero_()
self._solve_tiled(
ctx_curr.jacobian_out, self.residuals_3d, self.lambda_values, self.dq_dof, self.pred_reduction
)
self._integrate_dq(
joint_q,
dq_in=self.dq_dof,
joint_q_out=self.joint_q_proposed,
joint_qd_out=self.qd_zero,
step_size=step_size,
)
ctx_prop = self._ctx_solver(self.joint_q_proposed, residuals=self.residuals_proposed)
if self.jacobian_mode in (IKJacobianType.AUTODIFF, IKJacobianType.MIXED):
self._residuals_autodiff(ctx_prop)
else:
self._residuals_analytic(ctx_prop)
wp.launch(
compute_costs,
dim=self.n_batch,
inputs=[self.residuals_proposed, self.n_residuals],
outputs=[self.costs_proposed],
device=self.device,
)
wp.launch(
_accept_reject,
dim=self.n_batch,
inputs=[self.costs, self.costs_proposed, self.pred_reduction, self.rho_min],
outputs=[self.accept_flags],
device=self.device,
)
wp.launch(
_update_lm_state,
dim=self.n_batch,
inputs=[
self.joint_q_proposed,
self.residuals_proposed,
self.costs_proposed,
self.accept_flags,
self.n_coords,
self.n_residuals,
self.lambda_factor,
self.lambda_min,
self.lambda_max,
],
outputs=[joint_q, self.residuals, self.costs, self.lambda_values],
device=self.device,
)
def reset(self) -> None:
"""Clear LM damping and accept/reject state before a new solve."""
self.lambda_values.zero_()
self.accept_flags.zero_()
def compute_costs(self, joint_q: wp.array2d[wp.float32]) -> wp.array[wp.float32]:
"""Evaluate squared residual costs for a batch of joint configurations.
Args:
joint_q: Joint coordinates to evaluate, shape [n_batch, joint_coord_count].
Returns:
Costs for each batch row, shape [n_batch].
"""
self._compute_residuals(joint_q)
wp.launch(
compute_costs,
dim=self.n_batch,
inputs=[self.residuals, self.n_residuals],
outputs=[self.costs],
device=self.device,
)
return self.costs
def _solve_tiled(
self,
jacobian: wp.array3d[wp.float32],
residuals: wp.array3d[wp.float32],
lambda_values: wp.array[wp.float32],
dq_dof: wp.array2d[wp.float32],
pred_reduction: wp.array[wp.float32],
) -> None:
raise NotImplementedError("This method should be overridden by specialized solver")
@classmethod
def _build_specialized(cls, key: tuple[int, int, str]) -> type[IKOptimizerLM]:
"""Build a specialized IKOptimizerLM subclass with tiled solver for given dimensions."""
C, R, _ = key
def _template(
jacobians: wp.array3d[wp.float32], # (n_batch, n_residuals, n_dofs)
residuals: wp.array3d[wp.float32], # (n_batch, n_residuals, 1)
lambda_values: wp.array[wp.float32], # (n_batch)
# outputs
dq_dof: wp.array2d[wp.float32], # (n_batch, n_dofs)
pred_reduction_out: wp.array[wp.float32], # (n_batch)
):
row = wp.tid()
RES = _Specialized.TILE_N_RESIDUALS
DOF = _Specialized.TILE_N_DOFS
J = wp.tile_load(jacobians[row], shape=(RES, DOF))
r = wp.tile_load(residuals[row], shape=(RES, 1))
lam = lambda_values[row]
Jt = wp.tile_transpose(J)
JtJ = wp.tile_zeros(shape=(DOF, DOF), dtype=wp.float32)
wp.tile_matmul(Jt, J, JtJ)
diag = wp.tile_zeros(shape=(DOF,), dtype=wp.float32)
for i in range(DOF):
diag[i] = lam
A = wp.tile_diag_add(JtJ, diag)
g = wp.tile_zeros(shape=(DOF,), dtype=wp.float32)
tmp2d = wp.tile_zeros(shape=(DOF, 1), dtype=wp.float32)
wp.tile_matmul(Jt, r, tmp2d)
for i in range(DOF):
g[i] = tmp2d[i, 0]
rhs = wp.tile_map(wp.neg, g)
L = wp.tile_cholesky(A)
delta = wp.tile_cholesky_solve(L, rhs)
wp.tile_store(dq_dof[row], delta)
lambda_delta = wp.tile_zeros(shape=(DOF,), dtype=wp.float32)
for i in range(DOF):
lambda_delta[i] = lam * delta[i]
diff = wp.tile_map(wp.sub, lambda_delta, g)
prod = wp.tile_map(wp.mul, delta, diff)
red = wp.tile_sum(prod)[0]
pred_reduction_out[row] = 0.5 * red
_template.__name__ = f"_lm_solve_tiled_{C}_{R}"
_template.__qualname__ = f"_lm_solve_tiled_{C}_{R}"
_lm_solve_tiled = wp.kernel(enable_backward=False, module="unique")(_template)
# late-import jcalc_motion, jcalc_transform to avoid circular import error
from ...solvers.featherstone.kernels import ( # noqa: PLC0415
jcalc_integrate,
jcalc_motion,
jcalc_transform,
)
@wp.kernel
def _integrate_dq_dof(
# model-wide
joint_type: wp.array[wp.int32], # (n_joints)
joint_q_start: wp.array[wp.int32], # (n_joints + 1)
joint_qd_start: wp.array[wp.int32], # (n_joints + 1)
joint_dof_dim: wp.array2d[wp.int32], # (n_joints, 2) → (lin, ang)
# per-row
joint_q_curr: wp.array2d[wp.float32], # (n_batch, n_coords)
joint_qd_curr: wp.array2d[wp.float32], # (n_batch, n_dofs) (typically all-zero)
dq_dof: wp.array2d[wp.float32], # (n_batch, n_dofs) ← update direction (q̇)
dt: float, # step scale (usually 1.0)
# outputs
joint_q_out: wp.array2d[wp.float32], # (n_batch, n_coords)
joint_qd_out: wp.array2d[wp.float32], # (n_batch, n_dofs)
):
"""
Integrate the candidate update ``dq_dof`` (interpreted as a
joint-space velocity times ``dt``) into a new configuration.
q_out = integrate(q_curr, dq_dof)
One thread handles one joint of one batch row. All joint types
supported by ``jcalc_integrate`` (revolute, prismatic, ball,
free, D6, ...) work out of the box.
"""
row, joint_idx = wp.tid()
# Static joint metadata
t = joint_type[joint_idx]
coord_start = joint_q_start[joint_idx]
dof_start = joint_qd_start[joint_idx]
lin_axes = joint_dof_dim[joint_idx, 0]
ang_axes = joint_dof_dim[joint_idx, 1]
# Views into the current batch row
q_row = joint_q_curr[row]
qd_row = joint_qd_curr[row] # typically zero
delta_row = dq_dof[row] # update vector
q_out_row = joint_q_out[row]
qd_out_row = joint_qd_out[row]
# Treat `delta_row` as acceleration with dt=1:
# qd_new = 0 + delta (qd ← delta)
# q_new = q + qd_new * dt (q ← q + delta)
jcalc_integrate(
t,
q_row,
qd_row,
delta_row, # passed as joint_qdd
coord_start,
dof_start,
lin_axes,
ang_axes,
dt,
q_out_row,
qd_out_row,
)
@wp.kernel(module="unique")
def _compute_motion_subspace_2d(
joint_type: wp.array[wp.int32], # (n_joints)
joint_parent: wp.array[wp.int32], # (n_joints)
joint_qd_start: wp.array[wp.int32], # (n_joints + 1)
joint_qd: wp.array2d[wp.float32], # (n_batch, n_joint_dof_count)
joint_axis: wp.array[wp.vec3], # (n_joint_dof_count)
joint_dof_dim: wp.array2d[wp.int32], # (n_joints, 2)
body_q: wp.array2d[wp.transform], # (n_batch, n_bodies)
joint_X_p: wp.array[wp.transform], # (n_joints)
# outputs
joint_S_s: wp.array2d[wp.spatial_vector], # (n_batch, n_joint_dof_count)
):
row, joint_idx = wp.tid()
type = joint_type[joint_idx]
parent = joint_parent[joint_idx]
qd_start = joint_qd_start[joint_idx]
X_pj = joint_X_p[joint_idx]
X_wpj = X_pj
if parent >= 0:
X_wpj = body_q[row, parent] * X_pj
lin_axis_count = joint_dof_dim[joint_idx, 0]
ang_axis_count = joint_dof_dim[joint_idx, 1]
joint_qd_1d = joint_qd[row]
S_s_out = joint_S_s[row]
jcalc_motion(
type,
joint_axis,
lin_axis_count,
ang_axis_count,
X_wpj,
joint_qd_1d,
qd_start,
S_s_out,
)
@wp.kernel(module="unique")
def _fk_local(
joint_type: wp.array[wp.int32], # (n_joints)
joint_q: wp.array2d[wp.float32], # (n_batch, n_coords)
joint_q_start: wp.array[wp.int32], # (n_joints + 1)
joint_qd_start: wp.array[wp.int32], # (n_joints + 1)
joint_axis: wp.array[wp.vec3], # (n_axes)
joint_dof_dim: wp.array2d[wp.int32], # (n_joints, 2) → (lin, ang)
joint_X_p: wp.array[wp.transform], # (n_joints)
joint_X_c: wp.array[wp.transform], # (n_joints)
# outputs
X_local_out: wp.array2d[wp.transform], # (n_batch, n_joints)
):
row, local_joint_idx = wp.tid()
t = joint_type[local_joint_idx]
q_start = joint_q_start[local_joint_idx]
axis_start = joint_qd_start[local_joint_idx]
lin_axes = joint_dof_dim[local_joint_idx, 0]
ang_axes = joint_dof_dim[local_joint_idx, 1]
X_j = jcalc_transform(
t,
joint_axis,
axis_start,
lin_axes,
ang_axes,
joint_q[row], # 1-D row slice
q_start,
)
X_rel = joint_X_p[local_joint_idx] * X_j * wp.transform_inverse(joint_X_c[local_joint_idx])
X_local_out[row, local_joint_idx] = X_rel
def _fk_two_pass(model, joint_q, body_q, X_local, n_batch):
"""Compute forward kinematics using two-pass algorithm.
Args:
model: newton.Model instance
joint_q: 2D array [n_batch, joint_coord_count]
body_q: 2D array [n_batch, body_count] (output)
X_local: 2D array [n_batch, joint_count] (workspace)
n_batch: Number of rows to process
"""
wp.launch(
_fk_local,
dim=[n_batch, model.joint_count],
inputs=[
model.joint_type,
joint_q,
model.joint_q_start,
model.joint_qd_start,
model.joint_axis,
model.joint_dof_dim,
model.joint_X_p,
model.joint_X_c,
],
outputs=[
X_local,
],
device=model.device,
)
wp.launch(
fk_accum,
dim=[n_batch, model.joint_count],
inputs=[
model.joint_parent,
X_local,
],
outputs=[
body_q,
],
device=model.device,
)
class _Specialized(IKOptimizerLM):
TILE_N_DOFS = wp.constant(C)
TILE_N_RESIDUALS = wp.constant(R)
TILE_THREADS = wp.constant(32)
def _solve_tiled(
self,
jac: wp.array3d[wp.float32],
res: wp.array3d[wp.float32],
lam: wp.array[wp.float32],
dq: wp.array2d[wp.float32],
pred: wp.array[wp.float32],
) -> None:
wp.launch_tiled(
_lm_solve_tiled,
dim=[self.n_batch],
inputs=[jac, res, lam, dq, pred],
block_dim=self.TILE_THREADS,
device=self.device,
)
_Specialized.__name__ = f"IK_{C}x{R}"
_Specialized._integrate_dq_dof = staticmethod(_integrate_dq_dof)
_Specialized._compute_motion_subspace_2d = staticmethod(_compute_motion_subspace_2d)
_Specialized._fk_two_pass = staticmethod(_fk_two_pass)
return _Specialized
================================================
FILE: newton/_src/sim/ik/ik_objectives.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Objective definitions for inverse kinematics."""
from __future__ import annotations
import numpy as np
import warp as wp
from ..model import Model
from .ik_common import IKJacobianType
class IKObjective:
"""Base class for inverse-kinematics objectives.
Each objective contributes one or more residual rows to the global IK
system and can optionally provide an analytic Jacobian. Objective
instances are shared across a batch of problems, so per-problem data such
as targets should live in device arrays and be indexed through the
``problem_idx`` mapping supplied at evaluation time.
Subclasses should override :meth:`residual_dim`,
:meth:`compute_residuals`, and :meth:`compute_jacobian_autodiff`. They can
additionally override :meth:`supports_analytic` and
:meth:`compute_jacobian_analytic` when an analytic Jacobian is available.
"""
def __init__(self) -> None:
# Optimizers assign these before first use via set_batch_layout().
self.total_residuals = None
self.residual_offset = None
self.n_batch = None
def set_batch_layout(self, total_residuals: int, residual_offset: int, n_batch: int) -> None:
"""Register this objective's rows inside the optimizer's global system.
Args:
total_residuals: Total number of residual rows across all
objectives.
residual_offset: First row reserved for this objective inside the
global residual vector and Jacobian.
n_batch: Number of evaluation rows processed together. This is the
expanded solver batch, not necessarily the number of base IK
problems.
.. note::
Per-problem buffers such as targets should still be sized by the
base problem count and accessed through the ``problem_idx`` mapping
supplied during residual and Jacobian evaluation.
"""
self.total_residuals = total_residuals
self.residual_offset = residual_offset
self.n_batch = n_batch
def _require_batch_layout(self) -> None:
if self.total_residuals is None or self.residual_offset is None or self.n_batch is None:
raise RuntimeError(f"Batch layout not assigned for {type(self).__name__}; call set_batch_layout() first")
def residual_dim(self) -> int:
"""Return the number of residual rows contributed by this objective."""
raise NotImplementedError
def compute_residuals(
self,
body_q: wp.array2d[wp.transform],
joint_q: wp.array2d[wp.float32],
model: Model,
residuals: wp.array2d[wp.float32],
start_idx: int,
problem_idx: wp.array[wp.int32],
) -> None:
"""Write this objective's residual block into a global buffer.
Args:
body_q: Batched body transforms for the current evaluation rows,
shape [n_batch, body_count].
joint_q: Batched joint coordinates [m or rad] for the current
evaluation rows, shape [n_batch, joint_coord_count].
model: Shared articulation model.
residuals: Global residual buffer that receives this objective's
residual rows, shape [n_batch, total_residual_count].
start_idx: First residual row reserved for this objective inside
the global residual buffer.
problem_idx: Mapping from evaluation rows to base problem
indices, shape [n_batch]. Use this when objective data is
stored once per original problem but the solver expands rows
for multiple seeds or line-search candidates.
"""
raise NotImplementedError
def compute_jacobian_autodiff(
self,
tape: wp.Tape,
model: Model,
jacobian: wp.array3d[wp.float32],
start_idx: int,
dq_dof: wp.array2d[wp.float32],
) -> None:
"""Fill this objective's Jacobian block with autodiff gradients.
Args:
tape: Recorded Warp tape whose output is the global residual
buffer.
model: Shared articulation model.
jacobian: Global Jacobian buffer to update,
shape [n_batch, total_residual_count, joint_dof_count].
start_idx: First residual row reserved for this objective.
dq_dof: Differentiable joint update variable for the current
batch, shape [n_batch, joint_dof_count].
"""
raise NotImplementedError
def supports_analytic(self) -> bool:
"""Return ``True`` when this objective implements an analytic Jacobian."""
return False
def bind_device(self, device: wp.DeviceLike) -> None:
"""Bind this objective to the Warp device used by the solver."""
self.device = device
def init_buffers(self, model: Model, jacobian_mode: IKJacobianType) -> None:
"""Allocate any per-objective buffers needed by the chosen backend.
Args:
model: Shared articulation model.
jacobian_mode: Jacobian backend that will evaluate this
objective.
"""
pass
def compute_jacobian_analytic(
self,
body_q: wp.array2d[wp.transform],
joint_q: wp.array2d[wp.float32],
model: Model,
jacobian: wp.array3d[wp.float32],
joint_S_s: wp.array2d[wp.spatial_vector],
start_idx: int,
) -> None:
"""Fill this objective's Jacobian block analytically, if supported.
Args:
body_q: Batched body transforms for the current evaluation rows,
shape [n_batch, body_count].
joint_q: Batched joint coordinates for the current evaluation rows,
shape [n_batch, joint_coord_count].
model: Shared articulation model.
jacobian: Global Jacobian buffer to update,
shape [n_batch, total_residual_count, joint_dof_count].
joint_S_s: Batched motion-subspace columns,
shape [n_batch, joint_dof_count].
start_idx: First residual row reserved for this objective.
"""
pass
@wp.kernel
def _pos_residuals(
body_q: wp.array2d[wp.transform], # (n_batch, n_bodies)
target_pos: wp.array[wp.vec3], # (n_problems)
link_index: int,
link_offset: wp.vec3,
start_idx: int,
weight: float,
problem_idx_map: wp.array[wp.int32],
# outputs
residuals: wp.array2d[wp.float32], # (n_batch, n_residuals)
):
row = wp.tid()
base = problem_idx_map[row]
body_tf = body_q[row, link_index]
ee_pos = wp.transform_point(body_tf, link_offset)
error = target_pos[base] - ee_pos
residuals[row, start_idx + 0] = weight * error[0]
residuals[row, start_idx + 1] = weight * error[1]
residuals[row, start_idx + 2] = weight * error[2]
@wp.kernel
def _pos_jac_fill(
q_grad: wp.array2d[wp.float32], # (n_batch, n_dofs)
n_dofs: int,
start_idx: int,
component: int,
# outputs
jacobian: wp.array3d[wp.float32], # (n_batch, n_residuals, n_dofs)
):
problem_idx = wp.tid()
residual_idx = start_idx + component
for j in range(n_dofs):
jacobian[problem_idx, residual_idx, j] = q_grad[problem_idx, j]
@wp.kernel
def _update_position_target(
problem_idx: int,
new_position: wp.vec3,
# outputs
target_array: wp.array[wp.vec3], # (n_problems)
):
target_array[problem_idx] = new_position
@wp.kernel
def _update_position_targets(
new_positions: wp.array[wp.vec3], # (n_problems)
# outputs
target_array: wp.array[wp.vec3], # (n_problems)
):
problem_idx = wp.tid()
target_array[problem_idx] = new_positions[problem_idx]
@wp.kernel
def _pos_jac_analytic(
link_index: int,
link_offset: wp.vec3,
affects_dof: wp.array[wp.uint8], # (n_dofs)
body_q: wp.array2d[wp.transform], # (n_batch, n_bodies)
joint_S_s: wp.array2d[wp.spatial_vector], # (n_batch, n_dofs)
start_idx: int,
n_dofs: int,
weight: float,
# output
jacobian: wp.array3d[wp.float32], # (n_batch, n_residuals, n_dofs)
):
# one thread per (problem, dof)
problem_idx, dof_idx = wp.tid()
# skip if this DoF cannot move the EE
if affects_dof[dof_idx] == 0:
return
# world-space EE position
body_tf = body_q[problem_idx, link_index]
rot_w = wp.quat(body_tf[3], body_tf[4], body_tf[5], body_tf[6])
pos_w = wp.vec3(body_tf[0], body_tf[1], body_tf[2])
ee_pos_world = pos_w + wp.quat_rotate(rot_w, link_offset)
# motion sub-space column S
S = joint_S_s[problem_idx, dof_idx]
v_orig = wp.vec3(S[0], S[1], S[2])
omega = wp.vec3(S[3], S[4], S[5])
v_ee = v_orig + wp.cross(omega, ee_pos_world)
# write three Jacobian rows (x, y, z)
jacobian[problem_idx, start_idx + 0, dof_idx] = -weight * v_ee[0]
jacobian[problem_idx, start_idx + 1, dof_idx] = -weight * v_ee[1]
jacobian[problem_idx, start_idx + 2, dof_idx] = -weight * v_ee[2]
class IKObjectivePosition(IKObjective):
"""Match the world-space position of a point on a link.
Args:
link_index: Body index whose frame defines the constrained link.
link_offset: Point in the link's local frame [m].
target_positions: Target positions [m], shape [problem_count].
weight: Scalar multiplier applied to the residual and Jacobian rows.
"""
def __init__(
self,
link_index: int,
link_offset: wp.vec3,
target_positions: wp.array[wp.vec3],
weight: float = 1.0,
) -> None:
super().__init__()
self.link_index = link_index
self.link_offset = link_offset
self.target_positions = target_positions
self.weight = weight
self.affects_dof = None
self.e_arrays = None
def init_buffers(self, model: Model, jacobian_mode: IKJacobianType) -> None:
"""Initialize caches used by analytic or autodiff Jacobian evaluation.
Args:
model: Shared articulation model.
jacobian_mode: Jacobian backend selected for this objective.
"""
self._require_batch_layout()
if jacobian_mode == IKJacobianType.ANALYTIC:
joint_qd_start_np = model.joint_qd_start.numpy()
dof_to_joint_np = np.empty(joint_qd_start_np[-1], dtype=np.int32)
for j in range(len(joint_qd_start_np) - 1):
dof_to_joint_np[joint_qd_start_np[j] : joint_qd_start_np[j + 1]] = j
links_per_problem = model.body_count
joint_child_np = model.joint_child.numpy()
body_to_joint_np = np.full(links_per_problem, -1, np.int32)
for j in range(model.joint_count):
child = joint_child_np[j]
if child != -1:
body_to_joint_np[child] = j
joint_q_start_np = model.joint_q_start.numpy()
ancestors = np.zeros(len(joint_q_start_np) - 1, dtype=bool)
joint_parent_np = model.joint_parent.numpy()
body = self.link_index
while body != -1:
j = body_to_joint_np[body]
if j != -1:
ancestors[j] = True
body = joint_parent_np[j] if j != -1 else -1
affects_dof_np = ancestors[dof_to_joint_np]
self.affects_dof = wp.array(affects_dof_np.astype(np.uint8), device=self.device)
elif jacobian_mode == IKJacobianType.AUTODIFF:
self.e_arrays = []
for component in range(3):
e = np.zeros((self.n_batch, self.total_residuals), dtype=np.float32)
for prob_idx in range(self.n_batch):
e[prob_idx, self.residual_offset + component] = 1.0
self.e_arrays.append(wp.array(e.flatten(), dtype=wp.float32, device=self.device))
def supports_analytic(self) -> bool:
"""Return ``True`` because this objective has an analytic Jacobian."""
return True
def set_target_position(self, problem_idx: int, new_position: wp.vec3) -> None:
"""Update the target position for a single base IK problem.
Args:
problem_idx: Base problem index to update.
new_position: Replacement target position [m] in world
coordinates.
"""
self._require_batch_layout()
wp.launch(
_update_position_target,
dim=1,
inputs=[problem_idx, new_position],
outputs=[self.target_positions],
device=self.device,
)
def set_target_positions(self, new_positions: wp.array[wp.vec3]) -> None:
"""Replace the target positions for all base IK problems.
Args:
new_positions: Target positions [m], shape [problem_count].
"""
self._require_batch_layout()
expected = self.target_positions.shape[0]
if new_positions.shape[0] != expected:
raise ValueError(f"Expected {expected} target positions, got {new_positions.shape[0]}")
wp.launch(
_update_position_targets,
dim=expected,
inputs=[new_positions],
outputs=[self.target_positions],
device=self.device,
)
def residual_dim(self) -> int:
"""Return the three translational residual rows for this objective."""
return 3
def compute_residuals(
self,
body_q: wp.array2d[wp.transform],
joint_q: wp.array2d[wp.float32],
model: Model,
residuals: wp.array2d[wp.float32],
start_idx: int,
problem_idx: wp.array[wp.int32],
) -> None:
"""Write weighted position errors into the global residual buffer.
Args:
body_q: Batched body transforms for the evaluation rows,
shape [n_batch, body_count].
joint_q: Batched joint coordinates, shape [n_batch, joint_coord_count].
Present for interface compatibility and not used directly by
this objective.
model: Shared articulation model. Present for interface
compatibility.
residuals: Global residual buffer to update,
shape [n_batch, total_residual_count].
start_idx: First residual row reserved for this objective.
problem_idx: Mapping from evaluation rows to base problem
indices, shape [n_batch], used to fetch ``target_positions``.
"""
count = body_q.shape[0]
wp.launch(
_pos_residuals,
dim=count,
inputs=[
body_q,
self.target_positions,
self.link_index,
self.link_offset,
start_idx,
self.weight,
problem_idx,
],
outputs=[residuals],
device=self.device,
)
def compute_jacobian_autodiff(
self,
tape: wp.Tape,
model: Model,
jacobian: wp.array3d[wp.float32],
start_idx: int,
dq_dof: wp.array2d[wp.float32],
) -> None:
"""Fill the position Jacobian block using Warp autodiff.
Args:
tape: Recorded Warp tape whose output is the global residual
buffer.
model: Shared articulation model.
jacobian: Global Jacobian buffer to update,
shape [n_batch, total_residual_count, joint_dof_count].
start_idx: First residual row reserved for this objective.
dq_dof: Differentiable joint update variable for the current
batch, shape [n_batch, joint_dof_count].
"""
self._require_batch_layout()
for component in range(3):
tape.backward(grads={tape.outputs[0]: self.e_arrays[component].flatten()})
q_grad = tape.gradients[dq_dof]
n_dofs = model.joint_dof_count
wp.launch(
_pos_jac_fill,
dim=self.n_batch,
inputs=[
q_grad,
n_dofs,
start_idx,
component,
],
outputs=[
jacobian,
],
device=self.device,
)
tape.zero()
def compute_jacobian_analytic(
self,
body_q: wp.array2d[wp.transform],
joint_q: wp.array2d[wp.float32],
model: Model,
jacobian: wp.array3d[wp.float32],
joint_S_s: wp.array2d[wp.spatial_vector],
start_idx: int,
) -> None:
"""Fill the position Jacobian block from the analytic motion subspace.
Args:
body_q: Batched body transforms for the evaluation rows,
shape [n_batch, body_count].
joint_q: Batched joint coordinates, shape [n_batch, joint_coord_count].
Present for interface compatibility and not used directly by
this objective.
model: Shared articulation model.
jacobian: Global Jacobian buffer to update,
shape [n_batch, total_residual_count, joint_dof_count].
joint_S_s: Batched motion-subspace columns,
shape [n_batch, joint_dof_count].
start_idx: First residual row reserved for this objective.
"""
n_dofs = model.joint_dof_count
wp.launch(
_pos_jac_analytic,
dim=[body_q.shape[0], n_dofs],
inputs=[
self.link_index,
self.link_offset,
self.affects_dof,
body_q,
joint_S_s,
start_idx,
n_dofs,
self.weight,
],
outputs=[jacobian],
device=self.device,
)
@wp.kernel
def _limit_residuals(
joint_q: wp.array2d[wp.float32], # (n_batch, n_coords)
joint_limit_lower: wp.array[wp.float32], # (n_dofs)
joint_limit_upper: wp.array[wp.float32], # (n_dofs)
dof_to_coord: wp.array[wp.int32], # (n_dofs)
n_dofs: int,
weight: float,
start_idx: int,
# outputs
residuals: wp.array2d[wp.float32], # (n_batch, n_residuals)
):
problem, dof_idx = wp.tid()
coord_idx = dof_to_coord[dof_idx]
if coord_idx < 0:
return
q = joint_q[problem, coord_idx]
lower = joint_limit_lower[dof_idx]
upper = joint_limit_upper[dof_idx]
# treat huge ranges as no limit
if upper - lower > 9.9e5:
return
viol = wp.max(0.0, q - upper) + wp.max(0.0, lower - q)
residuals[problem, start_idx + dof_idx] = weight * viol
@wp.kernel
def _limit_jac_fill(
q_grad: wp.array2d[wp.float32], # (n_batch, n_dofs)
n_dofs: int,
start_idx: int,
# outputs
jacobian: wp.array3d[wp.float32],
):
problem_idx, dof_idx = wp.tid()
jacobian[problem_idx, start_idx + dof_idx, dof_idx] = q_grad[problem_idx, dof_idx]
@wp.kernel
def _limit_jac_analytic(
joint_q: wp.array2d[wp.float32], # (n_batch, n_coords)
joint_limit_lower: wp.array[wp.float32], # (n_dofs)
joint_limit_upper: wp.array[wp.float32], # (n_dofs)
dof_to_coord: wp.array[wp.int32], # (n_dofs)
n_dofs: int,
start_idx: int,
weight: float,
# outputs
jacobian: wp.array3d[wp.float32], # (n_batch, n_residuals, n_dofs)
):
problem, dof_idx = wp.tid()
coord_idx = dof_to_coord[dof_idx]
if coord_idx < 0:
return
q = joint_q[problem, coord_idx]
lower = joint_limit_lower[dof_idx]
upper = joint_limit_upper[dof_idx]
if upper - lower > 9.9e5:
return
grad = float(0.0)
if q >= upper:
grad = weight
elif q <= lower:
grad = -weight
jacobian[problem, start_idx + dof_idx, dof_idx] = grad
class IKObjectiveJointLimit(IKObjective):
"""Penalize violations of per-DoF joint limits.
Each DoF contributes one residual row whose value is zero inside the valid
range and increases linearly once the coordinate exceeds its lower or upper
bound.
Args:
joint_limit_lower: Lower joint limits [m or rad], shape
[joint_dof_count].
joint_limit_upper: Upper joint limits [m or rad], shape
[joint_dof_count].
weight: Scalar multiplier applied to each limit-violation residual
row.
"""
def __init__(
self,
joint_limit_lower: wp.array[wp.float32],
joint_limit_upper: wp.array[wp.float32],
weight: float = 0.1,
) -> None:
super().__init__()
self.joint_limit_lower = joint_limit_lower
self.joint_limit_upper = joint_limit_upper
self.e_array = None
self.weight = weight
self.n_dofs = len(joint_limit_lower)
self.dof_to_coord = None
def init_buffers(self, model: Model, jacobian_mode: IKJacobianType) -> None:
"""Initialize autodiff seeds and the DoF-to-coordinate lookup table.
Args:
model: Shared articulation model.
jacobian_mode: Jacobian backend selected for this objective.
"""
self._require_batch_layout()
if jacobian_mode == IKJacobianType.AUTODIFF:
e = np.zeros((self.n_batch, self.total_residuals), dtype=np.float32)
for prob_idx in range(self.n_batch):
for dof_idx in range(self.n_dofs):
e[prob_idx, self.residual_offset + dof_idx] = 1.0
self.e_array = wp.array(e.flatten(), dtype=wp.float32, device=self.device)
dof_to_coord_np = np.full(self.n_dofs, -1, dtype=np.int32)
q_start_np = model.joint_q_start.numpy()
qd_start_np = model.joint_qd_start.numpy()
joint_dof_dim_np = model.joint_dof_dim.numpy()
for j in range(model.joint_count):
dof0 = qd_start_np[j]
coord0 = q_start_np[j]
lin, ang = joint_dof_dim_np[j] # (#transl, #rot)
for k in range(lin + ang):
dof_to_coord_np[dof0 + k] = coord0 + k
self.dof_to_coord = wp.array(dof_to_coord_np, dtype=wp.int32, device=self.device)
def supports_analytic(self) -> bool:
"""Return ``True`` because this objective has an analytic Jacobian."""
return True
def residual_dim(self) -> int:
"""Return one residual row per joint DoF."""
return self.n_dofs
def compute_residuals(
self,
body_q: wp.array2d[wp.transform],
joint_q: wp.array2d[wp.float32],
model: Model,
residuals: wp.array2d[wp.float32],
start_idx: int,
problem_idx: wp.array[wp.int32],
) -> None:
"""Write weighted joint-limit violations into the global residual buffer.
Args:
body_q: Batched body transforms, shape [n_batch, body_count].
Present for interface compatibility and not used directly by
this objective.
joint_q: Batched joint coordinates for the evaluation rows, shape
[n_batch, joint_coord_count].
model: Shared articulation model. Present for interface
compatibility.
residuals: Global residual buffer to update,
shape [n_batch, total_residual_count].
start_idx: First residual row reserved for this objective.
problem_idx: Mapping from evaluation rows to base problems, shape
[n_batch]. Ignored because joint limits are shared across all
problems.
"""
count = joint_q.shape[0]
wp.launch(
_limit_residuals,
dim=[count, self.n_dofs],
inputs=[
joint_q,
self.joint_limit_lower,
self.joint_limit_upper,
self.dof_to_coord,
self.n_dofs,
self.weight,
start_idx,
],
outputs=[residuals],
device=self.device,
)
def compute_jacobian_autodiff(
self,
tape: wp.Tape,
model: Model,
jacobian: wp.array3d[wp.float32],
start_idx: int,
dq_dof: wp.array2d[wp.float32],
) -> None:
"""Fill the limit Jacobian block using Warp autodiff.
Args:
tape: Recorded Warp tape whose output is the global residual
buffer.
model: Shared articulation model.
jacobian: Global Jacobian buffer to update,
shape [n_batch, total_residual_count, joint_dof_count].
start_idx: First residual row reserved for this objective.
dq_dof: Differentiable joint update variable for the current
batch, shape [n_batch, joint_dof_count].
"""
self._require_batch_layout()
tape.backward(grads={tape.outputs[0]: self.e_array})
q_grad = tape.gradients[dq_dof]
wp.launch(
_limit_jac_fill,
dim=[self.n_batch, self.n_dofs],
inputs=[
q_grad,
self.n_dofs,
start_idx,
],
outputs=[jacobian],
device=self.device,
)
def compute_jacobian_analytic(
self,
body_q: wp.array2d[wp.transform],
joint_q: wp.array2d[wp.float32],
model: Model,
jacobian: wp.array3d[wp.float32],
joint_S_s: wp.array2d[wp.spatial_vector],
start_idx: int,
) -> None:
"""Fill the limit Jacobian block with the piecewise-linear derivative.
Args:
body_q: Batched body transforms, shape [n_batch, body_count].
Present for interface compatibility and not used directly by
this objective.
joint_q: Batched joint coordinates for the evaluation rows, shape
[n_batch, joint_coord_count].
model: Shared articulation model.
jacobian: Global Jacobian buffer to update,
shape [n_batch, total_residual_count, joint_dof_count].
joint_S_s: Batched motion-subspace columns, shape [n_batch, joint_dof_count].
Ignored because the joint-limit derivative is diagonal in
joint coordinates.
start_idx: First residual row reserved for this objective.
"""
count = joint_q.shape[0]
wp.launch(
_limit_jac_analytic,
dim=[count, self.n_dofs],
inputs=[
joint_q,
self.joint_limit_lower,
self.joint_limit_upper,
self.dof_to_coord,
self.n_dofs,
start_idx,
self.weight,
],
outputs=[jacobian],
device=self.device,
)
@wp.kernel
def _rot_residuals(
body_q: wp.array2d[wp.transform], # (n_batch, n_bodies)
target_rot: wp.array[wp.vec4], # (n_problems)
link_index: int,
link_offset_rotation: wp.quat,
canonicalize_quat_err: wp.bool,
start_idx: int,
weight: float,
problem_idx_map: wp.array[wp.int32],
# outputs
residuals: wp.array2d[wp.float32], # (n_batch, n_residuals)
):
row = wp.tid()
base = problem_idx_map[row]
body_tf = body_q[row, link_index]
body_rot = wp.quat(body_tf[3], body_tf[4], body_tf[5], body_tf[6])
actual_rot = body_rot * link_offset_rotation
target_quat_vec = target_rot[base]
target_quat = wp.quat(target_quat_vec[0], target_quat_vec[1], target_quat_vec[2], target_quat_vec[3])
q_err = actual_rot * wp.quat_inverse(target_quat)
if canonicalize_quat_err and wp.dot(actual_rot, target_quat) < 0.0:
q_err = -q_err
v_norm = wp.sqrt(q_err[0] * q_err[0] + q_err[1] * q_err[1] + q_err[2] * q_err[2])
angle = 2.0 * wp.atan2(v_norm, q_err[3])
eps = float(1e-8)
axis_angle = wp.vec3(0.0, 0.0, 0.0)
if v_norm > eps:
axis = wp.vec3(q_err[0] / v_norm, q_err[1] / v_norm, q_err[2] / v_norm)
axis_angle = axis * angle
else:
axis_angle = wp.vec3(2.0 * q_err[0], 2.0 * q_err[1], 2.0 * q_err[2])
residuals[row, start_idx + 0] = weight * axis_angle[0]
residuals[row, start_idx + 1] = weight * axis_angle[1]
residuals[row, start_idx + 2] = weight * axis_angle[2]
@wp.kernel
def _rot_jac_fill(
q_grad: wp.array2d[wp.float32], # (n_batch, n_dofs)
n_dofs: int,
start_idx: int,
component: int,
# outputs
jacobian: wp.array3d[wp.float32], # (n_batch, n_residuals, n_dofs)
):
problem_idx = wp.tid()
residual_idx = start_idx + component
for j in range(n_dofs):
jacobian[problem_idx, residual_idx, j] = q_grad[problem_idx, j]
@wp.kernel
def _update_rotation_target(
problem_idx: int,
new_rotation: wp.vec4,
# outputs
target_array: wp.array[wp.vec4], # (n_problems)
):
target_array[problem_idx] = new_rotation
@wp.kernel
def _update_rotation_targets(
new_rotation: wp.array[wp.vec4], # (n_problems)
# outputs
target_array: wp.array[wp.vec4], # (n_problems)
):
problem_idx = wp.tid()
target_array[problem_idx] = new_rotation[problem_idx]
@wp.kernel
def _rot_jac_analytic(
affects_dof: wp.array[wp.uint8], # (n_dofs)
joint_S_s: wp.array2d[wp.spatial_vector], # (n_batch, n_dofs)
start_idx: int, # first residual row for this objective
n_dofs: int, # width of the global Jacobian
weight: float,
# output
jacobian: wp.array3d[wp.float32], # (n_batch, n_residuals, n_dofs)
):
# one thread per (problem, dof)
problem_idx, dof_idx = wp.tid()
# skip if this DoF cannot influence the EE rotation
if affects_dof[dof_idx] == 0:
return
# ω column from motion sub-space
S = joint_S_s[problem_idx, dof_idx]
omega = wp.vec3(S[3], S[4], S[5])
# write three Jacobian rows (rx, ry, rz)
jacobian[problem_idx, start_idx + 0, dof_idx] = weight * omega[0]
jacobian[problem_idx, start_idx + 1, dof_idx] = weight * omega[1]
jacobian[problem_idx, start_idx + 2, dof_idx] = weight * omega[2]
class IKObjectiveRotation(IKObjective):
"""Match the world-space orientation of a link frame.
Args:
link_index: Body index whose frame defines the constrained link.
link_offset_rotation: Local rotation from the body frame to the
constrained frame, stored in ``(x, y, z, w)`` order.
target_rotations: Target orientations, shape [problem_count]. Each
quaternion is stored in ``(x, y, z, w)`` order.
canonicalize_quat_err: If ``True``, flip equivalent quaternions so the
residual follows the short rotational arc.
weight: Scalar multiplier applied to the residual and Jacobian rows.
"""
def __init__(
self,
link_index: int,
link_offset_rotation: wp.quat,
target_rotations: wp.array[wp.vec4],
canonicalize_quat_err: bool = True,
weight: float = 1.0,
) -> None:
super().__init__()
self.link_index = link_index
self.link_offset_rotation = link_offset_rotation
self.target_rotations = target_rotations
self.weight = weight
self.canonicalize_quat_err = canonicalize_quat_err
self.affects_dof = None
self.e_arrays = None
def init_buffers(self, model: Model, jacobian_mode: IKJacobianType) -> None:
"""Initialize caches used by analytic or autodiff Jacobian evaluation.
Args:
model: Shared articulation model.
jacobian_mode: Jacobian backend selected for this objective.
"""
self._require_batch_layout()
if jacobian_mode == IKJacobianType.ANALYTIC:
joint_qd_start_np = model.joint_qd_start.numpy()
dof_to_joint_np = np.empty(joint_qd_start_np[-1], dtype=np.int32)
for j in range(len(joint_qd_start_np) - 1):
dof_to_joint_np[joint_qd_start_np[j] : joint_qd_start_np[j + 1]] = j
links_per_problem = model.body_count
joint_child_np = model.joint_child.numpy()
body_to_joint_np = np.full(links_per_problem, -1, np.int32)
for j in range(model.joint_count):
child = joint_child_np[j]
if child != -1:
body_to_joint_np[child] = j
joint_q_start_np = model.joint_q_start.numpy()
ancestors = np.zeros(len(joint_q_start_np) - 1, dtype=bool)
joint_parent_np = model.joint_parent.numpy()
body = self.link_index
while body != -1:
j = body_to_joint_np[body]
if j != -1:
ancestors[j] = True
body = joint_parent_np[j] if j != -1 else -1
affects_dof_np = ancestors[dof_to_joint_np]
self.affects_dof = wp.array(affects_dof_np.astype(np.uint8), device=self.device)
elif jacobian_mode == IKJacobianType.AUTODIFF:
self.e_arrays = []
for component in range(3):
e = np.zeros((self.n_batch, self.total_residuals), dtype=np.float32)
for prob_idx in range(self.n_batch):
e[prob_idx, self.residual_offset + component] = 1.0
self.e_arrays.append(wp.array(e.flatten(), dtype=wp.float32, device=self.device))
def supports_analytic(self) -> bool:
"""Return ``True`` because this objective has an analytic Jacobian."""
return True
def set_target_rotation(self, problem_idx: int, new_rotation: wp.vec4) -> None:
"""Update the target orientation for a single base IK problem.
Args:
problem_idx: Base problem index to update.
new_rotation: Replacement target quaternion in ``(x, y, z, w)``
order.
"""
self._require_batch_layout()
wp.launch(
_update_rotation_target,
dim=1,
inputs=[problem_idx, new_rotation],
outputs=[self.target_rotations],
device=self.device,
)
def set_target_rotations(self, new_rotations: wp.array[wp.vec4]) -> None:
"""Replace the target orientations for all base IK problems.
Args:
new_rotations: Target quaternions, shape [problem_count], in
``(x, y, z, w)`` order.
"""
self._require_batch_layout()
expected = self.target_rotations.shape[0]
if new_rotations.shape[0] != expected:
raise ValueError(f"Expected {expected} target rotations, got {new_rotations.shape[0]}")
wp.launch(
_update_rotation_targets,
dim=expected,
inputs=[new_rotations],
outputs=[self.target_rotations],
device=self.device,
)
def residual_dim(self) -> int:
"""Return the three rotational residual rows for this objective."""
return 3
def compute_residuals(
self,
body_q: wp.array2d[wp.transform],
joint_q: wp.array2d[wp.float32],
model: Model,
residuals: wp.array2d[wp.float32],
start_idx: int,
problem_idx: wp.array[wp.int32],
) -> None:
"""Write weighted orientation errors into the global residual buffer.
Args:
body_q: Batched body transforms for the evaluation rows,
shape [n_batch, body_count].
joint_q: Batched joint coordinates, shape [n_batch, joint_coord_count].
Present for interface compatibility and not used directly by
this objective.
model: Shared articulation model. Present for interface
compatibility.
residuals: Global residual buffer to update,
shape [n_batch, total_residual_count].
start_idx: First residual row reserved for this objective.
problem_idx: Mapping from evaluation rows to base problem
indices, shape [n_batch], used to fetch ``target_rotations``.
"""
count = body_q.shape[0]
wp.launch(
_rot_residuals,
dim=count,
inputs=[
body_q,
self.target_rotations,
self.link_index,
self.link_offset_rotation,
self.canonicalize_quat_err,
start_idx,
self.weight,
problem_idx,
],
outputs=[residuals],
device=self.device,
)
def compute_jacobian_autodiff(
self,
tape: wp.Tape,
model: Model,
jacobian: wp.array3d[wp.float32],
start_idx: int,
dq_dof: wp.array2d[wp.float32],
) -> None:
"""Fill the rotation Jacobian block using Warp autodiff.
Args:
tape: Recorded Warp tape whose output is the global residual
buffer.
model: Shared articulation model.
jacobian: Global Jacobian buffer to update,
shape [n_batch, total_residual_count, joint_dof_count].
start_idx: First residual row reserved for this objective.
dq_dof: Differentiable joint update variable for the current
batch, shape [n_batch, joint_dof_count].
"""
self._require_batch_layout()
for component in range(3):
tape.backward(grads={tape.outputs[0]: self.e_arrays[component].flatten()})
q_grad = tape.gradients[dq_dof]
n_dofs = model.joint_dof_count
wp.launch(
_rot_jac_fill,
dim=self.n_batch,
inputs=[
q_grad,
n_dofs,
start_idx,
component,
],
outputs=[
jacobian,
],
device=self.device,
)
tape.zero()
def compute_jacobian_analytic(
self,
body_q: wp.array2d[wp.transform],
joint_q: wp.array2d[wp.float32],
model: Model,
jacobian: wp.array3d[wp.float32],
joint_S_s: wp.array2d[wp.spatial_vector],
start_idx: int,
) -> None:
"""Fill the rotation Jacobian block from the analytic motion subspace.
Args:
body_q: Batched body transforms for the evaluation rows,
shape [n_batch, body_count].
joint_q: Batched joint coordinates, shape [n_batch, joint_coord_count].
Present for interface compatibility and not used directly by
this objective.
model: Shared articulation model.
jacobian: Global Jacobian buffer to update,
shape [n_batch, total_residual_count, joint_dof_count].
joint_S_s: Batched motion-subspace columns,
shape [n_batch, joint_dof_count].
start_idx: First residual row reserved for this objective.
"""
n_dofs = model.joint_dof_count
wp.launch(
_rot_jac_analytic,
dim=[body_q.shape[0], n_dofs],
inputs=[
self.affects_dof, # lookup mask
joint_S_s,
start_idx,
n_dofs,
self.weight,
],
outputs=[jacobian],
device=self.device,
)
================================================
FILE: newton/_src/sim/ik/ik_solver.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Frontend wrapper for inverse-kinematics optimizers with sampling/selection."""
from __future__ import annotations
from collections.abc import Sequence
from enum import Enum
from typing import Any
import numpy as np
import warp as wp
from ..model import Model
from .ik_common import IKJacobianType
from .ik_lbfgs_optimizer import IKOptimizerLBFGS
from .ik_lm_optimizer import IKOptimizerLM
from .ik_objectives import IKObjective
class IKOptimizer(str, Enum):
"""Optimizer backends supported by :class:`~newton.ik.IKSolver`."""
LM = "lm"
"""Use a Levenberg-Marquardt optimizer."""
LBFGS = "lbfgs"
"""Use an L-BFGS quasi-Newton optimizer."""
class IKSampler(str, Enum):
"""Sampling strategies used by :class:`~newton.ik.IKSolver` before optimization."""
NONE = "none"
"""Disable sampling and use the input seed as-is."""
GAUSS = "gauss"
"""Perturb the input seed with Gaussian noise, clamped to joint limits."""
ROBERTS = "roberts"
"""Use a deterministic low-discrepancy (Roberts) sequence over joint limits."""
UNIFORM = "uniform"
"""Sample each bounded joint uniformly within its limits."""
@wp.kernel
def _sample_none_kernel(
joint_q_in: wp.array2d[wp.float32],
n_seeds: int,
n_coords: int,
joint_q_out: wp.array2d[wp.float32],
):
expanded_idx = wp.tid()
problem_idx = expanded_idx // n_seeds
for coord in range(n_coords):
joint_q_out[expanded_idx, coord] = joint_q_in[problem_idx, coord]
@wp.kernel
def _sample_gauss_kernel(
joint_q_in: wp.array2d[wp.float32],
n_seeds: int,
n_coords: int,
noise_std: float,
joint_lower: wp.array[wp.float32],
joint_upper: wp.array[wp.float32],
joint_bounded: wp.array[wp.int32],
base_seed: wp.array[wp.uint32],
joint_q_out: wp.array2d[wp.float32],
):
expanded_idx = wp.tid()
problem_idx = expanded_idx // n_seeds
seed_idx = expanded_idx % n_seeds
for coord in range(n_coords):
base = joint_q_in[problem_idx, coord]
if seed_idx == 0:
val = base
else:
seed = wp.int32(base_seed[0])
offset = wp.int32(expanded_idx) * wp.int32(n_coords) + wp.int32(coord)
state = wp.rand_init(seed, offset)
val = base + wp.randn(state) * noise_std
if joint_bounded[coord]:
lo = joint_lower[coord]
hi = joint_upper[coord]
val = wp.min(wp.max(val, lo), hi)
joint_q_out[expanded_idx, coord] = val
@wp.kernel
def _sample_uniform_kernel(
n_coords: int,
joint_lower: wp.array[wp.float32],
joint_upper: wp.array[wp.float32],
joint_bounded: wp.array[wp.int32],
base_seed: wp.array[wp.uint32],
joint_q_out: wp.array2d[wp.float32],
):
expanded_idx = wp.tid()
for coord in range(n_coords):
if joint_bounded[coord]:
lo = joint_lower[coord]
hi = joint_upper[coord]
span = hi - lo
seed = wp.int32(base_seed[0])
offset = wp.int32(expanded_idx) * wp.int32(n_coords) + wp.int32(coord)
state = wp.rand_init(seed, offset)
val = lo + wp.randf(state) * span
else:
val = 0.0
joint_q_out[expanded_idx, coord] = val
@wp.kernel
def _sample_roberts_kernel(
n_seeds: int,
n_coords: int,
roberts_basis: wp.array[wp.float32],
joint_lower: wp.array[wp.float32],
joint_upper: wp.array[wp.float32],
joint_bounded: wp.array[wp.int32],
joint_q_out: wp.array2d[wp.float32],
):
expanded_idx = wp.tid()
seed_idx = expanded_idx % n_seeds
for coord in range(n_coords):
if joint_bounded[coord]:
lo = joint_lower[coord]
hi = joint_upper[coord]
span = hi - lo
basis = roberts_basis[coord]
val = lo + wp.mod(float(seed_idx) * basis, 1.0) * span
else:
val = 0.0
joint_q_out[expanded_idx, coord] = val
@wp.kernel
def _select_best_seed_indices(
costs: wp.array[wp.float32],
n_seeds: int,
best: wp.array[wp.int32],
):
problem_idx = wp.tid()
base = problem_idx * n_seeds
best_seed = wp.int32(0)
best_cost = wp.float32(costs[base])
for seed_idx in range(1, n_seeds):
idx = base + seed_idx
cost = wp.float32(costs[idx])
if cost < best_cost:
best_cost = cost
best_seed = wp.int32(seed_idx)
best[problem_idx] = best_seed
@wp.kernel
def _gather_best_seed(
joint_q_expanded: wp.array2d[wp.float32],
best: wp.array[wp.int32],
n_seeds: int,
n_coords: int,
joint_q_out: wp.array2d[wp.float32],
):
problem_idx, coord_idx = wp.tid()
best_seed = best[problem_idx]
expanded_idx = problem_idx * n_seeds + best_seed
joint_q_out[problem_idx, coord_idx] = joint_q_expanded[expanded_idx, coord_idx]
@wp.kernel
def _pull_seed(
seed_state: wp.array[wp.uint32],
out_seed: wp.array[wp.uint32],
):
out_seed[0] = seed_state[0]
seed_state[0] = seed_state[0] + wp.uint32(1)
@wp.kernel
def _set_seed(
seed_state: wp.array[wp.uint32],
value: wp.uint32,
):
seed_state[0] = value
class IKSolver:
"""High-level inverse-kinematics front end with optional multi-seed sampling.
``IKSolver`` expands each base problem into one or more candidate seeds,
delegates optimization to :class:`~newton.ik.IKOptimizerLM` or
:class:`~newton.ik.IKOptimizerLBFGS`, and keeps the lowest-cost candidate for each
base problem.
Args:
model: Shared articulation model.
n_problems: Number of base IK problems solved together.
objectives: Ordered IK objectives shared by all problems.
optimizer: Optimizer backend to use.
jacobian_mode: Jacobian backend to use inside the optimizer.
sampler: Initial-seed sampling strategy.
n_seeds: Number of candidate seeds generated per base problem.
noise_std: Standard deviation used by
:attr:`~newton.ik.IKSampler.GAUSS` [m or rad].
rng_seed: Seed for stochastic samplers.
lambda_initial: Initial LM damping factor.
lambda_factor: LM damping update factor.
lambda_min: Minimum LM damping value.
lambda_max: Maximum LM damping value.
rho_min: Minimum LM acceptance ratio.
history_len: Number of correction pairs retained by L-BFGS.
h0_scale: Initial inverse-Hessian scale for L-BFGS.
line_search_alphas: Candidate line-search step sizes for L-BFGS.
wolfe_c1: Armijo constant for the L-BFGS line search.
wolfe_c2: Curvature constant for the L-BFGS line search.
"""
def __init__(
self,
model: Model,
n_problems: int,
objectives: Sequence[IKObjective],
*,
optimizer: IKOptimizer | str = IKOptimizer.LM,
jacobian_mode: IKJacobianType | str = IKJacobianType.AUTODIFF,
sampler: IKSampler | str = IKSampler.NONE,
n_seeds: int = 1,
noise_std: float = 0.1,
rng_seed: int = 12345,
# LM parameters
lambda_initial: float = 0.1,
lambda_factor: float = 2.0,
lambda_min: float = 1e-5,
lambda_max: float = 1e10,
rho_min: float = 1e-3,
# L-BFGS parameters
history_len: int = 10,
h0_scale: float = 1.0,
line_search_alphas: Sequence[float] | None = None,
wolfe_c1: float = 1e-4,
wolfe_c2: float = 0.9,
) -> None:
if isinstance(optimizer, str):
optimizer = IKOptimizer(optimizer)
if isinstance(jacobian_mode, str):
jacobian_mode = IKJacobianType(jacobian_mode)
if isinstance(sampler, str):
sampler = IKSampler(sampler)
if n_seeds < 1:
raise ValueError("n_seeds must be >= 1")
if sampler is IKSampler.NONE and n_seeds != 1:
raise ValueError("sampler 'none' requires n_seeds == 1")
self.model = model
self.device = model.device
self.objectives = objectives
self.optimizer_type = optimizer
self.sampler = sampler
self.n_problems = n_problems
self.n_seeds = n_seeds
self.n_expanded = n_problems * n_seeds
self.n_coords = model.joint_coord_count
self.noise_std = noise_std
self._rng_seed = np.uint32(rng_seed)
self.joint_q_expanded = wp.zeros((self.n_expanded, self.n_coords), dtype=wp.float32, device=self.device)
self.best_indices = wp.zeros(self.n_problems, dtype=wp.int32, device=self.device)
self._seed_state = wp.array(np.array([self._rng_seed], dtype=np.uint32), dtype=wp.uint32, device=self.device)
self._seed_tmp = wp.zeros(1, dtype=wp.uint32, device=self.device)
base_idx_np = np.repeat(np.arange(self.n_problems, dtype=np.int32), self.n_seeds)
self.problem_idx_expanded = wp.array(base_idx_np, dtype=wp.int32, device=self.device)
lower_np = model.joint_limit_lower.numpy()[: self.n_coords].astype(np.float32)
upper_np = model.joint_limit_upper.numpy()[: self.n_coords].astype(np.float32)
span_np = upper_np - lower_np
bounded_mask_np = (np.isfinite(lower_np) & np.isfinite(upper_np) & (np.abs(span_np) < 1.0e5)).astype(np.int32)
lower_np = np.where(bounded_mask_np, lower_np, 0.0).astype(np.float32)
upper_np = np.where(bounded_mask_np, upper_np, 0.0).astype(np.float32)
self.joint_lower = wp.array(lower_np, dtype=wp.float32, device=self.device)
self.joint_upper = wp.array(upper_np, dtype=wp.float32, device=self.device)
self.joint_bounded = wp.array(bounded_mask_np, dtype=wp.int32, device=self.device)
if sampler is IKSampler.ROBERTS:
roberts_basis = self._compute_roberts_basis(self.n_coords)
self.roberts_basis = wp.array(roberts_basis, dtype=wp.float32, device=self.device)
else:
self.roberts_basis = None
if optimizer is IKOptimizer.LM:
self._impl = IKOptimizerLM(
model,
self.n_expanded,
objectives,
problem_idx=self.problem_idx_expanded,
lambda_initial=lambda_initial,
jacobian_mode=jacobian_mode,
lambda_factor=lambda_factor,
lambda_min=lambda_min,
lambda_max=lambda_max,
rho_min=rho_min,
)
elif optimizer is IKOptimizer.LBFGS:
self._impl = IKOptimizerLBFGS(
model,
self.n_expanded,
objectives,
problem_idx=self.problem_idx_expanded,
jacobian_mode=jacobian_mode,
history_len=history_len,
h0_scale=h0_scale,
line_search_alphas=line_search_alphas,
wolfe_c1=wolfe_c1,
wolfe_c2=wolfe_c2,
)
else:
raise ValueError(f"Unsupported optimizer: {optimizer}")
self.costs_expanded = self._impl.costs
def step(
self,
joint_q_in: wp.array2d[wp.float32],
joint_q_out: wp.array2d[wp.float32],
iterations: int = 50,
step_size: float = 1.0,
) -> None:
"""Solve all base problems and write the best result for each one.
Args:
joint_q_in: Input joint coordinates [m or rad] for the base
problems, shape [n_problems, joint_coord_count].
joint_q_out: Output joint coordinates [m or rad] for the selected
solution of each base problem, shape [n_problems, joint_coord_count].
It may alias ``joint_q_in``.
iterations: Number of optimizer iterations to run for each sampled
seed.
step_size: Unitless LM step scale. Ignored by the L-BFGS backend.
"""
if joint_q_in.shape != (self.n_problems, self.n_coords):
raise ValueError("joint_q_in has incompatible shape")
if joint_q_out.shape != (self.n_problems, self.n_coords):
raise ValueError("joint_q_out has incompatible shape")
self._sample(joint_q_in)
self._impl.reset()
if self.optimizer_type is IKOptimizer.LM:
self._impl.step(self.joint_q_expanded, self.joint_q_expanded, iterations=iterations, step_size=step_size)
elif self.optimizer_type is IKOptimizer.LBFGS:
self._impl.step(self.joint_q_expanded, self.joint_q_expanded, iterations=iterations)
else:
raise RuntimeError(f"Unsupported optimizer: {self.optimizer_type}")
self._impl.compute_costs(self.joint_q_expanded)
if self.n_seeds == 1:
if joint_q_out.ptr != self.joint_q_expanded.ptr:
wp.copy(joint_q_out, self.joint_q_expanded)
return
wp.launch(
_select_best_seed_indices,
dim=self.n_problems,
inputs=[self.costs_expanded, self.n_seeds],
outputs=[self.best_indices],
device=self.device,
)
wp.launch(
_gather_best_seed,
dim=[self.n_problems, self.n_coords],
inputs=[self.joint_q_expanded, self.best_indices, self.n_seeds, self.n_coords],
outputs=[joint_q_out],
device=self.device,
)
def reset(self) -> None:
"""Reset optimizer state, selected seeds, and the sampler RNG."""
self._impl.reset()
self.best_indices.zero_()
wp.launch(
_set_seed,
dim=1,
inputs=[self._seed_state, int(self._rng_seed)],
device=self.device,
)
@property
def joint_q(self) -> wp.array2d[wp.float32]:
"""Expanded joint-coordinate buffer that stores all sampled seeds."""
return self.joint_q_expanded
@property
def costs(self) -> wp.array[wp.float32]:
"""Expanded per-seed objective costs from the most recent solve."""
return self.costs_expanded
def __getattr__(self, name: str) -> Any:
return getattr(self._impl, name)
def _sample(self, joint_q_in: wp.array2d[wp.float32]) -> None:
wp.launch(
_pull_seed,
dim=1,
inputs=[self._seed_state],
outputs=[self._seed_tmp],
device=self.device,
)
if self.sampler is IKSampler.NONE:
wp.launch(
_sample_none_kernel,
dim=self.n_expanded,
inputs=[joint_q_in, self.n_seeds, self.n_coords],
outputs=[self.joint_q_expanded],
device=self.device,
)
return
if self.sampler is IKSampler.GAUSS:
wp.launch(
_sample_gauss_kernel,
dim=self.n_expanded,
inputs=[
joint_q_in,
self.n_seeds,
self.n_coords,
self.noise_std,
self.joint_lower,
self.joint_upper,
self.joint_bounded,
self._seed_tmp,
],
outputs=[self.joint_q_expanded],
device=self.device,
)
return
if self.sampler is IKSampler.UNIFORM:
wp.launch(
_sample_uniform_kernel,
dim=self.n_expanded,
inputs=[
self.n_coords,
self.joint_lower,
self.joint_upper,
self.joint_bounded,
self._seed_tmp,
],
outputs=[self.joint_q_expanded],
device=self.device,
)
return
if self.sampler is IKSampler.ROBERTS:
wp.launch(
_sample_roberts_kernel,
dim=self.n_expanded,
inputs=[
self.n_seeds,
self.n_coords,
self.roberts_basis,
self.joint_lower,
self.joint_upper,
self.joint_bounded,
],
outputs=[self.joint_q_expanded],
device=self.device,
)
return
raise RuntimeError(f"Unsupported sampler: {self.sampler}")
@staticmethod
def _compute_roberts_basis(n_coords: int) -> np.ndarray:
x = 1.5
for _ in range(20):
f = x ** (n_coords + 1) - x - 1.0
df = (n_coords + 1) * x**n_coords - 1.0
x_next = x - f / df
if abs(x_next - x) < 1.0e-12:
break
x = x_next
basis = 1.0 - 1.0 / x ** (1 + np.arange(n_coords))
return basis.astype(np.float32)
================================================
FILE: newton/_src/sim/model.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Implementation of the Newton model class."""
from __future__ import annotations
from enum import IntEnum
from typing import TYPE_CHECKING, Any
import numpy as np
import warp as wp
from ..core.types import Devicelike
from .contacts import Contacts
from .control import Control
from .state import State
if TYPE_CHECKING:
from newton_actuators import Actuator
from ..utils.heightfield import HeightfieldData
from .collide import CollisionPipeline
class Model:
"""
Represents the static (non-time-varying) definition of a simulation model in Newton.
The Model class encapsulates all geometry, constraints, and parameters that describe a physical system
for simulation. It is designed to be constructed via the ModelBuilder, which handles the correct
initialization and population of all fields.
Key Features:
- Stores all static data for simulation: particles, rigid bodies, joints, shapes, soft/rigid elements, etc.
- Supports grouping of entities by world using world indices (e.g., `particle_world`, `body_world`, etc.).
- Index -1: global entities shared across all worlds.
- Indices 0, 1, 2, ...: world-specific entities.
- Grouping enables:
- Collision detection optimization (e.g., separating worlds)
- Visualization (e.g., spatially separating worlds)
- Parallel processing of independent worlds
Note:
It is strongly recommended to use the :class:`ModelBuilder` to construct a Model.
Direct instantiation and manual population of Model fields is possible but discouraged.
"""
class AttributeAssignment(IntEnum):
"""Enumeration of attribute assignment categories.
Defines which component of the simulation system owns and manages specific attributes.
This categorization determines where custom attributes are attached during simulation
object creation (Model, State, Control, or Contacts).
"""
MODEL = 0
"""Model attributes are attached to the :class:`~newton.Model` object."""
STATE = 1
"""State attributes are attached to the :class:`~newton.State` object."""
CONTROL = 2
"""Control attributes are attached to the :class:`~newton.Control` object."""
CONTACT = 3
"""Contact attributes are attached to the :class:`~newton.Contacts` object."""
class AttributeFrequency(IntEnum):
"""Enumeration of attribute frequency categories.
Defines the dimensional structure and indexing pattern for custom attributes.
This determines how many elements an attribute array should have and how it
should be indexed in relation to the model's entities such as joints, bodies, shapes, etc.
"""
ONCE = 0
"""Attribute frequency is a single value."""
JOINT = 1
"""Attribute frequency follows the number of joints (see :attr:`~newton.Model.joint_count`)."""
JOINT_DOF = 2
"""Attribute frequency follows the number of joint degrees of freedom (see :attr:`~newton.Model.joint_dof_count`)."""
JOINT_COORD = 3
"""Attribute frequency follows the number of joint positional coordinates (see :attr:`~newton.Model.joint_coord_count`)."""
JOINT_CONSTRAINT = 4
"""Attribute frequency follows the number of joint constraints (see :attr:`~newton.Model.joint_constraint_count`)."""
BODY = 5
"""Attribute frequency follows the number of bodies (see :attr:`~newton.Model.body_count`)."""
SHAPE = 6
"""Attribute frequency follows the number of shapes (see :attr:`~newton.Model.shape_count`)."""
ARTICULATION = 7
"""Attribute frequency follows the number of articulations (see :attr:`~newton.Model.articulation_count`)."""
EQUALITY_CONSTRAINT = 8
"""Attribute frequency follows the number of equality constraints (see :attr:`~newton.Model.equality_constraint_count`)."""
PARTICLE = 9
"""Attribute frequency follows the number of particles (see :attr:`~newton.Model.particle_count`)."""
EDGE = 10
"""Attribute frequency follows the number of edges (see :attr:`~newton.Model.edge_count`)."""
TRIANGLE = 11
"""Attribute frequency follows the number of triangles (see :attr:`~newton.Model.tri_count`)."""
TETRAHEDRON = 12
"""Attribute frequency follows the number of tetrahedra (see :attr:`~newton.Model.tet_count`)."""
SPRING = 13
"""Attribute frequency follows the number of springs (see :attr:`~newton.Model.spring_count`)."""
CONSTRAINT_MIMIC = 14
"""Attribute frequency follows the number of mimic constraints (see :attr:`~newton.Model.constraint_mimic_count`)."""
WORLD = 15
"""Attribute frequency follows the number of worlds (see :attr:`~newton.Model.world_count`)."""
class AttributeNamespace:
"""
A container for namespaced custom attributes.
Custom attributes are stored as regular instance attributes on this object,
allowing hierarchical organization of related properties.
"""
def __init__(self, name: str):
"""Initialize the namespace container.
Args:
name: The name of the namespace
"""
self._name: str = name
def __repr__(self):
"""Return a string representation showing the namespace and its attributes."""
# List all public attributes (not starting with _)
attrs = [k for k in self.__dict__ if not k.startswith("_")]
return f"AttributeNamespace('{self._name}', attributes={attrs})"
def __init__(self, device: Devicelike | None = None):
"""
Initialize a Model object.
Args:
device: Device on which the Model's data will be allocated.
"""
self.requires_grad: bool = False
"""Whether the model was finalized (see :meth:`ModelBuilder.finalize`) with gradient computation enabled."""
self.world_count: int = 0
"""Number of worlds added to the ModelBuilder."""
self.particle_q: wp.array[wp.vec3] | None = None
"""Particle positions [m], shape [particle_count, 3], float."""
self.particle_qd: wp.array[wp.vec3] | None = None
"""Particle velocities [m/s], shape [particle_count, 3], float."""
self.particle_mass: wp.array[wp.float32] | None = None
"""Particle mass [kg], shape [particle_count], float."""
self.particle_inv_mass: wp.array[wp.float32] | None = None
"""Particle inverse mass [1/kg], shape [particle_count], float."""
self.particle_radius: wp.array[wp.float32] | None = None
"""Particle radius [m], shape [particle_count], float."""
self.particle_max_radius: float = 0.0
"""Maximum particle radius [m] (useful for HashGrid construction)."""
self.particle_ke: float = 1.0e3
"""Particle normal contact stiffness [N/m] (used by :class:`~newton.solvers.SolverSemiImplicit`)."""
self.particle_kd: float = 1.0e2
"""Particle normal contact damping [N·s/m] (used by :class:`~newton.solvers.SolverSemiImplicit`)."""
self.particle_kf: float = 1.0e2
"""Particle friction force stiffness [N·s/m] (used by :class:`~newton.solvers.SolverSemiImplicit`)."""
self.particle_mu: float = 0.5
"""Particle friction coefficient [dimensionless]."""
self.particle_cohesion: float = 0.0
"""Particle cohesion strength [m]."""
self.particle_adhesion: float = 0.0
"""Particle adhesion strength [m]."""
self.particle_grid: wp.HashGrid | None = None
"""HashGrid instance for accelerated simulation of particle interactions."""
self.particle_flags: wp.array[wp.int32] | None = None
"""Particle enabled state, shape [particle_count], int."""
self.particle_max_velocity: float = 1e5
"""Maximum particle velocity [m/s] (to prevent instability)."""
self.particle_world: wp.array[wp.int32] | None = None
"""World index for each particle, shape [particle_count], int. -1 for global."""
self.particle_world_start: wp.array[wp.int32] | None = None
"""Start index of the first particle per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the particles belonging to that world. The second-last element (accessible
via index ``-2``) stores the start index of the global particles (i.e. with
world index ``-1``) added to the end of the model, and the last element
stores the total particle count.
The number of particles in a given world ``w`` can be computed as::
num_particles_in_world = particle_world_start[w + 1] - particle_world_start[w]
The total number of global particles can be computed as::
num_global_particles = particle_world_start[-1] - particle_world_start[-2] + particle_world_start[0]
"""
self.shape_label: list[str] = []
"""List of labels for each shape."""
self.shape_transform: wp.array[wp.transform] | None = None
"""Rigid shape transforms [m, unitless quaternion], shape [shape_count, 7], float."""
self.shape_body: wp.array[wp.int32] | None = None
"""Rigid shape body index, shape [shape_count], int."""
self.shape_flags: wp.array[wp.int32] | None = None
"""Rigid shape flags, shape [shape_count], int."""
self.body_shapes: dict[int, list[int]] = {}
"""Mapping from body index to list of attached shape indices."""
# Shape material properties
self.shape_material_ke: wp.array[wp.float32] | None = None
"""Shape contact elastic stiffness [N/m], shape [shape_count], float."""
self.shape_material_kd: wp.array[wp.float32] | None = None
"""Shape contact damping stiffness, shape [shape_count], float.
Interpretation is solver-dependent: used directly as damping [N·s/m] by SemiImplicit,
but multiplied by ke as a relative damping factor by VBD."""
self.shape_material_kf: wp.array[wp.float32] | None = None
"""Shape contact friction stiffness [N·s/m], shape [shape_count], float."""
self.shape_material_ka: wp.array[wp.float32] | None = None
"""Shape contact adhesion distance [m], shape [shape_count], float."""
self.shape_material_mu: wp.array[wp.float32] | None = None
"""Shape coefficient of friction [dimensionless], shape [shape_count], float."""
self.shape_material_restitution: wp.array[wp.float32] | None = None
"""Shape coefficient of restitution [dimensionless], shape [shape_count], float."""
self.shape_material_mu_torsional: wp.array[wp.float32] | None = None
"""Shape torsional friction coefficient [dimensionless] (resistance to spinning at contact point), shape [shape_count], float."""
self.shape_material_mu_rolling: wp.array[wp.float32] | None = None
"""Shape rolling friction coefficient [dimensionless] (resistance to rolling motion), shape [shape_count], float."""
self.shape_material_kh: wp.array[wp.float32] | None = None
"""Shape hydroelastic stiffness coefficient [N/m^3], shape [shape_count], float.
Contact stiffness is computed as ``area * kh``, yielding an effective spring constant [N/m]."""
self.shape_gap: wp.array[wp.float32] | None = None
"""Shape additional contact detection gap [m], shape [shape_count], float."""
# Shape geometry properties
self.shape_type: wp.array[wp.int32] | None = None
"""Shape geometry type, shape [shape_count], int32."""
self.shape_is_solid: wp.array[wp.bool] | None = None
"""Whether shape is solid or hollow, shape [shape_count], bool."""
self.shape_margin: wp.array[wp.float32] | None = None
"""Shape surface margin [m], shape [shape_count], float."""
self.shape_source: list[object | None] = []
"""List of source geometry objects (e.g., :class:`~newton.Mesh`) used for broadphase collision detection and rendering, shape [shape_count]."""
self.shape_source_ptr: wp.array[wp.uint64] | None = None
"""Geometry source pointers to be used inside the Warp kernels which are generated by finalizing the geometry objects, see for example :meth:`newton.Mesh.finalize`, shape [shape_count], uint64."""
self.shape_scale: wp.array[wp.vec3] | None = None
"""Shape 3D scale, shape [shape_count], vec3."""
self.shape_color: wp.array[wp.vec3] | None = None
"""Shape display colors [0, 1], shape [shape_count], vec3."""
self.shape_filter: wp.array[wp.int32] | None = None
"""Shape filter group, shape [shape_count], int."""
self.shape_collision_group: wp.array[wp.int32] | None = None
"""Collision group of each shape, shape [shape_count], int. Array populated during finalization."""
self.shape_collision_filter_pairs: set[tuple[int, int]] = set()
"""Pairs of shape indices (s1, s2) that should not collide. Pairs are in canonical order: s1 < s2."""
self.shape_collision_radius: wp.array[wp.float32] | None = None
"""Collision radius [m] for bounding sphere broadphase, shape [shape_count], float. Not supported by :class:`~newton.solvers.SolverMuJoCo`."""
self.shape_contact_pairs: wp.array[wp.vec2i] | None = None
"""Pairs of shape indices that may collide, shape [contact_pair_count, 2], int."""
self.shape_contact_pair_count: int = 0
"""Number of shape contact pairs."""
self.shape_world: wp.array[wp.int32] | None = None
"""World index for each shape, shape [shape_count], int. -1 for global."""
self.shape_world_start: wp.array[wp.int32] | None = None
"""Start index of the first shape per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the shapes belonging to that world. The second-last element (accessible via
index ``-2``) stores the start index of the global shapes (i.e. with world
index ``-1``) added to the end of the model, and the last element stores the
total shape count.
The number of shapes in a given world ``w`` can be computed as::
num_shapes_in_world = shape_world_start[w + 1] - shape_world_start[w]
The total number of global shapes can be computed as::
num_global_shapes = shape_world_start[-1] - shape_world_start[-2] + shape_world_start[0]
"""
# Gaussians
self.gaussians_count = 0
"""Number of gaussians."""
self.gaussians_data = None
"""Data for Gaussian Splats, shape [gaussians_count], Gaussian.Data."""
# Heightfield collision data (compact table + per-shape index indirection)
self.shape_heightfield_index: wp.array[wp.int32] | None = None
"""Per-shape heightfield index, shape [shape_count]. -1 means shape has no heightfield."""
self.heightfield_data: wp.array[HeightfieldData] | None = None
"""Compact array of HeightfieldData structs, one per actual heightfield shape."""
self.heightfield_elevations: wp.array[wp.float32] | None = None
"""Concatenated 1D elevation array for all heightfields. Kernels index via HeightfieldData.data_offset."""
# SDF storage (compact table + per-shape index indirection)
self.shape_sdf_index: wp.array[wp.int32] | None = None
"""Per-shape SDF index, shape [shape_count]. -1 means shape has no SDF."""
self.sdf_block_coords: wp.array[wp.vec3us] | None = None
"""Compact flat array of active SDF block coordinates."""
self.sdf_index2blocks: wp.array[wp.vec2i] | None = None
"""Per-SDF [start, end) indices into sdf_block_coords, shape [num_sdfs, 2]."""
# Texture SDF storage
self.texture_sdf_data = None
"""Compact array of TextureSDFData structs, shape [num_sdfs]."""
self.texture_sdf_coarse_textures = []
"""Coarse 3D textures matching texture_sdf_data by index. Kept for reference counting."""
self.texture_sdf_subgrid_textures = []
"""Subgrid 3D textures matching texture_sdf_data by index. Kept for reference counting."""
self.texture_sdf_subgrid_start_slots = []
"""Subgrid start slot arrays matching texture_sdf_data by index. Kept for reference counting."""
# Local AABB and voxel grid for contact reduction
# Note: These are stored in Model (not Contacts) because they are static geometry properties
# computed once during finalization, not per-frame contact data.
self.shape_collision_aabb_lower: wp.array[wp.vec3] | None = None
"""Scaled local-space AABB lower bound [m] for each shape, shape [shape_count, 3], float.
Includes shape scale but excludes margin and gap (those are applied at runtime).
Used for broadphase AABB computation and voxel-based contact reduction."""
self.shape_collision_aabb_upper: wp.array[wp.vec3] | None = None
"""Scaled local-space AABB upper bound [m] for each shape, shape [shape_count, 3], float.
Includes shape scale but excludes margin and gap (those are applied at runtime).
Used for broadphase AABB computation and voxel-based contact reduction."""
self._shape_voxel_resolution: wp.array[wp.vec3i] | None = None
"""Voxel grid resolution (nx, ny, nz) for each shape, shape [shape_count, 3], int. Used for voxel-based contact reduction."""
self.spring_indices: wp.array[wp.int32] | None = None
"""Particle spring indices, shape [spring_count*2], int."""
self.spring_rest_length: wp.array[wp.float32] | None = None
"""Particle spring rest length [m], shape [spring_count], float."""
self.spring_stiffness: wp.array[wp.float32] | None = None
"""Particle spring stiffness [N/m], shape [spring_count], float."""
self.spring_damping: wp.array[wp.float32] | None = None
"""Particle spring damping [N·s/m], shape [spring_count], float."""
self.spring_control: wp.array[wp.float32] | None = None
"""Particle spring activation [dimensionless], shape [spring_count], float."""
self.spring_constraint_lambdas: wp.array[wp.float32] | None = None
"""Lagrange multipliers for spring constraints (internal use)."""
self.tri_indices: wp.array[wp.int32] | None = None
"""Triangle element indices, shape [tri_count*3], int."""
self.tri_poses: wp.array[wp.mat22] | None = None
"""Triangle element rest pose, shape [tri_count, 2, 2], float."""
self.tri_activations: wp.array[wp.float32] | None = None
"""Triangle element activations, shape [tri_count], float."""
self.tri_materials: wp.array2d[wp.float32] | None = None
"""Triangle element materials, shape [tri_count, 5], float.
Components: [0] k_mu [Pa], [1] k_lambda [Pa], [2] k_damp [Pa·s], [3] k_drag [Pa·s], [4] k_lift [Pa].
Stored per-element; kernels multiply by rest area internally."""
self.tri_areas: wp.array[wp.float32] | None = None
"""Triangle element rest areas [m²], shape [tri_count], float."""
self.edge_indices: wp.array[wp.int32] | None = None
"""Bending edge indices, shape [edge_count*4], int, each row is [o0, o1, v1, v2], where v1, v2 are on the edge."""
self.edge_rest_angle: wp.array[wp.float32] | None = None
"""Bending edge rest angle [rad], shape [edge_count], float."""
self.edge_rest_length: wp.array[wp.float32] | None = None
"""Bending edge rest length [m], shape [edge_count], float."""
self.edge_bending_properties: wp.array2d[wp.float32] | None = None
"""Bending edge stiffness and damping, shape [edge_count, 2], float.
Components: [0] stiffness [N·m/rad], [1] damping [N·s]."""
self.edge_constraint_lambdas: wp.array[wp.float32] | None = None
"""Lagrange multipliers for edge constraints (internal use)."""
self.tet_indices: wp.array[wp.int32] | None = None
"""Tetrahedral element indices, shape [tet_count*4], int."""
self.tet_poses: wp.array[wp.mat33] | None = None
"""Tetrahedral rest poses, shape [tet_count, 3, 3], float."""
self.tet_activations: wp.array[wp.float32] | None = None
"""Tetrahedral volumetric activations, shape [tet_count], float."""
self.tet_materials: wp.array2d[wp.float32] | None = None
"""Tetrahedral elastic parameters in form :math:`k_{mu}, k_{lambda}, k_{damp}`, shape [tet_count, 3].
Components: [0] k_mu [Pa], [1] k_lambda [Pa], [2] k_damp [Pa·s].
Stored per-element; kernels multiply by rest volume internally."""
self.muscle_start: wp.array[wp.int32] | None = None
"""Start index of the first muscle point per muscle, shape [muscle_count], int."""
self.muscle_params: wp.array2d[wp.float32] | None = None
"""Muscle parameters, shape [muscle_count, 5], float.
Components: [0] f0 [N] (force scaling), [1] lm [m] (muscle fiber length), [2] lt [m] (tendon slack length),
[3] lmax [m] (max efficient length), [4] pen [dimensionless] (penalty factor)."""
self.muscle_bodies: wp.array[wp.int32] | None = None
"""Body indices of the muscle waypoints, int."""
self.muscle_points: wp.array[wp.vec3] | None = None
"""Local body offset of the muscle waypoints, float."""
self.muscle_activations: wp.array[wp.float32] | None = None
"""Muscle activations [dimensionless, 0 to 1], shape [muscle_count], float."""
self.body_q: wp.array[wp.transform] | None = None
"""Rigid body poses [m, unitless quaternion] for state initialization, shape [body_count, 7], float."""
self.body_qd: wp.array[wp.spatial_vector] | None = None
"""Rigid body velocities [m/s, rad/s] for state initialization, shape [body_count, 6], float."""
self.body_com: wp.array[wp.vec3] | None = None
"""Rigid body center of mass [m] (in local frame), shape [body_count, 3], float."""
self.body_inertia: wp.array[wp.mat33] | None = None
"""Rigid body inertia tensor [kg·m²] (relative to COM), shape [body_count, 3, 3], float."""
self.body_inv_inertia: wp.array[wp.mat33] | None = None
"""Rigid body inverse inertia tensor [1/(kg·m²)] (relative to COM), shape [body_count, 3, 3], float."""
self.body_mass: wp.array[wp.float32] | None = None
"""Rigid body mass [kg], shape [body_count], float."""
self.body_inv_mass: wp.array[wp.float32] | None = None
"""Rigid body inverse mass [1/kg], shape [body_count], float."""
self.body_flags: wp.array[wp.int32] | None = None
"""Rigid body flags (:class:`~newton.BodyFlags`), shape [body_count], int."""
self.body_label: list[str] = []
"""Rigid body labels, shape [body_count], str."""
self.body_world: wp.array[wp.int32] | None = None
"""World index for each body, shape [body_count], int. Global entities have index -1."""
self.body_world_start: wp.array[wp.int32] | None = None
"""Start index of the first body per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the bodies belonging to that world. The second-last element (accessible via
index ``-2``) stores the start index of the global bodies (i.e. with world
index ``-1``) added to the end of the model, and the last element stores the
total body count.
The number of bodies in a given world ``w`` can be computed as::
num_bodies_in_world = body_world_start[w + 1] - body_world_start[w]
The total number of global bodies can be computed as::
num_global_bodies = body_world_start[-1] - body_world_start[-2] + body_world_start[0]
"""
self.joint_q: wp.array[wp.float32] | None = None
"""Generalized joint positions [m or rad, depending on joint type] for state initialization, shape [joint_coord_count], float."""
self.joint_qd: wp.array[wp.float32] | None = None
"""Generalized joint velocities [m/s or rad/s, depending on joint type] for state initialization, shape [joint_dof_count], float."""
self.joint_f: wp.array[wp.float32] | None = None
"""Generalized joint forces [N or N·m, depending on joint type] for state initialization, shape [joint_dof_count], float."""
self.joint_target_pos: wp.array[wp.float32] | None = None
"""Generalized joint position targets [m or rad, depending on joint type], shape [joint_dof_count], float."""
self.joint_target_vel: wp.array[wp.float32] | None = None
"""Generalized joint velocity targets [m/s or rad/s, depending on joint type], shape [joint_dof_count], float."""
self.joint_act: wp.array[wp.float32] | None = None
"""Per-DOF feedforward actuation input for control initialization, shape [joint_dof_count], float."""
self.joint_type: wp.array[wp.int32] | None = None
"""Joint type, shape [joint_count], int."""
self.joint_articulation: wp.array[wp.int32] | None = None
"""Joint articulation index (-1 if not in any articulation), shape [joint_count], int."""
self.joint_parent: wp.array[wp.int32] | None = None
"""Joint parent body indices, shape [joint_count], int."""
self.joint_child: wp.array[wp.int32] | None = None
"""Joint child body indices, shape [joint_count], int."""
self.joint_ancestor: wp.array[wp.int32] | None = None
"""Maps from joint index to the index of the joint that has the current joint parent body as child (-1 if no such joint ancestor exists), shape [joint_count], int."""
self.joint_X_p: wp.array[wp.transform] | None = None
"""Joint transform in parent frame [m, unitless quaternion], shape [joint_count, 7], float."""
self.joint_X_c: wp.array[wp.transform] | None = None
"""Joint mass frame in child frame [m, unitless quaternion], shape [joint_count, 7], float."""
self.joint_axis: wp.array[wp.vec3] | None = None
"""Joint axis in child frame, shape [joint_dof_count, 3], float."""
self.joint_armature: wp.array[wp.float32] | None = None
"""Armature [kg·m² (rotational) or kg (translational)] for each joint axis (used by :class:`~newton.solvers.SolverMuJoCo` and :class:`~newton.solvers.SolverFeatherstone`), shape [joint_dof_count], float."""
self.joint_target_mode: wp.array[wp.int32] | None = None
"""Joint target mode per DOF, see :class:`newton.JointTargetMode`. Shape [joint_dof_count], dtype int32."""
self.joint_target_ke: wp.array[wp.float32] | None = None
"""Joint stiffness [N/m or N·m/rad, depending on joint type], shape [joint_dof_count], float."""
self.joint_target_kd: wp.array[wp.float32] | None = None
"""Joint damping [N·s/m or N·m·s/rad, depending on joint type], shape [joint_dof_count], float."""
self.joint_effort_limit: wp.array[wp.float32] | None = None
"""Joint effort (force/torque) limits [N or N·m, depending on joint type], shape [joint_dof_count], float."""
self.joint_velocity_limit: wp.array[wp.float32] | None = None
"""Joint velocity limits [m/s or rad/s, depending on joint type], shape [joint_dof_count], float."""
self.joint_friction: wp.array[wp.float32] | None = None
"""Joint friction force/torque [N or N·m, depending on joint type], shape [joint_dof_count], float."""
self.joint_dof_dim: wp.array2d[wp.int32] | None = None
"""Number of linear and angular dofs per joint, shape [joint_count, 2], int."""
self.joint_enabled: wp.array[wp.bool] | None = None
"""Controls which joint is simulated (bodies become disconnected if False, supported by :class:`~newton.solvers.SolverXPBD`, :class:`~newton.solvers.SolverVBD`, and :class:`~newton.solvers.SolverSemiImplicit`), shape [joint_count], bool."""
self.joint_limit_lower: wp.array[wp.float32] | None = None
"""Joint lower position limits [m or rad, depending on joint type], shape [joint_dof_count], float."""
self.joint_limit_upper: wp.array[wp.float32] | None = None
"""Joint upper position limits [m or rad, depending on joint type], shape [joint_dof_count], float."""
self.joint_limit_ke: wp.array[wp.float32] | None = None
"""Joint position limit stiffness [N/m or N·m/rad, depending on joint type] (used by :class:`~newton.solvers.SolverSemiImplicit` and :class:`~newton.solvers.SolverFeatherstone`), shape [joint_dof_count], float."""
self.joint_limit_kd: wp.array[wp.float32] | None = None
"""Joint position limit damping [N·s/m or N·m·s/rad, depending on joint type] (used by :class:`~newton.solvers.SolverSemiImplicit` and :class:`~newton.solvers.SolverFeatherstone`), shape [joint_dof_count], float."""
self.joint_twist_lower: wp.array[wp.float32] | None = None
"""Joint lower twist limit [rad], shape [joint_count], float."""
self.joint_twist_upper: wp.array[wp.float32] | None = None
"""Joint upper twist limit [rad], shape [joint_count], float."""
self.joint_q_start: wp.array[wp.int32] | None = None
"""Start index of the first position coordinate per joint (last value is a sentinel for dimension queries), shape [joint_count + 1], int."""
self.joint_qd_start: wp.array[wp.int32] | None = None
"""Start index of the first velocity coordinate per joint (last value is a sentinel for dimension queries), shape [joint_count + 1], int."""
self.joint_label: list[str] = []
"""Joint labels, shape [joint_count], str."""
self.joint_world: wp.array[wp.int32] | None = None
"""World index for each joint, shape [joint_count], int. -1 for global."""
self.joint_world_start: wp.array[wp.int32] | None = None
"""Start index of the first joint per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the joints belonging to that world. The second-last element (accessible via
index ``-2``) stores the start index of the global joints (i.e. with world
index ``-1``) added to the end of the model, and the last element stores the
total joint count.
The number of joints in a given world ``w`` can be computed as::
num_joints_in_world = joint_world_start[w + 1] - joint_world_start[w]
The total number of global joints can be computed as::
num_global_joints = joint_world_start[-1] - joint_world_start[-2] + joint_world_start[0]
"""
self.joint_dof_world_start: wp.array[wp.int32] | None = None
"""Start index of the first joint degree of freedom per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the joint DOFs belonging to that world. The second-last element (accessible
via index ``-2``) stores the start index of the global joint DOFs (i.e. with
world index ``-1``) added to the end of the model, and the last element
stores the total joint DOF count.
The number of joint DOFs in a given world ``w`` can be computed as::
num_joint_dofs_in_world = joint_dof_world_start[w + 1] - joint_dof_world_start[w]
The total number of global joint DOFs can be computed as::
num_global_joint_dofs = joint_dof_world_start[-1] - joint_dof_world_start[-2] + joint_dof_world_start[0]
"""
self.joint_coord_world_start: wp.array[wp.int32] | None = None
"""Start index of the first joint coordinate per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the joint coordinates belonging to that world. The second-last element
(accessible via index ``-2``) stores the start index of the global joint
coordinates (i.e. with world index ``-1``) added to the end of the model,
and the last element stores the total joint coordinate count.
The number of joint coordinates in a given world ``w`` can be computed as::
num_joint_coords_in_world = joint_coord_world_start[w + 1] - joint_coord_world_start[w]
The total number of global joint coordinates can be computed as::
num_global_joint_coords = joint_coord_world_start[-1] - joint_coord_world_start[-2] + joint_coord_world_start[0]
"""
self.joint_constraint_world_start: wp.array[wp.int32] | None = None
"""Start index of the first joint constraint per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the joint constraints belonging to that world. The second-last element
(accessible via index ``-2``) stores the start index of the global joint
constraints (i.e. with world index ``-1``) added to the end of the model,
and the last element stores the total joint constraint count.
The number of joint constraints in a given world ``w`` can be computed as::
num_joint_constraints_in_world = joint_constraint_world_start[w + 1] - joint_constraint_world_start[w]
The total number of global joint constraints can be computed as::
num_global_joint_constraints = joint_constraint_world_start[-1] - joint_constraint_world_start[-2] + joint_constraint_world_start[0]
"""
self.articulation_start: wp.array[wp.int32] | None = None
"""Articulation start index, shape [articulation_count], int."""
self.articulation_label: list[str] = []
"""Articulation labels, shape [articulation_count], str."""
self.articulation_world: wp.array[wp.int32] | None = None
"""World index for each articulation, shape [articulation_count], int. -1 for global."""
self.articulation_world_start: wp.array[wp.int32] | None = None
"""Start index of the first articulation per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the articulations belonging to that world. The second-last element
(accessible via index ``-2``) stores the start index of the global
articulations (i.e. with world index ``-1``) added to the end of the model,
and the last element stores the total articulation count.
The number of articulations in a given world ``w`` can be computed as::
num_articulations_in_world = articulation_world_start[w + 1] - articulation_world_start[w]
The total number of global articulations can be computed as::
num_global_articulations = articulation_world_start[-1] - articulation_world_start[-2] + articulation_world_start[0]
"""
self.max_joints_per_articulation: int = 0
"""Maximum number of joints in any articulation (used for IK kernel dimensioning)."""
self.max_dofs_per_articulation: int = 0
"""Maximum number of degrees of freedom in any articulation (used for Jacobian/mass matrix computation)."""
self.soft_contact_ke: float = 1.0e3
"""Stiffness of soft contacts [N/m] (used by :class:`~newton.solvers.SolverSemiImplicit` and :class:`~newton.solvers.SolverFeatherstone`)."""
self.soft_contact_kd: float = 10.0
"""Damping of soft contacts (used by :class:`~newton.solvers.SolverSemiImplicit` and :class:`~newton.solvers.SolverFeatherstone`).
Interpretation is solver-dependent: used directly as damping [N·s/m] by SemiImplicit,
but multiplied by ke as a relative damping factor by VBD."""
self.soft_contact_kf: float = 1.0e3
"""Stiffness of friction force in soft contacts [N·s/m] (used by :class:`~newton.solvers.SolverSemiImplicit` and :class:`~newton.solvers.SolverFeatherstone`)."""
self.soft_contact_mu: float = 0.5
"""Friction coefficient of soft contacts [dimensionless]."""
self.soft_contact_restitution: float = 0.0
"""Restitution coefficient of soft contacts [dimensionless] (used by :class:`SolverXPBD`)."""
self.rigid_contact_max: int = 0
"""Number of potential contact points between rigid bodies."""
self.up_axis: int = 2
"""Up axis: 0 for x, 1 for y, 2 for z."""
self.gravity: wp.array[wp.vec3] | None = None
"""Per-world gravity vectors [m/s²], shape [world_count, 3], dtype :class:`vec3`."""
self.equality_constraint_type: wp.array[wp.int32] | None = None
"""Type of equality constraint, shape [equality_constraint_count], int."""
self.equality_constraint_body1: wp.array[wp.int32] | None = None
"""First body index, shape [equality_constraint_count], int."""
self.equality_constraint_body2: wp.array[wp.int32] | None = None
"""Second body index, shape [equality_constraint_count], int."""
self.equality_constraint_anchor: wp.array[wp.vec3] | None = None
"""Anchor point on first body, shape [equality_constraint_count, 3], float."""
self.equality_constraint_torquescale: wp.array[wp.float32] | None = None
"""Torque scale, shape [equality_constraint_count], float."""
self.equality_constraint_relpose: wp.array[wp.transform] | None = None
"""Relative pose, shape [equality_constraint_count, 7], float."""
self.equality_constraint_joint1: wp.array[wp.int32] | None = None
"""First joint index, shape [equality_constraint_count], int."""
self.equality_constraint_joint2: wp.array[wp.int32] | None = None
"""Second joint index, shape [equality_constraint_count], int."""
self.equality_constraint_polycoef: wp.array2d[wp.float32] | None = None
"""Polynomial coefficients, shape [equality_constraint_count, 5], float."""
self.equality_constraint_label: list[str] = []
"""Constraint name/label, shape [equality_constraint_count], str."""
self.equality_constraint_enabled: wp.array[wp.bool] | None = None
"""Whether constraint is active, shape [equality_constraint_count], bool."""
self.equality_constraint_world: wp.array[wp.int32] | None = None
"""World index for each constraint, shape [equality_constraint_count], int."""
self.equality_constraint_world_start: wp.array[wp.int32] | None = None
"""Start index of the first equality constraint per world, shape [world_count + 2], int.
The entries at indices ``0`` to ``world_count - 1`` store the start index of
the equality constraints belonging to that world. The second-last element
(accessible via index ``-2``) stores the start index of the global equality
constraints (i.e. with world index ``-1``) added to the end of the model,
and the last element stores the total equality constraint count.
The number of equality constraints in a given world ``w`` can be computed as::
num_equality_constraints_in_world = equality_constraint_world_start[w + 1] - equality_constraint_world_start[w]
The total number of global equality constraints can be computed as::
num_global_equality_constraints = equality_constraint_world_start[-1] - equality_constraint_world_start[-2] + equality_constraint_world_start[0]
"""
self.constraint_mimic_joint0: wp.array[wp.int32] | None = None
"""Follower joint index (``joint0 = coef0 + coef1 * joint1``), shape [constraint_mimic_count], int."""
self.constraint_mimic_joint1: wp.array[wp.int32] | None = None
"""Leader joint index (``joint0 = coef0 + coef1 * joint1``), shape [constraint_mimic_count], int."""
self.constraint_mimic_coef0: wp.array[wp.float32] | None = None
"""Offset coefficient (coef0) for the mimic constraint (``joint0 = coef0 + coef1 * joint1``), shape [constraint_mimic_count], float."""
self.constraint_mimic_coef1: wp.array[wp.float32] | None = None
"""Scale coefficient (coef1) for the mimic constraint (``joint0 = coef0 + coef1 * joint1``), shape [constraint_mimic_count], float."""
self.constraint_mimic_enabled: wp.array[wp.bool] | None = None
"""Whether constraint is active, shape [constraint_mimic_count], bool."""
self.constraint_mimic_label: list[str] = []
"""Constraint name/label, shape [constraint_mimic_count], str."""
self.constraint_mimic_world: wp.array[wp.int32] | None = None
"""World index for each constraint, shape [constraint_mimic_count], int."""
self.particle_count: int = 0
"""Total number of particles in the system."""
self.body_count: int = 0
"""Total number of bodies in the system."""
self.shape_count: int = 0
"""Total number of shapes in the system."""
self.joint_count: int = 0
"""Total number of joints in the system."""
self.tri_count: int = 0
"""Total number of triangles in the system."""
self.tet_count: int = 0
"""Total number of tetrahedra in the system."""
self.edge_count: int = 0
"""Total number of edges in the system."""
self.spring_count: int = 0
"""Total number of springs in the system."""
self.muscle_count: int = 0
"""Total number of muscles in the system."""
self.articulation_count: int = 0
"""Total number of articulations in the system."""
self.joint_dof_count: int = 0
"""Total number of velocity degrees of freedom of all joints. Equals the number of joint axes."""
self.joint_coord_count: int = 0
"""Total number of position degrees of freedom of all joints."""
self.joint_constraint_count: int = 0
"""Total number of joint constraints of all joints."""
self.equality_constraint_count: int = 0
"""Total number of equality constraints in the system."""
self.constraint_mimic_count: int = 0
"""Total number of mimic constraints in the system."""
# indices of particles sharing the same color
self.particle_color_groups: list[wp.array[wp.int32]] = []
"""Coloring of all particles for Gauss-Seidel iteration (see :class:`~newton.solvers.SolverVBD`). Each array contains indices of particles sharing the same color."""
self.particle_colors: wp.array[wp.int32] | None = None
"""Color assignment for every particle."""
self.body_color_groups: list[wp.array[wp.int32]] = []
"""Coloring of all rigid bodies for Gauss-Seidel iteration (see :class:`~newton.solvers.SolverVBD`). Each array contains indices of bodies sharing the same color."""
self.body_colors: wp.array[wp.int32] | None = None
"""Color assignment for every rigid body."""
self.device: wp.Device = wp.get_device(device)
"""Device on which the Model was allocated."""
self.attribute_frequency: dict[str, Model.AttributeFrequency | str] = {}
"""Classifies each attribute using Model.AttributeFrequency enum values (per body, per joint, per DOF, etc.)
or custom frequencies for custom entity types (e.g., ``"mujoco:pair"``)."""
self.custom_frequency_counts: dict[str, int] = {}
"""Counts for custom frequencies (e.g., ``{"mujoco:pair": 5}``). Set during finalize()."""
self.attribute_assignment: dict[str, Model.AttributeAssignment] = {}
"""Assignment for custom attributes using Model.AttributeAssignment enum values.
If an attribute is not in this dictionary, it is assumed to be a Model attribute (assignment=Model.AttributeAssignment.MODEL)."""
self._requested_state_attributes: set[str] = set()
self._collision_pipeline: CollisionPipeline | None = None
# cached collision pipeline
self._requested_contact_attributes: set[str] = set()
# attributes per body
self.attribute_frequency["body_q"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_qd"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_com"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_inertia"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_inv_inertia"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_mass"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_inv_mass"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_flags"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_f"] = Model.AttributeFrequency.BODY
# Extended state attributes — these live on State (not Model) and are only
# allocated when explicitly requested via request_state_attributes().
self.attribute_frequency["body_qdd"] = Model.AttributeFrequency.BODY
self.attribute_frequency["body_parent_f"] = Model.AttributeFrequency.BODY
# attributes per joint
self.attribute_frequency["joint_type"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_parent"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_child"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_ancestor"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_articulation"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_X_p"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_X_c"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_dof_dim"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_enabled"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_twist_lower"] = Model.AttributeFrequency.JOINT
self.attribute_frequency["joint_twist_upper"] = Model.AttributeFrequency.JOINT
# attributes per joint coord
self.attribute_frequency["joint_q"] = Model.AttributeFrequency.JOINT_COORD
# attributes per joint dof
self.attribute_frequency["joint_qd"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_f"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_armature"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_target_pos"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_target_vel"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_act"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_axis"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_target_mode"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_target_ke"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_target_kd"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_limit_lower"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_limit_upper"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_limit_ke"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_limit_kd"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_effort_limit"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_friction"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["joint_velocity_limit"] = Model.AttributeFrequency.JOINT_DOF
self.attribute_frequency["mujoco:qfrc_actuator"] = Model.AttributeFrequency.JOINT_DOF
# attributes per shape
self.attribute_frequency["shape_transform"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_body"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_flags"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_ke"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_kd"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_kf"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_ka"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_mu"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_restitution"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_mu_torsional"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_mu_rolling"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_material_kh"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_gap"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_type"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_is_solid"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_margin"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_source_ptr"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_scale"] = Model.AttributeFrequency.SHAPE
self.attribute_frequency["shape_filter"] = Model.AttributeFrequency.SHAPE
self.actuators: list[Actuator] = []
"""List of actuator instances for this model."""
def state(self, requires_grad: bool | None = None) -> State:
"""
Create and return a new :class:`State` object for this model.
The returned state is initialized with the initial configuration from the model description.
Args:
requires_grad: Whether the state variables should have `requires_grad` enabled.
If None, uses the model's :attr:`requires_grad` setting.
Returns:
The state object.
"""
requested = self.get_requested_state_attributes()
s = State()
if requires_grad is None:
requires_grad = self.requires_grad
# particles
if self.particle_count:
s.particle_q = wp.clone(self.particle_q, requires_grad=requires_grad)
s.particle_qd = wp.clone(self.particle_qd, requires_grad=requires_grad)
s.particle_f = wp.zeros_like(self.particle_qd, requires_grad=requires_grad)
# rigid bodies
if self.body_count:
s.body_q = wp.clone(self.body_q, requires_grad=requires_grad)
s.body_qd = wp.clone(self.body_qd, requires_grad=requires_grad)
s.body_f = wp.zeros_like(self.body_qd, requires_grad=requires_grad)
# joints
if self.joint_count:
s.joint_q = wp.clone(self.joint_q, requires_grad=requires_grad)
s.joint_qd = wp.clone(self.joint_qd, requires_grad=requires_grad)
if "body_qdd" in requested:
s.body_qdd = wp.zeros_like(self.body_qd, requires_grad=requires_grad)
if "body_parent_f" in requested:
s.body_parent_f = wp.zeros_like(self.body_qd, requires_grad=requires_grad)
if "mujoco:qfrc_actuator" in requested:
if not hasattr(s, "mujoco"):
s.mujoco = Model.AttributeNamespace("mujoco")
s.mujoco.qfrc_actuator = wp.zeros_like(self.joint_qd, requires_grad=requires_grad)
# attach custom attributes with assignment==STATE
self._add_custom_attributes(s, Model.AttributeAssignment.STATE, requires_grad=requires_grad)
return s
def control(self, requires_grad: bool | None = None, clone_variables: bool = True) -> Control:
"""
Create and return a new :class:`Control` object for this model.
The returned control object is initialized with the control inputs from the model description.
Args:
requires_grad: Whether the control variables should have `requires_grad` enabled.
If None, uses the model's :attr:`requires_grad` setting.
clone_variables: If True, clone the control input arrays; if False, use references.
Returns:
The initialized control object.
"""
c = Control()
if requires_grad is None:
requires_grad = self.requires_grad
if clone_variables:
if self.joint_count:
c.joint_target_pos = wp.clone(self.joint_target_pos, requires_grad=requires_grad)
c.joint_target_vel = wp.clone(self.joint_target_vel, requires_grad=requires_grad)
c.joint_act = wp.clone(self.joint_act, requires_grad=requires_grad)
c.joint_f = wp.clone(self.joint_f, requires_grad=requires_grad)
if self.tri_count:
c.tri_activations = wp.clone(self.tri_activations, requires_grad=requires_grad)
if self.tet_count:
c.tet_activations = wp.clone(self.tet_activations, requires_grad=requires_grad)
if self.muscle_count:
c.muscle_activations = wp.clone(self.muscle_activations, requires_grad=requires_grad)
else:
c.joint_target_pos = self.joint_target_pos
c.joint_target_vel = self.joint_target_vel
c.joint_act = self.joint_act
c.joint_f = self.joint_f
c.tri_activations = self.tri_activations
c.tet_activations = self.tet_activations
c.muscle_activations = self.muscle_activations
# attach custom attributes with assignment==CONTROL
self._add_custom_attributes(
c, Model.AttributeAssignment.CONTROL, requires_grad=requires_grad, clone_arrays=clone_variables
)
return c
def set_gravity(
self,
gravity: tuple[float, float, float] | list | wp.vec3 | np.ndarray,
world: int | None = None,
) -> None:
"""
Set gravity for runtime modification.
Args:
gravity: Gravity vector (3,) or per-world array (world_count, 3).
world: If provided, set gravity only for this world.
Note:
Call ``solver.notify_model_changed(SolverNotifyFlags.MODEL_PROPERTIES)`` after.
Global entities (particles/bodies not assigned to a specific world) use
gravity from world 0.
"""
gravity_np = np.asarray(gravity, dtype=np.float32)
if world is not None:
if gravity_np.shape != (3,):
raise ValueError("Expected single gravity vector (3,) when world is specified")
if world < 0 or world >= self.world_count:
raise IndexError(f"world {world} out of range [0, {self.world_count})")
current = self.gravity.numpy()
current[world] = gravity_np
self.gravity.assign(current)
elif gravity_np.ndim == 1:
self.gravity.fill_(gravity_np)
else:
if len(gravity_np) != self.world_count:
raise ValueError(f"Expected {self.world_count} gravity vectors, got {len(gravity_np)}")
self.gravity.assign(gravity_np)
def _init_collision_pipeline(self):
"""
Initialize a :class:`CollisionPipeline` for this model.
This method creates a default collision pipeline for the model. The pipeline is cached on
the model for subsequent use by :meth:`collide`.
"""
from .collide import CollisionPipeline # noqa: PLC0415
self._collision_pipeline = CollisionPipeline(self, broad_phase="explicit")
def contacts(
self: Model,
collision_pipeline: CollisionPipeline | None = None,
) -> Contacts:
"""
Create and return a :class:`Contacts` object for this model.
This method initializes a collision pipeline with default arguments (when not already
cached) and allocates a contacts buffer suitable for storing collision detection results.
Call :meth:`collide` to run the collision detection and populate the contacts object.
Note:
Rigid contact gaps are controlled per-shape via :attr:`Model.shape_gap`, which is populated
from ``ModelBuilder.ShapeConfig.gap`` [m] during model building. If a shape doesn't specify a gap [m],
it defaults to ``builder.rigid_gap`` [m]. To adjust contact gaps [m], set them before calling
:meth:`ModelBuilder.finalize`.
Returns:
The contact object containing collision information.
"""
if collision_pipeline is not None:
self._collision_pipeline = collision_pipeline
if self._collision_pipeline is None:
self._init_collision_pipeline()
return self._collision_pipeline.contacts()
def collide(
self,
state: State,
contacts: Contacts | None = None,
*,
collision_pipeline: CollisionPipeline | None = None,
) -> Contacts:
"""
Generate contact points for the particles and rigid bodies in the model using the default collision
pipeline.
Args:
state: The current simulation state.
contacts: The contacts buffer to populate (will be cleared first). If None, a new
contacts buffer is allocated via :meth:`contacts`.
collision_pipeline: Optional collision pipeline override.
"""
if collision_pipeline is not None:
self._collision_pipeline = collision_pipeline
if self._collision_pipeline is None:
self._init_collision_pipeline()
if contacts is None:
contacts = self._collision_pipeline.contacts()
self._collision_pipeline.collide(state, contacts)
return contacts
def request_state_attributes(self, *attributes: str) -> None:
"""
Request that specific state attributes be allocated when creating a State object.
See :ref:`extended_state_attributes` for details and usage.
Args:
*attributes: Variable number of attribute names (strings).
"""
State.validate_extended_attributes(attributes)
self._requested_state_attributes.update(attributes)
def request_contact_attributes(self, *attributes: str) -> None:
"""
Request that specific contact attributes be allocated when creating a Contacts object.
Args:
*attributes: Variable number of attribute names (strings).
"""
Contacts.validate_extended_attributes(attributes)
self._requested_contact_attributes.update(attributes)
def get_requested_contact_attributes(self) -> set[str]:
"""
Get the set of requested contact attribute names.
Returns:
The set of requested contact attributes.
"""
return self._requested_contact_attributes
def _add_custom_attributes(
self,
destination: object,
assignment: Model.AttributeAssignment,
requires_grad: bool = False,
clone_arrays: bool = True,
) -> None:
"""
Add custom attributes of a specific assignment type to a destination object.
Args:
destination: The object to add attributes to (State, Control, or Contacts)
assignment: The assignment type to filter attributes by
requires_grad: Whether cloned arrays should have requires_grad enabled
clone_arrays: Whether to clone wp.arrays (True) or use references (False)
"""
for full_name, _freq in self.attribute_frequency.items():
if self.attribute_assignment.get(full_name, Model.AttributeAssignment.MODEL) != assignment:
continue
# Parse namespace from full_name (format: "namespace:attr_name" or "attr_name")
if ":" in full_name:
namespace, attr_name = full_name.split(":", 1)
# Get source from namespaced location on model
ns_obj = getattr(self, namespace, None)
if ns_obj is None:
raise AttributeError(f"Namespace '{namespace}' does not exist on the model")
src = getattr(ns_obj, attr_name, None)
if src is None:
raise AttributeError(
f"Attribute '{namespace}.{attr_name}' is registered but does not exist on the model"
)
# Create namespace on destination if it doesn't exist
if not hasattr(destination, namespace):
setattr(destination, namespace, Model.AttributeNamespace(namespace))
dest = getattr(destination, namespace)
else:
# Non-namespaced attribute - add directly to destination
attr_name = full_name
src = getattr(self, attr_name, None)
if src is None:
raise AttributeError(
f"Attribute '{attr_name}' is registered in attribute_frequency but does not exist on the model"
)
dest = destination
# Add attribute to the determined destination (either destination or dest_ns)
if isinstance(src, wp.array):
if clone_arrays:
setattr(dest, attr_name, wp.clone(src, requires_grad=requires_grad))
else:
setattr(dest, attr_name, src)
else:
setattr(dest, attr_name, src)
def add_attribute(
self,
name: str,
attrib: wp.array | list[Any],
frequency: Model.AttributeFrequency | str,
assignment: Model.AttributeAssignment | None = None,
namespace: str | None = None,
):
"""
Add a custom attribute to the model.
Args:
name: Name of the attribute.
attrib: The array to add as an attribute. Can be a wp.array for
numeric types or a list for string attributes.
frequency: The frequency of the attribute.
Can be a Model.AttributeFrequency enum value or a string for custom frequencies.
assignment: The assignment category using Model.AttributeAssignment enum.
Determines which object will hold the attribute.
namespace: Namespace for the attribute.
If None, attribute is added directly to the assignment object (e.g., model.attr_name).
If specified, attribute is added to a namespace object (e.g., model.namespace_name.attr_name).
Raises:
AttributeError: If the attribute already exists or is on the wrong device.
"""
if isinstance(attrib, wp.array) and attrib.device != self.device:
raise AttributeError(f"Attribute '{name}' device mismatch (model={self.device}, got={attrib.device})")
# Handle namespaced attributes
if namespace:
# Create namespace object if it doesn't exist
if not hasattr(self, namespace):
setattr(self, namespace, Model.AttributeNamespace(namespace))
ns_obj = getattr(self, namespace)
if hasattr(ns_obj, name):
raise AttributeError(f"Attribute already exists: {namespace}.{name}")
setattr(ns_obj, name, attrib)
full_name = f"{namespace}:{name}"
else:
# Add directly to model
if hasattr(self, name):
raise AttributeError(f"Attribute already exists: {name}")
setattr(self, name, attrib)
full_name = name
self.attribute_frequency[full_name] = frequency
if assignment is not None:
self.attribute_assignment[full_name] = assignment
def get_attribute_frequency(self, name: str) -> Model.AttributeFrequency | str:
"""
Get the frequency of an attribute.
Args:
name: Name of the attribute.
Returns:
The frequency of the attribute.
Either a Model.AttributeFrequency enum value or a string for custom frequencies.
Raises:
KeyError: If the attribute frequency is not known.
"""
frequency = self.attribute_frequency.get(name)
if frequency is None:
raise KeyError(f"Attribute frequency of '{name}' is not known")
return frequency
def get_custom_frequency_count(self, frequency: str) -> int:
"""
Get the count for a custom frequency.
Args:
frequency: The custom frequency (e.g., ``"mujoco:pair"``).
Returns:
The count of elements with this frequency.
Raises:
KeyError: If the frequency is not known.
"""
if frequency not in self.custom_frequency_counts:
raise KeyError(f"Custom frequency '{frequency}' is not known")
return self.custom_frequency_counts[frequency]
def get_requested_state_attributes(self) -> list[str]:
"""
Get the list of requested state attribute names that have been requested on the model.
See :ref:`extended_state_attributes` for details.
Returns:
The list of requested state attributes.
"""
attributes = []
if self.particle_count:
attributes.extend(
(
"particle_q",
"particle_qd",
"particle_f",
)
)
if self.body_count:
attributes.extend(
(
"body_q",
"body_qd",
"body_f",
)
)
if self.joint_count:
attributes.extend(("joint_q", "joint_qd"))
attributes.extend(self._requested_state_attributes.difference(attributes))
return attributes
================================================
FILE: newton/_src/sim/state.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warp as wp
class State:
"""
Represents the time-varying state of a :class:`Model` in a simulation.
The State object holds all dynamic quantities that change over time during simulation,
such as particle and rigid body positions, velocities, and forces, as well as joint coordinates.
State objects are typically created via :meth:`newton.Model.state()` and are used to
store and update the simulation's current configuration and derived data.
"""
EXTENDED_ATTRIBUTES: frozenset[str] = frozenset(
(
"body_qdd",
"body_parent_f",
"mujoco:qfrc_actuator",
)
)
"""
Names of optional extended state attributes that are not allocated by default.
These can be requested via :meth:`newton.ModelBuilder.request_state_attributes` or
:meth:`newton.Model.request_state_attributes` before calling :meth:`newton.Model.state`.
See :ref:`extended_state_attributes` for details and usage.
"""
@classmethod
def validate_extended_attributes(cls, attributes: tuple[str, ...]) -> None:
"""Validate names passed to request_state_attributes().
Only extended state attributes listed in :attr:`EXTENDED_ATTRIBUTES` are accepted.
Args:
attributes: Tuple of attribute names to validate.
Raises:
ValueError: If any attribute name is not in :attr:`EXTENDED_ATTRIBUTES`.
"""
if not attributes:
return
invalid = sorted(set(attributes).difference(cls.EXTENDED_ATTRIBUTES))
if invalid:
allowed = ", ".join(sorted(cls.EXTENDED_ATTRIBUTES))
bad = ", ".join(invalid)
raise ValueError(f"Unknown extended state attribute(s): {bad}. Allowed: {allowed}.")
def __init__(self) -> None:
"""
Initialize an empty State object.
To ensure that the attributes are properly allocated create the State object via :meth:`newton.Model.state` instead.
"""
self.particle_q: wp.array | None = None
"""3D positions of particles [m], shape (particle_count,), dtype :class:`vec3`."""
self.particle_qd: wp.array | None = None
"""3D velocities of particles [m/s], shape (particle_count,), dtype :class:`vec3`."""
self.particle_f: wp.array | None = None
"""3D forces on particles [N], shape (particle_count,), dtype :class:`vec3`."""
self.body_q: wp.array | None = None
"""Rigid body transforms (7-DOF) [m, unitless quaternion], shape (body_count,), dtype :class:`transform`."""
self.body_qd: wp.array | None = None
"""Rigid body velocities (spatial) [m/s, rad/s], shape (body_count,), dtype :class:`spatial_vector`.
First three entries: linear velocity [m/s] relative to the body's center of mass in world frame;
last three: angular velocity [rad/s] in world frame.
See :ref:`Twist conventions in Newton ` for more information."""
self.body_q_prev: wp.array | None = None
"""Previous rigid body transforms [m, unitless quaternion] for finite-difference velocity computation."""
self.body_qdd: wp.array | None = None
"""Rigid body accelerations (spatial) [m/s², rad/s²], shape (body_count,), dtype :class:`spatial_vector`.
First three entries: linear acceleration [m/s²] relative to the body's center of mass in world frame;
last three: angular acceleration [rad/s²] in world frame.
This is an extended state attribute; see :ref:`extended_state_attributes` for more information.
"""
self.body_f: wp.array | None = None
"""Rigid body forces (spatial) [N, N·m], shape (body_count,), dtype :class:`spatial_vector`.
First three entries: linear force [N] in world frame applied at the body's center of mass (COM).
Last three: torque (moment) [N·m] in world frame.
.. note::
:attr:`body_f` represents an external wrench in world frame with the body's center of mass (COM) as reference point.
"""
self.body_parent_f: wp.array | None = None
"""Parent interaction forces [N, N·m], shape (body_count,), dtype :class:`spatial_vector`.
First three entries: linear force [N]; last three: torque [N·m].
This is an extended state attribute; see :ref:`extended_state_attributes` for more information.
.. note::
:attr:`body_parent_f` represents incoming joint wrenches in world frame, referenced to the body's center of mass (COM).
"""
self.joint_q: wp.array | None = None
"""Generalized joint position coordinates [m or rad, depending on joint type], shape (joint_coord_count,), dtype float."""
self.joint_qd: wp.array | None = None
"""Generalized joint velocity coordinates [m/s or rad/s, depending on joint type], shape (joint_dof_count,), dtype float."""
def clear_forces(self) -> None:
"""
Clear all force arrays (for particles and bodies) in the state object.
Sets all entries of :attr:`particle_f` and :attr:`body_f` to zero, if present.
"""
with wp.ScopedTimer("clear_forces", False):
if self.particle_count:
self.particle_f.zero_()
if self.body_count:
self.body_f.zero_()
def assign(self, other: State) -> None:
"""
Copies the array attributes of another State object into this one.
This can be useful for swapping states in a simulation when using CUDA graphs.
If the number of substeps is odd, the last state needs to be explicitly copied for the graph to be captured correctly:
.. code-block:: python
# Assume we are capturing the following simulation loop in a CUDA graph
for i in range(sim_substeps):
state_0.clear_forces()
solver.step(state_0, state_1, control, contacts, sim_dt)
# Swap states - handle CUDA graph case specially
if sim_substeps % 2 == 1 and i == sim_substeps - 1:
# Swap states by copying the state arrays for graph capture
state_0.assign(state_1)
else:
# We can just swap the state references
state_0, state_1 = state_1, state_0
Args:
other: The source State object to copy from.
Raises:
ValueError: If the states have mismatched attributes (one has an array allocated where the other is None).
"""
attributes = set(self.__dict__).union(other.__dict__)
for attr in attributes:
val_self = getattr(self, attr, None)
val_other = getattr(other, attr, None)
if val_self is None and val_other is None:
continue
array_self = isinstance(val_self, wp.array)
array_other = isinstance(val_other, wp.array)
if not array_self and not array_other:
continue
if val_self is None or not array_self:
raise ValueError(f"State is missing array for '{attr}' which is present in the other state.")
if val_other is None or not array_other:
raise ValueError(f"Other state is missing array for '{attr}' which is present in this state.")
val_self.assign(val_other)
@property
def requires_grad(self) -> bool:
"""Indicates whether the state arrays have gradient computation enabled."""
if self.particle_q:
return self.particle_q.requires_grad
if self.body_q:
return self.body_q.requires_grad
return False
@property
def body_count(self) -> int:
"""The number of bodies represented in the state."""
if self.body_q is None:
return 0
return len(self.body_q)
@property
def particle_count(self) -> int:
"""The number of particles represented in the state."""
if self.particle_q is None:
return 0
return len(self.particle_q)
@property
def joint_coord_count(self) -> int:
"""The number of generalized joint position coordinates represented in the state."""
if self.joint_q is None:
return 0
return len(self.joint_q)
@property
def joint_dof_count(self) -> int:
"""The number of generalized joint velocity coordinates represented in the state."""
if self.joint_qd is None:
return 0
return len(self.joint_qd)
================================================
FILE: newton/_src/solvers/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .featherstone import SolverFeatherstone
from .flags import SolverNotifyFlags
from .implicit_mpm import SolverImplicitMPM
from .kamino import SolverKamino
from .mujoco import SolverMuJoCo
from .semi_implicit import SolverSemiImplicit
from .solver import SolverBase
from .style3d.solver_style3d import SolverStyle3D
from .vbd import SolverVBD
from .xpbd import SolverXPBD
__all__ = [
"SolverBase",
"SolverFeatherstone",
"SolverImplicitMPM",
"SolverKamino",
"SolverMuJoCo",
"SolverNotifyFlags",
"SolverSemiImplicit",
"SolverStyle3D",
"SolverVBD",
"SolverXPBD",
]
================================================
FILE: newton/_src/solvers/featherstone/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .solver_featherstone import SolverFeatherstone
__all__ = [
"SolverFeatherstone",
]
================================================
FILE: newton/_src/solvers/featherstone/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warp as wp
from ...math import transform_twist, velocity_at_point
from ...sim import BodyFlags, JointType, Model, State
from ...sim.articulation import (
compute_2d_rotational_dofs,
compute_3d_rotational_dofs,
)
from ..semi_implicit.kernels_body import joint_force
@wp.kernel
def compute_spatial_inertia(
body_inertia: wp.array[wp.mat33],
body_mass: wp.array[float],
# outputs
body_I_m: wp.array[wp.spatial_matrix],
):
tid = wp.tid()
I = body_inertia[tid]
m = body_mass[tid]
# fmt: off
body_I_m[tid] = wp.spatial_matrix(
m, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, m, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, m, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, I[0, 0], I[0, 1], I[0, 2],
0.0, 0.0, 0.0, I[1, 0], I[1, 1], I[1, 2],
0.0, 0.0, 0.0, I[2, 0], I[2, 1], I[2, 2],
)
# fmt: on
@wp.kernel
def compute_com_transforms(
body_com: wp.array[wp.vec3],
# outputs
body_X_com: wp.array[wp.transform],
):
tid = wp.tid()
com = body_com[tid]
body_X_com[tid] = wp.transform(com, wp.quat_identity())
@wp.kernel
def zero_kinematic_body_forces(
body_flags: wp.array[wp.int32],
body_f: wp.array[wp.spatial_vector],
):
"""Zero accumulated spatial forces for kinematic bodies."""
tid = wp.tid()
if (body_flags[tid] & BodyFlags.KINEMATIC) == 0:
return
body_f[tid] = wp.spatial_vector()
@wp.func
def transform_spatial_inertia(t: wp.transform, I: wp.spatial_matrix):
"""
Transform a spatial inertia tensor to a new coordinate frame.
This computes the change of coordinates for a spatial inertia tensor under a rigid-body
transformation `t`. The result is mathematically equivalent to:
adj_t^-T * I * adj_t^-1
where `adj_t` is the adjoint transformation matrix of `t`, and `I` is the spatial inertia
tensor in the original frame. This operation is described in Frank & Park, "Modern Robotics",
Section 8.2.3 (pg. 290).
Args:
t (wp.transform): The rigid-body transform (destination ← source).
I (wp.spatial_matrix): The spatial inertia tensor in the source frame.
Returns:
wp.spatial_matrix: The spatial inertia tensor expressed in the destination frame.
"""
t_inv = wp.transform_inverse(t)
q = wp.transform_get_rotation(t_inv)
p = wp.transform_get_translation(t_inv)
r1 = wp.quat_rotate(q, wp.vec3(1.0, 0.0, 0.0))
r2 = wp.quat_rotate(q, wp.vec3(0.0, 1.0, 0.0))
r3 = wp.quat_rotate(q, wp.vec3(0.0, 0.0, 1.0))
R = wp.matrix_from_cols(r1, r2, r3)
S = wp.skew(p) @ R
T = wp.spatial_matrix(
R[0, 0],
R[0, 1],
R[0, 2],
S[0, 0],
S[0, 1],
S[0, 2],
R[1, 0],
R[1, 1],
R[1, 2],
S[1, 0],
S[1, 1],
S[1, 2],
R[2, 0],
R[2, 1],
R[2, 2],
S[2, 0],
S[2, 1],
S[2, 2],
0.0,
0.0,
0.0,
R[0, 0],
R[0, 1],
R[0, 2],
0.0,
0.0,
0.0,
R[1, 0],
R[1, 1],
R[1, 2],
0.0,
0.0,
0.0,
R[2, 0],
R[2, 1],
R[2, 2],
)
return wp.mul(wp.mul(wp.transpose(T), I), T)
# compute transform across a joint
@wp.func
def jcalc_transform(
type: int,
joint_axis: wp.array[wp.vec3],
axis_start: int,
lin_axis_count: int,
ang_axis_count: int,
joint_q: wp.array[float],
q_start: int,
):
if type == JointType.PRISMATIC:
q = joint_q[q_start]
axis = joint_axis[axis_start]
X_jc = wp.transform(axis * q, wp.quat_identity())
return X_jc
if type == JointType.REVOLUTE:
q = joint_q[q_start]
axis = joint_axis[axis_start]
X_jc = wp.transform(wp.vec3(), wp.quat_from_axis_angle(axis, q))
return X_jc
if type == JointType.BALL:
qx = joint_q[q_start + 0]
qy = joint_q[q_start + 1]
qz = joint_q[q_start + 2]
qw = joint_q[q_start + 3]
X_jc = wp.transform(wp.vec3(), wp.quat(qx, qy, qz, qw))
return X_jc
if type == JointType.FIXED:
X_jc = wp.transform_identity()
return X_jc
if type == JointType.FREE or type == JointType.DISTANCE:
px = joint_q[q_start + 0]
py = joint_q[q_start + 1]
pz = joint_q[q_start + 2]
qx = joint_q[q_start + 3]
qy = joint_q[q_start + 4]
qz = joint_q[q_start + 5]
qw = joint_q[q_start + 6]
X_jc = wp.transform(wp.vec3(px, py, pz), wp.quat(qx, qy, qz, qw))
return X_jc
if type == JointType.D6:
pos = wp.vec3(0.0)
rot = wp.quat_identity()
# unroll for loop to ensure joint actions remain differentiable
# (since differentiating through a for loop that updates a local variable is not supported)
if lin_axis_count > 0:
axis = joint_axis[axis_start + 0]
pos += axis * joint_q[q_start + 0]
if lin_axis_count > 1:
axis = joint_axis[axis_start + 1]
pos += axis * joint_q[q_start + 1]
if lin_axis_count > 2:
axis = joint_axis[axis_start + 2]
pos += axis * joint_q[q_start + 2]
ia = axis_start + lin_axis_count
iq = q_start + lin_axis_count
if ang_axis_count == 1:
axis = joint_axis[ia]
rot = wp.quat_from_axis_angle(axis, joint_q[iq])
if ang_axis_count == 2:
rot, _ = compute_2d_rotational_dofs(
joint_axis[ia + 0],
joint_axis[ia + 1],
joint_q[iq + 0],
joint_q[iq + 1],
0.0,
0.0,
)
if ang_axis_count == 3:
rot, _ = compute_3d_rotational_dofs(
joint_axis[ia + 0],
joint_axis[ia + 1],
joint_axis[ia + 2],
joint_q[iq + 0],
joint_q[iq + 1],
joint_q[iq + 2],
0.0,
0.0,
0.0,
)
X_jc = wp.transform(pos, rot)
return X_jc
# default case
return wp.transform_identity()
# compute motion subspace and velocity for a joint
@wp.func
def jcalc_motion(
type: int,
joint_axis: wp.array[wp.vec3],
lin_axis_count: int,
ang_axis_count: int,
X_sc: wp.transform,
joint_qd: wp.array[float],
qd_start: int,
# outputs
joint_S_s: wp.array[wp.spatial_vector],
):
if type == JointType.PRISMATIC:
axis = joint_axis[qd_start]
S_s = transform_twist(X_sc, wp.spatial_vector(axis, wp.vec3()))
v_j_s = S_s * joint_qd[qd_start]
joint_S_s[qd_start] = S_s
return v_j_s
if type == JointType.REVOLUTE:
axis = joint_axis[qd_start]
S_s = transform_twist(X_sc, wp.spatial_vector(wp.vec3(), axis))
v_j_s = S_s * joint_qd[qd_start]
joint_S_s[qd_start] = S_s
return v_j_s
if type == JointType.D6:
v_j_s = wp.spatial_vector()
if lin_axis_count > 0:
axis = joint_axis[qd_start + 0]
S_s = transform_twist(X_sc, wp.spatial_vector(axis, wp.vec3()))
v_j_s += S_s * joint_qd[qd_start + 0]
joint_S_s[qd_start + 0] = S_s
if lin_axis_count > 1:
axis = joint_axis[qd_start + 1]
S_s = transform_twist(X_sc, wp.spatial_vector(axis, wp.vec3()))
v_j_s += S_s * joint_qd[qd_start + 1]
joint_S_s[qd_start + 1] = S_s
if lin_axis_count > 2:
axis = joint_axis[qd_start + 2]
S_s = transform_twist(X_sc, wp.spatial_vector(axis, wp.vec3()))
v_j_s += S_s * joint_qd[qd_start + 2]
joint_S_s[qd_start + 2] = S_s
if ang_axis_count > 0:
axis = joint_axis[qd_start + lin_axis_count + 0]
S_s = transform_twist(X_sc, wp.spatial_vector(wp.vec3(), axis))
v_j_s += S_s * joint_qd[qd_start + lin_axis_count + 0]
joint_S_s[qd_start + lin_axis_count + 0] = S_s
if ang_axis_count > 1:
axis = joint_axis[qd_start + lin_axis_count + 1]
S_s = transform_twist(X_sc, wp.spatial_vector(wp.vec3(), axis))
v_j_s += S_s * joint_qd[qd_start + lin_axis_count + 1]
joint_S_s[qd_start + lin_axis_count + 1] = S_s
if ang_axis_count > 2:
axis = joint_axis[qd_start + lin_axis_count + 2]
S_s = transform_twist(X_sc, wp.spatial_vector(wp.vec3(), axis))
v_j_s += S_s * joint_qd[qd_start + lin_axis_count + 2]
joint_S_s[qd_start + lin_axis_count + 2] = S_s
return v_j_s
if type == JointType.BALL:
S_0 = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 1.0, 0.0, 0.0))
S_1 = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 1.0, 0.0))
S_2 = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 1.0))
joint_S_s[qd_start + 0] = S_0
joint_S_s[qd_start + 1] = S_1
joint_S_s[qd_start + 2] = S_2
return S_0 * joint_qd[qd_start + 0] + S_1 * joint_qd[qd_start + 1] + S_2 * joint_qd[qd_start + 2]
if type == JointType.FIXED:
return wp.spatial_vector()
if type == JointType.FREE or type == JointType.DISTANCE:
v_j_s = transform_twist(
X_sc,
wp.spatial_vector(
joint_qd[qd_start + 0],
joint_qd[qd_start + 1],
joint_qd[qd_start + 2],
joint_qd[qd_start + 3],
joint_qd[qd_start + 4],
joint_qd[qd_start + 5],
),
)
joint_S_s[qd_start + 0] = transform_twist(X_sc, wp.spatial_vector(1.0, 0.0, 0.0, 0.0, 0.0, 0.0))
joint_S_s[qd_start + 1] = transform_twist(X_sc, wp.spatial_vector(0.0, 1.0, 0.0, 0.0, 0.0, 0.0))
joint_S_s[qd_start + 2] = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 1.0, 0.0, 0.0, 0.0))
joint_S_s[qd_start + 3] = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 1.0, 0.0, 0.0))
joint_S_s[qd_start + 4] = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 1.0, 0.0))
joint_S_s[qd_start + 5] = transform_twist(X_sc, wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 1.0))
return v_j_s
wp.printf("jcalc_motion not implemented for joint type %d\n", type)
# default case
return wp.spatial_vector()
# computes joint space forces/torques in tau
@wp.func
def jcalc_tau(
type: int,
joint_target_ke: wp.array[float],
joint_target_kd: wp.array[float],
joint_limit_ke: wp.array[float],
joint_limit_kd: wp.array[float],
joint_S_s: wp.array[wp.spatial_vector],
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_f: wp.array[float],
joint_target_pos: wp.array[float],
joint_target_vel: wp.array[float],
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
coord_start: int,
dof_start: int,
lin_axis_count: int,
ang_axis_count: int,
body_f_s: wp.spatial_vector,
# outputs
tau: wp.array[float],
):
if type == JointType.BALL:
# target_ke = joint_target_ke[dof_start]
# target_kd = joint_target_kd[dof_start]
for i in range(3):
S_s = joint_S_s[dof_start + i]
# w = joint_qd[dof_start + i]
# r = joint_q[coord_start + i]
tau[dof_start + i] = -wp.dot(S_s, body_f_s) + joint_f[dof_start + i]
# tau -= w * target_kd - r * target_ke
return
if type == JointType.FREE or type == JointType.DISTANCE:
for i in range(6):
S_s = joint_S_s[dof_start + i]
tau[dof_start + i] = -wp.dot(S_s, body_f_s) + joint_f[dof_start + i]
return
if type == JointType.PRISMATIC or type == JointType.REVOLUTE or type == JointType.D6:
axis_count = lin_axis_count + ang_axis_count
for i in range(axis_count):
j = dof_start + i
S_s = joint_S_s[j]
q = joint_q[coord_start + i]
qd = joint_qd[j]
lower = joint_limit_lower[j]
upper = joint_limit_upper[j]
limit_ke = joint_limit_ke[j]
limit_kd = joint_limit_kd[j]
target_ke = joint_target_ke[j]
target_kd = joint_target_kd[j]
target_pos = joint_target_pos[j]
target_vel = joint_target_vel[j]
drive_f = joint_force(q, qd, target_pos, target_vel, target_ke, target_kd, lower, upper, limit_ke, limit_kd)
# total torque / force on the joint
t = -wp.dot(S_s, body_f_s) + drive_f + joint_f[j]
tau[j] = t
return
@wp.func
def jcalc_integrate(
type: int,
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_qdd: wp.array[float],
coord_start: int,
dof_start: int,
lin_axis_count: int,
ang_axis_count: int,
dt: float,
# outputs
joint_q_new: wp.array[float],
joint_qd_new: wp.array[float],
):
if type == JointType.FIXED:
return
# prismatic / revolute
if type == JointType.PRISMATIC or type == JointType.REVOLUTE:
qdd = joint_qdd[dof_start]
qd = joint_qd[dof_start]
q = joint_q[coord_start]
qd_new = qd + qdd * dt
q_new = q + qd_new * dt
joint_qd_new[dof_start] = qd_new
joint_q_new[coord_start] = q_new
return
# ball
if type == JointType.BALL:
m_j = wp.vec3(joint_qdd[dof_start + 0], joint_qdd[dof_start + 1], joint_qdd[dof_start + 2])
w_j = wp.vec3(joint_qd[dof_start + 0], joint_qd[dof_start + 1], joint_qd[dof_start + 2])
r_j = wp.quat(
joint_q[coord_start + 0], joint_q[coord_start + 1], joint_q[coord_start + 2], joint_q[coord_start + 3]
)
# symplectic Euler
w_j_new = w_j + m_j * dt
drdt_j = wp.quat(w_j_new, 0.0) * r_j * 0.5
# new orientation (normalized)
r_j_new = wp.normalize(r_j + drdt_j * dt)
# update joint coords
joint_q_new[coord_start + 0] = r_j_new[0]
joint_q_new[coord_start + 1] = r_j_new[1]
joint_q_new[coord_start + 2] = r_j_new[2]
joint_q_new[coord_start + 3] = r_j_new[3]
# update joint vel
joint_qd_new[dof_start + 0] = w_j_new[0]
joint_qd_new[dof_start + 1] = w_j_new[1]
joint_qd_new[dof_start + 2] = w_j_new[2]
return
if type == JointType.FREE or type == JointType.DISTANCE:
a_s = wp.vec3(joint_qdd[dof_start + 0], joint_qdd[dof_start + 1], joint_qdd[dof_start + 2])
m_s = wp.vec3(joint_qdd[dof_start + 3], joint_qdd[dof_start + 4], joint_qdd[dof_start + 5])
v_s = wp.vec3(joint_qd[dof_start + 0], joint_qd[dof_start + 1], joint_qd[dof_start + 2])
w_s = wp.vec3(joint_qd[dof_start + 3], joint_qd[dof_start + 4], joint_qd[dof_start + 5])
# symplectic Euler
w_s = w_s + m_s * dt
v_s = v_s + a_s * dt
p_s = wp.vec3(joint_q[coord_start + 0], joint_q[coord_start + 1], joint_q[coord_start + 2])
dpdt_s = v_s + wp.cross(w_s, p_s)
r_s = wp.quat(
joint_q[coord_start + 3], joint_q[coord_start + 4], joint_q[coord_start + 5], joint_q[coord_start + 6]
)
drdt_s = wp.quat(w_s, 0.0) * r_s * 0.5
# new orientation (normalized)
p_s_new = p_s + dpdt_s * dt
r_s_new = wp.normalize(r_s + drdt_s * dt)
# update transform
joint_q_new[coord_start + 0] = p_s_new[0]
joint_q_new[coord_start + 1] = p_s_new[1]
joint_q_new[coord_start + 2] = p_s_new[2]
joint_q_new[coord_start + 3] = r_s_new[0]
joint_q_new[coord_start + 4] = r_s_new[1]
joint_q_new[coord_start + 5] = r_s_new[2]
joint_q_new[coord_start + 6] = r_s_new[3]
joint_qd_new[dof_start + 0] = v_s[0]
joint_qd_new[dof_start + 1] = v_s[1]
joint_qd_new[dof_start + 2] = v_s[2]
joint_qd_new[dof_start + 3] = w_s[0]
joint_qd_new[dof_start + 4] = w_s[1]
joint_qd_new[dof_start + 5] = w_s[2]
return
# other joint types (compound, universal, D6)
if type == JointType.D6:
axis_count = lin_axis_count + ang_axis_count
for i in range(axis_count):
qdd = joint_qdd[dof_start + i]
qd = joint_qd[dof_start + i]
q = joint_q[coord_start + i]
qd_new = qd + qdd * dt
q_new = q + qd_new * dt
joint_qd_new[dof_start + i] = qd_new
joint_q_new[coord_start + i] = q_new
return
@wp.func
def compute_link_transform(
i: int,
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_q: wp.array[float],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
body_X_com: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
# outputs
body_q: wp.array[wp.transform],
body_q_com: wp.array[wp.transform],
):
# parent transform
parent = joint_parent[i]
child = joint_child[i]
# parent transform in spatial coordinates
X_pj = joint_X_p[i]
X_cj = joint_X_c[i]
# parent anchor frame in world space
X_wpj = X_pj
if parent >= 0:
X_wp = body_q[parent]
X_wpj = X_wp * X_wpj
type = joint_type[i]
qd_start = joint_qd_start[i]
lin_axis_count = joint_dof_dim[i, 0]
ang_axis_count = joint_dof_dim[i, 1]
coord_start = joint_q_start[i]
# compute transform across joint
X_j = jcalc_transform(type, joint_axis, qd_start, lin_axis_count, ang_axis_count, joint_q, coord_start)
# transform from world to joint anchor frame at child body
X_wcj = X_wpj * X_j
# transform from world to child body frame
X_wc = X_wcj * wp.transform_inverse(X_cj)
# compute transform of center of mass
X_cm = body_X_com[child]
X_sm = X_wc * X_cm
# store geometry transforms
body_q[child] = X_wc
body_q_com[child] = X_sm
@wp.kernel
def eval_rigid_fk(
articulation_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_q: wp.array[float],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
body_X_com: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
# outputs
body_q: wp.array[wp.transform],
body_q_com: wp.array[wp.transform],
):
# one thread per-articulation
index = wp.tid()
start = articulation_start[index]
end = articulation_start[index + 1]
for i in range(start, end):
compute_link_transform(
i,
joint_type,
joint_parent,
joint_child,
joint_q_start,
joint_qd_start,
joint_q,
joint_X_p,
joint_X_c,
body_X_com,
joint_axis,
joint_dof_dim,
body_q,
body_q_com,
)
@wp.func
def spatial_cross(a: wp.spatial_vector, b: wp.spatial_vector):
w_a = wp.spatial_bottom(a)
v_a = wp.spatial_top(a)
w_b = wp.spatial_bottom(b)
v_b = wp.spatial_top(b)
w = wp.cross(w_a, w_b)
v = wp.cross(w_a, v_b) + wp.cross(v_a, w_b)
return wp.spatial_vector(v, w)
@wp.func
def spatial_cross_dual(a: wp.spatial_vector, b: wp.spatial_vector):
w_a = wp.spatial_bottom(a)
v_a = wp.spatial_top(a)
w_b = wp.spatial_bottom(b)
v_b = wp.spatial_top(b)
w = wp.cross(w_a, w_b) + wp.cross(v_a, v_b)
v = wp.cross(w_a, v_b)
return wp.spatial_vector(v, w)
@wp.func
def dense_index(stride: int, i: int, j: int):
return i * stride + j
@wp.func
def compute_link_velocity(
i: int,
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_qd_start: wp.array[int],
joint_qd: wp.array[float],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_I_m: wp.array[wp.spatial_matrix],
body_q: wp.array[wp.transform],
body_q_com: wp.array[wp.transform],
joint_X_p: wp.array[wp.transform],
body_world: wp.array[wp.int32],
gravity: wp.array[wp.vec3],
# outputs
joint_S_s: wp.array[wp.spatial_vector],
body_I_s: wp.array[wp.spatial_matrix],
body_v_s: wp.array[wp.spatial_vector],
body_f_s: wp.array[wp.spatial_vector],
body_a_s: wp.array[wp.spatial_vector],
):
type = joint_type[i]
child = joint_child[i]
parent = joint_parent[i]
qd_start = joint_qd_start[i]
X_pj = joint_X_p[i]
# X_cj = joint_X_c[i]
# parent anchor frame in world space
X_wpj = X_pj
if parent >= 0:
X_wp = body_q[parent]
X_wpj = X_wp * X_wpj
# compute motion subspace and velocity across the joint (also stores S_s to global memory)
lin_axis_count = joint_dof_dim[i, 0]
ang_axis_count = joint_dof_dim[i, 1]
v_j_s = jcalc_motion(
type,
joint_axis,
lin_axis_count,
ang_axis_count,
X_wpj,
joint_qd,
qd_start,
joint_S_s,
)
# parent velocity
v_parent_s = wp.spatial_vector()
a_parent_s = wp.spatial_vector()
if parent >= 0:
v_parent_s = body_v_s[parent]
a_parent_s = body_a_s[parent]
# body velocity, acceleration
v_s = v_parent_s + v_j_s
a_s = a_parent_s + spatial_cross(v_s, v_j_s) # + joint_S_s[i]*self.joint_qdd[i]
# compute body forces
X_sm = body_q_com[child]
I_m = body_I_m[child]
# gravity and external forces (expressed in frame aligned with s but centered at body mass)
m = I_m[0, 0]
world_idx = body_world[child]
world_g = gravity[wp.max(world_idx, 0)]
f_g = m * world_g
r_com = wp.transform_get_translation(X_sm)
f_g_s = wp.spatial_vector(f_g, wp.cross(r_com, f_g))
# body forces
I_s = transform_spatial_inertia(X_sm, I_m)
f_b_s = I_s * a_s + spatial_cross_dual(v_s, I_s * v_s)
body_v_s[child] = v_s
body_a_s[child] = a_s
body_f_s[child] = f_b_s - f_g_s
body_I_s[child] = I_s
# Convert body forces from COM-frame to world-origin-frame and negate for use in Featherstone dynamics.
@wp.kernel
def convert_body_force_com_to_origin(
body_q: wp.array[wp.transform],
body_X_com: wp.array[wp.transform],
# outputs
body_f_ext: wp.array[wp.spatial_vector],
):
tid = wp.tid()
f_ext_com = body_f_ext[tid]
# skip if force is zero
if wp.length(f_ext_com) == 0.0:
return
body_q_com_val = body_q[tid] * body_X_com[tid]
r_com = wp.transform_get_translation(body_q_com_val)
force = wp.spatial_top(f_ext_com)
torque_com = wp.spatial_bottom(f_ext_com)
body_f_ext[tid] = -wp.spatial_vector(force, torque_com + wp.cross(r_com, force))
# Inverse dynamics via Recursive Newton-Euler algorithm (Featherstone Table 5.1)
@wp.kernel
def eval_rigid_id(
articulation_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_qd_start: wp.array[int],
joint_qd: wp.array[float],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_I_m: wp.array[wp.spatial_matrix],
body_q: wp.array[wp.transform],
body_q_com: wp.array[wp.transform],
joint_X_p: wp.array[wp.transform],
body_world: wp.array[wp.int32],
gravity: wp.array[wp.vec3],
# outputs
joint_S_s: wp.array[wp.spatial_vector],
body_I_s: wp.array[wp.spatial_matrix],
body_v_s: wp.array[wp.spatial_vector],
body_f_s: wp.array[wp.spatial_vector],
body_a_s: wp.array[wp.spatial_vector],
):
# one thread per-articulation
index = wp.tid()
start = articulation_start[index]
end = articulation_start[index + 1]
# compute link velocities and coriolis forces
for i in range(start, end):
compute_link_velocity(
i,
joint_type,
joint_parent,
joint_child,
joint_qd_start,
joint_qd,
joint_axis,
joint_dof_dim,
body_I_m,
body_q,
body_q_com,
joint_X_p,
body_world,
gravity,
joint_S_s,
body_I_s,
body_v_s,
body_f_s,
body_a_s,
)
@wp.kernel
def eval_rigid_tau(
articulation_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_dof_dim: wp.array2d[int],
joint_target_pos: wp.array[float],
joint_target_vel: wp.array[float],
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_f: wp.array[float],
joint_target_ke: wp.array[float],
joint_target_kd: wp.array[float],
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
joint_limit_ke: wp.array[float],
joint_limit_kd: wp.array[float],
joint_S_s: wp.array[wp.spatial_vector],
body_fb_s: wp.array[wp.spatial_vector],
body_f_ext: wp.array[wp.spatial_vector],
# outputs
body_ft_s: wp.array[wp.spatial_vector],
tau: wp.array[float],
):
# one thread per-articulation
index = wp.tid()
start = articulation_start[index]
end = articulation_start[index + 1]
count = end - start
# compute joint forces
for offset in range(count):
# for backwards traversal
i = end - offset - 1
type = joint_type[i]
parent = joint_parent[i]
child = joint_child[i]
dof_start = joint_qd_start[i]
coord_start = joint_q_start[i]
lin_axis_count = joint_dof_dim[i, 0]
ang_axis_count = joint_dof_dim[i, 1]
# total forces on body
f_b_s = body_fb_s[child]
f_t_s = body_ft_s[child]
f_ext = body_f_ext[child]
f_s = f_b_s + f_t_s + f_ext
# compute joint-space forces, writes out tau
jcalc_tau(
type,
joint_target_ke,
joint_target_kd,
joint_limit_ke,
joint_limit_kd,
joint_S_s,
joint_q,
joint_qd,
joint_f,
joint_target_pos,
joint_target_vel,
joint_limit_lower,
joint_limit_upper,
coord_start,
dof_start,
lin_axis_count,
ang_axis_count,
f_s,
tau,
)
# update parent forces, todo: check that this is valid for the backwards pass
if parent >= 0:
wp.atomic_add(body_ft_s, parent, f_s)
# builds spatial Jacobian J which is an (joint_count*6)x(dof_count) matrix
@wp.kernel
def eval_rigid_jacobian(
articulation_start: wp.array[int],
articulation_J_start: wp.array[int],
joint_ancestor: wp.array[int],
joint_qd_start: wp.array[int],
joint_S_s: wp.array[wp.spatial_vector],
# outputs
J: wp.array[float],
):
# one thread per-articulation
index = wp.tid()
joint_start = articulation_start[index]
joint_end = articulation_start[index + 1]
joint_count = joint_end - joint_start
J_offset = articulation_J_start[index]
articulation_dof_start = joint_qd_start[joint_start]
articulation_dof_end = joint_qd_start[joint_end]
articulation_dof_count = articulation_dof_end - articulation_dof_start
for i in range(joint_count):
row_start = i * 6
j = joint_start + i
while j != -1:
joint_dof_start = joint_qd_start[j]
joint_dof_end = joint_qd_start[j + 1]
joint_dof_count = joint_dof_end - joint_dof_start
# fill out each row of the Jacobian walking up the tree
for dof in range(joint_dof_count):
col = (joint_dof_start - articulation_dof_start) + dof
S = joint_S_s[joint_dof_start + dof]
for k in range(6):
J[J_offset + dense_index(articulation_dof_count, row_start + k, col)] = S[k]
j = joint_ancestor[j]
@wp.func
def spatial_mass(
body_I_s: wp.array[wp.spatial_matrix],
joint_start: int,
joint_count: int,
M_start: int,
# outputs
M: wp.array[float],
):
stride = joint_count * 6
for l in range(joint_count):
I = body_I_s[joint_start + l]
for i in range(6):
for j in range(6):
M[M_start + dense_index(stride, l * 6 + i, l * 6 + j)] = I[i, j]
@wp.kernel
def eval_rigid_mass(
articulation_start: wp.array[int],
articulation_M_start: wp.array[int],
body_I_s: wp.array[wp.spatial_matrix],
# outputs
M: wp.array[float],
):
# one thread per-articulation
index = wp.tid()
joint_start = articulation_start[index]
joint_end = articulation_start[index + 1]
joint_count = joint_end - joint_start
M_offset = articulation_M_start[index]
spatial_mass(body_I_s, joint_start, joint_count, M_offset, M)
@wp.func
def dense_gemm(
m: int,
n: int,
p: int,
transpose_A: bool,
transpose_B: bool,
add_to_C: bool,
A_start: int,
B_start: int,
C_start: int,
A: wp.array[float],
B: wp.array[float],
# outputs
C: wp.array[float],
):
# multiply a `m x p` matrix A by a `p x n` matrix B to produce a `m x n` matrix C
for i in range(m):
for j in range(n):
sum = float(0.0)
for k in range(p):
if transpose_A:
a_i = k * m + i
else:
a_i = i * p + k
if transpose_B:
b_j = j * p + k
else:
b_j = k * n + j
sum += A[A_start + a_i] * B[B_start + b_j]
if add_to_C:
C[C_start + i * n + j] += sum
else:
C[C_start + i * n + j] = sum
# @wp.func_grad(dense_gemm)
# def adj_dense_gemm(
# m: int,
# n: int,
# p: int,
# transpose_A: bool,
# transpose_B: bool,
# add_to_C: bool,
# A_start: int,
# B_start: int,
# C_start: int,
# A: wp.array[float],
# B: wp.array[float],
# # outputs
# C: wp.array[float],
# ):
# add_to_C = True
# if transpose_A:
# dense_gemm(p, m, n, False, True, add_to_C, A_start, B_start, C_start, B, wp.adjoint[C], wp.adjoint[A])
# dense_gemm(p, n, m, False, False, add_to_C, A_start, B_start, C_start, A, wp.adjoint[C], wp.adjoint[B])
# else:
# dense_gemm(
# m, p, n, False, not transpose_B, add_to_C, A_start, B_start, C_start, wp.adjoint[C], B, wp.adjoint[A]
# )
# dense_gemm(p, n, m, True, False, add_to_C, A_start, B_start, C_start, A, wp.adjoint[C], wp.adjoint[B])
def create_inertia_matrix_kernel(num_joints, num_dofs):
@wp.kernel
def eval_dense_gemm_tile(J_arr: wp.array3d[float], M_arr: wp.array3d[float], H_arr: wp.array3d[float]):
articulation = wp.tid()
J = wp.tile_load(J_arr[articulation], shape=(wp.static(6 * num_joints), num_dofs))
P = wp.tile_zeros(shape=(wp.static(6 * num_joints), num_dofs), dtype=float)
# compute P = M*J where M is a 6x6 block diagonal mass matrix
for i in range(int(num_joints)):
# 6x6 block matrices are on the diagonal
M_body = wp.tile_load(M_arr[articulation], shape=(6, 6), offset=(i * 6, i * 6))
# load a 6xN row from the Jacobian
J_body = wp.tile_view(J, offset=(i * 6, 0), shape=(6, num_dofs))
# compute weighted row
P_body = wp.tile_matmul(M_body, J_body)
# assign to the P slice
wp.tile_assign(P, P_body, offset=(i * 6, 0))
# compute H = J^T*P
H = wp.tile_matmul(wp.tile_transpose(J), P)
wp.tile_store(H_arr[articulation], H)
return eval_dense_gemm_tile
def create_batched_cholesky_kernel(num_dofs):
assert num_dofs == 18
@wp.kernel
def eval_tiled_dense_cholesky_batched(A: wp.array3d[float], R: wp.array2d[float], L: wp.array3d[float]):
articulation = wp.tid()
a = wp.tile_load(A[articulation], shape=(num_dofs, num_dofs), storage="shared")
r = wp.tile_load(R[articulation], shape=num_dofs, storage="shared")
a_r = wp.tile_diag_add(a, r)
l = wp.tile_cholesky(a_r)
wp.tile_store(L[articulation], wp.tile_transpose(l))
return eval_tiled_dense_cholesky_batched
def create_inertia_matrix_cholesky_kernel(num_joints, num_dofs):
@wp.kernel
def eval_dense_gemm_and_cholesky_tile(
J_arr: wp.array3d[float],
M_arr: wp.array3d[float],
R_arr: wp.array2d[float],
H_arr: wp.array3d[float],
L_arr: wp.array3d[float],
):
articulation = wp.tid()
J = wp.tile_load(J_arr[articulation], shape=(wp.static(6 * num_joints), num_dofs))
P = wp.tile_zeros(shape=(wp.static(6 * num_joints), num_dofs), dtype=float)
# compute P = M*J where M is a 6x6 block diagonal mass matrix
for i in range(int(num_joints)):
# 6x6 block matrices are on the diagonal
M_body = wp.tile_load(M_arr[articulation], shape=(6, 6), offset=(i * 6, i * 6))
# load a 6xN row from the Jacobian
J_body = wp.tile_view(J, offset=(i * 6, 0), shape=(6, num_dofs))
# compute weighted row
P_body = wp.tile_matmul(M_body, J_body)
# assign to the P slice
wp.tile_assign(P, P_body, offset=(i * 6, 0))
# compute H = J^T*P
H = wp.tile_matmul(wp.tile_transpose(J), P)
wp.tile_store(H_arr[articulation], H)
# cholesky L L^T = (H + diag(R))
R = wp.tile_load(R_arr[articulation], shape=num_dofs, storage="shared")
H_R = wp.tile_diag_add(H, R)
L = wp.tile_cholesky(H_R)
wp.tile_store(L_arr[articulation], L)
return eval_dense_gemm_and_cholesky_tile
@wp.kernel
def eval_dense_gemm_batched(
m: wp.array[int],
n: wp.array[int],
p: wp.array[int],
transpose_A: bool,
transpose_B: bool,
A_start: wp.array[int],
B_start: wp.array[int],
C_start: wp.array[int],
A: wp.array[float],
B: wp.array[float],
C: wp.array[float],
):
# on the CPU each thread computes the whole matrix multiply
# on the GPU each block computes the multiply with one output per-thread
batch = wp.tid() # /kNumThreadsPerBlock;
add_to_C = False
dense_gemm(
m[batch],
n[batch],
p[batch],
transpose_A,
transpose_B,
add_to_C,
A_start[batch],
B_start[batch],
C_start[batch],
A,
B,
C,
)
@wp.func
def dense_cholesky(
n: int,
A: wp.array[float],
R: wp.array[float],
A_start: int,
R_start: int,
# outputs
L: wp.array[float],
):
# compute the Cholesky factorization of A = L L^T with diagonal regularization R
for j in range(n):
s = A[A_start + dense_index(n, j, j)] + R[R_start + j]
for k in range(j):
r = L[A_start + dense_index(n, j, k)]
s -= r * r
s = wp.sqrt(s)
invS = 1.0 / s
L[A_start + dense_index(n, j, j)] = s
for i in range(j + 1, n):
s = A[A_start + dense_index(n, i, j)]
for k in range(j):
s -= L[A_start + dense_index(n, i, k)] * L[A_start + dense_index(n, j, k)]
L[A_start + dense_index(n, i, j)] = s * invS
@wp.func_grad(dense_cholesky)
def adj_dense_cholesky(
n: int,
A: wp.array[float],
R: wp.array[float],
A_start: int,
R_start: int,
# outputs
L: wp.array[float],
):
# nop, use dense_solve to differentiate through (A^-1)b = x
pass
@wp.kernel
def eval_dense_cholesky_batched(
A_starts: wp.array[int],
A_dim: wp.array[int],
R_starts: wp.array[int],
A: wp.array[float],
R: wp.array[float],
L: wp.array[float],
):
batch = wp.tid()
n = A_dim[batch]
A_start = A_starts[batch]
R_start = R_starts[batch]
dense_cholesky(n, A, R, A_start, R_start, L)
@wp.func
def dense_subs(
n: int,
L_start: int,
b_start: int,
L: wp.array[float],
b: wp.array[float],
# outputs
x: wp.array[float],
):
# Solves (L L^T) x = b for x given the Cholesky factor L
# forward substitution solves the lower triangular system L y = b for y
for i in range(n):
s = b[b_start + i]
for j in range(i):
s -= L[L_start + dense_index(n, i, j)] * x[b_start + j]
x[b_start + i] = s / L[L_start + dense_index(n, i, i)]
# backward substitution solves the upper triangular system L^T x = y for x
for i in range(n - 1, -1, -1):
s = x[b_start + i]
for j in range(i + 1, n):
s -= L[L_start + dense_index(n, j, i)] * x[b_start + j]
x[b_start + i] = s / L[L_start + dense_index(n, i, i)]
@wp.func
def dense_solve(
n: int,
L_start: int,
b_start: int,
A: wp.array[float],
L: wp.array[float],
b: wp.array[float],
# outputs
x: wp.array[float],
tmp: wp.array[float],
):
# helper function to include tmp argument for backward pass
dense_subs(n, L_start, b_start, L, b, x)
@wp.func_grad(dense_solve)
def adj_dense_solve(
n: int,
L_start: int,
b_start: int,
A: wp.array[float],
L: wp.array[float],
b: wp.array[float],
# outputs
x: wp.array[float],
tmp: wp.array[float],
):
if not tmp or not wp.adjoint[x] or not wp.adjoint[A] or not wp.adjoint[L]:
return
for i in range(n):
tmp[b_start + i] = 0.0
dense_subs(n, L_start, b_start, L, wp.adjoint[x], tmp)
for i in range(n):
wp.adjoint[b][b_start + i] += tmp[b_start + i]
# A* = -adj_b*x^T
for i in range(n):
for j in range(n):
wp.adjoint[L][L_start + dense_index(n, i, j)] += -tmp[b_start + i] * x[b_start + j]
for i in range(n):
for j in range(n):
wp.adjoint[A][L_start + dense_index(n, i, j)] += -tmp[b_start + i] * x[b_start + j]
@wp.kernel
def eval_dense_solve_batched(
L_start: wp.array[int],
L_dim: wp.array[int],
b_start: wp.array[int],
A: wp.array[float],
L: wp.array[float],
b: wp.array[float],
# outputs
x: wp.array[float],
tmp: wp.array[float],
):
batch = wp.tid()
dense_solve(L_dim[batch], L_start[batch], b_start[batch], A, L, b, x, tmp)
@wp.kernel
def integrate_generalized_joints(
joint_type: wp.array[int],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_dof_dim: wp.array2d[int],
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_qdd: wp.array[float],
dt: float,
# outputs
joint_q_new: wp.array[float],
joint_qd_new: wp.array[float],
):
# one thread per-articulation
index = wp.tid()
type = joint_type[index]
coord_start = joint_q_start[index]
dof_start = joint_qd_start[index]
lin_axis_count = joint_dof_dim[index, 0]
ang_axis_count = joint_dof_dim[index, 1]
jcalc_integrate(
type,
joint_q,
joint_qd,
joint_qdd,
coord_start,
dof_start,
lin_axis_count,
ang_axis_count,
dt,
joint_q_new,
joint_qd_new,
)
@wp.kernel
def zero_kinematic_joint_qdd(
joint_child: wp.array[int],
body_flags: wp.array[wp.int32],
joint_qd_start: wp.array[int],
joint_qdd: wp.array[float],
):
"""Zero joint accelerations for joints whose child body is kinematic."""
joint_id = wp.tid()
child = joint_child[joint_id]
if (body_flags[child] & BodyFlags.KINEMATIC) == 0:
return
dof_start = joint_qd_start[joint_id]
dof_end = joint_qd_start[joint_id + 1]
for i in range(dof_start, dof_end):
joint_qdd[i] = 0.0
@wp.kernel
def copy_kinematic_joint_state(
joint_child: wp.array[int],
body_flags: wp.array[wp.int32],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_q_in: wp.array[float],
joint_qd_in: wp.array[float],
joint_q_out: wp.array[float],
joint_qd_out: wp.array[float],
):
"""Copy prescribed joint state through the solve for kinematic child bodies."""
joint_id = wp.tid()
child = joint_child[joint_id]
if (body_flags[child] & BodyFlags.KINEMATIC) == 0:
return
q_start = joint_q_start[joint_id]
q_end = joint_q_start[joint_id + 1]
for i in range(q_start, q_end):
joint_q_out[i] = joint_q_in[i]
qd_start = joint_qd_start[joint_id]
qd_end = joint_qd_start[joint_id + 1]
for i in range(qd_start, qd_end):
joint_qd_out[i] = joint_qd_in[i]
# ============================================================================
# Forward Kinematics with Velocity Conversion for Featherstone
# ============================================================================
# Local copy of FK function that converts FREE/DISTANCE joint velocities from
# origin frame to COM frame, as required by the Featherstone solver.
@wp.func
def eval_single_articulation_fk_with_velocity_conversion(
joint_start: int,
joint_end: int,
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_com: wp.array[wp.vec3],
# outputs
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
):
for i in range(joint_start, joint_end):
parent = joint_parent[i]
child = joint_child[i]
# compute transform across the joint
type = joint_type[i]
X_pj = joint_X_p[i]
X_cj = joint_X_c[i]
q_start = joint_q_start[i]
qd_start = joint_qd_start[i]
lin_axis_count = joint_dof_dim[i, 0]
ang_axis_count = joint_dof_dim[i, 1]
X_j = wp.transform_identity()
v_j = wp.spatial_vector(wp.vec3(), wp.vec3())
if type == JointType.PRISMATIC:
axis = joint_axis[qd_start]
q = joint_q[q_start]
qd = joint_qd[qd_start]
X_j = wp.transform(axis * q, wp.quat_identity())
v_j = wp.spatial_vector(axis * qd, wp.vec3())
if type == JointType.REVOLUTE:
axis = joint_axis[qd_start]
q = joint_q[q_start]
qd = joint_qd[qd_start]
X_j = wp.transform(wp.vec3(), wp.quat_from_axis_angle(axis, q))
v_j = wp.spatial_vector(wp.vec3(), axis * qd)
if type == JointType.BALL:
r = wp.quat(joint_q[q_start + 0], joint_q[q_start + 1], joint_q[q_start + 2], joint_q[q_start + 3])
w = wp.vec3(joint_qd[qd_start + 0], joint_qd[qd_start + 1], joint_qd[qd_start + 2])
X_j = wp.transform(wp.vec3(), r)
v_j = wp.spatial_vector(wp.vec3(), w)
if type == JointType.FREE or type == JointType.DISTANCE:
t = wp.transform(
wp.vec3(joint_q[q_start + 0], joint_q[q_start + 1], joint_q[q_start + 2]),
wp.quat(joint_q[q_start + 3], joint_q[q_start + 4], joint_q[q_start + 5], joint_q[q_start + 6]),
)
v = wp.spatial_vector(
wp.vec3(joint_qd[qd_start + 0], joint_qd[qd_start + 1], joint_qd[qd_start + 2]),
wp.vec3(joint_qd[qd_start + 3], joint_qd[qd_start + 4], joint_qd[qd_start + 5]),
)
X_j = t
v_j = v
if type == JointType.D6:
pos = wp.vec3(0.0)
rot = wp.quat_identity()
vel_v = wp.vec3(0.0)
vel_w = wp.vec3(0.0)
# unroll for loop to ensure joint actions remain differentiable
# (since differentiating through a for loop that updates a local variable is not supported)
if lin_axis_count > 0:
axis = joint_axis[qd_start + 0]
pos += axis * joint_q[q_start + 0]
vel_v += axis * joint_qd[qd_start + 0]
if lin_axis_count > 1:
axis = joint_axis[qd_start + 1]
pos += axis * joint_q[q_start + 1]
vel_v += axis * joint_qd[qd_start + 1]
if lin_axis_count > 2:
axis = joint_axis[qd_start + 2]
pos += axis * joint_q[q_start + 2]
vel_v += axis * joint_qd[qd_start + 2]
iq = q_start + lin_axis_count
iqd = qd_start + lin_axis_count
if ang_axis_count == 1:
axis = joint_axis[iqd]
rot = wp.quat_from_axis_angle(axis, joint_q[iq])
vel_w = joint_qd[iqd] * axis
if ang_axis_count == 2:
rot, vel_w = compute_2d_rotational_dofs(
joint_axis[iqd + 0],
joint_axis[iqd + 1],
joint_q[iq + 0],
joint_q[iq + 1],
joint_qd[iqd + 0],
joint_qd[iqd + 1],
)
if ang_axis_count == 3:
rot, vel_w = compute_3d_rotational_dofs(
joint_axis[iqd + 0],
joint_axis[iqd + 1],
joint_axis[iqd + 2],
joint_q[iq + 0],
joint_q[iq + 1],
joint_q[iq + 2],
joint_qd[iqd + 0],
joint_qd[iqd + 1],
joint_qd[iqd + 2],
)
X_j = wp.transform(pos, rot)
v_j = wp.spatial_vector(vel_v, vel_w)
# transform from world to parent joint anchor frame
X_wpj = X_pj
if parent >= 0:
X_wp = body_q[parent]
X_wpj = X_wp * X_wpj
# transform from world to joint anchor frame at child body
X_wcj = X_wpj * X_j
# transform from world to child body frame
X_wc = X_wcj * wp.transform_inverse(X_cj)
v_parent_origin = wp.vec3()
w_parent = wp.vec3()
if parent >= 0:
v_wp = body_qd[parent]
w_parent = wp.spatial_bottom(v_wp)
v_parent_origin = velocity_at_point(
v_wp, wp.transform_get_translation(X_wc) - wp.transform_get_translation(X_wp)
)
linear_joint_anchor = wp.transform_vector(X_wpj, wp.spatial_top(v_j))
angular_joint_world = wp.transform_vector(X_wpj, wp.spatial_bottom(v_j))
child_origin_offset_world = wp.transform_get_translation(X_wc) - wp.transform_get_translation(X_wcj)
linear_joint_origin = linear_joint_anchor + wp.cross(angular_joint_world, child_origin_offset_world)
v_wc = wp.spatial_vector(v_parent_origin + linear_joint_origin, w_parent + angular_joint_world)
body_q[child] = X_wc
body_qd[child] = v_wc
@wp.kernel
def convert_articulation_free_distance_body_qd(
articulation_start: wp.array[int],
articulation_count: int,
articulation_mask: wp.array[bool],
articulation_indices: wp.array[int],
joint_type: wp.array[int],
joint_child: wp.array[int],
body_com: wp.array[wp.vec3],
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
):
tid = wp.tid()
if articulation_indices:
articulation_id = articulation_indices[tid]
else:
articulation_id = tid
if articulation_id < 0 or articulation_id >= articulation_count:
return
if articulation_mask:
if not articulation_mask[articulation_id]:
return
joint_start = articulation_start[articulation_id]
joint_end = articulation_start[articulation_id + 1]
for i in range(joint_start, joint_end):
type = joint_type[i]
if type != JointType.FREE and type != JointType.DISTANCE:
continue
child = joint_child[i]
X_wc = body_q[child]
v_wc = body_qd[child]
v_origin = wp.spatial_top(v_wc)
omega = wp.spatial_bottom(v_wc)
r_com = wp.transform_point(X_wc, body_com[child])
v_com = v_origin + wp.cross(omega, r_com)
body_qd[child] = wp.spatial_vector(v_com, omega)
@wp.kernel
def eval_articulation_fk_with_velocity_conversion(
articulation_start: wp.array[int],
articulation_count: int, # total number of articulations
articulation_mask: wp.array[
bool
], # used to enable / disable FK for an articulation, if None then treat all as enabled
articulation_indices: wp.array[int], # can be None, articulation indices to process
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_com: wp.array[wp.vec3],
# outputs
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
):
tid = wp.tid()
# Determine which articulation to process
if articulation_indices:
# Using indices - get actual articulation ID from array
articulation_id = articulation_indices[tid]
else:
# No indices - articulation ID is just the thread index
articulation_id = tid
# Bounds check
if articulation_id < 0 or articulation_id >= articulation_count:
return # Invalid articulation index
# early out if disabling FK for this articulation
if articulation_mask:
if not articulation_mask[articulation_id]:
return
joint_start = articulation_start[articulation_id]
joint_end = articulation_start[articulation_id + 1]
eval_single_articulation_fk_with_velocity_conversion(
joint_start,
joint_end,
joint_q,
joint_qd,
joint_q_start,
joint_qd_start,
joint_type,
joint_parent,
joint_child,
joint_X_p,
joint_X_c,
joint_axis,
joint_dof_dim,
body_com,
# outputs
body_q,
body_qd,
)
def eval_fk_with_velocity_conversion(
model: Model,
joint_q: wp.array[float],
joint_qd: wp.array[float],
state: State,
mask: wp.array[bool] | None = None,
indices: wp.array[int] | None = None,
):
"""
Evaluates the model's forward kinematics with velocity conversion for Featherstone solver.
This is a local copy that converts FREE/DISTANCE joint velocities from origin frame to COM frame,
as required by the Featherstone solver. Updates the state's body information (:attr:`State.body_q`
and :attr:`State.body_qd`).
Args:
model (Model): The model to evaluate.
joint_q (array): Generalized joint position coordinates, shape [joint_coord_count], float
joint_qd (array): Generalized joint velocity coordinates, shape [joint_dof_count], float
state (State): The state to update.
mask (array): The mask to use to enable / disable FK for an articulation. If None then treat all as enabled, shape [articulation_count], bool
indices (array): Integer indices of articulations to update. If None, updates all articulations.
Cannot be used together with mask parameter.
"""
# Validate inputs
if mask is not None and indices is not None:
raise ValueError("Cannot specify both mask and indices parameters")
# Determine launch dimensions
if indices is not None:
num_articulations = len(indices)
else:
num_articulations = model.articulation_count
wp.launch(
kernel=eval_articulation_fk_with_velocity_conversion,
dim=num_articulations,
inputs=[
model.articulation_start,
model.articulation_count,
mask,
indices,
joint_q,
joint_qd,
model.joint_q_start,
model.joint_qd_start,
model.joint_type,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_axis,
model.joint_dof_dim,
model.body_com,
],
outputs=[
state.body_q,
state.body_qd,
],
device=model.device,
)
wp.launch(
kernel=convert_articulation_free_distance_body_qd,
dim=num_articulations,
inputs=[
model.articulation_start,
model.articulation_count,
mask,
indices,
model.joint_type,
model.joint_child,
model.body_com,
],
outputs=[
state.body_q,
state.body_qd,
],
device=model.device,
)
================================================
FILE: newton/_src/solvers/featherstone/solver_featherstone.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import numpy as np
import warp as wp
from ...core.types import override
from ...sim import BodyFlags, Contacts, Control, Model, State
from ..flags import SolverNotifyFlags
from ..semi_implicit.kernels_contact import (
eval_body_contact,
eval_particle_body_contact_forces,
eval_particle_contact_forces,
)
from ..semi_implicit.kernels_muscle import (
eval_muscle_forces,
)
from ..semi_implicit.kernels_particle import (
eval_bending_forces,
eval_spring_forces,
eval_tetrahedra_forces,
eval_triangle_forces,
)
from ..solver import SolverBase
from .kernels import (
compute_com_transforms,
compute_spatial_inertia,
convert_body_force_com_to_origin,
copy_kinematic_joint_state,
create_inertia_matrix_cholesky_kernel,
create_inertia_matrix_kernel,
eval_dense_cholesky_batched,
eval_dense_gemm_batched,
eval_dense_solve_batched,
eval_fk_with_velocity_conversion,
eval_rigid_fk,
eval_rigid_id,
eval_rigid_jacobian,
eval_rigid_mass,
eval_rigid_tau,
integrate_generalized_joints,
zero_kinematic_body_forces,
zero_kinematic_joint_qdd,
)
class SolverFeatherstone(SolverBase):
"""A semi-implicit integrator using symplectic Euler that operates
on reduced (also called generalized) coordinates to simulate articulated rigid body dynamics
based on Featherstone's composite rigid body algorithm (CRBA).
See: Featherstone, Roy. Rigid Body Dynamics Algorithms. Springer US, 2014.
Instead of maximal coordinates :attr:`~newton.State.body_q` (rigid body positions) and :attr:`~newton.State.body_qd`
(rigid body velocities) as is the case in :class:`~newton.solvers.SolverSemiImplicit` and :class:`~newton.solvers.SolverXPBD`,
:class:`~newton.solvers.SolverFeatherstone` uses :attr:`~newton.State.joint_q` and :attr:`~newton.State.joint_qd` to represent
the positions and velocities of joints without allowing any redundant degrees of freedom.
After constructing :class:`~newton.Model` and :class:`~newton.State` objects this time-integrator
may be used to advance the simulation state forward in time.
Note:
Unlike :class:`~newton.solvers.SolverSemiImplicit` and :class:`~newton.solvers.SolverXPBD`, :class:`~newton.solvers.SolverFeatherstone`
does not simulate rigid bodies with nonzero mass as floating bodies if they are not connected through any joints.
Floating-base systems require an explicit free joint with which the body is connected to the world,
see :meth:`newton.ModelBuilder.add_joint_free`.
Semi-implicit time integration is a variational integrator that
preserves energy, however it not unconditionally stable, and requires a time-step
small enough to support the required stiffness and damping forces.
See: https://en.wikipedia.org/wiki/Semi-implicit_Euler_method
This solver uses the routines from :class:`~newton.solvers.SolverSemiImplicit` to simulate particles, cloth, and soft bodies.
Joint limitations:
- Supported joint types: PRISMATIC, REVOLUTE, BALL, FIXED, FREE, DISTANCE (treated as FREE), D6.
CABLE joints are not supported.
- :attr:`~newton.Model.joint_armature`, :attr:`~newton.Model.joint_limit_ke`/:attr:`~newton.Model.joint_limit_kd`,
:attr:`~newton.Model.joint_target_ke`/:attr:`~newton.Model.joint_target_kd`, and :attr:`~newton.Control.joint_f`
are supported.
- :attr:`~newton.Model.joint_friction`, :attr:`~newton.Model.joint_effort_limit`,
:attr:`~newton.Model.joint_velocity_limit`, :attr:`~newton.Model.joint_enabled`,
and :attr:`~newton.Model.joint_target_mode` are not supported.
- Equality and mimic constraints are not supported.
See :ref:`Joint feature support` for the full comparison across solvers.
Example
-------
.. code-block:: python
solver = newton.solvers.SolverFeatherstone(model)
# simulation loop
for i in range(100):
solver.step(state_in, state_out, control, contacts, dt)
state_in, state_out = state_out, state_in
"""
def __init__(
self,
model: Model,
angular_damping: float = 0.05,
update_mass_matrix_interval: int = 1,
friction_smoothing: float = 1.0,
use_tile_gemm: bool = False,
fuse_cholesky: bool = True,
):
"""
Args:
model: The model to be simulated.
angular_damping: Angular damping factor. Defaults to 0.05.
update_mass_matrix_interval: How often to update the mass matrix (every n-th time the :meth:`step` function gets called). Defaults to 1.
friction_smoothing: The delta value for the Huber norm (see :func:`warp.norm_huber() `) used for the friction velocity normalization. Defaults to 1.0.
use_tile_gemm: Whether to use operators from Warp's Tile API to solve for joint accelerations. Defaults to False.
fuse_cholesky: Whether to fuse the Cholesky decomposition into the inertia matrix evaluation kernel when using the Tile API. Only used if `use_tile_gemm` is true. Defaults to True.
"""
super().__init__(model)
self.angular_damping = angular_damping
self.update_mass_matrix_interval = update_mass_matrix_interval
self.friction_smoothing = friction_smoothing
self.use_tile_gemm = use_tile_gemm
self.fuse_cholesky = fuse_cholesky
self._step = 0
self._mass_matrix_dirty = False
self._update_kinematic_state()
self._compute_articulation_indices(model)
self._allocate_model_aux_vars(model)
if self.use_tile_gemm:
# create a custom kernel to evaluate the system matrix for this type
if self.fuse_cholesky:
self.eval_inertia_matrix_cholesky_kernel = create_inertia_matrix_cholesky_kernel(
int(self.joint_count), int(self.dof_count)
)
else:
self.eval_inertia_matrix_kernel = create_inertia_matrix_kernel(
int(self.joint_count), int(self.dof_count)
)
# ensure matrix is reloaded since otherwise an unload can happen during graph capture
# todo: should not be necessary?
wp.load_module(device=wp.get_device())
def _update_kinematic_state(self):
"""Recompute cached kinematic body/joint flags and effective armature."""
model = self.model
self.has_kinematic_bodies = False
self.has_kinematic_joints = False
self.joint_armature_effective = model.joint_armature
if model.body_count:
body_flags = model.body_flags.numpy()
kinematic_mask = (body_flags & int(BodyFlags.KINEMATIC)) != 0
self.has_kinematic_bodies = bool(np.any(kinematic_mask))
if model.joint_count and self.has_kinematic_bodies:
joint_child = model.joint_child.numpy()
joint_qd_start = model.joint_qd_start.numpy()
joint_armature = model.joint_armature.numpy().copy()
for joint_idx in range(model.joint_count):
if not kinematic_mask[joint_child[joint_idx]]:
continue
self.has_kinematic_joints = True
dof_start = int(joint_qd_start[joint_idx])
dof_end = int(joint_qd_start[joint_idx + 1])
joint_armature[dof_start:dof_end] = 1.0e10
if self.has_kinematic_joints:
self.joint_armature_effective = wp.array(joint_armature, dtype=float, device=model.device)
@override
def notify_model_changed(self, flags: int) -> None:
if flags & (SolverNotifyFlags.BODY_PROPERTIES | SolverNotifyFlags.JOINT_DOF_PROPERTIES):
self._update_kinematic_state()
self._mass_matrix_dirty = True
def _compute_articulation_indices(self, model):
# calculate total size and offsets of Jacobian and mass matrices for entire system
if model.joint_count:
self.J_size = 0
self.M_size = 0
self.H_size = 0
articulation_J_start = []
articulation_M_start = []
articulation_H_start = []
articulation_M_rows = []
articulation_H_rows = []
articulation_J_rows = []
articulation_J_cols = []
articulation_dof_start = []
articulation_coord_start = []
articulation_start = model.articulation_start.numpy()
joint_q_start = model.joint_q_start.numpy()
joint_qd_start = model.joint_qd_start.numpy()
for i in range(model.articulation_count):
first_joint = articulation_start[i]
last_joint = articulation_start[i + 1]
first_coord = joint_q_start[first_joint]
first_dof = joint_qd_start[first_joint]
last_dof = joint_qd_start[last_joint]
joint_count = last_joint - first_joint
dof_count = last_dof - first_dof
articulation_J_start.append(self.J_size)
articulation_M_start.append(self.M_size)
articulation_H_start.append(self.H_size)
articulation_dof_start.append(first_dof)
articulation_coord_start.append(first_coord)
# bit of data duplication here, but will leave it as such for clarity
articulation_M_rows.append(joint_count * 6)
articulation_H_rows.append(dof_count)
articulation_J_rows.append(joint_count * 6)
articulation_J_cols.append(dof_count)
if self.use_tile_gemm:
# store the joint and dof count assuming all
# articulations have the same structure
self.joint_count = joint_count
self.dof_count = dof_count
self.J_size += 6 * joint_count * dof_count
self.M_size += 6 * joint_count * 6 * joint_count
self.H_size += dof_count * dof_count
# matrix offsets for batched gemm
self.articulation_J_start = wp.array(articulation_J_start, dtype=wp.int32, device=model.device)
self.articulation_M_start = wp.array(articulation_M_start, dtype=wp.int32, device=model.device)
self.articulation_H_start = wp.array(articulation_H_start, dtype=wp.int32, device=model.device)
self.articulation_M_rows = wp.array(articulation_M_rows, dtype=wp.int32, device=model.device)
self.articulation_H_rows = wp.array(articulation_H_rows, dtype=wp.int32, device=model.device)
self.articulation_J_rows = wp.array(articulation_J_rows, dtype=wp.int32, device=model.device)
self.articulation_J_cols = wp.array(articulation_J_cols, dtype=wp.int32, device=model.device)
self.articulation_dof_start = wp.array(articulation_dof_start, dtype=wp.int32, device=model.device)
self.articulation_coord_start = wp.array(articulation_coord_start, dtype=wp.int32, device=model.device)
def _allocate_model_aux_vars(self, model):
# allocate mass, Jacobian matrices, and other auxiliary variables pertaining to the model
if model.joint_count:
# system matrices
self.M = wp.zeros((self.M_size,), dtype=wp.float32, device=model.device, requires_grad=model.requires_grad)
self.J = wp.zeros((self.J_size,), dtype=wp.float32, device=model.device, requires_grad=model.requires_grad)
self.P = wp.empty_like(self.J, requires_grad=model.requires_grad)
self.H = wp.empty((self.H_size,), dtype=wp.float32, device=model.device, requires_grad=model.requires_grad)
# zero since only upper triangle is set which can trigger NaN detection
self.L = wp.zeros_like(self.H)
if model.body_count:
self.body_I_m = wp.empty(
(model.body_count,), dtype=wp.spatial_matrix, device=model.device, requires_grad=model.requires_grad
)
wp.launch(
compute_spatial_inertia,
model.body_count,
inputs=[model.body_inertia, model.body_mass],
outputs=[self.body_I_m],
device=model.device,
)
self.body_X_com = wp.empty(
(model.body_count,), dtype=wp.transform, device=model.device, requires_grad=model.requires_grad
)
wp.launch(
compute_com_transforms,
model.body_count,
inputs=[model.body_com],
outputs=[self.body_X_com],
device=model.device,
)
def _allocate_state_aux_vars(self, model, target, requires_grad):
# allocate auxiliary variables that vary with state
if model.body_count:
# joints
target.joint_qdd = wp.zeros_like(model.joint_qd, requires_grad=requires_grad)
target.joint_tau = wp.empty_like(model.joint_qd, requires_grad=requires_grad)
if requires_grad:
# used in the custom grad implementation of eval_dense_solve_batched
target.joint_solve_tmp = wp.zeros_like(model.joint_qd, requires_grad=True)
else:
target.joint_solve_tmp = None
target.joint_S_s = wp.empty(
(model.joint_dof_count,),
dtype=wp.spatial_vector,
device=model.device,
requires_grad=requires_grad,
)
# derived rigid body data (maximal coordinates)
target.body_q_com = wp.empty_like(model.body_q, requires_grad=requires_grad)
target.body_I_s = wp.empty(
(model.body_count,), dtype=wp.spatial_matrix, device=model.device, requires_grad=requires_grad
)
target.body_v_s = wp.empty(
(model.body_count,), dtype=wp.spatial_vector, device=model.device, requires_grad=requires_grad
)
target.body_a_s = wp.empty(
(model.body_count,), dtype=wp.spatial_vector, device=model.device, requires_grad=requires_grad
)
target.body_f_s = wp.zeros(
(model.body_count,), dtype=wp.spatial_vector, device=model.device, requires_grad=requires_grad
)
target.body_ft_s = wp.zeros(
(model.body_count,), dtype=wp.spatial_vector, device=model.device, requires_grad=requires_grad
)
target._featherstone_augmented = True
@override
def step(
self,
state_in: State,
state_out: State,
control: Control,
contacts: Contacts,
dt: float,
) -> None:
requires_grad = state_in.requires_grad
# optionally create dynamical auxiliary variables
if requires_grad:
state_aug = state_out
else:
state_aug = self
model = self.model
if not getattr(state_aug, "_featherstone_augmented", False):
self._allocate_state_aux_vars(model, state_aug, requires_grad)
if control is None:
control = model.control(clone_variables=False)
with wp.ScopedTimer("simulate", False):
particle_f = None
body_f = None
if state_in.particle_count:
particle_f = state_in.particle_f
if state_in.body_count:
body_f = state_in.body_f
wp.launch(
convert_body_force_com_to_origin,
dim=model.body_count,
inputs=[state_in.body_q, self.body_X_com],
outputs=[body_f],
device=model.device,
)
# damped springs
eval_spring_forces(model, state_in, particle_f)
# triangle elastic and lift/drag forces
eval_triangle_forces(model, state_in, control, particle_f)
# triangle bending
eval_bending_forces(model, state_in, particle_f)
# tetrahedral FEM
eval_tetrahedra_forces(model, state_in, control, particle_f)
# particle-particle interactions
eval_particle_contact_forces(model, state_in, particle_f)
# particle shape contact
eval_particle_body_contact_forces(model, state_in, contacts, particle_f, body_f, body_f_in_world_frame=True)
# muscles
if False:
eval_muscle_forces(model, state_in, control, body_f)
# ----------------------------
# articulations
if model.joint_count:
# evaluate body transforms
wp.launch(
eval_rigid_fk,
dim=model.articulation_count,
inputs=[
model.articulation_start,
model.joint_type,
model.joint_parent,
model.joint_child,
model.joint_q_start,
model.joint_qd_start,
state_in.joint_q,
model.joint_X_p,
model.joint_X_c,
self.body_X_com,
model.joint_axis,
model.joint_dof_dim,
],
outputs=[state_in.body_q, state_aug.body_q_com],
device=model.device,
)
# print("body_X_sc:")
# print(state_in.body_q.numpy())
# evaluate joint inertias, motion vectors, and forces
state_aug.body_f_s.zero_()
wp.launch(
eval_rigid_id,
dim=model.articulation_count,
inputs=[
model.articulation_start,
model.joint_type,
model.joint_parent,
model.joint_child,
model.joint_qd_start,
state_in.joint_qd,
model.joint_axis,
model.joint_dof_dim,
self.body_I_m,
state_in.body_q,
state_aug.body_q_com,
model.joint_X_p,
model.body_world,
model.gravity,
],
outputs=[
state_aug.joint_S_s,
state_aug.body_I_s,
state_aug.body_v_s,
state_aug.body_f_s,
state_aug.body_a_s,
],
device=model.device,
)
if contacts is not None and contacts.rigid_contact_max:
wp.launch(
kernel=eval_body_contact,
dim=contacts.rigid_contact_max,
inputs=[
state_in.body_q,
state_aug.body_v_s,
model.body_com,
model.shape_material_ke,
model.shape_material_kd,
model.shape_material_kf,
model.shape_material_ka,
model.shape_material_mu,
model.shape_body,
contacts.rigid_contact_count,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_normal,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
contacts.rigid_contact_stiffness,
contacts.rigid_contact_damping,
contacts.rigid_contact_friction,
True,
self.friction_smoothing,
],
outputs=[body_f],
device=model.device,
)
if self.has_kinematic_bodies and body_f is not None:
wp.launch(
zero_kinematic_body_forces,
dim=model.body_count,
inputs=[model.body_flags],
outputs=[body_f],
device=model.device,
)
if model.articulation_count:
# evaluate joint torques
state_aug.body_ft_s.zero_()
wp.launch(
eval_rigid_tau,
dim=model.articulation_count,
inputs=[
model.articulation_start,
model.joint_type,
model.joint_parent,
model.joint_child,
model.joint_q_start,
model.joint_qd_start,
model.joint_dof_dim,
control.joint_target_pos,
control.joint_target_vel,
state_in.joint_q,
state_in.joint_qd,
control.joint_f,
model.joint_target_ke,
model.joint_target_kd,
model.joint_limit_lower,
model.joint_limit_upper,
model.joint_limit_ke,
model.joint_limit_kd,
state_aug.joint_S_s,
state_aug.body_f_s,
body_f,
],
outputs=[
state_aug.body_ft_s,
state_aug.joint_tau,
],
device=model.device,
)
# print("joint_tau:")
# print(state_aug.joint_tau.numpy())
# print("body_q:")
# print(state_in.body_q.numpy())
# print("body_qd:")
# print(state_in.body_qd.numpy())
if self._mass_matrix_dirty or self._step % self.update_mass_matrix_interval == 0:
# build J
wp.launch(
eval_rigid_jacobian,
dim=model.articulation_count,
inputs=[
model.articulation_start,
self.articulation_J_start,
model.joint_ancestor,
model.joint_qd_start,
state_aug.joint_S_s,
],
outputs=[self.J],
device=model.device,
)
# build M
wp.launch(
eval_rigid_mass,
dim=model.articulation_count,
inputs=[
model.articulation_start,
self.articulation_M_start,
state_aug.body_I_s,
],
outputs=[self.M],
device=model.device,
)
if self.use_tile_gemm:
# reshape arrays
M_tiled = self.M.reshape((-1, 6 * self.joint_count, 6 * self.joint_count))
J_tiled = self.J.reshape((-1, 6 * self.joint_count, self.dof_count))
R_tiled = self.joint_armature_effective.reshape((-1, self.dof_count))
H_tiled = self.H.reshape((-1, self.dof_count, self.dof_count))
L_tiled = self.L.reshape((-1, self.dof_count, self.dof_count))
assert H_tiled.shape == (model.articulation_count, 18, 18)
assert L_tiled.shape == (model.articulation_count, 18, 18)
assert R_tiled.shape == (model.articulation_count, 18)
if self.fuse_cholesky:
wp.launch_tiled(
self.eval_inertia_matrix_cholesky_kernel,
dim=model.articulation_count,
inputs=[J_tiled, M_tiled, R_tiled],
outputs=[H_tiled, L_tiled],
device=model.device,
block_dim=64,
)
else:
wp.launch_tiled(
self.eval_inertia_matrix_kernel,
dim=model.articulation_count,
inputs=[J_tiled, M_tiled],
outputs=[H_tiled],
device=model.device,
block_dim=256,
)
wp.launch(
eval_dense_cholesky_batched,
dim=model.articulation_count,
inputs=[
self.articulation_H_start,
self.articulation_H_rows,
self.articulation_dof_start,
self.H,
self.joint_armature_effective,
],
outputs=[self.L],
device=model.device,
)
# import numpy as np
# J = J_tiled.numpy()
# M = M_tiled.numpy()
# R = R_tiled.numpy()
# for i in range(model.articulation_count):
# r = R[i,:,0]
# H = J[i].T @ M[i] @ J[i]
# L = np.linalg.cholesky(H + np.diag(r))
# np.testing.assert_allclose(H, H_tiled.numpy()[i], rtol=1e-2, atol=1e-2)
# np.testing.assert_allclose(L, L_tiled.numpy()[i], rtol=1e-1, atol=1e-1)
else:
# form P = M*J
wp.launch(
eval_dense_gemm_batched,
dim=model.articulation_count,
inputs=[
self.articulation_M_rows,
self.articulation_J_cols,
self.articulation_J_rows,
False,
False,
self.articulation_M_start,
self.articulation_J_start,
# P start is the same as J start since it has the same dims as J
self.articulation_J_start,
self.M,
self.J,
],
outputs=[self.P],
device=model.device,
)
# form H = J^T*P
wp.launch(
eval_dense_gemm_batched,
dim=model.articulation_count,
inputs=[
self.articulation_J_cols,
self.articulation_J_cols,
# P rows is the same as J rows
self.articulation_J_rows,
True,
False,
self.articulation_J_start,
# P start is the same as J start since it has the same dims as J
self.articulation_J_start,
self.articulation_H_start,
self.J,
self.P,
],
outputs=[self.H],
device=model.device,
)
# compute decomposition
wp.launch(
eval_dense_cholesky_batched,
dim=model.articulation_count,
inputs=[
self.articulation_H_start,
self.articulation_H_rows,
self.articulation_dof_start,
self.H,
self.joint_armature_effective,
],
outputs=[self.L],
device=model.device,
)
# print("joint_target:")
# print(control.joint_target.numpy())
# print("joint_tau:")
# print(state_aug.joint_tau.numpy())
# print("H:")
# print(self.H.numpy())
# print("L:")
# print(self.L.numpy())
self._mass_matrix_dirty = False
# solve for qdd
state_aug.joint_qdd.zero_()
wp.launch(
eval_dense_solve_batched,
dim=model.articulation_count,
inputs=[
self.articulation_H_start,
self.articulation_H_rows,
self.articulation_dof_start,
self.H,
self.L,
state_aug.joint_tau,
],
outputs=[
state_aug.joint_qdd,
state_aug.joint_solve_tmp,
],
device=model.device,
)
if self.has_kinematic_joints:
wp.launch(
zero_kinematic_joint_qdd,
dim=model.joint_count,
inputs=[model.joint_child, model.body_flags, model.joint_qd_start],
outputs=[state_aug.joint_qdd],
device=model.device,
)
# print("joint_qdd:")
# print(state_aug.joint_qdd.numpy())
# print("\n\n")
# -------------------------------------
# integrate bodies
if model.joint_count:
wp.launch(
kernel=integrate_generalized_joints,
dim=model.joint_count,
inputs=[
model.joint_type,
model.joint_q_start,
model.joint_qd_start,
model.joint_dof_dim,
state_in.joint_q,
state_in.joint_qd,
state_aug.joint_qdd,
dt,
],
outputs=[state_out.joint_q, state_out.joint_qd],
device=model.device,
)
if self.has_kinematic_joints:
wp.launch(
copy_kinematic_joint_state,
dim=model.joint_count,
inputs=[
model.joint_child,
model.body_flags,
model.joint_q_start,
model.joint_qd_start,
state_in.joint_q,
state_in.joint_qd,
],
outputs=[state_out.joint_q, state_out.joint_qd],
device=model.device,
)
# update maximal coordinates using FK with velocity conversion
eval_fk_with_velocity_conversion(model, state_out.joint_q, state_out.joint_qd, state_out)
self.integrate_particles(model, state_in, state_out, dt)
self._step += 1
================================================
FILE: newton/_src/solvers/flags.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Solver flags."""
from enum import IntEnum
# model update flags - used for solver.notify_model_update()
class SolverNotifyFlags(IntEnum):
"""
Flags indicating which parts of the model have been updated and require the solver to be notified.
These flags are used with `solver.notify_model_update()` to specify which properties have changed,
allowing the solver to efficiently update only the necessary components.
"""
JOINT_PROPERTIES = 1 << 0
"""Indicates joint property updates: joint_q, joint_X_p, joint_X_c."""
JOINT_DOF_PROPERTIES = 1 << 1
"""Indicates joint DOF property updates: joint_target_ke, joint_target_kd, joint_effort_limit, joint_armature, joint_friction, joint_limit_ke, joint_limit_kd, joint_limit_lower, joint_limit_upper."""
BODY_PROPERTIES = 1 << 2
"""Indicates body property updates: body_q, body_qd, body_flags."""
BODY_INERTIAL_PROPERTIES = 1 << 3
"""Indicates body inertial property updates: body_com, body_inertia, body_inv_inertia, body_mass, body_inv_mass."""
SHAPE_PROPERTIES = 1 << 4
"""Indicates shape property updates: shape_transform, shape_scale, shape_collision_radius, shape_material_mu, shape_material_ke, shape_material_kd, rigid_contact_mu_torsional, rigid_contact_mu_rolling."""
MODEL_PROPERTIES = 1 << 5
"""Indicates model property updates: gravity and other global parameters."""
CONSTRAINT_PROPERTIES = 1 << 6
"""Indicates constraint property updates: equality constraints (equality_constraint_anchor, equality_constraint_relpose, equality_constraint_polycoef, equality_constraint_torquescale, equality_constraint_enabled, mujoco.eq_solref, mujoco.eq_solimp) and mimic constraints (constraint_mimic_coef0, constraint_mimic_coef1, constraint_mimic_enabled)."""
TENDON_PROPERTIES = 1 << 7
"""Indicates tendon properties: eg tendon_stiffness."""
ACTUATOR_PROPERTIES = 1 << 8
"""Indicates actuator property updates: gains, biases, limits, etc."""
ALL = (
JOINT_PROPERTIES
| JOINT_DOF_PROPERTIES
| BODY_PROPERTIES
| BODY_INERTIAL_PROPERTIES
| SHAPE_PROPERTIES
| MODEL_PROPERTIES
| CONSTRAINT_PROPERTIES
| TENDON_PROPERTIES
| ACTUATOR_PROPERTIES
)
"""Indicates all property updates."""
__all__ = [
"SolverNotifyFlags",
]
================================================
FILE: newton/_src/solvers/implicit_mpm/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .solver_implicit_mpm import SolverImplicitMPM
__all__ = ["SolverImplicitMPM"]
================================================
FILE: newton/_src/solvers/implicit_mpm/contact_solver_kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
import warp.sparse as sp
wp.set_module_options({"enable_backward": False})
@wp.kernel
def compute_collider_inv_mass(
J_mat_offsets: wp.array[int],
J_mat_columns: wp.array[int],
J_mat_values: wp.array[wp.mat33],
IJtm_mat_offsets: wp.array[int],
IJtm_mat_columns: wp.array[int],
IJtm_mat_values: wp.array[wp.mat33],
collider_inv_mass: wp.array[float],
):
i = wp.tid()
block_beg = J_mat_offsets[i]
block_end = J_mat_offsets[i + 1]
w_mat = wp.mat33(0.0)
for b in range(block_beg, block_end):
col = J_mat_columns[b]
transposed_block = sp.bsr_block_index(col, i, IJtm_mat_offsets, IJtm_mat_columns)
if transposed_block == -1:
continue
# Mass-splitting: divide by number of nodes overlapping with this body
multiplicity = float(IJtm_mat_offsets[col + 1] - IJtm_mat_offsets[col])
w_mat += (J_mat_values[b] @ IJtm_mat_values[transposed_block]) * multiplicity
_eigvecs, eigvals = wp.eig3(w_mat)
collider_inv_mass[i] = wp.max(0.0, wp.max(eigvals))
@wp.func
def project_on_friction_cone(
mu: float,
nor: wp.vec3,
r: wp.vec3,
):
"""Projects a stress vector ``r`` onto the Coulomb friction cone (non-orthogonally)."""
r_n = wp.dot(r, nor)
r_t = r - r_n * nor
r_n = wp.max(0.0, r_n)
mu_rn = mu * r_n
r_t_n2 = wp.length_sq(r_t)
if r_t_n2 > mu_rn * mu_rn:
r_t *= mu_rn / wp.sqrt(r_t_n2)
return r_n * nor + r_t
@wp.func
def solve_coulomb_isotropic(
mu: float,
nor: wp.vec3,
u: wp.vec3,
):
"""Solves for the relative velocity in the Coulomb friction model,
assuming an isotropic velocity-impulse relationship, u = r + b
"""
u_n = wp.dot(u, nor)
if u_n < 0.0:
u -= u_n * nor
tau = wp.length_sq(u)
alpha = mu * u_n
if tau <= alpha * alpha:
u = wp.vec3(0.0)
else:
u *= 1.0 + mu * u_n / wp.sqrt(tau)
return u
@wp.func
def filter_collider_impulse_warmstart(
friction: float,
nor: wp.vec3,
adhesion: float,
impulse: wp.vec3,
):
"""Filters the collider impulse to be within the friction cone"""
if friction < 0.0:
return wp.vec3(0.0)
return project_on_friction_cone(friction, nor, impulse + adhesion * nor) - adhesion * nor
@wp.kernel
def apply_nodal_impulse_warmstart(
collider_impulse: wp.array[wp.vec3],
collider_friction: wp.array[float],
collider_normals: wp.array[wp.vec3],
collider_adhesion: wp.array[float],
inv_mass: wp.array[float],
velocities: wp.array[wp.vec3],
delta_impulse: wp.array[wp.vec3],
):
"""
Applies pre-computed impulses to particles and colliders.
"""
i = wp.tid()
impulse = filter_collider_impulse_warmstart(
collider_friction[i], collider_normals[i], collider_adhesion[i], collider_impulse[i]
)
collider_impulse[i] = impulse
delta_impulse[i] = impulse
velocities[i] += inv_mass[i] * impulse
@wp.kernel
def solve_nodal_friction(
inv_mass: wp.array[float],
collider_friction: wp.array[float],
collider_adhesion: wp.array[float],
collider_normals: wp.array[wp.vec3],
collider_inv_mass: wp.array[float],
velocities: wp.array[wp.vec3],
collider_velocities: wp.array[wp.vec3],
impulse: wp.array[wp.vec3],
delta_impulse: wp.array[wp.vec3],
):
"""
Solves for frictional impulses at nodes interacting with colliders.
For each node (i) potentially in contact:
1. Skips if friction coefficient is negative (no friction).
2. Calculates the relative velocity `u0` between the particle and collider,
accounting for the existing impulse and adhesion.
3. Computes the effective inverse mass `w` for the interaction.
4. Calls `solve_coulomb_isotropic` to determine the change in relative
velocity `u` due to friction.
5. Calculates the change in impulse `delta_impulse` required to achieve this
change in relative velocity.
6. Updates the total impulse, particle velocity, and collider velocity.
"""
i = wp.tid()
friction_coeff = collider_friction[i]
if friction_coeff < 0.0:
return
n = collider_normals[i]
u0 = velocities[i] - collider_velocities[i]
w = inv_mass[i] + collider_inv_mass[i]
u = solve_coulomb_isotropic(friction_coeff, n, u0 - (impulse[i] + collider_adhesion[i] * n) * w)
delta_u = u - u0
delta_lambda = delta_u / w
delta_impulse[i] = delta_lambda
impulse[i] += delta_lambda
velocities[i] += inv_mass[i] * delta_lambda
@wp.kernel
def apply_subgrid_impulse(
tr_collider_mat_offsets: wp.array[int],
tr_collider_mat_columns: wp.array[int],
tr_collider_mat_values: wp.array[float],
inv_mass: wp.array[float],
impulses: wp.array[wp.vec3],
velocities: wp.array[wp.vec3],
):
"""
Applies pre-computed impulses to particles and colliders.
"""
u_i = wp.tid()
block_beg = tr_collider_mat_offsets[u_i]
block_end = tr_collider_mat_offsets[u_i + 1]
delta_f = wp.vec3(0.0)
for b in range(block_beg, block_end):
delta_f += tr_collider_mat_values[b] * impulses[tr_collider_mat_columns[b]]
velocities[u_i] += inv_mass[u_i] * delta_f
@wp.kernel
def apply_subgrid_impulse_warmstart(
collider_friction: wp.array[float],
collider_normals: wp.array[wp.vec3],
collider_adhesion: wp.array[float],
collider_impulse: wp.array[wp.vec3],
delta_impulse: wp.array[wp.vec3],
):
i = wp.tid()
impulse = filter_collider_impulse_warmstart(
collider_friction[i], collider_normals[i], collider_adhesion[i], collider_impulse[i]
)
collider_impulse[i] = impulse
delta_impulse[i] = impulse
@wp.kernel
def compute_collider_delassus_diagonal(
collider_mat_offsets: wp.array[int],
collider_mat_columns: wp.array[int],
collider_mat_values: wp.array[float],
collider_inv_mass: wp.array[float],
transposed_collider_mat_offsets: wp.array[int],
inv_volume: wp.array[float],
delassus_diagonal: wp.array[float],
):
i = wp.tid()
block_beg = collider_mat_offsets[i]
block_end = collider_mat_offsets[i + 1]
inv_mass = collider_inv_mass[i]
w = inv_mass
for b in range(block_beg, block_end):
u_i = collider_mat_columns[b]
weight = collider_mat_values[b]
multiplicity = transposed_collider_mat_offsets[u_i + 1] - transposed_collider_mat_offsets[u_i]
w += weight * weight * inv_volume[u_i] * float(multiplicity)
delassus_diagonal[i] = w
@wp.kernel
def solve_subgrid_friction(
velocity: wp.array[wp.vec3],
collider_mat_offsets: wp.array[int],
collider_mat_columns: wp.array[int],
collider_mat_values: wp.array[float],
collider_friction: wp.array[float],
collider_adhesion: wp.array[float],
collider_normals: wp.array[wp.vec3],
collider_delassus_diagonal: wp.array[float],
collider_velocities: wp.array[wp.vec3],
impulse: wp.array[wp.vec3],
delta_impulse: wp.array[wp.vec3],
):
i = wp.tid()
w = collider_delassus_diagonal[i]
friction_coeff = collider_friction[i]
if w <= 0.0 or friction_coeff < 0.0:
return
beg = collider_mat_offsets[i]
end = collider_mat_offsets[i + 1]
u0 = -collider_velocities[i]
for b in range(beg, end):
u_i = collider_mat_columns[b]
u0 += collider_mat_values[b] * velocity[u_i]
n = collider_normals[i]
u = solve_coulomb_isotropic(friction_coeff, n, u0 - (impulse[i] + collider_adhesion[i] * n) * w)
delta_u = u - u0
delta_lambda = delta_u / w
impulse[i] += delta_lambda
delta_impulse[i] = delta_lambda
================================================
FILE: newton/_src/solvers/implicit_mpm/implicit_mpm_model.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Implicit MPM model."""
import math
from typing import TYPE_CHECKING
import numpy as np
import warp as wp
import newton
from .rasterized_collisions import Collider
__all__ = ["ImplicitMPMModel"]
if TYPE_CHECKING:
from .solver_implicit_mpm import SolverImplicitMPM
_INFINITY = wp.constant(1.0e12)
"""Value above which quantities are considered infinite"""
_EPSILON = wp.constant(1.0 / _INFINITY)
"""Value below which quantities are considered zero"""
_DEFAULT_PROJECTION_THRESHOLD = 0.01
"""Default threshold for projection outside of collider, as a fraction of the voxel size"""
_DEFAULT_THICKNESS = 0.01
"""Default thickness for colliders, as a fraction of the voxel size"""
_DEFAULT_FRICTION = 0.5
"""Default friction coefficient for colliders"""
_DEFAULT_ADHESION = 0.0
"""Default adhesion coefficient for colliders (Pa)"""
def _particle_parameter(
num_particles, model_value: float | wp.array | None = None, default_value=None, model_scale: wp.array | None = None
):
"""Helper function to create a particle-wise parameter array, taking defaults either from the model
or the global options."""
if model_value is None:
return wp.full(num_particles, default_value, dtype=float)
elif isinstance(model_value, wp.array):
if model_value.shape[0] != num_particles:
raise ValueError(f"Model value array must have {num_particles} elements")
return model_value if model_scale is None else model_value * model_scale
else:
return wp.full(num_particles, model_value, dtype=float) if model_scale is None else model_value * model_scale
def _merge_meshes(
points: list[np.array] = (),
indices: list[np.array] = (),
shape_ids: np.array = (),
material_ids: np.array = (),
) -> tuple[wp.array, wp.array, wp.array, np.array]:
"""Merges the points and indices of several meshes into a single one"""
pt_count = np.array([len(pts) for pts in points])
face_count = np.array([len(idx) // 3 for idx in indices])
offsets = np.cumsum(pt_count) - pt_count
merged_points = np.vstack([pts[:, :3] for pts in points])
merged_indices = np.concatenate([idx + offsets[k] for k, idx in enumerate(indices)])
vertex_shape_ids = np.repeat(np.arange(len(points), dtype=int), repeats=pt_count)
face_shape_ids = np.repeat(np.arange(len(points), dtype=int), repeats=face_count)
return (
wp.array(merged_points, dtype=wp.vec3),
wp.array(merged_indices, dtype=int),
wp.array(shape_ids[vertex_shape_ids], dtype=int),
np.array(material_ids, dtype=int)[face_shape_ids],
)
def _get_shape_mesh(model: newton.Model, shape_id: int, geo_type: newton.GeoType, geo_scale: wp.vec3) -> newton.Mesh:
"""Get a shape mesh from a model."""
if geo_type == newton.GeoType.MESH:
src_mesh = model.shape_source[shape_id]
vertices = src_mesh.vertices * np.array(geo_scale)
indices = src_mesh.indices
return newton.Mesh(vertices, indices, compute_inertia=False)
if geo_type == newton.GeoType.PLANE:
# Handle "infinite" planes encoded with non-positive scales
width = geo_scale[0] if len(geo_scale) > 0 and geo_scale[0] > 0.0 else 1000.0
length = geo_scale[1] if len(geo_scale) > 1 and geo_scale[1] > 0.0 else 1000.0
mesh = newton.Mesh.create_plane(
width,
length,
compute_normals=False,
compute_uvs=False,
compute_inertia=False,
)
return mesh
elif geo_type == newton.GeoType.SPHERE:
radius = geo_scale[0]
mesh = newton.Mesh.create_sphere(
radius,
compute_normals=False,
compute_uvs=False,
compute_inertia=False,
)
return mesh
elif geo_type == newton.GeoType.CAPSULE:
radius, half_height = geo_scale[:2]
mesh = newton.Mesh.create_capsule(
radius,
half_height,
up_axis=newton.Axis.Z,
compute_normals=False,
compute_uvs=False,
compute_inertia=False,
)
return mesh
elif geo_type == newton.GeoType.CYLINDER:
radius, half_height = geo_scale[:2]
mesh = newton.Mesh.create_cylinder(
radius,
half_height,
up_axis=newton.Axis.Z,
compute_normals=False,
compute_uvs=False,
compute_inertia=False,
)
return mesh
elif geo_type == newton.GeoType.CONE:
radius, half_height = geo_scale[:2]
mesh = newton.Mesh.create_cone(
radius,
half_height,
up_axis=newton.Axis.Z,
compute_normals=False,
compute_uvs=False,
compute_inertia=False,
)
return mesh
elif geo_type == newton.GeoType.BOX:
if len(geo_scale) == 1:
ext = (geo_scale[0],) * 3
else:
ext = tuple(geo_scale[:3])
mesh = newton.Mesh.create_box(
ext[0],
ext[1],
ext[2],
duplicate_vertices=False,
compute_normals=False,
compute_uvs=False,
compute_inertia=False,
)
return mesh
raise NotImplementedError(f"Shape type {geo_type} not supported")
@wp.kernel
def _apply_shape_transforms(
points: wp.array[wp.vec3], shape_ids: wp.array[int], shape_transforms: wp.array[wp.transform]
):
v = wp.tid()
p = points[v]
shape_id = shape_ids[v]
shape_transform = shape_transforms[shape_id]
p = wp.transform_point(shape_transform, p)
points[v] = p
def _get_body_collision_shapes(model: newton.Model, body_index: int):
"""Returns the ids of the shapes of a body with active collision flags."""
shape_flags = model.shape_flags.numpy()
body_shape_ids = np.array(model.body_shapes[body_index], dtype=int)
return body_shape_ids[(shape_flags[body_shape_ids] & newton.ShapeFlags.COLLIDE_PARTICLES) > 0]
def _get_shape_collision_materials(model: newton.Model, shape_ids: list[int]):
"""Returns the collision materials from the model for a list of shapes"""
thicknesses = model.shape_margin.numpy()[shape_ids]
friction = model.shape_material_mu.numpy()[shape_ids]
return thicknesses, friction
def _create_body_collider_mesh(
model: newton.Model,
shape_ids: list[int],
material_ids: list[int],
):
"""Create a collider mesh from a body."""
shape_scale = model.shape_scale.numpy()
shape_type = model.shape_type.numpy()
shape_meshes = [_get_shape_mesh(model, sid, newton.GeoType(shape_type[sid]), shape_scale[sid]) for sid in shape_ids]
collider_points, collider_indices, vertex_shape_ids, face_material_ids = _merge_meshes(
points=[mesh.vertices for mesh in shape_meshes],
indices=[mesh.indices for mesh in shape_meshes],
shape_ids=shape_ids,
material_ids=material_ids,
)
wp.launch(
_apply_shape_transforms,
dim=collider_points.shape[0],
inputs=[
collider_points,
vertex_shape_ids,
model.shape_transform,
],
)
return wp.Mesh(collider_points, collider_indices, wp.zeros_like(collider_points)), face_material_ids
@wp.struct
class MaterialParameters:
"""Convenience struct for passing material parameters to kernels."""
young_modulus: wp.array[float]
"""Young's modulus for the material."""
poisson_ratio: wp.array[float]
"""Poisson's ratio for the material."""
damping: wp.array[float]
"""Damping for the material."""
friction: wp.array[float]
"""Friction for the material."""
yield_pressure: wp.array[float]
"""Yield pressure for the material."""
tensile_yield_ratio: wp.array[float]
"""Tensile yield ratio for the material."""
yield_stress: wp.array[float]
"""Yield stress for the material."""
viscosity: wp.array[float]
"""Viscosity for the material."""
hardening: wp.array[float]
"""Hardening for the material."""
hardening_rate: wp.array[float]
"""Hardening rate for the material."""
softening_rate: wp.array[float]
"""Softening rate for the material."""
dilatancy: wp.array[float]
"""Dilatancy for the material."""
class ImplicitMPMModel:
"""Wrapper augmenting a ``newton.Model`` with implicit MPM data and setup.
Holds particle material parameters, collider parameters, and convenience
arrays derived from the wrapped ``model`` and ``SolverImplicitMPM.Config``.
Consumed by ``SolverImplicitMPM`` during time stepping.
Args:
model: The base Newton model to augment.
options: Options controlling particle and collider defaults.
"""
def __init__(self, model: newton.Model, options: "SolverImplicitMPM.Config"):
self.model = model
self._options = options
# Global options from SolverImplicitMPM.Config
self.voxel_size = float(options.voxel_size)
"""Size of the grid voxels"""
self.critical_fraction = float(options.critical_fraction)
"""Maximum fraction of the grid volume that can be occupied by particles"""
self.air_drag = float(options.air_drag)
"""Drag for the background air"""
self.collider = Collider()
"""Collider struct"""
self.material_parameters = MaterialParameters()
"""Material parameters struct"""
self.collider_body_mass = None
self.collider_body_inv_inertia = None
self.collider_body_q = None
self.setup_particle_material()
self.setup_collider()
def notify_particle_material_changed(self):
"""Refresh cached extrema for material parameters.
Tracks the minimum Young's modulus and maximum hardening across
particles to quickly toggle code paths (e.g., compliant particles or
hardening enabled) without recomputing per step.
"""
model = self.model
mpm_ns = getattr(model, "mpm", None)
if mpm_ns is not None and hasattr(mpm_ns, "young_modulus"):
self.min_young_modulus = float(np.min(mpm_ns.young_modulus.numpy()))
else:
self.min_young_modulus = _INFINITY
if mpm_ns is not None and hasattr(mpm_ns, "hardening"):
self.max_hardening = float(np.max(mpm_ns.hardening.numpy()))
else:
self.max_hardening = 0.0
if mpm_ns is not None and hasattr(mpm_ns, "viscosity"):
self.has_viscosity = bool(np.any(mpm_ns.viscosity.numpy() > 0))
else:
self.has_viscosity = False
if mpm_ns is not None and hasattr(mpm_ns, "dilatancy"):
self.has_dilatancy = bool(np.any(mpm_ns.dilatancy.numpy() > 0))
else:
self.has_dilatancy = False
def notify_collider_changed(self):
"""Refresh cached extrema for collider parameters.
Tracks the minimum collider mass to determine whether compliant
colliders are present and to enable/disable related computations.
"""
body_ids = self.collider.collider_body_index.numpy()
body_mass = self.collider_body_mass.numpy()
dynamic_body_ids = body_ids[body_ids >= 0]
dynamic_body_ids = dynamic_body_ids[body_mass[dynamic_body_ids] > 0.0]
dynamic_body_masses = body_mass[dynamic_body_ids]
self.min_collider_mass = np.min(dynamic_body_masses, initial=np.inf)
self.collider.query_max_dist = self.voxel_size * math.sqrt(3.0)
self.collider_body_count = int(np.max(body_ids + 1, initial=0))
def setup_particle_material(self):
"""Initialize derived per-particle fields from the model.
Computes particle volumes and densities from the model's particle mass and radius.
Also caches extrema used by the solver for fast feature toggles.
Per-particle material parameters are read directly from the ``model.mpm.*`` namespace
(registered via :meth:`SolverImplicitMPM.register_custom_attributes`).
"""
model = self.model
num_particles = model.particle_q.shape[0]
with wp.ScopedDevice(model.device):
# Assume that particles represent a cuboid volume of space, i.e, V = 8 r**3
# (particles are typically laid out in a grid, and represent an uniform material)
self.particle_radius = _particle_parameter(num_particles, model.particle_radius)
self.particle_volume = wp.array(8.0 * self.particle_radius.numpy() ** 3)
self.particle_density = model.particle_mass / self.particle_volume
self.material_parameters.young_modulus = model.mpm.young_modulus
self.material_parameters.poisson_ratio = model.mpm.poisson_ratio
self.material_parameters.damping = model.mpm.damping
self.material_parameters.friction = model.mpm.friction
self.material_parameters.yield_pressure = model.mpm.yield_pressure
self.material_parameters.tensile_yield_ratio = model.mpm.tensile_yield_ratio
self.material_parameters.yield_stress = model.mpm.yield_stress
self.material_parameters.hardening = model.mpm.hardening
self.material_parameters.hardening_rate = model.mpm.hardening_rate
self.material_parameters.softening_rate = model.mpm.softening_rate
self.material_parameters.dilatancy = model.mpm.dilatancy
self.material_parameters.viscosity = model.mpm.viscosity
self.notify_particle_material_changed()
def setup_collider(
self,
collider_meshes: list[wp.Mesh] | None = None,
collider_body_ids: list[int] | None = None,
collider_thicknesses: list[float] | None = None,
collider_friction: list[float] | None = None,
collider_adhesion: list[float] | None = None,
collider_projection_threshold: list[float] | None = None,
model: newton.Model | None = None,
body_com: wp.array | None = None,
body_mass: wp.array | None = None,
body_inv_inertia: wp.array | None = None,
body_q: wp.array | None = None,
):
"""Initialize collider parameters and defaults from inputs.
Populates the ``Collider`` struct with meshes, body mapping, and per-material
properties (thickness, friction, adhesion, projection threshold).
By default, this will setup collisions against all collision shapes in the model with flag `newton.ShapeFlag.COLLIDE_PARTICLES`.
Rigid body colliders will be treated as kinematic if their mass is zero; for all model bodies to be treated as kinematic,
pass ``body_mass=wp.zeros_like(model.body_mass)``.
For any collider index `i`, only one of ``collider_meshes[i]`` and ``collider_body_ids`` may not be `None`.
If material properties are not provided for a collider, but a body index is provided,
the material will be read from the body shape material attributes on the model.
Args:
collider_meshes: Warp triangular meshes used as colliders.
collider_body_ids: For dynamic colliders, per-mesh body ids.
collider_thicknesses: Per-mesh signed distance offsets (m).
collider_friction: Per-mesh Coulomb friction coefficients.
collider_adhesion: Per-mesh adhesion (Pa).
collider_projection_threshold: Per-mesh projection threshold, i.e. how far below the surface the
particle may be before it is projected out. (m)
model: The model to read collider properties from. Default to self.model.
body_com: For dynamic colliders, per-body center of mass. Default to model.body_com.
body_mass: For dynamic colliders, per-body mass. Default to model.body_mass.
body_inv_inertia: For dynamic colliders, per-body inverse inertia. Default to model.body_inv_inertia.
body_q: For dynamic colliders, per-body initial transform. Default to model.body_q.
"""
if model is None:
model = self.model
if collider_body_ids is None:
if collider_meshes is None:
collider_body_ids = [
body_id
for body_id in range(-1, model.body_count)
if len(_get_body_collision_shapes(model, body_id)) > 0
]
else:
collider_body_ids = [None] * len(collider_meshes)
if collider_meshes is None:
collider_meshes = [None] * len(collider_body_ids)
for collider_id, (mesh, body_id) in enumerate(zip(collider_meshes, collider_body_ids, strict=True)):
if mesh is None:
if body_id is None:
raise ValueError(
f"Either a mesh or a body_id must be provided for each collider; collider {collider_id} is missing both"
)
elif body_id is not None:
raise ValueError(
f"Either a mesh or a body_id must be provided for each collider; collider {collider_id} provides both"
)
collider_count = len(collider_body_ids)
if collider_thicknesses is None:
collider_thicknesses = [None] * collider_count
if collider_projection_threshold is None:
collider_projection_threshold = [None] * collider_count
if collider_friction is None:
collider_friction = [None] * collider_count
if collider_adhesion is None:
collider_adhesion = [None] * collider_count
assert len(collider_body_ids) == len(collider_thicknesses)
assert len(collider_body_ids) == len(collider_projection_threshold)
assert len(collider_body_ids) == len(collider_friction)
assert len(collider_body_ids) == len(collider_adhesion)
if body_com is None:
body_com = model.body_com
if body_mass is None:
body_mass = model.body_mass
if body_inv_inertia is None:
body_inv_inertia = model.body_inv_inertia
if body_q is None:
body_q = model.body_q
# count materials and shapes
material_count = 1 # default material
body_shapes = {}
collider_material_ids = []
for body_id in collider_body_ids:
if body_id is not None:
shapes = _get_body_collision_shapes(model, body_id)
if len(shapes) == 0:
raise ValueError(f"Body {body_id} has no collision shapes")
body_shapes[body_id] = shapes
collider_material_ids.append(list(range(material_count, material_count + len(shapes))))
material_count += len(shapes)
else:
collider_material_ids.append([material_count])
material_count += 1
# assign material values
material_thickness = [_DEFAULT_THICKNESS * self.voxel_size] * material_count
material_friction = [_DEFAULT_FRICTION] * material_count
material_adhesion = [_DEFAULT_ADHESION] * material_count
material_projection_threshold = [_DEFAULT_PROJECTION_THRESHOLD * self.voxel_size] * material_count
def assign_material(
material_id: int,
thickness: float | None = None,
friction: float | None = None,
adhesion: float | None = None,
projection_threshold: float | None = None,
):
if thickness is not None:
material_thickness[material_id] = thickness
if friction is not None:
material_friction[material_id] = friction
if adhesion is not None:
material_adhesion[material_id] = adhesion
if projection_threshold is not None:
material_projection_threshold[material_id] = projection_threshold
def assign_collider_material(material_id: int, collider_id: int):
assign_material(
material_id,
collider_thicknesses[collider_id],
collider_friction[collider_id],
collider_adhesion[collider_id],
collider_projection_threshold[collider_id],
)
for collider_id, body_id in enumerate(collider_body_ids):
if body_id is not None:
for material_id, shape_margin, shape_friction in zip(
collider_material_ids[collider_id],
*_get_shape_collision_materials(model, body_shapes[body_id]),
strict=True,
):
# use material from shapes as default
assign_material(material_id, thickness=shape_margin, friction=shape_friction)
# override with user-provided material
assign_collider_material(material_id, collider_id)
else:
# user-provided collider, single material
assign_collider_material(collider_material_ids[collider_id][0], collider_id)
collider_max_thickness = [
max((material_thickness[material_id] for material_id in collider_material_ids[collider_id]), default=0.0)
for collider_id in range(collider_count)
]
# Create device arrays
with wp.ScopedDevice(self.model.device):
# Create collider meshes from bodies if necessary
face_material_ids = [[]]
for collider_id in range(collider_count):
body_index = collider_body_ids[collider_id]
if body_index is None:
# Set body index to -1 to indicate a static collider
# This may not correspond to the model's body -1, but as far as the collision kernels
# are concerned, it does not matter.
collider_body_ids[collider_id] = -1
material_id = collider_material_ids[collider_id][0]
face_count = collider_meshes[collider_id].indices.shape[0] // 3
mesh_face_material_ids = np.full(face_count, material_id, dtype=int)
else:
collider_meshes[collider_id], mesh_face_material_ids = _create_body_collider_mesh(
model, body_shapes[body_index], collider_material_ids[collider_id]
)
face_material_ids.append(mesh_face_material_ids)
self.collider.collider_body_index = wp.array(collider_body_ids, dtype=int)
self.collider.collider_mesh = wp.array([collider.id for collider in collider_meshes], dtype=wp.uint64)
self.collider.collider_max_thickness = wp.array(collider_max_thickness, dtype=float)
self.collider.face_material_index = wp.array(np.concatenate(face_material_ids), dtype=int)
self.collider.material_thickness = wp.array(material_thickness, dtype=float)
self.collider.material_friction = wp.array(material_friction, dtype=float)
self.collider.material_adhesion = wp.array(material_adhesion, dtype=float)
self.collider.material_projection_threshold = wp.array(material_projection_threshold, dtype=float)
self.collider.body_com = body_com
self.collider_body_mass = body_mass
self.collider_body_inv_inertia = body_inv_inertia
self.collider_body_q = body_q
self._collider_meshes = collider_meshes # Keep a ref so that meshes are not garbage collected
self.notify_collider_changed()
@property
def has_compliant_particles(self):
return self.min_young_modulus < _INFINITY
@property
def has_hardening(self):
return self.max_hardening > 0.0
@property
def has_compliant_colliders(self):
return self.min_collider_mass < _INFINITY
================================================
FILE: newton/_src/solvers/implicit_mpm/implicit_mpm_solver_kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from typing import Any
import warp as wp
import warp.fem as fem
import warp.sparse as wps
from warp.types import type_size
import newton
from .implicit_mpm_model import MaterialParameters
from .rheology_solver_kernels import YieldParamVec, project_stress
wp.set_module_options({"enable_backward": False})
USE_HENCKY_STRAIN_MEASURE = wp.constant(True)
"""Use Hencky instead of co-rotated elastic model (replaces (S - I) with log S in Hooke's law)"""
MIN_PRINCIPAL_STRAIN = wp.constant(1.0e-6 if USE_HENCKY_STRAIN_MEASURE else 1.0e-2)
"""Minimum elastic strain for the elastic model (singular value of the elastic deformation gradient)"""
MAX_PRINCIPAL_STRAIN = wp.constant(1.0e6 if USE_HENCKY_STRAIN_MEASURE else 1.0e2)
"""Maximum elastic strain for the elastic model (singular value of the elastic deformation gradient)"""
MIN_HARDENING_JP = wp.constant(0.1)
"""Minimum plastic compression ratio for the hardening law (determinant of the plastic deformation gradient)"""
MIN_JP_DELTA = wp.constant(0.01)
"""Minimum delta for the plastic deformation gradient"""
MAX_JP_DELTA = wp.constant(10.0)
"""Maximum delta for the plastic deformation gradient"""
INFINITY = wp.constant(1.0e12)
"""Value above which quantities are considered infinite"""
EPSILON = wp.constant(1.0 / INFINITY)
"""Value below which quantities are considered zero"""
_QR_TOLERANCE = wp.constant(1.0e-12)
"""Convergence tolerance for the QR eigenvalue decomposition"""
_NAN_THRESHOLD = wp.constant(1.0e16)
"""Threshold above which eigenvalue results are considered NaN/divergent"""
_EIGENVALUE_FLOOR = wp.constant(1.0e-6)
"""Eigenvalues at or below this value are treated as zero (mode is dropped)"""
vec6 = wp.types.vector(length=6, dtype=wp.float32)
mat66 = wp.types.matrix(shape=(6, 6), dtype=wp.float32)
mat63 = wp.types.matrix(shape=(6, 3), dtype=wp.float32)
mat36 = wp.types.matrix(shape=(3, 6), dtype=wp.float32)
mat13 = wp.types.matrix(shape=(1, 3), dtype=wp.float32)
mat31 = wp.types.matrix(shape=(3, 1), dtype=wp.float32)
mat11 = wp.types.matrix(shape=(1, 1), dtype=wp.float32)
YIELD_PARAM_LENGTH = type_size(YieldParamVec)
@fem.integrand
def integrate_fraction(s: fem.Sample, phi: fem.Field, domain: fem.Domain, inv_cell_volume: float):
return phi(s) * inv_cell_volume
@fem.integrand
def integrate_collider_fraction(
s: fem.Sample,
domain: fem.Domain,
phi: fem.Field,
sdf: fem.Field,
inv_cell_volume: float,
):
return phi(s) * wp.where(sdf(s) <= 0.0, inv_cell_volume, 0.0)
@fem.integrand
def integrate_collider_fraction_apic(
s: fem.Sample,
domain: fem.Domain,
phi: fem.Field,
sdf: fem.Field,
sdf_gradient: fem.Field,
inv_cell_volume: float,
):
# APIC collider fraction prediction
node_count = fem.node_count(sdf, s)
pos = domain(s)
min_sdf = float(INFINITY)
for k in range(node_count):
s_node = fem.at_node(sdf, s, k)
sdf_value = sdf(s_node, k)
sdf_gradient_value = sdf_gradient(s_node, k)
node_offset = pos - domain(s_node)
min_sdf = wp.min(min_sdf, sdf_value + wp.dot(sdf_gradient_value, node_offset))
return phi(s) * wp.where(min_sdf <= 0.0, inv_cell_volume, 0.0)
@fem.integrand
def integrate_mass(
s: fem.Sample,
phi: fem.Field,
domain: fem.Domain,
inv_cell_volume: float,
particle_density: wp.array[float],
):
# Particles with density == 0 are kinematic boundary conditions: they contribute
# infinite mass so the grid velocity at their location is prescribed.
# This is distinct from ~ACTIVE particles (checked in advect/strain updates),
# which are completely ignored during transfers.
density = wp.where(particle_density[s.qp_index] > 0.0, particle_density[s.qp_index], INFINITY)
return phi(s) * density * inv_cell_volume
@fem.integrand
def integrate_velocity(
s: fem.Sample,
domain: fem.Domain,
u: fem.Field,
velocities: wp.array[wp.vec3],
dt: float,
gravity: wp.array[wp.vec3],
particle_world: wp.array[wp.int32],
inv_cell_volume: float,
particle_density: wp.array[float],
):
vel_adv = velocities[s.qp_index]
world_idx = particle_world[s.qp_index]
world_g = gravity[wp.max(world_idx, 0)]
rho = particle_density[s.qp_index]
vel_adv = wp.where(
rho > 0.0,
rho * (vel_adv + dt * world_g),
INFINITY * vel_adv,
)
return wp.dot(u(s), vel_adv) * inv_cell_volume
@fem.integrand
def integrate_velocity_apic(
s: fem.Sample,
domain: fem.Domain,
u: fem.Field,
velocity_gradients: wp.array[wp.mat33],
inv_cell_volume: float,
particle_density: wp.array[float],
):
# APIC velocity prediction
node_offset = domain(fem.at_node(u, s)) - domain(s)
vel_apic = velocity_gradients[s.qp_index] * node_offset
rho = particle_density[s.qp_index]
vel_adv = wp.where(rho > 0.0, rho, INFINITY) * vel_apic
return wp.dot(u(s), vel_adv) * inv_cell_volume
@wp.kernel
def free_velocity(
velocity_int: wp.array[wp.vec3],
node_particle_mass: wp.array[float],
drag: float,
inv_mass_matrix: wp.array[float],
velocity_avg: wp.array[wp.vec3],
):
i = wp.tid()
pmass = node_particle_mass[i]
inv_particle_mass = 1.0 / (pmass + drag)
vel = velocity_int[i] * inv_particle_mass
inv_mass_matrix[i] = inv_particle_mass
velocity_avg[i] = vel
@wp.func
def hardening_law(Jp: float, hardening: float):
if hardening == 0.0:
return 1.0
eps = wp.log(wp.clamp(Jp, MIN_HARDENING_JP, 1.0))
h = wp.sinh(-hardening * eps)
return h
@wp.func
def get_elastic_parameters(
i: int,
material_parameters: MaterialParameters,
):
# Hardening only affects yield parameters, not elastic stiffness.
# This separates the elastic response from the plastic history.
E = material_parameters.young_modulus[i]
nu = material_parameters.poisson_ratio[i]
d = material_parameters.damping[i]
return wp.vec3(E, nu, d)
@wp.func
def extract_elastic_parameters(
params_vec: wp.vec3,
):
compliance = 1.0 / params_vec[0]
poisson = params_vec[1]
damping = params_vec[2]
return compliance, poisson, damping
@wp.func
def get_yield_parameters(i: int, material_parameters: MaterialParameters, particle_Jp: float, dt: float):
h = hardening_law(particle_Jp, material_parameters.hardening[i])
mu = material_parameters.friction[i]
return YieldParamVec.from_values(
mu,
material_parameters.yield_pressure[i] * h,
material_parameters.tensile_yield_ratio[i],
material_parameters.yield_stress[i] * h,
material_parameters.dilatancy[i],
material_parameters.viscosity[i] / dt,
)
@fem.integrand
def integrate_elastic_parameters(
s: fem.Sample,
u: fem.Field,
inv_cell_volume: float,
material_parameters: MaterialParameters,
):
i = s.qp_index
params_vec = get_elastic_parameters(i, material_parameters)
return wp.dot(u(s), params_vec) * inv_cell_volume
@fem.integrand
def integrate_yield_parameters(
s: fem.Sample,
u: fem.Field,
inv_cell_volume: float,
material_parameters: MaterialParameters,
particle_Jp: wp.array[float],
dt: float,
):
i = s.qp_index
params_vec = get_yield_parameters(i, material_parameters, particle_Jp[i], dt)
return wp.dot(u(s), params_vec) * inv_cell_volume
@fem.integrand
def integrate_particle_stress(
s: fem.Sample,
tau: fem.Field,
inv_cell_volume: float,
particle_stress: wp.array[wp.mat33],
):
i = s.qp_index
return wp.ddot(tau(s), particle_stress[i]) * inv_cell_volume
@wp.kernel
def average_yield_parameters(
yield_parameters_int: wp.array[YieldParamVec],
particle_volume: wp.array[float],
yield_parameters_avg: wp.array[YieldParamVec],
):
i = wp.tid()
pvol = particle_volume[i]
yield_parameters_avg[i] = wp.max(YieldParamVec(0.0), yield_parameters_int[i] / wp.max(pvol, EPSILON))
@wp.kernel
def average_elastic_parameters(
elastic_parameters_int: wp.array[wp.vec3],
particle_volume: wp.array[float],
elastic_parameters_avg: wp.array[wp.vec3],
):
i = wp.tid()
pvol = particle_volume[i]
elastic_parameters_avg[i] = elastic_parameters_int[i] / wp.max(pvol, EPSILON)
@fem.integrand
def advect_particles(
s: fem.Sample,
domain: fem.Domain,
grid_vel: fem.Field,
dt: float,
max_vel: float,
particle_flags: wp.array[wp.int32],
particle_volume: wp.array[float],
pos: wp.array[wp.vec3],
vel: wp.array[wp.vec3],
vel_grad: wp.array[wp.mat33],
):
if ~particle_flags[s.qp_index] & newton.ParticleFlags.ACTIVE:
return
p_vel = grid_vel(s)
vel_n_sq = wp.length_sq(p_vel)
p_vel_cfl = wp.where(vel_n_sq > max_vel * max_vel, p_vel * max_vel / wp.sqrt(vel_n_sq), p_vel)
p_vel_grad = fem.grad(grid_vel, s)
delta_pos = dt * p_vel_cfl
gimp_weight = s.qp_weight * fem.measure(domain, s) / particle_volume[s.qp_index]
wp.atomic_add(pos, s.qp_index, gimp_weight * delta_pos)
wp.atomic_add(vel, s.qp_index, gimp_weight * p_vel_cfl)
wp.atomic_add(vel_grad, s.qp_index, gimp_weight * p_vel_grad)
@fem.integrand
def update_particle_strains(
s: fem.Sample,
domain: fem.Domain,
grid_vel: fem.Field,
plastic_strain_delta: fem.Field,
elastic_strain_delta: fem.Field,
stress: fem.Field,
dt: float,
particle_flags: wp.array[wp.int32],
particle_density: wp.array[float],
particle_volume: wp.array[float],
material_parameters: MaterialParameters,
elastic_strain_prev: wp.array[wp.mat33],
particle_Jp_prev: wp.array[float],
elastic_strain: wp.array[wp.mat33],
particle_Jp: wp.array[float],
particle_stress: wp.array[wp.mat33],
):
if ~particle_flags[s.qp_index] & newton.ParticleFlags.ACTIVE:
elastic_strain[s.qp_index] = elastic_strain_prev[s.qp_index]
particle_Jp[s.qp_index] = particle_Jp_prev[s.qp_index]
return
if particle_density[s.qp_index] == 0.0:
elastic_strain[s.qp_index] = elastic_strain_prev[s.qp_index]
particle_Jp[s.qp_index] = particle_Jp_prev[s.qp_index]
return
# plastic strain
p_strain_delta = plastic_strain_delta(s)
p_rate = wp.trace(p_strain_delta)
delta_Jp = wp.exp(
p_rate
* wp.where(
p_rate < 0.0, material_parameters.hardening_rate[s.qp_index], material_parameters.softening_rate[s.qp_index]
)
)
particle_Jp_new = particle_Jp_prev[s.qp_index] * wp.clamp(delta_Jp, MIN_JP_DELTA, MAX_JP_DELTA)
elastic_parameters_vec = get_elastic_parameters(s.qp_index, material_parameters)
compliance, _poisson, _damping = extract_elastic_parameters(elastic_parameters_vec)
yield_parameters_vec = get_yield_parameters(s.qp_index, material_parameters, particle_Jp_new, dt)
stress_0 = fem.SymmetricTensorMapper.value_to_dof_3d(stress(s))
particle_stress_new = fem.SymmetricTensorMapper.dof_to_value_3d(project_stress(stress_0, yield_parameters_vec))
# elastic strain
prev_strain = elastic_strain_prev[s.qp_index]
vel_grad = fem.grad(grid_vel, s)
skew = 0.5 * dt * (vel_grad - wp.transpose(vel_grad))
strain_delta = elastic_strain_delta(s) + skew
# The skew-symmetric part of the velocity gradient is used as a linearized
# approximation of the finite rotation increment (matches standard deformation gradient update).
elastic_strain_new = prev_strain + strain_delta @ prev_strain
elastic_strain_new = project_particle_strain(elastic_strain_new, prev_strain, compliance)
gimp_weight = s.qp_weight * fem.measure(domain, s) / particle_volume[s.qp_index]
wp.atomic_add(particle_Jp, s.qp_index, gimp_weight * particle_Jp_new)
wp.atomic_add(particle_stress, s.qp_index, gimp_weight * particle_stress_new)
wp.atomic_add(elastic_strain, s.qp_index, gimp_weight * elastic_strain_new)
@wp.func
def project_particle_strain(
F: wp.mat33,
F_prev: wp.mat33,
compliance: float,
):
if compliance <= EPSILON:
return wp.identity(n=3, dtype=float)
_U, xi, _V = wp.svd3(F)
if wp.min(xi) < MIN_PRINCIPAL_STRAIN or wp.max(xi) > MAX_PRINCIPAL_STRAIN:
return F_prev # non-recoverable, discard update
return F
@wp.kernel
def update_particle_frames(
dt: float,
min_stretch: float,
max_stretch: float,
vel_grad: wp.array[wp.mat33],
transform_prev: wp.array[wp.mat33],
transform: wp.array[wp.mat33],
):
i = wp.tid()
p_vel_grad = vel_grad[i]
# transform, for grain-level rendering
F_prev = transform_prev[i]
# dX1/dx = dX1/dX0 dX0/dx
F = F_prev + dt * p_vel_grad @ F_prev
# clamp eigenvalues of F
if min_stretch >= 0.0 and max_stretch >= 0.0:
U = wp.mat33()
S = wp.vec3()
V = wp.mat33()
wp.svd3(F, U, S, V)
S = wp.max(wp.min(S, wp.vec3(max_stretch)), wp.vec3(min_stretch))
F = U @ wp.diag(S) @ wp.transpose(V)
transform[i] = F
@fem.integrand
def strain_delta_form(
s: fem.Sample,
u: fem.Field,
tau: fem.Field,
dt: float,
domain: fem.Domain,
inv_cell_volume: float,
):
# The full strain matrix can be recovered from this divergence
# see _symmetric_part_op in rheology_solver_kernels.py
return fem.div(u, s) * tau(s) * (dt * inv_cell_volume)
@wp.kernel
def compute_unilateral_strain_offset(
max_fraction: float,
particle_volume: wp.array[float],
collider_volume: wp.array[float],
node_volume: wp.array[float],
unilateral_strain_offset: wp.array[float],
):
i = wp.tid()
spherical_part = max_fraction * (node_volume[i] - collider_volume[i]) - particle_volume[i]
spherical_part = wp.max(spherical_part, 0.0)
unilateral_strain_offset[i] = spherical_part
@wp.func
def stress_strain_relationship(sig: wp.mat33, compliance: float, poisson: float):
return (sig * (1.0 + poisson) - poisson * (wp.trace(sig) * wp.identity(n=3, dtype=float))) * compliance
@fem.integrand
def strain_rhs(
s: fem.Sample,
tau: fem.Field,
elastic_parameters: fem.Field,
elastic_strains: wp.array[wp.mat33],
inv_cell_volume: float,
dt: float,
):
_compliance, _poisson, damping = extract_elastic_parameters(elastic_parameters(s))
alpha = 1.0 / (1.0 + damping / dt)
F_prev = elastic_strains[s.qp_index]
U_prev, xi_prev, _V_prev = wp.svd3(F_prev)
if wp.static(USE_HENCKY_STRAIN_MEASURE):
RlogSRt_prev = (
U_prev @ wp.diag(wp.vec3(wp.log(xi_prev[0]), wp.log(xi_prev[1]), wp.log(xi_prev[2]))) @ wp.transpose(U_prev)
)
strain = alpha * wp.ddot(tau(s), RlogSRt_prev)
else:
RSinvRt_prev = U_prev @ wp.diag(1.0 / xi_prev) @ wp.transpose(U_prev)
Id = wp.identity(n=3, dtype=float)
strain = -alpha * wp.ddot(tau(s), RSinvRt_prev - Id)
return strain * inv_cell_volume
@fem.integrand
def compliance_form(
s: fem.Sample,
domain: fem.Domain,
tau: fem.Field,
sig: fem.Field,
elastic_parameters: fem.Field,
elastic_strains: wp.array[wp.mat33],
inv_cell_volume: float,
dt: float,
):
F = elastic_strains[s.qp_index]
compliance, poisson, damping = extract_elastic_parameters(elastic_parameters(s))
gamma = compliance / (1.0 + damping / dt)
U, xi, V = wp.svd3(F)
Rt = V @ wp.transpose(U)
if wp.static(USE_HENCKY_STRAIN_MEASURE):
R = wp.transpose(Rt)
return wp.ddot(Rt @ tau(s) @ R, stress_strain_relationship(Rt @ sig(s) @ R, gamma, poisson)) * inv_cell_volume
else:
FinvT = U @ wp.diag(1.0 / xi) @ wp.transpose(V)
return (
wp.ddot(Rt @ tau(s) @ FinvT, stress_strain_relationship(Rt @ sig(s) @ FinvT, gamma, poisson))
* inv_cell_volume
)
@fem.integrand
def collision_weight_field(
s: fem.Sample,
normal: fem.Field,
trial: fem.Field,
):
n = normal(s)
if wp.length_sq(n) == 0.0:
# invalid normal, contact is disabled
return 0.0
return trial(s)
@fem.integrand
def mass_form(
s: fem.Sample,
p: fem.Field,
q: fem.Field,
inv_cell_volume: float,
):
return p(s) * q(s) * inv_cell_volume
@wp.kernel(module="unique")
def compute_eigenvalues(
offsets: wp.array[int],
columns: wp.array[int],
values: wp.array[Any],
yield_parameters: wp.array[YieldParamVec],
eigenvalues: wp.array2d[float],
eigenvectors: wp.array3d[float],
):
row = wp.tid()
diag_index = wps.bsr_block_index(row, row, offsets, columns)
if diag_index == -1:
ev = values.dtype(0.0)
scales = type(ev[0])(0.0)
else:
diag_block = values[diag_index]
scales, ev = fem.linalg.symmetric_eigenvalues_qr(diag_block, _QR_TOLERANCE)
# symmetric_eigenvalues_qr may return nans for small coefficients
if not (wp.ddot(ev, ev) < _NAN_THRESHOLD and wp.length_sq(scales) < _NAN_THRESHOLD):
scales = wp.get_diag(diag_block)
ev = wp.identity(n=scales.length, dtype=float)
nodes_per_elt = eigenvectors.shape[1]
for k in range(scales.length):
if scales[k] <= _EIGENVALUE_FLOOR:
scales[k] = 1.0
ev_s = 0.0
else:
s = float(0.0)
ys = float(0.0)
for j in range(scales.length):
node_index = row * nodes_per_elt + j
s += ev[k, j] * eigenvalues[row, j]
ys += ev[k, j] * yield_parameters[node_index][0]
ev_s = wp.where(s < 0.0, -1.0, 1.0)
if ys * ev_s < 0.0:
ev_s = 0.0
for j in range(scales.length):
ev[k, j] *= ev_s
size = int(scales.length)
for k in range(size):
eigenvalues[row, k] = scales[k]
for j in range(scales.length):
eigenvectors[row, k, j] = ev[k, j]
@wp.kernel(module="unique")
def rotate_matrix_rows(
eigenvectors: wp.array3d[float],
mat_offsets: wp.array[int],
mat_columns: wp.array[int],
mat_values: wp.array[Any],
mat_values_out: wp.array[Any],
):
block = wp.tid()
nodes_per_elt = eigenvectors.shape[1]
node_count = eigenvectors.shape[0] * nodes_per_elt
row = wps.bsr_row_index(mat_offsets, node_count, block)
if row == -1:
return
col = mat_columns[block]
element = row // nodes_per_elt
elt_node = row - element * nodes_per_elt
ev = eigenvectors[element]
val = mat_values.dtype(0.0)
for k in range(nodes_per_elt):
row_k = element * nodes_per_elt + k
block_k = wps.bsr_block_index(row_k, col, mat_offsets, mat_columns)
if block_k != -1:
val += ev[elt_node, k] * mat_values[block_k]
mat_values_out[block] = val
def make_rotate_vectors(nodes_per_element: int):
@fem.cache.dynamic_kernel(suffix=nodes_per_element, kernel_options={"enable_mathdx_gemm": False})
def rotate_vectors(
eigenvectors: wp.array3d[float],
strain_rhs: wp.array2d[float],
stress: wp.array2d[float],
yield_parameters: wp.array2d[float],
unilateral_strain_offset: wp.array2d[float],
):
elem = wp.tid()
ev = wp.tile_load(eigenvectors[elem], shape=(nodes_per_element, nodes_per_element))
strain_rhs_tile = wp.tile_load(strain_rhs, shape=(nodes_per_element, 6), offset=(elem * nodes_per_element, 0))
rotated_strain_rhs = wp.tile_matmul(ev, strain_rhs_tile)
wp.tile_store(strain_rhs, rotated_strain_rhs, offset=(elem * nodes_per_element, 0))
stress_tile = wp.tile_load(stress, shape=(nodes_per_element, 6), offset=(elem * nodes_per_element, 0))
rotated_stress = wp.tile_matmul(ev, stress_tile)
wp.tile_store(stress, rotated_stress, offset=(elem * nodes_per_element, 0))
yield_tile = wp.tile_load(
yield_parameters, shape=(nodes_per_element, YIELD_PARAM_LENGTH), offset=(elem * nodes_per_element, 0)
)
rotated_yield = wp.tile_matmul(ev, yield_tile)
wp.tile_store(yield_parameters, rotated_yield, offset=(elem * nodes_per_element, 0))
unilateral_strain_offset_tile = wp.tile_load(
unilateral_strain_offset, shape=(nodes_per_element, 1), offset=(elem * nodes_per_element, 0)
)
rotated_unilateral_strain_offset = wp.tile_matmul(ev, unilateral_strain_offset_tile)
wp.tile_store(unilateral_strain_offset, rotated_unilateral_strain_offset, offset=(elem * nodes_per_element, 0))
return rotate_vectors
def make_inverse_rotate_vectors(nodes_per_element: int):
@fem.cache.dynamic_kernel(suffix=nodes_per_element)
def inverse_rotate_vectors(
eigenvectors: wp.array3d[float],
plastic_strain: wp.array2d[float],
elastic_strain: wp.array2d[float],
stress: wp.array2d[float],
):
elem = wp.tid()
ev_t = wp.tile_transpose(wp.tile_load(eigenvectors[elem], shape=(nodes_per_element, nodes_per_element)))
stress_tile = wp.tile_load(stress, shape=(nodes_per_element, 6), offset=(elem * nodes_per_element, 0))
rotated_stress = wp.tile_matmul(ev_t, stress_tile)
wp.tile_store(stress, rotated_stress, offset=(elem * nodes_per_element, 0))
plastic_strain_tile = wp.tile_load(
plastic_strain, shape=(nodes_per_element, 6), offset=(elem * nodes_per_element, 0)
)
rotated_plastic_strain = wp.tile_matmul(ev_t, plastic_strain_tile)
wp.tile_store(plastic_strain, rotated_plastic_strain, offset=(elem * nodes_per_element, 0))
elastic_strain_tile = wp.tile_load(
elastic_strain, shape=(nodes_per_element, 6), offset=(elem * nodes_per_element, 0)
)
rotated_elastic_strain = wp.tile_matmul(ev_t, elastic_strain_tile)
wp.tile_store(elastic_strain, rotated_elastic_strain, offset=(elem * nodes_per_element, 0))
return inverse_rotate_vectors
@wp.kernel(module="unique")
def inverse_scale_vector(
eigenvalues: wp.array[float],
vector: wp.array[Any],
):
node = wp.tid()
scale = eigenvalues[node]
zero = vector.dtype(0.0)
vector[node] = wp.where(scale == 0.0, zero, vector[node] / scale)
wp.overload(inverse_scale_vector, {"vector": wp.array[YieldParamVec]})
@wp.kernel
def inverse_scale_sym_tensor(
eigenvalues: wp.array[float],
vector: wp.array[vec6],
):
node = wp.tid()
# Symmetric tensor norm is orthonormal to sig:tau/2
scale = eigenvalues[node] * 2.0
vector[node] = wp.where(scale == 0.0, vec6(0.0), vector[node] / scale)
@wp.kernel(module="unique")
def rotate_matrix_columns(
eigenvectors: wp.array3d[float],
mat_offsets: wp.array[int],
mat_columns: wp.array[int],
mat_values: wp.array[Any],
mat_values_out: wp.array[Any],
):
block = wp.tid()
nodes_per_elt = eigenvectors.shape[1]
node_count = eigenvectors.shape[0] * nodes_per_elt
row = wps.bsr_row_index(mat_offsets, node_count, block)
if row == -1:
return
col = mat_columns[block]
nodes_per_elt = eigenvectors.shape[1]
element = col // nodes_per_elt
elt_node = col - element * nodes_per_elt
ev = eigenvectors[element]
val = mat_values.dtype(0.0)
for k in range(nodes_per_elt):
col_k = element * nodes_per_elt + k
block_k = wps.bsr_block_index(row, col_k, mat_offsets, mat_columns)
if block_k != -1:
val += ev[elt_node, k] * mat_values[block_k]
mat_values_out[block] = val
@wp.kernel
def compute_bounds(
pos: wp.array[wp.vec3],
particle_flags: wp.array[wp.int32],
lower_bounds: wp.array[wp.vec3],
upper_bounds: wp.array[wp.vec3],
):
block_id, lane = wp.tid()
i = block_id * wp.block_dim() + lane
# pad with +- inf for min/max
# tile_min scalar only, so separate components
# no tile_atomic_min yet, extract first and use lane 0
if i >= pos.shape[0]:
valid = False
elif ~particle_flags[i] & newton.ParticleFlags.ACTIVE:
valid = False
else:
valid = True
if valid:
p = pos[i]
min_x = p[0]
min_y = p[1]
min_z = p[2]
max_x = p[0]
max_y = p[1]
max_z = p[2]
else:
min_x = INFINITY
min_y = INFINITY
min_z = INFINITY
max_x = -INFINITY
max_y = -INFINITY
max_z = -INFINITY
tile_min_x = wp.tile_min(wp.tile(min_x))[0]
tile_max_x = wp.tile_max(wp.tile(max_x))[0]
tile_min_y = wp.tile_min(wp.tile(min_y))[0]
tile_max_y = wp.tile_max(wp.tile(max_y))[0]
tile_min_z = wp.tile_min(wp.tile(min_z))[0]
tile_max_z = wp.tile_max(wp.tile(max_z))[0]
tile_min = wp.vec3(tile_min_x, tile_min_y, tile_min_z)
tile_max = wp.vec3(tile_max_x, tile_max_y, tile_max_z)
if lane == 0:
wp.atomic_min(lower_bounds, 0, tile_min)
wp.atomic_max(upper_bounds, 0, tile_max)
@wp.kernel
def clamp_coordinates(
coords: wp.array[wp.vec3],
):
i = wp.tid()
coords[i] = wp.min(wp.max(coords[i], wp.vec3(0.0)), wp.vec3(1.0))
@wp.kernel
def pad_voxels(particle_q: wp.array[wp.vec3i], padded_q: wp.array4d[wp.vec3i]):
pid = wp.tid()
for i in range(3):
for j in range(3):
for k in range(3):
padded_q[pid, i, j, k] = particle_q[pid] + wp.vec3i(i - 1, j - 1, k - 1)
@wp.func
def positive_modn(x: int, n: int):
return (x % n + n) % n
def allocate_by_voxels(particle_q, voxel_size, padding_voxels: int = 0):
volume = wp.Volume.allocate_by_voxels(
voxel_points=particle_q.flatten(),
voxel_size=voxel_size,
)
for _pad_i in range(padding_voxels):
voxels = wp.empty((volume.get_voxel_count(),), dtype=wp.vec3i)
volume.get_voxels(voxels)
padded_voxels = wp.zeros((voxels.shape[0], 3, 3, 3), dtype=wp.vec3i)
wp.launch(pad_voxels, voxels.shape[0], (voxels, padded_voxels))
volume = wp.Volume.allocate_by_voxels(
voxel_points=padded_voxels.flatten(),
voxel_size=voxel_size,
)
return volume
@wp.kernel
def node_color(
space_node_indices: wp.array[int],
stencil_size: int,
voxels: wp.array[wp.vec3i],
res: wp.vec3i,
colors: wp.array[int],
color_indices: wp.array[int],
):
nid = wp.tid()
vid = space_node_indices[nid]
if vid == fem.NULL_NODE_INDEX:
colors[nid] = _NULL_COLOR
color_indices[nid] = nid
return
if voxels:
c = voxels[vid]
else:
c = fem.Grid3D.get_cell(res, vid)
colors[nid] = (
positive_modn(c[0], stencil_size) * stencil_size * stencil_size
+ positive_modn(c[1], stencil_size) * stencil_size
+ positive_modn(c[2], stencil_size)
)
color_indices[nid] = nid
def make_cell_color_kernel(geo_partition: fem.GeometryPartition):
@fem.cache.dynamic_kernel(geo_partition.name)
def cell_color(
partition_arg: geo_partition.CellArg,
stencil_size: int,
voxels: wp.array[wp.vec3i],
res: wp.vec3i,
colors: wp.array[int],
color_indices: wp.array[int],
):
pid = wp.tid()
cell = geo_partition.cell_index(partition_arg, pid)
if cell == -1:
colors[pid] = _NULL_COLOR
color_indices[pid] = pid
return
if voxels:
c = voxels[cell]
else:
c = fem.Grid3D.get_cell(res, cell)
colors[pid] = (
positive_modn(c[0], stencil_size) * stencil_size * stencil_size
+ positive_modn(c[1], stencil_size) * stencil_size
+ positive_modn(c[2], stencil_size)
)
color_indices[pid] = pid
return cell_color
@wp.kernel
def fill_uniform_color_block_indices(
nodes_per_element: int,
color_indices: wp.array2d[int],
):
i = wp.tid()
elem_idx = color_indices[0, i]
color_indices[0, i] = elem_idx * nodes_per_element
color_indices[1, i] = (elem_idx + 1) * nodes_per_element
def make_dynamic_color_block_indices_kernel(geo_partition: fem.GeometryPartition):
@fem.cache.dynamic_kernel(geo_partition.name)
def fill_dynamic_color_block_indices(
partition_arg: geo_partition.CellArg,
cell_node_offsets: wp.array[int],
color_indices: wp.array2d[int],
):
i = wp.tid()
elem_idx = color_indices[0, i]
cell = geo_partition.cell_index(partition_arg, elem_idx)
if cell == -1:
color_indices[0, i] = 0
color_indices[1, i] = 0
return
color_indices[0, i] = cell_node_offsets[cell]
color_indices[1, i] = cell_node_offsets[cell + 1]
return fill_dynamic_color_block_indices
_NULL_COLOR = (1 << 31) - 1 # color for null nodes. make sure it is sorted last
@wp.kernel
def compute_color_offsets(
max_color_count: int,
unique_count: wp.array[int],
unique_colors: wp.array[int],
color_counts: wp.array[int],
color_offsets: wp.array[int],
):
current_sum = int(0)
count = unique_count[0]
for k in range(count):
color_offsets[k] = current_sum
color = unique_colors[k]
local_count = wp.where(color == _NULL_COLOR, 0, color_counts[k])
current_sum += local_count
for k in range(count, max_color_count + 1):
color_offsets[k] = current_sum
@fem.integrand
def mark_active_cells(
s: fem.Sample,
domain: fem.Domain,
positions: wp.array[wp.vec3],
particle_flags: wp.array[int],
active_cells: wp.array[int],
):
if ~particle_flags[s.qp_index] & newton.ParticleFlags.ACTIVE:
return
x = positions[s.qp_index]
s_grid = fem.lookup(domain, x)
if s_grid.element_index != fem.NULL_ELEMENT_INDEX:
active_cells[s_grid.element_index] = 1
@wp.kernel(module="unique")
def scatter_field_dof_values(
space_node_indices: wp.array[int],
src: wp.array[Any],
dest: wp.array[Any],
):
nid = wp.tid()
sid = space_node_indices[nid]
if sid != fem.NULL_NODE_INDEX:
dest[sid] = src[nid]
wp.overload(scatter_field_dof_values, {"src": wp.array[wp.vec3], "dest": wp.array[wp.vec3]})
wp.overload(scatter_field_dof_values, {"src": wp.array[vec6], "dest": wp.array[vec6]})
================================================
FILE: newton/_src/solvers/implicit_mpm/rasterized_collisions.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
import warp.fem as fem
import warp.sparse as wps
import newton
from .contact_solver_kernels import solve_coulomb_isotropic
__all__ = [
"Collider",
"build_rigidity_operator",
"interpolate_collider_normals",
"project_outside_collider",
"rasterize_collider",
]
_COLLIDER_ACTIVATION_DISTANCE = wp.constant(0.5)
"""Distance below which to activate the collider"""
_INFINITY = wp.constant(1.0e12)
"""Threshold over which values are considered infinite"""
_CLOSEST_POINT_NORMAL_EPSILON = wp.constant(1.0e-3)
"""Epsilon for closest point normal calculation"""
_SDF_SIGN_FROM_AVERAGE_NORMAL = True
"""If true, determine the sign of the sdf from the average normal of the faces around the closest point.
Otherwise, use Warp's default sign determination strategy (raycasts).
"""
_SMALL_ANGLE_EPS = wp.constant(1.0e-4)
"""Small angle threshold to use more robust and faster path for angular velocity calculations"""
_NULL_COLLIDER_ID = -1
"""Indicator for no collider"""
@wp.struct
class Collider:
"""Packed collider parameters and geometry queried during rasterization."""
collider_mesh: wp.array[wp.uint64]
"""Mesh of the collider. Shape (collider_count,)."""
collider_max_thickness: wp.array[float]
"""Max thickness of each collider mesh. Shape (collider_count,)."""
collider_body_index: wp.array[int]
"""Body index of each collider. Shape (collider_count,)"""
face_material_index: wp.array[int]
"""Material index for each collider mesh face. Shape (sum(mesh.face_count for mesh in meshes),)"""
material_thickness: wp.array[float]
"""Thickness for each collider material. Shape (material_count,)"""
material_friction: wp.array[float]
"""Friction coefficient for each collider material. Shape (material_count,)"""
material_adhesion: wp.array[float]
"""Adhesion coefficient for each collider material (Pa). Shape (material_count,)"""
material_projection_threshold: wp.array[float]
"""Projection threshold for each collider material. Shape (material_count,)"""
body_com: wp.array[wp.vec3]
"""Body center of mass of each collider. Shape (body_count,)"""
query_max_dist: float
"""Maximum distance to query collider sdf"""
@wp.func
def get_average_face_normal(
mesh_id: wp.uint64,
point: wp.vec3,
):
"""Computes the average face normal at a point on a mesh.
(average of face normals within an epsilon-distance of the point)
Args:
mesh_id: The mesh to query.
point: The point to query.
Returns:
The average face normal at the point.
"""
face_normal = wp.vec3(0.0)
vidx = wp.mesh_get(mesh_id).indices
points = wp.mesh_get(mesh_id).points
eps_sq = _CLOSEST_POINT_NORMAL_EPSILON * _CLOSEST_POINT_NORMAL_EPSILON
epsilon = wp.vec3(_CLOSEST_POINT_NORMAL_EPSILON)
aabb_query = wp.mesh_query_aabb(mesh_id, point - epsilon, point + epsilon)
face_index = wp.int32(0)
while wp.mesh_query_aabb_next(aabb_query, face_index):
V0 = points[vidx[face_index * 3 + 0]]
V1 = points[vidx[face_index * 3 + 1]]
V2 = points[vidx[face_index * 3 + 2]]
sq_dist, _coords = fem.geometry.closest_point.project_on_tri_at_origin(point - V0, V1 - V0, V2 - V0)
if sq_dist < eps_sq:
face_normal += wp.mesh_eval_face_normal(mesh_id, face_index)
return wp.normalize(face_normal)
@wp.func
def collision_sdf(
x: wp.vec3,
collider: Collider,
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_q_prev: wp.array[wp.transform],
dt: float,
):
min_sdf = float(_INFINITY)
sdf_grad = wp.vec3(0.0)
sdf_vel = wp.vec3(0.0)
closest_point = wp.vec3(0.0)
collider_id = int(_NULL_COLLIDER_ID)
material_id = int(0) # default material, always valid
# Find closest collider
global_face_id = int(0)
for m in range(collider.collider_mesh.shape[0]):
mesh = collider.collider_mesh[m]
thickness = collider.collider_max_thickness[m]
body_id = collider.collider_body_index[m]
if body_id >= 0:
b_pos = wp.transform_get_translation(body_q[body_id])
b_rot = wp.transform_get_rotation(body_q[body_id])
x_local = wp.quat_rotate_inv(b_rot, x - b_pos)
else:
x_local = x
max_dist = collider.query_max_dist + thickness
if wp.static(_SDF_SIGN_FROM_AVERAGE_NORMAL):
query = wp.mesh_query_point_no_sign(mesh, x_local, max_dist)
else:
query = wp.mesh_query_point(mesh, x_local, max_dist)
if query.result:
cp = wp.mesh_eval_position(mesh, query.face, query.u, query.v)
if wp.static(_SDF_SIGN_FROM_AVERAGE_NORMAL):
face_normal = get_average_face_normal(mesh, cp)
sign = wp.where(wp.dot(face_normal, x_local - cp) > 0.0, 1.0, -1.0)
else:
face_normal = wp.mesh_eval_face_normal(mesh, query.face)
sign = query.sign
mesh_material_id = collider.face_material_index[global_face_id + query.face]
thickness = collider.material_thickness[mesh_material_id]
offset = x_local - cp
d = wp.length(offset) * sign
sdf = d - thickness
if sdf < min_sdf:
min_sdf = sdf
if wp.abs(d) < _CLOSEST_POINT_NORMAL_EPSILON:
sdf_grad = face_normal
else:
sdf_grad = wp.normalize(offset) * sign
sdf_vel = wp.mesh_eval_velocity(mesh, query.face, query.u, query.v)
closest_point = cp
collider_id = m
material_id = mesh_material_id
global_face_id += wp.mesh_get(mesh).indices.shape[0] // 3
# If closest collider has rigid motion, transform back to world frame
# Do that as a second step to avoid requiring more registers inside bvh query loop
if collider_id >= 0:
body_id = collider.collider_body_index[collider_id]
if body_id >= 0:
b_xform = body_q[body_id]
b_rot = wp.transform_get_rotation(b_xform)
sdf_vel = wp.quat_rotate(b_rot, sdf_vel)
sdf_grad = wp.normalize(wp.quat_rotate(b_rot, sdf_grad))
# Compute rigid body velocity at the contact point
if body_q_prev:
# backward-differenced velocity from position change
b_xform_prev = body_q_prev[body_id]
closest_point_world = wp.transform_point(b_xform, closest_point)
closest_point_world_prev = wp.transform_point(b_xform_prev, closest_point)
sdf_vel += (closest_point_world - closest_point_world_prev) / dt
if body_qd:
b_v = wp.spatial_top(body_qd[body_id])
b_w = wp.spatial_bottom(body_qd[body_id])
b_com = collider.body_com[body_id]
com_offset_cur = wp.quat_rotate(b_rot, closest_point - b_com)
ang_vel = wp.length(b_w)
angle_delta = ang_vel * dt
if angle_delta > _SMALL_ANGLE_EPS:
# forward-differenced velocity from current velocity
# (using exponential map)
b_rot_delta = wp.quat_from_axis_angle(b_w / ang_vel, angle_delta)
com_offset_next = wp.quat_rotate(b_rot_delta, com_offset_cur)
sdf_vel += b_v + (com_offset_next - com_offset_cur) / dt
else:
# Instantaneous rigid body velocity (v + omega x r)
sdf_vel += b_v + wp.cross(b_w, com_offset_cur)
return min_sdf, sdf_grad, sdf_vel, collider_id, material_id
@wp.kernel
def collider_volumes_kernel(
cell_volume: float,
collider_ids: wp.array[int],
node_volumes: wp.array[float],
volumes: wp.array[float],
):
i = wp.tid()
collider_id = collider_ids[i]
if collider_id >= 0:
wp.atomic_add(volumes, collider_id, node_volumes[i] * cell_volume)
@wp.func
def collider_is_dynamic(collider_id: int, collider: Collider, body_mass: wp.array[float]):
if collider_id < 0:
return False
body_id = collider.collider_body_index[collider_id]
if body_id < 0:
return False
return body_mass[body_id] > 0.0
@wp.kernel
def project_outside_collider(
positions: wp.array[wp.vec3],
velocities: wp.array[wp.vec3],
velocity_gradients: wp.array[wp.mat33],
particle_flags: wp.array[wp.int32],
particle_mass: wp.array[float],
collider: Collider,
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_q_prev: wp.array[wp.transform],
dt: float,
positions_out: wp.array[wp.vec3],
velocities_out: wp.array[wp.vec3],
velocity_gradients_out: wp.array[wp.mat33],
):
"""Project particles outside colliders and apply Coulomb response.
For active particles, queries the nearest collider surface, computes the
penetration at the end of the step, applies a Coulomb friction response
against the collider velocity, projects positions outside by the required
signed distance, and rigidifies the particle velocity gradient. Inactive
and kinematic (zero-mass) particles are passed through unchanged.
Args:
positions: Current particle positions.
velocities: Current particle velocities.
velocity_gradients: Current particle velocity gradients.
particle_flags: Per-particle flags; particles without :attr:`ACTIVE` are skipped.
particle_mass: Per-particle mass; zero-mass (kinematic) particles are skipped.
collider: Collider description and geometry.
body_q: Rigid body transforms.
body_qd: Rigid body velocities.
body_q_prev: Previous rigid body transforms (for finite-difference velocity).
dt: Timestep length.
positions_out: Output particle positions.
velocities_out: Output particle velocities.
velocity_gradients_out: Output particle velocity gradients.
"""
i = wp.tid()
pos_adv = positions[i]
p_vel = velocities[i]
vel_grad = velocity_gradients[i]
if (~particle_flags[i] & newton.ParticleFlags.ACTIVE) or particle_mass[i] == 0.0:
positions_out[i] = positions[i]
velocities_out[i] = p_vel
velocity_gradients_out[i] = vel_grad
return
# project outside of collider
sdf, sdf_gradient, sdf_vel, _collider_id, material_id = collision_sdf(
pos_adv, collider, body_q, body_qd, body_q_prev, dt
)
sdf_end = sdf - wp.dot(sdf_vel, sdf_gradient) * dt + collider.material_projection_threshold[material_id]
if sdf_end < 0:
# remove normal vel
friction = collider.material_friction[material_id]
delta_vel = solve_coulomb_isotropic(friction, sdf_gradient, p_vel - sdf_vel) + sdf_vel - p_vel
p_vel += delta_vel
pos_adv += delta_vel * dt
# project out
pos_adv -= wp.min(0.0, sdf_end + dt * wp.dot(delta_vel, sdf_gradient)) * sdf_gradient # delta_vel * dt
# make velocity gradient rigid
vel_grad = 0.5 * (vel_grad - wp.transpose(vel_grad))
positions_out[i] = pos_adv
velocities_out[i] = p_vel
velocity_gradients_out[i] = vel_grad
@wp.kernel
def rasterize_collider_kernel(
collider: Collider,
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_q_prev: wp.array[wp.transform],
voxel_size: float,
activation_distance: float,
dt: float,
node_positions: wp.array[wp.vec3],
node_volumes: wp.array[float],
collider_sdf: wp.array[float],
collider_velocity: wp.array[wp.vec3],
collider_normals: wp.array[wp.vec3],
collider_friction: wp.array[float],
collider_adhesion: wp.array[float],
collider_ids: wp.array[int],
):
"""Sample collider data at grid nodes.
Writes per-node signed distance, contact normal, collider velocity, and
material parameters (friction and adhesion). Nodes that are too far from
any collider are marked inactive with a null id and zeroed outputs. The
adhesion value is scaled by ``dt * voxel_size`` to match the nodal impulse
units used by the solver.
Args:
collider: Collider description and geometry.
body_q: Rigid body transforms.
body_qd: Rigid body velocities.
body_q_prev: Previous rigid body transforms (for finite-difference velocity).
voxel_size: Grid voxel size [m], used to scale the activation distance.
activation_distance: Distance (in voxels) below which to activate the collider.
dt: Timestep length (used to scale adhesion and finite-difference velocity).
node_positions: Grid node positions to sample at.
node_volumes: Per-node integration volumes.
collider_sdf: Output signed distance per node.
collider_velocity: Output collider velocity per node.
collider_normals: Output contact normals per node.
collider_friction: Output friction coefficient per node, or -1 if inactive.
collider_adhesion: Output scaled adhesion per node.
collider_ids: Output collider id per node, or null id if inactive.
"""
i = wp.tid()
x = node_positions[i]
if x[0] == fem.OUTSIDE:
bc_active = False
sdf = _INFINITY
else:
sdf, sdf_gradient, sdf_vel, collider_id, material_id = collision_sdf(
x, collider, body_q, body_qd, body_q_prev, dt
)
bc_active = sdf < activation_distance * voxel_size
collider_sdf[i] = sdf
if not bc_active:
collider_velocity[i] = wp.vec3(0.0)
collider_normals[i] = wp.vec3(0.0)
collider_friction[i] = -1.0
collider_adhesion[i] = 0.0
collider_ids[i] = _NULL_COLLIDER_ID
return
collider_ids[i] = collider_id
collider_normals[i] = sdf_gradient
collider_friction[i] = collider.material_friction[material_id]
collider_adhesion[i] = collider.material_adhesion[material_id] * dt * node_volumes[i] / voxel_size
collider_velocity[i] = sdf_vel
@wp.kernel
def fill_collider_rigidity_matrices(
node_positions: wp.array[wp.vec3],
collider: Collider,
body_q: wp.array[wp.transform],
body_mass: wp.array[float],
body_inv_inertia: wp.array[wp.mat33],
cell_volume: float,
collider_ids: wp.array[int],
J_rows: wp.array[int],
J_cols: wp.array[int],
J_values: wp.array[wp.mat33],
IJtm_values: wp.array[wp.mat33],
):
i = wp.tid()
collider_id = collider_ids[i]
if collider_is_dynamic(collider_id, collider, body_mass):
body_id = collider.collider_body_index[collider_id]
J_rows[2 * i] = i
J_rows[2 * i + 1] = i
J_cols[2 * i] = 2 * body_id
J_cols[2 * i + 1] = 2 * body_id + 1
b_pos = wp.transform_get_translation(body_q[body_id])
b_rot = wp.transform_get_rotation(body_q[body_id])
R = wp.quat_to_matrix(b_rot)
x = node_positions[i]
W = wp.skew(b_pos + R * collider.body_com[body_id] - x)
Id = wp.identity(n=3, dtype=float)
J_values[2 * i] = W
J_values[2 * i + 1] = Id
# Grid impulses need to be scaled by cell_volume
world_inv_inertia = R @ body_inv_inertia[body_id] @ wp.transpose(R)
IJtm_values[2 * i] = -cell_volume * world_inv_inertia @ W
IJtm_values[2 * i + 1] = (cell_volume / body_mass[body_id]) * Id
else:
J_cols[2 * i] = -1
J_cols[2 * i + 1] = -1
J_rows[2 * i] = -1
J_rows[2 * i + 1] = -1
@fem.integrand
def world_position(
s: fem.Sample,
domain: fem.Domain,
):
return domain(s)
@fem.integrand
def collider_gradient_field(s: fem.Sample, domain: fem.Domain, distance: fem.Field, normal: fem.Field):
min_sdf = float(_INFINITY)
min_pos = wp.vec3(0.0)
min_grad = wp.vec3(0.0)
# min sdf over all nodes in the element
elem_count = fem.node_count(distance, s)
for k in range(elem_count):
s_node = fem.at_node(distance, s, k)
sdf = distance(s_node, k)
if sdf < min_sdf:
min_sdf = sdf
min_pos = domain(s_node)
min_grad = normal(s_node, k)
if min_sdf == _INFINITY:
return wp.vec3(0.0)
# compute gradient, filtering invalid values
sdf_gradient = wp.vec3(0.0)
for k in range(elem_count):
s_node = fem.at_node(distance, s, k)
sdf = distance(s_node, k)
pos = domain(s_node)
# if the sdf value is not acceptable (larger than min_sdf + distance between nodes),
# replace with linearized approximation
if sdf >= min_sdf + wp.length(pos - min_pos):
sdf = wp.min(sdf, min_sdf + wp.dot(min_grad, pos - min_pos))
sdf_gradient += sdf * fem.node_inner_weight_gradient(distance, s, k)
return sdf_gradient
@wp.kernel
def normalize_gradient(
gradient: wp.array[wp.vec3],
normal: wp.array[wp.vec3],
):
i = wp.tid()
normal[i] = wp.normalize(gradient[i])
def rasterize_collider(
collider: Collider,
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_q_prev: wp.array[wp.transform],
voxel_size: float,
dt: float,
collider_space_restriction: fem.SpaceRestriction,
collider_node_volume: wp.array[float],
collider_position_field: fem.DiscreteField,
collider_distance_field: fem.DiscreteField,
collider_normal_field: fem.DiscreteField,
collider_velocity: wp.array[wp.vec3],
collider_friction: wp.array[float],
collider_adhesion: wp.array[float],
collider_ids: wp.array[int],
temporary_store: fem.TemporaryStore,
):
"""Rasterize collider signed-distance, normals, velocity, and material onto grid nodes.
For each collision node, queries the nearest collider surface and writes the
signed distance, outward normal, collider velocity, friction, adhesion, and
collider id to the corresponding output arrays.
Args:
collider: Packed collider parameters and geometry.
body_q: Rigid body transforms.
body_qd: Rigid body velocities (spatial vectors).
body_q_prev: Previous rigid body transforms (for finite-difference velocity).
voxel_size: Grid voxel edge length [m].
dt: Timestep length [s].
collider_space_restriction: Space restriction for collision nodes.
collider_node_volume: Output per-node volume fractions.
collider_position_field: Output world-space node positions.
collider_distance_field: Output signed-distance values per node.
collider_normal_field: Output outward normals per node.
collider_velocity: Output collider velocity per node [m/s].
collider_friction: Output Coulomb friction coefficient per node.
collider_adhesion: Output adhesion per node [Pa].
collider_ids: Output collider index per node, or ``_NULL_COLLIDER_ID``.
temporary_store: Temporary storage for intermediate buffers.
"""
collision_node_count = collider_position_field.dof_values.shape[0]
collider_position_field.dof_values.fill_(wp.vec3(fem.OUTSIDE))
fem.interpolate(
world_position,
dest=collider_position_field,
at=collider_space_restriction,
reduction="first",
temporary_store=temporary_store,
)
activation_distance = (
0.0 if collider_position_field.degree == 0 else _COLLIDER_ACTIVATION_DISTANCE / collider_position_field.degree
)
wp.launch(
rasterize_collider_kernel,
dim=collision_node_count,
inputs=[
collider,
body_q,
body_qd,
body_q_prev,
voxel_size,
activation_distance,
dt,
collider_position_field.dof_values,
collider_node_volume,
collider_distance_field.dof_values,
collider_velocity,
collider_normal_field.dof_values,
collider_friction,
collider_adhesion,
collider_ids,
],
)
def interpolate_collider_normals(
collider_space_restriction: fem.SpaceRestriction,
collider_distance_field: fem.DiscreteField,
collider_normal_field: fem.DiscreteField,
temporary_store: fem.TemporaryStore,
):
"""Smooth collider normals by computing the gradient of the distance field.
Interpolates the gradient of ``collider_distance_field`` at each collision
node and normalizes the result to produce smoothed outward normals, which
are written back into ``collider_normal_field``.
Args:
collider_space_restriction: Space restriction for collision nodes.
collider_distance_field: Per-node signed-distance field.
collider_normal_field: Per-node normal field (updated in place).
temporary_store: Temporary storage for intermediate buffers.
"""
corrected_normal = wp.empty_like(collider_normal_field.dof_values)
fem.interpolate(
collider_gradient_field,
dest=corrected_normal,
dest_space=collider_normal_field.space,
at=collider_space_restriction,
fields={"distance": collider_distance_field, "normal": collider_normal_field},
reduction="mean",
temporary_store=temporary_store,
)
wp.launch(
normalize_gradient,
dim=collider_normal_field.dof_values.shape,
inputs=[corrected_normal, collider_normal_field.dof_values],
)
def build_rigidity_operator(
cell_volume: float,
node_volumes: wp.array[float],
node_positions: wp.array[wp.vec3],
collider: Collider,
body_q: wp.array[wp.transform],
body_mass: wp.array[float],
body_inv_inertia: wp.array[wp.mat33],
collider_ids: wp.array[int],
) -> tuple[wps.BsrMatrix, wps.BsrMatrix]:
"""Build the collider rigidity operator that couples collider impulses to rigid DOFs.
Builds a block-sparse matrix of size (3 N_vel_nodes) x (3 N_vel_nodes) that
maps nodal impulses to collider rigid-body displacements. Only nodes
with a valid collider id and only dynamic colliders (finite mass) produce
non-zero blocks.
Internally constructs:
- J: kinematic Jacobian blocks per node relating rigid velocity to nodal velocity.
- IJtm: mass- and inertia-scaled transpose mapping.
The returned operator is (J @ IJtm) and corresponds the rigid-body Delassus operator.
Args:
cell_volume: Grid cell volume as scaling factor to node_volumes.
node_volumes: Per-velocity-node volume fractions.
node_positions: World-space node positions (3D).
collider: Packed collider parameters and geometry handles.
body_q: Rigid body transforms.
body_mass: Rigid body masses.
body_inv_inertia: Rigid body inverse inertia tensors.
collider_ids: Per-velocity-node collider id, or `_NULL_COLLIDER_ID` when not active.
Returns:
A tuple of ``warp.sparse.BsrMatrix`` (J, IJtm) representing the rigidity coupling operator ``J @ IJtm``
"""
vel_node_count = node_volumes.shape[0]
body_count = body_q.shape[0]
J_rows = wp.empty(vel_node_count * 2, dtype=int)
J_cols = wp.empty(vel_node_count * 2, dtype=int)
J_values = wp.empty(vel_node_count * 2, dtype=wp.mat33)
IJtm_values = wp.empty(vel_node_count * 2, dtype=wp.mat33)
wp.launch(
fill_collider_rigidity_matrices,
dim=vel_node_count,
inputs=[
node_positions,
collider,
body_q,
body_mass,
body_inv_inertia,
cell_volume,
collider_ids,
J_rows,
J_cols,
J_values,
IJtm_values,
],
)
J = wps.bsr_from_triplets(
rows_of_blocks=vel_node_count,
cols_of_blocks=2 * body_count,
rows=J_rows,
columns=J_cols,
values=J_values,
)
IJtm = wps.bsr_from_triplets(
cols_of_blocks=vel_node_count,
rows_of_blocks=2 * body_count,
columns=J_rows,
rows=J_cols,
values=IJtm_values,
)
return J, IJtm
================================================
FILE: newton/_src/solvers/implicit_mpm/render_grains.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
import warp.fem as fem
import newton
__all__ = ["sample_render_grains", "update_render_grains"]
@wp.kernel
def sample_grains(
particles: wp.array[wp.vec3],
radius: wp.array[float],
positions: wp.array2d[wp.vec3],
):
pid, k = wp.tid()
rng = wp.rand_init(pid * positions.shape[1] + k)
pos_loc = 2.0 * wp.vec3(wp.randf(rng) - 0.5, wp.randf(rng) - 0.5, wp.randf(rng) - 0.5) * radius[pid]
positions[pid, k] = particles[pid] + pos_loc
@wp.kernel
def transform_grains(
particle_pos_prev: wp.array[wp.vec3],
particle_transform_prev: wp.array[wp.mat33],
particle_pos: wp.array[wp.vec3],
particle_transform: wp.array[wp.mat33],
positions: wp.array2d[wp.vec3],
):
pid, k = wp.tid()
pos_adv = positions[pid, k]
p_pos = particle_pos[pid]
p_frame = particle_transform[pid]
p_pos_prev = particle_pos_prev[pid]
p_frame_prev = particle_transform_prev[pid]
pos_loc = wp.inverse(p_frame_prev) @ (pos_adv - p_pos_prev)
p_pos_adv = p_frame @ pos_loc + p_pos
positions[pid, k] = p_pos_adv
@fem.integrand
def advect_grains(
s: fem.Sample,
domain: fem.Domain,
grid_vel: fem.Field,
dt: float,
positions: wp.array[wp.vec3],
):
x = domain(s)
vel = grid_vel(s)
pos_adv = x + dt * vel
positions[s.qp_index] = pos_adv
@wp.kernel
def advect_grains_from_particles(
dt: float,
particle_pos_prev: wp.array[wp.vec3],
particle_pos: wp.array[wp.vec3],
particle_vel_grad: wp.array[wp.mat33],
positions: wp.array2d[wp.vec3],
):
pid, k = wp.tid()
p_pos = particle_pos[pid]
p_pos_prev = particle_pos_prev[pid]
pos_loc = positions[pid, k] - p_pos_prev
p_vel_grad = particle_vel_grad[pid]
displ = dt * p_vel_grad * pos_loc + (p_pos - p_pos_prev)
positions[pid, k] += displ
@wp.kernel
def project_grains(
radius: wp.array[float],
particle_pos: wp.array[wp.vec3],
particle_frames: wp.array[wp.mat33],
positions: wp.array2d[wp.vec3],
):
pid, k = wp.tid()
pos_adv = positions[pid, k]
p_pos = particle_pos[pid]
p_frame = particle_frames[pid]
p_frame = (radius[pid] * radius[pid]) * p_frame * wp.transpose(p_frame)
pos_loc = pos_adv - p_pos
vn = wp.max(1.0, wp.dot(pos_loc, wp.inverse(p_frame) * pos_loc))
p_pos_adv = pos_loc / wp.sqrt(vn) + p_pos
positions[pid, k] = p_pos_adv
def sample_render_grains(state: newton.State, particle_radius: wp.array, grains_per_particle: int):
"""Generate per-particle point samples used for high-resolution rendering.
For each simulation particle, this creates ``grains_per_particle`` random
points uniformly within a cube of size ``2 * particle_radius`` centered at
the particle position. The resulting 2D array can be updated each time step
using ``update_render_grains`` to passively advect and project grains within
the affinely deformed particle shape.
Args:
state: Current Newton state providing particle positions.
particle_radius: Rendering grain sampling radius per particle.
grains_per_particle: Number of grains to sample per particle.
Returns:
A ``wp.array`` with shape ``(num_particles, grains_per_particle)`` of
type ``wp.vec3`` containing grain positions.
"""
grains = wp.empty((state.particle_count, grains_per_particle), dtype=wp.vec3, device=state.particle_q.device)
wp.launch(
sample_grains,
dim=grains.shape,
inputs=[
state.particle_q,
particle_radius,
grains,
],
device=state.particle_q.device,
)
return grains
def update_render_grains(
state_prev: newton.State,
state: newton.State,
grains: wp.array,
particle_radius: wp.array,
dt: float,
):
"""Advect grain samples with the grid velocity and keep them inside the deformed particle.
The grains are advanced by composing two motions within the time step:
1) a particle-local affine update using the particle velocity gradient and
particle positions (APIC-like), and 2) a grid-based PIC advection using
the current velocity field. After advection, positions are projected
back using an ellipsoidal approximation of the particle defined by its
deformation frame and ``particle_radius``.
If no velocity field is available in the ``state`` the function
returns without modification.
Args:
state_prev: Previous state (t_n) with particle positions and frames.
state: Current state (t_{n+1}) providing velocity field and particles.
grains: 2D array of grain positions per particle to be updated in place.
particle_radius: Per-particle radius used for projection.
dt: Time step duration.
"""
if state.velocity_field is None:
return
grain_pos = grains.flatten()
domain = fem.Cells(state.velocity_field.space.geometry)
grain_pic = fem.PicQuadrature(domain, positions=grain_pos)
wp.launch(
advect_grains_from_particles,
dim=grains.shape,
inputs=[
dt,
state_prev.particle_q,
state.particle_q,
state.mpm.particle_qd_grad,
grains,
],
device=grains.device,
)
fem.interpolate(
advect_grains,
quadrature=grain_pic,
values={
"dt": dt,
"positions": grain_pos,
},
fields={
"grid_vel": state.velocity_field,
},
device=grains.device,
)
wp.launch(
project_grains,
dim=grains.shape,
inputs=[
particle_radius,
state.particle_q,
state.mpm.particle_transform,
grains,
],
device=grains.device,
)
================================================
FILE: newton/_src/solvers/implicit_mpm/rheology_solver_kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from typing import Any
import warp as wp
import warp.fem as fem
import warp.sparse as sp
from warp.fem.linalg import symmetric_eigenvalues_qr
_DELASSUS_PROXIMAL_REG = wp.constant(1.0e-6)
"""Cutoff for the trace of the diagonal block of the Delassus operator to disable constraints"""
_SLIDING_NEWTON_TOL = wp.constant(1.0e-7)
"""Tolerance for the Newton method to solve for the sliding velocity"""
_INCLUDE_LEFTOVER_STRAIN = wp.constant(False)
"""Whether to include leftover strain (due to not fully-converged implicit solve) in the elastic strain.
More accurate, but less stable for stiff materials. Development toggle for experimentation."""
_USE_CAM_CLAY = wp.constant(False)
"""Use Modified Cam-Clay flow rule instead of the piecewise-linear anisotropic one. Development toggle for experimentation."""
_ISOTROPIC_LOCAL_LHS = wp.constant(False)
"""Use isotropic local left-hand side instead of anisotropic. Cheaper local solver but slower convergence. Development toggle for experimentation."""
vec6 = wp.types.vector(length=6, dtype=wp.float32)
mat66 = wp.types.matrix(shape=(6, 6), dtype=wp.float32)
mat55 = wp.types.matrix(shape=(5, 5), dtype=wp.float32)
mat13 = wp.vec3
wp.set_module_options({"enable_backward": False})
class YieldParamVec(wp.types.vector(length=6, dtype=wp.float32)):
"""Compact yield surface definition in an interpolation-friendly format.
Layout::
[0] p_max * sqrt(3/2) -- scaled compressive yield pressure
[1] p_min * sqrt(3/2) -- scaled tensile yield pressure
[2] s_max -- deviatoric yield stress
[3] mu * p_max -- frictional shear limit
[4] dilatancy -- dilatancy factor
[5] viscosity -- viscosity
The scaling by sqrt(3/2) is related to the orthogonal mapping from spherical/deviatoric
tensors to vectors in R^6.
"""
@wp.func
def from_values(
friction_coeff: float,
yield_pressure: float,
tensile_yield_ratio: float,
yield_stress: float,
dilatancy: float,
viscosity: float,
):
pressure_scale = wp.sqrt(3.0 / 2.0)
return YieldParamVec(
yield_pressure * pressure_scale,
tensile_yield_ratio * yield_pressure * pressure_scale,
yield_stress,
friction_coeff * yield_pressure,
dilatancy,
viscosity,
)
@wp.func
def get_dilatancy(yield_params: YieldParamVec):
return wp.clamp(yield_params[4], 0.0, 1.0)
@wp.func
def get_viscosity(yield_params: YieldParamVec):
return wp.max(0.0, yield_params[5])
@wp.func
def normal_yield_bounds(yield_params: YieldParamVec):
"""Extract bounds for the normal stress from the yield surface definition."""
return -wp.max(0.0, yield_params[1]), yield_params[0]
@wp.func
def shear_yield_stress(yield_params: YieldParamVec, r_N: float):
"""Maximum deviatoric stress for a given value of the normal stress."""
p_min, p_max = normal_yield_bounds(yield_params)
mu = wp.where(p_max > 0.0, wp.max(0.0, yield_params[3] / p_max), 0.0)
s = wp.max(yield_params[2], 0.0)
r_N = wp.clamp(r_N, p_min, p_max)
p1 = p_min + 0.5 * p_max
p2 = 0.5 * p_max
if r_N < p1:
return s + mu * (r_N - p_min), mu, p_min, p1
elif r_N > p2:
return s + mu * (p_max - r_N), -mu, p2, p_max
else:
return s + mu * p2, 0.0, p1, p2
@wp.func
def shear_yield_stress_camclay(yield_params: YieldParamVec, r_N: float):
r_N_min, r_N_max = normal_yield_bounds(yield_params)
mu = wp.where(r_N_max > 0.0, wp.max(0.0, yield_params[3] / r_N_max), 0.0)
r_N = wp.clamp(r_N, r_N_min, r_N_max)
beta_sq = mu * mu / (1.0 - 2.0 * (r_N_min / r_N_max))
y_sq = beta_sq * (r_N - r_N_min) * (r_N_max - r_N)
return wp.sqrt(y_sq), 0.0, r_N_min, r_N_max
@wp.func
def _symmetric_part_op(b: wp.vec3, u: wp.vec3):
return fem.SymmetricTensorMapper.value_to_dof_3d(wp.outer(u, b * 2.0))
@wp.func
def _symmetric_part_transposed_op(b: wp.vec3, sig: vec6):
return fem.SymmetricTensorMapper.dof_to_value_3d(sig) @ (b * 0.5)
@wp.kernel
def compute_delassus_diagonal(
split_mass: wp.bool,
strain_mat_offsets: wp.array[int],
strain_mat_columns: wp.array[int],
strain_mat_values: wp.array[mat13],
inv_volume: wp.array[float],
compliance_mat_offsets: wp.array[int],
compliance_mat_columns: wp.array[int],
compliance_mat_values: wp.array[mat66],
transposed_strain_mat_offsets: wp.array[int],
delassus_rotation: wp.array[mat55],
delassus_diagonal: wp.array[vec6],
):
"""Computes the diagonal blocks of the Delassus operator and performs
an eigendecomposition to decouple stress components.
For each constraint (tau_i) this kernel:
1. Assembles the diagonal block of the Delassus operator by summing
contributions from connected velocity nodes.
2. If mass splitting is enabled, scales contributions by the number
of constraints each velocity node participates in.
3. Zeros the shear-divergence coupling so the normal and deviatoric
components are decoupled.
4. Performs an eigendecomposition (``symmetric_eigenvalues_qr``) of
the deviatoric sub-block, falling back to the diagonal when the
decomposition is numerically unreliable.
5. Stores the eigenvalues (``delassus_diagonal``) and the transpose
of the deviatoric eigenvectors (``delassus_rotation``).
"""
tau_i = wp.tid()
block_beg = strain_mat_offsets[tau_i]
block_end = strain_mat_offsets[tau_i + 1]
compliance_diag_index = sp.bsr_block_index(tau_i, tau_i, compliance_mat_offsets, compliance_mat_columns)
if compliance_diag_index == -1:
diag_block = mat66(0.0)
else:
diag_block = compliance_mat_values[compliance_diag_index]
mass_ratio = float(1.0)
for b in range(block_beg, block_end):
u_i = strain_mat_columns[b]
if split_mass:
mass_ratio = float(transposed_strain_mat_offsets[u_i + 1] - transposed_strain_mat_offsets[u_i])
b_val = strain_mat_values[b]
inv_frac = inv_volume[u_i] * mass_ratio
b_v0 = _symmetric_part_op(b_val, wp.vec3(1.0, 0.0, 0.0))
diag_block += inv_frac * wp.outer(b_v0, b_v0)
b_v1 = _symmetric_part_op(b_val, wp.vec3(0.0, 1.0, 0.0))
diag_block += inv_frac * wp.outer(b_v1, b_v1)
b_v2 = _symmetric_part_op(b_val, wp.vec3(0.0, 0.0, 1.0))
diag_block += inv_frac * wp.outer(b_v2, b_v2)
diag_block += _DELASSUS_PROXIMAL_REG * wp.identity(n=6, dtype=float)
# Remove shear-divergence coupling
# (current implementation of solve_coulomb_aniso normal and tangential responses are independent)
# Ensures that only the tangential part is rotated
for k in range(1, 6):
diag_block[0, k] = 0.0
diag_block[k, 0] = 0.0
diag, ev = symmetric_eigenvalues_qr(diag_block, _DELASSUS_PROXIMAL_REG * 0.1)
# symmetric_eigenvalues_qr may return nans for small coefficients
if not (wp.ddot(ev, ev) < 1.0e16 and wp.length_sq(diag) < 1.0e16):
diag = wp.get_diag(diag_block)
ev = wp.identity(n=6, dtype=float)
if wp.static(_ISOTROPIC_LOCAL_LHS):
diag = vec6(wp.max(diag))
ev = wp.identity(n=6, dtype=float)
delassus_diagonal[tau_i] = diag
delassus_rotation[tau_i] = wp.transpose(ev[1:6, 1:6])
@wp.func
def unilateral_offset_to_strain_rhs(offset: float):
return fem.SymmetricTensorMapper.value_to_dof_3d(offset * (2.0 / 3.0) * wp.identity(n=3, dtype=float))
@wp.kernel
def preprocess_stress_and_strain(
unilateral_strain_offset: wp.array[float],
strain_rhs: wp.array[vec6],
stress: wp.array[vec6],
yield_stress: wp.array[YieldParamVec],
):
"""Prepare stress and strain for the rheology solve.
Adds the unilateral strain offset to ``strain_rhs`` (removed in
:func:`postprocess_stress_and_strain`), disables cohesion for nodes
with a positive offset, and projects the initial stress guess onto
the yield surface.
"""
tau_i = wp.tid()
yield_params = yield_stress[tau_i]
offset = unilateral_strain_offset[tau_i]
if offset > 0.0:
# add unilateral strain offset to strain rhs
# will be removed in postprocess_stress_and_strain
b = strain_rhs[tau_i]
b += unilateral_offset_to_strain_rhs(offset)
strain_rhs[tau_i] = b
yield_params[1] = 0.0 # disable cohesion if offset > 0 (not compact)
yield_stress[tau_i] = yield_params
sig = stress[tau_i]
stress[tau_i] = project_stress(sig, yield_params)
@wp.kernel
def postprocess_stress_and_strain(
compliance_mat_offsets: wp.array[int],
compliance_mat_columns: wp.array[int],
compliance_mat_values: wp.array[mat66],
strain_mat_offsets: wp.array[int],
strain_mat_columns: wp.array[int],
strain_mat_values: wp.array[mat13],
delassus_diagonal: wp.array[vec6],
delassus_rotation: wp.array[mat55],
unilateral_strain_offset: wp.array[float],
yield_params: wp.array[YieldParamVec],
strain_node_volume: wp.array[float],
strain_rhs: wp.array[vec6],
stress: wp.array[vec6],
velocity: wp.array[wp.vec3],
elastic_strain: wp.array[vec6],
plastic_strain: wp.array[vec6],
):
"""Computes elastic and plastic strain deltas after the solver iterations.
Uses the generic (non-specialized) flow-rule solver to ensure correct
results regardless of which compile-time flags were active during the
iterative solve.
"""
tau_i = wp.tid()
minus_elastic_strain = strain_rhs[tau_i]
minus_elastic_strain -= unilateral_offset_to_strain_rhs(unilateral_strain_offset[tau_i])
comp_block_beg = compliance_mat_offsets[tau_i]
comp_block_end = compliance_mat_offsets[tau_i + 1]
for b in range(comp_block_beg, comp_block_end):
sig_i = compliance_mat_columns[b]
minus_elastic_strain += compliance_mat_values[b] * stress[sig_i]
world_plastic_strain = minus_elastic_strain
block_beg = strain_mat_offsets[tau_i]
block_end = strain_mat_offsets[tau_i + 1]
for b in range(block_beg, block_end):
u_i = strain_mat_columns[b]
world_plastic_strain += _symmetric_part_op(strain_mat_values[b], velocity[u_i])
rot = delassus_rotation[tau_i]
diag = delassus_diagonal[tau_i]
loc_plastic_strain = _world_to_local(world_plastic_strain, rot)
loc_stress = _world_to_local(stress[tau_i], rot)
yp = yield_params[tau_i]
loc_plastic_strain_new = wp.static(make_solve_flow_rule())(
diag, loc_plastic_strain - wp.cw_mul(loc_stress, diag), loc_stress, yp, strain_node_volume[tau_i]
)
world_plastic_strain_new = _local_to_world(loc_plastic_strain_new, rot)
if _INCLUDE_LEFTOVER_STRAIN:
minus_elastic_strain -= world_plastic_strain - world_plastic_strain_new
elastic_strain[tau_i] = -minus_elastic_strain
plastic_strain[tau_i] = world_plastic_strain_new
@wp.func
def eval_sliding_residual(alpha: float, D: Any, b_T: Any, gamma: float, mu_rn: float):
"""Evaluates the sliding residual and its derivative w.r.t. ``alpha``.
The residual is ``f = |r(alpha)| * (1 - gamma * alpha) - mu_rn``
where ``r(alpha) = b_T / (D + alpha I)``.
"""
d_alpha = D + type(D)(alpha)
r_alpha = wp.cw_div(b_T, d_alpha)
r_alpha_norm = wp.length(r_alpha)
dr_dalpha = -wp.cw_div(r_alpha, d_alpha * r_alpha_norm)
g = 1.0 - gamma * alpha
f = r_alpha_norm * g - mu_rn
df_dalpha = wp.dot(r_alpha, dr_dalpha) * g - r_alpha_norm * gamma
return f, df_dalpha
@wp.func
def solve_sliding_no_dilatancy(
D: Any,
b: Any,
yield_stress: float,
):
"""Simplified sliding solver when dilatancy=0 (theta=0).
When there is no dilatancy coupling, gamma=0 and the residual
simplifies to ``|b_T / (D + alpha I)| - yield_stress``.
"""
b_T = b
b_T[0] = 0.0
if yield_stress <= 0.0:
return b_T
Dys = D * yield_stress
alpha_min = wp.max(0.0, wp.length(b_T) - wp.max(Dys))
alpha_max = wp.length(b_T) - wp.min(Dys)
alpha_cur = alpha_min
for _k in range(24):
d_alpha = Dys + type(D)(alpha_cur)
r_alpha = wp.cw_div(b_T, d_alpha)
r_alpha_norm = wp.length(r_alpha)
f = r_alpha_norm - 1.0
df = wp.dot(r_alpha, -wp.cw_div(r_alpha, d_alpha * r_alpha_norm))
delta = wp.min(-f / df, alpha_max - alpha_cur)
if delta < _SLIDING_NEWTON_TOL * alpha_max:
break
alpha_cur += delta
u = wp.cw_div(b_T * alpha_cur, Dys + type(D)(alpha_cur))
u[0] = 0.0
return u
@wp.func
def solve_sliding_aniso(
D: Any,
b: Any,
yield_stress: float,
yield_stress_deriv: float,
theta: float,
):
"""Solves the anisotropic sliding sub-problem with dilatancy coupling.
Finds the velocity ``u`` such that the tangential stress satisfies
the yield condition, accounting for the normal-tangential coupling
through ``yield_stress_deriv`` and the dilatancy parameter ``theta``.
Returns:
Full velocity vector ``u`` (tangential *and* normal components).
The normal component ``u[0]`` is set to
``theta * yield_stress_deriv * |u_T|``.
"""
# yield_stress = f_yield( r_N0 )
# r_N0 = ( u_N0 - b_N )/ D[0]
# |r_T| = yield_stress + yield_stress_deriv * (r_N - r_N0)
# |r_T| = yield_stress + yield_stress_deriv * (u_N - u_N0) / D[0]
# |r_T| = yield_stress_0 + yield_stress_deriv^2 * theta * |u_T| / D[0]
# |r_T| = yield_stress_0 + yield_stress_deriv^2 * theta / D[0] * alpha * |r_T|
# (1.0 - yield_stress_deriv^2 * theta / D[0] * alpha) |r_T| = yield_stress
yield_stress -= yield_stress_deriv * b[0] / D[0]
b_T = b
b_T[0] = 0.0
alpha_0 = wp.length(b_T)
gamma = theta * yield_stress_deriv * yield_stress_deriv / D[0]
ref_stress = yield_stress + gamma * alpha_0
if ref_stress <= 0.0:
return b_T
# (1.0 - gamma * alpha) |r_T| = yield_stress
# (1.0 - gamma * alpha) |(D + alpha I)^{-1} b_t| = yield_stress
# (1.0 - gamma * alpha) |(D ys + alpha ys I)^{-1} b_t| = 1
# change of var: alpha -> alpha /yield_stress
# (1.0 - gamma * alpha) |(D ys + alpha I)^{-1} b_t| = yield_stress/ref_stress
Dmu_rn = D * ref_stress
gamma = gamma / ref_stress
target = yield_stress / ref_stress
# Viscous shear opposite to tangential stress, zero divergence
# find alpha, r_t, mu_rn, (D + alpha/(mu r_n) I) r_t + b_t = 0, |r_t| = mu r_n
# find alpha, |(D mu r_n + alpha I)^{-1} b_t|^2 = 1.0
# |b_T| = tg * (Dz + alpha) / (1 - gamma * alpha)
# |b_T| (1 - gamma alpha) = tg * (Dz + alpha)
# |b_T| = (Dz tg + alpha (tg + gamma |b_T|)
# |b_T| = (Dz tg + alpha) as tg + gamma |b_T| = 1 for def of ref_stress
alpha_Dmin = alpha_0 - wp.max(Dmu_rn) * target
alpha_Dmax = alpha_0 - wp.min(Dmu_rn) * target
alpha_root = 1.0 / gamma
if target > 0.0:
alpha_min = wp.max(0.0, alpha_Dmin)
alpha_max = wp.min(alpha_Dmax, alpha_root)
elif target < 0.0:
alpha_min = wp.max(alpha_Dmax, alpha_root)
alpha_max = alpha_Dmin
else:
alpha_max = alpha_root
alpha_min = alpha_root
# We're looking for the root of an hyperbola, approach using Newton from the left
alpha_cur = alpha_min
for _k in range(24):
f_cur, df_dalpha = eval_sliding_residual(alpha_cur, Dmu_rn, b_T, gamma, target)
delta_alpha = wp.min(-f_cur / df_dalpha, alpha_max - alpha_cur)
if delta_alpha < _SLIDING_NEWTON_TOL * alpha_max:
break
alpha_cur += delta_alpha
u = wp.cw_div(b_T * alpha_cur, Dmu_rn + type(D)(alpha_cur))
u[0] = theta * yield_stress_deriv * wp.length(u)
return u
@wp.func
def solve_flow_rule_camclay(
D: vec6,
b: vec6,
r: vec6,
yield_params: YieldParamVec,
):
use_nacc = get_dilatancy(yield_params) == 0.0
if use_nacc:
r_0 = -wp.cw_div(b, D)
else:
u = wp.cw_mul(r, D) + b
r_0 = r - u / wp.max(D)
r_N0 = r_0[0]
r_T = r_0
r_T[0] = 0.0
ys, _dys, r_N_min, r_N_max = shear_yield_stress_camclay(yield_params, r_N0)
if r_N_max <= 0.0:
return b
if wp.length_sq(r_T) < ys * ys:
return vec6(0.0)
if use_nacc:
# Non-Associated Cam Clay
b_T = b
b_T[0] = 0.0
u = solve_sliding_no_dilatancy(D, b_T, ys)
r_N = wp.clamp(r_N0, r_N_min, r_N_max)
u[0] = D[0] * r_N + b[0]
return u
# Associated yield surface: project on 2d ellipse
mu = wp.where(r_N_max > 0.0, wp.max(0.0, yield_params[3] / r_N_max), 0.0)
beta_sq = mu * mu / (1.0 - 2.0 * (r_N_min / r_N_max))
# z = y^2 = beta_sq (r_N_max - r_N) (r_N - r_N_min) = - beta_sq (r_N - r_N_mid)^2 + c^2
# with c2 = beta_sq * (r_N_mid^2 - r_N_min * r_N_max)
r_mid = 0.5 * (r_N_min + r_N_max)
beta = wp.sqrt(beta_sq)
c_sq = beta_sq * (r_mid * r_mid - r_N_min * r_N_max)
c = wp.sqrt(c_sq)
# x = r_N - r_mid
# y^2 + beta_sq x^2 = c^2
y = wp.length(r_T)
x = r_N0 - r_mid
# Add a dummy normal component so we can reuse the sliding solver
W = wp.vec3(1.0, beta, 1.0)
W_sq = wp.vec3(1.0, beta_sq, 1.0)
W_sq_inv = wp.vec3(1.0, 1.0 / beta_sq, 1.0)
X0 = wp.vec3(0.0, x, y)
WinvX0 = wp.cw_div(X0, W)
# |Y| = c = |W X|
# W_inv Y + alpha W Y = X0
# W^-2 Y - W_inv X0 = - alpha Y = Z
Z = solve_sliding_no_dilatancy(W_sq_inv, -WinvX0, c)
Y = wp.cw_mul(W_sq, Z + WinvX0)
X = wp.cw_div(Y, W)
r_N = r_mid + X[1]
murn = wp.abs(X[2])
r = wp.normalize(r_T) * murn
r[0] = r_N
u = wp.cw_mul(r, D) + b
return u
def make_solve_flow_rule(has_viscosity: bool = True, has_dilatancy: bool = True):
key = (has_viscosity, has_dilatancy)
@fem.cache.dynamic_func(suffix=key)
def solve_flow_rule_aniso_impl(
D: vec6,
b: vec6,
r_guess: vec6,
yield_params: YieldParamVec,
strain_node_volume: float,
):
if wp.static(has_dilatancy):
dilatancy = get_dilatancy(yield_params)
else:
dilatancy = 0.0
if wp.static(has_viscosity):
D_visc = vec6(1.0) + get_viscosity(yield_params) / strain_node_volume * D
D = wp.cw_div(D, D_visc)
b = wp.cw_div(b, D_visc)
if wp.static(_USE_CAM_CLAY):
return solve_flow_rule_camclay(D, b, r_guess, yield_params)
r_0 = -wp.cw_div(b, D)
r_N0 = r_0[0]
ys, dys, pmin, pmax = shear_yield_stress(yield_params, r_N0)
u_N0 = D[0] * (wp.clamp(r_N0, pmin, pmax) - r_N0)
# u_T = 0 ok
r_T = r_0
r_T[0] = 0.0
r_T_n = wp.length(r_T)
if r_T_n <= ys:
u = vec6(0.0)
u[0] = u_N0
return u
# sliding
u = b
u[0] = u_N0
if wp.static(has_dilatancy):
u = solve_sliding_aniso(D, u, ys, dys, dilatancy)
else:
u = solve_sliding_no_dilatancy(D, u, ys)
# check for change of linear region
r_N_new = (u[0] - b[0]) / D[0]
r_N_clamp = wp.clamp(r_N_new, pmin, pmax)
if r_N_clamp == r_N_new:
return u
# moved from conic part to constant part. clamp and resolve tangent part
ys, dys, pmin, pmax = shear_yield_stress(yield_params, r_N_clamp)
if wp.static(has_dilatancy):
u = solve_sliding_aniso(D, b, ys, 0.0, dilatancy)
else:
u = solve_sliding_no_dilatancy(D, b, ys)
u[0] = D[0] * (r_N_clamp - r_N0)
return u
return solve_flow_rule_aniso_impl
@wp.func
def project_stress(
r: vec6,
yield_params: YieldParamVec,
):
"""Projects a stress vector onto the yield surface (non-orthogonally)."""
r_N = r[0]
r_T = r
r_T[0] = 0.0
if wp.static(_USE_CAM_CLAY):
ys, _dys, pmin, pmax = shear_yield_stress_camclay(yield_params, r_N)
else:
ys, _dys, pmin, pmax = shear_yield_stress(yield_params, r_N)
r_T_n2 = wp.length_sq(r_T)
if r_T_n2 > ys * ys:
r_T *= ys / wp.sqrt(r_T_n2)
r = r_T
r[0] = wp.clamp(r_N, pmin, pmax)
return r
@wp.func
def _world_to_local(
world_vec: vec6,
rotation: mat55,
):
local_vec = vec6(world_vec[0])
local_vec[1:6] = world_vec[1:6] @ rotation
return local_vec
@wp.func
def _local_to_world(
local_vec: vec6,
rotation: mat55,
):
world_vec = vec6(local_vec[0])
world_vec[1:6] = rotation @ local_vec[1:6]
return world_vec
def make_apply_stress_delta(strain_velocity_node_count: int = -1):
@fem.cache.dynamic_func(suffix=strain_velocity_node_count)
def apply_stress_delta_impl(
tau_i: int,
delta_stress: vec6,
strain_mat_offsets: wp.array[int],
strain_mat_columns: wp.array[int],
strain_mat_values: wp.array[mat13],
inv_mass_matrix: wp.array[float],
velocities: wp.array[wp.vec3],
):
"""Updates particle velocities from a local stress delta."""
block_beg = strain_mat_offsets[tau_i]
if wp.static(strain_velocity_node_count > 0):
for bk in range(strain_velocity_node_count):
b = block_beg + bk
u_i = strain_mat_columns[b]
delta_u = inv_mass_matrix[u_i] * _symmetric_part_transposed_op(strain_mat_values[b], delta_stress)
velocities[u_i] += delta_u
else:
block_end = strain_mat_offsets[tau_i + 1]
for b in range(block_beg, block_end):
u_i = strain_mat_columns[b]
delta_u = inv_mass_matrix[u_i] * _symmetric_part_transposed_op(strain_mat_values[b], delta_stress)
velocities[u_i] += delta_u
return apply_stress_delta_impl
@wp.kernel
def apply_stress_delta_jacobi(
transposed_strain_mat_offsets: wp.array[int],
transposed_strain_mat_columns: wp.array[int],
transposed_strain_mat_values: wp.array[mat13],
inv_mass_matrix: wp.array[float],
stress: wp.array[vec6],
velocities: wp.array[wp.vec3],
):
"""Updates particle velocities from a local stress delta."""
u_i = wp.tid()
inv_mass = inv_mass_matrix[u_i]
block_beg = transposed_strain_mat_offsets[u_i]
block_end = transposed_strain_mat_offsets[u_i + 1]
delta_u = wp.vec3(0.0)
for b in range(block_beg, block_end):
tau_i = transposed_strain_mat_columns[b]
delta_stress = stress[tau_i]
delta_u += _symmetric_part_transposed_op(transposed_strain_mat_values[b], delta_stress)
velocities[u_i] += inv_mass * delta_u
@wp.kernel
def apply_velocity_delta(
alpha: float,
beta: float,
strain_mat_offsets: wp.array[int],
strain_mat_columns: wp.array[int],
strain_mat_values: wp.array[mat13],
velocity_delta: wp.array[wp.vec3],
strain_prev: wp.array[vec6],
strain: wp.array[vec6],
):
"""Computes strain from a velocity delta: ``strain = alpha * B @ velocity_delta + beta * strain_prev``."""
tau_i = wp.tid()
block_beg = strain_mat_offsets[tau_i]
block_end = strain_mat_offsets[tau_i + 1]
delta_stress = vec6(0.0)
for b in range(block_beg, block_end):
u_i = strain_mat_columns[b]
delta_stress += _symmetric_part_op(strain_mat_values[b], velocity_delta[u_i])
delta_stress *= alpha
if beta != 0.0:
delta_stress += beta * strain_prev[tau_i]
strain[tau_i] = delta_stress
@wp.kernel
def apply_stress_gs(
color: int,
launch_dim: int,
color_offsets: wp.array[int],
color_blocks: wp.array2d[int],
strain_mat_offsets: wp.array[int],
strain_mat_columns: wp.array[int],
strain_mat_values: wp.array[mat13],
inv_mass_matrix: wp.array[float], # Note: Likely inv_volume in context
stress: wp.array[vec6],
velocities: wp.array[wp.vec3],
):
"""
Update particle velocities from the current stress. Uses a coloring approach to
avoid avoid race conditions. Used for Gauss-Seidel solver where the transposed
strain matrix is not assembled
"""
i = wp.tid()
color_beg = color_offsets[color] + i
color_end = color_offsets[color + 1]
for color_offset in range(color_beg, color_end, launch_dim):
beg, end = color_blocks[0, color_offset], color_blocks[1, color_offset]
for tau_i in range(beg, end):
cur_stress = stress[tau_i]
wp.static(make_apply_stress_delta())(
tau_i,
cur_stress,
strain_mat_offsets,
strain_mat_columns,
strain_mat_values,
inv_mass_matrix,
velocities,
)
def make_compute_local_strain(has_compliance_mat: bool = True, strain_velocity_node_count: int = -1):
@fem.cache.dynamic_func(suffix=(has_compliance_mat, strain_velocity_node_count))
def compute_local_strain_impl(
tau_i: int,
compliance_mat_offsets: wp.array[int],
compliance_mat_columns: wp.array[int],
compliance_mat_values: wp.array[mat66],
strain_mat_offsets: wp.array[int],
strain_mat_columns: wp.array[int],
strain_mat_values: wp.array[mat13],
local_strain_rhs: wp.array[vec6],
velocities: wp.array[wp.vec3],
local_stress: wp.array[vec6],
):
"""Computes the local strain based on the current stress and velocities."""
tau = local_strain_rhs[tau_i]
# tau += B v
block_beg = strain_mat_offsets[tau_i]
if wp.static(strain_velocity_node_count > 0):
for bk in range(strain_velocity_node_count):
b = block_beg + bk
u_i = strain_mat_columns[b]
tau += _symmetric_part_op(strain_mat_values[b], velocities[u_i])
else:
block_end = strain_mat_offsets[tau_i + 1]
for b in range(block_beg, block_end):
u_i = strain_mat_columns[b]
tau += _symmetric_part_op(strain_mat_values[b], velocities[u_i])
# tau += C sigma
if wp.static(has_compliance_mat):
comp_block_beg = compliance_mat_offsets[tau_i]
comp_block_end = compliance_mat_offsets[tau_i + 1]
for b in range(comp_block_beg, comp_block_end):
sig_i = compliance_mat_columns[b]
tau += compliance_mat_values[b] @ local_stress[sig_i]
return tau
return compute_local_strain_impl
def make_solve_local_stress(has_viscosity: bool, has_dilatancy: bool, has_rotation: bool = not _ISOTROPIC_LOCAL_LHS):
"""Return a specialized Warp func that applies the local stress projection for one strain node.
Optionally rotates strain and stress into the Delassus eigenbasis before solving
and back to world space on return. Each unique ``(has_viscosity, has_dilatancy,
has_rotation)`` combination is compiled once and cached.
"""
key = (has_viscosity, has_dilatancy, has_rotation)
@fem.cache.dynamic_func(suffix=key)
def solve_local_stress_impl(
tau_i: int,
strain_rhs: vec6,
yield_params: wp.array[YieldParamVec],
strain_node_volume: wp.array[float],
delassus_diagonal: wp.array[vec6],
delassus_rotation: wp.array[mat55],
cur_stress: wp.array[vec6],
):
D = delassus_diagonal[tau_i]
if wp.static(has_rotation):
rot = delassus_rotation[tau_i]
local_strain = _world_to_local(strain_rhs, rot)
local_stress = _world_to_local(cur_stress[tau_i], rot)
else:
local_strain = strain_rhs
local_stress = cur_stress[tau_i]
tau_new = wp.static(make_solve_flow_rule(has_viscosity, has_dilatancy))(
D,
local_strain - wp.cw_mul(local_stress, D),
local_stress,
yield_params[tau_i],
strain_node_volume[tau_i],
)
delta_stress_loc = wp.cw_div(tau_new - local_strain, D)
if wp.static(has_rotation):
return _local_to_world(delta_stress_loc, rot)
else:
return delta_stress_loc
return solve_local_stress_impl
def make_jacobi_solve_kernel(
has_viscosity: bool,
has_dilatancy: bool,
has_compliance_mat: bool,
strain_velocity_node_count: int = -1,
has_rotation: bool = not _ISOTROPIC_LOCAL_LHS,
):
"""Return a Jacobi-style per-node stress solve kernel specialized for the given feature flags.
Each unique combination of flags produces a separate Warp kernel compiled and cached
via :func:`fem.cache.dynamic_kernel`.
"""
key = (has_viscosity, has_dilatancy, has_compliance_mat, has_rotation, strain_velocity_node_count)
@fem.cache.dynamic_kernel(suffix=key, kernel_options={"fast_math": True})
def jacobi_solve_kernel_impl(
yield_params: wp.array[YieldParamVec],
strain_node_volume: wp.array[float],
compliance_mat_offsets: wp.array[int],
compliance_mat_columns: wp.array[int],
local_compliance_mat_values: wp.array[mat66],
strain_mat_offsets: wp.array[int],
strain_mat_columns: wp.array[int],
strain_mat_values: wp.array[mat13],
delassus_diagonal: wp.array[vec6],
delassus_rotation: wp.array[mat55],
local_strain_rhs: wp.array[vec6],
velocities: wp.array[wp.vec3],
local_stress: wp.array[vec6],
delta_correction: wp.array[vec6],
):
tau_i = wp.tid()
local_strain = wp.static(make_compute_local_strain(has_compliance_mat, strain_velocity_node_count))(
tau_i,
compliance_mat_offsets,
compliance_mat_columns,
local_compliance_mat_values,
strain_mat_offsets,
strain_mat_columns,
strain_mat_values,
local_strain_rhs,
velocities,
local_stress,
)
delta_correction[tau_i] = wp.static(make_solve_local_stress(has_viscosity, has_dilatancy, has_rotation))(
tau_i,
local_strain,
yield_params,
strain_node_volume,
delassus_diagonal,
delassus_rotation,
local_stress,
)
return jacobi_solve_kernel_impl
def make_gs_solve_kernel(
has_viscosity: bool,
has_dilatancy: bool,
has_compliance_mat: bool,
strain_velocity_node_count: int = -1,
has_rotation: bool = not _ISOTROPIC_LOCAL_LHS,
):
"""Return a Gauss-Seidel colored-block stress solve kernel specialized for the given feature flags.
The returned kernel processes strain nodes in color order to avoid write conflicts,
immediately propagating velocity updates within each color. Each unique combination
of flags produces a separate Warp kernel compiled and cached via
:func:`fem.cache.dynamic_kernel`.
"""
key = (has_viscosity, has_dilatancy, has_compliance_mat, has_rotation, strain_velocity_node_count)
@fem.cache.dynamic_kernel(suffix=key, kernel_options={"fast_math": True})
def gs_solve_kernel_impl(
color: int,
launch_dim: int,
color_offsets: wp.array[int],
color_blocks: wp.array2d[int],
yield_params: wp.array[YieldParamVec],
strain_node_volume: wp.array[float],
compliance_mat_offsets: wp.array[int],
compliance_mat_columns: wp.array[int],
compliance_mat_values: wp.array[mat66],
strain_mat_offsets: wp.array[int],
strain_mat_columns: wp.array[int],
strain_mat_values: wp.array[mat13],
delassus_diagonal: wp.array[vec6],
delassus_rotation: wp.array[mat55],
inv_mass_matrix: wp.array[float],
local_strain_rhs: wp.array[vec6],
velocities: wp.array[wp.vec3],
local_stress: wp.array[vec6],
delta_correction: wp.array[vec6],
):
i = wp.tid()
color_beg = color_offsets[color] + i
color_end = color_offsets[color + 1]
for color_offset in range(color_beg, color_end, launch_dim):
beg, end = color_blocks[0, color_offset], color_blocks[1, color_offset]
for tau_i in range(beg, end):
local_strain = wp.static(make_compute_local_strain(has_compliance_mat, strain_velocity_node_count))(
tau_i,
compliance_mat_offsets,
compliance_mat_columns,
compliance_mat_values,
strain_mat_offsets,
strain_mat_columns,
strain_mat_values,
local_strain_rhs,
velocities,
local_stress,
)
delta_stress = wp.static(make_solve_local_stress(has_viscosity, has_dilatancy, has_rotation))(
tau_i,
local_strain,
yield_params,
strain_node_volume,
delassus_diagonal,
delassus_rotation,
local_stress,
)
local_stress[tau_i] += delta_stress
delta_correction[tau_i] = delta_stress
wp.static(make_apply_stress_delta(strain_velocity_node_count))(
tau_i,
delta_stress,
strain_mat_offsets,
strain_mat_columns,
strain_mat_values,
inv_mass_matrix,
velocities,
)
return gs_solve_kernel_impl
@wp.kernel
def jacobi_preconditioner(
delassus_diagonal: wp.array[vec6],
delassus_rotation: wp.array[mat55],
x: wp.array[vec6],
y: wp.array[vec6],
z: wp.array[vec6],
alpha: float,
beta: float,
):
tau_i = wp.tid()
rot = delassus_rotation[tau_i]
diag = delassus_diagonal[tau_i]
Wx = _local_to_world(wp.cw_div(_world_to_local(x[tau_i], rot), diag), rot)
z[tau_i] = alpha * Wx + beta * y[tau_i]
@wp.kernel
def evaluate_strain_residual(
delta_stress: wp.array[vec6],
delassus_diagonal: wp.array[vec6],
delassus_rotation: wp.array[mat55],
residual: wp.array[float],
):
tau_i = wp.tid()
local_strain_delta = wp.cw_mul(
_world_to_local(delta_stress[tau_i], delassus_rotation[tau_i]), delassus_diagonal[tau_i]
)
r = wp.length_sq(local_strain_delta)
residual[tau_i] = r
================================================
FILE: newton/_src/solvers/implicit_mpm/solve_rheology.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import gc
import math
from dataclasses import dataclass
from typing import Any
import warp as wp
import warp.fem as fem
import warp.sparse as sp
from warp.optim.linear import LinearOperator, cg
from .contact_solver_kernels import (
apply_nodal_impulse_warmstart,
apply_subgrid_impulse,
apply_subgrid_impulse_warmstart,
compute_collider_delassus_diagonal,
compute_collider_inv_mass,
solve_nodal_friction,
solve_subgrid_friction,
)
from .rheology_solver_kernels import (
YieldParamVec,
apply_stress_delta_jacobi,
apply_stress_gs,
apply_velocity_delta,
compute_delassus_diagonal,
evaluate_strain_residual,
jacobi_preconditioner,
make_gs_solve_kernel,
make_jacobi_solve_kernel,
mat13,
mat55,
postprocess_stress_and_strain,
preprocess_stress_and_strain,
vec6,
)
_TILED_SUM_BLOCK_DIM = 512
@wp.kernel
def _tiled_sum_kernel(
data: wp.array2d[float],
partial_sums: wp.array2d[float],
):
block_id = wp.tid()
tile = wp.tile_load(data[0], shape=_TILED_SUM_BLOCK_DIM, offset=block_id * _TILED_SUM_BLOCK_DIM)
wp.tile_store(partial_sums[0], wp.tile_sum(tile), offset=block_id)
tile = wp.tile_load(data[1], shape=_TILED_SUM_BLOCK_DIM, offset=block_id * _TILED_SUM_BLOCK_DIM)
wp.tile_store(partial_sums[1], wp.tile_max(tile), offset=block_id)
class ArraySquaredNorm:
"""Utility to compute squared L2 norm of a large array via tiled reductions."""
def __init__(self, max_length: int, device=None, temporary_store=None):
self.tile_size = _TILED_SUM_BLOCK_DIM
self.device = device
num_blocks = (max_length + self.tile_size - 1) // self.tile_size
self.partial_sums_a = fem.borrow_temporary(
temporary_store, shape=(2, num_blocks), dtype=float, device=self.device
)
self.partial_sums_b = fem.borrow_temporary(
temporary_store, shape=(2, num_blocks), dtype=float, device=self.device
)
self.partial_sums_a.zero_()
self.partial_sums_b.zero_()
self.sum_launch: wp.Launch = wp.launch(
_tiled_sum_kernel,
dim=(num_blocks, self.tile_size),
inputs=(self.partial_sums_a,),
outputs=(self.partial_sums_b,),
block_dim=self.tile_size,
record_cmd=True,
)
# Result contains a single value, the sum of the array (will get updated by this function)
def compute_squared_norm(self, data: wp.array[Any]):
# cast vector types to float
if data.ndim != 2:
data = wp.array(
ptr=data.ptr,
shape=(2, data.shape[0]),
dtype=data.dtype,
strides=(0, data.strides[0]),
device=data.device,
)
array_length = data.shape[1]
flip_flop = False
while True:
num_blocks = (array_length + self.tile_size - 1) // self.tile_size
partial_sums = (self.partial_sums_a if flip_flop else self.partial_sums_b)[:, :num_blocks]
self.sum_launch.set_param_at_index(0, data[:, :array_length])
self.sum_launch.set_param_at_index(1, partial_sums)
self.sum_launch.set_dim((num_blocks, self.tile_size))
self.sum_launch.launch()
array_length = num_blocks
data = partial_sums
flip_flop = not flip_flop
if num_blocks == 1:
break
return data[:, :1]
def release(self):
"""Return borrowed temporaries to their pool."""
for attr in ("partial_sums_a", "partial_sums_b"):
temporary = getattr(self, attr, None)
if temporary is not None:
temporary.release()
setattr(self, attr, None)
def __del__(self):
self.release()
@wp.kernel
def update_condition(
residual_threshold: float,
l2_scale: float,
solve_granularity: int,
max_iterations: int,
residual: wp.array2d[float],
iteration: wp.array[int],
condition: wp.array[int],
):
cur_it = iteration[0] + solve_granularity
stop = (
residual[0, 0] < residual_threshold * l2_scale and residual[1, 0] < residual_threshold
) or cur_it > max_iterations
iteration[0] = cur_it
condition[0] = wp.where(stop, 0, 1)
def apply_rigidity_operator(rigidity_operator, delta_collider_impulse, collider_velocity, delta_body_qd):
"""Apply collider rigidity feedback to the current collider velocities.
Computes and applies a velocity correction induced by the rigid coupling
operator according to the relation::
delta_body_qd = -IJtm @ delta_collider_impulse
collider_velocity += J @ delta_body_qd
where ``(J, IJtm) = rigidity_operator`` are the block-sparse matrices
returned by ``build_rigidity_operator``.
Args:
rigidity_operator: Pair ``(J, IJtm)`` of block-sparse matrices returned
by ``build_rigidity_operator``.
delta_collider_impulse: Change in collider impulse to be applied.
collider_velocity: Current collider velocity vector to be corrected in place.
delta_body_qd: Change in body velocity to be applied.
"""
J, IJtm = rigidity_operator
sp.bsr_mv(IJtm, x=delta_collider_impulse, y=delta_body_qd, alpha=-1.0, beta=0.0)
sp.bsr_mv(J, x=delta_body_qd, y=collider_velocity, alpha=1.0, beta=1.0)
class _ScopedDisableGC:
"""Context manager to disable automatic garbage collection during graph capture.
Avoids capturing deallocations of arrays exterior to the capture scope.
"""
def __enter__(self):
self.was_enabled = gc.isenabled()
gc.disable()
def __exit__(self, exc_type, exc_value, traceback):
if self.was_enabled:
gc.enable()
@dataclass
class MomentumData:
"""Per-node momentum quantities used by the rheology solver.
Attributes:
inv_volume: Inverse volume (or inverse mass scaling) per velocity
node, shape ``[node_count]``.
velocity: Grid velocity DOFs to be updated in place [m/s],
shape ``[node_count, 3]``.
"""
inv_volume: wp.array
velocity: wp.array[wp.vec3]
@dataclass
class RheologyData:
"""Strain, compliance, yield, and coloring data for the rheology solve.
Attributes:
strain_mat: Strain-to-velocity block-sparse matrix (B).
transposed_strain_mat: BSR container for B^T, used by the Jacobi
solver path.
compliance_mat: Compliance (inverse stiffness) block-sparse matrix.
strain_node_volume: Volume associated with each strain node [m^3],
shape ``[strain_count]``.
yield_params: Yield-surface parameters per strain node,
shape ``[strain_count]``.
unilateral_strain_offset: Per-node offset enforcing unilateral
incompressibility (void/critical fraction),
shape ``[strain_count]``.
color_offsets: Coloring offsets for Gauss-Seidel iteration,
shape ``[num_colors + 1]``.
color_blocks: Per-color strain-node indices for Gauss-Seidel,
shape ``[num_colors, max_block_size]``.
elastic_strain_delta: Output elastic strain increment per strain
node, shape ``[strain_count, 6]``.
plastic_strain_delta: Output plastic strain increment per strain
node, shape ``[strain_count, 6]``.
stress: In/out stress per strain node (rotated internally),
shape ``[strain_count, 6]``.
"""
strain_mat: sp.BsrMatrix
transposed_strain_mat: sp.BsrMatrix
compliance_mat: sp.BsrMatrix
strain_node_volume: wp.array[float]
yield_params: wp.array[YieldParamVec]
unilateral_strain_offset: wp.array[float]
color_offsets: wp.array[int]
color_blocks: wp.array2d[int]
elastic_strain_delta: wp.array[vec6]
plastic_strain_delta: wp.array[vec6]
stress: wp.array[vec6]
has_viscosity: bool = False
has_dilatancy: bool = False
strain_velocity_node_count: int = -1
@dataclass
class CollisionData:
"""Collider contact data consumed by the rheology solver.
Attributes:
collider_mat: Block-sparse matrix mapping velocity nodes to
collider DOFs.
transposed_collider_mat: Transpose of ``collider_mat``.
collider_friction: Per-node friction coefficients; negative values
disable contact at that node, shape ``[node_count]``.
collider_adhesion: Per-node adhesion coefficients [N s / V0],
shape ``[node_count]``.
collider_normals: Per-node contact normals,
shape ``[node_count, 3]``.
collider_velocities: Per-node collider rigid-body velocities [m/s],
shape ``[node_count, 3]``.
rigidity_operator: Optional pair of BSR matrices coupling velocity
nodes to collider DOFs. ``None`` when unused.
collider_impulse: In/out stored collider impulses for warm-starting
[N s / V0], shape ``[node_count, 3]``.
"""
collider_mat: sp.BsrMatrix
transposed_collider_mat: sp.BsrMatrix
collider_friction: wp.array[float]
collider_adhesion: wp.array[float]
collider_normals: wp.array[wp.vec3]
collider_velocities: wp.array[wp.vec3]
rigidity_operator: tuple[sp.BsrMatrix, sp.BsrMatrix] | None
collider_impulse: wp.array[wp.vec3]
class _DelassusOperator:
def __init__(
self,
rheology: RheologyData,
momentum: MomentumData,
temporary_store: fem.TemporaryStore | None = None,
):
self.rheology = rheology
self.momentum = momentum
self.delassus_rotation = fem.borrow_temporary(temporary_store, shape=self.size, dtype=mat55)
self.delassus_diagonal = fem.borrow_temporary(temporary_store, shape=self.size, dtype=vec6)
self._computed = False
self._split_mass = False
self._has_strain_mat_transpose = False
self.preprocess_stress_and_strain()
def compute_diagonal_factorization(self, split_mass: bool):
if self._computed and self._split_mass == split_mass:
return
if split_mass:
self.require_strain_mat_transpose()
strain_mat_values = self.rheology.strain_mat.values.view(dtype=mat13)
wp.launch(
kernel=compute_delassus_diagonal,
dim=self.size,
inputs=[
split_mass,
self.rheology.strain_mat.offsets,
self.rheology.strain_mat.columns,
strain_mat_values,
self.momentum.inv_volume,
self.rheology.compliance_mat.offsets,
self.rheology.compliance_mat.columns,
self.rheology.compliance_mat.values,
self.rheology.transposed_strain_mat.offsets,
],
outputs=[
self.delassus_rotation,
self.delassus_diagonal,
],
)
self._computed = True
self._split_mass = split_mass
def require_strain_mat_transpose(self):
if not self._has_strain_mat_transpose:
sp.bsr_set_transpose(dest=self.rheology.transposed_strain_mat, src=self.rheology.strain_mat)
self._has_strain_mat_transpose = True
def preprocess_stress_and_strain(self):
# Project initial stress on yield surface
wp.launch(
kernel=preprocess_stress_and_strain,
dim=self.size,
inputs=[
self.rheology.unilateral_strain_offset,
self.rheology.elastic_strain_delta,
self.rheology.stress,
self.rheology.yield_params,
],
)
@property
def size(self):
return self.rheology.stress.shape[0]
def release(self):
self.delassus_rotation.release()
self.delassus_diagonal.release()
def apply_stress_delta(self, stress_delta: wp.array[vec6], velocity: wp.array[wp.vec3], record_cmd: bool = False):
return wp.launch(
kernel=apply_stress_delta_jacobi,
dim=self.momentum.velocity.shape[0],
inputs=[
self.rheology.transposed_strain_mat.offsets,
self.rheology.transposed_strain_mat.columns,
self.rheology.transposed_strain_mat.values.view(dtype=mat13),
self.momentum.inv_volume,
stress_delta,
],
outputs=[velocity],
record_cmd=record_cmd,
)
def apply_velocity_delta(
self,
velocity_delta: wp.array[wp.vec3],
strain_prev: wp.array[vec6],
strain: wp.array[vec6],
alpha: float = 1.0,
beta: float = 1.0,
record_cmd: bool = False,
):
return wp.launch(
kernel=apply_velocity_delta,
dim=self.size,
inputs=[
alpha,
beta,
self.rheology.strain_mat.offsets,
self.rheology.strain_mat.columns,
self.rheology.strain_mat.values.view(dtype=mat13),
velocity_delta,
strain_prev,
],
outputs=[
strain,
],
record_cmd=record_cmd,
)
def postprocess_stress_and_strain(self):
# Convert stress back to world space,
# and compute final elastic strain
wp.launch(
kernel=postprocess_stress_and_strain,
dim=self.size,
inputs=[
self.rheology.compliance_mat.offsets,
self.rheology.compliance_mat.columns,
self.rheology.compliance_mat.values,
self.rheology.strain_mat.offsets,
self.rheology.strain_mat.columns,
self.rheology.strain_mat.values.view(dtype=mat13),
self.delassus_diagonal,
self.delassus_rotation,
self.rheology.unilateral_strain_offset,
self.rheology.yield_params,
self.rheology.strain_node_volume,
self.rheology.elastic_strain_delta,
self.rheology.stress,
self.momentum.velocity,
],
outputs=[
self.rheology.elastic_strain_delta,
self.rheology.plastic_strain_delta,
],
)
class _RheologySolver:
def __init__(
self,
delassus_operator: _DelassusOperator,
split_mass: bool,
temporary_store: fem.TemporaryStore | None = None,
):
self.delassus_operator = delassus_operator
self.momentum = delassus_operator.momentum
self.rheology = delassus_operator.rheology
self.device = self.momentum.velocity.device
self.delta_stress = fem.borrow_temporary_like(self.rheology.stress, temporary_store)
self.strain_residual = fem.borrow_temporary(
temporary_store, shape=(self.size,), dtype=float, device=self.device
)
self.strain_residual.zero_()
self.delassus_operator.compute_diagonal_factorization(split_mass)
self._evaluate_strain_residual_launch = wp.launch(
kernel=evaluate_strain_residual,
dim=self.size,
inputs=[
self.delta_stress,
self.delassus_operator.delassus_diagonal,
self.delassus_operator.delassus_rotation,
],
outputs=[
self.strain_residual,
],
record_cmd=True,
)
# Utility to compute the squared norm of the residual
self._residual_squared_norm_computer = ArraySquaredNorm(
max_length=self.size,
device=self.device,
temporary_store=temporary_store,
)
@property
def size(self):
return self.rheology.stress.shape[0]
def eval_residual(self):
self._evaluate_strain_residual_launch.launch()
return self._residual_squared_norm_computer.compute_squared_norm(self.strain_residual)
def release(self):
self.delta_stress.release()
self.strain_residual.release()
self._residual_squared_norm_computer.release()
class _GaussSeidelSolver(_RheologySolver):
def __init__(
self,
delassus_operator: _DelassusOperator,
temporary_store: fem.TemporaryStore | None = None,
) -> None:
super().__init__(delassus_operator, split_mass=False, temporary_store=temporary_store)
self.color_count = self.rheology.color_offsets.shape[0] - 1
if self.device.is_cuda:
color_block_count = self.device.sm_count * 2
else:
color_block_count = 1
color_block_dim = 64
color_launch_dim = color_block_count * color_block_dim
self.apply_stress_launch = wp.launch(
kernel=apply_stress_gs,
dim=color_launch_dim,
inputs=[
0, # color
color_launch_dim,
self.rheology.color_offsets,
self.rheology.color_blocks,
self.rheology.strain_mat.offsets,
self.rheology.strain_mat.columns,
self.rheology.strain_mat.values.view(dtype=mat13),
self.momentum.inv_volume,
self.rheology.stress,
],
outputs=[
self.momentum.velocity,
],
block_dim=color_block_dim,
max_blocks=color_block_count,
record_cmd=True,
)
# Solve kernel
gs_kernel = make_gs_solve_kernel(
has_viscosity=self.rheology.has_viscosity,
has_dilatancy=self.rheology.has_dilatancy,
has_compliance_mat=self.rheology.compliance_mat.nnz > 0,
strain_velocity_node_count=self.rheology.strain_velocity_node_count,
)
self.solve_local_launch = wp.launch(
kernel=gs_kernel,
dim=color_launch_dim,
inputs=[
0, # color
color_launch_dim,
self.rheology.color_offsets,
self.rheology.color_blocks,
self.rheology.yield_params,
self.rheology.strain_node_volume,
self.rheology.compliance_mat.offsets,
self.rheology.compliance_mat.columns,
self.rheology.compliance_mat.values,
self.rheology.strain_mat.offsets,
self.rheology.strain_mat.columns,
self.rheology.strain_mat.values.view(dtype=mat13),
self.delassus_operator.delassus_diagonal,
self.delassus_operator.delassus_rotation,
self.momentum.inv_volume,
self.rheology.elastic_strain_delta,
],
outputs=[
self.momentum.velocity,
self.rheology.stress,
self.delta_stress,
],
block_dim=color_block_dim,
max_blocks=color_block_count,
record_cmd=True,
)
@property
def name(self):
return "Gauss-Seidel"
@property
def solve_granularity(self):
return 25
def apply_initial_guess(self):
for color in range(self.color_count):
self.apply_stress_launch.set_param_at_index(0, color)
self.apply_stress_launch.launch()
def solve(self):
for color in range(self.color_count):
self.solve_local_launch.set_param_at_index(0, color)
self.solve_local_launch.launch()
class _JacobiSolver(_RheologySolver):
def __init__(
self,
delassus_operator: _DelassusOperator,
temporary_store: fem.TemporaryStore | None = None,
) -> None:
super().__init__(delassus_operator, split_mass=True, temporary_store=temporary_store)
self.apply_stress_launch = self.delassus_operator.apply_stress_delta(
self.delta_stress,
self.momentum.velocity,
record_cmd=True,
)
# Solve kernel
jacobi_kernel = make_jacobi_solve_kernel(
has_viscosity=self.rheology.has_viscosity,
has_dilatancy=self.rheology.has_dilatancy,
has_compliance_mat=self.rheology.compliance_mat.nnz > 0,
strain_velocity_node_count=self.rheology.strain_velocity_node_count,
)
self.solve_local_launch = wp.launch(
kernel=jacobi_kernel,
dim=self.size,
inputs=[
self.rheology.yield_params,
self.rheology.strain_node_volume,
self.rheology.compliance_mat.offsets,
self.rheology.compliance_mat.columns,
self.rheology.compliance_mat.values,
self.rheology.strain_mat.offsets,
self.rheology.strain_mat.columns,
self.rheology.strain_mat.values.view(dtype=mat13),
self.delassus_operator.delassus_diagonal,
self.delassus_operator.delassus_rotation,
self.rheology.elastic_strain_delta,
self.momentum.velocity,
self.rheology.stress,
],
outputs=[
self.delta_stress,
],
record_cmd=True,
)
@property
def name(self):
return "Jacobi"
@property
def solve_granularity(self):
return 50
def apply_initial_guess(self):
# Apply initial guess
self.delta_stress.assign(self.rheology.stress)
self.apply_stress_launch.launch()
def solve(self):
self.solve_local_launch.launch()
# Add jacobi delta
self.apply_stress_launch.launch()
fem.utils.array_axpy(x=self.delta_stress, y=self.rheology.stress, alpha=1.0, beta=1.0)
class _CGSolver:
def __init__(
self,
delassus_operator: _DelassusOperator,
temporary_store: fem.TemporaryStore | None = None,
) -> None:
self.momentum = delassus_operator.momentum
self.rheology = delassus_operator.rheology
self.delassus_operator = delassus_operator
self.delassus_operator.require_strain_mat_transpose()
self.delassus_operator.compute_diagonal_factorization(split_mass=False)
self.delta_velocity = fem.borrow_temporary_like(self.momentum.velocity, temporary_store)
shape = self.rheology.compliance_mat.shape
dtype = self.rheology.compliance_mat.dtype
device = self.rheology.compliance_mat.device
self.linear_operator = LinearOperator(shape=shape, dtype=dtype, device=device, matvec=self._delassus_matvec)
self.preconditioner = LinearOperator(
shape=shape, dtype=dtype, device=device, matvec=self._preconditioner_matvec
)
def _delassus_matvec(self, x: wp.array[vec6], y: wp.array[vec6], z: wp.array[vec6], alpha: float, beta: float):
# dv = B^T x
self.delta_velocity.zero_()
self.delassus_operator.apply_stress_delta(x, self.delta_velocity)
# z = alpha B dv + beta * y
self.delassus_operator.apply_velocity_delta(self.delta_velocity, y, z, alpha, beta)
# z += C x
sp.bsr_mv(self.rheology.compliance_mat, x, z, alpha=alpha, beta=1.0)
def _preconditioner_matvec(self, x, y, z, alpha, beta):
wp.launch(
kernel=jacobi_preconditioner,
dim=self.delassus_operator.size,
inputs=[
self.delassus_operator.delassus_diagonal,
self.delassus_operator.delassus_rotation,
x,
y,
z,
alpha,
beta,
],
)
def solve(self, tol: float, tolerance_scale: float, max_iterations: int, use_graph: bool, verbose: bool):
self.delassus_operator.apply_velocity_delta(
self.momentum.velocity,
self.rheology.elastic_strain_delta,
self.rheology.plastic_strain_delta,
alpha=-1.0,
beta=-1.0,
)
with _ScopedDisableGC():
end_iter, residual, atol = cg(
A=self.linear_operator,
M=self.preconditioner,
b=self.rheology.plastic_strain_delta,
x=self.rheology.stress,
atol=tol * tolerance_scale,
tol=tol,
maxiter=max_iterations,
check_every=0 if use_graph else 10,
use_cuda_graph=use_graph,
)
if use_graph:
end_iter = end_iter.numpy()[0]
residual = residual.numpy()[0]
atol = atol.numpy()[0]
if verbose:
res = math.sqrt(residual) / tolerance_scale
print(f"{self.name} terminated after {end_iter} iterations with residual {res}")
@property
def name(self):
return "Conjugate Gradient"
def release(self):
self.delta_velocity.release()
class _ContactSolver:
def __init__(
self,
momentum: MomentumData,
collision: CollisionData,
temporary_store: fem.TemporaryStore | None = None,
) -> None:
self.momentum = momentum
self.collision = collision
self.delta_impulse = fem.borrow_temporary_like(self.collision.collider_impulse, temporary_store)
self.collider_inv_mass = fem.borrow_temporary_like(self.collision.collider_friction, temporary_store)
# Setup rigidity correction
if self.collision.rigidity_operator is not None:
J, IJtm = self.collision.rigidity_operator
self.delta_body_qd = fem.borrow_temporary(temporary_store, shape=J.shape[1], dtype=float)
wp.launch(
compute_collider_inv_mass,
dim=self.collision.collider_impulse.shape[0],
inputs=[
J.offsets,
J.columns,
J.values,
IJtm.offsets,
IJtm.columns,
IJtm.values,
],
outputs=[
self.collider_inv_mass,
],
)
else:
self.collider_inv_mass.zero_()
def release(self):
self.delta_impulse.release()
self.collider_inv_mass.release()
if self.collision.rigidity_operator is not None:
self.delta_body_qd.release()
def apply_rigidity_operator(self):
if self.collision.rigidity_operator is not None:
apply_rigidity_operator(
self.collision.rigidity_operator,
self.delta_impulse,
self.collision.collider_velocities,
self.delta_body_qd,
)
class _NodalContactSolver(_ContactSolver):
def __init__(
self,
momentum: MomentumData,
collision: CollisionData,
temporary_store: fem.TemporaryStore | None = None,
) -> None:
super().__init__(momentum, collision, temporary_store)
# define solve operation
self.solve_collider_launch = wp.launch(
kernel=solve_nodal_friction,
dim=self.collision.collider_impulse.shape[0],
inputs=[
self.momentum.inv_volume,
self.collision.collider_friction,
self.collision.collider_adhesion,
self.collision.collider_normals,
self.collider_inv_mass,
self.momentum.velocity,
self.collision.collider_velocities,
self.collision.collider_impulse,
self.delta_impulse,
],
record_cmd=True,
)
def apply_initial_guess(self):
# Apply initial impulse guess
wp.launch(
kernel=apply_nodal_impulse_warmstart,
dim=self.collision.collider_impulse.shape[0],
inputs=[
self.collision.collider_impulse,
self.collision.collider_friction,
self.collision.collider_normals,
self.collision.collider_adhesion,
self.momentum.inv_volume,
self.momentum.velocity,
self.delta_impulse,
],
)
self.apply_rigidity_operator()
def solve(self):
self.solve_collider_launch.launch()
self.apply_rigidity_operator()
class _SubgridContactSolver(_ContactSolver):
def __init__(
self,
momentum: MomentumData,
collision: CollisionData,
temporary_store: fem.TemporaryStore | None = None,
) -> None:
super().__init__(momentum, collision, temporary_store)
self.collider_delassus_diagonal = fem.borrow_temporary_like(self.collider_inv_mass, temporary_store)
sp.bsr_set_transpose(dest=self.collision.transposed_collider_mat, src=self.collision.collider_mat)
wp.launch(
compute_collider_delassus_diagonal,
dim=self.collision.collider_impulse.shape[0],
inputs=[
self.collision.collider_mat.offsets,
self.collision.collider_mat.columns,
self.collision.collider_mat.values,
self.collider_inv_mass,
self.collision.transposed_collider_mat.offsets,
self.momentum.inv_volume,
],
outputs=[
self.collider_delassus_diagonal,
],
)
# define solve operation
self.apply_collider_impulse_launch = wp.launch(
apply_subgrid_impulse,
dim=self.momentum.velocity.shape[0],
inputs=[
self.collision.transposed_collider_mat.offsets,
self.collision.transposed_collider_mat.columns,
self.collision.transposed_collider_mat.values,
self.momentum.inv_volume,
self.delta_impulse,
self.momentum.velocity,
],
record_cmd=True,
)
self.solve_collider_launch = wp.launch(
kernel=solve_subgrid_friction,
dim=self.collision.collider_impulse.shape[0],
inputs=[
self.momentum.velocity,
self.collision.collider_mat.offsets,
self.collision.collider_mat.columns,
self.collision.collider_mat.values,
self.collision.collider_friction,
self.collision.collider_adhesion,
self.collision.collider_normals,
self.collider_delassus_diagonal,
self.collision.collider_velocities,
self.collision.collider_impulse,
self.delta_impulse,
],
record_cmd=True,
)
def apply_initial_guess(self):
wp.launch(
apply_subgrid_impulse_warmstart,
dim=self.delta_impulse.shape[0],
inputs=[
self.collision.collider_friction,
self.collision.collider_normals,
self.collision.collider_adhesion,
self.collision.collider_impulse,
self.delta_impulse,
],
)
self.apply_collider_impulse_launch.launch()
self.apply_rigidity_operator()
def solve(self):
self.solve_collider_launch.launch()
self.apply_collider_impulse_launch.launch()
self.apply_rigidity_operator()
def release(self):
self.collider_delassus_diagonal.release()
super().release()
def _run_solver_loop(
rheology_solver: _RheologySolver,
contact_solver: _ContactSolver,
max_iterations: int,
tolerance: float,
l2_tolerance_scale: float,
use_graph: bool,
verbose: bool,
temporary_store: fem.TemporaryStore,
):
solve_graph = None
if use_graph:
solve_granularity = 5
iteration_and_condition = fem.borrow_temporary(temporary_store, shape=(2,), dtype=int)
iteration_and_condition.fill_(1)
iteration = iteration_and_condition[:1]
condition = iteration_and_condition[1:]
def do_iteration_with_condition():
for _k in range(solve_granularity):
contact_solver.solve()
rheology_solver.solve()
residual = rheology_solver.eval_residual()
wp.launch(
update_condition,
dim=1,
inputs=[
tolerance * tolerance,
l2_tolerance_scale * l2_tolerance_scale,
solve_granularity,
max_iterations,
residual,
iteration,
condition,
],
)
device = rheology_solver.device
if device.is_capturing:
with _ScopedDisableGC():
wp.capture_while(condition, do_iteration_with_condition)
else:
with _ScopedDisableGC():
with wp.ScopedCapture(force_module_load=False) as capture:
wp.capture_while(condition, do_iteration_with_condition)
solve_graph = capture.graph
wp.capture_launch(solve_graph)
if verbose:
residual = rheology_solver.eval_residual().numpy()
res_l2, res_linf = math.sqrt(residual[0, 0]) / l2_tolerance_scale, math.sqrt(residual[1, 0])
print(
f"{rheology_solver.name} terminated after {iteration_and_condition.numpy()[0]} iterations with residuals {res_l2}, {res_linf}"
)
iteration_and_condition.release()
else:
solve_granularity = rheology_solver.solve_granularity
for batch in range(max_iterations // solve_granularity):
for _k in range(solve_granularity):
contact_solver.solve()
rheology_solver.solve()
residual = rheology_solver.eval_residual().numpy()
res_l2, res_linf = math.sqrt(residual[0, 0]) / l2_tolerance_scale, math.sqrt(residual[1, 0])
if verbose:
print(
f"{rheology_solver.name} iteration #{(batch + 1) * solve_granularity} \t res(l2)={res_l2}, res(linf)={res_linf}"
)
if res_l2 < tolerance and res_linf < tolerance:
break
return solve_graph
def solve_rheology(
solver: str,
max_iterations: int,
tolerance: float,
momentum: MomentumData,
rheology: RheologyData,
collision: CollisionData,
jacobi_warmstart_smoother_iterations: int = 0,
temporary_store: fem.TemporaryStore | None = None,
use_graph: bool = True,
verbose: bool = wp.config.verbose,
):
"""Solve coupled plasticity and collider contact to compute grid velocities.
This function executes the implicit rheology loop that couples plastic
stress update and nodal frictional contact with colliders:
- Builds the Delassus operator diagonal blocks and rotates all local
quantities into the decoupled eigenbasis (normal vs tangential).
- Runs either Gauss-Seidel (with coloring) or Jacobi iterations to solve
the local stress projection problem per strain node.
- Applies collider impulses and, when provided, a rigidity coupling step on
collider velocities each iteration.
- Iterates until the residual on the stress update falls below
``tolerance`` or ``max_iterations`` is reached. Optionally records and
executes CUDA graphs to reduce CPU overhead.
On exit, the stress field is rotated back to world space and the elastic
strain increment and plastic strain delta fields are produced.
Args:
solver: Solver type string. ``"gauss-seidel"``, ``"jacobi"``,
``"cg"``, or ``"cg+"`` (CG as initial guess then
```` for the main solve).
Note that the ``cg`` solver only supports solid materials, without contacts.
max_iterations: Maximum number of nonlinear iterations.
tolerance: Solver tolerance for the stress residual (L2 norm).
momentum: :class:`MomentumData` containing per-node inverse volume
and velocity DOFs.
rheology: :class:`RheologyData` containing strain/compliance matrices,
yield parameters, coloring data, and output stress/strain arrays.
collision: :class:`CollisionData` containing collider matrices, friction,
adhesion, normals, velocities, rigidity operator, and impulse arrays.
jacobi_warmstart_smoother_iterations: Number of Jacobi smoother
iterations to run before the main Gauss-Seidel solve (ignored
for Jacobi solver).
temporary_store: Temporary storage arena for intermediate arrays.
use_graph: If True, uses conditional CUDA graphs for the iteration loop.
verbose: If True, prints residuals/iteration counts.
Returns:
A captured execution graph handle when ``use_graph`` is True and the
device supports it; otherwise ``None``.
"""
subgrid_collisions = collision.collider_mat.nnz > 0
if subgrid_collisions:
contact_solver = _SubgridContactSolver(momentum, collision, temporary_store)
else:
contact_solver = _NodalContactSolver(momentum, collision, temporary_store)
contact_solver.apply_initial_guess()
delassus_operator = _DelassusOperator(rheology, momentum, temporary_store)
tolerance_scale = math.sqrt(1 + delassus_operator.size)
if solver[:2] == "cg": # matches "cg" or "cg+xxx"
rheology_solver = _CGSolver(delassus_operator, temporary_store)
rheology_solver.solve(tolerance, tolerance_scale, max_iterations, use_graph, verbose)
rheology_solver.release()
if solver == "cg":
delassus_operator.apply_stress_delta(rheology.stress, momentum.velocity)
delassus_operator.postprocess_stress_and_strain()
delassus_operator.release()
contact_solver.release()
return None
# use only as initial guess for the next solver
solver = solver[3:]
if solver == "gauss-seidel" and jacobi_warmstart_smoother_iterations > 0:
# jacobi warmstart smoother
old_v = wp.clone(momentum.velocity)
warmstart_solver = _JacobiSolver(delassus_operator, temporary_store)
warmstart_solver.apply_initial_guess()
for _ in range(jacobi_warmstart_smoother_iterations):
warmstart_solver.solve()
warmstart_solver.release()
momentum.velocity.assign(old_v)
if solver == "gauss-seidel":
rheology_solver = _GaussSeidelSolver(delassus_operator, temporary_store)
elif solver == "jacobi":
rheology_solver = _JacobiSolver(delassus_operator, temporary_store)
else:
raise ValueError(f"Invalid solver: {solver}")
rheology_solver.apply_initial_guess()
solve_graph = _run_solver_loop(
rheology_solver, contact_solver, max_iterations, tolerance, tolerance_scale, use_graph, verbose, temporary_store
)
# release temporary storage
rheology_solver.release()
contact_solver.release()
delassus_operator.postprocess_stress_and_strain()
delassus_operator.release()
return solve_graph
================================================
FILE: newton/_src/solvers/implicit_mpm/solver_implicit_mpm.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Implicit MPM solver."""
import warnings
from dataclasses import dataclass
from typing import Literal
import numpy as np
import warp as wp
import warp.fem as fem
import warp.sparse as wps
import newton
from ...core.types import override
from ..solver import SolverBase
from .implicit_mpm_model import ImplicitMPMModel
from .rasterized_collisions import (
build_rigidity_operator,
interpolate_collider_normals,
project_outside_collider,
rasterize_collider,
)
from .render_grains import sample_render_grains, update_render_grains
from .solve_rheology import CollisionData, MomentumData, RheologyData, YieldParamVec, solve_rheology
__all__ = ["SolverImplicitMPM"]
from .implicit_mpm_solver_kernels import (
EPSILON,
INFINITY,
YIELD_PARAM_LENGTH,
advect_particles,
allocate_by_voxels,
average_elastic_parameters,
collision_weight_field,
compliance_form,
compute_bounds,
compute_color_offsets,
compute_eigenvalues,
compute_unilateral_strain_offset,
fill_uniform_color_block_indices,
free_velocity,
integrate_collider_fraction,
integrate_collider_fraction_apic,
integrate_elastic_parameters,
integrate_fraction,
integrate_mass,
integrate_particle_stress,
integrate_velocity,
integrate_velocity_apic,
integrate_yield_parameters,
inverse_scale_sym_tensor,
inverse_scale_vector,
make_cell_color_kernel,
make_dynamic_color_block_indices_kernel,
make_inverse_rotate_vectors,
make_rotate_vectors,
mark_active_cells,
mass_form,
mat11,
mat13,
mat31,
mat66,
node_color,
rotate_matrix_columns,
rotate_matrix_rows,
scatter_field_dof_values,
strain_delta_form,
strain_rhs,
update_particle_frames,
update_particle_strains,
)
def _as_2d_array(array, shape, dtype):
return wp.array(
data=None,
ptr=array.ptr,
capacity=array.capacity,
device=array.device,
shape=shape,
dtype=dtype,
grad=None if array.grad is None else _as_2d_array(array.grad, shape, dtype),
)
def _make_grid_basis_space(grid: fem.Geometry, basis_str: str, family: fem.Polynomial | None = None):
assert len(basis_str) >= 2
degree = int(basis_str[1])
discontinuous = degree == 0 or basis_str[-1] == "d"
if basis_str[0] == "B":
element_basis = fem.ElementBasis.BSPLINE
elif basis_str[0] == "Q":
element_basis = fem.ElementBasis.LAGRANGE
elif basis_str[0] == "S":
element_basis = fem.ElementBasis.SERENDIPITY
elif basis_str[0] == "P" and discontinuous:
element_basis = fem.ElementBasis.NONCONFORMING_POLYNOMIAL
else:
raise ValueError(
f"Unsupported basis: {basis_str}. Expected format: Q[d], S, or P[d] for tri-polynomial, serendipity, or non-conforming polynomial respectively."
)
return fem.make_polynomial_basis_space(
grid, degree=degree, element_basis=element_basis, family=family, discontinuous=discontinuous
)
def _make_pic_basis_space(pic: fem.PicQuadrature, basis_str: str):
try:
max_points_per_cell = int(basis_str[3:])
except ValueError:
max_points_per_cell = -1
return fem.PointBasisSpace(pic, max_nodes_per_element=max_points_per_cell, use_evaluation_point_index=True)
class ImplicitMPMScratchpad:
"""Per-step spaces, fields, and temporaries for the implicit MPM solver."""
def __init__(self):
self.grid = None
self.velocity_test = None
self.velocity_trial = None
self.fraction_test = None
self.sym_strain_test = None
self.sym_strain_trial = None
self.divergence_test = None
self.divergence_trial = None
self.fraction_field = None
self.elastic_parameters_field = None
self.plastic_strain_delta_field = None
self.elastic_strain_delta_field = None
self.strain_yield_parameters_field = None
self.strain_yield_parameters_test = None
self.strain_matrix = wps.bsr_zeros(0, 0, mat13)
self.transposed_strain_matrix = wps.bsr_zeros(0, 0, mat31)
self.compliance_matrix = wps.bsr_zeros(0, 0, mat66)
self.color_offsets = None
self.color_indices = None
self.inv_mass_matrix = None
self.collider_fraction_test = None
self.collider_normal_field = None
self.collider_distance_field = None
self.collider_velocity = None
self.collider_friction = None
self.collider_adhesion = None
self.collider_matrix = wps.bsr_zeros(0, 0, block_type=float)
self.transposed_collider_matrix = wps.bsr_zeros(0, 0, block_type=float)
self.strain_node_particle_volume = None
self.strain_node_volume = None
self.strain_node_collider_volume = None
self.collider_total_volumes = None
self.collider_node_volume = None
def rebuild_function_spaces(
self,
pic: fem.PicQuadrature,
velocity_basis_str: str,
strain_basis_str: str,
collider_basis_str: str,
max_cell_count: int,
temporary_store: fem.TemporaryStore,
):
"""Define velocity and strain function spaces over the given geometry."""
self.domain = pic.domain
use_pic_collider_basis = collider_basis_str[:3] == "pic"
use_pic_strain_basis = strain_basis_str[:3] == "pic"
if self.domain.geometry is not self.grid:
self.grid = self.domain.geometry
# Define function spaces: linear (Q1) for velocity and volume fraction,
# zero or first order for pressure
self._velocity_basis = _make_grid_basis_space(self.grid, velocity_basis_str)
if not use_pic_strain_basis:
self._strain_basis = _make_grid_basis_space(self.grid, strain_basis_str)
if not use_pic_collider_basis:
self._collision_basis = _make_grid_basis_space(
self.grid, collider_basis_str, family=fem.Polynomial.EQUISPACED_CLOSED
)
# Point-based basis space needs to be rebuilt even when the geo does not change
if use_pic_strain_basis:
self._strain_basis = _make_pic_basis_space(pic, strain_basis_str)
if use_pic_collider_basis:
self._collision_basis = _make_pic_basis_space(pic, collider_basis_str)
self._create_velocity_function_space(temporary_store, max_cell_count)
self._create_collider_function_space(temporary_store, max_cell_count)
self._create_strain_function_space(temporary_store, max_cell_count)
def _create_velocity_function_space(self, temporary_store: fem.TemporaryStore, max_cell_count: int = -1):
"""Create velocity and fraction spaces and their partition/restriction."""
domain = self.domain
velocity_space = fem.make_collocated_function_space(self._velocity_basis, dtype=wp.vec3)
# overly conservative
max_vel_node_count = (
velocity_space.topology.MAX_NODES_PER_ELEMENT * max_cell_count if max_cell_count >= 0 else -1
)
vel_space_partition = fem.make_space_partition(
space_topology=velocity_space.topology,
geometry_partition=domain.geometry_partition,
with_halo=False,
max_node_count=max_vel_node_count,
temporary_store=temporary_store,
)
vel_space_restriction = fem.make_space_restriction(
space_partition=vel_space_partition, domain=domain, temporary_store=temporary_store
)
self._velocity_space = velocity_space
self._vel_space_restriction = vel_space_restriction
def _create_collider_function_space(self, temporary_store: fem.TemporaryStore, max_cell_count: int = -1):
"""Create collider function space and its partition/restriction."""
if self._velocity_basis == self._collision_basis:
self._collision_space = self._velocity_space
self._collision_space_restriction = self._vel_space_restriction
return
domain = self.domain
collision_space = fem.make_collocated_function_space(self._collision_basis, dtype=wp.vec3)
if isinstance(collision_space.basis, fem.PointBasisSpace):
max_collision_node_count = collision_space.node_count()
else:
# overly conservative
max_collision_node_count = (
collision_space.topology.MAX_NODES_PER_ELEMENT * domain.element_count() if max_cell_count >= 0 else -1
)
collision_space_partition = fem.make_space_partition(
space_topology=collision_space.topology,
geometry_partition=domain.geometry_partition,
with_halo=False,
max_node_count=max_collision_node_count,
temporary_store=temporary_store,
)
collision_space_restriction = fem.make_space_restriction(
space_partition=collision_space_partition, domain=domain, temporary_store=temporary_store
)
self._collision_space = collision_space
self._collision_space_restriction = collision_space_restriction
def _create_strain_function_space(self, temporary_store: fem.TemporaryStore, max_cell_count: int = -1):
"""Create symmetric strain space (P0 or Q1) and its partition/restriction."""
domain = self.domain
sym_strain_space = fem.make_collocated_function_space(
self._strain_basis,
dof_mapper=fem.SymmetricTensorMapper(dtype=wp.mat33, mapping=fem.SymmetricTensorMapper.Mapping.DB16),
)
max_strain_node_count = (
sym_strain_space.topology.MAX_NODES_PER_ELEMENT * max_cell_count if max_cell_count >= 0 else -1
)
strain_space_partition = fem.make_space_partition(
space_topology=sym_strain_space.topology,
geometry_partition=domain.geometry_partition,
with_halo=False,
max_node_count=max_strain_node_count,
temporary_store=temporary_store,
)
strain_space_restriction = fem.make_space_restriction(
space_partition=strain_space_partition, domain=domain, temporary_store=temporary_store
)
self._sym_strain_space = sym_strain_space
self._strain_space_restriction = strain_space_restriction
def require_velocity_space_fields(self, has_compliant_particles: bool):
velocity_basis = self._velocity_basis
velocity_space = self._velocity_space
vel_space_restriction = self._vel_space_restriction
domain = vel_space_restriction.domain
vel_space_partition = vel_space_restriction.space_partition
if (
self.velocity_test is not None
and self.velocity_test.space_restriction.space_partition == vel_space_partition
):
return
fraction_space = fem.make_collocated_function_space(velocity_basis, dtype=float)
# test, trial and discrete fields
if self.velocity_test is None:
self.velocity_test = fem.make_test(velocity_space, domain=domain, space_restriction=vel_space_restriction)
self.fraction_test = fem.make_test(fraction_space, space_restriction=vel_space_restriction)
self.velocity_trial = fem.make_trial(velocity_space, domain=domain, space_partition=vel_space_partition)
self.fraction_trial = fem.make_trial(fraction_space, domain=domain, space_partition=vel_space_partition)
self.fraction_field = fem.make_discrete_field(fraction_space, space_partition=vel_space_partition)
else:
self.velocity_test.rebind(velocity_space, vel_space_restriction)
self.fraction_test.rebind(fraction_space, vel_space_restriction)
self.velocity_trial.rebind(velocity_space, vel_space_partition, domain)
self.fraction_trial.rebind(fraction_space, vel_space_partition, domain)
self.fraction_field.rebind(fraction_space, vel_space_partition)
if has_compliant_particles:
elastic_parameters_space = fem.make_collocated_function_space(velocity_basis, dtype=wp.vec3)
if self.elastic_parameters_field is None:
self.elastic_parameters_field = elastic_parameters_space.make_field(space_partition=vel_space_partition)
else:
self.elastic_parameters_field.rebind(elastic_parameters_space, vel_space_partition)
self.velocity_field = velocity_space.make_field(space_partition=vel_space_partition)
def require_collision_space_fields(self):
collision_basis = self._collision_basis
collision_space = self._collision_space
collision_space_restriction = self._collision_space_restriction
domain = collision_space_restriction.domain
collision_space_partition = collision_space_restriction.space_partition
if (
self.collider_fraction_test is not None
and self.collider_fraction_test.space_restriction.space_partition == collision_space_partition
):
return
collider_fraction_space = fem.make_collocated_function_space(collision_basis, dtype=float)
# test, trial and discrete fields
if self.collider_fraction_test is None:
self.collider_fraction_test = fem.make_test(
collider_fraction_space, space_restriction=collision_space_restriction
)
self.collider_distance_field = collider_fraction_space.make_field(space_partition=collision_space_partition)
self.collider_velocity_field = collision_space.make_field(space_partition=collision_space_partition)
self.collider_normal_field = collision_space.make_field(space_partition=collision_space_partition)
self.background_impulse_field = fem.UniformField(domain, wp.vec3(0.0))
else:
self.collider_fraction_test.rebind(collider_fraction_space, collision_space_restriction)
self.collider_distance_field.rebind(collider_fraction_space, collision_space_partition)
self.collider_velocity_field.rebind(collision_space, collision_space_partition)
self.collider_normal_field.rebind(collision_space, collision_space_partition)
self.background_impulse_field.domain = domain
self.impulse_field = collision_space.make_field(space_partition=collision_space_partition)
self.collider_position_field = collision_space.make_field(space_partition=collision_space_partition)
self.collider_ids = wp.empty(collision_space_partition.node_count(), dtype=int)
def require_strain_space_fields(self):
"""Ensure strain-space fields exist and match current spaces."""
strain_basis = self._strain_basis
sym_strain_space = self._sym_strain_space
strain_space_restriction = self._strain_space_restriction
domain = strain_space_restriction.domain
strain_space_partition = strain_space_restriction.space_partition
if (
self.sym_strain_test is not None
and self.sym_strain_test.space_restriction.space_partition == strain_space_partition
):
return
divergence_space = fem.make_collocated_function_space(strain_basis, dtype=float)
strain_yield_parameters_space = fem.make_collocated_function_space(strain_basis, dtype=YieldParamVec)
if self.sym_strain_test is None:
self.sym_strain_test = fem.make_test(sym_strain_space, space_restriction=strain_space_restriction)
self.divergence_test = fem.make_test(divergence_space, space_restriction=strain_space_restriction)
self.strain_yield_parameters_test = fem.make_test(
strain_yield_parameters_space, space_restriction=strain_space_restriction
)
self.sym_strain_trial = fem.make_trial(
sym_strain_space, domain=domain, space_partition=strain_space_partition
)
self.divergence_trial = fem.make_trial(
divergence_space, domain=domain, space_partition=strain_space_partition
)
self.elastic_strain_delta_field = sym_strain_space.make_field(space_partition=strain_space_partition)
self.plastic_strain_delta_field = sym_strain_space.make_field(space_partition=strain_space_partition)
self.strain_yield_parameters_field = strain_yield_parameters_space.make_field(
space_partition=strain_space_partition
)
self.background_stress_field = fem.UniformField(domain, wp.mat33(0.0))
else:
self.sym_strain_test.rebind(sym_strain_space, strain_space_restriction)
self.divergence_test.rebind(divergence_space, strain_space_restriction)
self.strain_yield_parameters_test.rebind(strain_yield_parameters_space, strain_space_restriction)
self.sym_strain_trial.rebind(sym_strain_space, strain_space_partition, domain)
self.divergence_trial.rebind(divergence_space, strain_space_partition, domain)
self.elastic_strain_delta_field.rebind(sym_strain_space, strain_space_partition)
self.plastic_strain_delta_field.rebind(sym_strain_space, strain_space_partition)
self.strain_yield_parameters_field.rebind(strain_yield_parameters_space, strain_space_partition)
self.background_stress_field.domain = domain
self.stress_field = sym_strain_space.make_field(space_partition=strain_space_partition)
@property
def collider_node_count(self) -> int:
return self._collision_space_restriction.space_partition.node_count()
@property
def velocity_node_count(self) -> int:
return self._vel_space_restriction.space_partition.node_count()
@property
def velocity_nodes_per_element(self) -> int:
return self._vel_space_restriction.space_partition.space_topology.MAX_NODES_PER_ELEMENT
@property
def strain_node_count(self) -> int:
return self._strain_space_restriction.space_partition.node_count()
@property
def strain_nodes_per_element(self) -> int:
return self._strain_space_restriction.space_partition.space_topology.MAX_NODES_PER_ELEMENT
def allocate_temporaries(
self,
collider_count: int,
has_compliant_bodies: bool,
has_critical_fraction: bool,
max_colors: int,
temporary_store: fem.TemporaryStore,
):
"""Allocate transient arrays sized to current grid and options."""
vel_node_count = self.velocity_node_count
collider_node_count = self.collider_node_count
strain_node_count = self.strain_node_count
self.inv_mass_matrix = fem.borrow_temporary(temporary_store, shape=(vel_node_count,), dtype=float)
self.collider_velocity = fem.borrow_temporary(temporary_store, shape=(collider_node_count,), dtype=wp.vec3)
self.collider_friction = fem.borrow_temporary(temporary_store, shape=(collider_node_count,), dtype=float)
self.collider_adhesion = fem.borrow_temporary(temporary_store, shape=(collider_node_count,), dtype=float)
self.collider_node_volume = fem.borrow_temporary(temporary_store, shape=collider_node_count, dtype=float)
self.strain_node_particle_volume = fem.borrow_temporary(temporary_store, shape=strain_node_count, dtype=float)
self.unilateral_strain_offset = fem.borrow_temporary(temporary_store, shape=strain_node_count, dtype=float)
wps.bsr_set_zero(self.strain_matrix, rows_of_blocks=strain_node_count, cols_of_blocks=vel_node_count)
wps.bsr_set_zero(self.compliance_matrix, rows_of_blocks=strain_node_count, cols_of_blocks=strain_node_count)
if has_critical_fraction:
self.strain_node_volume = fem.borrow_temporary(temporary_store, shape=strain_node_count, dtype=float)
self.strain_node_collider_volume = fem.borrow_temporary(
temporary_store, shape=strain_node_count, dtype=float
)
if has_compliant_bodies:
self.collider_total_volumes = fem.borrow_temporary(temporary_store, shape=collider_count, dtype=float)
if max_colors > 0:
self.color_indices = fem.borrow_temporary(temporary_store, shape=(2, strain_node_count), dtype=int)
self.color_offsets = fem.borrow_temporary(temporary_store, shape=max_colors + 1, dtype=int)
def release_temporaries(self):
"""Release previously allocated temporaries to the store."""
self.inv_mass_matrix.release()
self.collider_velocity.release()
self.collider_friction.release()
self.collider_adhesion.release()
self.collider_node_volume.release()
self.strain_node_particle_volume.release()
self.unilateral_strain_offset.release()
if self.strain_node_volume is not None:
self.strain_node_volume.release()
self.strain_node_collider_volume.release()
if self.collider_total_volumes is not None:
self.collider_total_volumes.release()
if self.color_indices is not None:
self.color_indices.release()
self.color_offsets.release()
class LastStepData:
"""Persistent solver state preserved across time steps.
Separate from ImplicitMPMScratchpad which is rebuilt when the grid changes.
Stores warmstart fields for the iterative solver and previous body transforms
for finite-difference velocity computation.
"""
def __init__(self):
self.ws_impulse_field = None # Warmstart for collision impulses
self.ws_stress_field = None # Warmstart for stress field
self.body_q_prev = None # Previous body transforms for finite-difference velocities
def _ws_stress_space(self, scratch: ImplicitMPMScratchpad, smoothed: bool):
sym_strain_space = scratch.sym_strain_test.space
if isinstance(sym_strain_space.basis, fem.PointBasisSpace) or not smoothed:
return sym_strain_space
else:
return fem.make_polynomial_space(scratch.grid, degree=1, dof_mapper=sym_strain_space.dof_mapper)
def require_strain_space_fields(self, scratch: ImplicitMPMScratchpad, smoothed: bool):
"""Ensure strain-space fields exist and match current spaces."""
if self.ws_stress_field is None:
self.ws_stress_field = self._ws_stress_space(scratch, smoothed).make_field()
def rebind_strain_space_fields(self, scratch: ImplicitMPMScratchpad, smoothed: bool):
if self.ws_stress_field.geometry != scratch.sym_strain_test.space.geometry:
ws_stress_space = self._ws_stress_space(scratch, smoothed)
self.ws_stress_field.rebind(
space=ws_stress_space,
space_partition=fem.make_space_partition(
space_topology=ws_stress_space.topology, geometry_partition=None
),
)
def require_collision_space_fields(self, scratch: ImplicitMPMScratchpad):
"""Ensure collision-space fields exist and match current spaces."""
if self.ws_impulse_field is None:
self.ws_impulse_field = scratch.impulse_field.space.make_field()
def rebind_collision_space_fields(self, scratch: ImplicitMPMScratchpad):
if self.ws_impulse_field.geometry != scratch.impulse_field.space.geometry:
self.ws_impulse_field.rebind(
space=scratch.impulse_field.space,
space_partition=fem.make_space_partition(
space_topology=scratch.impulse_field.space.topology,
geometry_partition=None,
),
)
def require_collider_previous_position(self, collider_body_q: wp.array | None):
if collider_body_q is None:
self.body_q_prev = None
elif self.body_q_prev is None or self.body_q_prev.shape != collider_body_q.shape:
self.body_q_prev = wp.clone(collider_body_q)
def save_collider_current_position(self, collider_body_q: wp.array | None):
self.require_collider_previous_position(collider_body_q)
if collider_body_q is not None:
self.body_q_prev.assign(collider_body_q)
class SolverImplicitMPM(SolverBase):
"""Implicit MPM solver for granular and elasto-plastic materials.
Implements an implicit Material Point Method (MPM) algorithm roughly
following [1], extended with a GPU-friendly rheology solver supporting
pressure-dependent yield (Drucker-Prager), viscosity, dilatancy, and
isotropic hardening/softening.
This variant is particularly well-suited for very stiff materials and
the fully inelastic limit. It is less versatile than traditional explicit
MPM but offers unconditional stability with respect to the time step.
Call :meth:`register_custom_attributes` on your :class:`~newton.ModelBuilder`
before building the model to enable the MPM-specific per-particle material
parameters and state variables (e.g. ``mpm:young_modulus``,
``mpm:friction``, ``mpm:particle_elastic_strain``).
[1] https://doi.org/10.1145/2897824.2925877
Args:
model: The model to simulate.
config: Solver configuration. See :class:`SolverImplicitMPM.Config`.
temporary_store: Optional Warp FEM temporary store for reusing scratch
allocations across steps.
verbose: Enable verbose solver output. Defaults to ``wp.config.verbose``.
enable_timers: Enable per-section wall-clock timings.
"""
@dataclass
class Config:
"""Configuration for :class:`SolverImplicitMPM`.
Per-particle properties can be configured using custom attributes on the Model.
See :meth:`SolverImplicitMPM.register_custom_attributes` for details.
"""
# numerics
max_iterations: int = 250
"""Maximum number of iterations for the rheology solver."""
tolerance: float = 1.0e-4
"""Tolerance for the rheology solver."""
solver: Literal["gauss-seidel", "jacobi", "cg"] = "gauss-seidel"
"""Solver to use for the rheology solver."""
warmstart_mode: Literal["none", "auto", "particles", "grid", "smoothed"] = "auto"
"""Warmstart mode to use for the rheology solver."""
collider_velocity_mode: Literal["forward", "backward", "instantaneous", "finite_difference"] = "forward"
"""Collider velocity computation mode. ``'forward'`` uses the current velocity,
``'backward'`` uses the previous timestep position.
.. deprecated::
Aliases ``'instantaneous'`` (= ``'forward'``) and ``'finite_difference'``
(= ``'backward'``) are deprecated and will be removed in a future release.
"""
# grid
voxel_size: float = 0.1
"""Size of the grid voxels."""
grid_type: Literal["sparse", "dense", "fixed"] = "sparse"
"""Type of grid to use."""
grid_padding: int = 0
"""Number of empty cells to add around particles when allocating the grid."""
max_active_cell_count: int = -1
"""Maximum number of active cells to use for active subsets of dense grids. -1 means unlimited."""
transfer_scheme: Literal["apic", "pic"] = "apic"
"""Transfer scheme to use for particle-grid transfers."""
integration_scheme: Literal["pic", "gimp"] = "pic"
"""Integration scheme controlling shape-function support."""
# material / background
critical_fraction: float = 0.0
"""Fraction for particles under which the yield surface collapses."""
air_drag: float = 1.0
"""Numerical drag for the background air."""
# experimental
collider_normal_from_sdf_gradient: bool = False
"""Compute collider normals from sdf gradient rather than closest point"""
collider_basis: str = "Q1"
"""Collider basis function string. Examples: P0 (piecewise constant), Q1 (trilinear), S2 (quadratic serendipity), pic8 (particle-based with max 8 points per cell)"""
strain_basis: str = "P0"
"""Strain basis functions. May be one of P0, P1d, Q1, Q1d, or pic[n]."""
velocity_basis: str = "Q1"
"""Velocity basis function string. Examples: B2 (quadratic b-spline), Q1 (trilinear)"""
@classmethod
def register_custom_attributes(cls, builder: newton.ModelBuilder) -> None:
"""Register MPM-specific custom attributes in the 'mpm' namespace.
This method registers per-particle material parameters and state variables
for the implicit MPM solver.
Attributes registered on Model (per-particle):
- ``mpm:young_modulus``: Young's modulus in Pa
- ``mpm:poisson_ratio``: Poisson's ratio for elasticity
- ``mpm:damping``: Elastic damping relaxation time in seconds
- ``mpm:friction``: Friction coefficient
- ``mpm:yield_pressure``: Yield pressure in Pa
- ``mpm:tensile_yield_ratio``: Tensile yield ratio
- ``mpm:yield_stress``: Deviatoric yield stress in Pa
- ``mpm:hardening``: Hardening factor for plasticity
- ``mpm:hardening_rate``: Hardening rate for plasticity
- ``mpm:softening_rate``: Softening rate for plasticity
- ``mpm:dilatancy``: Dilatancy factor for plasticity
- ``mpm:viscosity``: Viscosity for plasticity [Pa·s]
Attributes registered on State (per-particle):
- ``mpm:particle_qd_grad``: Velocity gradient for APIC transfer
- ``mpm:particle_elastic_strain``: Elastic deformation gradient
- ``mpm:particle_Jp``: Determinant of plastic deformation gradient
- ``mpm:particle_stress``: Cauchy stress tensor [Pa]
- ``mpm:particle_transform``: Overall deformation gradient for rendering
"""
# Per-particle material parameters
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="young_modulus",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=1.0e15,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="poisson_ratio",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.3,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="damping",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="hardening",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="friction",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.5,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="yield_pressure",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=1.0e15,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="tensile_yield_ratio",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="yield_stress",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="hardening_rate",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=1.0,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="softening_rate",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=1.0,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="dilatancy",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="viscosity",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mpm",
)
)
# Per-particle state attributes (attached to State objects)
identity = wp.mat33(np.eye(3))
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="particle_qd_grad",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.STATE,
dtype=wp.mat33,
default=wp.mat33(0.0),
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="particle_elastic_strain",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.STATE,
dtype=wp.mat33,
default=identity,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="particle_Jp",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.STATE,
dtype=wp.float32,
default=1.0,
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="particle_stress",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.STATE,
dtype=wp.mat33,
default=wp.mat33(0.0),
namespace="mpm",
)
)
builder.add_custom_attribute(
newton.ModelBuilder.CustomAttribute(
name="particle_transform",
frequency=newton.Model.AttributeFrequency.PARTICLE,
assignment=newton.Model.AttributeAssignment.STATE,
dtype=wp.mat33,
default=identity,
namespace="mpm",
)
)
def __init__(
self,
model: newton.Model,
config: Config,
temporary_store: fem.TemporaryStore | None = None,
verbose: bool | None = None,
enable_timers: bool = False,
):
super().__init__(model)
self._mpm_model = ImplicitMPMModel(model, config)
self.max_iterations = config.max_iterations
self.tolerance = float(config.tolerance)
self.temporary_store = temporary_store
self.verbose = verbose if verbose is not None else wp.config.verbose
self.enable_timers = enable_timers
self.velocity_basis = "Q1"
self.strain_basis = config.strain_basis
self.velocity_basis = config.velocity_basis
self.grid_padding = config.grid_padding
self.grid_type = config.grid_type
self.solver = config.solver
self.coloring = "gauss-seidel" in self.solver
self.apic = config.transfer_scheme == "apic"
self.gimp = config.integration_scheme == "gimp"
self.max_active_cell_count = config.max_active_cell_count
self.collider_normal_from_sdf_gradient = config.collider_normal_from_sdf_gradient
self.collider_basis = config.collider_basis
# Map deprecated aliases to canonical values
if config.collider_velocity_mode == "finite_difference":
warnings.warn(
"collider_velocity_mode='finite_difference' is deprecated, use 'backward' instead.",
DeprecationWarning,
stacklevel=2,
)
self.collider_velocity_mode = "backward"
elif config.collider_velocity_mode == "instantaneous":
warnings.warn(
"collider_velocity_mode='instantaneous' is deprecated, use 'forward' instead.",
DeprecationWarning,
stacklevel=2,
)
self.collider_velocity_mode = "forward"
else:
if config.collider_velocity_mode not in ("forward", "backward"):
raise ValueError(f"Invalid collider velocity mode: {config.collider_velocity_mode}")
self.collider_velocity_mode = config.collider_velocity_mode
if config.warmstart_mode == "none":
self._stress_warmstart = ""
elif config.warmstart_mode == "auto":
if self.strain_basis in ("P1d", "Q1d"):
self._stress_warmstart = "particles"
else:
self._stress_warmstart = "grid"
else:
if config.warmstart_mode not in ("particles", "grid", "smoothed"):
raise ValueError(f"Invalid warmstart mode: {config.warmstart_mode}")
self._stress_warmstart = config.warmstart_mode
self._use_cuda_graph = self.model.device.is_cuda and wp.is_conditional_graph_supported()
self._timers_use_nvtx = False
# Pre-allocate scratchpad and last step data so that step() can be graph-captured
self._scratchpad = None
self._last_step_data = LastStepData()
with wp.ScopedDevice(model.device):
pic = self._particles_to_cells(model.particle_q)
self._rebuild_scratchpad(pic)
self._require_velocity_space_fields(self._scratchpad, self._mpm_model.has_compliant_particles)
self._require_collision_space_fields(self._scratchpad, self._last_step_data)
self._require_strain_space_fields(self._scratchpad, self._last_step_data)
self._velocity_nodes_per_strain_sample = (
self._scratchpad.velocity_nodes_per_element
if self.strain_basis != "Q1" and self.velocity_basis == "Q1"
else -1
)
def setup_collider(
self,
collider_meshes: list[wp.Mesh] | None = None,
collider_body_ids: list[int] | None = None,
collider_margins: list[float] | None = None,
collider_friction: list[float] | None = None,
collider_adhesion: list[float] | None = None,
collider_projection_threshold: list[float] | None = None,
model: newton.Model | None = None,
body_com: wp.array | None = None,
body_mass: wp.array | None = None,
body_inv_inertia: wp.array | None = None,
body_q: wp.array | None = None,
) -> None:
"""Configure collider geometry and material properties.
By default, collisions are set up against all shapes in the model with
``newton.ShapeFlags.COLLIDE_PARTICLES``. Use this method to customize
collider sources, materials, or to read colliders from a different model.
Args:
collider_meshes: Warp triangular meshes used as colliders.
collider_body_ids: For dynamic colliders, per-mesh body ids.
collider_margins: Per-mesh signed distance offsets (m).
collider_friction: Per-mesh Coulomb friction coefficients.
collider_adhesion: Per-mesh adhesion (Pa).
collider_projection_threshold: Per-mesh projection threshold (m).
model: The model to read collider properties from. Default to solver's model.
body_com: For dynamic colliders, per-body center of mass.
body_mass: For dynamic colliders, per-body mass. Pass zeros for kinematic bodies.
body_inv_inertia: For dynamic colliders, per-body inverse inertia.
body_q: For dynamic colliders, per-body initial transform.
"""
self._mpm_model.setup_collider(
collider_meshes=collider_meshes,
collider_body_ids=collider_body_ids,
collider_thicknesses=collider_margins,
collider_friction=collider_friction,
collider_adhesion=collider_adhesion,
collider_projection_threshold=collider_projection_threshold,
model=model,
body_com=body_com,
body_mass=body_mass,
body_inv_inertia=body_inv_inertia,
body_q=body_q,
)
self._last_step_data.save_collider_current_position(self._mpm_model.collider_body_q)
@property
def voxel_size(self) -> float:
"""Grid voxel size used by the solver."""
return self._mpm_model.voxel_size
@override
def step(
self,
state_in: newton.State,
state_out: newton.State,
control: newton.Control,
contacts: newton.Contacts,
dt: float,
) -> None:
"""Advance the simulation by one time step.
Transfers particle data to the grid, solves the implicit rheology
system, and transfers the result back to update particle positions,
velocities, and stress.
Args:
state_in: Input state at the start of the step.
state_out: Output state written with updated particle data.
May be the same object as ``state_in`` for in-place stepping.
control: Control input (unused; material parameters come from the model).
contacts: Contact information (unused; collisions are handled internally).
dt: Time step duration [s].
"""
model = self.model
with wp.ScopedDevice(model.device):
pic = self._particles_to_cells(state_in.particle_q)
scratch = self._rebuild_scratchpad(pic)
self._step_impl(state_in, state_out, dt, pic, scratch)
scratch.release_temporaries()
@override
def notify_model_changed(self, flags: int) -> None:
self._mpm_model.notify_particle_material_changed()
def collect_collider_impulses(self, state: newton.State) -> tuple[wp.array, wp.array, wp.array]:
"""Collect current collider impulses and their application positions.
Returns a tuple of 3 arrays:
- Impulse values in world units.
- Collider positions in world units.
- Collider id, that can be mapped back to the model's body ids using the ``collider_body_index`` property.
"""
# Not stepped yet, read from preallocated scratchpad
if not hasattr(state, "impulse_field"):
state = self._scratchpad
cell_volume = self._mpm_model.voxel_size**3
return (
-cell_volume * state.impulse_field.dof_values,
state.collider_position_field.dof_values,
state.collider_ids,
)
@property
def collider_body_index(self) -> wp.array:
"""Array mapping collider indices to body indices.
Returns:
Per-collider body index array. Value is -1 for colliders that are not bodies.
"""
return self._mpm_model.collider.collider_body_index
def project_outside(self, state_in: newton.State, state_out: newton.State, dt: float, gap: float | None = None):
"""Project particles outside of colliders, and adjust their velocity and velocity gradients
Args:
state_in: The input state.
state_out: The output state. Only particle_q, particle_qd, and particle_qd_grad are written.
dt: The time step, for extrapolating the collider end-of-step positions from its current position and velocity.
gap: Maximum distance for closest-point queries. If None, the default is the voxel size times sqrt(3).
"""
if gap is not None:
# Update max query dist if provided
prev_gap, self._mpm_model.collider.query_max_dist = self._mpm_model.collider.query_max_dist, gap
self._last_step_data.require_collider_previous_position(state_in.body_q)
wp.launch(
project_outside_collider,
dim=state_in.particle_count,
inputs=[
state_in.particle_q,
state_in.particle_qd,
state_in.mpm.particle_qd_grad,
self.model.particle_flags,
self.model.particle_mass,
self._mpm_model.collider,
state_in.body_q,
state_in.body_qd if self.collider_velocity_mode == "forward" else None,
self._last_step_data.body_q_prev if self.collider_velocity_mode == "backward" else None,
dt,
],
outputs=[
state_out.particle_q,
state_out.particle_qd,
state_out.mpm.particle_qd_grad,
],
device=state_in.particle_q.device,
)
if gap is not None:
# Restore previous max query dist
self._mpm_model.collider.query_max_dist = prev_gap
def update_particle_frames(
self,
state_prev: newton.State,
state: newton.State,
dt: float,
min_stretch: float = 0.25,
max_stretch: float = 2.0,
) -> None:
"""Update per-particle deformation frames for rendering and projection.
Integrates the particle deformation gradient using the velocity gradient
and clamps its principal stretches to the provided bounds for
robustness.
"""
wp.launch(
update_particle_frames,
dim=state.particle_count,
inputs=[
dt,
min_stretch,
max_stretch,
state.mpm.particle_qd_grad,
state_prev.mpm.particle_transform,
state.mpm.particle_transform,
],
device=state.mpm.particle_qd_grad.device,
)
def sample_render_grains(self, state: newton.State, grains_per_particle: int) -> wp.array:
"""Generate per-particle point samples used for high-resolution rendering.
Args:
state: Current Newton state providing particle positions.
grains_per_particle: Number of grains to sample per particle.
Returns:
A ``wp.array`` with shape ``(num_particles, grains_per_particle)`` of
type ``wp.vec3`` containing grain positions.
"""
return sample_render_grains(state, self._mpm_model.particle_radius, grains_per_particle)
def update_render_grains(
self,
state_prev: newton.State,
state: newton.State,
grains: wp.array,
dt: float,
) -> None:
"""Advect grain samples with the grid velocity and keep them inside the deformed particle.
Args:
state_prev: Previous state (t_n).
state: Current state (t_{n+1}).
grains: 2D array of grain positions per particle to be updated in place. See ``sample_render_grains``.
dt: Time step duration.
"""
return update_render_grains(state_prev, state, grains, self._mpm_model.particle_radius, dt)
def _allocate_grid(
self,
positions: wp.array,
particle_flags: wp.array,
voxel_size: float,
temporary_store: fem.TemporaryStore,
padding_voxels: int = 0,
):
"""Create a grid (sparse or dense) covering all particle positions.
Uses a sparse ``Nanogrid`` when requested; otherwise computes an axis
aligned bounding box and instantiates a dense ``Grid3D`` with optional
padding in voxel units.
Args:
positions: Particle positions to bound.
particle_flags: Per-particle flags; inactive particles are excluded from bounds.
voxel_size: Grid voxel edge length.
temporary_store: Temporary storage for intermediate buffers.
padding_voxels: Additional empty voxels to add around the bounds.
Returns:
A geometry partition suitable for FEM field assembly.
"""
with self._timer("Allocate grid"):
if self.grid_type == "sparse":
volume = allocate_by_voxels(positions, voxel_size, padding_voxels=padding_voxels)
grid = fem.Nanogrid(volume, temporary_store=temporary_store)
else:
# Compute bounds and transfer to host
device = positions.device
if device.is_cuda:
min_dev = fem.borrow_temporary(temporary_store, shape=1, dtype=wp.vec3, device=device)
max_dev = fem.borrow_temporary(temporary_store, shape=1, dtype=wp.vec3, device=device)
min_dev.fill_(wp.vec3(INFINITY))
max_dev.fill_(wp.vec3(-INFINITY))
tile_size = 256
wp.launch(
compute_bounds,
dim=((positions.shape[0] + tile_size - 1) // tile_size, tile_size),
block_dim=tile_size,
inputs=[positions, particle_flags, min_dev, max_dev],
device=device,
)
min_host = fem.borrow_temporary(
temporary_store, shape=1, dtype=wp.vec3, device="cpu", pinned=device.is_cuda
)
max_host = fem.borrow_temporary(
temporary_store, shape=1, dtype=wp.vec3, device="cpu", pinned=device.is_cuda
)
wp.copy(src=min_dev, dest=min_host)
wp.copy(src=max_dev, dest=max_host)
wp.synchronize_stream()
bbox_min, bbox_max = min_host.numpy(), max_host.numpy()
else:
bbox_min, bbox_max = np.min(positions.numpy(), axis=0), np.max(positions.numpy(), axis=0)
# Round to nearest voxel
grid_min = np.floor(bbox_min / voxel_size) - padding_voxels
grid_max = np.ceil(bbox_max / voxel_size) + padding_voxels
grid = fem.Grid3D(
bounds_lo=wp.vec3(grid_min * voxel_size),
bounds_hi=wp.vec3(grid_max * voxel_size),
res=wp.vec3i((grid_max - grid_min).astype(int)),
)
return grid
def _create_geometry_partition(
self, grid: fem.Geometry, positions: wp.array, particle_flags: wp.array, max_cell_count: int
):
"""Create a geometry partition for the given positions."""
active_cells = fem.borrow_temporary(self.temporary_store, shape=grid.cell_count(), dtype=int)
active_cells.zero_()
fem.interpolate(
mark_active_cells,
dim=positions.shape[0],
at=fem.Cells(grid),
values={
"positions": positions,
"particle_flags": particle_flags,
"active_cells": active_cells,
},
temporary_store=self.temporary_store,
)
partition = fem.ExplicitGeometryPartition(
grid,
cell_mask=active_cells,
max_cell_count=max_cell_count,
max_side_count=0,
temporary_store=self.temporary_store,
)
active_cells.release()
return partition
def _rebuild_scratchpad(self, pic: fem.PicQuadrature):
"""(Re)create function spaces and allocate per-step temporaries.
Allocates the grid based on current particle positions, rebuilds
velocity and strain spaces as needed, configures collision data, and
optionally computes a Gauss-Seidel coloring for the strain nodes.
"""
if self._scratchpad is None:
self._scratchpad = ImplicitMPMScratchpad()
scratch = self._scratchpad
with self._timer("Scratchpad"):
scratch.rebuild_function_spaces(
pic,
strain_basis_str=self.strain_basis,
velocity_basis_str=self.velocity_basis,
collider_basis_str=self.collider_basis,
max_cell_count=self.max_active_cell_count,
temporary_store=self.temporary_store,
)
scratch.allocate_temporaries(
collider_count=self._mpm_model.collider.collider_mesh.shape[0],
has_compliant_bodies=self._mpm_model.has_compliant_colliders,
has_critical_fraction=self._mpm_model.critical_fraction > 0.0,
max_colors=self._max_colors(),
temporary_store=self.temporary_store,
)
if self.coloring:
self._compute_coloring(pic, scratch=scratch)
return scratch
def _particles_to_cells(self, positions: wp.array) -> fem.PicQuadrature:
"""Rebuild the grid and grid partition around particles, then assign particles to grid cells."""
# Rebuild grid
if self._scratchpad is not None and self.grid_type == "fixed":
grid = self._scratchpad.grid
else:
grid = self._allocate_grid(
positions,
self.model.particle_flags,
voxel_size=self._mpm_model.voxel_size,
temporary_store=self.temporary_store,
padding_voxels=self.grid_padding,
)
# Build active partition
with self._timer("Build active partition"):
if self.grid_type == "sparse":
max_cell_count = -1
geo_partition = grid
else:
max_cell_count = self.max_active_cell_count
geo_partition = self._create_geometry_partition(
grid, positions, self.model.particle_flags, max_cell_count
)
# Bin particles to grid cells
with self._timer("Bin particles"):
domain = fem.Cells(geo_partition)
if self.gimp:
particle_locations = self._particle_grid_locations_gimp(
domain, positions, self._mpm_model.particle_radius
)
else:
particle_locations = self._particle_grid_locations(domain, positions)
pic = fem.PicQuadrature(
domain=domain,
positions=particle_locations,
measures=self._mpm_model.particle_volume,
temporary_store=self.temporary_store,
use_domain_element_indices=True,
)
return pic
def _particle_grid_locations(self, domain: fem.GeometryDomain, positions: wp.array) -> wp.array:
"""Convert particle positions to grid locations."""
cell_lookup = domain.element_partition_lookup
@fem.cache.dynamic_kernel(suffix=domain.name)
def particle_locations(
cell_arg_value: domain.ElementArg,
domain_index_arg_value: domain.ElementIndexArg,
positions: wp.array[wp.vec3],
cell_index: wp.array[fem.ElementIndex],
cell_coords: wp.array[fem.Coords],
):
p = wp.tid()
domain_arg = domain.DomainArg(cell_arg_value, domain_index_arg_value)
sample = cell_lookup(domain_arg, positions[p])
cell_index[p] = domain.element_partition_index(domain_index_arg_value, sample.element_index)
cell_coords[p] = sample.element_coords
device = positions.device
cell_indices = fem.borrow_temporary(self.temporary_store, shape=positions.shape[0], dtype=fem.ElementIndex)
cell_coords = fem.borrow_temporary(self.temporary_store, shape=positions.shape[0], dtype=fem.Coords)
wp.launch(
particle_locations,
dim=positions.shape[0],
inputs=[
domain.element_arg_value(device=device),
domain.element_index_arg_value(device=device),
positions,
cell_indices,
cell_coords,
],
device=device,
)
return cell_indices, cell_coords
def _particle_grid_locations_gimp(
self, domain: fem.GeometryDomain, positions: wp.array, radii: wp.array
) -> wp.array:
"""Convert particle positions to grid locations."""
cell_lookup = domain.element_partition_lookup
cell_closest_point = domain.element_closest_point
@wp.func
def add_cell(
particle_cell_indices: wp.array[fem.ElementIndex],
particle_cell_coords: wp.array[fem.Coords],
particle_cell_fractions: wp.array[float],
cell_index: int,
cell_coords: fem.Coords,
cell_weight: float,
):
for i in range(8):
if particle_cell_indices[i] == fem.NULL_NODE_INDEX:
particle_cell_indices[i] = cell_index
particle_cell_coords[i] = cell_coords
particle_cell_fractions[i] = cell_weight
return
if particle_cell_indices[i] == cell_index:
particle_cell_fractions[i] += cell_weight
return
@fem.cache.dynamic_kernel(suffix=domain.name)
def particle_locations_gimp(
cell_arg_value: domain.ElementArg,
domain_index_arg_value: domain.ElementIndexArg,
positions: wp.array[wp.vec3],
radii: wp.array[float],
cell_index: wp.array2d[fem.ElementIndex],
cell_coords: wp.array2d[fem.Coords],
cell_fractions: wp.array2d[float],
):
p = wp.tid()
domain_arg = domain.DomainArg(cell_arg_value, domain_index_arg_value)
center = positions[p]
radius = radii[p]
tot_weight = float(0.0)
# Find cell containing each corner of the particle,
# merging repeated cell indices
for vtx in range(8):
i = (vtx & 4) >> 2
j = (vtx & 2) >> 1
k = vtx & 1
pos = center - wp.vec3(radius) + 2.0 * radius * wp.vec3(float(i), float(j), float(k))
sample = cell_lookup(domain_arg, pos)
if sample.element_index == fem.NULL_ELEMENT_INDEX:
continue
elem_index = domain.element_partition_index(domain_index_arg_value, sample.element_index)
cell_weight = wp.min(wp.min(sample.element_coords), 1.0 - wp.max(sample.element_coords))
if cell_weight > 0.0:
tot_weight += cell_weight
cell_center_coords, _ = cell_closest_point(cell_arg_value, sample.element_index, center)
add_cell(
cell_index[p],
cell_coords[p],
cell_fractions[p],
elem_index,
cell_center_coords,
cell_weight,
)
# Normalize the weights over the cells
for vtx in range(8):
if cell_index[p, vtx] != fem.NULL_NODE_INDEX:
cell_fractions[p, vtx] /= tot_weight
device = positions.device
cell_indices = fem.borrow_temporary(self.temporary_store, shape=(positions.shape[0], 8), dtype=fem.ElementIndex)
cell_coords = fem.borrow_temporary(self.temporary_store, shape=(positions.shape[0], 8), dtype=fem.Coords)
cell_fractions = fem.borrow_temporary(self.temporary_store, shape=(positions.shape[0], 8), dtype=float)
cell_indices.fill_(fem.NULL_NODE_INDEX)
wp.launch(
particle_locations_gimp,
dim=positions.shape[0],
inputs=[
domain.element_arg_value(device=device),
domain.element_index_arg_value(device=device),
positions,
radii,
cell_indices,
cell_coords,
cell_fractions,
],
device=device,
)
return cell_indices, cell_coords, cell_fractions
def _step_impl(
self,
state_in: newton.State,
state_out: newton.State,
dt: float,
pic: fem.PicQuadrature,
scratch: ImplicitMPMScratchpad,
):
"""Single implicit MPM step: bin, rasterize, assemble, solve, advect.
Executes the full pipeline for one time step, including particle
binning, collider rasterization, RHS assembly, strain/compliance matrix
computation, warm-starting, coupled rheology/contact solve, strain
updates, and particle advection.
Args:
state_in: Input state at the beginning of the timestep.
state_out: Output state to write to.
dt: Timestep length.
pic: Particle-in-cell quadrature data.
scratch: Scratchpad for temporary storage.
"""
cell_volume = self._mpm_model.voxel_size**3
inv_cell_volume = 1.0 / cell_volume
mpm_model = self._mpm_model
last_step_data = self._last_step_data
self._require_collision_space_fields(scratch, last_step_data)
self._require_velocity_space_fields(scratch, mpm_model.has_compliant_particles)
# Rasterize colliders to discrete space
self._rasterize_colliders(state_in, dt, last_step_data, scratch, inv_cell_volume)
# Velocity right-hand side and inverse mass matrix
self._compute_unconstrained_velocity(state_in, dt, pic, scratch, inv_cell_volume)
# Build collider rigidity matrix
rigidity_operator = self._build_collider_rigidity_operator(state_in, scratch, cell_volume)
self._require_strain_space_fields(scratch, last_step_data)
# Build elasticity compliance matrix and right-hand-side
self._build_elasticity_system(state_in, dt, pic, scratch, inv_cell_volume)
# Build strain matrix and offset, setup yield surface parameters
self._build_plasticity_system(state_in, dt, pic, scratch, inv_cell_volume)
# Solve implicit system
self._load_warmstart(state_in, last_step_data, scratch, pic, inv_cell_volume)
# Solve implicit system
# Keep _solve_graph alive until end of function as destruction may cause sync point
_solve_graph = self._solve_rheology(pic, scratch, rigidity_operator, last_step_data, inv_cell_volume)
self._save_for_next_warmstart(scratch, pic, last_step_data)
# Update and advect particles
self._update_particles(state_in, state_out, dt, pic, scratch)
# Save data for next step or further processing
self._save_data(state_in, scratch, last_step_data, state_out)
def _compute_unconstrained_velocity(
self,
state_in: newton.State,
dt: float,
pic: fem.PicQuadrature,
scratch: ImplicitMPMScratchpad,
inv_cell_volume: float,
):
"""Compute the unconstrained (ballistic) velocity at grid nodes, as well as inverse mass matrix."""
model = self.model
mpm_model = self._mpm_model
with self._timer("Unconstrained velocity"):
velocity_int = fem.integrate(
integrate_velocity,
quadrature=pic,
fields={"u": scratch.velocity_test},
values={
"velocities": state_in.particle_qd,
"dt": dt,
"gravity": model.gravity,
"particle_world": model.particle_world,
"particle_density": mpm_model.particle_density,
"inv_cell_volume": inv_cell_volume,
},
output_dtype=wp.vec3,
temporary_store=self.temporary_store,
)
if self.apic:
fem.integrate(
integrate_velocity_apic,
quadrature=pic,
fields={"u": scratch.velocity_test},
values={
"velocity_gradients": state_in.mpm.particle_qd_grad,
"particle_density": mpm_model.particle_density,
"inv_cell_volume": inv_cell_volume,
},
output=velocity_int,
add=True,
temporary_store=self.temporary_store,
)
node_particle_mass = fem.integrate(
integrate_mass,
quadrature=pic,
fields={"phi": scratch.fraction_test},
values={
"inv_cell_volume": inv_cell_volume,
"particle_density": mpm_model.particle_density,
},
output_dtype=float,
temporary_store=self.temporary_store,
)
drag = mpm_model.air_drag * dt
wp.launch(
free_velocity,
dim=scratch.velocity_node_count,
inputs=[
velocity_int,
node_particle_mass,
drag,
],
outputs=[
scratch.inv_mass_matrix,
scratch.velocity_field.dof_values,
],
)
def _rasterize_colliders(
self,
state_in: newton.State,
dt: float,
last_step_data: LastStepData,
scratch: ImplicitMPMScratchpad,
inv_cell_volume: float,
):
# Rasterize collider to grid
collider_node_count = scratch.collider_node_count
vel_node_count = scratch.velocity_node_count
with self._timer("Rasterize collider"):
# volume associated to each collider node
fem.integrate(
integrate_fraction,
fields={"phi": scratch.collider_fraction_test},
values={"inv_cell_volume": inv_cell_volume},
assembly="nodal",
output=scratch.collider_node_volume,
temporary_store=self.temporary_store,
)
# rasterize sdf and properties to grid
rasterize_collider(
self._mpm_model.collider,
state_in.body_q,
state_in.body_qd if self.collider_velocity_mode == "forward" else None,
last_step_data.body_q_prev if self.collider_velocity_mode == "backward" else None,
self._mpm_model.voxel_size,
dt,
scratch.collider_fraction_test.space_restriction,
scratch.collider_node_volume,
scratch.collider_position_field,
scratch.collider_distance_field,
scratch.collider_normal_field,
scratch.collider_velocity,
scratch.collider_friction,
scratch.collider_adhesion,
scratch.collider_ids,
temporary_store=self.temporary_store,
)
# normal interpolation
if self.collider_normal_from_sdf_gradient:
interpolate_collider_normals(
scratch.collider_fraction_test.space_restriction,
scratch.collider_distance_field,
scratch.collider_normal_field,
temporary_store=self.temporary_store,
)
# Subgrid collisions
if self.collider_basis != self.velocity_basis:
# Map from collider nodes to velocity nodes
wps.bsr_set_zero(
scratch.collider_matrix, rows_of_blocks=collider_node_count, cols_of_blocks=vel_node_count
)
fem.interpolate(
collision_weight_field,
dest=scratch.collider_matrix,
dest_space=scratch.collider_fraction_test.space,
at=scratch.collider_fraction_test.space_restriction,
reduction="first",
fields={"trial": scratch.fraction_trial, "normal": scratch.collider_normal_field},
temporary_store=self.temporary_store,
)
def _build_collider_rigidity_operator(
self,
state_in: newton.State,
scratch: ImplicitMPMScratchpad,
cell_volume: float,
):
has_compliant_colliders = self._mpm_model.min_collider_mass < INFINITY
if not has_compliant_colliders:
return None
with self._timer("Collider compliance"):
rigidity_operator = build_rigidity_operator(
cell_volume=cell_volume,
node_volumes=scratch.collider_node_volume,
node_positions=scratch.collider_position_field.dof_values,
collider=self._mpm_model.collider,
body_q=state_in.body_q,
body_mass=self._mpm_model.collider_body_mass,
body_inv_inertia=self._mpm_model.collider_body_inv_inertia,
collider_ids=scratch.collider_ids,
)
return rigidity_operator
def _build_elasticity_system(
self,
state_in: newton.State,
dt: float,
pic: fem.PicQuadrature,
scratch: ImplicitMPMScratchpad,
inv_cell_volume: float,
):
"""Build the elasticity and compliance system."""
mpm_model = self._mpm_model
if not mpm_model.has_compliant_particles:
scratch.elastic_strain_delta_field.dof_values.zero_()
return
with self._timer("Elasticity"):
node_particle_volume = fem.integrate(
integrate_fraction,
quadrature=pic,
fields={"phi": scratch.fraction_test},
values={"inv_cell_volume": inv_cell_volume},
output_dtype=float,
temporary_store=self.temporary_store,
)
elastic_parameters_int = fem.integrate(
integrate_elastic_parameters,
quadrature=pic,
fields={"u": scratch.velocity_test},
values={
"material_parameters": mpm_model.material_parameters,
"inv_cell_volume": inv_cell_volume,
},
output_dtype=wp.vec3,
temporary_store=self.temporary_store,
)
wp.launch(
average_elastic_parameters,
dim=scratch.elastic_parameters_field.space_partition.node_count(),
inputs=[
elastic_parameters_int,
node_particle_volume,
scratch.elastic_parameters_field.dof_values,
],
)
fem.integrate(
strain_rhs,
quadrature=pic,
fields={
"tau": scratch.sym_strain_test,
"elastic_parameters": scratch.elastic_parameters_field,
},
values={
"elastic_strains": state_in.mpm.particle_elastic_strain,
"inv_cell_volume": inv_cell_volume,
"dt": dt,
},
temporary_store=self.temporary_store,
output=scratch.elastic_strain_delta_field.dof_values,
)
fem.integrate(
compliance_form,
quadrature=pic,
fields={
"tau": scratch.sym_strain_test,
"sig": scratch.sym_strain_trial,
"elastic_parameters": scratch.elastic_parameters_field,
},
values={
"elastic_strains": state_in.mpm.particle_elastic_strain,
"inv_cell_volume": inv_cell_volume,
"dt": dt,
},
output=scratch.compliance_matrix,
temporary_store=self.temporary_store,
)
def _build_plasticity_system(
self,
state_in: newton.State,
dt: float,
pic: fem.PicQuadrature,
scratch: ImplicitMPMScratchpad,
inv_cell_volume: float,
):
mpm_model = self._mpm_model
with self._timer("Interpolated yield parameters"):
fem.integrate(
integrate_yield_parameters,
quadrature=pic,
fields={
"u": scratch.strain_yield_parameters_test,
},
values={
"particle_Jp": state_in.mpm.particle_Jp,
"material_parameters": mpm_model.material_parameters,
"inv_cell_volume": inv_cell_volume,
"dt": dt,
},
output=scratch.strain_yield_parameters_field.dof_values,
temporary_store=self.temporary_store,
)
fem.integrate(
integrate_fraction,
quadrature=pic,
fields={"phi": scratch.divergence_test},
values={"inv_cell_volume": inv_cell_volume},
output=scratch.strain_node_particle_volume,
)
# Void fraction (unilateral incompressibility offset)
if mpm_model.critical_fraction > 0.0:
with self._timer("Unilateral offset"):
fem.integrate(
integrate_fraction,
fields={"phi": scratch.divergence_test},
values={"inv_cell_volume": inv_cell_volume},
output=scratch.strain_node_volume,
temporary_store=self.temporary_store,
)
if isinstance(scratch.collider_distance_field.space.basis, fem.PointBasisSpace):
fem.integrate(
integrate_collider_fraction_apic,
fields={
"phi": scratch.divergence_test,
"sdf": scratch.collider_distance_field,
"sdf_gradient": scratch.collider_normal_field,
},
values={
"inv_cell_volume": inv_cell_volume,
},
output=scratch.strain_node_collider_volume,
temporary_store=self.temporary_store,
)
else:
fem.integrate(
integrate_collider_fraction,
fields={
"phi": scratch.divergence_test,
"sdf": scratch.collider_distance_field,
},
values={
"inv_cell_volume": inv_cell_volume,
},
output=scratch.strain_node_collider_volume,
temporary_store=self.temporary_store,
)
wp.launch(
compute_unilateral_strain_offset,
dim=scratch.strain_node_count,
inputs=[
mpm_model.critical_fraction,
scratch.strain_node_particle_volume,
scratch.strain_node_collider_volume,
scratch.strain_node_volume,
scratch.unilateral_strain_offset,
],
)
else:
scratch.unilateral_strain_offset.zero_()
# Strain jacobian
with self._timer("Strain matrix"):
fem.integrate(
strain_delta_form,
quadrature=pic,
fields={
"u": scratch.velocity_trial,
"tau": scratch.divergence_test,
},
values={
"dt": dt,
"inv_cell_volume": inv_cell_volume,
},
output_dtype=float,
output=scratch.strain_matrix,
temporary_store=self.temporary_store,
bsr_options={"prune_numerical_zeros": self._velocity_nodes_per_strain_sample < 0},
)
def _build_strain_eigenbasis(
self,
pic: fem.PicQuadrature,
scratch: ImplicitMPMScratchpad,
inv_cell_volume: float,
):
if self.strain_basis in ("Q1", "S2"):
scratch.strain_node_particle_volume += EPSILON
return None
elif self.strain_basis[:3] == "pic":
M_diag = scratch.strain_node_particle_volume
M_diag.assign(self._mpm_model.particle_volume * inv_cell_volume)
return None
# build mass matrix of PIC integration
M = fem.integrate(
mass_form,
quadrature=pic,
fields={"p": scratch.divergence_test, "q": scratch.divergence_trial},
values={"inv_cell_volume": inv_cell_volume},
output_dtype=float,
)
# extract diagonal blocks
nodes_per_elt = scratch.divergence_test.space.topology.MAX_NODES_PER_ELEMENT
M_elt_wise = wps.bsr_copy(M, block_shape=(nodes_per_elt, nodes_per_elt))
if M_elt_wise.block_shape == (1, 1):
M_values = M_elt_wise.values.view(dtype=mat11)
else:
M_values = M_elt_wise.values
M_ev = wp.empty(shape=(M_elt_wise.nrow, *M_elt_wise.block_shape), dtype=M_elt_wise.scalar_type)
M_diag = scratch.strain_node_particle_volume.reshape((-1, nodes_per_elt))
wp.launch(
compute_eigenvalues,
dim=M_elt_wise.nrow,
inputs=[
M_elt_wise.offsets,
M_elt_wise.columns,
M_values,
scratch.strain_yield_parameters_field.dof_values,
],
outputs=[
M_diag,
M_ev,
],
)
return M_ev
def _apply_strain_eigenbasis(
self,
scratch: ImplicitMPMScratchpad,
M_ev: wp.array3d[float],
):
node_count = scratch.strain_node_count
if M_ev is not None and M_ev.shape[1] > 1:
# Rotate matrix and vectors according to eigenbasis
nodes_per_elt = M_ev.shape[1]
elt_count = M_ev.shape[0]
B = scratch.strain_matrix
strain_mat_tmp = wp.empty_like(B.values)
wp.launch(rotate_matrix_rows, dim=B.nnz, inputs=[M_ev, B.offsets, B.columns, B.values, strain_mat_tmp])
B.values = strain_mat_tmp
C = scratch.compliance_matrix
compliance_mat_tmp = wp.empty_like(C.values)
wp.launch(rotate_matrix_rows, dim=C.nnz, inputs=[M_ev, C.offsets, C.columns, C.values, compliance_mat_tmp])
wp.launch(
rotate_matrix_columns, dim=C.nnz, inputs=[M_ev, C.offsets, C.columns, compliance_mat_tmp, C.values]
)
rotate_vectors = make_rotate_vectors(nodes_per_elt)
wp.launch_tiled(
rotate_vectors,
dim=elt_count,
block_dim=32,
inputs=[
M_ev,
_as_2d_array(scratch.elastic_strain_delta_field.dof_values, shape=(node_count, 6), dtype=float),
_as_2d_array(scratch.stress_field.dof_values, shape=(node_count, 6), dtype=float),
_as_2d_array(
scratch.strain_yield_parameters_field.dof_values,
shape=(node_count, YIELD_PARAM_LENGTH),
dtype=float,
),
_as_2d_array(scratch.unilateral_strain_offset, shape=(node_count, 1), dtype=float),
],
)
M_diag = scratch.strain_node_particle_volume
if self._stress_warmstart == "particles":
# Particle stresses are integrated, need scale with inverse node volume
wp.launch(
inverse_scale_sym_tensor,
dim=node_count,
inputs=[M_diag, scratch.stress_field.dof_values],
)
# Yield parameters are integrated, need scale with inverse node volume
wp.launch(
inverse_scale_vector,
dim=node_count,
inputs=[M_diag, scratch.strain_yield_parameters_field.dof_values],
)
def _unapply_strain_eigenbasis(
self,
scratch: ImplicitMPMScratchpad,
M_ev: wp.array3d[float],
):
node_count = scratch.strain_node_count
# Un-integrate strains by scaling with inverse node volume
M_diag = scratch.strain_node_particle_volume
if self._mpm_model.has_compliant_particles:
wp.launch(
inverse_scale_sym_tensor,
dim=node_count,
inputs=[M_diag, scratch.elastic_strain_delta_field.dof_values],
)
if self._mpm_model.has_hardening:
wp.launch(
inverse_scale_sym_tensor,
dim=node_count,
inputs=[M_diag, scratch.plastic_strain_delta_field.dof_values],
)
if M_ev is not None and M_ev.shape[1] > 1:
# Un-rotate vectors according to eigenbasis
elt_count = M_ev.shape[0]
nodes_per_elt = M_ev.shape[1]
inverse_rotate_vectors = make_inverse_rotate_vectors(nodes_per_elt)
wp.launch_tiled(
inverse_rotate_vectors,
dim=elt_count,
block_dim=32,
inputs=[
M_ev,
_as_2d_array(scratch.stress_field.dof_values, shape=(node_count, 6), dtype=float),
_as_2d_array(scratch.elastic_strain_delta_field.dof_values, shape=(node_count, 6), dtype=float),
_as_2d_array(scratch.plastic_strain_delta_field.dof_values, shape=(node_count, 6), dtype=float),
],
)
def _solve_rheology(
self,
pic: fem.PicQuadrature,
scratch: ImplicitMPMScratchpad,
rigidity_operator: tuple[wps.BsrMatrix, wps.BsrMatrix, wps.BsrMatrix] | None,
last_step_data: LastStepData,
inv_cell_volume: float,
):
M_ev = self._build_strain_eigenbasis(pic, scratch, inv_cell_volume)
self._apply_strain_eigenbasis(scratch, M_ev)
with self._timer("Strain solve"):
momentum_data = MomentumData(
inv_volume=scratch.inv_mass_matrix,
velocity=scratch.velocity_field.dof_values,
)
rheology_data = RheologyData(
strain_mat=scratch.strain_matrix,
transposed_strain_mat=scratch.transposed_strain_matrix,
compliance_mat=scratch.compliance_matrix,
strain_node_volume=scratch.strain_node_particle_volume,
yield_params=scratch.strain_yield_parameters_field.dof_values,
unilateral_strain_offset=scratch.unilateral_strain_offset,
color_offsets=scratch.color_offsets,
color_blocks=scratch.color_indices,
elastic_strain_delta=scratch.elastic_strain_delta_field.dof_values,
plastic_strain_delta=scratch.plastic_strain_delta_field.dof_values,
stress=scratch.stress_field.dof_values,
has_viscosity=self._mpm_model.has_viscosity,
has_dilatancy=self._mpm_model.has_dilatancy,
strain_velocity_node_count=self._velocity_nodes_per_strain_sample,
)
collision_data = CollisionData(
collider_mat=scratch.collider_matrix,
transposed_collider_mat=scratch.transposed_collider_matrix,
collider_friction=scratch.collider_friction,
collider_adhesion=scratch.collider_adhesion,
collider_normals=scratch.collider_normal_field.dof_values,
collider_velocities=scratch.collider_velocity,
rigidity_operator=rigidity_operator,
collider_impulse=scratch.impulse_field.dof_values,
)
# Retain graph to avoid immediate CPU synch
solve_graph = solve_rheology(
self.solver,
self.max_iterations,
self.tolerance,
momentum_data,
rheology_data,
collision_data,
temporary_store=self.temporary_store,
use_graph=self._use_cuda_graph,
verbose=self.verbose,
)
self._unapply_strain_eigenbasis(scratch, M_ev)
return solve_graph
def _update_particles(
self,
state_in: newton.State,
state_out: newton.State,
dt: float,
pic: fem.PicQuadrature,
scratch: ImplicitMPMScratchpad,
):
"""Update particle quantities (strains, velocities, ...) from grid fields an advect them."""
model = self.model
mpm_model = self._mpm_model
has_compliant_particles = mpm_model.min_young_modulus < INFINITY
has_hardening = mpm_model.max_hardening > 0.0
if self._stress_warmstart == "particles" or has_compliant_particles or has_hardening:
with self._timer("Particle strain update"):
# Update particle elastic strain from grid strain delta
if state_in is state_out:
elastic_strain_prev = wp.clone(state_in.mpm.particle_elastic_strain)
particle_Jp_prev = wp.clone(state_in.mpm.particle_Jp)
else:
elastic_strain_prev = state_in.mpm.particle_elastic_strain
particle_Jp_prev = state_in.mpm.particle_Jp
state_out.mpm.particle_Jp.zero_()
state_out.mpm.particle_stress.zero_()
state_out.mpm.particle_elastic_strain.zero_()
fem.interpolate(
update_particle_strains,
at=pic,
values={
"dt": dt,
"particle_flags": model.particle_flags,
"particle_density": mpm_model.particle_density,
"particle_volume": mpm_model.particle_volume,
"elastic_strain_prev": elastic_strain_prev,
"elastic_strain": state_out.mpm.particle_elastic_strain,
"particle_stress": state_out.mpm.particle_stress,
"particle_Jp_prev": particle_Jp_prev,
"particle_Jp": state_out.mpm.particle_Jp,
"material_parameters": mpm_model.material_parameters,
},
fields={
"grid_vel": scratch.velocity_field,
"plastic_strain_delta": scratch.plastic_strain_delta_field,
"elastic_strain_delta": scratch.elastic_strain_delta_field,
"stress": scratch.stress_field,
},
temporary_store=self.temporary_store,
)
# (A)PIC advection
with self._timer("Advection"):
state_out.particle_qd.zero_()
state_out.mpm.particle_qd_grad.zero_()
state_out.particle_q.assign(state_in.particle_q)
fem.interpolate(
advect_particles,
at=pic,
values={
"particle_flags": model.particle_flags,
"particle_volume": mpm_model.particle_volume,
"pos": state_out.particle_q,
"vel": state_out.particle_qd,
"vel_grad": state_out.mpm.particle_qd_grad,
"dt": dt,
"max_vel": model.particle_max_velocity,
},
fields={
"grid_vel": scratch.velocity_field,
},
temporary_store=self.temporary_store,
)
def _save_data(
self,
state_in: newton.State,
scratch: ImplicitMPMScratchpad,
last_step_data: LastStepData,
state_out: newton.State,
):
"""Save data for next step or further processing."""
# Copy current body_q to last_step_data.body_q_prev for next step's velocity computation
last_step_data.save_collider_current_position(state_in.body_q)
# Necessary fields for two-way coupling
state_out.impulse_field = scratch.impulse_field
state_out.collider_ids = scratch.collider_ids
state_out.collider_position_field = scratch.collider_position_field
state_out.collider_distance_field = scratch.collider_distance_field
state_out.collider_normal_field = scratch.collider_normal_field
# Necessary fields for grains rendering
# Re-generated at each step, defined on space partition
state_out.velocity_field = scratch.velocity_field
def _require_velocity_space_fields(self, scratch: ImplicitMPMScratchpad, has_compliant_particles: bool):
"""Ensure velocity-space fields exist and match current spaces."""
scratch.require_velocity_space_fields(has_compliant_particles)
def _require_collision_space_fields(self, scratch: ImplicitMPMScratchpad, last_step_data: LastStepData):
"""Ensure collision-space fields exist and match current spaces."""
scratch.require_collision_space_fields()
last_step_data.require_collision_space_fields(scratch)
last_step_data.require_collider_previous_position(self._mpm_model.collider_body_q)
def _require_strain_space_fields(self, scratch: ImplicitMPMScratchpad, last_step_data: LastStepData):
"""Ensure strain-space fields exist and match current spaces."""
scratch.require_strain_space_fields()
last_step_data.require_strain_space_fields(scratch, smoothed=self._stress_warmstart == "smoothed")
def _load_warmstart(
self,
state_in: newton.State,
last_step_data: LastStepData,
scratch: ImplicitMPMScratchpad,
pic: fem.PicQuadrature,
inv_cell_volume: float,
):
with self._timer("Warmstart fields"):
self._warmstart_fields(last_step_data, scratch, pic)
if self._stress_warmstart == "particles":
fem.integrate(
integrate_particle_stress,
quadrature=pic,
fields={
"tau": scratch.sym_strain_test,
},
values={
"particle_stress": state_in.mpm.particle_stress,
"inv_cell_volume": inv_cell_volume,
},
output=scratch.stress_field.dof_values,
)
elif not self._stress_warmstart:
scratch.stress_field.dof_values.zero_()
def _warmstart_fields(
self,
last_step_data: LastStepData,
scratch: ImplicitMPMScratchpad,
pic: fem.PicQuadrature,
):
"""Interpolate previous grid fields into the current grid layout.
Transfers impulse and stress fields from the previous grid to the new
grid (handling nonconforming cases), and initializes the output state's
grid fields to the current scratchpad fields.
"""
prev_impulse_field = last_step_data.ws_impulse_field
prev_stress_field = last_step_data.ws_stress_field
domain = scratch.velocity_test.domain
if isinstance(prev_impulse_field.space.basis, fem.PointBasisSpace):
# point-based collisions, simply copy the previous impulses
scratch.impulse_field.dof_values.assign(prev_impulse_field.dof_values[pic.cell_particle_indices])
else:
# Interpolate previous impulse
prev_impulse_field = fem.NonconformingField(
domain, prev_impulse_field, background=scratch.background_impulse_field
)
fem.interpolate(
prev_impulse_field,
dest=scratch.impulse_field,
at=scratch.collider_fraction_test.space_restriction,
reduction="first",
temporary_store=self.temporary_store,
)
# Interpolate previous stress
if isinstance(prev_stress_field.space.basis, fem.PointBasisSpace):
scratch.stress_field.dof_values.assign(prev_stress_field.dof_values[pic.cell_particle_indices])
elif self._stress_warmstart in ("grid", "smoothed"):
prev_stress_field = fem.NonconformingField(
domain, prev_stress_field, background=scratch.background_stress_field
)
fem.interpolate(
prev_stress_field,
dest=scratch.stress_field,
at=scratch.sym_strain_test.space_restriction,
reduction="first",
temporary_store=self.temporary_store,
)
def _save_for_next_warmstart(
self, scratch: ImplicitMPMScratchpad, pic: fem.PicQuadrature, last_step_data: LastStepData
):
with self._timer("Save warmstart fields"):
last_step_data.rebind_collision_space_fields(scratch)
if isinstance(last_step_data.ws_impulse_field.space.basis, fem.PointBasisSpace):
# point-based collisions, simply copy the previous impulses
last_step_data.ws_impulse_field.dof_values[pic.cell_particle_indices].assign(
scratch.impulse_field.dof_values
)
else:
last_step_data.ws_impulse_field.dof_values.zero_()
wp.launch(
scatter_field_dof_values,
dim=scratch.impulse_field.space_partition.node_count(),
inputs=[
scratch.impulse_field.space_partition.space_node_indices(),
scratch.impulse_field.dof_values,
last_step_data.ws_impulse_field.dof_values,
],
)
last_step_data.rebind_strain_space_fields(scratch, smoothed=self._stress_warmstart == "smoothed")
if isinstance(last_step_data.ws_stress_field.space.basis, fem.PointBasisSpace):
last_step_data.ws_stress_field.dof_values[pic.cell_particle_indices].assign(
scratch.stress_field.dof_values
)
else:
last_step_data.ws_stress_field.dof_values.zero_()
fem.interpolate(
scratch.stress_field,
dest=last_step_data.ws_stress_field,
)
def _max_colors(self):
if not self.coloring:
return 0
return 27 if self.strain_basis == "Q1" else self._scratchpad.velocity_nodes_per_element
def _compute_coloring(
self,
pic: fem.PicQuadrature,
scratch: ImplicitMPMScratchpad,
):
"""Compute Gauss-Seidel coloring of strain nodes to avoid write conflicts.
Writes scratch.color_offsets, scratch.color_indices.
"""
space_partition = scratch._strain_space_restriction.space_partition
grid = space_partition.geo_partition.geometry
is_pic = self.strain_basis[:3] == "pic"
if not is_pic:
nodes_per_color_element = scratch.strain_nodes_per_element
is_dg = space_partition.space_topology.node_count() == nodes_per_color_element * grid.cell_count()
if is_pic or is_dg:
# cell-based coloring
# nodes in each element solved sequentially
stencil_size = int(np.round(np.cbrt(scratch.velocity_nodes_per_element)))
if isinstance(grid, fem.Nanogrid):
voxels = grid._cell_ijk
res = wp.vec3i(0)
else:
voxels = None
res = grid.res
colored_element_count = space_partition.geo_partition.cell_count()
partition_arg = space_partition.geo_partition.cell_arg_value(device=scratch.color_indices.device)
colors = fem.borrow_temporary(self.temporary_store, shape=colored_element_count * 2 + 1, dtype=int)
color_indices = scratch.color_indices.flatten()
wp.launch(
make_cell_color_kernel(space_partition.geo_partition),
dim=colored_element_count,
inputs=[
partition_arg,
stencil_size,
voxels,
res,
colors,
color_indices,
],
)
elif self.strain_basis == "Q1":
nodes_per_color_element = 1
stencil_size = 3
if isinstance(grid, fem.Nanogrid):
voxels = grid._node_ijk
res = wp.vec3i(0)
else:
voxels = None
res = grid.res + wp.vec3i(1)
colored_element_count = space_partition.node_count()
space_node_indices = space_partition.space_node_indices()
colors = fem.borrow_temporary(self.temporary_store, shape=colored_element_count * 2 + 1, dtype=int)
color_indices = scratch.color_indices.flatten()
wp.launch(
node_color,
dim=colored_element_count,
inputs=[
space_node_indices,
stencil_size,
voxels,
res,
colors,
color_indices,
],
)
else:
raise RuntimeError("Unsupported strain basis for coloring")
wp.utils.radix_sort_pairs(
keys=colors,
values=color_indices,
count=colored_element_count,
)
unique_colors = colors[colored_element_count:]
color_count = unique_colors[colored_element_count:]
color_node_counts = color_indices[colored_element_count:]
wp.utils.runlength_encode(
colors,
value_count=colored_element_count,
run_values=unique_colors,
run_lengths=color_node_counts,
run_count=color_count,
)
wp.launch(
compute_color_offsets,
dim=1,
inputs=[self._max_colors(), color_count, unique_colors, color_node_counts, scratch.color_offsets],
)
# build color ranges from cell/node color indices
if is_pic:
wp.launch(
make_dynamic_color_block_indices_kernel(space_partition.geo_partition),
dim=colored_element_count,
inputs=[partition_arg, pic.cell_particle_offsets, scratch.color_indices],
)
else:
wp.launch(
fill_uniform_color_block_indices,
dim=colored_element_count,
inputs=[nodes_per_color_element, scratch.color_indices],
)
colors.release()
def _timer(self, name: str):
return wp.ScopedTimer(
name,
active=self.enable_timers,
use_nvtx=self._timers_use_nvtx,
synchronize=not self._timers_use_nvtx,
)
================================================
FILE: newton/_src/solvers/kamino/README.md
================================================
# Kamino
⚠️ Disclaimer ⚠️
`SolverKamino` is currently in BETA (`BETA 1`).
At present time we discourage users of Newton from depending on it.
Similarly, and due to limited bandwidth of the development team, we will NOT be accepting contributions from the community.
A more stable `BETA 2` version is planned for release during the summer of 2026.
## Introduction
`SolverKamino` is a physics solver for simulating arbitrary mechanical assemblies that may feature kinematic loops and under-/overactuation.
It currently supports:
- Constrained rigid multi-body systems with arbitrary joint topologies (i.e. a kinematic tree is not assumed)
- A large set of common and advanced bilateral joint constraints
- Unilateral joint-limit and contact constraints with spatial friction and restitutive impacts
- Fully configurable constraint stabilization that can be specified per constraint subset
- Hard joint-limit and contact constraints enforced via an advanced Proximal-ADMM forward dynamics solver
Kamino is being developed and maintained by [Disney Research](https://www.disneyresearch.com/) in collaboration with [NVIDIA](https://www.nvidia.com/) and [Google DeepMind](https://deepmind.google/).
## Getting Started
### Installing
For plain users, the official [installation instructions](https://newton-physics.github.io/newton/latest/guide/installation.html) of Newton are recommended.
For developers, please refer to the [Development](#development) section below.
### Running Examples
A set of examples is provided in `newton/_src/solvers/kamino/examples`.
These can be run directly as stand-alone scripts, for example:
```bash
cd newton/_src/solvers/kamino/examples
python sim/example_sim_dr_legs.py
```
All examples will eventually be migrated, and integrated, into the main set of examples provided in Newton located in `newton/examples`.
### Running Unit Tests
```bash
cd newton/_src/solvers/kamino/tests
python -m unittest discover -s . -p 'test_*.py'
```
Please refer to the [Unit Tests](./tests/README.md) `README.md` for further instructions for how to run unit tests using IDEs such as VS Code.
All tests will eventually be migrated, and integrated, into the main set of unit-tests provided in Newton located in `newton/tests`.
## Development
Development of Kamino requires the installation of [Newton](https://github.com/newton-physics/newton) from source and the latest version of [Warp](https://github.com/NVIDIA/warp) either through nightly builds or also from source.
The first step involves setting-up a python environment.
The simplest is to create a new `virtualenv`. Alternatively one could
follow the [instructions](https://newton-physics.github.io/newton/latest/guide/installation.html) from Newton.
### Virtual environments using `pyenv`
Because we're working on a fork of the main Newton repository, it can be useful to create two `virtualenv|conda|uv` environments.
Using `pyenv` and `virtualenv` it would look something like this:
- one for development of Kamino within our fork
```bash
pyenv virtualenv newton
pyenv activate newton
pip install -U pip
```
A similar setup can be achieved via `conda|uv`. We've used the `*-dev` suffix to denote environments were the packages will be installed from source, while this can be omitted when creating environments to test installations when installing from `pip` wheels.
### APT (Only Required for Linux)
On Linux platforms, e.g. Ubuntu, the following base APT packages must be installed:
```bash
sudo apt-get update
sudo apt-get install -y libx11-dev libxrandr-dev libxinerama-dev libxcursor-dev libxi-dev libgl1-mesa-dev
```
### MuJoCo
The first `pip` package to install is MuJoCo:
```bash
pip install mujoco --pre -f https://py.mujoco.org/
```
**NOTE**:
This must be installed first as it will pull the required version of foundational dependencies such as `numpy`.
### Warp
Nightly builds of Warp can be installed using:
```bash
pip install warp-lang --pre -U -f https://pypi.nvidia.com/warp-lang/
```
Alternatively, Warp can be installed from source (recommended) using:
```bash
git clone git@github.com:NVIDIA/warp.git
cd warp
python build_lib.py
pip install -e .[dev,benchmark]
```
**NOTE**:
Many new features and fixes in Warp that are requested by Newton developers come quite often, so keeping up to date with Warp `main` can prove useful.
### MuJoCo Warp
MuJoCo Warp (a.k.a. MJWarp) can be installed from source using:
```bash
pip install git+https://github.com/google-deepmind/mujoco_warp.git@main
```
For development purposes, it can also be installed explicitly with optional dependencies from source using:
```bash
git clone git@github.com:google-deepmind/mujoco_warp.git
cd mujoco_warp
pip install -e .[dev,cuda]
```
### Newton
Newton needs to be installed from source for Kamino development using:
```bash
git clone git@github.com:newton-physics/newton-usd-schemas.git
cd newton-usd-schemas
pip install -e .[dev,docs,notebook]
```
```bash
git clone git@github.com:newton-physics/newton-actuators.git
cd newton-actuators
pip install -e .[dev,docs,notebook]
```
```bash
git clone git@github.com:newton-physics/newton.git
cd newton
pip install -e .[dev,docs,notebook]
```
## Further Reading
The following [technical report](https://arxiv.org/abs/2504.19771) provides an in-depth description of the problem formulation and algorithms used within the solver:
```bibtex
@article{tsounis:2025,
title={On Solving the Dynamics of Constrained Rigid Multi-Body Systems with Kinematic Loops},
author={Vassilios Tsounis and Ruben Grandia and Moritz Bächer},
year={2025},
eprint={2504.19771},
archivePrefix={arXiv},
primaryClass={cs.RO},
url={https://arxiv.org/abs/2504.19771},
}
```
----
================================================
FILE: newton/_src/solvers/kamino/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Kamino: A Newton physics solver for simulating arbitrary mechanical assemblies, (i.e.
constrained rigid multi-body systems) with kinematic loops and under-/overactuation.
"""
from .solver_kamino import SolverKamino
###
# Kamino API
###
__all__ = ["SolverKamino"]
================================================
FILE: newton/_src/solvers/kamino/_src/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Kamino: A physics back-end for Newton for constrained multi-body body simulation.
"""
from .core.bodies import (
convert_base_origin_to_com,
convert_body_com_to_origin,
convert_body_origin_to_com,
)
from .core.control import ControlKamino
from .core.conversions import convert_model_joint_transforms
from .core.gravity import convert_model_gravity
from .core.model import ModelKamino
from .core.state import StateKamino
from .geometry.contacts import (
ContactsKamino,
convert_contacts_kamino_to_newton,
convert_contacts_newton_to_kamino,
)
from .geometry.detector import CollisionDetector
from .solver_kamino_impl import SolverKaminoImpl
from .utils import logger as msg
###
# Kamino API
###
__all__ = [
"CollisionDetector",
"ContactsKamino",
"ControlKamino",
"ModelKamino",
"SolverKaminoImpl",
"StateKamino",
"convert_base_origin_to_com",
"convert_body_com_to_origin",
"convert_body_origin_to_com",
"convert_contacts_kamino_to_newton",
"convert_contacts_newton_to_kamino",
"convert_model_gravity",
"convert_model_joint_transforms",
"msg",
]
================================================
FILE: newton/_src/solvers/kamino/_src/core/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Core Module
"""
from .builder import ModelBuilderKamino
from .control import ControlKamino
from .data import DataKamino
from .model import ModelKamino
from .state import StateKamino
###
# Module interface
###
__all__ = ["ControlKamino", "DataKamino", "ModelBuilderKamino", "ModelKamino", "StateKamino"]
================================================
FILE: newton/_src/solvers/kamino/_src/core/bodies.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Defines Types & Containers for Rigid Body Entities."""
from __future__ import annotations
from dataclasses import dataclass, field
import warp as wp
from .types import Descriptor, int32, mat33f, override, transformf, vec3f, vec6f
###
# Module interface
###
__all__ = [
"RigidBodiesData",
"RigidBodiesModel",
"RigidBodyDescriptor",
"convert_base_origin_to_com",
"convert_body_com_to_origin",
"convert_body_origin_to_com",
"convert_geom_offset_origin_to_com",
"update_body_inertias",
"update_body_wrenches",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Rigid-Body Containers
###
@dataclass
class RigidBodyDescriptor(Descriptor):
"""
A container to describe a single rigid body in the model builder.
Attributes:
name (str): The name of the body.
uid (str): The unique identifier of the body.
m_i (float): Mass of the body (in kg).
i_r_com_i (vec3f): Translational offset of the body center of mass (in meters).
i_I_i (mat33f): Moment of inertia matrix (in local coordinates) of the body (in kg*m^2).
q_i_0 (transformf): Initial absolute pose of the body (in world coordinates).
u_i_0 (vec6f): Initial absolute twist of the body (in world coordinates).
wid (int): Index of the world to which the body belongs.
bid (int): Index of the body w.r.t. its world.
"""
###
# Attributes
###
m_i: float = 0.0
"""Mass of the body."""
i_r_com_i: vec3f = field(default_factory=vec3f)
"""Translational offset of the body center of mass w.r.t the reference frame expressed in local coordinates."""
i_I_i: mat33f = field(default_factory=mat33f)
"""Moment of inertia matrix of the body expressed in local coordinates."""
q_i_0: transformf = field(default_factory=transformf)
"""Initial absolute pose of the body expressed in world coordinates."""
u_i_0: vec6f = field(default_factory=vec6f)
"""Initial absolute twist of the body expressed in world coordinates."""
###
# Metadata - to be set by the WorldDescriptor when added
###
wid: int = -1
"""
Index of the world to which the body belongs.\n
Defaults to `-1`, indicating that the body has not yet been added to a world.
"""
bid: int = -1
"""
Index of the body w.r.t. its world.\n
Defaults to `-1`, indicating that the body has not yet been added to a world.
"""
@override
def __repr__(self) -> str:
"""Returns a human-readable string representation of the RigidBodyDescriptor."""
return (
f"RigidBodyDescriptor(\n"
f"name: {self.name},\n"
f"uid: {self.uid},\n"
f"m_i: {self.m_i},\n"
f"i_I_i:\n{self.i_I_i},\n"
f"q_i_0: {self.q_i_0},\n"
f"u_i_0: {self.u_i_0}\n"
f"wid: {self.wid},\n"
f"bid: {self.bid},\n"
f")"
)
@dataclass
class RigidBodiesModel:
"""
An SoA-based container to hold time-invariant model data of a set of rigid body elements.
Attributes:
num_bodies (int): The total number of body elements in the model (host-side).
wid (wp.array | None): World index each body.\n
Shape of ``(num_bodies,)`` and type :class:`int`.
bid (wp.array | None): Body index of each body w.r.t it's world.\n
Shape of ``(num_bodies,)`` and type :class:`int`.
m_i (wp.array | None): Mass of each body.\n
Shape of ``(num_bodies,)`` and type :class:`float`.
inv_m_i (wp.array | None): Inverse mass (1/m_i) of each body.\n
Shape of ``(num_bodies,)`` and type :class:`float`.
i_I_i (wp.array | None): Local moment of inertia of each body.\n
Shape of ``(num_bodies,)`` and type :class:`mat33`.
inv_i_I_i (wp.array | None): Inverse of the local moment of inertia of each body.\n
Shape of ``(num_bodies,)`` and type :class:`mat33`.
q_i_0 (wp.array | None): Initial pose of each body.\n
Shape of ``(num_bodies,)`` and type :class:`transform`.
u_i_0 (wp.array | None): Initial twist of each body.\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
###
# Meta-Data
###
num_bodies: int = 0
"""Total number of body elements in the model (host-side)."""
label: list[str] | None = None
"""
A list containing the label of each body.\n
Length of ``num_bodies`` and type :class:`str`.
"""
###
# Identifiers
###
wid: wp.array | None = None
"""
World index each body.\n
Shape of ``(num_bodies,)`` and type :class:`int`.
"""
bid: wp.array | None = None
"""
Body index of each body w.r.t it's world.\n
Shape of ``(num_bodies,)`` and type :class:`int`.
"""
###
# Parameterization
###
i_r_com_i: wp.array | None = None
"""
Translational offset of the center of mass w.r.t the body's reference frame.\n
Shape of ``(num_bodies,)`` and type :class:`vec3`.
"""
m_i: wp.array | None = None
"""
Mass of each body.\n
Shape of ``(num_bodies,)`` and type :class:`float`.
"""
inv_m_i: wp.array | None = None
"""
Inverse mass (1/m_i) of each body.\n
Shape of ``(num_bodies,)`` and type :class:`float`.
"""
i_I_i: wp.array | None = None
"""
Local moment of inertia of each body.\n
Shape of ``(num_bodies,)`` and type :class:`mat33`.
"""
inv_i_I_i: wp.array | None = None
"""
Inverse of the local moment of inertia of each body.\n
Shape of ``(num_bodies,)`` and type :class:`mat33`.
"""
###
# Initial State
###
q_i_0: wp.array | None = None
"""
Initial pose of each body.\n
Shape of ``(num_bodies,)`` and type :class:`transform`.
"""
u_i_0: wp.array | None = None
"""
Initial twist of each body.\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
@dataclass
class RigidBodiesData:
"""
An SoA-based container to hold time-varying data of a set of rigid body entities.
"""
num_bodies: int = 0
"""Total number of body entities in the model (host-side)."""
q_i: wp.array | None = None
"""
Absolute poses of each body (in world coordinates).\n
Shape of ``(num_bodies,)`` and type :class:`transform`.
"""
u_i: wp.array | None = None
"""
Absolute twists of each body (in world coordinates).\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
I_i: wp.array | None = None
"""
Moment of inertia (in world coordinates) of each body.\n
Shape of ``(num_bodies,)`` and type :class:`mat33`.
"""
inv_I_i: wp.array | None = None
"""
Inverse moment of inertia (in world coordinates) of each body.\n
Shape of ``(num_bodies,)`` and type :class:`mat33`.
"""
w_i: wp.array | None = None
"""
Total wrench applied to each body (in world coordinates).\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
w_a_i: wp.array | None = None
"""
Joint actuation wrench applied to each body (in world coordinates).\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
w_j_i: wp.array | None = None
"""
Joint constraint wrench applied to each body (in world coordinates).\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
w_l_i: wp.array | None = None
"""
Joint limit wrench applied to each body (in world coordinates).\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
w_c_i: wp.array | None = None
"""
Contact wrench applied to each body (in world coordinates).\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
w_e_i: wp.array | None = None
"""
External wrench applied to each body (in world coordinates).\n
Shape of ``(num_bodies,)`` and type :class:`vec6`.
"""
def clear_all_wrenches(self):
"""
Clears all body wrenches, total and components, setting them to zeros.
"""
self.w_i.zero_()
self.w_a_i.zero_()
self.w_j_i.zero_()
self.w_l_i.zero_()
self.w_c_i.zero_()
self.w_e_i.zero_()
def clear_constraint_wrenches(self):
"""
Clears all constraint wrenches, setting them to zeros.
"""
self.w_j_i.zero_()
self.w_l_i.zero_()
self.w_c_i.zero_()
def clear_actuation_wrenches(self):
"""
Clears actuation wrenches, setting them to zeros.
"""
self.w_a_i.zero_()
def clear_external_wrenches(self):
"""
Clears external wrenches, setting them to zeros.
"""
self.w_e_i.zero_()
###
# Functions
###
# TODO: Use Warp generics to be applicable to other numeric types
@wp.func
def make_symmetric(A: mat33f) -> mat33f:
"""
Makes a given matrix symmetric by averaging it with its transpose.
Args:
A (mat33f): The input matrix.
Returns:
mat33f: The symmetric matrix.
"""
return 0.5 * (A + wp.transpose(A))
@wp.func
def transform_body_inertial_properties(
p_i: transformf,
i_I_i: mat33f,
inv_i_I_i: mat33f,
) -> tuple[mat33f, mat33f]:
"""
Transforms the inertial properties of a rigid body, specified in
local coordinates, to world coordinates given its pose. The inertial
properties include the moment of inertia matrix and its inverse.
Args:
p_i (transformf): The absolute pose of the body in world coordinates.
i_I_i (mat33f): The local moment of inertia of the body.
inv_i_I_i (mat33f): The inverse of the local moment of inertia of the body.
Returns:
tuple[mat33f, mat33f]: The moment of inertia and its inverse in world coordinates.
"""
# Compute the moment of inertia matrices in world coordinates
R_i = wp.quat_to_matrix(wp.transform_get_rotation(p_i))
I_i = R_i @ i_I_i @ wp.transpose(R_i)
inv_I_i = R_i @ inv_i_I_i @ wp.transpose(R_i)
# Ensure symmetry of the inertia matrices (to avoid numerical issues)
I_i = make_symmetric(I_i)
inv_I_i = make_symmetric(inv_I_i)
# Return the computed moment of inertia matrices in world coordinates
return I_i, inv_I_i
###
# Kernels
###
@wp.kernel
def _update_body_inertias(
# Inputs:
model_bodies_i_I_i_in: wp.array(dtype=mat33f),
model_bodies_inv_i_I_i_in: wp.array(dtype=mat33f),
state_bodies_q_i_in: wp.array(dtype=transformf),
# Outputs:
state_bodies_I_i_out: wp.array(dtype=mat33f),
state_bodies_inv_I_i_out: wp.array(dtype=mat33f),
):
# Retrieve the thread index as the body index
bid = wp.tid()
# Retrieve the model data
p_i = state_bodies_q_i_in[bid]
i_I_i = model_bodies_i_I_i_in[bid]
inv_i_I_i = model_bodies_inv_i_I_i_in[bid]
# Compute the moment of inertia matrices in world coordinates
I_i, inv_I_i = transform_body_inertial_properties(p_i, i_I_i, inv_i_I_i)
# Store results in the output arrays
state_bodies_I_i_out[bid] = I_i
state_bodies_inv_I_i_out[bid] = inv_I_i
@wp.kernel
def _update_body_wrenches(
# Inputs
state_bodies_w_a_i_in: wp.array(dtype=vec6f),
state_bodies_w_j_i_in: wp.array(dtype=vec6f),
state_bodies_w_l_i_in: wp.array(dtype=vec6f),
state_bodies_w_c_i_in: wp.array(dtype=vec6f),
state_bodies_w_e_i_in: wp.array(dtype=vec6f),
# Outputs
state_bodies_w_i_out: wp.array(dtype=vec6f),
):
# Retrieve the thread index as the body index
bid = wp.tid()
# Retrieve the model data
w_a_i = state_bodies_w_a_i_in[bid]
w_j_i = state_bodies_w_j_i_in[bid]
w_l_i = state_bodies_w_l_i_in[bid]
w_c_i = state_bodies_w_c_i_in[bid]
w_e_i = state_bodies_w_e_i_in[bid]
# Compute the total wrench applied to the body
w_i = w_a_i + w_j_i + w_l_i + w_c_i + w_e_i
# Store results in the output arrays
state_bodies_w_i_out[bid] = w_i
@wp.kernel
def _convert_body_origin_to_com(
# Inputs
world_mask: wp.array(dtype=int32),
body_wid: wp.array(dtype=int32),
body_com: wp.array(dtype=vec3f),
body_q: wp.array(dtype=transformf),
# Outputs
body_q_com: wp.array(dtype=transformf),
):
bid = wp.tid()
if world_mask:
assert body_wid
if not world_mask[body_wid[bid]]:
return
com = body_com[bid]
q = body_q[bid]
body_r = wp.transform_get_translation(q)
body_rot = wp.transform_get_rotation(q)
r_com = wp.quat_rotate(body_rot, com)
body_q_com[bid] = transformf(body_r + r_com, body_rot)
@wp.kernel
def _convert_body_com_to_origin(
# Inputs
world_mask: wp.array(dtype=int32),
body_wid: wp.array(dtype=int32),
body_com: wp.array(dtype=vec3f),
body_q_com: wp.array(dtype=transformf),
# Outputs
body_q: wp.array(dtype=transformf),
):
bid = wp.tid()
if world_mask:
assert body_wid
if not world_mask[body_wid[bid]]:
return
com = body_com[bid]
q = body_q_com[bid]
body_r_com = wp.transform_get_translation(q)
body_rot = wp.transform_get_rotation(q)
r_com = wp.quat_rotate(body_rot, com)
body_q[bid] = transformf(body_r_com - r_com, body_rot)
@wp.kernel
def _convert_base_origin_to_com(
# Inputs
base_body_index: wp.array(dtype=int32),
body_com: wp.array(dtype=vec3f),
base_q: wp.array(dtype=transformf),
# Outputs
base_q_com: wp.array(dtype=transformf),
):
wid = wp.tid()
base_bid = base_body_index[wid]
if base_bid < 0:
return
com = body_com[base_bid]
q = base_q[wid]
rot = wp.transform_get_rotation(q)
r_com = wp.quat_rotate(rot, com)
base_q_com[wid] = transformf(wp.transform_get_translation(q) + r_com, rot)
@wp.kernel
def _convert_geom_offset_origin_to_com(
# Inputs
body_com: wp.array(dtype=vec3f),
geom_bid: wp.array(dtype=int32),
geom_offset: wp.array(dtype=transformf),
# Outputs
geom_offset_com: wp.array(dtype=transformf),
):
gid = wp.tid()
bid = geom_bid[gid]
if bid >= 0:
com = body_com[bid]
X = geom_offset[gid]
pos = wp.transform_get_translation(X)
rot = wp.transform_get_rotation(X)
geom_offset_com[gid] = transformf(pos - com, rot)
else:
geom_offset_com[gid] = geom_offset[gid]
###
# Launchers
###
def convert_geom_offset_origin_to_com(
body_com: wp.array,
geom_bid: wp.array,
geom_offset: wp.array,
geom_offset_com: wp.array,
):
wp.launch(
_convert_geom_offset_origin_to_com,
dim=geom_bid.shape[0],
inputs=[body_com, geom_bid, geom_offset],
outputs=[geom_offset_com],
device=body_com.device,
)
def update_body_inertias(model: RigidBodiesModel, data: RigidBodiesData):
wp.launch(
_update_body_inertias,
dim=model.num_bodies,
inputs=[
# Inputs:
model.i_I_i,
model.inv_i_I_i,
data.q_i,
# Outputs:
data.I_i,
data.inv_I_i,
],
)
def update_body_wrenches(model: RigidBodiesModel, data: RigidBodiesData):
wp.launch(
_update_body_wrenches,
dim=model.num_bodies,
inputs=[
# Inputs:
data.w_a_i,
data.w_j_i,
data.w_l_i,
data.w_c_i,
data.w_e_i,
# Outputs:
data.w_i,
],
)
def convert_body_origin_to_com(
body_com: wp.array,
body_q: wp.array,
body_q_com: wp.array,
body_wid: wp.array | None = None,
world_mask: wp.array | None = None,
):
wp.launch(
_convert_body_origin_to_com,
dim=body_com.shape[0],
inputs=[world_mask, body_wid, body_com, body_q],
outputs=[body_q_com],
device=body_com.device,
)
def convert_body_com_to_origin(
body_com: wp.array,
body_q_com: wp.array,
body_q: wp.array,
body_wid: wp.array | None = None,
world_mask: wp.array | None = None,
):
wp.launch(
_convert_body_com_to_origin,
dim=body_com.shape[0],
inputs=[world_mask, body_wid, body_com, body_q_com],
outputs=[body_q],
device=body_com.device,
)
def convert_base_origin_to_com(
base_body_index: wp.array,
body_com: wp.array,
base_q: wp.array,
base_q_com: wp.array,
):
wp.launch(
_convert_base_origin_to_com,
dim=base_body_index.shape[0],
inputs=[base_body_index, body_com, base_q],
outputs=[base_q_com],
)
================================================
FILE: newton/_src/solvers/kamino/_src/core/builder.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Constrained Rigid Multi-Body Model Builder
"""
from __future__ import annotations
import copy
import numpy as np
import warp as wp
from .....geometry.flags import ShapeFlags
from .bodies import RigidBodiesModel, RigidBodyDescriptor
from .geometry import GeometriesModel, GeometryDescriptor
from .gravity import GravityDescriptor, GravityModel
from .joints import (
JointActuationType,
JointDescriptor,
JointDoFType,
JointsModel,
)
from .materials import MaterialDescriptor, MaterialManager, MaterialPairProperties, MaterialPairsModel, MaterialsModel
from .math import FLOAT32_EPS
from .model import ModelKamino, ModelKaminoInfo
from .shapes import ShapeDescriptorType, is_explicit_geo_type, max_contacts_for_shape_pair
from .size import SizeKamino
from .time import TimeModel
from .types import Axis, float32, int32, mat33f, transformf, vec2i, vec3f, vec4f, vec6f
from .world import WorldDescriptor
###
# Module interface
###
__all__ = [
"ModelBuilderKamino",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Containers
###
class ModelBuilderKamino:
"""
A class to facilitate construction of simulation models.
"""
def __init__(self, default_world: bool = False):
"""
Initializes a new empty model builder.
Args:
default_world (bool): Whether to create a default world upon initialization.
If True, a default world will be created. Defaults to False.
"""
# Meta-data
self._num_worlds: int = 0
self._device: wp.DeviceLike = None
self._requires_grad: bool = False
# Declare and initialize counters
self._num_bodies: int = 0
self._num_joints: int = 0
self._num_geoms: int = 0
self._num_materials: int = 0
self._num_bdofs: int = 0
self._num_joint_coords: int = 0
self._num_joint_dofs: int = 0
self._num_joint_passive_coords: int = 0
self._num_joint_passive_dofs: int = 0
self._num_joint_actuated_coords: int = 0
self._num_joint_actuated_dofs: int = 0
self._num_joint_cts: int = 0
self._num_joint_kinematic_cts: int = 0
self._num_joint_dynamic_cts: int = 0
# Contact capacity settings
self._max_contacts_per_pair: int | None = None
# Declare per-world model descriptor sets
self._up_axes: list[Axis] = []
self._worlds: list[WorldDescriptor] = []
self._gravity: list[GravityDescriptor] = []
self._bodies: list[RigidBodyDescriptor] = []
self._joints: list[JointDescriptor] = []
self._geoms: list[GeometryDescriptor] = []
# Declare a global material manager
self._materials: MaterialManager = MaterialManager()
self._num_materials = 1
# Create a default world if requested
if default_world:
self.add_world()
@property
def max_contacts_per_pair(self) -> int | None:
"""Maximum contacts per geometry pair override. When set, caps the per-pair contact count
in `compute_required_contact_capacity()`, reducing the Delassus matrix size."""
return self._max_contacts_per_pair
@max_contacts_per_pair.setter
def max_contacts_per_pair(self, value: int | None):
self._max_contacts_per_pair = value
@property
def num_worlds(self) -> int:
"""Returns the number of worlds represented in the model."""
return self._num_worlds
@property
def num_bodies(self) -> int:
"""Returns the number of bodies contained in the model."""
return self._num_bodies
@property
def num_joints(self) -> int:
"""Returns the number of joints contained in the model."""
return self._num_joints
@property
def num_geoms(self) -> int:
"""Returns the number of geometries contained in the model."""
return self._num_geoms
@property
def num_materials(self) -> int:
"""Returns the number of materials contained in the model."""
return self._num_materials
@property
def num_body_dofs(self) -> int:
"""Returns the number of body degrees of freedom contained in the model."""
return self._num_bdofs
@property
def num_joint_coords(self) -> int:
"""Returns the number of joint coordinates contained in the model."""
return self._num_joint_coords
@property
def num_joint_dofs(self) -> int:
"""Returns the number of joint degrees of freedom contained in the model."""
return self._num_joint_dofs
@property
def num_passive_joint_coords(self) -> int:
"""Returns the number of passive joint coordinates contained in the model."""
return self._num_joint_passive_coords
@property
def num_passive_joint_dofs(self) -> int:
"""Returns the number of passive joint degrees of freedom contained in the model."""
return self._num_joint_passive_dofs
@property
def num_actuated_joint_coords(self) -> int:
"""Returns the number of actuated joint coordinates contained in the model."""
return self._num_joint_actuated_coords
@property
def num_actuated_joint_dofs(self) -> int:
"""Returns the number of actuated joint degrees of freedom contained in the model."""
return self._num_joint_actuated_dofs
@property
def num_joint_cts(self) -> int:
"""Returns the total number of joint constraints contained in the model."""
return self._num_joint_cts
@property
def num_dynamic_joint_cts(self) -> int:
"""Returns the number of dynamic joint constraints contained in the model."""
return self._num_joint_dynamic_cts
@property
def num_kinematic_joint_cts(self) -> int:
"""Returns the number of kinematic joint constraints contained in the model."""
return self._num_joint_kinematic_cts
@property
def worlds(self) -> list[WorldDescriptor]:
"""Returns the list of world descriptors contained in the model."""
return self._worlds
@property
def up_axes(self) -> list[Axis]:
"""Returns the list of up axes for each world contained in the model."""
return self._up_axes
@property
def gravity(self) -> list[GravityDescriptor]:
"""Returns the list of gravity descriptors for each world contained in the model."""
return self._gravity
@property
def bodies(self) -> list[RigidBodyDescriptor]:
"""Returns the list of body descriptors contained in the model."""
return self._bodies
@property
def joints(self) -> list[JointDescriptor]:
"""Returns the list of joint descriptors contained in the model."""
return self._joints
@property
def geoms(self) -> list[GeometryDescriptor]:
"""Returns the list of geometry descriptors contained in the model."""
return self._geoms
@property
def materials(self) -> list[MaterialDescriptor]:
"""Returns the list of material descriptors contained in the model."""
return self._materials.materials
###
# Model Construction
###
def add_world(
self,
name: str = "world",
uid: str | None = None,
up_axis: Axis | None = None,
gravity: GravityDescriptor | None = None,
) -> int:
"""
Add a new world to the model.
Args:
name (str): The name of the world.
uid (str | None): The unique identifier of the world.\n
If None, a UUID will be generated.
up_axis (Axis | None): The up axis of the world.\n
If None, Axis.Z will be used.
gravity (GravityDescriptor | None): The gravity descriptor of the world.\n
If None, a default gravity descriptor will be used.
Returns:
int: The index of the newly added world.
"""
# Create a new world descriptor
self._worlds.append(WorldDescriptor(name=name, uid=uid, wid=self._num_worlds))
# Set up axis
if up_axis is None:
up_axis = Axis.Z
self._up_axes.append(up_axis)
# Set gravity
if gravity is None:
gravity = GravityDescriptor()
self._gravity.append(gravity)
# Register the default material in the new world
self._worlds[-1].add_material(self._materials.default)
# Update world counter
self._num_worlds += 1
# Return the new world index
return self._worlds[-1].wid
def add_rigid_body(
self,
m_i: float,
i_I_i: mat33f,
q_i_0: transformf,
u_i_0: vec6f | None = None,
name: str | None = None,
uid: str | None = None,
world_index: int = 0,
) -> int:
"""
Add a rigid body entity to the model using explicit specifications.
Args:
m_i (float): The mass of the body.
i_I_i (mat33f): The inertia tensor of the body.
q_i_0 (transformf): The initial pose of the body.
u_i_0 (vec6f): The initial velocity of the body.
name (str | None): The name of the body.
uid (str | None): The unique identifier of the body.
world_index (int): The index of the world to which the body will be added.\n
Defaults to the first world with index `0`.
Returns:
int: The index of the newly added body.
"""
# Create a rigid body descriptor from the provided specifications
# NOTE: Specifying a name is required by the base descriptor class,
# but we allow it to be optional here for convenience. Thus, we
# generate a default name if none is provided.
body = RigidBodyDescriptor(
name=name if name is not None else f"body_{self._num_bodies}",
uid=uid,
m_i=m_i,
i_I_i=i_I_i,
q_i_0=q_i_0,
u_i_0=u_i_0 if u_i_0 is not None else vec6f(0.0),
)
# Add the body descriptor to the model
return self.add_rigid_body_descriptor(body, world_index=world_index)
def add_rigid_body_descriptor(self, body: RigidBodyDescriptor, world_index: int = 0) -> int:
"""
Add a rigid body entity to the model using a descriptor object.
Args:
body (RigidBodyDescriptor): The body descriptor to be added.
world_index (int): The index of the world to which the body will be added.\n
Defaults to the first world with index `0`.
Returns:
int: The body index of the newly added body w.r.t its world.
"""
# Check if the descriptor is valid
if not isinstance(body, RigidBodyDescriptor):
raise TypeError(f"Invalid body descriptor type: {type(body)}. Must be `RigidBodyDescriptor`.")
# Check if body properties are valid
self._check_body_inertia(body.m_i, body.i_I_i)
self._check_body_pose(body.q_i_0)
# Check if the world index is valid
world = self._check_world_index(world_index)
# Append body model data
world.add_body(body)
self._insert_entity(self._bodies, body, world_index=world_index)
# Update model-wide counters
self._num_bodies += 1
self._num_bdofs += 6
# Return the new body index
return body.bid
def add_joint(
self,
act_type: JointActuationType,
dof_type: JointDoFType,
bid_B: int,
bid_F: int,
B_r_Bj: vec3f,
F_r_Fj: vec3f,
X_j: mat33f,
q_j_min: list[float] | float | None = None,
q_j_max: list[float] | float | None = None,
dq_j_max: list[float] | float | None = None,
tau_j_max: list[float] | float | None = None,
a_j: list[float] | float | None = None,
b_j: list[float] | float | None = None,
k_p_j: list[float] | float | None = None,
k_d_j: list[float] | float | None = None,
name: str | None = None,
uid: str | None = None,
world_index: int = 0,
) -> int:
"""
Add a joint entity to the model using explicit specifications.
Args:
act_type (JointActuationType): The actuation type of the joint.
dof_type (JointDoFType): The degree of freedom type of the joint.
bid_B (int): The index of the body on the "base" side of the joint.
bid_F (int): The index of the body on the "follower" side of the joint.
B_r_Bj (vec3f): The position of the joint in the base body frame.
F_r_Fj (vec3f): The position of the joint in the follower body frame.
X_j (mat33f): The orientation of the joint frame relative to the base body frame.
q_j_min (list[float] | float | None): The minimum joint coordinate limits.
q_j_max (list[float] | float | None): The maximum joint coordinate limits.
dq_j_max (list[float] | float | None): The maximum joint velocity limits.
tau_j_max (list[float] | float | None): The maximum joint effort limits.
a_j (list[float] | float | None): The joint armature along each DoF.
b_j (list[float] | float | None): The joint damping along each DoF.
k_p_j (list[float] | float | None): The joint proportional gain along each DoF.
k_d_j (list[float] | float | None): The joint derivative gain along each DoF.
name (str | None): The name of the joint.
uid (str | None): The unique identifier of the joint.
world_index (int): The index of the world to which the joint will be added.\n
Defaults to the first world with index `0`.
Returns:
int: The index of the newly added joint.
"""
# Check if the actuation type is valid
if not isinstance(act_type, JointActuationType):
raise TypeError(f"Invalid actuation type: {act_type}. Must be `JointActuationType`.")
# Check if the DoF type is valid
if not isinstance(dof_type, JointDoFType):
raise TypeError(f"Invalid DoF type: {dof_type}. Must be `JointDoFType`.")
# Create a joint descriptor from the provided specifications
# NOTE: Specifying a name is required by the base descriptor class,
# but we allow it to be optional here for convenience. Thus, we
# generate a default name if none is provided.
joint = JointDescriptor(
name=name if name is not None else f"joint_{self._num_joints}",
uid=uid,
act_type=act_type,
dof_type=dof_type,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=B_r_Bj,
F_r_Fj=F_r_Fj,
X_j=X_j,
q_j_min=q_j_min,
q_j_max=q_j_max,
dq_j_max=dq_j_max,
tau_j_max=tau_j_max,
a_j=a_j,
b_j=b_j,
k_p_j=k_p_j,
k_d_j=k_d_j,
)
# Add the body descriptor to the model
return self.add_joint_descriptor(joint, world_index=world_index)
def add_joint_descriptor(self, joint: JointDescriptor, world_index: int = 0) -> int:
"""
Add a joint entity to the model by descriptor.
Args:
joint (JointDescriptor):
The joint descriptor to be added.
world_index (int):
The index of the world to which the joint will be added.\n
Defaults to the first world with index `0`.
Returns:
int: The joint index of the newly added joint w.r.t its world.
"""
# Check if the descriptor is valid
if not isinstance(joint, JointDescriptor):
raise TypeError(f"Invalid joint descriptor type: {type(joint)}. Must be `JointDescriptor`.")
# Check if the world index is valid
world = self._check_world_index(world_index)
# Append joint model data
world.add_joint(joint)
self._insert_entity(self._joints, joint, world_index=world_index)
# Update model-wide counters
self._num_joints += 1
self._num_joint_coords += joint.num_coords
self._num_joint_dofs += joint.num_dofs
self._num_joint_passive_coords += joint.num_passive_coords
self._num_joint_passive_dofs += joint.num_passive_dofs
self._num_joint_actuated_coords += joint.num_actuated_coords
self._num_joint_actuated_dofs += joint.num_actuated_dofs
self._num_joint_cts += joint.num_cts
self._num_joint_dynamic_cts += joint.num_dynamic_cts
self._num_joint_kinematic_cts += joint.num_kinematic_cts
# Return the new joint index
return joint.jid
def add_geometry(
self,
body: int = -1,
shape: ShapeDescriptorType | None = None,
offset: transformf | None = None,
material: str | int | None = None,
group: int = 1,
collides: int = 1,
max_contacts: int = 0,
gap: float = 0.0,
margin: float = 0.0,
name: str | None = None,
uid: str | None = None,
world_index: int = 0,
) -> int:
"""
Add a geometry entity to the model using explicit specifications.
Args:
body (int):
The index of the body to which the geometry will be attached.\n
Defaults to -1 (world).
shape (ShapeDescriptorType | None):
The shape descriptor of the geometry.
offset (transformf | None):
The local offset of the geometry relative to the body frame.
material (str | int | None):
The name or index of the material assigned to the geometry.
max_contacts (int):
The maximum number of contact points for the geometry.\n
Defaults to 0 (unlimited).
group (int):
The collision group of the geometry.\n
Defaults to 1.
collides (int):
The collision mask of the geometry.\n
Defaults to 1.
gap (float):
The collision detection gap of the geometry.\n
Defaults to 0.0.
margin (float):
The artificial surface margin of the geometry.\n
Defaults to 0.0.
name (str | None):
The name of the geometry.\n
If `None`, a default name will be generated based on the current number of geometries in the model.
uid (str | None):
The unique identifier of the geometry.\n
If `None`, a UUID will be generated.
world_index (int):
The index of the world to which the geometry will be added.\n
Defaults to the first world with index `0`.
Returns:
int: The index of the newly added collision geometry.
"""
# Set the default material if not provided
if material is None:
material = self._materials.default.name
# Otherwise, check if the material exists
else:
if not self._materials.has_material(material):
raise ValueError(
f"Material '{material}' does not exist. "
"Please add the material using `add_material()` before assigning it to a geometry."
)
# If the shape is already provided, check if it's valid
if shape is not None:
if not isinstance(shape, ShapeDescriptorType):
raise ValueError(
f"Shape '{shape}' must be a valid type.\n"
"See `ShapeDescriptorType` for the list of supported shapes."
)
# Create a joint descriptor from the provided specifications
# NOTE: Specifying a name is required by the base descriptor class,
# but we allow it to be optional here for convenience. Thus, we
# generate a default name if none is provided.
geom = GeometryDescriptor(
name=name if name is not None else f"cgeom_{self._num_geoms}",
uid=uid,
body=body,
offset=offset if offset is not None else transformf(),
shape=shape,
material=self._materials[material],
mid=self._materials.index(material),
group=group,
collides=collides,
max_contacts=max_contacts,
gap=gap,
margin=margin,
)
# Add the body descriptor to the model
return self.add_geometry_descriptor(geom, world_index=world_index)
def add_geometry_descriptor(self, geom: GeometryDescriptor, world_index: int = 0) -> int:
"""
Add a geometry to the model by descriptor.
Args:
geom (GeometryDescriptor):
The geometry descriptor to be added.
world_index (int):
The index of the world to which the geometry will be added.\n
Defaults to the first world with index `0`.
Returns:
int: The geometry index of the newly added geometry w.r.t its world.
"""
# Check if the descriptor is valid
if not isinstance(geom, GeometryDescriptor):
raise TypeError(f"Invalid geometry descriptor type: {type(geom)}. Must be `GeometryDescriptor`.")
# Check if the world index is valid
world = self._check_world_index(world_index)
# If the geom material is not assigned, set it to the global default
if geom.mid is None:
geom.mid = self._materials.default.mid
# Append body model data
world.add_geometry(geom)
self._insert_entity(self._geoms, geom, world_index=world_index)
# Update model-wide counters
self._num_geoms += 1
# Return the new geometry index
return geom.gid
def add_material(self, material: MaterialDescriptor, world_index: int = 0) -> int:
"""
Add a material to the model.
Args:
material (MaterialDescriptor): The material descriptor to be added.
world_index (int): The index of the world to which the material will be added.\n
Defaults to the first world with index `0`.
"""
# Check if the world index is valid
world = self._check_world_index(world_index)
# Check if the material is valid
if not isinstance(material, MaterialDescriptor):
raise TypeError(f"Invalid material type: {type(material)}. Must be `MaterialDescriptor`.")
# Register the material in the material manager
world.add_material(material)
# Update model-wide counter
self._num_materials += 1
return self._materials.register(material)
def add_builder(self, other: ModelBuilderKamino):
"""
Extends the contents of the current ModelBuilderKamino with those of another.
Each builder represents a distinct world, and this method allows for the
combination of multiple worlds into a single model. The method ensures that the
indices of the elements in the other builder are adjusted to account for the
existing elements in the current builder, preventing any index conflicts.
Arguments:
other (ModelBuilderKamino): The other ModelBuilderKamino whose contents are to be added to the current.
Raises:
ValueError: If the provided builder is not of type `ModelBuilderKamino`.
"""
# Check if the other builder is of valid type
if not isinstance(other, ModelBuilderKamino):
raise TypeError(f"Invalid builder type: {type(other)}. Must be a ModelBuilderKamino instance.")
# Make a deep copy of the other builder to avoid modifying the original
# TODO: How can we avoid this deep copy to improve performance
# while avoiding copying expensive data like meshes?
_other = copy.deepcopy(other)
# Append the other per-world descriptors
self._worlds.extend(_other._worlds)
self._gravity.extend(_other._gravity)
self._up_axes.extend(_other._up_axes)
# Append the other per-entity descriptors
self._bodies.extend(_other._bodies)
self._joints.extend(_other._joints)
self._geoms.extend(_other._geoms)
# Append the other materials
self._materials.merge(_other._materials)
# Update the world index of the entities in the
# other builder and update model-wide counters
for w, world in enumerate(_other._worlds):
# Offset world index of the other builder's world
world.wid = self._num_worlds + w
# Offset world indices of the other builders entities
for body in self._bodies[self._num_bodies : self._num_bodies + world.num_bodies]:
body.wid = self._num_worlds + w
for joint in self._joints[self._num_joints : self._num_joints + world.num_joints]:
joint.wid = self._num_worlds + w
for geom in self._geoms[self._num_geoms : self._num_geoms + world.num_geoms]:
geom.wid = self._num_worlds + w
# Update model-wide counters
self._num_bodies += world.num_bodies
self._num_joints += world.num_joints
self._num_geoms += world.num_geoms
self._num_bdofs += 6 * world.num_bodies
self._num_joint_coords += world.num_joint_coords
self._num_joint_dofs += world.num_joint_dofs
self._num_joint_passive_coords += world.num_passive_joint_coords
self._num_joint_passive_dofs += world.num_passive_joint_dofs
self._num_joint_actuated_coords += world.num_actuated_joint_coords
self._num_joint_actuated_dofs += world.num_actuated_joint_dofs
self._num_joint_cts += world.num_joint_cts
self._num_joint_dynamic_cts += world.num_dynamic_joint_cts
self._num_joint_kinematic_cts += world.num_kinematic_joint_cts
# Update the number of worlds
self._num_worlds += _other._num_worlds
###
# Configurations
###
def set_up_axis(self, axis: Axis, world_index: int = 0):
"""
Set the up axis for a specific world.
Args:
axis (Axis): The new up axis to be set.
world_index (int): The index of the world for which to set the up axis.\n
Defaults to the first world with index `0`.
Raises:
TypeError: If the provided axis is not of type `Axis`.
"""
# Check if the world index is valid
self._check_world_index(world_index)
# Check if the axis is valid
if not isinstance(axis, Axis):
raise TypeError(f"ModelBuilderKamino: Invalid axis type: {type(axis)}. Must be `Axis`.")
# Set the new up axis
self._up_axes[world_index] = axis
def set_gravity(self, gravity: GravityDescriptor, world_index: int = 0):
"""
Set the gravity descriptor for a specific world.
Args:
gravity (GravityDescriptor): The new gravity descriptor to be set.
world_index (int): The index of the world for which to set the gravity descriptor.\n
Defaults to the first world with index `0`.
Raises:
TypeError: If the provided gravity descriptor is not of type `GravityDescriptor`.
"""
# Check if the world index is valid
self._check_world_index(world_index)
# Check if the gravity descriptor is valid
if not isinstance(gravity, GravityDescriptor):
raise TypeError(f"Invalid gravity descriptor type: {type(gravity)}. Must be `GravityDescriptor`.")
# Set the new gravity configurations
self._gravity[world_index] = gravity
def set_default_material(self, material: MaterialDescriptor, world_index: int = 0):
"""
Sets the default material for the model.
Raises an error if the material is not registered.
"""
# Check if the world index is valid
world = self._check_world_index(world_index)
# Check if the material is valid
if not isinstance(material, MaterialDescriptor):
raise TypeError(f"Invalid material type: {type(material)}. Must be `MaterialDescriptor`.")
# Reset the default material of the world
world.set_material(material, 0)
# Set the default material in the material manager
self._materials.default = material
def set_material_pair(
self,
first: int | str | MaterialDescriptor,
second: int | str | MaterialDescriptor,
material_pair: MaterialPairProperties,
world_index: int = 0,
):
"""
Sets the material pair properties for two materials.
Args:
first (int | str | MaterialDescriptor): The first material (by index, name, or descriptor).
second (int | str | MaterialDescriptor): The second material (by index, name, or descriptor).
material_pair (MaterialPairProperties): The material pair properties to be set.
world_index (int): The index of the world for which to set the material pair properties.\n
Defaults to the first world with index `0`.
"""
# Check if the world index is valid
self._check_world_index(world_index)
# Extract the material names if arguments are descriptors
first_id = first.name if isinstance(first, MaterialDescriptor) else first
second_id = second.name if isinstance(second, MaterialDescriptor) else second
# Register the material pair in the material manager
self._materials.configure_pair(first=first_id, second=second_id, material_pair=material_pair)
def set_base_body(self, body_key: int | str, world_index: int = 0):
"""
Set the base body for a specific world specified either by name or by index.
Args:
body_key (int | str): Identifier of the body to be set as the base body.
Can be either the body's index (within the world) or its name.
world_index (int): The index of the world for which to set the base body.\n
Defaults to the first world with index `0`.
"""
# Check if the world index is valid
world = self._check_world_index(world_index)
# Find the body and set it as base in the world descriptor
if isinstance(body_key, int):
world.set_base_body(body_key)
return
elif isinstance(body_key, str):
for body in self.bodies:
if body.wid == world_index and body.name == body_key:
world.set_base_body(body.bid)
return
raise ValueError(f"Failed to identify the base body in world `{world_index}` given key `{body_key}`.")
def set_base_joint(self, joint_key: int | str, world_index: int = 0):
"""
Set the base joint for a specific world specified either by name or by index.
Args:
joint_key (int | str): Identifier of the joint to be set as the base joint.
Can be either the joint's index (within the world) or its name.
world_index (int): The index of the world for which to set the base joint.\n
Defaults to the first world with index `0`.
"""
# Check if the world index is valid
world = self._check_world_index(world_index)
# Find the joint and set it as base in the world descriptor
if isinstance(joint_key, int):
world.set_base_joint(joint_key)
return
elif isinstance(joint_key, str):
for joint in self.joints:
if joint.wid == world_index and joint.name == joint_key:
world.set_base_joint(joint.jid)
return
raise ValueError(f"Failed to identify the base joint in world `{world_index}` given key `{joint_key}`.")
###
# Model Compilation
###
def finalize(
self, device: wp.DeviceLike = None, requires_grad: bool = False, base_auto: bool = True
) -> ModelKamino:
"""
Constructs a ModelKamino object from the current ModelBuilderKamino.
All description data contained in the builder is compiled into a ModelKamino
object, allocating the necessary data structures on the target device.
Args:
device (wp.DeviceLike): The target device for the model data.\n
If None, the default/preferred device will determined by Warp.
requires_grad (bool): Whether the model data should support gradients.\n
Defaults to False.
base_auto (bool): Whether to automatically select a base body,
and if possible, a base joint, if neither was set.
Returns:
ModelKamino: The constructed ModelKamino object containing the time-invariant simulation data.
"""
# Number of model worlds
num_worlds = len(self._worlds)
if num_worlds == 0:
raise ValueError("ModelBuilderKamino: Cannot finalize an empty model with zero worlds.")
if num_worlds != self._num_worlds:
raise ValueError(
"ModelBuilderKamino: Inconsistent number of worlds: "
f"expected {self._num_worlds}, but found {num_worlds}."
)
###
# Pre-processing
###
# First compute per-world offsets before proceeding
# NOTE: Computing world offsets only during the finalization step allows
# users to add entities in any manner. For example, users can import a model
# via USD, and then ad-hoc modify the model by adding bodies, joints, geoms, etc.
self._compute_world_offsets()
# Validate base body/joint data for each world, and fill in missing data if possible
for w, world in enumerate(self._worlds):
if world.has_base_joint:
joint_idx = world.joints_idx_offset + world.base_joint_idx
follower_idx = self._joints[joint_idx].bid_F # Note: index among world bodies
if world.has_base_body: # Ensure base joint & body are compatible if both were set
if world.base_body_idx != follower_idx:
raise ValueError(
f"ModelBuilderKamino: Inconsistent base body and base joint for world {world.name} ({w})"
)
else: # Set base body to be the follower of the base joint
world.set_base_body(follower_idx)
elif not world.has_base_body and base_auto:
world.set_base_body(0) # Set the base body as the first body
for jt_idx, joint in enumerate(
self._joints[world.joints_idx_offset : world.joints_idx_offset + world.num_joints]
):
if joint.wid == w and joint.is_unary and joint.is_connected_to_body(world.base_body_idx):
# If we find a unary joint connecting the base body to the world, we set this as the base joint
world.set_base_joint(jt_idx)
break
###
# ModelKamino data collection
###
# Initialize the info data collections
info_nb = []
info_nj = []
info_njp = []
info_nja = []
info_nji = []
info_ng = []
info_nbd = []
info_njq = []
info_njd = []
info_njpq = []
info_njpd = []
info_njaq = []
info_njad = []
info_njc = []
info_njdc = []
info_njkc = []
info_bio = []
info_jio = []
info_gio = []
info_bdio = []
info_jqio = []
info_jdio = []
info_jpqio = []
info_jpdio = []
info_jaqio = []
info_jadio = []
info_jcio = []
info_jdcio = []
info_jkcio = []
info_base_bid = []
info_base_jid = []
info_mass_min = []
info_mass_max = []
info_mass_total = []
info_inertia_total = []
# Initialize the gravity data collections
gravity_g_dir_acc = []
gravity_vector = []
# Initialize the body data collections
bodies_label = []
bodies_wid = []
bodies_bid = []
bodies_i_r_com_i = []
bodies_m_i = []
bodies_inv_m_i = []
bodies_i_I_i = []
bodies_inv_i_I_i = []
bodies_q_i_0 = []
bodies_u_i_0 = []
# Initialize the joint data collections
joints_label = []
joints_wid = []
joints_jid = []
joints_dofid = []
joints_actid = []
joints_q_j_0 = []
joints_dq_j_0 = []
joints_bid_B = []
joints_bid_F = []
joints_B_r_Bj = []
joints_F_r_Fj = []
joints_X_j = []
joints_q_j_min = []
joints_q_j_max = []
joints_qd_j_max = []
joints_tau_j_max = []
joints_a_j = []
joints_b_j = []
joints_k_p_j = []
joints_k_d_j = []
joints_ncoords_j = []
joints_ndofs_j = []
joints_ncts_j = []
joints_nkincts_j = []
joints_ndyncts_j = []
joints_q_start = []
joints_dq_start = []
joints_pq_start = []
joints_pdq_start = []
joints_aq_start = []
joints_adq_start = []
joints_cts_start = []
joints_dcts_start = []
joints_kcts_start = []
# Initialize the collision geometry data collections
geoms_label = []
geoms_wid = []
geoms_gid = []
geoms_bid = []
geoms_type = []
geoms_flags = []
geoms_ptr = []
geoms_params = []
geoms_offset = []
geoms_material = []
geoms_group = []
geoms_collides = []
geoms_gap = []
geoms_margin = []
# Initialize the material data collections
materials_rest = []
materials_static_fric = []
materials_dynamic_fric = []
mpairs_rest = []
mpairs_static_fric = []
mpairs_dynamic_fric = []
# A helper function to collect model info data
def collect_model_info_data():
for world in self._worlds:
# First collect the immutable counts and
# index offsets for bodies and joints
info_nb.append(world.num_bodies)
info_nj.append(world.num_joints)
info_njp.append(world.num_passive_joints)
info_nja.append(world.num_actuated_joints)
info_nji.append(world.num_dynamic_joints)
info_ng.append(world.num_geoms)
info_nbd.append(world.num_body_dofs)
info_njq.append(world.num_joint_coords)
info_njd.append(world.num_joint_dofs)
info_njpq.append(world.num_passive_joint_coords)
info_njpd.append(world.num_passive_joint_dofs)
info_njaq.append(world.num_actuated_joint_coords)
info_njad.append(world.num_actuated_joint_dofs)
info_njc.append(world.num_joint_cts)
info_njdc.append(world.num_dynamic_joint_cts)
info_njkc.append(world.num_kinematic_joint_cts)
info_bio.append(world.bodies_idx_offset)
info_jio.append(world.joints_idx_offset)
info_gio.append(world.geoms_idx_offset)
# Collect the model mass and inertia data
info_mass_min.append(world.mass_min)
info_mass_max.append(world.mass_max)
info_mass_total.append(world.mass_total)
info_inertia_total.append(world.inertia_total)
# Collect the index offsets for bodies and joints
for world in self._worlds:
info_bdio.append(world.body_dofs_idx_offset)
info_jqio.append(world.joint_coords_idx_offset)
info_jdio.append(world.joint_dofs_idx_offset)
info_jpqio.append(world.joint_passive_coords_idx_offset)
info_jpdio.append(world.joint_passive_dofs_idx_offset)
info_jaqio.append(world.joint_actuated_coords_idx_offset)
info_jadio.append(world.joint_actuated_dofs_idx_offset)
info_jcio.append(world.joint_cts_idx_offset)
info_jdcio.append(world.joint_dynamic_cts_idx_offset)
info_jkcio.append(world.joint_kinematic_cts_idx_offset)
info_base_bid.append((world.base_body_idx + world.bodies_idx_offset) if world.has_base_body else -1)
info_base_jid.append((world.base_joint_idx + world.joints_idx_offset) if world.has_base_joint else -1)
# A helper function to collect model gravity data
def collect_gravity_model_data():
for w in range(num_worlds):
gravity_g_dir_acc.append(self._gravity[w].dir_accel())
gravity_vector.append(self._gravity[w].vector())
# A helper function to collect model bodies data
def collect_body_model_data():
for body in self._bodies:
bodies_label.append(body.name)
bodies_wid.append(body.wid)
bodies_bid.append(body.bid)
bodies_i_r_com_i.append(body.i_r_com_i)
bodies_m_i.append(body.m_i)
bodies_inv_m_i.append(1.0 / body.m_i)
bodies_i_I_i.append(body.i_I_i)
bodies_inv_i_I_i.append(wp.inverse(body.i_I_i))
bodies_q_i_0.append(body.q_i_0)
bodies_u_i_0.append(body.u_i_0)
# A helper function to collect model joints data
def collect_joint_model_data():
for joint in self._joints:
world_bio = self._worlds[joint.wid].bodies_idx_offset
joints_label.append(joint.name)
joints_wid.append(joint.wid)
joints_jid.append(joint.jid)
joints_dofid.append(joint.dof_type.value)
joints_actid.append(joint.act_type.value)
joints_B_r_Bj.append(joint.B_r_Bj)
joints_F_r_Fj.append(joint.F_r_Fj)
joints_X_j.append(joint.X_j)
joints_q_j_0.extend(joint.dof_type.reference_coords)
joints_dq_j_0.extend(joint.dof_type.num_dofs * [0.0])
joints_q_j_min.extend(joint.q_j_min)
joints_q_j_max.extend(joint.q_j_max)
joints_qd_j_max.extend(joint.dq_j_max)
joints_tau_j_max.extend(joint.tau_j_max)
joints_a_j.extend(joint.a_j)
joints_b_j.extend(joint.b_j)
joints_k_p_j.extend(joint.k_p_j)
joints_k_d_j.extend(joint.k_d_j)
joints_ncoords_j.append(joint.num_coords)
joints_ndofs_j.append(joint.num_dofs)
joints_ncts_j.append(joint.num_cts)
joints_ndyncts_j.append(joint.num_dynamic_cts)
joints_nkincts_j.append(joint.num_kinematic_cts)
joints_q_start.append(joint.coords_offset)
joints_dq_start.append(joint.dofs_offset)
joints_pq_start.append(joint.passive_coords_offset)
joints_pdq_start.append(joint.passive_dofs_offset)
joints_aq_start.append(joint.actuated_coords_offset)
joints_adq_start.append(joint.actuated_dofs_offset)
joints_cts_start.append(joint.cts_offset)
joints_dcts_start.append(joint.dynamic_cts_offset)
joints_kcts_start.append(joint.kinematic_cts_offset)
joints_bid_B.append(joint.bid_B + world_bio if joint.bid_B >= 0 else -1)
joints_bid_F.append(joint.bid_F + world_bio if joint.bid_F >= 0 else -1)
# A helper function to create geometry pointers
# NOTE: This also finalizes the mesh/SDF/HField data on the device
def make_geometry_source_pointer(geom: GeometryDescriptor, mesh_geoms: dict, device) -> int:
# Append to data pointers array of the shape has a Mesh, SDF or HField source
if is_explicit_geo_type(geom.shape.type):
geom_uid = geom.uid
# If the geometry has a Mesh, SDF or HField source,
# finalize it and retrieve the mesh pointer/index
if geom_uid not in mesh_geoms:
mesh_geoms[geom_uid] = geom.shape.data.finalize(device=device)
# Return the mesh data pointer/index
return mesh_geoms[geom_uid]
# Otherwise, append a null (i.e. zero-valued) pointer
else:
return 0
# A helper function to collect model collision geometries data
def collect_geometry_model_data():
cgeom_meshes = {}
for geom in self._geoms:
geoms_label.append(geom.name)
geoms_wid.append(geom.wid)
geoms_gid.append(geom.gid)
geoms_bid.append(geom.body + self._worlds[geom.wid].bodies_idx_offset if geom.body >= 0 else -1)
geoms_type.append(geom.shape.type.value)
geoms_flags.append(geom.flags)
geoms_params.append(geom.shape.paramsvec)
geoms_offset.append(geom.offset)
geoms_material.append(geom.mid)
geoms_group.append(geom.group)
geoms_collides.append(geom.collides)
geoms_gap.append(geom.gap)
geoms_margin.append(geom.margin)
geoms_ptr.append(make_geometry_source_pointer(geom, cgeom_meshes, device))
# A helper function to collect model material-pairs data
def collect_material_pairs_model_data():
materials_rest.append(self._materials.restitution_vector())
materials_static_fric.append(self._materials.static_friction_vector())
materials_dynamic_fric.append(self._materials.dynamic_friction_vector())
mpairs_rest.append(self._materials.restitution_matrix())
mpairs_static_fric.append(self._materials.static_friction_matrix())
mpairs_dynamic_fric.append(self._materials.dynamic_friction_matrix())
# Collect model data
collect_model_info_data()
collect_gravity_model_data()
collect_body_model_data()
collect_joint_model_data()
collect_geometry_model_data()
collect_material_pairs_model_data()
# Post-processing of reference coords of FREE joints to match body frames
for joint in self._joints:
if joint.dof_type == JointDoFType.FREE:
body = self._bodies[joint.bid_F + self._worlds[joint.wid].bodies_idx_offset]
qj_start = joint.coords_offset + self._worlds[joint.wid].joint_coords_idx_offset
joints_q_j_0[qj_start : qj_start + joint.num_coords] = [*body.q_i_0]
###
# Host-side model size meta-data
###
# Compute the sum/max of model entities
model_size = SizeKamino(
num_worlds=num_worlds,
sum_of_num_bodies=self._num_bodies,
max_of_num_bodies=max([world.num_bodies for world in self._worlds]),
sum_of_num_joints=self._num_joints,
max_of_num_joints=max([world.num_joints for world in self._worlds]),
sum_of_num_passive_joints=sum([world.num_passive_joints for world in self._worlds]),
max_of_num_passive_joints=max([world.num_passive_joints for world in self._worlds]),
sum_of_num_actuated_joints=sum([world.num_actuated_joints for world in self._worlds]),
max_of_num_actuated_joints=max([world.num_actuated_joints for world in self._worlds]),
sum_of_num_dynamic_joints=sum([world.num_dynamic_joints for world in self._worlds]),
max_of_num_dynamic_joints=max([world.num_dynamic_joints for world in self._worlds]),
sum_of_num_geoms=self._num_geoms,
max_of_num_geoms=max([world.num_geoms for world in self._worlds]),
sum_of_num_materials=self._materials.num_materials,
max_of_num_materials=self._materials.num_materials,
sum_of_num_material_pairs=self._materials.num_material_pairs,
max_of_num_material_pairs=self._materials.num_material_pairs,
# Compute the sum/max of model coords, DoFs and constraints
sum_of_num_body_dofs=self._num_bdofs,
max_of_num_body_dofs=max([world.num_body_dofs for world in self._worlds]),
sum_of_num_joint_coords=self._num_joint_coords,
max_of_num_joint_coords=max([world.num_joint_coords for world in self._worlds]),
sum_of_num_joint_dofs=self._num_joint_dofs,
max_of_num_joint_dofs=max([world.num_joint_dofs for world in self._worlds]),
sum_of_num_passive_joint_coords=self._num_joint_passive_coords,
max_of_num_passive_joint_coords=max([world.num_passive_joint_coords for world in self._worlds]),
sum_of_num_passive_joint_dofs=self._num_joint_passive_dofs,
max_of_num_passive_joint_dofs=max([world.num_passive_joint_dofs for world in self._worlds]),
sum_of_num_actuated_joint_coords=self._num_joint_actuated_coords,
max_of_num_actuated_joint_coords=max([world.num_actuated_joint_coords for world in self._worlds]),
sum_of_num_actuated_joint_dofs=self._num_joint_actuated_dofs,
max_of_num_actuated_joint_dofs=max([world.num_actuated_joint_dofs for world in self._worlds]),
sum_of_num_joint_cts=self._num_joint_cts,
max_of_num_joint_cts=max([world.num_joint_cts for world in self._worlds]),
sum_of_num_dynamic_joint_cts=self._num_joint_dynamic_cts,
max_of_num_dynamic_joint_cts=max([world.num_dynamic_joint_cts for world in self._worlds]),
sum_of_num_kinematic_joint_cts=self._num_joint_kinematic_cts,
max_of_num_kinematic_joint_cts=max([world.num_kinematic_joint_cts for world in self._worlds]),
# Initialize unilateral counts (limits, and contacts) to zero
sum_of_max_limits=0,
max_of_max_limits=0,
sum_of_max_contacts=0,
max_of_max_contacts=0,
sum_of_max_unilaterals=0,
max_of_max_unilaterals=0,
# Initialize total constraint counts to the same as the joint constraint counts
sum_of_max_total_cts=self._num_joint_cts,
max_of_max_total_cts=max([world.num_joint_cts for world in self._worlds]),
)
###
# Collision detection and contact-allocation meta-data
###
# Generate the lists of collidable and excluded geometry pairs for the entire model
model_collidable_pairs = self.make_collision_candidate_pairs()
model_excluded_pairs = self.make_collision_excluded_pairs()
# Retrieve the number of collidable geoms for each world and
# for the entire model based on the generated candidate pairs
_, model_num_collidables = self.compute_num_collidable_geoms(collidable_geom_pairs=model_collidable_pairs)
# Compute the maximum number of contacts required for the model and each world
# NOTE: This is a conservative estimate based on the maximum per-world geom-pairs
model_required_contacts, world_required_contacts = self.compute_required_contact_capacity(
collidable_geom_pairs=model_collidable_pairs,
max_contacts_per_pair=self._max_contacts_per_pair,
)
###
# On-device data allocation
###
# Allocate the model data on the target device
with wp.ScopedDevice(device):
# Create the immutable model info arrays from the collected data
model_info = ModelKaminoInfo(
num_worlds=num_worlds,
num_bodies=wp.array(info_nb, dtype=int32),
num_joints=wp.array(info_nj, dtype=int32),
num_passive_joints=wp.array(info_njp, dtype=int32),
num_actuated_joints=wp.array(info_nja, dtype=int32),
num_dynamic_joints=wp.array(info_nji, dtype=int32),
num_geoms=wp.array(info_ng, dtype=int32),
num_body_dofs=wp.array(info_nbd, dtype=int32),
num_joint_coords=wp.array(info_njq, dtype=int32),
num_joint_dofs=wp.array(info_njd, dtype=int32),
num_passive_joint_coords=wp.array(info_njpq, dtype=int32),
num_passive_joint_dofs=wp.array(info_njpd, dtype=int32),
num_actuated_joint_coords=wp.array(info_njaq, dtype=int32),
num_actuated_joint_dofs=wp.array(info_njad, dtype=int32),
num_joint_cts=wp.array(info_njc, dtype=int32),
num_joint_dynamic_cts=wp.array(info_njdc, dtype=int32),
num_joint_kinematic_cts=wp.array(info_njkc, dtype=int32),
bodies_offset=wp.array(info_bio, dtype=int32),
joints_offset=wp.array(info_jio, dtype=int32),
geoms_offset=wp.array(info_gio, dtype=int32),
body_dofs_offset=wp.array(info_bdio, dtype=int32),
joint_coords_offset=wp.array(info_jqio, dtype=int32),
joint_dofs_offset=wp.array(info_jdio, dtype=int32),
joint_passive_coords_offset=wp.array(info_jpqio, dtype=int32),
joint_passive_dofs_offset=wp.array(info_jpdio, dtype=int32),
joint_actuated_coords_offset=wp.array(info_jaqio, dtype=int32),
joint_actuated_dofs_offset=wp.array(info_jadio, dtype=int32),
joint_cts_offset=wp.array(info_jcio, dtype=int32),
joint_dynamic_cts_offset=wp.array(info_jdcio, dtype=int32),
joint_kinematic_cts_offset=wp.array(info_jkcio, dtype=int32),
base_body_index=wp.array(info_base_bid, dtype=int32),
base_joint_index=wp.array(info_base_jid, dtype=int32),
mass_min=wp.array(info_mass_min, dtype=float32),
mass_max=wp.array(info_mass_max, dtype=float32),
mass_total=wp.array(info_mass_total, dtype=float32),
inertia_total=wp.array(info_inertia_total, dtype=float32),
)
# Create the model time data
model_time = TimeModel(dt=wp.zeros(num_worlds, dtype=float32), inv_dt=wp.zeros(num_worlds, dtype=float32))
# Construct model gravity data
model_gravity = GravityModel(
g_dir_acc=wp.array(gravity_g_dir_acc, dtype=vec4f),
vector=wp.array(gravity_vector, dtype=vec4f, requires_grad=requires_grad),
)
# Create the bodies model
model_bodies = RigidBodiesModel(
num_bodies=model_size.sum_of_num_bodies,
label=bodies_label,
wid=wp.array(bodies_wid, dtype=int32),
bid=wp.array(bodies_bid, dtype=int32),
i_r_com_i=wp.array(bodies_i_r_com_i, dtype=vec3f, requires_grad=requires_grad),
m_i=wp.array(bodies_m_i, dtype=float32, requires_grad=requires_grad),
inv_m_i=wp.array(bodies_inv_m_i, dtype=float32, requires_grad=requires_grad),
i_I_i=wp.array(bodies_i_I_i, dtype=mat33f, requires_grad=requires_grad),
inv_i_I_i=wp.array(bodies_inv_i_I_i, dtype=mat33f, requires_grad=requires_grad),
q_i_0=wp.array(bodies_q_i_0, dtype=transformf, requires_grad=requires_grad),
u_i_0=wp.array(bodies_u_i_0, dtype=vec6f, requires_grad=requires_grad),
)
# Create the joints model
model_joints = JointsModel(
num_joints=model_size.sum_of_num_joints,
label=joints_label,
wid=wp.array(joints_wid, dtype=int32),
jid=wp.array(joints_jid, dtype=int32),
dof_type=wp.array(joints_dofid, dtype=int32),
act_type=wp.array(joints_actid, dtype=int32),
bid_B=wp.array(joints_bid_B, dtype=int32),
bid_F=wp.array(joints_bid_F, dtype=int32),
B_r_Bj=wp.array(joints_B_r_Bj, dtype=vec3f, requires_grad=requires_grad),
F_r_Fj=wp.array(joints_F_r_Fj, dtype=vec3f, requires_grad=requires_grad),
X_j=wp.array(joints_X_j, dtype=mat33f, requires_grad=requires_grad),
q_j_min=wp.array(joints_q_j_min, dtype=float32, requires_grad=requires_grad),
q_j_max=wp.array(joints_q_j_max, dtype=float32, requires_grad=requires_grad),
dq_j_max=wp.array(joints_qd_j_max, dtype=float32, requires_grad=requires_grad),
tau_j_max=wp.array(joints_tau_j_max, dtype=float32, requires_grad=requires_grad),
a_j=wp.array(joints_a_j, dtype=float32, requires_grad=requires_grad),
b_j=wp.array(joints_b_j, dtype=float32, requires_grad=requires_grad),
k_p_j=wp.array(joints_k_p_j, dtype=float32, requires_grad=requires_grad),
k_d_j=wp.array(joints_k_d_j, dtype=float32, requires_grad=requires_grad),
q_j_0=wp.array(joints_q_j_0, dtype=float32, requires_grad=requires_grad),
dq_j_0=wp.array(joints_dq_j_0, dtype=float32, requires_grad=requires_grad),
num_coords=wp.array(joints_ncoords_j, dtype=int32),
num_dofs=wp.array(joints_ndofs_j, dtype=int32),
num_cts=wp.array(joints_ncts_j, dtype=int32),
num_dynamic_cts=wp.array(joints_ndyncts_j, dtype=int32),
num_kinematic_cts=wp.array(joints_nkincts_j, dtype=int32),
coords_offset=wp.array(joints_q_start, dtype=int32),
dofs_offset=wp.array(joints_dq_start, dtype=int32),
passive_coords_offset=wp.array(joints_pq_start, dtype=int32),
passive_dofs_offset=wp.array(joints_pdq_start, dtype=int32),
actuated_coords_offset=wp.array(joints_aq_start, dtype=int32),
actuated_dofs_offset=wp.array(joints_adq_start, dtype=int32),
cts_offset=wp.array(joints_cts_start, dtype=int32),
dynamic_cts_offset=wp.array(joints_dcts_start, dtype=int32),
kinematic_cts_offset=wp.array(joints_kcts_start, dtype=int32),
)
# Create the collision geometries model
model_geoms = GeometriesModel(
num_geoms=model_size.sum_of_num_geoms,
num_collidable=model_num_collidables,
num_collidable_pairs=len(model_collidable_pairs),
num_excluded_pairs=len(model_excluded_pairs),
model_minimum_contacts=model_required_contacts,
world_minimum_contacts=world_required_contacts,
label=geoms_label,
wid=wp.array(geoms_wid, dtype=int32),
gid=wp.array(geoms_gid, dtype=int32),
bid=wp.array(geoms_bid, dtype=int32),
type=wp.array(geoms_type, dtype=int32),
flags=wp.array(geoms_flags, dtype=int32),
ptr=wp.array(geoms_ptr, dtype=wp.uint64),
params=wp.array(geoms_params, dtype=vec4f),
offset=wp.array(geoms_offset, dtype=transformf),
material=wp.array(geoms_material, dtype=int32),
group=wp.array(geoms_group, dtype=int32),
gap=wp.array(geoms_gap, dtype=float32),
margin=wp.array(geoms_margin, dtype=float32),
collidable_pairs=wp.array(np.array(model_collidable_pairs), dtype=vec2i),
excluded_pairs=wp.array(np.array(model_excluded_pairs), dtype=vec2i),
)
# Create the material pairs model
model_materials = MaterialsModel(
num_materials=model_size.sum_of_num_materials,
restitution=wp.array(materials_rest[0], dtype=float32),
static_friction=wp.array(materials_static_fric[0], dtype=float32),
dynamic_friction=wp.array(materials_dynamic_fric[0], dtype=float32),
)
# Create the material pairs model
model_material_pairs = MaterialPairsModel(
num_material_pairs=model_size.sum_of_num_material_pairs,
restitution=wp.array(mpairs_rest[0], dtype=float32),
static_friction=wp.array(mpairs_static_fric[0], dtype=float32),
dynamic_friction=wp.array(mpairs_dynamic_fric[0], dtype=float32),
)
# Construct and return the complete model container
return ModelKamino(
_device=device,
_requires_grad=requires_grad,
size=model_size,
info=model_info,
time=model_time,
gravity=model_gravity,
bodies=model_bodies,
joints=model_joints,
geoms=model_geoms,
materials=model_materials,
material_pairs=model_material_pairs,
)
###
# Utilities
###
def make_collision_candidate_pairs(self, allow_neighbors: bool = False) -> list[tuple[int, int]]:
"""
Constructs the collision pair candidates.
Filtering steps:
1. filter out self-collisions
2. filter out same-body collisions
3. filter out collision between different worlds
4. filter out collisions according to the collision groupings
5. filter out neighbor collisions for fixed joints
6. (optional) filter out neighbor collisions for joints w/ DoFs
Args:
allow_neighbors (bool, optional):
If True, includes geom-pairs with corresponding
bodies that are neighbors via joints with DoF.
Returns:
A sorted list of geom index pairs (gid1, gid2) that are candidates for collision detection.
"""
# Retrieve the number of worlds
nw = self.num_worlds
# Extract the per-world info from the builder
ncg = [self._worlds[i].num_geoms for i in range(nw)]
# Initialize the lists to store the collision candidate pairs and their properties of each world
model_candidate_pairs = []
joint_idx_min = [len(self.joints)] * nw
joint_idx_max = [0] * nw
for i, joint in enumerate(self.joints):
joint_idx_min[joint.wid] = min(i, joint_idx_min[joint.wid])
joint_idx_max[joint.wid] = max(i, joint_idx_max[joint.wid])
# Iterate over each world and construct the collision geometry pairs info
ncg_offset = 0
for wid in range(nw):
# Initialize the lists to store the collision candidate pairs and their properties
world_candidate_pairs = []
# Iterate over each gid pair and filtering out pairs not viable for collision detection
# NOTE: k=1 skips diagonal entries to exclude self-collisions
for gid1_, gid2_ in zip(*np.triu_indices(ncg[wid], k=1), strict=False):
# Convert the per-world local gids to model gid integers
gid1 = int(gid1_) + ncg_offset
gid2 = int(gid2_) + ncg_offset
# Get references to the geometries
geom1, geom2 = self.geoms[gid1], self.geoms[gid2]
# Skip if either geometry is non-collidable
if not geom1.is_collidable or not geom2.is_collidable:
continue
# Get body indices of each geom
bid1, bid2 = geom1.body, geom2.body
# Get world indices of each geom
wid1, wid2 = geom1.wid, geom2.wid
# 2. Check for same-body collision
is_self_collision = bid1 == bid2
# 3. Check for different-world collision
in_same_world = wid1 == wid2
# 4. Check for collision according to the collision groupings
are_collidable = ((geom1.group & geom2.collides) != 0) and ((geom2.group & geom1.collides) != 0)
# Skip this pair if it does not pass the first round of filtering
if is_self_collision or not in_same_world or not are_collidable:
continue
# 5. Check for neighbor collision for fixed and DoF joints
are_fixed_neighbors = False
are_dof_neighbors = False
for joint in self.joints[joint_idx_min[wid1] : joint_idx_max[wid1] + 1]:
if (joint.bid_B == bid1 and joint.bid_F == bid2) or (joint.bid_B == bid2 and joint.bid_F == bid1):
if joint.dof_type == JointDoFType.FIXED:
are_fixed_neighbors = True
elif joint.bid_B < 0:
pass
else:
are_dof_neighbors = True
break
# Skip this pair if they are fixed-joint neighbors, or are DoF
# neighbor collisions and self-collisions are not allowed
if ((not allow_neighbors) and are_dof_neighbors) or are_fixed_neighbors:
continue
# Append the geometry pair to the list of world collision candidates
world_candidate_pairs.append((min(gid1, gid2), max(gid1, gid2)))
# Append the world collision pairs to the model lists
model_candidate_pairs.extend(world_candidate_pairs)
# Update the geometry index offset for the next world
ncg_offset += ncg[wid]
# Sort the excluded pairs list for efficient lookup
# on the device if there are any pairs to exclude
if len(model_candidate_pairs) > 0:
model_candidate_pairs.sort()
# Return the model total candidate pairs
return model_candidate_pairs
def make_collision_excluded_pairs(self, allow_neighbors: bool = False) -> list[tuple[int, int]]:
"""
Builds a sorted array of shape pairs that the NXN/SAP broadphase should exclude.
Encodes the same filtering rules as
:meth:`ModelBuilderKamino.make_collision_candidate_pairs` (same-body, group/collides
bitmask, fixed-joint and DoF-joint neighbours) but returns the *complement*:
pairs that should **not** collide.
Args:
allow_neighbors (bool, optional):
If True, does not exclude geom-pairs with corresponding
bodies that are neighbors via joints with DoF.
Returns:
A sorted list of geom index pairs (gid1, gid2) that should be excluded from collision detection.
"""
# Pre-index joints per world for fast lookup
joint_ranges: list[tuple[int, int]] = []
for w in range(self.num_worlds):
lo = len(self.joints)
hi = 0
for i, j in enumerate(self.joints):
if j.wid == w:
lo = min(lo, i)
hi = max(hi, i)
joint_ranges.append((lo, hi))
model_excluded_pairs: list[tuple[int, int]] = []
ncg_offset = 0
for wid in range(self.num_worlds):
ncg = self._worlds[wid].num_geoms
for idx1 in range(ncg):
gid1 = idx1 + ncg_offset
geom1 = self.geoms[gid1]
for idx2 in range(idx1 + 1, ncg):
gid2 = idx2 + ncg_offset
geom2 = self.geoms[gid2]
# Skip if either geometry is non-collidable since they won't be considered in the broadphase anyway
if (geom1.flags & ShapeFlags.COLLIDE_SHAPES == 0) or (geom2.flags & ShapeFlags.COLLIDE_SHAPES == 0):
continue
# Form the candidate pair tuple with sorted geom index order
candidate_pair = (min(gid1, gid2), max(gid1, gid2))
# Same-body collision
if geom1.body == geom2.body:
model_excluded_pairs.append(candidate_pair)
continue
# Group/collides bitmask check
if not ((geom1.group & geom2.collides) != 0 and (geom2.group & geom1.collides) != 0):
model_excluded_pairs.append(candidate_pair)
continue
# Fixed-joint / DoF-joint neighbour check
jlo, jhi = joint_ranges[wid]
is_excluded_neighbour = False
for joint in self.joints[jlo : jhi + 1]:
is_pair = (joint.bid_B == geom1.body and joint.bid_F == geom2.body) or (
joint.bid_B == geom2.body and joint.bid_F == geom1.body
)
if is_pair:
if joint.dof_type == JointDoFType.FIXED:
is_excluded_neighbour = True
elif joint.bid_B >= 0:
is_excluded_neighbour = True
break
if is_excluded_neighbour:
model_excluded_pairs.append(candidate_pair)
ncg_offset += ncg
# Sort the excluded pairs list for efficient lookup
# on the device if there are any pairs to exclude
if len(model_excluded_pairs) > 0:
model_excluded_pairs.sort()
# Return the model total excluded pairs and their properties
return model_excluded_pairs
def compute_num_collidable_geoms(
self, collidable_geom_pairs: list[tuple[int, int]] | None = None
) -> tuple[list[int], int]:
"""
Computes the number of unique collidable geometries from the provided list of collidable geometry pairs.
Args:
collidable_geom_pairs (list[tuple[int, int]], optional):
A list of geom-pair indices `(gid1, gid2)` (absolute w.r.t the model).\n
If `None`, the number of collidable geometries will
be extracted by exhaustively checking all geometries.
Returns:
(world_num_collidables, model_num_collidables):
A tuple containing a list of unique collidable geometries per world and the total over the model.
"""
# If an explicit list of collidable geometry pairs is provided,
# compute the number of unique collidable geometries from the pairs
if collidable_geom_pairs is not None:
collidable_geoms: set[int] = set()
world_num_collidables = [0] * self.num_worlds
for pair in collidable_geom_pairs:
collidable_geoms.add(pair[0])
collidable_geoms.add(pair[1])
for gid in collidable_geoms:
world_num_collidables[self.geoms[gid].wid] += 1
return world_num_collidables, len(collidable_geoms)
# Otherwise, compute the number of collidable geometries by checking all geometries
world_num_collidables = [0] * self.num_worlds
for geom in self.geoms:
if geom.is_collidable:
world_num_collidables[geom.wid] += 1
return world_num_collidables, sum(world_num_collidables)
def compute_required_contact_capacity(
self,
collidable_geom_pairs: list[tuple[int, int]] | None = None,
max_contacts_per_pair: int | None = None,
max_contacts_per_world: int | None = None,
) -> tuple[int, list[int]]:
# First check if there are any collision geometries
if self._num_geoms == 0:
return 0, [0] * self.num_worlds
# Generate the collision candidate pairs if not provided
if collidable_geom_pairs is None:
collidable_geom_pairs = self.make_collision_candidate_pairs()
# Compute the maximum possible number of geom pairs per world
world_max_contacts = [0] * self.num_worlds
for geom_pair in collidable_geom_pairs:
g1 = int(geom_pair[0])
g2 = int(geom_pair[1])
geom1 = self._geoms[g1]
geom2 = self._geoms[g2]
if geom1.shape.type > geom2.shape.type:
g1, g2 = g2, g1
geom1, geom2 = geom2, geom1
num_contacts_a, num_contacts_b = max_contacts_for_shape_pair(
type_a=geom1.shape.type,
type_b=geom2.shape.type,
)
num_contacts = num_contacts_a + num_contacts_b
if max_contacts_per_pair is not None:
world_max_contacts[geom1.wid] += min(num_contacts, max_contacts_per_pair)
else:
world_max_contacts[geom1.wid] += num_contacts
# Override the per-world maximum contacts if specified in the settings
if max_contacts_per_world is not None:
for w in range(self.num_worlds):
world_max_contacts[w] = min(world_max_contacts[w], max_contacts_per_world)
# Return the per-world maximum contacts list
return sum(world_max_contacts), world_max_contacts
###
# Internals
###
def _check_world_index(self, world_index: int) -> WorldDescriptor:
"""
Checks if the provided world index is valid.
Args:
world_index (int): The index of the world to be checked.
Raises:
ValueError: If the world index is out of range.
"""
if self._num_worlds == 0:
raise ValueError(
"Model does not contain any worlds. "
"Please add at least one using `add_world()` before adding model entities."
)
if world_index < 0 or world_index >= self._num_worlds:
raise ValueError(f"Invalid world index (wid): {world_index}. Must be between 0 and {self._num_worlds - 1}.")
return self._worlds[world_index]
def _compute_world_offsets(self):
"""
Computes and sets the model offsets for each world in the model.
"""
# Initialize the model offsets
bodies_idx_offset: int = 0
joints_idx_offset: int = 0
geoms_idx_offset: int = 0
body_dofs_idx_offset: int = 0
joint_coords_idx_offset: int = 0
joint_dofs_idx_offset: int = 0
joint_passive_coords_idx_offset: int = 0
joint_passive_dofs_idx_offset: int = 0
joint_actuated_coords_idx_offset: int = 0
joint_actuated_dofs_idx_offset: int = 0
joint_cts_idx_offset: int = 0
joint_dynamic_cts_idx_offset: int = 0
joint_kinematic_cts_idx_offset: int = 0
# Iterate over each world and set their model offsets
for world in self._worlds:
# Set the offsets in the world descriptor to the current values
world.bodies_idx_offset = int(bodies_idx_offset)
world.joints_idx_offset = int(joints_idx_offset)
world.geoms_idx_offset = int(geoms_idx_offset)
world.body_dofs_idx_offset = int(body_dofs_idx_offset)
world.joint_coords_idx_offset = int(joint_coords_idx_offset)
world.joint_dofs_idx_offset = int(joint_dofs_idx_offset)
world.joint_passive_coords_idx_offset = int(joint_passive_coords_idx_offset)
world.joint_passive_dofs_idx_offset = int(joint_passive_dofs_idx_offset)
world.joint_actuated_coords_idx_offset = int(joint_actuated_coords_idx_offset)
world.joint_actuated_dofs_idx_offset = int(joint_actuated_dofs_idx_offset)
world.joint_cts_idx_offset = int(joint_cts_idx_offset)
world.joint_dynamic_cts_idx_offset = int(joint_dynamic_cts_idx_offset)
world.joint_kinematic_cts_idx_offset = int(joint_kinematic_cts_idx_offset)
# Update the offsets for the next world
bodies_idx_offset += world.num_bodies
joints_idx_offset += world.num_joints
geoms_idx_offset += world.num_geoms
body_dofs_idx_offset += 6 * world.num_bodies
joint_coords_idx_offset += world.num_joint_coords
joint_dofs_idx_offset += world.num_joint_dofs
joint_passive_coords_idx_offset += world.num_passive_joint_coords
joint_passive_dofs_idx_offset += world.num_passive_joint_dofs
joint_actuated_coords_idx_offset += world.num_actuated_joint_coords
joint_actuated_dofs_idx_offset += world.num_actuated_joint_dofs
joint_cts_idx_offset += world.num_joint_cts
joint_dynamic_cts_idx_offset += world.num_dynamic_joint_cts
joint_kinematic_cts_idx_offset += world.num_kinematic_joint_cts
def _collect_geom_max_contact_hints(self) -> tuple[int, list[int]]:
"""
Collects the `max_contacts` hints from collision geometries.
"""
model_max_contacts = 0
world_max_contacts = [0] * self.num_worlds
for w in range(len(self._worlds)):
for geom_maxnc in self._worlds[w].geometry_max_contacts:
model_max_contacts += geom_maxnc
world_max_contacts[w] += geom_maxnc
return model_max_contacts, world_max_contacts
EntityDescriptorType = RigidBodyDescriptor | JointDescriptor | GeometryDescriptor
"""A type alias for model entity descriptors."""
@staticmethod
def _insert_entity(entity_list: list[EntityDescriptorType], entity: EntityDescriptorType, world_index: int = 0):
"""
Inserts an entity descriptor into the provided entity list at
the end of the entities belonging to the specified world index.
Insertion preserves the order of entities per world.
Args:
entity_list (list[EntityDescriptorType]): The list of entity descriptors.
entity (EntityDescriptorType): The entity descriptor to be inserted.
world_index (int): The world index to insert the entity into.
"""
# NOTE: We initialize the last entity index to the length of the list
# so that if no entities belong to the specified world, the new entity
# is simply appended to the end of the list.
last_entity_index = len(entity_list)
for i, e in enumerate(entity_list):
if e.wid == world_index:
last_entity_index = i
# NOTE: Insert the entity after the last entity of the specified
# world so that the order of entities per world is preserved.
entity_list.insert(last_entity_index + 1, entity)
@staticmethod
def _check_body_inertia(m_i: float, i_I_i: mat33f):
"""
Checks if the body inertia is valid.
Args:
i_I_i (mat33f): The inertia matrix to be checked.
Raises:
ValueError: If the inertia matrix is not symmetric of positive definite.
"""
# Convert to numpy array for easier checks
i_I_i_np = np.ndarray(buffer=i_I_i, shape=(3, 3), dtype=np.float32)
# Perform checks on the inertial properties
if m_i <= 0.0:
raise ValueError(f"Invalid body mass: {m_i}. Must be greater than 0.0")
if not np.allclose(i_I_i_np, i_I_i_np.T, atol=float(FLOAT32_EPS)):
raise ValueError(f"Invalid body inertia matrix:\n{i_I_i}\nMust be symmetric.")
if not np.all(np.linalg.eigvals(i_I_i_np) > 0.0):
raise ValueError(f"Invalid body inertia matrix:\n{i_I_i}\nMust be positive definite.")
@staticmethod
def _check_body_pose(q_i: transformf):
"""
Checks if the body pose is valid.
Args:
q_i_0 (transformf): The pose of the body to be checked.
Raises:
ValueError: If the body pose is not a valid transformation.
"""
if not isinstance(q_i, transformf):
raise TypeError(f"Invalid body pose type: {type(q_i)}. Must be `transformf`.")
# Extract the orientation quaternion
if not np.isclose(wp.length(q_i.q), 1.0, atol=float(FLOAT32_EPS)):
raise ValueError(f"Invalid body pose orientation quaternion: {q_i.q}. Must be a unit quaternion.")
================================================
FILE: newton/_src/solvers/kamino/_src/core/control.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Defines the control container of Kamino."""
from __future__ import annotations
from dataclasses import dataclass
import warp as wp
from .....sim.control import Control
###
# Types
###
@dataclass
class ControlKamino:
"""
Time-varying control data for a :class:`ModelKamino`.
Time-varying control data currently consists of generalized joint actuation forces, with
the intention that external actuator models or controllers will populate these attributes.
The exact attributes depend on the contents of the model. ControlKamino objects
should generally be created using the :func:`kamino.ModelKamino.control()` function.
We adopt the following notational conventions for the control attributes:
- Generalized joint actuation forces are denoted by ``tau``
- Subscripts ``_j`` denote joint-indexed quantities, e.g. :attr:`tau_j`.
"""
###
# Attributes
###
tau_j: wp.array | None = None
"""
Array of generalized joint actuation forces.\n
Shape is ``(sum(d_j),)`` and dtype is :class:`float32`,\n
where ``d_j`` is the number of DoFs of each joint ``j``.
"""
q_j_ref: wp.array | None = None
"""
Array of reference generalized joint coordinates for implicit PD control.\n
Shape of ``(sum(c_j),)`` and type :class:`float`,
where ``c_j`` is the number of coordinates of joint ``j``.
"""
dq_j_ref: wp.array | None = None
"""
Array of reference generalized joint velocities for implicit PD control.\n
Shape of ``(sum(d_j),)`` and type :class:`float`,
where ``d_j`` is the number of DoFs of joint ``j``.
"""
tau_j_ref: wp.array | None = None
"""
Array of reference feed-forward generalized joint forces for implicit PD control.\n
Shape of ``(sum(d_j),)`` and type :class:`float`,
where ``d_j`` is the number of DoFs of joint ``j``.
"""
###
# Operations
###
def copy_to(self, other: ControlKamino) -> None:
"""
Copies the ControlKamino data to another ControlKamino object.
Args:
other: The target ControlKamino object to copy data into.
"""
if self.tau_j is None or other.tau_j is None:
raise ValueError("Error copying from/to uninitialized ControlKamino")
wp.copy(other.tau_j, self.tau_j)
def copy_from(self, other: ControlKamino) -> None:
"""
Copies the ControlKamino data from another ControlKamino object.
Args:
other: The source ControlKamino object to copy data from.
"""
if self.tau_j is None or other.tau_j is None:
raise ValueError("Error copying from/to uninitialized ControlKamino")
wp.copy(self.tau_j, other.tau_j)
@staticmethod
def from_newton(control: Control) -> ControlKamino:
"""
Constructs a :class:`kamino.ControlKamino` object from a :class:`newton.Control` object.
This operation serves only as an adaptor-like constructor to interface a
:class:`newton.Control`, effectively creating an alias without copying data.
Args:
control: The source :class:`newton.Control` object to be adapted.
"""
return ControlKamino(
tau_j=control.joint_f,
tau_j_ref=control.joint_act,
q_j_ref=control.joint_target_pos,
dq_j_ref=control.joint_target_vel,
)
@staticmethod
def to_newton(control: ControlKamino) -> Control:
"""
Constructs a :class:`newton.Control` object from a :class:`kamino.ControlKamino` object.
This operation serves only as an adaptor-like constructor to interface a
:class:`kamino.ControlKamino`, effectively creating an alias without copying data.
Args:
control: The source :class:`kamino.ControlKamino` object to be adapted.
"""
control_newton = Control()
control_newton.joint_f = control.tau_j
control_newton.joint_act = control.tau_j_ref
control_newton.joint_target_pos = control.q_j_ref
control_newton.joint_target_vel = control.dq_j_ref
return control_newton
================================================
FILE: newton/_src/solvers/kamino/_src/core/conversions.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Provides a set of conversion utilities to bridge Kamino and Newton."""
import numpy as np
import warp as wp
from .....sim.model import Model
from ..utils import logger as msg
from .joints import JointDoFType, JointsModel
from .shapes import max_contacts_for_shape_pair
###
# Module interface
###
__all__ = [
"compute_required_contact_capacity",
"convert_entity_local_transforms",
"convert_model_joint_transforms",
]
###
# Functions
###
def convert_entity_local_transforms(model: Model) -> dict[str, np.ndarray]:
"""
Converts all entity-local transforms (i.e. of bodies, joints and shapes) in the
given Newton model to a format that is compatible with Kamino's constraint system.
This involves absorbing any non-identity :attr:`Model.joint_X_c`
rotations into the child body frames, and updating all downstream
:attr:`Model.joint_X_p` and :attr:`Model.shape_transform` accordingly.
Args:
model (Model): Newton model to be modified in-place.
"""
# ---------------------------------------------------------------------------
# Pre-processing: absorb non-identity joint_X_c rotations into child body
# frames so that Kamino sees aligned joint frames on both sides.
#
# Kamino's constraint system assumes a single joint frame X_j valid for both
# the base (parent) and follower (child) bodies. At q = 0 it requires
# q_base^{-1} * q_follower = identity
#
# Newton, however, allows different parent / child joint-frame orientations
# via joint_X_p and joint_X_c. At q = 0 Newton's FK gives:
# q_follower = q_parent * q_pj * inv(q_cj)
# so q_base^{-1} * q_follower = q_pj * inv(q_cj) which is generally not
# identity.
#
# To fix this we apply a per-body correction rotation q_corr = q_cj * inv(q_pj)
# (applied on the right) to each child body's frame:
# q_body_new = q_body_old * q_corr
#
# This makes q_base^{-1} * q_follower_new = identity at q = 0, and the joint
# rotation axis R(q_pj) * axis is preserved.
#
# All body-local quantities (CoM, inertia, shapes) are re-expressed in the
# rotated frame, and downstream joint_X_p transforms are updated to account
# for the parent body's frame change.
# ---------------------------------------------------------------------------
def _to_wpq(q):
return wp.quat(float(q[0]), float(q[1]), float(q[2]), float(q[3]))
def _from_wpq(q):
return np.array([q[0], q[1], q[2], q[3]], dtype=np.float64)
def _quat_is_identity(q, tol=1e-5):
return abs(abs(q[3]) - 1.0) < tol
def _quat_mul(a, b):
return _from_wpq(_to_wpq(a) * _to_wpq(b))
def _quat_inv(q):
return _from_wpq(wp.quat_inverse(_to_wpq(q)))
def _quat_rotate_vec(q, v):
r = wp.quat_rotate(_to_wpq(q), wp.vec3(float(v[0]), float(v[1]), float(v[2])))
return np.array([r[0], r[1], r[2]], dtype=np.float64)
def _quat_to_mat33(q):
return np.array(wp.quat_to_matrix(_to_wpq(q)), dtype=np.float64).reshape(3, 3)
# Work on copies so the original Newton model is not mutated
body_com_np: np.ndarray = model.body_com.numpy().copy()
body_q_np: np.ndarray = model.body_q.numpy().copy()
body_qd_np: np.ndarray = model.body_qd.numpy().copy()
body_inertia_np: np.ndarray = model.body_inertia.numpy().copy()
body_inv_inertia_np: np.ndarray = model.body_inv_inertia.numpy().copy()
joint_parent_np: np.ndarray = model.joint_parent.numpy().copy()
joint_child_np: np.ndarray = model.joint_child.numpy().copy()
joint_X_p_np: np.ndarray = model.joint_X_p.numpy().copy()
joint_X_c_np: np.ndarray = model.joint_X_c.numpy().copy()
shape_transform_np: np.ndarray = model.shape_transform.numpy().copy()
shape_body_np: np.ndarray = model.shape_body.numpy().copy()
# Process joints in tree order (Newton stores them parent-before-child).
# For each joint whose q_pj * inv(q_cj) is not identity, we apply a
# correction q_corr to the child body's frame and immediately propagate
# to all downstream joints that reference the corrected body as parent.
body_corr: dict[int, np.ndarray] = {} # body_index -> cumulative q_corr
for j in range(model.joint_count):
parent = int(joint_parent_np[j])
child = int(joint_child_np[j])
# If the parent body was previously corrected, first update this
# joint's parent-side transform to the new parent frame.
if parent >= 0 and parent in body_corr:
q_par_corr_inv = _quat_inv(body_corr[parent])
p_pos = joint_X_p_np[j, :3].astype(np.float64)
joint_X_p_np[j, :3] = _quat_rotate_vec(q_par_corr_inv, p_pos)
p_quat = joint_X_p_np[j, 3:7].astype(np.float64)
joint_X_p_np[j, 3:7] = _quat_mul(q_par_corr_inv, p_quat)
# Now compute the correction for this joint's child body
q_cj = joint_X_c_np[j, 3:7].astype(np.float64)
q_pj = joint_X_p_np[j, 3:7].astype(np.float64)
q_corr = _quat_mul(q_cj, _quat_inv(q_pj))
if child < 0 or _quat_is_identity(q_corr):
continue
if child in body_corr:
msg.warning(
"Body %d is the child of multiple joints requiring joint_X_c "
"correction. The previous correction will be overwritten, which "
"may produce incorrect joint constraints for loop-closing joints.",
child,
)
body_corr[child] = q_corr.copy()
# Update child-side joint transform: rotation becomes identity,
# position re-expressed in the new child frame
q_corr_inv = _quat_inv(q_corr)
c_pos = joint_X_c_np[j, :3].astype(np.float64)
joint_X_c_np[j, :3] = _quat_rotate_vec(q_corr_inv, c_pos)
joint_X_c_np[j, 3:7] = [0.0, 0.0, 0.0, 1.0]
# Rotate the child body's local quantities
R_inv_corr = _quat_to_mat33(q_corr_inv)
q_old = body_q_np[child, 3:7].astype(np.float64)
body_q_np[child, 3:7] = _quat_mul(q_old, q_corr)
body_com_np[child] = _quat_rotate_vec(q_corr_inv, body_com_np[child].astype(np.float64))
body_inertia_np[child] = R_inv_corr @ body_inertia_np[child].astype(np.float64) @ R_inv_corr.T
body_inv_inertia_np[child] = R_inv_corr @ body_inv_inertia_np[child].astype(np.float64) @ R_inv_corr.T
# TODO: Do these need be converted? Aren't they already computed at body CoM?
body_qd_np[child, :3] = R_inv_corr @ body_qd_np[child, :3].astype(np.float64)
body_qd_np[child, 3:6] = R_inv_corr @ body_qd_np[child, 3:6].astype(np.float64)
for s in range(model.shape_count):
if int(shape_body_np[s]) != child:
continue
s_pos = shape_transform_np[s, :3].astype(np.float64)
s_quat = shape_transform_np[s, 3:7].astype(np.float64)
shape_transform_np[s, :3] = _quat_rotate_vec(q_corr_inv, s_pos)
shape_transform_np[s, 3:7] = _quat_mul(q_corr_inv, s_quat)
if body_corr:
msg.debug(
"Absorbed joint_X_c rotations for %d child bodies: %s",
len(body_corr),
list(body_corr.keys()),
)
# Return the converted transforms as numpy arrays
# to be used for constructing the Kamino model
return {
"body_q": body_q_np,
"body_qd": body_qd_np,
"body_com": body_com_np,
"body_inertia": body_inertia_np,
"body_inv_inertia": body_inv_inertia_np,
"shape_transform": shape_transform_np,
"joint_X_p": joint_X_p_np,
"joint_X_c": joint_X_c_np,
}
def compute_required_contact_capacity(
model: Model,
max_contacts_per_pair: int | None = None,
max_contacts_per_world: int | None = None,
) -> tuple[int, list[int]]:
"""
Computes the required contact capacity for a given Newton model.
The outputs are used to determine the minimum number of contacts
to be allocated, according to the shapes present in the model.
Args:
model (Model):
The Newton model for which to compute the required contact capacity.
max_contacts_per_pair (int, optional):
Optional maximum number of contacts to allocate per shape pair.
If `None`, no per-pair limit is applied.
max_contacts_per_world (int, optional):
Optional maximum number of contacts to allocate per world.
If `None`, no per-world limit is applied, otherwise it will
override the computed per-world requirements if it is larger.
Returns:
(model_required_contacts, world_required_contacts):
A tuple containing:
- `model_required_contacts` (int):
The total number of contacts required for the entire model.
- `world_required_contacts` (list[int]):
A list of required contacts per world, where the length of the
list is equal to `model.world_count` and each entry corresponds
to the required contacts for that world.
"""
# Retrieve the shape types, worlds and collision candidate pairs from the model
shape_type_np = model.shape_type.numpy()
shape_world_np = model.shape_world.numpy()
shape_contact_pairs = model.shape_contact_pairs.numpy().tolist()
# First check if there are any collision geometries
if model.shape_count == 0:
return 0, [0] * model.world_count
# Compute the maximum possible number of geom pairs per world.
# Global shapes (world=-1, e.g. ground plane) can collide with shapes
# from any world — attribute their contacts to the non-global shape's world.
world_max_contacts = [0] * model.world_count
for shape_pair in shape_contact_pairs:
s1 = int(shape_pair[0])
s2 = int(shape_pair[1])
type1 = int(shape_type_np[s1])
type2 = int(shape_type_np[s2])
if type1 > type2:
s1, s2 = s2, s1
type1, type2 = type2, type1
num_contacts_a, num_contacts_b = max_contacts_for_shape_pair(
type_a=type1,
type_b=type2,
)
num_contacts = num_contacts_a + num_contacts_b
# Determine the world for this pair — fall back to other shape if one is global
w1 = int(shape_world_np[s1])
w2 = int(shape_world_np[s2])
wid = w1 if w1 >= 0 else w2
if wid < 0:
continue # Both shapes are global — skip
if max_contacts_per_pair is not None:
world_max_contacts[wid] += min(num_contacts, max_contacts_per_pair)
else:
world_max_contacts[wid] += num_contacts
# Override the per-world maximum contacts if specified in the settings
if max_contacts_per_world is not None:
for w in range(model.world_count):
world_max_contacts[w] = min(world_max_contacts[w], max_contacts_per_world)
# Return the per-world maximum contacts list
return sum(world_max_contacts), world_max_contacts
# TODO: Re-implement this using a kernel to run in parallel on the GPU if possible
def convert_model_joint_transforms(model: Model, joints: JointsModel) -> None:
"""
Converts the joint model parameterization of Newton's to Kamino's format.
This essentially involves computing the B_r_Bj, F_r_Fj and
X_j arrays from the joint_X_p and joint_X_c transforms.
Args:
- model (Model):
The input Newton model containing the joint information to be converted.
- joints (JointsModel):
The output JointsModel instance where the converted joint data will be stored.
This function modifies the `joints` object in-place.
"""
joint_X_p_np = model.joint_X_p.numpy()
joint_X_c_np = model.joint_X_c.numpy()
body_com_np = model.body_com.numpy()
joint_parent_np = model.joint_parent.numpy()
joint_child_np = model.joint_child.numpy()
joint_axis_np = model.joint_axis.numpy()
joint_dof_dim_np = model.joint_dof_dim.numpy()
joint_qd_start_np = model.joint_qd_start.numpy()
joint_limit_lower_np = model.joint_limit_lower.numpy()
joint_limit_upper_np = model.joint_limit_upper.numpy()
dof_type_np = joints.dof_type.numpy()
n_joints = model.joint_count
B_r_Bj_np = np.zeros((n_joints, 3), dtype=np.float32)
F_r_Fj_np = np.zeros((n_joints, 3), dtype=np.float32)
X_j_np = np.zeros((n_joints, 9), dtype=np.float32)
for j in range(n_joints):
dof_type_j = JointDoFType(int(dof_type_np[j]))
dof_dim_j = (int(joint_dof_dim_np[j][0]), int(joint_dof_dim_np[j][1]))
dofs_start_j = int(joint_qd_start_np[j])
ndofs_j = dof_type_j.num_dofs
joint_axes_j = joint_axis_np[dofs_start_j : dofs_start_j + ndofs_j]
joint_q_min_j = joint_limit_lower_np[dofs_start_j : dofs_start_j + ndofs_j]
joint_q_max_j = joint_limit_upper_np[dofs_start_j : dofs_start_j + ndofs_j]
R_axis_j = JointDoFType.from_newton(dof_type_j, dof_dim_j, joint_axes_j, joint_q_min_j, joint_q_max_j)
parent_bid = int(joint_parent_np[j])
p_r_p_com = wp.vec3f(body_com_np[parent_bid]) if parent_bid >= 0 else wp.vec3f(0.0)
c_r_c_com = wp.vec3f(body_com_np[int(joint_child_np[j])])
X_p_j = wp.transformf(*joint_X_p_np[j, :])
X_c_j = wp.transformf(*joint_X_c_np[j, :])
q_p_j = wp.transform_get_rotation(X_p_j)
p_r_p_j = wp.transform_get_translation(X_p_j)
c_r_c_j = wp.transform_get_translation(X_c_j)
B_r_Bj_np[j, :] = p_r_p_j - p_r_p_com
F_r_Fj_np[j, :] = c_r_c_j - c_r_c_com
X_j_np[j, :] = wp.quat_to_matrix(q_p_j) @ R_axis_j
joints.B_r_Bj.assign(B_r_Bj_np)
joints.F_r_Fj.assign(F_r_Fj_np)
joints.X_j.assign(X_j_np.reshape((n_joints, 3, 3)))
================================================
FILE: newton/_src/solvers/kamino/_src/core/data.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Defines the Kamino-specific data containers to hold time-varying simulation data."""
from dataclasses import dataclass
import warp as wp
from .bodies import RigidBodiesData
from .control import ControlKamino
from .geometry import GeometriesData
from .joints import JointsData
from .state import StateKamino
from .time import TimeData
###
# Module interface
###
__all__ = [
"DataKamino",
"DataKaminoInfo",
]
###
# Types
###
@dataclass
class DataKaminoInfo:
"""
A container to hold the time-varying information about the set of active constraints.
"""
###
# Total Constraints
###
num_total_cts: wp.array | None = None
"""
The total number of active constraints.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
###
# Limits
###
num_limits: wp.array | None = None
"""
The number of active limits in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
num_limit_cts: wp.array | None = None
"""
The number of active limit constraints.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
limit_cts_group_offset: wp.array | None = None
"""
The index offset of the limit constraints group within the constraints block of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
###
# Contacts
###
num_contacts: wp.array | None = None
"""
The number of active contacts in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
num_contact_cts: wp.array | None = None
"""
The number of active contact constraints.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
contact_cts_group_offset: wp.array | None = None
"""
The index offset of the contact constraints group within the constraints block of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
@dataclass
class DataKamino:
"""
A container to hold the time-varying data of the model entities.
It includes all model-specific intermediate quantities used throughout the simulation, as needed
to update the state of rigid bodies, joints, geometries, active constraints and time-keeping.
"""
info: DataKaminoInfo | None = None
"""The info container holding information about the set of active constraints."""
time: TimeData | None = None
"""Time-varying time-keeping data, including the current simulation step and time."""
bodies: RigidBodiesData | None = None
"""
Time-varying data of all rigid bodies in the model: poses, twists,
wrenches, and moments of inertia computed in world coordinates.
"""
joints: JointsData | None = None
"""
Time-varying data of joints in the model: joint frames computed in world coordinates,
constraint residuals and reactions, and generalized (DoF) quantities.
"""
geoms: GeometriesData | None = None
"""Time-varying data of geometries in the model: poses computed in world coordinates."""
###
# Operations
###
def copy_body_state_from(self, state: StateKamino) -> None:
"""
Copies the rigid bodies data from the given :class:`StateKamino`.
This operation copies:
- Body poses
- Body twists
Args:
state (StateKamino):
The state container holding time-varying state of the simulation.
"""
# Ensure bodies data has been allocated
if self.bodies is None:
raise RuntimeError("DataKamino.bodies is not finalized.")
# Update rigid bodies data from the model state
wp.copy(self.bodies.q_i, state.q_i)
wp.copy(self.bodies.u_i, state.u_i)
def copy_body_state_to(self, state: StateKamino) -> None:
"""
Copies the rigid bodies data to the given :class:`StateKamino`.
This operation copies:
- Body poses
- Body twists
- Body wrenches
Args:
state (StateKamino):
The state container holding time-varying state of the simulation.
"""
# Ensure bodies data has been allocated
if self.bodies is None:
raise RuntimeError("DataKamino.bodies is not finalized.")
# Update rigid bodies data from the model state
wp.copy(state.q_i, self.bodies.q_i)
wp.copy(state.u_i, self.bodies.u_i)
wp.copy(state.w_i, self.bodies.w_i)
def copy_joint_state_from(self, state: StateKamino) -> None:
"""
Copies the joint state data from the given :class:`StateKamino`.
This operation copies:
- Joint coordinates
- Joint velocities
Args:
state (StateKamino):
The state container holding time-varying state of the simulation.
"""
# Ensure joints data has been allocated
if self.joints is None:
raise RuntimeError("DataKamino.joints is not finalized.")
# Update joint data from the model state
wp.copy(self.joints.q_j, state.q_j)
wp.copy(self.joints.q_j_p, state.q_j_p)
wp.copy(self.joints.dq_j, state.dq_j)
def copy_joint_state_to(self, state: StateKamino) -> None:
"""
Copies the joint state data to the given :class:`StateKamino`.
This operation copies:
- Joint coordinates
- Joint velocities
- Joint constraint reactions
Args:
state (StateKamino):
The state container holding time-varying state of the simulation.
"""
# Ensure joints data has been allocated
if self.joints is None:
raise RuntimeError("DataKamino.joints is not finalized.")
# Update joint data from the model state
wp.copy(state.q_j, self.joints.q_j)
wp.copy(state.q_j_p, self.joints.q_j_p)
wp.copy(state.dq_j, self.joints.dq_j)
wp.copy(state.lambda_j, self.joints.lambda_j)
def copy_joint_control_from(self, control: ControlKamino) -> None:
"""
Copies the joint control inputs from the given :class:`ControlKamino`.
This operation copies:
- Joint direct efforts
- Joint position targets
- Joint velocity targets
- Joint feedforward efforts
Args:
control (ControlKamino):
The control container holding the joint control inputs.
"""
# Ensure joints data has been allocated
if self.joints is None:
raise RuntimeError("DataKamino.joints is not finalized.")
# Update joint data from the control inputs
wp.copy(self.joints.tau_j, control.tau_j)
wp.copy(self.joints.q_j_ref, control.q_j_ref)
wp.copy(self.joints.dq_j_ref, control.dq_j_ref)
wp.copy(self.joints.tau_j_ref, control.tau_j_ref)
================================================
FILE: newton/_src/solvers/kamino/_src/core/geometry.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Geometry Model Types & Containers
"""
from dataclasses import dataclass, field
import warp as wp
# TODO: from .....sim.builder import ModelBuilder
from .....geometry.flags import ShapeFlags
from .shapes import ShapeDescriptorType
from .types import Descriptor, float32, int32, override, transformf
###
# Module interface
###
__all__ = [
"GeometriesData",
"GeometriesModel",
"GeometryDescriptor",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Base Geometry Containers
###
@dataclass
class GeometryDescriptor(Descriptor):
"""
A container to describe a geometry entity.
A geometry entity is an abstraction to represent the composition
of a shape, with a pose w.r.t the world frame of a scene. Each
geometry descriptor bundles the unique object identifiers of the
entity, indices to the associated body, the offset pose w.r.t.
the body, and a shape descriptor.
"""
###
# Basic Attributes
###
body: int = -1
"""
Index of the body to which the geometry entity is attached.\n
Defaults to `-1`, indicating that the geometry has not yet been assigned to a body.\n
The value `-1` also indicates that the geometry, by default, is statically attached to the world.
"""
shape: ShapeDescriptorType | None = None
"""Definition of the shape of the geometry entity of type :class:`ShapeDescriptorType`."""
offset: transformf = field(default_factory=wp.transform_identity)
"""Offset pose of the geometry entity w.r.t. its corresponding body, of type :class:`transformf`."""
# TODO: Use Model.ShapeConfig instead of all these individual fields
# config: ModelBuilder.ShapeConfig = field(default_factory=ModelBuilder.ShapeConfig)
###
# Collision Attributes
###
material: str | int | None = None
"""
The material assigned to the collision geometry instance.\n
Can be specified either as a string name or an integer index.\n
Defaults to `None`, indicating the default material.
"""
group: int = 1
"""
The collision group assigned to the collision geometry.\n
Defaults to the default group with value `1`.
"""
collides: int = 1
"""
The collision groups with which the collision geometry can collide.\n
Defaults to enabling collisions with the default group with value `1`.
"""
max_contacts: int = 0
"""
The maximum number of contacts to generate for the collision geometry.\n
This value provides a hint to the model builder when allocating memory for contacts.\n
Defaults to `0`, indicating no limit is imposed on the number of contacts generated for this geometry.
"""
gap: float = 0.0
"""
Additional detection threshold [m] for this geometry.
Pairwise effect is additive (``g_a + g_b``): the broadphase expands each shape's
bounding volume by ``margin + gap``, and the narrowphase keeps a contact when
``d <= gap_a + gap_b``(with ``d`` measured relative to margin-shifted surfaces).
Defaults to `0.0`.
"""
margin: float = 0.0
"""
Surface offset [m] for this geometry.
Pairwise effect is additive (``m_a + m_b``): contacts are
evaluated against the signed distance to the margin-shifted
surfaces, so resting separation equals ``m_a + m_b``.
Defaults to `0.0`.
"""
###
# Metadata - to be set by the model builder when added
###
wid: int = -1
"""
Index of the world to which the body belongs.\n
Defaults to `-1`, indicating that the body has not yet been added to a world.
"""
gid: int = -1
"""
Index of the geometry w.r.t. its world.\n
Defaults to `-1`, indicating that the geometry has not yet been added to a world.
"""
mid: int = -1
"""
The material index assigned to the collision geometry.\n
Defaults to `-1` indicating that the default material will be assigned.
"""
flags: int = ShapeFlags.VISIBLE | ShapeFlags.COLLIDE_SHAPES | ShapeFlags.COLLIDE_PARTICLES
"""
Shape flags of the geometry entity, used to specify additional properties of the geometry.\n
Defaults to `ShapeFlags.VISIBLE | ShapeFlags.COLLIDE_SHAPES | ShapeFlags.COLLIDE_PARTICLES`,
indicating that the geometry is visible and can collide with shapes and particles.
"""
###
# Operations
###
@property
def is_collidable(self) -> bool:
"""Returns `True` if the geometry is collidable (i.e., group > 0)."""
return self.group > 0
@override
def __hash__(self):
"""Returns a hash computed using the shape descriptor's hash implementation."""
# NOTE: The name-uid-based hash implementation is called if no shape is defined
if self.shape is None:
return super().__hash__()
# Otherwise, use the shape's hash implementation
return self.shape.__hash__()
@override
def __repr__(self):
"""Returns a human-readable string representation of the GeometryDescriptor."""
return (
f"GeometryDescriptor(\n"
f"name: {self.name},\n"
f"uid: {self.uid},\n"
f"body: {self.body},\n"
f"shape: {self.shape},\n"
f"offset: {self.offset},\n"
f"material: {self.material},\n"
f"group: {self.group},\n"
f"collides: {self.collides},\n"
f"max_contacts: {self.max_contacts}\n"
f"gap: {self.gap},\n"
f"margin: {self.margin},\n"
f"wid: {self.wid},\n"
f"gid: {self.gid},\n"
f"mid: {self.mid},\n"
f")"
)
@dataclass
class GeometriesModel:
"""
An SoA-based container to hold time-invariant model data of a set of generic geometry elements.
"""
###
# Meta-Data
###
num_geoms: int = 0
"""Total number of geometry entities in the model (host-side)."""
num_collidable: int = 0
"""Total number of collidable geometry entities in the model (host-side)."""
num_collidable_pairs: int = 0
"""Total number of collidable geometry pairs in the model (host-side)."""
num_excluded_pairs: int = 0
"""Total number of excluded geometry pairs in the model (host-side)."""
model_minimum_contacts: int = 0
"""The minimum number of contacts required for the entire model (host-side)."""
world_minimum_contacts: list[int] | None = None
"""
List of the minimum number of contacts required for each world in the model (host-side).\n
The sum of all elements in `world_minimum_contacts` should equal `model_minimum_contacts`.
"""
label: list[str] | None = None
"""
A list containing the label of each geometry.\n
Length of ``num_geoms`` and type :class:`str`.
"""
###
# Identifiers
###
wid: wp.array | None = None
"""
World index of each geometry entity.\n
Shape of ``(num_geoms,)`` and type :class:`int32`.
"""
gid: wp.array | None = None
"""
Geometry index of each geometry entity w.r.t its world.\n
Shape of ``(num_geoms,)`` and type :class:`int32`.
"""
bid: wp.array | None = None
"""
Body index of each geometry entity.\n
Shape of ``(num_geoms,)`` and type :class:`int32`.
"""
###
# Parameterization
###
type: wp.array | None = None
"""
Shape index of each geometry entity.\n
Shape of ``(num_geoms,)`` and type :class:`int32`.
"""
flags: wp.array | None = None
"""
Shape flags of each geometry entity.\n
Shape of ``(num_geoms,)`` and type :class:`int32`.
"""
ptr: wp.array | None = None
"""
Pointer to the source data of the shape.\n
For primitive shapes this is `0` indicating NULL, otherwise it points to
the shape data, which can correspond to a mesh, heightfield, or SDF.\n
Shape of ``(num_geoms,)`` and type :class:`uint64`.
"""
params: wp.array | None = None
"""
Shape parameters of each geometry entity if they are shape primitives.\n
Shape of ``(num_geoms,)`` and type :class:`vec4f`.
"""
offset: wp.array | None = None
"""
Offset poses of the geometry elements w.r.t. their corresponding bodies.\n
Shape of ``(num_geoms,)`` and type :class:`transformf`.
"""
###
# Collisions
###
material: wp.array | None = None
"""
Material index assigned to each collision geometry.\n
Shape of ``(num_geoms,)`` and type :class:`int32`.
"""
group: wp.array | None = None
"""
Collision group assigned to each collision geometry.\n
Shape of ``(num_geoms,)`` and type :class:`uint32`.
"""
gap: wp.array | None = None
"""
Additional detection threshold [m] for each collision geometry.\n
Pairwise additive. Used by both broadphase (AABB expansion) and
narrowphase (contact retention).\n
Shape of ``(num_geoms,)`` and type :class:`float32`.
"""
margin: wp.array | None = None
"""
Surface offset [m] for each collision geometry.\n
Pairwise additive. Determines resting separation between shapes.\n
Shape of ``(num_geoms,)`` and type :class:`float32`.
"""
collidable_pairs: wp.array | None = None
"""
Geometry-pair indices that are explicitly considered for collision detection.
This array is used in broad-phase collision detection.\n
Shape of ``(num_collidable_pairs,)`` and type :class:`vec2i`.
"""
excluded_pairs: wp.array | None = None
"""
Geometry-pair indices that are explicitly excluded from collision detection.\n
This array is used in broad-phase collision detection.\n
Shape of ``(num_excluded_geom_pairs,)`` and type :class:`vec2i`.
"""
@dataclass
class GeometriesData:
"""
An SoA-based container to hold time-varying data of a set of generic geometry entities.
Attributes:
num_geoms (int32): The total number of geometry entities in the model (host-side).
pose (wp.array | None): The poses of the geometry entities in world coordinates.\n
Shape of ``(num_geoms,)`` and type :class:`transformf`.
"""
num_geoms: int = 0
"""Total number of geometry entities in the model (host-side)."""
pose: wp.array | None = None
"""
The poses of the geometry entities in world coordinates.\n
Shape of ``(num_geoms,)`` and type :class:`transformf`.
"""
###
# Kernels
###
@wp.kernel
def _update_geometries_state(
# Inputs:
geom_bid: wp.array(dtype=int32),
geom_offset: wp.array(dtype=transformf),
body_pose: wp.array(dtype=transformf),
# Outputs:
geom_pose: wp.array(dtype=transformf),
):
"""
A kernel to update poses of geometry entities in world
coordinates from the poses of their associated bodies.
**Inputs**:
body_pose (wp.array):
Array of per-body poses in world coordinates.\n
Shape of ``(num_bodies,)`` and type :class:`transformf`.
geom_bid (wp.array):
Array of per-geom body indices.\n
Shape of ``(num_geoms,)`` and type :class:`int32`.
geom_offset (wp.array):
Array of per-geom pose offsets w.r.t. their associated bodies.\n
Shape of ``(num_geoms,)`` and type :class:`transformf`.
**Outputs**:
geom_pose (wp.array):
Array of per-geom poses in world coordinates.\n
Shape of ``(num_geoms,)`` and type :class:`transformf`.
"""
# Retrieve the geometry index from the thread grid
gid = wp.tid()
# Retrieve the body index associated with the geometry
bid = geom_bid[gid]
# Retrieve the pose of the corresponding body
X_b = wp.transform_identity(dtype=float32)
if bid > -1:
X_b = body_pose[bid]
# Retrieve the geometry offset pose w.r.t. the body
X_bg = geom_offset[gid]
# Compute the geometry pose in world coordinates
X_g = wp.transform_multiply(X_b, X_bg)
# Store the updated geometry pose
geom_pose[gid] = X_g
###
# Launchers
###
def update_geometries_state(
body_poses: wp.array,
geom_model: GeometriesModel,
geom_data: GeometriesData,
):
"""
Launches a kernel to update poses of geometry entities in
world coordinates from the poses of their associated bodies.
Args:
body_poses (wp.array):
The poses of the bodies in world coordinates.\n
Shape of ``(num_bodies,)`` and type :class:`transformf`.
geom_model (GeometriesModel):
The model container holding time-invariant geometry data.
geom_data (GeometriesData):
The data container of the geometry elements.
"""
wp.launch(
_update_geometries_state,
dim=geom_model.num_geoms,
inputs=[geom_model.bid, geom_model.offset, body_poses],
outputs=[geom_data.pose],
device=body_poses.device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/core/gravity.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""The gravity descriptor and model used throughout Kamino"""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
import warp as wp
from .....sim.model import Model
from ..utils import logger as msg
from .types import ArrayLike, Descriptor, override, vec3f, vec4f
###
# Module interface
###
__all__ = [
"GRAVITY_ACCEL_DEFAULT",
"GRAVITY_DIREC_DEFAULT",
"GRAVITY_NAME_DEFAULT",
"GravityDescriptor",
"GravityModel",
"convert_model_gravity",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
GRAVITY_NAME_DEFAULT = "Earth"
"""The default gravity descriptor name, set as 'Earth'."""
GRAVITY_ACCEL_DEFAULT = 9.8067
"""
The default gravitational acceleration in m/s^2.
Equal to Earth's standard gravity of approximately 9.8067 m/s^2.
"""
GRAVITY_DIREC_DEFAULT = [0.0, 0.0, -1.0]
"""The default direction of gravity defined as -Z."""
###
# Containers
###
class GravityDescriptor(Descriptor):
"""
A container to describe a world's gravity.
Attributes:
name (str): The name of the gravity descriptor.
uid (str): The unique identifier of the gravity descriptor.
enabled (bool): Whether gravity is enabled.
acceleration (float): The gravitational acceleration magnitude in m/s^2.
direction (vec3f): The normalized direction vector of gravity.
"""
def __init__(
self,
enabled: bool = True,
acceleration: float = GRAVITY_ACCEL_DEFAULT,
direction: ArrayLike = GRAVITY_DIREC_DEFAULT,
name: str = GRAVITY_NAME_DEFAULT,
uid: str | None = None,
):
"""
Initialize the gravity descriptor.
Args:
enabled (bool): Whether gravity is enabled.\n
Defaults to `True` to enable gravity by default.
acceleration (float): The gravitational acceleration magnitude in m/s^2.\n
Defaults to 9.8067 m/s^2 (Earth's gravity).
direction (vec3f): The normalized direction vector of gravity.\n
Defaults to pointing down the -Z axis.
name (str): The name of the gravity descriptor.
uid (str | None): Optional unique identifier of the gravity descriptor.
"""
super().__init__(name, uid)
self._enabled: bool = enabled
self._acceleration: float = acceleration
self._direction: vec3f = wp.normalize(vec3f(direction))
@override
def __repr__(self):
"""Returns a human-readable string representation of the GravityDescriptor."""
return (
f"GravityDescriptor(\n"
f"name={self.name},\n"
f"uid={self.uid},\n"
f"enabled={self.enabled},\n"
f"acceleration={self.acceleration},\n"
f"direction={self.direction}\n"
f")"
)
@property
def enabled(self) -> bool:
"""Returns whether gravity is enabled."""
return self._enabled
@enabled.setter
def enabled(self, on: bool):
"""Sets whether gravity is enabled."""
self._enabled = on
@property
def acceleration(self) -> float:
"""Returns the gravitational acceleration."""
return self._acceleration
@acceleration.setter
def acceleration(self, g: float):
"""Sets the gravitational acceleration."""
self._acceleration = g
@property
def direction(self) -> vec3f:
"""Returns the normalized direction vector of gravity."""
return self._direction
@direction.setter
def direction(self, dir: vec3f):
"""Sets the normalized direction vector of gravity."""
self._direction = wp.normalize(dir)
def dir_accel(self) -> vec4f:
"""Returns the gravity direction and acceleration as compactly as a :class:`vec4f`."""
return vec4f([self.direction[0], self.direction[1], self.direction[2], self.acceleration])
def vector(self) -> vec4f:
"""Returns the effective gravity vector and enabled flag compactly as a :class:`vec4f`."""
g = vec3f(self.acceleration * self.direction)
return vec4f([g[0], g[1], g[2], float(self.enabled)])
@dataclass
class GravityModel:
"""
A container to hold the time-invariant gravity model data.
Attributes:
g_dir_acc (wp.array): The gravity direction and acceleration vector as ``[g_dir_x, g_dir_y, g_dir_z, g_accel]``.
vector (wp.array): The gravity vector defined as ``[g_x, g_y, g_z, enabled]``.
"""
g_dir_acc: wp.array | None = None
"""
The gravity direction and acceleration vector.\n
Shape of ``(num_worlds,)`` and type :class:`vec4f`.
"""
vector: wp.array | None = None
"""
The gravity vector defined as ``[g_x, g_y, g_z, enabled]``.\n
Shape of ``(num_worlds,)`` and type :class:`vec4f`.
"""
###
# Operations
###
@staticmethod
def from_newton(model_in: Model) -> GravityModel:
return convert_model_gravity(model_in)
###
# Utilities
###
# TODO: Re-implement using kernels
def convert_model_gravity(model_in: Model, gravity_out: GravityModel | None = None) -> GravityModel:
"""
Converts the gravity representation from the Newton model to the Kamino format.
Args:
model_in (Model):
The input Newton model containing the gravity information to be converted.
gravity_out (GravityModel, optional):
The output GravityModel instance where the converted gravity data will be stored.\n
If `None`, a new GravityModel instance will be created and returned.\n
If the arrays within `gravity_out` are not already allocated
with the appropriate shapes, this function will allocate them.
"""
# Capture the necessary properties from source model
gravity_np = model_in.gravity.numpy().copy()
# Allocate data for the conversion
g_dir_acc_np = np.zeros((model_in.world_count, 4), dtype=np.float32)
vector_np = np.zeros((model_in.world_count, 4), dtype=np.float32)
# Convert each world's gravity vector into direction
# and acceleration, and pack into the output arrays
for w in range(model_in.world_count):
g_vec = gravity_np[w, :]
accel = float(np.linalg.norm(g_vec))
if accel > 0.0:
direction = g_vec / accel
else:
direction = np.array([0.0, 0.0, -1.0])
g_dir_acc_np[w, :3] = direction
g_dir_acc_np[w, 3] = accel
vector_np[w, :3] = g_vec
vector_np[w, 3] = 1.0 if accel > 0.0 else 0.0
# If the output gravity model is not provided, create a new one with allocated arrays;
if gravity_out is None:
with wp.ScopedDevice(model_in.device):
gravity_out = GravityModel(
g_dir_acc=wp.array(g_dir_acc_np, dtype=vec4f),
vector=wp.array(vector_np, dtype=vec4f),
)
# Otherwise, ensure the provided model has allocated arrays of the
# correct shape and type, and copy the converted data into them.
else:
# Ensure that the output GravityModel has allocated arrays of the correct shape and type
if gravity_out.g_dir_acc is None or gravity_out.g_dir_acc.shape != (model_in.world_count, 4):
msg.warning("Output `GravityModel.g_dir_acc` array does not have matching shape. Allocating a new array.")
gravity_out.g_dir_acc = wp.array(g_dir_acc_np, dtype=vec4f)
else:
gravity_out.g_dir_acc.assign(g_dir_acc_np)
if gravity_out.vector is None or gravity_out.vector.shape != (model_in.world_count, 4):
msg.warning("Output `GravityModel.vector` array does not have matching shape. Allocating a new array.")
gravity_out.vector = wp.array(vector_np, dtype=vec4f)
else:
gravity_out.vector.assign(vector_np)
# Return the output gravity model
return gravity_out
================================================
FILE: newton/_src/solvers/kamino/_src/core/inertia.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides functions to compute moments
of inertia for solid geometric bodies.
"""
from .types import mat33f
###
# Module interface
###
__all__ = [
"solid_cone_body_moment_of_inertia",
"solid_cuboid_body_moment_of_inertia",
"solid_cylinder_body_moment_of_inertia",
"solid_ellipsoid_body_moment_of_inertia",
"solid_sphere_body_moment_of_inertia",
]
###
# Functions
###
def solid_sphere_body_moment_of_inertia(m: float, r: float) -> mat33f:
Ia = 0.4 * m * r * r
i_I_i = mat33f([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ia]])
return i_I_i
def solid_cylinder_body_moment_of_inertia(m: float, r: float, h: float) -> mat33f:
Ia = 1.0 / 12.0 * m * (3.0 * r * r + h * h)
Ib = 0.5 * m * r * r
i_I_i = mat33f([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]])
return i_I_i
def solid_cone_body_moment_of_inertia(m: float, r: float, h: float) -> mat33f:
Ia = 0.15 * m * (r * r + 0.25 * h * h)
Ib = 0.3 * m * r * r
i_I_i = mat33f([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]])
return i_I_i
def solid_ellipsoid_body_moment_of_inertia(m: float, a: float, b: float, c: float) -> mat33f:
Ia = 0.2 * m * (b * b + c * c)
Ib = 0.2 * m * (a * a + c * c)
Ic = 0.2 * m * (a * a + b * b)
i_I_i = mat33f([[Ia, 0.0, 0.0], [0.0, Ib, 0.0], [0.0, 0.0, Ic]])
return i_I_i
def solid_cuboid_body_moment_of_inertia(m: float, w: float, h: float, d: float) -> mat33f:
Ia = (1.0 / 12.0) * m * (h * h + d * d)
Ib = (1.0 / 12.0) * m * (w * w + d * d)
Ic = (1.0 / 12.0) * m * (w * w + h * h)
i_I_i = mat33f([[Ia, 0.0, 0.0], [0.0, Ib, 0.0], [0.0, 0.0, Ic]])
return i_I_i
================================================
FILE: newton/_src/solvers/kamino/_src/core/joints.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Provides definitions of core joint types & containers"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
from enum import IntEnum
import numpy as np
import warp as wp
from warp._src.types import Any, Int, Vector
from .....core.types import MAXVAL
from .....sim import JointTargetMode, JointType
from .math import FLOAT32_MAX, FLOAT32_MIN, PI, TWO_PI
from .types import (
ArrayLike,
Descriptor,
mat33f,
override,
quatf,
transformf,
vec1f,
vec1i,
vec2f,
vec2i,
vec3f,
vec3i,
vec4f,
vec4i,
vec5i,
vec6i,
vec7f,
)
###
# Module interface
###
__all__ = [
"JointActuationType",
"JointCorrectionMode",
"JointDescriptor",
"JointDoFType",
"JointsData",
"JointsModel",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
JOINT_QMIN: float = -MAXVAL
""" Sentinel value indicating the minimum joint coordinate limit."""
JOINT_QMAX: float = MAXVAL
""" Sentinel value indicating the maximum joint coordinate limit."""
JOINT_DQMAX: float = 1e6
""" Sentinel value indicating the maximum joint velocity limit."""
JOINT_TAUMAX: float = 1e6
""" Sentinel value indicating the maximum joint effort limit."""
###
# Enumerations
###
class JointActuationType(IntEnum):
"""
An enumeration of the joint actuation types.
"""
PASSIVE = 0
"""Passive joint type, i.e. not actuated."""
FORCE = 1
"""Force-controlled joint type, i.e. actuated by set of joint-space forces and/or torques."""
POSITION = 2
"""Position-controlled joint type, i.e. actuated by set of joint-space coordinate targets."""
VELOCITY = 3
"""Velocity-controlled joint type, i.e. actuated by set of joint-space velocity targets."""
POSITION_VELOCITY = 4
"""Position-velocity-controlled joint type, i.e. actuated by set of joint-space coordinate and velocity targets."""
POSITION_VELOCITY_FORCE = 5
"""
Position + velocity + force-controlled joint type, i.e. actuated
by set of joint-space coordinate, velocity, and force targets.
"""
@override
def __str__(self):
"""Returns a string representation of the joint actuation type."""
return f"JointActuationType.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the joint actuation type."""
return self.__str__()
@staticmethod
def to_newton(act_type: JointActuationType) -> JointTargetMode:
"""
Converts a `JointActuationType` to the corresponding `JointTargetMode`.
Args:
type (JointActuationType): The joint actuation type to convert.
Returns:
JointTargetMode:
The corresponding Newton joint target mode, or None if not applicable.
"""
_MAP_TO_NEWTON: dict[JointActuationType, JointTargetMode | None] = {
JointActuationType.PASSIVE: JointTargetMode.NONE,
JointActuationType.FORCE: JointTargetMode.EFFORT,
JointActuationType.POSITION: JointTargetMode.POSITION,
JointActuationType.VELOCITY: JointTargetMode.VELOCITY,
JointActuationType.POSITION_VELOCITY: JointTargetMode.POSITION_VELOCITY,
# No direct mapping to a single Newton target mode since it
# involves both position/velocity targets and force targets
JointActuationType.POSITION_VELOCITY_FORCE: None,
}
target_mode = _MAP_TO_NEWTON.get(act_type, None)
if target_mode is None:
raise ValueError(f"Unsupported joint actuation type for conversion to Newton joint target mode: {act_type}")
return target_mode
@staticmethod
def from_newton(target_mode: JointTargetMode) -> JointActuationType:
"""
Converts a `JointTargetMode` to the corresponding `JointActuationType`.
Args:
mode (JointTargetMode): The Newton joint target mode to convert.
Returns:
JointActuationType | None:
The corresponding joint actuation type, or None if not applicable.
"""
_MAP_FROM_NEWTON: dict[JointTargetMode, JointActuationType] = {
JointTargetMode.NONE: JointActuationType.PASSIVE,
JointTargetMode.EFFORT: JointActuationType.FORCE,
JointTargetMode.POSITION: JointActuationType.POSITION,
JointTargetMode.VELOCITY: JointActuationType.VELOCITY,
JointTargetMode.POSITION_VELOCITY: JointActuationType.POSITION_VELOCITY,
}
act_type = _MAP_FROM_NEWTON.get(target_mode, None)
if act_type is None:
raise ValueError(f"Unsupported joint target mode for conversion to joint actuation type: {target_mode}")
return act_type
class JointCorrectionMode(IntEnum):
"""
An enumeration of the correction modes applicable to rotational joint coordinates.
"""
TWOPI = 0
"""
Rotational joint coordinates are computed to always lie within ``[-2*pi, 2*pi]``.\n
This is the default correction mode for all joints with rotational DoFs.
"""
CONTINUOUS = 1
"""
Rotational joint coordinates are continuously accumulated and thus unbounded.\n
This means that joint coordinates can increase/decrease indefinitely over time,
but are limited to numerical precision limits (i.e. ``[JOINT_QMIN, JOINT_QMAX]``).
"""
NONE = -1
"""
No joint coordinate correction is applied.\n
Rotational joint coordinates are computed to lie within ``[-pi, pi]``.
"""
@property
def bound(self) -> float:
"""
Returns the numerical bound imposed by the correction mode.
"""
if self.value == self.TWOPI:
return float(TWO_PI)
elif self.value == self.CONTINUOUS:
return float(JOINT_QMAX)
elif self.value == self.NONE:
return float(PI)
else:
raise ValueError(f"Unknown joint correction mode: {self.value}")
@classmethod
def from_string(cls, s: str) -> JointCorrectionMode:
"""Converts a string to a JointCorrectionMode enum value."""
try:
return cls[s.upper()]
except KeyError as e:
raise ValueError(f"Invalid JointCorrectionMode: {s}. Valid options are: {[e.name for e in cls]}") from e
@override
def __str__(self):
"""Returns a string representation of the joint correction mode."""
return f"JointCorrectionMode.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the joint correction mode."""
return self.__str__()
@staticmethod
def parse_usd_attribute(value: str, context: dict[str, Any] | None = None) -> str:
"""Parse joint correction option imported from USD, following the KaminoSceneAPI schema."""
if not isinstance(value, str):
raise TypeError("Parser expects input of type 'str'.")
mapping = {"none": "none", "twopi": "twopi", "continuous": "continuous"}
lower_value = value.lower().strip()
if lower_value not in mapping:
raise ValueError(f"Joint correction parameter '{value}' is not a valid option.")
return mapping[lower_value]
class JointDoFType(IntEnum):
"""
An enumeration of the supported joint Degrees-of-Freedom (DoF) types.
Joint "DoFs" are defined as the local directions of admissible motion, and
thus always equal `num_dofs = 6 - num_cts`, where `6` are the number of
DoFs for unconstrained rigid motions in SE(3) and `num_cts` is the number
of bilateral equality constraints imposed by the joint. Thus DoFs can be
intuited as corresponding to the velocity-level description of the motion.
Joint "coordinates" are defined as the variables used to parameterize the
space of configurations (i.e. translations and rotations) admissible by
the joint. Thus, the number of coordinates `num_coords` is generally not
equal to the number of DoFs `num_dofs`, i.e. `num_coords != num_dofs`,
since joints may use redundant or non-minimal parameterizations. For example,
a spherical joint has `num_dofs = 3` underlying DoFs (at velocity-level),
yet it is commonly parameterized using a 4D unit-quaternion, i.e.
`num_coords = 4` at configuration-level.
This class also provides property methods to query the number of:
- Generalized coordinates
- Degrees of Freedom (DoFs)
- Equality constraints
Conventions:
- Each joint connects a Base body `B` to a Follower body `F`.
- The relative motion of body `F' w.r.t. body `B` defines the positive direction of the joint's DoFs.
- `R_x`, `R_y`, `R_z`: denote rotational DoFs about the local x, y, z axes respectively.
- `T_x`, `T_y`, `T_z`: denote translational DoFs along the local x, y, z axes respectively.
- Joints are indexed by `j`, and we often employ the subscript notation `*_j`.
- `c_j` | `num_coords`: denote the number of generalized coordinates defined by joint `j`.
- `d_j` | `num_dofs`: denote the number of DoFs defined by joint `j`.
- `e_j` | `num_dynamic_cts`: denote the number of dynamic equality constraints imposed by joint `j`.
- `f_j` | `num_kinematic_cts`: denote the number of kinematic equality constraints imposed by joint `j`.
"""
FREE = 0
"""
A 6-DoF free-floating joint, with rotational + translational DoFs
along {`R_x`, `R_y`, `R_z`, `T_x`, `T_y`, `T_z`}.
Coordinates:
7D transform: 3D position + 4D unit quaternion
DoFs:
6D twist: 3D angular velocity + 3D linear velocity
Constraints:
None
"""
REVOLUTE = 1
"""
A 1-DoF revolute joint, with rotational DoF along {`R_x`}.
Coordinates:
1D angle: {`R_x`}
DoFs:
1D angular velocity: {`R_x`}
Constraints:
5D vector: {`T_x`, `T_y`, `T_z`, `R_y`, `R_z`}
"""
PRISMATIC = 2
"""
A 1-DoF prismatic joint, with translational DoF along {`T_x`}.
Coordinates:
1D distance: {`T_x`}
DoFs:
1D linear velocity: {`T_x`}
Constraints:
5D vector: {`T_y`, `T_z`, `R_x`, `R_y`, `R_z`}
"""
CYLINDRICAL = 3
"""
A 2-DoF cylindrical joint, with rotational + translational DoFs along {`R_x`, `T_x`}.
Coordinates:
2D vector of angle {`R_x`} + 1D distance {`T_x`}
DoFs:
2D vector of angular velocity {`R_x`} + linear velocity {`T_x`}
"""
# TODO: Add support for PLANAR joints with 2D linear DOFS along {`T_x`, `T_y`}
# and 1D angular DOF along {`R_z`}, with constraints for {`T_z`, `R_x`, `R_y`}
UNIVERSAL = 4
"""
A 2-DoF universal joint, with rotational DoFs along {`R_x`, `R_y`}.
This universal joint is implemented as being equivalent to two consecutive
revolute joints, rotating an intermediate (virtual) body about `R_x` w.r.t
the Base body `B`, then rotating the Follower body `F` about `R_y` of the
intermediate body. Thus, this implementation necessarily assumes the first
rotation is always about `R_x` followed by the rotation about `R_y`.
Coordinates:
2D angles: {`R_x`, `R_y`}
DoFs:
2D angular velocities: {`R_x`, `R_y`}
Constraints:
4D vector: {`T_x`, `T_y`, `T_z`, `R_z`}
"""
SPHERICAL = 5
"""
A 3-DoF spherical joint, with rotational DoFs along {`R_x`, `R_y`, `R_z`}.
Coordinates:
4D unit-quaternion to parameterize {`R_x`, `R_y`, `R_z`}
DoFs:
3D angular velocities: {`R_x`, `R_y`, `R_z`}
Constraints:
3D vector: {`T_x`, `T_y`, `T_z`}
"""
GIMBAL = 6
"""
A 3-DoF gimbal joint, with rotational DoFs along {`R_x`, `R_y`, `R_z`}.
**DISCLAIMER**: This joint is not yet fully supported, and currently behaves
identically to the SPHERICAL joint. We do not recommend using it at present time.
Coordinates:
3D euler angles: {`R_x`, `R_y`, `R_z`}
DoFs:
3D angular velocities: {`R_x`, `R_y`, `R_z`}
Constraints:
3D vector: {`T_x`, `T_y`, `T_z`}
"""
CARTESIAN = 7
"""
A 3-DoF Cartesian joint, with translational DoFs along {`T_x`, `T_y`, `T_z`}.
Coordinates:
3D distances: {`T_x`, `T_y`, `T_z`}
DoFs:
3D linear velocities: {`T_x`, `T_y`, `T_z`}
Constraints:
3D vector: {`R_x`, `R_y`, `R_z`}
"""
FIXED = 8
"""
A 0-DoF fixed joint, fully constraining the relative motion between the connected bodies.
Coordinates:
None
DoFs:
None
Constraints:
6D vector: {`T_x`, `T_y`, `T_z`, `R_x`, `R_y`, `R_z`}
"""
###
# Operations
###
@override
def __str__(self):
"""Returns a string representation of the joint DoF type."""
return f"JointDoFType.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the joint DoF type."""
return self.__str__()
@property
def num_coords(self) -> int:
"""
Returns the number of generalized coordinates defined by the joint DoF type.
"""
if self.value == self.FREE:
return 7 # 3D position + 4D quaternion
elif self.value == self.REVOLUTE:
return 1 # 1D angle
elif self.value == self.PRISMATIC:
return 1 # 1D distance
elif self.value == self.CYLINDRICAL:
return 2 # 2D vector of angle + distance
elif self.value == self.UNIVERSAL:
return 2 # 2D angles
elif self.value == self.SPHERICAL:
return 4 # 4D unit-quaternion
elif self.value == self.GIMBAL:
return 3 # 3D euler angles
elif self.value == self.CARTESIAN:
return 3 # 3D distances
elif self.value == self.FIXED:
return 0 # None
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
@property
def num_dofs(self) -> int:
"""
Returns the number of DoFs defined by the joint DoF type.
"""
if self.value == self.FREE:
return 6 # 3D angular velocity + 3D linear velocity
elif self.value == self.REVOLUTE:
return 1 # 1D angular velocity
elif self.value == self.PRISMATIC:
return 1 # 1D linear velocity
elif self.value == self.CYLINDRICAL:
return 2 # 1D angular velocity + 1D linear velocity
elif self.value == self.UNIVERSAL:
return 2 # 2D angular velocities
elif self.value == self.SPHERICAL:
return 3 # 3D angular velocities
elif self.value == self.GIMBAL:
return 3 # 3D angular velocities
elif self.value == self.CARTESIAN:
return 3 # 3D linear velocities
elif self.value == self.FIXED:
return 0 # None
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
@property
def num_cts(self) -> int:
"""
Returns the number of constraints defined by the joint DoF type.
"""
if self.value == self.FREE:
return 0 # None
elif self.value == self.REVOLUTE:
return 5 # 5D vector for `{T_x, T_y, T_z, R_y, R_z}`
elif self.value == self.PRISMATIC:
return 5 # 5D vector for `{T_x, T_y, T_z, R_y, R_z}`
elif self.value == self.CYLINDRICAL:
return 4 # 4D vector for `{T_x, T_y, R_y, R_z}`
elif self.value == self.UNIVERSAL:
return 4 # 4D vector for `{R_x, R_y, R_z, R_w}`
elif self.value == self.SPHERICAL:
return 3 # 3D vector for `{R_x, R_y, R_z}`
elif self.value == self.GIMBAL:
return 3 # 3D vector for `{R_x, R_y, R_z}`
elif self.value == self.CARTESIAN:
return 3 # 3D vector for `{T_x, T_y, T_z}`
elif self.value == self.FIXED:
return 6 # 6D vector for `{T_x, T_y, T_z, R_x, R_y, R_z}`
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
@property
def cts_axes(self) -> Vector[Any, Int]:
"""
Returns the indices of the joint's constraint axes.
"""
if self.value == self.FREE:
return [] # Empty vector (TODO: wp.constant(vec0i()))
if self.value == self.REVOLUTE:
return wp.constant(vec5i(0, 1, 2, 4, 5))
elif self.value == self.PRISMATIC:
return wp.constant(vec5i(1, 2, 3, 4, 5))
elif self.value == self.CYLINDRICAL:
return wp.constant(vec4i(1, 2, 4, 5))
elif self.value == self.UNIVERSAL:
return wp.constant(vec4i(0, 1, 2, 5))
elif self.value == self.SPHERICAL:
return wp.constant(vec3i(0, 1, 2))
elif self.value == self.GIMBAL:
return wp.constant(vec3i(0, 1, 2))
elif self.value == self.CARTESIAN:
return wp.constant(vec3i(3, 4, 5))
elif self.value == self.FIXED:
return wp.constant(vec6i(0, 1, 2, 3, 4, 5))
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
@property
def dofs_axes(self) -> Vector[Any, Int]:
"""
Returns the indices of the joint's DoF axes.
"""
if self.value == self.FREE:
return wp.constant(vec6i(0, 1, 2, 3, 4, 5))
if self.value == self.REVOLUTE:
return wp.constant(vec1i(3))
elif self.value == self.PRISMATIC:
return wp.constant(vec1i(0))
elif self.value == self.CYLINDRICAL:
return wp.constant(vec2i(0, 3))
elif self.value == self.UNIVERSAL:
return wp.constant(vec2i(3, 4))
elif self.value == self.SPHERICAL:
return wp.constant(vec3i(3, 4, 5))
elif self.value == self.GIMBAL:
return wp.constant(vec3i(3, 4, 5))
elif self.value == self.CARTESIAN:
return wp.constant(vec3i(0, 1, 2))
elif self.value == self.FIXED:
return [] # Empty vector (TODO: wp.constant(vec0i()))
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
@property
def coords_storage_type(self) -> Any:
"""
Returns the data type required to store the joint's generalized coordinates.
"""
if self.value == self.FREE:
return vec7f
elif self.value == self.REVOLUTE:
return vec1f
elif self.value == self.PRISMATIC:
return vec1f
elif self.value == self.CYLINDRICAL:
return vec2f
elif self.value == self.UNIVERSAL:
return vec2f
elif self.value == self.SPHERICAL:
return vec4f
elif self.value == self.GIMBAL:
return vec3f
elif self.value == self.CARTESIAN:
return vec3f
elif self.value == self.FIXED:
return None
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
@property
def coords_physical_type(self) -> Any:
"""
Returns the data type required to represent the joint's generalized coordinates.
"""
if self.value == self.FREE:
return transformf
elif self.value == self.REVOLUTE:
return vec1f
elif self.value == self.PRISMATIC:
return vec1f
elif self.value == self.CYLINDRICAL:
return vec2f
elif self.value == self.UNIVERSAL:
return vec2f
elif self.value == self.SPHERICAL:
return quatf
elif self.value == self.GIMBAL:
return vec3f
elif self.value == self.CARTESIAN:
return vec3f
elif self.value == self.FIXED:
return None
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
@property
def reference_coords(self) -> list[float]:
"""
Returns the data type required to represent the joint's generalized coordinates.
"""
if self.value == self.FREE:
return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
elif self.value == self.REVOLUTE:
return [0.0]
elif self.value == self.PRISMATIC:
return [0.0]
elif self.value == self.CYLINDRICAL:
return [0.0, 0.0]
elif self.value == self.UNIVERSAL:
return [0.0, 0.0]
elif self.value == self.SPHERICAL:
return [0.0, 0.0, 0.0, 1.0]
elif self.value == self.GIMBAL:
return [0.0, 0.0, 0.0]
elif self.value == self.CARTESIAN:
return [0.0, 0.0, 0.0]
elif self.value == self.FIXED:
return []
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
def coords_bound(self, correction: JointCorrectionMode) -> list[float]:
"""
Returns a list of numeric bounds for the generalized coordinates,
of the joint DoF type, imposed by the specified correction mode.
"""
rotation_bound = correction.bound
if self.value == self.FREE:
return [JOINT_QMAX] * 7
elif self.value == self.REVOLUTE:
return [rotation_bound]
elif self.value == self.PRISMATIC:
return [JOINT_QMAX]
elif self.value == self.CYLINDRICAL:
return [JOINT_QMAX, rotation_bound]
elif self.value == self.UNIVERSAL:
return [rotation_bound, rotation_bound]
elif self.value == self.SPHERICAL:
return [JOINT_QMAX] * 4
elif self.value == self.GIMBAL:
return [rotation_bound] * 3
elif self.value == self.CARTESIAN:
return [JOINT_QMAX] * 3
elif self.value == self.FIXED:
return []
else:
raise ValueError(f"Unknown joint DoF type: {self.value}")
@staticmethod
def to_newton(dof_type: JointDoFType) -> JointType | None:
"""
Converts a `JointDoFType` to the corresponding `JointType`.
Args:
dof_type (JointDoFType): The joint DoF type to convert.
Returns:
JointType | None: The corresponding Newton joint type, or None if unsupported.
"""
_MAP_TO_NEWTON: dict[JointDoFType, JointType] = {
# All trivially supported DoF types map directly
# to their corresponding Newton joint types
JointDoFType.FREE: JointType.FREE,
JointDoFType.REVOLUTE: JointType.REVOLUTE,
JointDoFType.PRISMATIC: JointType.PRISMATIC,
JointDoFType.SPHERICAL: JointType.BALL,
JointDoFType.FIXED: JointType.FIXED,
# All kamino-specific joint types map to D6
JointDoFType.CARTESIAN: JointType.D6,
JointDoFType.CYLINDRICAL: JointType.D6,
JointDoFType.UNIVERSAL: JointType.D6,
JointDoFType.GIMBAL: JointType.D6,
}
joint_type = _MAP_TO_NEWTON.get(dof_type, None)
if joint_type is None:
raise ValueError(f"Unsupported joint DoF type for conversion to Newton joint type: {dof_type}")
return joint_type
@staticmethod
def from_newton(
type: JointType,
q_count: int,
qd_count: int,
dof_dim: tuple[int, int],
limit_lower: np.ndarray,
limit_upper: np.ndarray,
) -> JointDoFType:
"""
Converts a `JointType` to the corresponding `JointDoFType`.
Args:
type (JointType): The Newton joint type to convert.
Returns:
JointDoFType: The corresponding joint DoF type.
"""
# First try directly mapping the trivially supported types
_MAP_TO_KAMINO: dict[JointType, JointDoFType | None] = {
JointType.FREE: JointDoFType.FREE,
JointType.REVOLUTE: JointDoFType.REVOLUTE,
JointType.PRISMATIC: JointDoFType.PRISMATIC,
JointType.BALL: JointDoFType.SPHERICAL,
JointType.FIXED: JointDoFType.FIXED,
# NOTE: D6 joints require special handling
# to infer the corresponding DoF type
JointType.D6: None,
}
dof_type = _MAP_TO_KAMINO.get(type, None)
if dof_type is not None:
return dof_type
# If the type is not directly supported, attempt to infer the DoF type based on the number of DoFs
if dof_type is None or type == JointType.D6:
# Ensure that q_count and qd_count are provided for inference
if q_count is None or qd_count is None:
raise ValueError("q_count and qd_count must be provided for inference of unsupported joint types.")
# Ensure dof_dim is provided for inference
if dof_dim is None or not isinstance(dof_dim, tuple) or len(dof_dim) != 2:
raise ValueError(
"dof_dim must be provided as a tuple of length 2 for inference of unsupported joint types."
)
# Ensure the limits are provided for inference
if limit_lower is None or limit_upper is None:
raise ValueError(
"limit_lower and limit_upper must be provided for inference of unsupported joint types."
)
if not isinstance(limit_lower, np.ndarray) or not isinstance(limit_upper, np.ndarray):
raise TypeError(
"limit_lower and limit_upper must be numpy arrays for inference of unsupported joint types."
)
if limit_lower.shape != limit_upper.shape:
raise ValueError(
f"limit_lower and limit_upper must have the same shape, got: "
f"limit_lower.shape: {limit_lower.shape}, limit_upper.shape: {limit_upper.shape}."
)
if limit_lower.shape[0] != qd_count or limit_upper.shape[0] != qd_count:
raise ValueError(
f"The length of limit_lower and limit_upper must match qd_count ({qd_count}), got:"
f"\n limit_lower: {limit_lower} (shape={limit_lower.shape})"
f"\n limit_upper: {limit_upper} (shape={limit_upper.shape})"
)
# Map to the DoF type based on the dimensions of the joint
if q_count == 0 and qd_count == 0 and dof_dim == (0, 0):
dof_type = JointDoFType.FIXED
elif q_count == 1 and qd_count == 1 and dof_dim == (1, 0):
dof_type = JointDoFType.PRISMATIC
elif q_count == 1 and qd_count == 1 and dof_dim == (0, 1):
dof_type = JointDoFType.REVOLUTE
elif q_count == 2 and qd_count == 2 and dof_dim == (0, 2):
dof_type = JointDoFType.UNIVERSAL
elif q_count == 2 and qd_count == 2 and dof_dim == (1, 1):
dof_type = JointDoFType.CYLINDRICAL
elif q_count == 3 and qd_count == 3 and dof_dim == (3, 0):
dof_type = JointDoFType.CARTESIAN
elif q_count == 3 and qd_count == 3 and dof_dim == (0, 3):
# TODO: dof_type = JointDoFType.GIMBAL
raise ValueError("Unsupported joint type: GIMBAL joints are not currently supported.")
elif q_count == 4 and qd_count == 3 and dof_dim == (0, 3):
dof_type = JointDoFType.SPHERICAL
elif q_count == 7 and qd_count == 6:
if np.any(limit_lower <= JOINT_QMIN) or np.any(limit_upper >= JOINT_QMAX):
dof_type = JointDoFType.FREE
else:
raise ValueError(
f"Unsupported joint type with 7 coordinates and 6 DoFs but unrecognized limits:\n"
f"\n limit_lower: {limit_lower}"
f"\n limit_upper: {limit_upper}"
)
else:
raise ValueError(
f"Unsupported joint type with:"
f"\n type: {type}"
f"\n dof_dim: {dof_dim}"
f"\n q_count: {q_count}"
f"\n qd_count: {qd_count}"
f"\n limit_lower: {limit_lower}"
f"\n limit_upper: {limit_upper}"
)
# Return the inferred DoF type
return dof_type
@staticmethod
def axes_matrix_from_joint_type(
dof_type: JointDoFType,
dof_dim: tuple[int, int],
dof_axes: np.ndarray,
) -> wp.mat33f | None:
"""
Returns the joint axes rotation matrix `R_axis_j` for the
specified joint DoF type, based on the provided DoF axes.
Args:
dof_type (JointDoFType):
The joint DoF type for which to compute the axes matrix.
dof_dim (tuple[int, int]):
A tuple containing the number of translational and rotational DoFs, respectively.
dof_axes (np.ndarray):
A 2D array of shape `(num_dofs, 3)` containing the local
axes of the joint's DoFs in the order they are defined.
Returns:
wp.mat33f | None:
The joint axes rotation matrix `R_axis_j` if applicable, or
`None` if the joint type does not require an axes matrix.
"""
# Initialize the joint axes rotation matrix to identity by default
R_axis_j = wp.quat_to_matrix(wp.quat_identity())
def _axis_rotmatn_from_vec3f(vec: np.ndarray) -> wp.mat33f:
n = np.linalg.norm(vec)
if n < 1e-12:
raise ValueError(f"Joint axis vector has near-zero length: {vec}")
ax = vec / n
dominant = np.argmax(np.abs(ax))
ref = np.zeros(3)
ref[(dominant + 2) % 3] = 1.0
ay = np.cross(ref, ax)
ay /= np.linalg.norm(ay)
az = np.cross(ax, ay)
return wp.matrix_from_cols(wp.vec3f(ax), wp.vec3f(ay), wp.vec3f(az))
# Ensure that dof_axes has the correct shape based on the number of DoFs
if dof_axes.shape != (dof_dim[0] + dof_dim[1], 3):
raise ValueError(f"Invalid shape of dof_axes: {dof_axes.shape}. Expected: {(dof_dim[0] + dof_dim[1], 3)}.")
# Retrieve the number of DoFs for the joint type
# num_dofs = dof_dim[0] + dof_dim[1]
# print(f"{dof_type} with {num_dofs} DoFs and axis:\n{dof_axes}")
# Determine the joint axes matrix based on the DoF type and axes
if dof_type == JointDoFType.FIXED:
pass # R_axis_j is already set to identity
elif dof_type in (JointDoFType.REVOLUTE, JointDoFType.PRISMATIC, JointDoFType.CYLINDRICAL):
R_axis_j = _axis_rotmatn_from_vec3f(dof_axes[0, :])
elif dof_type == JointDoFType.UNIVERSAL:
ax = dof_axes[0, :]
ay = dof_axes[1, :]
az = np.cross(ax, ay)
R_axis_j = wp.mat33f(*ax.tolist(), *ay.tolist(), *az.tolist())
elif dof_type in (JointDoFType.SPHERICAL, JointDoFType.CARTESIAN):
R_axis_j = wp.mat33f(*dof_axes.tolist())
elif dof_type in (JointDoFType.FIXED, JointDoFType.FREE):
ax_lin = dof_axes[0, :]
ay_lin = dof_axes[1, :]
az_lin = dof_axes[2, :]
ax_rot = dof_axes[3, :]
ay_rot = dof_axes[4, :]
az_rot = dof_axes[5, :]
if not np.allclose(ax_lin, ax_rot) or not np.allclose(ay_lin, ay_rot) or not np.allclose(az_lin, az_rot):
raise ValueError(
f"For FREE joints, the first 3 axes (linear) must match the last 3 axes (rotational), got:"
f"\nax_lin: {ax_lin}, ax_rot: {ax_rot}\nay_lin: {ay_lin}, "
f"ay_rot: {ay_rot}\naz_lin: {az_lin}, az_rot: {az_rot}"
)
R_axis_j = wp.mat33f(*ax_lin.tolist(), *ay_lin.tolist(), *az_lin.tolist())
else:
raise ValueError(f"Unsupported joint DOF type: {dof_type}")
# print(f"R_axis_j for {dof_type}:\n{R_axis_j}")
# Return the computed joint axes rotation matrix or None
# if the joint type does not require an axes matrix
return R_axis_j
###
# Containers
###
@dataclass
class JointDescriptor(Descriptor):
"""
A container to describe a single joint in the model builder.
"""
###
# Attributes
###
act_type: JointActuationType = JointActuationType.PASSIVE
"""Actuation type of the joint."""
dof_type: JointDoFType = JointDoFType.FREE
"""DoF type of the joint."""
bid_B: int = -1
"""
The Base body index of the joint (-1 for world, >=0 for bodies).\n
Defaults to `-1`, indicating that the joint has not been assigned a base body.
"""
bid_F: int = -1
"""
The Follower body index of the joint (must always be >=0 to index a body).\n
Defaults to `-1`, indicating that the joint has not been assigned a follower body.
"""
# TODO: Change this to a transformf
B_r_Bj: vec3f = field(default_factory=vec3f)
"""The relative position of the joint in the base body coordinates."""
# TODO: Change this to a transformf
F_r_Fj: vec3f = field(default_factory=vec3f)
"""The relative position of the joint in the follower body coordinates."""
# TODO: Remove this when body offsets become transforms
X_j: mat33f = field(default_factory=mat33f)
"""The constant axes matrix of the joint."""
q_j_min: ArrayLike | float | None = None
"""
Minimum DoF limits of the joint.
If `None`, then no limits are applied to the joint DoFs,
and the maximum limits default to `-inf` for lower limits.
If specified as a single float value, it will
be applied uniformly to all DoFs of the joint.
If specified as a type conforming to the `ArrayLike`
union, then the number of elements must equal number of
DoFs of the joint, i.e. `num_dofs = dof_type.num_dofs`.
For rotational DoFs, limits are expected in radians,
while for translational DoFs, limits are expected in
the same units as the world units.
**Warning**:
These limits are dimensioned according to the number of `num_dofs`,
even though joint coordinates are actually dimensioned according to
`num_coords`. This is because some joints (e.g. SPHERICAL) may use
redundant or non-minimal parameterizations at configuration-level.
In order to support configuration-level limits regardless of the
underlying parameterization, a mapping is performed in the solver
that translates the limits from DoF space to coordinate space.
"""
q_j_max: ArrayLike | float | None = None
"""
Maximum DoF limits of the joint.
If `None`, then no limits are applied to the joint DoFs,
and the maximum limits default to `-inf` for lower limits.
If specified as a single float value, it will
be applied uniformly to all DoFs of the joint.
If specified as a type conforming to the `ArrayLike`
union, then the number of elements must equal number of
DoFs of the joint, i.e. `num_dofs = dof_type.num_dofs`.
**Warning**:
These limits are dimensioned according to the number of `num_dofs`,
even though joint coordinates are actually dimensioned according to
`num_coords`. This is because some joints (e.g. SPHERICAL) may use
redundant or non-minimal parameterizations at configuration-level.
In order to support configuration-level limits regardless of the
underlying parameterization, a mapping is performed in the solver
that translates the limits from DoF space to coordinate space.
"""
dq_j_max: ArrayLike | float | None = None
"""
Maximum velocity limits of the joint.
If `None`, then no limits are applied
to the joint's generalized velocities.
If specified as a single float value, it will
be applied uniformly to all DoFs of the joint.
If specified as a type conforming to the `ArrayLike`
union, then the number of elements must equal number of
DoFs of the joint, i.e. `num_dofs = dof_type.num_dofs`.
"""
tau_j_max: ArrayLike | float | None = None
"""
Maximum effort (i.e. generalized force) limits of the joint.
If `None`, then no limits are applied
to the joint's generalized forces.
If specified as a single float value, it will
be applied uniformly to all DoFs of the joint.
If specified as a type conforming to the `ArrayLike`
union, then the number of elements must equal number of
DoFs of the joint, i.e. `num_dofs = dof_type.num_dofs`.
"""
a_j: ArrayLike | float | None = None
"""
Internal inertia of the joint (a.k.a. joint armature),
used for implicit integration of joint dynamics.
This represents effects like rotor inertia of rotary motors,
potentially transferred over a transmission, and compounding
the inertia of the gearbox. This is often referred to as so
called "reflected inertia" of an actuator as seen at the joint.
If specified as a type conforming to the `ArrayLike`
union, then the number of elements must equal number of
DoFs of the joint, i.e. `num_dofs = dof_type.num_dofs`.
Defaults to `[0.0] * num_dofs` if not specified, indicating
that the joint has no internal inertia and is thus massless.
"""
b_j: ArrayLike | float | None = None
"""
Internal damping of the joint used for implicit integration of joint dynamics.
This represents effects like viscous friction in rotary motors,
potentially transferred over a transmission, and compounding
the friction of the gearbox.
If specified as a type conforming to the `ArrayLike`
union, then the number of elements must equal number of
DoFs of the joint, i.e. `num_dofs = dof_type.num_dofs`.
Defaults to `[0.0] * num_dofs` if not specified, indicating
that the joint has no internal damping and is thus frictionless.
"""
k_p_j: ArrayLike | float | None = None
"""
Implicit PD-control proportional gain.
If specified as a type conforming to the `ArrayLike`
union, then the number of elements must equal number of
DoFs of the joint, i.e. `num_dofs = dof_type.num_dofs`.
Defaults to `[0.0] * num_dofs` if not specified, indicating
that the joint has no implicit proportional gain.
"""
k_d_j: ArrayLike | float | None = None
"""
Implicit PD-control derivative gain.
If specified as a type conforming to the `ArrayLike`
union, then the number of elements must equal number of
DoFs of the joint, i.e. `num_dofs = dof_type.num_dofs`.
Defaults to `[0.0] * num_dofs` if not specified, indicating
that the joint has no implicit derivative gain.
"""
###
# Metadata - to be set by the WorldDescriptor when added
###
wid: int = -1
"""
Index of the world to which the joint belongs.\n
Defaults to `-1`, indicating that the joint has not yet been added to a world.
"""
jid: int = -1
"""
Index of the joint w.r.t. its world.\n
Defaults to `-1`, indicating that the joint has not yet been added to a world.
"""
coords_offset: int = -1
"""
Index offset of this joint's coordinates among
all joint coordinates in the world it belongs to.
"""
dofs_offset: int = -1
"""
Index offset of this joint's DoFs among
all joint DoFs in the world it belongs to.
"""
passive_coords_offset: int = -1
"""
Index offset of this joint's passive coordinates among all
passive joint coordinates in the world it belongs to.
"""
passive_dofs_offset: int = -1
"""
Index offset of this joint's passive DoFs among all
passive joint DoFs in the world it belongs to.
"""
actuated_coords_offset: int = -1
"""
Index offset of this joint's actuated coordinates among
all actuated joint coordinates in the world it belongs to.
"""
actuated_dofs_offset: int = -1
"""
Index offset of this joint's actuated DoFs among
all actuated joint DoFs in the world it belongs to.
"""
cts_offset: int = -1
"""
Index offset of this joint's constraints among all
joint constraints in the world it belongs to.
"""
dynamic_cts_offset: int = -1
"""
Index offset of this joint's dynamic constraints among all
dynamic joint constraints in the world it belongs to.
"""
kinematic_cts_offset: int = -1
"""
Index offset of this joint's kinematic constraints among all
kinematic joint constraints in the world it belongs to.
"""
###
# Properties
###
@property
def num_coords(self) -> int:
"""
Returns the number of coordinates for this joint.
"""
return self.dof_type.num_coords
@property
def num_dofs(self) -> int:
"""
Returns the number of DoFs for this joint.
"""
return self.dof_type.num_dofs
@property
def num_passive_coords(self) -> int:
"""
Returns the number of passive coordinates for this joint.
"""
return self.dof_type.num_coords if self.is_passive else 0
@property
def num_passive_dofs(self) -> int:
"""
Returns the number of passive DoFs for this joint.
"""
return self.dof_type.num_dofs if self.is_passive else 0
@property
def num_actuated_coords(self) -> int:
"""
Returns the number of actuated coordinates for this joint.
"""
return self.dof_type.num_coords if self.is_actuated else 0
@property
def num_actuated_dofs(self) -> int:
"""
Returns the number of actuated DoFs for this joint.
"""
return self.dof_type.num_dofs if self.is_actuated else 0
@property
def num_cts(self) -> int:
"""
Returns the total number of constraints introduced by this joint.
"""
return self.num_dynamic_cts + self.num_kinematic_cts
@property
def num_dynamic_cts(self) -> int:
"""
Returns the number of dynamic constraints introduced by this joint.
"""
return self.dof_type.num_dofs if self.is_dynamic or self.is_implicit_pd else 0
@property
def num_kinematic_cts(self) -> int:
"""
Returns the number of kinematic constraints introduced by this joint.
"""
return self.dof_type.num_cts
@property
def is_binary(self) -> bool:
"""
Returns whether the joint is binary (i.e. connected to two bodies).
"""
return self.bid_B != -1 and self.bid_F != -1
@property
def is_unary(self) -> bool:
"""
Returns whether the joint is unary (i.e. connected to the world).
"""
return self.bid_B == -1 or self.bid_F == -1
@property
def is_passive(self) -> bool:
"""
Returns whether the joint is passive.
"""
return self.act_type == JointActuationType.PASSIVE
@property
def is_actuated(self) -> bool:
"""
Returns whether the joint is actuated.
"""
return self.act_type > JointActuationType.PASSIVE
@property
def is_dynamic(self) -> bool:
"""
Returns whether the joint's dynamics is simulated implicitly.
"""
return np.any(self.a_j) or np.any(self.b_j)
@property
def is_implicit_pd(self) -> bool:
"""
Returns whether the joint's dynamics is simulated using implicit PD control.
"""
return np.any(self.k_p_j) or np.any(self.k_d_j)
def has_base_body(self, bid: int) -> bool:
"""
Returns whether the joint has assigned the specified body as Base.
The body index `bid` must be given w.r.t the world.
"""
return self.bid_B == bid
def has_follower_body(self, bid: int) -> bool:
"""
Returns whether the joint has assigned the specified body as Follower.
The body index `bid` must be given w.r.t the world.
"""
return self.bid_F == bid
def is_connected_to_body(self, bid: int) -> bool:
"""
Returns whether the joint is connected to the specified body.
The body index `bid` must be given w.r.t the world.
"""
return self.has_base_body(bid) or self.has_follower_body(bid)
###
# Operations
###
def __post_init__(self):
"""Post-initialization processing to validate and set up joint limits."""
# Ensure base descriptor post-init is called first
# NOTE: This ensures that the UID is properly set before proceeding
super().__post_init__()
# Check if DoF type + actuation type are compatible
if self.dof_type == JointDoFType.FREE and self.is_binary:
raise ValueError(f"Invalid joint: FREE joints cannot be binary (name={self.name}, uid={self.uid}).")
if self.act_type == JointActuationType.FORCE and self.dof_type == JointDoFType.FIXED:
raise ValueError(f"Invalid joint: FIXED joints cannot be actuated (name={self.name}, uid={self.uid}).")
# Check if DoF type + dynamic/implicit PD settings are compatible
if self.is_implicit_pd and self.dof_type == JointDoFType.FREE:
raise ValueError(
f"Invalid joint: FREE joints cannot have implicit PD gains (name={self.name}, uid={self.uid})."
)
if self.is_dynamic and self.dof_type == JointDoFType.FIXED:
raise ValueError(f"Invalid joint: FIXED joints cannot be dynamic (name={self.name}, uid={self.uid}).")
if self.is_implicit_pd and self.dof_type == JointDoFType.FIXED:
raise ValueError(
f"Invalid joint: FIXED joints cannot have implicit PD gains (name={self.name}, uid={self.uid})."
)
# Set default values for joint limits if not provided
self.q_j_min = self._check_dofs_array(self.q_j_min, self.num_dofs, float(JOINT_QMIN))
self.q_j_max = self._check_dofs_array(self.q_j_max, self.num_dofs, float(JOINT_QMAX))
self.dq_j_max = self._check_dofs_array(self.dq_j_max, self.num_dofs, float(JOINT_DQMAX))
self.tau_j_max = self._check_dofs_array(self.tau_j_max, self.num_dofs, float(JOINT_TAUMAX))
# Set default values for internal inertia, damping, and implicit PD gains if not provided
self.a_j = self._check_dofs_array(self.a_j, self.num_dofs, 0.0)
self.b_j = self._check_dofs_array(self.b_j, self.num_dofs, 0.0)
self.k_p_j = self._check_dofs_array(self.k_p_j, self.num_dofs, 0.0)
self.k_d_j = self._check_dofs_array(self.k_d_j, self.num_dofs, 0.0)
# Validate that the specified parameters are valid
self._check_parameter_values()
# TODO: Add support for dynamic multi-dof joints in the future.
# Ensure that only revolute and prismatic joints are dynamically constrained
supported_implicit_joint_types = (JointDoFType.REVOLUTE, JointDoFType.PRISMATIC)
if (self.is_dynamic or self.is_implicit_pd) and self.dof_type not in supported_implicit_joint_types:
raise ValueError(
"Invalid joint: Kamino currently supports dynamic/implicit joints "
f"for those that are REVOLUTE or PRISMATIC (name={self.name}, uid={self.uid})."
)
# TODO: Add more checks based on JointDoFType because how do we
# handle iterating in DoF-like CTS space when num_coords != num_dofs?
# Ensure that PD gains are only specified for actuated joints
if self.is_passive and (np.any(self.k_p_j) or np.any(self.k_d_j)):
raise ValueError(
f"Joint `{self.name}` has non-zero PD gains but the joint is defined as passive:"
f"\n k_p_j: {self.k_p_j}"
f"\n k_d_j: {self.k_d_j}"
)
if self.act_type == JointActuationType.FORCE and (np.any(self.k_p_j) or np.any(self.k_d_j)):
raise ValueError(
f"Joint `{self.name}` is defined as FORCE actuated but has non-zero PD gains:"
f"\n k_p_j: {self.k_p_j}"
f"\n k_d_j: {self.k_d_j}"
)
if self.act_type == JointActuationType.POSITION and not np.any(self.k_p_j):
raise ValueError(
f"Joint `{self.name}` is defined as POSITION actuated but has zero-valued PD gains:"
f"\n k_p_j: {self.k_p_j}"
f"\n k_d_j: {self.k_d_j}"
)
if self.act_type == JointActuationType.VELOCITY and not np.any(self.k_d_j):
raise ValueError(
f"Joint `{self.name}` is defined as VELOCITY actuated but has zero-valued PD gains:"
f"\n k_p_j: {self.k_p_j}"
f"\n k_d_j: {self.k_d_j}"
)
if self.act_type == JointActuationType.POSITION_VELOCITY and not (np.any(self.k_p_j) or np.any(self.k_d_j)):
raise ValueError(
f"Joint `{self.name}` is defined as POSITION_VELOCITY actuated but has zero-valued PD gains:"
f"\n k_p_j: {self.k_p_j}"
f"\n k_d_j: {self.k_d_j}"
)
@override
def __repr__(self):
"""Returns a human-readable string representation of the JointDescriptor."""
return (
f"JointDescriptor(\n"
f"name: {self.name},\n"
f"uid: {self.uid},\n"
"----------------------------------------------\n"
f"act_type: {self.act_type},\n"
f"dof_type: {self.dof_type},\n"
"----------------------------------------------\n"
f"bid_B: {self.bid_B},\n"
f"bid_F: {self.bid_F},\n"
"----------------------------------------------\n"
f"B_r_Bj: {self.B_r_Bj},\n"
f"F_r_Fj: {self.F_r_Fj},\n"
f"X_j:\n{self.X_j},\n"
"----------------------------------------------\n"
f"q_j_min: {self.q_j_min},\n"
f"q_j_max: {self.q_j_max},\n"
f"dq_j_max: {self.dq_j_max},\n"
f"tau_j_max: {self.tau_j_max}\n"
"----------------------------------------------\n"
f"a_j: {self.a_j},\n"
f"b_j: {self.b_j},\n"
f"k_p_j: {self.k_p_j},\n"
f"k_d_j: {self.k_d_j},\n"
"----------------------------------------------\n"
f"wid: {self.wid},\n"
f"jid: {self.jid},\n"
"----------------------------------------------\n"
f"num_coords: {self.num_coords},\n"
f"num_dofs: {self.num_dofs},\n"
f"num_dynamic_cts: {self.num_dynamic_cts},\n"
f"num_kinematic_cts: {self.num_kinematic_cts},\n"
"----------------------------------------------\n"
f"coords_offset: {self.coords_offset},\n"
f"dofs_offset: {self.dofs_offset},\n"
f"dynamic_cts_offset: {self.dynamic_cts_offset},\n"
f"kinematic_cts_offset: {self.kinematic_cts_offset},\n"
"----------------------------------------------\n"
f"passive_coords_offset: {self.passive_coords_offset},\n"
f"passive_dofs_offset: {self.passive_dofs_offset},\n"
f"actuated_coords_offset: {self.actuated_coords_offset},\n"
f"actuated_dofs_offset: {self.actuated_dofs_offset},\n"
f")"
)
###
# Operations - Internal
###
@staticmethod
def _check_dofs_array(x: ArrayLike | float | None, size: int, default: float = float(FLOAT32_MAX)) -> list[float]:
"""
Processes a specified limit value to ensure it is a list of floats.
Notes:
- If the input is None, a list of default values is returned.
- If the input is a single float, it is converted to a list of the specified length.
- If the input is an empty list, a list of default values is returned.
- If the input is a non-empty list, it is validated to ensure it
contains only floats and matches the specified length.
Args:
x (List[float] | float | None): The DOF array to be processed.
size (int): The number of degrees of freedom to determine the length of the output list.
default (float): The default value to use if DOF array is None or an empty list.
Returns:
List[float]: The processed list of DOF values.
Raises:
ValueError: If the length of the DOF array does not match num_dofs.
TypeError: If the DOF array contains non-float types.
"""
if x is None:
return [float(default) for _ in range(size)]
if isinstance(x, (int, float, np.floating)):
if x == math.inf:
return [float(FLOAT32_MAX) for _ in range(size)]
elif x == -math.inf:
return [float(FLOAT32_MIN) for _ in range(size)]
else:
return [x] * size
if isinstance(x, ArrayLike):
if len(x) == 0:
return [float(default) for _ in range(size)]
if len(x) != size:
raise ValueError(f"Invalid DOF array length: {len(x)} != {size}")
if all(isinstance(x, (float, np.floating)) for x in x):
for i in range(len(x)):
if x[i] == math.inf:
x[i] = float(FLOAT32_MAX)
elif x[i] == -math.inf:
x[i] = float(FLOAT32_MIN)
return x
else:
raise TypeError(f"Unsupported DOF array type: {type(x)!r}; expected float, iterable of floats, or None")
def _check_parameter_values(self):
"""
Validates the joint parameters to ensure they are consistent and within expected ranges.
Raises:
ValueError: If any of the joint parameters are invalid, such as:
- q_j_min >= q_j_max for any DoF
- dq_j_max <= 0 for any DoF
- tau_j_max <= 0 for any DoF
- a_j < 0 for any DoF
- b_j < 0 for any DoF
- k_p_j < 0 for any DoF
- k_d_j < 0 for any DoF
"""
for i in range(self.num_dofs):
if self.q_j_min[i] >= self.q_j_max[i]:
raise ValueError(
f"Invalid joint limits: q_j_min[{i}] >= q_j_max[{i}] (name={self.name}, uid={self.uid})."
)
if self.dq_j_max[i] <= 0:
raise ValueError(
f"Invalid joint velocity limit: dq_j_max[{i}] <= 0 (name={self.name}, uid={self.uid})."
)
if self.tau_j_max[i] <= 0:
raise ValueError(f"Invalid joint effort limit: tau_j_max[{i}] <= 0 (name={self.name}, uid={self.uid}).")
if self.a_j[i] < 0:
raise ValueError(f"Invalid joint armature: a_j[{i}] < 0 (name={self.name}, uid={self.uid}).")
if self.b_j[i] < 0:
raise ValueError(f"Invalid joint damping: b_j[{i}] < 0 (name={self.name}, uid={self.uid}).")
if self.k_p_j[i] < 0:
raise ValueError(f"Invalid joint proportional gain: k_p_j[{i}] < 0 (name={self.name}, uid={self.uid}).")
if self.k_d_j[i] < 0:
raise ValueError(f"Invalid joint derivative gain: k_d_j[{i}] < 0 (name={self.name}, uid={self.uid}).")
@dataclass
class JointsModel:
"""
An SoA-based container to hold time-invariant model data of joints.
"""
###
# Meta-Data
###
num_joints: int = 0
"""Total number of joints in the model (host-side)."""
label: list[str] | None = None
"""
A list containing the label of each joint entity.\n
Length of ``num_joints`` and type :class:`str`.
"""
###
# Identifiers
###
wid: wp.array | None = None
"""
Index each the world in which each joint is defined.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
jid: wp.array | None = None
"""
Index of each joint w.r.t the world.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
###
# Parameterization
###
dof_type: wp.array | None = None
"""
Joint DoF type ID of each joint.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
act_type: wp.array | None = None
"""
Joint actuation type ID of each joint.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
bid_B: wp.array | None = None
"""
Base body index of each joint w.r.t the model.\n
Equals `-1` for world, `>=0` for bodies.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
bid_F: wp.array | None = None
"""
Follower body index of each joint w.r.t the model.\n
Equals `-1` for world, `>=0` for bodies.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
B_r_Bj: wp.array | None = None
"""
Relative position of the joint, expressed in and w.r.t the base body coordinate frame.\n
Shape of ``(num_joints, 3)`` and type :class:`vec3`.
"""
F_r_Fj: wp.array | None = None
"""
Relative position of the joint, expressed in and w.r.t the follower body coordinate frame.\n
Shape of ``(num_joints, 3)`` and type :class:`vec3`.
"""
X_j: wp.array | None = None
"""
Joint axes matrix (local coordinates) of each joint.\n
Indicates the relative orientation of the the joint
frame w.r.t the base body coordinate frame.\n
Shape of ``(num_joints, 3, 3)`` and type :class:`mat33`.
"""
###
# Limits
###
q_j_min: wp.array | None = None
"""
Minimum (a.k.a. lower) joint DoF limits of each joint (as flat array).\n
Limits are dimensioned according to the number of DoFs of each joint,
as opposed to the number of coordinates in order to handle cases such
where joints have more coordinates than DoFs (e.g. spherical joints).\n
Shape of ``(sum_of_num_joint_dofs,)`` and type :class:`float`,\n
where `sum_of_num_joint_dofs := sum(d_j)`, and ``d_j``
is the number of DoFs of joint ``j``.
"""
q_j_max: wp.array | None = None
"""
Maximum (a.k.a. upper) joint DoF limits of each joint (as flat array).\n
Limits are dimensioned according to the number of DoFs of each joint,
as opposed to the number of coordinates in order to handle cases such
where joints have more coordinates than DoFs (e.g. spherical joints).\n
Shape of ``(sum_of_num_joint_dofs,)`` and type :class:`float`,\n
where `sum_of_num_joint_dofs := sum(d_j)`, and ``d_j``
is the number of DoFs of joint ``j``.
"""
dq_j_max: wp.array | None = None
"""
Maximum joint velocity limits of each joint (as flat array).\n
Shape of ``(sum(d_j),)`` and type :class:`float`,\n
where ``d_j`` is the number of DoFs of joint ``j``.
"""
tau_j_max: wp.array | None = None
"""
Maximum joint torque limits of each joint (as flat array).\n
Shape of ``(sum(d_j),)`` and type :class:`float`,\n
where ``d_j`` is the number of DoFs of joint ``j``.
"""
###
# Dynamics
###
a_j: wp.array | None = None
"""
Internal inertia of each joint (as flat array), used for implicit integration of joint dynamics.\n
Shape of ``(sum(d_j),)`` and type :class:`float`,\n
where ``d_j`` is the number of DoFs of joint ``j``.
"""
b_j: wp.array | None = None
"""
Internal damping of each joint (as flat array) used for implicit integration of joint dynamics.\n
Shape of ``(sum(d_j),)`` and type :class:`float`,\n
where ``d_j`` is the number of DoFs of joint ``j``.
"""
k_p_j: wp.array | None = None
"""
Implicit PD-control proportional gain of each joint (as flat array).\n
Shape of ``(sum(d_j),)`` and type :class:`float`,\n
where ``d_j`` is the number of DoFs of joint ``j``.
"""
k_d_j: wp.array | None = None
"""
Implicit PD-control derivative gain of each joint (as flat array).\n
Shape of ``(sum(d_j),)`` and type :class:`float`,\n
where ``d_j`` is the number of DoFs of joint ``j``.
"""
###
# Initial State
###
q_j_0: wp.array | None = None
"""
The initial coordinates of each joint (as flat array),
indicating the "rest" or "neutral" position of each joint.
These are used for resetting joint positions when multi-turn
correction for revolute DoFs is enabled in the simulation.
Shape of ``(sum(c_j),)`` and type :class:`float`,\n
where ``c_j`` is the number of coordinates of joint ``j``.
"""
dq_j_0: wp.array | None = None
"""
The initial velocities of each joint (as flat array),
indicating the "rest" or "neutral" velocity of each joint.
These are used for resetting joint velocities when multi-turn
correction for revolute DoFs is enabled in the simulation.
Shape of ``(sum(c_j),)`` and type :class:`float`,\n
where ``c_j`` is the number of coordinates of joint ``j``.
"""
###
# Metadata
###
num_coords: wp.array | None = None
"""
Number of coordinates of each joint.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
num_dofs: wp.array | None = None
"""
Number of DoFs of each joint.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
# TODO: Consider making this a vec2i containing
# both dynamic and kinematic constraint counts
num_cts: wp.array | None = None
"""
Number of total constraints of each joint.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
num_dynamic_cts: wp.array | None = None
"""
Number of dynamic constraints of each joint.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
num_kinematic_cts: wp.array | None = None
"""
Number of kinematic constraints of each joint.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
coords_offset: wp.array | None = None
"""
Index offset of each joint's coordinates w.r.t the start
index of joint coordinates of the corresponding world.\n
Used to index into joint-specific blocks of:
- array of initial joint generalized coordinates :attr:`JointsModel.q_j_0`
- array of joint generalized coordinates :attr:`JointsData.q_j`
- array of previous joint generalized coordinates :attr:`JointsData.q_j_p`
Shape of ``(num_joints,)`` and type :class:`int`.
"""
dofs_offset: wp.array | None = None
"""
Index offset of each joint's DoFs w.r.t the start
index of joint DoFs of the corresponding world.
Used to index into joint-specific blocks of:
- array of initial joint generalized velocities :attr:`JointsModel.dq_j_0`
- array of joint generalized velocities :attr:`JointsData.dq_j`
- array of joint generalized forces :attr:`JointsData.tau_j`
Shape of ``(num_joints,)`` and type :class:`int`.
"""
passive_coords_offset: wp.array | None = None
"""
Index offset of each joint's passive coordinates w.r.t the start
index of passive joint coordinates of the corresponding world.\n
Used to index into passive-specific blocks of flattened passive joint coordinates arrays.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
passive_dofs_offset: wp.array | None = None
"""
Index offset of each joint's passive DoFs w.r.t the start
index of passive joint DoFs of the corresponding world.\n
Used to index into passive-specific blocks of flattened passive joint coordinates and DoFs arrays.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
actuated_coords_offset: wp.array | None = None
"""
Index offset of each joint's actuated coordinates w.r.t the start
index of actuated joint coordinates of the corresponding world.\n
Used to index into actuator-specific blocks of flattened actuator coordinates arrays.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
actuated_dofs_offset: wp.array | None = None
"""
Index offset of each joint's actuated DoFs w.r.t the start
index of actuated joint DoFs of the corresponding world.\n
Used to index into actuator-specific blocks of flattened actuator DoFs arrays.\n
Shape of ``(num_joints,)`` and type :class:`int`.
"""
cts_offset: wp.array | None = None
"""
Index offset of each joint's constraints w.r.t the start
index of constraints of the corresponding world.
Shape of ``(num_joints,)`` and type :class:`int`.
Used together with :attr:`ModelKaminoInfo.joint_cts_offset`
to index into the joint-specific blocks of:
- array of joint constraint Lagrange multipliers `lambda_j`
For a joint `j` of world `w`, its constraint multipliers can be accessed as:
```
# Retrieve dimensions and start indices
joint_num_cts = model.joints.num_cts[j]
world_cts_start_idx = model.info.joint_cts_offset[w]
joint_cts_start_idx = model.joints.cts_offset[j]
# Compute the start and end indices for the joint's constraints
start_idx = world_cts_start_idx + joint_cts_start_idx
end_idx = start_idx + joint_num_cts
# Access the joint's constraint multipliers
lambda_j = lambda_j[start_idx:end_idx]
```
"""
dynamic_cts_offset: wp.array | None = None
"""
Index offset of each joint's dynamic constraints w.r.t the start
index of dynamic joint constraints of the corresponding world.\n
Used together with :attr:`ModelKaminoInfo.joint_dynamic_cts_offset`
to index into the joint-specific blocks of:
- array of effective joint-space inertia :attr:`JointsData.m_j`
- array of joint-space damping :attr:`JointsData.b_j`
- array of joint-space P gains :attr:`JointsData.k_p_j`
- array of joint-space D gains :attr:`JointsData.k_d_j`
Shape of ``(num_joints,)`` and type :class:`int`.
"""
kinematic_cts_offset: wp.array | None = None
"""
Index offset of each joint's kinematic constraints w.r.t the start
index of kinematic joint constraints of the corresponding world.\n
Used together with :attr:`ModelKaminoInfo.joint_kinematic_cts_offset`
to index into the joint-specific blocks of:
- array of joint constraint residuals :attr:`JointsData.r_j`
- array of joint constraint residual time-derivatives :attr:`JointsData.dr_j`
Shape of ``(num_joints,)`` and type :class:`int`.
"""
@dataclass
class JointsData:
"""
An SoA-based container to hold time-varying data of a joint system.
"""
num_joints: int = 0
"""Total number of joints in the model (host-side)."""
###
# State
###
p_j: wp.array | None = None
"""
Array of joint frame pose transforms in world coordinates.\n
Shape of ``(num_joints,)`` and type :class:`transform`.
"""
q_j: wp.array | None = None
"""
Flat array of generalized coordinates of the joints.\n
Shape of ``(sum_of_num_joint_coords,)`` and type :class:`float`,\n
where `sum_of_num_joint_coords := sum(c_j)`, and ``c_j``
is the number of coordinates of joint ``j``.
"""
q_j_p: wp.array | None = None
"""
Flat array of previous generalized coordinates of the joints.\n
Shape of ``(sum_of_num_joint_coords,)`` and type :class:`float`,\n
where `sum_of_num_joint_coords := sum(c_j)`, and ``c_j``
is the number of coordinates of joint ``j``.
"""
dq_j: wp.array | None = None
"""
Flat array of generalized velocities of the joints.\n
Shape of ``(sum_of_num_joint_dofs,)`` and type :class:`float`,\n
where `sum_of_num_joint_dofs := sum(d_j)`, and ``d_j``
is the number of DoFs of joint ``j``.
"""
tau_j: wp.array | None = None
"""
Flat array of generalized forces of the joints.\n
Shape of ``(sum_of_num_joint_dofs,)`` and type :class:`float`,
where `sum_of_num_joint_dofs := sum(d_j)`, and ``d_j``
is the number of DoFs of joint ``j``.
"""
###
# Constraints
###
r_j: wp.array | None = None
"""
Flat array of joint kinematic constraint residuals.
To access the constraint residuals of a specific world `w` use:
- to get the start index: ``model.info.joint_kinematic_cts_offset[w]``
- to get the size: ``model.info.num_joint_kinematic_cts[w]``
Shape of ``(sum_of_num_kinematic_joint_cts,)`` and type :class:`float`,\n
where `sum_of_num_kinematic_joint_cts := sum(f_j)`, and ``f_j``
is the number of kinematic constraints of joint ``j``.
"""
dr_j: wp.array | None = None
"""
Flat array of joint kinematic constraint residual time-derivatives.
To access the constraint residuals of a specific world `w` use:
- to get the start index: ``model.info.joint_kinematic_cts_offset[w]``
- to get the size: ``model.info.num_joint_kinematic_cts[w]``
Shape of ``(sum_of_num_kinematic_joint_cts,)`` and type :class:`float`,\n
where `sum_of_num_kinematic_joint_cts := sum(f_j)`, and ``f_j``
is the number of kinematic constraints of joint ``j``.
"""
lambda_j: wp.array | None = None
"""
Flat array of joint constraint Lagrange multipliers.
To access the constraint multipliers of a specific world `w` use:
- to get the start index: ``model.info.joint_cts_offset[w]``
- to get the size: ``model.info.num_joint_cts[w]``
Then to access the individual dynamic or kinematic constraint blocks, use:
- dynamic constraints:
``model.info.joint_dynamic_cts_group_offset[w]`` and ``model.info.num_joint_dynamic_cts[w]``
- kinematic constraints:
``model.info.joint_kinematic_cts_group_offset[w]`` and ``model.info.num_joint_kinematic_cts[w]``
Shape of ``(sum_of_num_joint_cts,)`` and type :class:`float`,\n
where `sum_of_num_joint_cts := sum(e_j) + sum(f_j)`, and ``e_j`` and ``f_j``
are the number of dynamic and kinematic constraints of joint ``j``, respectively.
"""
###
# Dynamics
###
m_j: wp.array | None = None
"""
Internal effective inertia of each joint (as flat array),
used for implicit integration of joint dynamics.
``m_j := a_j + dt * (b_j + k_d_j) + dt^2 * k_p_j``,\n
where dt is the simulation time step.
Shape of ``(sum(e_j),)`` and type :class:`float`,\n
where ``e_j`` is the number of dynamic constraints of joint ``j``.
"""
inv_m_j: wp.array | None = None
"""
Internal effective inverse inertia of each joint (as flat
array), used for implicit integration of joint dynamics.
``inv_m_j := 1 / m_j``, computed element-wise,\n
where ``m_j := a_j + dt * (b_j + k_d_j) + dt^2 * k_p_j``,
and dt is the simulation time step.
Note that all ``inv_m_j>0``, otherwise the DoF would not be
part of the dynamic constraints.
Shape of ``(sum(e_j),)`` and type :class:`float`,
where ``e_j`` is the number of dynamic constraints of joint ``j``.
"""
dq_b_j: wp.array | None = None
"""
The velocity bias of the joint dynamic constraints (as flat array).
Each joint has local actuation and PD control dynamics:\n
```
m_j * dq_j^{+} = a_j * dq_j^{-} + dt * h_j
```
and is contributes to the dynamice of the system through the constraint equation:\n
```
dq_j^{+} = J_q_j * u^{+}
```
where ``dq_j^{-}`` and ``dq_j^{+}`` are the pre- and post-event joint-space
velocities, and ``u^{+}`` are the post-event generalized velocities of the
system computed implicitly as a result of solving the forward dynamics problem
with the joint dynamic constraints. `J_q_j` is the block of the joint-space
projection Jacobian matrix corresponding to the rows of DoFs of joint `j`.
This results in the following dynamic constraint equation for each joint `j`:
```
dq_j^{+} + m_j^{-1} * lambda_q_j = m_j^{-1} * (a_j * dq_j^{-} + dt * h_j)
dq_j^{+} + m_j^{-1} * lambda_q_j = dq_b_j
J_q_j * u^{+} + m_j^{-1} * lambda_q_j = dq_b_j
```
and thus the velocity bias term of the joint-space dynamics of each joint `j` is computed as:\n
```
tau_j := dt * ( tau_j_ff + k_p_j * (q_j_ref - q_j^{-} ) + k_d_j * dq_j_ref )
h_j := a_j * dq_j^{-} + dt * tau_j
dq_b_j := inv_m_j * h_j
```
where dt is the simulation time step.
Shape of ``(sum(e_j),)`` and type :class:`float`,
where ``e_j`` is the number of dynamic constraints of joint ``j``.
"""
###
# Reference State
###
q_j_ref: wp.array | None = None
"""
Array of reference generalized joint coordinates for implicit PD control.\n
Shape of ``(sum(c_j),)`` and type :class:`float`,
where ``c_j`` is the number of coordinates of joint ``j``.
"""
dq_j_ref: wp.array | None = None
"""
Array of reference generalized joint velocities for implicit PD control.\n
Shape of ``(sum(d_j),)`` and type :class:`float`,
where ``d_j`` is the number of DoFs of joint ``j``.
"""
tau_j_ref: wp.array | None = None
"""
Array of reference feed-forward generalized joint forces for implicit PD control.\n
Shape of ``(sum(d_j),)`` and type :class:`float`,
where ``d_j`` is the number of DoFs of joint ``j``.
"""
###
# Per-Body Wrenches
###
j_w_j: wp.array | None = None
"""
Total wrench applied by each joint, expressed
in and about the corresponding joint frame.\n
Its direction follows the convention that
joints act on the follower by the base body.\n
Shape of ``(num_joints,)`` and type :class:`vec6`.
"""
j_w_a_j: wp.array | None = None
"""
Actuation wrench applied by each joint, expressed
in and about the corresponding joint frame.\n
Its direction is defined by the convention that positive wrenches
in the joint frame are those inducing a positive change in the
twist of the follower body relative to the base body.\n
Shape of ``(num_joints,)`` and type :class:`vec6`.
"""
j_w_c_j: wp.array | None = None
"""
Constraint wrench applied by each joint, expressed
in and about the corresponding joint frame.\n
Its direction is defined by the convention that positive wrenches
in the joint frame are those inducing a positive change in the
twist of the follower body relative to the base body.\n
Shape of ``(num_joints,)`` and type :class:`vec6`.
"""
j_w_l_j: wp.array | None = None
"""
Joint-limit wrench applied by each joint, expressed
in and about the corresponding joint frame.\n
Its direction is defined by the convention that positive wrenches
in the joint frame are those inducing a positive change in the
twist of the follower body relative to the base body.\n
Shape of ``(num_joints,)`` and type :class:`vec6`.
"""
###
# Operations
###
def reset_state(self, q_j_0: wp.array | None = None):
"""
Resets all generalized joint coordinates to either zero or the provided
reference coordinates and all generalized joint velocities to zero.
"""
if q_j_0 is not None:
if q_j_0.size != self.q_j.size:
raise ValueError(f"Invalid size of q_j_0: {q_j_0.size}. Expected: {self.q_j.size}.")
wp.copy(self.q_j, q_j_0)
wp.copy(self.q_j_p, q_j_0)
else:
self.q_j.zero_()
self.q_j_p.zero_()
self.dq_j.zero_()
def reset_references(self, q_j_ref: wp.array | None = None, dq_j_ref: wp.array | None = None):
"""
Resets all reference coordinates and velocities to either zero or the provided
reference values.
"""
if q_j_ref is not None:
if q_j_ref.size != self.q_j_ref.size:
raise ValueError(f"Invalid size of q_j_ref: {q_j_ref.size}. Expected: {self.q_j_ref.size}.")
wp.copy(self.q_j_ref, q_j_ref)
else:
self.q_j_ref.zero_()
if dq_j_ref is not None:
if dq_j_ref.size != self.dq_j_ref.size:
raise ValueError(f"Invalid size of dq_j_ref: {dq_j_ref.size}. Expected: {self.dq_j_ref.size}.")
wp.copy(self.dq_j_ref, dq_j_ref)
else:
self.dq_j_ref.zero_()
def clear_residuals(self):
"""
Resets all joint state variables to zero.
"""
self.r_j.zero_()
self.dr_j.zero_()
def clear_constraint_reactions(self):
"""
Resets all joint constraint reactions to zero.
"""
self.lambda_j.zero_()
def clear_actuation_forces(self):
"""
Resets all joint actuation forces to zero.
"""
self.tau_j.zero_()
def clear_wrenches(self):
"""
Resets all joint wrenches to zero.
"""
if self.j_w_j is not None:
self.j_w_j.zero_()
self.j_w_c_j.zero_()
self.j_w_a_j.zero_()
self.j_w_l_j.zero_()
def clear_all(self):
"""
Resets all joint state variables, constraint reactions,
actuation forces, and wrenches to zero.
"""
self.clear_residuals()
self.clear_constraint_reactions()
self.clear_actuation_forces()
self.clear_wrenches()
================================================
FILE: newton/_src/solvers/kamino/_src/core/materials.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Mechanisms for defining and managing materials and their properties.
This module provides a set of data types and operations that realize configurable
material properties that can be queried at simulation runtime. It includes:
- :class:`MaterialDescriptor`: A container to represent a managed material.
- :class:`MaterialPairProperties`: A container to represent the properties of a pair
of materials, including friction and restitution coefficients.
- :class:`MaterialManager`: A class to manage materials used in simulations, including
their properties and pairwise interactions.
- :class:`MaterialsModel`: A container to hold and manage per-material properties.
- :class:`MaterialPairsModel`: A container to hold and manage per-material-pair properties.
"""
from dataclasses import dataclass
from enum import IntEnum
import numpy as np
import warp as wp
from ..utils import logger as msg
from .math import tril_index
from .types import Descriptor, float32, int32, override
###
# Module interface
###
__all__ = [
"DEFAULT_DENSITY",
"DEFAULT_FRICTION",
"DEFAULT_RESTITUTION",
"MaterialDescriptor",
"MaterialManager",
"MaterialPairProperties",
"MaterialPairsModel",
"MaterialsModel",
]
###
# Constants
###
DEFAULT_DENSITY = 1000.0
"""
The global default density for materials, in kg/m^3.\n
Equals ``1000.0`` kg/m^3.
"""
DEFAULT_RESTITUTION = 0.0
"""
The global default restitution coefficient for material pairs.
Equals ``0.0``.
"""
DEFAULT_FRICTION = 0.7
"""
The global default friction coefficient for material pairs.
Equals ``0.7``.
"""
###
# Types
###
class MaterialMuxMode(IntEnum):
"""
An enumeration defining the heuristic modes for deriving
pairwise material properties from individual materials.
This is used when no specific material-pair properties
are defined and the properties must be derived.
"""
AVERAGE = 0
"""Pairwise property is the average of the two material properties."""
MAX = 1
"""Pairwise property is the maximum of the two material properties."""
MIN = 2
"""Pairwise property is the minimum of the two material properties."""
@dataclass
class MaterialDescriptor(Descriptor):
"""
A container to represent a managed material.
This descriptor holds both intrinsic and extrinsic properties of a material. While the former
are truly dependent on the material itself (e.g., density), the latter are actually dependent
on the pairwise interactions of the material with others (e.g., friction, restitution). These
extrinsic properties are stored here to support model specifications such as USD which
currently do not support material-pair definitions.
Attributes:
name (`str`):
The name of the material.
uid (`str`):
The unique identifier (UUID) of the material.
density (`float`):
The density of the material, in kg/m^3.\n
Defaults to the global default of ``1000.0`` kg/m^3.
restitution (`float`):
The coefficient of restitution, according to the Newtonian impact model.\n
Defaults to the global default of ``0.0``.
static_friction (`float`):
The coefficient of static friction, according to the Coulomb friction model.\n
Defaults to the global default of ``0.7``.
dynamic_friction (`float`):
The coefficient of dynamic friction, according to the Coulomb friction model.\n
Defaults to the global default of ``0.7``.
wid (`int`):
Index of the world to which the material belongs.\n
Defaults to `-1`, indicating that the material has not yet been added to a world.
mid (`int`):
Index of the material w.r.t. the world.\n
Defaults to `-1`, indicating that the material has not yet been added to a world.
"""
###
# Attributes
###
density: float = DEFAULT_DENSITY
"""
The density of the material, in kg/m^3.\n
Defaults to the global default of ``1000.0`` kg/m^3.
"""
restitution: float = DEFAULT_RESTITUTION
"""
The coefficient of restitution, according to the Newtonian impact model.\n
Defaults to the global default of ``0.0``.
"""
static_friction: float = DEFAULT_FRICTION
"""
The coefficient of static friction, according to the isotropic Coulomb friction model.\n
Defaults to the global default of ``0.7``.
"""
dynamic_friction: float = DEFAULT_FRICTION
"""
The coefficient of dynamic friction, according to the isotropicCoulomb friction model.\n
Defaults to the global default of ``0.7``.
"""
###
# Metadata - to be set by the WorldDescriptor when added
###
wid: int = -1
"""
Index of the world to which the material belongs.\n
Defaults to `-1`, indicating that the material has not yet been added to a world.
"""
mid: int = -1
"""
Index of the material w.r.t. the world.\n
Defaults to `-1`, indicating that the material has not yet been added to a world.
"""
@override
def __repr__(self) -> str:
"""Returns a human-readable string representation of the MaterialDescriptor."""
return (
f"MaterialDescriptor(\n"
f"name: {self.name},\n"
f"uid: {self.uid},\n"
f"density: {self.density},\n"
f"restitution: {self.restitution},\n"
f"static_friction: {self.static_friction},\n"
f"dynamic_friction: {self.dynamic_friction}\n"
f"wid: {self.wid},\n"
f"mid: {self.mid},\n"
f")"
)
@dataclass
class MaterialPairProperties:
"""
A container to represent the properties of a pair of materials, including friction and restitution coefficients.
Attributes:
restitution (`float`):
The coefficient of restitution, according to the Newtonian impact model.\n
Defaults to the global default of ``0.0``.
static_friction (`float`):
The coefficient of static surface friction, according to the Coulomb friction model.\n
Defaults to the global default of ``0.7``.
dynamic_friction (`float`):
The coefficient of dynamic surface friction, according to the Coulomb friction model.\n
Defaults to the global default of ``0.7``.
"""
restitution: float = DEFAULT_RESTITUTION
"""
The coefficient of restitution, according to the Newtonian impact model.\n
Defaults to the global default of ``0.0``.
"""
static_friction: float = DEFAULT_FRICTION
"""
The coefficient of static surface friction, according to the Coulomb friction model.\n
Defaults to the global default of ``0.7``.
"""
dynamic_friction: float = DEFAULT_FRICTION
"""
The coefficient of dynamic surface friction, according to the Coulomb friction model.\n
Defaults to the global default of ``0.7``.
"""
###
# Containers
###
@dataclass
class MaterialsModel:
"""
A container to hold and manage per-material properties.
Each material property is stored as an array ordered according
to the material index (`mid`) defined by the MaterialManager.
Attributes:
num_pairs (int):
Total number of material pairs in the model.
restitution (wp.array):
Array of restitution coefficients for each registered material.\n
Shape of ``(num_materials, num_materials)`` and type :class:`float`.
static_friction (wp.array):
Array of static friction coefficients for each registered material.\n
Shape of ``(num_materials, num_materials)`` and type :class:`float`.
dynamic_friction (wp.array):
Array of dynamic friction coefficients for each registered material.\n
Shape of ``(num_materials, num_materials)`` and type :class:`float`.
"""
num_materials: int = 0
"""Total number of materials represented in the model."""
density: wp.array | None = None
"""
Array of material density values of each registered material.\n
Shape of ``(num_materials,)`` and type :class:`float`.
"""
restitution: wp.array | None = None
"""
Array of restitution coefficients for each registered material.\n
Shape of ``(num_materials,)`` and type :class:`float`.
"""
# TODO: Switch to vec3f for anisotropic+torsional friction?
static_friction: wp.array | None = None
"""
Array of static friction coefficients for each registered material.\n
Shape of ``(num_materials,)`` and type :class:`float`.
"""
# TODO: Switch to vec3f for anisotropic+torsional friction?
dynamic_friction: wp.array | None = None
"""
Array of dynamic friction coefficients for each registered material.\n
Shape of ``(num_materials,)`` and type :class:`float`.
"""
@dataclass
class MaterialPairsModel:
"""
A container to hold and manage per-material-pair properties.
Each material-pair property is stored as a flat array containing the elements of
the lower-triangular part of the corresponding symmetric matrix, where the entry
at row `i` and column `j` corresponds to the material pair `(i, j)`. The indices
`i,j` correspond to the material indices (`mid`) defined by the MaterialManager.
Attributes:
num_material_pairs (int):
Total number of material pairs represented in the model.
restitution (wp.array):
Lower-triangular matrix of material-pair restitution coefficients.\n
Shape of ``(num_material_pairs,)`` and type :class:`float`.
static_friction (wp.array):
Lower-triangular matrix of material-pair static friction coefficients.\n
Shape of ``(num_material_pairs,)`` and type :class:`float`.
dynamic_friction (wp.array):
Lower-triangular matrix of material-pair dynamic friction coefficients.\n
Shape of ``(num_material_pairs,)`` and type :class:`float`.
"""
num_material_pairs: int = 0
"""Total number of material pairs represented in the model."""
restitution: wp.array | None = None
"""
Lower-triangular matrix of material-pair restitution coefficients.\n
Shape of ``(num_material_pairs,)`` and type :class:`float`.
"""
# TODO: Switch to vec3f for anisotropic+torsional friction?
static_friction: wp.array | None = None
"""
Lower-triangular matrix of material-pair static friction coefficients.\n
Shape of ``(num_material_pairs,)`` and type :class:`float`.
"""
# TODO: Switch to vec3f for anisotropic+torsional friction?
dynamic_friction: wp.array | None = None
"""
Lower-triangular matrix of material-pair dynamic friction coefficients.\n
Shape of ``(num_material_pairs,)`` and type :class:`float`.
"""
###
# Functions
###
@wp.func
def material_average(
value1: float32,
value2: float32,
) -> float32:
"""
Computes the average of two material property values.
Args:
value1 (float32): The first material property value.
value2 (float32): The second material property value.
Returns:
float32: The average of the two material property values.
"""
return 0.5 * (value1 + value2)
@wp.func
def material_max(
value1: float32,
value2: float32,
) -> float32:
"""
Computes the maximum of two material property values.
Args:
value1 (float32): The first material property value.
value2 (float32): The second material property value.
Returns:
float32: The maximum of the two material property values.
"""
return wp.max(value1, value2)
@wp.func
def material_min(
value1: float32,
value2: float32,
) -> float32:
"""
Computes the maximinimum of two material property values.
Args:
value1 (float32): The first material property value.
value2 (float32): The second material property value.
Returns:
float32: The minimum of the two material property values.
"""
return wp.min(value1, value2)
def make_get_material_pair_properties(muxmode: MaterialMuxMode = MaterialMuxMode.MAX):
"""
Generates a Warp function to retrieve material pair
properties based on the specified muxing mode.
Args:
muxmode (MaterialMuxMode): The muxing mode to use for material pair properties.
Returns:
function: A Warp function that retrieves material pair properties.
"""
# Select the appropriate muxing function based on the muxing mode
match muxmode:
case MaterialMuxMode.AVERAGE:
mix_func = material_average
case MaterialMuxMode.MAX:
mix_func = material_max
case MaterialMuxMode.MIN:
mix_func = material_min
case _:
raise ValueError(f"Unsupported material muxing mode: {muxmode}")
# Define the Warp function to retrieve material pair properties
@wp.func
def _get_material_pair_properties(
mid1: int32,
mid2: int32,
material_restitution: wp.array(dtype=float32),
material_static_friction: wp.array(dtype=float32),
material_dynamic_friction: wp.array(dtype=float32),
material_pair_restitution: wp.array(dtype=float32),
material_pair_static_friction: wp.array(dtype=float32),
material_pair_dynamic_friction: wp.array(dtype=float32),
) -> tuple[float32, float32, float32]:
"""
Retrieves the properties of a material pair given their material indices.
If material-pair properties are not defined (i.e., negative values) for the given
material indices `mid1, mid2`, the properties are computed from the individual
materials using the configured muxing method.
Args:
mid1 (int32): The index of the first material.
mid2 (int32): The index of the second material.
material_restitution (wp.array): The per-material restitution coefficients.
material_static_friction (wp.array): The per-material static friction coefficients.
material_dynamic_friction (wp.array): The per-material dynamic friction coefficients.
material_pair_restitution (wp.array): The per-material-pair restitution coefficients.
material_pair_static_friction (wp.array): The per-material-pair static friction coefficients.
material_pair_dynamic_friction (wp.array): The per-material-pair dynamic friction coefficients.
Returns:
tuple: A tuple containing the restitution, static friction,
and dynamic friction coefficients for the material pair.
"""
# Compute the index in the flattened lower-triangular matrix
mid_tril_idx = tril_index(mid1, mid2)
# Retrieve the material pair properties
restitution = material_pair_restitution[mid_tril_idx]
static_friction = material_pair_static_friction[mid_tril_idx]
dynamic_friction = material_pair_dynamic_friction[mid_tril_idx]
# If any property is negative, compute the material pair properties using the set muxing method
if restitution < 0.0:
restitution = mix_func(material_restitution[mid1], material_restitution[mid2])
if static_friction < 0.0:
static_friction = mix_func(material_static_friction[mid1], material_static_friction[mid2])
if dynamic_friction < 0.0:
dynamic_friction = mix_func(material_dynamic_friction[mid1], material_dynamic_friction[mid2])
# Return the material pair properties
return restitution, static_friction, dynamic_friction
# Return the generated Warp function
return _get_material_pair_properties
###
# Interfaces
###
class MaterialManager:
"""
A class to manage materials used in simulations, including their properties and pairwise interactions.
Attributes:
num_materials (int): The number of materials managed by this MaterialManager.
materials (list[MaterialDescriptor]): A list of materials managed by this MaterialManager.
pairs (list[list[MaterialPairProperties]]): A 2D list representing the properties of material pairs.
default (MaterialDescriptor): The default material managed by this MaterialManager.
"""
def __init__(
self,
default_material: MaterialDescriptor | None = None,
default_restitution: float = DEFAULT_RESTITUTION,
default_static_friction: float = DEFAULT_FRICTION,
default_dynamic_friction: float = DEFAULT_FRICTION,
):
"""
Initializes the MaterialManager with an optional default material and its properties.
Args:
default_material (MaterialDescriptor, optional): The default material to register.\n
If None, a default material with the name 'default' will be created.
default_static_friction (float): The default friction coefficient for material pairs.\n
Defaults to `DEFAULT_FRICTION`.
default_dynamic_friction (float): The default restitution coefficient for material pairs.\n
Defaults to `DEFAULT_RESTITUTION`.
"""
# Declare the materials and material-pairs lists
self._materials: list[MaterialDescriptor] = []
self._pair_properties: list[list[MaterialPairProperties]] = []
# Construct the default material if not provided
if default_material is None:
default_material = MaterialDescriptor("default")
# Initialize a list of managed materials with the default material
self.register(default_material)
# Configure the default material pair properties
self.register_pair(
first=default_material,
second=default_material,
material_pair=MaterialPairProperties(
restitution=default_restitution,
static_friction=default_static_friction,
dynamic_friction=default_dynamic_friction,
),
)
@property
def num_materials(self) -> int:
"""
Returns the number of materials managed by this MaterialManager.
"""
return len(self._materials)
@property
def num_material_pairs(self) -> int:
"""
Returns the number of material pairs managed by this MaterialManager.
"""
N = len(self._materials)
return N * (N + 1) // 2
@property
def materials(self) -> list[MaterialDescriptor]:
"""
Returns the list of materials managed by this MaterialManager.
"""
return self._materials
@property
def pairs(self) -> list[list[MaterialPairProperties]]:
"""
Returns the list of material-pair properties managed by this MaterialManager.
"""
return self._pair_properties
@property
def default(self) -> MaterialDescriptor:
"""
Returns the default material managed by this MaterialManager.
"""
return self._materials[0]
@default.setter
def default(self, material: MaterialDescriptor):
"""
Sets the default material to the provided material descriptor.
Args:
material (`MaterialDescriptor`): The material to set as the default.
Raises:
TypeError: If the provided material is not an instance of MaterialDescriptor.
"""
if not isinstance(material, MaterialDescriptor):
raise TypeError("`material` must be an instance of MaterialDescriptor.")
self._materials[0] = material
@property
def default_pair(self) -> MaterialPairProperties:
"""
Returns the properties of the default material pair managed by this MaterialManager.
"""
return self._pair_properties[0][0]
def has_material(self, name: str) -> bool:
"""
Checks if a material with the given name exists in the manager.
Args:
name (str): The name of the material to check.
Returns:
bool: True if the material exists, False otherwise.
"""
for m in self._materials:
if m.name == name:
return True
return False
def register(self, material: MaterialDescriptor) -> int:
"""
Registers a new material with the manager.
Args:
material (MaterialDescriptor): The material descriptor to register.
Returns:
int: The index of the newly registered material.
Raises:
ValueError: If a material with the same name or UID already exists.
"""
# Get current bid from the number of bodies
material.mid = self.num_materials
# Check if the material already exists
if material.name in [m.name for m in self.materials]:
raise ValueError(f"Material name '{material.name}' already exists.")
if material.uid in [m.uid for m in self.materials]:
raise ValueError(f"Material UID '{material.uid}' already exists.")
# Add the new material to the list of materials
self.materials.append(material)
msg.debug("Registered new material:\n%s", material)
# Add placeholder entries in the material pair properties list
# NOTE: These are initialized to None and are to be set when the material pair is registered
self._pair_properties.append([None] * (material.mid + 1))
for i in range(material.mid):
self._pair_properties[i].append(None)
# Return the index of the new material
return material.mid
def register_pair(
self, first: MaterialDescriptor, second: MaterialDescriptor, material_pair: MaterialPairProperties
):
"""
Registers a new material pair with the manager.
Args:
first (MaterialDescriptor): The first material in the pair.
second (MaterialDescriptor): The second material in the pair.
material_pair (MaterialPairProperties): The properties of the material pair.
Raises:
ValueError: If either material is not already registered.
"""
# Register the first material if it is not already registered
if first.name not in [m.name for m in self.materials]:
self.register(first)
# Register the second material if it is not already registered
if second.name not in [m.name for m in self.materials]:
self.register(second)
# Configure the material pair properties
self.configure_pair(first=first.name, second=second.name, material_pair=material_pair)
msg.debug("Registered new material pair: %s - %s", first.name, second.name)
def configure_pair(self, first: int | str, second: int | str, material_pair: MaterialPairProperties):
"""
Configures the properties of an existing material pair.
Args:
first (int | str): The index or name of the first material in the pair.
second (int | str): The index or name of the second material in the pair.
material_pair (MaterialPairProperties): The properties to set for the material pair.
Raises:
ValueError: If either material is not found.
"""
# Get indices of the materials
mid1 = self.index(first)
mid2 = self.index(second)
# Set the material pair properties
self._pair_properties[mid1][mid2] = self._pair_properties[mid2][mid1] = material_pair
msg.debug("Configured material pair: %s - %s", self.materials[mid1].name, self.materials[mid2].name)
def merge(self, other: "MaterialManager"):
"""
Merges another MaterialManager into this one, combining their materials and material-pair properties.
Args:
other (MaterialManager): The other MaterialManager to merge.
Raises:
ValueError: If there are conflicting material names or UIDs.
"""
# Iterate over the materials in the other manager
for mat in other.materials:
if not self.has_material(mat.name):
self.register(mat)
# Iterate over the material pairs in the other manager
for i, mat1 in enumerate(other.materials):
for j, mat2 in enumerate(other.materials):
# Get the material pair properties from the other manager
pair_props = other.pairs[i][j]
# Configure the material pair properties in this manager if they exist
if pair_props is not None:
self.configure_pair(first=mat1.name, second=mat2.name, material_pair=pair_props)
def __getitem__(self, key) -> MaterialDescriptor:
"""
Retrieves a material descriptor by its index or name.
Args:
key (str | int): The name or index of the material.
Returns:
MaterialDescriptor: The material descriptor.
Raises:
IndexError: If the index is out of range.
ValueError: If the material is not found.
"""
# Check if the key is an integer
if isinstance(key, int):
# Check if the key is within the range of materials
if key < 0 or key >= len(self.materials):
raise IndexError(f"Material index '{key}' out of range.")
# Return the material descriptor
return self.materials[key]
# Check if the key is a string
elif isinstance(key, str):
# Check if the key is a valid material name
for m in self.materials:
if m.name == key:
return m
# If not found, raise an error
raise ValueError(f"Material with name '{key}' not found.")
def index(self, key: str | int) -> int:
"""
Retrieves the index of a material by its name or index.
Args:
key (str | int): The name or index of the material.
Returns:
int: The index of the material.
Raises:
ValueError: If the material is not found.
TypeError: If the key is not a string or integer.
"""
# Check if the name exists in the materials list
if isinstance(key, str):
for i in range(self.num_materials):
if key == self.materials[i].name:
return i
elif isinstance(key, int):
# If the name is an integer, return it directly if it is a valid index
if 0 <= key < self.num_materials:
return key
else:
raise TypeError("Name argument must be a string or integer.")
# If not found, raise an error
raise ValueError(f"Material with key '{key}' not found.")
###
# Material Properties Data
###
def restitution_vector(self) -> np.ndarray:
"""
Generates a vector of restitution coefficients over all materials.
Returns:
np.ndarray: A 1D numpy array containing per-material restitution coefficients.
"""
# Get the number of materials
num_materials = len(self._materials)
# Initialize the restitution matrix
restitution = np.full((num_materials,), -1, dtype=np.float32)
# Fill the matrix with the restitution coefficients
for i in range(num_materials):
restitution[i] = self._materials[i].restitution
# Return the restitution matrix as a numpy array
return restitution
def restitution_matrix(self) -> np.ndarray:
"""
Generates a matrix of restitution coefficients for all material pairs.
Returns:
np.ndarray: A 2D numpy array containing restitution coefficients.
"""
# Get the number of materials
num_materials = len(self._materials)
num_material_pairs = num_materials * (num_materials + 1) // 2
# Initialize the restitution matrix
restitution = np.full((num_material_pairs,), -1, dtype=np.float32)
# Fill the matrix with the restitution coefficients
for i in range(num_materials):
for j in range(0, i + 1):
# Check if the material pair properties exist
if self._pair_properties[i][j] is not None:
ij = i * (i + 1) // 2 + j
restitution[ij] = self._pair_properties[i][j].restitution
else:
msg.debug(
f"Material-pair properties not set for materials:"
f"({self.materials[i].name}, {self.materials[j].name})"
)
# Return the restitution matrix as a numpy array
return restitution
def static_friction_vector(self) -> np.ndarray:
"""
Generates a vector of static friction coefficients over all materials.
Returns:
np.ndarray: A 1D numpy array containing per-material static friction coefficients.
"""
# Get the number of materials
num_materials = len(self._materials)
# Initialize the restitution matrix
static_friction = np.full((num_materials,), -1, dtype=np.float32)
# Fill the matrix with the restitution coefficients
for i in range(num_materials):
static_friction[i] = self._materials[i].static_friction
# Return the restitution matrix as a numpy array
return static_friction
def static_friction_matrix(self) -> np.ndarray:
"""
Generates a matrix of friction coefficients for all material pairs.
Returns:
np.ndarray: A 2D numpy array containing static friction coefficients.
"""
# Get the number of materials
num_materials = len(self._materials)
num_material_pairs = num_materials * (num_materials + 1) // 2
# Initialize the friction matrix
static_friction = np.full((num_material_pairs,), -1, dtype=np.float32)
# Fill the matrix with the friction coefficients
for i in range(num_materials):
for j in range(0, i + 1):
# Check if the material pair properties exist
if self._pair_properties[i][j] is not None:
ij = i * (i + 1) // 2 + j
static_friction[ij] = self._pair_properties[i][j].static_friction
else:
msg.debug(
f"Material-pair properties not set for materials:"
f"({self.materials[i].name}, {self.materials[j].name})"
)
# Return the friction matrix as a numpy array
return static_friction
def dynamic_friction_vector(self) -> np.ndarray:
"""
Generates a vector of dynamic friction coefficients over all materials.
Returns:
np.ndarray: A 1D numpy array containing per-material dynamic friction coefficients.
"""
# Get the number of materials
num_materials = len(self._materials)
# Initialize the restitution matrix
dynamic_friction = np.full((num_materials,), -1, dtype=np.float32)
# Fill the matrix with the restitution coefficients
for i in range(num_materials):
dynamic_friction[i] = self._materials[i].dynamic_friction
# Return the restitution matrix as a numpy array
return dynamic_friction
def dynamic_friction_matrix(self) -> np.ndarray:
"""
Generates a matrix of friction coefficients for all material pairs.
Returns:
np.ndarray: A 2D numpy array containing dynamic friction coefficients.
"""
# Get the number of materials
num_materials = len(self._materials)
num_material_pairs = num_materials * (num_materials + 1) // 2
# Initialize the friction matrix
dynamic_friction = np.full((num_material_pairs,), -1, dtype=np.float32)
# Fill the matrix with the friction coefficients
for i in range(num_materials):
for j in range(0, i + 1):
# Check if the material pair properties exist
if self._pair_properties[i][j] is not None:
ij = i * (i + 1) // 2 + j
dynamic_friction[ij] = self._pair_properties[i][j].dynamic_friction
else:
msg.debug(
f"Material-pair properties not set for materials:"
f"({self.materials[i].name}, {self.materials[j].name})"
)
# Return the friction matrix as a numpy array
return dynamic_friction
================================================
FILE: newton/_src/solvers/kamino/_src/core/math.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Math Operations
"""
from __future__ import annotations
import numpy as np
import warp as wp
from warp._src.types import Any, Float
from .types import (
float32,
mat22f,
mat33f,
mat34f,
mat44f,
mat63f,
mat66f,
quatf,
transformf,
vec3f,
vec4f,
vec6f,
)
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
FLOAT32_MIN = wp.constant(float32(np.finfo(np.float32).min))
"""The lowest 32-bit floating-point value."""
FLOAT32_MAX = wp.constant(float32(np.finfo(np.float32).max))
"""The highest 32-bit floating-point value."""
FLOAT32_EPS = wp.constant(float32(np.finfo(np.float32).eps))
"""Machine epsilon for 32-bit float: the smallest value such that 1.0 + eps != 1.0."""
UNIT_X = wp.constant(vec3f(1.0, 0.0, 0.0))
""" 3D unit vector for the X axis """
UNIT_Y = wp.constant(vec3f(0.0, 1.0, 0.0))
""" 3D unit vector for the Y axis """
UNIT_Z = wp.constant(vec3f(0.0, 0.0, 1.0))
""" 3D unit vector for the Z axis """
PI = wp.constant(3.141592653589793)
"""Convenience constant for PI"""
TWO_PI = wp.constant(6.283185307179586)
"""Convenience constant for 2 * PI"""
HALF_PI = wp.constant(1.5707963267948966)
"""Convenience constant for PI / 2"""
COS_PI_6 = wp.constant(0.8660254037844387)
"""Convenience constant for cos(PI / 6)"""
I_2 = wp.constant(mat22f(1, 0, 0, 1))
""" The 2x2 identity matrix."""
I_3 = wp.constant(mat33f(1, 0, 0, 0, 1, 0, 0, 0, 1))
""" The 3x3 identity matrix."""
I_4 = wp.constant(mat44f(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1))
""" The 4x4 identity matrix."""
I_6 = wp.constant(
mat66f(1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1)
)
""" The 6x6 identity matrix."""
###
# General-purpose functions
###
@wp.func
def squared_norm(x: Any) -> Float:
return wp.dot(x, x)
###
# Rotation matrices
###
@wp.func
def R_x(theta: float32) -> mat33f:
"""
Computes the rotation matrix around the X axis.
Args:
theta (float32): The angle in radians.
Returns:
mat33f: The rotation matrix.
"""
c = wp.cos(theta)
s = wp.sin(theta)
return mat33f(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c)
@wp.func
def R_y(theta: float32) -> mat33f:
"""
Computes the rotation matrix around the Y axis.
Args:
theta (float32): The angle in radians.
Returns:
mat33f: The rotation matrix.
"""
c = wp.cos(theta)
s = wp.sin(theta)
return mat33f(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c)
@wp.func
def R_z(theta: float32) -> mat33f:
"""
Computes the rotation matrix around the Z axis.
Args:
theta (float32): The angle in radians.
Returns:
mat33f: The rotation matrix.
"""
c = wp.cos(theta)
s = wp.sin(theta)
return mat33f(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0)
@wp.func
def unskew(S: mat33f) -> vec3f:
"""
Extracts the 3D vector from a 3x3 skew-symmetric matrix.
Args:
S (mat33f): The 3x3 skew-symmetric matrix.
Returns:
vec3f: The vector extracted from the skew-symmetric matrix.
"""
return vec3f(S[2, 1], S[0, 2], S[1, 0])
###
# Quaternions
###
@wp.func
def G_of(q: quatf) -> mat34f:
"""
Computes the G matrix from a quaternion.
Args:
q (quatf): The quaternion.
Returns:
mat34f: The G matrix.
"""
G = mat34f(0.0)
G[0, 0] = q.w
G[0, 1] = -q.z
G[0, 2] = q.y
G[0, 3] = -q.x
G[1, 0] = q.z
G[1, 1] = q.w
G[1, 2] = -q.x
G[1, 3] = -q.y
G[2, 0] = -q.y
G[2, 1] = q.x
G[2, 2] = q.w
G[2, 3] = -q.z
return G
@wp.func
def H_of(q: quatf) -> mat34f:
"""
Computes the H matrix from a quaternion.
Args:
q (quatf): The quaternion.
Returns:
mat34f: The H matrix.
"""
H = mat34f(0.0)
H[0, 0] = q.w
H[0, 1] = q.z
H[0, 2] = -q.y
H[0, 3] = -q.x
H[1, 0] = -q.z
H[1, 1] = q.w
H[1, 2] = q.x
H[1, 3] = -q.y
H[2, 0] = q.y
H[2, 1] = -q.x
H[2, 2] = q.w
H[2, 3] = -q.z
return H
@wp.func
def quat_from_vec4(v: vec4f) -> quatf:
"""
Convert a vec4f to a quaternion type.
"""
return quatf(v[0], v[1], v[2], v[3])
@wp.func
def quat_to_vec4(q: quatf) -> vec4f:
"""
Convert a quaternion type to a vec4f.
"""
return vec4f(q.x, q.y, q.z, q.w)
@wp.func
def quat_conj(q: quatf) -> quatf:
"""
Compute the conjugate of a quaternion.
The conjugate of a quaternion q = (x, y, z, w) is defined as: q_conj = (x, y, z, -w)
"""
return quatf(q.x, q.y, q.z, -q.w)
@wp.func
def quat_positive(q: quatf) -> quatf:
"""
Compute the positive representation of a quaternion.
The positive representation is defined as the quaternion with a non-negative scalar part.
"""
if q.w < 0.0:
s = -1.0
else:
s = 1.0
return s * q
@wp.func
def quat_imaginary(q: quatf) -> vec3f:
"""
Extract the imaginary part of a quaternion.
The imaginary part is defined as the vector part of the quaternion (x, y, z).
"""
return vec3f(q.x, q.y, q.z)
@wp.func
def quat_apply(q: quatf, v: vec3f) -> vec3f:
"""
Apply a quaternion to a vector.
The quaternion is applied to the vector using the formula:
v' = s * v + q.w * uv + qv x uv, where s = ||q||^2, uv = 2 * qv x v, and qv is the imaginary part of the quaternion.
"""
qv = quat_imaginary(q)
uv = 2.0 * wp.cross(qv, v)
s = wp.dot(q, q)
return s * v + q.w * uv + wp.cross(qv, uv)
@wp.func
def quat_derivative(q: quatf, omega: vec3f) -> quatf:
"""
Computes the quaternion derivative from a quaternion and angular velocity.
Args:
q (quatf): The quaternion of the current pose of the body.
omega (vec3f): The angular velocity of the body.
Returns:
quatf: The quaternion derivative.
"""
vdq = 0.5 * wp.transpose(G_of(q)) * omega
dq = wp.quaternion(vdq.x, vdq.y, vdq.z, vdq.w, dtype=float32)
return dq
@wp.func
def quat_log(q: quatf) -> vec3f:
"""
Computes the logarithm of a quaternion using the stable
`4 * atan()` formulation to render a rotation vector.
"""
p = quat_positive(q)
pv = quat_imaginary(p)
pv_norm_sq = wp.dot(pv, pv)
pw_sq = p.w * p.w
pv_norm = wp.sqrt(pv_norm_sq)
# Check if the norm of the imaginary part is infinitesimal
if pv_norm_sq > FLOAT32_EPS:
# Regular solution for larger angles
# Use more stable 4 * atan() formulation over the 2 * atan(pv_norm / pw)
# TODO: angle = 4.0 * wp.atan2(pv_norm, (p.w + wp.sqrt(pw_sq + pv_norm_sq)))
angle = 4.0 * wp.atan(pv_norm / (p.w + wp.sqrt(pw_sq + pv_norm_sq)))
c = angle / pv_norm
else:
# Taylor expansion solution for small angles
# For the alternative branch use the limit of angle / pv_norm for angle -> 0.0
c = (2.0 - wp.static(2.0 / 3.0) * (pv_norm_sq / pw_sq)) / p.w
# Return the scaled imaginary part of the quaternion
return c * pv
@wp.func
def quat_log_decomposed(q: quatf) -> vec4f:
"""
Computes the logarithm of a quaternion using the stable
`4 * atan()` formulation to render an angle-axis vector.
The output is a vec4f with the following format:
- `a = [x, y, z, c]` is the angle-axis output
- `[x, y, z]` is the axis of rotation
- `c` is the angle.
"""
p = quat_positive(q)
pv = quat_imaginary(p)
pv_norm_sq = wp.dot(pv, pv)
pw_sq = p.w * p.w
pv_norm = wp.sqrt(pv_norm_sq)
# Check if the norm of the imaginary part is infinitesimal
if pv_norm_sq > FLOAT32_EPS:
# Regular solution for larger angles
# Use more stable 4 * atan() formulation over the 2 * atan(pv_norm / pw)
# TODO: angle = 4.0 * wp.atan2(pv_norm, (p.w + wp.sqrt(pw_sq + pv_norm_sq)))
angle = 4.0 * wp.atan(pv_norm / (p.w + wp.sqrt(pw_sq + pv_norm_sq)))
c = angle / pv_norm
else:
# Taylor expansion solution for small angles
# For the alternative branch use the limit of angle / pv_norm for angle -> 0.0
c = (2.0 - wp.static(2.0 / 3.0) * (pv_norm_sq / pw_sq)) / p.w
# Return the scaled imaginary part of the quaternion
return vec4f(pv.x, pv.y, pv.z, c)
@wp.func
def quat_exp(v: vec3f) -> quatf:
"""
Computes the exponential map of a 3D vector as a quaternion.
using Rodrigues' formula: R = I + sin(θ)*K (1-cos(θ)*K^2),
were q = quat(R).
Args:
v (vec3f): The 3D rotation vector to be mapped to quaternion space.
Returns:
quatf: The quaternion resulting from the exponential map of the input rotation vector.
"""
eps = FLOAT32_EPS
q = wp.quat_identity(dtype=float32)
vn = wp.length(v)
if vn > eps:
a = 0.5 * vn
sina = wp.sin(a)
cosa = wp.cos(a)
vu = wp.normalize(v)
q.x = sina * vu.x
q.y = sina * vu.y
q.z = sina * vu.z
q.w = cosa
else:
q.x = 0.5 * v.x
q.y = 0.5 * v.y
q.z = 0.5 * v.z
q.w = 1.0
return q
@wp.func
def quat_product(q1: quatf, q2: quatf) -> quatf:
"""
Computes the quaternion product of two quaternions.
Args:
q1 (quatf): The first quaternion.
q2 (quatf): The second quaternion.
Returns:
quatf: The result of the quaternion product.
"""
q3 = wp.quat_identity(dtype=float32)
q3.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y
q3.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x
q3.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w
q3.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z
return q3
@wp.func
def quat_box_plus(q: quatf, v: vec3f) -> quatf:
"""
Computes the box-plus operation for a quaternion and a vector:
R(q) [+] v == exp(v) * R(q), where R(q) is the rotation matrix of the quaternion q.
Args:
q (vec3f): The quaternion.
v (vec3f): The vector.
Returns:
quatf: The result of the box-plus operation.
"""
return quat_product(quat_exp(v), q)
@wp.func
def quat_from_x_rot(angle_rad: float32) -> quatf:
"""
Computes a unit quaternion corresponding to rotation by given angle about the x axis
"""
return wp.quatf(wp.sin(0.5 * angle_rad), 0.0, 0.0, wp.cos(0.5 * angle_rad))
@wp.func
def quat_from_y_rot(angle_rad: float32) -> quatf:
"""
Computes a unit quaternion corresponding to rotation by given angle about the y axis
"""
return wp.quatf(0.0, wp.sin(0.5 * angle_rad), 0.0, wp.cos(0.5 * angle_rad))
@wp.func
def quat_from_z_rot(angle_rad: float32) -> quatf:
"""
Computes a unit quaternion corresponding to rotation by given angle about the z axis
"""
return wp.quatf(0.0, 0.0, wp.sin(0.5 * angle_rad), wp.cos(0.5 * angle_rad))
@wp.func
def quat_to_euler_xyz(q: quatf) -> vec3f:
"""
Converts a unit quaternion to XYZ Euler angles (also known as Cardan angles).
"""
rpy = vec3f(0.0)
R_20 = -2.0 * (q.x * q.z - q.w * q.y)
if wp.abs(R_20) < 1.0:
rpy[1] = wp.asin(-R_20)
rpy[0] = wp.atan2(2.0 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)
rpy[2] = wp.atan2(2.0 * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z)
else: # Gimbal lock
rpy[0] = wp.atan2(-2.0 * (q.x * q.y - q.w * q.z), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z)
rpy[1] = wp.half_pi if R_20 <= -1.0 else -wp.half_pi
rpy[2] = 0.0
return rpy
@wp.func
def quat_from_euler_xyz(rpy: vec3f) -> quatf:
"""
Converts XYZ Euler angles (also known as Cardan angles) to a unit quaternion.
"""
return wp.quat_from_matrix(R_z(rpy.z) @ R_y(rpy.y) @ R_x(rpy.x))
@wp.func
def quat_left_jacobian_inverse(q: quatf) -> mat33f:
"""
Computes the left-Jacobian inverse of the quaternion log map
"""
p = quat_positive(q)
pv = quat_imaginary(p)
pv_norm_sq = wp.dot(pv, pv)
pw_sq = p.w * p.w
pv_norm = wp.sqrt(pv_norm_sq)
# Check if the norm of the imaginary part is infinitesimal
if pv_norm_sq > FLOAT32_EPS:
# Regular solution for larger angles
c0 = 2.0 * wp.atan(pv_norm / (p.w + wp.sqrt(pw_sq + pv_norm_sq))) / pv_norm
c1 = (1.0 - c0 * p.w) / pv_norm_sq
else:
# Taylor expansion solution for small angles
c1 = wp.static(1.0 / 3.0) / pw_sq
c0 = (1.0 - c1 * pv_norm_sq) / p.w
return wp.identity(3, dtype=float32) - wp.skew(c0 * pv) + wp.skew(c1 * pv) * wp.skew(pv)
@wp.func
def quat_normalized_apply(q: quatf, v: vec3f) -> vec3f:
"""
Combines quaternion normalization and applying a unit quaternion to a vector
"""
qv = quat_imaginary(q)
s = wp.dot(q, q)
uv_s = (2.0 / s) * wp.cross(qv, v)
return v + q[3] * uv_s + wp.cross(qv, uv_s)
@wp.func
def quat_conj_normalized_apply(q: quatf, v: vec3f) -> vec3f:
"""
Combines quaternion conjugation, normalization and applying a unit quaternion to a vector
"""
qv = quat_imaginary(q)
s = wp.dot(q, q)
uv_s = (2.0 / s) * wp.cross(qv, v)
return v - q[3] * uv_s + wp.cross(qv, uv_s)
@wp.func
def quat_twist_angle(q: quatf, axis: vec3f) -> wp.float32:
"""
Computes the twist angle of a quaternion around a specific axis.
This function isolates the rotation component of ``q`` that occurs purely
around the provided ``axis`` (Twist-Swing decomposition) and returns
its angle in [-pi, pi].
"""
# positive quaternion guarantees angle is in [-pi, pi]
p = quat_positive(q)
pv = quat_imaginary(p)
angle = 2.0 * wp.atan2(wp.dot(pv, axis), p.w)
return angle
###
# Unit Quaternions
###
@wp.func
def unit_quat_apply(q: quatf, v: vec3f) -> vec3f:
"""
Applies a unit quaternion to a vector (making use of the unit norm assumption to simplify the result)
"""
qv = quat_imaginary(q)
uv = 2.0 * wp.cross(qv, v)
return v + q.w * uv + wp.cross(qv, uv)
@wp.func
def unit_quat_conj_apply(q: quatf, v: vec3f) -> vec3f:
"""
Applies the conjugate of a unit quaternion to a vector (making use of the unit norm assumption to simplify
the result)
"""
qv = quat_imaginary(q)
uv = 2.0 * wp.cross(qv, v)
return v - q.w * uv + wp.cross(qv, uv)
@wp.func
def unit_quat_to_rotation_matrix(q: quatf) -> mat33f:
"""
Converts a unit quaternion to a rotation matrix (making use of the unit norm assumption to simplify the result)
"""
xx = 2.0 * q.x * q.x
xy = 2.0 * q.x * q.y
xz = 2.0 * q.x * q.z
wx = 2.0 * q.w * q.x
yy = 2.0 * q.y * q.y
yz = 2.0 * q.y * q.z
wy = 2.0 * q.w * q.y
zz = 2.0 * q.z * q.z
wz = 2.0 * q.w * q.z
return mat33f(1.0 - yy - zz, xy - wz, xz + wy, xy + wz, 1.0 - xx - zz, yz - wx, xz - wy, yz + wx, 1.0 - xx - yy)
@wp.func
def unit_quat_conj_to_rotation_matrix(q: quatf) -> mat33f:
"""
Converts the conjugate of a unit quaternion to a rotation matrix (making use of the unit norm assumption
to simplify the result); this is simply the transpose of unit_quat_to_rotation_matrix(q)
"""
xx = 2.0 * q.x * q.x
xy = 2.0 * q.x * q.y
xz = 2.0 * q.x * q.z
wx = 2.0 * q.w * q.x
yy = 2.0 * q.y * q.y
yz = 2.0 * q.y * q.z
wy = 2.0 * q.w * q.y
zz = 2.0 * q.z * q.z
wz = 2.0 * q.w * q.z
return mat33f(1.0 - yy - zz, xy + wz, xz - wy, xy - wz, 1.0 - xx - zz, yz + wx, xz + wy, yz - wx, 1.0 - xx - yy)
@wp.func
def unit_quat_apply_jacobian(q: quatf, v: vec3f) -> mat34f:
"""
Returns the Jacobian of unit_quat_apply(q, v) with respect to q
"""
xX = 2.0 * q.x * v[0]
xY = 2.0 * q.x * v[1]
xZ = 2.0 * q.x * v[2]
yX = 2.0 * q.y * v[0]
yY = 2.0 * q.y * v[1]
yZ = 2.0 * q.y * v[2]
zX = 2.0 * q.z * v[0]
zY = 2.0 * q.z * v[1]
zZ = 2.0 * q.z * v[2]
wX = 2.0 * q.w * v[0]
wY = 2.0 * q.w * v[1]
wZ = 2.0 * q.w * v[2]
return mat34f(
yY + zZ,
-2.0 * yX + xY + wZ,
-2.0 * zX + xZ - wY,
yZ - zY,
-2.0 * xY + yX - wZ,
xX + zZ,
-2.0 * zY + yZ + wX,
zX - xZ,
-2.0 * xZ + zX + wY,
-2.0 * yZ + zY - wX,
xX + yY,
xY - yX,
)
@wp.func
def unit_quat_conj_apply_jacobian(q: quatf, v: vec3f) -> mat34f:
"""
Returns the Jacobian of unit_quat_conj_apply(q, v) with respect to q
"""
xX = 2.0 * q.x * v[0]
xY = 2.0 * q.x * v[1]
xZ = 2.0 * q.x * v[2]
yX = 2.0 * q.y * v[0]
yY = 2.0 * q.y * v[1]
yZ = 2.0 * q.y * v[2]
zX = 2.0 * q.z * v[0]
zY = 2.0 * q.z * v[1]
zZ = 2.0 * q.z * v[2]
wX = 2.0 * q.w * v[0]
wY = 2.0 * q.w * v[1]
wZ = 2.0 * q.w * v[2]
return mat34f(
yY + zZ,
-2.0 * yX + xY - wZ,
-2.0 * zX + xZ + wY,
zY - yZ,
-2.0 * xY + yX + wZ,
xX + zZ,
-2.0 * zY + yZ - wX,
xZ - zX,
-2.0 * xZ + zX - wY,
-2.0 * yZ + zY + wX,
xX + yY,
yX - xY,
)
###
# Screws
###
@wp.func
def screw(linear: vec3f, angular: vec3f) -> vec6f:
"""
Constructs a 6D screw (as `vec6f`) from 3D linear and angular components.
Args:
linear (vec3f): The linear component of the screw.
angular (vec3f): The angular component of the screw.
Returns:
vec6f: The resulting screw represented as a 6D vector.
"""
return vec6f(linear[0], linear[1], linear[2], angular[0], angular[1], angular[2])
@wp.func
def screw_linear(s: vec6f) -> vec3f:
"""
Extracts the linear component from a 6D screw vector.
Args:
s (vec6f): The 6D screw vector.
Returns:
vec3f: The linear component of the screw.
"""
return vec3f(s[0], s[1], s[2])
@wp.func
def screw_angular(s: vec6f) -> vec3f:
"""
Extracts the angular component from a 6D screw vector.
Args:
s (vec6f): The 6D screw vector.
Returns:
vec3f: The angular component of the screw.
"""
return vec3f(s[3], s[4], s[5])
@wp.func
def screw_transform_matrix_from_points(r_A: vec3f, r_B: vec3f) -> mat66f:
"""
Generates a 6x6 screw transformation matrix given the starting (`r_A`)
and ending (`r_B`) positions defining the line-of-action of the screw.
Both positions are assumed to be expressed in world coordinates,
and the line-of-action can be thought of as an effective lever-arm
from point `A` to point `B`.
This function thus renders the matrix screw transformation from point `A` to point `B` as:
`W_BA := [[I_3 , 0_3],[S_BA , I_3]]`,
where `S_BA` is the skew-symmetric matrix of the vector `r_BA = r_A - r_B`.
Args:
r_A (vec3f): The starting position of the line-of-action in world coordinates.
r_B (vec3f): The ending position of the line-of-action in world coordinates.
Returns:
mat66f: The 6x6 screw transformation matrix.
"""
# Initialize the wrench matrix
W_BA = I_6
# Fill the lower left block with the skew-symmetric matrix
S_BA = wp.skew(r_A - r_B)
for i in range(3):
for j in range(3):
W_BA[3 + i, j] = S_BA[i, j]
# Return the wrench matrix
return W_BA
###
# Wrenches
###
W_C_I = wp.constant(mat63f(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0))
"""Identity-like matrix to initialize contact wrench matrices."""
@wp.func
def contact_wrench_matrix_from_points(r_k: vec3f, r_i: vec3f) -> mat63f:
"""
Generates a 6x3 screw transformation matrix given the contact (`r_k`)
and body CoM (`r_i`) positions defining the line-of-action of the force.
Both positions are assumed to be expressed in world coordinates,
and the line-of-action can be thought of as an effective lever-arm
from point `k` to point `i`.
This function thus renders the matrix screw transformation from point `k` to point `i` as:
`W_ki := [[I_3],[S_ki]]`,
where `S_ki` is the skew-symmetric matrix of the vector `r_ki = r_k - r_i`.
Args:
r_k (vec3f): The position of the contact point in world coordinates.
r_i (vec3f): The position of the body CoM in world coordinates.
Returns:
mat66f: The 6x6 screw transformation matrix.
"""
# Initialize the wrench matrix
W_ki = W_C_I
# Fill the lower left block with the skew-symmetric matrix
S_ki = wp.skew(r_k - r_i)
for i in range(3):
for j in range(3):
W_ki[3 + i, j] = S_ki[i, j]
# Return the wrench matrix
return W_ki
@wp.func
def expand6d(X: mat33f) -> mat66f:
"""
Expands a 3x3 rotation matrix to a 6x6 matrix operator by filling
the upper left and lower right blocks with the input matrix.
Args:
X (mat33f): The 3x3 matrix to be expanded.
Returns:
mat66: The expanded 6x6 matrix.
"""
# Initialize the 6D matrix
X_6d = mat66f(0.0)
# Fill the upper left 3x3 block with the input matrix
for i in range(3):
for j in range(3):
X_6d[i, j] = X[i, j]
X_6d[3 + i, 3 + j] = X[i, j]
# Return the expanded matrix
return X_6d
###
# Dynamics
###
@wp.func
def compute_body_twist_update_with_eom(
dt: float32,
g: vec3f,
inv_m_i: float32,
I_i: mat33f,
inv_I_i: mat33f,
u_i: vec6f,
w_i: vec6f,
) -> tuple[vec3f, vec3f]:
# Extract linear and angular parts
v_i = screw_linear(u_i)
omega_i = screw_angular(u_i)
S_i = wp.skew(omega_i)
f_i = screw_linear(w_i)
tau_i = screw_angular(w_i)
# Compute velocity update equations
v_i_n = v_i + dt * (g + inv_m_i * f_i)
omega_i_n = omega_i + dt * inv_I_i @ (-S_i @ (I_i @ omega_i) + tau_i)
# Return the updated velocities
return v_i_n, omega_i_n
@wp.func
def compute_body_pose_update_with_logmap(
dt: float32,
p_i: transformf,
v_i: vec3f,
omega_i: vec3f,
) -> transformf:
# Extract linear and angular parts
r_i = wp.transform_get_translation(p_i)
q_i = wp.transform_get_rotation(p_i)
# Compute configuration update equations
r_i_n = r_i + dt * v_i
q_i_n = quat_box_plus(q_i, dt * omega_i)
p_i_n = transformf(r_i_n, q_i_n)
# Return the new pose and twist
return p_i_n
###
# Indexing
###
@wp.func
def tril_index(row: Any, col: Any) -> Any:
"""
Computes the index in a flattened lower-triangular matrix.
Args:
row (Any): The row index.
col (Any): The column index.
Returns:
Any: The index in the flattened lower-triangular matrix.
"""
return (row * (row + 1)) // 2 + col
================================================
FILE: newton/_src/solvers/kamino/_src/core/model.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Defines the model container of Kamino."""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
import warp as wp
# Newton imports
from .....geometry import GeoType, ShapeFlags
from .....sim import JointTargetMode, JointType, Model
# Kamino imports
from .bodies import RigidBodiesData, RigidBodiesModel, convert_geom_offset_origin_to_com
from .control import ControlKamino
from .conversions import (
compute_required_contact_capacity,
convert_entity_local_transforms,
)
from .data import DataKamino, DataKaminoInfo
from .geometry import GeometriesData, GeometriesModel
from .gravity import GravityModel
from .joints import (
JOINT_DQMAX,
JOINT_QMAX,
JOINT_QMIN,
JOINT_TAUMAX,
JointActuationType,
JointDoFType,
JointsData,
JointsModel,
)
from .materials import MaterialDescriptor, MaterialManager, MaterialPairsModel, MaterialsModel
from .size import SizeKamino
from .state import StateKamino
from .time import TimeData, TimeModel
from .types import float32, int32, mat33f, transformf, vec2i, vec3f, vec4f, vec6f
###
# Module interface
###
__all__ = [
"ModelKamino",
"ModelKaminoInfo",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
@dataclass
class ModelKaminoInfo:
"""
A container to hold the time-invariant information and meta-data of a model.
"""
###
# Host-side Summary Counts
###
num_worlds: int = 0
"""The number of worlds represented in the model."""
###
# Entity Counts
###
num_bodies: wp.array | None = None
"""
The number of bodies in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_joints: wp.array | None = None
"""
The number of joints in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_passive_joints: wp.array | None = None
"""
The number of passive joints in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_actuated_joints: wp.array | None = None
"""
The number of actuated joints in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_dynamic_joints: wp.array | None = None
"""
The number of dynamic joints in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_geoms: wp.array | None = None
"""
The number of geometries in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
max_limits: wp.array | None = None
"""
The maximum number of limits in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
max_contacts: wp.array | None = None
"""
The maximum number of contacts in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
###
# DoF Counts
###
num_body_dofs: wp.array | None = None
"""
The number of body DoFs of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_joint_coords: wp.array | None = None
"""
The number of joint coordinates of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_joint_dofs: wp.array | None = None
"""
The number of joint DoFs of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_passive_joint_coords: wp.array | None = None
"""
The number of passive joint coordinates of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_passive_joint_dofs: wp.array | None = None
"""
The number of passive joint DoFs of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_actuated_joint_coords: wp.array | None = None
"""
The number of actuated joint coordinates of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_actuated_joint_dofs: wp.array | None = None
"""
The number of actuated joint DoFs of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
###
# Constraint Counts
###
# TODO: We could make this a vec2i to store dynamic
# and kinematic joint constraint counts separately
num_joint_cts: wp.array | None = None
"""
The number of joint constraints of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_joint_dynamic_cts: wp.array | None = None
"""
The number of dynamic joint constraints of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
num_joint_kinematic_cts: wp.array | None = None
"""
The number of kinematic joint constraints of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
max_limit_cts: wp.array | None = None
"""
The maximum number of active limit constraints of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
max_contact_cts: wp.array | None = None
"""
The maximum number of active contact constraints of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
max_total_cts: wp.array | None = None
"""
The maximum total number of active constraints of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
###
# Entity Offsets
###
bodies_offset: wp.array | None = None
"""
The body index offset of each world w.r.t the model.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joints_offset: wp.array | None = None
"""
The joint index offset of each world w.r.t the model.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
geoms_offset: wp.array | None = None
"""
The geom index offset of each world w.r.t. the model.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
limits_offset: wp.array | None = None
"""
The limit index offset of each world w.r.t the model.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
contacts_offset: wp.array | None = None
"""
The contact index offset of world w.r.t the model.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
unilaterals_offset: wp.array | None = None
"""
The index offset of the unilaterals (limits + contacts) block of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
###
# DoF Offsets
###
body_dofs_offset: wp.array | None = None
"""
The index offset of the body DoF block of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_coords_offset: wp.array | None = None
"""
The index offset of the joint coordinates block of each world.\n
Used to index into arrays that contain flattened joint coordinate-sized data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_dofs_offset: wp.array | None = None
"""
The index offset of the joint DoF block of each world.\n
Used to index into arrays that contain flattened joint DoF-sized data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_passive_coords_offset: wp.array | None = None
"""
The index offset of the passive joint coordinates block of each world.\n
Used to index into arrays that contain flattened passive joint coordinate-sized data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_passive_dofs_offset: wp.array | None = None
"""
The index offset of the passive joint DoF block of each world.\n
Used to index into arrays that contain flattened passive joint DoF-sized data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_actuated_coords_offset: wp.array | None = None
"""
The index offset of the actuated joint coordinates block of each world.\n
Used to index into arrays that contain flattened actuated joint coordinate-sized data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_actuated_dofs_offset: wp.array | None = None
"""
The index offset of the actuated joint DoF block of each world.\n
Used to index into arrays that contain flattened actuated joint DoF-sized data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
###
# Constraint Offsets
###
joint_cts_offset: wp.array | None = None
"""
The index offset of the joint constraints block of each world.\n
Used to index into arrays that contain flattened and
concatenated dynamic and kinematic joint constraint data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_dynamic_cts_offset: wp.array | None = None
"""
The index offset of the dynamic joint constraints block of each world.\n
Used to index into arrays that contain flattened dynamic joint constraint data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_kinematic_cts_offset: wp.array | None = None
"""
The index offset of the kinematic joint constraints block of each world.\n
Used to index into arrays that contain flattened kinematic joint constraint data.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
# TODO: We could make this an array of vec5i and store the absolute
# startindex of each constraint group in the constraint array `lambda`:
# - [0]: total_cts_offset
# - [1]: joint_dynamic_cts_group_offset
# - [2]: joint_kinematic_cts_group_offset
# - [3]: limit_cts_group_offset
# - [4]: contact_cts_group_offset
# TODO: We could then provide helper functions to get the start-end of each block
total_cts_offset: wp.array | None = None
"""
The index offset of the total constraints block of each world.\n
Used to index into constraint-space arrays, e.g. constraint residuals and reactions.\n
This offset should be used together with:
- joint_dynamic_cts_group_offset
- joint_kinematic_cts_group_offset
- limit_cts_group_offset
- contact_cts_group_offset
Example:
```
# To index into the dynamic joint constraint reactions of world `w`:
world_cts_start = model_info.total_cts_offset[w]
local_joint_dynamic_cts_start = model_info.joint_dynamic_cts_group_offset[w]
local_joint_kinematic_cts_start = model_info.joint_kinematic_cts_group_offset[w]
local_limit_cts_start = model_info.limit_cts_group_offset[w]
local_contact_cts_start = model_info.contact_cts_group_offset[w]
# Now compute the starting index of each constraint group within the total constraints block of world `w`:
world_dynamic_joint_cts_start = world_cts_start + local_joint_dynamic_cts_start
world_kinematic_joint_cts_start = world_cts_start + local_joint_kinematic_cts_start
world_limit_cts_start = world_cts_start + local_limit_cts_start
world_contact_cts_start = world_cts_start + local_contact_cts_start
```
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_dynamic_cts_group_offset: wp.array | None = None
"""
The index offset of the dynamic joint constraints group within the constraints block of each world.\n
Used to index into constraint-space arrays, e.g. constraint residuals and reactions.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
joint_kinematic_cts_group_offset: wp.array | None = None
"""
The index offset of the kinematic joint constraints group within the constraints block of each world.\n
Used to index into constraint-space arrays, e.g. constraint residuals and reactions.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
###
# Base Properties
###
base_body_index: wp.array | None = None
"""
The index of the base body assigned in each world w.r.t the model.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
base_joint_index: wp.array | None = None
"""
The index of the base joint assigned in each world w.r.t the model.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
###
# Inertial Properties
###
mass_min: wp.array | None = None
"""
Smallest mass amongst all bodies in each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
mass_max: wp.array | None = None
"""
Largest mass amongst all bodies in each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
mass_total: wp.array | None = None
"""
Total mass over all bodies in each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
inertia_total: wp.array | None = None
"""
Total diagonal inertia over all bodies in each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
@dataclass
class ModelKamino:
"""
A container to hold the time-invariant system model data.
"""
_model: Model | None = None
"""The base :class:`newton.Model` instance from which this :class:`kamino.ModelKamino` was created."""
_device: wp.DeviceLike | None = None
"""The Warp device on which the model data is allocated."""
_requires_grad: bool = False
"""Whether the model was finalized (see :meth:`ModelBuilder.finalize`) with gradient computation enabled."""
size: SizeKamino | None = None
"""
Host-side cache of the model summary sizes.\n
This is used for memory allocations and kernel thread dimensions.
"""
info: ModelKaminoInfo | None = None
"""The model info container holding the information and meta-data of the model."""
time: TimeModel | None = None
"""The time model container holding time-step of each world."""
gravity: GravityModel | None = None
"""The gravity model container holding the gravity configurations for each world."""
bodies: RigidBodiesModel | None = None
"""The rigid bodies model container holding all rigid body entities in the model."""
joints: JointsModel | None = None
"""The joints model container holding all joint entities in the model."""
geoms: GeometriesModel | None = None
"""The geometries model container holding all geometry entities in the model."""
materials: MaterialsModel | None = None
"""
The materials model container holding all material entities in the model.\n
The materials data is currently defined globally to be shared by all worlds.
"""
material_pairs: MaterialPairsModel | None = None
"""
The material pairs model container holding all material pairs in the model.\n
The material-pairs data is currently defined globally to be shared by all worlds.
"""
###
# Properties
###
@property
def device(self) -> wp.DeviceLike:
"""The Warp device on which the model data is allocated."""
return self._device
@property
def requires_grad(self) -> bool:
"""Whether the model was finalized (see :meth:`ModelBuilder.finalize`) with gradient computation enabled."""
return self._requires_grad
###
# Factories
###
def data(
self,
unilateral_cts: bool = False,
joint_wrenches: bool = False,
requires_grad: bool = False,
device: wp.DeviceLike = None,
) -> DataKamino:
"""
Creates a model data container with the initial state of the model entities.
Parameters:
unilateral_cts (`bool`, optional):
Whether to include unilateral constraints (limits and contacts) in the model data. Defaults to `True`.
joint_wrenches (`bool`, optional):
Whether to include joint wrenches in the model data. Defaults to `False`.
requires_grad (`bool`, optional):
Whether the model data should require gradients. Defaults to `False`.
device (`wp.DeviceLike`, optional):
The device to create the model data on. If not specified, the model's device is used.
Defaults to `None`. If not specified, the model's device is used.
"""
# If no device is specified, use the model's device
if device is None:
device = self.device
# Retrieve entity counts
nw = self.size.num_worlds
nb = self.size.sum_of_num_bodies
nj = self.size.sum_of_num_joints
ng = self.size.sum_of_num_geoms
# Retrieve the joint coordinate, DoF and constraint counts
njcoords = self.size.sum_of_num_joint_coords
njdofs = self.size.sum_of_num_joint_dofs
njcts = self.size.sum_of_num_joint_cts
njdyncts = self.size.sum_of_num_dynamic_joint_cts
njkincts = self.size.sum_of_num_kinematic_joint_cts
# Construct the model data on the specified device
with wp.ScopedDevice(device=device):
# Create a new model data info with the total constraint
# counts initialized to the joint constraints count
info = DataKaminoInfo(
num_total_cts=wp.clone(self.info.num_joint_cts),
num_limits=wp.zeros(shape=nw, dtype=int32) if unilateral_cts else None,
num_contacts=wp.zeros(shape=nw, dtype=int32) if unilateral_cts else None,
num_limit_cts=wp.zeros(shape=nw, dtype=int32) if unilateral_cts else None,
num_contact_cts=wp.zeros(shape=nw, dtype=int32) if unilateral_cts else None,
limit_cts_group_offset=wp.zeros(shape=nw, dtype=int32) if unilateral_cts else None,
contact_cts_group_offset=wp.zeros(shape=nw, dtype=int32) if unilateral_cts else None,
)
# Construct the time data with the initial step and time set to zero for all worlds
time = TimeData(
steps=wp.zeros(shape=nw, dtype=int32, requires_grad=requires_grad),
time=wp.zeros(shape=nw, dtype=float32, requires_grad=requires_grad),
)
# Construct the rigid bodies data from the model's initial state
bodies = RigidBodiesData(
num_bodies=nb,
I_i=wp.zeros(shape=nb, dtype=mat33f, requires_grad=requires_grad),
inv_I_i=wp.zeros(shape=nb, dtype=mat33f, requires_grad=requires_grad),
q_i=wp.clone(self.bodies.q_i_0, requires_grad=requires_grad),
u_i=wp.clone(self.bodies.u_i_0, requires_grad=requires_grad),
w_i=wp.zeros_like(self.bodies.u_i_0, requires_grad=requires_grad),
w_a_i=wp.zeros_like(self.bodies.u_i_0, requires_grad=requires_grad),
w_j_i=wp.zeros_like(self.bodies.u_i_0, requires_grad=requires_grad),
w_l_i=wp.zeros_like(self.bodies.u_i_0, requires_grad=requires_grad),
w_c_i=wp.zeros_like(self.bodies.u_i_0, requires_grad=requires_grad),
w_e_i=wp.zeros_like(self.bodies.u_i_0, requires_grad=requires_grad),
)
# Construct the joints data from the model's initial state
joints = JointsData(
num_joints=nj,
p_j=wp.zeros(shape=nj, dtype=transformf, requires_grad=requires_grad),
q_j=wp.zeros(shape=njcoords, dtype=float32, requires_grad=requires_grad),
q_j_p=wp.zeros(shape=njcoords, dtype=float32, requires_grad=requires_grad),
dq_j=wp.zeros(shape=njdofs, dtype=float32, requires_grad=requires_grad),
tau_j=wp.zeros(shape=njdofs, dtype=float32, requires_grad=requires_grad),
r_j=wp.zeros(shape=njkincts, dtype=float32, requires_grad=requires_grad),
dr_j=wp.zeros(shape=njkincts, dtype=float32, requires_grad=requires_grad),
lambda_j=wp.zeros(shape=njcts, dtype=float32, requires_grad=requires_grad),
m_j=wp.zeros(shape=njdyncts, dtype=float32, requires_grad=requires_grad),
inv_m_j=wp.zeros(shape=njdyncts, dtype=float32, requires_grad=requires_grad),
dq_b_j=wp.zeros(shape=njdyncts, dtype=float32, requires_grad=requires_grad),
# TODO: Should we make these optional and only include them when implicit joints are present?
q_j_ref=wp.clone(self.joints.q_j_0, requires_grad=requires_grad),
dq_j_ref=wp.clone(self.joints.dq_j_0, requires_grad=requires_grad),
tau_j_ref=wp.zeros(shape=njdofs, dtype=float32, requires_grad=requires_grad),
j_w_j=wp.zeros(shape=nj, dtype=vec6f, requires_grad=requires_grad) if joint_wrenches else None,
j_w_c_j=wp.zeros(shape=nj, dtype=vec6f, requires_grad=requires_grad) if joint_wrenches else None,
j_w_a_j=wp.zeros(shape=nj, dtype=vec6f, requires_grad=requires_grad) if joint_wrenches else None,
j_w_l_j=wp.zeros(shape=nj, dtype=vec6f, requires_grad=requires_grad) if joint_wrenches else None,
)
# Construct the geometries data from the model's initial state
geoms = GeometriesData(
num_geoms=ng,
pose=wp.zeros(shape=ng, dtype=transformf, requires_grad=requires_grad),
)
# Assemble and return the new data container
return DataKamino(
info=info,
time=time,
bodies=bodies,
joints=joints,
geoms=geoms,
)
def state(self, requires_grad: bool = False, device: wp.DeviceLike = None) -> StateKamino:
"""
Creates state container initialized to the initial body state defined in the model.
Parameters:
requires_grad (`bool`, optional):
Whether the state should require gradients. Defaults to `False`.
device (`wp.DeviceLike`, optional):
The device to create the state on. If not specified, the model's device is used.
"""
# If no device is specified, use the model's device
if device is None:
device = self.device
# Create a new state container with the initial state of the model entities on the specified device
with wp.ScopedDevice(device=device):
state = StateKamino(
q_i=wp.clone(self.bodies.q_i_0, requires_grad=requires_grad),
u_i=wp.clone(self.bodies.u_i_0, requires_grad=requires_grad),
w_i=wp.zeros_like(self.bodies.u_i_0, requires_grad=requires_grad),
w_i_e=wp.zeros_like(self.bodies.u_i_0, requires_grad=requires_grad),
q_j=wp.clone(self.joints.q_j_0, requires_grad=requires_grad),
q_j_p=wp.clone(self.joints.q_j_0, requires_grad=requires_grad),
dq_j=wp.zeros(shape=self.size.sum_of_num_joint_dofs, dtype=float32, requires_grad=requires_grad),
lambda_j=wp.zeros(shape=self.size.sum_of_num_joint_cts, dtype=float32, requires_grad=requires_grad),
)
# Return the constructed state container
return state
def control(self, requires_grad: bool = False, device: wp.DeviceLike = None) -> ControlKamino:
"""
Creates a control container with all values initialized to zeros.
Parameters:
requires_grad (`bool`, optional):
Whether the control container should require gradients. Defaults to `False`.
device (`wp.DeviceLike`, optional):
The device to create the control container on. If not specified, the model's device is used.
"""
# If no device is specified, use the model's device
if device is None:
device = self.device
# Create a new control container on the specified device
with wp.ScopedDevice(device=device):
control = ControlKamino(
tau_j=wp.zeros(shape=self.size.sum_of_num_joint_dofs, dtype=float32, requires_grad=requires_grad),
q_j_ref=wp.zeros(shape=self.size.sum_of_num_joint_dofs, dtype=float32, requires_grad=requires_grad),
dq_j_ref=wp.zeros(shape=self.size.sum_of_num_joint_dofs, dtype=float32, requires_grad=requires_grad),
tau_j_ref=wp.zeros(shape=self.size.sum_of_num_joint_dofs, dtype=float32, requires_grad=requires_grad),
)
# Return the constructed control container
return control
@staticmethod
def from_newton(model: Model) -> ModelKamino:
"""
Finalizes the :class:`ModelKamino` from an existing instance of :class:`newton.Model`.
"""
# Ensure the base model is valid
if model is None:
raise ValueError("Cannot finalize ModelKamino from a None newton.Model instance.")
elif not isinstance(model, Model):
raise TypeError("Cannot finalize ModelKamino from an invalid newton.Model instance.")
# Single-world Newton models may have world index -1 (unassigned).
# Normalize to 0 so downstream world-based grouping works correctly.
if model.world_count == 1:
for attr in ("body_world", "joint_world", "shape_world"):
arr = getattr(model, attr)
arr_np = arr.numpy()
if np.any(arr_np < 0):
arr_np[arr_np < 0] = 0
arr.assign(arr_np)
# ---------------------------------------------------------------------------
# Pre-processing: absorb non-identity joint_X_c rotations into child body
# frames so that Kamino sees aligned joint frames on both sides.
#
# Kamino's constraint system assumes a single joint frame X_j valid for both
# the base (parent) and follower (child) bodies. At q = 0 it requires
# q_base^{-1} * q_follower = identity
# Newton, however, allows different parent / child joint-frame orientations
# via joint_X_p and joint_X_c. At q = 0 Newton's FK gives:
# q_follower = q_parent * q_pj * inv(q_cj)
# so q_base^{-1} * q_follower = q_pj * inv(q_cj) which is generally not
# identity.
#
# To fix this we apply a per-body correction rotation q_corr = q_cj * inv(q_pj)
# (applied on the right) to each child body's frame:
# q_body_new = q_body_old * q_corr
# This makes q_base^{-1} * q_follower_new = identity at q = 0, and the joint
# rotation axis R(q_pj) * axis is preserved.
#
# All body-local quantities (CoM, inertia, shapes) are re-expressed in the
# rotated frame, and downstream joint_X_p transforms are updated to account
# for the parent body's frame change.
# ---------------------------------------------------------------------------
converted = convert_entity_local_transforms(model)
# ----------------------------------------------------------------------------
def _to_wpq(q):
return wp.quat(float(q[0]), float(q[1]), float(q[2]), float(q[3]))
def _compute_entity_indices_wrt_world(entity_world: wp.array) -> np.ndarray:
wid_np = entity_world.numpy()
eid_np = np.zeros_like(wid_np)
for e in range(wid_np.size):
eid_np[e] = np.sum(wid_np[:e] == wid_np[e])
return eid_np
def _compute_num_entities_per_world(entity_world: wp.array, num_worlds: int) -> np.ndarray:
wid_np = entity_world.numpy()
counts = np.zeros(num_worlds, dtype=int)
for w in range(num_worlds):
counts[w] = np.sum(wid_np == w)
return counts
# Compute the entity indices of each body w.r.t the corresponding world
body_bid_np = _compute_entity_indices_wrt_world(model.body_world)
joint_jid_np = _compute_entity_indices_wrt_world(model.joint_world)
shape_sid_np = _compute_entity_indices_wrt_world(model.shape_world)
# Compute the number of entities per world
num_bodies_np = _compute_num_entities_per_world(model.body_world, model.world_count)
num_joints_np = _compute_num_entities_per_world(model.joint_world, model.world_count)
num_shapes_np = _compute_num_entities_per_world(model.shape_world, model.world_count)
# Compute body coord/DoF counts per world
num_body_dofs_np = num_bodies_np * 6
# Compute joint coord/DoF/constraint counts per world
num_passive_joints_np = np.zeros((model.world_count,), dtype=int)
num_actuated_joints_np = np.zeros((model.world_count,), dtype=int)
num_dynamic_joints_np = np.zeros((model.world_count,), dtype=int)
num_joint_coords_np = np.zeros((model.world_count,), dtype=int)
num_joint_dofs_np = np.zeros((model.world_count,), dtype=int)
num_joint_passive_coords_np = np.zeros((model.world_count,), dtype=int)
num_joint_passive_dofs_np = np.zeros((model.world_count,), dtype=int)
num_joint_actuated_coords_np = np.zeros((model.world_count,), dtype=int)
num_joint_actuated_dofs_np = np.zeros((model.world_count,), dtype=int)
num_joint_cts_np = np.zeros((model.world_count,), dtype=int)
num_joint_dynamic_cts_np = np.zeros((model.world_count,), dtype=int)
num_joint_kinematic_cts_np = np.zeros((model.world_count,), dtype=int)
# TODO
joint_dof_type_np = np.zeros((model.joint_count,), dtype=int)
joint_act_type_np = np.zeros((model.joint_count,), dtype=int)
joint_B_r_Bj_np = np.zeros((model.joint_count, 3), dtype=float)
joint_F_r_Fj_np = np.zeros((model.joint_count, 3), dtype=float)
joint_X_j_np = np.zeros((model.joint_count, 9), dtype=float)
joint_num_coords_np = np.zeros((model.joint_count,), dtype=int)
joint_num_dofs_np = np.zeros((model.joint_count,), dtype=int)
joint_num_cts_np = np.zeros((model.joint_count,), dtype=int)
joint_num_dynamic_cts_np = np.zeros((model.joint_count,), dtype=int)
joint_num_kinematic_cts_np = np.zeros((model.joint_count,), dtype=int)
joint_coord_start_np = np.zeros((model.joint_count,), dtype=int)
joint_dofs_start_np = np.zeros((model.joint_count,), dtype=int)
joint_actuated_coord_start_np = np.zeros((model.joint_count,), dtype=int)
joint_actuated_dofs_start_np = np.zeros((model.joint_count,), dtype=int)
joint_passive_coord_start_np = np.zeros((model.joint_count,), dtype=int)
joint_passive_dofs_start_np = np.zeros((model.joint_count,), dtype=int)
joint_cts_start_np = np.zeros((model.joint_count,), dtype=int)
joint_dynamic_cts_start_np = np.zeros((model.joint_count,), dtype=int)
joint_kinematic_cts_start_np = np.zeros((model.joint_count,), dtype=int)
# Unpack converted quantities
body_q_np = converted["body_q"]
body_qd_np = converted["body_qd"]
body_com_np = converted["body_com"]
body_inertia_np = converted["body_inertia"]
body_inv_inertia_np = converted["body_inv_inertia"]
shape_transform_np = converted["shape_transform"]
joint_X_p_np = converted["joint_X_p"]
joint_X_c_np = converted["joint_X_c"]
# TODO
joint_wid_np: np.ndarray = model.joint_world.numpy().copy()
joint_type_np: np.ndarray = model.joint_type.numpy().copy()
joint_target_mode_np: np.ndarray = model.joint_target_mode.numpy().copy()
joint_parent_np: np.ndarray = model.joint_parent.numpy().copy()
joint_child_np: np.ndarray = model.joint_child.numpy().copy()
joint_axis_np: np.ndarray = model.joint_axis.numpy().copy()
joint_dof_dim_np: np.ndarray = model.joint_dof_dim.numpy().copy()
joint_q_start_np: np.ndarray = model.joint_q_start.numpy().copy()
joint_qd_start_np: np.ndarray = model.joint_qd_start.numpy().copy()
joint_limit_lower_np: np.ndarray = model.joint_limit_lower.numpy().copy()
joint_limit_upper_np: np.ndarray = model.joint_limit_upper.numpy().copy()
joint_velocity_limit_np = model.joint_velocity_limit.numpy().copy()
joint_effort_limit_np = model.joint_effort_limit.numpy().copy()
joint_armature_np: np.ndarray = model.joint_armature.numpy().copy()
joint_friction_np: np.ndarray = model.joint_friction.numpy().copy()
joint_target_ke_np: np.ndarray = model.joint_target_ke.numpy().copy()
joint_target_kd_np: np.ndarray = model.joint_target_kd.numpy().copy()
for j in range(model.joint_count):
# TODO
wid_j = joint_wid_np[j]
# TODO
joint_coord_start_np[j] = num_joint_coords_np[wid_j]
joint_dofs_start_np[j] = num_joint_dofs_np[wid_j]
joint_actuated_coord_start_np[j] = num_joint_actuated_coords_np[wid_j]
joint_actuated_dofs_start_np[j] = num_joint_actuated_dofs_np[wid_j]
joint_passive_coord_start_np[j] = num_joint_passive_coords_np[wid_j]
joint_passive_dofs_start_np[j] = num_joint_passive_dofs_np[wid_j]
joint_cts_start_np[j] = num_joint_cts_np[wid_j]
joint_dynamic_cts_start_np[j] = num_joint_dynamic_cts_np[wid_j]
joint_kinematic_cts_start_np[j] = num_joint_kinematic_cts_np[wid_j]
# TODO
type_j = int(joint_type_np[j])
dof_dim_j = (int(joint_dof_dim_np[j][0]), int(joint_dof_dim_np[j][1]))
q_count_j = int(joint_q_start_np[j + 1] - joint_q_start_np[j])
qd_count_j = int(joint_qd_start_np[j + 1] - joint_qd_start_np[j])
limit_upper_j = joint_limit_upper_np[joint_qd_start_np[j] : joint_qd_start_np[j + 1]].astype(float)
limit_lower_j = joint_limit_lower_np[joint_qd_start_np[j] : joint_qd_start_np[j + 1]].astype(float)
dof_type_j = JointDoFType.from_newton(
JointType(type_j), q_count_j, qd_count_j, dof_dim_j, limit_lower_j, limit_upper_j
)
# TODO
ncoords_j = dof_type_j.num_coords
ndofs_j = dof_type_j.num_dofs
ncts_j = dof_type_j.num_cts
# TODO
joint_dof_type_np[j] = dof_type_j.value
num_joint_coords_np[wid_j] += ncoords_j
num_joint_dofs_np[wid_j] += ndofs_j
joint_num_coords_np[j] = ncoords_j
joint_num_dofs_np[j] = ndofs_j
# TODO
dofs_start_j = joint_qd_start_np[j]
dof_axes_j = joint_axis_np[dofs_start_j : dofs_start_j + ndofs_j]
joint_dofs_target_mode_j = joint_target_mode_np[dofs_start_j : dofs_start_j + ndofs_j]
act_type_j = JointActuationType.from_newton(
JointTargetMode(max(joint_dofs_target_mode_j) if len(joint_dofs_target_mode_j) > 0 else 0)
)
joint_act_type_np[j] = act_type_j.value
# Infer if the joint requires dynamic constraints
is_dynamic_j = False
if ndofs_j > 0:
a_j = joint_armature_np[joint_qd_start_np[j] : joint_qd_start_np[j + 1]]
b_j = joint_friction_np[joint_qd_start_np[j] : joint_qd_start_np[j + 1]]
ke_j = joint_target_ke_np[joint_qd_start_np[j] : joint_qd_start_np[j + 1]]
kd_j = joint_target_kd_np[joint_qd_start_np[j] : joint_qd_start_np[j + 1]]
a_j_min = float(a_j.min())
b_j_min = float(b_j.min())
ke_j_min = float(ke_j.min())
kd_j_min = float(kd_j.min())
a_j_max = float(a_j.max())
b_j_max = float(b_j.max())
ke_j_max = float(ke_j.max())
kd_j_max = float(kd_j.max())
if (a_j_min < 0.0) or (b_j_min < 0.0) or (ke_j_min < 0.0) or (kd_j_min < 0.0):
raise ValueError(
f"Joint {j} in world {wid_j} has negative armature, friction "
"or target stiffness/damping values, which is not supported."
)
if (a_j_min < a_j_max) or (b_j_min < b_j_max) or (ke_j_min < ke_j_max) or (kd_j_min < kd_j_max):
raise ValueError(
f"Joint {j} in world {wid_j} has non-constant armature, friction "
"or target stiffness/damping values, which is not supported."
)
is_dynamic_j = (a_j_max > 0.0) or (b_j_max > 0.0) or (ke_j_max > 0.0) or (kd_j_max > 0.0)
# TODO
if is_dynamic_j:
joint_num_dynamic_cts_np[j] = ndofs_j
joint_dynamic_cts_start_np[j] = num_joint_dynamic_cts_np[wid_j]
num_joint_dynamic_cts_np[wid_j] += ndofs_j
num_joint_cts_np[wid_j] += ndofs_j
num_dynamic_joints_np[wid_j] += 1
else:
joint_dynamic_cts_start_np[j] = -1
# TODO
num_joint_cts_np[wid_j] += ncts_j
num_joint_kinematic_cts_np[wid_j] += ncts_j
if act_type_j > JointActuationType.PASSIVE:
num_actuated_joints_np[wid_j] += 1
num_joint_actuated_coords_np[wid_j] += ncoords_j
num_joint_actuated_dofs_np[wid_j] += ndofs_j
joint_passive_coord_start_np[j] = -1
joint_passive_dofs_start_np[j] = -1
else:
num_passive_joints_np[wid_j] += 1
num_joint_passive_coords_np[wid_j] += ncoords_j
num_joint_passive_dofs_np[wid_j] += ndofs_j
joint_actuated_coord_start_np[j] = -1
joint_actuated_dofs_start_np[j] = -1
joint_num_kinematic_cts_np[j] = ncts_j
joint_num_cts_np[j] = joint_num_dynamic_cts_np[j] + joint_num_kinematic_cts_np[j]
# TODO
parent_bid = joint_parent_np[j]
p_r_p_com = vec3f(body_com_np[parent_bid]) if parent_bid >= 0 else vec3f(0.0, 0.0, 0.0)
c_r_c_com = vec3f(body_com_np[joint_child_np[j]])
X_p_j = transformf(*joint_X_p_np[j, :])
X_c_j = transformf(*joint_X_c_np[j, :])
q_p_j = wp.transform_get_rotation(X_p_j)
p_r_p_j = wp.transform_get_translation(X_p_j)
c_r_c_j = wp.transform_get_translation(X_c_j)
# TODO
R_axis_j = JointDoFType.axes_matrix_from_joint_type(dof_type_j, dof_dim_j, dof_axes_j)
B_r_Bj = p_r_p_j - p_r_p_com
F_r_Fj = c_r_c_j - c_r_c_com
X_j = wp.quat_to_matrix(q_p_j) @ R_axis_j
joint_B_r_Bj_np[j, :] = B_r_Bj
joint_F_r_Fj_np[j, :] = F_r_Fj
joint_X_j_np[j, :] = X_j
# Convert joint limits and effort/velocity limits to np.float32 and clip to supported ranges
np.clip(a=joint_limit_lower_np, a_min=JOINT_QMIN, a_max=JOINT_QMAX, out=joint_limit_lower_np)
np.clip(a=joint_limit_upper_np, a_min=JOINT_QMIN, a_max=JOINT_QMAX, out=joint_limit_upper_np)
np.clip(a=joint_velocity_limit_np, a_min=-JOINT_DQMAX, a_max=JOINT_DQMAX, out=joint_velocity_limit_np)
np.clip(a=joint_effort_limit_np, a_min=-JOINT_TAUMAX, a_max=JOINT_TAUMAX, out=joint_effort_limit_np)
# Set up materials
materials_manager = MaterialManager()
default_material = materials_manager.materials[0]
shape_friction_np = model.shape_material_mu.numpy().tolist()
shape_restitution_np = model.shape_material_restitution.numpy().tolist()
geom_material_np = np.zeros((model.shape_count,), dtype=int)
# TODO: Integrate world index for shape material
# shape_world_np = model.shape_world.numpy()
material_param_indices: dict[tuple[float, float], int] = {}
# Adding default material from material manager, making sure the values undergo the same
# transformation as any material parameters in the Newton model (conversion to np.float32)
default_mu = float(np.float32(default_material.static_friction))
default_restitution = float(np.float32(default_material.restitution))
material_param_indices[(default_mu, default_restitution)] = 0
for s in range(model.shape_count):
# Check if material with these parameters already exists
material_desc = (shape_friction_np[s], shape_restitution_np[s])
if material_desc in material_param_indices:
material_id = material_param_indices[material_desc]
else:
material = MaterialDescriptor(
name=f"{model.shape_label[s]}_material",
restitution=shape_restitution_np[s],
static_friction=shape_friction_np[s],
dynamic_friction=shape_friction_np[s],
# wid=shape_world_np[s],
)
material_id = materials_manager.register(material)
material_param_indices[material_desc] = material_id
geom_material_np[s] = material_id
# Store Newton shape type and scale directly (no Kamino conversion needed).
# The geoms.type array stores GeoType values, and geoms.params stores
# Newton's shape_scale convention (half-extents, half-heights, etc.)
# padded to vec4f. Plane shapes are special: params store the plane
# normal (derived from the shape transform) instead of scale.
shape_type_np = model.shape_type.numpy()
shape_scale_np = model.shape_scale.numpy()
shape_flags_np = model.shape_flags.numpy()
geom_shape_collision_group_np = model.shape_collision_group.numpy()
geom_shape_type_np = np.array(shape_type_np, dtype=int)
geom_shape_params_np = np.zeros((model.shape_count, 4), dtype=float)
model_num_collidable_geoms = 0
for s in range(model.shape_count):
scale = shape_scale_np[s]
geom_shape_params_np[s, 0] = scale[0]
geom_shape_params_np[s, 1] = scale[1]
geom_shape_params_np[s, 2] = scale[2]
if (shape_flags_np[s] & ShapeFlags.COLLIDE_SHAPES) != 0 and geom_shape_collision_group_np[s] > 0:
model_num_collidable_geoms += 1
else:
geom_material_np[s] = -1 # Ensure non-collidable geoms no material assigned
# Plane normals: derive from the shape transform rotation (local Z-axis)
# and store in params since Newton's shape_scale doesn't encode plane normals.
for s in range(model.shape_count):
if shape_type_np[s] == GeoType.PLANE:
tf = shape_transform_np[s, :]
q_rot = _to_wpq(np.array([tf[3], tf[4], tf[5], tf[6]]))
normal = wp.quat_rotate(q_rot, vec3f(0.0, 0.0, 1.0))
geom_shape_params_np[s, 0] = float(normal[0])
geom_shape_params_np[s, 1] = float(normal[1])
geom_shape_params_np[s, 2] = float(normal[2])
geom_shape_params_np[s, 3] = 0.0
# Compute total number of required contacts per world
if model.rigid_contact_max > 0:
model_min_contacts = int(model.rigid_contact_max)
min_contacts_per_world = model.rigid_contact_max // model.world_count
world_min_contacts = [min_contacts_per_world] * model.world_count
else:
model_min_contacts, world_min_contacts = compute_required_contact_capacity(model)
# Compute offsets per world
world_shape_offset_np = np.zeros((model.world_count,), dtype=int)
world_body_offset_np = np.zeros((model.world_count,), dtype=int)
world_body_dof_offset_np = np.zeros((model.world_count,), dtype=int)
world_joint_offset_np = np.zeros((model.world_count,), dtype=int)
world_joint_coord_offset_np = np.zeros((model.world_count,), dtype=int)
world_joint_dof_offset_np = np.zeros((model.world_count,), dtype=int)
world_actuated_joint_coord_offset_np = np.zeros((model.world_count,), dtype=int)
world_actuated_joint_dofs_offset_np = np.zeros((model.world_count,), dtype=int)
world_passive_joint_coord_offset_np = np.zeros((model.world_count,), dtype=int)
world_passive_joint_dofs_offset_np = np.zeros((model.world_count,), dtype=int)
world_joint_cts_offset_np = np.zeros((model.world_count,), dtype=int)
world_joint_dynamic_cts_offset_np = np.zeros((model.world_count,), dtype=int)
world_joint_kinematic_cts_offset_np = np.zeros((model.world_count,), dtype=int)
for w in range(1, model.world_count):
world_shape_offset_np[w] = world_shape_offset_np[w - 1] + num_shapes_np[w - 1]
world_body_offset_np[w] = world_body_offset_np[w - 1] + num_bodies_np[w - 1]
world_body_dof_offset_np[w] = world_body_dof_offset_np[w - 1] + num_body_dofs_np[w - 1]
world_joint_offset_np[w] = world_joint_offset_np[w - 1] + num_joints_np[w - 1]
world_joint_coord_offset_np[w] = world_joint_coord_offset_np[w - 1] + num_joint_coords_np[w - 1]
world_joint_dof_offset_np[w] = world_joint_dof_offset_np[w - 1] + num_joint_dofs_np[w - 1]
world_actuated_joint_coord_offset_np[w] = (
world_actuated_joint_coord_offset_np[w - 1] + num_joint_actuated_coords_np[w - 1]
)
world_actuated_joint_dofs_offset_np[w] = (
world_actuated_joint_dofs_offset_np[w - 1] + num_joint_actuated_dofs_np[w - 1]
)
world_passive_joint_coord_offset_np[w] = (
world_passive_joint_coord_offset_np[w - 1] + num_joint_passive_coords_np[w - 1]
)
world_passive_joint_dofs_offset_np[w] = (
world_passive_joint_dofs_offset_np[w - 1] + num_joint_passive_dofs_np[w - 1]
)
world_joint_cts_offset_np[w] = world_joint_cts_offset_np[w - 1] + num_joint_cts_np[w - 1]
world_joint_dynamic_cts_offset_np[w] = (
world_joint_dynamic_cts_offset_np[w - 1] + num_joint_dynamic_cts_np[w - 1]
)
world_joint_kinematic_cts_offset_np[w] = (
world_joint_kinematic_cts_offset_np[w - 1] + num_joint_kinematic_cts_np[w - 1]
)
# Determine the base body and joint indices per world
base_body_idx_np = np.full((model.world_count,), -1, dtype=int)
base_joint_idx_np = np.full((model.world_count,), -1, dtype=int)
body_world_np = model.body_world.numpy()
joint_world_np = model.joint_world.numpy()
body_world_start_np = model.body_world_start.numpy()
# Check for articulations
if model.articulation_count > 0:
articulation_start_np = model.articulation_start.numpy()
articulation_world_np = model.articulation_world.numpy()
# For each articulation, assign its base body and joint to the corresponding world
# NOTE: We only assign the first articulation found in each world
for aid in range(model.articulation_count):
wid = articulation_world_np[aid]
base_joint = articulation_start_np[aid]
base_body = joint_child_np[base_joint]
if base_body_idx_np[wid] == -1 and base_joint_idx_np[wid] == -1:
base_body_idx_np[wid] = base_body
base_joint_idx_np[wid] = base_joint
# Check for root joint (i.e. joint with no parent body (= -1))
elif model.joint_count > 0:
# TODO: How to handle no free joint being defined?
# Create a list of joint indices with parent body == -1 for each world
world_parent_joints: dict[int, list[int]] = {w: [] for w in range(model.world_count)}
for j in range(model.joint_count):
wid_j = joint_world_np[j]
parent_j = joint_parent_np[j]
if parent_j == -1:
world_parent_joints[wid_j].append(j)
# For each world, assign the base body and joint based on the first joint with parent == -1,
# If no joint with parent == -1 is found in a world, then assign the first body as base
# If multiple joints with parent == -1 are found in a world, then assign the first one as the base
for w in range(model.world_count):
if len(world_parent_joints[w]) > 0:
j = world_parent_joints[w][0]
base_joint_idx_np[w] = j
base_body_idx_np[w] = int(joint_child_np[j])
else:
base_body_idx_np[w] = int(body_world_start_np[w])
base_joint_idx_np[w] = -1
# Fall-back: first body and joint in the world
else:
for w in range(model.world_count):
# Base body: first body in the world
for b in range(model.body_count):
if body_world_np[b] == w:
base_body_idx_np[w] = b
break
# Base joint: first joint in the world
for j in range(model.joint_count):
if joint_world_np[j] == w:
base_joint_idx_np[w] = j
break
# Ensure that all worlds have a base body assigned
for w in range(model.world_count):
if base_body_idx_np[w] == -1:
raise ValueError(f"World {w} does not have a base body assigned (index is -1).")
# Construct per-world inertial summaries
mass_min_np = np.zeros((model.world_count,), dtype=float)
mass_max_np = np.zeros((model.world_count,), dtype=float)
mass_total_np = np.zeros((model.world_count,), dtype=float)
inertia_total_np = np.zeros((model.world_count,), dtype=float)
body_mass_np = model.body_mass.numpy()
for w in range(model.world_count):
masses_w = []
for b in range(model.body_count):
if body_world_np[b] == w:
mass_b = body_mass_np[b]
masses_w.append(mass_b)
mass_total_np[w] += mass_b
inertia_total_np[w] += 3.0 * mass_b + body_inertia_np[b].diagonal().sum()
mass_min_np[w] = min(masses_w)
mass_max_np[w] = max(masses_w)
# Construct the per-material and per-material-pair properties
materials_rest = [materials_manager.restitution_vector()]
materials_static_fric = [materials_manager.static_friction_vector()]
materials_dynamic_fric = [materials_manager.dynamic_friction_vector()]
mpairs_rest = [materials_manager.restitution_matrix()]
mpairs_static_fric = [materials_manager.static_friction_matrix()]
mpairs_dynamic_fric = [materials_manager.dynamic_friction_matrix()]
# model.body_q stores body-origin world poses, but Kamino expects
# COM world poses (joint attachment vectors are COM-relative).
q_i_0_np = np.empty((model.body_count, 7), dtype=np.float32)
for i in range(model.body_count):
pos = body_q_np[i, :3]
rot = wp.quatf(*body_q_np[i, 3:7])
com_world = pos + np.array(wp.quat_rotate(rot, wp.vec3f(*body_com_np[i])))
q_i_0_np[i, :3] = com_world
q_i_0_np[i, 3:7] = body_q_np[i, 3:7]
###
# Model Attributes
###
# Construct SizeKamino from the newton.Model instance
model_size = SizeKamino(
num_worlds=model.world_count,
sum_of_num_bodies=int(num_bodies_np.sum()),
max_of_num_bodies=int(num_bodies_np.max()),
sum_of_num_joints=int(num_joints_np.sum()),
max_of_num_joints=int(num_joints_np.max()),
sum_of_num_passive_joints=int(num_passive_joints_np.sum()),
max_of_num_passive_joints=int(num_passive_joints_np.max()),
sum_of_num_actuated_joints=int(num_actuated_joints_np.sum()),
max_of_num_actuated_joints=int(num_actuated_joints_np.max()),
sum_of_num_dynamic_joints=int(num_dynamic_joints_np.sum()),
max_of_num_dynamic_joints=int(num_dynamic_joints_np.max()),
sum_of_num_geoms=int(num_shapes_np.sum()),
max_of_num_geoms=int(num_shapes_np.max()),
sum_of_num_materials=materials_manager.num_materials,
max_of_num_materials=materials_manager.num_materials,
sum_of_num_material_pairs=materials_manager.num_material_pairs,
max_of_num_material_pairs=materials_manager.num_material_pairs,
sum_of_num_body_dofs=int(num_body_dofs_np.sum()),
max_of_num_body_dofs=int(num_body_dofs_np.max()),
sum_of_num_joint_coords=int(num_joint_coords_np.sum()),
max_of_num_joint_coords=int(num_joint_coords_np.max()),
sum_of_num_joint_dofs=int(num_joint_dofs_np.sum()),
max_of_num_joint_dofs=int(num_joint_dofs_np.max()),
sum_of_num_passive_joint_coords=int(num_joint_passive_coords_np.sum()),
max_of_num_passive_joint_coords=int(num_joint_passive_coords_np.max()),
sum_of_num_passive_joint_dofs=int(num_joint_passive_dofs_np.sum()),
max_of_num_passive_joint_dofs=int(num_joint_passive_dofs_np.max()),
sum_of_num_actuated_joint_coords=int(num_joint_actuated_coords_np.sum()),
max_of_num_actuated_joint_coords=int(num_joint_actuated_coords_np.max()),
sum_of_num_actuated_joint_dofs=int(num_joint_actuated_dofs_np.sum()),
max_of_num_actuated_joint_dofs=int(num_joint_actuated_dofs_np.max()),
sum_of_num_joint_cts=int(num_joint_cts_np.sum()),
max_of_num_joint_cts=int(num_joint_cts_np.max()),
sum_of_num_dynamic_joint_cts=int(num_joint_dynamic_cts_np.sum()),
max_of_num_dynamic_joint_cts=int(num_joint_dynamic_cts_np.max()),
sum_of_num_kinematic_joint_cts=int(num_joint_kinematic_cts_np.sum()),
max_of_num_kinematic_joint_cts=int(num_joint_kinematic_cts_np.max()),
sum_of_max_total_cts=int(num_joint_cts_np.sum()),
max_of_max_total_cts=int(num_joint_cts_np.max()),
)
# Construct the model entities from the newton.Model instance
with wp.ScopedDevice(device=model.device):
# Per-world heterogeneous model info
model_info = ModelKaminoInfo(
num_worlds=model.world_count,
num_bodies=wp.array(num_bodies_np, dtype=int32),
num_joints=wp.array(num_joints_np, dtype=int32),
num_passive_joints=wp.array(num_passive_joints_np, dtype=int32),
num_actuated_joints=wp.array(num_actuated_joints_np, dtype=int32),
num_dynamic_joints=wp.array(num_dynamic_joints_np, dtype=int32),
num_geoms=wp.array(num_shapes_np, dtype=int32),
num_body_dofs=wp.array(num_body_dofs_np, dtype=int32),
num_joint_coords=wp.array(num_joint_coords_np, dtype=int32),
num_joint_dofs=wp.array(num_joint_dofs_np, dtype=int32),
num_passive_joint_coords=wp.array(num_joint_passive_coords_np, dtype=int32),
num_passive_joint_dofs=wp.array(num_joint_passive_dofs_np, dtype=int32),
num_actuated_joint_coords=wp.array(num_joint_actuated_coords_np, dtype=int32),
num_actuated_joint_dofs=wp.array(num_joint_actuated_dofs_np, dtype=int32),
num_joint_cts=wp.array(num_joint_cts_np, dtype=int32),
num_joint_dynamic_cts=wp.array(num_joint_dynamic_cts_np, dtype=int32),
num_joint_kinematic_cts=wp.array(num_joint_kinematic_cts_np, dtype=int32),
bodies_offset=wp.array(world_body_offset_np, dtype=int32),
joints_offset=wp.array(world_joint_offset_np, dtype=int32),
geoms_offset=wp.array(world_shape_offset_np, dtype=int32),
body_dofs_offset=wp.array(world_body_dof_offset_np, dtype=int32),
joint_coords_offset=wp.array(world_joint_coord_offset_np, dtype=int32),
joint_dofs_offset=wp.array(world_joint_dof_offset_np, dtype=int32),
joint_passive_coords_offset=wp.array(world_passive_joint_coord_offset_np, dtype=int32),
joint_passive_dofs_offset=wp.array(world_passive_joint_dofs_offset_np, dtype=int32),
joint_actuated_coords_offset=wp.array(world_actuated_joint_coord_offset_np, dtype=int32),
joint_actuated_dofs_offset=wp.array(world_actuated_joint_dofs_offset_np, dtype=int32),
joint_cts_offset=wp.array(world_joint_cts_offset_np, dtype=int32),
joint_dynamic_cts_offset=wp.array(world_joint_dynamic_cts_offset_np, dtype=int32),
joint_kinematic_cts_offset=wp.array(world_joint_kinematic_cts_offset_np, dtype=int32),
base_body_index=wp.array(base_body_idx_np, dtype=int32),
base_joint_index=wp.array(base_joint_idx_np, dtype=int32),
mass_min=wp.array(mass_min_np, dtype=float32),
mass_max=wp.array(mass_max_np, dtype=float32),
mass_total=wp.array(mass_total_np, dtype=float32),
inertia_total=wp.array(inertia_total_np, dtype=float32),
)
# Per-world time
model_time = TimeModel(
dt=wp.zeros(shape=(model.world_count,), dtype=float32),
inv_dt=wp.zeros(shape=(model.world_count,), dtype=float32),
)
# Per-world gravity
model_gravity = GravityModel.from_newton(model)
# Bodies
model_bodies = RigidBodiesModel(
num_bodies=model.body_count,
label=model.body_label,
wid=model.body_world,
bid=wp.array(body_bid_np, dtype=int32), # TODO: Remove
m_i=model.body_mass,
inv_m_i=model.body_inv_mass,
i_r_com_i=wp.array(body_com_np, dtype=vec3f),
i_I_i=wp.array(body_inertia_np, dtype=mat33f),
inv_i_I_i=wp.array(body_inv_inertia_np, dtype=mat33f),
q_i_0=wp.array(q_i_0_np, dtype=wp.transformf),
u_i_0=wp.array(body_qd_np, dtype=vec6f),
)
# Joints
model_joints = JointsModel(
num_joints=model.joint_count,
label=model.joint_label,
wid=model.joint_world,
jid=wp.array(joint_jid_np, dtype=int32), # TODO: Remove
dof_type=wp.array(joint_dof_type_np, dtype=int32),
act_type=wp.array(joint_act_type_np, dtype=int32),
bid_B=model.joint_parent,
bid_F=model.joint_child,
B_r_Bj=wp.array(joint_B_r_Bj_np, dtype=wp.vec3f),
F_r_Fj=wp.array(joint_F_r_Fj_np, dtype=wp.vec3f),
X_j=wp.array(joint_X_j_np.reshape((model.joint_count, 3, 3)), dtype=wp.mat33f),
q_j_min=wp.array(joint_limit_lower_np, dtype=float32),
q_j_max=wp.array(joint_limit_upper_np, dtype=float32),
dq_j_max=wp.array(joint_velocity_limit_np, dtype=float32),
tau_j_max=wp.array(joint_effort_limit_np, dtype=float32),
a_j=model.joint_armature,
b_j=model.joint_friction, # TODO: Is this the right attribute?
k_p_j=model.joint_target_ke,
k_d_j=model.joint_target_kd,
q_j_0=model.joint_q,
dq_j_0=model.joint_qd,
num_coords=wp.array(joint_num_coords_np, dtype=int32),
num_dofs=wp.array(joint_num_dofs_np, dtype=int32),
num_cts=wp.array(joint_num_cts_np, dtype=int32),
num_dynamic_cts=wp.array(joint_num_dynamic_cts_np, dtype=int32),
num_kinematic_cts=wp.array(joint_num_kinematic_cts_np, dtype=int32),
coords_offset=wp.array(joint_coord_start_np, dtype=int32),
dofs_offset=wp.array(joint_dofs_start_np, dtype=int32),
passive_coords_offset=wp.array(joint_passive_coord_start_np, dtype=int32),
passive_dofs_offset=wp.array(joint_passive_dofs_start_np, dtype=int32),
actuated_coords_offset=wp.array(joint_actuated_coord_start_np, dtype=int32),
actuated_dofs_offset=wp.array(joint_actuated_dofs_start_np, dtype=int32),
cts_offset=wp.array(joint_cts_start_np, dtype=int32),
dynamic_cts_offset=wp.array(joint_dynamic_cts_start_np, dtype=int32),
kinematic_cts_offset=wp.array(joint_kinematic_cts_start_np, dtype=int32),
)
# Geometries
model_geoms = GeometriesModel(
num_geoms=model.shape_count,
num_collidable=model_num_collidable_geoms,
num_collidable_pairs=model.shape_contact_pair_count,
num_excluded_pairs=len(model.shape_collision_filter_pairs),
model_minimum_contacts=model_min_contacts,
world_minimum_contacts=world_min_contacts,
label=model.shape_label,
wid=model.shape_world,
gid=wp.array(shape_sid_np, dtype=int32), # TODO: Remove
bid=model.shape_body,
type=wp.array(geom_shape_type_np, dtype=int32),
flags=model.shape_flags,
ptr=model.shape_source_ptr,
params=wp.array(geom_shape_params_np, dtype=vec4f),
offset=wp.zeros_like(model.shape_transform),
material=wp.array(geom_material_np, dtype=int32),
group=model.shape_collision_group,
gap=model.shape_gap,
margin=model.shape_margin,
collidable_pairs=model.shape_contact_pairs,
excluded_pairs=wp.array(sorted(model.shape_collision_filter_pairs), dtype=vec2i),
)
# Per-material properties
model_materials = MaterialsModel(
num_materials=model_size.sum_of_num_materials,
restitution=wp.array(materials_rest[0], dtype=float32),
static_friction=wp.array(materials_static_fric[0], dtype=float32),
dynamic_friction=wp.array(materials_dynamic_fric[0], dtype=float32),
)
# Per-material-pair properties
model_material_pairs = MaterialPairsModel(
num_material_pairs=model_size.sum_of_num_material_pairs,
restitution=wp.array(mpairs_rest[0], dtype=float32),
static_friction=wp.array(mpairs_static_fric[0], dtype=float32),
dynamic_friction=wp.array(mpairs_dynamic_fric[0], dtype=float32),
)
###
# Post-processing
###
# Modify the model's body COM and shape transform properties in-place to convert from body-frame-relative
# NOTE: These are modified only so that the visualizer correctly
# shows the shape poses, joints frames and body inertial properties
model.body_com.assign(body_com_np)
model.body_inertia.assign(body_inertia_np)
model.shape_transform.assign(shape_transform_np)
model.joint_X_p.assign(joint_X_p_np)
model.joint_X_c.assign(joint_X_c_np)
# Convert shape offsets from body-frame-relative to COM-relative
convert_geom_offset_origin_to_com(
model_bodies.i_r_com_i,
model.shape_body,
wp.array(shape_transform_np, dtype=wp.transformf, device=model.device),
model_geoms.offset,
)
# Construct and return the new ModelKamino instance
return ModelKamino(
_model=model,
_device=model.device,
_requires_grad=model.requires_grad,
size=model_size,
info=model_info,
time=model_time,
gravity=model_gravity,
bodies=model_bodies,
joints=model_joints,
geoms=model_geoms,
materials=model_materials,
material_pairs=model_material_pairs,
)
================================================
FILE: newton/_src/solvers/kamino/_src/core/shapes.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Shape Types & Containers"""
from __future__ import annotations
from abc import ABC, abstractmethod
from collections.abc import Sequence
import numpy as np
import warp as wp
from .....core.types import Vec2, Vec3
from .....geometry.types import GeoType, Heightfield, Mesh
from .types import Descriptor, override, vec4f
###
# Module interface
###
__all__ = [
"BoxShape",
"CapsuleShape",
"ConeShape",
"CylinderShape",
"EllipsoidShape",
"EmptyShape",
"GeoType",
"MeshShape",
"PlaneShape",
"ShapeDescriptor",
"SphereShape",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Containers
###
ShapeDataLike = None | Mesh | Heightfield
"""A type union that can represent any shape data, including None, Mesh, and Heightfield."""
def is_primitive_geo_type(geo_type: GeoType) -> bool:
"""Returns whether the geo type is a primitive shape."""
return geo_type in {
GeoType.SPHERE,
GeoType.CYLINDER,
GeoType.CONE,
GeoType.CAPSULE,
GeoType.BOX,
GeoType.ELLIPSOID,
GeoType.PLANE,
}
def is_explicit_geo_type(geo_type: GeoType) -> bool:
"""Returns whether the geo type is an explicit shape (mesh, convex, heightfield)."""
return geo_type in {
GeoType.MESH,
GeoType.CONVEX_MESH,
GeoType.HFIELD,
}
class ShapeDescriptor(ABC, Descriptor):
"""Abstract base class for all shape descriptors."""
def __init__(self, geo_type: GeoType, name: str = "", uid: str | None = None):
"""
Initialize the shape descriptor.
Args:
geo_type: The geometry type from Newton's :class:`GeoType`.
name: The name of the shape descriptor.
uid: Optional unique identifier of the shape descriptor.
"""
super().__init__(name, uid)
self._type: GeoType = geo_type
@override
def __hash__(self) -> int:
"""Returns a hash of the ShapeDescriptor based on its name, uid, type and params."""
return hash((self.type, self.params))
@override
def __repr__(self):
"""Returns a human-readable string representation of the ShapeDescriptor."""
return f"ShapeDescriptor(\ntype: {self.type},\nname: {self.name},\nuid: {self.uid},\n)"
@property
def type(self) -> GeoType:
"""Returns the geometry type of the shape."""
return self._type
@property
def is_solid(self) -> bool:
"""Returns whether the shape is solid (i.e., not empty)."""
return self._type != GeoType.NONE
@property
@abstractmethod
def paramsvec(self) -> vec4f:
return vec4f(0.0)
@property
@abstractmethod
def params(self) -> ShapeParamsLike:
return None
@property
@abstractmethod
def data(self) -> ShapeDataLike:
return None
###
# Primitive Shapes
###
class EmptyShape(ShapeDescriptor):
"""
A shape descriptor for the empty shape that can serve as a placeholder.
"""
def __init__(self, name: str = "empty", uid: str | None = None):
super().__init__(GeoType.NONE, name, uid)
@override
def __repr__(self):
"""Returns a human-readable string representation of the EmptyShape."""
return f"EmptyShape(\nname: {self.name},\nuid: {self.uid}\n)"
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(0.0)
@property
@override
def params(self) -> ShapeParamsLike:
return None
@property
@override
def data(self) -> None:
return None
class SphereShape(ShapeDescriptor):
"""
A shape descriptor for spheres.
Attributes:
radius: The radius of the sphere [m].
"""
def __init__(self, radius: float, name: str = "sphere", uid: str | None = None):
super().__init__(GeoType.SPHERE, name, uid)
self.radius: float = radius
@override
def __repr__(self):
"""Returns a human-readable string representation of the SphereShape."""
return f"SphereShape(\nname: {self.name},\nuid: {self.uid},\nradius: {self.radius}\n)"
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(self.radius, 0.0, 0.0, 0.0)
@property
@override
def params(self) -> float:
return self.radius
@property
@override
def data(self) -> None:
return None
class CylinderShape(ShapeDescriptor):
"""
A shape descriptor for cylinders.
Attributes:
radius: The radius of the cylinder [m].
half_height: The half-height of the cylinder [m].
"""
def __init__(self, radius: float, half_height: float, name: str = "cylinder", uid: str | None = None):
super().__init__(GeoType.CYLINDER, name, uid)
self.radius: float = radius
self.half_height: float = half_height
@override
def __repr__(self):
"""Returns a human-readable string representation of the CylinderShape."""
return f"CylinderShape(\nname: {self.name},\nuid: {self.uid},\nradius: {self.radius},\nhalf_height: {self.half_height}\n)"
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(self.radius, self.half_height, 0.0, 0.0)
@property
@override
def params(self) -> tuple[float, float]:
return (self.radius, self.half_height)
@property
@override
def data(self) -> None:
return None
class ConeShape(ShapeDescriptor):
"""
A shape descriptor for cones.
Attributes:
radius: The radius of the cone [m].
half_height: The half-height of the cone [m].
"""
def __init__(self, radius: float, half_height: float, name: str = "cone", uid: str | None = None):
super().__init__(GeoType.CONE, name, uid)
self.radius: float = radius
self.half_height: float = half_height
@override
def __repr__(self):
"""Returns a human-readable string representation of the ConeShape."""
return f"ConeShape(\nname: {self.name},\nuid: {self.uid},\nradius: {self.radius},\nhalf_height: {self.half_height}\n)"
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(self.radius, self.half_height, 0.0, 0.0)
@property
@override
def params(self) -> tuple[float, float]:
return (self.radius, self.half_height)
@property
@override
def data(self) -> None:
return None
class CapsuleShape(ShapeDescriptor):
"""
A shape descriptor for capsules.
Attributes:
radius: The radius of the capsule [m].
half_height: The half-height of the capsule (cylindrical part) [m].
"""
def __init__(self, radius: float, half_height: float, name: str = "capsule", uid: str | None = None):
super().__init__(GeoType.CAPSULE, name, uid)
self.radius: float = radius
self.half_height: float = half_height
@override
def __repr__(self):
"""Returns a human-readable string representation of the CapsuleShape."""
return f"CapsuleShape(\nname: {self.name},\nuid: {self.uid},\nradius: {self.radius},\nhalf_height: {self.half_height}\n)"
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(self.radius, self.half_height, 0.0, 0.0)
@property
@override
def params(self) -> tuple[float, float]:
return (self.radius, self.half_height)
@property
@override
def data(self) -> None:
return None
class BoxShape(ShapeDescriptor):
"""
A shape descriptor for boxes.
Attributes:
hx: The half-extent along the local X-axis [m].
hy: The half-extent along the local Y-axis [m].
hz: The half-extent along the local Z-axis [m].
"""
def __init__(self, hx: float, hy: float, hz: float, name: str = "box", uid: str | None = None):
super().__init__(GeoType.BOX, name, uid)
self.hx: float = hx
self.hy: float = hy
self.hz: float = hz
@override
def __repr__(self):
"""Returns a human-readable string representation of the BoxShape."""
return f"BoxShape(\nname: {self.name},\nuid: {self.uid},\nhx: {self.hx},\nhy: {self.hy},\nhz: {self.hz}\n)"
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(self.hx, self.hy, self.hz, 0.0)
@property
@override
def params(self) -> tuple[float, float, float]:
return (self.hx, self.hy, self.hz)
@property
@override
def data(self) -> None:
return None
class EllipsoidShape(ShapeDescriptor):
"""
A shape descriptor for ellipsoids.
Attributes:
a (float): The semi-axis length along the X-axis.
b (float): The semi-axis length along the Y-axis.
c (float): The semi-axis length along the Z-axis.
"""
def __init__(self, a: float, b: float, c: float, name: str = "ellipsoid", uid: str | None = None):
super().__init__(GeoType.ELLIPSOID, name, uid)
self.a: float = a
self.b: float = b
self.c: float = c
@override
def __repr__(self):
"""Returns a human-readable string representation of the EllipsoidShape."""
return f"EllipsoidShape(\nname: {self.name},\nuid: {self.uid},\na: {self.a},\nb: {self.b},\nc: {self.c}\n)"
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(self.a, self.b, self.c, 0.0)
@property
@override
def params(self) -> tuple[float, float, float]:
return (self.a, self.b, self.c)
@property
@override
def data(self) -> None:
return None
class PlaneShape(ShapeDescriptor):
"""
A shape descriptor for planes.
Attributes:
normal (Vec3): The normal vector of the plane.
distance (float): The distance from the origin to the plane along its normal.
"""
def __init__(self, normal: Vec3, distance: float, name: str = "plane", uid: str | None = None):
super().__init__(GeoType.PLANE, name, uid)
self.normal: Vec3 = normal
self.distance: float = distance
@override
def __repr__(self):
"""Returns a human-readable string representation of the PlaneShape."""
return (
f"PlaneShape(\nname: {self.name},\nuid: {self.uid},\nnormal: {self.normal},\ndistance: {self.distance}\n)"
)
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(self.normal[0], self.normal[1], self.normal[2], self.distance)
@property
@override
def params(self) -> tuple[float, float, float, float]:
return (self.normal[0], self.normal[1], self.normal[2], self.distance)
@property
@override
def data(self) -> None:
return None
###
# Explicit Shapes
###
class MeshShape(ShapeDescriptor):
"""
A shape descriptor for mesh shapes.
This class is a lightweight wrapper around the newton.Mesh geometry type,
that provides the necessary interfacing to be used with the Kamino solver.
Attributes:
vertices (np.ndarray): The vertices of the mesh.
indices (np.ndarray): The triangle indices of the mesh.
normals (np.ndarray | None): The vertex normals of the mesh.
uvs (np.ndarray | None): The texture coordinates of the mesh.
color (Vec3 | None): The color of the mesh.
is_solid (bool): Whether the mesh is solid.
is_convex (bool): Whether the mesh is convex.
"""
MAX_HULL_VERTICES = Mesh.MAX_HULL_VERTICES
"""Utility attribute to expose this constant without needing to import the newton.Mesh class directly."""
def __init__(
self,
vertices: Sequence[Vec3] | np.ndarray,
indices: Sequence[int] | np.ndarray,
normals: Sequence[Vec3] | np.ndarray | None = None,
uvs: Sequence[Vec2] | np.ndarray | None = None,
color: Vec3 | None = None,
maxhullvert: int | None = None,
compute_inertia: bool = True,
is_solid: bool = True,
is_convex: bool = False,
name: str = "mesh",
uid: str | None = None,
):
"""
Initialize the mesh shape descriptor.
Args:
vertices (Sequence[Vec3] | np.ndarray): The vertices of the mesh.
indices (Sequence[int] | np.ndarray): The triangle indices of the mesh.
normals (Sequence[Vec3] | np.ndarray | None): The vertex normals of the mesh.
uvs (Sequence[Vec2] | np.ndarray | None): The texture coordinates of the mesh.
color (Vec3 | None): The color of the mesh.
maxhullvert (int): The maximum number of hull vertices for convex shapes.
compute_inertia (bool): Whether to compute inertia for the mesh.
is_solid (bool): Whether the mesh is solid.
is_convex (bool): Whether the mesh is convex.
name (str): The name of the shape descriptor.
uid (str | None): Optional unique identifier of the shape descriptor.
"""
# Determine the mesh shape type, and adapt default name if necessary
if is_convex:
geo_type = GeoType.CONVEX_MESH
name = "convex" if name == "mesh" else name
else:
geo_type = GeoType.MESH
# Initialize the base shape descriptor
super().__init__(geo_type, name, uid)
# Create the underlying mesh data container
self._data: Mesh = Mesh(
vertices=vertices,
indices=indices,
normals=normals,
uvs=uvs,
compute_inertia=compute_inertia,
is_solid=is_solid,
maxhullvert=maxhullvert,
color=color,
)
@override
def __hash__(self) -> int:
"""Returns a hash computed using the underlying newton.Mesh hash implementation."""
return self._data.__hash__()
@override
def __repr__(self):
"""Returns a human-readable string representation of the MeshShape."""
label = "ConvexShape" if self.type == GeoType.CONVEX_MESH else "MeshShape"
normals_shape = self._data._normals.shape if self._data._normals is not None else None
return (
f"{label}(\n"
f"name: {self.name},\n"
f"uid: {self.uid},\n"
f"vertices: {self._data.vertices.shape},\n"
f"indices: {self._data.indices.shape},\n"
f"normals: {normals_shape},\n"
f")"
)
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(1.0, 1.0, 1.0, 0.0)
@property
@override
def params(self) -> tuple[float, float, float]:
"""Returns the XYZ scaling of the mesh."""
return 1.0, 1.0, 1.0
@property
@override
def data(self) -> Mesh:
return self._data
@property
def vertices(self) -> np.ndarray:
"""Returns the vertices of the mesh."""
return self._data.vertices
@property
def indices(self) -> np.ndarray:
"""Returns the indices of the mesh."""
return self._data.indices
@property
def normals(self) -> np.ndarray | None:
"""Returns the normals of the mesh."""
return self._data._normals
@property
def uvs(self) -> np.ndarray | None:
"""Returns the UVs of the mesh."""
return self._data._uvs
@property
def color(self) -> Vec3 | None:
"""Returns the color of the mesh."""
return self._data._color
class HFieldShape(ShapeDescriptor):
"""
A shape descriptor for height-field shapes.
WARNING: This class is not yet implemented.
"""
def __init__(self, name: str = "hfield", uid: str | None = None):
super().__init__(GeoType.HFIELD, name, uid)
# TODO: Remove this when HFieldShape is implemented
raise NotImplementedError("HFieldShape is not yet implemented.")
@override
def __repr__(self):
return f"HFieldShape(\nname: {self.name},\nuid: {self.uid}\n)"
@property
@override
def paramsvec(self) -> vec4f:
return vec4f(1.0, 1.0, 1.0, 0.0)
@property
@override
def params(self) -> tuple[float, float, float]:
"""Returns the XYZ scaling of the height-field."""
return 1.0, 1.0, 1.0
###
# Aliases
###
ShapeDescriptorType = (
EmptyShape
| SphereShape
| CylinderShape
| ConeShape
| CapsuleShape
| BoxShape
| EllipsoidShape
| PlaneShape
| MeshShape
| HFieldShape
)
"""A type union that can represent any shape descriptor, including primitive and explicit shapes."""
###
# Utilities
###
def max_contacts_for_shape_pair(type_a: int, type_b: int) -> tuple[int, int]:
"""
Count the number of potential contact points for a collision pair in both
directions of the collision pair (collisions from A to B and from B to A).
Inputs must be canonicalized such that the type of shape A is less than or equal to the type of shape B.
Args:
type_a: First shape type as :class:`GeoType` integer value.
type_b: Second shape type as :class:`GeoType` integer value.
Returns:
tuple[int, int]: Number of contact points for collisions between A->B and B->A.
"""
# Ensure the shape types are ordered canonically
if type_a > type_b:
type_a, type_b = type_b, type_a
if type_a == GeoType.SPHERE:
return 1, 0
elif type_a == GeoType.CAPSULE:
if type_b == GeoType.CAPSULE:
return 2, 2
elif type_b == GeoType.ELLIPSOID:
return 8, 8
elif type_b == GeoType.CYLINDER:
return 4, 4
elif type_b == GeoType.BOX:
return 8, 8
elif type_b == GeoType.MESH or type_b == GeoType.CONVEX_MESH:
pass # TODO: WHAT TO RETURN WHEN MESH SUPPORT IS ADDED?
elif type_b == GeoType.CONE:
return 4, 4
elif type_b == GeoType.PLANE:
return 8, 8
elif type_a == GeoType.ELLIPSOID:
if type_b == GeoType.ELLIPSOID:
return 4, 4
elif type_b == GeoType.CYLINDER:
return 4, 4
elif type_b == GeoType.BOX:
return 8, 8
elif type_b == GeoType.MESH or type_b == GeoType.CONVEX_MESH:
pass # TODO: WHAT TO RETURN WHEN MESH SUPPORT IS ADDED?
elif type_b == GeoType.CONE:
return 8, 8
elif type_b == GeoType.PLANE:
return 4, 4
elif type_a == GeoType.CYLINDER:
if type_b == GeoType.CYLINDER:
return 4, 4
elif type_b == GeoType.BOX:
return 8, 8
elif type_b == GeoType.MESH or type_b == GeoType.CONVEX_MESH:
pass # TODO: WHAT TO RETURN WHEN MESH SUPPORT IS ADDED?
elif type_b == GeoType.CONE:
return 4, 4
elif type_b == GeoType.PLANE:
return 6, 6
elif type_a == GeoType.BOX:
if type_b == GeoType.BOX:
return 12, 12
elif type_b == GeoType.MESH or type_b == GeoType.CONVEX_MESH:
pass # TODO: WHAT TO RETURN WHEN MESH SUPPORT IS ADDED?
elif type_b == GeoType.CONE:
return 8, 8
elif type_b == GeoType.PLANE:
return 12, 12
elif type_a == GeoType.MESH or type_a == GeoType.CONVEX_MESH:
if type_b == GeoType.HFIELD:
pass # TODO: WHAT TO RETURN WHEN MESH SUPPORT IS ADDED?
elif type_b == GeoType.CONE:
pass # TODO: WHAT TO RETURN WHEN MESH SUPPORT IS ADDED?
elif type_b == GeoType.PLANE:
pass # TODO: WHAT TO RETURN WHEN MESH SUPPORT IS ADDED?
else:
pass # TODO: WHAT TO RETURN WHEN MESH SUPPORT IS ADDED?
elif type_a == GeoType.CONE:
if type_b == GeoType.CONE:
return 4, 4
elif type_b == GeoType.PLANE:
return 8, 8
elif type_a == GeoType.PLANE:
pass
# unsupported type combination
return 0, 0
================================================
FILE: newton/_src/solvers/kamino/_src/core/size.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Defines a utility container for Kamino model sizes."""
from __future__ import annotations
from dataclasses import dataclass
import warp as wp
###
# Module interface
###
__all__ = [
"SizeKamino",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
@dataclass
class SizeKamino:
"""
A container to hold the summary size of memory allocations and thread dimensions.
Notes:
- The sums are used for memory allocations.
- The maximums are used to define 2D thread shapes: (num_worlds, max_of_max_XXX)
- Where `XXX` is the maximum number of limits, contacts, unilaterals, or constraints in any world.
"""
num_worlds: int = 0
"""The number of worlds represented in the model."""
sum_of_num_bodies: int = 0
"""The total number of bodies in the model across all worlds."""
max_of_num_bodies: int = 0
"""The maximum number of bodies in any world."""
sum_of_num_joints: int = 0
"""The total number of joints in the model across all worlds."""
max_of_num_joints: int = 0
"""The maximum number of joints in any world."""
sum_of_num_passive_joints: int = 0
"""The total number of passive joints in the model across all worlds."""
max_of_num_passive_joints: int = 0
"""The maximum number of passive joints in any world."""
sum_of_num_actuated_joints: int = 0
"""The total number of actuated joints in the model across all worlds."""
max_of_num_actuated_joints: int = 0
"""The maximum number of actuated joints in any world."""
sum_of_num_dynamic_joints: int = 0
"""The total number of dynamic joints in the model across all worlds."""
max_of_num_dynamic_joints: int = 0
"""The maximum number of dynamic joints in any world."""
sum_of_num_geoms: int = 0
"""The total number of geometries in the model across all worlds."""
max_of_num_geoms: int = 0
"""The maximum number of geometries in any world."""
sum_of_num_materials: int = 0
"""
The total number of materials in the model across all worlds.
In the present implementation, this will be equal to `max_of_num_materials`,
since model materials are defined globally for all worlds. We plan to also
introduce per-world materials in the future.
"""
max_of_num_materials: int = 0
"""
The maximum number of materials in any world.
In the present implementation, this will be equal to `sum_of_num_materials`,
since model materials are defined globally for all worlds. We plan to also
introduce per-world materials in the future.
"""
sum_of_num_material_pairs: int = 0
"""The total number of material pairs in the model across all worlds."""
max_of_num_material_pairs: int = 0
"""The maximum number of material pairs in any world."""
sum_of_num_body_dofs: int = 0
"""The total number of body DoFs in the model across all worlds."""
max_of_num_body_dofs: int = 0
"""The maximum number of body DoFs in any world."""
sum_of_num_joint_coords: int = 0
"""The total number of joint coordinates in the model across all worlds."""
max_of_num_joint_coords: int = 0
"""The maximum number of joint coordinates in any world."""
sum_of_num_joint_dofs: int = 0
"""The total number of joint DoFs in the model across all worlds."""
max_of_num_joint_dofs: int = 0
"""The maximum number of joint DoFs in any world."""
sum_of_num_passive_joint_coords: int = 0
"""The total number of passive joint coordinates in the model across all worlds."""
max_of_num_passive_joint_coords: int = 0
"""The maximum number of passive joint coordinates in any world."""
sum_of_num_passive_joint_dofs: int = 0
"""The total number of passive joint DoFs in the model across all worlds."""
max_of_num_passive_joint_dofs: int = 0
"""The maximum number of passive joint DoFs in any world."""
sum_of_num_actuated_joint_coords: int = 0
"""The total number of actuated joint coordinates in the model across all worlds."""
max_of_num_actuated_joint_coords: int = 0
"""The maximum number of actuated joint coordinates in any world."""
sum_of_num_actuated_joint_dofs: int = 0
"""The total number of actuated joint DoFs in the model across all worlds."""
max_of_num_actuated_joint_dofs: int = 0
"""The maximum number of actuated joint DoFs in any world."""
sum_of_num_joint_cts: int = 0
"""The total number of joint constraints in the model across all worlds."""
max_of_num_joint_cts: int = 0
"""The maximum number of joint constraints in any world."""
sum_of_num_dynamic_joint_cts: int = 0
"""The total number of dynamic joint constraints in the model across all worlds."""
max_of_num_dynamic_joint_cts: int = 0
"""The maximum number of dynamic joint constraints in any world."""
sum_of_num_kinematic_joint_cts: int = 0
"""The total number of kinematic joint constraints in the model across all worlds."""
max_of_num_kinematic_joint_cts: int = 0
"""The maximum number of kinematic joint constraints in any world."""
sum_of_max_limits: int = 0
"""The total maximum number of limits allocated for the model across all worlds."""
max_of_max_limits: int = 0
"""The maximum number of active limits of any world."""
sum_of_max_contacts: int = 0
"""The total maximum number of contacts allocated for the model across all worlds."""
max_of_max_contacts: int = 0
"""The maximum number of active contacts of any world."""
sum_of_max_unilaterals: int = 0
"""The maximum number of active unilateral entities, i.e. joint-limits and contacts."""
max_of_max_unilaterals: int = 0
"""The maximum number of active unilaterals of any world."""
sum_of_max_total_cts: int = 0
"""The total maximum number of active constraints allocated for the model across all worlds."""
max_of_max_total_cts: int = 0
"""The maximum number of active constraints of any world."""
def __repr__(self):
"""Returns a human-readable string representation of the SizeKamino as a formatted table."""
# List of (row title, sum attr, max attr)
rows = [
("num_bodies", "sum_of_num_bodies", "max_of_num_bodies"),
("num_joints", "sum_of_num_joints", "max_of_num_joints"),
("num_passive_joints", "sum_of_num_passive_joints", "max_of_num_passive_joints"),
("num_actuated_joints", "sum_of_num_actuated_joints", "max_of_num_actuated_joints"),
("num_dynamic_joints", "sum_of_num_dynamic_joints", "max_of_num_dynamic_joints"),
("num_geoms", "sum_of_num_geoms", "max_of_num_geoms"),
("num_material_pairs", "sum_of_num_material_pairs", "max_of_num_material_pairs"),
("num_body_dofs", "sum_of_num_body_dofs", "max_of_num_body_dofs"),
("num_joint_coords", "sum_of_num_joint_coords", "max_of_num_joint_coords"),
("num_joint_dofs", "sum_of_num_joint_dofs", "max_of_num_joint_dofs"),
("num_passive_joint_coords", "sum_of_num_passive_joint_coords", "max_of_num_passive_joint_coords"),
("num_passive_joint_dofs", "sum_of_num_passive_joint_dofs", "max_of_num_passive_joint_dofs"),
("num_actuated_joint_coords", "sum_of_num_actuated_joint_coords", "max_of_num_actuated_joint_coords"),
("num_actuated_joint_dofs", "sum_of_num_actuated_joint_dofs", "max_of_num_actuated_joint_dofs"),
("num_joint_cts", "sum_of_num_joint_cts", "max_of_num_joint_cts"),
("num_dynamic_joint_cts", "sum_of_num_dynamic_joint_cts", "max_of_num_dynamic_joint_cts"),
("num_kinematic_joint_cts", "sum_of_num_kinematic_joint_cts", "max_of_num_kinematic_joint_cts"),
("max_limits", "sum_of_max_limits", "max_of_max_limits"),
("max_contacts", "sum_of_max_contacts", "max_of_max_contacts"),
("max_unilaterals", "sum_of_max_unilaterals", "max_of_max_unilaterals"),
("max_total_cts", "sum_of_max_total_cts", "max_of_max_total_cts"),
]
# Compute column widths
name_width = max(len("Name"), *(len(r[0]) for r in rows))
sum_width = max(len("Sum"), *(len(str(getattr(self, r[1]))) for r in rows))
max_width = max(len("Max"), *(len(str(getattr(self, r[2]))) for r in rows))
# Write SizeKamino members as a formatted table
lines = []
lines.append("-" * (name_width + 1 + sum_width + 1 + max_width))
lines.append(f"{'Name':<{name_width}} {'Sum':>{sum_width}} {'Max':>{max_width}}")
lines.append("-" * (name_width + 1 + sum_width + 1 + max_width))
for name, sum_attr, max_attr in rows:
sum_val = getattr(self, sum_attr)
max_val = getattr(self, max_attr)
line = f"{name:<{name_width}} {sum_val:>{sum_width}} {max_val:>{max_width}}"
lines.append(line)
lines.append("-" * (name_width + 1 + sum_width + 1 + max_width))
# Join the lines into a single string
return "\n".join(lines)
================================================
FILE: newton/_src/solvers/kamino/_src/core/state.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Defines the state container of Kamino."""
from __future__ import annotations
from dataclasses import dataclass
import warp as wp
from .....sim import Model, State
from .bodies import convert_body_com_to_origin, convert_body_origin_to_com
from .size import SizeKamino
from .types import vec6f
###
# Module interface
###
__all__ = [
"StateKamino",
]
###
# Types
###
@dataclass
class StateKamino:
"""
Represents the time-varying state of a :class:`ModelKamino` in a simulation.
The :class:`StateKamino` object holds all dynamic quantities that change over time during
simulation, such as rigid body poses, twists, and wrenches, as well as joint coordinates,
velocities, and constraint forces.
:class:`StateKamino` objects are typically created via :meth:`kamino.ModelKamino.state()`
and are used to store and update the simulation's current configuration and derived data.
For constrained rigid multi-body system, the state is defined formally using either:
1. maximal-coordinates, as the absolute poses and twists of all bodies expressed in world coordinates, or
2. minimal-coordinates, as the joint coordinates and velocities along with the
pose and twist of a base body when it is a so-called "floating-base" system.
In Kamino, we formally adopt the maximal-coordinate formulation in order to compute the physics of the
system, but we are also interested in the state of the joints for the purposes of control and analysis.
Thus, this container incorporates the data of both representations, and in addition also includes the per-body
total (i.e. net) wrenches expressed in world coordinates, as well as the joint constraint forces. Thus, it
provides a complete description of the dynamic state of the constrained rigid multi-body system.
We adopt the following notational conventions for the state attributes:
- Generalized coordinates, whether maximal or minimal, are universally denoted by ``q``
- Generalized velocities for bodies are denoted by ``u`` since they are twists
- Generalized velocities for joints are denoted by ``dq`` since they are time-derivatives of ``q``
- Wrenches (forces + torques) are denoted by ``w``
- Constraint forces are denoted by ``lambda`` since they are effectively Lagrange multipliers
- Subscripts ``_i`` denote body-indexed quantities, e.g. :attr:`q_i`, :attr:`u_i`, :attr:`w_i`.
- Subscripts ``_j`` denote joint-indexed quantities, e.g. :attr:`q_j`, :attr:`dq_j`, :attr:`lambda_j`.
"""
###
# Attributes
###
q_i: wp.array | None = None
"""
Array of absolute body CoM poses expressed in world coordinates.\n
Each element is a 7D transform consisting of a 3D position + 4D unit quaternion.\n
Shape is ``(num_bodies,)`` and dtype is :class:`transformf`.
"""
u_i: wp.array | None = None
"""
Array of absolute body CoM twists expressed in world coordinates.\n
Each element is a 6D vector comprising a 3D linear + 3D angular components.\n
Shape is ``(num_bodies,)`` and dtype is :class:`vec6f`.
"""
w_i: wp.array | None = None
"""
Array of total body CoM wrenches expressed in world coordinates.\n
Each element is a 6D vector comprising a 3D linear + 3D angular components.\n
Shape is ``(num_bodies,)`` and dtype is :class:`vec6f`.
"""
w_i_e: wp.array | None = None
"""
Array of external body CoM wrenches expressed in world coordinates.\n
Each element is a 6D vector comprising a 3D linear + 3D angular components.\n
Shape is ``(num_bodies,)`` and dtype is :class:`vec6f`.
"""
q_j: wp.array | None = None
"""
Array of generalized joint coordinates.\n
Shape is ``(sum_of_num_joint_coords,)`` and dtype is :class:`float32`.
"""
q_j_p: wp.array | None = None
"""
Array of previous generalized joint coordinates.\n
Shape is ``(sum_of_num_joint_coords,)`` and dtype is :class:`float32`.
"""
dq_j: wp.array | None = None
"""
Array of generalized joint velocities.\n
Shape is ``(sum_of_num_joint_dofs,)`` and dtype is :class:`float32`.
"""
lambda_j: wp.array | None = None
"""
Array of generalized joint constraint forces.\n
Shape is ``(sum_of_num_joint_cts,)`` and dtype is :class:`float32`.
"""
###
# Operations
###
def copy_to(self, other: StateKamino) -> None:
"""
Copy the current data to another :class:`StateKamino` object.
Args:
other: The target :class:`StateKamino` object to copy data into.
"""
if other is None:
raise ValueError("A StateKamino instance must be provided to copy to.")
if not isinstance(other, StateKamino):
raise TypeError(f"Expected state of type StateKamino, but got {type(other)}.")
other.copy_from(self)
def copy_from(self, other: StateKamino) -> None:
"""
Copy the data from another :class:`StateKamino` object into the current.
Args:
other: The source :class:`StateKamino` object to copy data from.
"""
if other is None:
raise ValueError("A StateKamino instance must be provided to copy from.")
if not isinstance(other, StateKamino):
raise TypeError(f"Expected state of type StateKamino, but got {type(other)}.")
if self.q_i is None or other.q_i is None:
raise ValueError("Error copying from/to uninitialized StateKamino")
wp.copy(self.q_i, other.q_i)
wp.copy(self.u_i, other.u_i)
wp.copy(self.w_i, other.w_i)
wp.copy(self.w_i_e, other.w_i_e)
wp.copy(self.q_j, other.q_j)
wp.copy(self.q_j_p, other.q_j_p)
wp.copy(self.dq_j, other.dq_j)
wp.copy(self.lambda_j, other.lambda_j)
def convert_to_body_com_state(
self,
model: Model,
world_mask: wp.array | None = None,
body_wid: wp.array | None = None,
) -> None:
"""
Convert the body-frame state to body center-of-mass (CoM)
state using the provided body center-of-mass offsets.
Args:
model (Model):
The model container holding the time-invariant parameters of the simulation.
world_mask: optional per-world mask selecting which worlds to process.
body_wid: body-to-world index mapping, required when ``world_mask`` is given.
"""
# Ensure the model is valid
if model is None:
raise ValueError("Model must be provided to convert to body CoM state.")
if not isinstance(model, Model):
raise TypeError(f"Expected model of type Model, but got {type(model)}.")
if model.body_com is None:
raise ValueError("Model must have body_com defined to convert to body CoM state.")
convert_body_origin_to_com(
body_com=model.body_com,
body_q=self.q_i,
body_q_com=self.q_i,
body_wid=body_wid,
world_mask=world_mask,
)
def convert_to_body_frame_state(
self,
model: Model,
world_mask: wp.array | None = None,
body_wid: wp.array | None = None,
) -> None:
"""
Convert the body center-of-mass (CoM) state to body-frame
state using the provided body center-of-mass offsets.
Args:
model (Model):
The model container holding the time-invariant parameters of the simulation.
world_mask: optional per-world mask selecting which worlds to process.
body_wid: body-to-world index mapping, required when ``world_mask`` is given.
"""
# Ensure the model is valid
if model is None:
raise ValueError("Model must be provided to convert to body CoM state.")
if not isinstance(model, Model):
raise TypeError(f"Expected model of type Model, but got {type(model)}.")
if model.body_com is None:
raise ValueError("Model must have body_com defined to convert to body CoM state.")
convert_body_com_to_origin(
body_com=model.body_com,
body_q_com=self.q_i,
body_q=self.q_i,
body_wid=body_wid,
world_mask=world_mask,
)
@staticmethod
def from_newton(
size: SizeKamino,
model: Model,
state: State,
initialize_state_prev: bool = False,
convert_to_com_frame: bool = False,
) -> StateKamino:
"""
Constructs a :class:`kamino.StateKamino` object from a :class:`newton.State` object.
This operation serves only as an adaptor-like constructor to interface a
:class:`newton.State`, effectively creating an alias without copying data.
Args:
state (State):
The source :class:`newton.State` object to be adapted.
initialize_state_prev (bool):
If True, initialize the `joint_q_prev` attribute to
match the current `joint_q` from the newton.State.
convert_to_com_frame (bool):
If True, convert body poses to local center-of-mass frames.
Returns:
A :class:`kamino.StateKamino` object that aliases the data of the input :class:`newton.State` object.
"""
# Ensure the state is valid
if state is None:
raise ValueError("A State instance must be provided to convert to StateKamino.")
if not isinstance(state, State):
raise TypeError(f"Expected state of type State, but got {type(state)}.")
# Retrieve the device of the state container
device = None
if state.body_q is not None:
device = state.body_q.device
elif state.joint_q is not None:
device = state.joint_q.device
else:
raise ValueError("State must have at least body_q or joint_q defined to determine device for StateKamino.")
# If the state contains the Kamino-specific `body_f_total` custom attribute,
# capture a reference to it; otherwise, create a new array for it.
if hasattr(state, "body_f_total"):
body_f_total = state.body_f_total
else:
body_f_total = wp.zeros_like(state.body_f)
state.body_f_total = body_f_total
# If the state contains the Kamino-specific `joint_q_prev` custom attribute,
# capture a reference to it; otherwise, create a new array for it.
if hasattr(state, "joint_q_prev"):
joint_q_prev = state.joint_q_prev
else:
joint_q_prev = wp.clone(state.joint_q)
state.joint_q_prev = joint_q_prev
# If the state contains the Kamino-specific `joint_lambdas` custom attribute,
# capture a reference to it; otherwise, create a new array for it.
is_joint_lambdas_valid = (
hasattr(state, "joint_lambdas")
and state.joint_lambdas is not None
and state.joint_lambdas.shape == (size.sum_of_num_joint_cts,)
)
if is_joint_lambdas_valid:
joint_lambdas = state.joint_lambdas
else:
joint_lambdas = wp.zeros(shape=(size.sum_of_num_joint_cts,), dtype=wp.float32, device=device)
state.joint_lambdas = joint_lambdas
# Optionally initialize the `joint_q_prev` array to match the current `joint_q`
if initialize_state_prev:
wp.copy(joint_q_prev, state.joint_q)
# Create a new StateKamino object, aliasing the relevant data from the input newton.State
state_kamino = StateKamino(
q_i=state.body_q,
u_i=state.body_qd.view(dtype=vec6f), # TODO: change to wp.spatial_vector
w_i=body_f_total.view(dtype=vec6f), # TODO: change to wp.spatial_vector
w_i_e=state.body_f.view(dtype=vec6f), # TODO: change to wp.spatial_vector
q_j=state.joint_q,
q_j_p=joint_q_prev,
dq_j=state.joint_qd,
lambda_j=joint_lambdas,
)
# Optionally convert body poses to CoM frame
if convert_to_com_frame:
state_kamino.convert_to_body_com_state(model)
# Return the StateKamino object, aliasing the
# relevant data from the input newton.State
return state_kamino
@staticmethod
def to_newton(model: Model, state: StateKamino, convert_to_body_frame: bool = False) -> State:
"""
Constructs a :class:`newton.State` object from a :class:`kamino.StateKamino` object.
This operation serves only as an adaptor-like constructor to interface a
:class:`kamino.StateKamino`, effectively creating an alias without copying data.
Args:
state (StateKamino):
The source :class:`kamino.StateKamino` object to be adapted.
convert_to_body_frame (bool):
If True, convert body poses to body-local frames.
Returns:
A :class:`newton.State` object that aliases the data of the input :class:`kamino.StateKamino` object.
"""
# Ensure the model is valid
if model is None:
raise ValueError("A Model instance must be provided to convert to StateKamino.")
if not isinstance(model, Model):
raise TypeError(f"Expected model of type Model, but got {type(model)}.")
# Ensure the state is valid
if state is None:
raise ValueError("A StateKamino instance must be provided to convert to State.")
if not isinstance(state, StateKamino):
raise TypeError(f"Expected state of type StateKamino, but got {type(state)}.")
# Optionally convert body poses to body frame
if convert_to_body_frame:
state.convert_to_body_frame_state(model)
# Create a new State object, aliasing the relevant
# data from the input kamino.StateKamino
state_newton = State()
state_newton.body_q = state.q_i
state_newton.body_qd = state.u_i.view(dtype=wp.spatial_vectorf)
state_newton.body_f = state.w_i_e.view(dtype=wp.spatial_vectorf)
state_newton.joint_q = state.q_j
state_newton.joint_qd = state.dq_j
# Add Kamino-specific custom attributes to the newton.State object
state_newton.body_f_total = state.w_i.view(dtype=wp.spatial_vectorf)
state_newton.joint_q_prev = state.q_j_p
state_newton.joint_lambdas = state.lambda_j
# Return the new newton.State object
return state_newton
================================================
FILE: newton/_src/solvers/kamino/_src/core/time.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Defines containers for time-keeping across heterogeneous worlds simulated in parallel.
"""
from dataclasses import dataclass
import numpy as np
import warp as wp
from .types import float32, int32
###
# Module interface
###
__all__ = [
"TimeData",
"TimeModel",
"advance_time",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Containers
###
@dataclass
class TimeModel:
"""
A container to hold heterogeneous model time-step.
Attributes:
dt (wp.array | None): The discrete time-step size of each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
inv_dt (wp.array | None): The inverse of the discrete time-step size of each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
dt: wp.array | None = None
"""
The discrete time-step size of each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
inv_dt: wp.array | None = None
"""
The inverse of the discrete time-step size of each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
def set_uniform_timestep(self, dt: float):
"""
Sets a uniform discrete time-step for all worlds.
Args:
dt (float): The time-step size to set.
"""
# Ensure that the provided time-step is a floating-point value
if not isinstance(dt, float):
raise TypeError(f"Invalid dt type: {type(dt)}. Expected: float.")
# Ensure that the provided time-step is positive
if dt <= 0.0:
raise ValueError(f"Invalid dt value: {dt}. Expected: positive float.")
# Assign the target time-step uniformly to all worlds
self.dt.fill_(dt)
self.inv_dt.fill_(1.0 / dt)
def set_timesteps(self, dt: list[float] | np.ndarray):
"""
Sets the discrete time-step of each world explicitly.
Args:
dt (list[float] | np.ndarray): An iterable collection of time-steps over all worlds.
"""
# Ensure that the length of the input matches the number of worlds
if len(dt) != self.dt.size:
raise ValueError(f"Invalid dt size: {len(dt)}. Expected: {self.dt.size}.")
# If the input is a list, convert it to a numpy array
if isinstance(dt, list):
dt = np.array(dt, dtype=np.float32)
# Ensure that the input is a numpy array of the correct dtype
if not isinstance(dt, np.ndarray):
raise TypeError(f"Invalid dt type: {type(dt)}. Expected: np.ndarray.")
if dt.dtype != np.float32:
raise TypeError(f"Invalid dt dtype: {dt.dtype}. Expected: np.float32.")
# Assign the values to the internal arrays
self.dt.assign(dt)
self.inv_dt.assign(1.0 / dt)
@dataclass
class TimeData:
"""
A container to hold heterogeneous model time-keeping data.
Attributes:
steps (wp.array | None): The current number of simulation steps of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
time (wp.array | None): The current simulation time of each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
steps: wp.array | None = None
"""
The current number of simulation steps of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int`.
"""
time: wp.array | None = None
"""
The current simulation time of each world.\n
Shape of ``(num_worlds,)`` and type :class:`float`.
"""
def reset(self):
"""
Resets the time state to zero.
"""
self.steps.fill_(0)
self.time.fill_(0.0)
###
# Kernels
###
@wp.kernel
def _advance_time(
# Inputs
dt: wp.array(dtype=float32),
# Outputs
steps: wp.array(dtype=int32), # TODO: Make this uint64
time: wp.array(dtype=float32),
):
"""
Advances the time-keeping state of each world by one time-step.
For each world index ``wid``, this kernel increments the step counter
``steps[wid]`` by 1 and increases the simulation time ``time[wid]``
by the corresponding time increment ``dt[wid]``.
"""
# Retrieve the thread index as the world index
wid = wp.tid()
# Update the time and step count
steps[wid] += 1
time[wid] += dt[wid]
###
# Launchers
###
def advance_time(model: TimeModel, data: TimeData):
"""
Advances the time-keeping state of each world by one time-step.
For each world index ``wid``, this kernel increments the step counter
``steps[wid]`` by 1 and increases the simulation time ``time[wid]``
by the corresponding time increment ``dt[wid]``.
Args:
model (TimeModel):
The time model containing the time-step information.
data (TimeData):
The time data containing the current time-keeping state.
"""
# Ensure the model is valid
if model is None:
raise ValueError("'model' must be initialized, is None.")
elif not isinstance(model, TimeModel):
raise TypeError("'model' must be an instance of TimeModel.")
if model.dt is None:
raise ValueError("'model' must contain a `model.dt` array, is None.")
# Ensure the state is valid
if data is None:
raise ValueError("'data' must be initialized, is None.")
elif not isinstance(data, TimeData):
raise TypeError("'data' must be an instance of TimeData.")
if data.steps is None:
raise ValueError("'data' must contain a `data.steps` array, is None.")
# Launch the kernel to advance the time state of each world by one step
wp.launch(
_advance_time,
dim=model.dt.size,
inputs=[
# Inputs:
model.dt,
# Outputs:
data.steps,
data.time,
],
)
================================================
FILE: newton/_src/solvers/kamino/_src/core/types.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""The core data types used throughout Kamino."""
from __future__ import annotations
import sys
import uuid
from collections.abc import Iterable
from dataclasses import dataclass
from enum import IntEnum
from typing import Literal
import numpy as np
import warp as wp
if sys.version_info >= (3, 12):
from typing import override
else:
try:
from typing_extensions import override
except ImportError:
# Fallback no-op decorator if typing_extensions is not available
def override(func):
return func
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Generics
###
Vec3 = list[float]
Vec4 = list[float]
Vec6 = list[float]
Quat = list[float]
Mat33 = list[float]
Transform = tuple[Vec3, Quat]
###
# Scalars
###
uint8 = wp.uint8
uint16 = wp.uint16
uint32 = wp.uint32
uint64 = wp.uint64
int8 = wp.int8
int16 = wp.int16
int32 = wp.int32
int64 = wp.int64
float16 = wp.float16
float32 = wp.float32
float64 = wp.float64
###
# Unions
###
FloatType = float16 | float32 | float64
IntType = int16 | int32 | int64
VecIntType = wp.vec2s | wp.vec2i | wp.vec2l
###
# Vectors
###
class vec1i(wp.types.vector(length=1, dtype=int32)):
pass
class vec1f(wp.types.vector(length=1, dtype=float32)):
pass
class vec2i(wp.types.vector(length=2, dtype=int32)):
pass
class vec2f(wp.types.vector(length=2, dtype=float32)):
pass
class vec3i(wp.types.vector(length=3, dtype=int32)):
pass
class vec3f(wp.types.vector(length=3, dtype=float32)):
pass
class vec4i(wp.types.vector(length=4, dtype=int32)):
pass
class vec4f(wp.types.vector(length=4, dtype=float32)):
pass
class vec5i(wp.types.vector(length=5, dtype=int32)):
pass
class vec5f(wp.types.vector(length=5, dtype=float32)):
pass
class vec6i(wp.types.vector(length=6, dtype=int32)):
pass
class vec6f(wp.types.vector(length=6, dtype=float32)):
pass
class vec7f(wp.types.vector(length=7, dtype=float32)):
pass
class vec8f(wp.types.vector(length=8, dtype=float32)):
pass
class vec14f(wp.types.vector(length=14, dtype=float32)):
pass
###
# Matrices
###
class mat22f(wp.types.matrix(shape=(2, 2), dtype=float32)):
pass
class mat33f(wp.types.matrix(shape=(3, 3), dtype=float32)):
pass
class mat44f(wp.types.matrix(shape=(4, 4), dtype=float32)):
pass
class mat61f(wp.types.matrix(shape=(6, 1), dtype=float32)):
pass
class mat16f(wp.types.matrix(shape=(1, 6), dtype=float32)):
pass
class mat62f(wp.types.matrix(shape=(6, 2), dtype=float32)):
pass
class mat26f(wp.types.matrix(shape=(2, 6), dtype=float32)):
pass
class mat63f(wp.types.matrix(shape=(6, 3), dtype=float32)):
pass
class mat36f(wp.types.matrix(shape=(3, 6), dtype=float32)):
pass
class mat64f(wp.types.matrix(shape=(6, 4), dtype=float32)):
pass
class mat46f(wp.types.matrix(shape=(4, 6), dtype=float32)):
pass
class mat65f(wp.types.matrix(shape=(6, 5), dtype=float32)):
pass
class mat56f(wp.types.matrix(shape=(5, 6), dtype=float32)):
pass
class mat66f(wp.types.matrix(shape=(6, 6), dtype=float32)):
pass
class mat34f(wp.types.matrix(shape=(3, 4), dtype=float32)):
pass
class mat43f(wp.types.matrix(shape=(4, 3), dtype=float32)):
pass
class mat38f(wp.types.matrix(shape=(3, 8), dtype=float32)):
pass
class mat83f(wp.types.matrix(shape=(8, 3), dtype=float32)):
pass
###
# Quaternions
###
class quatf(wp.types.quaternion(dtype=float32)):
pass
###
# Transforms
###
class transformf(wp.types.transformation(dtype=float32)):
pass
###
# Axis
###
class Axis(IntEnum):
"""Enum for representing the three axes in 3D space."""
X = 0
Y = 1
Z = 2
@classmethod
def from_string(cls, axis_str: str) -> Axis:
axis_str = axis_str.lower()
if axis_str == "x":
return cls.X
elif axis_str == "y":
return cls.Y
elif axis_str == "z":
return cls.Z
raise ValueError(f"Invalid axis string: {axis_str}")
@classmethod
def from_any(cls, value: AxisType) -> Axis:
if isinstance(value, cls):
return value
if isinstance(value, str):
return cls.from_string(value)
if type(value) in {int, wp.int32, wp.int64, np.int32, np.int64}:
return cls(value)
raise TypeError(f"Cannot convert {type(value)} to Axis")
@override
def __str__(self):
return self.name.capitalize()
@override
def __repr__(self):
return f"Axis.{self.name.capitalize()}"
@override
def __eq__(self, other):
if isinstance(other, str):
return self.name.lower() == other.lower()
if type(other) in {int, wp.int32, wp.int64, np.int32, np.int64}:
return self.value == int(other)
return NotImplemented
@override
def __hash__(self):
return hash(self.name)
def to_vector(self) -> tuple[float, float, float]:
if self == Axis.X:
return (1.0, 0.0, 0.0)
elif self == Axis.Y:
return (0.0, 1.0, 0.0)
else:
return (0.0, 0.0, 1.0)
def to_matrix(self) -> Mat33:
if self == Axis.X:
return [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
elif self == Axis.Y:
return [0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0]
else:
return [0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0]
def to_vec3(self) -> vec3f:
return vec3f(*self.to_vector())
def to_mat33(self) -> mat33f:
return mat33f(*self.to_matrix())
AxisType = Axis | Literal["X", "Y", "Z"] | Literal[0, 1, 2] | int | str
"""Type that can be used to represent an axis, including the enum, string, and integer representations."""
def axis_to_vec3(axis: AxisType | Vec3) -> vec3f:
"""Convert an axis representation to a 3D vector."""
if isinstance(axis, list | tuple | np.ndarray):
return vec3f(*axis)
elif wp.types.type_is_vector(type(axis)):
return vec3f(*axis)
else:
return Axis.from_any(axis).to_vec3()
def axis_to_mat33(axis: AxisType | Vec3) -> mat33f:
"""Convert an axis representation to a 3x3 matrix."""
if isinstance(axis, list | tuple | np.ndarray):
return mat33f(*axis)
elif wp.types.type_is_vector(type(axis)):
return mat33f(*axis)
else:
return Axis.from_any(axis).to_mat33()
###
# Descriptor
###
@dataclass
class Descriptor:
"""
Base class for entity descriptor objects.
A descriptor object is one with a designated name and a unique identifier (UID).
"""
name: str
"""The name of the entity descriptor."""
uid: str | None = None
"""The unique identifier (UID) of the entity descriptor."""
@staticmethod
def _assert_valid_uid(uid: str) -> str:
"""
Check if a given UID string is valid.
Args:
uid (str): The UID string to validate.
Returns:
str: The validated UID string.
Raises:
ValueError: If the UID string is not valid.
"""
try:
val = uuid.UUID(uid, version=4)
except ValueError as err:
raise ValueError("Invalid UID string.") from err
return str(val)
def __post_init__(self):
"""Post-initialization to handle UID generation and validation."""
if self.uid is None:
# Generate a new UID if none is provided
self.uid = str(uuid.uuid4())
else:
# Otherwise, validate the provided UID
self.uid = self._assert_valid_uid(self.uid)
def __hash__(self):
"""Returns a hash of the Descriptor based on its UID."""
return hash((self.name, self.uid))
def __repr__(self):
"""Returns a human-readable string representation of the Descriptor."""
return f"Descriptor(\nname={self.name},\nuid={self.uid}\n)"
###
# Utilities
###
ArrayLike = np.ndarray | list | tuple | Iterable
"""An Array-like structure for aliasing various data types compatible with numpy."""
FloatArrayLike = np.ndarray | list[float] | list[list[float]]
"""An Array-like structure for aliasing various floating-point data types compatible with numpy."""
IntArrayLike = np.ndarray | list[int] | list[list[int]]
"""An Array-like structure for aliasing various integer data types compatible with numpy."""
================================================
FILE: newton/_src/solvers/kamino/_src/core/world.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Provides a host-side container to summarily describe simulation world."""
import math
from dataclasses import dataclass, field
import warp as wp
from .bodies import RigidBodyDescriptor
from .geometry import GeometryDescriptor
from .joints import JointActuationType, JointDescriptor, JointDoFType
from .materials import MaterialDescriptor
from .types import Descriptor
###
# Module interface
###
__all__ = ["WorldDescriptor"]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Containers
###
@dataclass
class WorldDescriptor(Descriptor):
"""
A container to describe the problem dimensions and elements of a single world.
"""
wid: int = 0
"""
Index of the world w.r.t. the entire model. Defaults to `0`.\n
Used to identify the world in construction of multi-world models.
"""
###
# Entity Counts
###
num_bodies: int = 0
"""
The number of rigid bodies defined in the world.
"""
num_joints: int = 0
"""
The number of joints defined in the world.
"""
num_passive_joints: int = 0
"""
The number of joints that are passive.\n
This is less than or equal to `num_joints`.
"""
num_actuated_joints: int = 0
"""
The number of joints that are actuated.\n
This is less than or equal to `num_joints`.
"""
num_dynamic_joints: int = 0
"""
The number of joints that are dynamic.\n
This is less than or equal to `num_joints`.
"""
num_geoms: int = 0
"""
The number of geometries defined in the world.
"""
num_materials: int = 0
"""
The number of materials defined in the world.
"""
###
# Coordinates, DoFs & Constraints Counts
###
num_body_coords: int = 0
"""
The total number of body coordinates.\n
This is always equal to `7 * num_bodies`.
"""
num_body_dofs: int = 0
"""
The total number of body DoFs.\n
This is always equal to `6 * num_bodies`.
"""
num_joint_coords: int = 0
"""
The total number of joint coordinates.\n
This is equal to the sum of the coordinates of all joints in the world.
"""
num_joint_dofs: int = 0
"""
The total number of joint DoFs.\n
This is equal to the sum of the DoFs of all joints in the world.
"""
num_passive_joint_coords: int = 0
"""
The number of passive joint coordinates.\n
This is equal to the sum of the coordinates of all passive joints defined
in the world, and is always less than or equal to `num_joint_coords`.\n
"""
num_passive_joint_dofs: int = 0
"""
The number of passive joint DoFs.\n
This is equal to the sum of the DoFs of all passive joints defined
in the world, and is always less than or equal to `num_joint_dofs`.\n
"""
num_actuated_joint_coords: int = 0
"""
The number of actuated joint coordinates.\n
This is equal to the sum of the coordinates of all actuated joints defined
in the world, and is always less than or equal to `num_joint_coords`.\n
"""
num_actuated_joint_dofs: int = 0
"""
The number of actuated joint DoFs.\n
This is equal to the sum of the DoFs of all actuated joints defined
in the world, and is always less than or equal to `num_joint_dofs`.\n
"""
num_joint_cts: int = 0
"""
The total number of joint constraints.\n
This is equal to the sum of the constraints of all dynamic joints defined in the world.
"""
num_dynamic_joint_cts: int = 0
"""
The total number of joint dynamics constraints.\n
This is equal to the sum of the dynamic constraints of all dynamic joints defined in the world.
"""
num_kinematic_joint_cts: int = 0
"""
The total number of joint kinematics constraints.\n
This is equal to the sum of the kinematics constraints of all joints defined in the world.
"""
joint_coords: list[int] = field(default_factory=list)
"""
The list of all joint coordinates.\n
This list is ordered according the joint indices in the world,
and the sum of all elements is equal to `num_joint_coords`.\n
"""
joint_dofs: list[int] = field(default_factory=list)
"""
The list of all joint DoFs.\n
This list is ordered according the joint indices in the world,
and the sum of all elements is equal to `num_joint_dofs`.\n
"""
joint_passive_coords: list[int] = field(default_factory=list)
"""
The list of all passive joint coordinates.\n
This list is ordered according the joint indices in the world,
and the sum of all elements is equal to `num_passive_joint_coords`.\n
"""
joint_passive_dofs: list[int] = field(default_factory=list)
"""
The list of all passive joint DoFs.\n
This list is ordered according the joint indices in the world,
and the sum of all elements is equal to `num_passive_joint_dofs`.\n
"""
joint_actuated_coords: list[int] = field(default_factory=list)
"""
The list of all actuated joint coordinates.\n
This list is ordered according the joint indices in the world,
and the sum of all elements is equal to `num_actuated_joint_coords`.\n
"""
joint_actuated_dofs: list[int] = field(default_factory=list)
"""
The list of all actuated joint DoFs.\n
This list is ordered according the joint indices in the world,
and the sum of all elements is equal to `num_actuated_joint_dofs`.\n
"""
joint_dynamic_cts: list[int] = field(default_factory=list)
"""
The list of all joint dynamics constraints.\n
This list is ordered according the joint indices in the world,
and the sum of all elements is equal to `num_dynamic_joint_cts`.\n
"""
joint_kinematic_cts: list[int] = field(default_factory=list)
"""
The list of all joint kinematics constraints.\n
This list is ordered according the joint indices in the world,
and the sum of all elements is equal to `num_kinematic_joint_cts`.\n
"""
###
# Entity Offsets
###
bodies_idx_offset: int = 0
"""Index offset of the world's bodies w.r.t the entire model."""
joints_idx_offset: int = 0
"""Index offset of the world's joints w.r.t the entire model."""
geoms_idx_offset: int = 0
"""Index offset of the world's geometries w.r.t the entire model."""
###
# Constraint & DoF Offsets
###
body_dofs_idx_offset: int = 0
"""Index offset of the world's body DoFs w.r.t the entire model."""
joint_coords_idx_offset: int = 0
"""Index offset of the world's joint coordinates w.r.t the entire model."""
joint_dofs_idx_offset: int = 0
"""Index offset of the world's joint DoFs w.r.t the entire model."""
joint_passive_coords_idx_offset: int = 0
"""Index offset of the world's passive joint coordinates w.r.t the entire model."""
joint_passive_dofs_idx_offset: int = 0
"""Index offset of the world's passive joint DoFs w.r.t the entire model."""
joint_actuated_coords_idx_offset: int = 0
"""Index offset of the world's actuated joint coordinates w.r.t the entire model."""
joint_actuated_dofs_idx_offset: int = 0
"""Index offset of the world's actuated joint DoFs w.r.t the entire model."""
joint_cts_idx_offset: int = 0
"""Index offset of the world's joint constraints w.r.t the entire model."""
joint_dynamic_cts_idx_offset: int = 0
"""Index offset of the world's joint dynamics constraints w.r.t the entire model."""
joint_kinematic_cts_idx_offset: int = 0
"""Index offset of the world's joint kinematics constraints w.r.t the entire model."""
###
# Entity Identifiers
###
body_names: list[str] = field(default_factory=list[str])
"""List of body names."""
body_uids: list[str] = field(default_factory=list[str])
"""List of body unique identifiers (UIDs)."""
joint_names: list[str] = field(default_factory=list[str])
"""List of joint names."""
joint_uids: list[str] = field(default_factory=list[str])
"""List of joint unique identifiers (UIDs)."""
geom_names: list[str] = field(default_factory=list[str])
"""List of geometry names."""
geom_uids: list[str] = field(default_factory=list[str])
"""List of geometry unique identifiers (UIDs)."""
material_names: list[str] = field(default_factory=list[str])
"""List of material names."""
material_uids: list[str] = field(default_factory=list[str])
"""List of material unique identifiers (UIDs)."""
unary_joint_names: list[str] = field(default_factory=list[str])
"""List of unary joint names."""
fixed_joint_names: list[str] = field(default_factory=list[str])
"""List of fixed joint names."""
passive_joint_names: list[str] = field(default_factory=list[str])
"""List of passive joint names."""
actuated_joint_names: list[str] = field(default_factory=list[str])
"""List of actuated joint names."""
dynamic_joint_names: list[str] = field(default_factory=list[str])
"""List of dynamic joint names."""
geometry_max_contacts: list[int] = field(default_factory=list)
"""List of maximum contacts prescribed for each geometry."""
###
# Base Properties
###
base_body_idx: int | None = None
"""
Index of the `base body` w.r.t. the world, i.e., index of the central node of the
body-joint connectivity graph.\n
The `base body` is connected to the world through a `base joint`, which, if not specified
is considered to be an implicit 6D free joint, indicating a floating-base system.
Otherwise, the `base joint` must be a unary joint connecting the base body to the world.\n
For articulated systems, the base body is the root body of the kinematic tree.\n
For general mechanical assemblies, e.g. particle systems, rigid clusters or overconstrained
multi-body systems, the base body serves only as a reference body for managing the system's
pose in the world, and can thus be assigned arbitrarily to any body in the system.
"""
base_joint_idx: int | None = None
"""
Index of the base joint w.r.t. the world, i.e. the joint connecting the base body to the world.\n
See `base_body_idx` for more details.
"""
@property
def has_base_body(self):
"""Whether the world has an assigned base body."""
return self.base_body_idx is not None
@property
def base_body_name(self):
"""Name of the base body if set, otherwise empty string"""
return self.body_names[self.base_body_idx] if self.base_body_idx is not None else ""
@property
def has_base_joint(self):
"""Whether the world has an assigned base joint."""
return self.base_joint_idx is not None
@property
def base_joint_name(self):
"""Name of the base joint if set, otherwise empty string"""
return self.joint_names[self.base_joint_idx] if self.base_joint_idx is not None else ""
has_passive_dofs: bool = False
"""Whether the world has passive DoFs."""
has_actuated_dofs: bool = False
"""Whether the world has actuated DoFs."""
has_implicit_dofs: bool = False
"""Whether the world has implicit DoFs."""
###
# Inertial Properties
###
mass_min: float = math.inf
"""Smallest mass of any body in the world."""
mass_max: float = 0.0
"""Largest mass of any body in the world."""
mass_total: float = 0.0
"""Total mass of all bodies in the world."""
inertia_total: float = 0.0
"""
Total diagonal inertia over all bodies in the world.\n
Equals the trace of the maximal-coordinate generalized mass matrix of the world.
"""
###
# Operations
###
def add_body(self, body: RigidBodyDescriptor):
# Check if the body has already been added to a world
if body.name in self.body_names:
raise ValueError(f"Body name '{body.name}' already exists in world '{self.name}' ({self.wid}).")
if body.uid in self.body_uids:
raise ValueError(f"Body UID '{body.uid}' already exists in world '{self.name}' ({self.wid}).")
# Assign body metadata based on the current contents of the world
body.wid = self.wid
body.bid = self.num_bodies
# Append body info to world metadata
self.body_names.append(body.name)
self.body_uids.append(body.uid)
# Update body entity counts
self.num_bodies += 1
self.num_body_coords += 7
self.num_body_dofs += 6
# Append body inertial properties to world totals
self.mass_min = min(self.mass_min, body.m_i)
self.mass_max = max(self.mass_max, body.m_i)
self.mass_total += body.m_i
self.inertia_total += 3.0 * body.m_i + float(body.i_I_i[0, 0] + body.i_I_i[1, 1] + body.i_I_i[2, 2])
def add_joint(self, joint: JointDescriptor):
# Check if the joint has already been added to a world
if joint.name in self.joint_names:
raise ValueError(f"Joint name '{joint.name}' already exists in world '{self.name}' ({self.wid}).")
if joint.uid in self.joint_uids:
raise ValueError(f"Joint UID '{joint.uid}' already exists in world '{self.name}' ({self.wid}).")
# Check if the specified Base-Follower body indices are valid
if joint.bid_F < 0:
raise ValueError(
f"Invalid follower body index: bid_F={joint.bid_F}.\n\
- ==-1 indicates the world body, >=0 indicates finite rigid bodies\n\
- Follower BIDs must be in [0, {self.num_bodies - 1}]"
)
if joint.bid_B >= self.num_bodies or joint.bid_F >= self.num_bodies:
raise ValueError(
f"Invalid body indices: bid_B={joint.bid_B}, bid_F={joint.bid_F}.\n\
- ==-1 indicates the world body, >=0 indicates finite rigid bodies\n\
- Base BIDs must be in [-1, {self.num_bodies - 1}]\n\
- Follower BIDs must be in [0, {self.num_bodies - 1}]"
)
# Assign joint metadata based on the current contents of the world
joint.wid = int(self.wid)
joint.jid = int(self.num_joints)
joint.coords_offset = int(self.num_joint_coords)
joint.dofs_offset = int(self.num_joint_dofs)
joint.passive_coords_offset = int(self.num_passive_joint_coords) if joint.is_passive else -1
joint.passive_dofs_offset = int(self.num_passive_joint_dofs) if joint.is_passive else -1
joint.actuated_coords_offset = int(self.num_actuated_joint_coords) if joint.is_actuated else -1
joint.actuated_dofs_offset = int(self.num_actuated_joint_dofs) if joint.is_actuated else -1
joint.cts_offset = int(self.num_joint_cts)
joint.dynamic_cts_offset = int(self.num_dynamic_joint_cts) if joint.num_dynamic_cts > 0 else -1
joint.kinematic_cts_offset = int(self.num_kinematic_joint_cts)
# Append joint identifiers
self.joint_names.append(joint.name)
self.joint_uids.append(joint.uid)
# Append joint dimensions
self.joint_coords.append(joint.num_coords)
self.joint_dofs.append(joint.num_dofs)
self.joint_kinematic_cts.append(joint.num_kinematic_cts)
# Update joint entity counts
self.num_joints += 1
self.num_joint_coords += joint.num_coords
self.num_joint_dofs += joint.num_dofs
self.num_joint_cts += joint.num_cts
self.num_dynamic_joint_cts += joint.num_dynamic_cts
self.num_kinematic_joint_cts += joint.num_kinematic_cts
# Append joint connection group info
if joint.bid_B < 0:
self.unary_joint_names.append(joint.name)
# Append joint DoF group info
if joint.dof_type == JointDoFType.FIXED:
self.fixed_joint_names.append(joint.name)
# Append joint control group info
if joint.act_type == JointActuationType.PASSIVE:
joint.passive_coords_offset = int(self.num_passive_joint_coords)
joint.passive_dofs_offset = int(self.num_passive_joint_dofs)
self.has_passive_dofs = True
self.num_passive_joints += 1
self.num_passive_joint_coords += joint.num_coords
self.num_passive_joint_dofs += joint.num_dofs
self.joint_passive_coords.append(joint.num_coords)
self.joint_passive_dofs.append(joint.num_dofs)
self.passive_joint_names.append(joint.name)
else:
joint.actuated_coords_offset = int(self.num_actuated_joint_coords)
joint.actuated_dofs_offset = int(self.num_actuated_joint_dofs)
self.has_actuated_dofs = True
self.num_actuated_joints += 1
self.num_actuated_joint_coords += joint.num_coords
self.num_actuated_joint_dofs += joint.num_dofs
self.joint_actuated_coords.append(joint.num_coords)
self.joint_actuated_dofs.append(joint.num_dofs)
self.actuated_joint_names.append(joint.name)
# Append joint dynamics group info
if joint.num_dynamic_cts > 0:
self.has_implicit_dofs = True
self.num_dynamic_joints += 1
self.joint_dynamic_cts.append(joint.num_dynamic_cts)
self.dynamic_joint_names.append(joint.name)
def add_geometry(self, geom: GeometryDescriptor):
# Check if the geometry has already been added to a world
if geom.name in self.geom_names:
raise ValueError(f"Geometry name '{geom.name}' already exists in world '{self.name}' ({self.wid}).")
if geom.uid in self.geom_uids:
raise ValueError(f"Geometry UID '{geom.uid}' already exists in world '{self.name}' ({self.wid}).")
# Assign geometry metadata based on the current contents of the world
geom.wid = self.wid
geom.gid = self.num_geoms
# Update geometry entity counts
self.num_geoms += 1
# Append geometry info
self.geom_names.append(geom.name)
self.geom_uids.append(geom.uid)
self.geometry_max_contacts.append(geom.max_contacts)
def add_material(self, material: MaterialDescriptor):
# Check if the material has already been added to a world
if material.name in self.material_names:
raise ValueError(f"Material name '{material.name}' already exists in world '{self.name}' ({self.wid}).")
if material.uid in self.material_uids:
raise ValueError(f"Material UID '{material.uid}' already exists in world '{self.name}' ({self.wid}).")
# Assign material metadata based on the current contents of the world
material.wid = self.wid
material.mid = self.num_materials
# Update material entity counts
self.num_materials += 1
# # nm=1 -> nmp=1: (0, 0)
# # nm=2 -> nmp=3: (0, 0), (0, 1), (1, 1)
# # nm=3 -> nmp=6: (0, 0), (0, 1), (0, 2), (1, 1), (1, 2), (2, 2)
# # nm=N -> nmp=N*(N+1)/2
# self.num_material_pairs = self.num_materials * (self.num_materials + 1) // 2
# Append material info
self.material_names.append(material.name)
self.material_uids.append(material.uid)
def set_material(self, material: MaterialDescriptor, index: int):
# Ensure index is valid
if index < 0 or index >= self.num_materials:
raise ValueError(f"Material index '{index}' out of range. Must be between 0 and {self.num_materials - 1}.")
# Assign material metadata based on the current contents of the world
material.wid = self.wid
material.mid = index
# Set material info
self.material_names[index] = material.name
self.material_uids[index] = material.uid
def set_base_body(self, body_idx: int):
# Ensure no different base body was already set
if self.has_base_body and self.base_body_idx != body_idx:
raise ValueError(
f"World '{self.name}' ({self.wid}) already has a base body "
f"assigned as '{self.body_names[self.base_body_idx]}' ({self.base_body_idx})."
)
# Ensure index is valid
if body_idx < 0 or body_idx >= self.num_bodies:
raise ValueError(f"Base body index '{body_idx}' out of range. Must be between 0 and {self.num_bodies - 1}.")
# Set base body index
self.base_body_idx = body_idx
def set_base_joint(self, joint_idx: int):
# Ensure no different base joint was already set
if self.has_base_joint and self.base_joint_idx != joint_idx:
raise ValueError(
f"World '{self.name}' ({self.wid}) already has a base joint "
f"assigned as '{self.joint_names[self.base_joint_idx]}' ({self.base_joint_idx})."
)
# Ensure index is valid
if joint_idx < 0 or joint_idx >= self.num_joints:
raise ValueError(
f"Base joint index '{joint_idx}' out of range. Must be between 0 and {self.num_joints - 1}."
)
# Ensure joint is unary
if self.joint_names[joint_idx] not in self.unary_joint_names:
raise ValueError(
f"Base joint name '{self.joint_names[joint_idx]}' not found in the registry of unary joints."
)
# Set base joint index
self.base_joint_idx = joint_idx
================================================
FILE: newton/_src/solvers/kamino/_src/dynamics/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""The Kamino Dynamics Module"""
from .delassus import DelassusOperator
from .dual import DualProblem, DualProblemData
from .wrenches import compute_constraint_body_wrenches, compute_joint_dof_body_wrenches
###
# Module interface
###
__all__ = [
"DelassusOperator",
"DualProblem",
"DualProblemData",
"compute_constraint_body_wrenches",
"compute_joint_dof_body_wrenches",
]
================================================
FILE: newton/_src/solvers/kamino/_src/dynamics/delassus.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides containers to represent and operate Delassus operators.
A Delassus operator is a symmetric semi-positive-definite matrix that
represents the apparent inertia within the space defined by the set of
active constraints imposed on a constrained rigid multi-body system.
This module thus provides building-blocks to realize Delassus operators across multiple
worlds contained in a :class:`ModelKamino`. The :class:`DelassusOperator` class provides a
high-level interface to encapsulate both the data representation as well as the
relevant operations. It provides methods to allocate the necessary data arrays, build
the Delassus matrix given the current state of the model and the active constraints,
add diagonal regularization, and solve linear systems of the form `D @ x = v` given
arrays holding the right-hand-side (rhs) vectors v. Moreover, it supports the use of
different linear solvers as a back-end for performing the aforementioned linear system
solve. Construction of the Delassus operator is realized using a set of Warp kernels
that parallelize the computation using various strategies.
Typical usage example:
# Create a model builder and add bodies, joints, geoms, etc.
builder = ModelBuilder()
...
# Create a model from the builder and construct additional
# containers to hold joint-limits, contacts, Jacobians
model = builder.finalize()
data = model.data()
limits = LimitsKamino(model)
contacts = ContactsKamino(builder)
jacobians = DenseSystemJacobians(model, limits, contacts)
# Define a linear solver type to use as a back-end for the
# Delassus operator computations such as factorization and
# solving the linear system when a rhs vector is provided
linear_solver = LLTBlockedSolver
...
# Build the Jacobians for the model and active limits and contacts
jacobians.build(model, data, limits, contacts)
...
# Create a Delassus operator and build it using the current model data
# and active unilateral constraints (i.e. for limits and contacts).
delassus = DelassusOperator(model, limits, contacts, linear_solver)
delassus.build(model, data, jacobians)
# Add diagonal regularization the Delassus matrix
eta = ...
delassus.regularize(eta=eta)
# Factorize the Delassus matrix using the Cholesky factorization
delassus.compute()
# Solve a linear system using the Delassus operator
rhs = ...
solution = ...
delassus.solve(b=rhs, x=solution)
"""
import copy
import functools
from typing import Any
import numpy as np
import warp as wp
from ..core.data import DataKamino
from ..core.model import ModelKamino
from ..core.size import SizeKamino
from ..core.types import FloatType, float32, int32, mat33f, vec3f, vec6f
from ..geometry.contacts import ContactsKamino
from ..kinematics.constraints import get_max_constraints_per_world
from ..kinematics.jacobians import ColMajorSparseConstraintJacobians, DenseSystemJacobians, SparseSystemJacobians
from ..kinematics.limits import LimitsKamino
from ..linalg import DenseLinearOperatorData, DenseSquareMultiLinearInfo, LinearSolverType
from ..linalg.linear import IterativeSolver
from ..linalg.sparse_matrix import BlockDType, BlockSparseMatrices
from ..linalg.sparse_operator import BlockSparseLinearOperators
###
# Module interface
###
__all__ = [
"BlockSparseMatrixFreeDelassusOperator",
"DelassusOperator",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _build_delassus_elementwise_dense(
# Inputs:
model_info_num_bodies: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
model_bodies_inv_m_i: wp.array(dtype=float32),
data_bodies_inv_I_i: wp.array(dtype=mat33f),
jacobians_cts_offset: wp.array(dtype=int32),
jacobians_cts_data: wp.array(dtype=float32),
delassus_dim: wp.array(dtype=int32),
delassus_mio: wp.array(dtype=int32),
# Outputs:
delassus_D: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index and Delassus element index
wid, tid = wp.tid()
# Retrieve the world dimensions
nb = model_info_num_bodies[wid]
bio = model_info_bodies_offset[wid]
# Retrieve the problem dimensions
ncts = delassus_dim[wid]
# Skip if world has no constraints
if ncts == 0:
return
# Compute i (row) and j (col) indices from the tid
i = tid // ncts
j = tid % ncts
# Skip if indices exceed the problem size
if i >= ncts or j >= ncts:
return
# Retrieve the world's matrix offsets
dmio = delassus_mio[wid]
cjmio = jacobians_cts_offset[wid]
# Compute the number of body DoFs of the world
nbd = 6 * nb
# Buffers
# tmp = vec3f(0.0)
Jv_i = vec3f(0.0)
Jv_j = vec3f(0.0)
Jw_i = vec3f(0.0)
Jw_j = vec3f(0.0)
D_ij = float32(0.0)
D_ji = float32(0.0)
# Loop over rigid body blocks
# NOTE: k is the body index w.r.t the world
for k in range(nb):
# Body index (bid) of body k w.r.t the model
bid_k = bio + k
# DoF index offset (dio) of body k in the flattened Jacobian matrix
# NOTE: Equivalent to the column index in the matrix-form of the Jacobian matrix
dio_k = 6 * k
# Jacobian index offsets
jio_ik = cjmio + nbd * i + dio_k
jio_jk = cjmio + nbd * j + dio_k
# Load the Jacobian blocks of body k
for d in range(3):
# Load the i-th row block
Jv_i[d] = jacobians_cts_data[jio_ik + d]
Jw_i[d] = jacobians_cts_data[jio_ik + d + 3]
# Load the j-th row block
Jv_j[d] = jacobians_cts_data[jio_jk + d]
Jw_j[d] = jacobians_cts_data[jio_jk + d + 3]
# Linear term: inv_m_k * dot(Jv_i, Jv_j)
inv_m_k = model_bodies_inv_m_i[bid_k]
lin_ij = inv_m_k * wp.dot(Jv_i, Jv_j)
lin_ji = inv_m_k * wp.dot(Jv_j, Jv_i)
# Angular term: dot(Jw_i.T * I_k, Jw_j)
inv_I_k = data_bodies_inv_I_i[bid_k]
ang_ij = wp.dot(Jw_i, inv_I_k @ Jw_j)
ang_ji = wp.dot(Jw_j, inv_I_k @ Jw_i)
# Accumulate
D_ij += lin_ij + ang_ij
D_ji += lin_ji + ang_ji
# Store the result in the Delassus matrix
delassus_D[dmio + ncts * i + j] = 0.5 * (D_ij + D_ji)
@wp.kernel
def _build_delassus_elementwise_sparse(
# Inputs:
model_info_bodies_offset: wp.array(dtype=int32),
model_bodies_inv_m_i: wp.array(dtype=float32),
data_bodies_inv_I_i: wp.array(dtype=mat33f),
jacobian_cts_num_nzb: wp.array(dtype=int32),
jacobian_cts_nzb_start: wp.array(dtype=int32),
jacobian_cts_nzb_coords: wp.array2d(dtype=int32),
jacobian_cts_nzb_values: wp.array(dtype=vec6f),
delassus_dim: wp.array(dtype=int32),
delassus_mio: wp.array(dtype=int32),
# Outputs:
delassus_D: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index and Jacobian block index pair
wid, tid = wp.tid()
# Retrieve the problem dimensions
ncts = delassus_dim[wid]
# Skip if world has no constraints
if ncts == 0:
return
# Retrieve the world dimensions
bio = model_info_bodies_offset[wid]
# Retrieve the number of non-zero blocks
num_nzb = jacobian_cts_num_nzb[wid]
# Compute Jacobian block indices from the tid
block_id_i = tid // num_nzb
block_id_j = tid % num_nzb
# Skip if index exceeds problem size
if block_id_i >= num_nzb:
return
nzb_start = jacobian_cts_nzb_start[wid]
global_block_id_i = nzb_start + block_id_i
global_block_id_j = nzb_start + block_id_j
# Get block coordinates
block_coords_i = jacobian_cts_nzb_coords[global_block_id_i]
block_coords_j = jacobian_cts_nzb_coords[global_block_id_j]
# Skip if blocks don't affect the same body
if block_coords_i[1] != block_coords_j[1]:
return
# Body index (bid) of body k w.r.t the model, from Jacobian block coords
bid_k = bio + block_coords_i[1] // 6
# Get block values
block_i = jacobian_cts_nzb_values[global_block_id_i]
block_j = jacobian_cts_nzb_values[global_block_id_j]
# Retrieve the world's matrix offsets
dmio = delassus_mio[wid]
# Load the Jacobian blocks components for body
Jv_i = vec3f(block_i[0], block_i[1], block_i[2])
Jv_j = vec3f(block_j[0], block_j[1], block_j[2])
Jw_i = vec3f(block_i[3], block_i[4], block_i[5])
Jw_j = vec3f(block_j[3], block_j[4], block_j[5])
# Linear term: inv_m_k * dot(Jv_i, Jv_j)
inv_m_k = model_bodies_inv_m_i[bid_k]
lin_ij = inv_m_k * wp.dot(Jv_i, Jv_j)
lin_ji = inv_m_k * wp.dot(Jv_j, Jv_i)
# Angular term: dot(Jw_i.T * I_k, Jw_j)
inv_I_k = data_bodies_inv_I_i[bid_k]
ang_ij = wp.dot(Jw_i, inv_I_k @ Jw_j)
ang_ji = wp.dot(Jw_j, inv_I_k @ Jw_i)
# Compute sum
D_ij = lin_ij + ang_ij
D_ji = lin_ji + ang_ji
# Store the result in the Delassus matrix
wp.atomic_add(delassus_D, dmio + ncts * block_coords_i[0] + block_coords_j[0], 0.5 * (D_ij + D_ji))
@wp.kernel
def _add_joint_armature_diagonal_regularization_dense(
# Inputs:
model_info_num_joint_dynamic_cts: wp.array(dtype=int32),
model_info_joint_dynamic_cts_offset: wp.array(dtype=int32),
model_joint_inv_m_j: wp.array(dtype=float32),
delassus_dim: wp.array(dtype=int32),
delassus_mio: wp.array(dtype=int32),
# Outputs:
delassus_D: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index and Delassus element index
wid, tid = wp.tid()
# Retrieve the world dimensions
num_joint_dyn_cts = model_info_num_joint_dynamic_cts[wid]
# Skip if world has no dynamic joint constraints or indices exceed the problem size
if num_joint_dyn_cts == 0 or tid >= num_joint_dyn_cts:
return
# Retrieve the world's Delassus matrix dimension and offset
ncts = delassus_dim[wid]
dmio = delassus_mio[wid]
# Retrieve the dynamic constraint index offset of the world
world_joint_dynamic_cts_offset = model_info_joint_dynamic_cts_offset[wid]
# Retrieve the joint's inverse mass for armature regularization
inv_m_j = model_joint_inv_m_j[world_joint_dynamic_cts_offset + tid]
# Add the armature regularization to the diagonal element of the Delassus matrix
delassus_D[dmio + ncts * tid + tid] += inv_m_j
@wp.kernel
def _regularize_delassus_diagonal_dense(
# Inputs:
delassus_dim: wp.array(dtype=int32),
delassus_vio: wp.array(dtype=int32),
delassus_mio: wp.array(dtype=int32),
eta: wp.array(dtype=float32),
# Outputs:
delassus_D: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the problem dimensions and matrix block index offset
dim = delassus_dim[wid]
vio = delassus_vio[wid]
mio = delassus_mio[wid]
# Skip if row index exceed the problem size
if tid >= dim:
return
# Regularize the diagonal element
delassus_D[mio + dim * tid + tid] += eta[vio + tid]
@wp.kernel
def _merge_inv_mass_matrix_kernel(
model_info_bodies_offset: wp.array(dtype=int32),
model_bodies_inv_m_i: wp.array(dtype=float32),
data_bodies_inv_I_i: wp.array(dtype=mat33f),
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=vec6f),
):
"""
Kernel to merge the inverse mass matrix into an existing sparse matrix, so that the resulting
matrix is given as `A <- A @ M^-1`.
"""
mat_id, block_idx = wp.tid()
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
body_id = block_coord[1] // 6
# Index of body w.r.t the model
global_body_id = model_info_bodies_offset[mat_id] + body_id
# Load the inverse mass and inverse inertia for this body
inv_m = model_bodies_inv_m_i[global_body_id]
inv_I = data_bodies_inv_I_i[global_body_id]
# Apply inverse mass matrices to Jacobian block
v = inv_m * vec3f(block[0], block[1], block[2])
w = inv_I @ vec3f(block[3], block[4], block[5])
# Write back values
block[0] = v[0]
block[1] = v[1]
block[2] = v[2]
block[3] = w[0]
block[4] = w[1]
block[5] = w[2]
nzb_values[global_block_idx] = block
@functools.cache
def _make_merge_preconditioner_kernel(block_type: BlockDType):
"""
Generates a kernel to merge the (diagonal) preconditioning into a sparse matrix.
This effectively applies the preconditioning to the left (row-space) of the Jacobian.
"""
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def merge_preconditioner_kernel(
# Inputs:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
row_start: wp.array(dtype=int32),
preconditioner: wp.array(dtype=float32),
# Outputs:
nzb_values: wp.array(dtype=block_type.warp_type),
):
mat_id, block_idx = wp.tid()
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
if wp.static(n_block_rows == 1):
vec_coord = block_coord[0] + row_start[mat_id]
p_value = preconditioner[vec_coord]
block = block * p_value
else:
vec_coord_start = block_coord[0] + row_start[mat_id]
for i in range(n_block_rows):
p_value = preconditioner[vec_coord_start + i]
for j in range(n_block_cols):
block[i, j] = block[i, j] * p_value
nzb_values[global_block_idx] = block
return merge_preconditioner_kernel
@wp.kernel
def _add_armature_regularization_sparse(
# Inputs:
model_info_num_joint_dynamic_cts: wp.array(dtype=int32),
model_info_joint_dynamic_cts_offset: wp.array(dtype=int32),
row_start: wp.array(dtype=int32),
model_joint_inv_m_j: wp.array(dtype=float32),
# Outputs:
combined_regularization: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index and joint dynamics index
wid, tid = wp.tid()
# Retrieve the world dimensions
num_joint_dyn_cts = model_info_num_joint_dynamic_cts[wid]
# Skip if world has no dynamic joint constraints or indices exceed the problem size
if num_joint_dyn_cts == 0 or tid >= num_joint_dyn_cts:
return
# Retrieve the dynamic constraint index offset of the world
world_joint_dynamic_cts_offset = model_info_joint_dynamic_cts_offset[wid]
# Retrieve the joint's inverse mass for armature regularization
inv_m_j = model_joint_inv_m_j[world_joint_dynamic_cts_offset + tid]
# Get the index into the regularization
vec_id = row_start[wid] + tid
# Add the armature regularization
combined_regularization[vec_id] += inv_m_j
@wp.kernel
def _add_armature_regularization_preconditioned_sparse(
# Inputs:
model_info_num_joint_dynamic_cts: wp.array(dtype=int32),
model_info_joint_dynamic_cts_offset: wp.array(dtype=int32),
model_joint_inv_m_j: wp.array(dtype=float32),
row_start: wp.array(dtype=int32),
preconditioner: wp.array(dtype=float32),
# Outputs:
combined_regularization: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index and joint dynamics index
wid, tid = wp.tid()
# Retrieve the world dimensions
num_joint_dyn_cts = model_info_num_joint_dynamic_cts[wid]
# Skip if world has no dynamic joint constraints or indices exceed the problem size
if num_joint_dyn_cts == 0 or tid >= num_joint_dyn_cts:
return
# Retrieve the dynamic constraint index offset of the world
world_joint_dynamic_cts_offset = model_info_joint_dynamic_cts_offset[wid]
# Retrieve the joint's inverse mass for armature regularization
inv_m_j = model_joint_inv_m_j[world_joint_dynamic_cts_offset + tid]
# Get the index into the preconditioner and regularization
vec_id = row_start[wid] + tid
# Retrieve preconditioner value
p = preconditioner[vec_id]
# Add the armature regularization
combined_regularization[vec_id] += p * p * inv_m_j
@wp.kernel
def _compute_block_sparse_delassus_diagonal(
# Inputs:
model_info_bodies_offset: wp.array(dtype=int32),
model_bodies_inv_m_i: wp.array(dtype=float32),
data_bodies_inv_I_i: wp.array(dtype=mat33f),
bsm_nzb_start: wp.array(dtype=int32),
bsm_num_nzb: wp.array(dtype=int32),
bsm_nzb_coords: wp.array2d(dtype=int32),
bsm_nzb_values: wp.array(dtype=vec6f),
vec_start: wp.array(dtype=int32),
# Outputs:
diag: wp.array(dtype=float32),
):
"""
Computes the diagonal entries of the Delassus matrix by summing up the contributions of each
non-zero block of the Jacobian: D_ii = sum_k J_ik @ M_kk^-1 @ (J_ik)^T
This kernel processes one non-zero block per thread and accumulates all contributions.
"""
# Retrieve the thread index as the world index and block index
world_id, block_idx_local = wp.tid()
# Skip if block index exceeds the number of non-zero blocks
if block_idx_local >= bsm_num_nzb[world_id]:
return
# Compute the global block index
block_idx = bsm_nzb_start[world_id] + block_idx_local
# Get the row and column for this block
row = bsm_nzb_coords[block_idx, 0]
col = bsm_nzb_coords[block_idx, 1]
# Get the body index offset for this world
body_index_offset = model_info_bodies_offset[world_id]
# Get the Jacobian block and extract linear and angular components
J_block = bsm_nzb_values[block_idx]
Jv = J_block[0:3]
Jw = J_block[3:6]
# Get the body index from the column
body_idx = col // 6
body_idx_global = body_index_offset + body_idx
# Load the inverse mass and inverse inertia for this body
inv_m = model_bodies_inv_m_i[body_idx_global]
inv_I = data_bodies_inv_I_i[body_idx_global]
# Compute linear contribution: Jv^T @ inv_m @ Jv
diag_kk = inv_m * wp.dot(Jv, Jv)
# Compute angular contribution: Jw^T @ inv_I @ Jw
diag_kk += wp.dot(Jw, inv_I @ Jw)
# Atomically add contribution to the diagonal element
wp.atomic_add(diag, vec_start[world_id] + row, diag_kk)
@wp.kernel
def _add_matrix_diag_product(
model_data_num_total_cts: wp.array(dtype=int32),
row_start: wp.array(dtype=int32),
d: wp.array(dtype=float32),
x: wp.array(dtype=float32),
y: wp.array(dtype=float32),
alpha: float,
world_mask: wp.array(dtype=int32),
):
"""
Adds the product of a vector with a diagonal matrix to another vector: y += alpha * diag(d) @ x
This is used to apply a regularization to the Delassus matrix-vector product.
"""
# Retrieve the thread index as the world index and constraint index
world_id, ct_id = wp.tid()
# Terminate early if world or constraint is inactive
if world_mask[world_id] == 0 or ct_id >= model_data_num_total_cts[world_id]:
return
idx = row_start[world_id] + ct_id
y[idx] += alpha * d[idx] * x[idx]
@wp.kernel
def _scale_row_vector_kernel(
# Matrix data:
matrix_dims: wp.array2d(dtype=int32),
# Vector block offsets:
row_start: wp.array(dtype=int32),
# Inputs:
x: wp.array(dtype=Any),
beta: Any,
# Mask:
matrix_mask: wp.array(dtype=int32),
):
"""
Computes a vector scaling for all active entries: y = beta * y
"""
mat_id, entry_id = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0 or entry_id >= matrix_dims[mat_id, 0]:
return
idx = row_start[mat_id] + entry_id
x[idx] = beta * x[idx]
@functools.cache
def _make_block_sparse_gemv_regularization_kernel(alpha: float32):
# Note: this kernel factory allows to optimize for the common case alpha = 1.0. In use cases where
# alpha changes over time, this would need to be revisited (to avoid multiple recompilations)
@wp.kernel
def _block_sparse_gemv_regularization_kernel(
# Matrix data:
dims: wp.array2d(dtype=int32),
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=vec6f),
# Vector block offsets:
row_start: wp.array(dtype=int32),
col_start: wp.array(dtype=int32),
# Regularization:
eta: wp.array(dtype=float32),
# Vector:
x: wp.array(dtype=float32),
y: wp.array(dtype=float32),
z: wp.array(dtype=float32),
# Mask:
matrix_mask: wp.array(dtype=int32),
):
"""
Computes a generalized matrix-vector product with an added diagonal regularization component:
y <- y + alpha * (M @ x) + alpha * (diag(eta) @ z)
"""
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: z += alpha * A_block @ x_block
x_idx_base = col_start[mat_id] + block_coord[1]
acc = float32(0.0)
for j in range(6):
acc += block[j] * x[x_idx_base + j]
if wp.static(alpha != 1.0):
acc *= alpha
wp.atomic_add(y, row_start[mat_id] + block_coord[0], acc)
if block_idx < dims[mat_id][0]:
vec_idx = row_start[mat_id] + block_idx
if wp.static(alpha != 1.0):
vec_val = z[vec_idx] * alpha * eta[vec_idx]
else:
vec_val = z[vec_idx] * eta[vec_idx]
wp.atomic_add(y, vec_idx, vec_val)
return _block_sparse_gemv_regularization_kernel
###
# Interfaces
###
class DelassusOperator:
"""
A container to represent the Delassus matrix operator.
"""
def __init__(
self,
model: ModelKamino | None = None,
data: DataKamino | None = None,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
solver: LinearSolverType = None,
solver_kwargs: dict[str, Any] | None = None,
device: wp.DeviceLike = None,
):
"""
Creates a Delassus operator for the given model, limits and contacts containers.
This class also supports deferred allocation, i.e. it can be initialized without
a model, limits, or contacts, and the allocation can be performed later using the
`allocate` method. This is useful for scenarios where the model or constraints are
not known at the time of Delassus operator creation, but will be available later.
The dimension of a Delassus matrix is defined as the sum over active
joint, limit, and contact constraints, and the maximum dimension is
the maximum number of constraints that can be active in each world.
Args:
model (ModelKamino): The model container for which the Delassus operator is built.
data (DataKamino, optional): The model data container holding the state info and data.
limits (LimitsKamino, optional): The container holding the allocated joint-limit data.
contacts (ContactsKamino, optional): The container holding the allocated contacts data.
device (wp.DeviceLike, optional): The device identifier for the Delassus operator. Defaults to None.
factorizer (CholeskyFactorizer, optional): An optional Cholesky factorization object. Defaults to None.
"""
# Declare and initialize the host-side cache of the necessary memory allocations
self._num_worlds: int = 0
self._model_maxdims: int = 0
self._model_maxsize: int = 0
self._world_maxdims: list[int] = []
self._world_maxsize: list[int] = []
self._max_of_max_total_D_size: int = 0
# Cache the requested device
self._device: wp.DeviceLike = device
# Declare the model size cache
self._size: SizeKamino | None = None
# Initialize the Delassus data container
self._operator: DenseLinearOperatorData | None = None
# Declare the optional Cholesky factorization
self._solver: LinearSolverType | None = None
# Allocate the Delassus operator data if at least the model is provided
if model is not None:
self.finalize(
model=model,
data=data,
limits=limits,
contacts=contacts,
solver=solver,
solver_kwargs=solver_kwargs,
device=device,
)
@property
def num_worlds(self) -> int:
"""
Returns the number of worlds represented by the Delassus operator.
This is equal to the number of matrix blocks contained in the flat array.
"""
return self._num_worlds
@property
def num_maxdims(self) -> int:
"""
Returns the maximum dimension of the Delassus matrix across all worlds.
This is the sum of per matrix block maximum dimensions.
"""
return self._model_maxdims
@property
def num_maxsize(self) -> int:
"""
Returns the maximum size of the Delassus matrix across all worlds.
This is the sum over the sizes of all matrix blocks.
"""
return self._model_maxsize
@property
def operator(self) -> DenseLinearOperatorData:
"""
Returns a reference to the flat Delassus matrix array.
"""
return self._operator
@property
def solver(self) -> LinearSolverType:
"""
The linear solver object for the Delassus operator.
This is used to perform the factorization of the Delassus matrix.
"""
return self._solver
@property
def info(self) -> DenseSquareMultiLinearInfo:
"""
Returns a reference to the flat Delassus matrix array.
"""
return self._operator.info
@property
def D(self) -> wp.array:
"""
Returns a reference to the flat Delassus matrix array.
"""
return self._operator.mat
def finalize(
self,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
solver: LinearSolverType = None,
device: wp.DeviceLike = None,
solver_kwargs: dict[str, Any] | None = None,
):
"""
Allocates the Delassus operator with the specified dimensions and device.
Args
----
dims (List[int]): The dimensions of the Delassus matrix for each world.
device (wp.DeviceLike, optional): The device identifier for the Delassus operator. Defaults to None.
factorizer (CholeskyFactorizer, optional): An optional Cholesky factorization object. Defaults to None.
"""
# Ensure the model container is valid
if model is None:
raise ValueError(
"A model container of type `ModelKamino` must be provided to allocate the Delassus operator."
)
elif not isinstance(model, ModelKamino):
raise ValueError("Invalid model provided. Must be an instance of `ModelKamino`.")
# Ensure the data container is valid if provided
if data is None:
raise ValueError(
"A data container of type `DataKamino` must be provided to allocate the Delassus operator."
)
elif not isinstance(data, DataKamino):
raise ValueError("Invalid data container provided. Must be an instance of `DataKamino`.")
# Ensure the limits container is valid if provided
if limits is not None:
if not isinstance(limits, LimitsKamino):
raise ValueError("Invalid limits container provided. Must be an instance of `LimitsKamino`.")
# Ensure the contacts container is valid if provided
if contacts is not None:
if not isinstance(contacts, ContactsKamino):
raise ValueError("Invalid contacts container provided. Must be an instance of `ContactsKamino`.")
# Capture reference to the model size
self._size = model.size
# Extract required maximum number of constraints for each world
maxdims = get_max_constraints_per_world(model, limits, contacts)
# Update the allocation meta-data the specified constraint dimensions
self._num_worlds = model.size.num_worlds
self._world_dims = maxdims
self._world_size = [maxdims[i] * maxdims[i] for i in range(self._num_worlds)]
self._model_maxdims = sum(self._world_dims)
self._model_maxsize = sum(self._world_size)
self._max_of_max_total_D_size = max(self._world_size)
# Override the device identifier if specified, otherwise use the current device
if device is not None:
self._device = device
# Construct the Delassus operator data structure
self._operator = DenseLinearOperatorData()
self._operator.info = DenseSquareMultiLinearInfo()
self._operator.mat = wp.zeros(shape=(self._model_maxsize,), dtype=float32, device=self._device)
if (model.info is not None) and (data.info is not None):
mat_offsets = [0] + [sum(self._world_size[:i]) for i in range(1, self._num_worlds + 1)]
self._operator.info.assign(
maxdim=model.info.max_total_cts,
dim=data.info.num_total_cts,
vio=model.info.total_cts_offset,
mio=wp.array(mat_offsets[: self._num_worlds], dtype=int32, device=self._device),
dtype=float32,
device=self._device,
)
else:
self._operator.info.finalize(dimensions=maxdims, dtype=float32, itype=int32, device=self._device)
# Optionally initialize the linear system solver if one is specified
if solver is not None:
if not issubclass(solver, LinearSolverType):
raise ValueError("Invalid solver provided. Must be a subclass of `LinearSolverType`.")
solver_kwargs = solver_kwargs or {}
self._solver = solver(operator=self._operator, device=self._device, **solver_kwargs)
def zero(self):
"""
Sets all values of the Delassus matrix to zero.
This is useful for resetting the operator before recomputing it.
"""
self._operator.mat.zero_()
def build(
self,
model: ModelKamino,
data: DataKamino,
jacobians: DenseSystemJacobians | SparseSystemJacobians,
reset_to_zero: bool = True,
):
"""
Builds the Delassus matrix using the provided ModelKamino, DataKamino, and constraint Jacobians.
Args:
model (ModelKamino): The model for which the Delassus operator is built.
data (DataKamino): The current data of the model.
jacobians (DenseSystemJacobians | SparseSystemJacobians): The current Jacobians of the model.
reset_to_zero (bool, optional): If True (default), resets the Delassus matrix to zero before building.
Raises:
ValueError: If the model, data, or Jacobians are not valid.
ValueError: If the Delassus matrix is not allocated.
"""
# Ensure the model is valid
if model is None or not isinstance(model, ModelKamino):
raise ValueError("A valid model of type `ModelKamino` must be provided to build the Delassus operator.")
# Ensure the data is valid
if data is None or not isinstance(data, DataKamino):
raise ValueError("A valid model data of type `DataKamino` must be provided to build the Delassus operator.")
# Ensure the Jacobians are valid
if jacobians is None or not (
isinstance(jacobians, DenseSystemJacobians) or isinstance(jacobians, SparseSystemJacobians)
):
raise ValueError(
"A valid Jacobians data container of type `DenseSystemJacobians` or "
"`SparseSystemJacobians` must be provided to build the Delassus operator."
)
# Ensure the Delassus matrix is allocated
if self._operator.mat is None:
raise ValueError("Delassus matrix is not allocated. Call finalize() first.")
# Initialize the Delassus matrix to zero
if reset_to_zero:
self.zero()
# Build the Delassus matrix parallelized element-wise
if isinstance(jacobians, DenseSystemJacobians):
wp.launch(
kernel=_build_delassus_elementwise_dense,
dim=(self._size.num_worlds, self._max_of_max_total_D_size),
inputs=[
# Inputs:
model.info.num_bodies,
model.info.bodies_offset,
model.bodies.inv_m_i,
data.bodies.inv_I_i,
jacobians.data.J_cts_offsets,
jacobians.data.J_cts_data,
self._operator.info.dim,
self._operator.info.mio,
# Outputs:
self._operator.mat,
],
)
else:
jacobian_cts = jacobians._J_cts.bsm
wp.launch(
kernel=_build_delassus_elementwise_sparse,
dim=(self._size.num_worlds, jacobian_cts.max_of_num_nzb * jacobian_cts.max_of_num_nzb),
inputs=[
# Inputs:
model.info.bodies_offset,
model.bodies.inv_m_i,
data.bodies.inv_I_i,
jacobian_cts.num_nzb,
jacobian_cts.nzb_start,
jacobian_cts.nzb_coords,
jacobian_cts.nzb_values,
self._operator.info.dim,
self._operator.info.mio,
# Outputs:
self._operator.mat,
],
)
# Add armature regularization to the upper diagonal if dynamic joint constraints are present
if model.size.sum_of_num_dynamic_joints > 0:
wp.launch(
kernel=_add_joint_armature_diagonal_regularization_dense,
dim=(self._size.num_worlds, model.size.max_of_num_dynamic_joint_cts),
inputs=[
# Inputs:
model.info.num_joint_dynamic_cts,
model.info.joint_dynamic_cts_offset,
data.joints.inv_m_j,
self._operator.info.dim,
self._operator.info.mio,
# Outputs:
self._operator.mat,
],
)
def regularize(self, eta: wp.array):
"""
Adds diagonal regularization to each matrix block of the Delassus operator.
Args:
eta (wp.array): The regularization values to add to the diagonal of each matrix block.
This should be an array of shape `(maxdims,)` and type :class:`float32`.
Each value in `eta` corresponds to the regularization along each constraint.
"""
wp.launch(
kernel=_regularize_delassus_diagonal_dense,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[self._operator.info.dim, self._operator.info.vio, self._operator.info.mio, eta, self._operator.mat],
)
def compute(self, reset_to_zero: bool = True):
"""
Runs Delassus pre-computation operations in preparation for linear systems solves.
Depending on the configured solver type, this may perform different
pre-computation, e.g. Cholesky factorization for direct solvers.
Args:
reset_to_zero (bool):
If True, resets the Delassus matrix to zero.\n
This is useful for ensuring that the matrix is in a clean state before pre-computation.
"""
# Ensure the Delassus matrix is allocated
if self._operator.mat is None:
raise ValueError("Delassus matrix is not allocated. Call finalize() first.")
# Ensure the solver is available if pre-computation is requested
if self._solver is None:
raise ValueError("A linear system solver is not available. Allocate with solver=LINEAR_SOLVER_TYPE.")
# Optionally initialize the factorization matrix before factorizing
if reset_to_zero:
self._solver.reset()
# Perform the Cholesky factorization
self._solver.compute(A=self._operator.mat)
def solve(self, v: wp.array, x: wp.array):
"""
Solves the linear system D * x = v using the Cholesky factorization.
Args:
v (wp.array): The right-hand side vector of the linear system.
x (wp.array): The array to hold the solution.
Raises:
ValueError: If the Delassus matrix is not allocated or the factorizer is not available.
ValueError: If a factorizer has not been configured set.
"""
# Ensure the Delassus matrix is allocated
if self._operator.mat is None:
raise ValueError("Delassus matrix is not allocated. Call finalize() first.")
# Ensure the solver is available if solving is requested
if self._solver is None:
raise ValueError("A linear system solver is not available. Allocate with solver=LINEAR_SOLVER_TYPE.")
# Solve the linear system using the factorized matrix
return self._solver.solve(b=v, x=x)
def solve_inplace(self, x: wp.array):
"""
Solves the linear system D * x = v in-place.\n
This modifies the input array x to contain the solution assuming it is initialized as x=v.
Args:
x (wp.array): The array to hold the solution. It should be initialized with the right-hand side vector v.
Raises:
ValueError: If the Delassus matrix is not allocated or the factorizer is not available.
ValueError: If a factorizer has not been configured set.
"""
# Ensure the Delassus matrix is allocated
if self._operator.mat is None:
raise ValueError("Delassus matrix is not allocated. Call finalize() first.")
# Ensure the solvers is available if solving in-place is requested
if self._solver is None:
raise ValueError("A linear system solver is not available. Allocate with solver=LINEAR_SOLVER_TYPE.")
# Solve the linear system in-place
return self._solver.solve_inplace(x=x)
class BlockSparseMatrixFreeDelassusOperator(BlockSparseLinearOperators):
"""
A matrix-free Delassus operator for representing and operating on multiple independent sparse
linear systems.
In contrast to the dense :class:`DelassusOperator`, this operator only provides functions to
compute matrix-vector products with the Delassus matrix, not solve linear systems.
The Delassus operator D is implicitly defined as D = J @ M^-1 @ J^T, where J is the constraint
Jacobian and M is the mass matrix. It supports diagonal regularization and diagonal
preconditioning.
For a given diagonal regularization matrix R and a diagonal preconditioning matrix P, the
final operator is defined by the matrix P @ D @ P + R.
Typical usage example:
.. code-block:: python
# Create a model builder and add bodies, joints, geoms, etc.
builder = ModelBuilder()
...
# Create a model from the builder and construct additional
# containers to hold joint-limits, contacts, Jacobians
model = builder.finalize()
data = model.data()
limits = LimitsKamino(model)
contacts = ContactsKamino(builder)
jacobians = SparseSystemJacobians(model, limits, contacts)
# Build the Jacobians for the model and active limits and contacts
jacobians.build(model, data, limits, contacts)
...
# Create a Delassus operator from the model data and Jacobians
delassus = BlockSparseMatrixFreeDelassusOperator(model, data, jacobians)
# Add diagonal regularization to the Delassus operator
eta = ...
delassus.set_regularization(eta=eta)
# Add preconditioning to the Delassus operator
P = ...
delassus.set_preconditioner(preconditioner=P)
# Compute the matrix-vector product `y = D @ x` using the Delassus operator
x = ...
y = ...
world_mask = ...
delassus.matvec(x=x, y=y, world_mask=world_mask)
"""
def __init__(
self,
model: ModelKamino | None = None,
data: DataKamino | None = None,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
jacobians: SparseSystemJacobians | None = None,
solver: LinearSolverType = None,
solver_kwargs: dict[str, Any] | None = None,
device: wp.DeviceLike = None,
):
"""
Creates a Delassus operator for the given model.
This class also supports deferred allocation, i.e. it can be initialized without a model,
and the allocation can be performed later using the `finalize` method. This is useful for
scenarios where the model or constraints are not known at the time of Delassus operator
creation, but will be available later.
The dimension of a Delassus matrix is defined as the sum over active joint, limit, and
contact constraints, and the maximum dimension is the maximum number of constraints that can
be active in each world.
Args:
model (ModelKamino):
The model container for which the Delassus operator is built.
data (DataKamino, optional):
The model data container holding the state info and data.
limits (LimitsKamino, optional):
Limits data container for joint limit constraints.
contacts (ContactsKamino, optional):
Contacts data container for contact constraints.
jacobians (SparseSystemJacobians, optional):
The sparse Jacobians container.
Can be assigned later via the `assign` method.
solver (LinearSolverType, optional):
The linear solver class to use for solving linear systems.
Must be a subclass of `IterativeSolver`.
solver_kwargs (dict, optional):
Additional keyword arguments to pass to the solver constructor.
device (wp.DeviceLike, optional):
The device identifier for the Delassus operator. Defaults to None.
"""
super().__init__()
# self.bsm represents the constraint Jacobian
self._model: ModelKamino | None = None
self._data: DataKamino | None = None
self._limits: LimitsKamino | None = None
self._contacts: ContactsKamino | None = None
self._preconditioner: wp.array | None = None
self._eta: wp.array | None = None
self._jacobians: SparseSystemJacobians | None = None
# Problem info object
# TODO: Create more general info object independent of dense matrix representation
self._info: DenseSquareMultiLinearInfo | None = None
# Cache the requested device
self._device: wp.DeviceLike = device
# Declare the optional (iterative) solver
self._solver: LinearSolverType | None = None
# Flag to indicate that the operator needs an update to its data structure
self._needs_update: bool = False
# Temporary vector to store results, sized to the number of body dofs in a model.
self._vec_temp_body_space: wp.array | None = None
self._col_major_jacobian: ColMajorSparseConstraintJacobians | None = None
self._transpose_op_matrix: BlockSparseMatrices | None = None
# Combined regularization vector for implicit joint dynamics
self._combined_regularization: wp.array | None = None
# Allocate the Delassus operator data if at least the model is provided
if model is not None:
self.finalize(
model=model,
data=data,
limits=limits,
contacts=contacts,
jacobians=jacobians,
solver=solver,
device=device,
solver_kwargs=solver_kwargs,
)
def finalize(
self,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None,
contacts: ContactsKamino | None,
jacobians: SparseSystemJacobians | None = None,
solver: LinearSolverType = None,
device: wp.DeviceLike = None,
solver_kwargs: dict[str, Any] | None = None,
):
"""
Allocates the Delassus operator with the specified dimensions and device.
Args:
model (ModelKamino):
The model container for which the Delassus operator is built.
data (DataKamino):
The model data container holding the state info and data.
limits (LimitsKamino, optional):
Limits data container for joint limit constraints.
contacts (ContactsKamino, optional):
Contacts data container for contact constraints.
jacobians (SparseSystemJacobians, optional):
The sparse Jacobians container.
Can be assigned later via the `assign` method.
solver (LinearSolverType, optional):
The linear solver class to use for solving linear systems.
Must be a subclass of `IterativeSolver`.
device (wp.DeviceLike, optional):
The device identifier for the Delassus operator.
Defaults to None.
solver_kwargs (dict, optional):
Additional keyword arguments to pass to the solver constructor.
"""
# Ensure the model container is valid
if model is None:
raise ValueError(
"A model container of type `ModelKamino` must be provided to allocate the Delassus operator."
)
elif not isinstance(model, ModelKamino):
raise ValueError("Invalid model provided. Must be an instance of `ModelKamino`.")
# Ensure the data container is valid if provided
if data is None:
raise ValueError(
"A data container of type `DataKamino` must be provided to allocate the Delassus operator."
)
elif not isinstance(data, DataKamino):
raise ValueError("Invalid data container provided. Must be an instance of `DataKamino`.")
# Ensure the solver is iterative if provided
if solver is not None and not issubclass(solver, IterativeSolver):
raise ValueError("Invalid solver provided. Must be a subclass of `IterativeSolver`.")
self._model = model
self._data = data
self._limits = limits
self._contacts = contacts
# Override the device identifier if specified, otherwise use the current device
if device is not None:
self._device = device
self._info = DenseSquareMultiLinearInfo()
if model.info is not None and data.info is not None:
self._info.assign(
maxdim=model.info.max_total_cts,
dim=data.info.num_total_cts,
vio=model.info.total_cts_offset,
mio=wp.empty((self.num_matrices,), dtype=int32, device=self._device),
dtype=float32,
device=self._device,
)
else:
self._info.finalize(
dimensions=model.info.max_total_cts.numpy(),
dtype=float32,
itype=int32,
device=self._device,
)
self._active_rows = wp.array(
dtype=wp.int32,
shape=(self._model.size.num_worlds,),
ptr=self._data.info.num_total_cts.ptr,
copy=False,
)
self._active_cols = wp.array(
dtype=wp.int32,
shape=(self._model.size.num_worlds,),
ptr=self._data.info.num_total_cts.ptr,
copy=False,
)
# Initialize temporary memory
self._vec_temp_body_space = wp.empty(
(self._model.size.sum_of_num_body_dofs,), dtype=float32, device=self._device
)
# Initialize memory for combined regularization, if necessary
if self._model.size.max_of_num_dynamic_joint_cts > 0:
self._combined_regularization = wp.empty(
(self._model.size.sum_of_max_total_cts,), dtype=float32, device=self._device
)
# Check whether any of the maximum row dimensions of the Jacobians is smaller than six.
# If so, we avoid building the column-major Jacobian due to potential memory access issues.
min_of_max_rows = np.min(self._model.info.max_total_cts.numpy())
if min_of_max_rows >= 6:
self._col_major_jacobian = ColMajorSparseConstraintJacobians(
model=self._model,
limits=self._limits,
contacts=self._contacts,
jacobians=self._jacobians,
device=self.device,
)
self._transpose_op_matrix = self._col_major_jacobian.bsm
else:
self._col_major_jacobian = None
# Assign Jacobian if specified
if jacobians is not None:
self.assign(jacobians)
# Optionally initialize the iterative linear system solver if one is specified
if solver is not None:
solver_kwargs = solver_kwargs or {}
self._solver = solver(operator=self, device=self._device, **solver_kwargs)
self.set_needs_update()
def assign(self, jacobians: SparseSystemJacobians):
"""
Assigns the constraint Jacobian to the Delassus operator.
This method links the Delassus operator to the block sparse Jacobian matrix,
which is used to compute the implicit Delassus matrix-vector products.
Args:
jacobians (SparseSystemJacobians):
The sparse system Jacobians container holding the
constraint Jacobian matrix in block sparse format.
"""
self._jacobians = jacobians
if self._col_major_jacobian is None and self._transpose_op_matrix is None:
# Create copy of constraint Jacobian with separate non-zero block values, so we can apply
# preconditioning directly to the Jacobian.
self._transpose_op_matrix = copy.copy(jacobians._J_cts.bsm)
self._transpose_op_matrix.nzb_values = wp.empty_like(self.constraint_jacobian.nzb_values)
# Create a shallow copy of the constraint Jacobian, but with a separate array for non-zero block values.
# The resulting sparse matrix will reference the structure of the original Jacobian, but we can apply
# preconditioning and the inverse mass matrix to the non-zero blocks without affecting the original Jacobian.
if self.bsm is None:
self.bsm = copy.copy(jacobians._J_cts.bsm)
self.bsm.nzb_values = wp.empty_like(self.constraint_jacobian.nzb_values)
self.update()
def set_needs_update(self):
"""
Flags the operator as needing to update its data structure.
"""
self._needs_update = True
def update(self):
"""
Updates any internal data structures that depend on the model, limits, contacts, or system Jacobians.
"""
if self._jacobians is None:
return
# Update column-major constraint Jacobian based on current system Jacobian
if self._col_major_jacobian is None:
wp.copy(self._transpose_op_matrix.nzb_values, self.constraint_jacobian.nzb_values)
else:
self._col_major_jacobian.update(self._model, self._jacobians, self._limits, self._contacts)
# Copy current Jacobian values to local constraint Jacobian
wp.copy(self.bsm.nzb_values, self.constraint_jacobian.nzb_values)
# Apply inverse mass matrix to (copy of) constraint Jacobian
wp.launch(
kernel=_merge_inv_mass_matrix_kernel,
dim=(self.bsm.num_matrices, self.bsm.max_of_num_nzb),
inputs=[
# Inputs:
self._model.info.bodies_offset,
self._model.bodies.inv_m_i,
self._data.bodies.inv_I_i,
self.bsm.num_nzb,
self.bsm.nzb_start,
self.bsm.nzb_coords,
# Outputs:
self.bsm.nzb_values,
],
device=self.bsm.device,
)
if self._preconditioner is not None:
# Apply preconditioner to (copy of) constraint Jacobian
wp.launch(
kernel=_make_merge_preconditioner_kernel(self.bsm.nzb_dtype),
dim=(self.bsm.num_matrices, self.bsm.max_of_num_nzb),
inputs=[
# Inputs:
self.bsm.num_nzb,
self.bsm.nzb_start,
self.bsm.nzb_coords,
self.bsm.row_start,
self._preconditioner,
# Outputs:
self.bsm.nzb_values,
],
device=self.bsm.device,
)
# Apply preconditioner to column-major constraint Jacobian
wp.launch(
kernel=_make_merge_preconditioner_kernel(self._transpose_op_matrix.nzb_dtype),
dim=(self._transpose_op_matrix.num_matrices, self._transpose_op_matrix.max_of_num_nzb),
inputs=[
# Inputs:
self._transpose_op_matrix.num_nzb,
self._transpose_op_matrix.nzb_start,
self._transpose_op_matrix.nzb_coords,
self._transpose_op_matrix.row_start,
self._preconditioner,
# Outputs:
self._transpose_op_matrix.nzb_values,
],
device=self._transpose_op_matrix.device,
)
# Update combined regularization term, which includes the regular regularization (eta) as
# well as the terms of the armature regularization. Since the armature regularization is
# applied to the original Delassus matrix, preconditioning has to be applied to it if
# present.
if self._combined_regularization is not None:
# Set the combined regularization to the regular regularization if present, otherwise
# initialize to zero.
if self._eta is not None:
wp.copy(self._combined_regularization, self._eta)
else:
self._combined_regularization.zero_()
if self._preconditioner is None:
# If there is no preconditioner, we add the armature regularization directly to the
# combined regularization term.
wp.launch(
kernel=_add_armature_regularization_sparse,
dim=(self.num_matrices, self._model.size.max_of_num_dynamic_joint_cts),
inputs=[
# Inputs:
self._model.info.num_joint_dynamic_cts,
self._model.info.joint_dynamic_cts_offset,
self.bsm.row_start,
self._data.joints.inv_m_j,
# Outputs:
self._combined_regularization,
],
device=self._device,
)
else:
# If there is a preconditioner, we need to scale the armature regularization with
# the preconditioner terms (the square of the preconditioner, to be exact) before
# adding it to the combined regularization term.
wp.launch(
kernel=_add_armature_regularization_preconditioned_sparse,
dim=(self.num_matrices, self._model.size.max_of_num_dynamic_joint_cts),
inputs=[
# Inputs:
self._model.info.num_joint_dynamic_cts,
self._model.info.joint_dynamic_cts_offset,
self._data.joints.inv_m_j,
self.bsm.row_start,
self._preconditioner,
# Outputs:
self._combined_regularization,
],
device=self._device,
)
self._needs_update = False
def set_regularization(self, eta: wp.array | None):
"""
Adds diagonal regularization to each matrix block of the Delassus operator, replacing any
previously set regularization.
The regularized Delassus matrix is defined as D = J @ M^-1 @ J^T + diag(eta)
Args:
eta (wp.array): The regularization values to add to the diagonal of each matrix block,
with each value corresponding to the regularization along a constraint.
This should be an array of shape `(sum_of_max_total_cts,)` and type :class:`float32`,
or `None` if no regularization should be applied.
"""
self._eta = eta
self.set_needs_update()
def set_preconditioner(self, preconditioner: wp.array | None):
"""
Sets the diagonal preconditioner for the Delassus operator, replacing any previously set
preconditioner.
With preconditioning, the effective operator becomes P @ D @ P, where P = diag(preconditioner).
Args:
preconditioner (wp.array): The diagonal preconditioner values to apply to the Delassus
operator, with each value corresponding to a constraint. This should be an array of
shape `(sum_of_max_total_cts,)` and type :class:`float32`, or `None` to disable
preconditioning.
"""
self._preconditioner = preconditioner
self.set_needs_update()
def diagonal(self, diag: wp.array):
"""Stores the diagonal of the Delassus matrix in the given array.
Note:
This uses the diagonal of the pure Delassus matrix, without any regularization or
preconditioning.
Args:
diag (wp.array): Output vector for the Delassus matrix diagonal entries.
Shape `(sum_of_max_total_cts,)` and type :class:`float32`.
"""
if self._model is None or self._data is None:
raise RuntimeError("ModelKamino and data must be assigned before computing diagonal.")
if self.bsm is None:
raise RuntimeError("Jacobian must be assigned before computing diagonal.")
diag.zero_()
# Launch kernel over all non-zero blocks
wp.launch(
kernel=_compute_block_sparse_delassus_diagonal,
dim=(self._model.size.num_worlds, self.bsm.max_of_num_nzb),
inputs=[
self._model.info.bodies_offset,
self._model.bodies.inv_m_i,
self._data.bodies.inv_I_i,
self.constraint_jacobian.nzb_start,
self.constraint_jacobian.num_nzb,
self.constraint_jacobian.nzb_coords,
self.constraint_jacobian.nzb_values,
self.constraint_jacobian.row_start,
diag,
],
device=self._device,
)
# Add armature regularization
wp.launch(
kernel=_add_armature_regularization_sparse,
dim=(self.num_matrices, self._model.size.max_of_num_dynamic_joint_cts),
inputs=[
# Inputs:
self._model.info.num_joint_dynamic_cts,
self._model.info.joint_dynamic_cts_offset,
self.bsm.row_start,
self._data.joints.inv_m_j,
# Outputs:
diag,
],
device=self._device,
)
def compute(self, reset_to_zero: bool = True):
"""
Runs Delassus pre-computation operations in preparation for linear systems solves.
Depending on the configured solver type, this may perform different pre-computation.
Args:
reset_to_zero (bool):
If True, resets the Delassus matrix to zero.\n
This is useful for ensuring that the matrix is in a clean state before pre-computation.
"""
# Ensure that `finalize()` was called
if self._info is None:
raise ValueError("Data structure is not allocated. Call finalize() first.")
# Ensure the Jacobian is set
if self.bsm is None:
raise ValueError("Jacobian matrix is not set. Call assign() first.")
# Ensure the solver is available if pre-computation is requested
if self._solver is None:
raise ValueError("A linear system solver is not available. Allocate with solver=LINEAR_SOLVER_TYPE.")
# Update if data has changed
if self._needs_update:
self.update()
# Optionally initialize the solver
if reset_to_zero:
self._solver.reset()
# Perform the pre-computation
self._solver.compute()
def solve(self, v: wp.array, x: wp.array):
"""
Solves the linear system D * x = v using the assigned solver.
Args:
v (wp.array): The right-hand side vector of the linear system.
x (wp.array): The array to hold the solution.
Raises:
ValueError: If the Delassus matrix is not allocated or the solver is not available.
"""
# Ensure that `finalize()` was called
if self._info is None:
raise ValueError("Data structure is not allocated. Call finalize() first.")
# Ensure the Jacobian is set
if self.bsm is None:
raise ValueError("Jacobian matrix is not set. Call assign() first.")
# Ensure the solver is available
if self._solver is None:
raise ValueError("A linear system solver is not available. Allocate with solver=LINEAR_SOLVER_TYPE.")
# Update if data has changed
if self._needs_update:
self.update()
# Solve the linear system
return self._solver.solve(b=v, x=x)
def solve_inplace(self, x: wp.array):
"""
Solves the linear system D * x = v in-place.\n
This modifies the input array x to contain the solution assuming it is initialized as x=v.
Args:
x (wp.array): The array to hold the solution. It should be initialized with the right-hand side vector v.
Raises:
ValueError: If the Delassus matrix is not allocated or the solver is not available.
"""
# Ensure that `finalize()` was called
if self._info is None:
raise ValueError("Data structure is not allocated. Call finalize() first.")
# Ensure the Jacobian is set
if self.bsm is None:
raise ValueError("Jacobian matrix is not set. Call assign() first.")
# Ensure the solver is available if pre-computation is requested
if self._solver is None:
raise ValueError("A linear system solver is not available. Allocate with solver=LINEAR_SOLVER_TYPE.")
# Update if data has changed
if self._needs_update:
self.update()
# Solve the linear system in-place
return self._solver.solve_inplace(x=x)
###
# Properties
###
@property
def info(self) -> DenseSquareMultiLinearInfo | None:
"""
Returns the info object for the Delassus problem dimensions and sizes.
"""
return self._info
@property
def num_matrices(self) -> int:
"""
Returns the number of matrices represented by the Delassus operator.
"""
return self._model.size.num_worlds
@property
def max_of_max_dims(self) -> tuple[int, int]:
"""
Returns the maximum dimension of any Delassus matrix across all worlds.
"""
max_jac_rows = self._model.size.max_of_max_total_cts
return (max_jac_rows, max_jac_rows)
@property
def sum_of_max_dims(self) -> int:
"""
Returns the sum of maximum dimensions of the Delassus matrix across all worlds.
"""
return self._model.size.sum_of_max_total_cts
@property
def dtype(self) -> FloatType:
return self._info.dtype
@property
def device(self) -> wp.DeviceLike:
return self._model.device
@property
def constraint_jacobian(self) -> BlockSparseMatrices:
return self._jacobians._J_cts.bsm
###
# Operations
###
def matvec(self, x: wp.array, y: wp.array, world_mask: wp.array):
"""
Performs the sparse matrix-vector product `y = D @ x`, applying regularization and
preconditioning if configured.
"""
if self.Ax_op is None:
raise RuntimeError("No `A@x` operator has been assigned.")
if self.ATy_op is None:
raise RuntimeError("No `A^T@y` operator has been assigned.")
# Update if data has changed
if self._needs_update:
self.update()
v = self._vec_temp_body_space
v.zero_()
# Compute first Jacobian matrix-vector product: v <- (P @ J)^T @ x
self.ATy_op(self._transpose_op_matrix, x, v, world_mask)
if self._eta is None and self._combined_regularization is None:
# Compute second Jacobian matrix-vector product: y <- (P @ J @ M^-1) @ v
self.Ax_op(self.bsm, v, y, world_mask)
else:
y.zero_()
# Compute y <- (P @ J @ M^-1) @ v + diag(eta) @ x
wp.launch(
kernel=_make_block_sparse_gemv_regularization_kernel(1.0),
dim=(self.bsm.num_matrices, self.bsm.max_of_num_nzb),
inputs=[
self.bsm.dims,
self.bsm.num_nzb,
self.bsm.nzb_start,
self.bsm.nzb_coords,
self.bsm.nzb_values,
self.bsm.row_start,
self.bsm.col_start,
self._eta if self._combined_regularization is None else self._combined_regularization,
v,
y,
x,
world_mask,
],
device=self.device,
)
def matvec_transpose(self, y: wp.array, x: wp.array, world_mask: wp.array):
"""
Performs the sparse matrix-transpose-vector product `x = D^T @ y`.
Note:
Since the Delassus matrix is symmetric, this is equivalent to `matvec`.
"""
# Update if data has changed
if self._needs_update:
self.update()
self.matvec(x, y, world_mask)
def gemv(self, x: wp.array, y: wp.array, world_mask: wp.array, alpha: float = 1.0, beta: float = 0.0):
"""
Performs a BLAS-like generalized sparse matrix-vector product `y = alpha * D @ x + beta * y`,
applying regularization and preconditioning if configured.
"""
if self.gemv_op is None:
raise RuntimeError("No BLAS-like `GEMV` operator has been assigned.")
if self.ATy_op is None:
raise RuntimeError("No `A^T@y` operator has been assigned.")
# Update if data has changed
if self._needs_update:
self.update()
v = self._vec_temp_body_space
v.zero_()
# Compute first Jacobian matrix-vector product: v <- (P @ J)^T @ x
self.ATy_op(self._transpose_op_matrix, x, v, world_mask)
if self._eta is None and self._combined_regularization is None:
# Compute second Jacobian matrix-vector product as general matrix-vector product:
# y <- alpha * (P @ J @ M^-1) @ v + beta * y
self.gemv_op(self.bsm, v, y, alpha, beta, world_mask)
else:
# Scale y <- beta * y
wp.launch(
kernel=_scale_row_vector_kernel,
dim=(self.bsm.num_matrices, self.bsm.max_of_max_dims[0]),
inputs=[self.bsm.dims, self.bsm.row_start, y, beta, world_mask],
device=self.device,
)
# Compute y <- alpha * (P @ J @ M^-1) @ v + y + alpha * diag(eta) @ x
wp.launch(
kernel=_make_block_sparse_gemv_regularization_kernel(alpha),
dim=(self.bsm.num_matrices, self.bsm.max_of_num_nzb),
inputs=[
self.bsm.dims,
self.bsm.num_nzb,
self.bsm.nzb_start,
self.bsm.nzb_coords,
self.bsm.nzb_values,
self.bsm.row_start,
self.bsm.col_start,
self._eta if self._combined_regularization is None else self._combined_regularization,
v,
y,
x,
world_mask,
],
device=self.device,
)
def gemv_transpose(self, y: wp.array, x: wp.array, world_mask: wp.array, alpha: float = 1.0, beta: float = 0.0):
"""
Performs a BLAS-like generalized sparse matrix-transpose-vector product
`x = alpha * D^T @ y + beta * x`.
Note:
Since the Delassus matrix is symmetric, this is equivalent to `gemv` with swapped arguments.
"""
# Update if data has changed
if self._needs_update:
self.update()
self.gemv(y, x, world_mask, alpha, beta)
================================================
FILE: newton/_src/solvers/kamino/_src/dynamics/dual.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides a data container and relevant operations to
represent and construct a dual forward dynamics problem.
The dual forward dynamics problem arises from the formulation of
the equations of motion in terms of constraint reactions.
`lambdas = argmin_{x} 1/2 * x^T D x + lambda^T (v_f + Gamma(v_plus(x)))`
This module thus provides building-blocks to realize Delassus operators across multiple
worlds contained in a :class:`ModelKamino`. The :class:`DelassusOperator` class provides a
high-level interface to encapsulate both the data representation as well as the
relevant operations. It provides methods to allocate the necessary data arrays, build
the Delassus matrix given the current state of the model and the active constraints,
add diagonal regularization, and solve linear systems of the form `D @ x = v` given
arrays holding the right-hand-side (rhs) vectors v. Moreover, it supports the use of
different linear solvers as a back-end for performing the aforementioned linear system
solve. Construction of the Delassus operator is realized using a set of Warp kernels
that parallelize the computation using various strategies.
Typical usage example:
# Create a model builder and add bodies, joints, geoms, etc.
builder = ModelBuilder()
...
# Create a model from the builder and construct additional
# containers to hold joint-limits, contacts, Jacobians
model = builder.finalize()
data = model.data()
limits = LimitsKamino(model)
contacts = ContactsKamino(builder)
jacobians = DenseSystemJacobians(model, limits, contacts)
# Define a linear solver type to use as a back-end for the
# Delassus operator computations such as factorization and
# solving the linear system when a rhs vector is provided
linear_solver = LLTBlockedSolver
...
# Build the Jacobians for the model and active limits and contacts
jacobians.build(model, data, limits, contacts)
...
# Create a dual forward dynamics problem and build it using the current model
# data and active unilateral constraints (i.e. for limits and contacts).
dual = DualProblem(model, limits, contacts, jacobians, linear_solver)
dual.build(model, data, jacobians)
"""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any
import warp as wp
from .....core.types import override
from ...config import ConfigBase, ConstrainedDynamicsConfig, ConstraintStabilizationConfig
from ..core.data import DataKamino
from ..core.math import FLOAT32_EPS, UNIT_Z, screw, screw_angular, screw_linear
from ..core.model import ModelKamino
from ..core.size import SizeKamino
from ..core.types import float32, int32, mat33f, vec2f, vec3f, vec4f, vec6f
from ..dynamics.delassus import BlockSparseMatrixFreeDelassusOperator, DelassusOperator
from ..geometry.contacts import ContactsKamino
from ..kinematics.jacobians import DenseSystemJacobians, SparseSystemJacobians
from ..kinematics.limits import LimitsKamino
from ..linalg import LinearSolverType
###
# Module interface
###
__all__ = [
"DualProblem",
"DualProblemData",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
@wp.struct
class DualProblemConfigStruct:
"""
A Warp struct to hold on-device configuration parameters of a dual problem.
"""
alpha: float32
"""Baumgarte stabilization parameter for bilateral joint constraints."""
beta: float32
"""Baumgarte stabilization parameter for unilateral joint limit constraints."""
gamma: float32
"""Baumgarte stabilization parameter for unilateral contact constraints."""
delta: float32
"""Contact penetration margin used for unilateral contact constraints"""
preconditioning: wp.bool
"""Flag to enable preconditioning of the dual problem."""
@dataclass
class DualProblemData:
"""
A container to hold the the dual forward dynamics problem data over multiple worlds.
"""
num_worlds: int = 0
"""The number of worlds represented in the dual problem."""
max_of_maxdims: int = 0
"""The largest maximum number of dual problem dimensions (i.e. constraints) across all worlds."""
###
# Problem configurations
###
config: wp.array | None = None
"""
Problem configuration parameters for each world.\n
Shape of `(num_worlds,)` and type :class:`DualProblemConfigStruct`.
"""
###
# Constraints info
###
njc: wp.array | None = None
"""
The number of active joint constraints in each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.
"""
nl: wp.array | None = None
"""
The number of active limit constraints in each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.
"""
nc: wp.array | None = None
"""
The number of active contact constraints in each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.
"""
lio: wp.array | None = None
"""
The limit index offset of each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.\n
"""
cio: wp.array | None = None
"""
The contact index offset of each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.\n
"""
uio: wp.array | None = None
"""
The unilateral index offset of each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.\n
"""
lcgo: wp.array | None = None
"""
The limit constraint group offset of each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.\n
"""
ccgo: wp.array | None = None
"""
The contact constraint group offset of each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.\n
"""
###
# Delassus operator
###
maxdim: wp.array | None = None
"""
The maximum number of dual problem dimensions of each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.
"""
dim: wp.array | None = None
"""
The active number of dual problem dimensions of each world.\n
Shape of `(num_worlds,)` and type :class:`int32`.
"""
mio: wp.array | None = None
"""
The matrix index offset of each Delassus matrix block.\n
This is applicable to `D` as well as to its (optional) factorizations.\n
Shape of `(num_worlds,)` and type :class:`int32`.
"""
vio: wp.array | None = None
"""
The vector index offset of each constraint dimension vector block.\n
This is applicable to `v_b`, `v_i` and `v_f`.\n
Shape of `(num_worlds,)` and type :class:`int32`.
"""
D: wp.array | None = None
"""
The flat array of Delassus matrix blocks (constraint-space apparent inertia).\n
Shape of `(sum_of_max_total_delassus_size,)` and type :class:`float32`.
"""
P: wp.array | None = None
"""
The flat array of Delassus diagonal preconditioner blocks.\n
Shape of `(sum_of_max_total_cts,)` and type :class:`float32`.
"""
###
# Problem vectors
###
h: wp.array | None = None
"""
Stack of non-linear generalized forces vectors of each world.\n
Computed as:
`h = dt * (w_e + w_gc + w_a)`
where:
- `dt` is the simulation time step
- `w_e` is the stack of per-body purely external wrenches
- `w_gc` is the stack of per-body gravitational + Coriolis wrenches
- `w_a` is the stack of per-body jointactuation wrenches
Construction of this term is optional, as it's contributions are already
incorporated in the computation of the generalized free-velocity `u_f`.
It is can be optionally built for analysis or debugging purposes.
Shape of `(sum_of_num_body_dofs,)` and type :class:`vec6f`.
"""
u_f: wp.array | None = None
"""
Stack of unconstrained generalized velocity vectors.\n
Computed as:
`u_f = u_minus + dt * M^{-1} @ h`
where:
- `u_minus` is the stack of per-body generalized velocities at the beginning of the time step
- `M^{-1}` is the block-diagonal inverse generalized mass matrix
- `h` is the stack of non-linear generalized forces vectors
Shape of `(sum_of_num_body_dofs,)` and type :class:`vec6f`.
"""
v_b: wp.array | None = None
"""
Stack of free-velocity constraint bias vectors (in constraint-space).\n
Computed as:
`v_b = [ v_b_dynamics;
alpha * inv_dt * r_joints;
beta * inv_dt * r_limits;
gamma * inv_dt * r_contacts ]`
where:
- `v_b_dynamics` is the joint dynamics velocity bias.
- `dt` and `inv_dt` is the simulation time step and it inverse
- `r_joints` is the stack of joint constraint residuals
- `r_limits` is the stack of limit constraint residuals
- `r_contacts` is the stack of contact constraint residuals
- `alpha`, `beta`, `gamma` are the Baumgarte stabilization
parameters for joints, limits and contacts, respectively
Shape of `(sum_of_max_total_cts,)` and type :class:`float32`.
"""
v_i: wp.array | None = None
"""
The stack of free-velocity impact biases vector (in constraint-space).\n
Computed as:
`v_i = epsilon @ (J_cts @ u_minus)`
where:
- `epsilon` is the stack of per-contact restitution coefficients
- `J_cts` is the constraint Jacobian matrix
- `u_minus` is the stack of per-body generalized velocities at the beginning of the time step
Shape of `(sum_of_max_total_cts,)` and type :class:`float32`.
"""
v_f: wp.array | None = None
"""
Stack of free-velocity vectors (constraint-space unconstrained velocity).\n
Computed as:
`v_f = J_cts @ u_f + v_b + v_i`
where:
- `J_cts` is the constraint Jacobian matrix
- `u_f` is the stack of unconstrained generalized velocity vectors
- `v_b` is the stack of free-velocity stabilization biases vectors
- `v_i` is the stack of free-velocity impact biases vectors
Shape of `(sum_of_max_total_cts,)` and type :class:`float32`.
"""
mu: wp.array | None = None
"""
Stack of per-contact constraint friction coefficient vectors.\n
Shape of `(sum_of_max_contacts,)` and type :class:`float32`.
"""
###
# Functions
###
@wp.func
def gravity_plus_coriolis_wrench(
g: vec3f,
m_i: float32,
I_i: mat33f,
omega_i: vec3f,
) -> vec6f:
"""
Compute the gravitational + Coriolis wrench acting on a body.
"""
f_gi_i = m_i * g
tau_gi_i = -wp.skew(omega_i) @ (I_i @ omega_i)
return screw(f_gi_i, tau_gi_i)
@wp.func
def gravity_plus_coriolis_wrench_split(
g: vec3f,
m_i: float32,
I_i: mat33f,
omega_i: vec3f,
):
"""
Compute the gravitational+inertial wrench on a body.
"""
f_gi_i = m_i * g
tau_gi_i = -wp.skew(omega_i) @ (I_i @ omega_i)
return f_gi_i, tau_gi_i
###
# Kernels
###
@wp.kernel
def _build_nonlinear_generalized_force(
# Inputs:
model_time_dt: wp.array(dtype=float32),
model_gravity_vector: wp.array(dtype=vec4f),
model_bodies_wid: wp.array(dtype=int32),
model_bodies_m_i: wp.array(dtype=float32),
state_bodies_u_i: wp.array(dtype=vec6f),
state_bodies_I_i: wp.array(dtype=mat33f),
state_bodies_w_e_i: wp.array(dtype=vec6f),
state_bodies_w_a_i: wp.array(dtype=vec6f),
# Outputs:
problem_h: wp.array(dtype=vec6f),
):
# Retrieve the body index as the thread index
bid = wp.tid()
# Retrieve the body model and data
wid = model_bodies_wid[bid]
m_i = model_bodies_m_i[bid]
I_i = state_bodies_I_i[bid]
u_i = state_bodies_u_i[bid]
w_e_i = state_bodies_w_e_i[bid]
w_a_i = state_bodies_w_a_i[bid]
# Get world data
dt = model_time_dt[wid]
gv = model_gravity_vector[wid]
# Extract the effective gravity vector
g = gv.w * vec3f(gv.x, gv.y, gv.z)
# Extract the linear and angular components of the generalized velocity
omega_i = screw_angular(u_i)
# Compute the net external wrench on the body
h_i = w_e_i + w_a_i + gravity_plus_coriolis_wrench(g, m_i, I_i, omega_i)
# Store the generalized free-velocity vector
problem_h[bid] = dt * h_i
@wp.kernel
def _build_generalized_free_velocity(
# Inputs:
model_time_dt: wp.array(dtype=float32),
model_gravity_vector: wp.array(dtype=vec4f),
model_bodies_wid: wp.array(dtype=int32),
model_bodies_m_i: wp.array(dtype=float32),
model_bodies_inv_m_i: wp.array(dtype=float32),
state_bodies_u_i: wp.array(dtype=vec6f),
state_bodies_I_i: wp.array(dtype=mat33f),
state_bodies_inv_I_i: wp.array(dtype=mat33f),
state_bodies_w_e_i: wp.array(dtype=vec6f),
state_bodies_w_a_i: wp.array(dtype=vec6f),
# Outputs:
problem_u_f: wp.array(dtype=vec6f),
):
# Retrieve the body index as the thread index
bid = wp.tid()
# Retrieve the body model and data
wid = model_bodies_wid[bid]
m_i = model_bodies_m_i[bid]
I_i = state_bodies_I_i[bid]
inv_m_i = model_bodies_inv_m_i[bid]
inv_I_i = state_bodies_inv_I_i[bid]
u_i = state_bodies_u_i[bid]
w_e_i = state_bodies_w_e_i[bid]
w_a_i = state_bodies_w_a_i[bid]
# Get world data
dt = model_time_dt[wid]
gv = model_gravity_vector[wid]
# Extract the effective gravity vector
g = gv.w * vec3f(gv.x, gv.y, gv.z)
# Extract the linear and angular components of the generalized velocity
v_i = screw_linear(u_i)
omega_i = screw_angular(u_i)
# Compute the net external wrench on the body
h_i = w_e_i + w_a_i + gravity_plus_coriolis_wrench(g, m_i, I_i, omega_i)
f_h_i = screw_linear(h_i)
tau_h_i = screw_angular(h_i)
# Compute the generalized free-velocity vector components
v_f_i = v_i + dt * (inv_m_i * f_h_i)
omega_f_i = omega_i + dt * (inv_I_i @ tau_h_i)
# Store the generalized free-velocity vector
problem_u_f[bid] = screw(v_f_i, omega_f_i)
@wp.kernel
def _build_free_velocity_bias_joint_dynamics(
# Inputs:
model_info_joint_dynamic_cts_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_group_offset: wp.array(dtype=int32),
model_joints_wid: wp.array(dtype=int32),
model_joints_num_dynamic_cts: wp.array(dtype=int32),
model_joints_dynamic_cts_offset: wp.array(dtype=int32),
data_joints_dq_b_j: wp.array(dtype=float32),
problem_vio: wp.array(dtype=int32),
# Outputs:
problem_v_b: wp.array(dtype=float32),
):
# Retrieve the joint index as the thread index
jid = wp.tid()
# Retrieve the joint-specific model data
wid = model_joints_wid[jid]
num_dyn_cts_j = model_joints_num_dynamic_cts[jid]
dyn_cts_start_j = model_joints_dynamic_cts_offset[jid]
# Skip operation if the joint has no dynamic constraints
if num_dyn_cts_j == 0 or dyn_cts_start_j < 0:
return
# Retrieve the joint constraint index offsets in the:
# - arrays of only dynamic constraints (i.e. for residuals)
# - arrays of all constraints (i.e. including joint dynamics+kinematics, limits and contacts)
dyn_cts_start = model_info_joint_dynamic_cts_offset[wid]
dyn_cts_group_start = model_info_joint_dynamic_cts_group_offset[wid]
# Retrieve the index offset of the vector block of the world
world_total_cts_start = problem_vio[wid]
# Compute block offsets for the constraint and velocity
bias_row_start_j = dyn_cts_start + dyn_cts_start_j
cts_row_start_j = world_total_cts_start + dyn_cts_group_start + dyn_cts_start_j
# Compute the free-velocity bias for the joint
for j in range(num_dyn_cts_j):
problem_v_b[cts_row_start_j + j] = -data_joints_dq_b_j[bias_row_start_j + j]
@wp.kernel
def _build_free_velocity_bias_joint_kinematics(
# Inputs:
model_info_joint_kinematic_cts_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_time_inv_dt: wp.array(dtype=float32),
model_joints_wid: wp.array(dtype=int32),
model_joints_num_kinematic_cts: wp.array(dtype=int32),
model_joints_kinematic_cts_offset: wp.array(dtype=int32),
data_joints_r_j: wp.array(dtype=float32),
problem_config: wp.array(dtype=DualProblemConfigStruct),
problem_vio: wp.array(dtype=int32),
# Outputs:
problem_v_b: wp.array(dtype=float32),
):
# Retrieve the joint index as the thread index
jid = wp.tid()
# Retrieve the joint-specific model data
wid = model_joints_wid[jid]
num_kin_cts_j = model_joints_num_kinematic_cts[jid]
kin_cts_start_j = model_joints_kinematic_cts_offset[jid]
# Retrieve the joint constraint index offsets in the:
# - arrays of only kinematic constraints (i.e. for residuals)
# - arrays of all constraints (i.e. including joint dynamics+kinematics, limits and contacts)
kin_cts_start = model_info_joint_kinematic_cts_offset[wid]
kin_cts_group_start = model_info_joint_kinematic_cts_group_offset[wid]
# Retrieve the model time step
inv_dt = model_time_inv_dt[wid]
# Retrieve the dual problem config
config = problem_config[wid]
# Retrieve the index offset of the vector block of the world
world_total_cts_start = problem_vio[wid]
# Compute baumgarte constraint stabilization coefficient
c_b = config.alpha * inv_dt
# Compute block offsets for the constraint and residual vectors
res_row_start_j = kin_cts_start + kin_cts_start_j
cts_row_start_j = world_total_cts_start + kin_cts_group_start + kin_cts_start_j
# Compute the free-velocity bias for the joint
for j in range(num_kin_cts_j):
problem_v_b[cts_row_start_j + j] = c_b * data_joints_r_j[res_row_start_j + j]
@wp.kernel
def _build_free_velocity_bias_limits(
# Inputs:
model_time_inv_dt: wp.array(dtype=float32),
data_info_limit_cts_group_offset: wp.array(dtype=int32),
limits_model_max: int32,
limits_model_num: wp.array(dtype=int32),
limits_wid: wp.array(dtype=int32),
limits_lid: wp.array(dtype=int32),
limits_r_q: wp.array(dtype=float32),
problem_config: wp.array(dtype=DualProblemConfigStruct),
problem_vio: wp.array(dtype=int32),
# Outputs:
problem_v_b: wp.array(dtype=float32),
):
# Retrieve the limit index as the thread index
tid = wp.tid()
# Retrieve the number of contacts active in the model
model_nl = wp.min(limits_model_num[0], limits_model_max)
# Skip if cid is greater than the number of contacts active in the world
if tid >= model_nl:
return
# Retrieve the limit entity data
wid = limits_wid[tid]
lid = limits_lid[tid]
r_q = limits_r_q[tid]
# Retrieve the world-specific data
inv_dt = model_time_inv_dt[wid]
config = problem_config[wid]
vio = problem_vio[wid]
lcio = data_info_limit_cts_group_offset[wid]
# Compute the total constraint index offset of the current contact
lcio_l = vio + lcio + lid
# Compute the contact constraint stabilization bias
problem_v_b[lcio_l] = config.beta * inv_dt * wp.min(0.0, r_q)
@wp.kernel
def _build_free_velocity_bias_contacts(
# Inputs:
model_time_inv_dt: wp.array(dtype=float32),
model_info_contacts_offset: wp.array(dtype=int32),
data_info_contact_cts_group_offset: wp.array(dtype=int32),
contacts_model_max: int32,
contacts_model_num: wp.array(dtype=int32),
contacts_wid: wp.array(dtype=int32),
contacts_cid: wp.array(dtype=int32),
contacts_gapfunc: wp.array(dtype=vec4f),
contacts_material: wp.array(dtype=vec2f),
problem_config: wp.array(dtype=DualProblemConfigStruct),
problem_vio: wp.array(dtype=int32),
# Outputs:
problem_v_b: wp.array(dtype=float32),
problem_v_i: wp.array(dtype=float32),
problem_mu: wp.array(dtype=float32),
):
# Retrieve the contact index as the thread index
tid = wp.tid()
# Retrieve the number of contacts active in the model
model_nc = wp.min(contacts_model_num[0], contacts_model_max)
# Skip if cid is greater than the number of contacts active in the world
if tid >= model_nc:
return
# Retrieve the contact entity data
wid_k = contacts_wid[tid]
cid_k = contacts_cid[tid]
material_k = contacts_material[tid]
penetration_k = contacts_gapfunc[tid][3]
# Retrieve the world-specific data
inv_dt = model_time_inv_dt[wid_k]
cio = model_info_contacts_offset[wid_k]
ccio = data_info_contact_cts_group_offset[wid_k]
vio = problem_vio[wid_k]
config = problem_config[wid_k]
# Compute the total constraint index offset of the current contact
ccio_k = vio + ccio + 3 * cid_k
# Compute the total contact index offset of the current contact
cio_k = cio + cid_k
# Retrieve the contact material properties
mu_k = material_k.x # Friction coefficient
epsilon_k = material_k.y # Penetration reduction coefficient
# The gap-function value (penetration_k) is the margin-shifted signed
# distance: negative means penetration past the resting separation,
# zero means at rest, positive means within the detection gap.
# Pass the full value through so that one-sided Baumgarte stabilization
# (xi_relaxed) can generate a positive bias for gap contacts, guiding
# objects toward d = 0. A dead-zone of config.delta filters out
# floating-point noise on nearly-touching contacts that would otherwise
# destabilize accelerated solvers (e.g. Nesterov momentum in PADMM).
distance_k = wp.where(wp.abs(penetration_k) < config.delta, 0.0, penetration_k)
# Compute the per-contact penetration error reduction term
# NOTE#1: Penetrations are represented as xi < 0 (hence the sign inversion)
# NOTE#2: xi_p_relaxed corresponds to one-sided Baumgarte-like stabilization
xi = inv_dt * distance_k
xi_relaxed = config.gamma * wp.min(0.0, xi) + wp.max(0.0, xi)
if epsilon_k == 1.0:
alpha = 0.0
else:
alpha = 1.0
# Compute the contact constraint stabilization bias
v_b_k = alpha * xi_relaxed * UNIT_Z
# Store the contact constraint stabilization bias in the output vector
for i in range(3):
problem_v_b[ccio_k + i] = v_b_k[i]
# Initialize the restitutive Newton-type impact model term
problem_v_i[ccio_k] = 0.0
problem_v_i[ccio_k + 1] = 0.0
problem_v_i[ccio_k + 2] = epsilon_k
# Store the contact friction coefficient in the output vector
problem_mu[cio_k] = mu_k
@wp.kernel
def _build_free_velocity(
# Inputs:
model_info_num_bodies: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
data_bodies_u_i: wp.array(dtype=vec6f),
jacobians_J_cts_offsets: wp.array(dtype=int32),
jacobians_J_cts_data: wp.array(dtype=float32),
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_u_f: wp.array(dtype=vec6f),
problem_v_b: wp.array(dtype=float32),
problem_v_i: wp.array(dtype=float32),
# Outputs:
problem_v_f: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the problem dimensions and matrix block index offset
ncts = problem_dim[wid]
# Skip if row index exceed the problem size
if tid >= ncts:
return
# Retrieve the world-specific data
nb = model_info_num_bodies[wid]
bio = model_info_bodies_offset[wid]
cjmio = jacobians_J_cts_offsets[wid]
vio = problem_vio[wid]
# Compute the number of Jacobian rows, i.e. the number of body DoFs
nbd = 6 * nb
# Compute the thread-specific index offset
cts_offset = vio + tid
# Append the column offset to the Jacobian index
cjmio += nbd * tid
# Extract the cached impact bias scaling (i.e. restitution coefficient)
# NOTE: This is a quick hack to avoid multiple kernels. The
# proper way would be to perform this op only for contacts
epsilon_j = problem_v_i[cts_offset]
# Retrieve the cached velocity bias term for the constraint
v_b_j = problem_v_b[cts_offset]
# Buffers
J_i = vec6f(0.0)
v_f_j = float32(0.0)
# Iterate over each body to accumulate velocity contributions
for i in range(nb):
# Compute the Jacobian block index
m_ji = cjmio + 6 * i
# Extract the twist and unconstrained velocity of the body
u_i = data_bodies_u_i[bio + i]
u_f_i = problem_u_f[bio + i]
# Extract the Jacobian block J_ji
# TODO: use slicing operation when available
for d in range(6):
J_i[d] = jacobians_J_cts_data[m_ji + d]
# Accumulate J_i @ u_i
v_f_j += wp.dot(J_i, u_f_i)
# Accumulate the impact bias term
v_f_j += epsilon_j * wp.dot(J_i, u_i)
# Store sum of velocity bias terms
problem_v_f[cts_offset] = v_f_j + v_b_j
@wp.kernel
def _build_free_velocity_sparse(
# Inputs:
model_info_bodies_offset: wp.array(dtype=int32),
state_bodies_u_i: wp.array(dtype=vec6f),
jac_num_nzb: wp.array(dtype=int32),
jac_nzb_start: wp.array(dtype=int32),
jac_nzb_coords: wp.array2d(dtype=int32),
jac_nzb_values: wp.array(dtype=vec6f),
problem_vio: wp.array(dtype=int32),
problem_u_f: wp.array(dtype=vec6f),
problem_v_i: wp.array(dtype=float32),
# Outputs:
problem_v_f: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, nzb_id = wp.tid()
# Skip if block index exceed the number of blocks
if nzb_id >= jac_num_nzb[wid]:
return
# Retrieve block data
global_block_idx = jac_nzb_start[wid] + nzb_id
jac_block_coord = jac_nzb_coords[global_block_idx]
jac_block = jac_nzb_values[global_block_idx]
# Retrieve the world-specific data
bio = model_info_bodies_offset[wid]
vio = problem_vio[wid]
# Compute the thread-specific index offset
thread_offset = vio + jac_block_coord[0]
# Extract the cached impact bias scaling (i.e. restitution coefficient)
# NOTE: This is a quick hack to avoid multiple kernels. The
# proper way would be to perform this op only for contacts
epsilon_j = problem_v_i[thread_offset]
# Buffers
v_f_j = float32(0.0)
# Iterate over each body to accumulate velocity contributions
bid = jac_block_coord[1] // 6
# Extract the twist and unconstrained velocity of the body
u_i = state_bodies_u_i[bio + bid]
u_f_i = problem_u_f[bio + bid]
# Accumulate J_i @ u_i
v_f_j += wp.dot(jac_block, u_f_i)
# Accumulate the impact bias term
v_f_j += epsilon_j * wp.dot(jac_block, u_i)
# Store sum of velocity bias terms
wp.atomic_add(problem_v_f, thread_offset, v_f_j)
@wp.kernel
def _build_dual_preconditioner_all_constraints(
# Inputs:
problem_config: wp.array(dtype=DualProblemConfigStruct),
problem_dim: wp.array(dtype=int32),
problem_mio: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_njc: wp.array(dtype=int32),
problem_nl: wp.array(dtype=int32),
problem_D: wp.array(dtype=float32),
# Outputs:
problem_P: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the world-specific problem config
config = problem_config[wid]
# Retrieve the number of active constraints in the world
ncts = problem_dim[wid]
# Skip if row index exceed the problem size
if tid >= ncts or not config.preconditioning:
return
# Retrieve the matrix index offset of the world
mio = problem_mio[wid]
# Retrieve the vector index offset of the world
vio = problem_vio[wid]
# Retrieve the number of active joint and limit constraints of the world
njc = problem_njc[wid]
nl = problem_nl[wid]
njlc = njc + nl
# Compute the preconditioner entry for the current constraint
# First handle joint and limit constraints, then contact constraints
if tid < njlc:
# Retrieve the diagonal entry of the Delassus matrix
D_ii = problem_D[mio + ncts * tid + tid]
# Compute the corresponding Jacobi preconditioner entry
problem_P[vio + tid] = wp.sqrt(1.0 / (wp.abs(D_ii) + FLOAT32_EPS))
else:
# Compute the contact constraint index
ccid = tid - njlc
# Only the thread of the first contact constraint dimension computes the preconditioner
if ccid % 3 == 0:
# Retrieve the diagonal entries of the Delassus matrix for the contact constraint set
D_kk_0 = problem_D[mio + ncts * (tid + 0) + (tid + 0)]
D_kk_1 = problem_D[mio + ncts * (tid + 1) + (tid + 1)]
D_kk_2 = problem_D[mio + ncts * (tid + 2) + (tid + 2)]
# Compute the effective diagonal entry
# D_kk = (D_kk_0 + D_kk_1 + D_kk_2) / 3.0
# D_kk = wp.min(vec3f(D_kk_0, D_kk_1, D_kk_2))
D_kk = wp.max(vec3f(D_kk_0, D_kk_1, D_kk_2))
# Compute the corresponding Jacobi preconditioner entry
P_k = wp.sqrt(1.0 / (wp.abs(D_kk) + FLOAT32_EPS))
problem_P[vio + tid] = P_k
problem_P[vio + tid + 1] = P_k
problem_P[vio + tid + 2] = P_k
@wp.kernel
def _build_dual_preconditioner_all_constraints_sparse(
# Inputs:
problem_config: wp.array(dtype=DualProblemConfigStruct),
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_njc: wp.array(dtype=int32),
problem_nl: wp.array(dtype=int32),
# Outputs:
problem_P: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the world-specific problem config
config = problem_config[wid]
# Retrieve the number of active constraints in the world
ncts = problem_dim[wid]
# Skip if row index exceed the problem size
if tid >= ncts or not config.preconditioning:
return
# Retrieve the vector index offset of the world
vio = problem_vio[wid]
# Retrieve the number of active joint and limit constraints of the world
njc = problem_njc[wid]
nl = problem_nl[wid]
njlc = njc + nl
# Compute the preconditioner entry for the current constraint
# First handle joint and limit constraints, then contact constraints
if tid < njlc:
# Retrieve the diagonal entry of the Delassus matrix
D_ii = problem_P[vio + tid]
# Compute the corresponding Jacobi preconditioner entry
problem_P[vio + tid] = wp.sqrt(1.0 / (wp.abs(D_ii) + FLOAT32_EPS))
else:
# Compute the contact constraint index
ccid = tid - njlc
# Only the thread of the first contact constraint dimension computes the preconditioner
if ccid % 3 == 0:
# Retrieve the diagonal entries of the Delassus matrix for the contact constraint set
D_kk_0 = problem_P[vio + tid]
D_kk_1 = problem_P[vio + tid + 1]
D_kk_2 = problem_P[vio + tid + 2]
# Compute the effective diagonal entry
# D_kk = (D_kk_0 + D_kk_1 + D_kk_2) / 3.0
# D_kk = wp.min(vec3f(D_kk_0, D_kk_1, D_kk_2))
D_kk = wp.max(vec3f(D_kk_0, D_kk_1, D_kk_2))
# Compute the corresponding Jacobi preconditioner entry
P_k = wp.sqrt(1.0 / (wp.abs(D_kk) + FLOAT32_EPS))
problem_P[vio + tid] = P_k
problem_P[vio + tid + 1] = P_k
problem_P[vio + tid + 2] = P_k
@wp.kernel
def _apply_dual_preconditioner_to_matrix(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_mio: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_P: wp.array(dtype=float32),
# Outputs:
X: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the number of active constraints in the world
ncts = problem_dim[wid]
# Skip if there no constraints ar active
if ncts == 0:
return
# Compute i (row) and j (col) indices from the tid
i = tid // ncts
j = tid % ncts
# Skip if indices exceed the problem size
if i >= ncts or j >= ncts:
return
# Retrieve the matrix index offset of the world
mio = problem_mio[wid]
# Retrieve the vector index offset of the world
vio = problem_vio[wid]
# Compute the global index of the matrix entry
m_ij = mio + ncts * i + j
# Retrieve the i,j-th entry of the target matrix
X_ij = X[m_ij]
# Retrieve the i,j-th entries of the diagonal preconditioner
P_i = problem_P[vio + i]
P_j = problem_P[vio + j]
# Store the preconditioned i,j-th entry of the matrix
X[m_ij] = P_i * (P_j * X_ij)
@wp.kernel
def _apply_dual_preconditioner_to_vector(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_P: wp.array(dtype=float32),
# Outputs:
x: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the number of active constraints in the world
ncts = problem_dim[wid]
# Skip if row index exceed the problem size
if tid >= ncts:
return
# Retrieve the vector index offset of the world
vio = problem_vio[wid]
# Compute the global index of the vector entry
v_i = vio + tid
# Retrieve the i-th entry of the target vector
x_i = x[v_i]
# Retrieve the i-th entry of the diagonal preconditioner
P_i = problem_P[v_i]
# Store the preconditioned i-th entry of the vector
x[v_i] = P_i * x_i
###
# Interfaces
###
class DualProblem:
"""
A container to hold, manage and operate a dynamics dual problem.
"""
@dataclass
class Config(ConfigBase):
"""
Configuration class for :class:`DualProblem`.
"""
constraints: ConstraintStabilizationConfig = field(default_factory=ConstraintStabilizationConfig)
"""Constraint stabilization global defaults/override configurations."""
dynamics: ConstrainedDynamicsConfig = field(default_factory=ConstrainedDynamicsConfig)
"""Constrained dynamics problem construction configurations."""
def to_struct(self) -> DualProblemConfigStruct:
"""
Converts the config to a DualProblemConfigStruct struct.
"""
config_struct = DualProblemConfigStruct()
config_struct.alpha = wp.float32(self.constraints.alpha)
config_struct.beta = wp.float32(self.constraints.beta)
config_struct.gamma = wp.float32(self.constraints.gamma)
config_struct.delta = wp.float32(self.constraints.delta)
config_struct.preconditioning = wp.bool(self.dynamics.preconditioning)
return config_struct
@override
def validate(self) -> None:
"""
Validates the current values held by the :class:`DualProblem.Config` instance.
"""
self.constraints.validate()
self.dynamics.validate()
@override
def __post_init__(self):
"""Post-initialization to validate config."""
self.validate()
def __init__(
self,
model: ModelKamino | None = None,
data: DataKamino | None = None,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
solver: LinearSolverType | None = None,
solver_kwargs: dict[str, Any] | None = None,
config: list[DualProblem.Config] | DualProblem.Config | None = None,
compute_h: bool = False,
device: wp.DeviceLike = None,
sparse: bool = True,
):
"""
Constructs a dual problem interface container.
If `model`, `limits` and/or `contacts` containers are provided, it allocates the dual problem data members.
Only the `model` is strictly required for the allocation, but the resulting dual problem will only represent
bilateral (i.e. equality) joint constraints and possibly some unilateral (i.e. inequality) joint limits, but
not contact constraints. The `contacts` container is required if the dual problem is to also incorporate
contact constraints. If no `model` is provided at construction time, then deferred allocation is possible
by calling the `finalize()` method at a later point.
Args:
model (ModelKamino, optional):
The model to build the dual problem for.
contacts (ContactsKamino, optional):
The contacts container to use for the dual problem.
solver (LinearSolverType, optional):
The linear solver to use for the Delassus operator. Defaults to None.
config (List[DualProblem.Config] | DualProblem.Config, optional):
The config for the dual problem.\n
If a single `DualProblem.Config` object is provided, it will be replicated for all worlds.
Defaults to `None`, indicating that default config will be used for all worlds.
compute_h (bool, optional):
Set to `True` to enable the computation of the nonlinear
generalized forces vectors in construction of the dual problem.\n
Defaults to `False`.
device (wp.DeviceLike, optional):
The device to allocate the dual problem on.\n
Defaults to `None`.
"""
# Cache the requested device
self._device: wp.DeviceLike = device
# Declare the model size cache
self._size: SizeKamino | None = None
self._config: list[DualProblem.Config] = []
"""Host-side cache of the list of per world dual problem config."""
self._delassus: DelassusOperator | BlockSparseMatrixFreeDelassusOperator | None = None
"""The Delassus operator interface container."""
self._data: DualProblemData | None = None
"""The dual problem data container bundling are relevant memory allocations."""
self._sparse: bool = sparse
"""Flag to indicate whether the dual uses a sparse data representation."""
# Finalize the dual problem data if a model is provided
if model is not None:
self.finalize(
model=model,
data=data,
limits=limits,
contacts=contacts,
solver=solver,
solver_kwargs=solver_kwargs,
config=config,
compute_h=compute_h,
device=device,
)
###
# Properties
###
@property
def device(self) -> wp.DeviceLike:
"""
Returns the device the dual problem is allocated on.
"""
return self._device
@property
def size(self) -> SizeKamino:
"""
Returns the model size of the dual problem.
This is the size of the model that the dual problem is built for.
"""
if self._size is None:
raise ValueError("ModelKamino size is not allocated. Call `finalize()` first.")
return self._size
@property
def config(self) -> list[DualProblem.Config]:
"""
Returns the list of per world dual problem config.
"""
return self._config
@config.setter
def config(self, value: list[DualProblem.Config] | DualProblem.Config):
"""
Sets the list of per world dual problem config.
If a single `DualProblem.Config` object is provided, it will be replicated for all worlds.
"""
self._config = self._check_config(value, self._data.num_worlds)
@property
def delassus(self) -> DelassusOperator | BlockSparseMatrixFreeDelassusOperator:
"""
Returns the Delassus operator interface.
"""
if self._delassus is None:
raise ValueError("Delassus operator is not allocated. Call `finalize()` first.")
return self._delassus
@property
def data(self) -> DualProblemData:
"""
Returns the dual problem data container.
"""
return self._data
@property
def sparse(self) -> bool:
"""
Returns whether the dual problem is using sparse operators.
"""
return self._sparse
###
# Operations
###
def finalize(
self,
model: ModelKamino,
data: DataKamino | None = None,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
solver: LinearSolverType | None = None,
solver_kwargs: dict[str, Any] | None = None,
config: list[DualProblem.Config] | DualProblem.Config | None = None,
compute_h: bool = False,
device: wp.DeviceLike = None,
):
"""
Finalizes all memory allocations of the dual problem data
for the given model, limits, contacts and Jacobians.
Args:
model (ModelKamino, optional):
The model to build the dual problem for.
contacts (ContactsKamino, optional):
The contacts container to use for the dual problem.
solver (LinearSolverType, optional):
The linear solver to use for the Delassus operator.\n
Defaults to `None`.
config (List[DualProblem.Config] | DualProblem.Config, optional):
The config for the dual problem.\n
If a single `DualProblem.Config` object is provided, it will be replicated for all worlds.
Defaults to `None`, indicating that default config will be used for all worlds.
compute_nonlinear_forces (bool, optional):
Set to `True` to enable the computation of the nonlinear
generalized forces vectors in construction of the dual problem.\n
Defaults to `False`.
device (wp.DeviceLike, optional):
The device to allocate the dual problem on. Defaults to None.
"""
# Ensure the model is valid
if model is None:
raise ValueError("A model of type `ModelKamino` must be provided to allocate the Delassus operator.")
elif not isinstance(model, ModelKamino):
raise ValueError("Invalid model provided. Must be an instance of `ModelKamino`.")
# Ensure the data container is valid if provided
if data is not None:
if not isinstance(data, DataKamino):
raise ValueError("Invalid data container provided. Must be an instance of `DataKamino`.")
# Ensure the limits container is valid if provided
if limits is not None:
if not isinstance(limits, LimitsKamino):
raise ValueError("Invalid limits container provided. Must be an instance of `LimitsKamino`.")
# Ensure the contacts container is valid if provided
if contacts is not None:
if not isinstance(contacts, ContactsKamino):
raise ValueError("Invalid contacts container provided. Must be an instance of `ContactsKamino`.")
# Capture reference to the model size
self._size = model.size
# Check config validity and update cache
self._config = self._check_config(config, model.info.num_worlds)
self._compute_h = compute_h
# Determine the maximum number of contacts supported by the model
# in order to allocate corresponding per-friction-cone parameters
model_max_contacts_host = contacts.model_max_contacts_host if contacts is not None else 0
# Construct the Delassus operator first since it will already process the necessary
# model and contacts allocation sizes and will create some of the necessary arrays
if self._sparse:
self._delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=contacts,
solver=solver,
solver_kwargs=solver_kwargs,
device=device,
)
# Assign identity regularization, to be modified by solver
self._delassus.set_regularization(
wp.zeros(
(model.size.sum_of_max_total_cts,),
dtype=float32,
device=device,
)
)
else:
self._delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=contacts,
solver=solver,
solver_kwargs=solver_kwargs,
device=device,
)
# Construct the dual problem data container
with wp.ScopedDevice(device):
if self._sparse:
self._data = DualProblemData(
# Set the host-side caches of the maximal problem dimensions
num_worlds=self._delassus.num_matrices,
max_of_maxdims=self._delassus.max_of_max_dims,
# Capture references to the mode and data info arrays
njc=model.info.num_joint_cts,
nl=data.info.num_limits,
nc=data.info.num_contacts,
lio=model.info.limits_offset,
cio=model.info.contacts_offset,
uio=model.info.unilaterals_offset,
lcgo=data.info.limit_cts_group_offset,
ccgo=data.info.contact_cts_group_offset,
# Capture references to arrays already create by the Delassus operator
maxdim=self._delassus.info.maxdim,
dim=self._delassus.info.dim,
mio=None,
vio=self._delassus.info.vio,
D=None,
# Allocate new memory for the remaining dual problem quantities
config=wp.array([c.to_struct() for c in self.config], dtype=DualProblemConfigStruct),
h=wp.zeros(shape=(model.size.sum_of_num_bodies,), dtype=vec6f) if self._compute_h else None,
u_f=wp.zeros(shape=(model.size.sum_of_num_bodies,), dtype=vec6f),
v_b=wp.zeros(shape=(self._delassus.sum_of_max_dims,), dtype=float32),
v_i=wp.zeros(shape=(self._delassus.sum_of_max_dims,), dtype=float32),
v_f=wp.zeros(shape=(self._delassus.sum_of_max_dims,), dtype=float32),
mu=wp.zeros(shape=(model_max_contacts_host,), dtype=float32),
P=wp.ones(shape=(self._delassus.sum_of_max_dims,), dtype=float32),
)
# Connect Delassus preconditioner to data array
self._delassus.set_preconditioner(self._data.P)
else:
self._data = DualProblemData(
# Set the host-side caches of the maximal problem dimensions
num_worlds=self._delassus.num_worlds,
max_of_maxdims=self._delassus.num_maxdims,
# Capture references to the mode and data info arrays
njc=model.info.num_joint_cts,
nl=data.info.num_limits,
nc=data.info.num_contacts,
lio=model.info.limits_offset,
cio=model.info.contacts_offset,
uio=model.info.unilaterals_offset,
lcgo=data.info.limit_cts_group_offset,
ccgo=data.info.contact_cts_group_offset,
# Capture references to arrays already create by the Delassus operator
maxdim=self._delassus.info.maxdim,
dim=self._delassus.info.dim,
mio=self._delassus.info.mio,
vio=self._delassus.info.vio,
D=self._delassus.D,
# Allocate new memory for the remaining dual problem quantities
config=wp.array([c.to_struct() for c in self.config], dtype=DualProblemConfigStruct),
h=wp.zeros(shape=(model.size.sum_of_num_bodies,), dtype=vec6f) if self._compute_h else None,
u_f=wp.zeros(shape=(model.size.sum_of_num_bodies,), dtype=vec6f),
v_b=wp.zeros(shape=(self._delassus.num_maxdims,), dtype=float32),
v_i=wp.zeros(shape=(self._delassus.num_maxdims,), dtype=float32),
v_f=wp.zeros(shape=(self._delassus.num_maxdims,), dtype=float32),
mu=wp.zeros(shape=(model_max_contacts_host,), dtype=float32),
P=wp.ones(shape=(self._delassus.num_maxdims,), dtype=float32),
)
def zero(self):
if self._compute_h:
self._data.h.zero_()
self._data.u_f.zero_()
self._data.v_b.zero_()
self._data.v_i.zero_()
self._data.v_f.zero_()
self._data.mu.zero_()
self._data.P.fill_(1.0)
if self._sparse:
self._delassus.set_needs_update()
def build(
self,
model: ModelKamino,
data: DataKamino,
jacobians: DenseSystemJacobians | SparseSystemJacobians,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
reset_to_zero: bool = True,
):
"""
Builds the dual problem for the given model, data, limits and contacts data.
"""
if self._sparse and not isinstance(jacobians, SparseSystemJacobians):
raise TypeError("Dual problem in sparse configuration requires sparse jacobians.")
# Initialize problem data
if reset_to_zero:
self.zero()
# Build the Delassus operator
# NOTE: We build this first since it will update the arrays of active constraints
if self._sparse:
self._delassus.assign(jacobians=jacobians)
else:
self._delassus.build(
model=model,
data=data,
jacobians=jacobians,
reset_to_zero=reset_to_zero,
)
# Optionally also build the non-linear generalized force vector
if self._compute_h:
self._build_nonlinear_generalized_force(model, data)
# Build the generalized free-velocity vector
self._build_generalized_free_velocity(model, data)
# Build the free-velocity bias terms
self._build_free_velocity_bias(model, data, limits, contacts)
# Build the free-velocity vector
if isinstance(jacobians, SparseSystemJacobians):
wp.copy(self._data.v_f, self._data.v_b)
J_cts = jacobians._J_cts.bsm
wp.launch(
_build_free_velocity_sparse,
dim=(self._size.num_worlds, J_cts.max_of_num_nzb),
inputs=[
# Inputs:
model.info.bodies_offset,
data.bodies.u_i,
J_cts.num_nzb,
J_cts.nzb_start,
J_cts.nzb_coords,
J_cts.nzb_values,
self._data.vio,
self._data.u_f,
self._data.v_i,
# Outputs:
self._data.v_f,
],
)
else:
wp.launch(
_build_free_velocity,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
model.info.num_bodies,
model.info.bodies_offset,
data.bodies.u_i,
jacobians.data.J_cts_offsets,
jacobians.data.J_cts_data,
self._data.dim,
self._data.vio,
self._data.u_f,
self._data.v_b,
self._data.v_i,
# Outputs:
self._data.v_f,
],
)
# Optionally build and apply the Delassus diagonal preconditioner
if any(s.dynamics.preconditioning for s in self._config):
self._build_dual_preconditioner()
self._apply_dual_preconditioner_to_dual()
###
# Internals
###
@staticmethod
def _check_config(
config: list[DualProblem.Config] | DualProblem.Config | None, num_worlds: int
) -> list[DualProblem.Config]:
"""
Checks and prepares the config for the dual problem.
If a single `DualProblemConfig` object is provided, it will be replicated for all worlds.
If a list of configs is provided, it will ensure that the number of configs matches the number of worlds.
"""
if config is None:
# If no config is provided, use default config
return [DualProblem.Config()] * num_worlds
elif isinstance(config, DualProblem.Config):
# If a single config object is provided, replicate it for all worlds
return [config] * num_worlds
elif isinstance(config, list):
# Ensure the configs are of the correct type and length
if len(config) != num_worlds:
raise ValueError(f"Expected {num_worlds} configs, got {len(config)}")
for c in config:
if not isinstance(c, DualProblem.Config):
raise TypeError(f"Expected DualProblem.Config, got {type(c)}")
return config
else:
raise TypeError(f"Expected List[DualProblem.Config] or DualProblem.Config, got {type(config)}")
def _build_nonlinear_generalized_force(model: ModelKamino, data: DataKamino, problem: DualProblemData):
"""
Builds the nonlinear generalized force vector `h`.
"""
wp.launch(
_build_nonlinear_generalized_force,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
model.time.dt,
model.gravity.vector,
model.bodies.wid,
model.bodies.m_i,
data.bodies.u_i,
data.bodies.I_i,
data.bodies.w_e_i,
data.bodies.w_a_i,
# Outputs:
problem.h,
],
)
def _build_generalized_free_velocity(self, model: ModelKamino, data: DataKamino):
"""
Builds the generalized free-velocity vector (i.e. unconstrained) `u_f`.
"""
wp.launch(
_build_generalized_free_velocity,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
model.time.dt,
model.gravity.vector,
model.bodies.wid,
model.bodies.m_i,
model.bodies.inv_m_i,
data.bodies.u_i,
data.bodies.I_i,
data.bodies.inv_I_i,
data.bodies.w_e_i,
data.bodies.w_a_i,
# Outputs:
self._data.u_f,
],
)
def _build_free_velocity_bias(
self,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
):
"""
Builds the free-velocity bias vector `v_b`.
"""
if model.size.sum_of_num_joints > 0:
if model.size.sum_of_num_dynamic_joints > 0:
wp.launch(
_build_free_velocity_bias_joint_dynamics,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.joint_dynamic_cts_offset,
model.info.joint_dynamic_cts_group_offset,
model.joints.wid,
model.joints.num_dynamic_cts,
model.joints.dynamic_cts_offset,
data.joints.dq_b_j,
self._data.vio,
# Outputs:
self._data.v_b,
],
)
wp.launch(
_build_free_velocity_bias_joint_kinematics,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.joint_kinematic_cts_offset,
model.info.joint_kinematic_cts_group_offset,
model.time.inv_dt,
model.joints.wid,
model.joints.num_kinematic_cts,
model.joints.kinematic_cts_offset,
data.joints.r_j,
self._data.config,
self._data.vio,
# Outputs:
self._data.v_b,
],
)
if limits is not None and limits.model_max_limits_host > 0:
wp.launch(
_build_free_velocity_bias_limits,
dim=limits.model_max_limits_host,
inputs=[
# Inputs:
model.time.inv_dt,
data.info.limit_cts_group_offset,
limits.model_max_limits_host,
limits.model_active_limits,
limits.wid,
limits.lid,
limits.r_q,
self._data.config,
self._data.vio,
# Outputs:
self._data.v_b,
],
)
if contacts is not None and contacts.model_max_contacts_host > 0:
wp.launch(
_build_free_velocity_bias_contacts,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs:
model.time.inv_dt,
model.info.contacts_offset,
data.info.contact_cts_group_offset,
contacts.model_max_contacts_host,
contacts.model_active_contacts,
contacts.wid,
contacts.cid,
contacts.gapfunc,
contacts.material,
self._data.config,
self._data.vio,
# Outputs:
self._data.v_b,
self._data.v_i,
self._data.mu,
],
)
def _build_free_velocity(self, model: ModelKamino, data: DataKamino, jacobians: DenseSystemJacobians):
"""
Builds the free-velocity vector `v_f`.
"""
wp.launch(
_build_free_velocity,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
model.info.num_bodies,
model.info.bodies_offset,
data.bodies.u_i,
jacobians.data.J_cts_offsets,
jacobians.data.J_cts_data,
self._data.dim,
self._data.vio,
self._data.u_f,
self._data.v_b,
self._data.v_i,
# Outputs:
self._data.v_f,
],
)
def _build_dual_preconditioner(self):
"""
Builds the diagonal preconditioner 'P' according to the current Delassus operator.
"""
if self._sparse:
self._delassus.diagonal(self._data.P)
wp.launch(
_build_dual_preconditioner_all_constraints_sparse,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
self._data.config,
self._data.dim,
self._data.vio,
self._data.njc,
self._data.nl,
# Outputs:
self._data.P,
],
)
else:
wp.launch(
_build_dual_preconditioner_all_constraints,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
self._data.config,
self._data.dim,
self._data.mio,
self._data.vio,
self._data.njc,
self._data.nl,
self._data.D,
# Outputs:
self._data.P,
],
)
def _apply_dual_preconditioner_to_dual(self):
"""
Applies the diagonal preconditioner 'P' to the
Delassus operator 'D' and free-velocity vector `v_f`.
"""
if self._sparse:
# Preconditioner has already been connected to appropriate array
pass
else:
wp.launch(
_apply_dual_preconditioner_to_matrix,
dim=(self._size.num_worlds, self.delassus._max_of_max_total_D_size),
inputs=[
# Inputs:
self._data.dim,
self._data.mio,
self._data.vio,
self._data.P,
# Outputs:
self._data.D,
],
)
wp.launch(
_apply_dual_preconditioner_to_vector,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
self._data.dim,
self._data.vio,
self._data.P,
# Outputs:
self._data.v_f,
],
)
def _apply_dual_preconditioner_to_matrix(self, X: wp.array):
"""
Applies the diagonal preconditioner 'P' to a given matrix.
"""
wp.launch(
_apply_dual_preconditioner_to_matrix,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
self._data.dim,
self._data.mio,
self._data.vio,
self._data.P,
# Outputs:
X,
],
)
def _apply_dual_preconditioner_to_vector(self, x: wp.array):
"""
Applies the diagonal preconditioner 'P' to a given vector.
"""
wp.launch(
_apply_dual_preconditioner_to_vector,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
self._data.dim,
self._data.vio,
self._data.P,
# Outputs:
x,
],
)
================================================
FILE: newton/_src/solvers/kamino/_src/dynamics/wrenches.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Dynamics: Wrenches
"""
import warp as wp
from ..core.data import DataKamino
from ..core.model import ModelKamino
from ..core.types import float32, int32, mat63f, vec2i, vec3f, vec6f
from ..geometry.contacts import ContactsKamino
from ..kinematics.jacobians import DenseSystemJacobians, SparseSystemJacobians
from ..kinematics.limits import LimitsKamino
###
# Module interface
###
__all__ = [
"compute_constraint_body_wrenches",
"compute_joint_dof_body_wrenches",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _compute_joint_dof_body_wrenches_dense(
# Inputs:
model_info_num_body_dofs: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_joints_num_dofs: wp.array(dtype=int32),
model_joints_dofs_offset: wp.array(dtype=int32),
model_joints_wid: wp.array(dtype=int32),
model_joints_bid_B: wp.array(dtype=int32),
model_joints_bid_F: wp.array(dtype=int32),
data_joints_tau_j: wp.array(dtype=float32),
jacobian_dofs_offsets: wp.array(dtype=int32),
jacobian_dofs_data: wp.array(dtype=float32),
# Outputs:
data_bodies_w_a: wp.array(dtype=vec6f),
):
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the world index of the joint
wid = model_joints_wid[jid]
# Retrieve the body indices of the joint
# NOTE: these indices are w.r.t the model
bid_F_j = model_joints_bid_F[jid]
bid_B_j = model_joints_bid_B[jid]
# Retrieve the size and index offset of the joint DoFs
d_j = model_joints_num_dofs[jid]
dio_j = model_joints_dofs_offset[jid]
# Retrieve the number of body DoFs in the world
nbd = model_info_num_body_dofs[wid]
# Retrieve the element index offset of the bodies of the world
bio = model_info_bodies_offset[wid]
# Retrieve the DoF block index offsets of the world's actuation
# Jacobian matrix and generalized joint actuation force vector
mio = jacobian_dofs_offsets[wid]
vio = model_info_joint_dofs_offset[wid]
# Append offsets to the current joint's DoFs
vio += dio_j
mio += nbd * dio_j
# Compute and store the joint actuation wrench for the Follower body
w_j_F = vec6f(0.0)
dio_F = 6 * (bid_F_j - bio)
for j in range(d_j):
mio_j = mio + nbd * j + dio_F
vio_j = vio + j
tau_j = data_joints_tau_j[vio_j]
for i in range(6):
w_j_F[i] += jacobian_dofs_data[mio_j + i] * tau_j
wp.atomic_add(data_bodies_w_a, bid_F_j, w_j_F)
# Compute and store the joint actuation wrench for the Base body if bid_B >= 0
if bid_B_j >= 0:
w_j_B = vec6f(0.0)
dio_B = 6 * (bid_B_j - bio)
for j in range(d_j):
mio_j = mio + nbd * j + dio_B
vio_j = vio + j
tau_j = data_joints_tau_j[vio_j]
for i in range(6):
w_j_B[i] += jacobian_dofs_data[mio_j + i] * tau_j
wp.atomic_add(data_bodies_w_a, bid_B_j, w_j_B)
@wp.kernel
def _compute_joint_dof_body_wrenches_sparse(
# Inputs:
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_joints_num_dofs: wp.array(dtype=int32),
model_joints_dofs_offset: wp.array(dtype=int32),
model_joints_wid: wp.array(dtype=int32),
model_joints_bid_B: wp.array(dtype=int32),
model_joints_bid_F: wp.array(dtype=int32),
data_joints_tau_j: wp.array(dtype=float32),
jac_joint_nzb_offsets: wp.array(dtype=int32),
jac_nzb_values: wp.array(dtype=vec6f),
# Outputs:
data_bodies_w_a: wp.array(dtype=vec6f),
):
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the world index of the joint
wid = model_joints_wid[jid]
# Retrieve the body indices of the joint
# NOTE: these indices are w.r.t the model
bid_F_j = model_joints_bid_F[jid]
bid_B_j = model_joints_bid_B[jid]
# Retrieve the size and index offset of the joint DoFs
d_j = model_joints_num_dofs[jid]
dio_j = model_joints_dofs_offset[jid]
# Retrieve the DoF block index offsets of the world's actuation
# Jacobian matrix and generalized joint actuation force vector
vio = model_info_joint_dofs_offset[wid]
# Append offsets to the current joint's DoFs
vio += dio_j
# Retrieve the starting index for the non-zero blocks for the current joint
jac_j_nzb_start = jac_joint_nzb_offsets[jid]
# Compute and store the joint actuation wrench for the Follower body
w_j_F = vec6f(0.0)
for j in range(d_j):
jac_block = jac_nzb_values[jac_j_nzb_start + j]
vio_j = vio + j
tau_j = data_joints_tau_j[vio_j]
w_j_F += jac_block * tau_j
wp.atomic_add(data_bodies_w_a, bid_F_j, w_j_F)
# Compute and store the joint actuation wrench for the Base body if bid_B >= 0
if bid_B_j >= 0:
w_j_B = vec6f(0.0)
for j in range(d_j):
jac_block = jac_nzb_values[jac_j_nzb_start + d_j + j]
vio_j = vio + j
tau_j = data_joints_tau_j[vio_j]
w_j_B += jac_block * tau_j
wp.atomic_add(data_bodies_w_a, bid_B_j, w_j_B)
@wp.kernel
def _compute_joint_cts_body_wrenches_dense(
# Inputs:
model_info_num_body_dofs: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_group_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_time_inv_dt: wp.array(dtype=float32),
model_joints_wid: wp.array(dtype=int32),
model_joints_num_dynamic_cts: wp.array(dtype=int32),
model_joints_num_kinematic_cts: wp.array(dtype=int32),
model_joints_dynamic_cts_offset: wp.array(dtype=int32),
model_joints_kinematic_cts_offset: wp.array(dtype=int32),
model_joints_bid_B: wp.array(dtype=int32),
model_joints_bid_F: wp.array(dtype=int32),
jacobian_cts_offset: wp.array(dtype=int32),
jacobian_cts_data: wp.array(dtype=float32),
lambdas_offsets: wp.array(dtype=int32),
lambdas_data: wp.array(dtype=float32),
# Outputs:
data_bodies_w_j: wp.array(dtype=vec6f),
):
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the world index of the joint
wid = model_joints_wid[jid]
# Retrieve the body indices of the joint
# NOTE: these indices are w.r.t the model
bid_F_j = model_joints_bid_F[jid]
bid_B_j = model_joints_bid_B[jid]
# Retrieve the size and index offset of the joint constraint
num_dyn_cts_j = model_joints_num_dynamic_cts[jid]
num_kin_cts_j = model_joints_num_kinematic_cts[jid]
dyn_cts_start_j = model_joints_dynamic_cts_offset[jid]
kin_cts_start_j = model_joints_kinematic_cts_offset[jid]
# Retrieve the number of body DoFs in the world
nbd = model_info_num_body_dofs[wid]
# Retrieve the element index offset of the bodies of the world
bio = model_info_bodies_offset[wid]
# Retrieve the index offsets of the active joint dynamic and kinematic constraints of the world
world_jdcgo = model_info_joint_dynamic_cts_group_offset[wid]
world_jkcgo = model_info_joint_kinematic_cts_group_offset[wid]
# Retrieve the inverse time-step of the world
inv_dt = model_time_inv_dt[wid]
# Retrieve the constraint block index offsets of the
# Jacobian matrix and multipliers vector of the world
world_jacobian_start = jacobian_cts_offset[wid]
world_cts_start = lambdas_offsets[wid]
# Compute and store the joint constraint wrench for the Follower body
# NOTE: We need to scale by the time-step because the lambdas are impulses
w_j_F = vec6f(0.0)
col_F_start = 6 * (bid_F_j - bio)
for j in range(num_dyn_cts_j):
row_j = world_jdcgo + dyn_cts_start_j + j
mio_j = world_jacobian_start + nbd * row_j + col_F_start
vio_j = world_cts_start + row_j
lambda_j = inv_dt * lambdas_data[vio_j]
for i in range(6):
w_j_F[i] += jacobian_cts_data[mio_j + i] * lambda_j
for j in range(num_kin_cts_j):
row_j = world_jkcgo + kin_cts_start_j + j
mio_j = world_jacobian_start + nbd * row_j + col_F_start
vio_j = world_cts_start + row_j
lambda_j = inv_dt * lambdas_data[vio_j]
for i in range(6):
w_j_F[i] += jacobian_cts_data[mio_j + i] * lambda_j
wp.atomic_add(data_bodies_w_j, bid_F_j, w_j_F)
# Compute and store the joint constraint wrench for the Base body if bid_B >= 0
# NOTE: We need to scale by the time-step because the lambdas are impulses
if bid_B_j >= 0:
w_j_B = vec6f(0.0)
col_B_start = 6 * (bid_B_j - bio)
for j in range(num_dyn_cts_j):
row_j = world_jdcgo + dyn_cts_start_j + j
mio_j = world_jacobian_start + nbd * row_j + col_B_start
vio_j = world_cts_start + row_j
lambda_j = inv_dt * lambdas_data[vio_j]
for i in range(6):
w_j_B[i] += jacobian_cts_data[mio_j + i] * lambda_j
for j in range(num_kin_cts_j):
row_j = world_jkcgo + kin_cts_start_j + j
mio_j = world_jacobian_start + nbd * row_j + col_B_start
vio_j = world_cts_start + row_j
lambda_j = inv_dt * lambdas_data[vio_j]
for i in range(6):
w_j_B[i] += jacobian_cts_data[mio_j + i] * lambda_j
wp.atomic_add(data_bodies_w_j, bid_B_j, w_j_B)
@wp.kernel
def _compute_limit_cts_body_wrenches_dense(
# Inputs:
model_info_num_body_dofs: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
data_info_limit_cts_group_offset: wp.array(dtype=int32),
model_time_inv_dt: wp.array(dtype=float32),
limits_model_num: wp.array(dtype=int32),
limits_model_max: int32,
limits_wid: wp.array(dtype=int32),
limits_lid: wp.array(dtype=int32),
limits_bids: wp.array(dtype=vec2i),
jacobian_cts_offset: wp.array(dtype=int32),
jacobian_cts_data: wp.array(dtype=float32),
lambdas_offsets: wp.array(dtype=int32),
lambdas_data: wp.array(dtype=float32),
# Outputs:
data_bodies_w_l: wp.array(dtype=vec6f),
):
# Retrieve the thread index
tid = wp.tid()
# Skip if tid is greater than the number of active limits in the model
if tid >= wp.min(limits_model_num[0], limits_model_max):
return
# Retrieve the limit index of the limit w.r.t the world
lid = limits_lid[tid]
# Retrieve the world index of the limit
wid = limits_wid[tid]
# Extract the body indices associated with the limit
# NOTE: These indices are w.r.t the model
bids = limits_bids[tid]
bid_B = bids[0]
bid_F = bids[1]
# Retrieve the inverse time-step of the world
inv_dt = model_time_inv_dt[wid]
# Retrieve the world-specific info
nbd = model_info_num_body_dofs[wid]
bio = model_info_bodies_offset[wid]
mio = jacobian_cts_offset[wid]
vio = lambdas_offsets[wid]
# Retrieve the index offset of the active limit constraints of the world
lcgo = data_info_limit_cts_group_offset[wid]
# Compute the index offsets of the limit constraint
cio_l = lcgo + lid
vio_l = vio + cio_l
mio_l = mio + nbd * cio_l
# Extract the limit force/torque from the impulse
# NOTE: We need to scale by the time-step because the lambdas are impulses
lambda_l = inv_dt * lambdas_data[vio_l]
# Extract the limit constraint Jacobian for the follower body
JT_l_F = vec6f(0.0)
dio_F = 6 * (bid_F - bio)
mio_lF = mio_l + dio_F
for i in range(6):
JT_l_F[i] = jacobian_cts_data[mio_lF + i]
# Compute the limit constraint wrench for the follower body
w_l_F = JT_l_F * lambda_l
# Store the limit constraint wrench for the follower body
wp.atomic_add(data_bodies_w_l, bid_F, w_l_F)
# Compute the limit constraint wrench for the joint base body if bid_B >= 0
if bid_B >= 0:
# Extract the limit constraint Jacobian for the base body
JT_l_B = vec6f(0.0)
dio_B = 6 * (bid_B - bio)
mio_lB = mio_l + dio_B
for i in range(6):
JT_l_B[i] = jacobian_cts_data[mio_lB + i]
# Compute the limit constraint wrench for the base body
w_l_B = JT_l_B * lambda_l
# Store the limit constraint wrench for the base body
wp.atomic_add(data_bodies_w_l, bid_B, w_l_B)
@wp.kernel
def _compute_contact_cts_body_wrenches_dense(
# Inputs:
model_info_num_body_dofs: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
data_info_contact_cts_group_offset: wp.array(dtype=int32),
model_time_inv_dt: wp.array(dtype=float32),
contacts_model_num: wp.array(dtype=int32),
contacts_model_max: int32,
contacts_wid: wp.array(dtype=int32),
contacts_cid: wp.array(dtype=int32),
contacts_bid_AB: wp.array(dtype=vec2i),
jacobian_cts_offset: wp.array(dtype=int32),
jacobian_cts_data: wp.array(dtype=float32),
lambdas_offsets: wp.array(dtype=int32),
lambdas_data: wp.array(dtype=float32),
# Outputs:
data_bodies_w_c: wp.array(dtype=vec6f),
):
# Retrieve the thread index
tid = wp.tid()
# Skip if tid is greater than the number of active contacts in the model
if tid >= wp.min(contacts_model_num[0], contacts_model_max):
return
# Retrieve the contact index of the contact w.r.t the world
cid = contacts_cid[tid]
# Retrieve the world index of the contact
wid = contacts_wid[tid]
# Extract the body indices associated with the contact
# NOTE: These indices are w.r.t the model
bid_AB = contacts_bid_AB[tid]
bid_A = bid_AB[0]
bid_B = bid_AB[1]
# Retrieve the inverse time-step of the world
inv_dt = model_time_inv_dt[wid]
# Retrieve the world-specific info data
nbd = model_info_num_body_dofs[wid]
bio = model_info_bodies_offset[wid]
mio = jacobian_cts_offset[wid]
vio = lambdas_offsets[wid]
# Retrieve the index offset of the active contact constraints of the world
ccgo = data_info_contact_cts_group_offset[wid]
# Compute the index offsets of the contact constraint
k = 3 * cid
cio_k = ccgo + k
vio_k = vio + cio_k
mio_k = mio + nbd * cio_k
# Extract the 3D contact force
# NOTE: We need to scale by the time-step because the lambdas are impulses
lambda_c = inv_dt * vec3f(lambdas_data[vio_k], lambdas_data[vio_k + 1], lambdas_data[vio_k + 2])
# Extract the contact constraint Jacobian for body B
JT_c_B = mat63f(0.0)
dio_B = 6 * (bid_B - bio)
for j in range(3):
mio_kj = mio_k + nbd * j + dio_B
for i in range(6):
JT_c_B[i, j] = jacobian_cts_data[mio_kj + i]
# Compute the contact constraint wrench for body B
w_c_B = JT_c_B @ lambda_c
# Store the contact constraint wrench for body B
wp.atomic_add(data_bodies_w_c, bid_B, w_c_B)
# Compute the contact constraint wrench for body A if bid_A >= 0
if bid_A >= 0:
# Extract the contact constraint Jacobian for body A
JT_c_A = mat63f(0.0)
dio_A = 6 * (bid_A - bio)
for j in range(3):
mio_kj = mio_k + nbd * j + dio_A
for i in range(6):
JT_c_A[i, j] = jacobian_cts_data[mio_kj + i]
# Compute the contact constraint wrench for body A
w_c_A = JT_c_A @ lambda_c
# Store the contact constraint wrench for body A
wp.atomic_add(data_bodies_w_c, bid_A, w_c_A)
@wp.kernel
def _compute_cts_body_wrenches_sparse(
# Inputs:
model_time_inv_dt: wp.array(dtype=float32),
model_info_bodies_offset: wp.array(dtype=int32),
data_info_limit_cts_group_offset: wp.array(dtype=int32),
data_info_contact_cts_group_offset: wp.array(dtype=int32),
jac_num_nzb: wp.array(dtype=int32),
jac_nzb_start: wp.array(dtype=int32),
jac_nzb_coords: wp.array2d(dtype=int32),
jac_nzb_values: wp.array(dtype=vec6f),
lambdas_offsets: wp.array(dtype=int32),
lambdas_data: wp.array(dtype=float32),
# Outputs:
data_bodies_w_j_i: wp.array(dtype=vec6f),
data_bodies_w_l_i: wp.array(dtype=vec6f),
data_bodies_w_c_i: wp.array(dtype=vec6f),
):
# Retrieve the world and non-zero
# block indices from the thread grid
wid, nzbid = wp.tid()
# Skip if the non-zero block index is greater than
# the number of active non-zero blocks for the world
if nzbid >= jac_num_nzb[wid]:
return
# Retrieve the inverse time-step of the world
inv_dt = model_time_inv_dt[wid]
# Retrieve world-specific index offsets
world_bid_start = model_info_bodies_offset[wid]
J_cts_nzb_start = jac_nzb_start[wid]
world_cts_start = lambdas_offsets[wid]
limit_cts_group_start = data_info_limit_cts_group_offset[wid]
contact_cts_group_start = data_info_contact_cts_group_offset[wid]
# Retrieve the Jacobian matrix block coordinates
# and values for the current non-zero block
global_nzb_idx = J_cts_nzb_start + nzbid
J_ji_coords = jac_nzb_coords[global_nzb_idx]
J_ji = jac_nzb_values[global_nzb_idx]
# Get constraint and body from the block coordinates
cts_row = J_ji_coords[0]
bid_j = J_ji_coords[1] // 6
# Get global body index, i.e. w.r.t the model
global_bid_j = world_bid_start + bid_j
# Retrieve the constraint reaction of the current constraint row
# NOTE: We need to scale by the time-step because the lambdas are impulses
lambda_j = inv_dt * lambdas_data[world_cts_start + cts_row]
# Compute the joint constraint wrench for the body
w_ij = lambda_j * J_ji
# Add the wrench to the appropriate array
if cts_row >= contact_cts_group_start:
wp.atomic_add(data_bodies_w_c_i, global_bid_j, w_ij)
elif cts_row >= limit_cts_group_start:
wp.atomic_add(data_bodies_w_l_i, global_bid_j, w_ij)
else:
wp.atomic_add(data_bodies_w_j_i, global_bid_j, w_ij)
###
# Launchers
###
def compute_joint_dof_body_wrenches_dense(
model: ModelKamino, data: DataKamino, jacobians: DenseSystemJacobians, reset_to_zero: bool = True
):
"""
Update the actuation wrenches of the bodies based on the active joint torques.
"""
# First check that the Jacobians are dense
if not isinstance(jacobians, DenseSystemJacobians):
raise ValueError(f"Expected `DenseSystemJacobians` but got {type(jacobians)}.")
# Clear the previous actuation wrenches, because the kernel computing them
# uses an atomic add to accumulate contributions from each joint DoF, and
# thus assumes the target array is zeroed out before each call
if reset_to_zero:
data.bodies.w_a_i.zero_()
# Then compute the body wrenches resulting from the current generalized actuation forces
wp.launch(
_compute_joint_dof_body_wrenches_dense,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.num_body_dofs,
model.info.bodies_offset,
model.info.joint_dofs_offset,
model.joints.num_dofs,
model.joints.dofs_offset,
model.joints.wid,
model.joints.bid_B,
model.joints.bid_F,
data.joints.tau_j,
jacobians.data.J_dofs_offsets,
jacobians.data.J_dofs_data,
# Outputs:
data.bodies.w_a_i,
],
)
def compute_joint_dof_body_wrenches_sparse(
model: ModelKamino, data: DataKamino, jacobians: SparseSystemJacobians, reset_to_zero: bool = True
) -> None:
"""
Update the actuation wrenches of the bodies based on the active joint torques.
"""
# First check that the Jacobians are sparse
if not isinstance(jacobians, SparseSystemJacobians):
raise ValueError(f"Expected `SparseSystemJacobians` but got {type(jacobians)}.")
# Clear the previous actuation wrenches, because the kernel computing them
# uses an atomic add to accumulate contributions from each joint DoF, and
# thus assumes the target array is zeroed out before each call
if reset_to_zero:
data.bodies.w_a_i.zero_()
# Then compute the body wrenches resulting from the current generalized actuation forces
wp.launch(
_compute_joint_dof_body_wrenches_sparse,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.joint_dofs_offset,
model.joints.num_dofs,
model.joints.dofs_offset,
model.joints.wid,
model.joints.bid_B,
model.joints.bid_F,
data.joints.tau_j,
jacobians._J_dofs_joint_nzb_offsets,
jacobians._J_dofs.bsm.nzb_values,
# Outputs:
data.bodies.w_a_i,
],
)
def compute_joint_dof_body_wrenches(
model: ModelKamino,
data: DataKamino,
jacobians: DenseSystemJacobians | SparseSystemJacobians,
reset_to_zero: bool = True,
) -> None:
"""
Update the actuation wrenches of the bodies based on the active joint torques.
"""
if isinstance(jacobians, DenseSystemJacobians):
compute_joint_dof_body_wrenches_dense(model, data, jacobians, reset_to_zero)
elif isinstance(jacobians, SparseSystemJacobians):
compute_joint_dof_body_wrenches_sparse(model, data, jacobians, reset_to_zero)
else:
raise ValueError(f"Expected `DenseSystemJacobians` or `SparseSystemJacobians` but got {type(jacobians)}.")
def compute_constraint_body_wrenches_dense(
model: ModelKamino,
data: DataKamino,
jacobians: DenseSystemJacobians,
lambdas_offsets: wp.array,
lambdas_data: wp.array,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
reset_to_zero: bool = True,
):
"""
Launches the kernels to compute the body-wise constraint wrenches.
"""
# First check that the Jacobians are dense
if not isinstance(jacobians, DenseSystemJacobians):
raise ValueError(f"Expected `DenseSystemJacobians` but got {type(jacobians)}.")
# Proceed by constraint type, since the Jacobian and lambda data are
# stored in separate blocks for each constraint type in the dense case
if model.size.sum_of_num_joints > 0:
if reset_to_zero:
data.bodies.w_j_i.zero_()
wp.launch(
_compute_joint_cts_body_wrenches_dense,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.num_body_dofs,
model.info.bodies_offset,
model.info.joint_dynamic_cts_group_offset,
model.info.joint_kinematic_cts_group_offset,
model.time.inv_dt,
model.joints.wid,
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
model.joints.bid_B,
model.joints.bid_F,
jacobians.data.J_cts_offsets,
jacobians.data.J_cts_data,
lambdas_offsets,
lambdas_data,
# Outputs:
data.bodies.w_j_i,
],
)
if limits is not None and limits.model_max_limits_host > 0:
if reset_to_zero:
data.bodies.w_l_i.zero_()
wp.launch(
_compute_limit_cts_body_wrenches_dense,
dim=limits.model_max_limits_host,
inputs=[
# Inputs:
model.info.num_body_dofs,
model.info.bodies_offset,
data.info.limit_cts_group_offset,
model.time.inv_dt,
limits.model_active_limits,
limits.model_max_limits_host,
limits.wid,
limits.lid,
limits.bids,
jacobians.data.J_cts_offsets,
jacobians.data.J_cts_data,
lambdas_offsets,
lambdas_data,
# Outputs:
data.bodies.w_l_i,
],
)
if contacts is not None and contacts.model_max_contacts_host > 0:
if reset_to_zero:
data.bodies.w_c_i.zero_()
wp.launch(
_compute_contact_cts_body_wrenches_dense,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs:
model.info.num_body_dofs,
model.info.bodies_offset,
data.info.contact_cts_group_offset,
model.time.inv_dt,
contacts.model_active_contacts,
contacts.model_max_contacts_host,
contacts.wid,
contacts.cid,
contacts.bid_AB,
jacobians.data.J_cts_offsets,
jacobians.data.J_cts_data,
lambdas_offsets,
lambdas_data,
# Outputs:
data.bodies.w_c_i,
],
)
def compute_constraint_body_wrenches_sparse(
model: ModelKamino,
data: DataKamino,
jacobians: SparseSystemJacobians,
lambdas_offsets: wp.array,
lambdas_data: wp.array,
reset_to_zero: bool = True,
):
"""
Launches the kernels to compute the body-wise constraint wrenches.
"""
# First check that the Jacobians are sparse
if not isinstance(jacobians, SparseSystemJacobians):
raise ValueError(f"Expected `SparseSystemJacobians` but got {type(jacobians)}.")
# Optionally clear the previous constraint wrenches, because the kernel computing them
# uses an `wp.atomic_add` op to accumulate contributions from each constraint non-zero
# block, and thus assumes the target arrays are zeroed out before each call
if reset_to_zero:
data.bodies.w_j_i.zero_()
data.bodies.w_l_i.zero_()
data.bodies.w_c_i.zero_()
# Then compute the body wrenches resulting from the current active constraints
wp.launch(
_compute_cts_body_wrenches_sparse,
dim=(model.size.num_worlds, jacobians._J_cts.bsm.max_of_num_nzb),
inputs=[
# Inputs:
model.time.inv_dt,
model.info.bodies_offset,
data.info.limit_cts_group_offset,
data.info.contact_cts_group_offset,
jacobians._J_cts.bsm.num_nzb,
jacobians._J_cts.bsm.nzb_start,
jacobians._J_cts.bsm.nzb_coords,
jacobians._J_cts.bsm.nzb_values,
lambdas_offsets,
lambdas_data,
# Outputs:
data.bodies.w_j_i,
data.bodies.w_l_i,
data.bodies.w_c_i,
],
)
def compute_constraint_body_wrenches(
model: ModelKamino,
data: DataKamino,
jacobians: DenseSystemJacobians | SparseSystemJacobians,
lambdas_offsets: wp.array,
lambdas_data: wp.array,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
reset_to_zero: bool = True,
):
"""
Launches the kernels to compute the body-wise constraint wrenches.
"""
if isinstance(jacobians, DenseSystemJacobians):
compute_constraint_body_wrenches_dense(
model=model,
data=data,
jacobians=jacobians,
lambdas_offsets=lambdas_offsets,
lambdas_data=lambdas_data,
limits=limits,
contacts=contacts,
reset_to_zero=reset_to_zero,
)
elif isinstance(jacobians, SparseSystemJacobians):
compute_constraint_body_wrenches_sparse(
model=model,
data=data,
jacobians=jacobians,
lambdas_offsets=lambdas_offsets,
lambdas_data=lambdas_data,
reset_to_zero=reset_to_zero,
)
else:
raise ValueError(f"Expected `DenseSystemJacobians` or `SparseSystemJacobians` but got {type(jacobians)}.")
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
The geometry module of Kamino, providing data types and
pipelines (i.e. backends) for Collision Detection (CD).
This module provides a front-end defined by:
- :class:`ContactMode`:
An enumeration defining the different modes a contact can be in, such as `OPEN`,
`STICKING`, and `SLIDING`, and defines utilities for computing contacts modes.
- :class:`ContactsKaminoData`:
A simple dataclass defining the data layout and contents of discrete contacts.
- :class:`ContactsKamino`:
A data interface class for allocating and managing contacts data. This
serves as the container with which collision detection pipelines operate,
storing all generated contacts.
- :class:`CollisionDetector`:
A high-level interface for wrapping collision detection pipelines (i.e. backends).
This class provides a unified interface for performing collision detection
using different pipelines, and is responsible for determining the necessary
allocations of contacts data based on the contents of the simulation.
- :class:`CollisionPipelineType`:
An enumeration defining the different collision detection pipelines
(i.e. backends) supported by Kamino.
- :class:`BroadPhaseType`:
An enumeration defining the different broad-phase
algorithms supported by Kamino's CD pipelines.
- :class:`BoundingVolumeType`:
An enumeration defining the different types of bounding volumes
supported by Kamino's broad-phase collision detection back-ends.
- :class:`CollisionPipelineUnifiedKamino`:
A collision detection pipeline wrapping and specializing a unified CD pipeline of Newton.
- :class:`CollisionPipelinePrimitive`:
A collision detection pipeline optimized for primitive shapes.
This pipeline uses an `EXPLICIT` broad-phase operating on pre-computed
geometry pairs and a narrow-phase based on the primitive colliders of Newton.
"""
from .aggregation import ContactAggregation, ContactAggregationData
from .contacts import ContactMode, ContactsKamino, ContactsKaminoData
from .detector import (
BroadPhaseType,
CollisionDetector,
CollisionPipelineType,
)
from .primitive import BoundingVolumeType, CollisionPipelinePrimitive
from .unified import CollisionPipelineUnifiedKamino
###
# Module interface
###
__all__ = [
"BoundingVolumeType",
"BroadPhaseType",
"CollisionDetector",
"CollisionPipelinePrimitive",
"CollisionPipelineType",
"CollisionPipelineUnifiedKamino",
"ContactAggregation",
"ContactAggregationData",
"ContactMode",
"ContactsKamino",
"ContactsKaminoData",
]
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/aggregation.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Contact aggregation for RL applications.
This module provides functionality to aggregate per-contact data from Kamino's
ContactsKaminoData into per-body and per-geom summaries suitable for RL observations.
The aggregation is performed on GPU using efficient atomic operations.
"""
from dataclasses import dataclass
import warp as wp
from ..core.model import ModelKamino
from ..core.types import int32, quatf, vec2i, vec3f
from .contacts import ContactMode, ContactsKamino
###
# Module interface
###
__all__ = [
"ContactAggregation",
"ContactAggregationData",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _aggregate_contact_force_per_body(
# Inputs:
model_info_bodies_start: wp.array(dtype=int32), # Per-world bodies start index
model_active_contacts: wp.array(dtype=int32), # contacts over all worlds
contact_wid: wp.array(dtype=int32), # world index per contact
contact_bid_AB: wp.array(dtype=vec2i), # body pair per contact (global body indices)
contact_reaction: wp.array(dtype=vec3f), # force in local contact frame
contact_frame: wp.array(dtype=quatf), # contact frame (rotation quaternion)
contact_mode: wp.array(dtype=int32), # contact mode
# Outputs:
body_net_force: wp.array3d(dtype=wp.float32), # [num_worlds, max_bodies, 3]
body_contact_flag: wp.array2d(dtype=int32), # [num_worlds, max_bodies]
):
"""
Aggregate contact force and flags per body across all contacts.
Each thread processes one contact. Forces are transformed from local
contact frame to world frame, then atomically accumulated to both
bodies in the contact pair. Contact flags are set for both bodies.
Args:
model_info_bodies_start: Array of start indices for bodies in each world
model_active_contacts: Number of active contacts over all worlds
wid: World index for each contact
bid_AB: Body index pair (A, B) for each contact
reaction: 3D contact force in local contact frame [normal, tangent1, tangent2]
frame: Contact frame as rotation quaternion w.r.t world
mode: Contact mode (INACTIVE, OPENING, STICKING, SLIDING)
body_net_force: Output array for net force per body (world frame)
body_contact_flag: Output array for contact flag per body
"""
# Retrieve the contact index for this thread
contact_idx = wp.tid()
# Early exit if this thread is beyond active contacts
if contact_idx >= model_active_contacts[0]:
return
# Skip inactive contacts
if contact_mode[contact_idx] == ContactMode.INACTIVE:
return
# Get contact-specific data
world_idx = contact_wid[contact_idx]
bid_AB = contact_bid_AB[contact_idx]
global_body_A = bid_AB[0] # Global body index
global_body_B = bid_AB[1] # Global body index
# Retrieve the start index for bodies in this world to convert global body IDs to per-world indices
bodies_start = model_info_bodies_start[world_idx]
# Transform force from local contact frame to world frame
force_local = contact_reaction[contact_idx]
contact_quat = contact_frame[contact_idx]
force_world = wp.quat_rotate(contact_quat, force_local)
# Accumulate force to both bodies (equal and opposite)
# Skip static bodies (bid < 0, e.g., ground plane)
# Convert global body indices to per-world body indices for array indexing
# Need to add each component separately for atomic operations on 3D arrays
if global_body_A >= 0:
body_A_in_world = global_body_A - bodies_start # Convert to per-world index
for i in range(3):
wp.atomic_add(body_net_force, world_idx, body_A_in_world, i, -force_world[i])
wp.atomic_max(body_contact_flag, world_idx, body_A_in_world, int32(1))
if global_body_B >= 0:
body_B_in_world = global_body_B - bodies_start # Convert to per-world index
for i in range(3):
wp.atomic_add(body_net_force, world_idx, body_B_in_world, i, force_world[i])
wp.atomic_max(body_contact_flag, world_idx, body_B_in_world, int32(1))
@wp.kernel
def _aggregate_static_contact_flag_per_body(
# Inputs:
model_info_bodies_start: wp.array(dtype=int32), # Per-world bodies start index
model_active_contacts: wp.array(dtype=int32), # contacts over all worlds
contact_wid: wp.array(dtype=int32), # world index per contact
contact_bid_AB: wp.array(dtype=vec2i), # body pair per contact (global body indices)
contact_mode: wp.array(dtype=int32), # contact mode
# Outputs:
static_contact_flag: wp.array2d(dtype=int32), # [num_worlds, max_bodies]
):
"""
Identify which bodies are in contact with static geometries.
Each thread processes one contact. If either geometry in the contact
pair is marked as static, the corresponding non-static body's static
contact flag is set.
Args:
model_active_contacts: Number of active contacts over all worlds
contact_wid: World index for each contact
contact_bid_AB: Body index pair (A, B) for each contact
contact_gid_AB: Geometry index pair (A, B) for each contact
contact_mode: Contact mode (INACTIVE, OPENING, STICKING, SLIDING)
static_contact_flag: Output array for static contact flag per body
"""
# Retrieve the contact index for this thread
contact_idx = wp.tid()
# Early exit if this thread is beyond active contacts
if contact_idx >= model_active_contacts[0]:
return
# Skip inactive contacts
if contact_mode[contact_idx] == ContactMode.INACTIVE:
return
# Retrieve contact-specific data
world_idx = contact_wid[contact_idx]
bid_AB = contact_bid_AB[contact_idx]
global_body_A = bid_AB[0] # Global body index
global_body_B = bid_AB[1] # Global body index
# Retrieve the start index for bodies in this world to convert global body IDs to per-world indices
bodies_start = model_info_bodies_start[world_idx]
# Set static contact flag for non-static body
# Convert global body indices to per-world body indices for array indexing
# Skip static bodies (bid < 0, e.g., static plane)
if global_body_B < 0 and global_body_A >= 0:
# Body A is in contact with static (geom B)
body_A_in_world = global_body_A - bodies_start
wp.atomic_max(static_contact_flag, world_idx, body_A_in_world, int32(1))
if global_body_A < 0 and global_body_B >= 0:
# Body B is in contact with static (geom A)
body_B_in_world = global_body_B - bodies_start
wp.atomic_max(static_contact_flag, world_idx, body_B_in_world, int32(1))
@wp.kernel
def _aggregate_contact_force_per_body_geom(
# Inputs:
model_info_geoms_start: wp.array(dtype=int32), # Offset to convert global geom ID to per-world index
model_active_contacts: wp.array(dtype=int32), # contacts over all worlds
contact_wid: wp.array(dtype=int32), # world index per contact
contact_gid_AB: wp.array(dtype=vec2i), # geometry pair per contact
contact_bid_AB: wp.array(dtype=vec2i), # geometry pair per contact
contact_reaction: wp.array(dtype=vec3f), # force in local contact frame
contact_frame: wp.array(dtype=quatf), # contact frame (rotation quaternion)
contact_mode: wp.array(dtype=int32), # contact mode
# Outputs:
geom_net_force: wp.array3d(dtype=wp.float32), # [num_worlds, max_geoms, 3]
geom_contact_flag: wp.array2d(dtype=int32), # [num_worlds, max_geoms]
):
"""
Aggregate contact force and flags per geometry across all contacts.
Similar to _aggregate_contact_force_per_body, but aggregates to geometry
level instead of body level. Useful for detailed contact analysis in RL.
Args:
model_info_geoms_start: Start index of per-world geoms
world_active_contacts: Number of active contacts per world
contact_wid: World index for each contact
contact_gid_AB: Geometry index pair (A, B) for each contact
contact_reaction: 3D contact force in local contact frame [normal, tangent1, tangent2]
contact_frame: Contact frame as rotation quaternion w.r.t world
contact_mode: Contact mode (INACTIVE, OPENING, STICKING, SLIDING)
geom_net_force: Output array for net force per geometry (world frame)
geom_contact_flag: Output array for contact flag per geometry
"""
# Retrieve the contact index for this thread
contact_idx = wp.tid()
# Early exit if this thread is beyond active contacts
if contact_idx >= model_active_contacts[0]:
return
# Skip inactive contacts
if contact_mode[contact_idx] == ContactMode.INACTIVE:
return
# Get contact-specific data
world_idx = contact_wid[contact_idx]
gid_AB = contact_gid_AB[contact_idx]
bid_AB = contact_bid_AB[contact_idx]
global_geom_A = gid_AB[0] # Global geom index
global_geom_B = gid_AB[1] # Global geom index
global_body_A = bid_AB[0] # Global body index
global_body_B = bid_AB[1] # Global body index
# Compute in-world geom indices
world_geom_start = model_info_geoms_start[world_idx]
# Transform force from local contact frame to world frame
force_local = contact_reaction[contact_idx]
contact_quat = contact_frame[contact_idx]
force_world = wp.quat_rotate(contact_quat, force_local)
# Accumulate force to both geometries (equal and opposite)
# Need to add each component separately for atomic operations on 3D arrays
if global_body_A >= 0:
world_geom_A = global_geom_A - world_geom_start # Convert to per-world index
for i in range(3):
wp.atomic_add(geom_net_force, world_idx, world_geom_A, i, force_world[i])
wp.atomic_max(geom_contact_flag, world_idx, world_geom_A, int32(1))
if global_body_B >= 0:
world_geom_B = global_geom_B - world_geom_start # Convert to per-world index
for i in range(3):
wp.atomic_add(geom_net_force, world_idx, world_geom_B, i, force_world[i])
wp.atomic_max(geom_contact_flag, world_idx, world_geom_B, int32(1))
@wp.kernel
def _aggregate_body_pair_contact_flag_per_world(
# Input: Kamino ContactsData
wid: wp.array(dtype=int32), # world index per contact
bid_AB: wp.array(dtype=vec2i), # body pair per contact (global body indices)
mode: wp.array(dtype=int32), # contact mode
world_active_contacts: wp.array(dtype=int32), # contacts per world
# Model data for global to per-world body ID conversion
model_body_bid: wp.array(dtype=int32), # Per-world body ID for each global body
num_worlds: int,
# Target body pair (per-world body indices)
target_body_a: int,
target_body_b: int,
# Output
body_pair_contact_flag: wp.array(dtype=int32), # [num_worlds]
):
"""
Detect contact between a specific pair of bodies across all worlds.
Each thread processes one contact. If the contact involves the target
body pair (in either order), the per-world flag is set.
Args:
wid: World index for each contact
bid_AB: Body index pair (A, B) for each contact
mode: Contact mode (INACTIVE, OPENING, STICKING, SLIDING)
world_active_contacts: Number of active contacts per world
model_body_bid: Mapping from global body index to per-world body index
num_worlds: Total number of worlds
target_body_a: Per-world body index of the first body in the target pair
target_body_b: Per-world body index of the second body in the target pair
body_pair_contact_flag: Output flag per world (1 if pair is in contact)
"""
contact_idx = wp.tid()
# Calculate total active contacts across all worlds
total_contacts = int32(0)
for w in range(num_worlds):
total_contacts += world_active_contacts[w]
# Early exit if this thread is beyond active contacts
if contact_idx >= total_contacts:
return
# Skip inactive contacts
if mode[contact_idx] == ContactMode.INACTIVE:
return
# Get contact data
world_idx = wid[contact_idx]
body_pair = bid_AB[contact_idx]
global_body_A = body_pair[0]
global_body_B = body_pair[1]
# Skip static bodies (bid < 0)
if global_body_A < 0 or global_body_B < 0:
return
# Convert global body indices to per-world body indices
body_A_in_world = model_body_bid[global_body_A]
body_B_in_world = model_body_bid[global_body_B]
# Check if this contact matches the target pair (in either order)
if (body_A_in_world == target_body_a and body_B_in_world == target_body_b) or (
body_A_in_world == target_body_b and body_B_in_world == target_body_a
):
wp.atomic_max(body_pair_contact_flag, world_idx, int32(1))
###
# Types
###
@dataclass
class ContactAggregationData:
"""
Pre-allocated arrays for aggregating contact data per world and body.
Designed for efficient GPU computation and zero-copy PyTorch access.
"""
# === Per-Body Aggregated Data (for RL interface) ===
body_net_contact_force: wp.array | None = None
"""
Net contact force per body (world frame).\n
Shape `(num_worlds, max_bodies_per_world)` and dtype=:class:`vec3f`.
"""
body_contact_flag: wp.array | None = None
"""
Binary contact flag per body (any contact).
Shape `(num_worlds, max_bodies_per_world)` and dtype=:class:`int32` (0 or 1).
"""
body_static_contact_flag: wp.array | None = None
"""
Static contact flag per body (contact with static geoms).\n
Shape `(num_worlds, max_bodies_per_world)` and dtype=:class:`int32` (0 or 1).
"""
# === Per-Geom Detailed Data (for advanced RL) ===
geom_net_contact_force: wp.array | None = None
"""
Net contact force per geometry (world frame).\n
Shape `(num_worlds, max_geoms_per_world, 3)` and dtype=:class:`float32`.
"""
geom_contact_flag: wp.array | None = None
"""
Contact flags per geometry.
Shape `(num_worlds, max_geoms_per_world)` and dtype=:class:`int32` (0 or 1).
"""
# === Contact Position/Normal Data (optional, for visualization) ===
body_contact_position: wp.array | None = None
"""
Average contact position per body (world frame).\n
Shape `(num_worlds, max_bodies_per_world, 3)` and dtype=:class:`float32`.
"""
body_contact_normal: wp.array | None = None
"""
Average contact normal per body (world frame).\n
Shape `(num_worlds, max_bodies_per_world, 3)` and dtype=:class:`float32`.
"""
body_num_contacts: wp.array | None = None
"""
Number of contacts per body.\n
Shape `(num_worlds, max_bodies_per_world)` and dtype=:class:`int32`.
"""
# === Body-Pair Contact Detection ===
body_pair_contact_flag: wp.array | None = None
"""
Per-world flag indicating contact between a specific body pair.\n
Shape `(num_worlds,)` and dtype=:class:`int32` (0 or 1).
"""
###
# Interfaces
###
class ContactAggregation:
"""
High-level interface for aggregating Kamino contact data for RL.
This class efficiently aggregates per-contact data from Kamino's ContactsKaminoData
into per-body and per-geom summaries suitable for RL observations. All computation
is performed on GPU using atomic operations for efficiency.
Usage:
aggregation = ContactAggregation(model, contacts, static_geom_ids=[0])
aggregation.compute() # Call after simulator.step()
# Access via PyTorch tensors (zero-copy)
net_force = wp.to_torch(aggregation.body_net_force)
contact_flag = wp.to_torch(aggregation.body_contact_flag)
"""
def __init__(
self,
model: ModelKamino | None = None,
contacts: ContactsKamino | None = None,
enable_positions_normals: bool = False,
device: wp.DeviceLike = None,
):
"""Initialize contact aggregation.
Args:
model (ModelKamino | None):
The model container describing the system to be simulated.
If None, call ``finalize()`` later.
contacts (ContactsKamino | None):
The contacts container with per-contact data.
If None, call ``finalize()`` later.
device: Device for computation.
If None, uses model's device.
enable_positions_normals:
Whether to compute average contact positions and normals per body.
"""
# Cache the device
self._device: wp.DeviceLike | None = device
# Forward declarations
self._model: ModelKamino | None = None
self._contacts: ContactsKamino | None = None
self._data: ContactAggregationData | None = None
self._enable_positions_normals: bool = enable_positions_normals
# Body-pair filter (set via set_body_pair_filter)
self._body_pair_target_a: int = -1
self._body_pair_target_b: int = -1
# Proceed with memory allocations if model and contacts are provided
if model is not None and contacts is not None:
self.finalize(
model=model, contacts=contacts, enable_positions_normals=enable_positions_normals, device=device
)
###
# Properties
###
@property
def body_net_force(self) -> wp.array:
"""Net force per body [num_worlds, max_bodies, 3]"""
return self._data.body_net_contact_force
@property
def body_contact_flag(self) -> wp.array:
"""Contact flags per body [num_worlds, max_bodies]"""
return self._data.body_contact_flag
@property
def body_static_contact_flag(self) -> wp.array:
"""Static contact flag per body [num_worlds, max_bodies]"""
return self._data.body_static_contact_flag
@property
def geom_net_force(self) -> wp.array:
"""Net force per geom [num_worlds, max_geoms, 3]"""
return self._data.geom_net_contact_force
@property
def geom_contact_flag(self) -> wp.array:
"""Contact flags per geom [num_worlds, max_geoms]"""
return self._data.geom_contact_flag
@property
def body_pair_contact_flag(self) -> wp.array:
"""Per-world body-pair contact flag [num_worlds]."""
return self._data.body_pair_contact_flag
###
# Operations
###
def finalize(
self,
model: ModelKamino,
contacts: ContactsKamino,
enable_positions_normals: bool = False,
device: wp.DeviceLike = None,
) -> None:
"""Finalizes memory allocations for the contact aggregation data.
Args:
model (ModelKamino): The model container describing the system to be simulated.
contacts (ContactsKamino): The contacts container with per-contact data.
device (wp.DeviceLike | None): Device for computation. If None, uses model's device.
"""
# Override the device if specified
if device is not None:
self._device = device
if self._device is None:
self._device = model.device
# Override the positions/normals flag if different from current setting
if enable_positions_normals != self._enable_positions_normals:
self._enable_positions_normals = enable_positions_normals
# Cache references to source model and contacts containers
self._model = model
self._contacts = contacts
# Create locals for better readability
num_worlds = model.size.num_worlds
max_bodies = model.size.max_of_num_bodies
max_geoms = model.size.max_of_num_geoms
extended = self._enable_positions_normals
# Allocate arrays for aggregated data based on model dimensions on the target device
with wp.ScopedDevice(self._device):
self._data = ContactAggregationData(
body_net_contact_force=wp.zeros((num_worlds, max_bodies, 3), dtype=wp.float32),
body_contact_flag=wp.zeros((num_worlds, max_bodies), dtype=wp.int32),
body_static_contact_flag=wp.zeros((num_worlds, max_bodies), dtype=wp.int32),
body_contact_position=wp.zeros((num_worlds, max_bodies, 3), dtype=wp.float32) if extended else None,
body_contact_normal=wp.zeros((num_worlds, max_bodies, 3), dtype=wp.float32) if extended else None,
body_num_contacts=wp.zeros((num_worlds, max_bodies), dtype=wp.int32) if extended else None,
geom_net_contact_force=wp.zeros((num_worlds, max_geoms, 3), dtype=wp.float32),
geom_contact_flag=wp.zeros((num_worlds, max_geoms), dtype=wp.int32),
)
def compute(self, skip_if_no_contacts: bool = False):
"""
Compute aggregated contact data from current ContactsKaminoData.
This method should be called after simulator.step() to update contact
force and flags. It launches GPU kernels to efficiently aggregate
per-contact data into per-body and per-geom summaries.
Args:
skip_if_no_contacts:
If True, check for zero contacts and return early.\n
Set to False when using CUDA graphs to avoid GPU-to-CPU copies.
"""
# Zero out previous results
self._data.body_net_contact_force.zero_()
self._data.body_contact_flag.zero_()
self._data.body_static_contact_flag.zero_()
self._data.geom_net_contact_force.zero_()
self._data.geom_contact_flag.zero_()
if self._enable_positions_normals:
self._data.body_contact_position.zero_()
self._data.body_contact_normal.zero_()
self._data.body_num_contacts.zero_()
# Get contact data
contacts_data = self._contacts.data
# Optionally check if there are any active contacts
# TODO @agon-serifi: Please check, but I think this might cause CPU-to-GPU transfer
# command during graph capture, which can be problematic. We might want to require
# the caller to check this before calling compute() when using graphs.
# TODO: Might be better to just let the kernels early-exit since they already do this
if skip_if_no_contacts:
total_active = contacts_data.model_active_contacts.numpy()[0]
if total_active == 0:
return # No contacts, nothing to aggregate
# Launch aggregation kernel for per-body force
wp.launch(
_aggregate_contact_force_per_body,
dim=contacts_data.model_max_contacts_host,
inputs=[
self._model.info.bodies_offset,
contacts_data.model_active_contacts,
contacts_data.wid,
contacts_data.bid_AB,
contacts_data.reaction,
contacts_data.frame,
contacts_data.mode,
],
outputs=[
self._data.body_net_contact_force,
self._data.body_contact_flag,
],
device=self._device,
)
# Launch aggregation kernel for static contact flag
wp.launch(
_aggregate_static_contact_flag_per_body,
dim=contacts_data.model_max_contacts_host,
inputs=[
self._model.info.bodies_offset,
contacts_data.model_active_contacts,
contacts_data.wid,
contacts_data.bid_AB,
contacts_data.mode,
],
outputs=[
self._data.body_static_contact_flag,
],
device=self._device,
)
# Launch aggregation kernel for per body-geom force
# NOTE: body-geom, in this case, refers to geoms belonging to dynamic bodies, meaning that static geoms are excluded
wp.launch(
_aggregate_contact_force_per_body_geom,
dim=contacts_data.model_max_contacts_host,
inputs=[
self._model.info.geoms_offset,
contacts_data.model_active_contacts,
contacts_data.wid,
contacts_data.gid_AB,
contacts_data.bid_AB,
contacts_data.reaction,
contacts_data.frame,
contacts_data.mode,
],
outputs=[
self._data.geom_net_contact_force,
self._data.geom_contact_flag,
],
device=self._device,
)
# ------------------------------------------------------------------
# Body-pair contact detection
# ------------------------------------------------------------------
def set_body_pair_filter(self, body_a_idx: int, body_b_idx: int) -> None:
"""Configure detection of contacts between a specific body pair.
After calling this, use :meth:`compute_body_pair_contact` to detect
whether the specified bodies are in contact in each world.
Args:
body_a_idx: Per-world body index of the first body.
body_b_idx: Per-world body index of the second body.
"""
self._body_pair_target_a = body_a_idx
self._body_pair_target_b = body_b_idx
# Allocate output array if not yet allocated
num_worlds = self._model.size.num_worlds
self._data.body_pair_contact_flag = wp.zeros(num_worlds, dtype=wp.int32, device=self._device)
def compute_body_pair_contact(self) -> None:
"""Detect contact between the configured body pair.
Must be called after :meth:`set_body_pair_filter`. This method is
separate from :meth:`compute` so it can be called outside of CUDA
graph capture when the body pair is configured after graph creation.
Raises:
RuntimeError: If no body pair filter has been configured.
"""
if self._body_pair_target_a < 0 or self._body_pair_target_b < 0:
return
self._data.body_pair_contact_flag.zero_()
contacts_data = self._contacts.data
num_worlds = self._model.size.num_worlds
wp.launch(
_aggregate_body_pair_contact_flag_per_world,
dim=contacts_data.model_max_contacts_host,
inputs=[
contacts_data.wid,
contacts_data.bid_AB,
contacts_data.mode,
contacts_data.world_active_contacts,
self._model.bodies.bid,
num_worlds,
self._body_pair_target_a,
self._body_pair_target_b,
],
outputs=[
self._data.body_pair_contact_flag,
],
device=self._device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/contacts.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Defines the representation of discrete contacts in Kamino.
This module provides a set of data types and operations that define
the data layout and conventions used to represent discrete contacts
within the Kamino solver. It includes:
- The :class:`ContactsKaminoData` dataclass defining the structure of contact data.
- The :class:`ContactMode` enumeration defining the discrete contact modes
and a member function that generates Warp functions to compute the contact
mode based on local contact velocities.
- Utility functions for constructing contact-local coordinate frames
supporting both a Z-up and X-up convention.
- The :class:`ContactsKamino` container which provides a high-level interface to
manage contact data, including allocations, access, and common operations,
and fundamentally serves as the primary output of collision detectors
as well as a cache of contact data to warm-start physics solvers.
"""
from __future__ import annotations
from dataclasses import dataclass, field
from enum import IntEnum
import warp as wp
from .....sim.contacts import Contacts
from .....sim.model import Model
from .....sim.state import State
from ..core.math import COS_PI_6, UNIT_X, UNIT_Y
from ..core.types import (
float32,
int32,
mat33f,
quatf,
uint32,
vec2f,
vec2i,
vec3f,
vec4f,
)
from ..utils import logger as msg
from .keying import build_pair_key2
###
# Module interface
###
__all__ = [
"DEFAULT_GEOM_PAIR_CONTACT_GAP",
"DEFAULT_GEOM_PAIR_MAX_CONTACTS",
"DEFAULT_TRIANGLE_MAX_PAIRS",
"DEFAULT_WORLD_MAX_CONTACTS",
"ContactMode",
"ContactsKamino",
"ContactsKaminoData",
"convert_contacts_kamino_to_newton",
"convert_contacts_newton_to_kamino",
"make_contact_frame_xnorm",
"make_contact_frame_znorm",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
DEFAULT_MODEL_MAX_CONTACTS: int = 1000
"""
The global default for maximum number of contacts per model.\n
Used when allocating contact data without a specified capacity.\n
Set to `1000`.
"""
DEFAULT_WORLD_MAX_CONTACTS: int = 128
"""
The global default for maximum number of contacts per world.\n
Used when allocating contact data without a specified capacity.\n
Set to `128`.
"""
DEFAULT_GEOM_PAIR_MAX_CONTACTS: int = 12
"""
The global default for maximum number of contacts per geom-pair.\n
Used when allocating contact data without a specified capacity.\n
Ignored for mesh-based collisions.\n
Set to `12` (with box-box collisions being a prototypical case).
"""
DEFAULT_TRIANGLE_MAX_PAIRS: int = 1_000_000
"""
The global default for maximum number of triangle pairs to consider in the narrow-phase.\n
Used only when the model contains triangle meshes or heightfields.\n
Defaults to `1_000_000`.
"""
DEFAULT_GEOM_PAIR_CONTACT_GAP: float = 1e-5
"""
The global default for the per-geometry detection gap [m].\n
Applied as a floor to each per-geometry gap value during pipeline
initialization so that every geometry has at least this detection
threshold.\n
Set to `1e-5`.
"""
###
# Types
###
class ContactMode(IntEnum):
"""An enumeration of discrete-contact modes."""
###
# Contact Modes
###
INACTIVE = -1
"""Indicates that contact is inactive (i.e. separated)."""
OPENING = 0
"""Indicates that contact was previously closed (i.e. STICKING or SLIDING) and is now opening."""
STICKING = 1
"""Indicates that contact is persisting (i.e. closed) without relative tangential motion."""
SLIDING = 2
"""Indicates that contact is persisting (i.e. closed) with relative tangential motion."""
###
# Utility Constants
###
DEFAULT_VN_MIN = 1e-3
"""The minimum normal velocity threshold for determining contact open or closed modes."""
DEFAULT_VT_MIN = 1e-3
"""The minimum tangential velocity threshold for determining contact stick or slip modes."""
###
# Utility Functions
###
@staticmethod
def make_compute_mode_func(vn_tol: float = DEFAULT_VN_MIN, vt_tol: float = DEFAULT_VT_MIN):
# Ensure tolerances are non-negative
if vn_tol < 0.0:
raise ValueError("ContactMode: vn_tol must be non-negative")
if vt_tol < 0.0:
raise ValueError("ContactMode: vt_tol must be non-negative")
# Generate the compute mode function based on the specified tolerances
@wp.func
def _compute_mode(v: vec3f) -> int32:
"""
Computes the discrete contact mode based on the contact velocity.
Args:
v (vec3f): The contact velocity expressed in the local contact frame.
Returns:
int32: The discrete contact mode as an integer value.
"""
# Decompose the velocity into the normal and tangential components
v_N = v.z
v_T_norm = wp.sqrt(v.x * v.x + v.y * v.y)
# Determine the contact mode
mode = int32(ContactMode.OPENING)
if v_N <= float32(vn_tol):
if v_T_norm <= float32(vt_tol):
mode = ContactMode.STICKING
else:
mode = ContactMode.SLIDING
# Return the resulting contact mode integer
return mode
# Return the generated compute mode function
return _compute_mode
@dataclass
class ContactsKaminoData:
"""
An SoA-based container to hold time-varying contact data of a set of contact elements.
This container is intended as the final output of collision detectors and as input to solvers.
"""
@staticmethod
def _default_num_world_max_contacts() -> list[int]:
return [0]
model_max_contacts_host: int = 0
"""
Host-side cache of the maximum number of contacts allocated across all worlds.\n
Intended for managing data allocations and setting thread sizes in kernels.
"""
world_max_contacts_host: list[int] = field(default_factory=_default_num_world_max_contacts)
"""
Host-side cache of the maximum number of contacts allocated per world.\n
Intended for managing data allocations and setting thread sizes in kernels.
"""
model_max_contacts: wp.array | None = None
"""
The number of contacts pre-allocated across all worlds in the model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
model_active_contacts: wp.array | None = None
"""
The number of active contacts detected across all worlds in the model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
world_max_contacts: wp.array | None = None
"""
The maximum number of contacts pre-allocated for each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
world_active_contacts: wp.array | None = None
"""
The number of active contacts detected in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
wid: wp.array | None = None
"""
The world index of each active contact.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`int32`.
"""
cid: wp.array | None = None
"""
The contact index of each active contact w.r.t its world.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`int32`.
"""
gid_AB: wp.array | None = None
"""
The geometry indices of the geometry-pair AB associated with each active contact.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec2i`.
"""
bid_AB: wp.array | None = None
"""
The body indices of the body-pair AB associated with each active contact.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec2i`.
"""
position_A: wp.array | None = None
"""
The position of each active contact on the associated body-A in world coordinates.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec3f`.
"""
position_B: wp.array | None = None
"""
The position of each active contact on the associated body-B in world coordinates.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec3f`.
"""
gapfunc: wp.array | None = None
"""
Gap-function of each active contact, format ``(xyz: normal, w: signed_distance)``.\n
The ``w`` component stores the signed distance between margin-shifted surfaces:
negative means penetration past the resting separation, positive means separation
within the detection gap.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec4f`.
"""
frame: wp.array | None = None
"""
The coordinate frame of each active contact as a rotation quaternion w.r.t the world.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`quatf`.
"""
material: wp.array | None = None
"""
The material properties of each active contact with format `(0: friction, 1: restitution)`.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec2f`.
"""
key: wp.array | None = None
"""
Integer key uniquely identifying each active contact.\n
The per-contact key assignment is implementation-dependent, but is typically
computed from the A/B geom-pair index as well as additional information such as:
- the triangle index
- shape-specific topological data
- contact index w.r.t the geom-pair\n
Shape of ``(model_max_contacts_host,)`` and type :class:`uint64`.
"""
reaction: wp.array | None = None
"""
The 3D contact reaction (force/impulse) expressed in the respective local contact frame.\n
This is to be set by solvers at each step, and also facilitates contact visualization and warm-starting.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec3f`.
"""
velocity: wp.array | None = None
"""
The 3D contact velocity expressed in the respective local contact frame.\n
This is to be set by solvers at each step, and also facilitates contact visualization and warm-starting.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec3f`.
"""
mode: wp.array | None = None
"""
The discrete contact mode expressed as an integer value.\n
The possible values correspond to those of the :class:`ContactMode`.\n
This is to be set by solvers at each step, and also facilitates contact visualization and warm-starting.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`int32`.
"""
def clear(self):
"""
Clears the count of active contacts.
"""
self.model_active_contacts.zero_()
self.world_active_contacts.zero_()
def reset(self):
"""
Clears the count of active contacts and resets contact data
to sentinel values, indicating an empty set of contacts.
"""
self.clear()
self.wid.fill_(-1)
self.cid.fill_(-1)
self.gid_AB.fill_(vec2i(-1, -1))
self.bid_AB.fill_(vec2i(-1, -1))
self.mode.fill_(ContactMode.INACTIVE)
self.reaction.zero_()
self.velocity.zero_()
###
# Functions
###
@wp.func
def make_contact_frame_znorm(n: vec3f) -> mat33f:
n = wp.normalize(n)
if wp.abs(wp.dot(n, UNIT_X)) < COS_PI_6:
e = UNIT_X
else:
e = UNIT_Y
o = wp.normalize(wp.cross(n, e))
t = wp.normalize(wp.cross(o, n))
return mat33f(t.x, o.x, n.x, t.y, o.y, n.y, t.z, o.z, n.z)
@wp.func
def make_contact_frame_xnorm(n: vec3f) -> mat33f:
n = wp.normalize(n)
if wp.abs(wp.dot(n, UNIT_X)) < COS_PI_6:
e = UNIT_X
else:
e = UNIT_Y
o = wp.normalize(wp.cross(n, e))
t = wp.normalize(wp.cross(o, n))
return mat33f(n.x, t.x, o.x, n.y, t.y, o.y, n.z, t.z, o.z)
###
# Interfaces
###
class ContactsKamino:
"""
Provides a high-level interface to manage contact data,
including allocations, access, and common operations.
This container provides the primary output of collision detectors
as well as a cache of contact data to warm-start physics solvers.
"""
def __init__(
self,
capacity: int | list[int] | None = None,
default_max_contacts: int | None = None,
device: wp.DeviceLike = None,
):
# Declare and initialize the default maximum number of contacts per world
self._default_max_world_contacts: int = DEFAULT_WORLD_MAX_CONTACTS
if default_max_contacts is not None:
self._default_max_world_contacts = default_max_contacts
# Cache the target device for all memory allocations
self._device: wp.DeviceLike = None
# Declare the contacts data container and initialize it to empty
self._data: ContactsKaminoData = ContactsKaminoData()
# If a capacity is specified, finalize the contacts data allocation
if capacity is not None:
self.finalize(capacity=capacity, device=device)
###
# Properties
###
@property
def default_max_world_contacts(self) -> int:
"""
Returns the default maximum number of contacts per world.\n
This value is used when the capacity at allocation-time is unspecified or equals 0.
"""
return self._default_max_world_contacts
@default_max_world_contacts.setter
def default_max_world_contacts(self, max_contacts: int):
"""
Sets the default maximum number of contacts per world.
Args:
max_contacts (int): The maximum number of contacts per world.
"""
if max_contacts < 0:
raise ValueError("max_contacts must be a non-negative integer")
self._default_max_world_contacts = max_contacts
@property
def device(self) -> wp.DeviceLike:
"""
Returns the device on which the contacts data is allocated.
"""
return self._device
@property
def data(self) -> ContactsKaminoData:
"""
Returns the managed contacts data container.
"""
self._assert_has_data()
return self._data
@property
def model_max_contacts_host(self) -> int:
"""
Returns the host-side cache of the maximum number of contacts allocated across all worlds.\n
Intended for managing data allocations and setting thread sizes in kernels.
"""
self._assert_has_data()
return self._data.model_max_contacts_host
@property
def world_max_contacts_host(self) -> list[int]:
"""
Returns the host-side cache of the maximum number of contacts allocated per world.\n
Intended for managing data allocations and setting thread sizes in kernels.
"""
self._assert_has_data()
return self._data.world_max_contacts_host
@property
def model_max_contacts(self) -> wp.array:
"""
Returns the number of active contacts per model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.model_max_contacts
@property
def model_active_contacts(self) -> wp.array:
"""
Returns the number of active contacts detected across all worlds in the model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.model_active_contacts
@property
def world_max_contacts(self) -> wp.array:
"""
Returns the maximum number of contacts pre-allocated for each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.world_max_contacts
@property
def world_active_contacts(self) -> wp.array:
"""
Returns the number of active contacts detected in each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.world_active_contacts
@property
def wid(self) -> wp.array:
"""
Returns the world index of each active contact.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.wid
@property
def cid(self) -> wp.array:
"""
Returns the contact index of each active contact w.r.t its world.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.cid
@property
def gid_AB(self) -> wp.array:
"""
Returns the geometry indices of the geometry-pair AB associated with each active contact.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec2i`.
"""
self._assert_has_data()
return self._data.gid_AB
@property
def bid_AB(self) -> wp.array:
"""
Returns the body indices of the body-pair AB associated with each active contact.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec2i`.
"""
self._assert_has_data()
return self._data.bid_AB
@property
def position_A(self) -> wp.array:
"""
Returns the position of each active contact on the associated body-A in world coordinates.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec3f`.
"""
self._assert_has_data()
return self._data.position_A
@property
def position_B(self) -> wp.array:
"""
Returns the position of each active contact on the associated body-B in world coordinates.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec3f`.
"""
self._assert_has_data()
return self._data.position_B
@property
def gapfunc(self) -> wp.array:
"""
Returns the gap-function (i.e. signed-distance) of each
active contact with format `(xyz: normal, w: penetration)`.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec4f`.
"""
self._assert_has_data()
return self._data.gapfunc
@property
def frame(self) -> wp.array:
"""
Returns the coordinate frame of each active contact as a rotation quaternion w.r.t the world.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`quatf`.
"""
self._assert_has_data()
return self._data.frame
@property
def material(self) -> wp.array:
"""
Returns the material properties of each active contact with format `(0: friction, 1: restitution)`.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec2f`.
"""
self._assert_has_data()
return self._data.material
@property
def key(self) -> wp.array:
"""
Returns the integer key uniquely identifying each active contact.\n
The per-contact key assignment is implementation-dependent, but is typically
computed from the A/B geom-pair index as well as additional information such as:
- the triangle index
- shape-specific topological data
- contact index w.r.t the geom-pair\n
Shape of ``(model_max_contacts_host,)`` and type :class:`uint64`.
"""
self._assert_has_data()
return self._data.key
@property
def reaction(self) -> wp.array:
"""
Returns the 3D contact reaction (force/impulse) expressed in the respective local contact frame.\n
This is to be set by solvers at each step, and also facilitates contact visualization and warm-starting.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec3f`.
"""
self._assert_has_data()
return self._data.reaction
@property
def velocity(self) -> wp.array:
"""
Returns the 3D contact velocity expressed in the respective local contact frame.\n
This is to be set by solvers at each step, and also facilitates contact visualization and warm-starting.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`vec3f`.
"""
self._assert_has_data()
return self._data.velocity
@property
def mode(self) -> wp.array:
"""
Returns the discrete contact mode expressed as an integer value.\n
The possible values correspond to those of the :class:`ContactMode`.\n
This is to be set by solvers at each step, and also facilitates contact visualization and warm-starting.\n
Shape of ``(model_max_contacts_host,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.mode
###
# Operations
###
def finalize(self, capacity: int | list[int], device: wp.DeviceLike = None):
"""
Finalizes the contacts data allocations based on the specified capacity.
Args:
capacity (int | list[int]):
The maximum number of contacts to allocate.\n
If an integer is provided, it specifies the capacity for a single world.\n
If a list of integers is provided, it specifies the capacity for each world.
device (wp.DeviceLike, optional):
The device on which to allocate the contacts data.
"""
# The memory allocation requires the total number of contacts (over multiple worlds)
# as well as the contacts capacities for each world. Corresponding sizes are defaulted to 0 (empty).
model_max_contacts = 0
world_max_contacts = [0]
# If the capacity is a list, this means we are allocating for multiple worlds
if isinstance(capacity, list):
if len(capacity) == 0:
raise ValueError("`capacity` must be an non-empty list")
for i in range(len(capacity)):
if capacity[i] < 0:
raise ValueError(f"`capacity[{i}]` must be a non-negative integer")
if capacity[i] == 0:
capacity[i] = self._default_max_world_contacts
model_max_contacts = sum(capacity)
world_max_contacts = capacity
# If the capacity is a single integer, this means we are allocating for a single world
elif isinstance(capacity, int):
if capacity < 0:
raise ValueError("`capacity` must be a non-negative integer")
if capacity == 0:
capacity = self._default_max_world_contacts
model_max_contacts = capacity
world_max_contacts = [capacity]
else:
raise TypeError("`capacity` must be an integer or a list of integers")
# Skip allocation if there are no contacts to allocate
if model_max_contacts == 0:
msg.debug("ContactsKamino: Skipping contact data allocations since total requested capacity was `0`.")
return
# Override the device if specified
if device is not None:
self._device = device
# Allocate the contacts data on the specified device
with wp.ScopedDevice(self._device):
self._data = ContactsKaminoData(
model_max_contacts_host=model_max_contacts,
world_max_contacts_host=world_max_contacts,
model_max_contacts=wp.array([model_max_contacts], dtype=int32),
model_active_contacts=wp.zeros(shape=1, dtype=int32),
world_max_contacts=wp.array(world_max_contacts, dtype=int32),
world_active_contacts=wp.zeros(shape=len(world_max_contacts), dtype=int32),
wid=wp.full(value=-1, shape=(model_max_contacts,), dtype=int32),
cid=wp.full(value=-1, shape=(model_max_contacts,), dtype=int32),
gid_AB=wp.full(value=vec2i(-1, -1), shape=(model_max_contacts,), dtype=vec2i),
bid_AB=wp.full(value=vec2i(-1, -1), shape=(model_max_contacts,), dtype=vec2i),
position_A=wp.zeros(shape=(model_max_contacts,), dtype=vec3f),
position_B=wp.zeros(shape=(model_max_contacts,), dtype=vec3f),
gapfunc=wp.zeros(shape=(model_max_contacts,), dtype=vec4f),
frame=wp.zeros(shape=(model_max_contacts,), dtype=quatf),
material=wp.zeros(shape=(model_max_contacts,), dtype=vec2f),
key=wp.zeros(shape=(model_max_contacts,), dtype=wp.uint64),
reaction=wp.zeros(shape=(model_max_contacts,), dtype=vec3f),
velocity=wp.zeros(shape=(model_max_contacts,), dtype=vec3f),
mode=wp.full(value=ContactMode.INACTIVE, shape=(model_max_contacts,), dtype=int32),
)
def clear(self):
"""
Clears the count of active contacts.
"""
self._assert_has_data()
if self._data.model_max_contacts_host > 0:
self._data.clear()
def reset(self):
"""
Clears the count of active contacts and resets data to sentinel values.
"""
self._assert_has_data()
if self._data.model_max_contacts_host > 0:
self._data.reset()
###
# Internals
###
def _assert_has_data(self):
if self._data.model_max_contacts_host == 0:
raise RuntimeError("ContactsKaminoData has not been allocated. Call `finalize()` before accessing data.")
###
# Conversions - Kernels
###
@wp.kernel
def _convert_contacts_newton_to_kamino(
kamino_num_worlds: int32,
kamino_max_contacts: int32,
# Newton contact inputs
newton_contact_count: wp.array(dtype=int32),
newton_shape0: wp.array(dtype=int32),
newton_shape1: wp.array(dtype=int32),
newton_point0: wp.array(dtype=vec3f),
newton_point1: wp.array(dtype=vec3f),
newton_normal: wp.array(dtype=vec3f),
newton_thickness0: wp.array(dtype=float32),
newton_thickness1: wp.array(dtype=float32),
# Model lookups
shape_body: wp.array(dtype=int32),
shape_world: wp.array(dtype=int32),
shape_mu: wp.array(dtype=float32),
shape_restitution: wp.array(dtype=float32),
body_q: wp.array(dtype=wp.transformf),
kamino_world_max_contacts: wp.array(dtype=int32),
# Kamino contact outputs
kamino_model_active: wp.array(dtype=int32),
kamino_world_active: wp.array(dtype=int32),
kamino_wid: wp.array(dtype=int32),
kamino_cid: wp.array(dtype=int32),
kamino_gid_AB: wp.array(dtype=vec2i),
kamino_bid_AB: wp.array(dtype=vec2i),
kamino_position_A: wp.array(dtype=vec3f),
kamino_position_B: wp.array(dtype=vec3f),
kamino_gapfunc: wp.array(dtype=vec4f),
kamino_frame: wp.array(dtype=quatf),
kamino_material: wp.array(dtype=vec2f),
kamino_key: wp.array(dtype=wp.uint64),
):
"""
Convert Newton Contacts to Kamino's ContactsKamino format.
Reads body-local contact points from Newton, transforms them to world-space,
and populates the Kamino contact arrays with the A/B convention that Kamino's
solver core expects (bid_B >= 0, normal points A -> B).
Newton's ``rigid_contact_normal`` points from shape0 toward shape1 (A -> B).
"""
tid = wp.tid()
nc = newton_contact_count[0]
if tid >= nc or tid >= kamino_max_contacts:
return
s0 = newton_shape0[tid]
s1 = newton_shape1[tid]
b0 = shape_body[s0]
b1 = shape_body[s1]
# Determine the world index. Global shapes (shape_world == -1) can
# collide with shapes from any world, so fall back to the other shape.
w0 = shape_world[s0]
w1 = shape_world[s1]
wid = w0
if w0 < 0:
wid = w1
if wid < 0 or wid >= kamino_num_worlds:
return
# Body-local → world-space
X0 = wp.transform_identity()
if b0 >= 0:
X0 = body_q[b0]
X1 = wp.transform_identity()
if b1 >= 0:
X1 = body_q[b1]
p0_world = wp.transform_point(X0, newton_point0[tid])
p1_world = wp.transform_point(X1, newton_point1[tid])
# Newton normal points from shape0 → shape1 (A → B).
# Kamino convention: normal points A → B, with bid_B >= 0.
n_newton = newton_normal[tid]
# Reconstruct Newton signed contact distance d from exported fields:
# d = dot((p1 - p0), n_a_to_b) - (offset0 + offset1),
# with n_newton = n_a_to_b and offset* stored in rigid_contact_thickness*.
d_newton = wp.dot(p1_world - p0_world, n_newton) - (newton_thickness0[tid] + newton_thickness1[tid])
if b1 < 0:
# shape1 is world-static → make it Kamino A, shape0 becomes Kamino B.
# Kamino A→B = shape1→shape0, opposite of Newton's shape0→shape1, so negate.
gid_A = s1
gid_B = s0
bid_A = b1
bid_B = b0
pos_A = p1_world
pos_B = p0_world
normal = vec3f(-n_newton[0], -n_newton[1], -n_newton[2])
else:
# Both dynamic or shape0 is static → keep A=shape0, B=shape1.
# Newton normal already points A→B, matching Kamino convention.
gid_A = s0
gid_B = s1
bid_A = b0
bid_B = b1
pos_A = p0_world
pos_B = p1_world
normal = vec3f(n_newton[0], n_newton[1], n_newton[2])
distance = d_newton
if distance > 0.0:
return
gapfunc = vec4f(normal[0], normal[1], normal[2], float32(distance))
q_frame = wp.quat_from_matrix(make_contact_frame_znorm(normal))
mu = float32(0.5) * (shape_mu[s0] + shape_mu[s1])
rest = float32(0.5) * (shape_restitution[s0] + shape_restitution[s1])
mcid = wp.atomic_add(kamino_model_active, 0, 1)
wcid = wp.atomic_add(kamino_world_active, wid, 1)
world_max = kamino_world_max_contacts[wid]
if mcid < kamino_max_contacts and wcid < world_max:
kamino_wid[mcid] = wid
kamino_cid[mcid] = wcid
kamino_gid_AB[mcid] = vec2i(gid_A, gid_B)
kamino_bid_AB[mcid] = vec2i(bid_A, bid_B)
kamino_position_A[mcid] = pos_A
kamino_position_B[mcid] = pos_B
kamino_gapfunc[mcid] = gapfunc
kamino_frame[mcid] = q_frame
kamino_material[mcid] = vec2f(mu, rest)
kamino_key[mcid] = build_pair_key2(uint32(gid_A), uint32(gid_B))
else:
wp.atomic_sub(kamino_model_active, 0, 1)
wp.atomic_sub(kamino_world_active, wid, 1)
@wp.kernel
def _convert_contacts_kamino_to_newton(
max_output: int32,
model_active_contacts: wp.array(dtype=int32),
kamino_gid_AB: wp.array(dtype=vec2i),
kamino_position_A: wp.array(dtype=vec3f),
kamino_position_B: wp.array(dtype=vec3f),
kamino_gapfunc: wp.array(dtype=vec4f),
shape_body: wp.array(dtype=int32),
body_q: wp.array(dtype=wp.transformf),
# outputs
rigid_contact_count: wp.array(dtype=int32),
rigid_contact_shape0: wp.array(dtype=int32),
rigid_contact_shape1: wp.array(dtype=int32),
rigid_contact_point0: wp.array(dtype=vec3f),
rigid_contact_point1: wp.array(dtype=vec3f),
rigid_contact_normal: wp.array(dtype=vec3f),
):
"""Converts Kamino's internal contact representation to Newton's Contacts format."""
# Retrieve the contact index for this thread
cid = wp.tid()
# Determine the total number of contacts to convert, which is the smaller
# of the number of active contacts and the maximum output capacity.
ncmax = wp.min(model_active_contacts[0], max_output)
# The first thread stores the model-wide number of active contacts
if cid == 0:
rigid_contact_count[0] = ncmax
# Skip conversion if this contact index exceeds the
# number of active contacts or the output capacity
if cid >= ncmax:
return
# Retrieve contact-specific data
gid_AB = kamino_gid_AB[cid]
position_A = kamino_position_A[cid]
position_B = kamino_position_B[cid]
gapfunc = kamino_gapfunc[cid]
# Retrieve the geometry indices for this contact and use
# them to look up the corresponding shapes and bodies.
shape0 = gid_AB[0]
shape1 = gid_AB[1]
body_a = shape_body[shape0]
body_b = shape_body[shape1]
# Transform the world-space contact positions
# back to body-local coordinates for Newton.
X_inv_a = wp.transform_identity()
if body_a >= 0:
X_inv_a = wp.transform_inverse(body_q[body_a])
X_inv_b = wp.transform_identity()
if body_b >= 0:
X_inv_b = wp.transform_inverse(body_q[body_b])
# Store the converted contact data in the Newton format
rigid_contact_shape0[cid] = shape0
rigid_contact_shape1[cid] = shape1
rigid_contact_normal[cid] = vec3f(gapfunc[0], gapfunc[1], gapfunc[2])
rigid_contact_point0[cid] = wp.transform_point(X_inv_a, position_A)
rigid_contact_point1[cid] = wp.transform_point(X_inv_b, position_B)
###
# Conversions - Launchers
###
def convert_contacts_newton_to_kamino(
model: Model,
state: State,
contacts_in: Contacts,
contacts_out: ContactsKamino,
):
"""
Converts Newton's :class:`Contacts` to Kamino's :class:`ContactsKamino` format.
Transforms body-local contact points to world-space, applies the A/B
convention expected by Kamino (bid_B >= 0, normal A -> B), and populates
all required ContactsKamino fields.
Args:
model (Model):
The :class:`newton.Model` object providing shape and body information
used to interpret Newton's contact data and populate Kamino's contact data.
state (State):
The :class:`newton.State` object providing ``body_q``
used transform contact points to world coordinates.
contacts_in (Contacts):
The :class:`newton.Contacts` object containing contact information to be converted.
contacts_out (ContactsKamino):
The :class:`ContactsKamino` object to populate with the converted contact data.
"""
# Skip conversion if there are no contacts to convert or no capacity to store them.
if contacts_out.model_max_contacts_host == 0 or contacts_in.rigid_contact_max == 0:
return
# First clear the output contacts to reset the active contact
# counts and optionally reset contact data to sentinel values.
contacts_out.clear()
# Launch the conversion kernel to convert Newton contacts to Kamino's format
# NOTE: To reduce overhead, the total thread count is set to the smallest of
# the number of contacts detected and the maximum capacity of the output contacts.
wp.launch(
_convert_contacts_newton_to_kamino,
dim=min(contacts_in.rigid_contact_max, contacts_out.model_max_contacts_host),
inputs=[
int32(model.world_count),
int32(contacts_out.model_max_contacts_host),
contacts_in.rigid_contact_count,
contacts_in.rigid_contact_shape0,
contacts_in.rigid_contact_shape1,
contacts_in.rigid_contact_point0,
contacts_in.rigid_contact_point1,
contacts_in.rigid_contact_normal,
contacts_in.rigid_contact_margin0,
contacts_in.rigid_contact_margin1,
model.shape_body,
model.shape_world,
model.shape_material_mu,
model.shape_material_restitution,
state.body_q,
contacts_out.world_max_contacts,
],
outputs=[
contacts_out.model_active_contacts,
contacts_out.world_active_contacts,
contacts_out.wid,
contacts_out.cid,
contacts_out.gid_AB,
contacts_out.bid_AB,
contacts_out.position_A,
contacts_out.position_B,
contacts_out.gapfunc,
contacts_out.frame,
contacts_out.material,
contacts_out.key,
],
device=model.device,
)
def convert_contacts_kamino_to_newton(
model: Model,
state: State,
contacts_in: ContactsKamino,
contacts_out: Contacts,
) -> None:
"""
Converts Kamino :class:`ContactsKamino` to Newton's :class:`Contacts` format.
Args:
model (Model):
The :class:`newton.Model` object providing shape and body information
used to interpret Kamino's contact data and populate Newton's contact data.
state (State):
The :class:`newton.State` object providing ``body_q``
used to transform contact points to world coordinates.
contacts_in (ContactsKamino):
The :class:`ContactsKamino` object containing contact information to be converted.
contacts_out (Contacts):
The :class:`newton.Contacts` object to populate with the converted contact data.
"""
# Skip conversion if there are no contacts to convert or no capacity to store them.
if contacts_in.data.model_max_contacts_host == 0 or contacts_out.rigid_contact_max == 0:
return
# Issue warning to the user if the number of contacts to convert exceeds the capacity of the output contacts.
if contacts_in.data.model_max_contacts_host > contacts_out.rigid_contact_max:
msg.warning(
"Kamino `model_max_contacts` (%d) exceeds Newton `rigid_contact_max` (%d); contacts will be truncated.",
contacts_in.data.model_max_contacts_host,
contacts_out.rigid_contact_max,
)
# Launch the conversion kernel to convert Kamino contacts to Newton's format.
# NOTE: To reduce overhead, the total thread count is set to the smallest of the
# number of contacts detected and the maximum capacity of the output contacts.
wp.launch(
_convert_contacts_kamino_to_newton,
dim=min(contacts_in.data.model_max_contacts_host, contacts_out.rigid_contact_max),
inputs=[
int32(contacts_out.rigid_contact_max),
contacts_in.data.model_active_contacts,
contacts_in.data.gid_AB,
contacts_in.data.position_A,
contacts_in.data.position_B,
contacts_in.data.gapfunc,
model.shape_body,
state.body_q,
],
outputs=[
contacts_out.rigid_contact_count,
contacts_out.rigid_contact_shape0,
contacts_out.rigid_contact_shape1,
contacts_out.rigid_contact_point0,
contacts_out.rigid_contact_point1,
contacts_out.rigid_contact_normal,
],
device=model.device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/detector.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides a unified interface for performing Collision Detection in Kamino.
Usage example:
# Create a model builder
builder = ModelBuilderKamino()
# ... add bodies and collision geometries to the builder ...
# Finalize the model
model = builder.finalize(device="cuda:0")
# Create a collision detector with desired config
config = CollisionDetector.Config(
pipeline="unified",
broadphase="explicit",
bvtype="aabb",
)
# Create the collision detector
detector = CollisionDetector(model=model, config=config)
"""
from __future__ import annotations
from enum import IntEnum
import warp as wp
from .....core.types import override
from ...config import CollisionDetectorConfig
from ..core.data import DataKamino
from ..core.model import ModelKamino
from ..core.state import StateKamino
from ..geometry.contacts import ContactsKamino
from ..geometry.primitive import CollisionPipelinePrimitive
from ..geometry.unified import CollisionPipelineUnifiedKamino
###
# Module interface
###
__all__ = [
"BroadPhaseType",
"CollisionDetector",
"CollisionPipelineType",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
class CollisionPipelineType(IntEnum):
"""Defines the collision detection pipelines supported in Kamino."""
PRIMITIVE = 0
"""
Use the "fast" collision detection pipeline specialized for geometric
primitives using an "explicit" broad-phase on pre-computed collision
shape pairs and a narrow-phase using Newton's primitive colliders.
"""
UNIFIED = 1
"""
Use Newton's unified collision-detection pipeline using a configurable
broad-phase that supports `NXN`, `SAP`, or `EXPLICIT` modes, and a
unified GJK/MPR-based narrow-phase. This pipeline is more general and
supports arbitrary collision geometries, including meshes and SDFs.
"""
@classmethod
def from_string(cls, s: str) -> CollisionPipelineType:
"""Converts a string to a CollisionPipelineType enum value."""
try:
return cls[s.upper()]
except KeyError as e:
raise ValueError(f"Invalid CollisionPipelineType: {s}. Valid options are: {[e.name for e in cls]}") from e
@override
def __str__(self):
"""Returns a string representation of the collision detector mode."""
return f"CollisionDetectorMode.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the collision detector mode."""
return self.__str__()
class BroadPhaseType(IntEnum):
"""Defines the broad-phase collision detection modes supported in Kamino."""
NXN = 0
"""
Use an `NXN` broad-phase that considers all possible pairs of collision shapes as candidates.
This mode is simple but can be inefficient for models with many collision shapes.
"""
SAP = 1
"""
Use a Sweep and Prune (SAP) broad-phase that sorts collision shapes along a chosen axis
and only considers overlapping shapes as candidates for narrow-phase collision detection.
This mode is more efficient than `NXN` for models with many collision
shapes, especially when they are sparsely distributed in space.
"""
EXPLICIT = 2
"""
Use an explicit broad-phase that relies on pre-computed candidate pairs
of collision shapes, typically generated by the ModelBuilderKamino based
on heuristics such as proximity and connectivity.
This mode can be the most efficient when the candidate pairs are
well-chosen, but it requires additional setup during model building.
"""
@classmethod
def from_string(cls, s: str) -> BroadPhaseType:
"""Converts a string to a BroadPhaseType enum value."""
try:
return cls[s.upper()]
except KeyError as e:
raise ValueError(f"Invalid BroadPhaseType: {s}. Valid options are: {[e.name for e in cls]}") from e
@override
def __str__(self):
"""Returns a string representation of the broad-phase type."""
return f"BroadPhaseType.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the broad-phase type."""
return self.__str__()
###
# Interfaces
###
class CollisionDetector:
"""
Provides a Collision Detection (CD) front-end for Kamino.
This class is responsible for performing collision detection as well
as managing the collision containers and their memory allocations.
Supports two collision pipeline types:
- `PRIMITIVE`: A fast collision pipeline with specialized for geometric
primitives using an "explicit" broad-phase on pre-computed collision
shape pairs and a narrow-phase using Newton's primitive colliders.
- `UNIFIED`: Newton's unified collision-detection pipeline using a configurable
broad-phase that supports `NXN`, `SAP`, or `EXPLICIT` modes, and a unified
GJK/MPR-based narrow-phase. This pipeline is more general and supports arbitrary
collision geometries, including meshes and SDFs.
"""
Config = CollisionDetectorConfig
"""
The configuration dataclass for the CollisionDetector, which includes parameters
for selecting the collision pipeline type, broad-phase mode, bounding volume type,
contact generation parameters, and other settings related to collision detection.
See :class:`CollisionDetectorConfig` for the full
list of configuration options and their descriptions.
"""
def __init__(
self,
model: ModelKamino | None = None,
config: CollisionDetector.Config | None = None,
device: wp.DeviceLike = None,
):
"""
Initialize the CollisionDetector.
Args:
model (`ModelKamino`, optional):
The model container holding the time-invariant data of the system being simulated.\n
If provided, the detector will be finalized using the provided model and config.\n
If `None`, the detector will be created empty without allocating data, and
can be finalized later by providing a model to the `finalize` method.\n
device (`wp.DeviceLike`, optional):
The target Warp device for allocation and execution.\n
If `None`, the `model.device` will be used if a model is provided, otherwise
it will default to the device preferred by Warp on the given platform.
"""
# Cache the target device
self._device: wp.DeviceLike = device
# Cache a reference to the target model
self._model: ModelKamino | None = model
# Cache the collision detector config
self._config: CollisionDetector.Config | None = config
# Declare the contacts container
self._contacts: ContactsKamino | None = None
# Declare the collision detection pipelines
self._pipeline_type: CollisionPipelineType | None = None
self._unified_pipeline: CollisionPipelineUnifiedKamino | None = None
self._primitive_pipeline: CollisionPipelinePrimitive | None = None
# Declare and initialize the caches of contacts allocation sizes
self._model_max_contacts: int = 0
self._world_max_contacts: list[int] = [0]
# Finalize the collision detector if a model is provided
if model is not None:
self.finalize(model=model, config=config, device=device)
###
# Properties
###
@property
def device(self) -> wp.DeviceLike:
"""Returns the device on which the CollisionDetector data is allocated and executes."""
return self._device
@property
def model(self) -> ModelKamino | None:
"""Returns the model associated with the CollisionDetector."""
return self._model
@property
def config(self) -> CollisionDetector.Config | None:
"""Returns the config used to configure the CollisionDetector."""
return self._config
@property
def contacts(self) -> ContactsKamino | None:
"""Returns the ContactsKamino container managed by the CollisionDetector."""
return self._contacts
@property
def model_max_contacts(self) -> int:
"""Returns the total maximum number of contacts allocated for the model across all worlds."""
return self._model_max_contacts
@property
def world_max_contacts(self) -> list[int]:
"""Returns the maximum number of contacts allocated for each world."""
return self._world_max_contacts
###
# Operations
###
def finalize(
self,
model: ModelKamino | None = None,
config: CollisionDetector.Config | None = None,
device: wp.DeviceLike = None,
):
"""
Allocates CollisionDetector data on the target device.
Args:
model (ModelKamino, optional):
The model container holding the time-invariant data of the system being simulated.\n
If provided, the detector will be finalized using the provided model and config.\n
If `None`, the detector will be created empty without allocating data, and
can be finalized later by providing a model to the `finalize` method.\n
config (CollisionDetector.Config, optional):
Config for the CollisionDetector.\n
If `None`, uses default config.
device (wp.DeviceLike, optional):
The target Warp device for allocation and execution.\n
If `None`, the `model.device` will be used if a model is provided, otherwise
it will default to the device preferred by Warp on the given platform.
"""
# Override the model if specified explicitly
if model is not None:
self._model = model
# Check that the model is valid
if self._model is None:
raise ValueError("Cannot finalize CollisionDetector: model is `None`")
elif not isinstance(self._model, ModelKamino):
raise TypeError(f"Cannot finalize CollisionDetector: expected ModelKamino, got {type(self._model)}")
# Override the device if specified explicitly
if device is not None:
self._device = device
# Otherwise, use the device of the model
else:
self._device = self._model.device
# Override the config if specified, ensuring that they are valid
if config is not None:
if not isinstance(config, CollisionDetector.Config):
raise TypeError(
f"Cannot finalize CollisionDetector: expected CollisionDetector.Config, got {type(config)}"
)
self._config = config
# If no config is provided, use the defaults
if self._config is None:
self._config = CollisionDetector.Config()
# Configure the collision detection pipeline type based on the config
self._pipeline_type = CollisionPipelineType.from_string(self._config.pipeline)
# TODO: FIX THIS SO THAT PER-WORLD MAX IS ACTUALLY BASED ON THE NUM OF COLLIDABLE
# GOEMS IN EACH WORLD, INSTEAD OF JUST DIVIDING THE MODEL MAX BY THE NUM WORLDS
# For collision pipeline, we don't multiply by per-pair factors since broad phase
# discovers pairs dynamically. Users can provide rigid_contact_max explicitly,
# otherwise it is estimated from shape count and broad phase mode.
if self._model.geoms.model_minimum_contacts > 0:
self._model_max_contacts = self._model.geoms.model_minimum_contacts
self._world_max_contacts = self._model.geoms.world_minimum_contacts
else:
# Estimate based on broad phase mode and available information
if self._config.broadphase == "explicit" and self._model.geoms.collidable_pairs is not None:
# For EXPLICIT mode, we know the maximum possible pairs
# Estimate ~10 contacts per shape pair (conservative for mesh-mesh contacts)
self._model_max_contacts = max(self._config.max_contacts, self._model.geoms.num_collidable_pairs * 10)
else:
# For NXN/SAP dynamic broad phase, estimate based on shape count
# Assume each shape contacts ~20 others on average (conservative estimate)
# This scales much better than O(N²) while still being safe
self._model_max_contacts = max(self._config.max_contacts, self._model.geoms.num_collidable * 20)
# Set the world max contacts to be the same for all worlds in the model
num_worlds = self._model.size.num_worlds
self._world_max_contacts = [self._model_max_contacts // num_worlds] * num_worlds
# Override per-world max contacts if config specifies it.
if self._config.max_contacts_per_world is not None:
num_worlds = self._model.size.num_worlds
per_world = self._config.max_contacts_per_world
self._world_max_contacts = [per_world] * num_worlds
self._model_max_contacts = per_world * num_worlds
# Create the contacts interface which will allocate all contacts data arrays
# NOTE: If internal allocations happen, then they will contain
# the contacts generated by the collision detection pipelines
self._contacts = ContactsKamino(capacity=self._world_max_contacts, device=self._device)
# Proceed with allocations only if the model admits contacts, which
# occurs when collision geometries defined in the builder and model
if self._model_max_contacts > 0:
# Initialize the configured collision detection pipeline
match self._pipeline_type:
case CollisionPipelineType.PRIMITIVE:
self._primitive_pipeline = CollisionPipelinePrimitive(
device=self._device,
model=self._model,
bvtype=self._config.bvtype,
default_gap=self._config.default_gap,
)
case CollisionPipelineType.UNIFIED:
self._unified_pipeline = CollisionPipelineUnifiedKamino(
device=self._device,
model=self._model,
broadphase=self._config.broadphase,
default_gap=self._config.default_gap,
max_triangle_pairs=self._config.max_triangle_pairs,
max_contacts_per_pair=self._config.max_contacts_per_pair,
)
case _:
raise ValueError(f"Unsupported CollisionPipelineType: {self._pipeline_type}")
def collide(self, data: DataKamino, state: StateKamino, contacts: ContactsKamino | None = None):
"""
Executes collision detection given a model and its associated data.
This operation will use the `primitive` or `unified` pipeline depending on
the configuration set during the initialization of the CollisionDetector.
Args:
data (DataKamino):
The solver data container holding solver-specific internal geome/shape data.
state (StateKamino):
The state container holding the time-varying state of simulation.
contacts (ContactsKamino, optional):
An optional ContactsKamino container to store the generated contacts.
If `None`, uses the internal ContactsKamino container managed by the CollisionDetector.
"""
# If no contacts can be generated, skip collision detection
if contacts is not None:
_contacts = contacts
else:
_contacts = self._contacts
# Skip this operation if no contacts data has been allocated
if _contacts is None or _contacts.model_max_contacts_host <= 0:
return
# Ensure that a collision detection pipeline has been created
if self._primitive_pipeline is None and self._unified_pipeline is None:
raise RuntimeError("Cannot perform collision detection: a collision pipeline has not been created")
# Ensure that the data is valid
if data is None:
raise ValueError("Cannot perform collision detection: data is None")
if not isinstance(data, DataKamino):
raise TypeError(f"Cannot perform collision detection: expected DataKamino, got {type(data)}")
# Ensure that the state is valid
if state is None:
raise ValueError("Cannot perform collision detection: state is None")
if not isinstance(state, StateKamino):
raise TypeError(f"Cannot perform collision detection: expected StateKamino, got {type(state)}")
# Execute the configured collision detection pipeline
match self._pipeline_type:
case CollisionPipelineType.PRIMITIVE:
self._primitive_pipeline.collide(data, state, _contacts)
case CollisionPipelineType.UNIFIED:
self._unified_pipeline.collide(data, state, _contacts)
case _:
raise ValueError(f"Unsupported CollisionPipelineType: {self._pipeline_type}")
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/keying.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides functions for generating and searching for unique keys for pairs and triplets of indices.
TODO: Add more detailed description and documentation.
"""
import warp as wp
from ..core.types import int32, int64, uint32, uint64, vec2i
###
# Module interface
###
__all__ = [
"binary_search_find_pair",
"binary_search_find_range_start",
"build_pair_key2",
"make_build_pair_key3_func",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Functions
###
def make_bitmask(num_bits: int) -> int:
"""
Returns an all-ones mask for the requested number of bits.
Examples:
num_bits=20 -> 0x00000000000FFFFF
num_bits=23 -> 0x00000000007FFFFF
Args:
num_bits (int): Number of bits to set in the mask.
Returns:
int: Bitmask with the specified number of lower bits set to `1`.
"""
# Ensure the number of bits is valid
if num_bits <= 0 or num_bits > 64:
raise ValueError(f"`num_bits` was {num_bits}, but must be positive integer in the range [1, 64].")
# Handle the special case for 64 bits
if num_bits == 64:
return 0xFFFFFFFFFFFFFFFF
# General case
return (1 << num_bits) - 1
@wp.func
def build_pair_key2(index_A: wp.uint32, index_B: wp.uint32) -> wp.uint64:
"""
Build a 63-bit key from two indices with the following layout:
- The highest bit is always `0`, reserved as a sign bit to support conversion to signed int64.
- Upper 31 bits: lower 31 bits of index_A
- Lower 32 bits: all 32 bits of index_B
Args:
index_A (wp.uint32): First index.
index_B (wp.uint32): Second index.
Returns:
wp.uint64: Combined 64-bit key.
"""
key = wp.uint64(index_A & wp.uint32(wp.static(make_bitmask(31))))
key = key << wp.uint64(32)
key = key | wp.uint64(index_B)
return key
def make_build_pair_key3_func(main_key_bits: int, aux_key_bits: int | None = None):
"""
Generates a function that builds a 63-bit key from three indices with the following layout:
- The highest bit is always `0`, reserved as a sign bit to support conversion to signed int64.
- Upper `main_key_bits` bits: lower `main_key_bits` bits of index_A
- Middle `main_key_bits` bits: lower `main_key_bits` bits of index_B
- Lower `aux_key_bits` bits: lower `aux_key_bits` bits of index_C
Note:
- The total number of bits used is `2 * main_key_bits + aux_key_bits`, which must be less than or equal to 63.
Args:
main_key_bits (int): Number of bits to allocate for index_A and index_B.
aux_key_bits (int, optional): Number of bits to allocate for index_C.
If `None`, it will be set to `63 - 2 * main_key_bits`.
Returns:
function: A Warp function that takes three `wp.uint32` indices and returns a combined `wp.uint64` key.
"""
# Ensure the number of bits is valid
if main_key_bits <= 0 or main_key_bits > 32:
raise ValueError(f"`main_key_bits` was {main_key_bits}, but must be positive integer in the range [1, 32].")
if aux_key_bits is None:
aux_key_bits = 63 - 2 * main_key_bits
if aux_key_bits <= 0 or aux_key_bits > 32:
raise ValueError(f"`aux_key_bits` was {aux_key_bits}, but must be positive integer in the range [1, 32].")
if 2 * main_key_bits + aux_key_bits != 63:
raise ValueError(
f"`2 * main_key_bits + aux_key_bits` was {2 * main_key_bits + aux_key_bits}, but must be equal to 63 bits."
)
# Precompute bitmasks for the specified bit widths
MAIN_BITMASK = make_bitmask(main_key_bits)
AUX_BITMASK = make_bitmask(aux_key_bits)
# Define the function
@wp.func
def _build_pair_key3(index_A: uint32, index_B: uint32, index_C: uint32) -> uint64:
key = wp.uint64(index_A & wp.uint32(MAIN_BITMASK))
key = key << wp.uint64(main_key_bits)
key = key | wp.uint64(index_B & wp.uint32(MAIN_BITMASK))
key = key << wp.uint64(aux_key_bits)
key = key | wp.uint64(index_C & wp.uint32(AUX_BITMASK))
return key
# Return the generated function
return _build_pair_key3
@wp.func
def binary_search_find_pair(
num_pairs: int32,
target: vec2i,
pairs: wp.array(dtype=vec2i),
) -> int32:
"""
Performs binary-search over a sorted array of pairs to find the index of a target pair.
Assumes that pairs are sorted in ascending lexicographical
order, i.e. first by the first element, then by the second.
Args:
num_pairs (int32): Number of "active" pairs in the array.\n
This is required because not all elements may be active.
target (vec2i): The target pair to search for.
pairs (wp.array(dtype=vec2i)): Sorted array of pairs to search within.
Returns:
Index of the target pair if found, otherwise `-1`.
"""
lower = int32(0)
upper = num_pairs
while lower < upper:
mid = (lower + upper) >> 1
mid_pair = pairs[mid]
# Compare pairs lexicographically (first by the first element, then by the second)
if mid_pair[0] < target[0] or (mid_pair[0] == target[0] and mid_pair[1] < target[1]):
lower = mid + 1
elif mid_pair[0] > target[0] or (mid_pair[0] == target[0] and mid_pair[1] > target[1]):
upper = mid
else:
# Found exact match
return mid
# Not found
return -1
@wp.func
def binary_search_find_range_start(
lower: int32,
upper: int32,
target: uint64,
keys: wp.array(dtype=uint64),
) -> int32:
"""
Performs binary-search over a sorted array of integer keys
to find the start index of the first occurrence of target.
Assumes that keys are sorted in ascending order.
Args:
lower (wp.int32): Lower bound index for the search (inclusive).
upper (wp.int32): Upper bound index for the search (exclusive).
target (wp.uint64): The target key to search for.
keys (wp.array(dtype=wp.uint64)): Sorted array of keys to search within.
Returns:
Index of the first occurrence of target if found, otherwise `-1`.
"""
# Find lower bound: first position where keys[i] >= target
left = lower
right = upper
while left < right:
mid = left + (right - left) // 2
if keys[mid] < target:
left = mid + 1
else:
right = mid
# Check if the key was actually found
if left >= upper or keys[left] != target:
return -1
# Return the index of the first occurrence of the target key
return left
@wp.func_native("""return 0x7FFFFFFFFFFFFFFFull;""")
def uint64_sentinel_value() -> wp.uint64: ...
###
# Kernels
###
@wp.kernel
def _prepare_key_sort(
# Inputs:
num_active_keys: wp.array(dtype=wp.int32),
keys_source: wp.array(dtype=wp.uint64),
# Outputs:
keys: wp.array(dtype=wp.uint64),
sorted_to_unsorted_map: wp.array(dtype=wp.int32),
):
"""
Prepares keys and sorting-maps for radix sort.
Args:
num_active_keys (wp.array(dtype=wp.int32)): Number of active keys to copy
keys_source (wp.array(dtype=wp.uint64)): Source array of keys
keys (wp.array(dtype=wp.uint64)): Destination array of keys for sorting
sorted_to_unsorted_map (wp.array(dtype=wp.int32)): Map from sorted indices to original unsorted indices
"""
# Retrieve the thread index
tid = wp.tid()
# Copy active keys and initialize the sorted-to-unsorted index map
if tid < num_active_keys[0]:
keys[tid] = keys_source[tid]
sorted_to_unsorted_map[tid] = tid
# Otherwise fill unused slots with the sentinel value
# NOTE: This ensures that these entries sort to the end when treated as signed int64
else:
# keys[tid] = wp.static(make_bitmask(63))
keys[tid] = uint64_sentinel_value()
###
# Launchers
###
def prepare_key_sort(
num_active: wp.array,
unsorted: wp.array,
sorted: wp.array,
sorted_to_unsorted_map: wp.array,
):
"""
Prepares keys and sorting-maps for radix sort.
Args:
num_active (wp.array):
An array containing the number of active keys to be sorted.
unsorted (wp.array):
The source array of keys to be sorted.
sorted (wp.array):
The destination array where sorted keys will be stored.
sorted_to_unsorted_map (wp.array):
An array of index-mappings from sorted to source key indices
"""
wp.launch(
kernel=_prepare_key_sort,
dim=sorted.size,
inputs=[num_active, unsorted, sorted, sorted_to_unsorted_map],
device=sorted.device,
)
###
# Interfaces
###
class KeySorter:
"""
A utility class for sorting integer keys using radix sort.
"""
def __init__(self, max_num_keys: int, device: wp.DeviceLike = None):
"""
Creates a KeySorter instance to sort keys using radix sort.
Args:
max_num_keys (int): Maximum number of keys to sort.
device (wp.DeviceLike, optional): Device to allocate buffers on (None for default).
"""
# Declare and initialize the maximum number of keys
# NOTE: This is used set dimensions of all kernel launches
self._max_num_keys = max_num_keys
# Cache the Warp device on which data will be
# allocated and all kernels will be executed
self._device = device
# Allocate data buffers for key sorting
# NOTE: Allocations are multiplied by a factor of
# 2 as required by the Warp radix sort algorithm
with wp.ScopedDevice(device):
self._sorted_keys = wp.zeros(2 * self._max_num_keys, dtype=uint64)
self._sorted_to_unsorted_map = wp.zeros(2 * self._max_num_keys, dtype=int32)
# Define a view of the sorted keys as int64
# NOTE: This required in order to use Warp's radix_sort_pairs, which only supports signed integers
self._sorted_keys_int64 = wp.array(
ptr=self._sorted_keys.ptr,
shape=self._sorted_keys.shape,
device=self._sorted_keys.device,
dtype=int64,
copy=False,
)
@property
def device(self) -> wp.DeviceLike:
"""Returns the device on which the KeySorter is allocated."""
return self._device
@property
def sorted_keys(self) -> wp.array:
"""Returns the sorted keys array."""
return self._sorted_keys
@property
def sorted_keys_int64(self) -> wp.array:
"""Returns the sorted keys array as an int64 view."""
return self._sorted_keys_int64
@property
def sorted_to_unsorted_map(self) -> wp.array:
"""Returns the sorted-to-unsorted index map array."""
return self._sorted_to_unsorted_map
def sort(self, num_active_keys: wp.array, keys: wp.array):
"""
Sorts the provided keys using radix sort.
Args:
num_active_keys (wp.array(dtype=int32)): Number of active keys to sort.
keys (wp.array(dtype=uint64)): The source keys to be sorted.
"""
# Check compatibility of input sizes
if num_active_keys.device != self._device:
raise ValueError("`num_active_keys` device does not match the KeySorter device.")
if keys.device != self._device:
raise ValueError("`keys` device does not match the KeySorter device.")
if keys.dtype != self._sorted_keys.dtype:
raise ValueError(
f"`keys` dtype ({keys.dtype}) does not match the sorted_keys dtype ({self._sorted_keys.dtype})"
)
if keys.size > 2 * self._max_num_keys:
raise ValueError(
"`keys` size exceeds the supported maximum number of keys."
f"keys must be at most {2 * self._max_num_keys}, but is {keys.size}."
)
# First prepare keys and sorting maps for radix sort
wp.launch(
kernel=_prepare_key_sort,
dim=self._max_num_keys,
inputs=[num_active_keys, keys, self._sorted_keys, self._sorted_to_unsorted_map],
device=self._device,
)
# Perform the radix sort on the key-index pairs
wp.utils.radix_sort_pairs(self._sorted_keys_int64, self._sorted_to_unsorted_map, self._max_num_keys)
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/primitive/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides a collision detection pipeline (i.e. backend) optimized for primitive shapes.
This pipeline is provided by:
- :class:`CollisionPipelinePrimitive`:
A collision detection pipeline optimized for primitive shapes.
This pipeline uses an `EXPLICIT` broad-phase operating on pre-computed
geometry pairs and a narrow-phase based on the primitive colliders of Newton.
- :class:`BoundingVolumeType`:
An enumeration defining the different types of bounding volumes
supported by the primitive broad-phase collision detection back-end.
"""
from .broadphase import BoundingVolumeType
from .pipeline import CollisionPipelinePrimitive
###
# Module interface
###
__all__ = [
"BoundingVolumeType",
"CollisionPipelinePrimitive",
]
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/primitive/broadphase.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides implementations of broad-phase collision
detection used by the primitive collision pipeline.
"""
from __future__ import annotations
from dataclasses import dataclass, field
from enum import IntEnum
import warp as wp
from ......geometry.types import GeoType
from ...core.geometry import GeometriesData, GeometriesModel
from ...core.types import float32, int32, override, transformf, vec2i, vec3f, vec4f, vec6f, vec8f
###
# Module interface
###
__all__ = [
"BoundingVolumeType",
"BoundingVolumesData",
"CollisionCandidatesData",
"CollisionCandidatesModel",
"primitive_broadphase_explicit",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES: list[GeoType] = [
GeoType.SPHERE,
GeoType.CYLINDER,
GeoType.CONE,
GeoType.CAPSULE,
GeoType.BOX,
GeoType.ELLIPSOID,
]
"""
List of primitive shape combinations supported by the primitive narrow-phase collider.
"""
###
# Types
###
class BoundingVolumeType(IntEnum):
"""Defines the types of bounding volumes supported for broad-phase collision detection."""
AABB = 0
"""Axis-Aligned Bounding Box (AABB)"""
BS = 1
"""Bounding Sphere (BS)"""
@classmethod
def from_string(cls, s: str) -> BoundingVolumeType:
"""Converts a string to a BoundingVolumeType enum value."""
try:
return cls[s.upper()]
except KeyError as e:
raise ValueError(f"Invalid BoundingVolumeType: {s}. Valid options are: {[e.name for e in cls]}") from e
@override
def __str__(self):
"""Returns a string representation of the bounding volume type."""
return f"BoundingVolumeType.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the bounding volume type."""
return self.__str__()
@dataclass
class BoundingVolumesData:
"""
A container to hold time-varying bounding-volume data computed for each collision geometry.
Supports broad-phase collision detection algorithms using both Axis-Aligned Bounding Boxes (AABB)
and Bounding Spheres (BS), and allocations are conditioned on the broad-phase algorithm used.
Attributes:
aabb (wp.array | None):
The vertices of the Axis-Aligned Bounding Box (AABB) of each collision geometry.\n
Shape of ``(sum_of_num_geoms,)`` and type :class:`mat83f`.
radius (wp.array | None):
The radius of the Bounding Sphere (BS) of each collision geometry.\n
Shape of ``(sum_of_num_geoms,)`` and type :class:`float32`.
"""
aabb: wp.array | None = None
"""
The min/max extents of the Axis-Aligned Bounding Box (AABB) of each collision geometry.\n
Shape of ``(sum_of_num_geoms,)`` and type :class:`vec6f`.
"""
radius: wp.array | None = None
"""
The radius of the Bounding Sphere (BS) of each collision geometry.\n
Shape of ``(sum_of_num_geoms,)`` and type :class:`float32`.
"""
@dataclass
class CollisionCandidatesModel:
"""
A container to hold time-invariant data for each candidate geom-pair. Used for
explicit broad-phase collision detection on pre-computed collision shape pairs.
"""
@staticmethod
def _default_world_geom_pairs() -> list[int]:
return [0]
num_model_geom_pairs: int = 0
"""(host-side) Total number of collision pairs in the model across all worlds."""
num_world_geom_pairs: list[int] = field(default_factory=_default_world_geom_pairs)
"""(host-side) Number of collision pairs per world."""
model_num_pairs: wp.array | None = None
"""
Total number of collisions pairs in the model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
world_num_pairs: wp.array | None = None
"""
The number of collisions pairs per world.
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
wid: wp.array | None = None
"""
World index of each collision pair.\n
Shape of ``(sum_of_num_candidate_pairs,)`` and type :class:`int32`.
"""
geom_pair: wp.array | None = None
"""
Geometry indices of each collision pair.\n
Shape of ``(sum_of_num_candidate_pairs,)`` and type :class:`vec2i`.
"""
@dataclass
class CollisionCandidatesData:
"""
A container to hold time-varying data for each active candidate geom-pair generated by
the broad-phase collision detection. Used for explicit broad-phase collision detection
on pre-computed collision shape pairs.
"""
num_model_geom_pairs: int = 0
"""(host-side) Total number of candidate collision pairs in the model across all worlds."""
model_num_collisions: wp.array | None = None
"""
Number of collisions detected across all worlds in the model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
world_num_collisions: wp.array | None = None
"""
Number of collisions detected per world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
wid: wp.array | None = None
"""
World index of each active collision pair.\n
Shape of ``(num_geom_pairs,)`` and type :class:`int32`.
"""
geom_pair: wp.array | None = None
"""
Geometry indices of each active collision pair.\n
Shape of ``(num_geom_pairs,)`` and type :class:`vec2i`.
"""
def clear(self):
"""
Clears the active number of collision candidates.
"""
self.model_num_collisions.zero_()
self.world_num_collisions.zero_()
def zero(self):
"""
Clears the active number of collision candidates and resets all data arrays to zero.
"""
self.clear()
self.wid.zero_()
self.geom_pair.zero_()
###
# Bounding-Spheres (BS) Functions
###
@wp.func
def bs_sphere(radius: float32) -> float32:
return radius
@wp.func
def bs_cylinder(radius: float32, half_height: float32) -> float32:
return wp.sqrt(half_height * half_height + radius * radius)
@wp.func
def bs_cone(radius: float32, half_height: float32) -> float32:
return wp.sqrt(half_height * half_height + radius * radius)
@wp.func
def bs_capsule(radius: float32, half_height: float32) -> float32:
return half_height + radius
@wp.func
def bs_ellipsoid(abc: vec3f) -> float32:
return wp.max(abc[0], wp.max(abc[1], abc[2]))
@wp.func
def bs_box(half_extents: vec3f) -> float32:
hx, hy, hz = half_extents[0], half_extents[1], half_extents[2]
return wp.sqrt(hx * hx + hy * hy + hz * hz)
# TODO: Implement proper BS for planes
@wp.func
def bs_plane(normal: vec3f, distance: float32) -> float32:
return float32(0.0)
@wp.func
def bs_geom(sid: int32, params: vec4f, margin: float32) -> float32:
"""
Compute the radius of the Bounding Sphere (BS) of a geometry element.
Args:
sid (int32): Shape index of the geometry element.
params (vec4f): Shape parameters of the geometry element.
Returns:
float32: The radius of the BS of the geometry element.
"""
r = float32(0.0)
if sid == GeoType.SPHERE:
r = bs_sphere(params[0] + margin)
elif sid == GeoType.CYLINDER:
r = bs_cylinder(params[0] + margin, params[1] + margin)
elif sid == GeoType.CONE:
r = bs_cone(params[0] + margin, params[1] + margin)
elif sid == GeoType.CAPSULE:
r = bs_capsule(params[0] + margin, params[1] + margin)
elif sid == GeoType.ELLIPSOID:
r = bs_ellipsoid(vec3f(params[0] + margin, params[1] + margin, params[2] + margin))
elif sid == GeoType.BOX:
r = bs_box(vec3f(params[0] + margin, params[1] + margin, params[2] + margin))
return r
@wp.func
def has_bs_overlap(pose1: transformf, pose2: transformf, radius1: float32, radius2: float32) -> wp.bool:
"""
Check for overlap between two bounding spheres.
Args:
pose1 (transformf): Pose of the first bounding sphere in world coordinates.
pose2 (transformf): Pose of the second bounding sphere in world coordinates.
radius1 (float32): Radius of the first bounding sphere.
radius2 (float32): Radius of the second bounding sphere.
"""
position1 = wp.transform_get_translation(pose1)
position2 = wp.transform_get_translation(pose2)
distance = wp.length(position2 - position1)
return distance <= (radius1 + radius2)
###
# Axis-Aligned Bounding Box (AABB) Functions
###
@wp.func
def compute_tight_aabb_from_local_extents(pose: transformf, extents: vec3f, margin: float32) -> vec6f:
R_g = wp.quat_to_matrix(wp.transform_get_rotation(pose))
r_g = wp.transform_get_translation(pose)
dx = extents[0] + margin
dy = extents[1] + margin
dz = extents[2] + margin
b_v_0 = vec3f(-dx, -dy, -dz)
b_v_1 = vec3f(-dx, -dy, dz)
b_v_2 = vec3f(-dx, dy, -dz)
b_v_3 = vec3f(-dx, dy, dz)
b_v_4 = vec3f(dx, -dy, -dz)
b_v_5 = vec3f(dx, -dy, dz)
b_v_6 = vec3f(dx, dy, -dz)
b_v_7 = vec3f(dx, dy, dz)
w_v_0 = r_g + (R_g @ b_v_0)
w_v_1 = r_g + (R_g @ b_v_1)
w_v_2 = r_g + (R_g @ b_v_2)
w_v_3 = r_g + (R_g @ b_v_3)
w_v_4 = r_g + (R_g @ b_v_4)
w_v_5 = r_g + (R_g @ b_v_5)
w_v_6 = r_g + (R_g @ b_v_6)
w_v_7 = r_g + (R_g @ b_v_7)
min_x = wp.min(vec8f(w_v_0[0], w_v_1[0], w_v_2[0], w_v_3[0], w_v_4[0], w_v_5[0], w_v_6[0], w_v_7[0]))
max_x = wp.max(vec8f(w_v_0[0], w_v_1[0], w_v_2[0], w_v_3[0], w_v_4[0], w_v_5[0], w_v_6[0], w_v_7[0]))
min_y = wp.min(vec8f(w_v_0[1], w_v_1[1], w_v_2[1], w_v_3[1], w_v_4[1], w_v_5[1], w_v_6[1], w_v_7[1]))
max_y = wp.max(vec8f(w_v_0[1], w_v_1[1], w_v_2[1], w_v_3[1], w_v_4[1], w_v_5[1], w_v_6[1], w_v_7[1]))
min_z = wp.min(vec8f(w_v_0[2], w_v_1[2], w_v_2[2], w_v_3[2], w_v_4[2], w_v_5[2], w_v_6[2], w_v_7[2]))
max_z = wp.max(vec8f(w_v_0[2], w_v_1[2], w_v_2[2], w_v_3[2], w_v_4[2], w_v_5[2], w_v_6[2], w_v_7[2]))
aabb = vec6f(min_x, min_y, min_z, max_x, max_y, max_z)
return aabb
@wp.func
def aabb_sphere(pose: transformf, radius: float32, margin: float32) -> vec6f:
r_g = wp.transform_get_translation(pose)
extents = vec3f(radius + margin, radius + margin, radius + margin)
min = r_g - extents
max = r_g + extents
aabb = vec6f(min.x, min.y, min.z, max.x, max.y, max.z)
return aabb
@wp.func
def aabb_cylinder(pose: transformf, radius: float32, half_height: float32, margin: float32) -> vec6f:
extents = vec3f(radius, radius, half_height)
return compute_tight_aabb_from_local_extents(pose, extents, margin)
@wp.func
def aabb_cone(pose: transformf, radius: float32, half_height: float32, margin: float32) -> vec6f:
extents = vec3f(radius, radius, half_height)
return compute_tight_aabb_from_local_extents(pose, extents, margin)
@wp.func
def aabb_capsule(pose: transformf, radius: float32, half_height: float32, margin: float32) -> vec6f:
extents = vec3f(radius, radius, half_height + radius)
return compute_tight_aabb_from_local_extents(pose, extents, margin)
@wp.func
def aabb_ellipsoid(pose: transformf, abc: vec3f, margin: float32) -> vec6f:
extents = vec3f(abc[0], abc[1], abc[2])
return compute_tight_aabb_from_local_extents(pose, extents, margin)
@wp.func
def aabb_box(pose: transformf, half_extents: vec3f, margin: float32) -> vec6f:
return compute_tight_aabb_from_local_extents(pose, half_extents, margin)
# TODO: Implement proper AABB for planes
@wp.func
def aabb_plane(pose: transformf, normal: vec3f, distance: float32, margin: float32) -> vec6f:
return vec6f(0.0)
@wp.func
def aabb_geom(sid: int32, params: vec4f, margin: float32, pose: transformf) -> vec6f:
"""
Compute the Axis-Aligned Bounding Box (AABB) vertices of a geometry element.
Args:
pose (transformf): Pose of the geometry element in world coordinates.
params (vec4f): Shape parameters of the geometry element.
sid (int32): Shape index of the geometry element.
Returns:
vec6f: The vertices of the AABB of the geometry element.
"""
aabb = vec6f(0.0)
if sid == GeoType.SPHERE:
aabb = aabb_sphere(pose, params[0], margin)
elif sid == GeoType.CYLINDER:
aabb = aabb_cylinder(pose, params[0], params[1], margin)
elif sid == GeoType.CONE:
aabb = aabb_cone(pose, params[0], params[1], margin)
elif sid == GeoType.CAPSULE:
aabb = aabb_capsule(pose, params[0], params[1], margin)
elif sid == GeoType.ELLIPSOID:
aabb = aabb_ellipsoid(pose, vec3f(params[0], params[1], params[2]), margin)
elif sid == GeoType.BOX:
aabb = aabb_box(pose, vec3f(params[0], params[1], params[2]), margin)
elif sid == GeoType.PLANE:
aabb = aabb_plane(pose, vec3f(params[0], params[1], params[2]), params[3], margin)
return aabb
@wp.func
def has_aabb_overlap(aabb1: vec6f, aabb2: vec6f) -> wp.bool:
overlap_x = (aabb1[0] <= aabb2[3]) and (aabb1[3] >= aabb2[0])
overlap_y = (aabb1[1] <= aabb2[4]) and (aabb1[4] >= aabb2[1])
overlap_z = (aabb1[2] <= aabb2[5]) and (aabb1[5] >= aabb2[2])
return overlap_x and overlap_y and overlap_z
###
# Common Functions
###
@wp.func
def add_active_pair(
# Inputs:
wid_in: int32,
gid1_in: int32,
gid2_in: int32,
sid1_in: int32,
sid2_in: int32,
model_num_pairs_in: int32,
world_num_pairs_in: int32,
# Outputs:
model_num_collisions_out: wp.array(dtype=int32),
world_num_collisions_out: wp.array(dtype=int32),
collision_wid_out: wp.array(dtype=int32),
collision_geom_pair_out: wp.array(dtype=vec2i),
):
# Increment the number of collisions detected for
# the model and world and retrieve the pair indices
model_pairid_out = wp.atomic_add(model_num_collisions_out, 0, 1)
world_pairid_out = wp.atomic_add(world_num_collisions_out, wid_in, 1)
# Check if we have exceeded the maximum number of collision pairs
# allowed and if yes then roll-back the atomic adds and exit
if model_pairid_out >= model_num_pairs_in or world_pairid_out >= world_num_pairs_in:
wp.atomic_sub(model_num_collisions_out, 0, 1)
wp.atomic_sub(world_num_collisions_out, wid_in, 1)
return
# Correct the pair id order in order to invoke the correct near-phase function
if sid1_in > sid2_in:
pair_out = wp.vec2i(gid2_in, gid1_in)
else:
pair_out = wp.vec2i(gid1_in, gid2_in)
# Store the active collision output data
collision_wid_out[model_pairid_out] = wid_in
collision_geom_pair_out[model_pairid_out] = pair_out
###
# Kernels
###
@wp.kernel
def _update_geometries_state_and_aabb(
# Inputs:
default_gap: float32,
geom_bid: wp.array(dtype=int32),
geom_sid: wp.array(dtype=int32),
geom_params: wp.array(dtype=vec4f),
geom_margin: wp.array(dtype=float32),
geom_gap: wp.array(dtype=float32),
geom_offset: wp.array(dtype=transformf),
body_pose: wp.array(dtype=transformf),
# Outputs:
geom_pose: wp.array(dtype=transformf),
geom_aabb: wp.array(dtype=vec6f),
):
"""
Updates the state of each geometry and computes its Axis-Aligned Bounding Box (AABB).
The AABB is expanded by ``margin + max(default_gap, gap)`` per shape so
the broadphase catches contacts within the detection threshold.
"""
gid = wp.tid()
bid = geom_bid[gid]
sid = geom_sid[gid]
X_bg = geom_offset[gid]
params = geom_params[gid]
margin = geom_margin[gid]
gap = geom_gap[gid]
X_b = wp.transform_identity(dtype=float32)
if bid > -1:
X_b = body_pose[bid]
X_g = wp.transform_multiply(X_b, X_bg)
expansion = margin + wp.max(default_gap, gap)
aabb_g = aabb_geom(sid, params, expansion, X_g)
geom_pose[gid] = X_g
geom_aabb[gid] = aabb_g
@wp.kernel
def _update_geometries_state_and_bs(
# Inputs:
default_gap: float32,
geom_bid: wp.array(dtype=int32),
geom_sid: wp.array(dtype=int32),
geom_params: wp.array(dtype=vec4f),
geom_margin: wp.array(dtype=float32),
geom_gap: wp.array(dtype=float32),
geom_offset: wp.array(dtype=transformf),
body_pose: wp.array(dtype=transformf),
# Outputs:
geom_pose: wp.array(dtype=transformf),
geom_bs_radius: wp.array(dtype=float32),
):
"""
Updates the state of each geometry and computes its bounding sphere (BS).
The bounding sphere is expanded by ``margin + max(default_gap, gap)`` per
shape so the broadphase catches contacts within the detection threshold.
"""
gid = wp.tid()
bid = geom_bid[gid]
sid = geom_sid[gid]
X_bg = geom_offset[gid]
params = geom_params[gid]
margin = geom_margin[gid]
gap = geom_gap[gid]
X_b = wp.transform_identity(dtype=float32)
if bid > -1:
X_b = body_pose[bid]
X_g = wp.transform_multiply(X_b, X_bg)
expansion = margin + wp.max(default_gap, gap)
bs_g = bs_geom(sid, params, expansion)
geom_pose[gid] = X_g
geom_bs_radius[gid] = bs_g
@wp.kernel
def _nxn_broadphase_aabb(
# Inputs:
geom_sid: wp.array(dtype=int32),
geom_aabb_minmax: wp.array(dtype=vec6f),
cmodel_model_num_pairs: wp.array(dtype=int32),
cmodel_world_num_pairs: wp.array(dtype=int32),
cmodel_wid: wp.array(dtype=int32),
cmodel_geom_pair: wp.array(dtype=vec2i),
# Outputs:
cdata_model_num_collisions: wp.array(dtype=int32),
cdata_world_num_collisions: wp.array(dtype=int32),
cdata_wid: wp.array(dtype=int32),
cdata_geom_pair: wp.array(dtype=vec2i),
):
"""
A kernel that performs broad-phase collision detection using Axis-Aligned Bounding Boxes (AABB).
Inputs:
geom_sid (wp.array(dtype=int32)):
Shape index for each geometry.
geom_aabb_minmax (wp.array(dtype=vec6f)):
Minimum and maximum coordinates for each geometry's Axis-Aligned Bounding Box (AABB).
cmodel_model_num_pairs (wp.array(dtype=int32)):
Total number of collision pairs in the model.
cmodel_world_num_pairs (wp.array(dtype=int32)):
Number of collision pairs per world.
cmodel_wid (wp.array(dtype=int32)):
World index for each collision pair candidate.
cmodel_geom_pair (wp.array(dtype=vec2i)):
Geometry indices for each collision pair candidate.
Outputs:
cdata_model_num_collisions (wp.array(dtype=int32)):
Number of collisions detected across all worlds in the model.
cdata_world_num_collisions (wp.array(dtype=int32)):
Number of collisions detected per world.
cdata_wid (wp.array(dtype=int32)):
World index for each detected collision.
cdata_geom_pair (wp.array(dtype=vec2i)):
Geometry indices for each detected collision.
"""
# Retrieve the geom-pair index from the thread grid
gpid = wp.tid()
# Retrieve the world index for the current geom-pair
wid = cmodel_wid[gpid]
# Retrieve the geometry pair counts
num_model_pairs = cmodel_model_num_pairs[0]
num_world_pairs = cmodel_world_num_pairs[wid]
# Retrieve the geometry indices for the current geom-pair
geom_pair = cmodel_geom_pair[gpid]
gid1 = geom_pair[0]
gid2 = geom_pair[1]
# Retrieve the geometry-specific data for both geometries
sid1 = geom_sid[gid1]
aabb1 = geom_aabb_minmax[gid1]
sid2 = geom_sid[gid2]
aabb2 = geom_aabb_minmax[gid2]
# Check for BV overlap and if yes then add to active collision pairs
if has_aabb_overlap(aabb1, aabb2):
add_active_pair(
wid,
gid1,
gid2,
sid1,
sid2,
num_model_pairs,
num_world_pairs,
cdata_model_num_collisions,
cdata_world_num_collisions,
cdata_wid,
cdata_geom_pair,
)
@wp.kernel
def _nxn_broadphase_bs(
# Inputs:
geom_sid: wp.array(dtype=int32),
geom_pose: wp.array(dtype=transformf),
geom_bs_radius: wp.array(dtype=float32),
cmodel_model_num_pairs: wp.array(dtype=int32),
cmodel_world_num_pairs: wp.array(dtype=int32),
cmodel_wid: wp.array(dtype=int32),
cmodel_geom_pair: wp.array(dtype=vec2i),
# Outputs:
cdata_model_num_collisions: wp.array(dtype=int32),
cdata_world_num_collisions: wp.array(dtype=int32),
cdata_wid: wp.array(dtype=int32),
cdata_geom_pair: wp.array(dtype=vec2i),
):
"""
A kernel that performs broad-phase collision detection using bounding spheres (BS).
Inputs:
geom_sid (wp.array(dtype=int32)):
Shape index for each geometry.
geom_pose (wp.array(dtype=transformf)):
Pose of each geometry in world coordinates.
geom_bs_radius (wp.array(dtype=float32)):
Radius of the bounding sphere for each geometry.
cmodel_model_num_pairs (wp.array(dtype=int32)):
Total number of collision pairs in the model.
cmodel_world_num_pairs (wp.array(dtype=int32)):
Number of collision pairs per world.
cmodel_wid (wp.array(dtype=int32)):
World index for each collision pair candidate.
cmodel_geom_pair (wp.array(dtype=vec2i)):
Geometry indices for each collision pair candidate.
Outputs:
cdata_model_num_collisions (wp.array(dtype=int32)):
Number of collisions detected across all worlds in the model.
cdata_world_num_collisions (wp.array(dtype=int32)):
Number of collisions detected per world.
cdata_wid (wp.array(dtype=int32)):
World index of each active collision pair.
cdata_geom_pair (wp.array(dtype=vec2i)):
Geometry indices for each active collision pair.
"""
# Retrieve the geom-pair index from the thread grid
gpid = wp.tid()
# Retrieve the world index for the current geom-pair
wid = cmodel_wid[gpid]
# Retrieve the geometry pair counts
num_model_pairs = cmodel_model_num_pairs[0]
num_world_pairs = cmodel_world_num_pairs[wid]
# Retrieve the geometry indices for the current geom-pair
geom_pair = cmodel_geom_pair[gpid]
gid1 = geom_pair[0]
gid2 = geom_pair[1]
# Retrieve the geometry-specific data for geometry 1
sid1 = geom_sid[gid1]
pose1 = geom_pose[gid1]
radius1 = geom_bs_radius[gid1]
# Retrieve the geometry-specific data for geometry 2
sid2 = geom_sid[gid2]
pose2 = geom_pose[gid2]
radius2 = geom_bs_radius[gid2]
# Check for BV overlap and if yes then add to active collision pairs
if has_bs_overlap(pose1, pose2, radius1, radius2):
add_active_pair(
wid,
gid1,
gid2,
sid1,
sid2,
num_model_pairs,
num_world_pairs,
cdata_model_num_collisions,
cdata_world_num_collisions,
cdata_wid,
cdata_geom_pair,
)
###
# Launchers
###
def update_geoms_aabb(
# Inputs:
body_poses: wp.array,
geoms_model: GeometriesModel,
geoms_data: GeometriesData,
# Outputs:
bv_data: BoundingVolumesData,
# Options
default_gap: float | None = None,
):
"""
Launches a kernel to update the state of each geometry and compute its AABB.
Args:
body_poses: Pose of each body in world coordinates.
geoms_model: Model data for collision geometries.
geoms_data: Data for collision geometries.
bv_data: Data for bounding volumes containing AABB vertices.
default_gap: Default detection gap [m] applied as a floor to per-geometry gaps.
"""
wp.launch(
_update_geometries_state_and_aabb,
dim=geoms_model.num_geoms,
inputs=[
float32(default_gap) if default_gap is not None else float32(0.0),
geoms_model.bid,
geoms_model.type,
geoms_model.params,
geoms_model.margin,
geoms_model.gap,
geoms_model.offset,
body_poses,
],
outputs=[geoms_data.pose, bv_data.aabb],
device=body_poses.device,
)
def nxn_broadphase_aabb(
# Inputs:
geoms_model: GeometriesModel,
bv_data: BoundingVolumesData,
# Outputs:
candidates_model: CollisionCandidatesModel,
candidates_data: CollisionCandidatesData,
):
"""
Launches a kernel to perform broad-phase collision detection using Axis-Aligned Bounding Boxes (AABB).
Args:
geoms_model (CollisionGeometriesModel): Model data for collision geometries.
bv_data (BoundingVolumesData): Data for bounding volumes containing Axis-Aligned Bounding Boxes (AABB) vertices.
candidates_model (CollisionCandidatesModel): Model data for collision candidates.
candidates_data (CollisionCandidatesData): Data for collision candidates.
"""
wp.launch(
_nxn_broadphase_aabb,
dim=candidates_model.num_model_geom_pairs,
inputs=[
geoms_model.type,
bv_data.aabb,
candidates_model.model_num_pairs,
candidates_model.world_num_pairs,
candidates_model.wid,
candidates_model.geom_pair,
],
outputs=[
candidates_data.model_num_collisions,
candidates_data.world_num_collisions,
candidates_data.wid,
candidates_data.geom_pair,
],
device=geoms_model.type.device,
)
def update_geoms_bs(
# Inputs:
body_poses: wp.array,
geoms_model: GeometriesModel,
geoms_data: GeometriesData,
# Outputs:
bv_data: BoundingVolumesData,
# Options
default_gap: float | None = None,
):
"""
Launches a kernel to update the state of each geometry and compute its bounding sphere (BS).
Args:
body_poses: Pose of each body in world coordinates.
geoms_model: Model data for collision geometries.
geoms_data: Data for collision geometries.
bv_data: Data for bounding volumes containing bounding sphere radii.
default_gap: Default detection gap [m] applied as a floor to per-geometry gaps.
"""
wp.launch(
_update_geometries_state_and_bs,
dim=geoms_model.num_geoms,
inputs=[
float32(default_gap) if default_gap is not None else float32(0.0),
geoms_model.bid,
geoms_model.type,
geoms_model.params,
geoms_model.margin,
geoms_model.gap,
geoms_model.offset,
body_poses,
],
outputs=[geoms_data.pose, bv_data.radius],
device=body_poses.device,
)
def nxn_broadphase_bs(
# Inputs:
geoms_model: GeometriesModel,
geoms_data: GeometriesData,
bv_data: BoundingVolumesData,
# Outputs:
candidates_model: CollisionCandidatesModel,
candidates_data: CollisionCandidatesData,
):
"""
Launches a kernel to perform broad-phase collision detection using bounding spheres (BS).
Args:
geoms_model (CollisionGeometriesModel): Model data for collision geometries.
geoms_data (GeometriesData): Data for collision geometries.
bv_data (BoundingVolumesData): Data for bounding volumes containing bounding sphere radii.
candidates_model (CollisionCandidatesModel): Model data for collision candidates.
candidates_data (CollisionCandidatesData): Data for collision candidates.
"""
wp.launch(
_nxn_broadphase_bs,
dim=candidates_model.num_model_geom_pairs,
inputs=[
geoms_model.type,
geoms_data.pose,
bv_data.radius,
candidates_model.model_num_pairs,
candidates_model.world_num_pairs,
candidates_model.wid,
candidates_model.geom_pair,
],
outputs=[
candidates_data.model_num_collisions,
candidates_data.world_num_collisions,
candidates_data.wid,
candidates_data.geom_pair,
],
device=geoms_model.type.device,
)
def primitive_broadphase_explicit(
# Inputs:
body_poses: wp.array,
geoms_model: GeometriesModel,
geoms_data: GeometriesData,
bv_data: BoundingVolumesData,
bv_type: BoundingVolumeType,
# Outputs:
candidates_model: CollisionCandidatesModel,
candidates_data: CollisionCandidatesData,
# Options
default_gap: float | None = None,
):
"""
Runs explicit broad-phase collision detection between all geometry pairs
defined in the collision candidates model using the specified bounding volume type.
Args:
body_poses: Pose of each body in world coordinates.
geoms_model: Model data for collision geometries.
geoms_data: Data for collision geometries.
bv_data: Data for bounding volumes.
bv_type: Type of bounding volume to use for broad-phase collision detection.
candidates_model: Model data for collision candidates.
candidates_data: Data for collision candidates.
default_gap: Default detection gap [m] applied as a floor to per-geometry gaps.
"""
match bv_type:
case BoundingVolumeType.AABB:
update_geoms_aabb(body_poses, geoms_model, geoms_data, bv_data, default_gap)
nxn_broadphase_aabb(geoms_model, bv_data, candidates_model, candidates_data)
case BoundingVolumeType.BS:
update_geoms_bs(body_poses, geoms_model, geoms_data, bv_data, default_gap)
nxn_broadphase_bs(geoms_model, geoms_data, bv_data, candidates_model, candidates_data)
case _:
raise ValueError(f"Unsupported bounding volume type: {bv_type}")
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/primitive/narrowphase.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides a narrow-phase Collision Detection (CD) backend optimized for geometric primitives.
This narrow-phase CD back-end uses the primitive colliders of Newton to compute
discrete contacts, but conforms to the data layout and required by Kamino.
"""
from typing import Any
import warp as wp
from ......geometry.collision_primitive import (
MAXVAL,
collide_box_box,
collide_capsule_box,
collide_capsule_capsule,
collide_plane_box,
collide_plane_capsule,
collide_plane_cylinder,
collide_plane_ellipsoid,
collide_plane_sphere,
collide_sphere_box,
collide_sphere_capsule,
collide_sphere_cylinder,
collide_sphere_sphere,
)
from ......geometry.types import GeoType
from ...core.data import DataKamino
from ...core.materials import make_get_material_pair_properties
from ...core.model import ModelKamino
from ...core.types import (
float32,
int32,
mat33f,
quatf,
transformf,
uint32,
uint64,
vec2f,
vec2i,
vec3f,
vec4f,
)
from ...geometry.contacts import ContactsKaminoData, make_contact_frame_znorm
from ...geometry.keying import build_pair_key2
from .broadphase import CollisionCandidatesData
###
# Module interface
###
__all__ = [
"primitive_narrowphase",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
PRIMITIVE_NARROWPHASE_SUPPORTED_SHAPE_PAIRS: list[tuple[GeoType, GeoType]] = [
(GeoType.BOX, GeoType.BOX),
(GeoType.CAPSULE, GeoType.BOX),
(GeoType.CAPSULE, GeoType.CAPSULE),
(GeoType.PLANE, GeoType.BOX),
(GeoType.PLANE, GeoType.CAPSULE),
(GeoType.PLANE, GeoType.CYLINDER),
(GeoType.PLANE, GeoType.ELLIPSOID),
(GeoType.PLANE, GeoType.SPHERE),
(GeoType.SPHERE, GeoType.BOX),
(GeoType.SPHERE, GeoType.CAPSULE),
(GeoType.SPHERE, GeoType.CYLINDER),
(GeoType.SPHERE, GeoType.SPHERE),
]
"""
List of primitive shape combinations supported by the primitive narrow-phase collider.
"""
###
# Geometry helper Types
###
@wp.struct
class Box:
gid: int32
bid: int32
pos: vec3f
rot: mat33f
size: vec3f
@wp.struct
class Sphere:
gid: int32
bid: int32
pos: vec3f
rot: mat33f
radius: float32
@wp.struct
class Capsule:
gid: int32
bid: int32
pos: vec3f
rot: mat33f
axis: vec3f
radius: float32
half_length: float32
@wp.struct
class Cylinder:
gid: int32
bid: int32
pos: vec3f
rot: mat33f
axis: vec3f
radius: float32
half_height: float32
@wp.struct
class Plane:
gid: int32
bid: int32
normal: vec3f
pos: vec3f
@wp.struct
class Ellipsoid:
gid: int32
bid: int32
pos: vec3f
rot: mat33f
size: vec3f
@wp.func
def make_box(pose: transformf, params: vec4f, gid: int32, bid: int32) -> Box:
box = Box()
box.gid = gid
box.bid = bid
box.pos = wp.transform_get_translation(pose)
box.rot = wp.quat_to_matrix(wp.transform_get_rotation(pose))
box.size = vec3f(params[0], params[1], params[2])
return box
@wp.func
def make_sphere(pose: transformf, params: vec4f, gid: int32, bid: int32) -> Sphere:
sphere = Sphere()
sphere.gid = gid
sphere.bid = bid
sphere.pos = wp.transform_get_translation(pose)
sphere.rot = wp.quat_to_matrix(wp.transform_get_rotation(pose))
sphere.radius = params[0]
return sphere
@wp.func
def make_capsule(pose: transformf, params: vec4f, gid: int32, bid: int32) -> Capsule:
capsule = Capsule()
capsule.gid = gid
capsule.bid = bid
capsule.pos = wp.transform_get_translation(pose)
rot_mat = wp.quat_to_matrix(wp.transform_get_rotation(pose))
capsule.rot = rot_mat
# Capsule axis is along the local Z-axis
capsule.axis = vec3f(rot_mat[0, 2], rot_mat[1, 2], rot_mat[2, 2])
capsule.radius = params[0]
capsule.half_length = params[1]
return capsule
@wp.func
def make_cylinder(pose: transformf, params: vec4f, gid: int32, bid: int32) -> Cylinder:
cylinder = Cylinder()
cylinder.gid = gid
cylinder.bid = bid
cylinder.pos = wp.transform_get_translation(pose)
rot_mat = wp.quat_to_matrix(wp.transform_get_rotation(pose))
cylinder.rot = rot_mat
# Cylinder axis is along the local Z-axis
cylinder.axis = vec3f(rot_mat[0, 2], rot_mat[1, 2], rot_mat[2, 2])
cylinder.radius = params[0]
cylinder.half_height = params[1]
return cylinder
@wp.func
def make_plane(pose: transformf, params: vec4f, gid: int32, bid: int32) -> Plane:
plane = Plane()
plane.gid = gid
plane.bid = bid
# Plane normal is stored in params[0:3]
plane.normal = vec3f(params[0], params[1], params[2])
# Plane position is the transform translation
plane.pos = wp.transform_get_translation(pose)
return plane
@wp.func
def make_ellipsoid(pose: transformf, params: vec4f, gid: int32, bid: int32) -> Ellipsoid:
ellipsoid = Ellipsoid()
ellipsoid.gid = gid
ellipsoid.bid = bid
ellipsoid.pos = wp.transform_get_translation(pose)
ellipsoid.rot = wp.quat_to_matrix(wp.transform_get_rotation(pose))
# Ellipsoid size (radii) stored in params[0:3]
ellipsoid.size = vec3f(params[0], params[1], params[2])
return ellipsoid
###
# Common Functions
###
@wp.func
def add_single_contact(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
wid: int32,
gid_1: int32,
gid_2: int32,
bid_1: int32,
bid_2: int32,
margin_plus_gap: float32,
margin: float32,
distance: float32,
position: vec3f,
normal: vec3f,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Skip if the contact distance exceeds the detection threshold
if (distance - margin_plus_gap) > 0.0:
return
# Increment the active contact counters
mcid = wp.atomic_add(contact_model_num, 0, 1)
wcid = wp.atomic_add(contact_world_num, wid, 1)
# If within max allocated contacts, write the new contact data
if mcid < model_max_contacts and wcid < world_max_contacts:
# Perform A/B geom and body assignment
# NOTE: We want the normal to always point from A to B,
# and hence body B to be the "effected" body in the contact
# so we have to ensure that bid_B is always non-negative
if bid_2 < 0:
gid_AB = vec2i(gid_2, gid_1)
bid_AB = vec2i(bid_2, bid_1)
normal = -normal
else:
gid_AB = vec2i(gid_1, gid_2)
bid_AB = vec2i(bid_1, bid_2)
# Compute absolute penetration distance
distance_abs = wp.abs(distance)
# The colliders compute the contact point in the middle, and thus to get the
# per-geom contact points we need to offset by the penetration depth along the normal
position_A = position + 0.5 * distance_abs * normal
position_B = position - 0.5 * distance_abs * normal
# Store margin-shifted distance in gapfunc.w: negative means penetration
# past the resting separation, zero means at rest, positive means within
# the detection gap but not yet at rest.
d = distance - margin
gapfunc = vec4f(normal.x, normal.y, normal.z, d)
q_frame = wp.quat_from_matrix(make_contact_frame_znorm(normal))
material = vec2f(friction, restitution)
key = build_pair_key2(uint32(gid_AB[0]), uint32(gid_AB[1]))
# Store the active contact output data
contact_wid[mcid] = wid
contact_cid[mcid] = wcid
contact_gid_AB[mcid] = gid_AB
contact_bid_AB[mcid] = bid_AB
contact_position_A[mcid] = position_A
contact_position_B[mcid] = position_B
contact_gapfunc[mcid] = gapfunc
contact_frame[mcid] = q_frame
contact_material[mcid] = material
contact_key[mcid] = key
# Otherwise roll-back the atomic add if we exceeded limits
else:
wp.atomic_sub(contact_model_num, 0, 1)
wp.atomic_sub(contact_world_num, wid, 1)
def make_add_multiple_contacts(MAX_CONTACTS: int, SHARED_NORMAL: bool):
# Define the function to add multiple contacts
@wp.func
def add_multiple_contacts(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
wid: int32,
gid_1: int32,
gid_2: int32,
bid_1: int32,
bid_2: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
distances: wp.types.vector(MAX_CONTACTS, wp.float32),
positions: wp.types.matrix((MAX_CONTACTS, 3), wp.float32),
normals: Any,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Count valid contacts (those within the detection threshold)
num_contacts = wp.int32(0)
for k in range(MAX_CONTACTS):
if distances[k] != wp.inf and distances[k] <= margin_plus_gap:
num_contacts += 1
# Skip operation if no contacts were detected
if num_contacts == 0:
return
# Perform A/B geom and body assignment
# NOTE: We want the normal to always point from A to B,
# and hence body B to be the "effected" body in the contact
# so we have to ensure that bid_B is always non-negative
if bid_2 < 0:
gid_AB = vec2i(gid_2, gid_1)
bid_AB = vec2i(bid_2, bid_1)
else:
gid_AB = vec2i(gid_1, gid_2)
bid_AB = vec2i(bid_1, bid_2)
# Increment the active contact counter
mcio = wp.atomic_add(contact_model_num, 0, num_contacts)
wcio = wp.atomic_add(contact_world_num, wid, num_contacts)
# Retrieve the maximum number of contacts that can be stored for this geom pair
max_num_contacts = wp.min(wp.min(model_max_contacts - mcio, world_max_contacts - wcio), num_contacts)
# Create the common material for this contact set
material = vec2f(friction, restitution)
key = build_pair_key2(uint32(gid_AB[0]), uint32(gid_AB[1]))
# Define a separate active contact index
# NOTE: This is different from k since some contacts
# may be not meet the criteria for being active
active_contact_idx = wp.int32(0)
# Add generated contacts data to the output arrays
for k in range(MAX_CONTACTS):
# Break if we've reached the maximum number of contacts for this geom pair
if active_contact_idx >= max_num_contacts:
break
# If contact is valid, store it
if distances[k] != wp.inf and distances[k] <= margin_plus_gap:
# Compute the global contact index
mcid = mcio + active_contact_idx
# Extract contact data based on whether we have shared or per-contact normals
distance = distances[k]
position = vec3f(positions[k, 0], positions[k, 1], positions[k, 2])
if wp.static(SHARED_NORMAL):
normal = normals
else:
normal = vec3f(normals[k, 0], normals[k, 1], normals[k, 2])
distance_abs = wp.abs(distance)
# Adjust normal direction based on body assignment
if bid_2 < 0:
normal = -normal
# This collider computes the contact point in the middle, and thus to get the
# per-geom contact we need to offset the contact point by the penetration depth
position_A = position + 0.5 * normal * distance_abs
position_B = position - 0.5 * normal * distance_abs
# Store margin-shifted distance in gapfunc.w
d = distance - margin
gapfunc = vec4f(normal.x, normal.y, normal.z, d)
q_frame = wp.quat_from_matrix(make_contact_frame_znorm(normal))
# Store contact data
contact_wid[mcid] = wid
contact_cid[mcid] = wcio + active_contact_idx
contact_gid_AB[mcid] = gid_AB
contact_bid_AB[mcid] = bid_AB
contact_position_A[mcid] = position_A
contact_position_B[mcid] = position_B
contact_gapfunc[mcid] = gapfunc
contact_frame[mcid] = q_frame
contact_material[mcid] = material
contact_key[mcid] = key
# Increment active contact index
active_contact_idx += 1
# Roll-back the atomic add if we exceeded limits
if active_contact_idx < num_contacts:
wp.atomic_sub(contact_model_num, 0, num_contacts - active_contact_idx)
wp.atomic_sub(contact_world_num, wid, num_contacts - active_contact_idx)
# Return the generated function
return add_multiple_contacts
###
# Primitive Colliders
###
@wp.func
def sphere_sphere(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
sphere1: Sphere,
sphere2: Sphere,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Run the respective collider function to detect sphere-sphere contacts
distance, position, normal = collide_sphere_sphere(sphere1.pos, sphere1.radius, sphere2.pos, sphere2.radius)
# Add the active contact to the global contacts arrays
add_single_contact(
model_max_contacts,
world_max_contacts,
wid,
sphere1.gid,
sphere2.gid,
sphere1.bid,
sphere2.bid,
margin_plus_gap,
margin,
distance,
position,
normal,
friction,
restitution,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def sphere_cylinder(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
sphere1: Sphere,
cylinder2: Cylinder,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distance, position, normal = collide_sphere_cylinder(
sphere1.pos,
sphere1.radius,
cylinder2.pos,
cylinder2.axis,
cylinder2.radius,
cylinder2.half_height,
)
# Add the active contact to the global contacts arrays
add_single_contact(
model_max_contacts,
world_max_contacts,
wid,
sphere1.gid,
cylinder2.gid,
sphere1.bid,
cylinder2.bid,
margin_plus_gap,
margin,
distance,
position,
normal,
friction,
restitution,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def sphere_cone():
pass
@wp.func
def sphere_capsule(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
sphere1: Sphere,
capsule2: Capsule,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distance, position, normal = collide_sphere_capsule(
sphere1.pos,
sphere1.radius,
capsule2.pos,
capsule2.axis,
capsule2.radius,
capsule2.half_length,
)
# Add the active contact to the global contacts arrays
add_single_contact(
model_max_contacts,
world_max_contacts,
wid,
sphere1.gid,
capsule2.gid,
sphere1.bid,
capsule2.bid,
margin_plus_gap,
margin,
distance,
position,
normal,
friction,
restitution,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def sphere_box(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
sphere1: Sphere,
box2: Box,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distance, position, normal = collide_sphere_box(sphere1.pos, sphere1.radius, box2.pos, box2.rot, box2.size)
# Add the active contact to the global contacts arrays
add_single_contact(
model_max_contacts,
world_max_contacts,
wid,
sphere1.gid,
box2.gid,
sphere1.bid,
box2.bid,
margin_plus_gap,
margin,
distance,
position,
normal,
friction,
restitution,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def sphere_ellipsoid():
pass
@wp.func
def cylinder_cylinder():
pass
@wp.func
def cylinder_cone():
pass
@wp.func
def cylinder_capsule():
pass
@wp.func
def cylinder_box():
pass
@wp.func
def cylinder_ellipsoid():
pass
@wp.func
def cone_cone():
pass
@wp.func
def cone_capsule():
pass
@wp.func
def cone_box():
pass
@wp.func
def cone_ellipsoid():
pass
@wp.func
def capsule_capsule(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
capsule1: Capsule,
capsule2: Capsule,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distance, position, normal = collide_capsule_capsule(
capsule1.pos,
capsule1.axis,
capsule1.radius,
capsule1.half_length,
capsule2.pos,
capsule2.axis,
capsule2.radius,
capsule2.half_length,
)
# Add the active contact to the global contacts arrays
for k in range(2):
if distance[k] != MAXVAL:
add_single_contact(
model_max_contacts,
world_max_contacts,
wid,
capsule1.gid,
capsule2.gid,
capsule1.bid,
capsule2.bid,
margin_plus_gap,
margin,
distance[k],
position[k],
normal,
friction,
restitution,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def capsule_box(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
capsule1: Capsule,
box2: Box,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distances, positions, normals = collide_capsule_box(
capsule1.pos,
capsule1.axis,
capsule1.radius,
capsule1.half_length,
box2.pos,
box2.rot,
box2.size,
)
# Add the active contacts to the global contacts arrays (up to 2 contacts with per-contact normals)
wp.static(make_add_multiple_contacts(2, False))(
model_max_contacts,
world_max_contacts,
wid,
capsule1.gid,
box2.gid,
capsule1.bid,
box2.bid,
margin_plus_gap,
margin,
friction,
restitution,
distances,
positions,
normals,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def capsule_ellipsoid():
pass
@wp.func
def box_box(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
box1: Box,
box2: Box,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distances, positions, normals = collide_box_box(
box1.pos, box1.rot, box1.size, box2.pos, box2.rot, box2.size, margin_plus_gap
)
# Add the active contacts to the global contacts arrays (up to 8 contacts with per-contact normals)
wp.static(make_add_multiple_contacts(8, False))(
model_max_contacts,
world_max_contacts,
wid,
box1.gid,
box2.gid,
box1.bid,
box2.bid,
margin_plus_gap,
margin,
friction,
restitution,
distances,
positions,
normals,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def box_ellipsoid():
pass
@wp.func
def ellipsoid_ellipsoid():
pass
@wp.func
def plane_sphere(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
plane1: Plane,
sphere2: Sphere,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
# Note: collide_plane_sphere returns (distance, position) without normal
distance, position = collide_plane_sphere(plane1.normal, plane1.pos, sphere2.pos, sphere2.radius)
# Use plane normal as contact normal
normal = plane1.normal
# Add the active contact to the global contacts arrays
add_single_contact(
model_max_contacts,
world_max_contacts,
wid,
plane1.gid,
sphere2.gid,
plane1.bid,
sphere2.bid,
margin_plus_gap,
margin,
distance,
position,
normal,
friction,
restitution,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def plane_box(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
plane1: Plane,
box2: Box,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distances, positions, normal = collide_plane_box(
plane1.normal, plane1.pos, box2.pos, box2.rot, box2.size, margin_plus_gap
)
# Add the active contacts to the global contacts arrays (up to 4 contacts with shared normal)
wp.static(make_add_multiple_contacts(4, True))(
model_max_contacts,
world_max_contacts,
wid,
plane1.gid,
box2.gid,
plane1.bid,
box2.bid,
margin_plus_gap,
margin,
friction,
restitution,
distances,
positions,
normal,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def plane_ellipsoid(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
plane1: Plane,
ellipsoid2: Ellipsoid,
wid: int32,
margin_plus_gap: float32,
margin: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distance, position, normal = collide_plane_ellipsoid(
plane1.normal, plane1.pos, ellipsoid2.pos, ellipsoid2.rot, ellipsoid2.size
)
# Add the active contact to the global contacts arrays
add_single_contact(
model_max_contacts,
world_max_contacts,
wid,
plane1.gid,
ellipsoid2.gid,
plane1.bid,
ellipsoid2.bid,
margin_plus_gap,
margin,
distance,
position,
normal,
friction,
restitution,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
@wp.func
def plane_capsule(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
plane1: Plane,
capsule2: Capsule,
wid: int32,
threshold: float32,
rest_offset: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
# Note: collide_plane_capsule returns a contact frame, not individual normals
distances, positions, frame = collide_plane_capsule(
plane1.normal, plane1.pos, capsule2.pos, capsule2.axis, capsule2.radius, capsule2.half_length
)
# Manually add contacts since plane_capsule returns a contact frame instead of normals
# Count valid contacts
num_contacts = int32(0)
for k in range(2):
if distances[k] != wp.inf and distances[k] <= threshold:
num_contacts += 1
# Skip operation if no contacts were detected
if num_contacts == 0:
return
# Extract normal from the contact frame (first column)
normal = vec3f(frame[0, 0], frame[1, 0], frame[2, 0])
# Perform A/B geom and body assignment
# NOTE: We want the normal to always point from A to B,
# and hence body B to be the "effected" body in the contact
# so we have to ensure that bid_B is always non-negative
if capsule2.bid < 0:
gid_AB = vec2i(capsule2.gid, plane1.gid)
bid_AB = vec2i(capsule2.bid, plane1.bid)
normal = -normal
else:
gid_AB = vec2i(plane1.gid, capsule2.gid)
bid_AB = vec2i(plane1.bid, capsule2.bid)
# Increment the active contact counter
mcio = wp.atomic_add(contact_model_num, 0, num_contacts)
wcio = wp.atomic_add(contact_world_num, wid, num_contacts)
# Retrieve the maximum number of contacts that can be stored
max_num_contacts = wp.min(wp.min(model_max_contacts - mcio, world_max_contacts - wcio), num_contacts)
# Create the common properties shared by all contacts in the current set
q_frame = wp.quat_from_matrix(make_contact_frame_znorm(normal))
material = vec2f(friction, restitution)
key = build_pair_key2(uint32(gid_AB[0]), uint32(gid_AB[1]))
# Add generated contacts data to the output arrays
active_contact_idx = int32(0)
for k in range(2):
# Break if we've reached the maximum number of contacts
if active_contact_idx >= max_num_contacts:
break
# If contact is valid, store it
if distances[k] != wp.inf and distances[k] <= threshold:
# Compute the global contact index
mcid = mcio + active_contact_idx
# Get contact data
distance = distances[k]
position = vec3f(positions[k, 0], positions[k, 1], positions[k, 2])
distance_abs = wp.abs(distance)
# Offset contact point by penetration depth
position_A = position + 0.5 * normal * distance_abs
position_B = position - 0.5 * normal * distance_abs
# Generate the gap-function and coordinate frame for this contact
gapfunc = vec4f(normal.x, normal.y, normal.z, distance - rest_offset)
# Store contact data
contact_wid[mcid] = wid
contact_cid[mcid] = wcio + active_contact_idx
contact_gid_AB[mcid] = gid_AB
contact_bid_AB[mcid] = bid_AB
contact_position_A[mcid] = position_A
contact_position_B[mcid] = position_B
contact_gapfunc[mcid] = gapfunc
contact_frame[mcid] = q_frame
contact_material[mcid] = material
contact_key[mcid] = key
# Increment active contact index
active_contact_idx += 1
@wp.func
def plane_cylinder(
# Inputs:
model_max_contacts: int32,
world_max_contacts: int32,
plane1: Plane,
cylinder2: Cylinder,
wid: int32,
threshold: float32,
rest_offset: float32,
friction: float32,
restitution: float32,
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Use the tested collision calculation from collision_primitive.py
distances, positions, normal = collide_plane_cylinder(
plane1.normal, plane1.pos, cylinder2.pos, cylinder2.axis, cylinder2.radius, cylinder2.half_height
)
# Add the active contacts to the global contacts arrays (up to 4 contacts with shared normal)
wp.static(make_add_multiple_contacts(4, True))(
model_max_contacts,
world_max_contacts,
wid,
plane1.gid,
cylinder2.gid,
plane1.bid,
cylinder2.bid,
threshold,
rest_offset,
friction,
restitution,
distances,
positions,
normal,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
###
# Kernels
###
@wp.kernel
def _primitive_narrowphase(
# Inputs
default_gap: float32,
geom_bid: wp.array(dtype=int32),
geom_sid: wp.array(dtype=int32),
geom_mid: wp.array(dtype=int32),
geom_params: wp.array(dtype=vec4f),
geom_gap: wp.array(dtype=float32),
geom_margin: wp.array(dtype=float32),
geom_pose: wp.array(dtype=transformf),
candidate_model_num_pairs: wp.array(dtype=int32),
candidate_wid: wp.array(dtype=int32),
candidate_geom_pair: wp.array(dtype=vec2i),
contact_model_max_num: wp.array(dtype=int32),
contact_world_max_num: wp.array(dtype=int32),
material_restitution: wp.array(dtype=float32),
material_static_friction: wp.array(dtype=float32),
material_dynamic_friction: wp.array(dtype=float32),
material_pair_restitution: wp.array(dtype=float32),
material_pair_static_friction: wp.array(dtype=float32),
material_pair_dynamic_friction: wp.array(dtype=float32),
# Outputs:
contact_model_num: wp.array(dtype=int32),
contact_world_num: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gid_AB: wp.array(dtype=vec2i),
contact_bid_AB: wp.array(dtype=vec2i),
contact_position_A: wp.array(dtype=vec3f),
contact_position_B: wp.array(dtype=vec3f),
contact_gapfunc: wp.array(dtype=vec4f),
contact_frame: wp.array(dtype=quatf),
contact_material: wp.array(dtype=vec2f),
contact_key: wp.array(dtype=uint64),
):
# Retrieve the geom-pair index (gpid) from the thread grid
gpid = wp.tid()
# Skip if the thread id is greater than the number of pairs
if gpid >= candidate_model_num_pairs[0]:
return
# Retrieve the world index
wid = candidate_wid[gpid]
# Retrieve the maximum number of contacts allocated
model_max_contacts = contact_model_max_num[0]
world_max_contacts = contact_world_max_num[wid]
# Retrieve the geometry indices
geom_pair = candidate_geom_pair[gpid]
gid1 = geom_pair[0]
gid2 = geom_pair[1]
bid1 = geom_bid[gid1]
sid1 = geom_sid[gid1]
mid1 = geom_mid[gid1]
params1 = geom_params[gid1]
gap1 = geom_gap[gid1]
margin1 = geom_margin[gid1]
pose1 = geom_pose[gid1]
bid2 = geom_bid[gid2]
sid2 = geom_sid[gid2]
mid2 = geom_mid[gid2]
params2 = geom_params[gid2]
gap2 = geom_gap[gid2]
margin2 = geom_margin[gid2]
pose2 = geom_pose[gid2]
# Pairwise additive rest offset (margin) determines resting separation
margin_12 = margin1 + margin2
# Effective detection threshold: margin + gap (contacts accepted when
# surface_distance <= margin_12 + gap_12)
contact_gap_12 = wp.max(default_gap, gap1) + wp.max(default_gap, gap2)
threshold_12 = margin_12 + contact_gap_12
# Retrieve the material properties for the geom pair
restitution_12, _, mu_12 = wp.static(make_get_material_pair_properties())(
mid1,
mid2,
material_restitution,
material_static_friction,
material_dynamic_friction,
material_pair_restitution,
material_pair_static_friction,
material_pair_dynamic_friction,
)
# TODO(team): static loop unrolling to remove unnecessary branching
if sid1 == GeoType.SPHERE and sid2 == GeoType.SPHERE:
sphere_sphere(
model_max_contacts,
world_max_contacts,
make_sphere(pose1, params1, gid1, bid1),
make_sphere(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.SPHERE and sid2 == GeoType.CYLINDER:
sphere_cylinder(
model_max_contacts,
world_max_contacts,
make_sphere(pose1, params1, gid1, bid1),
make_cylinder(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.SPHERE and sid2 == GeoType.CONE:
sphere_cone()
elif sid1 == GeoType.SPHERE and sid2 == GeoType.CAPSULE:
sphere_capsule(
model_max_contacts,
world_max_contacts,
make_sphere(pose1, params1, gid1, bid1),
make_capsule(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.SPHERE and sid2 == GeoType.BOX:
sphere_box(
model_max_contacts,
world_max_contacts,
make_sphere(pose1, params1, gid1, bid1),
make_box(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.SPHERE and sid2 == GeoType.ELLIPSOID:
sphere_ellipsoid()
elif sid1 == GeoType.CYLINDER and sid2 == GeoType.CYLINDER:
cylinder_cylinder()
elif sid1 == GeoType.CYLINDER and sid2 == GeoType.CONE:
cylinder_cone()
elif sid1 == GeoType.CYLINDER and sid2 == GeoType.CAPSULE:
cylinder_capsule()
elif sid1 == GeoType.CYLINDER and sid2 == GeoType.BOX:
cylinder_box()
elif sid1 == GeoType.CYLINDER and sid2 == GeoType.ELLIPSOID:
cylinder_ellipsoid()
elif sid1 == GeoType.CONE and sid2 == GeoType.CONE:
cone_cone()
elif sid1 == GeoType.CONE and sid2 == GeoType.CAPSULE:
cone_capsule()
elif sid1 == GeoType.CONE and sid2 == GeoType.BOX:
cone_box()
elif sid1 == GeoType.CONE and sid2 == GeoType.ELLIPSOID:
cone_ellipsoid()
elif sid1 == GeoType.CAPSULE and sid2 == GeoType.CAPSULE:
capsule_capsule(
model_max_contacts,
world_max_contacts,
make_capsule(pose1, params1, gid1, bid1),
make_capsule(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.CAPSULE and sid2 == GeoType.BOX:
capsule_box(
model_max_contacts,
world_max_contacts,
make_capsule(pose1, params1, gid1, bid1),
make_box(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.CAPSULE and sid2 == GeoType.ELLIPSOID:
capsule_ellipsoid()
elif sid1 == GeoType.BOX and sid2 == GeoType.BOX:
box_box(
model_max_contacts,
world_max_contacts,
make_box(pose1, params1, gid1, bid1),
make_box(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.BOX and sid2 == GeoType.ELLIPSOID:
box_ellipsoid()
elif sid1 == GeoType.ELLIPSOID and sid2 == GeoType.ELLIPSOID:
ellipsoid_ellipsoid()
# Plane collisions (plane is always geometry 1, other shapes are geometry 2)
elif sid1 == GeoType.PLANE and sid2 == GeoType.SPHERE:
plane_sphere(
model_max_contacts,
world_max_contacts,
make_plane(pose1, params1, gid1, bid1),
make_sphere(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.PLANE and sid2 == GeoType.BOX:
plane_box(
model_max_contacts,
world_max_contacts,
make_plane(pose1, params1, gid1, bid1),
make_box(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.PLANE and sid2 == GeoType.ELLIPSOID:
plane_ellipsoid(
model_max_contacts,
world_max_contacts,
make_plane(pose1, params1, gid1, bid1),
make_ellipsoid(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.PLANE and sid2 == GeoType.CAPSULE:
plane_capsule(
model_max_contacts,
world_max_contacts,
make_plane(pose1, params1, gid1, bid1),
make_capsule(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
elif sid1 == GeoType.PLANE and sid2 == GeoType.CYLINDER:
plane_cylinder(
model_max_contacts,
world_max_contacts,
make_plane(pose1, params1, gid1, bid1),
make_cylinder(pose2, params2, gid2, bid2),
wid,
threshold_12,
margin_12,
mu_12,
restitution_12,
contact_model_num,
contact_world_num,
contact_wid,
contact_cid,
contact_gid_AB,
contact_bid_AB,
contact_position_A,
contact_position_B,
contact_gapfunc,
contact_frame,
contact_material,
contact_key,
)
###
# Kernel Launcher
###
def primitive_narrowphase(
model: ModelKamino,
data: DataKamino,
candidates: CollisionCandidatesData,
contacts: ContactsKaminoData,
default_gap: float | None = None,
):
"""
Launches the narrow-phase collision detection kernel optimized for primitive shapes.
Args:
model: The model containing the collision geometries.
data: The data containing the current state of the geometries.
candidates: The collision container holding collision pairs.
contacts: The contacts container to store detected contacts.
default_gap: Default detection gap [m] applied as a floor to per-geometry gaps.
If None, ``0.0`` is used.
"""
if default_gap is None:
default_gap = 0.0
if not isinstance(default_gap, float):
raise TypeError("default_gap must be of type `float`")
wp.launch(
_primitive_narrowphase,
dim=candidates.num_model_geom_pairs,
inputs=[
float32(default_gap),
model.geoms.bid,
model.geoms.type,
model.geoms.material,
model.geoms.params,
model.geoms.gap,
model.geoms.margin,
data.geoms.pose,
candidates.model_num_collisions,
candidates.wid,
candidates.geom_pair,
contacts.model_max_contacts,
contacts.world_max_contacts,
model.materials.restitution,
model.materials.static_friction,
model.materials.dynamic_friction,
model.material_pairs.restitution,
model.material_pairs.static_friction,
model.material_pairs.dynamic_friction,
],
outputs=[
contacts.model_active_contacts,
contacts.world_active_contacts,
contacts.wid,
contacts.cid,
contacts.gid_AB,
contacts.bid_AB,
contacts.position_A,
contacts.position_B,
contacts.gapfunc,
contacts.frame,
contacts.material,
contacts.key,
],
device=model.device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/primitive/pipeline.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
A collision detection pipeline optimized for primitive shapes.
This pipeline uses an `explicit` broad-phase operating on pre-computed
geometry pairs and a narrow-phase based on the primitive colliders of Newton.
"""
from __future__ import annotations
from typing import Literal
import numpy as np
import warp as wp
from ......geometry.types import GeoType
from ...core.data import DataKamino
from ...core.model import ModelKamino
from ...core.state import StateKamino
from ...core.types import float32, int32, vec6f
from ..contacts import DEFAULT_GEOM_PAIR_CONTACT_GAP, ContactsKamino
from .broadphase import (
PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES,
BoundingVolumesData,
BoundingVolumeType,
CollisionCandidatesData,
CollisionCandidatesModel,
primitive_broadphase_explicit,
)
from .narrowphase import PRIMITIVE_NARROWPHASE_SUPPORTED_SHAPE_PAIRS, primitive_narrowphase
###
# Interfaces
###
class CollisionPipelinePrimitive:
"""
A collision detection pipeline optimized for primitive shapes.
This pipeline uses an `explicit` broad-phase operating on pre-computed
geometry pairs and a narrow-phase based on the primitive colliders of Newton.
"""
def __init__(
self,
model: ModelKamino | None = None,
bvtype: Literal["aabb", "bs"] = "aabb",
default_gap: float = DEFAULT_GEOM_PAIR_CONTACT_GAP,
device: wp.DeviceLike = None,
):
"""
Initialize an instance of Kamino's optimized primitive collision detection pipeline.
Args:
model (`ModelKamino`, optional):
The model container holding the time-invariant data of the system being simulated.\n
If provided, the detector will be finalized using the provided model and settings.\n
If `None`, the detector will be created empty without allocating data, and
can be finalized later by providing a model to the `finalize` method.\n
bvtype (`Literal["aabb", "bs"]`, optional):
Type of bounding volume to use in broad-phase.
default_gap (`float`, optional):
Default detection gap [m] applied as a floor to per-geometry gaps.
device (`wp.DeviceLike`, optional):
The target Warp device for allocation and execution.\n
If `None`, the `model.device` will be used if a model is provided, otherwise
it will default to the device preferred by Warp on the given platform.
"""
# Cache the model reference, target device and settings
self._model: ModelKamino | None = model
self._default_gap: float = default_gap
self._device: wp.DeviceLike = device
# Convert the bounding volume type from string to enum if necessary
self._bvtype: BoundingVolumeType = BoundingVolumeType.from_string(bvtype)
# Declare the internal data containers
self._cmodel: CollisionCandidatesModel | None = None
self._cdata: CollisionCandidatesData | None = None
self._bvdata: BoundingVolumesData | None = None
# If a builder is provided, proceed to finalize all data allocations
if model is not None:
self.finalize(model, bvtype, device)
###
# Properties
###
@property
def device(self) -> wp.DeviceLike:
"""Returns the Warp device the pipeline operates on."""
return self._device
###
# Operations
###
def finalize(
self,
model: ModelKamino,
bvtype: Literal["aabb", "bs"] | None = None,
device: wp.DeviceLike = None,
):
"""
Finalizes the collision detection pipeline by allocating all necessary data structures.
Args:
model (`ModelKamino`, optional):
The model container holding the time-invariant data of the system being simulated.\n
If provided, the detector will be finalized using the provided model and settings.\n
If `None`, the detector will be created empty without allocating data, and
can be finalized later by providing a model to the `finalize` method.\n
bvtype (`Literal["aabb", "bs"]`, optional):
Type of bounding volume to use in broad-phase.
device (`wp.DeviceLike`, optional):
The target Warp device for allocation and execution.\n
If `None`, the `model.device` will be used if a model is provided, otherwise
it will default to the device preferred by Warp on the given platform.
"""
# Override the model if specified
if model is not None:
self._model = model
if self._model is None:
raise ValueError("Model must be provided to finalize the CollisionPipelinePrimitive.")
elif not isinstance(self._model, ModelKamino):
raise TypeError("CollisionPipelinePrimitive only supports models of type ModelKamino.")
# Override the device if specified
if device is not None:
self._device = device
# Override the device if specified
if bvtype is not None:
self._bvtype = BoundingVolumeType.from_string(bvtype)
# Retrieve the number of world
num_worlds = self._model.size.num_worlds
num_geoms = self._model.geoms.num_geoms
# Ensure that all shape types are supported by the primitive
# broad-phase and narrow-phase back-ends before proceeding
world_num_geom_pairs, geom_pair_wid = self._assert_shapes_supported(self._model)
# Allocate the collision model data
with wp.ScopedDevice(self._device):
# Allocate the bounding volumes data
self._bvdata = BoundingVolumesData()
match self._bvtype:
case BoundingVolumeType.AABB:
self._bvdata.aabb = wp.zeros(shape=(num_geoms,), dtype=vec6f)
case BoundingVolumeType.BS:
self._bvdata.radius = wp.zeros(shape=(num_geoms,), dtype=float32)
case _:
raise ValueError(f"Unsupported BoundingVolumeType: {self._bvtype}")
# Allocate the time-invariant collision candidates model
self._cmodel = CollisionCandidatesModel(
num_model_geom_pairs=self._model.geoms.num_collidable_pairs,
num_world_geom_pairs=world_num_geom_pairs,
model_num_pairs=wp.array([self._model.geoms.num_collidable_pairs], dtype=int32),
world_num_pairs=wp.array(world_num_geom_pairs, dtype=int32),
wid=wp.array(geom_pair_wid, dtype=int32),
geom_pair=self._model.geoms.collidable_pairs,
)
# Allocate the time-varying collision candidates data
self._cdata = CollisionCandidatesData(
num_model_geom_pairs=self._model.geoms.num_collidable_pairs,
model_num_collisions=wp.zeros(shape=(1,), dtype=int32),
world_num_collisions=wp.zeros(shape=(num_worlds,), dtype=int32),
wid=wp.zeros(shape=(self._model.geoms.num_collidable_pairs,), dtype=int32),
geom_pair=wp.zeros_like(self._model.geoms.collidable_pairs),
)
def collide(self, data: DataKamino, state: StateKamino, contacts: ContactsKamino):
"""
Runs the unified collision detection pipeline to generate discrete contacts.
Args:
data (DataKamino):
The data container holding internal time-varying state of the solver.
state (StateKamino):
The state container holding the time-varying state of the simulation.
contacts (ContactsKamino):
Output contacts container (will be cleared and populated)
"""
# Ensure that the pipeline has been finalized
# before proceeding with actual operations
self._assert_finalized()
# Clear all active collision candidates and contacts
self._cdata.clear()
contacts.clear()
# Perform the broad-phase collision detection to generate candidate pairs
primitive_broadphase_explicit(
body_poses=state.q_i,
geoms_model=self._model.geoms,
geoms_data=data.geoms,
bv_type=self._bvtype,
bv_data=self._bvdata,
candidates_model=self._cmodel,
candidates_data=self._cdata,
default_gap=self._default_gap,
)
# Perform the narrow-phase collision detection to generate active contacts
primitive_narrowphase(self._model, data, self._cdata, contacts, default_gap=self._default_gap)
###
# Internals
###
def _assert_finalized(self):
"""
Asserts that the collision detection pipeline has been finalized.
Raises:
RuntimeError: If the pipeline has not been finalized.
"""
if self._cmodel is None or self._cdata is None or self._bvdata is None:
raise RuntimeError(
"CollisionPipelinePrimitive has not been finalized. "
"Please call `finalize(builder, device)` before using the pipeline."
)
@staticmethod
def _assert_shapes_supported(model: ModelKamino, skip_checks: bool = False) -> tuple[list[int], np.ndarray]:
"""
Checks whether all collidable geometries in the provided
model are supported by the primitive narrow-phase collider.
Args:
model (ModelKamino):
The model container holding the time-invariant parameters of the simulation.
Raises:
ValueError: If any unsupported shape type is found.
"""
# Iterate over each candidate geometry pair
geom_type = model.geoms.type.numpy()
geom_wid = model.geoms.wid.numpy()
geom_pairs = model.geoms.collidable_pairs.numpy()
world_num_geom_pairs: list[int] = [0] * model.size.num_worlds
geom_pair_wid: np.ndarray = np.zeros(shape=(geom_pairs.shape[0],), dtype=np.int32)
for gid_12 in range(geom_pairs.shape[0]):
# Retrieve the shape types and world indices of the geometry pair
gid_1 = geom_pairs[gid_12, 0]
gid_2 = geom_pairs[gid_12, 1]
shape_1 = GeoType(geom_type[gid_1])
shape_2 = GeoType(geom_type[gid_2])
candidate_pair = (min((shape_1, shape_2)), max((shape_1, shape_2)))
# First check if both shapes are supported by the primitive broad-phase
if not skip_checks and shape_1 not in PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES:
raise ValueError(
f"Builder contains shape '{shape_1}' which is currently not supported by the primitive broad-phase."
"\nPlease consider using the `UNIFIED` collision pipeline, or using alternative shape types."
)
if not skip_checks and shape_2 not in PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES:
raise ValueError(
f"Builder contains shape '{shape_2}' which is currently not supported by the primitive broad-phase."
"\nPlease consider using the `UNIFIED` collision pipeline, or using alternative shape types."
)
# Then check if the shape-pair combination is supported by the primitive narrow-phase
if not skip_checks and candidate_pair not in PRIMITIVE_NARROWPHASE_SUPPORTED_SHAPE_PAIRS:
raise ValueError(
f"Builder contains shape-pair `{candidate_pair}` with pair index `{gid_12}`, "
"but it is currently not supported by the primitive narrow-phase."
"\nPlease consider using the `UNIFIED` collision pipeline, or using alternative shape types."
)
# Store the world index for this geometry pair
geom_pair_12_wid = geom_wid[gid_1]
geom_pair_wid[gid_12] = geom_pair_12_wid
world_num_geom_pairs[geom_pair_12_wid] += 1
# Return the per-world geometry pair counts and the per-geom-pair world indices
return world_num_geom_pairs, geom_pair_wid
================================================
FILE: newton/_src/solvers/kamino/_src/geometry/unified.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides a specialization of Newton's unified collision-detection pipeline for Kamino.
This module provides interfaces and data-conversion specializations for Kamino that wraps
the broad-phase and narrow-phase of Newton's CollisionPipelineUnified, writing generated
contacts data directly into Kamino's respective format.
"""
from typing import Literal
import warp as wp
# Newton imports
from .....geometry.broad_phase_nxn import BroadPhaseAllPairs, BroadPhaseExplicit
from .....geometry.broad_phase_sap import BroadPhaseSAP
from .....geometry.collision_core import compute_tight_aabb_from_support
from .....geometry.contact_data import ContactData
from .....geometry.flags import ShapeFlags
from .....geometry.narrow_phase import NarrowPhase
from .....geometry.sdf_texture import TextureSDFData
from .....geometry.support_function import GenericShapeData, SupportMapDataProvider, pack_mesh_ptr
from .....geometry.types import GeoType
# Kamino imports
from ..core.data import DataKamino
from ..core.materials import DEFAULT_FRICTION, DEFAULT_RESTITUTION, make_get_material_pair_properties
from ..core.model import ModelKamino
from ..core.state import StateKamino
from ..core.types import float32, int32, quatf, transformf, uint32, uint64, vec2f, vec2i, vec3f, vec4f
from ..geometry.contacts import (
DEFAULT_GEOM_PAIR_CONTACT_GAP,
DEFAULT_GEOM_PAIR_MAX_CONTACTS,
ContactsKamino,
make_contact_frame_znorm,
)
from ..geometry.keying import build_pair_key2
from ..utils import logger as _msg
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
@wp.struct
class ContactWriterDataKamino:
"""Contact writer data for writing contacts directly in Kamino format."""
# Contact limits
model_max_contacts: int32
world_max_contacts: wp.array(dtype=int32)
# Geometry information arrays
geom_wid: wp.array(dtype=int32) # World ID for each geometry
geom_bid: wp.array(dtype=int32) # Body ID for each geometry
geom_mid: wp.array(dtype=int32) # Material ID for each geometry
geom_gap: wp.array(dtype=float32) # Detection gap for each geometry [m]
# Material properties (indexed by material pair)
material_restitution: wp.array(dtype=float32)
material_static_friction: wp.array(dtype=float32)
material_dynamic_friction: wp.array(dtype=float32)
material_pair_restitution: wp.array(dtype=float32)
material_pair_static_friction: wp.array(dtype=float32)
material_pair_dynamic_friction: wp.array(dtype=float32)
# Contact limit and active count (Newton interface)
contact_max: int32
contact_count: wp.array(dtype=int32)
# Output arrays (Kamino Contacts format)
contacts_model_num_active: wp.array(dtype=int32)
contacts_world_num_active: wp.array(dtype=int32)
contact_wid: wp.array(dtype=int32)
contact_cid: wp.array(dtype=int32)
contact_gid_AB: wp.array(dtype=vec2i)
contact_bid_AB: wp.array(dtype=vec2i)
contact_position_A: wp.array(dtype=vec3f)
contact_position_B: wp.array(dtype=vec3f)
contact_gapfunc: wp.array(dtype=vec4f)
contact_frame: wp.array(dtype=quatf)
contact_material: wp.array(dtype=vec2f)
contact_key: wp.array(dtype=uint64)
###
# Functions
###
@wp.func
def convert_geom_params_to_newton_scale(geo_type: int32, params: vec4f) -> vec3f:
"""
Convert geometry params to Newton shape scale.
Since Kamino now stores Newton's :class:`GeoType` and parameter convention
directly, this is mostly a passthrough. Special cases are
:attr:`GeoType.PLANE` (params hold the plane normal, not a scale) and
:attr:`GeoType.MESH` / :attr:`GeoType.HFIELD` (scale is unused).
:attr:`GeoType.CONVEX_MESH` retains its per-mesh scale from params.
Args:
geo_type: The Newton :class:`GeoType` integer value.
params: Shape parameters as :class:`vec4f` (Newton convention).
Returns:
The Newton shape scale as :class:`vec3f`.
"""
scale = vec3f(params[0], params[1], params[2])
if geo_type == GeoType.PLANE:
scale = vec3f(0.0, 0.0, 0.0)
elif geo_type == GeoType.MESH or geo_type == GeoType.HFIELD:
scale = vec3f(0.0, 0.0, 0.0)
return scale
@wp.func
def write_contact_unified_kamino(
contact_data: ContactData,
writer_data: ContactWriterDataKamino,
output_index: int,
):
"""
Write a contact to Kamino-compatible output arrays.
This function is used as a custom contact writer for NarrowPhase.launch_custom_write().
It converts ContactData from the narrow phase directly to Kamino's contact format,
using the same distance computation as Newton core's ``write_contact``.
Args:
contact_data: ContactData struct from narrow phase containing contact information.
writer_data: ContactWriterDataKamino struct containing output arrays.
output_index: If < 0, apply gap-based filtering before writing.
If >= 0, skip filtering (narrowphase already validated the contact).
In both cases the model-level index is allocated from
:attr:`ContactWriterDataKamino.contacts_model_num_active`.
"""
contact_normal_a_to_b = wp.normalize(contact_data.contact_normal_a_to_b)
# After narrow-phase post-processing (collision_core.py), contact_distance
# is always the surface-to-surface signed distance regardless of kernel
# (primitive or GJK), and contact_point_center is the midpoint between
# the surface contact points on each shape.
half_d = 0.5 * contact_data.contact_distance
a_contact_world = contact_data.contact_point_center - contact_normal_a_to_b * half_d
b_contact_world = contact_data.contact_point_center + contact_normal_a_to_b * half_d
# Margin-shifted signed distance (negative = penetrating beyond margin)
d = contact_data.contact_distance - (contact_data.margin_a + contact_data.margin_b)
# Determine world ID — global shapes (wid=-1) can collide with any world,
# so fall back to the other shape's world when one is global.
wid_a = writer_data.geom_wid[contact_data.shape_a]
wid_b = writer_data.geom_wid[contact_data.shape_b]
wid = wid_a
if wid_a < 0:
wid = wid_b
world_max_contacts = writer_data.world_max_contacts[wid]
if output_index < 0:
# Use per-shape detection gap (additive, matching Newton core)
gap_a = writer_data.geom_gap[contact_data.shape_a]
gap_b = writer_data.geom_gap[contact_data.shape_b]
contact_gap = gap_a + gap_b
if d > contact_gap:
return
# Always allocate from the model-level counter so the active count
# stays accurate regardless of whether the narrowphase pre-allocated
# an output_index (primitive kernel) or left it to the writer (-1).
mcid = wp.atomic_add(writer_data.contacts_model_num_active, 0, 1)
if mcid >= writer_data.model_max_contacts:
wp.atomic_sub(writer_data.contacts_model_num_active, 0, 1)
return
# Atomically increment the world-specific contact counter and
# roll-back the atomic add if the respective limit is exceeded
wcid = wp.atomic_add(writer_data.contacts_world_num_active, wid, 1)
if wcid >= world_max_contacts:
wp.atomic_sub(writer_data.contacts_world_num_active, wid, 1)
return
# Retrieve the geom/body/material indices
gid_a = contact_data.shape_a
gid_b = contact_data.shape_b
bid_a = writer_data.geom_bid[contact_data.shape_a]
bid_b = writer_data.geom_bid[contact_data.shape_b]
mid_a = writer_data.geom_mid[contact_data.shape_a]
mid_b = writer_data.geom_mid[contact_data.shape_b]
# Ensure the static body is always body A so that the normal
# always points from A to B and bid_B is non-negative
if bid_b < 0:
gid_AB = vec2i(gid_b, gid_a)
bid_AB = vec2i(bid_b, bid_a)
normal = -contact_normal_a_to_b
pos_A = b_contact_world
pos_B = a_contact_world
else:
gid_AB = vec2i(gid_a, gid_b)
bid_AB = vec2i(bid_a, bid_b)
normal = contact_normal_a_to_b
pos_A = a_contact_world
pos_B = b_contact_world
# Retrieve the material properties for the geom pair
restitution_ab, _, mu_ab = wp.static(make_get_material_pair_properties())(
mid_a,
mid_b,
writer_data.material_restitution,
writer_data.material_static_friction,
writer_data.material_dynamic_friction,
writer_data.material_pair_restitution,
writer_data.material_pair_static_friction,
writer_data.material_pair_dynamic_friction,
)
material = vec2f(mu_ab, restitution_ab)
# Generate the gap-function (normal.x, normal.y, normal.z, distance),
# contact frame (z-norm aligned with contact normal)
gapfunc = vec4f(normal[0], normal[1], normal[2], d)
q_frame = wp.quat_from_matrix(make_contact_frame_znorm(normal))
key = build_pair_key2(uint32(gid_AB[0]), uint32(gid_AB[1]))
# Store contact data in Kamino format
writer_data.contact_wid[mcid] = wid
writer_data.contact_cid[mcid] = wcid
writer_data.contact_gid_AB[mcid] = gid_AB
writer_data.contact_bid_AB[mcid] = bid_AB
writer_data.contact_position_A[mcid] = pos_A
writer_data.contact_position_B[mcid] = pos_B
writer_data.contact_gapfunc[mcid] = gapfunc
writer_data.contact_frame[mcid] = q_frame
writer_data.contact_material[mcid] = material
writer_data.contact_key[mcid] = key
###
# Kernels
###
@wp.func
def _compute_collision_radius(geo_type: int32, scale: vec3f) -> float32:
"""Compute the bounding-sphere radius for broadphase AABB fallback.
Mirrors :func:`newton._src.geometry.utils.compute_shape_radius` for the
primitive shape types that Kamino currently supports.
"""
radius = float32(10.0)
if geo_type == GeoType.SPHERE:
radius = scale[0]
elif geo_type == GeoType.BOX:
radius = wp.length(scale)
elif geo_type == GeoType.CAPSULE or geo_type == GeoType.CYLINDER or geo_type == GeoType.CONE:
radius = scale[0] + scale[1]
elif geo_type == GeoType.ELLIPSOID:
radius = wp.max(wp.max(scale[0], scale[1]), scale[2])
elif geo_type == GeoType.PLANE:
if scale[0] > 0.0 and scale[1] > 0.0:
radius = wp.length(scale)
else:
radius = float32(1.0e6)
return radius
@wp.kernel
def _convert_geom_data_kamino_to_newton(
# Inputs:
default_gap: float32,
geom_sid: wp.array(dtype=int32),
geom_params: wp.array(dtype=vec4f),
geom_margin: wp.array(dtype=float32),
# Outputs:
geom_gap: wp.array(dtype=float32),
geom_type: wp.array(dtype=int32),
geom_data: wp.array(dtype=vec4f),
shape_collision_radius: wp.array(dtype=float32),
):
"""
Converts Kamino geometry data to Newton-compatible format.
Converts geometry params to Newton scale, stores the per-geometry surface
margin offset in ``geom_data.w``, applies a default floor to the
per-geometry detection gap, and computes the bounding-sphere radius used
for AABB fallback (planes, meshes, heightfields).
"""
# Retrieve the geometry index from the thread grid
gid = wp.tid()
# Retrieve the geom-specific data
sid = geom_sid[gid]
params = geom_params[gid]
margin = geom_margin[gid]
gap = geom_gap[gid]
# Convert params to Newton scale (identity for most types; special-cased for planes/meshes)
scale = convert_geom_params_to_newton_scale(sid, params)
# Store converted geometry data
# NOTE: the per-geom margin is overridden because
# the unified pipeline needs it during narrow-phase
geom_type[gid] = sid
geom_data[gid] = vec4f(scale[0], scale[1], scale[2], margin)
geom_gap[gid] = wp.max(default_gap, gap)
shape_collision_radius[gid] = _compute_collision_radius(sid, scale)
@wp.kernel
def _update_geom_poses_and_compute_aabbs(
# Inputs:
geom_type: wp.array(dtype=int32),
geom_data: wp.array(dtype=vec4f),
geom_bid: wp.array(dtype=int32),
geom_ptr: wp.array(dtype=wp.uint64),
geom_offset: wp.array(dtype=transformf),
geom_margin: wp.array(dtype=float32),
geom_gap: wp.array(dtype=float32),
shape_collision_radius: wp.array(dtype=float32),
body_pose: wp.array(dtype=transformf),
# Outputs:
geom_pose: wp.array(dtype=transformf),
shape_aabb_lower: wp.array(dtype=vec3f),
shape_aabb_upper: wp.array(dtype=vec3f),
):
"""
Updates the pose of each Kamino geometry in world coordinates and computes its AABB.
AABBs are enlarged by the per-shape ``margin + gap`` to ensure the broadphase
catches all contacts within the detection threshold.
"""
gid = wp.tid()
geo_type = geom_type[gid]
geo_data = geom_data[gid]
bid = geom_bid[gid]
margin = geom_margin[gid]
gap = geom_gap[gid]
X_bg = geom_offset[gid]
X_b = wp.transform_identity(dtype=float32)
if bid > -1:
X_b = body_pose[bid]
X_g = wp.transform_multiply(X_b, X_bg)
r_g = wp.transform_get_translation(X_g)
q_g = wp.transform_get_rotation(X_g)
# Format is (vec3f scale, float32 margin_offset)
scale = vec3f(geo_data[0], geo_data[1], geo_data[2])
# Enlarge AABB by margin + gap per shape (matching Newton core convention)
expansion = margin + gap
margin_vec = wp.vec3(expansion, expansion, expansion)
# Check if this is an infinite plane or mesh - use bounding sphere fallback
is_infinite_plane = (geo_type == GeoType.PLANE) and (scale[0] == 0.0 and scale[1] == 0.0)
is_mesh = geo_type == GeoType.MESH
is_hfield = geo_type == GeoType.HFIELD
# Compute the geometry AABB in world coordinates
aabb_lower = wp.vec3(0.0)
aabb_upper = wp.vec3(0.0)
if is_infinite_plane or is_mesh or is_hfield:
# Use conservative bounding sphere approach
radius = shape_collision_radius[gid]
half_extents = wp.vec3(radius, radius, radius)
aabb_lower = r_g - half_extents - margin_vec
aabb_upper = r_g + half_extents + margin_vec
else:
# Use support function to compute tight AABB
# Create generic shape data
shape_data = GenericShapeData()
shape_data.shape_type = geo_type
shape_data.scale = scale
shape_data.auxiliary = wp.vec3(0.0, 0.0, 0.0)
# For CONVEX_MESH, pack the mesh pointer
if geo_type == GeoType.CONVEX_MESH:
shape_data.auxiliary = pack_mesh_ptr(geom_ptr[gid])
# Compute tight AABB using helper function
data_provider = SupportMapDataProvider()
aabb_min_world, aabb_max_world = compute_tight_aabb_from_support(shape_data, q_g, r_g, data_provider)
aabb_lower = aabb_min_world - margin_vec
aabb_upper = aabb_max_world + margin_vec
# Store the updated geometry pose in world coordinates and computed AABB
geom_pose[gid] = X_g
shape_aabb_lower[gid] = aabb_lower
shape_aabb_upper[gid] = aabb_upper
###
# Interfaces
###
class CollisionPipelineUnifiedKamino:
"""
A specialization of the Newton's unified collision detection pipeline for Kamino.
This pipeline uses the same broad phase algorithms (NXN, SAP, EXPLICIT) and narrow phase
(NarrowPhase with GJK/MPR) as Newton's CollisionPipelineUnified, but writes contacts
directly in Kamino's format using a custom contact writer.
"""
def __init__(
self,
model: ModelKamino,
broadphase: Literal["nxn", "sap", "explicit"] = "explicit",
max_contacts: int | None = None,
max_contacts_per_pair: int = DEFAULT_GEOM_PAIR_MAX_CONTACTS,
max_triangle_pairs: int = 1_000_000,
default_gap: float = DEFAULT_GEOM_PAIR_CONTACT_GAP,
default_friction: float = DEFAULT_FRICTION,
default_restitution: float = DEFAULT_RESTITUTION,
device: wp.DeviceLike = None,
):
"""
Initialize an instance of Kamino's wrapper of the unified collision detection pipeline.
Args:
model: The Kamino model containing the geometry to perform collision detection on.
broadphase: Broad-phase back-end to use (NXN, SAP, or EXPLICIT).
max_contacts: Maximum contacts for the entire model (overrides computed value).
max_contacts_per_pair: Maximum contacts per colliding geometry pair.
max_triangle_pairs: Maximum triangle pairs for mesh/mesh and mesh/hfield collisions.
default_gap: Default detection gap [m] applied as a floor to per-geometry gaps.
default_friction: Default contact friction coefficient.
default_restitution: Default impact restitution coefficient.
device: Warp device used to allocate memory and operate on.
"""
# Cache a reference to the Kamino model
self._model: ModelKamino = model
# Determine device to use for pipeline data and computations
self._device: wp.DeviceLike = None
if device is not None:
self._device = device
else:
self._device = self._model.device
# Cache pipeline settings
self._broadphase: str = broadphase
self._default_gap: float = default_gap
self._default_friction: float = default_friction
self._default_restitution: float = default_restitution
self._max_contacts_per_pair: int = max_contacts_per_pair
self._max_triangle_pairs: int = max_triangle_pairs
# Get geometry count from model
self._num_geoms: int = self._model.geoms.num_geoms
# Compute the maximum possible number of geom pairs (worst-case, needed for NXN/SAP)
self._max_shape_pairs: int = (self._num_geoms * (self._num_geoms - 1)) // 2
self._max_contacts: int = self._max_shape_pairs * self._max_contacts_per_pair
# Override max contacts if specified explicitly
if max_contacts is not None:
self._max_contacts = max_contacts
# Build shape pairs for EXPLICIT mode
self.shape_pairs_filtered: wp.array | None = None
if broadphase == "explicit":
self.shape_pairs_filtered = self._model.geoms.collidable_pairs
self._max_shape_pairs = self._model.geoms.num_collidable_pairs
self._max_contacts = self._model.geoms.model_minimum_contacts
# Build excluded pairs for NXN/SAP broadphase filtering.
# Kamino uses a bitmask group/collides system that is more expressive than
# Newton's integer collision groups. We keep all broadphase groups at 1
# (same-group, all pairs pass group check) and instead supply an explicit
# list of excluded pairs that encodes same-body, group/collides, and
# neighbor-joint filtering.
geom_collision_group_list = [1] * self._num_geoms
self._excluded_pairs: wp.array | None = None
self._num_excluded_pairs: int = 0
if broadphase in ("nxn", "sap"):
self._excluded_pairs = self._model.geoms.excluded_pairs
self._num_excluded_pairs = self._model.geoms.num_excluded_pairs
# Capture a reference to per-geometry world indices already present in the model
self.geom_wid: wp.array = self._model.geoms.wid
# Define default shape flags for all geometries
default_shape_flag: int = (
ShapeFlags.VISIBLE # Mark as visible for debugging/visualization
| ShapeFlags.COLLIDE_SHAPES # Enable shape-shape collision
| ShapeFlags.COLLIDE_PARTICLES # Enable shape-particle collision
)
# Allocate internal data needed by the pipeline that
# the Kamino model and data do not yet provide
with wp.ScopedDevice(self._device):
self.geom_type = wp.zeros(self._num_geoms, dtype=int32)
self.geom_data = wp.zeros(self._num_geoms, dtype=vec4f)
self.geom_collision_group = wp.array(geom_collision_group_list, dtype=int32)
self.shape_collision_radius = wp.zeros(self._num_geoms, dtype=float32)
self.shape_flags = wp.full(self._num_geoms, default_shape_flag, dtype=int32)
self.shape_aabb_lower = wp.zeros(self._num_geoms, dtype=wp.vec3)
self.shape_aabb_upper = wp.zeros(self._num_geoms, dtype=wp.vec3)
self.broad_phase_pairs = wp.zeros(self._max_shape_pairs, dtype=wp.vec2i)
self.broad_phase_pair_count = wp.zeros(1, dtype=wp.int32)
self.narrow_phase_contact_count = wp.zeros(1, dtype=int32)
# TODO: These are currently left empty just to satisfy the narrow phase interface
# but we need to implement SDF/mesh/heightfield support in Kamino to make use of them.
# With has_meshes=False, these arrays are never accessed.
self.shape_sdf_data = wp.empty(shape=(0,), dtype=TextureSDFData)
self.shape_sdf_index = wp.full_like(self.geom_type, -1)
self.shape_collision_aabb_lower = wp.empty(shape=(0,), dtype=wp.vec3)
self.shape_collision_aabb_upper = wp.empty(shape=(0,), dtype=wp.vec3)
self.shape_voxel_resolution = wp.empty(shape=(0,), dtype=wp.vec3i)
self.shape_heightfield_index = None # TODO
self.heightfield_data = None # TODO
self.heightfield_elevations = None # TODO
# Initialize the broad-phase backend depending on the selected mode
match self._broadphase:
case "nxn":
self.nxn_broadphase = BroadPhaseAllPairs(self.geom_wid, shape_flags=None, device=self._device)
case "sap":
self.sap_broadphase = BroadPhaseSAP(self.geom_wid, shape_flags=None, device=self._device)
case "explicit":
self.explicit_broadphase = BroadPhaseExplicit()
case _:
raise ValueError(f"Unsupported broad phase mode: {self._broadphase}")
# Initialize narrow-phase backend with the contact writer customized for Kamino
# Note: has_meshes=False since Kamino doesn't support mesh collisions yet
self.narrow_phase = NarrowPhase(
max_candidate_pairs=self._max_shape_pairs,
max_triangle_pairs=self._max_triangle_pairs,
device=self._device,
shape_aabb_lower=self.shape_aabb_lower,
shape_aabb_upper=self.shape_aabb_upper,
contact_writer_warp_func=write_contact_unified_kamino,
has_meshes=False,
)
# Convert geometry data from Kamino to Newton format
self._convert_geometry_data()
###
# Properties
###
@property
def device(self) -> wp.DeviceLike:
"""Returns the Warp device the pipeline operates on."""
return self._device
@property
def model(self) -> ModelKamino:
"""Returns the Kamino model for which the pipeline is configured."""
return self._model
###
# Operations
###
def collide(self, data: DataKamino, state: StateKamino, contacts: ContactsKamino):
"""
Runs the unified collision detection pipeline to generate discrete contacts.
Args:
data (DataKamino): The data container holding the time-varying state of the simulation.
state (StateKamino): The state container holding the current simulation state.
contacts (ContactsKamino): Output contacts container (will be cleared and populated)
"""
# Check if contacts is allocated on the same device
if contacts.device != self._device:
raise ValueError(
f"ContactsKamino container device ({contacts.device}) "
f"does not match the CD pipeline device ({self._device})."
)
# Check if contacts can hold the maximum number of contacts.
# When max_contacts_per_world is set, the buffer is intentionally smaller
# than the theoretical maximum — excess contacts are dropped per world.
if contacts.model_max_contacts_host < self._max_contacts:
if not getattr(self, "_capacity_warning_shown", False):
_msg.warning(
f"ContactsKamino capacity ({contacts.model_max_contacts_host}) is less than "
f"the theoretical maximum ({self._max_contacts}). "
f"Per-world contact limits will cap actual contacts."
)
self._capacity_warning_shown = True
# Clear contacts
contacts.clear()
# Clear internal contact counts
self.narrow_phase_contact_count.zero_()
# Update geometry poses from body states and compute respective AABBs
self._update_geom_data(data, state)
# Run broad-phase collision detection to get candidate shape pairs
self._run_broadphase()
# Run narrow-phase collision detection to generate contacts
self._run_narrowphase(data, contacts)
###
# Internals
###
def _convert_geometry_data(self):
"""
Converts Kamino geometry data to the Newton format.
This operation needs to be called only once during initialization.
Args:
model (ModelKamino):
The model container holding the time-invariant parameters of the simulation.
"""
wp.launch(
kernel=_convert_geom_data_kamino_to_newton,
dim=self._num_geoms,
inputs=[
self._default_gap,
self._model.geoms.type,
self._model.geoms.params,
self._model.geoms.margin,
],
outputs=[
self._model.geoms.gap,
self.geom_type,
self.geom_data,
self.shape_collision_radius,
],
device=self._device,
)
def _update_geom_data(self, data: DataKamino, state: StateKamino):
"""
Updates geometry poses from corresponding body states and computes respective AABBs.
Args:
data (DataKamino):
The data container holding the time-varying state of the simulation.
state (StateKamino):
The state container holding the current simulation state.
"""
wp.launch(
kernel=_update_geom_poses_and_compute_aabbs,
dim=self._num_geoms,
inputs=[
self.geom_type,
self.geom_data,
self._model.geoms.bid,
self._model.geoms.ptr,
self._model.geoms.offset,
self._model.geoms.margin,
self._model.geoms.gap,
self.shape_collision_radius,
state.q_i,
],
outputs=[
data.geoms.pose,
self.shape_aabb_lower,
self.shape_aabb_upper,
],
device=self._device,
)
def _run_broadphase(self):
"""
Runs broad-phase collision detection to generate candidate geom/shape pairs.
"""
# First clear broad phase counter
self.broad_phase_pair_count.zero_()
# Then launch the configured broad-phase collision detection
match self._broadphase:
case "nxn":
self.nxn_broadphase.launch(
self.shape_aabb_lower,
self.shape_aabb_upper,
None, # AABBs are pre-expanded
self.geom_collision_group,
self.geom_wid,
self._num_geoms,
self.broad_phase_pairs,
self.broad_phase_pair_count,
device=self._device,
filter_pairs=self._excluded_pairs,
num_filter_pairs=self._num_excluded_pairs,
)
case "sap":
self.sap_broadphase.launch(
self.shape_aabb_lower,
self.shape_aabb_upper,
None, # AABBs are pre-expanded
self.geom_collision_group,
self.geom_wid,
self._num_geoms,
self.broad_phase_pairs,
self.broad_phase_pair_count,
device=self._device,
filter_pairs=self._excluded_pairs,
num_filter_pairs=self._num_excluded_pairs,
)
case "explicit":
self.explicit_broadphase.launch(
self.shape_aabb_lower,
self.shape_aabb_upper,
None, # AABBs are pre-expanded
self.shape_pairs_filtered,
len(self.shape_pairs_filtered),
self.broad_phase_pairs,
self.broad_phase_pair_count,
device=self._device,
)
case _:
raise ValueError(f"Unsupported broad phase mode: {self._broadphase}")
def _run_narrowphase(self, data: DataKamino, contacts: ContactsKamino):
"""
Runs narrow-phase collision detection to generate contacts.
Args:
data (DataKamino):
The data container holding the time-varying state of the simulation.
contacts (ContactsKamino):
Output contacts container (will be populated by this function)
"""
# Create a writer data struct to bundle all necessary input/output
# arrays into a single object for the narrow phase custom writer
# NOTE: Unfortunately, we need to do this on every call in python,
# but graph-capture ensures this actually happens only once
writer_data = ContactWriterDataKamino()
writer_data.model_max_contacts = int32(contacts.model_max_contacts_host)
writer_data.world_max_contacts = contacts.world_max_contacts
writer_data.geom_bid = self._model.geoms.bid
writer_data.geom_wid = self._model.geoms.wid
writer_data.geom_mid = self._model.geoms.material
writer_data.geom_gap = self._model.geoms.gap
writer_data.material_restitution = self._model.materials.restitution
writer_data.material_static_friction = self._model.materials.static_friction
writer_data.material_dynamic_friction = self._model.materials.dynamic_friction
writer_data.material_pair_restitution = self._model.material_pairs.restitution
writer_data.material_pair_static_friction = self._model.material_pairs.static_friction
writer_data.material_pair_dynamic_friction = self._model.material_pairs.dynamic_friction
writer_data.contact_max = int32(contacts.model_max_contacts_host)
writer_data.contact_count = self.narrow_phase_contact_count
writer_data.contacts_model_num_active = contacts.model_active_contacts
writer_data.contacts_world_num_active = contacts.world_active_contacts
writer_data.contact_wid = contacts.wid
writer_data.contact_cid = contacts.cid
writer_data.contact_gid_AB = contacts.gid_AB
writer_data.contact_bid_AB = contacts.bid_AB
writer_data.contact_position_A = contacts.position_A
writer_data.contact_position_B = contacts.position_B
writer_data.contact_gapfunc = contacts.gapfunc
writer_data.contact_frame = contacts.frame
writer_data.contact_material = contacts.material
writer_data.contact_key = contacts.key
# Run narrow phase with the custom Kamino contact writer
self.narrow_phase.launch_custom_write(
candidate_pair=self.broad_phase_pairs,
candidate_pair_count=self.broad_phase_pair_count,
shape_types=self.geom_type,
shape_data=self.geom_data,
shape_transform=data.geoms.pose,
shape_source=self._model.geoms.ptr,
texture_sdf_data=self.shape_sdf_data,
shape_sdf_index=self.shape_sdf_index,
shape_gap=self._model.geoms.gap,
shape_collision_radius=self.shape_collision_radius,
shape_flags=self.shape_flags,
shape_collision_aabb_lower=self.shape_collision_aabb_lower,
shape_collision_aabb_upper=self.shape_collision_aabb_upper,
shape_voxel_resolution=self.shape_voxel_resolution,
shape_heightfield_index=self.shape_heightfield_index,
heightfield_data=self.heightfield_data,
heightfield_elevations=self.heightfield_elevations,
writer_data=writer_data,
device=self._device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/integrators/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
The integrators module of Kamino provides implementations of time-integration methods.
This module provides a front-end defined by:
- :class:`IntegratorEuler`:
A classical semi-implicit Euler time-stepping
integrator formulated in velocity-impulse form.
- :class:`IntegratorMoreauJean`:
A semi-implicit Moreau-Jean time-stepping
integrator formulated in velocity-impulse
form for non-smooth dynamical systems.
"""
from .euler import IntegratorEuler
from .moreau import IntegratorMoreauJean
##
# Module interface
##
__all__ = [
"IntegratorEuler",
"IntegratorMoreauJean",
]
================================================
FILE: newton/_src/solvers/kamino/_src/integrators/euler.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides an implementation of a Semi-Implicit Euler time-integrator.
"""
from __future__ import annotations
from collections.abc import Callable
import warp as wp
from .....core.types import override
from ..core.control import ControlKamino
from ..core.data import DataKamino
from ..core.math import (
compute_body_pose_update_with_logmap,
compute_body_twist_update_with_eom,
screw,
)
from ..core.model import ModelKamino
from ..core.state import StateKamino
from ..core.types import (
float32,
int32,
mat33f,
transformf,
vec3f,
vec4f,
vec6f,
)
from ..geometry.contacts import ContactsKamino
from ..geometry.detector import CollisionDetector
from ..kinematics.limits import LimitsKamino
from .integrator import IntegratorBase
###
# Module interface
###
__all__ = ["IntegratorEuler"]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Functions
###
@wp.func
def euler_semi_implicit_with_logmap(
alpha: float32,
dt: float32,
g: vec3f,
inv_m_i: float32,
I_i: mat33f,
inv_I_i: mat33f,
p_i: transformf,
u_i: vec6f,
w_i: vec6f,
) -> tuple[transformf, vec6f]:
# Integrate the body twist using the maximal coordinate forward dynamics equations
v_i_n, omega_i_n = compute_body_twist_update_with_eom(
dt=dt,
g=g,
inv_m_i=inv_m_i,
I_i=I_i,
inv_I_i=inv_I_i,
u_i=u_i,
w_i=w_i,
)
# Apply damping to angular velocity
omega_i_n *= 1.0 - alpha * dt
# Integrate the body pose using the updated twist
p_i_n = compute_body_pose_update_with_logmap(
dt=dt,
p_i=p_i,
v_i=v_i_n,
omega_i=omega_i_n,
)
# Return the new pose and twist
return p_i_n, screw(v_i_n, omega_i_n)
###
# Kernels
###
@wp.kernel
def _integrate_semi_implicit_euler_inplace(
# Inputs:
alpha: float,
model_dt: wp.array(dtype=float32),
model_gravity: wp.array(dtype=vec4f),
model_bodies_wid: wp.array(dtype=int32),
model_bodies_inv_m: wp.array(dtype=float32),
model_bodies_I: wp.array(dtype=mat33f),
model_bodies_inv_I: wp.array(dtype=mat33f),
state_bodies_w: wp.array(dtype=vec6f),
# Outputs:
state_bodies_q: wp.array(dtype=transformf),
state_bodies_u: wp.array(dtype=vec6f),
):
# Retrieve the thread index
tid = wp.tid()
# Retrieve the world index
wid = model_bodies_wid[tid]
# Retrieve the time step and gravity vector
dt = model_dt[wid]
gv = model_gravity[wid]
g = gv.w * vec3f(gv.x, gv.y, gv.z)
# Retrieve the model data
inv_m_i = model_bodies_inv_m[tid]
I_i = model_bodies_I[tid]
inv_I_i = model_bodies_inv_I[tid]
# Retrieve the current state of the body
q_i = state_bodies_q[tid]
u_i = state_bodies_u[tid]
w_i = state_bodies_w[tid]
# Compute the next pose and twist
q_i_n, u_i_n = euler_semi_implicit_with_logmap(
alpha,
dt,
g,
inv_m_i,
I_i,
inv_I_i,
q_i,
u_i,
w_i,
)
# Store the computed next pose and twist
state_bodies_q[tid] = q_i_n
state_bodies_u[tid] = u_i_n
###
# Launchers
###
def integrate_euler_semi_implicit(model: ModelKamino, data: DataKamino, alpha: float = 0.0):
wp.launch(
_integrate_semi_implicit_euler_inplace,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
alpha, # alpha: angular damping
model.time.dt,
model.gravity.vector,
model.bodies.wid,
model.bodies.inv_m_i,
data.bodies.I_i,
data.bodies.inv_I_i,
data.bodies.w_i,
# Outputs:
data.bodies.q_i,
data.bodies.u_i,
],
)
###
# Interfaces
###
class IntegratorEuler(IntegratorBase):
"""
Provides an implementation of a Semi-Implicit Euler time-stepping integrator.
Effectively, the Semi-Implicit Euler scheme involves an implicit solve of the
forward dynamics to render constraint reactions at the start of the time-step,
followed by an explicit forward integration step to compute the next state:
```
lambda = f_fd(q_p, u_p, tau_j)
u_n = u_p + M^{-1} * ( dt * h(q_p, u_p) + dt * J_a(q_p)^T * tau_j + J_c(q_p)^T * lambda )
q_n = q_p + dt * G(q_p) @ u_n
```
where `q_p` and `u_p` are the generalized coordinates and velocities at the start of the
time-step, `q_n` and `u_n` are the generalized coordinates and velocities at the end of
the time-step, `M` is the generalized mass matrix, `h` is the vector of generalized
non-linear forces, `J_a` is the actuation Jacobian matrix, `tau_j` is the vector of
generalized forces, `J_c` is the constraint Jacobian matrix, and `lambda` are the
constraint reactions.
"""
def __init__(self, model: ModelKamino, alpha: float | None = None):
"""
Initializes the Semi-Implicit Euler integrator with the given :class:`ModelKamino` instance.
Args:
model (`ModelKamino`):
The model container holding the time-invariant parameters of the system being simulated.
alpha (`float`, optional):
The angular damping coefficient. Defaults to 0.0 if `None` is provided.
"""
super().__init__(model)
self._alpha: float = alpha if alpha is not None else 0.0
"""
Damping coefficient for angular velocity used to improve numerical stability of the integrator.\n
Defaults to `0.0`, corresponding to no damping being applied.
"""
###
# Operations
###
@override
def integrate(
self,
forward: Callable,
model: ModelKamino,
data: DataKamino,
state_in: StateKamino,
state_out: StateKamino,
control: ControlKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
detector: CollisionDetector | None = None,
):
"""
Solves the time integration sub-problem using a Semi-Implicit Euler scheme
to integrate the current state of the system over a single time-step.
Args:
forward (`Callable`):
An operator that calls the underlying solver for the forward dynamics sub-problem.
model (`ModelKamino`):
The model container holding the time-invariant parameters of the system being simulated.
data (`DataKamino`):
The data container holding the time-varying parameters of the system being simulated.
state_in (`StateKamino`):
The state of the system at the current time-step.
state_out (`StateKamino`):
The state of the system at the next time-step.
control (`ControlKamino`):
The control inputs applied to the system at the current time-step.
limits (`LimitsKamino`, optional):
The joint limits of the system at the current time-step.
If `None`, no joint limits are considered for the current time-step.
contacts (`ContactsKamino`, optional):
The set of active contacts of the system at the current time-step.
If `None`, no contacts are considered for the current time-step.
detector (`CollisionDetector`, optional):
The collision detector to use for generating the set of active contacts at the current time-step.\n
If `None`, no collision detection is performed for the current time-step,
and active contacts must be provided via the `contacts` argument.
"""
# Solve the forward dynamics sub-problem to compute the
# constraint reactions at the mid-point of the step
forward(
state_in=state_in,
state_out=state_out,
control=control,
limits=limits,
contacts=contacts,
detector=detector,
)
# Perform forward integration to compute the next state of the system
integrate_euler_semi_implicit(model=model, data=data, alpha=self._alpha)
================================================
FILE: newton/_src/solvers/kamino/_src/integrators/integrator.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Defines the base class for time-integrators.
"""
from __future__ import annotations
from collections.abc import Callable
import warp as wp
from ..core.control import ControlKamino
from ..core.data import DataKamino
from ..core.model import ModelKamino
from ..core.state import StateKamino
from ..geometry.contacts import ContactsKamino
from ..geometry.detector import CollisionDetector
from ..kinematics.limits import LimitsKamino
###
# Module interface
###
__all__ = ["IntegratorBase"]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Interfaces
###
class IntegratorBase:
"""
Provides a base class that defines a common interface for time-integrators.
A time-integrator is responsible for solving the time integration sub-problem to
renderthe next state of the system given the current state, control inputs, and
time-varying inequality constraints induced by joint limits and contacts.
"""
def __init__(self, model: ModelKamino):
"""
Initializes the time-integrator with the given :class:`ModelKamino` instance.
Args:
model (`ModelKamino`):
The model container holding the time-invariant parameters of the system being simulated.
"""
self._model = model
###
# Operations
###
def integrate(
self,
forward: Callable,
model: ModelKamino,
data: DataKamino,
state_in: StateKamino,
state_out: StateKamino,
control: ControlKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
detector: CollisionDetector | None = None,
):
"""
Solves the time integration sub-problem to compute the next state of the system.
Args:
forward (`Callable`):
An operator that calls the underlying solver for the forward dynamics sub-problem.
model (`ModelKamino`):
The model container holding the time-invariant parameters of the system being simulated.
data (`DataKamino`):
The data container holding the time-varying parameters of the system being simulated.
state_in (`StateKamino`):
The state of the system at the current time-step.
state_out (`StateKamino`):
The state of the system at the next time-step.
control (`ControlKamino`):
The control inputs applied to the system at the current time-step.
limits (`LimitsKamino`, optional):
The joint limits of the system at the current time-step.
If `None`, no joint limits are considered for the current time-step.
contacts (`ContactsKamino`, optional):
The set of active contacts of the system at the current time-step.
If `None`, no contacts are considered for the current time-step.
detector (`CollisionDetector`, optional):
The collision detector to use for generating the set of active contacts at the current time-step.\n
If `None`, no collision detection is performed for the current time-step,
and active contacts must be provided via the `contacts` argument.
"""
raise NotImplementedError("Integrator is an abstract base class")
================================================
FILE: newton/_src/solvers/kamino/_src/integrators/moreau.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides an implementation of a Semi-Implicit Moreau-Jean
mid-point integration scheme for non-smooth dynamical systems.
"""
from __future__ import annotations
from collections.abc import Callable
import warp as wp
from .....core.types import override
from ..core.control import ControlKamino
from ..core.data import DataKamino
from ..core.math import (
compute_body_pose_update_with_logmap,
compute_body_twist_update_with_eom,
screw,
screw_angular,
screw_linear,
)
from ..core.model import ModelKamino
from ..core.state import StateKamino
from ..core.types import (
float32,
int32,
mat33f,
transformf,
vec3f,
vec4f,
vec6f,
)
from ..geometry.contacts import ContactsKamino
from ..geometry.detector import CollisionDetector
from ..kinematics.limits import LimitsKamino
from .integrator import IntegratorBase
###
# Module interface
###
__all__ = ["IntegratorMoreauJean"]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Functions
###
@wp.func
def moreau_jean_semi_implicit_with_logmap(
alpha: float32,
dt: float32,
g: vec3f,
inv_m_i: float32,
I_i: mat33f,
inv_I_i: mat33f,
p_i: transformf,
u_i: vec6f,
w_i: vec6f,
) -> tuple[transformf, vec6f]:
# Integrate the body twist using the maximal coordinate forward dynamics equations
v_i_n, omega_i_n = compute_body_twist_update_with_eom(
dt=dt,
g=g,
inv_m_i=inv_m_i,
I_i=I_i,
inv_I_i=inv_I_i,
u_i=u_i,
w_i=w_i,
)
# Apply damping to angular velocity
omega_i_n *= 1.0 - alpha * dt
# Integrate the body pose using the updated twist
p_i_n = compute_body_pose_update_with_logmap(
dt=0.5 * dt,
p_i=p_i,
v_i=v_i_n,
omega_i=omega_i_n,
)
# Return the new pose and twist
return p_i_n, screw(v_i_n, omega_i_n)
###
# Kernels
###
@wp.kernel
def _integrate_moreau_jean_first_inplace(
# Inputs:
model_dt: wp.array(dtype=float32),
model_bodies_wid: wp.array(dtype=int32),
bodies_u: wp.array(dtype=vec6f),
# Outputs:
bodies_q: wp.array(dtype=transformf),
):
# Retrieve the thread index
tid = wp.tid()
# Retrieve the world index
wid = model_bodies_wid[tid]
# Retrieve the configured time-step of the corresponding world
dt = model_dt[wid]
# Retrieve the current state of the body
q_i = bodies_q[tid]
u_i = bodies_u[tid]
# Integrate the body pose using the updated twist
q_i_m = compute_body_pose_update_with_logmap(
dt=0.5 * dt,
p_i=q_i,
v_i=screw_linear(u_i),
omega_i=screw_angular(u_i),
)
# Store the computed next pose and twist
bodies_q[tid] = q_i_m
@wp.kernel
def _integrate_moreau_jean_second_inplace(
# Inputs:
alpha: float,
model_dt: wp.array(dtype=float32),
model_gravity: wp.array(dtype=vec4f),
model_bodies_wid: wp.array(dtype=int32),
model_bodies_inv_m: wp.array(dtype=float32),
model_bodies_I: wp.array(dtype=mat33f),
model_bodies_inv_I: wp.array(dtype=mat33f),
state_bodies_w: wp.array(dtype=vec6f),
# Outputs:
state_bodies_q: wp.array(dtype=transformf),
state_bodies_u: wp.array(dtype=vec6f),
):
# Retrieve the thread index
tid = wp.tid()
# Retrieve the world index
wid = model_bodies_wid[tid]
# Retrieve the configured time-step and the
# gravity vector of the corresponding world
dt = model_dt[wid]
gv = model_gravity[wid]
g = gv.w * vec3f(gv.x, gv.y, gv.z)
# Retrieve the model data
inv_m_i = model_bodies_inv_m[tid]
I_i = model_bodies_I[tid]
inv_I_i = model_bodies_inv_I[tid]
# Retrieve the current state of the body
q_i_m = state_bodies_q[tid]
u_i = state_bodies_u[tid]
w_i = state_bodies_w[tid]
# Compute the next pose and twist
q_i_n, u_i_n = moreau_jean_semi_implicit_with_logmap(
alpha,
dt,
g,
inv_m_i,
I_i,
inv_I_i,
q_i_m,
u_i,
w_i,
)
# Store the computed next pose and twist
state_bodies_q[tid] = q_i_n
state_bodies_u[tid] = u_i_n
###
# Interfaces
###
class IntegratorMoreauJean(IntegratorBase):
"""
Provides an implementation of a semi-implicit Moreau-Jean
time-stepping integrator for non-smooth dynamical systems.
Effectively, the Moreau-Jean scheme involves the following three steps:
1. An initial explicit forward integration of the generalized coordinates
using the generalized velocities at the start of the time-step to render
an intermediate configuration at the mid-point of the time-step.
2. An implicit solve of the forward dynamics using the generalized coordinates
evaluated at the mid-point of the discrete time-step together with the initial
generalized velocities to render constraint reactions.
3. A final explicit forward integration of the generalized coordinates and velocities
using the constraint reactions computed at the mid-point of the time-step to render
the next state of the system at the end of the time-step.
These steps can be summarized by the following equations:
```
1: q_m = q_p + 1/2 * dt * G(q_p) * u_p
2: lambdas = f_fd(q_m, u_p, tau_j)
3: u_n = u_p + M(q_m)^{-1} * ( dt * h(q_m, u_p) + dt * J_a(q_m)^T * tau_j + J_c(q_m)^T * lambdas )
4: q_n = q_m + 1/2 * dt * G(q_m) @ u_n
```
where `q_p` and `u_p` are the generalized coordinates and velocities at the start of the
time-step, `q_m` is the intermediate configuration at the mid-point of the time-step,
`q_n` and `u_n` are the generalized coordinates and velocities at the end of the time-step.
`M(q_m)` is the generalized mass matrix, `h(q_m, u_p)` is the vector of generalized
non-linear forces, `J_a(q_m)` is the actuation Jacobian matrix, `J_c(q_m)` is the
constraint Jacobian matrix, all evaluated at the mid-point configuration `q_m`.
`tau_j` is the vector of generalized forces provided at the start of the
time-step, and `lambdas` are the resulting constraint reactions computed
at the mid-point from the forward dynamics sub-problem.
"""
def __init__(self, model: ModelKamino, alpha: float | None = None):
"""
Initializes the semi-implicit Moreau-Jean integrator with the given :class:`ModelKamino` instance.
Args:
model (`ModelKamino`):
The model container holding the time-invariant parameters of the system being simulated.
alpha (`float`, optional):
The angular damping coefficient. Defaults to 0.0 if `None` is provided.
"""
super().__init__(model)
self._alpha: float = alpha if alpha is not None else 0.0
"""
Damping coefficient for angular velocity used to improve numerical stability of the integrator.\n
Defaults to `0.0`, corresponding to no damping being applied.
"""
###
# Operations
###
@override
def integrate(
self,
forward: Callable,
model: ModelKamino,
data: DataKamino,
state_in: StateKamino,
state_out: StateKamino,
control: ControlKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
detector: CollisionDetector | None = None,
):
"""
Solves the time integration sub-problem using a semi-implicit Moreau-Jean
scheme to integrate the current state of the system over a single time-step.
Args:
forward (`Callable`):
An operator that calls the underlying solver for the forward dynamics sub-problem.
model (`ModelKamino`):
The model container holding the time-invariant parameters of the system being simulated.
data (`DataKamino`):
The data container holding the time-varying parameters of the system being simulated.
state_in (`StateKamino`):
The state of the system at the current time-step.
state_out (`StateKamino`):
The state of the system at the next time-step.
control (`ControlKamino`):
The control inputs applied to the system at the current time-step.
limits (`LimitsKamino`, optional):
The joint limits of the system at the current time-step.
If `None`, no joint limits are considered for the current time-step.
contacts (`ContactsKamino`, optional):
The set of active contacts of the system at the current time-step.
If `None`, no contacts are considered for the current time-step.
detector (`CollisionDetector`, optional):
The collision detector to use for generating the set of active contacts at the current time-step.\n
If `None`, no collision detection is performed for the current time-step,
and active contacts must be provided via the `contacts` argument.
"""
# Take the first semi-step until the mid-point of the step
# NOTE: We only integrate on configuration level
# q_M = q_i + 1/2 * dt * G(q_i) * u_i
self._integrate1(model=model, data=data)
# Solve the forward dynamics sub-problem to compute the
# constraint reactions at the mid-point of the step
forward(
state_in=state_in,
state_out=state_out,
control=control,
limits=limits,
contacts=contacts,
detector=detector,
)
# Take the second semi-step until the end of the step
# u_E = u_S + dt * M(q_M)^{-1} * (dt * h(q_M, u_S) + dt * J_a^T(q_M) * tau_j + J_c^T(q_M) * lambda)
# q_E = q_M + 1/2 * dt * G(q_M) * u_E
self._integrate2(model=model, data=data)
###
# Operations
###
def _integrate1(self, model: ModelKamino, data: DataKamino):
"""
Executes the first semi-step of the Moreau-Jean scheme to
integrate the generalized coordinates of the system from
the start of the time-step to the mid-point of the time-step
using the initial generalized velocities of the system.
Args:
model (`ModelKamino`):
The model container holding the time-invariant parameters of the system being simulated.
data (`DataKamino`):
The data container holding the time-varying parameters of the system being simulated.
"""
wp.launch(
_integrate_moreau_jean_first_inplace,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
model.time.dt,
model.bodies.wid,
data.bodies.u_i,
# Outputs:
data.bodies.q_i,
],
)
def _integrate2(self, model: ModelKamino, data: DataKamino):
"""
Executes the second semi-step of the Moreau-Jean scheme to
integrate the generalized coordinates and velocities of the
system from the mid-point the end of the time-step using
the constraint reactions computed from the forward dynamics.
Args:
model (`ModelKamino`):
The model container holding the time-invariant parameters of the system being simulated.
data (`DataKamino`):
The data container holding the time-varying parameters of the system being simulated.
"""
wp.launch(
_integrate_moreau_jean_second_inplace,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
self._alpha,
model.time.dt,
model.gravity.vector,
model.bodies.wid,
model.bodies.inv_m_i,
data.bodies.I_i,
data.bodies.inv_I_i,
data.bodies.w_i,
# Outputs:
data.bodies.q_i,
data.bodies.u_i,
],
)
================================================
FILE: newton/_src/solvers/kamino/_src/kinematics/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Kinematics Module
"""
from .constraints import (
get_max_constraints_per_world,
make_unilateral_constraints_info,
unpack_constraint_solutions,
update_constraints_info,
)
from .jacobians import DenseSystemJacobians, DenseSystemJacobiansData
from .joints import compute_joints_data, extract_actuators_state_from_joints, extract_joints_state_from_actuators
from .limits import LimitsKamino, LimitsKaminoData
###
# Module interface
###
__all__ = [
"DenseSystemJacobians",
"DenseSystemJacobiansData",
"LimitsKamino",
"LimitsKaminoData",
"compute_joints_data",
"extract_actuators_state_from_joints",
"extract_joints_state_from_actuators",
"get_max_constraints_per_world",
"make_unilateral_constraints_info",
"unpack_constraint_solutions",
"update_constraints_info",
]
================================================
FILE: newton/_src/solvers/kamino/_src/kinematics/constraints.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides mechanisms to define and manage constraints and their associated input/output data.
"""
import warp as wp
from ..core.data import DataKamino
from ..core.model import ModelKamino
from ..core.types import float32, int32, vec3f
from ..geometry.contacts import ContactMode, ContactsKamino
from ..kinematics.limits import LimitsKamino
###
# Module interface
###
__all__ = [
"get_max_constraints_per_world",
"make_unilateral_constraints_info",
"unpack_constraint_solutions",
"update_constraints_info",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Functions
###
def get_max_constraints_per_world(
model: ModelKamino,
limits: LimitsKamino | None,
contacts: ContactsKamino | None,
) -> list[int]:
"""
Returns the maximum number of constraints for each world in the model.
Args:
model (ModelKamino): The model for which to compute the maximum constraints.
limits (LimitsKamino, optional): The container holding the allocated joint-limit data.
contacts (ContactsKamino, optional): The container holding the allocated contacts data.
Returns:
List[int]: A list of the maximum constraints for each world in the model.
"""
# Ensure the model container is valid
if model is None:
raise ValueError("`model` is required but got `None`.")
else:
if not isinstance(model, ModelKamino):
raise TypeError(f"`model` is required to be of type `ModelKamino` but got {type(model)}.")
# Ensure the limits container is valid
if limits is not None:
if not isinstance(limits, LimitsKamino):
raise TypeError(f"`limits` is required to be of type `LimitsKamino` but got {type(limits)}.")
# Ensure the contacts container is valid
if contacts is not None:
if not isinstance(contacts, ContactsKamino):
raise TypeError(f"`contacts` is required to be of type `ContactsKamino` but got {type(contacts)}.")
# Compute the maximum number of constraints per world
nw = model.info.num_worlds
njc = model.info.num_joint_cts.numpy()
maxnl = limits.world_max_limits_host if limits and limits.model_max_limits_host > 0 else [0] * nw
maxnc = contacts.world_max_contacts_host if contacts and contacts.model_max_contacts_host > 0 else [0] * nw
maxncts = [njc[i] + maxnl[i] + 3 * maxnc[i] for i in range(nw)]
return maxncts
def make_unilateral_constraints_info(
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
device: wp.DeviceLike = None,
):
"""
Constructs constraints entries in the ModelKaminoInfo member of a model.
Args:
model (ModelKamino): The model container holding time-invariant data.
data (DataKamino): The solver container holding time-varying data.
limits (LimitsKamino, optional): The limits container holding the joint-limit data.
contacts (ContactsKamino, optional): The contacts container holding the contact data.
device (wp.DeviceLike, optional): The device on which to allocate the constraint info arrays.\n
If None, the model's device will be used.
"""
# Ensure the model is valid
if not isinstance(model, ModelKamino):
raise TypeError("`model` must be an instance of `ModelKamino`")
# Ensure the data is valid
if not isinstance(data, DataKamino):
raise TypeError("`data` must be an instance of `DataKamino`")
# Device is not specified, use the model's device
if device is None:
device = model.device
# Retrieve the number of worlds in the model
num_worlds = model.size.num_worlds
# Declare the lists of per-world maximum limits and contacts
# NOTE: These will either be captured by reference from the limits and contacts
# containers or initialized to zero if no limits or contacts are provided.
world_maxnl: list[int] = []
world_maxnc: list[int] = []
###
# Helper functions
###
def _assign_model_limits_info():
nonlocal world_maxnl
world_maxnl = limits.world_max_limits_host
model.size.sum_of_max_limits = limits.model_max_limits_host
model.size.max_of_max_limits = max(limits.world_max_limits_host)
model.info.max_limits = limits.world_max_limits
data.info.num_limits = limits.world_active_limits
def _make_empty_model_limits_info():
nonlocal world_maxnl
world_maxnl = [0] * num_worlds
model.size.sum_of_max_limits = 0
model.size.max_of_max_limits = 0
with wp.ScopedDevice(device):
model.info.max_limits = wp.zeros(shape=(num_worlds,), dtype=int32)
data.info.num_limits = wp.zeros(shape=(num_worlds,), dtype=int32)
def _assign_model_contacts_info():
nonlocal world_maxnc
world_maxnc = contacts.world_max_contacts_host
model.size.sum_of_max_contacts = contacts.model_max_contacts_host
model.size.max_of_max_contacts = max(contacts.world_max_contacts_host)
model.info.max_contacts = contacts.world_max_contacts
data.info.num_contacts = contacts.world_active_contacts
def _make_empty_model_contacts_info():
nonlocal world_maxnc
world_maxnc = [0] * num_worlds
model.size.sum_of_max_contacts = 0
model.size.max_of_max_contacts = 0
with wp.ScopedDevice(device):
model.info.max_contacts = wp.zeros(shape=(num_worlds,), dtype=int32)
data.info.num_contacts = wp.zeros(shape=(num_worlds,), dtype=int32)
# If a limits container is provided, ensure it is valid
# and then assign the entity counters to the model info.
if limits is not None:
if not isinstance(limits, LimitsKamino):
raise TypeError("`limits` must be an instance of `LimitsKamino`")
if limits.data is not None and limits.model_max_limits_host > 0:
_assign_model_limits_info()
else:
_make_empty_model_limits_info()
else:
_make_empty_model_limits_info()
# If a contacts container is provided, ensure it is valid
# and then assign the entity counters to the model info.
if contacts is not None:
if not isinstance(contacts, ContactsKamino):
raise TypeError("`contacts` must be an instance of `ContactsKamino`")
if contacts.data is not None and contacts.model_max_contacts_host > 0:
_assign_model_contacts_info()
else:
_make_empty_model_contacts_info()
else:
_make_empty_model_contacts_info()
# Compute the maximum number of unilateral entities (limits and contacts) per world
world_max_unilaterals: list[int] = [nl + nc for nl, nc in zip(world_maxnl, world_maxnc, strict=False)]
model.size.sum_of_max_unilaterals = sum(world_max_unilaterals)
model.size.max_of_max_unilaterals = max(world_max_unilaterals)
# Compute the maximum number of constraints per world: limits, contacts, and total
world_maxnlc: list[int] = list(world_maxnl)
world_maxncc: list[int] = [3 * maxnc for maxnc in world_maxnc]
world_njc = [0] * num_worlds
world_njdc = [0] * num_worlds
world_njkc = [0] * num_worlds
joints_world = model.joints.wid.numpy().tolist()
joints_num_cts = model.joints.num_cts.numpy().tolist()
joints_num_dynamic_cts = model.joints.num_dynamic_cts.numpy().tolist()
joints_num_kinematic_cts = model.joints.num_kinematic_cts.numpy().tolist()
for jid in range(model.size.sum_of_num_joints):
wid_j = joints_world[jid]
world_njc[wid_j] += joints_num_cts[jid]
world_njdc[wid_j] += joints_num_dynamic_cts[jid]
world_njkc[wid_j] += joints_num_kinematic_cts[jid]
world_maxncts = [
njc + maxnl + maxnc for njc, maxnl, maxnc in zip(world_njc, world_maxnlc, world_maxncc, strict=False)
]
model.size.sum_of_max_total_cts = sum(world_maxncts)
model.size.max_of_max_total_cts = max(world_maxncts)
# Compute the entity index offsets for limits, contacts and unilaterals
# NOTE: unilaterals is simply the concatenation of limits and contacts
world_lio = [0] + [sum(world_maxnl[:i]) for i in range(1, num_worlds + 1)]
world_cio = [0] + [sum(world_maxnc[:i]) for i in range(1, num_worlds + 1)]
world_uio = [0] + [sum(world_maxnl[:i]) + sum(world_maxnc[:i]) for i in range(1, num_worlds + 1)]
# Compute the per-world absolute total constraint block offsets
# NOTE: These are the per-world start indices of arrays like the constraint multipliers `lambda`.
world_ctsio = [0] + [
sum(world_njc[:i]) + sum(world_maxnlc[:i]) + sum(world_maxncc[:i]) for i in range(1, num_worlds + 1)
]
# Compute the initial values of the absolute constraint group
# offsets for joints (dynamic + kinematic), limits, contacts
# TODO: Consider using absolute start indices for each group
# world_jdcio = [world_ctsio[i] for i in range(num_worlds)]
world_jdcio = [0] * num_worlds
world_jkcio = [world_jdcio[i] + world_njdc[i] for i in range(num_worlds)]
world_lcio = [world_jkcio[i] + world_njkc[i] for i in range(num_worlds)]
world_ccio = [world_lcio[i] for i in range(num_worlds)]
# Allocate all constraint info arrays on the target device
with wp.ScopedDevice(device):
# Allocate the per-world max constraints count arrays
model.info.max_total_cts = wp.array(world_maxncts, dtype=int32)
model.info.max_limit_cts = wp.array(world_maxnlc, dtype=int32)
model.info.max_contact_cts = wp.array(world_maxncc, dtype=int32)
# Allocate the per-world active constraints count arrays
# data.info.num_total_cts = wp.clone(model.info.num_joint_cts)
data.info.num_limit_cts = wp.zeros(shape=(num_worlds,), dtype=int32)
data.info.num_contact_cts = wp.zeros(shape=(num_worlds,), dtype=int32)
# Allocate the per-world entity start arrays
model.info.limits_offset = wp.array(world_lio[:num_worlds], dtype=int32)
model.info.contacts_offset = wp.array(world_cio[:num_worlds], dtype=int32)
model.info.unilaterals_offset = wp.array(world_uio[:num_worlds], dtype=int32)
# Allocate the per-world constraint block/group arrays
model.info.total_cts_offset = wp.array(world_ctsio[:num_worlds], dtype=int32)
model.info.joint_dynamic_cts_group_offset = wp.array(world_jdcio[:num_worlds], dtype=int32)
model.info.joint_kinematic_cts_group_offset = wp.array(world_jkcio[:num_worlds], dtype=int32)
data.info.limit_cts_group_offset = wp.array(world_lcio[:num_worlds], dtype=int32)
data.info.contact_cts_group_offset = wp.array(world_ccio[:num_worlds], dtype=int32)
###
# Kernels
###
@wp.kernel
def _update_constraints_info(
# Inputs:
model_info_num_joint_cts: wp.array(dtype=int32),
data_info_num_limits: wp.array(dtype=int32),
data_info_num_contacts: wp.array(dtype=int32),
# Outputs:
data_info_num_total_cts: wp.array(dtype=int32),
data_info_num_limit_cts: wp.array(dtype=int32),
data_info_num_contact_cts: wp.array(dtype=int32),
data_info_limit_cts_group_offset: wp.array(dtype=int32),
data_info_contact_cts_group_offset: wp.array(dtype=int32),
):
# Retrieve the thread index as the world index
wid = wp.tid()
# Retrieve the number of joint constraints for this world
njc = model_info_num_joint_cts[wid]
# Retrieve the number of unilaterals for this world
nl = data_info_num_limits[wid]
nc = data_info_num_contacts[wid]
# Set the number of active constraints for each group and the total
nlc = nl # NOTE: Each limit currently introduces only a single constraint
ncc = 3 * nc
ncts = njc + nlc + ncc
# Set the constraint group offsets, i.e. the starting index
# of each group within the block allocated for each world
lcgo = njc
ccgo = njc + nlc
# Store the state info for this world
data_info_num_total_cts[wid] = ncts
data_info_num_limit_cts[wid] = nlc
data_info_num_contact_cts[wid] = ncc
data_info_limit_cts_group_offset[wid] = lcgo
data_info_contact_cts_group_offset[wid] = ccgo
@wp.kernel
def _unpack_joint_constraint_solutions(
# Inputs:
model_info_joint_cts_offset: wp.array(dtype=int32),
model_info_total_cts_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_group_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_time_inv_dt: wp.array(dtype=float32),
model_joint_wid: wp.array(dtype=int32),
model_joints_num_dynamic_cts: wp.array(dtype=int32),
model_joints_num_kinematic_cts: wp.array(dtype=int32),
model_joints_dynamic_cts_offset: wp.array(dtype=int32),
model_joints_kinematic_cts_offset: wp.array(dtype=int32),
lambdas: wp.array(dtype=float32),
# Outputs:
joint_lambda_j: wp.array(dtype=float32),
):
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the joint-specific model info
wid = model_joint_wid[jid]
num_dyn_cts_j = model_joints_num_dynamic_cts[jid]
num_kin_cts_j = model_joints_num_kinematic_cts[jid]
dyn_cts_start_j = model_joints_dynamic_cts_offset[jid]
kin_cts_start_j = model_joints_kinematic_cts_offset[jid]
# Retrieve the world-specific info
inv_dt = model_time_inv_dt[wid]
world_joint_cts_start = model_info_joint_cts_offset[wid]
world_total_cts_start = model_info_total_cts_offset[wid]
world_jdcgo = model_info_joint_dynamic_cts_group_offset[wid]
world_jkcgo = model_info_joint_kinematic_cts_group_offset[wid]
# Compute block offsets of the joint's constraints within
# the joint-only constraints and total constraints arrays
joint_dyn_cts_start_j = world_joint_cts_start + world_jdcgo + dyn_cts_start_j
joint_kin_cts_start_j = world_joint_cts_start + world_jkcgo + kin_cts_start_j
dyn_cts_row_start_j = world_total_cts_start + world_jdcgo + dyn_cts_start_j
kin_cts_row_start_j = world_total_cts_start + world_jkcgo + kin_cts_start_j
# Compute and store the joint-constraint reaction forces
for j in range(num_dyn_cts_j):
joint_lambda_j[joint_dyn_cts_start_j + j] = inv_dt * lambdas[dyn_cts_row_start_j + j]
for j in range(num_kin_cts_j):
joint_lambda_j[joint_kin_cts_start_j + j] = inv_dt * lambdas[kin_cts_row_start_j + j]
@wp.kernel
def _unpack_limit_constraint_solutions(
# Inputs:
model_time_inv_dt: wp.array(dtype=float32),
model_info_total_cts_offset: wp.array(dtype=int32),
data_info_limit_cts_group_offset: wp.array(dtype=int32),
limit_model_num_limits: wp.array(dtype=int32),
limit_wid: wp.array(dtype=int32),
limit_lid: wp.array(dtype=int32),
lambdas: wp.array(dtype=float32),
v_plus: wp.array(dtype=float32),
# Outputs:
limit_reaction: wp.array(dtype=float32),
limit_velocity: wp.array(dtype=float32),
):
# Retrieve the thread index as the contact index
lid = wp.tid()
# Retrieve the number of limits active in the model
model_nl = limit_model_num_limits[0]
# Skip if lid is greater than the number of limits active in the model
if lid >= model_nl:
return
# Retrieve the world index and the world-relative limit index for this limit
wid = limit_wid[lid]
lid_l = limit_lid[lid]
# Retrieve the world-specific info
inv_dt = model_time_inv_dt[wid]
total_cts_offset = model_info_total_cts_offset[wid]
limit_cts_offset = data_info_limit_cts_group_offset[wid]
# Compute the global constraint index for this limit
limit_cts_idx = total_cts_offset + limit_cts_offset + lid_l
# Load the limit reaction and velocity from the global constraint arrays
lambda_l = lambdas[limit_cts_idx]
v_plus_l = v_plus[limit_cts_idx]
# Scale the contact reaction by the time step to convert from lagrange impulse to force
lambda_l = inv_dt * lambda_l
# Store the computed limit state
limit_reaction[lid] = lambda_l
limit_velocity[lid] = v_plus_l
@wp.kernel
def _unpack_contact_constraint_solutions(
# Inputs:
model_time_inv_dt: wp.array(dtype=float32),
model_info_total_cts_offset: wp.array(dtype=int32),
data_info_contact_cts_group_offset: wp.array(dtype=int32),
contact_model_num_contacts: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
lambdas: wp.array(dtype=float32),
v_plus: wp.array(dtype=float32),
# Outputs:
contact_mode: wp.array(dtype=int32),
contact_reaction: wp.array(dtype=vec3f),
contact_velocity: wp.array(dtype=vec3f),
):
# Retrieve the thread index as the contact index
cid = wp.tid()
# Retrieve the number of contacts active in the model
model_nc = contact_model_num_contacts[0]
# Skip if cid is greater than the number of contacts active in the model
if cid >= model_nc:
return
# Retrieve the world index and the world-relative contact index for this contact
wid = contact_wid[cid]
cid_k = contact_cid[cid]
# Retrieve the world-specific info
inv_dt = model_time_inv_dt[wid]
total_cts_offset = model_info_total_cts_offset[wid]
contact_cts_offset = data_info_contact_cts_group_offset[wid]
# Compute block offsets of the contact constraints within
# the contact-only constraints and total constraints arrays
contact_cts_start = total_cts_offset + contact_cts_offset + 3 * cid_k
# Load the contact reaction and velocity from the global constraint arrays
lambda_k = vec3f(0.0)
v_plus_k = vec3f(0.0)
for k in range(3):
lambda_k[k] = lambdas[contact_cts_start + k]
v_plus_k[k] = v_plus[contact_cts_start + k]
# Scale the contact reaction by the time step to convert from lagrange impulse to force
lambda_k = inv_dt * lambda_k
# Compute the discrete contact mode based on the reaction magnitude and velocity
mode_k = wp.static(ContactMode.make_compute_mode_func())(v_plus_k)
# Store the computed contact state
contact_mode[cid] = mode_k
contact_reaction[cid] = lambda_k
contact_velocity[cid] = v_plus_k
###
# Launchers
###
def update_constraints_info(
model: ModelKamino,
data: DataKamino,
):
"""
Updates the active constraints info for the given model and current data.
Args:
model (ModelKamino): The model container holding time-invariant data.
data (DataKamino): The solver container holding time-varying data.
"""
wp.launch(
_update_constraints_info,
dim=model.info.num_worlds,
inputs=[
# Inputs:
model.info.num_joint_cts,
data.info.num_limits,
data.info.num_contacts,
# Outputs:
data.info.num_total_cts,
data.info.num_limit_cts,
data.info.num_contact_cts,
data.info.limit_cts_group_offset,
data.info.contact_cts_group_offset,
],
)
def unpack_constraint_solutions(
lambdas: wp.array,
v_plus: wp.array,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
):
"""
Unpacks the constraint reactions and velocities into respective data containers.
Args:
lambdas (wp.array): The array of constraint reactions (i.e. lagrange multipliers).
v_plus (wp.array): The array of post-event constraint velocities.
data (DataKamino): The solver container holding time-varying data.
limits (LimitsKamino, optional): The limits container holding the joint-limit data.\n
If None, limits will be skipped.
contacts (ContactsKamino, optional): The contacts container holding the contact data.\n
If None, contacts will be skipped.
"""
# Unpack joint constraint multipliers if the model has joints
if model.size.sum_of_num_joints > 0:
wp.launch(
kernel=_unpack_joint_constraint_solutions,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.joint_cts_offset,
model.info.total_cts_offset,
model.info.joint_dynamic_cts_group_offset,
model.info.joint_kinematic_cts_group_offset,
model.time.inv_dt,
model.joints.wid,
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
lambdas,
# Outputs:
data.joints.lambda_j,
],
)
# Unpack limit constraint multipliers if a limits container is provided
if limits is not None:
wp.launch(
kernel=_unpack_limit_constraint_solutions,
dim=limits.model_max_limits_host,
inputs=[
# Inputs:
model.time.inv_dt,
model.info.total_cts_offset,
data.info.limit_cts_group_offset,
limits.model_active_limits,
limits.wid,
limits.lid,
lambdas,
v_plus,
# Outputs:
limits.reaction,
limits.velocity,
],
)
# Unpack contact constraint multipliers if a contacts container is provided
if contacts is not None:
wp.launch(
kernel=_unpack_contact_constraint_solutions,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs:
model.time.inv_dt,
model.info.total_cts_offset,
data.info.contact_cts_group_offset,
contacts.model_active_contacts,
contacts.wid,
contacts.cid,
lambdas,
v_plus,
# Outputs:
contacts.mode,
contacts.reaction,
contacts.velocity,
],
)
================================================
FILE: newton/_src/solvers/kamino/_src/kinematics/jacobians.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Kinematics: Jacobians
"""
from __future__ import annotations
from typing import Any
import warp as wp
from ..core.data import DataKamino
from ..core.joints import JointDoFType
from ..core.math import (
FLOAT32_MAX,
FLOAT32_MIN,
contact_wrench_matrix_from_points,
expand6d,
screw_transform_matrix_from_points,
)
from ..core.model import ModelKamino
from ..core.types import (
float32,
int32,
mat66f,
quatf,
transformf,
vec2i,
vec3f,
vec6f,
)
from ..geometry.contacts import ContactsKamino
from ..kinematics.limits import LimitsKamino
from ..linalg.sparse_matrix import BlockDType, BlockSparseMatrices
from ..linalg.sparse_operator import BlockSparseLinearOperators
###
# Module interface
###
__all__ = [
"ColMajorSparseConstraintJacobians",
"DenseSystemJacobians",
"DenseSystemJacobiansData",
"SparseSystemJacobians",
"SystemJacobiansType",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Functions
###
def make_store_joint_jacobian_dense_func(axes: Any):
"""
Generates a warp function to store body-pair Jacobian blocks into a target flat
data array given a vector of Jacobian row indices (i.e. selection vector).
"""
@wp.func
def _store_JT_column(
idx: int,
axis: int,
JT: mat66f,
J_data: wp.array(dtype=float32),
):
for i in range(6):
J_data[idx + i] = JT[i, axis]
@wp.func
def store_joint_jacobian_dense(
J_row_offset: int,
row_size: int,
bid_offset: int,
bid_B: int,
bid_F: int,
JT_B_j: mat66f,
JT_F_j: mat66f,
J_data: wp.array(dtype=float32),
):
"""
Stores the Jacobian blocks of a joint into the provided flat data array at the specified offset.
Args:
J_row_offset (int): The offset at which the Jacobian matrix block of the corresponding world starts.
row_size (int): The number of columns in the world's Jacobian block.
bid_offset (int): The body index offset of the world's bodies w.r.t the model.
bid_B (int): The body index of the base body of the joint w.r.t the model.
bid_F (int): The body index of the follower body of the joint w.r.t the model.
JT_B_j (mat66f): The 6x6 Jacobian transpose block of the joint's base body.
JT_F_j (mat66f): The 6x6 Jacobian transpose block of the joint's follower body.
J_data (wp.array(dtype=float32)): The flat data array holding the Jacobian matrix blocks.
"""
# Set the number of rows in the output Jacobian block
# NOTE: This is evaluated statically at compile time
num_jac_rows = wp.static(len(axes))
# Fill the data row by row
body_offset_F = 6 * (bid_F - bid_offset)
body_offset_B = 6 * (bid_B - bid_offset)
for j in range(num_jac_rows):
# Store the Jacobian block for the follower body
_store_JT_column(J_row_offset + body_offset_F, axes[j], JT_F_j, J_data)
# If the base body is not the world (:= -1), store the respective Jacobian block
if bid_B > -1:
_store_JT_column(J_row_offset + body_offset_B, axes[j], JT_B_j, J_data)
# Increment to next Jacobian row
J_row_offset += row_size
# Return the function
return store_joint_jacobian_dense
def make_store_joint_jacobian_sparse_func(axes: Any):
"""
Generates a warp function to store body-pair Jacobian blocks into a target
block sparse data structure.
"""
@wp.func
def store_joint_jacobian_sparse(
is_binary: bool,
JT_B_j: mat66f,
JT_F_j: mat66f,
J_nzb_offset: int,
J_nzb_values: wp.array(dtype=vec6f),
):
"""
Function extracting rows corresponding to joint axes from the 6x6 joint jacobians,
and storing them into the appropriate non-zero blocks.
Args:
is_binary (bool): Whether the joint is binary
JT_B_j (mat66f): The 6x6 Jacobian transpose block of the joint's base body.
JT_F_j (mat66f): The 6x6 Jacobian transpose block of the joint's follower body.
J_nzb_offset (int): The index of the first nzb corresponding to this joint.
J_nzb_values (wp.array(dtype=vec6f)): Array storing the non-zero blocks of the Jacobians.
"""
# Set the number of rows in the output Jacobian block
# NOTE: This is evaluated statically at compile time
num_jac_rows = wp.static(len(axes))
# Store the Jacobian block for the follower body
for i in range(num_jac_rows):
nzb_id = J_nzb_offset + i
J_nzb_values[nzb_id] = JT_F_j[:, axes[i]]
# Store the Jacobian block for the base body, for binary joints
if is_binary:
for i in range(num_jac_rows):
nzb_id = J_nzb_offset + num_jac_rows + i
J_nzb_values[nzb_id] = JT_B_j[:, axes[i]]
# Return the function
return store_joint_jacobian_sparse
@wp.func
def store_joint_cts_jacobian_dense(
dof_type: int,
J_row_offset: int,
num_body_dofs: int,
bid_offset: int,
bid_B: int,
bid_F: int,
JT_B: mat66f,
JT_F: mat66f,
J_data: wp.array(dtype=float32),
):
"""
Stores the constraints Jacobian block of a joint into the provided flat data array at the given offset.
"""
if dof_type == JointDoFType.REVOLUTE:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.REVOLUTE.cts_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.PRISMATIC:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.PRISMATIC.cts_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.CYLINDRICAL:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.CYLINDRICAL.cts_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.UNIVERSAL:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.UNIVERSAL.cts_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.SPHERICAL:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.SPHERICAL.cts_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.GIMBAL:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.GIMBAL.cts_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.CARTESIAN:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.CARTESIAN.cts_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.FIXED:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.FIXED.cts_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
@wp.func
def store_joint_dofs_jacobian_dense(
dof_type: int,
J_row_offset: int,
num_body_dofs: int,
bid_offset: int,
bid_B: int,
bid_F: int,
JT_B: mat66f,
JT_F: mat66f,
J_data: wp.array(dtype=float32),
):
"""
Stores the DoFs Jacobian block of a joint into the provided flat data array at the given offset.
"""
if dof_type == JointDoFType.REVOLUTE:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.REVOLUTE.dofs_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.PRISMATIC:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.PRISMATIC.dofs_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.CYLINDRICAL:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.CYLINDRICAL.dofs_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.UNIVERSAL:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.UNIVERSAL.dofs_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.SPHERICAL:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.SPHERICAL.dofs_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.GIMBAL:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.GIMBAL.dofs_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.CARTESIAN:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.CARTESIAN.dofs_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
elif dof_type == JointDoFType.FREE:
wp.static(make_store_joint_jacobian_dense_func(JointDoFType.FREE.dofs_axes))(
J_row_offset, num_body_dofs, bid_offset, bid_B, bid_F, JT_B, JT_F, J_data
)
@wp.func
def store_joint_cts_jacobian_sparse(
dof_type: int,
is_binary: bool,
JT_B_j: mat66f,
JT_F_j: mat66f,
J_nzb_offset: int,
J_nzb_values: wp.array(dtype=vec6f),
):
"""
Stores the constraints Jacobian block of a joint into the provided flat data array at the given offset.
"""
if dof_type == JointDoFType.REVOLUTE:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.REVOLUTE.cts_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.PRISMATIC:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.PRISMATIC.cts_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.CYLINDRICAL:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.CYLINDRICAL.cts_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.UNIVERSAL:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.UNIVERSAL.cts_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.SPHERICAL:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.SPHERICAL.cts_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.GIMBAL:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.GIMBAL.cts_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.CARTESIAN:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.CARTESIAN.cts_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.FIXED:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.FIXED.cts_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
@wp.func
def store_joint_dofs_jacobian_sparse(
dof_type: int,
is_binary: bool,
JT_B_j: mat66f,
JT_F_j: mat66f,
J_nzb_offset: int,
J_nzb_values: wp.array(dtype=vec6f),
):
"""
Stores the DoFs Jacobian block of a joint into the provided flat data array at the given offset.
"""
if dof_type == JointDoFType.REVOLUTE:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.REVOLUTE.dofs_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.PRISMATIC:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.PRISMATIC.dofs_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.CYLINDRICAL:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.CYLINDRICAL.dofs_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.UNIVERSAL:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.UNIVERSAL.dofs_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.SPHERICAL:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.SPHERICAL.dofs_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.GIMBAL:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.GIMBAL.dofs_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.CARTESIAN:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.CARTESIAN.dofs_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
elif dof_type == JointDoFType.FREE:
wp.static(make_store_joint_jacobian_sparse_func(JointDoFType.FREE.dofs_axes))(
is_binary, JT_B_j, JT_F_j, J_nzb_offset, J_nzb_values
)
###
# Kernels
###
@wp.kernel
def _build_joint_jacobians_dense(
# Inputs
model_info_num_body_dofs: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_group_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_joints_wid: wp.array(dtype=int32),
model_joints_dof_type: wp.array(dtype=int32),
model_joints_dofs_offset: wp.array(dtype=int32),
model_joints_dynamic_cts_offset: wp.array(dtype=int32),
model_joints_kinematic_cts_offset: wp.array(dtype=int32),
model_joints_bid_B: wp.array(dtype=int32),
model_joints_bid_F: wp.array(dtype=int32),
state_joints_p: wp.array(dtype=transformf),
state_bodies_q: wp.array(dtype=transformf),
jac_cts_offsets: wp.array(dtype=int32),
jac_dofs_offsets: wp.array(dtype=int32),
# Outputs
jac_cts_data: wp.array(dtype=float32),
jac_dofs_data: wp.array(dtype=float32),
):
"""
A kernel to compute the Jacobians (constraints and actuated DoFs) for the joints in a model.
"""
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the joint model data
wid = model_joints_wid[jid]
dof_type = model_joints_dof_type[jid]
bid_B = model_joints_bid_B[jid]
bid_F = model_joints_bid_F[jid]
dofs_offset = model_joints_dofs_offset[jid]
dyn_cts_offset = model_joints_dynamic_cts_offset[jid]
kin_cts_offset = model_joints_kinematic_cts_offset[jid]
# Retrieve the number of body DoFs for corresponding world
nbd = model_info_num_body_dofs[wid]
bio = model_info_bodies_offset[wid]
jdcgo = model_info_joint_dynamic_cts_group_offset[wid]
jkcgo = model_info_joint_kinematic_cts_group_offset[wid]
# Retrieve the Jacobian block offset for this world
J_cjmio = jac_cts_offsets[wid]
J_djmio = jac_dofs_offsets[wid]
# Constraint Jacobian row offsets for this joint
J_jdof_row_start = J_djmio + nbd * dofs_offset
J_jdc_row_start = J_cjmio + nbd * (jdcgo + dyn_cts_offset)
J_jkc_row_start = J_cjmio + nbd * (jkcgo + kin_cts_offset)
# Retrieve the pose transform of the joint
T_j = state_joints_p[jid]
r_j = wp.transform_get_translation(T_j)
R_X_j = wp.quat_to_matrix(wp.transform_get_rotation(T_j))
# Retrieve the pose transforms of each body
T_B_j = wp.transform_identity()
if bid_B > -1:
T_B_j = state_bodies_q[bid_B]
T_F_j = state_bodies_q[bid_F]
r_B_j = wp.transform_get_translation(T_B_j)
r_F_j = wp.transform_get_translation(T_F_j)
# Compute the wrench matrices
# TODO: Since the lever-arm is a relative position, can we just use B_r_Bj and F_r_Fj instead?
W_j_B = screw_transform_matrix_from_points(r_j, r_B_j)
W_j_F = screw_transform_matrix_from_points(r_j, r_F_j)
# Compute the effective projector to joint frame and expand to 6D
R_X_bar_j = expand6d(R_X_j)
# Compute the extended jacobians, i.e. without the selection-matrix multiplication
JT_B_j = -W_j_B @ R_X_bar_j # Reaction is on the Base body body ; (6 x 6)
JT_F_j = W_j_F @ R_X_bar_j # Action is on the Follower body ; (6 x 6)
# Store joint dynamic constraint jacobians if applicable
# NOTE: We use the extraction method for DoFs since dynamic constraints are in DoF-space
if dyn_cts_offset >= 0: # Negative dynamic constraint offset means the joint is not implicit
store_joint_dofs_jacobian_dense(dof_type, J_jdc_row_start, nbd, bio, bid_B, bid_F, JT_B_j, JT_F_j, jac_cts_data)
# Store joint kinematic constraint jacobians
store_joint_cts_jacobian_dense(dof_type, J_jkc_row_start, nbd, bio, bid_B, bid_F, JT_B_j, JT_F_j, jac_cts_data)
# Store the actuation Jacobian block if the joint is actuated
store_joint_dofs_jacobian_dense(dof_type, J_jdof_row_start, nbd, bio, bid_B, bid_F, JT_B_j, JT_F_j, jac_dofs_data)
@wp.kernel
def _configure_jacobians_sparse(
# Input:
model_num_joint_cts: wp.array(dtype=int32),
num_limits: wp.array(dtype=int32),
num_contacts: wp.array(dtype=int32),
# Output:
jac_cts_rows: wp.array(dtype=int32),
):
world_id = wp.tid()
jac_cts_rows[world_id] = model_num_joint_cts[world_id] + num_limits[world_id] + 3 * num_contacts[world_id]
@wp.kernel
def _build_joint_jacobians_sparse(
# Inputs
model_joints_dof_type: wp.array(dtype=int32),
model_joints_num_dofs: wp.array(dtype=int32),
model_joints_bid_B: wp.array(dtype=int32),
model_joints_bid_F: wp.array(dtype=int32),
model_joints_dynamic_cts_offset: wp.array(dtype=int32),
state_joints_p: wp.array(dtype=transformf),
state_bodies_q: wp.array(dtype=transformf),
jacobian_cts_nzb_offsets: wp.array(dtype=int32),
jacobian_dofs_nzb_offsets: wp.array(dtype=int32),
# Outputs
jacobian_cts_nzb_values: wp.array(dtype=vec6f),
jacobian_dofs_nzb_values: wp.array(dtype=vec6f),
):
"""
A kernel to compute the Jacobians (constraints and actuated DoFs) for the joints in a model.
"""
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the joint model data
dof_type = model_joints_dof_type[jid]
num_dofs = model_joints_num_dofs[jid]
bid_B = model_joints_bid_B[jid]
bid_F = model_joints_bid_F[jid]
dyn_cts_offset = model_joints_dynamic_cts_offset[jid]
# Retrieve the pose transform of the joint
T_j = state_joints_p[jid]
r_j = wp.transform_get_translation(T_j)
R_X_j = wp.quat_to_matrix(wp.transform_get_rotation(T_j))
# Retrieve the pose transforms of each body
T_B_j = wp.transform_identity()
if bid_B > -1:
T_B_j = state_bodies_q[bid_B]
T_F_j = state_bodies_q[bid_F]
r_B_j = wp.transform_get_translation(T_B_j)
r_F_j = wp.transform_get_translation(T_F_j)
# Compute the wrench matrices
# TODO: Since the lever-arm is a relative position, can we just use B_r_Bj and F_r_Fj instead?
W_j_B = screw_transform_matrix_from_points(r_j, r_B_j)
W_j_F = screw_transform_matrix_from_points(r_j, r_F_j)
# Compute the effective projector to joint frame and expand to 6D
R_X_bar_j = expand6d(R_X_j)
# Compute the extended jacobians, i.e. without the selection-matrix multiplication
JT_B_j = -W_j_B @ R_X_bar_j # Reaction is on the Base body body ; (6 x 6)
JT_F_j = W_j_F @ R_X_bar_j # Action is on the Follower body ; (6 x 6)
# Store joint dynamic constraint jacobians if applicable
# NOTE: We use the extraction method for DoFs since dynamic constraints are in DoF-space
if dyn_cts_offset >= 0: # Negative dynamic constraint offset means the joint is not implicit
store_joint_dofs_jacobian_sparse(
dof_type,
bid_B > -1,
JT_B_j,
JT_F_j,
jacobian_cts_nzb_offsets[jid],
jacobian_cts_nzb_values,
)
# Store the constraint Jacobian block
kinematic_nzb_offset = 0 if dyn_cts_offset < 0 else (2 * num_dofs if bid_B > -1 else num_dofs)
store_joint_cts_jacobian_sparse(
dof_type,
bid_B > -1,
JT_B_j,
JT_F_j,
jacobian_cts_nzb_offsets[jid] + kinematic_nzb_offset,
jacobian_cts_nzb_values,
)
# Store the actuation Jacobian block if the joint is actuated
store_joint_dofs_jacobian_sparse(
dof_type,
bid_B > -1,
JT_B_j,
JT_F_j,
jacobian_dofs_nzb_offsets[jid],
jacobian_dofs_nzb_values,
)
@wp.kernel
def _build_limit_jacobians_dense(
# Inputs:
model_info_num_body_dofs: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
data_info_limit_cts_group_offset: wp.array(dtype=int32),
limits_model_num: wp.array(dtype=int32),
limits_model_max: int32,
limits_wid: wp.array(dtype=int32),
limits_lid: wp.array(dtype=int32),
limits_bids: wp.array(dtype=vec2i),
limits_dof: wp.array(dtype=int32),
limits_side: wp.array(dtype=float32),
jacobian_dofs_offsets: wp.array(dtype=int32),
jacobian_dofs_data: wp.array(dtype=float32),
jacobian_cts_offsets: wp.array(dtype=int32),
# Outputs:
jacobian_cts_data: wp.array(dtype=float32),
):
"""
A kernel to compute the Jacobians (constraints and actuated DoFs) for the joints in a model.
"""
# Retrieve the thread index as the limit index
lid = wp.tid()
# Skip if cid is greater than the total number of active limits in the model
if lid >= wp.min(limits_model_num[0], limits_model_max):
return
# Retrieve the world index of the active limit
wid_l = limits_wid[lid]
# Retrieve the limit description info
# NOTE: *_l is used to denote a subscript for the limit index
lid_l = limits_lid[lid]
bids_l = limits_bids[lid]
dof_l = limits_dof[lid]
side_l = limits_side[lid]
# Retrieve the relevant model info of the world
nbd = model_info_num_body_dofs[wid_l]
bio = model_info_bodies_offset[wid_l]
lcgo = data_info_limit_cts_group_offset[wid_l]
ajmio = jacobian_dofs_offsets[wid_l]
cjmio = jacobian_cts_offsets[wid_l]
# Append the index offsets to the corresponding rows of the Jacobians
ajmio += nbd * dof_l
cjmio += nbd * (lcgo + lid_l)
# Extract the body ids
bid_B_l = bids_l[0]
bid_F_l = bids_l[1]
# Set the constraint Jacobian block for the follower body from the actuation Jacobian block
bio_F = 6 * (bid_F_l - bio)
act_kj = ajmio + bio_F
cts_kj = cjmio + bio_F
for i in range(6):
jacobian_cts_data[cts_kj + i] = side_l * jacobian_dofs_data[act_kj + i]
# If not the world body, set the constraint Jacobian block for the base body from the actuation Jacobian block
if bid_B_l > -1:
bio_B = 6 * (bid_B_l - bio)
act_kj = ajmio + bio_B
cts_kj = cjmio + bio_B
for i in range(6):
jacobian_cts_data[cts_kj + i] = side_l * jacobian_dofs_data[act_kj + i]
@wp.kernel
def _build_limit_jacobians_sparse(
# Inputs:
model_info_bodies_offset: wp.array(dtype=int32),
model_joints_dofs_offset: wp.array(dtype=int32),
model_joints_num_dofs: wp.array(dtype=int32),
state_info_limit_cts_group_offset: wp.array(dtype=int32),
limits_model_num: wp.array(dtype=int32),
limits_model_max: int32,
limits_wid: wp.array(dtype=int32),
limits_jid: wp.array(dtype=int32),
limits_lid: wp.array(dtype=int32),
limits_bids: wp.array(dtype=vec2i),
limits_dof: wp.array(dtype=int32),
limits_side: wp.array(dtype=float32),
jacobian_dofs_joint_nzb_offsets: wp.array(dtype=int32),
jacobian_dofs_nzb_values: wp.array(dtype=vec6f),
jacobian_cts_nzb_start: wp.array(dtype=int32),
# Outputs:
jacobian_cts_num_nzb: wp.array(dtype=int32),
jacobian_cts_nzb_coords: wp.array2d(dtype=int32),
jacobian_cts_nzb_values: wp.array(dtype=vec6f),
jacobian_cts_limit_nzb_offsets: wp.array(dtype=int32),
):
"""
A kernel to compute the Jacobians (constraints and actuated DoFs) for the joints in a model.
"""
# Retrieve the thread index as the limit index
limit_id = wp.tid()
# Skip if cid is greater than the total number of active limits in the model
if limit_id >= wp.min(limits_model_num[0], limits_model_max):
return
# Retrieve the world index of the active limit
world_id = limits_wid[limit_id]
# Retrieve the limit description info
# NOTE: *_l is used to denote a subscript for the limit index
limit_id_l = limits_lid[limit_id]
body_ids_l = limits_bids[limit_id]
body_id_B_l = body_ids_l[0]
body_id_F_l = body_ids_l[1]
dof_l = limits_dof[limit_id]
side_l = limits_side[limit_id]
joint_id = limits_jid[limit_id]
# Resolve which NZB of the dofs Jacobian corresponds to the limit's dof (on the follower)
dof_id = dof_l - model_joints_dofs_offset[joint_id] # Id of the dof among the joint's dof
jac_dofs_nzb_idx = jacobian_dofs_joint_nzb_offsets[joint_id] + dof_id
# Retrieve the relevant model info of the world
body_index_offset = model_info_bodies_offset[world_id]
limit_cts_offset = state_info_limit_cts_group_offset[world_id]
# Create NZB(s)
num_limit_nzb = 2 if body_id_B_l > -1 else 1
jac_cts_nzb_offset_world = wp.atomic_add(jacobian_cts_num_nzb, world_id, num_limit_nzb)
jac_cts_nzb_idx = jacobian_cts_nzb_start[world_id] + jac_cts_nzb_offset_world
jacobian_cts_limit_nzb_offsets[limit_id] = jac_cts_nzb_idx
# Set the constraint Jacobian block for the follower body from the actuation Jacobian block
jacobian_cts_nzb_values[jac_cts_nzb_idx] = side_l * jacobian_dofs_nzb_values[jac_dofs_nzb_idx]
jacobian_cts_nzb_coords[jac_cts_nzb_idx, 0] = limit_cts_offset + limit_id_l
jacobian_cts_nzb_coords[jac_cts_nzb_idx, 1] = 6 * (body_id_F_l - body_index_offset)
# If not the world body, set the constraint Jacobian block for the base body from the actuation Jacobian block
if body_id_B_l > -1:
nzb_stride = model_joints_num_dofs[joint_id]
jacobian_cts_nzb_values[jac_cts_nzb_idx + 1] = side_l * jacobian_dofs_nzb_values[jac_dofs_nzb_idx + nzb_stride]
jacobian_cts_nzb_coords[jac_cts_nzb_idx + 1, 0] = limit_cts_offset + limit_id_l
jacobian_cts_nzb_coords[jac_cts_nzb_idx + 1, 1] = 6 * (body_id_B_l - body_index_offset)
@wp.kernel
def _build_contact_jacobians_dense(
# Inputs:
model_info_num_body_dofs: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
data_info_contact_cts_group_offset: wp.array(dtype=int32),
state_bodies_q: wp.array(dtype=transformf),
contacts_model_num: wp.array(dtype=int32),
contacts_model_max: int32,
contacts_wid: wp.array(dtype=int32),
contacts_cid: wp.array(dtype=int32),
contacts_bid_AB: wp.array(dtype=vec2i),
contacts_position_A: wp.array(dtype=vec3f),
contacts_position_B: wp.array(dtype=vec3f),
contacts_frame: wp.array(dtype=quatf),
jacobian_cts_offsets: wp.array(dtype=int32),
# Outputs:
jacobian_cts_data: wp.array(dtype=float32),
):
"""
A kernel to compute the Jacobians (constraints and actuated DoFs) for the joints in a model.
"""
# Retrieve the thread index as the contact index
cid = wp.tid()
# Skip if cid is greater than the total number of active contacts in the model
if cid >= wp.min(contacts_model_num[0], contacts_model_max):
return
# Retrieve the contact index w.r.t the world
# NOTE: k denotes a notational subscript for the
# contact index, i.e. C_k is the k-th contact entity
cid_k = contacts_cid[cid]
# Retrieve the the contact-specific data
wid = contacts_wid[cid]
q_k = contacts_frame[cid]
bid_AB_k = contacts_bid_AB[cid]
r_Ac_k = contacts_position_A[cid]
r_Bc_k = contacts_position_B[cid]
# Retrieve the relevant model info for the world
nbd = model_info_num_body_dofs[wid]
bio = model_info_bodies_offset[wid]
ccgo = data_info_contact_cts_group_offset[wid]
cjmio = jacobian_cts_offsets[wid]
# Append the index offset for the contact Jacobian block in the constraint Jacobian
cjmio += ccgo * nbd
# Extract the individual body indices
bid_A_k = bid_AB_k[0]
bid_B_k = bid_AB_k[1]
# Compute the rotation matrix from the contact frame quaternion
R_k = wp.quat_to_matrix(q_k) # (3 x 3)
# Set the constraint index offset for this contact
cio_k = 3 * cid_k
# Compute and store the revolute Jacobian block for the follower body (subject of action)
r_B_k = wp.transform_get_translation(state_bodies_q[bid_B_k])
W_B_k = contact_wrench_matrix_from_points(r_Bc_k, r_B_k)
JT_c_B_k = W_B_k @ R_k # Action is on the follower body (B) ; (6 x 3)
bio_B = 6 * (bid_B_k - bio)
for j in range(3):
kj = cjmio + nbd * (cio_k + j) + bio_B
for i in range(6):
jacobian_cts_data[kj + i] = JT_c_B_k[i, j]
# If not the world body, compute and store the revolute Jacobian block for the base body (subject of reaction)
if bid_A_k > -1:
r_A_k = wp.transform_get_translation(state_bodies_q[bid_A_k])
W_A_k = contact_wrench_matrix_from_points(r_Ac_k, r_A_k)
JT_c_A_k = -W_A_k @ R_k # Reaction is on the base body (A) ; (6 x 3)
bio_A = 6 * (bid_A_k - bio)
for j in range(3):
kj = cjmio + nbd * (cio_k + j) + bio_A
for i in range(6):
jacobian_cts_data[kj + i] = JT_c_A_k[i, j]
@wp.kernel
def _build_contact_jacobians_sparse(
# Inputs:
model_info_bodies_offset: wp.array(dtype=int32),
state_info_contact_cts_group_offset: wp.array(dtype=int32),
state_bodies_q: wp.array(dtype=transformf),
contacts_model_num: wp.array(dtype=int32),
contacts_model_max: int32,
contacts_wid: wp.array(dtype=int32),
contacts_cid: wp.array(dtype=int32),
contacts_bid_AB: wp.array(dtype=vec2i),
contacts_position_A: wp.array(dtype=vec3f),
contacts_position_B: wp.array(dtype=vec3f),
contacts_frame: wp.array(dtype=quatf),
jacobian_cts_nzb_start: wp.array(dtype=int32),
# Outputs:
jacobian_cts_num_nzb: wp.array(dtype=int32),
jacobian_cts_nzb_coords: wp.array2d(dtype=int32),
jacobian_cts_nzb_values: wp.array(dtype=vec6f),
jacobian_cts_contact_nzb_offsets: wp.array(dtype=int32),
):
"""
A kernel to compute the Jacobians (constraints and actuated DoFs) for the joints in a model.
"""
# Retrieve the thread index as the contact index
contact_id = wp.tid()
# Skip if cid is greater than the total number of active contacts in the model
if contact_id >= wp.min(contacts_model_num[0], contacts_model_max):
return
# Retrieve the contact index w.r.t the world
# NOTE: k denotes a notational subscript for the
# contact index, i.e. C_k is the k-th contact entity
contact_id_k = contacts_cid[contact_id]
# Retrieve the the contact-specific data
world_id = contacts_wid[contact_id]
q_k = contacts_frame[contact_id]
body_ids_k = contacts_bid_AB[contact_id]
body_id_A_k = body_ids_k[0]
body_id_B_k = body_ids_k[1]
r_Ac_k = contacts_position_A[contact_id]
r_Bc_k = contacts_position_B[contact_id]
# Retrieve the relevant model info for the world
body_idx_offset = model_info_bodies_offset[world_id]
contact_cts_offset = state_info_contact_cts_group_offset[world_id]
# Compute the rotation matrix from the contact frame quaternion
R_k = wp.quat_to_matrix(q_k) # (3 x 3)
# Set the start constraint index for this contact
cts_idx_start = 3 * contact_id_k + contact_cts_offset
# Compute and store the revolute Jacobian block for the follower body (subject of action)
r_B_k = wp.transform_get_translation(state_bodies_q[body_id_B_k])
W_B_k = contact_wrench_matrix_from_points(r_Bc_k, r_B_k)
JT_c_B_k = W_B_k @ R_k # Action is on the follower body (B) ; (6 x 3)
body_idx_offset_B = 6 * (body_id_B_k - body_idx_offset)
num_contact_nzb = 6 if body_id_A_k > -1 else 3
# Allocate non-zero blocks in the Jacobian by incrementing the number of NZB
jac_cts_nzb_offset_world = wp.atomic_add(jacobian_cts_num_nzb, world_id, num_contact_nzb)
jac_cts_nzb_offset = jacobian_cts_nzb_start[world_id] + jac_cts_nzb_offset_world
jacobian_cts_contact_nzb_offsets[contact_id] = jac_cts_nzb_offset
# Store 6x3 Jacobian block as three separate 6x1 blocks
for j in range(3):
jacobian_cts_nzb_values[jac_cts_nzb_offset + j] = JT_c_B_k[:, j]
jacobian_cts_nzb_coords[jac_cts_nzb_offset + j, 0] = cts_idx_start + j
jacobian_cts_nzb_coords[jac_cts_nzb_offset + j, 1] = body_idx_offset_B
# If not the world body, compute and store the revolute Jacobian block for the base body (subject of reaction)
if body_id_A_k > -1:
r_A_k = wp.transform_get_translation(state_bodies_q[body_id_A_k])
W_A_k = contact_wrench_matrix_from_points(r_Ac_k, r_A_k)
JT_c_A_k = -W_A_k @ R_k # Reaction is on the base body (A) ; (6 x 3)
body_idx_offset_A = 6 * (body_id_A_k - body_idx_offset)
# Store 6x3 Jacobian block as three separate 6x1 blocks
for j in range(3):
jacobian_cts_nzb_values[jac_cts_nzb_offset + 3 + j] = JT_c_A_k[:, j]
jacobian_cts_nzb_coords[jac_cts_nzb_offset + 3 + j, 0] = cts_idx_start + j
jacobian_cts_nzb_coords[jac_cts_nzb_offset + 3 + j, 1] = body_idx_offset_A
@wp.func
def store_col_major_jacobian_block(
nzb_id: int32,
row_id: int32,
col_id: int32,
block: mat66f,
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=wp.types.matrix(shape=(6, 1), dtype=float32)),
):
for i in range(6):
nzb_id_i = nzb_id + i
nzb_coords[nzb_id_i, 0] = row_id
nzb_coords[nzb_id_i, 1] = col_id + i
for j in range(6):
nzb_values[nzb_id_i][j, 0] = block[j, i]
@wp.kernel
def _update_col_major_joint_jacobians(
# Inputs
model_joints_num_dynamic_cts: wp.array(dtype=int32),
model_joints_num_kinematic_cts: wp.array(dtype=int32),
model_joints_bid_B: wp.array(dtype=int32),
jac_cts_row_major_joint_nzb_offsets: wp.array(dtype=int32),
jac_cts_row_major_nzb_coords: wp.array2d(dtype=int32),
jac_cts_row_major_nzb_values: wp.array(dtype=vec6f),
jac_cts_col_major_joint_nzb_offsets: wp.array(dtype=int32),
# Outputs
jac_cts_col_major_nzb_values: wp.array(dtype=wp.types.matrix(shape=(6, 1), dtype=float32)),
):
"""
A kernel to compute the Jacobians (constraints and actuated DoFs) for the joints in a model.
"""
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the joint model data
num_dynamic_cts = model_joints_num_dynamic_cts[jid]
num_kinematic_cts = model_joints_num_kinematic_cts[jid]
bid_B = model_joints_bid_B[jid]
# Retrieve the Jacobian data
dynamic_nzb_start_rm_j = jac_cts_row_major_joint_nzb_offsets[jid]
kinematic_nzb_start_rm_j = dynamic_nzb_start_rm_j
dynamic_nzb_offset_cm = jac_cts_col_major_joint_nzb_offsets[jid]
kinematic_nzb_offset_cm = dynamic_nzb_offset_cm
# Offset the Jacobian rows within the 6x6 block to avoid exceeding matrix dimensions.
# Since we might not fill the full 6x6 block with Jacobian entries, shifting the block upwards
# and filling the bottom part will prevent the block lying outside the matrix dimensions.
# We additional guard against the case where the shift would push the block above the start of
# the matrix by taking the minimum of the full shift and `nzb_row_init`.
if num_dynamic_cts > 0:
dynamic_nzb_row_init = jac_cts_row_major_nzb_coords[dynamic_nzb_start_rm_j, 0]
dynamic_block_row_init = min(6 - num_dynamic_cts, dynamic_nzb_row_init)
for i in range(num_dynamic_cts):
nzb_idx_rm = dynamic_nzb_start_rm_j + i
block_rm = jac_cts_row_major_nzb_values[nzb_idx_rm]
for k in range(6):
jac_cts_col_major_nzb_values[dynamic_nzb_offset_cm + k][dynamic_block_row_init + i, 0] = block_rm[k]
if bid_B > -1:
for i in range(num_dynamic_cts):
nzb_idx_rm = dynamic_nzb_start_rm_j + num_dynamic_cts + i
block_rm = jac_cts_row_major_nzb_values[nzb_idx_rm]
for k in range(6):
jac_cts_col_major_nzb_values[dynamic_nzb_offset_cm + 6 + k][dynamic_block_row_init + i, 0] = (
block_rm[k]
)
kinematic_nzb_start_rm_j += 2 * num_dynamic_cts
kinematic_nzb_offset_cm += 12
else:
kinematic_nzb_start_rm_j += num_dynamic_cts
kinematic_nzb_offset_cm += 6
kinematic_nzb_row_init = jac_cts_row_major_nzb_coords[kinematic_nzb_start_rm_j, 0]
kinematic_block_row_init = min(6 - num_kinematic_cts, kinematic_nzb_row_init)
for i in range(num_kinematic_cts):
nzb_idx_rm = kinematic_nzb_start_rm_j + i
block_rm = jac_cts_row_major_nzb_values[nzb_idx_rm]
for k in range(6):
jac_cts_col_major_nzb_values[kinematic_nzb_offset_cm + k][kinematic_block_row_init + i, 0] = block_rm[k]
if bid_B > -1:
for i in range(num_kinematic_cts):
nzb_idx_rm = kinematic_nzb_start_rm_j + num_kinematic_cts + i
block_rm = jac_cts_row_major_nzb_values[nzb_idx_rm]
for k in range(6):
jac_cts_col_major_nzb_values[kinematic_nzb_offset_cm + 6 + k][kinematic_block_row_init + i, 0] = (
block_rm[k]
)
@wp.kernel
def _update_col_major_limit_jacobians(
# Inputs
limits_model_num: wp.array(dtype=int32),
limits_model_max: int32,
limits_wid: wp.array(dtype=int32),
limits_bids: wp.array(dtype=vec2i),
jac_cts_row_major_limit_nzb_offsets: wp.array(dtype=int32),
jac_cts_row_major_nzb_coords: wp.array2d(dtype=int32),
jac_cts_row_major_nzb_values: wp.array(dtype=vec6f),
jac_cts_col_major_nzb_start: wp.array(dtype=int32),
# Outputs
jac_cts_col_major_num_nzb: wp.array(dtype=int32),
jac_cts_col_major_nzb_coords: wp.array2d(dtype=int32),
jac_cts_col_major_nzb_values: wp.array(dtype=wp.types.matrix(shape=(6, 1), dtype=float32)),
):
"""
A kernel to assemble the limit constraint Jacobian in a model.
"""
# Retrieve the thread index as the limit index
limit_id = wp.tid()
# Skip if cid is greater than the total number of active limits in the model
if limit_id >= wp.min(limits_model_num[0], limits_model_max):
return
# Retrieve the world index of the active limit
world_id = limits_wid[limit_id]
# Retrieve the limit description info
# NOTE: *_l is used to denote a subscript for the limit index
body_ids_l = limits_bids[limit_id]
body_id_B_l = body_ids_l[0]
# Set the constraint Jacobian block for the follower body from the actuation Jacobian block
num_limit_nzb = 12 if body_id_B_l > -1 else 6
nzb_offset_cm = wp.atomic_add(jac_cts_col_major_num_nzb, world_id, num_limit_nzb)
# Retrieve the Jacobian data
nzb_start_rm_l = jac_cts_row_major_limit_nzb_offsets[limit_id]
nzb_row_init = jac_cts_row_major_nzb_coords[nzb_start_rm_l, 0]
nzb_col_init_F = jac_cts_row_major_nzb_coords[nzb_start_rm_l, 1]
nzb_start_cm = jac_cts_col_major_nzb_start[world_id]
# Offset the Jacobian rows within the 6x6 block to avoid exceeding matrix dimensions.
# Since we might not fill the full 6x6 block with Jacobian entries, shifting the block upwards
# and filling the bottom part will prevent the block lying outside the matrix dimensions.
# We additional guard against the case where the shift would push the block above the start of
# the matrix by taking the minimum of the full shift and `nzb_row_init`.
block_row_init = min(5, nzb_row_init)
nzb_row_init -= block_row_init
block_F = mat66f(0.0)
block_F[block_row_init] = jac_cts_row_major_nzb_values[nzb_start_rm_l]
store_col_major_jacobian_block(
nzb_start_cm + nzb_offset_cm,
nzb_row_init,
nzb_col_init_F,
block_F,
jac_cts_col_major_nzb_coords,
jac_cts_col_major_nzb_values,
)
if body_id_B_l > -1:
nzb_col_init_B = jac_cts_row_major_nzb_coords[nzb_start_rm_l + 1, 1]
block_B = mat66f(0.0)
block_B[block_row_init] = jac_cts_row_major_nzb_values[nzb_start_rm_l + 1]
store_col_major_jacobian_block(
nzb_start_cm + nzb_offset_cm + 6,
nzb_row_init,
nzb_col_init_B,
block_B,
jac_cts_col_major_nzb_coords,
jac_cts_col_major_nzb_values,
)
@wp.kernel
def _update_col_major_contact_jacobians(
# Inputs:
contacts_model_num: wp.array(dtype=int32),
contacts_model_max: int32,
contacts_wid: wp.array(dtype=int32),
contacts_bid_AB: wp.array(dtype=vec2i),
jac_cts_row_major_contact_nzb_offsets: wp.array(dtype=int32),
jac_cts_row_major_nzb_coords: wp.array2d(dtype=int32),
jac_cts_row_major_nzb_values: wp.array(dtype=vec6f),
jac_cts_col_major_nzb_start: wp.array(dtype=int32),
# Outputs
jac_cts_col_major_num_nzb: wp.array(dtype=int32),
jac_cts_col_major_nzb_coords: wp.array2d(dtype=int32),
jac_cts_col_major_nzb_values: wp.array(dtype=wp.types.matrix(shape=(6, 1), dtype=float32)),
):
"""
A kernel to assemble the contact constraint Jacobian in a model.
"""
# Retrieve the thread index as the contact index
contact_id = wp.tid()
# Skip if cid is greater than the total number of active contacts in the model
if contact_id >= wp.min(contacts_model_num[0], contacts_model_max):
return
# Retrieve the the contact-specific data
world_id = contacts_wid[contact_id]
body_ids_k = contacts_bid_AB[contact_id]
body_id_A_k = body_ids_k[0]
# Set the constraint Jacobian block for the follower body from the actuation Jacobian block
num_contact_nzb = 12 if body_id_A_k > -1 else 6
nzb_offset_cm = wp.atomic_add(jac_cts_col_major_num_nzb, world_id, num_contact_nzb)
# Retrieve the Jacobian data
nzb_start_rm_c = jac_cts_row_major_contact_nzb_offsets[contact_id]
nzb_row_init = jac_cts_row_major_nzb_coords[nzb_start_rm_c, 0]
nzb_col_init_F = jac_cts_row_major_nzb_coords[nzb_start_rm_c, 1]
nzb_start_cm = jac_cts_col_major_nzb_start[world_id]
# Offset the Jacobian rows within the 6x6 block to avoid exceeding matrix dimensions.
# Since we might not fill the full 6x6 block with Jacobian entries, shifting the block upwards
# and filling the bottom part will prevent the block lying outside the matrix dimensions.
# We additional guard against the case where the shift would push the block above the start of
# the matrix by taking the minimum of the full shift and `nzb_row_init`.
block_row_init = min(3, nzb_row_init)
nzb_row_init -= block_row_init
block_F = mat66f(0.0)
for i in range(3):
block_F[block_row_init + i] = jac_cts_row_major_nzb_values[nzb_start_rm_c + i]
store_col_major_jacobian_block(
nzb_start_cm + nzb_offset_cm,
nzb_row_init,
nzb_col_init_F,
block_F,
jac_cts_col_major_nzb_coords,
jac_cts_col_major_nzb_values,
)
if body_id_A_k > -1:
nzb_col_init_B = jac_cts_row_major_nzb_coords[nzb_start_rm_c + 3, 1]
block_B = mat66f(0.0)
for i in range(3):
block_B[block_row_init + i] = jac_cts_row_major_nzb_values[nzb_start_rm_c + 3 + i]
store_col_major_jacobian_block(
nzb_start_cm + nzb_offset_cm + 6,
nzb_row_init,
nzb_col_init_B,
block_B,
jac_cts_col_major_nzb_coords,
jac_cts_col_major_nzb_values,
)
###
# Types
###
class DenseSystemJacobiansData:
"""
Container to hold time-varying Jacobians of the system.
"""
def __init__(self):
###
# Constraint Jacobian
###
self.J_cts_offsets: wp.array | None = None
"""
The index offset of the constraint Jacobian matrix block of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.\n
"""
self.J_cts_data: wp.array | None = None
"""
A flat array containing the constraint Jacobian matrix data of all worlds.\n
Shape of ``(sum(ncts_w * nbd_w),)`` and type :class:`float32`.
"""
###
# DoFs Jacobian
###
self.J_dofs_offsets: wp.array | None = None
"""
The index offset of the DoF Jacobian matrix block of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.\n
"""
self.J_dofs_data: wp.array | None = None
"""
A flat array containing the joint DoF Jacobian matrix data of all worlds.\n
Shape of ``(sum(njad_w * nbd_w),)`` and type :class:`float32`.
"""
###
# Interfaces
###
class DenseSystemJacobians:
"""
Container to hold time-varying Jacobians of the system.
"""
def __init__(
self,
model: ModelKamino | None = None,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
device: wp.DeviceLike = None,
):
"""
Creates a :class:`DenseSystemJacobians` container and allocates the Jacobian data if a model is provided.\n
The Jacobians are stored in dense format as flat arrays, and the matrix blocks of each world are stored
contiguously with the corresponding index offsets. The Jacobian matrix blocks of each world are stored
in the order of joints, limits, and contacts. For example, the constraint Jacobian matrix blocks of world
``w`` are stored in the order of joint constraint Jacobian blocks, limit constraint Jacobian blocks, and
contact constraint Jacobian blocks, and the DoF Jacobian matrix block of world ``w`` is stored after the
constraint Jacobian matrix blocks of world ``w``.
Args:
model (`ModelKamino`, optional):
The model container describing the system structure and properties, used
to allocate the Jacobian data and compute the matrix block sizes and index offsets.
limits (`LimitsKamino`, optional):
The limits container describing the active limits in the system, used
to compute the matrix block sizes and index offsets if provided.
contacts (`ContactsKamino`, optional):
The contacts container describing the active contacts in the system,
used to compute the matrix block sizes and index offsets if provided.
device (`Device` or `str`, optional):
The device on which the Jacobian data should be allocated.\n
If `None`, the Jacobian data will be allocated on same device as the model.
"""
# Declare and initialize the Jacobian data container
self._data = DenseSystemJacobiansData()
# If a model is provided, allocate the Jacobians data
if model is not None:
self.finalize(model=model, limits=limits, contacts=contacts, device=device)
@property
def data(self) -> DenseSystemJacobiansData:
"""
Returns the internal data container holding the Jacobians data.
"""
return self._data
def finalize(
self,
model: ModelKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
device: wp.DeviceLike = None,
):
# Ensure the model container is valid
if model is None:
raise ValueError("`model` is required but got `None`.")
else:
if not isinstance(model, ModelKamino):
raise TypeError(f"`model` is required to be of type `ModelKamino` but got {type(model)}.")
# Ensure the limits container is valid
if limits is not None:
if not isinstance(limits, LimitsKamino):
raise TypeError(f"`limits` is required to be of type `LimitsKamino` but got {type(limits)}.")
# Ensure the contacts container is valid
if contacts is not None:
if not isinstance(contacts, ContactsKamino):
raise TypeError(f"`contacts` is required to be of type `ContactsKamino` but got {type(contacts)}.")
# Extract the constraint and DoF sizes of each world
nw = model.info.num_worlds
nbd = model.info.num_body_dofs.numpy().tolist()
njc = model.info.num_joint_cts.numpy().tolist()
njd = model.info.num_joint_dofs.numpy().tolist()
maxnl = limits.world_max_limits_host if limits and limits.model_max_limits_host > 0 else [0] * nw
maxnc = contacts.world_max_contacts_host if contacts and contacts.model_max_contacts_host > 0 else [0] * nw
maxncts = [njc[w] + maxnl[w] + 3 * maxnc[w] for w in range(nw)]
# Compute the sizes of the Jacobian matrix data for each world
J_cts_sizes = [maxncts[i] * nbd[i] for i in range(nw)]
J_dofs_sizes = [njd[i] * nbd[i] for i in range(nw)]
# Compute the total size of the Jacobian matrix data
total_J_cts_size = sum(J_cts_sizes)
total_J_dofs_size = sum(J_dofs_sizes)
# Compute matrix index offsets of each Jacobian block
J_cts_offsets = [0] * nw
J_dofs_offsets = [0] * nw
for w in range(1, nw):
J_cts_offsets[w] = J_cts_offsets[w - 1] + J_cts_sizes[w - 1]
J_dofs_offsets[w] = J_dofs_offsets[w - 1] + J_dofs_sizes[w - 1]
# Set the device to that of the model if not specified
if device is None:
device = model.device
# Allocate the Jacobian arrays
with wp.ScopedDevice(device):
self._data.J_cts_offsets = wp.array(J_cts_offsets, dtype=int32)
self._data.J_dofs_offsets = wp.array(J_dofs_offsets, dtype=int32)
self._data.J_cts_data = wp.zeros(shape=(total_J_cts_size,), dtype=float32)
self._data.J_dofs_data = wp.zeros(shape=(total_J_dofs_size,), dtype=float32)
def build(
self,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
reset_to_zero: bool = True,
):
"""
Builds the system DoF and constraint Jacobians for the given
data of the provided model, data, limits and contacts containers.
Args:
model (`ModelKamino`):
The model container describing the system structure
and properties, used to compute the Jacobians.
data (`DataKamino`):
The data container describing the time-varying state
of the system, used to compute the Jacobians.
limits (`LimitsKamino`, optional):
The limits container describing the active limits in the system,
used to compute the limit constraint Jacobians if provided.
contacts (`ContactsKamino`, optional):
The contacts container describing the active contacts in the system,
used to compute the contact constraint Jacobians if provided.
reset_to_zero (`bool`, optional):
Whether to reset the Jacobian values to zero before building.\n
If false, the Jacobian values will be accumulated onto existing values.\n
Defaults to `True`.
"""
# Optionally reset the Jacobian array data to zero
if reset_to_zero:
self._data.J_cts_data.zero_()
self._data.J_dofs_data.zero_()
# Build the joint constraints and actuation Jacobians
if model.size.sum_of_num_joints > 0:
wp.launch(
_build_joint_jacobians_dense,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.num_body_dofs,
model.info.bodies_offset,
model.info.joint_dynamic_cts_group_offset,
model.info.joint_kinematic_cts_group_offset,
model.joints.wid,
model.joints.dof_type,
model.joints.dofs_offset,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
model.joints.bid_B,
model.joints.bid_F,
data.joints.p_j,
data.bodies.q_i,
self._data.J_cts_offsets,
self._data.J_dofs_offsets,
# Outputs:
self._data.J_cts_data,
self._data.J_dofs_data,
],
)
# Build the limit constraints Jacobians if a limits data container is provided
if limits is not None and limits.model_max_limits_host > 0:
wp.launch(
_build_limit_jacobians_dense,
dim=limits.model_max_limits_host,
inputs=[
# Inputs:
model.info.num_body_dofs,
model.info.bodies_offset,
data.info.limit_cts_group_offset,
limits.model_active_limits,
limits.model_max_limits_host,
limits.wid,
limits.lid,
limits.bids,
limits.dof,
limits.side,
self._data.J_dofs_offsets,
self._data.J_dofs_data,
self._data.J_cts_offsets,
# Outputs:
self._data.J_cts_data,
],
)
# Build the contact constraints Jacobians if a contacts data container is provided
if contacts is not None and contacts.model_max_contacts_host > 0:
wp.launch(
_build_contact_jacobians_dense,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs:
model.info.num_body_dofs,
model.info.bodies_offset,
data.info.contact_cts_group_offset,
data.bodies.q_i,
contacts.model_active_contacts,
contacts.model_max_contacts_host,
contacts.wid,
contacts.cid,
contacts.bid_AB,
contacts.position_A,
contacts.position_B,
contacts.frame,
self._data.J_cts_offsets,
# Outputs:
self._data.J_cts_data,
],
)
class SparseSystemJacobians:
"""
Container to hold time-varying Jacobians of the system in block-sparse format.
"""
def __init__(
self,
model: ModelKamino | None = None,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
device: wp.DeviceLike = None,
):
"""
Creates a :class:`SparseSystemJacobians` container and allocates the Jacobian data if a model is provided.\n
The Jacobians are stored in block-sparse format using the :class:`BlockSparseLinearOperators` class, and
the non-zero block coordinates are stored as local offsets for each world, joint, limit, and contact.
Args:
model (`ModelKamino`, optional):
The model container describing the system structure and properties, used
to allocate the Jacobian data and compute the non-zero block coordinates.
limits (`LimitsKamino`, optional):
The limits container describing the active limits in the system, used to
compute the non-zero block coordinates of the limit constraint Jacobian.
contacts (`ContactsKamino`, optional):
The contacts container describing the active contacts in the system, used to
compute the non-zero block coordinates of the contact constraint Jacobian.
device (`wp.DeviceLike`, optional):
The device on which to allocate the Jacobian data.
"""
# Declare and initialize the Jacobian data containers
self._J_cts: BlockSparseLinearOperators | None = None
self._J_dofs: BlockSparseLinearOperators | None = None
# Local (in-world) offsets for the non-zero blocks of the constraint and dofs Jacobian for
# each (global) joint, limit, and contact
self._J_cts_joint_nzb_offsets: wp.array | None = None
self._J_cts_limit_nzb_offsets: wp.array | None = None
self._J_cts_contact_nzb_offsets: wp.array | None = None
self._J_dofs_joint_nzb_offsets: wp.array | None = None
# Lists of number of non-zero blocks in each world connected to joint constraints
self._J_cts_num_joint_nzb: wp.array | None = None
# If a model is provided, allocate the Jacobians data
if model is not None:
self.finalize(model=model, limits=limits, contacts=contacts, device=device)
def finalize(
self,
model: ModelKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
device: wp.DeviceLike = None,
):
"""
Finalizes the Jacobian data by allocating the Jacobian arrays and computing the non-zero block coordinates
for each world, joint, limit, and contact based on the provided model, limits, and contacts containers.
Args:
model (`ModelKamino`):
The model container describing the system structure and properties, used
to allocate the Jacobian data and compute the non-zero block coordinates.
limits (`LimitsKamino`, optional):
The limits container describing the active limits in the system, used to
compute the non-zero block coordinates of the limit constraint Jacobian.
contacts (`ContactsKamino`, optional):
The contacts container describing the active contacts in the system, used to
compute the non-zero block coordinates of the contact constraint Jacobian.
device (`wp.DeviceLike`, optional):
The device on which to allocate the Jacobian data.\n
If `None`, the Jacobian data will be allocated on same device as the model.
"""
# Ensure the model container is valid
if model is None:
raise ValueError("`model` is required but got `None`.")
else:
if not isinstance(model, ModelKamino):
raise TypeError(f"`model` is required to be of type `ModelKamino` but got {type(model)}.")
# Ensure the limits container is valid
if limits is not None:
if not isinstance(limits, LimitsKamino):
raise TypeError(f"`limits` is required to be of type `LimitsKamino` but got {type(limits)}.")
# Ensure the contacts container is valid
if contacts is not None:
if not isinstance(contacts, ContactsKamino):
raise TypeError(f"`contacts` is required to be of type `ContactsKamino` but got {type(contacts)}.")
# Extract the constraint and DoF sizes of each world
num_worlds = model.info.num_worlds
num_body_dofs = model.info.num_body_dofs.numpy().tolist()
num_joint_cts = model.info.num_joint_cts.numpy().tolist()
num_joint_dofs = model.info.num_joint_dofs.numpy().tolist()
max_num_limits = (
limits.world_max_limits_host if limits and limits.model_max_limits_host > 0 else [0] * num_worlds
)
max_num_contacts = (
contacts.world_max_contacts_host if contacts and contacts.model_max_contacts_host > 0 else [0] * num_worlds
)
max_num_constraints = [
num_joint_cts[w] + max_num_limits[w] + 3 * max_num_contacts[w] for w in range(num_worlds)
]
# Compute the number of non-zero blocks required for each Jacobian matrix, as well as the
# per-joint and per-dof offsets, and nzb coordinates.
joint_wid = model.joints.wid.numpy()
joint_bid_B = model.joints.bid_B.numpy()
joint_bid_F = model.joints.bid_F.numpy()
joint_num_cts = model.joints.num_cts.numpy()
joint_num_kinematic_cts = model.joints.num_kinematic_cts.numpy()
joint_num_dynamic_cts = model.joints.num_dynamic_cts.numpy()
joint_num_dofs = model.joints.num_dofs.numpy()
joint_q_j_min = model.joints.q_j_min.numpy()
joint_q_j_max = model.joints.q_j_max.numpy()
joint_dynamic_cts_offset = model.joints.dynamic_cts_offset.numpy()
joint_kinematic_cts_offset = model.joints.kinematic_cts_offset.numpy()
joint_dynamic_cts_group_offset = model.info.joint_dynamic_cts_group_offset.numpy()
joint_kinematic_cts_group_offset = model.info.joint_kinematic_cts_group_offset.numpy()
joint_dofs_offset = model.joints.dofs_offset.numpy()
bodies_offset = model.info.bodies_offset.numpy()
J_cts_nnzb_min = [0] * num_worlds
J_cts_nnzb_max = [0] * num_worlds
J_dofs_nnzb = [0] * num_worlds
J_cts_joint_nzb_offsets = [0] * model.size.sum_of_num_joints
J_dofs_joint_nzb_offsets = [0] * model.size.sum_of_num_joints
J_cts_nzb_row = [[] for _ in range(num_worlds)]
J_cts_nzb_col = [[] for _ in range(num_worlds)]
J_dofs_nzb_row = [[] for _ in range(num_worlds)]
J_dofs_nzb_col = [[] for _ in range(num_worlds)]
dofs_start = 0
# Add non-zero blocks for joints and joint limits
for _j in range(model.size.sum_of_num_joints):
w = joint_wid[_j]
J_cts_joint_nzb_offsets[_j] = J_cts_nnzb_min[w]
J_dofs_joint_nzb_offsets[_j] = J_dofs_nnzb[w]
# Joint nzb counts
is_binary = joint_bid_B[_j] > -1
num_adjacent_bodies = 2 if is_binary else 1
num_cts = int(joint_num_cts[_j])
num_dynamic_cts = int(joint_num_dynamic_cts[_j])
num_kinematic_cts = int(joint_num_kinematic_cts[_j])
num_dofs = int(joint_num_dofs[_j])
J_cts_nnzb_min[w] += num_adjacent_bodies * num_cts
J_cts_nnzb_max[w] += num_adjacent_bodies * num_cts
J_dofs_nnzb[w] += num_adjacent_bodies * num_dofs
# Joint nzb coordinates
dynamic_cts_offset = joint_dynamic_cts_offset[_j] + joint_dynamic_cts_group_offset[w]
kinematic_cts_offset = joint_kinematic_cts_offset[_j] + joint_kinematic_cts_group_offset[w]
dofs_offset = joint_dofs_offset[_j]
column_ids = [6 * (joint_bid_F[_j] - bodies_offset[w])]
if is_binary:
column_ids.append(6 * (joint_bid_B[_j] - bodies_offset[w]))
for col_id in column_ids:
for i in range(num_dynamic_cts):
J_cts_nzb_row[w].append(dynamic_cts_offset + i)
J_cts_nzb_col[w].append(col_id)
for col_id in column_ids:
for i in range(num_kinematic_cts):
J_cts_nzb_row[w].append(kinematic_cts_offset + i)
J_cts_nzb_col[w].append(col_id)
for col_id in column_ids:
for i in range(num_dofs):
J_dofs_nzb_row[w].append(dofs_offset + i)
J_dofs_nzb_col[w].append(col_id)
# Limit nzb counts (maximum)
if max_num_limits[w] > 0:
for d_j in range(num_dofs):
if joint_q_j_min[dofs_start + d_j] > float(FLOAT32_MIN) or joint_q_j_max[dofs_start + d_j] < float(
FLOAT32_MAX
):
J_cts_nnzb_max[w] += num_adjacent_bodies
dofs_start += num_dofs
# Add non-zero blocks for contacts
# TODO: Use the candidate geom-pair info to compute maximum possible contact constraint blocks more accurately
if contacts is not None and contacts.model_max_contacts_host > 0:
for w in range(num_worlds):
J_cts_nnzb_max[w] += 2 * 3 * max_num_contacts[w]
# Compute the sizes of the Jacobian matrix data for each world
J_cts_dims_max = [(max_num_constraints[i], num_body_dofs[i]) for i in range(num_worlds)]
J_dofs_dims = [(num_joint_dofs[i], num_body_dofs[i]) for i in range(num_worlds)]
# Flatten nzb coordinates
for w in range(num_worlds):
J_cts_nzb_row[w] += [0] * (J_cts_nnzb_max[w] - J_cts_nnzb_min[w])
J_cts_nzb_col[w] += [0] * (J_cts_nnzb_max[w] - J_cts_nnzb_min[w])
J_cts_nzb_row = [i for rows in J_cts_nzb_row for i in rows]
J_cts_nzb_col = [j for cols in J_cts_nzb_col for j in cols]
J_dofs_nzb_row = [i for rows in J_dofs_nzb_row for i in rows]
J_dofs_nzb_col = [j for cols in J_dofs_nzb_col for j in cols]
# Set the device to that of the model if not specified
if device is None:
device = model.device
# Allocate the block-sparse linear-operator data to represent each system Jacobian
with wp.ScopedDevice(device):
# First allocate the geometric constraint Jacobian
bsm_cts = BlockSparseMatrices(
num_matrices=num_worlds,
nzb_dtype=BlockDType(dtype=float32, shape=(6,)),
device=device,
)
bsm_cts.finalize(max_dims=J_cts_dims_max, capacities=J_cts_nnzb_max)
self._J_cts = BlockSparseLinearOperators(bsm=bsm_cts)
# Then allocate the geometric DoFs Jacobian
bsm_dofs = BlockSparseMatrices(
num_matrices=num_worlds,
nzb_dtype=BlockDType(dtype=float32, shape=(6,)),
device=device,
)
bsm_dofs.finalize(max_dims=J_dofs_dims, capacities=J_dofs_nnzb)
self._J_dofs = BlockSparseLinearOperators(bsm=bsm_dofs)
# Set all constant values into BSMs (corresponding to joint dofs/cts)
if bsm_cts.max_of_max_dims[0] * bsm_cts.max_of_max_dims[1] > 0:
bsm_cts.nzb_row.assign(J_cts_nzb_row)
bsm_cts.nzb_col.assign(J_cts_nzb_col)
bsm_cts.num_cols.assign(num_body_dofs)
if bsm_dofs.max_of_max_dims[0] * bsm_dofs.max_of_max_dims[1] > 0:
bsm_dofs.nzb_row.assign(J_dofs_nzb_row)
bsm_dofs.nzb_col.assign(J_dofs_nzb_col)
bsm_dofs.num_rows.assign(num_joint_dofs)
bsm_dofs.num_cols.assign(num_body_dofs)
bsm_dofs.num_nzb.assign(J_dofs_nnzb)
# Convert per-world nzb offsets to global nzb offsets
J_cts_nzb_start = bsm_cts.nzb_start.numpy()
J_dofs_nzb_start = bsm_dofs.nzb_start.numpy()
for _j in range(model.size.sum_of_num_joints):
w = joint_wid[_j]
J_cts_joint_nzb_offsets[_j] += J_cts_nzb_start[w]
J_dofs_joint_nzb_offsets[_j] += J_dofs_nzb_start[w]
# Create/move precomputed helper arrays to device
self._J_cts_joint_nzb_offsets = wp.array(J_cts_joint_nzb_offsets, dtype=int32, device=device)
self._J_cts_limit_nzb_offsets = wp.zeros(shape=(model.size.sum_of_max_limits,), dtype=int32, device=device)
self._J_cts_contact_nzb_offsets = wp.zeros(
shape=(model.size.sum_of_max_contacts,), dtype=int32, device=device
)
self._J_dofs_joint_nzb_offsets = wp.array(J_dofs_joint_nzb_offsets, dtype=int32, device=device)
self._J_cts_num_joint_nzb = wp.array(J_cts_nnzb_min, dtype=int32, device=device)
def build(
self,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
reset_to_zero: bool = True,
):
"""
Builds the system DoF and constraint Jacobians for the given
data of the provided model, data, limits and contacts containers.
Args:
model (`ModelKamino`):
The model containing the system's kinematic structure.
data (`DataKamino`):
The data container holding the time-varying state of the system.
limits (`LimitsKamino`, optional):
LimitsKamino data container for joint-limit constraints. Needs to
be provided if the Jacobian also has limit constraints.
contacts (`ContactsKamino`, optional):
Contacts data container for contact constraints. Needs to
be provided if the Jacobian also has contact constraints.
reset_to_zero (`bool`, optional):
Whether to reset the Jacobian values to zero before building.\n
If false, the Jacobian values will be accumulated onto existing values.\n
Defaults to `True`.
"""
# Ensure the Jacobians have been finalized
if self._J_cts is None or self._J_dofs is None:
raise RuntimeError("SparseSystemJacobians.build() called before finalize().")
jacobian_cts = self._J_cts.bsm
jacobian_dofs = self._J_dofs.bsm
# Optionally reset the Jacobian array data to zero
if reset_to_zero:
jacobian_cts.zero()
jacobian_dofs.zero()
# Compute active rows of constraints Jacobian
# TODO: Compute num_nzb and offsets for limit and contact entries to avoid atomic_add in those kernels
wp.launch(
_configure_jacobians_sparse,
dim=model.size.num_worlds,
inputs=[
# Inputs:
model.info.num_joint_cts,
data.info.num_limits,
data.info.num_contacts,
# Outputs:
jacobian_cts.num_rows,
],
)
# Build the joint constraints and actuation Jacobians
if model.size.sum_of_num_joints > 0:
wp.launch(
_build_joint_jacobians_sparse,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.joints.dof_type,
model.joints.num_dofs,
model.joints.bid_B,
model.joints.bid_F,
model.joints.dynamic_cts_offset,
data.joints.p_j,
data.bodies.q_i,
self._J_cts_joint_nzb_offsets,
self._J_dofs_joint_nzb_offsets,
# Outputs:
jacobian_cts.nzb_values,
jacobian_dofs.nzb_values,
],
)
# Initialize the number of NZB with the number of NZB for all joints
wp.copy(jacobian_cts.num_nzb, self._J_cts_num_joint_nzb)
# Build the limit constraints Jacobians if a limits data container is provided
if limits is not None and limits.model_max_limits_host > 0:
wp.launch(
_build_limit_jacobians_sparse,
dim=limits.model_max_limits_host,
inputs=[
# Inputs:
model.info.bodies_offset,
model.joints.dofs_offset,
model.joints.num_dofs,
data.info.limit_cts_group_offset,
limits.model_active_limits,
limits.model_max_limits_host,
limits.wid,
limits.jid,
limits.lid,
limits.bids,
limits.dof,
limits.side,
self._J_dofs_joint_nzb_offsets,
jacobian_dofs.nzb_values,
jacobian_cts.nzb_start,
# Outputs:
jacobian_cts.num_nzb,
jacobian_cts.nzb_coords,
jacobian_cts.nzb_values,
self._J_cts_limit_nzb_offsets,
],
)
# Build the contact constraints Jacobians if a contacts data container is provided
if contacts is not None and contacts.model_max_contacts_host > 0:
wp.launch(
_build_contact_jacobians_sparse,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs:
model.info.bodies_offset,
data.info.contact_cts_group_offset,
data.bodies.q_i,
contacts.model_active_contacts,
contacts.model_max_contacts_host,
contacts.wid,
contacts.cid,
contacts.bid_AB,
contacts.position_A,
contacts.position_B,
contacts.frame,
jacobian_cts.nzb_start,
# Outputs:
jacobian_cts.num_nzb,
jacobian_cts.nzb_coords,
jacobian_cts.nzb_values,
self._J_cts_contact_nzb_offsets,
],
)
class ColMajorSparseConstraintJacobians(BlockSparseLinearOperators):
"""
Container to hold a column-major version of the constraint Jacobian
that uses 6x1 blocks instead of the regular 1x6 blocks.
Note:
This version of the Jacobian is more efficient when computing the product of the transpose
Jacobian with a vector.
If a Jacobian matrix has a maximum number of rows of fewer than six, this Jacobian variant
might lead to issues due to potential memory access outside of the allocated arrays. Avoid
using this Jacobian variant for such cases.
"""
def __init__(
self,
model: ModelKamino | None = None,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
jacobians: SparseSystemJacobians | None = None,
device: wp.DeviceLike = None,
):
"""
Constructs a column-major sparse constraint Jacobian.
Args:
model (`ModelKamino`, optional):
The model containing the system's kinematic structure. If provided,
the Jacobian will be immediately finalized with the given model.
limits (`LimitsKamino`, optional):
LimitsKamino data container for joint limit constraints. Needs to
be provided if the regular Jacobian also has limit constraints.
contacts (`ContactsKamino`, optional):
Contacts data container for contact constraints. Needs to be
provided if the regular Jacobian also has contact constraints.
jacobians (`SparseSystemJacobians`, optional):
Row-major sparse Jacobians. If provided, the column-major Jacobian will be
immediately updated with values from the provided Jacobians after allocation.
device (`wp.DeviceLike`, optional):
The device on which to allocate memory for the Jacobian data structures.\n
If `None`, the device of the model will be used.
"""
super().__init__()
self._joint_nzb_offsets: wp.array | None = None
self._num_joint_nzb: wp.array | None = None
if model is not None:
self.finalize(model=model, limits=limits, contacts=contacts, jacobians=jacobians, device=device)
def finalize(
self,
model: ModelKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
jacobians: SparseSystemJacobians | None = None,
device: wp.DeviceLike = None,
):
"""
Initializes the data structure of the column-major constraint Jacobian.
Args:
model (`ModelKamino`):
The model containing the system's kinematic structure.
limits (`LimitsKamino`, optional):
LimitsKamino data container for joint limit constraints. Needs to
be provided if the regular Jacobian also has limit constraints.
contacts (`ContactsKamino`, optional):
Contacts data container for contact constraints. Needs to be
provided if the regular Jacobian also has contact constraints.
jacobians (`SparseSystemJacobians`, optional):
Row-major sparse Jacobians. If provided, the column-major Jacobian will be
immediately updated with values from the provided Jacobians after allocation.
device (`wp.DeviceLike`, optional):
The device on which to allocate memory for the Jacobian data structures.\n
If `None`, the device of the model will be used.
"""
# Extract the constraint and DoF sizes of each world
num_worlds = model.info.num_worlds
num_body_dofs = model.info.num_body_dofs.numpy().tolist()
num_joint_cts = model.info.num_joint_cts.numpy().tolist()
max_num_limits = (
limits.world_max_limits_host if limits and limits.model_max_limits_host > 0 else [0] * num_worlds
)
max_num_contacts = (
contacts.world_max_contacts_host if contacts and contacts.model_max_contacts_host > 0 else [0] * num_worlds
)
max_num_constraints = [
num_joint_cts[w] + max_num_limits[w] + 3 * max_num_contacts[w] for w in range(num_worlds)
]
# Compute the number of non-zero blocks required for Jacobian matrix, using 6 1x6 blocks per
# body per joint/limit/contact
joint_wid = model.joints.wid.numpy()
joint_bid_B = model.joints.bid_B.numpy()
joint_bid_F = model.joints.bid_F.numpy()
joint_num_kinematic_cts = model.joints.num_kinematic_cts.numpy()
joint_num_dynamic_cts = model.joints.num_dynamic_cts.numpy()
joint_num_dofs = model.joints.num_dofs.numpy()
joint_q_j_min = model.joints.q_j_min.numpy()
joint_q_j_max = model.joints.q_j_max.numpy()
joint_dynamic_cts_offset = model.joints.dynamic_cts_offset.numpy()
joint_kinematic_cts_offset = model.joints.kinematic_cts_offset.numpy()
joint_dynamic_cts_group_offset = model.info.joint_dynamic_cts_group_offset.numpy()
joint_kinematic_cts_group_offset = model.info.joint_kinematic_cts_group_offset.numpy()
bodies_offset = model.info.bodies_offset.numpy()
J_cts_cm_nnzb_min = [0] * num_worlds
J_cts_cm_nnzb_max = [0] * num_worlds
J_cts_cm_joint_nzb_offsets = [0] * model.size.sum_of_num_joints
J_cts_nzb_row = [[] for _ in range(num_worlds)]
J_cts_nzb_col = [[] for _ in range(num_worlds)]
dofs_start = 0
# Add non-zero blocks for joints and joint limits
for _j in range(model.size.sum_of_num_joints):
w = joint_wid[_j]
J_cts_cm_joint_nzb_offsets[_j] = J_cts_cm_nnzb_min[w]
# Joint nzb counts
is_binary = joint_bid_B[_j] > -1
num_adjacent_bodies = 2 if is_binary else 1
num_dynamic_cts = joint_num_dynamic_cts[_j]
J_cts_cm_nnzb_min[w] += num_adjacent_bodies * (12 if num_dynamic_cts > 0 else 6)
J_cts_cm_nnzb_max[w] += num_adjacent_bodies * (12 if num_dynamic_cts > 0 else 6)
# Joint nzb coordinates
# Note: compared to the row-major Jacobian, for joints with less than 6 constraints, instead
# of padding the bottom 6 - num_cts rows with zeros, we shift the block start upward and zero-pad
# the top 6 - num_cts rows instead, to prevent exceeding matrix rows.
# We additionally guard against the case where the shift would push the block above the start of
# the matrix. Note that this strategy requires at least 6 rows.
col_ids = [int(6 * (joint_bid_F[_j] - bodies_offset[w]))]
if is_binary:
col_ids.append(int(6 * (joint_bid_B[_j] - bodies_offset[w])))
# Dynamic constraint blocks
if num_dynamic_cts > 0:
dynamic_cts_offset = joint_dynamic_cts_offset[_j] + joint_dynamic_cts_group_offset[w]
dynamic_nzb_row = max(0, dynamic_cts_offset + num_dynamic_cts - 6)
for col_id in col_ids:
for i in range(6):
J_cts_nzb_row[w].append(dynamic_nzb_row)
J_cts_nzb_col[w].append(col_id + i)
# Kinematic constraint blocks
kinematic_cts_offset = joint_kinematic_cts_offset[_j] + joint_kinematic_cts_group_offset[w]
num_kinematic_cts = int(joint_num_kinematic_cts[_j])
kinematic_nzb_row = max(0, kinematic_cts_offset + num_kinematic_cts - 6)
for col_id in col_ids:
for i in range(6):
J_cts_nzb_row[w].append(kinematic_nzb_row)
J_cts_nzb_col[w].append(col_id + i)
# Limit nzb counts (maximum)
if max_num_limits[w] > 0:
for d_j in range(joint_num_dofs[_j]):
if joint_q_j_min[dofs_start + d_j] > float(FLOAT32_MIN) or joint_q_j_max[dofs_start + d_j] < float(
FLOAT32_MAX
):
J_cts_cm_nnzb_max[w] += 6 * num_adjacent_bodies
dofs_start += joint_num_dofs[_j]
# Add non-zero blocks for contacts
# TODO: Use the candidate geom-pair info to compute maximum possible contact constraint blocks more accurately
if contacts is not None and contacts.model_max_contacts_host > 0:
for w in range(num_worlds):
J_cts_cm_nnzb_max[w] += 12 * max_num_contacts[w]
# Compute the sizes of the Jacobian matrix data for each world
J_cts_cm_dims_max = [(max_num_constraints[i], num_body_dofs[i]) for i in range(num_worlds)]
# Flatten nzb coordinates
for w in range(num_worlds):
J_cts_nzb_row[w] += [0] * (J_cts_cm_nnzb_max[w] - J_cts_cm_nnzb_min[w])
J_cts_nzb_col[w] += [0] * (J_cts_cm_nnzb_max[w] - J_cts_cm_nnzb_min[w])
J_cts_nzb_row = [i for rows in J_cts_nzb_row for i in rows]
J_cts_nzb_col = [j for cols in J_cts_nzb_col for j in cols]
# Set the device to that of the model if not specified
if device is None:
device = model.device
# Allocate the block-sparse linear-operator data to represent each system Jacobian
with wp.ScopedDevice(device):
# Allocate the column-major constraint Jacobian
self.bsm = BlockSparseMatrices(
num_matrices=num_worlds,
nzb_dtype=BlockDType(dtype=float32, shape=(6, 1)),
device=device,
)
self.bsm.finalize(max_dims=J_cts_cm_dims_max, capacities=J_cts_cm_nnzb_max)
# Set all constant values into BSM
if self.bsm.max_of_max_dims[0] * self.bsm.max_of_max_dims[1] > 0:
self.bsm.nzb_row.assign(J_cts_nzb_row)
self.bsm.nzb_col.assign(J_cts_nzb_col)
self.bsm.num_cols.assign(num_body_dofs)
# Convert per-world nzb offsets to global nzb offsets
nzb_start = self.bsm.nzb_start.numpy()
for _j in range(model.size.sum_of_num_joints):
w = joint_wid[_j]
J_cts_cm_joint_nzb_offsets[_j] += nzb_start[w]
# Move precomputed helper arrays to device
self._joint_nzb_offsets = wp.array(J_cts_cm_joint_nzb_offsets, dtype=int32, device=device)
self._num_joint_nzb = wp.array(J_cts_cm_nnzb_min, dtype=int32, device=device)
if jacobians is not None:
self.update(model=model, jacobians=jacobians, limits=limits, contacts=contacts)
def update(
self,
model: ModelKamino,
jacobians: SparseSystemJacobians,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
):
"""
Fills the column-major constraint Jacobian with the
values of an already assembled row-major Jacobian.
Args:
jacobians (`SparseSystemJacobians`):
The row-major sparse system Jacobians containing the constraint Jacobians.
model (`ModelKamino`):
The model containing the system's kinematic structure.
limits (`LimitsKamino`, optional):
LimitsKamino data container for joint limit constraints. Needs to be
provided if the regular Jacobian also has limit constraints.
contacts (`ContactsKamino`, optional):
Contacts data container for contact constraints. Needs to be
provided if the regular Jacobian also has contact constraints.
Note:
The finalize() method must be called before update() to allocate the necessary data structures.
The dimensions of the column-major Jacobian will be set to match the input row-major Jacobian.
"""
J_cts = jacobians._J_cts.bsm
# Set dimensions from input Jacobian
self.bsm.dims.assign(J_cts.dims)
# Update the joint constraints Jacobians
if model.size.sum_of_num_joints > 0:
wp.launch(
kernel=_update_col_major_joint_jacobians,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.bid_B,
jacobians._J_cts_joint_nzb_offsets,
J_cts.nzb_coords,
J_cts.nzb_values,
self._joint_nzb_offsets,
# Outputs:
self.bsm.nzb_values,
],
)
# Initialize the number of NZB with the number of NZB for all joints
wp.copy(self.bsm.num_nzb, self._num_joint_nzb)
# Update the limit constraints Jacobians if a limits data container is provided
if limits is not None and limits.model_max_limits_host > 0:
wp.launch(
_update_col_major_limit_jacobians,
dim=limits.model_max_limits_host,
inputs=[
# Inputs:
limits.model_active_limits,
limits.model_max_limits_host,
limits.wid,
limits.bids,
jacobians._J_cts_limit_nzb_offsets,
J_cts.nzb_coords,
J_cts.nzb_values,
self.bsm.nzb_start,
# Outputs:
self.bsm.num_nzb,
self.bsm.nzb_coords,
self.bsm.nzb_values,
],
)
# Build the contact constraints Jacobians if a contacts data container is provided
if contacts is not None and contacts.model_max_contacts_host > 0:
wp.launch(
_update_col_major_contact_jacobians,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs:
contacts.model_active_contacts,
contacts.model_max_contacts_host,
contacts.wid,
contacts.bid_AB,
jacobians._J_cts_contact_nzb_offsets,
J_cts.nzb_coords,
J_cts.nzb_values,
self.bsm.nzb_start,
# Outputs:
self.bsm.num_nzb,
self.bsm.nzb_coords,
self.bsm.nzb_values,
],
)
###
# Utilities
###
SystemJacobiansType = DenseSystemJacobians | SparseSystemJacobians
"""A utility type union of te supported system Jacobian container types."""
================================================
FILE: newton/_src/solvers/kamino/_src/kinematics/joints.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Kinematics: Joints
"""
from __future__ import annotations
from functools import cache
import warp as wp
from ..core.data import DataKamino
from ..core.joints import JointActuationType, JointCorrectionMode, JointDoFType
from ..core.math import (
FLOAT32_MAX,
TWO_PI,
quat_log,
quat_to_euler_xyz,
quat_to_vec4,
quat_twist_angle,
screw,
screw_angular,
screw_linear,
squared_norm,
)
from ..core.model import ModelKamino
from ..core.types import (
float32,
int32,
mat33f,
quatf,
transformf,
vec1f,
vec2f,
vec3f,
vec4f,
vec6f,
vec7f,
)
###
# Module interface
###
__all__ = [
"compute_joints_data",
"extract_actuators_state_from_joints",
"extract_joints_state_from_actuators",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
DEFAULT_LIMIT_V1F = vec1f(FLOAT32_MAX)
DEFAULT_LIMIT_V2F = vec2f(FLOAT32_MAX)
DEFAULT_LIMIT_V3F = vec3f(FLOAT32_MAX)
DEFAULT_LIMIT_V4F = vec4f(FLOAT32_MAX)
DEFAULT_LIMIT_V7F = vec7f(FLOAT32_MAX)
###
# Functions - Coordinate Correction
###
@wp.func
def correct_rotational_coord(q_j_in: float32, q_j_ref: float32 = 0.0, q_j_limit: float32 = FLOAT32_MAX) -> float32:
"""
Corrects a rotational joint coordinate to be as close as possible to a reference coordinate.
"""
q_j_in += wp.round((q_j_ref - q_j_in) / TWO_PI) * TWO_PI
q_j_in = wp.mod(q_j_in, q_j_limit)
return q_j_in
@wp.func
def correct_quat_vector_coord(q_j_in: vec4f, q_j_ref: vec4f) -> vec4f:
"""
Corrects a quaternion joint coordinate to be as close as possible to a reference coordinate.
This ensures that the quaternion `q_j_in` is chosen such that it is
closer to the reference quaternion `q_j_ref`, accounting for the fact
that quaternions `q` and `-q` represent the same rotation.
"""
if squared_norm(q_j_in + q_j_ref) < squared_norm(q_j_in - q_j_ref):
q_j_in *= -1.0
return q_j_in
@wp.func
def correct_joint_coord_free(q_j_in: vec7f, q_j_ref: vec7f, q_j_limit: vec7f = DEFAULT_LIMIT_V7F) -> vec7f:
"""Corrects the orientation quaternion coordinate of a free joint."""
q_j_in[0:4] = correct_quat_vector_coord(q_j_in[0:4], q_j_ref[0:4])
return q_j_in
@wp.func
def correct_joint_coord_revolute(q_j_in: vec1f, q_j_ref: vec1f, q_j_limit: vec1f = DEFAULT_LIMIT_V1F) -> vec1f:
"""Corrects the rotational joint coordinate."""
q_j_in[0] = correct_rotational_coord(q_j_in[0], q_j_ref[0], q_j_limit[0])
return q_j_in
@wp.func
def correct_joint_coord_prismatic(q_j_in: vec1f, q_j_ref: vec1f, q_j_limit: vec1f = DEFAULT_LIMIT_V1F) -> vec1f:
"""No correction needed for prismatic coordinates."""
return q_j_in
@wp.func
def correct_joint_coord_cylindrical(q_j_in: vec2f, q_j_ref: vec2f, q_j_limit: vec2f = DEFAULT_LIMIT_V2F) -> vec2f:
"""Corrects only the rotational joint coordinate."""
q_j_in[1] = correct_rotational_coord(q_j_in[1], q_j_ref[1], q_j_limit[1])
return q_j_in
@wp.func
def correct_joint_coord_universal(q_j_in: vec2f, q_j_ref: vec2f, q_j_limit: vec2f = DEFAULT_LIMIT_V2F) -> vec2f:
"""Corrects each of the two rotational joint coordinates individually."""
q_j_in[0] = correct_rotational_coord(q_j_in[0], q_j_ref[0], q_j_limit[0])
q_j_in[1] = correct_rotational_coord(q_j_in[1], q_j_ref[1], q_j_limit[1])
return q_j_in
@wp.func
def correct_joint_coord_spherical(q_j_in: vec4f, q_j_ref: vec4f, q_j_limit: vec4f = DEFAULT_LIMIT_V4F) -> vec4f:
"""Corrects a quaternion joint coordinate to be as close as possible to a reference."""
return correct_quat_vector_coord(q_j_in, q_j_ref)
@wp.func
def correct_joint_coord_gimbal(q_j_in: vec3f, q_j_ref: vec3f, q_j_limit: vec3f = DEFAULT_LIMIT_V3F) -> vec3f:
"""Corrects each of the XYZ Euler angles individually."""
q_j_in[0] = correct_rotational_coord(q_j_in[0], q_j_ref[0], q_j_limit[0])
q_j_in[1] = correct_rotational_coord(q_j_in[1], q_j_ref[1], q_j_limit[1])
q_j_in[2] = correct_rotational_coord(q_j_in[2], q_j_ref[2], q_j_limit[2])
return q_j_in
@wp.func
def correct_joint_coord_cartesian(q_j_in: vec3f, q_j_ref: vec3f, q_j_limit: vec3f = DEFAULT_LIMIT_V3F) -> vec3f:
"""No correction needed for Cartesian coordinates."""
return q_j_in
def get_joint_coord_correction_function(dof_type: JointDoFType):
"""
Retrieves the function to correct joint
coordinates based on the type of joint DoF.
"""
if dof_type == JointDoFType.FREE:
return correct_joint_coord_free
elif dof_type == JointDoFType.REVOLUTE:
return correct_joint_coord_revolute
elif dof_type == JointDoFType.PRISMATIC:
return correct_joint_coord_prismatic
elif dof_type == JointDoFType.CYLINDRICAL:
return correct_joint_coord_cylindrical
elif dof_type == JointDoFType.UNIVERSAL:
return correct_joint_coord_universal
elif dof_type == JointDoFType.SPHERICAL:
return correct_joint_coord_spherical
elif dof_type == JointDoFType.GIMBAL:
return correct_joint_coord_gimbal
elif dof_type == JointDoFType.CARTESIAN:
return correct_joint_coord_cartesian
elif dof_type == JointDoFType.FIXED:
return None
else:
raise ValueError(f"Unknown joint DoF type: {dof_type}")
###
# Functions - Coordinate Mappings
###
@wp.func
def map_to_joint_coords_free(j_r_j: vec3f, j_q_j: quatf) -> vec7f:
"""Returns the full 7D representation of joint pose (3D translation + 4D rotation)."""
# TODO: Is there a more efficient way to construct a vec7f?
return vec7f(j_r_j[0], j_r_j[1], j_r_j[2], j_q_j.x, j_q_j.y, j_q_j.z, j_q_j.w)
@wp.func
def map_to_joint_coords_revolute(j_r_j: vec3f, j_q_j: quatf) -> vec1f:
"""Returns the 1D rotation angle about the local X-axis."""
# Measure rotation around the x-axis only
axis = vec3f(1.0, 0.0, 0.0)
return vec1f(quat_twist_angle(j_q_j, axis))
@wp.func
def map_to_joint_coords_prismatic(j_r_j: vec3f, j_q_j: quatf) -> vec1f:
"""Returns the 1D translation distance along the local X-axis."""
return vec1f(j_r_j[0])
@wp.func
def map_to_joint_coords_cylindrical(j_r_j: vec3f, j_q_j: quatf) -> vec2f:
"""Returns the 2D vector of translation and rotation about the local X-axis."""
j_p_j = quat_log(j_q_j)
return vec2f(j_r_j[0], j_p_j[0])
@wp.func
def map_to_joint_coords_universal(j_r_j: vec3f, j_q_j: quatf) -> vec2f:
"""Returns the 2D vector of joint angles for the two revolute DoFs."""
# TODO: Fix this so that the order of rotations is consistent
j_theta_j = quat_log(j_q_j)
return j_theta_j[0:2]
@wp.func
def map_to_joint_coords_spherical(j_r_j: vec3f, j_q_j: quatf) -> vec4f:
"""Returns the 4D unit-quaternion representing the joint rotation."""
return quat_to_vec4(j_q_j)
@wp.func
def map_to_joint_coords_gimbal(j_r_j: vec3f, j_q_j: quatf) -> vec3f:
"""Returns the 3D XYZ Euler angles (roll, pitch, yaw)."""
# How can we make this safer?
return quat_to_euler_xyz(j_q_j)
@wp.func
def map_to_joint_coords_cartesian(j_r_j: vec3f, j_q_j: quatf) -> vec3f:
"""Returns the 3D translational."""
return j_r_j
def get_joint_coords_mapping_function(dof_type: JointDoFType):
"""
Retrieves the function to map joint relative poses to
joint coordinates based on the type of joint DoF.
"""
if dof_type == JointDoFType.FREE:
return map_to_joint_coords_free
elif dof_type == JointDoFType.REVOLUTE:
return map_to_joint_coords_revolute
elif dof_type == JointDoFType.PRISMATIC:
return map_to_joint_coords_prismatic
elif dof_type == JointDoFType.CYLINDRICAL:
return map_to_joint_coords_cylindrical
elif dof_type == JointDoFType.UNIVERSAL:
return map_to_joint_coords_universal
elif dof_type == JointDoFType.SPHERICAL:
return map_to_joint_coords_spherical
elif dof_type == JointDoFType.GIMBAL:
return map_to_joint_coords_gimbal
elif dof_type == JointDoFType.CARTESIAN:
return map_to_joint_coords_cartesian
elif dof_type == JointDoFType.FIXED:
return None
else:
raise ValueError(f"Unknown joint DoF type: {dof_type}")
###
# Functions - Constraint residual
###
@wp.func
def joint_constraint_angular_residual_free(j_q_j: quatf) -> vec3f:
return vec3f(0.0, 0.0, 0.0)
@wp.func
def joint_constraint_angular_residual_revolute(j_q_j: quatf) -> vec3f:
"""Returns the joint constraint residual for a revolute joint that rotates about the local X-axis."""
# x-axis attached to the base body
j_x_B = vec3f(1.0, 0.0, 0.0)
# x-axis attached to the follower body, expressed in the joint frame on the base body.
j_x_F = wp.quat_rotate(j_q_j, j_x_B)
# Residual vector = sin(res_angle) * axis, where axis only has y, and z components.
# For small angles this is equal to res_angle * axis
return wp.cross(j_x_B, j_x_F)
@wp.func
def joint_constraint_angular_residual_universal(j_q_j: quatf) -> vec3f:
"""Returns the joint constraint residual for a universal joint."""
# TODO: Fix, using log-based residual as a placeholder
return quat_log(j_q_j)
@wp.func
def joint_constraint_angular_residual_fixed(j_q_j: quatf) -> vec3f:
"""Returns the joint constraint residual for a fixed joint."""
return quat_log(j_q_j)
def get_joint_constraint_angular_residual_function(dof_type: JointDoFType):
"""
Retrieves the function that computes the joint constraint residual as a 6D local vector
"""
# Use the fixed joint residual as the generic implementation.
if dof_type == JointDoFType.FREE:
return joint_constraint_angular_residual_free
elif dof_type == JointDoFType.REVOLUTE:
return joint_constraint_angular_residual_revolute
elif dof_type == JointDoFType.PRISMATIC:
return joint_constraint_angular_residual_fixed
elif dof_type == JointDoFType.CYLINDRICAL:
return joint_constraint_angular_residual_revolute
elif dof_type == JointDoFType.UNIVERSAL:
return joint_constraint_angular_residual_universal
elif dof_type == JointDoFType.SPHERICAL:
return joint_constraint_angular_residual_free
elif dof_type == JointDoFType.GIMBAL:
return joint_constraint_angular_residual_free
elif dof_type == JointDoFType.CARTESIAN:
return joint_constraint_angular_residual_fixed
elif dof_type == JointDoFType.FIXED:
return joint_constraint_angular_residual_fixed
else:
raise ValueError(f"Unknown joint DoF type: {dof_type}")
###
# Functions - State Writes
###
def make_typed_write_joint_data(dof_type: JointDoFType, correction: JointCorrectionMode = JointCorrectionMode.TWOPI):
"""
Generates functions to store the joint state according to the
constraint and DoF dimensions specific to the type of joint.
"""
# Retrieve the joint constraint and DoF axes
dof_axes = dof_type.dofs_axes
cts_axes = dof_type.cts_axes
# Retrieve the number of constraints and dofs
num_coords = dof_type.num_coords
num_dofs = dof_type.num_dofs
num_cts = dof_type.num_cts
# Define a vector type for the joint coordinates
_coordsvec = dof_type.coords_storage_type
# Define the coordinate bound for correction
q_j_limit = _coordsvec(dof_type.coords_bound(correction)) if _coordsvec is not None else None
# Generate a joint type-specific function to write the
# computed joint state into the model data arrays
@wp.func
def _write_typed_joint_data(
# Inputs:
cts_offset: int32, # Index offset of the joint constraints
dofs_offset: int32, # Index offset of the joint DoFs
coords_offset: int32, # Index offset of the joint coordinates
j_r_j: vec3f, # 3D vector of the joint-local relative pose
j_q_j: quatf, # 4D unit-quaternion of the joint-local relative pose
j_u_j: vec6f, # 6D vector ofthe joint-local relative twist
q_j_p: wp.array(dtype=float32), # Reference joint coordinates for correction
# Outputs:
r_j_out: wp.array(dtype=float32), # Flat array of joint constraint residuals
dr_j_out: wp.array(dtype=float32), # Flat array of joint constraint velocities
q_j_out: wp.array(dtype=float32), # Flat array of joint DoF coordinates
dq_j_out: wp.array(dtype=float32), # Flat array of joint DoF velocities
):
# Only write the constraint residual and velocity if the joint defines constraints
# NOTE: This will be disabled for free joints
if wp.static(num_cts > 0):
# Construct a 6D residual vector
j_theta_j = wp.static(get_joint_constraint_angular_residual_function(dof_type))(j_q_j)
j_p_j = screw(j_r_j, j_theta_j)
# Store the joint constraint residuals
for j in range(num_cts):
r_j_out[cts_offset + j] = j_p_j[cts_axes[j]]
dr_j_out[cts_offset + j] = j_u_j[cts_axes[j]]
# Only write the DoF coordinates and velocities if the joint defines DoFs
# NOTE: This will be disabled for fixed joints
if wp.static(num_dofs > 0):
# Map the joint relative pose to joint DoF coordinates
q_j = wp.static(get_joint_coords_mapping_function(dof_type))(j_r_j, j_q_j)
# Optionally generate code to correct the joint coordinates
if wp.static(correction != JointCorrectionMode.NONE):
q_j_prev = _coordsvec()
for j in range(num_coords):
q_j_prev[j] = q_j_p[coords_offset + j]
q_j = wp.static(get_joint_coord_correction_function(dof_type))(q_j, q_j_prev, q_j_limit)
# Store the joint DoF coordinates
for j in range(num_coords):
q_j_out[coords_offset + j] = q_j[j]
# Store the joint DoF velocities
for j in range(num_dofs):
dq_j_out[dofs_offset + j] = j_u_j[dof_axes[j]]
# Return the function
return _write_typed_joint_data
def make_write_joint_data(correction: JointCorrectionMode = JointCorrectionMode.TWOPI):
"""
Generates functions to store the joint state according to the
constraint and DoF dimensions specific to the type of joint.
"""
@wp.func
def _write_joint_data(
# Inputs:
dof_type: int32,
cts_offset: int32,
dofs_offset: int32,
coords_offset: int32,
j_r_j: vec3f,
j_q_j: quatf,
j_u_j: vec6f,
q_j_p: wp.array(dtype=float32),
# Outputs:
data_r_j: wp.array(dtype=float32),
data_dr_j: wp.array(dtype=float32),
data_q_j: wp.array(dtype=float32),
data_dq_j: wp.array(dtype=float32),
):
"""
Stores the joint constraint residuals and DoF motion based on the joint type.
Args:
dof_type (int32): The type of joint DoF.
cts_offset (int32): Index offset of the joint constraints.
dofs_offset (int32): Index offset of the joint DoFs.
coords_offset (int32): Index offset of the joint coordinates.
j_r_j (vec3f): 3D vector of the joint-local relative translation.
j_q_j (quatf): 4D unit-quaternion of the joint-local relative rotation.
j_u_j (vec6f): 6D vector of the joint-local relative twist.
data_r_j (wp.array): Flat array of joint constraint residuals.
data_dr_j (wp.array): Flat array of joint constraint residuals.
data_q_j (wp.array): Flat array of joint DoF coordinates.
data_dq_j (wp.array): Flat array of joint DoF velocities.
"""
# TODO: Use wp.static to include conditionals at compile time based on the joint types present in the builder
if dof_type == JointDoFType.REVOLUTE:
wp.static(make_typed_write_joint_data(JointDoFType.REVOLUTE, correction))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
elif dof_type == JointDoFType.PRISMATIC:
wp.static(make_typed_write_joint_data(JointDoFType.PRISMATIC))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
elif dof_type == JointDoFType.CYLINDRICAL:
wp.static(make_typed_write_joint_data(JointDoFType.CYLINDRICAL, correction))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
elif dof_type == JointDoFType.UNIVERSAL:
wp.static(make_typed_write_joint_data(JointDoFType.UNIVERSAL, correction))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
elif dof_type == JointDoFType.SPHERICAL:
wp.static(make_typed_write_joint_data(JointDoFType.SPHERICAL))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
elif dof_type == JointDoFType.GIMBAL:
wp.static(make_typed_write_joint_data(JointDoFType.GIMBAL, correction))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
elif dof_type == JointDoFType.CARTESIAN:
wp.static(make_typed_write_joint_data(JointDoFType.CARTESIAN))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
elif dof_type == JointDoFType.FIXED:
wp.static(make_typed_write_joint_data(JointDoFType.FIXED))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
elif dof_type == JointDoFType.FREE:
wp.static(make_typed_write_joint_data(JointDoFType.FREE))(
cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
# Return the function
return _write_joint_data
###
# Functions - State Computation
###
@wp.func
def compute_joint_pose_and_relative_motion(
T_B_j: transformf,
T_F_j: transformf,
u_B_j: vec6f,
u_F_j: vec6f,
B_r_Bj: vec3f,
F_r_Fj: vec3f,
X_j: mat33f,
) -> tuple[transformf, vec3f, quatf, vec6f]:
"""
Computes the relative motion of a joint given the states of its Base and Follower bodies.
Args:
T_B_j (transformf): The absolute pose of the Base body in world coordinates.
T_F_j (transformf): The absolute pose of the Follower body in world coordinates.
u_B_j (vec6f): The absolute twist of the Base body in world coordinates.
u_F_j (vec6f): The absolute twist of the Follower body in world coordinates.
B_r_Bj (vec3f): The position of the joint frame in the Base body's local coordinates.
F_r_Fj (vec3f): The position of the joint frame in the Follower body's local coordinates.
X_j (mat33f): The joint transformation matrix.
Returns:
tuple[transformf, vec6f, vec6f]: The absolute pose of the joint frame in world coordinates,
and two 6D vectors encoding the relative motion of the bodies in the frame of the joint.
"""
# Joint frame as quaternion
q_X_j = wp.quat_from_matrix(X_j)
# Extract the decomposed state of the Base body
r_B_j = wp.transform_get_translation(T_B_j)
q_B_j = wp.transform_get_rotation(T_B_j)
v_B_j = screw_linear(u_B_j)
omega_B_j = screw_angular(u_B_j)
# Extract the decomposed state of the Follower body
r_F_j = wp.transform_get_translation(T_F_j)
q_F_j = wp.transform_get_rotation(T_F_j)
v_F_j = screw_linear(u_F_j)
omega_F_j = screw_angular(u_F_j)
# Local joint frame quantities
r_Bj = wp.quat_rotate(q_B_j, B_r_Bj)
q_Bj = q_B_j * q_X_j
r_Fj = wp.quat_rotate(q_F_j, F_r_Fj)
q_Fj = q_F_j * q_X_j
# Compute the pose of the joint frame via the Base body
r_j_B = r_B_j + r_Bj
p_j = wp.transformation(r_j_B, q_Bj, dtype=float32)
# Compute the relative pose between the representations of joint frame w.r.t. the two bodies
# NOTE: The pose is decomposed into a translation vector `j_r_j` and a rotation quaternion `j_q_j`
j_r_j = wp.quat_rotate_inv(q_Bj, r_F_j + r_Fj - r_j_B)
j_q_j = wp.quat_inverse(q_Bj) * q_Fj
# Compute the 6D relative twist vector between the representations of joint frame w.r.t. the two bodies
# TODO: How can we simplify this expression and make it more efficient?
j_v_j = wp.quat_rotate_inv(q_Bj, v_F_j - v_B_j + wp.cross(omega_F_j, r_Fj) - wp.cross(omega_B_j, r_Bj))
j_omega_j = wp.quat_rotate_inv(q_Bj, omega_F_j - omega_B_j)
j_u_j = screw(j_v_j, j_omega_j)
# Return the computed joint frame pose and relative motion vectors
return p_j, j_r_j, j_q_j, j_u_j
@wp.func
def compute_and_write_joint_implicit_dynamics(
# Constants:
dt: float32,
coords_offset: int32,
dofs_offset: int32,
num_dynamic_cts: int32,
dynamic_cts_offset: int32,
# Inputs:
model_joint_a_j: wp.array(dtype=float32),
model_joint_b_j: wp.array(dtype=float32),
model_joint_k_p_j: wp.array(dtype=float32),
model_joint_k_d_j: wp.array(dtype=float32),
data_joint_q_j: wp.array(dtype=float32),
data_joint_dq_j: wp.array(dtype=float32),
data_joint_q_j_ref: wp.array(dtype=float32),
data_joint_dq_j_ref: wp.array(dtype=float32),
data_joint_tau_j_ref: wp.array(dtype=float32),
# Outputs:
data_joint_m_j: wp.array(dtype=float32),
data_joint_inv_m_j: wp.array(dtype=float32),
data_joint_dq_b_j: wp.array(dtype=float32),
):
# Iterate over the dynamic constraints of the joint and
# compute and store the implicit dynamics intermediates
# TODO: We currently do not handle implicit dynamics of
# multi-dof joints, but we should generalize this.
for j in range(num_dynamic_cts):
coords_offset_j = coords_offset + j
dofs_offset_j = dofs_offset + j
dynamic_cts_offset_j = dynamic_cts_offset + j
# Retrieve the current joint state
# TODO: How can we avoid the extra memory load and
# instead just get them from `make_write_joint_data`?
q_j = data_joint_q_j[dofs_offset_j]
dq_j = data_joint_dq_j[dofs_offset_j]
# Retrieve the implicit joint dynamics and PD control parameters
a_j = model_joint_a_j[dofs_offset_j]
b_j = model_joint_b_j[dofs_offset_j]
k_p_j = model_joint_k_p_j[dofs_offset_j]
k_d_j = model_joint_k_d_j[dofs_offset_j]
# Retrieve PD control references
pd_q_j_ref = data_joint_q_j_ref[coords_offset_j]
pd_dq_j_ref = data_joint_dq_j_ref[dofs_offset_j]
pd_tau_j_ff = data_joint_tau_j_ref[dofs_offset_j]
# Compute the implicit joint dynamics intermedates
m_j = a_j + dt * (b_j + k_d_j) + dt * dt * k_p_j
inv_m_j = 1.0 / m_j
tau_j = pd_tau_j_ff + k_p_j * (pd_q_j_ref - q_j) + k_d_j * pd_dq_j_ref
h_j = a_j * dq_j + dt * tau_j
dq_b_j = inv_m_j * h_j
# Store the resulting joint dynamics intermadiates
data_joint_m_j[dynamic_cts_offset_j] = m_j
data_joint_inv_m_j[dynamic_cts_offset_j] = inv_m_j
data_joint_dq_b_j[dynamic_cts_offset_j] = dq_b_j
###
# Kernels
###
@cache
def make_compute_joints_data_kernel(correction: JointCorrectionMode = JointCorrectionMode.TWOPI):
"""
Generates the kernel to compute the joint states based on the current body states.
"""
@wp.kernel
def _compute_joints_data(
# Inputs:
model_info_joint_coords_offset: wp.array(dtype=int32),
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_offset: wp.array(dtype=int32),
model_time_dt: wp.array(dtype=float32),
model_joint_wid: wp.array(dtype=int32),
model_joint_dof_type: wp.array(dtype=int32),
model_joint_num_dynamic_cts: wp.array(dtype=int32),
model_joint_coords_offset: wp.array(dtype=int32),
model_joint_dofs_offset: wp.array(dtype=int32),
model_joint_dynamic_cts_offset: wp.array(dtype=int32),
model_joint_kinematic_cts_offset: wp.array(dtype=int32),
model_joint_bid_B: wp.array(dtype=int32),
model_joint_bid_F: wp.array(dtype=int32),
model_joint_B_r_Bj: wp.array(dtype=vec3f),
model_joint_F_r_Fj: wp.array(dtype=vec3f),
model_joint_X_j: wp.array(dtype=mat33f),
model_joint_a_j: wp.array(dtype=float32),
model_joint_b_j: wp.array(dtype=float32),
model_joint_k_p_j: wp.array(dtype=float32),
model_joint_k_d_j: wp.array(dtype=float32),
data_body_q_i: wp.array(dtype=transformf),
data_body_u_i: wp.array(dtype=vec6f),
data_joint_q_j_ref: wp.array(dtype=float32),
data_joint_dq_j_ref: wp.array(dtype=float32),
data_joint_tau_j_ref: wp.array(dtype=float32),
q_j_p: wp.array(dtype=float32),
# Outputs:
data_joint_p_j: wp.array(dtype=transformf),
data_joint_r_j: wp.array(dtype=float32),
data_joint_dr_j: wp.array(dtype=float32),
data_joint_q_j: wp.array(dtype=float32),
data_joint_dq_j: wp.array(dtype=float32),
data_joint_m_j: wp.array(dtype=float32),
data_joint_inv_m_j: wp.array(dtype=float32),
data_joint_dq_b_j: wp.array(dtype=float32),
):
# Retrieve the thread index
jid = wp.tid()
# Retrieve the joint model data
wid = model_joint_wid[jid]
dof_type = model_joint_dof_type[jid]
num_dynamic_cts = model_joint_num_dynamic_cts[jid]
bid_B = model_joint_bid_B[jid]
bid_F = model_joint_bid_F[jid]
B_r_Bj = model_joint_B_r_Bj[jid]
F_r_Fj = model_joint_F_r_Fj[jid]
X_j = model_joint_X_j[jid]
# Retrieve the time step
dt = model_time_dt[wid]
# Retrieve world offsets
# NOTE: We load data together for better memory coalescing
world_coords_offset = model_info_joint_coords_offset[wid]
world_dofs_offset = model_info_joint_dofs_offset[wid]
world_dynamic_cts_offset = model_info_joint_dynamic_cts_offset[wid]
world_kinematic_cts_offset = model_info_joint_kinematic_cts_offset[wid]
# Retrieve joint-specific offsets
# NOTE: We load data together for better memory coalescing
joint_coords_offset = model_joint_coords_offset[jid]
joint_dofs_offset = model_joint_dofs_offset[jid]
joint_dynamic_cts_offset = model_joint_dynamic_cts_offset[jid]
joint_kinematic_cts_offset = model_joint_kinematic_cts_offset[jid]
# Construct offsets from world + current joint offset
coords_offset = world_coords_offset + joint_coords_offset
dofs_offset = world_dofs_offset + joint_dofs_offset
dynamic_cts_offset = world_dynamic_cts_offset + joint_dynamic_cts_offset
kinematic_cts_offset = world_kinematic_cts_offset + joint_kinematic_cts_offset
# If the Base body is the world (bid=-1), use the identity transform (frame
# of the world's origin), otherwise retrieve the Base body's pose and twist
T_B_j = wp.transform_identity(dtype=float32)
u_B_j = vec6f(0.0)
if bid_B > -1:
T_B_j = data_body_q_i[bid_B]
u_B_j = data_body_u_i[bid_B]
# Retrieve the Follower body's pose and twist
T_F_j = data_body_q_i[bid_F]
u_F_j = data_body_u_i[bid_F]
# Compute the joint frame pose and relative motion
p_j, j_r_j, j_q_j, j_u_j = compute_joint_pose_and_relative_motion(
T_B_j, T_F_j, u_B_j, u_F_j, B_r_Bj, F_r_Fj, X_j
)
# Store the absolute pose of the joint frame in world coordinates
data_joint_p_j[jid] = p_j
# Store the joint constraint residuals and motion
wp.static(make_write_joint_data(correction))(
dof_type,
kinematic_cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
q_j_p,
data_joint_r_j,
data_joint_dr_j,
data_joint_q_j,
data_joint_dq_j,
)
# Compute and store the implicit dynamics
# for the dynamic constraints of the joint
compute_and_write_joint_implicit_dynamics(
dt,
coords_offset,
dofs_offset,
num_dynamic_cts,
dynamic_cts_offset,
model_joint_a_j,
model_joint_b_j,
model_joint_k_p_j,
model_joint_k_d_j,
data_joint_q_j,
data_joint_dq_j,
data_joint_q_j_ref,
data_joint_dq_j_ref,
data_joint_tau_j_ref,
data_joint_m_j,
data_joint_inv_m_j,
data_joint_dq_b_j,
)
# Return the kernel
return _compute_joints_data
@wp.kernel
def _extract_actuators_state_from_joints(
# Inputs:
world_mask: wp.array(dtype=int32),
model_info_joint_coords_offset: wp.array(dtype=int32),
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_actuated_coords_offset: wp.array(dtype=int32),
model_info_joint_actuated_dofs_offset: wp.array(dtype=int32),
model_joint_wid: wp.array(dtype=int32),
model_joint_act_type: wp.array(dtype=int32),
model_joint_num_coords: wp.array(dtype=int32),
model_joint_num_dofs: wp.array(dtype=int32),
model_joint_coords_offset: wp.array(dtype=int32),
model_joint_dofs_offset: wp.array(dtype=int32),
model_joint_actuated_coords_offset: wp.array(dtype=int32),
model_joint_actuated_dofs_offset: wp.array(dtype=int32),
joint_q: wp.array(dtype=float32),
joint_u: wp.array(dtype=float32),
# Outputs:
actuator_q: wp.array(dtype=float32),
actuator_u: wp.array(dtype=float32),
):
# Retrieve the joint index from the thread grid
jid = wp.tid()
# Retrieve the world index and actuation type of the joint
wid = model_joint_wid[jid]
act_type = model_joint_act_type[jid]
# Early exit the operation if the joint's world is flagged as skipped or if the joint is not actuated
if world_mask[wid] == 0 or act_type == JointActuationType.PASSIVE:
return
# Retrieve the joint model data
num_coords = model_joint_num_coords[jid]
num_dofs = model_joint_num_dofs[jid]
coords_offset = model_joint_coords_offset[jid]
dofs_offset = model_joint_dofs_offset[jid]
actuated_coords_offset = model_joint_actuated_coords_offset[jid]
actuated_dofs_offset = model_joint_actuated_dofs_offset[jid]
# Retrieve the index offsets of the joint's constraint and DoF dimensions
world_joint_coords_offset = model_info_joint_coords_offset[wid]
world_joint_dofs_offset = model_info_joint_dofs_offset[wid]
world_joint_actuated_coords_offset = model_info_joint_actuated_coords_offset[wid]
world_joint_actuated_dofs_offset = model_info_joint_actuated_dofs_offset[wid]
# Append the index offsets of the world's joint blocks
jq_start = world_joint_coords_offset + coords_offset
jd_start = world_joint_dofs_offset + dofs_offset
aq_start = world_joint_actuated_coords_offset + actuated_coords_offset
ad_start = world_joint_actuated_dofs_offset + actuated_dofs_offset
# TODO: Change to use array slice assignment when supported in Warp
# # Store the actuated joint coordinates and velocities
# actuator_q[aq_start : aq_start + num_coords] = joint_q[jq_start : jq_start + num_coords]
# actuator_u[ad_start : ad_start + num_dofs] = joint_u[jd_start : jd_start + num_dofs]
# Store the actuated joint coordinates and velocities
for j in range(num_coords):
actuator_q[aq_start + j] = joint_q[jq_start + j]
for j in range(num_dofs):
actuator_u[ad_start + j] = joint_u[jd_start + j]
@wp.kernel
def _extract_joints_state_from_actuators(
# Inputs:
world_mask: wp.array(dtype=int32),
model_info_joint_coords_offset: wp.array(dtype=int32),
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_actuated_coords_offset: wp.array(dtype=int32),
model_info_joint_actuated_dofs_offset: wp.array(dtype=int32),
model_joint_wid: wp.array(dtype=int32),
model_joint_act_type: wp.array(dtype=int32),
model_joint_num_coords: wp.array(dtype=int32),
model_joint_num_dofs: wp.array(dtype=int32),
model_joint_coords_offset: wp.array(dtype=int32),
model_joint_dofs_offset: wp.array(dtype=int32),
model_joint_actuated_coords_offset: wp.array(dtype=int32),
model_joint_actuated_dofs_offset: wp.array(dtype=int32),
actuator_q: wp.array(dtype=float32),
actuator_u: wp.array(dtype=float32),
# Outputs:
joint_q: wp.array(dtype=float32),
joint_u: wp.array(dtype=float32),
):
# Retrieve the joint index from the thread grid
jid = wp.tid()
# Retrieve the world index and actuation type of the joint
wid = model_joint_wid[jid]
act_type = model_joint_act_type[jid]
# Early exit the operation if the joint's world is flagged as skipped or if the joint is not actuated
if world_mask[wid] == 0 or act_type == JointActuationType.PASSIVE:
return
# Retrieve the joint model data
num_coords = model_joint_num_coords[jid]
num_dofs = model_joint_num_dofs[jid]
coords_offset = model_joint_coords_offset[jid]
dofs_offset = model_joint_dofs_offset[jid]
actuated_coords_offset = model_joint_actuated_coords_offset[jid]
actuated_dofs_offset = model_joint_actuated_dofs_offset[jid]
# Retrieve the index offsets of the joint's constraint and DoF dimensions
world_joint_coords_offset = model_info_joint_coords_offset[wid]
world_joint_dofs_offset = model_info_joint_dofs_offset[wid]
world_joint_actuated_coords_offset = model_info_joint_actuated_coords_offset[wid]
world_joint_actuated_dofs_offset = model_info_joint_actuated_dofs_offset[wid]
# Append the index offsets of the world's joint blocks
jq_start = world_joint_coords_offset + coords_offset
jd_start = world_joint_dofs_offset + dofs_offset
aq_start = world_joint_actuated_coords_offset + actuated_coords_offset
ad_start = world_joint_actuated_dofs_offset + actuated_dofs_offset
# TODO: Change to use array slice assignment when supported in Warp
# # Store the actuated joint coordinates and velocities
# joint_q[jq_start : jq_start + num_coords] = actuator_q[aq_start : aq_start + num_coords]
# joint_u[jd_start : jd_start + num_dofs] = actuator_u[ad_start : ad_start + num_dofs]
# Store the actuated joint coordinates and velocities
for j in range(num_coords):
joint_q[jq_start + j] = actuator_q[aq_start + j]
for j in range(num_dofs):
joint_u[jd_start + j] = actuator_u[ad_start + j]
###
# Launchers
###
def compute_joints_data(
model: ModelKamino,
data: DataKamino,
q_j_p: wp.array,
correction: JointCorrectionMode = JointCorrectionMode.TWOPI,
) -> None:
"""
Computes the states of the joints based on the current body states.
The computed joint state data includes both the generalized coordinates and velocities
corresponding to the respective degrees of freedom (DoFs), as well as the constraint-space
residuals and velocities of the applied bilateral constraints.
Args:
model (`ModelKamino`):
The model container holding the time-invariant data of the simulation.
q_j_p (`wp.array`):
An array of previous joint DoF coordinates used for coordinate correction.\n
Only used for revolute DoFs of the relevant joints to enforce angle continuity.\n
Shape of ``(sum_of_num_joint_coords,)`` and type :class:`float`.
data (`DataKamino`):
The solver data container holding the internal time-varying state of the simulation.
"""
# Generate the kernel to compute the joint states
# conditioned on the type coordinate correction
_kernel = make_compute_joints_data_kernel(correction)
# Launch the kernel to compute the joint states
wp.launch(
_kernel,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.joint_coords_offset,
model.info.joint_dofs_offset,
model.info.joint_dynamic_cts_offset,
model.info.joint_kinematic_cts_offset,
model.time.dt,
model.joints.wid,
model.joints.dof_type,
model.joints.num_dynamic_cts,
model.joints.coords_offset,
model.joints.dofs_offset,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
model.joints.bid_B,
model.joints.bid_F,
model.joints.B_r_Bj,
model.joints.F_r_Fj,
model.joints.X_j,
model.joints.a_j,
model.joints.b_j,
model.joints.k_p_j,
model.joints.k_d_j,
data.bodies.q_i,
data.bodies.u_i,
data.joints.q_j_ref,
data.joints.dq_j_ref,
data.joints.tau_j_ref,
q_j_p,
# Outputs:
data.joints.p_j,
data.joints.r_j,
data.joints.dr_j,
data.joints.q_j,
data.joints.dq_j,
data.joints.m_j,
data.joints.inv_m_j,
data.joints.dq_b_j,
],
device=model.device,
)
def extract_actuators_state_from_joints(
model: ModelKamino,
world_mask: wp.array,
joint_q: wp.array,
joint_u: wp.array,
actuator_q: wp.array,
actuator_u: wp.array,
):
"""
Extracts the states of the actuated joints from the full joint state arrays.
Only joints that are marked as actuated and belong to worlds
that are not masked will have their states extracted.
Args:
model (`ModelKamino`):
The model container holding the time-invariant data of the simulation.
joint_q (`wp.array`):
The full array of joint coordinates.\n
Shape of ``(sum_of_num_joint_coords,)`` and type :class:`float`.
joint_u (`wp.array`):
The full array of joint velocities.\n
Shape of ``(sum_of_num_joint_dofs,)`` and type :class:`float`.
actuator_q (`wp.array`):
The output array to store the actuated joint coordinates.\n
Shape of ``(sum_of_num_actuated_joint_coords,)`` and type :class:`float`.
actuator_u (`wp.array`):
The output array to store the actuated joint velocities.\n
Shape of ``(sum_of_actuated_joint_dofs,)`` and type :class:`float`.
world_mask (`wp.array`):
An array indicating which worlds are active (1) or skipped (0).\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
wp.launch(
_extract_actuators_state_from_joints,
dim=model.size.sum_of_num_joints,
inputs=[
world_mask,
model.info.joint_coords_offset,
model.info.joint_dofs_offset,
model.info.joint_actuated_coords_offset,
model.info.joint_actuated_dofs_offset,
model.joints.wid,
model.joints.act_type,
model.joints.num_coords,
model.joints.num_dofs,
model.joints.coords_offset,
model.joints.dofs_offset,
model.joints.actuated_coords_offset,
model.joints.actuated_dofs_offset,
joint_q,
joint_u,
],
outputs=[
actuator_q,
actuator_u,
],
device=model.device,
)
def extract_joints_state_from_actuators(
model: ModelKamino,
world_mask: wp.array,
actuator_q: wp.array,
actuator_u: wp.array,
joint_q: wp.array,
joint_u: wp.array,
):
"""
Extracts the states of the actuated joints from the full joint state arrays.
Only joints that are marked as actuated and belong to worlds
that are not masked will have their states extracted.
Args:
model (`ModelKamino`):
The model container holding the time-invariant data of the simulation.
joint_q (`wp.array`):
The full array of joint coordinates.\n
Shape of ``(sum_of_num_joint_coords,)`` and type :class:`float`.
joint_u (`wp.array`):
The full array of joint velocities.\n
Shape of ``(sum_of_num_joint_dofs,)`` and type :class:`float`.
actuator_q (`wp.array`):
The output array to store the actuated joint coordinates.\n
Shape of ``(sum_of_num_actuated_joint_coords,)`` and type :class:`float`.
actuator_u (`wp.array`):
The output array to store the actuated joint velocities.\n
Shape of ``(sum_of_actuated_joint_dofs,)`` and type :class:`float`.
world_mask (`wp.array`):
An array indicating which worlds are active (1) or skipped (0).\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
wp.launch(
_extract_joints_state_from_actuators,
dim=model.size.sum_of_num_joints,
inputs=[
world_mask,
model.info.joint_coords_offset,
model.info.joint_dofs_offset,
model.info.joint_actuated_coords_offset,
model.info.joint_actuated_dofs_offset,
model.joints.wid,
model.joints.act_type,
model.joints.num_coords,
model.joints.num_dofs,
model.joints.coords_offset,
model.joints.dofs_offset,
model.joints.actuated_coords_offset,
model.joints.actuated_dofs_offset,
actuator_q,
actuator_u,
],
outputs=[
joint_q,
joint_u,
],
device=model.device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/kinematics/limits.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Provides data types, operations & interfaces for joint-limit detection."""
from dataclasses import dataclass, field
import warp as wp
from ..core.data import DataKamino
from ..core.joints import JOINT_QMAX, JOINT_QMIN, JointDoFType
from ..core.math import (
quat_from_vec4,
quat_log,
screw,
)
from ..core.model import ModelKamino
from ..core.types import (
float32,
int32,
uint32,
uint64,
vec1f,
vec2f,
vec2i,
vec3f,
vec4f,
vec6f,
vec7f,
)
from ..geometry.keying import build_pair_key2, make_bitmask
from ..utils import logger as msg
###
# Module interface
###
__all__ = ["LimitsKamino", "LimitsKaminoData"]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Containers
###
@dataclass
class LimitsKaminoData:
"""
An SoA-based container to hold time-varying data of a set of active joint-limits.
This container is intended as the final output of limit detectors and as input to solvers.
"""
model_max_limits_host: int = 0
"""
Host-side cache of the maximum number of limits allocated across all worlds.\n
The number of allocated limits in the model is determined by the ModelBuilder when finalizing
a ``ModelKamino``, and is equal to the sum over all finite-valued limits defined by each joint.\n
The single entry is then less than or equal to the total ``num_joint_dofs`` of the entire model.\n
This is cached on the host-side for managing data allocations and setting thread sizes in kernels.
"""
world_max_limits_host: list[int] = field(default_factory=list)
"""
Host-side cache of the maximum number of limits allocated per world.\n
The number of allocated limits per world is determined by the ModelBuilder when finalizing a
``ModelKamino``, and is equal to the sum over all finite-valued limits defined by each joint of each world.\n
Each entry is then less than or equal to the total ``num_joint_dofs`` of the corresponding world.\n
This is cached on the host-side for managing data allocations and setting thread sizes in kernels.
"""
model_max_limits: wp.array | None = None
"""
The maximum number of limits allocated for the model across all worlds.\n
The number of allocated limits in the model is determined by the ModelBuilder when finalizing
a ``ModelKamino``, and is equal to the sum over all finite-valued limits defined by each joint.\n
The single entry is then less than or equal to the total ``num_joint_dofs`` of the entire model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
model_active_limits: wp.array | None = None
"""
The total number of active limits currently active in the model across all worlds.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
world_max_limits: wp.array | None = None
"""
The maximum number of limits allocated per world.\n
The number of allocated limits per world is determined by the ModelBuilder when finalizing a
``ModelKamino``, and is equal to the sum over all finite-valued limits defined by each joint of each world.\n
Each entry is then less than or equal to the total ``num_joint_dofs`` of the corresponding world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
world_active_limits: wp.array | None = None
"""
The total number of active limits currently active per world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
wid: wp.array | None = None
"""
The world index of each limit.\n
Shape of ``(model_max_limits_host,)`` and type :class:`int32`.
"""
lid: wp.array | None = None
"""
The element index of each limit w.r.t its world.\n
Shape of ``(model_max_limits_host,)`` and type :class:`int32`.
"""
jid: wp.array | None = None
"""
The element index of the corresponding joint w.r.t the model.\n
Shape of ``(model_max_limits_host,)`` and type :class:`int32`.
"""
bids: wp.array | None = None
"""
The element indices of the interacting bodies w.r.t the model.\n
Shape of ``(model_max_limits_host,)`` and type :class:`vec2i`.
"""
dof: wp.array | None = None
"""
The DoF indices along which limits are active w.r.t the world.\n
Shape of ``(model_max_limits_host,)`` and type :class:`int32`.
"""
side: wp.array | None = None
"""
The direction (i.e. side) of the active limit.\n
`1.0` for active min limits, `-1.0` for active max limits.\n
Shape of ``(model_max_limits_host,)`` and type :class:`float32`.
"""
r_q: wp.array | None = None
"""
The amount of generalized coordinate violation per joint-limit.\n
Shape of ``(model_max_limits_host,)`` and type :class:`float32`.
"""
key: wp.array | None = None
"""
Integer key uniquely identifying each limit.\n
The per-limit key assignment is implementation-dependent, but is typically
computed from the associated joint index as well as additional information such as:
- limit index w.r.t the associated B/F body-pair\n
Shape of ``(model_max_limits_host,)`` and type :class:`uint64`.
"""
reaction: wp.array | None = None
"""
The constraint reaction per joint-limit.\n
This is to be set by solvers at each step, and also
facilitates limit visualization and warm-starting.\n
Shape of ``(model_max_limits_host,)`` and type :class:`float32`.
"""
velocity: wp.array | None = None
"""
The constraint velocity per joint-limit.\n
This is to be set by solvers at each step, and also
facilitates limit visualization and warm-starting.\n
Shape of ``(model_max_limits_host,)`` and type :class:`float32`.
"""
def clear(self):
"""
Clears the count of active limits.
"""
self.model_active_limits.zero_()
self.world_active_limits.zero_()
def reset(self):
"""
Clears the count of active limits and resets limit data
to sentinel values, indicating an empty set of limits.
"""
self.clear()
self.wid.fill_(-1)
self.jid.fill_(-1)
self.bids.fill_(vec2i(-1, -1))
self.dof.fill_(-1)
self.key.fill_(make_bitmask(63))
self.reaction.zero_()
self.velocity.zero_()
###
# Functions
###
@wp.func
def map_joint_coords_to_dofs_free(q_j: vec7f) -> vec6f:
"""Maps free joint quaternion to a local axes-aligned rotation vector."""
v_j = quat_log(quat_from_vec4(q_j[3:7]))
return screw(q_j[0:3], v_j)
@wp.func
def map_joint_coords_to_dofs_revolute(q_j: vec1f) -> vec1f:
"""No mapping needed for revolute joints."""
return q_j
@wp.func
def map_joint_coords_to_dofs_prismatic(q_j: vec1f) -> vec1f:
"""No mapping needed for prismatic joints."""
return q_j
@wp.func
def map_joint_coords_to_dofs_cylindrical(q_j: vec2f) -> vec2f:
"""No mapping needed for cylindrical joints."""
return q_j
@wp.func
def map_joint_coords_to_dofs_universal(q_j: vec2f) -> vec2f:
"""No mapping needed for universal joints."""
return q_j
@wp.func
def map_joint_coords_to_dofs_spherical(q_j: vec4f) -> vec3f:
"""Maps quaternion coordinates of a spherical
joint to a local axes-aligned rotation vector."""
return quat_log(quat_from_vec4(q_j))
@wp.func
def map_joint_coords_to_dofs_gimbal(q_j: vec3f) -> vec3f:
"""No mapping needed for gimbal joints."""
return q_j
@wp.func
def map_joint_coords_to_dofs_cartesian(q_j: vec3f) -> vec3f:
"""No mapping needed for cartesian joints."""
return q_j
def get_joint_coords_to_dofs_mapping_function(dof_type: JointDoFType):
"""
Retrieves the function to map joint
type-specific coordinates to DoF space.
"""
if dof_type == JointDoFType.FREE:
return map_joint_coords_to_dofs_free
elif dof_type == JointDoFType.REVOLUTE:
return map_joint_coords_to_dofs_revolute
elif dof_type == JointDoFType.PRISMATIC:
return map_joint_coords_to_dofs_prismatic
elif dof_type == JointDoFType.CYLINDRICAL:
return map_joint_coords_to_dofs_cylindrical
elif dof_type == JointDoFType.UNIVERSAL:
return map_joint_coords_to_dofs_universal
elif dof_type == JointDoFType.SPHERICAL:
return map_joint_coords_to_dofs_spherical
elif dof_type == JointDoFType.GIMBAL:
return map_joint_coords_to_dofs_gimbal
elif dof_type == JointDoFType.CARTESIAN:
return map_joint_coords_to_dofs_cartesian
elif dof_type == JointDoFType.FIXED:
return None
else:
raise ValueError(f"Unknown joint DoF type: {dof_type}")
def make_read_joint_coords_map_and_limits(dof_type: JointDoFType):
"""
Generates a function to read the joint type-specific dof-count,
limits, and coordinates, and map the latter to DoF space.
"""
# Retrieve the number of constraints and dofs
num_dofs = dof_type.num_dofs
num_coords = dof_type.num_coords
# Define a vector type for the joint coordinates
coordsvec_type = dof_type.coords_storage_type
# Generate a joint type-specific function to write the
# computed joint state into the model data arrays
@wp.func
def _read_joint_coords_map_and_limits(
# Inputs:
dofs_offset: int32, # Index offset of the joint DoFs
coords_offset: int32, # Index offset of the joint coordinates
model_joint_q_j_min: wp.array(dtype=float32),
model_joint_q_j_max: wp.array(dtype=float32),
state_joints_q_j: wp.array(dtype=float32),
) -> tuple[int32, vec6f, vec6f, vec6f]:
# Statically define the joint DoF counts
d_j = wp.static(num_dofs)
# Pre-allocate joint data for the largest-case (6 DoFs)
q_j_min = vec6f(0.0)
q_j_max = vec6f(0.0)
q_j_map = vec6f(0.0)
q_j = coordsvec_type(0.0)
# Only write the DoF coordinates and velocities if the joint defines DoFs
# NOTE: This will be disabled for fixed joints
if wp.static(num_dofs > 0):
# Read the joint DoF limits
for j in range(num_dofs):
q_j_min[j] = model_joint_q_j_min[dofs_offset + j]
q_j_max[j] = model_joint_q_j_max[dofs_offset + j]
# Read the joint coordinates
for j in range(num_coords):
q_j[j] = state_joints_q_j[coords_offset + j]
# Map the joint coordinates to DoF space
q_j_map[0:num_dofs] = wp.static(get_joint_coords_to_dofs_mapping_function(dof_type))(q_j)
# Return the constructed joint DoF count, limits and mapped coordinates
return d_j, q_j_min, q_j_max, q_j_map
# Return the function
return _read_joint_coords_map_and_limits
@wp.func
def read_joint_coords_map_and_limits(
dof_type: int32,
dofs_offset: int32,
coords_offset: int32,
model_joint_q_j_min: wp.array(dtype=float32),
model_joint_q_j_max: wp.array(dtype=float32),
state_joints_q_j: wp.array(dtype=float32),
) -> tuple[int32, vec6f, vec6f, vec6f]:
if dof_type == JointDoFType.REVOLUTE:
d_j, q_j_min, q_j_max, q_j_map = wp.static(make_read_joint_coords_map_and_limits(JointDoFType.REVOLUTE))(
dofs_offset,
coords_offset,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
elif dof_type == JointDoFType.PRISMATIC:
d_j, q_j_min, q_j_max, q_j_map = wp.static(make_read_joint_coords_map_and_limits(JointDoFType.PRISMATIC))(
dofs_offset,
coords_offset,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
elif dof_type == JointDoFType.CYLINDRICAL:
d_j, q_j_min, q_j_max, q_j_map = wp.static(make_read_joint_coords_map_and_limits(JointDoFType.CYLINDRICAL))(
dofs_offset,
coords_offset,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
elif dof_type == JointDoFType.UNIVERSAL:
d_j, q_j_min, q_j_max, q_j_map = wp.static(make_read_joint_coords_map_and_limits(JointDoFType.UNIVERSAL))(
dofs_offset,
coords_offset,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
elif dof_type == JointDoFType.SPHERICAL:
d_j, q_j_min, q_j_max, q_j_map = wp.static(make_read_joint_coords_map_and_limits(JointDoFType.SPHERICAL))(
dofs_offset,
coords_offset,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
elif dof_type == JointDoFType.GIMBAL:
d_j, q_j_min, q_j_max, q_j_map = wp.static(make_read_joint_coords_map_and_limits(JointDoFType.GIMBAL))(
dofs_offset,
coords_offset,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
elif dof_type == JointDoFType.CARTESIAN:
d_j, q_j_min, q_j_max, q_j_map = wp.static(make_read_joint_coords_map_and_limits(JointDoFType.CARTESIAN))(
dofs_offset,
coords_offset,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
elif dof_type == JointDoFType.FREE:
d_j, q_j_min, q_j_max, q_j_map = wp.static(make_read_joint_coords_map_and_limits(JointDoFType.FREE))(
dofs_offset,
coords_offset,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
else:
d_j = int32(0)
q_j_min = vec6f(0.0)
q_j_max = vec6f(0.0)
q_j_map = vec6f(0.0)
# Return the joint DoF count, limits and mapped coordinates
return d_j, q_j_min, q_j_max, q_j_map
@wp.func
def detect_active_dof_limit(
# Inputs:
model_max_limits: int32,
world_max_limits: int32,
wid: int32,
jid: int32,
dof: int32,
dofid: int32,
bid_B: int32,
bid_F: int32,
q: float32,
qmin: float32,
qmax: float32,
# Outputs:
limits_model_num: wp.array(dtype=int32),
limits_world_num: wp.array(dtype=int32),
limits_wid: wp.array(dtype=int32),
limits_lid: wp.array(dtype=int32),
limits_jid: wp.array(dtype=int32),
limits_bids: wp.array(dtype=vec2i),
limits_dof: wp.array(dtype=int32),
limits_side: wp.array(dtype=float32),
limits_r_q: wp.array(dtype=float32),
limits_key: wp.array(dtype=uint64),
):
# Retrieve the state of the joint
r_min = q - qmin
r_max = qmax - q
exceeds_min = r_min < 0.0
exceeds_max = r_max < 0.0
if exceeds_min or exceeds_max:
mlid = wp.atomic_add(limits_model_num, 0, 1)
wlid = wp.atomic_add(limits_world_num, wid, 1)
if mlid < model_max_limits and wlid < world_max_limits:
# Store the limit data
limits_wid[mlid] = wid
limits_lid[mlid] = wlid
limits_jid[mlid] = jid
limits_bids[mlid] = vec2i(bid_B, bid_F)
limits_dof[mlid] = dofid
limits_side[mlid] = 1.0 if exceeds_min else -1.0
limits_r_q[mlid] = r_min if exceeds_min else r_max
limits_key[mlid] = build_pair_key2(uint32(jid), uint32(dof))
###
# Kernels
###
@wp.kernel
def _detect_active_joint_configuration_limits(
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_coords_offset: wp.array(dtype=int32),
model_joint_wid: wp.array(dtype=int32),
model_joint_dof_type: wp.array(dtype=int32),
model_joint_dofs_offset: wp.array(dtype=int32),
model_joint_coords_offset: wp.array(dtype=int32),
model_joint_bid_B: wp.array(dtype=int32),
model_joint_bid_F: wp.array(dtype=int32),
model_joint_q_j_min: wp.array(dtype=float32),
model_joint_q_j_max: wp.array(dtype=float32),
state_joints_q_j: wp.array(dtype=float32),
limits_model_max: wp.array(dtype=int32),
limits_world_max: wp.array(dtype=int32),
# Outputs:
limits_model_num: wp.array(dtype=int32),
limits_world_num: wp.array(dtype=int32),
limits_wid: wp.array(dtype=int32),
limits_lid: wp.array(dtype=int32),
limits_jid: wp.array(dtype=int32),
limits_bids: wp.array(dtype=vec2i),
limits_dof: wp.array(dtype=int32),
limits_side: wp.array(dtype=float32),
limits_r_q: wp.array(dtype=float32),
limits_key: wp.array(dtype=uint64),
):
# Retrieve the joint index for the current thread
# This will be the index w.r.r the model
jid = wp.tid()
# Retrieve the joint-specific model data
wid = model_joint_wid[jid]
dof_type_j = model_joint_dof_type[jid]
dofs_offset_j = model_joint_dofs_offset[jid]
coords_offset_j = model_joint_coords_offset[jid]
bid_B_j = model_joint_bid_B[jid]
bid_F_j = model_joint_bid_F[jid]
# Retrieve the max limits of the model and world
model_max_limits = limits_model_max[0]
world_max_limits = limits_world_max[wid]
# Skip the joint limits check if:
# - the DoF type is fixed
# - if the world has not limits allocated
# - if the model has not limits allocated
if dof_type_j == JointDoFType.FIXED or world_max_limits == 0 or model_max_limits == 0:
return
# Extract the index offset of the world's joint DoFs w.r.t the model
world_dofs_offset = model_info_joint_dofs_offset[wid]
world_coords_offset = model_info_joint_coords_offset[wid]
# Compute total index offset of the joint's DoFs w.r.t the model
dofs_offset_total = dofs_offset_j + world_dofs_offset
coords_offset_total = coords_offset_j + world_coords_offset
# Read the joint DoF count, limits and coordinates mapped to DoF space
# NOTE: We need to map to DoF space to compare against the limits when
# the joint has non-minimal coordinates (e.g. spherical, free, etc.)
d_j, q_j_min, q_j_max, q_j_map = read_joint_coords_map_and_limits(
dof_type_j,
dofs_offset_total,
coords_offset_total,
model_joint_q_j_min,
model_joint_q_j_max,
state_joints_q_j,
)
# Iterate over each DoF and check if a limit is active
for dof in range(d_j):
detect_active_dof_limit(
# Inputs:
model_max_limits,
world_max_limits,
wid,
jid,
dof,
dofs_offset_j + dof,
bid_B_j,
bid_F_j,
q_j_map[dof],
q_j_min[dof],
q_j_max[dof],
# Outputs:
limits_model_num,
limits_world_num,
limits_wid,
limits_lid,
limits_jid,
limits_bids,
limits_dof,
limits_side,
limits_r_q,
limits_key,
)
###
# Interfaces
###
class LimitsKamino:
"""
A container to hold and manage time-varying joint-limits.
"""
def __init__(
self,
model: ModelKamino | None = None,
device: wp.DeviceLike = None,
):
# The device on which to allocate the limits data
self._device = device
# Declare the joint-limits data container and initialize it to empty
self._data: LimitsKaminoData = LimitsKaminoData()
# Perform memory allocation if max_limits is specified
if model is not None:
self.finalize(model=model, device=device)
###
# Properties
###
@property
def device(self) -> wp.DeviceLike:
"""
Returns the device on which the limits data is allocated.
"""
return self._device
@property
def data(self) -> LimitsKaminoData:
"""
Returns the managed limits data container.
"""
self._assert_has_data()
return self._data
@property
def model_max_limits_host(self) -> int:
"""
Returns the maximum number of limits allocated across all worlds.
"""
self._assert_has_data()
return self._data.model_max_limits_host
@property
def world_max_limits_host(self) -> list[int]:
"""
Returns the maximum number of limits allocated per world.
"""
self._assert_has_data()
return self._data.world_max_limits_host
@property
def model_max_limits(self) -> wp.array:
"""
Returns the total number of maximum limits for the model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.model_max_limits
@property
def model_active_limits(self) -> wp.array:
"""
Returns the total number of active limits for the model.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.model_active_limits
@property
def world_max_limits(self) -> wp.array:
"""
Returns the total number of maximum limits per world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.world_max_limits
@property
def world_active_limits(self) -> wp.array:
"""
Returns the total number of active limits per world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.world_active_limits
@property
def wid(self) -> wp.array:
"""
Returns the world index of each limit.\n
Shape of ``(model_max_limits_host,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.wid
@property
def lid(self) -> wp.array:
"""
Returns the element index of each limit w.r.t its world.\n
Shape of ``(model_max_limits_host,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.lid
@property
def jid(self) -> wp.array:
"""
Returns the element index of the corresponding joint w.r.t the world.\n
Shape of ``(model_max_limits_host,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.jid
@property
def bids(self) -> wp.array:
"""
Returns the element indices of the interacting bodies w.r.t the model.\n
Shape of ``(model_max_limits_host,)`` and type :class:`vec2i`.
"""
self._assert_has_data()
return self._data.bids
@property
def dof(self) -> wp.array:
"""
Returns the DoF indices along which limits are active w.r.t the world.\n
Shape of ``(model_max_limits_host,)`` and type :class:`int32`.
"""
self._assert_has_data()
return self._data.dof
@property
def side(self) -> wp.array:
"""
Returns the direction (i.e. side) of the active limit.\n
`1.0` for active min limits, `-1.0` for active max limits.\n
Shape of ``(model_max_limits_host,)`` and type :class:`float32`.
"""
self._assert_has_data()
return self._data.side
@property
def r_q(self) -> wp.array:
"""
Returns the amount of generalized coordinate violation per joint-limit.\n
Shape of ``(model_max_limits_host,)`` and type :class:`float32`.
"""
self._assert_has_data()
return self._data.r_q
@property
def key(self) -> wp.array:
"""
Returns the integer key uniquely identifying each limit.\n
Shape of ``(model_max_limits_host,)`` and type :class:`uint64`.
"""
self._assert_has_data()
return self._data.key
@property
def reaction(self) -> wp.array:
"""
Returns constraint velocity per joint-limit.\n
Shape of ``(model_max_limits_host,)`` and type :class:`float32`.
"""
self._assert_has_data()
return self._data.reaction
@property
def velocity(self) -> wp.array:
"""
Returns constraint velocity per joint-limit.\n
Shape of ``(model_max_limits_host,)`` and type :class:`float32`.
"""
self._assert_has_data()
return self._data.velocity
###
# Operations
###
def finalize(self, model: ModelKamino, device: wp.DeviceLike = None):
# Ensure the model is valid
if model is None:
raise ValueError("LimitsKamino: model must be specified for allocation (got None)")
elif not isinstance(model, ModelKamino):
raise TypeError("LimitsKamino: model must be an instance of ModelKamino")
# Extract the joint limits allocation sizes from the model
# The memory allocation requires the total number of limits (over multiple worlds)
# as well as the limit capacities for each world. Corresponding sizes are defaulted to 0 (empty).
model_max_limits = 0
world_max_limits = [0] * model.size.num_worlds
joint_wid = model.joints.wid.numpy()
joint_num_dofs = model.joints.num_dofs.numpy()
joint_q_j_min = model.joints.q_j_min.numpy()
joint_q_j_max = model.joints.q_j_max.numpy()
num_joints = len(joint_wid)
dofs_start = 0
for j in range(num_joints):
for dof in range(joint_num_dofs[j]):
if joint_q_j_min[dofs_start + dof] > JOINT_QMIN or joint_q_j_max[dofs_start + dof] < JOINT_QMAX:
model_max_limits += 1
world_max_limits[joint_wid[j]] += 1
dofs_start += joint_num_dofs[j]
# Skip allocation if there are no limits to allocate
if model_max_limits == 0:
msg.debug("LimitsKamino: Skipping joint-limit data allocations since total requested capacity was `0`.")
return
# Override the device if specified
if device is not None:
self._device = device
# Allocate the limits data on the specified device
with wp.ScopedDevice(self._device):
self._data = LimitsKaminoData(
model_max_limits_host=model_max_limits,
world_max_limits_host=world_max_limits,
model_max_limits=wp.array([model_max_limits], dtype=int32),
model_active_limits=wp.zeros(shape=1, dtype=int32),
world_max_limits=wp.array(world_max_limits, dtype=int32),
world_active_limits=wp.zeros(shape=len(world_max_limits), dtype=int32),
wid=wp.zeros(shape=model_max_limits, dtype=int32),
lid=wp.zeros(shape=model_max_limits, dtype=int32),
jid=wp.zeros(shape=model_max_limits, dtype=int32),
bids=wp.zeros(shape=model_max_limits, dtype=vec2i),
dof=wp.zeros(shape=model_max_limits, dtype=int32),
side=wp.zeros(shape=model_max_limits, dtype=float32),
r_q=wp.zeros(shape=model_max_limits, dtype=float32),
key=wp.full(shape=model_max_limits, value=make_bitmask(63), dtype=uint64),
reaction=wp.zeros(shape=model_max_limits, dtype=float32),
velocity=wp.zeros(shape=model_max_limits, dtype=float32),
)
def clear(self):
"""
Clears the active limits count.
"""
if self._data is not None and self._data.model_max_limits_host > 0:
self._data.clear()
def reset(self):
"""
Resets the limits data to sentinel values.
"""
if self._data is not None and self._data.model_max_limits_host > 0:
self._data.reset()
def detect(
self,
model: ModelKamino,
data: DataKamino,
):
"""
Detects the active joint limits in the model and updates the limits data.
Args:
model (ModelKamino): The model to detect limits for.
state (DataKamino): The current state of the model.
"""
# Skip this operation if no contacts data has been allocated
if self._data is None or self._data.model_max_limits_host <= 0:
return
# Ensure the model and state are valid
if model is None:
raise ValueError("LimitsKamino: model must be specified for detection (got None)")
elif not isinstance(model, ModelKamino):
raise TypeError("LimitsKamino: model must be an instance of ModelKamino")
if data is None:
raise ValueError("LimitsKamino: data must be specified for detection (got None)")
elif not isinstance(data, DataKamino):
raise TypeError("LimitsKamino: data must be an instance of DataKamino")
# Ensure the limits data is allocated on the same device as the model
if self._device is not None and self._device != model.device:
raise ValueError(f"LimitsKamino: data device {self._device} does not match model device {model.device}")
# Clear the current limits count
self.clear()
# Launch the detection kernel
wp.launch(
kernel=_detect_active_joint_configuration_limits,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.joint_dofs_offset,
model.info.joint_coords_offset,
model.joints.wid,
model.joints.dof_type,
model.joints.dofs_offset,
model.joints.coords_offset,
model.joints.bid_B,
model.joints.bid_F,
model.joints.q_j_min,
model.joints.q_j_max,
data.joints.q_j,
self._data.model_max_limits,
self._data.world_max_limits,
# Outputs:
self._data.model_active_limits,
self._data.world_active_limits,
self._data.wid,
self._data.lid,
self._data.jid,
self._data.bids,
self._data.dof,
self._data.side,
self._data.r_q,
self._data.key,
],
)
###
# Internals
###
def _assert_has_data(self):
"""
Asserts that the limits data has been allocated.
"""
if self._data is None:
raise ValueError("LimitsKamino: data has not been allocated. Please call 'finalize()' first.")
================================================
FILE: newton/_src/solvers/kamino/_src/kinematics/resets.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Provides a set of operations to reset the state of a physics simulation."""
import warp as wp
from ..core.bodies import transform_body_inertial_properties
from ..core.data import DataKamino
from ..core.math import screw, screw_angular, screw_linear
from ..core.model import ModelKamino
from ..core.state import StateKamino
from ..core.types import float32, int32, mat33f, transformf, vec3f, vec6f
from ..kinematics.joints import compute_joint_pose_and_relative_motion, make_write_joint_data
###
# Module interface
###
__all__ = [
"reset_body_net_wrenches",
"reset_joint_constraint_reactions",
"reset_select_worlds_to_initial_state",
"reset_select_worlds_to_state",
"reset_state_from_base_state",
"reset_state_from_bodies_state",
"reset_state_to_model_default",
"reset_time",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _reset_time_of_select_worlds(
# Inputs:
world_mask: wp.array(dtype=int32),
# Outputs:
data_time: wp.array(dtype=float32),
data_steps: wp.array(dtype=int32),
):
# Retrieve the world index from the 1D thread index
wid = wp.tid()
# Skip resetting time if the world has not been marked for reset
if world_mask[wid] == 0:
return
# Reset both the physical time and step count to zero
data_time[wid] = 0.0
data_steps[wid] = 0
@wp.kernel
def _reset_body_state_of_select_worlds(
# Inputs:
world_mask: wp.array(dtype=int32),
model_body_wid: wp.array(dtype=int32),
model_body_q_i_0: wp.array(dtype=transformf),
model_body_u_i_0: wp.array(dtype=vec6f),
# Outputs:
state_q_i: wp.array(dtype=transformf),
state_u_i: wp.array(dtype=vec6f),
state_w_i: wp.array(dtype=vec6f),
state_w_i_e: wp.array(dtype=vec6f),
):
# Retrieve the body index from the 1D thread index
bid = wp.tid()
# Retrieve the world index for this body
wid = model_body_wid[bid]
# Skip resetting this body if the world has not been marked for reset
if world_mask[wid] == 0:
return
# Retrieve the target state for this body
q_i_0 = model_body_q_i_0[bid]
u_i_0 = model_body_u_i_0[bid]
# Store the reset state in the output arrays and zero-out wrenches
state_q_i[bid] = q_i_0
state_u_i[bid] = u_i_0
state_w_i[bid] = vec6f(0.0)
state_w_i_e[bid] = vec6f(0.0)
@wp.kernel
def _reset_body_state_from_base(
# Inputs:
world_mask: wp.array(dtype=int32),
model_info_base_body_index: wp.array(dtype=int32),
model_body_wid: wp.array(dtype=int32),
model_bodies_q_i_0: wp.array(dtype=transformf),
base_q: wp.array(dtype=transformf),
base_u: wp.array(dtype=vec6f),
# Outputs:
state_q_i: wp.array(dtype=transformf),
state_u_i: wp.array(dtype=vec6f),
state_w_i: wp.array(dtype=vec6f),
):
# Retrieve the body index from the 1D thread index
bid = wp.tid()
# Retrieve the world index for this body
wid = model_body_wid[bid]
# Skip resetting this body if the world has not been marked for reset
if world_mask[wid] == 0:
return
# Retrieve the index of the base body for this world
base_bid = model_info_base_body_index[wid]
# Retrieve the initial pose of the base body
if base_bid >= 0:
q_b_0 = model_bodies_q_i_0[base_bid]
else:
# If there is no base body, use the identity transform
q_b_0 = wp.transform_identity(dtype=float32)
# Retrieve the initial pose for this body
q_i_0 = model_bodies_q_i_0[bid]
# Retrieve the target state of the base body
q_b = base_q[wid]
u_b = base_u[wid]
# Compute the relative pose transform that
# moves the base body to the target pose
X_b = wp.transform_multiply(q_b, wp.transform_inverse(q_b_0))
# Retrieve the position vectors of the base and current body
r_b_0 = wp.transform_get_translation(q_b_0)
r_i_0 = wp.transform_get_translation(q_i_0)
# Decompose the base body's target twist
v_b = screw_linear(u_b)
omega_b = screw_angular(u_b)
# Compute the target pose and twist for this body
q_i = wp.transform_multiply(X_b, q_i_0)
u_i = screw(v_b + wp.cross(omega_b, r_i_0 - r_b_0), omega_b)
# Store the reset state in the output arrays and zero-out wrenches
state_q_i[bid] = q_i
state_u_i[bid] = u_i
state_w_i[bid] = vec6f(0.0)
@wp.kernel
def _reset_joint_state_of_select_worlds(
# Inputs:
world_mask: wp.array(dtype=int32),
model_info_joint_coords_offset: wp.array(dtype=int32),
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_cts_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_group_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_joint_wid: wp.array(dtype=int32),
model_joint_num_coords: wp.array(dtype=int32),
model_joint_num_dofs: wp.array(dtype=int32),
model_joint_num_dynamic_cts: wp.array(dtype=int32),
model_joint_num_kinematic_cts: wp.array(dtype=int32),
model_joint_coords_offset: wp.array(dtype=int32),
model_joint_dofs_offset: wp.array(dtype=int32),
model_joint_dynamic_cts_offset: wp.array(dtype=int32),
model_joint_kinematic_cts_offset: wp.array(dtype=int32),
model_joint_q_j_ref: wp.array(dtype=float32),
# Outputs:
state_q_j: wp.array(dtype=float32),
state_q_j_p: wp.array(dtype=float32),
state_dq_j: wp.array(dtype=float32),
state_lambda_j: wp.array(dtype=float32),
):
# Retrieve the body index from the 1D thread index
jid = wp.tid()
# Retrieve the world index for this body
wid = model_joint_wid[jid]
# Skip resetting this joint if the world has not been marked for reset
if world_mask[wid] == 0:
return
# Retrieve the joint model data
num_coords = model_joint_num_coords[jid]
num_dofs = model_joint_num_dofs[jid]
num_dynamic_cts = model_joint_num_dynamic_cts[jid]
num_kinematic_cts = model_joint_num_kinematic_cts[jid]
coords_offset = model_joint_coords_offset[jid]
dofs_offset = model_joint_dofs_offset[jid]
dynamic_cts_offset = model_joint_dynamic_cts_offset[jid]
kinematic_cts_offset = model_joint_kinematic_cts_offset[jid]
# Retrieve the index offsets of the joint's constraint and DoF dimensions
world_joint_coords_offset = model_info_joint_coords_offset[wid]
world_joint_dofs_offset = model_info_joint_dofs_offset[wid]
world_joint_cts_offset = model_info_joint_cts_offset[wid]
world_joint_dynamic_cts_group_offset = model_info_joint_dynamic_cts_group_offset[wid]
world_joint_kinematic_cts_group_offset = model_info_joint_kinematic_cts_group_offset[wid]
# Append the index offsets of the world's joint blocks
coords_offset += world_joint_coords_offset
dofs_offset += world_joint_dofs_offset
dynamic_cts_offset += world_joint_cts_offset + world_joint_dynamic_cts_group_offset
kinematic_cts_offset += world_joint_cts_offset + world_joint_kinematic_cts_group_offset
# Reset all joint state data
for j in range(num_coords):
q_j_ref = model_joint_q_j_ref[coords_offset + j]
state_q_j[coords_offset + j] = q_j_ref
state_q_j_p[coords_offset + j] = q_j_ref
for j in range(num_dofs):
state_dq_j[dofs_offset + j] = 0.0
for j in range(num_dynamic_cts):
state_lambda_j[dynamic_cts_offset + j] = 0.0
for j in range(num_kinematic_cts):
state_lambda_j[kinematic_cts_offset + j] = 0.0
@wp.kernel
def _reset_bodies_of_select_worlds(
# Inputs:
mask: wp.array(dtype=int32),
# Inputs:
model_bid: wp.array(dtype=int32),
model_i_I_i: wp.array(dtype=mat33f),
model_inv_i_I_i: wp.array(dtype=mat33f),
state_q_i: wp.array(dtype=transformf),
state_u_i: wp.array(dtype=vec6f),
# Outputs:
data_q_i: wp.array(dtype=transformf),
data_u_i: wp.array(dtype=vec6f),
data_I_i: wp.array(dtype=mat33f),
data_inv_I_i: wp.array(dtype=mat33f),
data_w_i: wp.array(dtype=vec6f),
data_w_a_i: wp.array(dtype=vec6f),
data_w_j_i: wp.array(dtype=vec6f),
data_w_l_i: wp.array(dtype=vec6f),
data_w_c_i: wp.array(dtype=vec6f),
data_w_e_i: wp.array(dtype=vec6f),
):
# Retrieve the body index from the 1D thread index
bid = wp.tid()
# Retrieve the world index for this body
wid = model_bid[bid]
# Retrieve the reset flag for the corresponding world
world_has_reset = mask[wid]
# Skip resetting this body if the world has not been marked for reset
if not world_has_reset:
return
# Create a zero-valued vec6 to zero-out wrenches
zero6 = vec6f(0.0)
# Retrieve the target state for this body
q_i_0 = state_q_i[bid]
u_i_0 = state_u_i[bid]
# Retrieve the model data for this body
i_I_i = model_i_I_i[bid]
inv_i_I_i = model_inv_i_I_i[bid]
# Compute the moment of inertia matrices in world coordinates
I_i, inv_I_i = transform_body_inertial_properties(q_i_0, i_I_i, inv_i_I_i)
# Store the reset state and inertial properties
# in the output arrays and zero-out wrenches
data_q_i[bid] = q_i_0
data_u_i[bid] = u_i_0
data_I_i[bid] = I_i
data_inv_I_i[bid] = inv_I_i
data_w_i[bid] = zero6
data_w_a_i[bid] = zero6
data_w_j_i[bid] = zero6
data_w_l_i[bid] = zero6
data_w_c_i[bid] = zero6
data_w_e_i[bid] = zero6
@wp.kernel
def _reset_body_net_wrenches(
# Inputs:
world_mask: wp.array(dtype=int32),
body_wid: wp.array(dtype=int32),
# Outputs:
body_w_i: wp.array(dtype=vec6f),
):
# Retrieve the body index from the 1D thread grid
bid = wp.tid()
# Retrieve the world index for this body
wid = body_wid[bid]
# Skip resetting this body if the world has not been marked for reset
if world_mask[wid] == 0:
return
# Zero-out wrenches
body_w_i[bid] = vec6f(0.0)
@wp.kernel
def _reset_joint_constraint_reactions(
# Inputs:
world_mask: wp.array(dtype=int32),
model_info_joint_cts_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_group_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_joint_wid: wp.array(dtype=int32),
model_joint_num_dynamic_cts: wp.array(dtype=int32),
model_joint_num_kinematic_cts: wp.array(dtype=int32),
model_joint_dynamic_cts_offset: wp.array(dtype=int32),
model_joint_kinematic_cts_offset: wp.array(dtype=int32),
# Outputs:
lambda_j: wp.array(dtype=float32),
):
# Retrieve the joint index from the thread grid
jid = wp.tid()
# Retrieve the world index and actuation type of the joint
wid = model_joint_wid[jid]
# Early exit the operation if the joint's world is flagged as skipped or if the joint is not actuated
if world_mask[wid] == 0:
return
# Retrieve the joint model data
num_dynamic_cts = model_joint_num_dynamic_cts[jid]
num_kinematic_cts = model_joint_num_kinematic_cts[jid]
dynamic_cts_offset = model_joint_dynamic_cts_offset[jid]
kinematic_cts_offset = model_joint_kinematic_cts_offset[jid]
# Retrieve the index offsets of the joint's constraint dimensions
world_joint_cts_offset = model_info_joint_cts_offset[wid]
world_joint_dynamic_cts_group_offset = model_info_joint_dynamic_cts_group_offset[wid]
world_joint_kinematic_cts_group_offset = model_info_joint_kinematic_cts_group_offset[wid]
# Append the index offsets of the world's joint blocks
dynamic_cts_offset += world_joint_cts_offset + world_joint_dynamic_cts_group_offset
kinematic_cts_offset += world_joint_cts_offset + world_joint_kinematic_cts_group_offset
# Reset the joint constraint reactions
for j in range(num_dynamic_cts):
lambda_j[dynamic_cts_offset + j] = 0.0
for j in range(num_kinematic_cts):
lambda_j[kinematic_cts_offset + j] = 0.0
@wp.kernel
def _reset_joints_of_select_worlds(
# Inputs:
reset_constraints: bool,
mask: wp.array(dtype=int32),
model_info_joint_coords_offset: wp.array(dtype=int32),
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_cts_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_group_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_joint_wid: wp.array(dtype=int32),
model_joint_dof_type: wp.array(dtype=int32),
model_joint_num_dynamic_cts: wp.array(dtype=int32),
model_joint_num_kinematic_cts: wp.array(dtype=int32),
model_joint_coords_offset: wp.array(dtype=int32),
model_joint_dofs_offset: wp.array(dtype=int32),
model_joint_dynamic_cts_offset: wp.array(dtype=int32),
model_joint_kinematic_cts_offset: wp.array(dtype=int32),
model_joint_bid_B: wp.array(dtype=int32),
model_joint_bid_F: wp.array(dtype=int32),
model_joint_B_r_Bj: wp.array(dtype=vec3f),
model_joint_F_r_Fj: wp.array(dtype=vec3f),
model_joint_X_j: wp.array(dtype=mat33f),
model_joint_q_j_ref: wp.array(dtype=float32),
state_q_i: wp.array(dtype=transformf),
state_u_i: wp.array(dtype=vec6f),
state_lambda_j: wp.array(dtype=float32),
# Outputs:
data_p_j: wp.array(dtype=transformf),
data_r_j: wp.array(dtype=float32),
data_dr_j: wp.array(dtype=float32),
data_q_j: wp.array(dtype=float32),
data_dq_j: wp.array(dtype=float32),
data_lambda_j: wp.array(dtype=float32),
):
# Retrieve the body index from the 1D thread index
jid = wp.tid()
# Retrieve the world index for this body
wid = model_joint_wid[jid]
# Retrieve the reset flag for the corresponding world
world_has_reset = mask[wid]
# Skip resetting this joint if the world has not been marked for reset
if not world_has_reset:
return
# Retrieve the joint model data
dof_type = model_joint_dof_type[jid]
num_dynamic_cts = model_joint_num_dynamic_cts[jid]
num_kinematic_cts = model_joint_num_kinematic_cts[jid]
coords_offset = model_joint_coords_offset[jid]
dofs_offset = model_joint_dofs_offset[jid]
dynamic_cts_offset = model_joint_dynamic_cts_offset[jid]
kinematic_cts_offset = model_joint_kinematic_cts_offset[jid]
bid_B = model_joint_bid_B[jid]
bid_F = model_joint_bid_F[jid]
B_r_Bj = model_joint_B_r_Bj[jid]
F_r_Fj = model_joint_F_r_Fj[jid]
X_j = model_joint_X_j[jid]
# Retrieve the index offsets of the joint's constraint and DoF dimensions
world_joint_coords_offset = model_info_joint_coords_offset[wid]
world_joint_dofs_offset = model_info_joint_dofs_offset[wid]
world_joint_cts_offset = model_info_joint_cts_offset[wid]
world_joint_dynamic_cts_group_offset = model_info_joint_dynamic_cts_group_offset[wid]
world_joint_kinematic_cts_group_offset = model_info_joint_kinematic_cts_group_offset[wid]
# If the Base body is the world (bid=-1), use the identity transform (frame
# of the world's origin), otherwise retrieve the Base body's pose and twist
T_B_j = wp.transform_identity(dtype=float32)
u_B_j = vec6f(0.0)
if bid_B > -1:
T_B_j = state_q_i[bid_B]
u_B_j = state_u_i[bid_B]
# Retrieve the Follower body's pose and twist
T_F_j = state_q_i[bid_F]
u_F_j = state_u_i[bid_F]
# Append the index offsets of the world's joint blocks
coords_offset += world_joint_coords_offset
dofs_offset += world_joint_dofs_offset
dynamic_cts_offset += world_joint_cts_offset + world_joint_dynamic_cts_group_offset
kinematic_cts_offset += world_joint_cts_offset + world_joint_kinematic_cts_group_offset
# Compute the joint frame pose and relative motion
p_j, j_r_j, j_q_j, j_u_j = compute_joint_pose_and_relative_motion(T_B_j, T_F_j, u_B_j, u_F_j, B_r_Bj, F_r_Fj, X_j)
# Store the absolute pose of the joint frame in world coordinates
data_p_j[jid] = p_j
# Store the joint constraint residuals and motion
wp.static(make_write_joint_data())(
dof_type,
kinematic_cts_offset,
dofs_offset,
coords_offset,
j_r_j,
j_q_j,
j_u_j,
model_joint_q_j_ref,
data_r_j,
data_dr_j,
data_q_j,
data_dq_j,
)
# If requested, reset the joint constraint reactions to zero
if reset_constraints:
for j in range(num_dynamic_cts):
data_lambda_j[dynamic_cts_offset + j] = 0.0
for j in range(num_kinematic_cts):
data_lambda_j[kinematic_cts_offset + j] = 0.0
# Otherwise, copy the target constraint reactions from the target state
else:
for j in range(num_dynamic_cts):
data_lambda_j[dynamic_cts_offset + j] = state_lambda_j[dynamic_cts_offset + j]
for j in range(num_kinematic_cts):
data_lambda_j[kinematic_cts_offset + j] = state_lambda_j[kinematic_cts_offset + j]
###
# Launchers
###
def reset_time(
model: ModelKamino,
time: wp.array,
steps: wp.array,
world_mask: wp.array,
):
wp.launch(
_reset_time_of_select_worlds,
dim=model.size.num_worlds,
inputs=[
# Inputs:
world_mask,
# Outputs:
time,
steps,
],
)
def reset_body_net_wrenches(
model: ModelKamino,
body_w: wp.array,
world_mask: wp.array,
):
"""
Reset the body constraint wrenches of the selected worlds given an array of per-world flags.
Args:
model: Input model container holding the time-invariant data of the system.
body_w: Array of body constraint wrenches to be reset.
world_mask: Array of per-world flags indicating which worlds should be reset.
"""
wp.launch(
_reset_body_net_wrenches,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
world_mask,
model.bodies.wid,
# Outputs:
body_w,
],
)
def reset_joint_constraint_reactions(
model: ModelKamino,
lambda_j: wp.array,
world_mask: wp.array,
):
"""
Resets the joint constraint reaction forces/torques to zero.
This function is typically called at the beginning of a simulation step
to clear out any accumulated reaction forces from the previous step.
Args:
model (ModelKamino):
The model container holding the time-invariant data of the simulation.
lambda_j (wp.array):
The array of joint constraint reaction forces/torques.\n
Shape of ``(sum_of_num_joint_constraints,)`` and type :class:`float`.
world_mask (wp.array):
An array indicating which worlds are active (1) or skipped (0).\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
wp.launch(
_reset_joint_constraint_reactions,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
world_mask,
model.info.joint_cts_offset,
model.info.joint_dynamic_cts_group_offset,
model.info.joint_kinematic_cts_group_offset,
model.joints.wid,
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
# Outputs:
lambda_j,
],
device=model.device,
)
def reset_state_to_model_default(
model: ModelKamino,
state_out: StateKamino,
world_mask: wp.array,
):
"""
Reset the given `state_out` container to the initial state defined
in the model, but only for the worlds specified by the `world_mask`.
Args:
model (ModelKamino):
Input model container holding the time-invariant data of the system.
state_out (StateKamino):
Output state container to be reset to the model's default state.
world_mask (wp.array):
Array of per-world flags indicating which worlds should be reset.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
reset_state_from_bodies_state(
model,
state_out,
world_mask,
model.bodies.q_i_0,
model.bodies.u_i_0,
)
def reset_state_from_bodies_state(
model: ModelKamino,
state_out: StateKamino,
world_mask: wp.array,
bodies_q: wp.array,
bodies_u: wp.array,
):
"""
Resets the state of all bodies in the selected worlds based on their provided state.
The result is stored in the provided `state_out` container.
Args:
model (ModelKamino):
Input model container holding the time-invariant data of the system.
state_out (StateKamino):
Output state container to be reset to the model's default state.
world_mask (wp.array):
Array of per-world flags indicating which worlds should be reset.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
bodies_q (wp.array):
Array of target poses for the rigid bodies of each world.\n
Shape of ``(num_bodies,)`` and type :class:`transformf`.
bodies_u (wp.array):
Array of target twists for the rigid bodies of each world.\n
Shape of ``(num_bodies,)`` and type :class:`vec6f`.
"""
# Reset bodies
wp.launch(
_reset_body_state_of_select_worlds,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
world_mask,
model.bodies.wid,
bodies_q,
bodies_u,
# Outputs:
state_out.q_i,
state_out.u_i,
state_out.w_i,
state_out.w_i_e,
],
)
# Reset joints
wp.launch(
_reset_joint_state_of_select_worlds,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
world_mask,
model.info.joint_coords_offset,
model.info.joint_dofs_offset,
model.info.joint_cts_offset,
model.info.joint_dynamic_cts_group_offset,
model.info.joint_kinematic_cts_group_offset,
model.joints.wid,
model.joints.num_coords,
model.joints.num_dofs,
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.coords_offset,
model.joints.dofs_offset,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
model.joints.q_j_0,
# Outputs:
state_out.q_j,
state_out.q_j_p,
state_out.dq_j,
state_out.lambda_j,
],
)
def reset_state_from_base_state(
model: ModelKamino,
state_out: StateKamino,
world_mask: wp.array,
base_q: wp.array,
base_u: wp.array,
):
"""
Resets the state of all bodies in the selected worlds based on the state of their
respective base bodies. The result is stored in the provided `state_out` container.
More specifically, in each world, the reset operation rigidly transforms the initial pose of the
system so as to match the target pose of the base body, and sets body poses accordingly.
Furthermore, the twists of all bodies are set to that of the base body, but transformed to account
for the relative pose offset.
Args:
model (ModelKamino):
Input model container holding the time-invariant data of the system.
state_out (StateKamino):
Output state container to be reset based on the base body states.
world_mask (wp.array):
Array of per-world flags indicating which worlds should be reset.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
base_q (wp.array):
Array of target poses for the base bodies of each world.\n
Shape of ``(num_worlds,)`` and type :class:`transformf`.
base_u (wp.array):
Array of target twists for the base bodies of each world.\n
Shape of ``(num_worlds,)`` and type :class:`vec6f`.
"""
# Reset bodies based on base body states
wp.launch(
_reset_body_state_from_base,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
world_mask,
model.info.base_body_index,
model.bodies.wid,
model.bodies.q_i_0,
base_q,
base_u,
# Outputs:
state_out.q_i,
state_out.u_i,
state_out.w_i,
],
)
def reset_select_worlds_to_initial_state(
model: ModelKamino,
mask: wp.array,
data: DataKamino,
reset_constraints: bool = True,
):
"""
Reset the state of the selected worlds to the initial state
defined in the model given an array of per-world flags.
Args:
model: Input model container holding the time-invariant data of the system.
state: Input state container specifying the target state to be reset to.
mask: Array of per-world flags indicating which worlds should be reset.
data: Output solver data to be configured for the target state.
reset_constraints: Whether to reset joint constraint reactions to zero.
"""
# Reset time
wp.launch(
_reset_time_of_select_worlds,
dim=model.size.num_worlds,
inputs=[
# Inputs:
mask,
# Outputs:
data.time.time,
data.time.steps,
],
)
# Reset bodies
wp.launch(
_reset_bodies_of_select_worlds,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
mask,
model.bodies.wid,
model.bodies.i_I_i,
model.bodies.inv_i_I_i,
model.bodies.q_i_0,
model.bodies.u_i_0,
# Outputs:
data.bodies.q_i,
data.bodies.u_i,
data.bodies.I_i,
data.bodies.inv_I_i,
data.bodies.w_i,
data.bodies.w_a_i,
data.bodies.w_j_i,
data.bodies.w_l_i,
data.bodies.w_c_i,
data.bodies.w_e_i,
],
)
# Reset joints
wp.launch(
_reset_joints_of_select_worlds,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
reset_constraints,
mask,
model.info.joint_coords_offset,
model.info.joint_dofs_offset,
model.info.joint_cts_offset,
model.info.joint_dynamic_cts_group_offset,
model.info.joint_kinematic_cts_group_offset,
model.joints.wid,
model.joints.dof_type,
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.coords_offset,
model.joints.dofs_offset,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
model.joints.bid_B,
model.joints.bid_F,
model.joints.B_r_Bj,
model.joints.F_r_Fj,
model.joints.X_j,
model.joints.q_j_0,
model.bodies.q_i_0,
model.bodies.u_i_0,
data.joints.lambda_j,
# Outputs:
data.joints.p_j,
data.joints.r_j,
data.joints.dr_j,
data.joints.q_j,
data.joints.dq_j,
data.joints.lambda_j,
],
)
def reset_select_worlds_to_state(
model: ModelKamino,
state: StateKamino,
mask: wp.array,
data: DataKamino,
reset_constraints: bool = True,
):
"""
Reset the state of the selected worlds given an array of per-world flags.
Args:
model: Input model container holding the time-invariant data of the system.
state: Input state container specifying the target state to be reset to.
mask: Array of per-world flags indicating which worlds should be reset.
data: Output solver data to be configured for the target state.
"""
# Reset time
wp.launch(
_reset_time_of_select_worlds,
dim=model.size.num_worlds,
inputs=[
# Inputs:
mask,
# Outputs:
data.time.time,
data.time.steps,
],
)
# Reset bodies
wp.launch(
_reset_bodies_of_select_worlds,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
mask,
model.bodies.wid,
model.bodies.i_I_i,
model.bodies.inv_i_I_i,
state.q_i,
state.u_i,
# Outputs:
data.bodies.q_i,
data.bodies.u_i,
data.bodies.I_i,
data.bodies.inv_I_i,
data.bodies.w_i,
data.bodies.w_a_i,
data.bodies.w_j_i,
data.bodies.w_l_i,
data.bodies.w_c_i,
data.bodies.w_e_i,
],
)
# Reset joints
wp.launch(
_reset_joints_of_select_worlds,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
reset_constraints,
mask,
model.info.joint_coords_offset,
model.info.joint_dofs_offset,
model.info.joint_cts_offset,
model.info.joint_dynamic_cts_group_offset,
model.info.joint_kinematic_cts_group_offset,
model.joints.wid,
model.joints.dof_type,
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.coords_offset,
model.joints.dofs_offset,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
model.joints.bid_B,
model.joints.bid_F,
model.joints.B_r_Bj,
model.joints.F_r_Fj,
model.joints.X_j,
model.joints.q_j_0,
state.q_i,
state.u_i,
state.lambda_j,
# Outputs:
data.joints.p_j,
data.joints.r_j,
data.joints.dr_j,
data.joints.q_j,
data.joints.dq_j,
data.joints.lambda_j,
],
)
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""The Kamino Linear Algebra Module"""
from . import utils
from .core import (
DenseLinearOperatorData,
DenseRectangularMultiLinearInfo,
DenseSquareMultiLinearInfo,
)
from .linear import (
ConjugateGradientSolver,
ConjugateResidualSolver,
DirectSolver,
IterativeSolver,
LinearSolver,
LinearSolverNameToType,
LinearSolverType,
LinearSolverTypeToName,
LLTBlockedSolver,
LLTSequentialSolver,
)
###
# Module interface
###
__all__ = [
"ConjugateGradientSolver",
"ConjugateResidualSolver",
"DenseLinearOperatorData",
"DenseRectangularMultiLinearInfo",
"DenseSquareMultiLinearInfo",
"DirectSolver",
"IterativeSolver",
"LLTBlockedSolver",
"LLTSequentialSolver",
"LinearSolver",
"LinearSolverNameToType",
"LinearSolverType",
"LinearSolverTypeToName",
"utils",
]
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/blas.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""BLAS-like operations for multi-linear systems"""
import functools
from typing import Any
import warp as wp
from ..core.types import FloatType, float32, int32
from .sparse_matrix import BlockDType, BlockSparseMatrices
###
# Module interface
###
__all__ = [
"block_sparse_gemv",
"block_sparse_matvec",
"block_sparse_transpose_gemv",
"block_sparse_transpose_matvec",
"dense_gemv",
"diag_gemv",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
##
# Kernels
##
@wp.kernel
def _mult_left_right_diag_matrix_with_matrix(
# Inputs:
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
vio: wp.array(dtype=int32),
D: wp.array(dtype=float32),
X: wp.array(dtype=float32),
# Outputs:
Y: wp.array(dtype=float32),
):
# Retrieve the thread indices
wid, tid = wp.tid()
# Retrieve the number of active dimensions in the world
n = dim[wid]
# Compute i (row) and j (col) indices from the tid
i = tid // n
j = tid % n
# Skip if indices exceed the problem size
if i >= n or j >= n:
return
# Retrieve the matrix index offset of the world
m_0 = mio[wid]
# Retrieve the vector index offset of the world
v_0 = vio[wid]
# Compute the global index of the matrix entry
m_ij = m_0 + n * i + j
# Retrieve the ij entry of the input matrix
X_ij = X[m_ij]
# Retrieve the i,j entries of the diagonal matrix
D_i = D[v_0 + i]
D_j = D[v_0 + j]
# Compute the i,j entry of the output matrix
Y[m_ij] = D_i * D_j * X_ij
@wp.kernel
def _mult_left_diag_matrix_with_vector(
# Inputs:
dim: wp.array(dtype=int32),
vio: wp.array(dtype=int32),
D: wp.array(dtype=float32),
x: wp.array(dtype=float32),
# Outputs:
y: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the number of active constraints in the world
n = dim[wid]
# Skip if row index exceed the problem size
if tid >= n:
return
# Retrieve the vector index offset of the world
v_0 = vio[wid]
# Compute the global index of the vector entry
v_i = v_0 + tid
# Retrieve the i-th entry of the input vector
x_i = x[v_i]
# Retrieve the i-th entry of the diagonal matrix
D_i = D[v_i]
# Compute the i-th entry of the output vector
y[v_i] = D_i * x_i
@functools.cache
def _make_block_sparse_matvec_kernel(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_matvec_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Vector block offsets:
row_start: wp.array(dtype=int32),
col_start: wp.array(dtype=int32),
# Vector:
x: wp.array(dtype=block_type.dtype),
y: wp.array(dtype=block_type.dtype),
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: y_block += A_block @ x_block
if wp.static(n_block_rows == 1):
x_idx_base = col_start[mat_id] + block_coord[1]
acc = block_type.dtype(0.0)
for j in range(n_block_cols):
acc += block[j] * x[x_idx_base + j]
wp.atomic_add(y, row_start[mat_id] + block_coord[0], acc)
else:
x_idx_base = col_start[mat_id] + block_coord[1]
y_idx_base = row_start[mat_id] + block_coord[0]
for i in range(n_block_rows):
acc = block_type.dtype(0.0)
for j in range(n_block_cols):
acc += block[i, j] * x[x_idx_base + j]
wp.atomic_add(y, y_idx_base + i, acc)
return block_sparse_matvec_kernel
@functools.cache
def _make_block_sparse_matvec_kernel_2d(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_matvec_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Vector:
x: wp.array2d(dtype=block_type.dtype),
y: wp.array2d(dtype=block_type.dtype),
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: y_block += A_block @ x_block
if wp.static(n_block_rows == 1):
x_idx_base = block_coord[1]
acc = block_type.dtype(0.0)
for j in range(n_block_cols):
acc += block[j] * x[mat_id, x_idx_base + j]
wp.atomic_add(y, mat_id, block_coord[0], acc)
else:
x_idx_base = block_coord[1]
y_idx_base = block_coord[0]
for i in range(n_block_rows):
acc = block_type.dtype(0.0)
for j in range(n_block_cols):
acc += block[i, j] * x[mat_id, x_idx_base + j]
wp.atomic_add(y, mat_id, y_idx_base + i, acc)
return block_sparse_matvec_kernel
@functools.cache
def _make_block_sparse_transpose_matvec_kernel(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_transpose_matvec_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Vector block offsets:
row_start: wp.array(dtype=int32),
col_start: wp.array(dtype=int32),
# Vector:
y: wp.array(dtype=block_type.dtype),
x: wp.array(dtype=block_type.dtype),
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: x_block += A_block^T @ y_block
if wp.static(n_block_rows == 1):
x_idx_base = col_start[mat_id] + block_coord[1]
y_val = y[row_start[mat_id] + block_coord[0]]
for i in range(n_block_cols):
wp.atomic_add(x, x_idx_base + i, block[i] * y_val)
else:
x_idx_base = col_start[mat_id] + block_coord[1]
y_idx_base = row_start[mat_id] + block_coord[0]
for i in range(n_block_cols):
acc = block_type.dtype(0.0)
for j in range(n_block_rows):
acc += block[j, i] * y[y_idx_base + j]
wp.atomic_add(x, x_idx_base + i, acc)
return block_sparse_transpose_matvec_kernel
@functools.cache
def _make_block_sparse_transpose_matvec_kernel_2d(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_transpose_matvec_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Vector:
y: wp.array2d(dtype=block_type.dtype),
x: wp.array2d(dtype=block_type.dtype),
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: x_block += A_block^T @ y_block
if wp.static(n_block_rows == 1):
x_idx_base = block_coord[1]
y_val = y[mat_id, block_coord[0]]
for i in range(n_block_cols):
wp.atomic_add(x, mat_id, x_idx_base + i, block[i] * y_val)
else:
x_idx_base = block_coord[1]
y_idx_base = block_coord[0]
for i in range(n_block_cols):
acc = block_type.dtype(0.0)
for j in range(n_block_rows):
acc += block[j, i] * y[mat_id, y_idx_base + j]
wp.atomic_add(x, mat_id, x_idx_base + i, acc)
return block_sparse_transpose_matvec_kernel
@functools.cache
def _make_scale_vector_kernel(space_dim: int):
"""Creates a kernel that scales a vector, taking into account a matrix mask and how the current
size of a matrix affects the active entries of the vector.
Parameters
----------
space_dim : int
Space of the vector in reference to the matrices (0: row space, 1: column space).
"""
sp_dim = wp.constant(space_dim)
@wp.kernel
def scale_vector_kernel(
# Matrix data:
matrix_dims: wp.array2d(dtype=int32),
# Vector block offsets:
row_start: wp.array(dtype=int32),
col_start: wp.array(dtype=int32),
# Inputs:
x: wp.array(dtype=Any),
beta: Any,
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, entry_id = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0 or entry_id >= matrix_dims[mat_id, sp_dim]:
return
if wp.static(space_dim == 0):
idx = row_start[mat_id] + entry_id
x[idx] = beta * x[idx]
else:
idx = col_start[mat_id] + entry_id
x[idx] = beta * x[idx]
return scale_vector_kernel
@functools.cache
def _make_scale_vector_kernel_2d(space_dim: int):
"""Creates a kernel that scales a vector, taking into account a matrix mask and how the current
size of a matrix affects the active entries of the vector.
Parameters
----------
space_dim : int
Space of the vector in reference to the matrices (0: row space, 1: column space).
"""
sp_dim = wp.constant(space_dim)
@wp.kernel
def scale_vector_kernel(
# Matrix data:
matrix_dims: wp.array2d(dtype=int32),
# Inputs:
x: wp.array2d(dtype=Any),
beta: Any,
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, entry_id = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0 or entry_id >= matrix_dims[mat_id, sp_dim]:
return
x[mat_id, entry_id] = beta * x[mat_id, entry_id]
return scale_vector_kernel
@functools.cache
def _make_block_sparse_gemv_kernel(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_gemv_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Vector block offsets:
row_start: wp.array(dtype=int32),
col_start: wp.array(dtype=int32),
# Vector:
x: wp.array(dtype=block_type.dtype),
y: wp.array(dtype=block_type.dtype),
# Scaling:
alpha: block_type.dtype,
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: z += alpha * A_block @ x_block
if wp.static(n_block_rows == 1):
x_idx_base = col_start[mat_id] + block_coord[1]
acc = block_type.dtype(0.0)
for j in range(n_block_cols):
acc += alpha * block[j] * x[x_idx_base + j]
wp.atomic_add(y, row_start[mat_id] + block_coord[0], acc)
else:
x_idx_base = col_start[mat_id] + block_coord[1]
y_idx_base = row_start[mat_id] + block_coord[0]
for i in range(n_block_rows):
acc = block_type.dtype(0.0)
for j in range(n_block_cols):
acc += alpha * block[i, j] * x[x_idx_base + j]
wp.atomic_add(y, y_idx_base + i, acc)
return block_sparse_gemv_kernel
@functools.cache
def _make_block_sparse_gemv_kernel_2d(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_gemv_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Vector:
x: wp.array2d(dtype=block_type.dtype),
y: wp.array2d(dtype=block_type.dtype),
# Scaling:
alpha: block_type.dtype,
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: z += alpha * A_block @ x_block
if wp.static(n_block_rows == 1):
x_idx_base = block_coord[1]
acc = block_type.dtype(0.0)
for j in range(n_block_cols):
acc += alpha * block[j] * x[mat_id, x_idx_base + j]
wp.atomic_add(y, mat_id, block_coord[0], acc)
else:
x_idx_base = block_coord[1]
y_idx_base = block_coord[0]
for i in range(n_block_rows):
acc = block_type.dtype(0.0)
for j in range(n_block_cols):
acc += alpha * block[i, j] * x[mat_id, x_idx_base + j]
wp.atomic_add(y, mat_id, y_idx_base + i, acc)
return block_sparse_gemv_kernel
@functools.cache
def _make_block_sparse_transpose_gemv_kernel(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_transpose_gemv_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Vector block offsets:
row_start: wp.array(dtype=int32),
col_start: wp.array(dtype=int32),
# Vector:
y: wp.array(dtype=block_type.dtype),
x: wp.array(dtype=block_type.dtype),
# Scaling:
alpha: block_type.dtype,
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: z += alpha * A_block^T @ y_block
if wp.static(n_block_rows == 1):
x_idx_base = col_start[mat_id] + block_coord[1]
y_val = y[row_start[mat_id] + block_coord[0]]
for i in range(n_block_cols):
wp.atomic_add(x, x_idx_base + i, alpha * block[i] * y_val)
else:
x_idx_base = col_start[mat_id] + block_coord[1]
y_idx_base = row_start[mat_id] + block_coord[0]
for i in range(n_block_cols):
acc = block_type.dtype(0.0)
for j in range(n_block_rows):
acc += alpha * block[j, i] * y[y_idx_base + j]
wp.atomic_add(x, x_idx_base + i, acc)
return block_sparse_transpose_gemv_kernel
@functools.cache
def _make_block_sparse_transpose_gemv_kernel_2d(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_transpose_gemv_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Vector:
y: wp.array2d(dtype=block_type.dtype),
x: wp.array2d(dtype=block_type.dtype),
# Scaling:
alpha: block_type.dtype,
# Mask:
matrix_mask: wp.array(dtype=int32),
):
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_coord = nzb_coords[global_block_idx]
block = nzb_values[global_block_idx]
# Perform block matrix-vector multiplication: z += alpha * A_block^T @ y_block
if wp.static(n_block_rows == 1):
x_idx_base = block_coord[1]
y_val = y[mat_id, block_coord[0]]
for i in range(n_block_cols):
wp.atomic_add(x, mat_id, x_idx_base + i, alpha * block[i] * y_val)
else:
x_idx_base = block_coord[1]
y_idx_base = block_coord[0]
for i in range(n_block_cols):
acc = block_type.dtype(0.0)
for j in range(n_block_rows):
acc += alpha * block[j, i] * y[mat_id, y_idx_base + j]
wp.atomic_add(x, mat_id, x_idx_base + i, acc)
return block_sparse_transpose_gemv_kernel
@wp.kernel
def _diag_gemv_kernel(
x: wp.array2d(dtype=Any),
y: wp.array2d(dtype=Any),
D: wp.array2d(dtype=Any),
active_dims: wp.array(dtype=Any),
world_active: wp.array(dtype=wp.int32),
alpha: Any,
beta: Any,
):
"""Computes y[w] = alpha * D[w] * x[w] + beta * y[w] for each world w."""
world, row = wp.tid()
assert world < len(active_dims)
if world_active[world] == 0 or row >= active_dims[world]:
return
zero = type(alpha)(0)
s = y.dtype(0)
if alpha != zero:
s += alpha * D[world, row] * x[world, row]
if beta != zero:
s += beta * y[world, row]
y[world, row] = s
@wp.kernel
def _dense_gemv_kernel(
x: wp.array2d(dtype=Any),
y: wp.array2d(dtype=Any),
A: wp.array2d(dtype=Any),
active_dims: wp.array(dtype=Any),
world_active: wp.array(dtype=wp.int32),
alpha: Any,
beta: Any,
matrix_stride: int,
tile_size: int,
):
"""Computes y[w] = alpha * (A[w] @ x[w]) + beta * y[w] in-place for each world w."""
world, row, lane = wp.tid()
assert world < len(active_dims)
dim = active_dims[world]
if world_active[world] == 0 or row >= dim:
return
row_stride = active_dims[world]
zero = type(alpha)(0)
s = zero
if alpha != zero:
for col in range(lane, dim, tile_size):
s += A[world, row * row_stride + col] * x[world, col]
row_tile = wp.tile_sum(wp.tile(s * alpha))
if beta != zero:
row_tile += beta * wp.tile_load(y[world], shape=1, offset=row)
wp.tile_store(y[world], row_tile, offset=row)
@functools.cache
def _make_block_sparse_ATA_diagonal_kernel_2d(block_type: BlockDType):
# Determine (static) block size for kernel.
block_shape = block_type.shape
if isinstance(block_type.shape, int):
block_shape = (block_shape, block_shape)
elif len(block_shape) == 0:
block_shape = (1, 1)
elif len(block_shape) == 1:
block_shape = (1, block_shape[0])
@wp.kernel
def block_sparse_ATA_diagonal_kernel(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=block_type.warp_type),
# Output:
diag: wp.array2d(dtype=block_type.dtype),
# Mask:
matrix_mask: wp.array(dtype=int32),
):
"""
For a block sparse matrix (stack) A, computes the diagonal of A^T * A
"""
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
n_block_rows = wp.static(block_shape[0])
n_block_cols = wp.static(block_shape[1])
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_col = nzb_coords[global_block_idx][1]
block = nzb_values[global_block_idx]
# Accumulate coefficients contributed by non-zero block
if wp.static(n_block_rows == 1):
for j in range(n_block_cols):
val = block[j]
wp.atomic_add(diag, mat_id, block_col + j, val * val)
else:
for j in range(n_block_cols):
acc = block_type.dtype(0.0)
for i in range(n_block_rows):
val = block[i, j]
acc += val * val
wp.atomic_add(diag, mat_id, block_col + j, acc)
return block_sparse_ATA_diagonal_kernel
class nzb_type_7(BlockDType(dtype=wp.float32, shape=(7,)).warp_type):
pass
@wp.kernel
def block_sparse_ATA_diagonal_3_4_blocks_kernel_2d(
# Matrix data:
num_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
nzb_values: wp.array(dtype=nzb_type_7),
# Output:
blocks_3: wp.array2d(dtype=wp.float32),
blocks_4: wp.array2d(dtype=wp.float32),
# Mask:
matrix_mask: wp.array(dtype=int32),
):
"""
For a block sparse matrix (stack) A with 1x7 blocks, computes the blockwise-diagonal of A^T * A,
with alternating 3x3 and 4x4 blocks
3x3 and 4x4 blocks are flattened and concatenated in blocks_3 and blocks_4 (to allow atomic_add)
"""
mat_id, block_idx = wp.tid()
# Early exit if the matrix is flagged as inactive.
if matrix_mask[mat_id] == 0:
return
# Check if block index is valid for this matrix.
if block_idx >= num_nzb[mat_id]:
return
global_block_idx = nzb_start[mat_id] + block_idx
block_col = nzb_coords[global_block_idx][1]
block = nzb_values[global_block_idx]
block_col_7 = block_col // 7
# Accumulate coefficients contributed to 3x3 block
offset = 9 * block_col_7
for i in range(3):
val_i = block[i]
for j in range(3):
val_j = block[j]
wp.atomic_add(blocks_3, mat_id, offset + 3 * i + j, val_i * val_j)
# Accumulate coefficients contributed to 4x4 block
offset = 16 * block_col_7
for i in range(4):
val_i = block[3 + i]
for j in range(4):
val_j = block[3 + j]
wp.atomic_add(blocks_4, mat_id, offset + 4 * i + j, val_i * val_j)
@functools.cache
def _make_cwise_inverse_kernel_2d(dtype: FloatType):
@wp.kernel
def cwise_inverse_kernel(
# Inputs
x: wp.array2d(dtype=dtype),
dim: wp.array(dtype=wp.int32),
mask: wp.array(dtype=wp.int32),
):
mat_id, coeff_id = wp.tid()
if mat_id >= mask.shape[0] or mask[mat_id] == 0 or coeff_id >= dim[mat_id]:
return
x[mat_id, coeff_id] = 1.0 / x[mat_id, coeff_id]
return cwise_inverse_kernel
@wp.kernel
def blockwise_inverse_kernel_3_2d(
# Inputs
blocks: wp.array2d(dtype=wp.mat33f),
dim: wp.array(dtype=wp.int32),
mask: wp.array(dtype=wp.int32),
):
mat_id, block_id = wp.tid()
if mat_id >= mask.shape[0] or mask[mat_id] == 0 or 7 * block_id >= dim[mat_id]:
return
blocks[mat_id, block_id] = wp.inverse(blocks[mat_id, block_id])
@wp.kernel
def blockwise_inverse_kernel_4_2d(
# Inputs
blocks: wp.array2d(dtype=wp.mat44f),
dim: wp.array(dtype=wp.int32),
mask: wp.array(dtype=wp.int32),
):
mat_id, block_id = wp.tid()
if mat_id >= mask.shape[0] or mask[mat_id] == 0 or 7 * block_id >= dim[mat_id]:
return
blocks[mat_id, block_id] = wp.inverse(blocks[mat_id, block_id])
@wp.kernel
def _blockwise_diag_3_4_gemv_kernel_2d(
x: wp.array2d(dtype=wp.float32),
y: wp.array2d(dtype=wp.float32),
blocks_3: wp.array2d(dtype=wp.mat33f),
blocks_4: wp.array2d(dtype=wp.mat44f),
active_dims: wp.array(dtype=wp.int32),
world_active: wp.array(dtype=wp.int32),
alpha: wp.float32,
beta: wp.float32,
):
"""Computes y[w] = alpha * D[w] * x[w] + beta * y[w] for each world w.
where D is blockwise-diagonal, alternating 3x3 and 4x4 blocks"""
world, row_block_id = wp.tid()
row_id = 7 * row_block_id
assert world < len(active_dims)
if world_active[world] == 0 or row_id >= active_dims[world]:
return
zero = type(alpha)(0)
y_3 = wp.vec3f(0.0, 0.0, 0.0)
y_4 = wp.vec4f(0.0, 0.0, 0.0, 0.0)
if alpha != zero:
x_3 = wp.vec3f(x[world, row_id], x[world, row_id + 1], x[world, row_id + 2])
y_3 += alpha * (blocks_3[world, row_block_id] * x_3)
x_4 = wp.vec4f(x[world, row_id + 3], x[world, row_id + 4], x[world, row_id + 5], x[world, row_id + 6])
y_4 = alpha * (blocks_4[world, row_block_id] * x_4)
if beta != zero:
y_3 += beta * wp.vec3f(y[world, row_id], y[world, row_id + 1], y[world, row_id + 2])
y_4 += beta * wp.vec4f(y[world, row_id + 3], y[world, row_id + 4], y[world, row_id + 5], y[world, row_id + 6])
y[world, row_id] = y_3[0]
y[world, row_id + 1] = y_3[1]
y[world, row_id + 2] = y_3[2]
y[world, row_id + 3] = y_4[0]
y[world, row_id + 4] = y_4[1]
y[world, row_id + 5] = y_4[2]
y[world, row_id + 6] = y_4[3]
##
# Launchers
##
def diag_gemv(
D: wp.array2d,
x: wp.array2d,
y: wp.array2d,
active_dims: wp.array,
world_active: wp.array,
alpha: float,
beta: float,
):
"""
Launch kernel for diagonal matrix gemv: y = alpha * D * x + beta * y
Args:
D: Diagonal matrices stored as 2D array (n_worlds, max_dim).
x: Input vectors (n_worlds, max_dim).
y: Output vectors (n_worlds, max_dim), modified in-place.
active_dims: Active dimension per world.
world_active: Boolean mask for active worlds.
alpha: Scalar multiplier for D * x.
beta: Scalar multiplier for y.
"""
n_worlds, max_dim = x.shape
dtype = x.dtype
wp.launch(
_diag_gemv_kernel,
dim=(n_worlds, max_dim),
inputs=[x, y, D, active_dims, world_active, dtype(alpha), dtype(beta)],
device=x.device,
)
def dense_gemv(
A: wp.array2d,
x: wp.array2d,
y: wp.array2d,
active_dims: wp.array,
world_active: wp.array,
alpha: float,
beta: float,
matrix_stride: int,
block_dim: int = 64,
):
"""
Launch kernel for dense matrix gemv: y = alpha * A @ x + beta * y
Args:
A: Dense matrices stored as 2D array (n_worlds, max_dim * max_dim).
x: Input vectors (n_worlds, max_dim).
y: Output vectors (n_worlds, max_dim), modified in-place.
active_dims: Active dimension per world.
world_active: Boolean mask for active worlds.
alpha: Scalar multiplier for A * x.
beta: Scalar multiplier for y.
matrix_stride: Stride for matrix row indexing.
block_dim: Block dimension for tiled computation.
"""
n_worlds, max_dim = x.shape
dtype = x.dtype
if not x.device.is_cuda:
block_dim = 1
wp.launch(
_dense_gemv_kernel,
dim=(n_worlds, max_dim, block_dim),
inputs=[x, y, A, active_dims, world_active, dtype(alpha), dtype(beta), matrix_stride, block_dim],
device=x.device,
block_dim=block_dim,
)
def block_sparse_matvec(
A: BlockSparseMatrices,
x: wp.array,
y: wp.array,
matrix_mask: wp.array,
):
"""
Launch kernel for block-sparse matrix-vector product: y = A * x
Args:
A (BlockSparseMatrices): Sparse matrices.
x (wp.array): Stack of input vectors, expects either shape (sum_of_max_cols,) for the 1D flattened
version; or shape (num_matrices, max_of_max_cols) for the 2D version.
y (wp.array): Stack of output vectors, expects either shape (sum_of_max_rows,) for the 1D flattened
version; or shape (num_matrices, max_of_max_rows) for the 2D version.
matrix_mask (wp.array): Mask vector to skip matrices set to `0` in the mask.
"""
y.zero_()
if len(x.shape) == 1:
wp.launch(
kernel=_make_block_sparse_matvec_kernel(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
A.row_start,
A.col_start,
x,
y,
matrix_mask,
],
device=A.device,
)
else:
wp.launch(
kernel=_make_block_sparse_matvec_kernel_2d(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
x,
y,
matrix_mask,
],
device=A.device,
)
def block_sparse_transpose_matvec(
A: BlockSparseMatrices,
y: wp.array,
x: wp.array,
matrix_mask: wp.array,
):
"""
Launch kernel for block-sparse transpose matrix-vector product: x = A^T * y
Args:
A (BlockSparseMatrices): Sparse matrices.
y (wp.array): Stack of input vectors, expects either shape (sum_of_max_rows,) for the 1D flattened
version; or shape (num_matrices, max_of_max_rows) for the 2D version.
x (wp.array): Stack of output vectors, expects either shape (sum_of_max_cols,) for the 1D flattened
version; or shape (num_matrices, max_of_max_cols) for the 2D version.
matrix_mask (wp.array): Mask vector to skip matrices set to `0` in the mask.
"""
x.zero_()
if len(x.shape) == 1:
wp.launch(
kernel=_make_block_sparse_transpose_matvec_kernel(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
A.row_start,
A.col_start,
y,
x,
matrix_mask,
],
device=A.device,
)
else:
wp.launch(
kernel=_make_block_sparse_transpose_matvec_kernel_2d(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
y,
x,
matrix_mask,
],
device=A.device,
)
def block_sparse_gemv(
A: BlockSparseMatrices,
x: wp.array,
y: wp.array,
alpha: Any,
beta: Any,
matrix_mask: wp.array,
):
"""
Launch kernel for generalized block-sparse matrix-vector product: y = alpha * (A * x) + beta * y
Args:
A (BlockSparseMatrices): Sparse matrices.
x (wp.array): Stack of input vectors, expects either shape (sum_of_max_cols,) for the 1D flattened
version; or shape (num_matrices, max_of_max_cols) for the 2D version.
y (wp.array): Stack of input-output vectors, expects either shape (sum_of_max_rows,) for the 1D
flattened version; or shape (num_matrices, max_of_max_rows) for the 2D version.
alpha (Any): Input scaling for matrix-vector multiplication.
beta (Any): Input scaling for linear offset.
matrix_mask (wp.array): Mask vector to skip matrices set to `0` in the mask.
"""
if len(x.shape) == 1:
# Compute y <= beta * y
wp.launch(
kernel=_make_scale_vector_kernel(0),
dim=(A.num_matrices, A.max_of_max_dims[0]),
inputs=[A.dims, A.row_start, A.col_start, y, beta, matrix_mask],
device=A.device,
)
# Compute y += alpha * A @ x
wp.launch(
kernel=_make_block_sparse_gemv_kernel(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
A.row_start,
A.col_start,
x,
y,
alpha,
matrix_mask,
],
device=A.device,
)
else:
# Compute y <= beta * y
wp.launch(
kernel=_make_scale_vector_kernel_2d(0),
dim=(A.num_matrices, A.max_of_max_dims[0]),
inputs=[A.dims, A.row_start, A.col_start, y, beta, matrix_mask],
device=A.device,
)
# Compute y += alpha * A @ x
wp.launch(
kernel=_make_block_sparse_gemv_kernel_2d(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
x,
y,
alpha,
matrix_mask,
],
device=A.device,
)
def block_sparse_transpose_gemv(
A: BlockSparseMatrices,
y: wp.array,
x: wp.array,
alpha: Any,
beta: Any,
matrix_mask: wp.array,
):
"""
Launch kernel for generalized block-sparse transpose matrix-vector product: x = alpha * (A^T * y) + beta * x
Args:
A (BlockSparseMatrices): Sparse matrices.
y (wp.array): Stack of input vectors, expects either shape (sum_of_max_rows,) for the 1D flattened
version; or shape (num_matrices, max_of_max_rows) for the 2D version.
x (wp.array): Stack of input-output vectors, expects either shape (sum_of_max_cols,) for the 1D
flattened version; or shape (num_matrices, max_of_max_cols) for the 2D version.
alpha (Any): Input scaling for matrix-vector multiplication.
beta (Any): Input scaling for linear offset.
matrix_mask (wp.array): Mask vector to skip matrices set to `0` in the mask.
"""
if len(x.shape) == 1:
# Compute x <= beta * x
wp.launch(
kernel=_make_scale_vector_kernel(1),
dim=(A.num_matrices, A.max_of_max_dims[1]),
inputs=[A.dims, A.row_start, A.col_start, x, beta, matrix_mask],
device=A.device,
)
# Compute y += alpha * A^T @ y
wp.launch(
kernel=_make_block_sparse_transpose_gemv_kernel(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
A.row_start,
A.col_start,
y,
x,
alpha,
matrix_mask,
],
device=A.device,
)
else:
# Compute x <= beta * x
wp.launch(
kernel=_make_scale_vector_kernel(1),
dim=(A.num_matrices, A.max_of_max_dims[1]),
inputs=[A.dims, A.row_start, A.col_start, x, beta, matrix_mask],
device=A.device,
)
# Compute y += alpha * A^T @ y
wp.launch(
kernel=_make_block_sparse_transpose_gemv_kernel(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
y,
x,
alpha,
matrix_mask,
],
device=A.device,
)
def block_sparse_ATA_inv_diagonal_2d(A: BlockSparseMatrices, inv_diag: wp.array, matrix_mask: wp.array):
"""
Function computing the inverse of the diagonal of A^T * A given sparse matrix (stack) A.
Args:
A (BlockSparseMatrices): Sparse matrices.
inv_diag (wp.array): Stack of output vectors, expects shape (num_matrices, max_of_max_cols).
matrix_mask (wp.array): Mask vector to skip matrices set to `0` in the mask.
"""
inv_diag.zero_()
wp.launch(
kernel=_make_block_sparse_ATA_diagonal_kernel_2d(A.nzb_dtype),
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
inv_diag,
matrix_mask,
],
device=A.device,
)
int_size_bytes = 4 # Size of wp.int32 in bytes
cols = wp.array(
dtype=wp.int32,
shape=(A.num_matrices,),
ptr=A.dims.ptr + int_size_bytes,
strides=(2 * int_size_bytes,),
copy=False,
)
wp.launch(
kernel=_make_cwise_inverse_kernel_2d(A.nzb_dtype.dtype),
dim=(A.num_matrices, A.max_of_max_dims[1]),
inputs=[
inv_diag,
cols,
matrix_mask,
],
device=A.device,
)
def block_sparse_ATA_blockwise_3_4_inv_diagonal_2d(
A: BlockSparseMatrices, inv_blocks_3: wp.array, inv_blocks_4: wp.array, matrix_mask: wp.array
):
"""
Function computing the blockwise inverse of the diagonal of A^T * A given sparse matrix (stack) A,
with alternating 3x3 and 4x4 blocks
A must have block size 1x7
Args:
A (BlockSparseMatrices): Sparse matrices.
inv_blocks (wp.array): Stack of vectors of 3x3 blocks, expects shape (num_matrices, max_of_max_cols / 7).
matrix_mask (wp.array): Mask vector to skip matrices set to `0` in the mask.
"""
inv_blocks_3.zero_()
inv_blocks_4.zero_()
inv_blocks_3_flat = wp.array(
dtype=wp.float32,
ptr=inv_blocks_3.ptr,
shape=(A.num_matrices, 9 * inv_blocks_3.shape[1]),
copy=False,
device=A.device,
)
inv_blocks_4_flat = wp.array(
dtype=wp.float32,
ptr=inv_blocks_4.ptr,
shape=(A.num_matrices, 16 * inv_blocks_3.shape[1]),
copy=False,
device=A.device,
)
wp.launch(
kernel=block_sparse_ATA_diagonal_3_4_blocks_kernel_2d,
dim=(A.num_matrices, A.max_of_num_nzb),
inputs=[
A.num_nzb,
A.nzb_start,
A.nzb_coords,
A.nzb_values,
inv_blocks_3_flat,
inv_blocks_4_flat,
matrix_mask,
],
device=A.device,
)
int_size_bytes = 4 # Size of wp.int32 in bytes
cols = wp.array(
dtype=wp.int32,
shape=(A.num_matrices,),
ptr=A.dims.ptr + int_size_bytes,
strides=(2 * int_size_bytes,),
copy=False,
)
wp.launch(
kernel=blockwise_inverse_kernel_3_2d,
dim=inv_blocks_3.shape,
inputs=[
inv_blocks_3,
cols,
matrix_mask,
],
device=A.device,
)
wp.launch(
kernel=blockwise_inverse_kernel_4_2d,
dim=inv_blocks_4.shape,
inputs=[
inv_blocks_4,
cols,
matrix_mask,
],
device=A.device,
)
def get_blockwise_diag_3_4_gemv_2d(
blocks_3: wp.array2d(dtype=wp.mat33f),
blocks_4: wp.array2d(dtype=wp.mat44f),
active_dims: wp.array(dtype=wp.int32),
):
def gemv(
x: wp.array2d(dtype=wp.float32),
y: wp.array2d(dtype=wp.float32),
world_active: wp.array(dtype=wp.int32),
alpha: wp.float32,
beta: wp.float32,
):
wp.launch(
_blockwise_diag_3_4_gemv_kernel_2d,
dim=blocks_3.shape,
inputs=[
x,
y,
blocks_3,
blocks_4,
active_dims,
world_active,
alpha,
beta,
],
device=blocks_3.device,
)
return gemv
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/conjugate.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Conjugate gradient and conjugate residual solvers
"""
from __future__ import annotations
import functools
import math
from collections.abc import Callable
from typing import Any
import warp as wp
from . import blas
from .core import DenseLinearOperatorData
from .sparse_matrix import BlockSparseMatrices
from .sparse_operator import BlockSparseLinearOperators
# No need to auto-generate adjoint code for linear solvers
wp.set_module_options({"enable_backward": False})
# based on the warp.optim.linear implementation
__all__ = [
"BatchedLinearOperator",
"CGSolver",
"CRSolver",
"make_jacobi_preconditioner",
]
class BatchedLinearOperator:
"""Linear operator for batched matrix-vector products.
Supports dense, diagonal, and block-sparse matrices.
Use class methods to create instances.
"""
def __init__(
self,
gemv_fn: Callable,
n_worlds: int,
max_dim: int,
active_dims: wp.array,
device: wp.context.Device,
dtype: type,
matvec_fn: Callable | None = None,
):
self._gemv_fn = gemv_fn
self.n_worlds = n_worlds
self.max_dim = max_dim
self.active_dims = active_dims
self.device = device
self.dtype = dtype
self._matvec_fn = matvec_fn
@classmethod
def from_dense(cls, operator: DenseLinearOperatorData) -> BatchedLinearOperator:
"""Create operator from dense matrix data."""
info = operator.info
n_worlds = info.num_blocks
max_dim = info.max_dimension
A_mat = operator.mat.reshape((n_worlds, max_dim * max_dim))
active_dims = info.dim
def gemv_fn(x, y, world_active, alpha, beta):
blas.dense_gemv(A_mat, x, y, active_dims, world_active, alpha, beta, max_dim)
return cls(gemv_fn, n_worlds, max_dim, active_dims, info.device, info.dtype)
@classmethod
def from_diagonal(cls, D: wp.array2d, active_dims: wp.array) -> BatchedLinearOperator:
"""Create operator from diagonal matrix."""
n_worlds, max_dim = D.shape
def gemv_fn(x, y, world_active, alpha, beta):
blas.diag_gemv(D, x, y, active_dims, world_active, alpha, beta)
return cls(gemv_fn, n_worlds, max_dim, active_dims, D.device, D.dtype)
@classmethod
def from_block_sparse(cls, A: BlockSparseMatrices, active_dims: wp.array) -> BatchedLinearOperator:
"""Create operator from block-sparse matrix.
Requires all matrices to have the same max dimensions so that 2D arrays
can be reshaped to 1D for the sparse gemv kernel.
Args:
A: Block-sparse matrices container.
active_dims: 1D int array with active row dimension per matrix.
"""
max_rows, max_cols = A.max_of_max_dims
n_worlds = A.num_matrices
def gemv_fn(x, y, world_active, alpha, beta):
# Reshape 2D arrays to 1D for sparse gemv, then back
x_flat = x.reshape((n_worlds * max_cols,))
y_flat = y.reshape((n_worlds * max_rows,))
blas.block_sparse_gemv(A, x_flat, y_flat, alpha, beta, world_active)
def matvec_fn(x, y, world_active):
# Reshape 2D arrays to 1D for sparse matvec, then back
x_flat = x.reshape((n_worlds * max_cols,))
y_flat = y.reshape((n_worlds * max_rows,))
blas.block_sparse_matvec(A, x_flat, y_flat, world_active)
dtype = A.nzb_dtype.dtype if A.nzb_dtype is not None else None
return cls(gemv_fn, n_worlds, max_rows, active_dims, A.device, dtype, matvec_fn=matvec_fn)
@classmethod
def from_block_sparse_operator(cls, A: BlockSparseLinearOperators) -> BatchedLinearOperator:
"""Create operator from block-sparse operator.
Requires all matrices to have the same max dimensions so that 2D arrays
can be reshaped to 1D for the sparse gemv kernel.
Args:
A: Block-sparse matrices operator.
active_dims: 1D int array with active row dimension per matrix.
"""
max_rows, max_cols = A.max_of_max_dims
n_worlds = A.num_matrices
def gemv_fn(x, y, world_active, alpha, beta):
x_flat = x.reshape((n_worlds * max_cols,))
y_flat = y.reshape((n_worlds * max_rows,))
A.gemv(x_flat, y_flat, world_active, alpha, beta)
def matvec_fn(x, y, world_active):
x_flat = x.reshape((n_worlds * max_cols,))
y_flat = y.reshape((n_worlds * max_rows,))
A.matvec(x_flat, y_flat, world_active)
return cls(gemv_fn, n_worlds, max_rows, A.active_cols, A.device, A.dtype, matvec_fn=matvec_fn)
def gemv(self, x: wp.array2d, y: wp.array2d, world_active: wp.array, alpha: float, beta: float):
"""Compute y = alpha * A @ x + beta * y."""
self._gemv_fn(x, y, world_active, alpha, beta)
def matvec(self, x: wp.array2d, y: wp.array2d, world_active: wp.array):
if self._matvec_fn is not None:
return self._matvec_fn(x, y, world_active)
return self._gemv_fn(x, y, world_active, 1.0, 0.0)
# Implementations
# ---------------
@functools.cache
def make_termination_kernel(n_worlds):
@wp.kernel
def check_termination(
maxiter: wp.array(dtype=int),
cycle_size: int,
r_norm_sq: wp.array(dtype=Any),
atol_sq: wp.array(dtype=Any),
world_active: wp.array(dtype=wp.int32),
cur_iter: wp.array(dtype=int),
world_condition: wp.array(dtype=wp.int32),
batch_condition: wp.array(dtype=wp.int32),
):
thread = wp.tid()
active = wp.tile_load(world_active, (n_worlds,))
condition = wp.tile_load(world_condition, (n_worlds,))
world_stepped = wp.tile_map(wp.mul, active, condition)
iter = world_stepped * cycle_size + wp.tile_load(cur_iter, (n_worlds,))
wp.tile_store(cur_iter, iter)
cont_norm = wp.tile_astype(
wp.tile_map(lt_mask, wp.tile_load(atol_sq, (n_worlds,)), wp.tile_load(r_norm_sq, (n_worlds,))), wp.int32
)
cont_iter = wp.tile_map(lt_mask, iter, wp.tile_load(maxiter, (n_worlds,)))
cont = wp.tile_map(wp.mul, wp.tile_map(wp.mul, cont_iter, cont_norm), world_stepped)
wp.tile_store(world_condition, cont)
batch_cont = wp.where(wp.tile_sum(cont)[0] > 0, 1, 0)
if thread == 0:
batch_condition[0] = batch_cont
return check_termination
@wp.kernel
def _cg_kernel_1(
tol: wp.array(dtype=Any),
resid: wp.array(dtype=Any),
rz_old: wp.array(dtype=Any),
p_Ap: wp.array(dtype=Any),
p: wp.array2d(dtype=Any),
Ap: wp.array2d(dtype=Any),
x: wp.array2d(dtype=Any),
r: wp.array2d(dtype=Any),
):
e, i = wp.tid()
alpha = wp.where(resid[e] > tol[e] and p_Ap[e] > 0.0, rz_old[e] / p_Ap[e], rz_old.dtype(0.0))
x[e, i] = x[e, i] + alpha * p[e, i]
r[e, i] = r[e, i] - alpha * Ap[e, i]
@wp.kernel
def _cg_kernel_2(
tol: wp.array(dtype=Any),
resid_new: wp.array(dtype=Any),
rz_old: wp.array(dtype=Any),
rz_new: wp.array(dtype=Any),
z: wp.array2d(dtype=Any),
p: wp.array2d(dtype=Any),
):
# p = r + (rz_new / rz_old) * p;
e, i = wp.tid()
cond = resid_new[e] > tol[e]
beta = wp.where(cond and rz_old[e] > 0.0, rz_new[e] / rz_old[e], rz_old.dtype(0.0))
p[e, i] = z[e, i] + beta * p[e, i]
@wp.kernel
def _cr_kernel_1(
tol: wp.array(dtype=Any),
resid: wp.array(dtype=Any),
zAz_old: wp.array(dtype=Any),
y_Ap: wp.array(dtype=Any),
p: wp.array2d(dtype=Any),
Ap: wp.array2d(dtype=Any),
y: wp.array2d(dtype=Any),
x: wp.array2d(dtype=Any),
r: wp.array2d(dtype=Any),
z: wp.array2d(dtype=Any),
):
e, i = wp.tid()
alpha = wp.where(resid[e] > tol[e] and y_Ap[e] > 0.0, zAz_old[e] / y_Ap[e], zAz_old.dtype(0.0))
x[e, i] = x[e, i] + alpha * p[e, i]
r[e, i] = r[e, i] - alpha * Ap[e, i]
z[e, i] = z[e, i] - alpha * y[e, i]
@wp.kernel
def _cr_kernel_2(
tol: wp.array(dtype=Any),
resid: wp.array(dtype=Any),
zAz_old: wp.array(dtype=Any),
zAz_new: wp.array(dtype=Any),
z: wp.array2d(dtype=Any),
Az: wp.array2d(dtype=Any),
p: wp.array2d(dtype=Any),
Ap: wp.array2d(dtype=Any),
):
# p = r + (rz_new / rz_old) * p;
e, i = wp.tid()
beta = wp.where(resid[e] > tol[e] and zAz_old[e] > 0.0, zAz_new[e] / zAz_old[e], zAz_old.dtype(0.0))
p[e, i] = z[e, i] + beta * p[e, i]
Ap[e, i] = Az[e, i] + beta * Ap[e, i]
def _run_capturable_loop(
do_cycle: Callable,
r_norm_sq: wp.array,
world_active: wp.array(dtype=wp.int32),
cur_iter: wp.array(dtype=wp.int32),
conditions: wp.array(dtype=wp.int32),
maxiter: wp.array(dtype=int),
atol_sq: wp.array,
callback: Callable | None,
use_cuda_graph: bool,
use_graph_conditionals: bool = True,
maxiter_host: int | None = None,
cycle_size: int = 1,
termination_kernel=None,
):
device = atol_sq.device
n_worlds = maxiter.shape[0]
cur_iter.fill_(-1)
conditions.fill_(1)
world_condition, global_condition = conditions[:n_worlds], conditions[n_worlds:]
update_condition_launch = wp.launch(
termination_kernel,
dim=(1, n_worlds),
block_dim=n_worlds,
device=device,
inputs=[maxiter, cycle_size, r_norm_sq, atol_sq, world_active, cur_iter],
outputs=[world_condition, global_condition],
record_cmd=True,
)
if isinstance(callback, wp.Kernel):
callback_launch = wp.launch(
callback, dim=n_worlds, device=device, inputs=[cur_iter, r_norm_sq, atol_sq], record_cmd=True
)
else:
callback_launch = None
# TODO: consider using a spinlock for fusing kernels
# update_world_condition_launch.launch()
# update_global_condition_launch.launch()
update_condition_launch.launch()
if callback_launch is not None:
callback_launch.launch()
def do_cycle_with_condition():
# print("Global cond:", global_condition.numpy())
do_cycle()
update_condition_launch.launch()
if callback_launch is not None:
callback_launch.launch()
if use_cuda_graph and device.is_cuda and device.is_capturing:
if use_graph_conditionals:
wp.capture_while(global_condition, do_cycle_with_condition)
else:
for _ in range(0, int(maxiter_host), cycle_size):
do_cycle_with_condition()
else:
for _ in range(0, int(maxiter.numpy().max()), cycle_size):
do_cycle_with_condition()
if not global_condition.numpy()[0]:
# print("Exiting")
break
return cur_iter, r_norm_sq, atol_sq
@wp.func
def lt_mask(a: Any, b: Any):
"""Return 1 if a < b, else 0"""
return wp.where(a < b, type(a)(1), type(a)(0))
@wp.func
def mul_mask(mask: Any, value: Any):
"""Return value if mask is positive, else 0"""
return wp.where(mask > type(mask)(0), value, type(value)(0))
@wp.func
def less_than_op(i: wp.int32, threshold: wp.int32) -> wp.float32:
return 1.0 if i < threshold else 0.0
@functools.cache
def make_dot_kernel(tile_size: int, maxdim: int):
num_tiles = (maxdim + tile_size - 1) // tile_size
@wp.kernel(enable_backward=False)
def dot(
a: wp.array3d(dtype=Any),
b: wp.array3d(dtype=Any),
world_size: wp.array(dtype=wp.int32),
world_active: wp.array(dtype=wp.int32),
result: wp.array2d(dtype=Any),
):
"""Compute the dot products between the trailing-dim arrays in a and b using tiles and pairwise summation."""
col, world, tid = wp.tid()
if not world_active[world]:
return
n = world_size[world]
if wp.static(num_tiles > 1):
ts = wp.tile_zeros((num_tiles,), dtype=a.dtype, storage="shared")
for tile_id in range(num_tiles):
o_src = tile_id * tile_size
if o_src >= n:
break
ta = wp.tile_load(a[col, world], shape=tile_size, offset=o_src)
tb = wp.tile_load(b[col, world], shape=tile_size, offset=o_src)
prod = wp.tile_map(wp.mul, ta, tb)
if o_src > n - tile_size:
mask = wp.tile_map(less_than_op, wp.tile_arange(tile_size, dtype=wp.int32), n - o_src)
prod = wp.tile_map(mul_mask, mask, prod)
if wp.static(num_tiles > 1):
ts[tile_id] = wp.tile_sum(prod)[0]
else:
s = wp.tile_sum(prod)[0]
if wp.static(num_tiles > 1):
s = wp.tile_sum(ts)[0]
if tid == 0:
result[col, world] = s
return dot
@wp.kernel
def dot_sequential(
a: wp.array3d(dtype=Any),
b: wp.array3d(dtype=Any),
world_size: wp.array(dtype=wp.int32),
world_active: wp.array(dtype=wp.int32),
partial_sum: wp.array3d(dtype=Any),
):
col, world = wp.tid()
if not world_active[world]:
return
n = wp.int32(world_size[world])
for i in range((n + 1) // 2):
s = a[col, world, 2 * i] * b[col, world, 2 * i]
if 2 * i + 1 < n:
s += a[col, world, 2 * i + 1] * b[col, world, 2 * i + 1]
partial_sum[col, world, i] = s
n = (n + 1) // 2
while n > 1:
s = a.dtype(0)
if n & 1:
s += partial_sum[col, world, n - 1]
for i in range(n // 2):
s += partial_sum[col, world, 2 * i] + partial_sum[col, world, 2 * i + 1]
partial_sum[col, world, i] = s
s = a.dtype(0)
n = n // 2
@wp.kernel
def _initialize_tolerance_kernel(
rtol: wp.array(dtype=Any), atol: wp.array(dtype=Any), b_norm_sq: wp.array(dtype=Any), atol_sq: wp.array(dtype=Any)
):
world = wp.tid()
a, r = atol[world], rtol[world]
atol_sq[world] = wp.max(r * r * b_norm_sq[world], a * a)
@wp.kernel
def make_jacobi_preconditioner(
A: wp.array2d(dtype=Any), world_dims: wp.array(dtype=wp.int32), diag: wp.array2d(dtype=Any)
):
world, row = wp.tid()
world_dim = world_dims[world]
if row >= world_dim:
diag[world, row] = 0.0
return
el = A[world, row * world_dim + row]
el_inv = 1.0 / (el + 1e-9)
diag[world, row] = el_inv
class ConjugateSolver:
"""Base class for conjugate iterative solvers (CG, CR).
Solves batched linear systems Ax = b for multiple independent worlds in parallel.
Supports dense, diagonal, and block-sparse matrix operators with optional
preconditioning.
Note:
Temporary arrays are zero-initialized to avoid NaN propagation.
Args:
A: Linear operator representing the system matrix.
active_dims: Active dimension per world. If None, uses A.active_dims.
world_active: Int32 mask indicating which worlds are active (1) or inactive (0).
atol: Absolute tolerance for convergence. Scalar or per-world array.
rtol: Relative tolerance for convergence. Scalar or per-world array.
maxiter: Maximum iterations per world. If None, defaults to 1.5 * maxdims.
Mi: Operator applying the inverse preconditioner M^-1, such that Mi @ A has a smaller condition number than A.
callback: Optional callback kernel invoked each iteration.
use_cuda_graph: Whether to use CUDA graph capture for the solve loop.
"""
def __init__(
self,
A: BatchedLinearOperator,
active_dims: wp.array(dtype=Any) | None = None,
world_active: wp.array(dtype=wp.int32) | None = None,
atol: float | wp.array(dtype=Any) | None = None,
rtol: float | wp.array(dtype=Any) | None = None,
maxiter: wp.array = None,
Mi: BatchedLinearOperator | None = None,
callback: Callable | None = None,
use_cuda_graph: bool = True,
use_graph_conditionals: bool = True,
):
if not isinstance(A, BatchedLinearOperator):
raise ValueError("A must be a BatchedLinearOperator")
if Mi is not None and not isinstance(Mi, BatchedLinearOperator):
raise ValueError("Mi must be a BatchedLinearOperator or None")
self.scalar_type = wp.types.type_scalar_type(A.dtype)
self.n_worlds = A.n_worlds
self.maxdims = A.max_dim
self.A = A
self.Mi = Mi
self.device = A.device
self.active_dims = active_dims if active_dims is not None else A.active_dims
self.use_graph_conditionals = use_graph_conditionals
self.world_active = world_active
self.atol = atol
self.rtol = rtol
self.maxiter = maxiter
self.callback = callback
self.use_cuda_graph = use_cuda_graph
self.dot_tile_size = min(2048, 2 ** math.ceil(math.log(self.maxdims, 2)))
self.tiled_dot_kernel = make_dot_kernel(self.dot_tile_size, self.maxdims)
self._allocate()
def _allocate(self):
self.residual = wp.empty((self.n_worlds), dtype=self.scalar_type, device=self.device)
if self.maxiter is None:
maxiter = int(1.5 * self.maxdims)
self.maxiter = wp.full(self.n_worlds, maxiter, dtype=int, device=self.device)
self.maxiter_host = maxiter
else:
self.maxiter_host = int(max(self.maxiter.numpy()))
# TODO: non-tiled variant for CPU
if self.tiled_dot_product:
self.dot_product = wp.zeros((2, self.n_worlds), dtype=self.scalar_type, device=self.device)
else:
self.dot_partial_sums = wp.zeros((2, self.n_worlds, (self.maxdims + 1) // 2), device=self.device)
self.dot_product = self.dot_partial_sums[:, :, 0]
atol_val = self.atol if isinstance(self.atol, float) else 1e-8
rtol_val = self.rtol if isinstance(self.rtol, float) else 1e-8
if self.atol is None or isinstance(self.atol, float):
self.atol = wp.full(self.n_worlds, atol_val, dtype=self.scalar_type, device=self.device)
if self.rtol is None or isinstance(self.rtol, float):
self.rtol = wp.full(self.n_worlds, rtol_val, dtype=self.scalar_type, device=self.device)
self.atol_sq = wp.empty(self.n_worlds, dtype=self.scalar_type, device=self.device)
self.cur_iter = wp.empty(self.n_worlds, dtype=wp.int32, device=self.device)
self.conditions = wp.empty(self.n_worlds + 1, dtype=wp.int32, device=self.device)
self.termination_kernel = make_termination_kernel(self.n_worlds)
@property
def tiled_dot_product(self):
return wp.get_device(self.device).is_cuda
def compute_dot(self, a, b, active_dims, world_active, col_offset=0):
if a.ndim == 2:
a = a.reshape((1, *a.shape))
b = b.reshape((1, *b.shape))
if self.tiled_dot_product:
result = self.dot_product[col_offset:]
wp.launch_tiled(
self.tiled_dot_kernel,
dim=(a.shape[0], self.n_worlds),
block_dim=min(256, self.dot_tile_size // 8),
inputs=[a, b, active_dims, world_active],
outputs=[result],
device=self.device,
)
else:
partial_sums = self.dot_partial_sums[col_offset:]
wp.launch(
dot_sequential,
dim=(a.shape[0], self.n_worlds),
inputs=[a, b, active_dims, world_active],
outputs=[partial_sums],
device=self.device,
)
class CGSolver(ConjugateSolver):
"""Conjugate Gradient solver for symmetric positive definite systems.
The solver terminates when ||r||^2 < max(rtol^2 * ||b||^2, atol^2) or
when maxiter iterations are reached.
"""
def _allocate(self):
super()._allocate()
# Temp storage
self.r_and_z = wp.zeros((2, self.n_worlds, self.maxdims), dtype=self.scalar_type, device=self.device)
self.p_and_Ap = wp.zeros_like(self.r_and_z)
# (r, r) -- so we can compute r.z and r.r at once
self.r_repeated = _repeat_first(self.r_and_z)
if self.Mi is None:
# without preconditioner r == z
self.r_and_z = self.r_repeated
self.rz_new = self.dot_product[0]
else:
self.rz_new = self.dot_product[1]
def update_rr_rz(self, r, z, r_repeated, active_dims, world_active):
# z = M r
if self.Mi is None:
self.compute_dot(r, r, active_dims, world_active)
else:
self.Mi.matvec(r, z, world_active)
self.compute_dot(r_repeated, self.r_and_z, active_dims, world_active)
def solve(
self,
b: wp.array,
x: wp.array,
active_dims: wp.array(dtype=Any) | None = None,
world_active: wp.array(dtype=wp.int32) | None = None,
):
if active_dims is None:
if self.active_dims is None:
raise ValueError("Error, active_dims must be provided either to constructor or to solve()")
active_dims = self.active_dims
if world_active is None:
if self.world_active is None:
raise ValueError("Error, world_active must be provided either to constructor or to solve()")
world_active = self.world_active
r, z = self.r_and_z[0], self.r_and_z[1]
r_norm_sq = self.dot_product[0]
p, Ap = self.p_and_Ap[0], self.p_and_Ap[1]
self.compute_dot(b, b, active_dims, world_active)
wp.launch(
kernel=_initialize_tolerance_kernel,
dim=self.n_worlds,
device=self.device,
inputs=[self.rtol, self.atol, self.dot_product[0]],
outputs=[self.atol_sq],
)
r.assign(b)
self.A.gemv(x, r, world_active, alpha=-1.0, beta=1.0)
self.update_rr_rz(r, z, self.r_repeated, active_dims, world_active)
p.assign(z)
do_iteration = functools.partial(
self.do_iteration,
p=p,
Ap=Ap,
rz_old=self.residual,
rz_new=self.rz_new,
z=z,
x=x,
r=r,
r_norm_sq=r_norm_sq,
active_dims=active_dims,
world_active=world_active,
)
return _run_capturable_loop(
do_iteration,
r_norm_sq,
world_active,
self.cur_iter,
self.conditions,
self.maxiter,
self.atol_sq,
self.callback,
self.use_cuda_graph,
termination_kernel=self.termination_kernel,
use_graph_conditionals=self.use_graph_conditionals,
maxiter_host=self.maxiter_host,
)
def do_iteration(self, p, Ap, rz_old, rz_new, z, x, r, r_norm_sq, active_dims, world_active):
rz_old.assign(rz_new)
# Ap = A * p
self.A.matvec(p, Ap, world_active)
self.compute_dot(p, Ap, active_dims, world_active, col_offset=1)
p_Ap = self.dot_product[1]
wp.launch(
kernel=_cg_kernel_1,
dim=(self.n_worlds, self.maxdims),
inputs=[self.atol_sq, r_norm_sq, rz_old, p_Ap, p, Ap],
outputs=[x, r],
device=self.device,
)
self.update_rr_rz(r, z, self.r_repeated, active_dims, world_active)
wp.launch(
kernel=_cg_kernel_2,
dim=(self.n_worlds, self.maxdims),
inputs=[self.atol_sq, r_norm_sq, rz_old, rz_new, z],
outputs=[p],
device=self.device,
)
class CRSolver(ConjugateSolver):
"""Conjugate Residual solver for symmetric (possibly indefinite) systems.
The solver terminates when ||r||^2 < max(rtol^2 * ||b||^2, atol^2) or
when maxiter iterations are reached.
"""
def _allocate(self):
super()._allocate()
# Temp storage
self.r_and_z = wp.zeros((2, self.n_worlds, self.maxdims), dtype=self.scalar_type, device=self.device)
self.r_and_Az = wp.zeros_like(self.r_and_z)
self.y_and_Ap = wp.zeros_like(self.r_and_z)
self.p = wp.zeros((self.n_worlds, self.maxdims), dtype=self.scalar_type, device=self.device)
# (r, r) -- so we can compute r.z and r.r at once
if self.Mi is None:
# For the unpreconditioned case, z == r and y == Ap
self.r_and_z = _repeat_first(self.r_and_z)
self.y_and_Ap = _repeat_first(self.y_and_Ap)
def update_rr_zAz(self, z, Az, r, r_copy, active_dims, world_active):
self.A.matvec(z, Az, world_active)
r_copy.assign(r)
self.compute_dot(self.r_and_z, self.r_and_Az, active_dims, world_active)
def solve(
self,
b: wp.array,
x: wp.array,
active_dims: wp.array(dtype=Any) | None = None,
world_active: wp.array(dtype=wp.int32) | None = None,
):
if active_dims is None:
if self.active_dims is None:
raise ValueError("Error, active_dims must be provided either to constructor or to solve()")
active_dims = self.active_dims
if world_active is None:
if self.world_active is None:
raise ValueError("Error, world_active must be provided either to constructor or to solve()")
world_active = self.world_active
# named views
r, z = self.r_and_z[0], self.r_and_z[1]
r_copy, Az = self.r_and_Az[0], self.r_and_Az[1]
y, Ap = self.y_and_Ap[0], self.y_and_Ap[1]
r_norm_sq = self.dot_product[0]
# Initialize tolerance from right-hand-side norm
self.compute_dot(b, b, active_dims, world_active)
wp.launch(
kernel=_initialize_tolerance_kernel,
dim=self.n_worlds,
device=self.device,
inputs=[self.rtol, self.atol, self.dot_product[0]],
outputs=[self.atol_sq],
)
r.assign(b)
self.A.gemv(x, r, world_active, alpha=-1.0, beta=1.0)
# z = M r
if self.Mi is not None:
self.Mi.matvec(r, z, world_active)
self.update_rr_zAz(z, Az, r, r_copy, active_dims, world_active)
self.p.assign(z)
Ap.assign(Az)
do_iteration = functools.partial(
self.do_iteration,
p=self.p,
Ap=Ap,
Az=Az,
zAz_old=self.residual,
zAz_new=self.dot_product[1],
z=z,
y=y,
x=x,
r=r,
r_copy=r_copy,
r_norm_sq=r_norm_sq,
active_dims=active_dims,
world_active=world_active,
)
return _run_capturable_loop(
do_iteration,
r_norm_sq,
world_active,
self.cur_iter,
self.conditions,
self.maxiter,
self.atol_sq,
self.callback,
self.use_cuda_graph,
termination_kernel=self.termination_kernel,
use_graph_conditionals=self.use_graph_conditionals,
maxiter_host=self.maxiter_host,
)
def do_iteration(self, p, Ap, Az, zAz_old, zAz_new, z, y, x, r, r_copy, r_norm_sq, active_dims, world_active):
zAz_old.assign(zAz_new)
if self.Mi is not None:
self.Mi.matvec(Ap, y, world_active)
self.compute_dot(Ap, y, active_dims, world_active, col_offset=1)
y_Ap = self.dot_product[1]
if self.Mi is None:
# In non-preconditioned case, first kernel is same as CG
wp.launch(
kernel=_cg_kernel_1,
dim=(self.n_worlds, self.maxdims),
inputs=[self.atol_sq, r_norm_sq, zAz_old, y_Ap, p, Ap],
outputs=[x, r],
device=self.device,
)
else:
# In preconditioned case, we have one more vector to update
wp.launch(
kernel=_cr_kernel_1,
dim=(self.n_worlds, self.maxdims),
inputs=[self.atol_sq, r_norm_sq, zAz_old, y_Ap, p, Ap, y],
outputs=[x, r, z],
device=self.device,
)
self.update_rr_zAz(z, Az, r, r_copy, active_dims, world_active)
wp.launch(
kernel=_cr_kernel_2,
dim=(self.n_worlds, self.maxdims),
inputs=[self.atol_sq, r_norm_sq, zAz_old, zAz_new, z, Az],
outputs=[p, Ap],
device=self.device,
)
def _repeat_first(arr: wp.array):
# returns a view of the first element repeated arr.shape[0] times
view = wp.array(
ptr=arr.ptr,
shape=arr.shape,
dtype=arr.dtype,
strides=(0, *arr.strides[1:]),
device=arr.device,
)
view._ref = arr
return view
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/core.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Linear Algebra: Core types and utilities for multi-linear systems
This module provides data structures and utilities for managing multiple
independent linear systems, including rectangular and square systems.
"""
from dataclasses import dataclass
import numpy as np
import warp as wp
from ..core.types import FloatType, IntType, VecIntType, float32, int32
from ..utils import logger as msg
###
# Module interface
###
__all__ = [
"DenseLinearOperatorData",
"DenseRectangularMultiLinearInfo",
"DenseSquareMultiLinearInfo",
"make_dtype_tolerance",
]
###
# Types
###
@dataclass
class DenseRectangularMultiLinearInfo:
"""
A data structure for managing multiple rectangular linear systems of inhomogeneous and mutable shapes, i.e.:
`A_i @ x_i = b_i`, for `i = 1, ..., num_blocks`,
where:
- each `A_i` is a rectangular matrix of active shape `(dim[i][0], dim[i][1])` and
maximum shape `(maxdim[i][0], maxdim[i][1])` starting at offset `mio[i]`
- each `b_i` is a right-hand-side (rhs) vector of active shape `(dim[i][0],)`
and maximum shape `(maxdim[i][1],)` starting at offset `rvio[i]`
- each `x_i` is a input vector of active shape `(dim[i][1],)` and
maximum shape `(maxdim[i][0],)` starting at offset `ivio[i]`
- `num_blocks` is the number of linear systems managed by this data structure
The underlying data allocation is determined by the sum of `maxdim[i]` values, while the "active"
shapes are determined by the `dim[i]` values. Thus the allocated memory corresponds to:
- `sum(maxdim[i][0]*maxdim[i][1] for i in range(num_blocks))` for matrices
- `sum(maxdim[i][1] for i in range(num_blocks))` for rhs vectors
- `sum(maxdim[i][0] for i in range(num_blocks))` for input vectors
Kernels operating on data described by this structure can then use the max over `maxdims` to set
the multi-dimensional thread block size, while using the `dim` values at execution time to determine
the actual active shape of each block and access the correct data offsets using the `mio` and `vio` arrays.
"""
num_blocks: int = 0
"""Host-side cache of the number of data blocks represented in each flat data array."""
dimensions: list[tuple[int, int]] | None = None
"""Host-side cache of the dimensions of each rectangular linear system."""
max_dimensions: tuple[int, int] = (0, 0)
"""Host-side cache of the maximum dimension over all matrix blocks."""
total_mat_size: int = 0
"""
Host-side cache of the total size of the flat matrix data array.
This is equal to `sum(maxdim[i][0]*maxdim[i][1] for i in range(num_blocks))`.
"""
total_rhs_size: int = 0
"""
Host-side cache of the total size of the flat data array of rhs vectors.
This is equal to `sum(maxdim[i][1] for i in range(num_blocks))`.
"""
total_inp_size: int = 0
"""
Host-side cache of the total size of the flat data array of input vectors.
This is equal to `sum(maxdim[i][0] for i in range(num_blocks))`.
"""
dtype: FloatType = float32
"""The data type of the underlying matrix and vector data arrays."""
itype: IntType = int32
"""The integer type used for indexing the underlying data arrays."""
device: wp.DeviceLike | None = None
"""The device on which the data arrays are allocated."""
maxdim: wp.array | None = None
"""
The maximum dimensions of each rectangular matrix block.
Shape of ``(num_blocks,)`` and type :class:`vec2i`.
Each entry corresponds to the shape `(max_rows, max_cols)`.
"""
dim: wp.array | None = None
"""
The active dimensions of each rectangular matrix block.
Shape of ``(num_blocks,)`` and type :class:`vec2i`.
Each entry corresponds to the shape `(rows, cols)`.
"""
mio: wp.array | None = None
"""
The matrix index offset (mio) of each block in the flat data array.
Shape of ``(num_blocks,)`` and type :class:`int | int32 | int64`.
"""
rvio: wp.array | None = None
"""
The rhs vector index offset (vio) of each block in the flat data array.
Shape of ``(num_blocks,)`` and type :class:`int | int32 | int64`.
"""
ivio: wp.array | None = None
"""
The input vector index offset (vio) of each block in the flat data array.
Shape of ``(num_blocks,)`` and type :class:`int | int32 | int64`.
"""
@staticmethod
def _check_dimensions(dims: list[tuple[int, int]] | tuple[int, int]) -> list[tuple[int, int]]:
if isinstance(dims, tuple):
if len(dims) != 2:
raise ValueError("Dimension tuple must have exactly two entries.")
if dims[0] <= 0 or dims[1] <= 0:
raise ValueError("Dimensions must be positive integers.")
dims = [dims]
elif isinstance(dims, list):
if len(dims) > 0 and not all(
isinstance(d, tuple) and len(d) == 2 and all(isinstance(i, int) and i > 0 for i in d) for d in dims
):
raise ValueError("All dimensions must be tuples of two positive integers.")
else:
raise TypeError("Dimensions must be a pair of positive integers or a list of positive integer pairs.")
return dims
def finalize(
self,
dimensions: list[tuple[int, int]],
dtype: FloatType = float32,
itype: IntType = int32,
device: wp.DeviceLike = None,
) -> None:
"""
Constructs and allocates the data of the rectangular multi-linear system info on the specified device.
"""
# Ensure the problem dimensions are valid and cache them
self.dimensions = self._check_dimensions(dimensions)
# Ensure the dtype and itype are valid
if not issubclass(dtype, FloatType):
raise TypeError("Invalid dtype. Expected FloatType type, e.g. `wp.float32` or `wp.float64`.")
if not issubclass(itype, IntType):
raise TypeError("Invalid itype. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
self.dtype = dtype
self.itype = itype
# Override the device identifier if specified, otherwise use the current device
if device is not None:
self.device = device
# Compute the allocation sizes and offsets for the flat data arrays
mat_sizes = [m * n for m, n in self.dimensions]
mat_offsets = [0] + [sum(mat_sizes[:i]) for i in range(1, len(mat_sizes) + 1)]
mat_flat_size = sum(mat_sizes)
max_mat_rows = max(m for m, _ in self.dimensions)
max_mat_cols = max(n for _, n in self.dimensions)
rhs_sizes = [m for m, _ in self.dimensions]
rhs_offsets = [0] + [sum(rhs_sizes[:i]) for i in range(1, len(rhs_sizes) + 1)]
rhs_flat_size = sum(rhs_sizes)
inp_sizes = [n for _, n in self.dimensions]
inp_offsets = [0] + [sum(inp_sizes[:i]) for i in range(1, len(inp_sizes) + 1)]
inp_flat_size = sum(inp_sizes)
# Update the allocation meta-data the specified system dimensions
self.num_blocks = len(self.dimensions)
self.max_dimensions = (max_mat_rows, max_mat_cols)
self.total_mat_size = mat_flat_size
self.total_rhs_size = rhs_flat_size
self.total_inp_size = inp_flat_size
# Declare local 2D dimension type
class _vec2i(wp.types.vector(length=2, dtype=self.itype)):
pass
# Allocate the multi-linear square system info data on the specified device
with wp.ScopedDevice(self.device):
self.maxdim = wp.array(self.dimensions, dtype=_vec2i)
self.dim = wp.array(self.dimensions, dtype=_vec2i)
self.mio = wp.array(mat_offsets[: self.num_blocks], dtype=self.itype)
self.rvio = wp.array(rhs_offsets[: self.num_blocks], dtype=self.itype)
self.ivio = wp.array(inp_offsets[: self.num_blocks], dtype=self.itype)
def assign(
self,
maxdim: wp.array,
dim: wp.array,
mio: wp.array,
rvio: wp.array,
ivio: wp.array,
dtype: FloatType = float32,
device: wp.DeviceLike = None,
) -> None:
"""
Assigns the data of the square multi-linear system info from externally allocated arrays.
"""
# Ensure the problem dimensions are valid and cache them
self.dimensions = self._check_dimensions(maxdim.list())
# Ensure the dtype and itype are valid
if not issubclass(dtype, FloatType):
raise TypeError("Invalid dtype. Expected FloatType type, e.g. `wp.float32` or `wp.float64`.")
if not issubclass(maxdim.dtype, VecIntType):
raise TypeError(
"Invalid dtype of `maxdim` argument. Expected integer vector type, e.g. `wp.vec2i` or `wp.vec2l`."
)
if not issubclass(dim.dtype, VecIntType):
raise TypeError(
"Invalid dtype of `dim` argument. Expected integer vector type, e.g. `wp.vec2i` or `wp.vec2l`."
)
if not issubclass(mio.dtype, IntType):
raise TypeError("Invalid dtype of `mio` argument. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
if not issubclass(rvio.dtype, IntType):
raise TypeError("Invalid dtype of `rvio` argument. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
if not issubclass(ivio.dtype, IntType):
raise TypeError("Invalid dtype of `ivio` argument. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
# Cache the data type information
self.dtype = dtype
self.itype = maxdim.dtype
# Override the device identifier if specified, otherwise use the current device
if device is not None:
self.device = device
# Compute the allocation sizes and offsets for the flat data arrays
mat_sizes = [m * n for m, n in self.dimensions]
mat_flat_size = sum(mat_sizes)
max_mat_rows = max(m for m, _ in self.dimensions)
max_mat_cols = max(n for _, n in self.dimensions)
rhs_sizes = [m for m, _ in self.dimensions]
rhs_flat_size = sum(rhs_sizes)
inp_sizes = [n for _, n in self.dimensions]
inp_flat_size = sum(inp_sizes)
# Update the allocation meta-data the specified system dimensions
self.num_blocks = len(self.dimensions)
self.max_dimensions = (max_mat_rows, max_mat_cols)
self.total_mat_size = mat_flat_size
self.total_rhs_size = rhs_flat_size
self.total_inp_size = inp_flat_size
# Capture references the rectangular multi-linear system info data on the specified device
self.maxdim = maxdim
self.dim = dim
self.mio = mio
self.rvio = rvio
self.ivio = ivio
def is_matrix_compatible(self, A: wp.array) -> bool:
"""Checks if the provided matrix data array is compatible with the specified info structure."""
return A.dtype == self.dtype and A.size >= self.total_mat_size
def is_rhs_compatible(self, b: wp.array) -> bool:
"""Checks if the provided rhs vector data array is compatible with the specified info structure."""
return b.dtype == self.dtype and b.size >= self.total_rhs_size
def is_input_compatible(self, x: wp.array) -> bool:
"""Checks if the provided input vector data array is compatible with the specified info structure."""
return x.dtype == self.dtype and x.size >= self.total_inp_size
def __str__(self) -> str:
return (
f"DenseRectangularMultiLinearInfo(\n"
f" num_blocks={self.num_blocks},\n"
f" dimensions={self.dimensions},\n"
f" max_dimensions={self.max_dimensions},\n"
f" total_mat_size={self.total_mat_size},\n"
f" total_rhs_size={self.total_rhs_size},\n"
f" total_inp_size={self.total_inp_size},\n"
f" dtype={self.dtype},\n"
f" itype={self.itype},\n"
f" device={self.device}\n"
f")"
)
@dataclass
class DenseSquareMultiLinearInfo:
"""
A data structure for managing multiple square linear systems of inhomogeneous and mutable shapes, i.e.:
`A_i @ x_i = b_i`, for `i = 1, ..., num_blocks`,
where:
- each `A_i` is a square matrix of active shape `(dim[i], dim[i])` and
maximum shape `(maxdim[i], maxdim[i])` starting at offset `mio[i]`
- each `b_i` is a right-hand-side (rhs) vector of active shape `(dim[i],)`
and maximum shape `(maxdim[i],)` starting at offset `vio[i]`
- each `x_i` is a input vector of active shape `(dim[i],)` and
maximum shape `(maxdim[i],)` starting at offset `vio[i]`
- `num_blocks` is the number of linear systems managed by this data structure
The underlying data allocation is determined by the sum of `maxdim[i]` values, while the "active"
shapes are determined by the `dim[i]` values. Thus the allocated memory corresponds to:
- `sum(maxdim[i]*maxdim[i] for i in range(num_blocks))` for matrices
- `sum(maxdim[i] for i in range(num_blocks))` for rhs vectors
- `sum(maxdim[i] for i in range(num_blocks))` for input vectors
Kernels operating on data described by this structure can then use the max over `maxdims` to set
the multi-dimensional thread block size, while using the `dim` values at execution time to determine
the actual active shape of each block and access the correct data offsets using the `mio` and `vio` arrays.
"""
num_blocks: int = 0
"""Host-side cache of the number of data blocks represented in each flat data array."""
dimensions: list[int] | None = None
"""Host-side cache of the dimensions of each square linear system."""
max_dimension: int = 0
"""Host-side cache of the maximum dimension over all matrix blocks."""
total_mat_size: int = 0
"""
Host-side cache of the total size of the flat data array of matrix blocks.
This is equal to `sum(maxdim[i][0]*maxdim[i][1] for i in range(num_blocks))`.
"""
total_vec_size: int = 0
"""
Host-side cache of the total size of the flat data array of vector blocks.
This is equal to `sum(maxdim[i][1] for i in range(num_blocks))`.
"""
dtype: FloatType = float32
"""The data type of the underlying matrix and vector data arrays."""
itype: IntType = int32
"""The integer type used for indexing the underlying data arrays."""
device: wp.DeviceLike | None = None
"""The device on which the data arrays are allocated."""
maxdim: wp.array | None = None
"""
The maximum dimensions of each square matrix block.
Shape of ``(num_blocks,)`` and type :class:`int | int32 | int64`.
"""
dim: wp.array | None = None
"""
The active dimensions of each square matrix block.
Shape of ``(num_blocks,)`` and type :class:`int | int32 | int64`.
"""
mio: wp.array | None = None
"""
The matrix index offset (mio) of each matrix block in the flat data array.
Shape of ``(num_blocks,)`` and type :class:`int | int32 | int64`.
"""
vio: wp.array | None = None
"""
The vector index offset (vio) of each vector block in the flat data array.
Shape of ``(num_blocks,)`` and type :class:`int | int32 | int64`.
"""
@staticmethod
def _check_dimensions(dims: list[int] | int) -> list[int]:
if isinstance(dims, int):
if dims <= 0:
raise ValueError("Dimension must be a positive integer.")
dims = [dims]
elif isinstance(dims, list):
if len(dims) > 0 and not all(isinstance(d, int) and d > 0 for d in dims):
raise ValueError("All dimensions must be positive integers.")
else:
raise TypeError("Dimensions must be an positive integer or a list of positive integers.")
return dims
def finalize(
self, dimensions: list[int], dtype: FloatType = float32, itype: IntType = int32, device: wp.DeviceLike = None
) -> None:
"""
Constructs and allocates the data of the square multi-linear system info on the specified device.
"""
# Ensure the problem dimensions are valid and cache them
self.dimensions = self._check_dimensions(dimensions)
# Ensure the dtype and itype are valid
if not issubclass(dtype, FloatType):
raise TypeError("Invalid dtype. Expected FloatType type, e.g. `wp.float32` or `wp.float64`.")
if not issubclass(itype, IntType):
raise TypeError("Invalid itype. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
self.dtype = dtype
self.itype = itype
# Override the device identifier if specified, otherwise use the current device
if device is not None:
self.device = device
# Compute the allocation sizes and offsets for the flat data arrays
mat_sizes = [n * n for n in self.dimensions]
mat_offsets = [0] + [sum(mat_sizes[:i]) for i in range(1, len(mat_sizes) + 1)]
mat_flat_size = sum(mat_sizes)
vec_sizes = self.dimensions
vec_offsets = [0] + [sum(vec_sizes[:i]) for i in range(1, len(vec_sizes) + 1)]
vec_flat_size = sum(vec_sizes)
# Update the allocation meta-data the specified system dimensions
self.num_blocks = len(self.dimensions)
self.max_dimension = max(self.dimensions)
self.total_mat_size = mat_flat_size
self.total_vec_size = vec_flat_size
# Allocate the multi-linear square system info data on the specified device
with wp.ScopedDevice(self.device):
self.maxdim = wp.array(self.dimensions, dtype=self.itype)
self.dim = wp.array(self.dimensions, dtype=self.itype)
self.mio = wp.array(mat_offsets[: self.num_blocks], dtype=self.itype)
self.vio = wp.array(vec_offsets[: self.num_blocks], dtype=self.itype)
def assign(
self,
maxdim: wp.array,
dim: wp.array,
mio: wp.array,
vio: wp.array,
dtype: FloatType = float32,
device: wp.DeviceLike = None,
) -> None:
"""
Assigns the data of the square multi-linear system info from externally allocated arrays.
"""
# Ensure the problem dimensions are valid and cache them
self.dimensions = self._check_dimensions(maxdim.numpy().astype(int).tolist())
# Ensure the dtype and itype are valid
if not issubclass(dtype, FloatType):
raise TypeError("Invalid dtype. Expected FloatType type, e.g. `wp.float32` or `wp.float64`.")
if not issubclass(maxdim.dtype, IntType):
raise TypeError("Invalid dtype of `maxdim` argument. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
if not issubclass(dim.dtype, IntType):
raise TypeError("Invalid dtype of `dim` argument. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
if not issubclass(mio.dtype, IntType):
raise TypeError("Invalid dtype of `mio` argument. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
if not issubclass(vio.dtype, IntType):
raise TypeError("Invalid dtype of `vio` argument. Expected IntType type, e.g. `wp.int32` or `wp.int64`.")
# Cache the data type information
self.dtype = dtype
self.itype = maxdim.dtype
# Override the device identifier if specified, otherwise use the current device
if device is not None:
self.device = device
# Compute the allocation sizes and offsets for the flat data arrays
mat_sizes = [n * n for n in self.dimensions]
mat_flat_size = sum(mat_sizes)
vec_sizes = self.dimensions
vec_flat_size = sum(vec_sizes)
# Update the allocation meta-data the specified system dimensions
self.num_blocks = len(self.dimensions)
self.max_dimension = max(self.dimensions)
self.total_mat_size = mat_flat_size
self.total_vec_size = vec_flat_size
# Capture references the multi-linear square system info data on the specified device
self.maxdim = maxdim
self.dim = dim
self.mio = mio
self.vio = vio
def is_matrix_compatible(self, A: wp.array) -> bool:
"""Checks if the provided matrix data array is compatible with the specified info structure."""
return A.dtype == self.dtype and A.size >= self.total_mat_size
def is_rhs_compatible(self, b: wp.array) -> bool:
"""Checks if the provided rhs vector data array is compatible with the specified info structure."""
return b.dtype == self.dtype and b.size >= self.total_vec_size
def is_input_compatible(self, x: wp.array) -> bool:
"""Checks if the provided input vector data array is compatible with the specified info structure."""
return x.dtype == self.dtype and x.size >= self.total_vec_size
def __str__(self) -> str:
return (
f"DenseSquareMultiLinearInfo(\n"
f" num_blocks={self.num_blocks},\n"
f" dimensions={self.dimensions},\n"
f" max_dimension={self.max_dimension},\n"
f" total_mat_size={self.total_mat_size},\n"
f" total_vec_size={self.total_vec_size},\n"
f" dtype={self.dtype},\n"
f" itype={self.itype},\n"
f" device={self.device}\n"
f")"
)
@dataclass
class DenseLinearOperatorData:
"""
A data structure for encapsulating a multi-linear matrix operator.
This object essentially wraps a flattened memory allocation of multiple
matrix blocks along with a data structure describing the layout of the
blocks, and provides a unified interface for linear solvers to operate
on the encapsulated operator.
The `info` member can be owned by this object or captured by reference
from an external source to avoid unnecessary memory reallocations.
"""
info: DenseRectangularMultiLinearInfo | DenseSquareMultiLinearInfo | None = None
"""The multi-linear data structure describing the operator."""
mat: wp.array | None = None
"""The flat data array containing the matrix blocks."""
def zero(self) -> None:
self.mat.zero_()
###
# Utilities
###
def make_dtype_tolerance(tol: FloatType | float | None = None, dtype: FloatType = float32) -> FloatType:
# First ensure the specified dtype is a valid warp type
if not issubclass(dtype, FloatType):
raise ValueError("data type 'dtype' must be a FloatType, e.g. a `wp.float32` or `wp.float64` value etc.")
# Extract machine epsilon for the specified dtype
eps = float(np.finfo(wp.dtype_to_numpy(dtype)).eps)
# Default tolerance to machine epsilon if not provided
if tol is None:
return dtype(eps)
# Otherwise ensure the provided tolerance is valid and converted to the requested dtype
else:
# Ensure the provided tolerance is a valid FloatType value
if not isinstance(tol, FloatType | float):
raise ValueError(
"tolerance 'tol' must be a FloatType, i.e. a `float`, `wp.float32`, or `wp.float64` value."
)
# Ensure the provided tolerance is positive and non-zero
if float(tol) <= 0:
raise ValueError("tolerance 'tol' must be a positive value.")
# Issue warning if the provided tolerance is smaller than machine epsilon
if float(tol) < eps:
msg.warning(
f"tolerance 'tol' = {tol} is smaller than machine epsilon "
f"for the specified dtype '{dtype}' (eps = {eps}). Clamping to eps."
)
# Return the tolerance clamped to machine epsilon for the specified dtype
return dtype(max(float(tol), eps))
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/factorize/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Linear Algebra: Matrix factorization implementations (kernels and launchers)"""
from .llt_blocked import (
llt_blocked_factorize,
llt_blocked_solve,
llt_blocked_solve_inplace,
make_llt_blocked_factorize_kernel,
make_llt_blocked_solve_inplace_kernel,
make_llt_blocked_solve_kernel,
)
from .llt_sequential import (
_llt_sequential_factorize,
_llt_sequential_solve,
_llt_sequential_solve_inplace,
llt_sequential_factorize,
llt_sequential_solve,
llt_sequential_solve_inplace,
)
###
# Module API
###
__all__ = [
"_llt_sequential_factorize",
"_llt_sequential_solve",
"_llt_sequential_solve_inplace",
"llt_blocked_factorize",
"llt_blocked_solve",
"llt_blocked_solve_inplace",
"llt_sequential_factorize",
"llt_sequential_solve",
"llt_sequential_solve_inplace",
"make_llt_blocked_factorize_kernel",
"make_llt_blocked_solve_inplace_kernel",
"make_llt_blocked_solve_kernel",
]
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/factorize/llt_blocked.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Linear Algebra: Blocked LLT (i.e. Cholesky) factorization using Warp's Tile API."""
from ctypes import sizeof
from functools import cache
import warp as wp
from ...core.types import float32, int32
###
# Module interface
###
__all__ = [
"llt_blocked_factorize",
"llt_blocked_solve",
"llt_blocked_solve_inplace",
"make_llt_blocked_factorize_kernel",
"make_llt_blocked_solve_inplace_kernel",
"make_llt_blocked_solve_kernel",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Functions
###
get_array_ptr_cpp = """return (uint64_t)arr.data;"""
"""A native C++ function to get the raw pointer of a warp array."""
def make_get_array_offset_ptr_func(dtype):
"""Creates a function to get the offset pointer of a warp array."""
# Define a Warp wrapper around a native C++ function to get the raw pointer of a warp array
@wp.func_native(get_array_ptr_cpp)
def get_dtype_array_ptr(arr: wp.array(dtype=dtype)) -> wp.uint64: ...
# Define a Warp function to get the raw pointer of a warp array with an offset
@wp.func
def get_dtype_array_offset_ptr(arr: wp.array(dtype=dtype), start_index: int) -> wp.uint64:
return get_dtype_array_ptr(arr) + wp.uint64(start_index * wp.static(sizeof(dtype._type_)))
return get_dtype_array_offset_ptr
get_int32_array_offset_ptr = make_get_array_offset_ptr_func(wp.int32)
"""A Warp function to get the offset pointer of a int32 warp array."""
get_float32_array_offset_ptr = make_get_array_offset_ptr_func(wp.float32)
"""A Warp function to get the offset pointer of a float32 warp array."""
# @wp.func
# def tile_sum_func(a: wp.tile(dtype=float, shape=(TILE_M, TILE_N))):
# return wp.tile_sum(a) * 0.5
# def make_tile_pad(block_size: int, dtype=float32):
# """Creates a function to pad a tile with identity values where the tile exceeds the matrix dimensions."""
# @wp.func
# def tile_pad(
# T: wp.tile(dtype=dtype, shape=(block_size, block_size)),
# i: int,
# j: int,
# n_i: int,
# n_j: int,
# tid_block: int,
# num_threads_per_block: int,
# ) -> None:
# """Pads a tile T with identity values where the tile exceeds the matrix dimensions."""
# if i + block_size > n_i or j + block_size > n_j:
# num_tile_elements = block_size * block_size
# num_iterations = (num_tile_elements + num_threads_per_block - 1) // num_threads_per_block
# for ii in range(num_iterations):
# linear_index = tid_block + ii * num_threads_per_block
# linear_index = linear_index % num_tile_elements
# row = linear_index // block_size
# col = linear_index % block_size
# value = T[row, col]
# if i + row >= n_i or j + col >= n_j:
# value = wp.where(i + row == j + col, dtype(1), dtype(0))
# T[row, col] = value
# return tile_pad
###
# Kernels
###
@cache
def make_llt_blocked_factorize_kernel(block_size: int):
@wp.kernel
def llt_blocked_factorize_kernel(
# Inputs:
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
A: wp.array(dtype=float32),
# Outputs:
L: wp.array(dtype=float32),
):
# Retrieve the thread index and thread-block configuration
tid, tid_block = wp.tid()
num_threads_per_block = wp.block_dim()
# Retrieve the matrix block dimensions and size
n_i = dim[tid]
A_i_start = mio[tid]
# Retrieve a pointer to the start of the i-th matrix in A
A_i_ptr = get_float32_array_offset_ptr(A, A_i_start)
L_i_ptr = get_float32_array_offset_ptr(L, A_i_start)
# Create a temporary warp array pointing to the i-th matrix
A_i = wp.array(ptr=A_i_ptr, shape=(n_i, n_i), dtype=wp.float32)
L_i = wp.array(ptr=L_i_ptr, shape=(n_i, n_i), dtype=wp.float32)
# Round up active_matrix_size to next multiple of block_size
n_i_padded = ((n_i + block_size - 1) // block_size) * block_size
# Process the matrix in blocks along its leading dimension.
for k in range(0, n_i_padded, block_size):
end = k + block_size
# Load current diagonal block A[k:end, k:end]
# and update with contributions from previously computed blocks.
A_kk_tile = wp.tile_load(A_i, shape=(block_size, block_size), offset=(k, k), storage="shared")
# The following if pads the matrix if it is not divisible by block_size
if k + block_size > n_i:
num_tile_elements = block_size * block_size
num_iterations = (num_tile_elements + num_threads_per_block - 1) // num_threads_per_block
for i in range(num_iterations):
linear_index = tid_block + i * num_threads_per_block
linear_index = linear_index % num_tile_elements
row = linear_index // block_size
col = linear_index % block_size
value = A_kk_tile[row, col]
if k + row >= n_i or k + col >= n_i:
value = wp.where(row == col, float32(1), float32(0))
A_kk_tile[row, col] = value
# Update the diagonal block with contributions from previously computed blocks
if k > 0:
for j in range(0, k, block_size):
L_block = wp.tile_load(L_i, shape=(block_size, block_size), offset=(k, j))
L_block_T = wp.tile_transpose(L_block)
L_L_T_block = wp.tile_matmul(L_block, L_block_T)
A_kk_tile -= L_L_T_block
# Compute the Cholesky factorization for the block
L_kk_tile = wp.tile_cholesky(A_kk_tile)
wp.tile_store(L_i, L_kk_tile, offset=(k, k))
# Process the blocks below the current block
for i in range(end, n_i_padded, block_size):
# Load the current block A[i:end, k:end]
A_ik_tile = wp.tile_load(A_i, shape=(block_size, block_size), offset=(i, k), storage="shared")
# The following if pads the matrix if it is not divisible by block_size
if i + block_size > n_i or k + block_size > n_i:
num_tile_elements = block_size * block_size
num_iterations = (num_tile_elements + num_threads_per_block - 1) // num_threads_per_block
for ii in range(num_iterations):
linear_index = tid_block + ii * num_threads_per_block
linear_index = linear_index % num_tile_elements
row = linear_index // block_size
col = linear_index % block_size
value = A_ik_tile[row, col]
if i + row >= n_i or k + col >= n_i:
value = wp.where(i + row == k + col, float32(1), float32(0))
A_ik_tile[row, col] = value
# Update the block with contributions from previously computed blocks
if k > 0:
for j in range(0, k, block_size):
L_tile = wp.tile_load(L_i, shape=(block_size, block_size), offset=(i, j))
L_2_tile = wp.tile_load(L_i, shape=(block_size, block_size), offset=(k, j))
L_T_tile = wp.tile_transpose(L_2_tile)
L_L_T_tile = wp.tile_matmul(L_tile, L_T_tile)
A_ik_tile -= L_L_T_tile
# Solve for the current block
t = wp.tile_transpose(A_ik_tile)
tmp = wp.tile_lower_solve(L_kk_tile, t)
sol_tile = wp.tile_transpose(tmp)
wp.tile_store(L_i, sol_tile, offset=(i, k))
# Return the kernel function
return llt_blocked_factorize_kernel
@cache
def make_llt_blocked_solve_kernel(block_size: int):
@wp.kernel
def llt_blocked_solve_kernel(
# Inputs:
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
vio: wp.array(dtype=int32),
L: wp.array(dtype=float32),
b: wp.array(dtype=float32),
# Outputs:
y: wp.array(dtype=float32),
x: wp.array(dtype=float32),
):
# Retrieve the thread index and thread-block configuration
tid, tid_block = wp.tid()
num_threads_per_block = wp.block_dim()
# Retrieve the matrix block dimensions and size
n_i = dim[tid]
L_i_start = mio[tid]
v_i_start = vio[tid]
# Retrieve a pointer to the start of the i-th matrix in A
L_i_ptr = get_float32_array_offset_ptr(L, L_i_start)
b_i_ptr = get_float32_array_offset_ptr(b, v_i_start)
y_i_ptr = get_float32_array_offset_ptr(y, v_i_start)
x_i_ptr = get_float32_array_offset_ptr(x, v_i_start)
# Create a temporary warp array pointing to the i-th matrix
L_i = wp.array(ptr=L_i_ptr, shape=(n_i, n_i), dtype=wp.float32)
b_i = wp.array(ptr=b_i_ptr, shape=(n_i, 1), dtype=wp.float32)
y_i = wp.array(ptr=y_i_ptr, shape=(n_i, 1), dtype=wp.float32)
x_i = wp.array(ptr=x_i_ptr, shape=(n_i, 1), dtype=wp.float32)
# Round up n_i to next multiple of block_size
n_i_padded = ((n_i + block_size - 1) // block_size) * block_size
# Forward substitution: solve L y = b
for i in range(0, n_i_padded, block_size):
rhs_tile = wp.tile_load(b_i, shape=(block_size, 1), offset=(i, 0))
if i > 0:
for j in range(0, i, block_size):
L_block = wp.tile_load(L_i, shape=(block_size, block_size), offset=(i, j))
y_block = wp.tile_load(y_i, shape=(block_size, 1), offset=(j, 0))
Ly_block = wp.tile_matmul(L_block, y_block)
rhs_tile -= Ly_block
L_tile = wp.tile_load(L_i, shape=(block_size, block_size), offset=(i, i))
y_tile = wp.tile_lower_solve(L_tile, rhs_tile)
wp.tile_store(y_i, y_tile, offset=(i, 0))
# Backward substitution: solve L^T x = y
for i in range(n_i_padded - block_size, -1, -block_size):
i_end = i + block_size
rhs_tile = wp.tile_load(y_i, shape=(block_size, 1), offset=(i, 0))
if i_end < n_i_padded:
for j in range(i_end, n_i_padded, block_size):
L_tile = wp.tile_load(L_i, shape=(block_size, block_size), offset=(j, i))
L_T_tile = wp.tile_transpose(L_tile)
x_tile = wp.tile_load(x_i, shape=(block_size, 1), offset=(j, 0))
L_T_x_tile = wp.tile_matmul(L_T_tile, x_tile)
rhs_tile -= L_T_x_tile
L_tile = wp.tile_load(L_i, shape=(block_size, block_size), offset=(i, i))
# The following if pads the matrix if it is not divisible by block_size
if i + block_size > n_i:
num_tile_elements = block_size * block_size
num_iterations = (num_tile_elements + num_threads_per_block - 1) // num_threads_per_block
for ii in range(num_iterations):
linear_index = tid_block + ii * num_threads_per_block
linear_index = linear_index % num_tile_elements
row = linear_index // block_size
col = linear_index % block_size
value = L_tile[row, col]
if i + row >= n_i:
value = wp.where(i + row == i + col, float32(1), float32(0))
L_tile[row, col] = value
# wp.static(make_tile_pad(block_size=block_size, dtype=float32))(L_tile, i, i, n_i, n_i, tid_block, num_threads_per_block)
L_T_tile = wp.tile_transpose(L_tile)
x_tile = wp.tile_upper_solve(L_T_tile, rhs_tile)
wp.tile_store(x_i, x_tile, offset=(i, 0))
# Return the kernel function
return llt_blocked_solve_kernel
@cache
def make_llt_blocked_solve_inplace_kernel(block_size: int):
@wp.kernel
def llt_blocked_solve_inplace_kernel(
# Inputs:
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
vio: wp.array(dtype=int32),
L: wp.array(dtype=float32),
# Outputs:
y: wp.array(dtype=float32),
x: wp.array(dtype=float32),
):
# Retrieve the thread index and thread-block configuration
tid, tid_block = wp.tid()
num_threads_per_block = wp.block_dim()
# Retrieve the matrix block dimensions and size
n_i = dim[tid]
L_i_start = mio[tid]
v_i_start = vio[tid]
# Retrieve a pointer to the start of the i-th matrix in A
L_i_ptr = get_float32_array_offset_ptr(L, L_i_start)
y_i_ptr = get_float32_array_offset_ptr(y, v_i_start)
x_i_ptr = get_float32_array_offset_ptr(x, v_i_start)
# Create a temporary warp array pointing to the i-th matrix
L_i = wp.array(ptr=L_i_ptr, shape=(n_i, n_i), dtype=wp.float32)
y_i = wp.array(ptr=y_i_ptr, shape=(n_i, 1), dtype=wp.float32)
x_i = wp.array(ptr=x_i_ptr, shape=(n_i, 1), dtype=wp.float32)
# Round up n_i to next multiple of block_size
n_i_padded = ((n_i + block_size - 1) // block_size) * block_size
# Forward substitution: solve L y = b
for i in range(0, n_i_padded, block_size):
rhs_tile = wp.tile_load(x_i, shape=(block_size, 1), offset=(i, 0))
if i > 0:
for j in range(0, i, block_size):
L_block = wp.tile_load(L_i, shape=(block_size, block_size), offset=(i, j))
y_block = wp.tile_load(y_i, shape=(block_size, 1), offset=(j, 0))
Ly_block = wp.tile_matmul(L_block, y_block)
rhs_tile -= Ly_block
L_tile = wp.tile_load(L_i, shape=(block_size, block_size), offset=(i, i))
y_tile = wp.tile_lower_solve(L_tile, rhs_tile)
wp.tile_store(y_i, y_tile, offset=(i, 0))
# Backward substitution: solve L^T x = y
for i in range(n_i_padded - block_size, -1, -block_size):
i_end = i + block_size
rhs_tile = wp.tile_load(y_i, shape=(block_size, 1), offset=(i, 0))
if i_end < n_i_padded:
for j in range(i_end, n_i_padded, block_size):
L_tile = wp.tile_load(L_i, shape=(block_size, block_size), offset=(j, i))
L_T_tile = wp.tile_transpose(L_tile)
x_tile = wp.tile_load(x_i, shape=(block_size, 1), offset=(j, 0))
L_T_x_tile = wp.tile_matmul(L_T_tile, x_tile)
rhs_tile -= L_T_x_tile
L_tile = wp.tile_load(L_i, shape=(block_size, block_size), offset=(i, i))
# The following if pads the matrix if it is not divisible by block_size
if i + block_size > n_i:
num_tile_elements = block_size * block_size
num_iterations = (num_tile_elements + num_threads_per_block - 1) // num_threads_per_block
for ii in range(num_iterations):
linear_index = tid_block + ii * num_threads_per_block
linear_index = linear_index % num_tile_elements
row = linear_index // block_size
col = linear_index % block_size
value = L_tile[row, col]
if i + row >= n_i:
value = wp.where(i + row == i + col, float32(1), float32(0))
L_tile[row, col] = value
x_tile = wp.tile_upper_solve(wp.tile_transpose(L_tile), rhs_tile)
wp.tile_store(x_i, x_tile, offset=(i, 0))
# Return the kernel function
return llt_blocked_solve_inplace_kernel
###
# Launchers
###
def llt_blocked_factorize(
kernel,
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
A: wp.array(dtype=float32),
L: wp.array(dtype=float32),
num_blocks: int = 1,
block_dim: int = 128, # TODO: Rename this to be clearer that this is the number of threads per TILE block and not matrix block
device: wp.DeviceLike = None,
):
"""
Launches the blocked Cholesky factorization kernel for a block partitioned matrix.
Args:
kernel: The kernel function to use for the blocked factorization.
num_blocks (int): The number of matrix blocks to process.
block_dim (int): The dimension of the thread block to use for the kernel launch.
dim (wp.array): An array of shape `(num_blocks,)` containing the active dimensions of each matrix block.
mio (wp.array): An array of shape `(num_blocks,)` containing the matrix index offset (mio) of each matrix block.
A (wp.array): The flat input array containing the input matrix blocks to be factorized.
L (wp.array): The flat output array containing the factorization of each matrix block.
"""
wp.launch_tiled(kernel=kernel, dim=num_blocks, inputs=[dim, mio, A, L], block_dim=block_dim, device=device)
def llt_blocked_solve(
kernel,
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
vio: wp.array(dtype=int32),
L: wp.array(dtype=float32),
b: wp.array(dtype=float32),
y: wp.array(dtype=float32),
x: wp.array(dtype=float32),
num_blocks: int = 1,
block_dim: int = 64,
device: wp.DeviceLike = None,
):
"""
Launches the blocked Cholesky solve kernel for a block partitioned matrix.
Args:
num_blocks (int): The number of matrix blocks to process.
dim (wp.array): An array of shape `(num_blocks,)` containing the dimensions of each matrix block.
mio (wp.array): An array of shape `(num_blocks,)` containing the matrix index offsets of each matrix block.
vio (wp.array): An array of shape `(num_blocks,)` containing the vector index offsets of each vector block.
L (wp.array2d): The flat input array containing the Cholesky factorization of each matrix block.
b (wp.array): The flat input array containing the stacked right-hand side vectors.
y (wp.array): The output array where the intermediate result will be stored.
x (wp.array): The output array where the solution to the linear system `A @ x = b` will be stored.
kernel: The kernel function to use for the blocked solve.
block_dim (int): The dimension of the thread block to use for the kernel launch.
"""
wp.launch_tiled(
kernel=kernel, dim=num_blocks, inputs=[dim, mio, vio, L, b, y, x], block_dim=block_dim, device=device
)
def llt_blocked_solve_inplace(
kernel,
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
vio: wp.array(dtype=int32),
L: wp.array(dtype=float32),
y: wp.array(dtype=float32),
x: wp.array(dtype=float32),
num_blocks: int = 1,
block_dim: int = 64,
device: wp.DeviceLike = None,
):
"""
Launches the blocked Cholesky in-place solve kernel for a block partitioned matrix.
Args:
num_blocks (int): The number of matrix blocks to process.
dim (wp.array): An array of shape `(num_blocks,)` containing the dimensions of each matrix block.
mio (wp.array): An array of shape `(num_blocks,)` containing the matrix index offsets of each matrix block.
vio (wp.array): An array of shape `(num_blocks,)` containing the vector index offsets of each vector block.
L (wp.array2d): The flat input array containing the Cholesky factorization of each matrix block.
x (wp.array): The input/output array where the solution to the linear system `A @ x = b` will be stored in-place.
kernel: The kernel function to use for the blocked in-place solve.
block_dim (int): The dimension of the thread block to use for the kernel launch.
"""
wp.launch_tiled(kernel=kernel, dim=num_blocks, inputs=[dim, mio, vio, L, y, x], block_dim=block_dim, device=device)
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/factorize/llt_blocked_semi_sparse.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Linear Algebra: Blocked Semi-Sparse LLT (i.e. Cholesky) factorization using Warp's Tile API."""
from functools import cache
import numpy as np
import warp as wp
###
# Module interface
###
__all__ = ["SemiSparseBlockCholeskySolverBatched"]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
def cuthill_mckee_ordering(M):
"""
Given a symmetric binary matrix M (0/1, shape n x n), returns a permutation array P
such that reordering both the rows and columns of M by P (i.e., M[P][:, P]) produces
a matrix with reduced bandwidth according to the Cuthill-McKee algorithm (a minimal fill-in heuristic).
"""
n = M.shape[0]
visited = np.zeros(n, dtype=bool)
degrees = M.sum(axis=1)
order = []
for start in np.argsort(degrees):
if not visited[start]:
queue = [start]
visited[start] = True
while queue:
node = queue.pop(0)
order.append(node)
# Find unvisited neighbors
neighbors = np.where((M[node] != 0) & (~visited))[0]
# Sort neighbors by degree (ascending)
neighbors = neighbors[np.argsort(degrees[neighbors])]
for neighbor in neighbors:
visited[neighbor] = True
queue.extend(neighbors)
# For minimal fill-in, use reverse Cuthill-McKee
P = order[::-1]
return np.array(P, dtype=int)
def compute_inverse_ordering(ordering):
"""
Computes the inverse permutation of the given ordering.
Args:
ordering (np.ndarray): The permutation array used for reordering (length n).
Returns:
inv_ordering (np.ndarray): The inverse permutation array.
"""
inv_ordering = np.empty_like(ordering)
inv_ordering[ordering] = np.arange(len(ordering))
return inv_ordering
@wp.kernel
def reorder_rows_kernel(
src: wp.array3d(dtype=float),
dst: wp.array3d(dtype=float),
ordering: wp.array(dtype=int, ndim=2),
n_rows_arr: wp.array(dtype=int, ndim=1),
n_cols_arr: wp.array(dtype=int, ndim=1),
batch_mask: wp.array(dtype=int, ndim=1),
):
batch_id, i, j = wp.tid() # 2D launch: (n_rows, n_cols)
n_rows = n_rows_arr[batch_id]
n_cols = n_cols_arr[batch_id]
if i < n_rows and j < n_cols and batch_mask[batch_id] != 0:
src_row = ordering[batch_id, i]
src_col = ordering[batch_id, j]
dst[batch_id, i, j] = src[batch_id, src_row, src_col]
@wp.kernel
def reorder_rows_kernel_col_vector(
src: wp.array3d(dtype=float),
dst: wp.array3d(dtype=float),
ordering: wp.array(dtype=int, ndim=2),
n_rows_arr: wp.array(dtype=int, ndim=1),
batch_mask: wp.array(dtype=int, ndim=1),
):
batch_id, i = wp.tid()
n_rows = n_rows_arr[batch_id]
if i < n_rows and batch_mask[batch_id] != 0:
src_row = ordering[batch_id, i]
# For column vectors (2d arrays with shape (n, 1)), just copy columns directly
dst[batch_id, i, 0] = src[batch_id, src_row, 0]
def to_binary_matrix(M):
return (M != 0).astype(int)
def sparsity_to_tiles(sparsity_matrix, tile_size):
"""
Given a 2D 0/1 sparsity matrix and a tile size,
returns a 2D 0/1 matrix indicating which tiles are nonzero.
"""
n_rows, n_cols = sparsity_matrix.shape
n_tile_rows = (n_rows + tile_size - 1) // tile_size
n_tile_cols = (n_cols + tile_size - 1) // tile_size
tile_matrix = np.zeros((n_tile_rows, n_tile_cols), dtype=int)
for i in range(n_rows):
for j in range(n_cols):
if sparsity_matrix[i, j] != 0:
tile_row = i // tile_size
tile_col = j // tile_size
tile_matrix[tile_row, tile_col] = 1
return tile_matrix
def symbolic_cholesky_dense(M, tile_size):
"""
Given a symmetric 0/1 matrix M, returns the block sparsity pattern (lower-triangular, bool)
for the Cholesky factor L of M, using a block Cholesky symbolic analysis.
The output is a 2D 0/1 matrix of shape (n_tiles, n_tiles) indicating which tiles of L are nonzero.
This implementation follows the classical symbolic block Cholesky fill-in algorithm:
For each block row i, and for each block column j < i, if M[i, j] is nonzero or
there exists a k < j such that both L[i, k] and L[j, k] are nonzero, then L[i, j] is nonzero.
"""
# Dimensions
n = M.shape[0]
n_tiles = (n + tile_size - 1) // tile_size
# Compute block sparsity pattern of M
M_tile_pattern = sparsity_to_tiles(M, tile_size)
# Initialize L_tile_pattern as strictly lower triangle of M_tile_pattern
L_tile_pattern = np.zeros((n_tiles, n_tiles), dtype=bool)
for i in range(n_tiles):
for j in range(i + 1):
L_tile_pattern[i, j] = bool(M_tile_pattern[i, j])
# Symbolic block Cholesky fill-in
for j in range(n_tiles):
for i in range(j + 1, n_tiles):
if not L_tile_pattern[i, j]:
# Check for fill-in: does there exist k < j such that L[i, k] and L[j, k] are nonzero?
for k in range(j):
if L_tile_pattern[i, k] and L_tile_pattern[j, k]:
L_tile_pattern[i, j] = True
break
# Only lower triangle is relevant for Cholesky
L_tile_pattern = np.tril(L_tile_pattern, k=0)
return L_tile_pattern.astype(np.int32)
@cache
def create_blocked_cholesky_kernel(block_size: int):
@wp.kernel
def blocked_cholesky_kernel(
A_batched: wp.array(dtype=float, ndim=3),
L_batched: wp.array(dtype=float, ndim=3),
L_tile_pattern_batched: wp.array(dtype=int, ndim=3),
active_matrix_size_arr: wp.array(dtype=int, ndim=1),
batch_mask: wp.array(dtype=int, ndim=1),
):
"""
Batched Cholesky factorization of symmetric positive definite matrices in blocks.
For each matrix A in batch, computes lower-triangular L where A = L L^T.
Args:
A_batched: Input SPD matrices (batch_size, n, n)
L_batched: Output Cholesky factors (batch_size, n, n)
L_tile_pattern_batched: Sparsity pattern for L tiles (1=nonzero, 0=zero)
active_matrix_size_arr: Size of each active matrix in batch
batch_mask: Flag for each matrix in the batch, indicating whether to process it (0 = skip)
Notes:
- Parallel processing across batch dimension
- Block size = block_size x block_size
- Uses tile patterns to skip zero blocks
- A must support block reading
"""
batch_id, tid_block = wp.tid()
num_threads_per_block = wp.block_dim()
if batch_mask[batch_id] == 0:
return
A = A_batched[batch_id]
L = L_batched[batch_id]
L_tile_pattern = L_tile_pattern_batched[batch_id]
active_matrix_size = active_matrix_size_arr[batch_id]
# Round up active_matrix_size to next multiple of block_size
n = ((active_matrix_size + block_size - 1) // block_size) * block_size
# Process the matrix in blocks along its leading dimension.
for k in range(0, n, block_size):
end = k + block_size
# Check if this diagonal tile is nonzero
tile_k = k // block_size
# Load current diagonal block A[k:end, k:end]
# and update with contributions from previously computed blocks.
A_kk_tile = wp.tile_load(A, shape=(block_size, block_size), offset=(k, k), storage="shared")
# The following pads the matrix if it is not divisible by block_size
if k + block_size > active_matrix_size:
num_tile_elements = block_size * block_size
num_iterations = (num_tile_elements + num_threads_per_block - 1) // num_threads_per_block
for i in range(num_iterations):
linear_index = tid_block + i * num_threads_per_block
linear_index = linear_index % num_tile_elements
row = linear_index // block_size
col = linear_index % block_size
value = A_kk_tile[row, col]
if k + row >= active_matrix_size or k + col >= active_matrix_size:
value = wp.where(row == col, float(1), float(0))
A_kk_tile[row, col] = value
if k > 0:
for j in range(0, k, block_size):
tile_j = j // block_size
# Only update if both L_tile_pattern[tile_k, tile_j] is nonzero
if L_tile_pattern[tile_k, tile_j] == 0:
continue
L_block = wp.tile_load(L, shape=(block_size, block_size), offset=(k, j))
L_block_T = wp.tile_transpose(L_block)
L_L_T_block = wp.tile_matmul(L_block, L_block_T)
A_kk_tile -= L_L_T_block
# Compute the Cholesky factorization for the block
L_kk_tile = wp.tile_cholesky(A_kk_tile)
wp.tile_store(L, L_kk_tile, offset=(k, k))
# Process the blocks below the current block
for i in range(end, n, block_size):
tile_i = i // block_size
# Only store result if L_tile_pattern[tile_i, tile_k] is nonzero
if L_tile_pattern[tile_i, tile_k] == 0:
continue
A_ik_tile = wp.tile_load(A, shape=(block_size, block_size), offset=(i, k), storage="shared")
# The following if pads the matrix if it is not divisible by block_size
if i + block_size > active_matrix_size or k + block_size > active_matrix_size:
num_tile_elements = block_size * block_size
num_iterations = (num_tile_elements + num_threads_per_block - 1) // num_threads_per_block
for ii in range(num_iterations):
linear_index = tid_block + ii * num_threads_per_block
linear_index = linear_index % num_tile_elements
row = linear_index // block_size
col = linear_index % block_size
value = A_ik_tile[row, col]
if i + row >= active_matrix_size or k + col >= active_matrix_size:
value = wp.where(i + row == k + col, float(1), float(0))
A_ik_tile[row, col] = value
if k > 0:
for j in range(0, k, block_size):
tile_j = j // block_size
# Only update if both L_tile_pattern[tile_i, tile_j] and L_tile_pattern[tile_k, tile_j] are nonzero
if L_tile_pattern[tile_i, tile_j] == 0 or L_tile_pattern[tile_k, tile_j] == 0:
continue
L_tile = wp.tile_load(L, shape=(block_size, block_size), offset=(i, j))
L_2_tile = wp.tile_load(L, shape=(block_size, block_size), offset=(k, j))
L_T_tile = wp.tile_transpose(L_2_tile)
L_L_T_tile = wp.tile_matmul(L_tile, L_T_tile)
A_ik_tile -= L_L_T_tile
t = wp.tile_transpose(A_ik_tile)
tmp = wp.tile_lower_solve(L_kk_tile, t)
sol_tile = wp.tile_transpose(tmp)
wp.tile_store(L, sol_tile, offset=(i, k))
return blocked_cholesky_kernel
@cache
def create_blocked_cholesky_solve_kernel(block_size: int):
@wp.kernel
def blocked_cholesky_solve_kernel(
L_batched: wp.array(dtype=float, ndim=3),
L_tile_pattern_batched: wp.array(dtype=int, ndim=3),
b_batched: wp.array(dtype=float, ndim=3),
x_batched: wp.array(dtype=float, ndim=3),
y_batched: wp.array(dtype=float, ndim=3),
active_matrix_size_arr: wp.array(dtype=int, ndim=1),
batch_mask: wp.array(dtype=int, ndim=1),
):
"""
Batched blocked Cholesky solver kernel. For each batch, solves A x = b using L L^T = A.
Uses forward/backward substitution with block size optimization.
"""
batch_id, _tid_block = wp.tid()
if batch_mask[batch_id] == 0:
return
L = L_batched[batch_id]
b = b_batched[batch_id]
x = x_batched[batch_id]
y = y_batched[batch_id]
L_tile_pattern = L_tile_pattern_batched[batch_id]
active_matrix_size = active_matrix_size_arr[batch_id]
# Round up active_matrix_size to next multiple of block_size
n = ((active_matrix_size + block_size - 1) // block_size) * block_size
# Forward substitution: solve L y = b
for i in range(0, n, block_size):
tile_i = i // block_size
# Only process if diagonal block is present
if L_tile_pattern[tile_i, tile_i] == 0:
continue
i_end = i + block_size
rhs_tile = wp.tile_load(b, shape=(block_size, 1), offset=(i, 0))
if i > 0:
for j in range(0, i, block_size):
tile_j = j // block_size
# Only process if L_tile_pattern[tile_i, tile_j] is nonzero
if L_tile_pattern[tile_i, tile_j] == 0:
continue
L_block = wp.tile_load(L, shape=(block_size, block_size), offset=(i, j))
y_block = wp.tile_load(y, shape=(block_size, 1), offset=(j, 0))
Ly_block = wp.tile_matmul(L_block, y_block)
rhs_tile -= Ly_block
L_tile = wp.tile_load(L, shape=(block_size, block_size), offset=(i, i))
y_tile = wp.tile_lower_solve(L_tile, rhs_tile)
wp.tile_store(y, y_tile, offset=(i, 0))
# Backward substitution: solve L^T x = y
for i in range(n - block_size, -1, -block_size):
tile_i = i // block_size
# Only process if diagonal block is present
if L_tile_pattern[tile_i, tile_i] == 0:
continue
i_start = i
i_end = i_start + block_size
rhs_tile = wp.tile_load(y, shape=(block_size, 1), offset=(i_start, 0))
if i_end < n:
for j in range(i_end, n, block_size):
tile_j = j // block_size
# Only process if L_tile_pattern[tile_j, tile_i] is nonzero
if L_tile_pattern[tile_j, tile_i] == 0:
continue
L_tile = wp.tile_load(L, shape=(block_size, block_size), offset=(j, i_start))
L_T_tile = wp.tile_transpose(L_tile)
x_tile = wp.tile_load(x, shape=(block_size, 1), offset=(j, 0))
L_T_x_tile = wp.tile_matmul(L_T_tile, x_tile)
rhs_tile -= L_T_x_tile
L_tile = wp.tile_load(L, shape=(block_size, block_size), offset=(i_start, i_start))
x_tile = wp.tile_upper_solve(wp.tile_transpose(L_tile), rhs_tile)
wp.tile_store(x, x_tile, offset=(i_start, 0))
return blocked_cholesky_solve_kernel
class SemiSparseBlockCholeskySolverBatched:
"""
Batched solver for linear systems using block Cholesky factorization.
"Semi-sparse" because it uses dense storage but exploits sparsity by tracking zero tiles
to skip unnecessary computation. Handles multiple systems in parallel with fixed matrix size.
"""
def __init__(self, num_batches: int, max_num_equations: int, block_size=16, device="cuda", enable_reordering=True):
self.num_batches = num_batches
self.max_num_equations = max_num_equations
self.device = device
self.num_threads_per_block_factorize = 128
self.num_threads_per_block_solve = 64
self.active_matrix_size_int = -1
self.block_size = block_size
self.cholesky_kernel = create_blocked_cholesky_kernel(block_size)
self.solve_kernel = create_blocked_cholesky_solve_kernel(block_size)
# Allocate workspace arrays for factorization and solve
# Compute padded size rounded up to next multiple of block size
self.padded_num_equations = (
(self.max_num_equations + self.block_size - 1) // self.block_size
) * self.block_size
self.A_swizzled = wp.zeros(
shape=(num_batches, self.padded_num_equations, self.padded_num_equations), dtype=float, device=self.device
)
self.L = wp.zeros(
shape=(num_batches, self.padded_num_equations, self.padded_num_equations), dtype=float, device=self.device
)
self.y = wp.zeros(
shape=(num_batches, self.padded_num_equations, 1), dtype=float, device=self.device
) # temp memory
self.result_swizzled = wp.zeros(
shape=(num_batches, self.padded_num_equations, 1), dtype=float, device=self.device
) # temp memory
self.rhs_swizzled = wp.zeros(
shape=(num_batches, self.padded_num_equations, 1), dtype=float, device=self.device
) # temp memory
self.num_tiles = (self.padded_num_equations + self.block_size - 1) // self.block_size
self.L_tile_pattern = wp.zeros(
shape=(num_batches, self.num_tiles, self.num_tiles), dtype=int, device=self.device
)
self.enable_reordering = enable_reordering
def capture_sparsity_pattern(
self,
A: np.ndarray, # 3D array (batch_size, n, n)
A_reorder_size: np.ndarray, # 1D array (batch_size)
):
"""
Captures sparsity pattern and computes fill-reducing ordering for batched matrices.
Args:
A: Input SPD matrices of shape (batch_size, n, n), as float arrays or directly as binary 0/1 matrices
indicating the sparsity pattern (float arrays will be converted to binary automatically).
A_reorder_size: Per-batch size of top-left block to reorder for sparsity
Computes Cuthill-McKee ordering on top-left block, analyzes symbolic Cholesky factorization,
and stores tile-level sparsity patterns. Tiles beyond A_reorder_size are treated as dense.
"""
batch_size = A.shape[0]
# Convert to binary
A = to_binary_matrix(A)
A = A[:, : self.max_num_equations, : self.max_num_equations]
# Initialize arrays for each batch
orderings = np.zeros((batch_size, self.max_num_equations), dtype=np.int32)
inverse_orderings = np.zeros((batch_size, self.max_num_equations), dtype=np.int32)
L_tile_patterns = np.zeros((batch_size, self.num_tiles, self.num_tiles), dtype=np.int32)
# Process each batch independently
for batch_id in range(batch_size):
# Call cuthill_mckee_ordering on the binary version of A and store both orderings
reorder_size = A_reorder_size[batch_id]
ordering = cuthill_mckee_ordering(A[batch_id, :reorder_size, :reorder_size])
# Append sequential indices for remaining rows/cols
remaining_indices = np.arange(reorder_size, self.max_num_equations)
ordering = np.concatenate([ordering, remaining_indices])
orderings[batch_id] = ordering
inverse_ordering = compute_inverse_ordering(ordering)
inverse_orderings[batch_id] = inverse_ordering
# Reorder A and then extract the sparsity patterns
if self.enable_reordering:
A_reordered = A[batch_id][ordering][:, ordering]
else:
A_reordered = A[batch_id]
L_tile_pattern_np = symbolic_cholesky_dense(A_reordered, self.block_size)
# Set all tiles after A_reorder_size to 1
reorder_tile_idx = reorder_size // self.block_size # Conservative: Round down
for i in range(reorder_tile_idx, self.num_tiles):
for j in range(min(i + 1, self.num_tiles)): # Only set lower triangular part
L_tile_pattern_np[i, j] = 1
L_tile_patterns[batch_id] = L_tile_pattern_np
# Convert to warp arrays on the correct device
self.ordering = wp.array(orderings, dtype=int, device=self.device)
self.inverse_ordering = wp.array(inverse_orderings, dtype=int, device=self.device)
self.L_tile_pattern = wp.array(L_tile_patterns, dtype=int, device=self.device)
def factorize(
self,
A: wp.array(dtype=float, ndim=3),
num_active_equations: wp.array(dtype=int, ndim=1),
batch_mask: wp.array(dtype=int, ndim=1),
):
"""
Computes the Cholesky factorization of a symmetric positive definite matrix A in blocks.
It returns a lower-triangular matrix L such that A = L L^T.
Args:
A: Input SPD matrices of shape (batch_size, n, n).
num_active_equations: Size of the top-left block to factorize for each matrix in the batch.
batch_mask: Flag for each matrix in the batch, indicating whether to process it (0 = skip)
"""
self.num_active_equations = num_active_equations
# Reorder A and store in self.A_reordered
if self.enable_reordering:
wp.launch(
reorder_rows_kernel,
dim=[self.num_batches, self.max_num_equations, self.max_num_equations],
inputs=[
A,
self.A_swizzled,
self.ordering,
num_active_equations,
num_active_equations,
batch_mask,
],
device=self.device,
)
A = self.A_swizzled
wp.launch_tiled(
self.cholesky_kernel,
dim=self.num_batches,
inputs=[A, self.L, self.L_tile_pattern, num_active_equations, batch_mask],
block_dim=self.num_threads_per_block_factorize,
device=self.device,
)
def solve(
self,
rhs: wp.array(dtype=float, ndim=3),
result: wp.array(dtype=float, ndim=3),
batch_mask: wp.array(dtype=int, ndim=1),
):
"""
Solves A x = b given the Cholesky factor L (A = L L^T) using
blocked forward and backward substitution.
Args:
rhs: Input right-hand-side matrices of shape (batch_size, n, p).
result: Output solution matrices of shape (batch_size, n, p).
batch_mask: Flag for each matrix in the batch, indicating whether to process it (0 = skip)
"""
R = result
if self.enable_reordering:
R = self.result_swizzled
wp.launch(
reorder_rows_kernel_col_vector,
dim=[self.num_batches, self.max_num_equations],
inputs=[rhs, self.rhs_swizzled, self.ordering, self.num_active_equations, batch_mask],
device=self.device,
)
rhs = self.rhs_swizzled
# Then solve the system using blocked_cholesky_solve kernel
wp.launch_tiled(
self.solve_kernel,
dim=self.num_batches,
inputs=[self.L, self.L_tile_pattern, rhs, R, self.y, self.num_active_equations, batch_mask],
block_dim=self.num_threads_per_block_solve,
device=self.device,
)
if self.enable_reordering:
# Undo reordering
wp.launch(
reorder_rows_kernel_col_vector,
dim=[self.num_batches, self.max_num_equations],
inputs=[R, result, self.inverse_ordering, self.num_active_equations, batch_mask],
device=self.device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/factorize/llt_sequential.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Linear Algebra: Sequential LLT (i.e. Cholesky) factorization w/o intra-parallelism"""
from __future__ import annotations
import warp as wp
from ...core.math import FLOAT32_EPS
from ...core.types import float32, int32
###
# Module interface
###
__all__ = [
"llt_sequential_factorize",
"llt_sequential_solve",
"llt_sequential_solve_inplace",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _llt_sequential_factorize(
# Inputs:
dim_in: wp.array(dtype=int32),
mio_in: wp.array(dtype=int32),
A_in: wp.array(dtype=float32),
# Outputs:
L_out: wp.array(dtype=float32),
):
# Retrieve the thread index
tid = wp.tid()
# Retrieve the matrix start offset and dimension
mio = mio_in[tid]
n = dim_in[tid]
# Compute the Cholesky factorization sequentially
for i in range(n):
m_i = mio + n * i
m_ii = m_i + i
A_ii = A_in[m_ii]
for j in range(i + 1):
m_j = mio + n * j
m_jj = m_j + j
m_ij = m_i + j
A_ij = A_in[m_ij]
L_jj = L_out[m_jj]
sum = float32(0.0)
for k in range(j):
m_ik = m_i + k
m_jk = m_j + k
sum += L_out[m_ik] * L_out[m_jk]
if i == j:
L_out[m_ij] = wp.sqrt(wp.max(A_ii - sum, FLOAT32_EPS))
else:
L_out[m_ij] = (A_ij - sum) / L_jj
@wp.kernel
def _llt_sequential_solve(
# Inputs:
dim_in: wp.array(dtype=int32),
mio_in: wp.array(dtype=int32),
vio_in: wp.array(dtype=int32),
L_in: wp.array(dtype=float32),
b_in: wp.array(dtype=float32),
# Outputs:
y_out: wp.array(dtype=float32),
x_out: wp.array(dtype=float32),
):
# Retrieve the thread index
tid = wp.tid()
# Retrieve the start offsets and problem dimension
mio = mio_in[tid]
vio = vio_in[tid]
n = dim_in[tid]
# Forward substitution to solve L * y = b
for i in range(n):
m_i = mio + n * i
m_ii = m_i + i
L_ii = L_in[m_ii]
sum_i = b_in[vio + i]
for j in range(i):
m_ij = m_i + j
sum_i -= L_in[m_ij] * y_out[vio + j]
y_out[vio + i] = sum_i / L_ii
# Backward substitution to solve L^T * x = y
for i in range(n - 1, -1, -1):
m_i = mio + n * i
m_ii = m_i + i
LT_ii = L_in[m_ii]
sum_i = y_out[vio + i]
for j in range(i + 1, n):
m_ji = mio + n * j + i
sum_i -= L_in[m_ji] * x_out[vio + j]
x_out[vio + i] = sum_i / LT_ii
@wp.kernel
def _llt_sequential_solve_inplace(
# Inputs:
dim_in: wp.array(dtype=int32),
mio_in: wp.array(dtype=int32),
vio_in: wp.array(dtype=int32),
L_in: wp.array(dtype=float32),
x_inout: wp.array(dtype=float32),
):
# Retrieve the thread index
tid = wp.tid()
# Retrieve the start offsets and problem dimension
mio = mio_in[tid]
vio = vio_in[tid]
n = dim_in[tid]
# Forward substitution to solve L * y = b
for i in range(n):
m_i = mio + n * i
m_ii = m_i + i
L_ii = L_in[m_ii]
sum_i = x_inout[vio + i]
for j in range(i):
m_ij = m_i + j
sum_i -= L_in[m_ij] * x_inout[vio + j]
x_inout[vio + i] = sum_i / L_ii
# Backward substitution to solve L^T * x = y
for i in range(n - 1, -1, -1):
m_i = mio + n * i
m_ii = m_i + i
LT_ii = L_in[m_ii]
sum_i = x_inout[vio + i]
for j in range(i + 1, n):
m_ji = mio + n * j + i
sum_i -= L_in[m_ji] * x_inout[vio + j]
x_inout[vio + i] = sum_i / LT_ii
###
# Launchers
###
def llt_sequential_factorize(
num_blocks: int,
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
A: wp.array(dtype=float32),
L: wp.array(dtype=float32),
device: wp.DeviceLike = None,
):
"""
Launches the sequential Cholesky factorization kernel for a block partitioned matrix.
Args:
num_blocks (int): The number of matrix blocks to process.
dim (wp.array): An array of shape `(num_blocks,)` containing the dimensions of each matrix block.
mio (wp.array): An array of shape `(num_blocks,)` containing the start indices of each matrix block.
A (wp.array): The flat input array containing the input matrix blocks to be factorized.
L (wp.array): The flat output array containing the Cholesky factorization of each matrix block.
"""
wp.launch(
kernel=_llt_sequential_factorize,
dim=num_blocks,
inputs=[dim, mio, A, L],
device=device,
)
def llt_sequential_solve(
num_blocks: int,
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
vio: wp.array(dtype=int32),
L: wp.array(dtype=float32),
b: wp.array(dtype=float32),
y: wp.array(dtype=float32),
x: wp.array(dtype=float32),
device: wp.DeviceLike = None,
):
"""
Launches the sequential solve kernel using the Cholesky factorization of a block partitioned matrix.
Args:
num_blocks (int): The number of matrix blocks to process.
dim (wp.array): An array of shape `(num_blocks,)` containing the dimensions of each matrix block.
mio (wp.array): An array of shape `(num_blocks,)` containing the start indices of each matrix block.
vio (wp.array): An array of shape `(num_blocks,)` containing the start indices of each vector block.
L (wp.array): The flat input array containing the Cholesky factorization of each matrix block.
b (wp.array): The flat input array containing the stacked right-hand side vectors.
y (wp.array): The output array where the intermediate result will be stored.
x (wp.array): The output array where the solution to the linear system `A @ x = b` will be stored.
"""
wp.launch(
kernel=_llt_sequential_solve,
dim=num_blocks,
inputs=[dim, mio, vio, L, b, y, x],
device=device,
)
def llt_sequential_solve_inplace(
num_blocks: int,
dim: wp.array(dtype=int32),
mio: wp.array(dtype=int32),
vio: wp.array(dtype=int32),
L: wp.array(dtype=float32),
x: wp.array(dtype=float32),
device: wp.DeviceLike = None,
):
"""
Launches the sequential in-place solve kernel using the Cholesky factorization of a block partitioned matrix.
Args:
num_blocks (int): The number of matrix blocks to process.
dim (wp.array): An array of shape `(num_blocks,)` containing the dimensions of each matrix block.
mio (wp.array): An array of shape `(num_blocks,)` containing the start indices of each matrix block.
vio (wp.array): An array of shape `(num_blocks,)` containing the start indices of each vector block.
L (wp.array): The flat input array containing the Cholesky factorization of each matrix block.
x (wp.array): The array where the solution to the linear system `A @ x = b` will be stored in-place.
"""
wp.launch(
kernel=_llt_sequential_solve_inplace,
dim=num_blocks,
inputs=[dim, mio, vio, L, x],
device=device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/linear.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Linear system solvers for multiple independent linear systems.
This module provides interfaces for and implementations of linear
system solvers, that can solve multiple independent linear systems
in parallel, with support for both rectangular and square systems.
Depending on the particular solver implementation, both inter- and
intra-system parallelism may be exploited.
"""
from abc import ABC, abstractmethod
from typing import Any
import warp as wp
from ..core.types import FloatType, float32, override
from . import conjugate, factorize
from .core import DenseLinearOperatorData, DenseSquareMultiLinearInfo, make_dtype_tolerance
from .sparse_matrix import (
BlockSparseMatrices,
allocate_block_sparse_from_dense,
dense_to_block_sparse_copy_values,
)
from .sparse_operator import BlockSparseLinearOperators
###
# Module interface
###
__all__ = [
"ConjugateGradientSolver",
"ConjugateResidualSolver",
"DirectSolver",
"LLTBlockedSolver",
"LLTSequentialSolver",
"LinearSolver",
"LinearSolverType",
]
###
# Interfaces
###
class LinearSolver(ABC):
"""
An abstract base class for linear system solvers.
"""
def __init__(
self,
operator: DenseLinearOperatorData | None = None,
atol: float | None = None,
rtol: float | None = None,
dtype: FloatType = float32,
device: wp.DeviceLike | None = None,
**kwargs: dict[str, Any],
):
# Declare and initialize the internal reference to the matrix/operator data
self._operator: DenseLinearOperatorData | None = operator
# Override dtype if linear operator is provided
if operator is not None:
dtype = operator.info.dtype
# Declare and initialize internal meta-data
self._dtype: Any = dtype
self._atol: float = atol
self._rtol: float = rtol
# Declare and initialize the device identifier
self._device: wp.DeviceLike = device
# If an operator is provided, proceed with any necessary memory allocations
if operator is not None:
self.finalize(operator, **kwargs)
###
# Properties
###
@property
def operator(self) -> DenseLinearOperatorData:
if self._operator is None:
raise ValueError("No linear operator has been allocated!")
return self._operator
@property
def dtype(self) -> FloatType:
return self._dtype
@property
def device(self) -> wp.DeviceLike:
return self._device
###
# Internals
###
def _set_tolerance_dtype(self):
self._atol = make_dtype_tolerance(self._atol, dtype=self._dtype)
self._rtol = make_dtype_tolerance(self._rtol, dtype=self._dtype)
###
# Implementation API
###
@abstractmethod
def _allocate_impl(self, operator: DenseLinearOperatorData, **kwargs: dict[str, Any]) -> None:
raise NotImplementedError("An allocation operation is not implemented.")
@abstractmethod
def _reset_impl(self, A: wp.array, **kwargs: dict[str, Any]) -> None:
raise NotImplementedError("A reset operation is not implemented.")
@abstractmethod
def _compute_impl(self, A: wp.array, **kwargs: dict[str, Any]) -> None:
raise NotImplementedError("A compute operation is not implemented.")
@abstractmethod
def _solve_impl(self, b: wp.array, x: wp.array, **kwargs: dict[str, Any]) -> None:
raise NotImplementedError("A solve operation is not implemented.")
@abstractmethod
def _solve_inplace_impl(self, x: wp.array, **kwargs: dict[str, Any]) -> None:
raise NotImplementedError("A solve-in-place operation is not implemented.")
###
# Public API
###
def finalize(self, operator: DenseLinearOperatorData, **kwargs: dict[str, Any]) -> None:
"""
Ingest a linear operator and allocate any necessary internal memory
based on the multi-linear layout specified by the operator's info.
"""
# Check the operator is valid
if operator is None:
raise ValueError("A valid linear operator must be provided!")
if not isinstance(operator, DenseLinearOperatorData):
raise ValueError("The provided operator is not a DenseLinearOperatorData instance!")
if operator.info is None:
raise ValueError("The provided operator does not have any associated info!")
self._operator = operator
self._dtype = operator.info.dtype
self._set_tolerance_dtype()
self._allocate_impl(operator, **kwargs)
def reset(self) -> None:
"""Resets the internal solver data (e.g. possibly to zeros)."""
self._reset_impl()
def compute(self, A: wp.array, **kwargs: dict[str, Any]) -> None:
"""Ingest matrix data and pre-compute any rhs-independent intermediate data."""
if not self._operator.info.is_matrix_compatible(A):
raise ValueError("The provided flat matrix data array does not have enough memory!")
self._compute_impl(A=A, **kwargs)
def solve(self, b: wp.array, x: wp.array, **kwargs: dict[str, Any]) -> None:
"""Solves the multi-linear systems `A @ x = b`."""
if not self._operator.info.is_rhs_compatible(b):
raise ValueError("The provided flat rhs vector data array does not have enough memory!")
if not self._operator.info.is_input_compatible(x):
raise ValueError("The provided flat input vector data array does not have enough memory!")
self._solve_impl(b=b, x=x, **kwargs)
def solve_inplace(self, x: wp.array, **kwargs: dict[str, Any]) -> None:
"""Solves the multi-linear systems `A @ x = b` in-place, where `x` is initialized with rhs data."""
if not self._operator.info.is_input_compatible(x):
raise ValueError("The provided flat input vector data array does not have enough memory!")
self._solve_inplace_impl(x=x, **kwargs)
class DirectSolver(LinearSolver):
"""
An abstract base class for direct linear system solvers based on matrix factorization.
"""
def __init__(
self,
operator: DenseLinearOperatorData | None = None,
atol: float | None = None,
rtol: float | None = None,
ftol: float | None = None,
dtype: FloatType = float32,
device: wp.DeviceLike | None = None,
**kwargs: dict[str, Any],
):
# Default factorization tolerance to machine epsilon if not provided
ftol = make_dtype_tolerance(ftol, dtype=dtype)
# Initialize internal meta-data
self._ftol: float | None = ftol
self._has_factors: bool = False
# Initialize base class members
super().__init__(
operator=operator,
atol=atol,
rtol=rtol,
dtype=dtype,
device=device,
**kwargs,
)
###
# Internals
###
def _check_has_factorization(self):
"""Checks if the factorization has been computed, otherwise raises error."""
if not self._has_factors:
raise ValueError("A factorization has not been computed!")
###
# Implementation API
###
@abstractmethod
def _factorize_impl(self, A: wp.array, **kwargs: dict[str, Any]) -> None:
raise NotImplementedError("A matrix factorization implementation is not provided.")
@abstractmethod
def _reconstruct_impl(self, A: wp.array, **kwargs: dict[str, Any]) -> None:
raise NotImplementedError("A matrix reconstruction implementation is not provided.")
###
# Internals
###
@override
def _compute_impl(self, A: wp.array, **kwargs: dict[str, Any]):
self._factorize(A, **kwargs)
def _factorize(self, A: wp.array, ftol: float | None = None, **kwargs: dict[str, Any]) -> None:
# Override the current tolerance if provided otherwise ensure
# it does not exceed machine precision for the current dtype
if ftol is not None:
self._ftol = make_dtype_tolerance(ftol, dtype=self._dtype)
else:
self._ftol = make_dtype_tolerance(self._ftol, dtype=self._dtype)
# Factorize the specified matrix data and store any intermediate data
self._factorize_impl(A, **kwargs)
self._has_factors = True
###
# Public API
###
def reconstruct(self, A: wp.array, **kwargs: dict[str, Any]) -> None:
"""Reconstructs the original matrix from the current factorization."""
self._check_has_factorization()
self._reconstruct_impl(A, **kwargs)
class IterativeSolver(LinearSolver):
"""
An abstract base class for iterative linear system solvers.
"""
def __init__(
self,
operator: conjugate.BatchedLinearOperator | DenseLinearOperatorData | BlockSparseLinearOperators | None = None,
atol: float | wp.array | None = None,
rtol: float | wp.array | None = None,
dtype: FloatType = float32,
device: wp.DeviceLike | None = None,
maxiter: int | wp.array | None = None,
world_active: wp.array | None = None,
preconditioner: Any = None,
**kwargs: dict[str, Any],
):
self._maxiter: int | wp.array | None = maxiter
self._preconditioner: Any = preconditioner
self._world_active: wp.array | None = world_active
self.atol: float | wp.array | None = atol
self.rtol: float | wp.array | None = rtol
self._num_worlds: int | None = None
self._max_dim: int | None = None
self._batched_operator: conjugate.BatchedLinearOperator | None = None
self._use_graph_conditionals: bool = kwargs.pop("use_graph_conditionals", True)
# Sparse discovery settings (via kwargs)
self._discover_sparse: bool = kwargs.pop("discover_sparse", False)
self._sparse_block_size: int = kwargs.pop("sparse_block_size", 4)
self._sparse_threshold: float = kwargs.pop("sparse_threshold", 0.5)
self._sparse_bsm: BlockSparseMatrices | None = None
self._sparse_operator: conjugate.BatchedLinearOperator | None = None
self._sparse_solver: Any = None # Set by concrete classes
super().__init__(
operator=operator,
atol=atol,
rtol=rtol,
dtype=dtype,
device=device,
**kwargs,
)
def _to_batched_operator(
self, operator: conjugate.BatchedLinearOperator | DenseLinearOperatorData | BlockSparseLinearOperators
) -> conjugate.BatchedLinearOperator:
"""Convert various operator types to BatchedLinearOperator."""
if isinstance(operator, conjugate.BatchedLinearOperator):
return operator
elif isinstance(operator, DenseLinearOperatorData):
return conjugate.BatchedLinearOperator.from_dense(operator)
elif isinstance(operator, BlockSparseLinearOperators):
# For sparse, need uniform dimensions
return conjugate.BatchedLinearOperator.from_block_sparse_operator(operator)
else:
raise ValueError(f"Unsupported operator type: {type(operator)}")
@override
def finalize(
self,
operator: conjugate.BatchedLinearOperator | DenseLinearOperatorData | BlockSparseLinearOperators,
maxiter: int | wp.array | None = None,
world_active: wp.array | None = None,
preconditioner: Any = None,
**kwargs: dict[str, Any],
) -> None:
"""
Ingest a linear operator and allocate any necessary internal memory.
Accepts BatchedLinearOperator, DenseLinearOperatorData, or BlockSparseMatrices.
"""
if operator is None:
raise ValueError("A valid linear operator must be provided!")
# Update sparse settings from kwargs if provided
if "discover_sparse" in kwargs:
self._discover_sparse = kwargs.pop("discover_sparse")
if "sparse_block_size" in kwargs:
self._sparse_block_size = kwargs.pop("sparse_block_size")
if "sparse_threshold" in kwargs:
self._sparse_threshold = kwargs.pop("sparse_threshold")
self._batched_operator = self._to_batched_operator(operator)
if isinstance(operator, DenseLinearOperatorData):
self._operator = operator
self._dtype = operator.info.dtype
else:
self._operator = None
self._dtype = self._batched_operator.dtype
if maxiter is not None:
self._maxiter = maxiter
if world_active is not None:
self._world_active = world_active
if preconditioner is not None:
self._preconditioner = preconditioner
self._num_worlds = self._batched_operator.n_worlds
self._max_dim = self._batched_operator.max_dim
self._solve_iterations: wp.array | None = None
self._solve_residual_norm: wp.array | None = None
with wp.ScopedDevice(self._device):
if self._world_active is None:
self._world_active = wp.full(self._num_worlds, 1, dtype=wp.int32)
elif not isinstance(self._world_active, wp.array):
raise ValueError("The provided world_active is not a valid wp.array!")
if self._maxiter is None:
self._maxiter = wp.full(self._num_worlds, self._max_dim, dtype=wp.int32)
elif isinstance(self._maxiter, int):
self._maxiter = wp.full(self._num_worlds, self._maxiter, dtype=wp.int32)
elif not isinstance(self._maxiter, wp.array):
raise ValueError("The provided maxiter is not a valid wp.array or int!")
# Allocate block-sparse matrix if sparse discovery is enabled
if self._discover_sparse:
if not isinstance(operator, DenseLinearOperatorData):
raise ValueError("discover_sparse requires a DenseLinearOperatorData operator.")
self._sparse_bsm = allocate_block_sparse_from_dense(
dense_op=operator,
block_size=self._sparse_block_size,
sparsity_threshold=self._sparse_threshold,
device=self._device,
)
# Create sparse operator (will be populated at compute time)
self._sparse_operator = conjugate.BatchedLinearOperator.from_block_sparse(
self._sparse_bsm, self._batched_operator.active_dims
)
self._allocate_impl(operator, **kwargs)
@override
def solve(self, b: wp.array, x: wp.array, zero_x: bool = False, **kwargs: dict[str, Any]) -> None:
"""Solves the multi-linear systems `A @ x = b`."""
if self._operator is not None:
if not self._operator.info.is_rhs_compatible(b):
raise ValueError("The provided flat rhs vector data array does not have enough memory!")
if not self._operator.info.is_input_compatible(x):
raise ValueError("The provided flat input vector data array does not have enough memory!")
if zero_x:
x.zero_()
self._solve_impl(b=b, x=x, **kwargs)
def get_solve_metadata(self) -> dict[str, Any]:
return {"iterations": self._solve_iterations, "residual_norm": self._solve_residual_norm}
def _update_sparse_bsm(self) -> None:
"""Updates the block-sparse matrix from the dense operator. Called during compute()."""
if self._discover_sparse and self._sparse_bsm is not None and self._operator is not None:
dense_to_block_sparse_copy_values(
dense_op=self._operator,
bsm=self._sparse_bsm,
block_size=self._sparse_block_size,
)
###
# Direct solvers
###
class LLTSequentialSolver(DirectSolver):
"""
A LLT (i.e. Cholesky) factorization class computing each matrix block sequentially.\n
This parallelizes the factorization and solve operations over each block\n
and supports heterogeneous matrix block sizes.\n
"""
def __init__(
self,
operator: DenseLinearOperatorData | None = None,
atol: float | None = None,
rtol: float | None = None,
ftol: float | None = None,
dtype: FloatType = float32,
device: wp.DeviceLike | None = None,
**kwargs: dict[str, Any],
):
# Declare LLT-specific internal data
self._L: wp.array | None = None
"""A flat array containing the Cholesky factorization of each matrix block."""
self._y: wp.array | None = None
"""A flat array containing the intermediate results for the solve operation."""
# Initialize base class members
super().__init__(
operator=operator,
atol=atol,
rtol=rtol,
ftol=ftol,
dtype=dtype,
device=device,
**kwargs,
)
###
# Properties
###
@property
def L(self) -> wp.array:
if self._L is None:
raise ValueError("The factorization array has not been allocated!")
return self._L
@property
def y(self) -> wp.array:
if self._y is None:
raise ValueError("The intermediate result array has not been allocated!")
return self._y
###
# Implementation
###
@override
def _allocate_impl(self, A: DenseLinearOperatorData, **kwargs: dict[str, Any]) -> None:
# Check the operator has info
if A.info is None:
raise ValueError("The provided operator does not have any associated info!")
# Ensure that the underlying operator is compatible with LLT
if not isinstance(A.info, DenseSquareMultiLinearInfo):
raise ValueError("LLT factorization requires a square matrix.")
# Allocate the Cholesky factorization matrix and the
# intermediate result buffer on the specified device
with wp.ScopedDevice(self._device):
self._L = wp.zeros(shape=(self._operator.info.total_mat_size,), dtype=self._dtype)
self._y = wp.zeros(shape=(self._operator.info.total_vec_size,), dtype=self._dtype)
@override
def _reset_impl(self) -> None:
self._L.zero_()
self._y.zero_()
self._has_factors = False
@override
def _factorize_impl(self, A: wp.array) -> None:
factorize.llt_sequential_factorize(
num_blocks=self._operator.info.num_blocks,
dim=self._operator.info.dim,
mio=self._operator.info.mio,
A=A,
L=self._L,
device=self._device,
)
@override
def _reconstruct_impl(self, A: wp.array) -> None:
raise NotImplementedError("LLT matrix reconstruction is not yet implemented.")
@override
def _solve_impl(self, b: wp.array, x: wp.array) -> None:
# Solve the system L * y = b and L^T * x = y
factorize.llt_sequential_solve(
num_blocks=self._operator.info.num_blocks,
dim=self._operator.info.dim,
mio=self._operator.info.mio,
vio=self._operator.info.vio,
L=self._L,
b=b,
y=self._y,
x=x,
device=self._device,
)
@override
def _solve_inplace_impl(self, x: wp.array) -> None:
# Solve the system L * y = x and L^T * x = y
factorize.llt_sequential_solve_inplace(
num_blocks=self._operator.info.num_blocks,
dim=self._operator.info.dim,
mio=self._operator.info.mio,
vio=self._operator.info.vio,
L=self._L,
x=x,
)
class LLTBlockedSolver(DirectSolver):
"""
A Blocked LLT (i.e. Cholesky) factorization class computing each matrix block with Tile-based parallelism.\n
"""
def __init__(
self,
operator: DenseLinearOperatorData | None = None,
block_size: int = 16,
solve_block_dim: int = 64,
factortize_block_dim: int = 128,
atol: float | None = None,
rtol: float | None = None,
ftol: float | None = None,
dtype: FloatType = float32,
device: wp.DeviceLike | None = None,
**kwargs: dict[str, Any],
):
# Declare LLT-specific internal data
self._L: wp.array | None = None
"""A flat array containing the Cholesky factorization of each matrix block."""
self._y: wp.array | None = None
"""A flat array containing the intermediate results for the solve operation."""
# Cache the fixed block size
self._block_size: int = block_size
self._solve_block_dim: int = solve_block_dim
self._factortize_block_dim: int = factortize_block_dim
# Create the factorization and solve kernels
self._factorize_kernel = factorize.make_llt_blocked_factorize_kernel(block_size)
self._solve_kernel = factorize.make_llt_blocked_solve_kernel(block_size)
self._solve_inplace_kernel = factorize.make_llt_blocked_solve_inplace_kernel(block_size)
# Initialize base class members
super().__init__(
operator=operator,
atol=atol,
rtol=rtol,
ftol=ftol,
dtype=dtype,
device=device,
**kwargs,
)
###
# Properties
###
@property
def L(self) -> wp.array:
if self._L is None:
raise ValueError("The factorization array has not been allocated!")
return self._L
@property
def y(self) -> wp.array:
if self._y is None:
raise ValueError("The intermediate result array has not been allocated!")
return self._y
###
# Implementation
###
@override
def _allocate_impl(self, A: DenseLinearOperatorData, **kwargs: dict[str, Any]) -> None:
# Check the operator has info
if A.info is None:
raise ValueError("The provided operator does not have any associated info!")
# Ensure that the underlying operator is compatible with LLT
if not isinstance(A.info, DenseSquareMultiLinearInfo):
raise ValueError("LLT factorization requires a square matrix.")
# Allocate the Cholesky factorization matrix and the
# intermediate result buffer on the specified device
with wp.ScopedDevice(self._device):
self._L = wp.zeros(shape=(self._operator.info.total_mat_size,), dtype=self._dtype)
self._y = wp.zeros(shape=(self._operator.info.total_vec_size,), dtype=self._dtype)
@override
def _reset_impl(self) -> None:
self._L.zero_()
self._y.zero_()
self._has_factors = False
@override
def _factorize_impl(self, A: wp.array) -> None:
factorize.llt_blocked_factorize(
kernel=self._factorize_kernel,
num_blocks=self._operator.info.num_blocks,
block_dim=self._factortize_block_dim,
dim=self._operator.info.dim,
mio=self._operator.info.mio,
A=A,
L=self._L,
)
@override
def _reconstruct_impl(self, A: wp.array) -> None:
raise NotImplementedError("LLT matrix reconstruction is not yet implemented.")
@override
def _solve_impl(self, b: wp.array, x: wp.array) -> None:
# Solve the system L * y = b and L^T * x = y
factorize.llt_blocked_solve(
kernel=self._solve_kernel,
num_blocks=self._operator.info.num_blocks,
block_dim=self._solve_block_dim,
dim=self._operator.info.dim,
mio=self._operator.info.mio,
vio=self._operator.info.vio,
L=self._L,
b=b,
y=self._y,
x=x,
device=self._device,
)
@override
def _solve_inplace_impl(self, x: wp.array) -> None:
# Solve the system L * y = b and L^T * x = y
factorize.llt_blocked_solve_inplace(
kernel=self._solve_inplace_kernel,
num_blocks=self._operator.info.num_blocks,
block_dim=self._solve_block_dim,
dim=self._operator.info.dim,
mio=self._operator.info.mio,
vio=self._operator.info.vio,
L=self._L,
y=self._y,
x=x,
device=self._device,
)
###
# Iterative solvers
###
class ConjugateGradientSolver(IterativeSolver):
"""
A wrapper around the batched Conjugate Gradient implementation in `conjugate.cg`.
This solves multiple independent SPD systems using a batched operator.
"""
def __init__(
self,
**kwargs: dict[str, Any],
):
self._Mi = None
self._jacobi_preconditioner = None
self.solver = None
super().__init__(**kwargs)
@override
def _allocate_impl(self, operator, **kwargs: dict[str, Any]) -> None:
# Validate square operator for dense case
if isinstance(operator, DenseLinearOperatorData):
if not isinstance(operator.info, DenseSquareMultiLinearInfo):
raise ValueError("ConjugateGradientSolver requires a square matrix operator.")
dim_values = set(operator.info.maxdim.numpy().tolist())
if len(dim_values) > 1:
raise ValueError(
f"ConjugateGradientSolver requires all blocks to have the same dimension ({dim_values})."
)
if self._preconditioner == "jacobi":
self._jacobi_preconditioner = wp.zeros(
shape=(self._num_worlds, self._max_dim), dtype=self._dtype, device=self._device
)
self._Mi = conjugate.BatchedLinearOperator.from_diagonal(
self._jacobi_preconditioner, self._batched_operator.active_dims
)
elif self._preconditioner is not None:
raise ValueError(f"Unsupported preconditioner: {self._preconditioner}.")
else:
self._Mi = None
self.solver = conjugate.CGSolver(
A=self._batched_operator,
world_active=self._world_active,
atol=self.atol,
rtol=self.rtol,
maxiter=self._maxiter,
Mi=self._Mi,
callback=None,
use_cuda_graph=True,
use_graph_conditionals=self._use_graph_conditionals,
)
if self._discover_sparse and self._sparse_operator is not None:
self._sparse_solver = conjugate.CGSolver(
A=self._sparse_operator,
world_active=self._world_active,
atol=self.atol,
rtol=self.rtol,
maxiter=self._maxiter,
Mi=self._Mi,
callback=None,
use_cuda_graph=True,
use_graph_conditionals=self._use_graph_conditionals,
)
@override
def _reset_impl(self, A: wp.array | None = None, **kwargs: dict[str, Any]) -> None:
if self._jacobi_preconditioner is not None:
self._jacobi_preconditioner.zero_()
self._solve_iterations: wp.array = None
self._solve_residual_norm: wp.array = None
@override
def _compute_impl(self, A: wp.array, **kwargs: dict[str, Any]) -> None:
if self._operator is not None and A.ptr != self._operator.mat.ptr:
raise ValueError(f"{self.__class__.__name__} cannot be re-used with a different matrix.")
if self._Mi is not None:
self._update_preconditioner()
self._update_sparse_bsm()
@override
def _solve_inplace_impl(self, x: wp.array, **kwargs: dict[str, Any]) -> None:
self._solve_impl(x, x, **kwargs)
@override
def _solve_impl(self, b: wp.array, x: wp.array, **kwargs: dict[str, Any]) -> None:
solver = self._sparse_solver or self.solver
if solver is None:
raise ValueError("ConjugateGradientSolver.allocate() must be called before solve().")
self._solve_iterations, self._solve_residual_norm, _ = solver.solve(
b=b.reshape((self._num_worlds, self._max_dim)),
x=x.reshape((self._num_worlds, self._max_dim)),
)
def _update_preconditioner(self):
if self._operator is None:
raise ValueError("Jacobi preconditioner requires a DenseLinearOperatorData operator.")
wp.launch(
conjugate.make_jacobi_preconditioner,
dim=(self._num_worlds, self._max_dim),
inputs=[
self._operator.mat.reshape((self._num_worlds, self._max_dim * self._max_dim)),
self._batched_operator.active_dims,
],
outputs=[self._jacobi_preconditioner],
device=self._device,
)
class ConjugateResidualSolver(IterativeSolver):
"""
A wrapper around the batched Conjugate Residual implementation in `conjugate.cr`.
This solves multiple independent SPD systems using a batched operator.
"""
def __init__(
self,
**kwargs: dict[str, Any],
):
self._Mi = None
self._jacobi_preconditioner = None
self.solver = None
super().__init__(**kwargs)
@override
def _allocate_impl(self, operator, **kwargs: dict[str, Any]) -> None:
if isinstance(operator, DenseLinearOperatorData):
if not isinstance(operator.info, DenseSquareMultiLinearInfo):
raise ValueError("ConjugateResidualSolver requires a square matrix operator.")
dim_values = set(operator.info.maxdim.numpy().tolist())
if len(dim_values) > 1:
raise ValueError(
f"ConjugateResidualSolver requires all blocks to have the same dimension ({dim_values})."
)
if self._preconditioner == "jacobi":
self._jacobi_preconditioner = wp.zeros(
shape=(self._num_worlds, self._max_dim), dtype=self._dtype, device=self._device
)
self._Mi = conjugate.BatchedLinearOperator.from_diagonal(
self._jacobi_preconditioner, self._batched_operator.active_dims
)
elif self._preconditioner is not None:
raise ValueError(f"Unsupported preconditioner: {self._preconditioner}.")
else:
self._Mi = None
self.solver = conjugate.CRSolver(
A=self._batched_operator,
world_active=self._world_active,
atol=self.atol,
rtol=self.rtol,
maxiter=self._maxiter,
Mi=self._Mi,
callback=None,
use_cuda_graph=True,
use_graph_conditionals=self._use_graph_conditionals,
)
if self._discover_sparse and self._sparse_operator is not None:
self._sparse_solver = conjugate.CRSolver(
A=self._sparse_operator,
world_active=self._world_active,
atol=self.atol,
rtol=self.rtol,
maxiter=self._maxiter,
Mi=self._Mi,
callback=None,
use_cuda_graph=True,
use_graph_conditionals=self._use_graph_conditionals,
)
@override
def _reset_impl(self, A: wp.array | None = None, **kwargs: dict[str, Any]) -> None:
if self._jacobi_preconditioner is not None:
self._jacobi_preconditioner.zero_()
self._solve_iterations: wp.array = None
self._solve_residual_norm: wp.array = None
@override
def _compute_impl(self, A: wp.array, **kwargs: dict[str, Any]) -> None:
if self._operator is not None and A.ptr != self._operator.mat.ptr:
raise ValueError(f"{self.__class__.__name__} cannot be re-used with a different matrix.")
if self._Mi is not None:
self._update_preconditioner()
self._update_sparse_bsm()
@override
def _solve_inplace_impl(self, x: wp.array, **kwargs: dict[str, Any]) -> None:
self._solve_impl(x, x)
@override
def _solve_impl(self, b: wp.array, x: wp.array, **kwargs: dict[str, Any]) -> None:
solver = self._sparse_solver or self.solver
if solver is None:
raise ValueError("ConjugateResidualSolver.allocate() must be called before solve().")
self._solve_iterations, self._solve_residual_norm, _ = solver.solve(
b=b.reshape((self._num_worlds, self._max_dim)),
x=x.reshape((self._num_worlds, self._max_dim)),
)
def _update_preconditioner(self):
if self._operator is None:
raise ValueError("Jacobi preconditioner requires a DenseLinearOperatorData operator.")
wp.launch(
conjugate.make_jacobi_preconditioner,
dim=(self._num_worlds, self._max_dim),
inputs=[
self._operator.mat.reshape((self._num_worlds, self._max_dim * self._max_dim)),
self._batched_operator.active_dims,
],
outputs=[self._jacobi_preconditioner],
device=self._device,
)
###
# Summary
###
LinearSolverType = LLTSequentialSolver | LLTBlockedSolver | ConjugateGradientSolver | ConjugateResidualSolver
"""Type alias over all linear solvers."""
LinearSolverTypeToName = {
LLTSequentialSolver: "LLTS",
LLTBlockedSolver: "LLTB",
ConjugateGradientSolver: "CG",
ConjugateResidualSolver: "CR",
}
LinearSolverNameToType = {
"LLTS": LLTSequentialSolver,
"LLTB": LLTBlockedSolver,
"CG": ConjugateGradientSolver,
"CR": ConjugateResidualSolver,
}
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/sparse_matrix.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Linear Algebra: Core types and utilities for sparse multi-world linear systems
This module provides data structures and utilities for managing multiple
independent linear systems, including rectangular and square systems.
"""
import functools
from dataclasses import dataclass
from typing import TYPE_CHECKING, Any, get_args
import numpy as np
import warp as wp
from warp.types import type_size_in_bytes
from ..core.types import FloatType, IntType, float32, int32
from .core import DenseSquareMultiLinearInfo
if TYPE_CHECKING:
from .core import DenseLinearOperatorData
###
# Module interface
###
__all__ = [
"BlockDType",
"BlockSparseMatrices",
"allocate_block_sparse_from_dense",
"dense_to_block_sparse_copy_values",
]
###
# Types
###
class BlockDType:
"""A utility type for bundling meta-data about sparse-block types."""
def __init__(self, dtype: FloatType | IntType, shape: int | tuple[int] | tuple[int, int] | None = None):
"""
Constructs a new BlockDType descriptor given scalar Warp data-type and the block shape.
Args:
dtype (FloatType | IntType):
The underlying scalar Warp data-type of each sparse block.
shape (int | tuple[int] | tuple[int, int], optional):
The shape of each sparse block as an integer (for vectors) or a tuple of integers (for matrices).\n
If not provided, defaults to scalar blocks.
Raises:
TypeError:
If the `dtype` field is not a valid FloatType or IntType.
ValueError:
If the `shape` field is not a positive integer or a tuple of one or two positive integers.
"""
# Ensure the underlying scalar dtype is valid
if (dtype not in get_args(FloatType)) and (dtype not in get_args(IntType)):
raise TypeError("The `dtype` field must be a valid FloatType or IntType such as float32 or int32.")
# If no shape is provided, default to scalar blocks
if shape is None:
shape = ()
# Otherwise, ensure the shape is valid
elif isinstance(shape, int):
if shape <= 0:
raise ValueError("The `shape` field must be a positive integer.")
elif shape == 1:
shape = ()
else:
shape = (shape,)
elif isinstance(shape, tuple):
if len(shape) > 2:
raise ValueError(
"The `shape` field must be an int or a tuple of one "
"or two ints to indicate a vector or matrix block."
)
for dim in shape:
if dim <= 0:
raise ValueError("All dimensions in the `shape` field must be positive integers.")
else:
raise TypeError("The `shape` field must be an int or a tuple of ints.")
self._dtype: FloatType | IntType = dtype
"""The underlying data type of the sparse blocks."""
self._shape: int | tuple[int] | tuple[int, int] = shape
"""The shape of each sparse block."""
@property
def dtype(self) -> FloatType | IntType:
"""Returns the underlying data type of the sparse blocks."""
return self._dtype
@property
def shape(self) -> int | tuple[int] | tuple[int, int]:
"""Returns the shape of each sparse block."""
return self._shape
@property
def size(self) -> int:
"""Returns the number of elements contained in each sparse block."""
if isinstance(self._shape, int):
return self._shape
elif isinstance(self._shape, tuple):
size = 1
for dim in self._shape:
size *= dim
return size
raise RuntimeError("Unsupported block shape for size computation.")
@property
def warp_type(self) -> Any:
"""Returns the corresponding Warp type for this block-sparse type."""
if self._dtype is None or self._shape is None:
raise RuntimeError("Both `dtype` and `shape` fields must be specified to get the Warp type.")
if not isinstance(self._shape, tuple):
raise RuntimeError(f"Block shape should be a tuple but got {type(self._shape)}.")
if len(self._shape) == 0:
return self._dtype
elif len(self._shape) == 1:
class _vec_t(wp.types.vector(length=self._shape[0], dtype=self._dtype)):
pass
return _vec_t
elif len(self._shape) == 2:
class _mat_t(wp.types.matrix(shape=self._shape, dtype=self._dtype)):
pass
return _mat_t
else:
raise RuntimeError(f"Cannot convert to Warp type: Block shape is invalid: {self._shape}.")
@dataclass
class BlockSparseMatrices:
"""
A container to represent multiple block-sparse matrices of fixed non-zero block size.
"""
###
# Host-side Metadata
###
device: wp.DeviceLike | None = None
"""Host-side cache of the device on which all data arrays are allocated."""
nzb_dtype: BlockDType | None = None
"""Host-side cache of the fixed non-zero block data type contained in all sparse matrices."""
index_dtype: IntType = int32
"""Host-side cache of the integer type used for indexing the underlying data arrays."""
num_matrices: int = 0
"""
Host-side cache of the number of sparse matrices represented by this container.\n
When constructing the BSM via `finalize()`, this is inferred from the length of the provided capacities list.\n
Alternatively, it can be set directly if the BSM is constructed explicitly.
"""
sum_of_num_nzb: int = 0
"""
Host-side cache of the sum of the number of non-zero blocks over all sparse matrices.\n
When constructing the BSM via `finalize()`, this is computed from the provided capacities list.
Alternatively, it can be set directly if the BSM is constructed explicitly.
"""
max_of_num_nzb: int = 0
"""
Host-side cache of the maximum number of non-zero blocks over all sparse matrices.\n
When constructing the BSM via `finalize()`, this is computed from the provided capacities list.
Alternatively, it can be set directly if the BSM is constructed explicitly.
"""
max_of_max_dims: tuple[int, int] = (0, 0)
"""
Host-side cache of the maximum of the maximum matrix dimensions over all sparse matrices.
"""
###
# On-device Data (Constant)
###
# These arrays are expected to stay constant once this object is finalized
max_dims: wp.array | None = None
"""
The maximum dimensions of each sparse matrices.\n
Shape of ``(num_matrices,)`` and type :class:`vec2i`.
"""
max_nzb: wp.array | None = None
"""
The maximum number of non-zero blocks per sparse matrices.\n
Shape of ``(num_matrices,)`` and type :class:`int`.
"""
nzb_start: wp.array | None = None
"""
The index of the first non-zero block of each sparse matrices.\n
Shape of ``(num_matrices,)`` and type :class:`int`.
"""
row_start: wp.array | None = None
"""
The start index of each row vector block in a flattened data array of size sum_of_max_rows.\n
Shape of ``(num_matrices,)`` and type :class:`int`.
"""
col_start: wp.array | None = None
"""
The start index of each column vector block in a flattened data array of size sum_of_max_cols.\n
Shape of ``(num_matrices,)`` and type :class:`int`.
"""
###
# On-device Data (Variable)
###
# These are the arrays to update when assembling the matrices
dims: wp.array | None = None
"""
The active dimensions of each sparse matrices.\n
Shape of ``(num_matrices,)`` and type :class:`vec2i`.
"""
num_nzb: wp.array | None = None
"""
The active number of non-zero blocks per sparse matrices.\n
Shape of ``(num_matrices,)`` and type :class:`int`.
"""
nzb_coords: wp.array | None = None
"""
The row-column coordinates of each non-zero block within its corresponding sparse matrix.\n
Shape of ``(sum_of_num_nzb,)`` and type :class:`vec2i`.
"""
nzb_values: wp.array | None = None
"""
The flattened array containing all non-zero blocks over all sparse matrices.\n
Shape of ``(sum_of_num_nzb,)`` and type :class:`float | vector | matrix`.
"""
###
# Properties
###
@property
def max_rows(self) -> wp.array:
assert self.max_dims is not None and self.max_dims.ptr is not None
index_dtype_size_bytes = type_size_in_bytes(self.index_dtype)
return wp.array(
dtype=self.index_dtype,
shape=(self.num_matrices,),
ptr=self.max_dims.ptr,
strides=(2 * index_dtype_size_bytes,),
copy=False,
)
@property
def max_cols(self) -> wp.array:
assert self.max_dims is not None and self.max_dims.ptr is not None
index_dtype_size_bytes = type_size_in_bytes(self.index_dtype)
return wp.array(
dtype=self.index_dtype,
shape=(self.num_matrices,),
ptr=self.max_dims.ptr + index_dtype_size_bytes,
strides=(2 * index_dtype_size_bytes,),
copy=False,
)
@property
def num_rows(self) -> wp.array:
assert self.dims is not None and self.dims.ptr is not None
index_dtype_size_bytes = type_size_in_bytes(self.index_dtype)
return wp.array(
dtype=self.index_dtype,
shape=(self.num_matrices,),
ptr=self.dims.ptr,
strides=(2 * index_dtype_size_bytes,),
copy=False,
)
@property
def num_cols(self) -> wp.array:
assert self.dims is not None and self.dims.ptr is not None
index_dtype_size_bytes = type_size_in_bytes(self.index_dtype)
return wp.array(
dtype=self.index_dtype,
shape=(self.num_matrices,),
ptr=self.dims.ptr + index_dtype_size_bytes,
strides=(2 * index_dtype_size_bytes,),
copy=False,
)
@property
def nzb_row(self) -> wp.array:
assert self.nzb_coords is not None and self.nzb_coords.ptr is not None
index_dtype_size_bytes = type_size_in_bytes(self.index_dtype)
return wp.array(
dtype=self.index_dtype,
shape=(self.sum_of_num_nzb,),
ptr=self.nzb_coords.ptr,
strides=(2 * index_dtype_size_bytes,),
copy=False,
)
@property
def nzb_col(self) -> wp.array:
assert self.nzb_coords is not None and self.nzb_coords.ptr is not None
index_dtype_size_bytes = type_size_in_bytes(self.index_dtype)
return wp.array(
dtype=self.index_dtype,
shape=(self.sum_of_num_nzb,),
ptr=self.nzb_coords.ptr + index_dtype_size_bytes,
strides=(2 * index_dtype_size_bytes,),
copy=False,
)
###
# Operations
###
def finalize(
self,
max_dims: list[tuple[int, int]],
capacities: list[int],
nzb_dtype: BlockDType | None = None,
index_dtype: IntType | None = None,
device: wp.DeviceLike | None = None,
):
"""
Finalizes the block-sparse matrix container by allocating on-device data arrays.
Args:
max_dims (list[tuple[int, int]]):
A list of pairs of integers, specifying the maximum number of rows and columns for each matrix.
capacities (list[int]):
A list of integers specifying the maximum number of non-zero blocks for each sparse matrix.
block_type (BlockDType, optional):
An optional :class:`BlockDType` specifying the fixed type of each non-zero block.\n
If not provided, it must be set prior to finalization.
device (wp.DeviceLike, optional):
An optional device on which to allocate the data arrays.\n
If not provided, the existing device of the container will be used.
Raises:
RuntimeError:
If the `nzb_dtype` field has not been specified prior to finalization.
ValueError:
If the `capacities` field is not a non-empty list of non-negative integers.
"""
# Override the device if provided
if device is not None:
self.device = device
# Override the block type if provided
if nzb_dtype is not None:
self.nzb_dtype = nzb_dtype
# Override the index type if provided
if index_dtype is not None:
self.index_dtype = index_dtype
# Ensure that the block and index dtypes have been specified
if self.nzb_dtype is None:
raise RuntimeError("The `nzb_dtype` field must be specified before finalizing the data arrays.")
elif not isinstance(self.nzb_dtype, BlockDType):
raise TypeError("The `nzb_dtype` field must be a valid BlockDType instance.")
if self.index_dtype is None:
raise RuntimeError("The `index_type` field must be specified before finalizing the data arrays.")
elif self.index_dtype not in get_args(IntType):
raise TypeError("The `index_type` field must be a valid IntType such as int32 or int64.")
# Ensure that the max dimensions are valid
if not isinstance(max_dims, list) or len(max_dims) == 0:
raise ValueError("The `max_dims` field must be a non-empty list of integers 2-tuples.")
for dims in max_dims:
if not isinstance(dims, tuple) or len(dims) != 2:
raise ValueError("All entries in the `max_dims` field must be 2-tuples of non-negative integers.")
r, c = dims
if not isinstance(r, int) or not isinstance(c, int) or r < 0 or c < 0:
raise ValueError("All entries in the `max_dims` field must be 2-tuples of non-negative integers.")
# Ensure that the capacities are valid
if not isinstance(capacities, list) or len(capacities) == 0:
raise ValueError("The `capacities` field must be a non-empty list of integers.")
for cap in capacities:
if not isinstance(cap, int) or cap < 0:
raise ValueError("All entries in the `capacities` field must be non-negative integers.")
# Ensure that inputs are consistent
if len(max_dims) != len(capacities):
raise ValueError("The `max_dims`, and `capacities` fields must have the same size.")
# Update memory allocation meta-data caches
self.num_matrices = len(capacities)
self.max_of_max_dims = tuple(max(x) for x in zip(*max_dims, strict=True))
self.sum_of_num_nzb = sum(capacities)
self.max_of_num_nzb = max(capacities)
# Compute cumulated sums for rows, cols and nzb
dim_start_np = np.concatenate(([[0, 0]], np.asarray(max_dims).cumsum(axis=0)))[:-1]
nzb_start_np = np.concatenate(([0], np.asarray(capacities).cumsum()))[:-1]
# Initialize on-device warp arrays
with wp.ScopedDevice(self.device):
self.max_dims = wp.from_numpy(np.asarray(max_dims), shape=(self.num_matrices, 2), dtype=self.index_dtype)
self.dims = wp.zeros(shape=(self.num_matrices, 2), dtype=self.index_dtype)
self.row_start = wp.from_numpy(dim_start_np[:, 0], shape=(self.num_matrices,), dtype=self.index_dtype)
self.col_start = wp.from_numpy(dim_start_np[:, 1], shape=(self.num_matrices,), dtype=self.index_dtype)
self.max_nzb = wp.from_numpy(np.asarray(capacities), shape=(self.num_matrices,), dtype=self.index_dtype)
self.num_nzb = wp.zeros(shape=(self.num_matrices,), dtype=self.index_dtype)
self.nzb_start = wp.from_numpy(nzb_start_np, shape=(self.num_matrices,), dtype=self.index_dtype)
self.nzb_coords = wp.zeros(shape=(self.sum_of_num_nzb, 2), dtype=self.index_dtype)
self.nzb_values = wp.zeros(shape=(self.sum_of_num_nzb,), dtype=self.nzb_dtype.warp_type)
def clear(self):
"""Clears all variable non-zero blocks."""
self._assert_is_finalized()
self.dims.zero_()
self.num_nzb.zero_()
self.nzb_coords.zero_()
def zero(self):
"""Sets all non-zero block data to zero."""
self._assert_is_finalized()
self.nzb_values.zero_()
def assign(self, matrices: list[np.ndarray]):
"""
Assigns data to all sparse matrices from a list of dense NumPy arrays.
This operation assumes that:
- the sparse matrices have been finalized
- the provided dense arrays match the active dimensions of each sparse matrix specified in `dims`
- the non-zero blocks are filled in row-major order according to the current values of `nzb_coords`.
Args:
data (list[np.ndarray]):
A list of dense NumPy arrays to assign to each sparse matrix.
"""
# Ensure that the sparse matrices have been finalized
self._assert_is_finalized()
# Retrieve the fixed-size block dimensions
block_nrows, block_ncols = self._get_block_shape()
# Populate each sparse matrix from the provided dense arrays
nzb_values_np = np.zeros_like(self.nzb_values.numpy())
for m in range(self.num_matrices):
# Retrieve the active matrix dimensions
dims = self.dims.numpy()[m]
nrows, ncols = int(dims[0]), int(dims[1])
# Validate the provided dense array
dense_matrix = matrices[m]
if dense_matrix.shape != (nrows, ncols):
raise ValueError(
f"The provided dense array for matrix {m} has shape {dense_matrix.shape}, "
f"but expected shape is ({nrows}, {ncols})."
)
# Populate non-zero blocks
num_nzb = int(self.num_nzb.numpy()[m])
start_idx = int(self.nzb_start.numpy()[m])
coords = self.nzb_coords.numpy()[start_idx : start_idx + num_nzb]
for b in range(num_nzb):
row_idx, col_idx = int(coords[b][0]), int(coords[b][1])
block_value = dense_matrix[row_idx : row_idx + block_nrows, col_idx : col_idx + block_ncols]
nzb_values_np[start_idx + b] = block_value
# Copy the populated non-zero block values to the device
self.nzb_values.assign(nzb_values_np)
def numpy(self) -> list[np.ndarray]:
"""Converts all sparse matrices to a list of dense NumPy arrays."""
# Ensure that the sparse matrices have been finalized
self._assert_is_finalized()
# Retrieve the fixed-size block dimensions
block_nrows, block_ncols = self._get_block_shape()
# Retrieve sparse data from the device
dims_np = self.dims.numpy()
num_nzb_np = self.num_nzb.numpy()
nzb_start_np = self.nzb_start.numpy()
nzb_coords_np = self.nzb_coords.numpy()
nzb_values_np = self.nzb_values.numpy()
# Construct a list of dense NumPy matrices from the sparse representation
matrices: list[np.ndarray] = []
for m in range(self.num_matrices):
# Retrieve the active matrix dimensions
dims = dims_np[m]
nrows, ncols = int(dims[0]), int(dims[1])
# Allocate dense matrix initially filled with zeros
dense_matrix = np.zeros((nrows, ncols), dtype=wp.dtype_to_numpy(self.nzb_dtype.dtype))
# Populate non-zero blocks
num_nzb = int(num_nzb_np[m])
start_idx = int(nzb_start_np[m])
coords = nzb_coords_np[start_idx : start_idx + num_nzb]
values = nzb_values_np[start_idx : start_idx + num_nzb]
for b in range(num_nzb):
row_idx, col_idx = int(coords[b][0]), int(coords[b][1])
block_value = values[b].reshape((block_nrows, block_ncols))
dense_matrix[row_idx : row_idx + block_nrows, col_idx : col_idx + block_ncols] += block_value
matrices.append(dense_matrix)
# Return the list of dense matrices
return matrices
###
# Internals
###
def _has_valid_metadata(self) -> bool:
return (
self.num_matrices > 0 and self.sum_of_num_nzb > 0 and self.max_of_num_nzb > 0 and self.nzb_dtype is not None
)
def _is_finalized(self) -> bool:
return (
self.nzb_values is not None
and self.max_dims is not None
and self.dims is not None
and self.max_nzb is not None
and self.num_nzb is not None
and self.nzb_start is not None
and self.nzb_coords is not None
and self.nzb_values is not None
)
def _assert_is_finalized(self):
if not self._is_finalized():
raise RuntimeError("No data has been allocated. Call `finalize()` before use.")
def _get_block_shape(self) -> tuple[int, int]:
"""Retrieves the fixed-size block shape as number of rows and columns according to row-major ordering."""
# NOTE: Assumes row-major ordering
block_shape = self.nzb_dtype.shape
if isinstance(block_shape, int):
block_nrows = 1
block_ncols = block_shape
elif isinstance(block_shape, tuple):
if len(block_shape) == 0:
block_nrows = 1
block_ncols = 1
elif len(block_shape) == 1:
block_nrows = 1
block_ncols = block_shape[0]
elif len(block_shape) == 2:
block_nrows = block_shape[0]
block_ncols = block_shape[1]
else:
raise RuntimeError("Unsupported block shape for NumPy conversion.")
else:
raise RuntimeError("Unsupported block shape for NumPy conversion.")
return block_nrows, block_ncols
###
# Dense to Block-Sparse Conversion
###
@wp.kernel
def _copy_square_dims_kernel(
src_dim: wp.array(dtype=int32),
dst_dims: wp.array2d(dtype=int32),
):
"""Copies square dimensions from 1D array to 2D (n, n) format."""
wid = wp.tid()
d = src_dim[wid]
dst_dims[wid, 0] = d
dst_dims[wid, 1] = d
@functools.cache
def _make_dense_to_bsm_detect_kernel(block_size: int):
"""Creates a kernel that detects non-zero blocks in dense matrices and populates BSM coordinates.
Note: Dense matrices use canonical compact storage where stride = active dim (not maxdim).
"""
@wp.kernel
def kernel(
# Dense matrix info
dense_dim: wp.array(dtype=int32),
dense_mio: wp.array(dtype=int32),
dense_mat: wp.array(dtype=float32),
# BSM info
max_nzb: wp.array(dtype=int32),
nzb_start: wp.array(dtype=int32),
# Outputs
num_nzb: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
):
wid, bi, bj = wp.tid()
dim = dense_dim[wid]
bs = wp.static(block_size)
n_blocks = (dim + bs - 1) // bs
if bi >= n_blocks or bj >= n_blocks:
return
row_start = bi * bs
col_start = bj * bs
m_offset = dense_mio[wid]
# Check if any element in this block is non-zero
# Dense matrices use compact storage: stride = dim
nonzero_count = int(0)
for i in range(bs):
row = row_start + i
if row < dim:
for j in range(bs):
col = col_start + j
if col < dim:
idx = m_offset + row * dim + col
if dense_mat[idx] != float32(0.0):
nonzero_count = nonzero_count + 1
if nonzero_count > 0:
slot = wp.atomic_add(num_nzb, wid, 1)
cap = max_nzb[wid]
if slot < cap:
global_idx = nzb_start[wid] + slot
nzb_coords[global_idx, 0] = row_start
nzb_coords[global_idx, 1] = col_start
return kernel
@functools.cache
def _make_dense_to_bsm_copy_kernel(block_size: int):
"""Creates a kernel that copies block values from dense matrices to BSM storage.
Note: Dense matrices use canonical compact storage where stride = active dim (not maxdim).
"""
mat_type = wp.types.matrix(shape=(block_size, block_size), dtype=float32)
@wp.kernel
def kernel(
# Dense matrix info
dense_dim: wp.array(dtype=int32),
dense_mio: wp.array(dtype=int32),
dense_mat: wp.array(dtype=float32),
# BSM info
nzb_start: wp.array(dtype=int32),
num_nzb: wp.array(dtype=int32),
nzb_coords: wp.array2d(dtype=int32),
# Output
nzb_values: wp.array(dtype=mat_type),
):
wid, block_idx = wp.tid()
if block_idx >= num_nzb[wid]:
return
dim = dense_dim[wid]
m_offset = dense_mio[wid]
global_idx = nzb_start[wid] + block_idx
row_start = nzb_coords[global_idx, 0]
col_start = nzb_coords[global_idx, 1]
bs = wp.static(block_size)
block = mat_type()
# Dense matrices use compact storage: stride = dim
for i in range(bs):
row = row_start + i
for j in range(bs):
col = col_start + j
if row < dim and col < dim:
idx = m_offset + row * dim + col
block[i, j] = dense_mat[idx]
else:
block[i, j] = float32(0.0)
nzb_values[global_idx] = block
return kernel
def allocate_block_sparse_from_dense(
dense_op: "DenseLinearOperatorData",
block_size: int,
sparsity_threshold: float = 1.0,
device: wp.DeviceLike | None = None,
) -> BlockSparseMatrices:
"""
Allocates a BlockSparseMatrices container sized for converting from a dense operator.
Args:
dense_op: The dense linear operator to convert from.
block_size: The size of each square block.
sparsity_threshold: Fraction of maximum possible blocks to allocate for (0.0 to 1.0).
E.g., 0.5 allocates for up to 50% of blocks being non-zero. Default 1.0 (all blocks).
device: Device to allocate on. Defaults to the dense operator's device.
Returns:
A finalized but empty BlockSparseMatrices ready for use with dense_to_block_sparse_copy_values.
"""
if dense_op.info is None:
raise ValueError("Dense operator must have info set.")
if not isinstance(dense_op.info, DenseSquareMultiLinearInfo):
raise ValueError("Dense operator must be square (DenseSquareMultiLinearInfo).")
info = dense_op.info
if info.dimensions is None:
raise ValueError("Dense operator info must have dimensions set.")
if device is None:
device = info.device
max_dims_list: list[tuple[int, int]] = []
capacities: list[int] = []
for dim in info.dimensions:
n_blocks_per_dim = (dim + block_size - 1) // block_size
max_blocks = n_blocks_per_dim * n_blocks_per_dim
capacity = max(1, int(sparsity_threshold * max_blocks))
max_dims_list.append((dim, dim))
capacities.append(capacity)
nzb_dtype = BlockDType(dtype=info.dtype, shape=(block_size, block_size))
bsm = BlockSparseMatrices()
bsm.finalize(
max_dims=max_dims_list,
capacities=capacities,
nzb_dtype=nzb_dtype,
index_dtype=info.itype,
device=device,
)
return bsm
def dense_to_block_sparse_copy_values(
dense_op: "DenseLinearOperatorData",
bsm: BlockSparseMatrices,
block_size: int,
) -> None:
"""
Converts dense matrix values to block-sparse format (graph-capturable).
This function detects non-zero blocks and copies their values from the dense
operator to the block-sparse matrices container. It is fully GPU-based and
graph-capturable.
Args:
dense_op: The dense linear operator containing the matrix data.
bsm: A pre-allocated BlockSparseMatrices (from allocate_block_sparse_from_dense).
block_size: The block size (must match the BSM's block size).
"""
if dense_op.info is None:
raise ValueError("Dense operator must have info set.")
if not isinstance(dense_op.info, DenseSquareMultiLinearInfo):
raise ValueError("Dense operator must be square.")
if not bsm._is_finalized():
raise ValueError("BlockSparseMatrices must be finalized before use.")
info = dense_op.info
device = bsm.device
# Reset num_nzb counter for fresh detection
bsm.num_nzb.zero_()
# Copy active dimensions from dense to BSM
wp.launch(
_copy_square_dims_kernel,
dim=(info.num_blocks,),
inputs=[info.dim],
outputs=[bsm.dims],
device=device,
)
# Compute launch dimensions
max_dim = info.max_dimension
max_blocks_per_dim = (max_dim + block_size - 1) // block_size
# Get cached kernels
detect_kernel = _make_dense_to_bsm_detect_kernel(block_size)
copy_kernel = _make_dense_to_bsm_copy_kernel(block_size)
# Launch detection kernel
wp.launch(
detect_kernel,
dim=(info.num_blocks, max_blocks_per_dim, max_blocks_per_dim),
inputs=[
info.dim,
info.mio,
dense_op.mat,
bsm.max_nzb,
bsm.nzb_start,
],
outputs=[
bsm.num_nzb,
bsm.nzb_coords,
],
device=device,
)
# Launch copy kernel
wp.launch(
copy_kernel,
dim=(bsm.num_matrices, bsm.max_of_num_nzb),
inputs=[
info.dim,
info.mio,
dense_op.mat,
bsm.nzb_start,
bsm.num_nzb,
bsm.nzb_coords,
],
outputs=[
bsm.nzb_values,
],
device=device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/sparse_operator.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Linear Algebra: Core types and utilities for sparse multi-world linear systems
This module provides data structures and utilities for managing multiple
independent linear systems, including rectangular and square systems.
"""
from collections.abc import Callable
import warp as wp
from ..core.types import FloatType
from .blas import (
block_sparse_gemv,
block_sparse_matvec,
block_sparse_transpose_gemv,
block_sparse_transpose_matvec,
)
from .sparse_matrix import BlockSparseMatrices
###
# Module interface
###
__all__ = [
"BlockSparseLinearOperators",
]
class BlockSparseLinearOperators:
"""
A Block-Sparse Linear Operator container for representing
and operating on multiple independent sparse linear systems.
"""
def __init__(self, bsm: BlockSparseMatrices | None = None):
self.bsm = bsm
self.initialize_default_operators()
self._active_rows: wp.array | None = None
self._active_cols: wp.array | None = None
if self.bsm is not None:
self._active_rows = self.bsm.num_rows
self._active_cols = self.bsm.num_cols
###
# On-device Data
###
bsm: BlockSparseMatrices | None = None
"""
The underlying block-sparse matrix used by this operator.
"""
###
# Operators
###
precompute_op: Callable | None = None
"""
The operator function for precomputing any necessary data for the operators.\n
Signature: ``precompute_op(A: BlockSparseLinearOperators)``.
"""
Ax_op: Callable | None = None
"""
The operator function for performing sparse matrix-vector products `y = A @ x`.\n
Example signature: ``Ax_op(A: BlockSparseLinearMatrices, x: wp.array, y: wp.array, matrix_mask: wp.array)``.
"""
ATy_op: Callable | None = None
"""
The operator function for performing sparse matrix-transpose-vector products `x = A^T @ y`.\n
Example signature: ``ATy_op(A: BlockSparseLinearMatrices, y: wp.array, x: wp.array, matrix_mask: wp.array)``.
"""
gemv_op: Callable | None = None
"""
The operator function for performing generalized sparse matrix-vector products `y = alpha * A @ x + beta * y`.\n
Example signature: ``gemv_op(A: BlockSparseLinearMatrices, x: wp.array, y: wp.array, alpha: float, beta: float, matrix_mask: wp.array)``.
"""
gemvt_op: Callable | None = None
"""
The operator function for performing generalized sparse matrix-transpose-vector products `x = alpha * A^T @ y + beta * x`.\n
Example signature: ``gemvt_op(A: BlockSparseLinearMatrices, y: wp.array, x: wp.array, alpha: float, beta: float, matrix_mask: wp.array)``.
"""
###
# Properties
###
@property
def num_matrices(self) -> int:
return self.bsm.num_matrices
@property
def max_of_max_dims(self) -> tuple[int, int]:
return self.bsm.max_of_max_dims
@property
def dtype(self) -> FloatType:
return self.bsm.nzb_dtype.dtype
@property
def device(self) -> wp.DeviceLike:
return self.bsm.device
@property
def active_rows(self) -> wp.array:
return self._active_rows
@property
def active_cols(self) -> wp.array:
return self._active_cols
###
# Operations
###
def clear(self):
"""Clears all variable sub-blocks."""
self.bsm.clear()
def zero(self):
"""Sets all sub-block data to zero."""
self.bsm.zero()
def precompute(self):
"""Precomputes any necessary data for the operators."""
if self.precompute_op:
self.precompute_op(self)
def initialize_default_operators(self):
"""Sets all operator functions to their default implementations."""
self.Ax_op = block_sparse_matvec
self.ATy_op = block_sparse_transpose_matvec
self.gemv_op = block_sparse_gemv
self.gemvt_op = block_sparse_transpose_gemv
def matvec(self, x: wp.array, y: wp.array, matrix_mask: wp.array):
"""Performs the sparse matrix-vector product `y = A @ x`."""
if self.Ax_op is None:
raise RuntimeError("No `A@x` operator has been assigned.")
self.Ax_op(self.bsm, x, y, matrix_mask)
def matvec_transpose(self, y: wp.array, x: wp.array, matrix_mask: wp.array):
"""Performs the sparse matrix-transpose-vector product `x = A^T @ y`."""
if self.ATy_op is None:
raise RuntimeError("No `A^T@y` operator has been assigned.")
self.ATy_op(self.bsm, y, x, matrix_mask)
def gemv(self, x: wp.array, y: wp.array, matrix_mask: wp.array, alpha: float = 1.0, beta: float = 0.0):
"""Performs a BLAS-like generalized sparse matrix-vector product `y = alpha * A @ x + beta * y`."""
if self.gemv_op is None:
raise RuntimeError("No BLAS-like `GEMV` operator has been assigned.")
self.gemv_op(self.bsm, x, y, alpha, beta, matrix_mask)
def gemv_transpose(self, y: wp.array, x: wp.array, matrix_mask: wp.array, alpha: float = 1.0, beta: float = 0.0):
"""Performs a BLAS-like generalized sparse matrix-transpose-vector product `x = alpha * A^T @ y + beta * x`."""
if self.gemvt_op is None:
raise RuntimeError("No BLAS-like transposed `GEMV` operator has been assigned.")
self.gemvt_op(self.bsm, y, x, alpha, beta, matrix_mask)
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/utils/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Linear Algebra Utilities"""
from .matrix import (
MatrixComparison,
MatrixSign,
RectangularMatrixProperties,
SquareSymmetricMatrixProperties,
is_square_matrix,
is_symmetric_matrix,
)
from .rand import (
eigenvalues_from_distribution,
random_rhs_for_matrix,
random_spd_matrix,
random_symmetric_matrix,
)
from .range import (
in_range_via_gaussian_elimination,
in_range_via_left_nullspace,
in_range_via_projection,
in_range_via_rank,
in_range_via_residual,
)
###
# Module interface
###
__all__ = [
"MatrixComparison",
"MatrixSign",
"RectangularMatrixProperties",
"SquareSymmetricMatrixProperties",
"eigenvalues_from_distribution",
"in_range_via_gaussian_elimination",
"in_range_via_left_nullspace",
"in_range_via_lu",
"in_range_via_projection",
"in_range_via_rank",
"in_range_via_residual",
"is_square_matrix",
"is_symmetric_matrix",
"random_rhs_for_matrix",
"random_spd_matrix",
"random_symmetric_matrix",
]
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/utils/matrix.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Linear Algebra Utilities: Matrix properties"""
from enum import IntEnum
import numpy as np
###
# Module interface
###
__all__ = [
"DEFAULT_MATRIX_SYMMETRY_EPS",
"MatrixComparison",
"MatrixSign",
"SquareSymmetricMatrixProperties",
"assert_is_square_matrix",
"assert_is_symmetric_matrix",
"is_square_matrix",
"is_symmetric_matrix",
"symmetry_error_norm_l2",
]
###
# Constants
###
DEFAULT_MATRIX_SYMMETRY_EPS = 1e-10
"""A global constant to configure the tolerance on matrix symmetry checks."""
MAXLOG_FP64 = 709.782
"""Maximum log value for float64 to avoid overflow in exp()."""
###
# Types
###
class MatrixSign(IntEnum):
ZeroSign = 0
Indefinite = 1
PositiveSemiDef = 2
NegativeSemiDef = 3
PositiveDef = 4
NegativeDef = 5
###
# Utilities
###
def _safe_slogdet(A: np.ndarray) -> tuple[float, float, float]:
sign, logabsdet = np.linalg.slogdet(A)
sign = float(sign)
logabsdet = float(logabsdet)
if sign != 0.0 and logabsdet < MAXLOG_FP64:
det = sign * np.exp(logabsdet)
elif sign == 0.0:
det = 0.0
else:
det = np.inf
return sign, logabsdet, det
def _make_tolerance(tol: float | None = None, dtype: np.dtype = np.float64):
eps = np.finfo(dtype).eps
if tol is None:
tol = dtype.type(eps)
else:
if not isinstance(tol, float | np.float32 | np.float64):
raise ValueError("tolerance 'tol' must be a `float`, `np.float32`, or `np.float64` value.")
return dtype.type(max(tol, eps))
def is_square_matrix(A: np.ndarray) -> bool:
return A.shape[0] == A.shape[1]
def is_symmetric_matrix(A: np.ndarray, tol: float | None = None) -> bool:
tol = _make_tolerance(tol=tol, dtype=A.dtype)
return np.allclose(A, A.T, atol=tol, rtol=0.0)
def symmetry_error_norm_l2(A: np.ndarray) -> float:
return np.linalg.norm(A - A.T, ord=2)
def assert_is_square_matrix(A: np.ndarray):
if not is_square_matrix(A):
raise ValueError("Matrix is not square.")
def assert_is_symmetric_matrix(A: np.ndarray):
eps = max(_make_tolerance(dtype=A.dtype), A.dtype.type(DEFAULT_MATRIX_SYMMETRY_EPS))
if not is_symmetric_matrix(A, tol=eps):
error = symmetry_error_norm_l2(A)
raise ValueError(f"Matrix is not symmetric within tolerance {eps}, with error (inf-norm): {error}")
###
# Matrix Properties
###
class RectangularMatrixProperties:
def __init__(self, matrix: np.ndarray | None = None):
self.matrix: np.ndarray
"""Reference to the original matrix."""
# Matrix statistics
self.min: float = np.inf
"""The minimum element of the matrix."""
self.max: float = np.inf
"""The maximum element of the matrix."""
self.mean: float = np.inf
"""The mean of the matrix elements."""
self.std: float = np.inf
"""The standard deviation of the matrix elements."""
# Matrix dimensions
self.shape: tuple[int, int] = (0, 0)
"""The matrix shape (rows, cols)."""
# Matrix properties
self.rank: int = 0
"""The matrix rank compute using `numpy.linalg.matrix_rank()`."""
# Matrix norms
self.norm_fro: float = np.inf
"""The Frobenius norm compute using `numpy.linalg.norm()`."""
self.norm_inf: float = np.inf
"""The infinity norm compute using `numpy.linalg.norm()`."""
# SVD properties
self.sigma_min: float = np.inf
"""The smallest singular value."""
self.sigma_max: float = np.inf
"""The largest singular value."""
self.sigma_cond: float = np.inf
"""The condition number defined via the ratio of max/min singular values."""
# Caches
self.sigmas: np.ndarray | None = None
self.U: np.ndarray | None = None
self.Vt: np.ndarray | None = None
# Compute matrix properties if specified
if matrix is not None:
self.compute(matrix)
def compute(self, matrix: np.ndarray):
"""
Compute the properties of the rectangular matrix.
Args:
matrix (np.ndarray): The input matrix to analyze.
tol (float, optional): The tolerance for numerical stability.
Raises:
TypeError: If the input matrix is not a numpy array.
ValueError: If the input matrix is not 2D.
"""
# Check if the matrix is valid type and dimensions
if not isinstance(matrix, np.ndarray):
raise TypeError("Input must be a numpy array.")
if matrix.ndim != 2:
raise ValueError("Input must be a 2D matrix.")
# Capture the reference to the target matrix
self.matrix = matrix
# Then compute statistics over the coefficients
self.min = np.min(self.matrix)
self.max = np.max(self.matrix)
self.mean = np.mean(self.matrix)
self.std = np.std(self.matrix)
# Extract additional properties using numpy operations
self.shape = self.matrix.shape
self.rank = np.linalg.matrix_rank(self.matrix)
# Compute matrix norms
self.norm_fro = np.linalg.norm(self.matrix, ord="fro")
self.norm_inf = np.linalg.norm(self.matrix, ord=np.inf)
# Extract the matrix singular values
self.U, self.sigmas, self.Vt = np.linalg.svd(self.matrix, full_matrices=True, compute_uv=True, hermitian=False)
self.sigma_min = self.sigmas[-1]
self.sigma_max = self.sigmas[0]
self.sigma_cond = self.sigma_max / self.sigma_min
def __str__(self) -> str:
return (
f"Type:\n"
f" shape: {self.matrix.shape}\n"
f" dtype: {self.matrix.dtype}\n"
f"Statistics:\n"
f" min: {self.min}\n"
f" max: {self.max}\n"
f" mean: {self.mean}\n"
f" std: {self.std}\n"
f"Basics:\n"
f" rank: {self.rank}\n"
f"Norms:\n"
f" fro: {self.norm_fro}\n"
f" inf: {self.norm_inf}\n"
f"SVD:\n"
f" sigma min: {self.sigma_min}\n"
f" sigma max: {self.sigma_max}\n"
f" sigma cond: {self.sigma_cond}\n"
)
class SquareSymmetricMatrixProperties:
def __init__(self, matrix: np.ndarray | None = None, tol: float | None = None):
self.matrix: np.ndarray
"""Reference to the original matrix."""
# Matrix statistics
self.min: float = np.inf
"""The minimum element of the matrix."""
self.max: float = np.inf
"""The maximum element of the matrix."""
self.mean: float = np.inf
"""The mean of the matrix elements."""
self.std: float = np.inf
"""The standard deviation of the matrix elements."""
# Matrix dimensions
"""The matrix shape (rows, cols)."""
self.dim: int = 0
"""The matrix dimension (number of rows/columns) when square."""
self.symmetry_error: float = np.inf
"""The error measure of symmetry for the matrix."""
# Matrix properties
self.rank: int = 0
"""The matrix rank computed using `numpy.linalg.matrix_rank()`."""
self.trace: float = np.inf
"""The matrix trace computed using `numpy.trace()`."""
self.cond: float = np.inf
"""The matrix condition number computed using `numpy.linalg.cond()`."""
self.signdet: float = np.inf
"""The matrix determinant sign computed using `numpy.linalg.slogdet()`."""
self.logabsdet: float = np.inf
"""The matrix log absolute determinant computed using `numpy.linalg.slogdet()`."""
self.det: float = np.inf
"""The matrix determinant computed as `sign * exp(logabsdet)`."""
# Matrix norms
self.norm_fro: float = np.inf
"""The Frobenius norm computed using `numpy.linalg.norm()`."""
self.norm_inf: float = np.inf
"""The infinity norm computed using `numpy.linalg.norm()`."""
# Spectral properties
self.lambda_min: float = np.inf
"""The smallest eigenvalue."""
self.lambda_max: float = np.inf
"""The largest eigenvalue."""
self.lambda_cond: float = np.inf
"""The condition number defined via the ratio of max/min eigenvalues."""
# SVD properties
self.sigma_min: float = np.inf
"""The smallest singular value."""
self.sigma_max: float = np.inf
"""The largest singular value."""
self.sigma_cond: float = np.inf
"""The condition number defined via the ratio of max/min singular values."""
# Convexity properties
self.m: float = np.inf
"""The strong convexity parameter, defined as `m(A) := max(0, lambda_min(A))`."""
self.L: float = np.inf
"""The Lipschitz constant, defined as the spectral radius `L(A) := rho(A) := abs(lambda_max(A))`."""
self.kappa: float = np.inf
"""The condition number, defined as `kappa(A) := L(A) / m(A)`."""
# Matrix properties
self.is_square: bool = False
self.is_symmetric: bool = False
self.is_positive_definite: bool = False
self.is_positive_semi_definite: bool = False
# Caches
self.lambdas: np.ndarray | None = None
self.sigmas: np.ndarray | None = None
self.U: np.ndarray | None = None
self.Vt: np.ndarray | None = None
# Compute matrix properties if specified
if matrix is not None:
self.compute(matrix, tol)
def compute(self, matrix: np.ndarray, tol: float | None = None):
"""
Compute the properties of the matrix.
Args:
matrix (np.ndarray): The input matrix to analyze.
tol (float, optional): The tolerance for numerical stability.
Raises:
TypeError: If the input matrix is not a numpy array.
ValueError: If the input matrix is not 2D.
"""
# Check if the matrix is valid type and dimensions
if not isinstance(matrix, np.ndarray):
raise TypeError("Input must be a numpy array.")
if matrix.ndim != 2:
raise ValueError("Input must be a 2D matrix.")
# Extract the epsilon value either from the specified tolerance or on the dtype
if tol is not None:
if not isinstance(tol, float):
raise TypeError("tolerance parameter `tol` must be `float` type.")
eps = tol
eps_relaxed = tol
eps_symmetry = max(tol, DEFAULT_MATRIX_SYMMETRY_EPS)
else:
eps = float(np.finfo(matrix.dtype).eps)
eps_relaxed = 1e3 * eps
eps_symmetry = max(eps_relaxed, DEFAULT_MATRIX_SYMMETRY_EPS)
# Capture the reference to the target matrix
self.matrix = matrix
# First extract the basic properties of the matrix
self.is_square = self.matrix.shape[0] == self.matrix.shape[1]
self.is_symmetric = is_symmetric_matrix(self.matrix, tol=eps_symmetry)
self.symmetry_error = symmetry_error_norm_l2(self.matrix)
# Then compute statistics over the coefficients
self.min = np.min(self.matrix)
self.max = np.max(self.matrix)
self.mean = np.mean(self.matrix)
self.std = np.std(self.matrix)
# Extract additional properties using numpy operations
self.dim = self.matrix.shape[0]
self.rank = np.linalg.matrix_rank(self.matrix)
self.trace = np.trace(self.matrix)
self.cond = np.linalg.cond(self.matrix)
# Compute the determinant from the signed log-determinant
self.signdet, self.logabsdet, self.det = _safe_slogdet(self.matrix)
# Compute matrix norms
self.norm_fro = np.linalg.norm(self.matrix, ord="fro")
self.norm_inf = np.linalg.norm(self.matrix, ord=np.inf)
# Extract the matrix eigenvalues
self.lambdas = np.linalg.eigvals(self.matrix).real
self.lambda_min = self.lambdas.min()
self.lambda_max = self.lambdas.max()
self.lambda_cond = self.lambda_max / self.lambda_min
# Extract the matrix singular values
self.U, self.sigmas, self.Vt = np.linalg.svd(self.matrix, full_matrices=True, compute_uv=True, hermitian=False)
self.sigma_min = self.sigmas[-1]
self.sigma_max = self.sigmas[0]
self.sigma_cond = self.sigma_max / self.sigma_min
# Compute the convexity parameters
# self.m = np.abs(self.lambda_min)
self.m = max(0.0, self.lambda_min)
self.L = np.abs(self.lambda_max)
self.kappa = self.L / self.m if self.m > 0 else np.inf
# Determine the definiteness of the matrix
self.is_positive_definite = np.all(self.lambdas > eps_relaxed)
self.is_positive_semi_definite = np.all(self.lambdas > eps)
def __str__(self) -> str:
return (
f"Type:\n"
f" shape: {self.matrix.shape}\n"
f" dtype: {self.matrix.dtype}\n"
f"Info:\n"
f" square: {self.is_square}\n"
f" symm.: {self.is_symmetric} (err={self.symmetry_error})\n"
f" PSD: {self.is_positive_semi_definite}\n"
f" PD: {self.is_positive_definite}\n"
f"Statistics:\n"
f" min: {self.min}\n"
f" max: {self.max}\n"
f" mean: {self.mean}\n"
f" std: {self.std}\n"
f"Basics:\n"
f" rank: {self.rank}\n"
f" trace: {self.trace}\n"
f" cond: {self.cond}\n"
f"sign(det): {self.signdet}\n"
f" log|det|: {self.logabsdet}\n"
f" det: {self.det}\n"
f"Norms:\n"
f" fro: {self.norm_fro}\n"
f" inf: {self.norm_inf}\n"
f"Spectral:\n"
f" lambda min: {self.lambda_min}\n"
f" lambda max: {self.lambda_max}\n"
f" lambda cond: {self.lambda_cond}\n"
f"SVD:\n"
f" sigma min: {self.sigma_min}\n"
f" sigma max: {self.sigma_max}\n"
f" sigma cond: {self.sigma_cond}\n"
f"Convexity:\n"
f" L: {self.L}\n"
f" m: {self.m}\n"
f" kappa: {self.kappa}\n"
)
class MatrixComparison:
def __init__(self, A: np.ndarray, B: np.ndarray, tol: float = 0.0):
self.A = A
self.B = B
self.eps = np.finfo(self.A.dtype).eps
self.E = self.A - self.B
self.E_clip = self._error_clipped(tol=tol)
# Compute all metrics
self.frobenius_error = self._frobenius_error()
self.max_element_error = self._max_element_error()
self.relative_frobenius_error = self._relative_frobenius_error()
self.svd_error = self._svd_error()
self.relative_determinant_error = self._relative_determinant_error()
self.norm_1_error = self._norm_1_error()
self.norm_2_error = self._norm_2_error()
self.norm_inf_error = self._norm_inf_error()
def save(
self,
path: str,
title: str = "Matrix",
name_A: str = "A",
name_B: str = "B",
symbol_A: str = "A",
symbol_B: str = "B",
):
"""Save error visualizations to the specified path."""
import os # noqa: PLC0415
from newton._src.solvers.kamino._src.utils.sparse import sparseplot # noqa: PLC0415
os.makedirs(path, exist_ok=True)
sparseplot(self.A, title=f"{title} {name_A}", path=os.path.join(path, f"{symbol_A}.png"))
sparseplot(self.B, title=f"{title} {name_B}", path=os.path.join(path, f"{symbol_B}.png"))
sparseplot(self.E, title=f"{title} Error", path=os.path.join(path, f"{symbol_A}_err.png"))
sparseplot(self.E_clip, title=f"{title} Error (Clipped)", path=os.path.join(path, f"{symbol_A}_err_clip.png"))
def _error_clipped(self, tol: float = 0.0):
"""Clip small errors to zero for visualization."""
if tol > 0.0:
eps = tol
else:
eps = self.eps
E_clip = np.zeros_like(self.E)
for i in range(self.E.shape[0]):
for j in range(self.E.shape[1]):
if np.abs(self.E[i, j]) < eps:
E_clip[i, j] = 0.0
else:
E_clip[i, j] = self.E[i, j]
return E_clip
def _frobenius_error(self):
return np.linalg.norm(self.E, "fro")
def _max_element_error(self):
return np.max(np.abs(self.E))
def _relative_frobenius_error(self):
return np.linalg.norm(self.E, "fro") / np.linalg.norm(self.A, "fro")
def _svd_error(self):
# Singular value decomposition error
S_A = np.linalg.svd(self.A, compute_uv=False)
S_B = np.linalg.svd(self.B, compute_uv=False)
return np.linalg.norm(S_A - S_B) / np.linalg.norm(S_A)
def _relative_determinant_error(self):
det_A = np.linalg.det(self.A)
det_B = np.linalg.det(self.B)
if det_A == 0:
return np.abs(det_A - det_B) # Just the difference if det(A) is zero
return np.abs(det_A - det_B) / np.abs(det_A)
def _norm_1_error(self):
return np.linalg.norm(self.E, ord=1)
def _norm_2_error(self):
return np.linalg.norm(self.E, ord=2)
def _norm_inf_error(self):
return np.linalg.norm(self.E, ord=np.inf)
def __str__(self) -> str:
return (
f"Frobenius Error : {self.frobenius_error}\n"
f"Max Element Error : {self.max_element_error}\n"
f"Relative Frobenius Error : {self.relative_frobenius_error}\n"
f"SVD Error : {self.svd_error}\n"
f"Relative Determinant Error: {self.relative_determinant_error}\n"
f"1-Norm Error : {self.norm_1_error}\n"
f"2-Norm Error : {self.norm_2_error}\n"
f"Inf-Norm Error : {self.norm_inf_error}\n"
)
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/utils/rand.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Linear Algebra Utilities: Random matrix/rhs generation"""
import numpy as np
from ...core.types import FloatArrayLike
###
# Module interface
###
__all__ = [
"eigenvalues_from_distribution",
"random_rhs_for_matrix",
"random_spd_matrix",
"random_symmetric_matrix",
]
###
# Functions
###
def eigenvalues_from_distribution(
size: int,
num_pos: int | float = 0,
num_pos_eps: int | float = 0,
num_zero: int | float = 0,
num_neg_eps: int | float = 0,
num_neg: int | float = 0,
max_pos: float = 1e2,
min_pos: float = 1e-2,
eps_val: float = 1e-6,
max_neg: float = -1e-2,
min_neg: float = -1e2,
dtype: np.dtype = np.float64,
seed: int | None = None,
shuffle: bool = False,
) -> np.ndarray:
"""
Creates an array of eigen-values based on a specified distribution.
Notes:
- Default max/min/eps values are set in order to generate a moderately broad spectrum.
- The num_* arguments can be int (count) or float (percentage of size).
- The final counts are adjusted to sum to 'size'.
Args:
size (int): The total size of the eigenvalue distribution.
num_pos (int | float): The number of positive eigenvalues (count or percentage).
num_pos_eps (int | float): The number of positive epsilon eigenvalues (count or percentage).
num_zero (int | float): The number of zero eigenvalues (count or percentage).
num_neg_eps (int | float): The number of negative epsilon eigenvalues (count or percentage).
num_neg (int | float): The number of negative eigenvalues (count or percentage).
max_pos (float): The maximum value for positive eigenvalues.
min_pos (float): The minimum value for positive eigenvalues.
eps_val (float): The value for epsilon eigenvalues.
max_neg (float): The maximum value for negative eigenvalues.
min_neg (float): The minimum value for negative eigenvalues.
dtype (np.dtype): The data type for the eigenvalues.
shuffle (bool): Whether to shuffle the eigenvalues.
Returns:
np.ndarray: The generated eigenvalue array.
"""
# Helper to convert count/percentage to int
def resolve_count(val):
if isinstance(val, float):
return int(round(val * size))
return int(val)
# Interpret args as either counts or percentages
counts = {
"num_pos": resolve_count(num_pos),
"num_pos_eps": resolve_count(num_pos_eps),
"num_zero": resolve_count(num_zero),
"num_neg_eps": resolve_count(num_neg_eps),
"num_neg": resolve_count(num_neg),
}
# Check total counts and correct if necessary
total = sum(counts.values())
# If all counts are zero, assign all eigenvalues as positive
if total == 0:
counts["num_pos"] = size
# Otherwise, adjust counts to match 'size'
elif total != size:
# Distribute the difference to the largest group
diff = size - total
# Find the key with the largest count
if counts:
max_key = max(counts, key=lambda k: counts[k])
counts[max_key] += diff
# Generate the distribution of eigenvalues according to the specified counts
eigenvalues_pos = np.linspace(max_pos, min_pos, num=counts["num_pos"]) if counts["num_pos"] > 0 else np.array([])
eigenvalues_pos_eps = np.array([eps_val] * counts["num_pos_eps"]) if counts["num_pos_eps"] > 0 else np.array([])
eigenvalues_zero = np.zeros(counts["num_zero"]) if counts["num_zero"] > 0 else np.array([])
eigenvalues_neg_eps = np.array([-eps_val] * counts["num_neg_eps"]) if counts["num_neg_eps"] > 0 else np.array([])
eigenvalues_neg = np.linspace(max_neg, min_neg, num=counts["num_neg"]) if counts["num_neg"] > 0 else np.array([])
# Concatenate all eigenvalues into a single array of target dtype
eigenvalues = np.concatenate(
[
eigenvalues_pos.astype(dtype),
eigenvalues_pos_eps.astype(dtype),
eigenvalues_zero.astype(dtype),
eigenvalues_neg_eps.astype(dtype),
eigenvalues_neg.astype(dtype),
]
)
# Optionally shuffle the eigenvalues to randomize their order
if shuffle:
# Set the random seed if specified and valid
if seed is not None:
if not isinstance(seed, int):
raise TypeError("seed must be a int.")
# Initialize the random number generator
rng = np.random.default_rng(seed)
# Shuffle the eigenvalues in-place
rng.shuffle(eigenvalues)
# Finally return the constructed eigenvalues array
return eigenvalues
def random_symmetric_matrix(
dim: int,
dtype: np.dtype = np.float64,
scale: float | None = None,
seed: int | None = None,
rank: int | None = None,
eigenvalues: FloatArrayLike | None = None,
return_source: bool = False,
) -> np.ndarray | tuple[np.ndarray, np.ndarray]:
"""
Generate a random symmetric matrix of size (dim, dim).
Args:
- dim (int): The size of the matrix.
- dtype (data-type, optional): Data type of the matrix (default is float64).
- scale (float, optional): Scale factor for the matrix (default is 1.0).
- seed (int, optional): Seed for the random number generator.
- rank (int, optional): Rank of the matrix (must be <= dim).
- eigenvalues (array-like, optional): Eigenvalues for the matrix (must be of length dim).
- return_source (bool, optional): Whether to return the source matrix (default is False).
Returns:
- A (ndarray): A (dim, dim) symmetric matrix.
"""
# Set the random seed if specified and valid
if seed is not None:
if not isinstance(seed, int):
raise TypeError("seed must be a int.")
# Initialize the random number generator
rng = np.random.default_rng(seed)
# Set a default unit scale if unspecified
if scale is None:
scale = 1.0
sqrt_scale = 1.0
# Otherwise, check if scale is a float
else:
if not isinstance(scale, float):
raise TypeError("scale must be a float.")
sqrt_scale = np.sqrt(scale)
scale = dtype(scale)
sqrt_scale = dtype(sqrt_scale)
# Generate a symmetric matrix of random rank and eigenvalues, if unspecified
if eigenvalues is None and rank is None:
X = scale * rng.standard_normal((dim, dim)).astype(dtype)
# Make a symmetric matrix from the source random matrix
A = dtype(0.5) * (X + X.T)
# If eigenvalues are specified these take precedence
elif eigenvalues is not None:
if len(eigenvalues) != dim:
raise ValueError("The number of eigenvalues must match the matrix dimension.")
# Generate random square matrix
if np.all(eigenvalues == eigenvalues[0]):
X = rng.standard_normal((dim, dim)).astype(dtype)
else:
X, _ = np.linalg.qr(rng.standard_normal((dim, dim)).astype(dtype))
# Diagonal matrix of eigenvalues
D = np.diag(eigenvalues)
# A = X * D * X^T
A = scale * (X @ D @ X.T)
# Additional step to ensure symmetry
A = dtype(0.5) * (A + A.T)
# Otherwise generate a symmetric matrix of specified rank
elif rank is not None:
if rank > dim:
raise ValueError("Rank must not exceed matrix dimension.")
# Generate random rectangular matrix
X = sqrt_scale * rng.standard_normal((dim, rank)).astype(dtype)
# Make a rank-deficient symmetric matrix
A = X @ X.T
# Additional step to ensure symmetry
A = dtype(0.5) * (A + A.T)
# Optionally return both final and source matrices
if return_source:
return A, X
return A
def random_spd_matrix(
dim: int,
dtype: np.dtype = np.float64,
scale: float | None = None,
eta: float | None = None,
seed: int | None = None,
return_source: bool = False,
) -> np.ndarray | tuple[np.ndarray, np.ndarray]:
"""
Generate a symmetric positive definite (SPD) matrix of shape (n, n).
Args:
- n (int): The size of the matrix.
- dtype (data-type, optional): Data type of the matrix (default is float64).
- scale (float, optional): Scale factor for the matrix (default is 1.0).
- seed (int, optional): Seed for the random number generator.
- return_source (bool, optional): Whether to return the source matrix (default is False).
Returns:
- A (ndarray): An n x n symmetric positive definite matrix.
"""
# Set the random seed if specified and valid
if seed is not None:
if not isinstance(seed, int):
raise TypeError("seed must be a int.")
# Initialize the random number generator
rng = np.random.default_rng(seed)
# Set a default unit scale if unspecified
if scale is None:
scale = 1.0
sqrt_scale = 1.0
# Otherwise, check if scale is a float
else:
if not isinstance(scale, float):
raise TypeError("scale must be a float.")
sqrt_scale = np.sqrt(scale)
scale = dtype(scale)
sqrt_scale = dtype(sqrt_scale)
# Set a default diagonal regularizer `eta` value if unspecified
if eta is None:
eta = dim
elif not isinstance(eta, float):
raise TypeError("eta must be a float.")
eta = dtype(eta)
# Generate a random matrix
X = sqrt_scale * rng.standard_normal((dim, dim)).astype(dtype)
# Construct symmetric positive definite matrix: A.T @ A + eta * I
A = X.T @ X + eta * np.eye(dim, dtype=dtype)
# Ensure the matrix is symmetric
A = dtype(0.5) * (A + A.T)
# Optionally return both final and source matrices
if return_source:
return A, X
return A
def random_rhs_for_matrix(
A: np.ndarray, scale: float | None = None, seed: int | None = None, return_source: bool = False
) -> np.ndarray | tuple[np.ndarray, np.ndarray]:
"""
Generate a random RHS vector b that is in the range space of A.
Args:
A (np.ndarray): The input matrix.
scale (float): Scale factor for the random vector (default is 1.0).
return_source (bool): Whether to return the source vector used to generate the RHS (default is False).
Returns:
np.ndarray: A random RHS vector b in the range space of A.
"""
# Set the random seed if specified and valid
if seed is not None:
if not isinstance(seed, int):
raise TypeError("seed must be a int.")
# Initialize the random number generator
rng = np.random.default_rng(seed)
# Set a default unit scale if unspecified
if scale is None:
scale = 1.0
# Otherwise, check if scale is a float
else:
if not isinstance(scale, float):
raise TypeError("scale must be a float.")
scale = A.dtype.type(scale)
# Generate a random vector x and compute b = A @ x
x = scale * rng.standard_normal((A.shape[1],)).astype(A.dtype)
b = A @ x
# Optionally return both final and source vectors
if return_source:
return b, x
return b
================================================
FILE: newton/_src/solvers/kamino/_src/linalg/utils/range.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Linear Algebra Utilities: Operations to check if a rhs vector lies within the range of a matrix"""
import numpy as np
###
# Module interface
###
__all__ = [
"in_range_via_gaussian_elimination",
"in_range_via_left_nullspace",
"in_range_via_projection",
"in_range_via_rank",
"in_range_via_residual",
]
###
# Utilities
###
def _svd_rank(s: np.ndarray, shape: tuple, rcond: float | None = None):
"""
Determine numerical rank from singular values using a pinv-like threshold.
"""
m, n = shape
if rcond is None:
rcond = float(np.finfo(s.dtype).eps)
tol = rcond * max(m, n) * (s[0] if s.size else 0.0)
return int(np.sum(s > tol)), float(tol)
###
# Functions
###
def in_range_via_rank(A: np.ndarray, b: np.ndarray) -> bool:
"""
b is in range(A) iff rank(A) == rank([A|b])
"""
b = b.reshape(-1, 1)
rank_A = np.linalg.matrix_rank(A)
rank_Ab = np.linalg.matrix_rank(np.hstack([A, b]))
return rank_A == rank_Ab
def in_range_via_residual(A: np.ndarray, b: np.ndarray) -> bool:
"""
Solve min_x ||Ax - b||_2 and test if residual is ~0.
Tolerance scales with numerical precision and conditioning of A.
"""
b = b.reshape(-1, 1)
# Least-squares solution
x, *_ = np.linalg.lstsq(A, b, rcond=None)
r = b - A @ x
r_norm = float(np.linalg.norm(r))
# Compute singular values for scaling the tolerance
s = np.linalg.svd(A, compute_uv=False)
# Scale-aware tolerance: eps * max(n) * sigma_max(A) * ||b||
eps = float(np.finfo(s.dtype).eps)
sigma_max = float(s[0]) if s.size else float(1.0)
tol = eps * max(A.shape) * sigma_max * float(np.linalg.norm(b))
return r_norm <= tol, r_norm, float(tol), x.ravel()
def in_range_via_left_nullspace(U: np.ndarray, s: np.ndarray, b: np.ndarray, shape: tuple, rcond: float | None = None):
"""
b is in range(A) iff U0^T b ≈ 0, where U0 are left singular vectors for zero sigmas.
Returns (bool, residual_norm, tol).
"""
r, _ = _svd_rank(s, shape, rcond)
U0 = U[:, r:] # left-nullspace basis (empty if full rank)
if U0.size == 0:
return True, 0.0, 0.0
w = U0.T @ b
res = float(np.linalg.norm(w))
norm_b = float(np.linalg.norm(b))
eps = float(np.finfo(b.dtype).eps)
tol_b = eps * float(max(shape)) * norm_b
return res <= tol_b, res, tol_b
def in_range_via_projection(U: np.ndarray, s: np.ndarray, b: np.ndarray, shape: tuple, rcond: float | None = None):
"""
Project b onto span(U_r) and measure the leftover: ||(I - U_r U_r^T) b||.
Returns (bool, distance_to_range, tol, b_proj).
"""
r, _ = _svd_rank(s, shape, rcond)
Ur = U[:, :r]
b_proj = Ur @ (Ur.T @ b) if r > 0 else np.zeros_like(b)
residual = b - b_proj
norm_b = float(np.linalg.norm(b))
dist = float(np.linalg.norm(residual))
eps = float(np.finfo(b.dtype).eps)
tol_b = eps * float(max(shape)) * norm_b
return dist <= tol_b, dist, tol_b, b_proj
def in_range_via_gaussian_elimination(A: np.ndarray, b: np.ndarray, tol: float = 1e-12):
"""
Check if b is in the range (column space) of A by forming the augmented
matrix Ab = [A | b] and performing Gaussian elimination without pivoting.
Parameters
----------
A : (m, n) ndarray
Coefficient matrix.
b : (m,) or (m,1) ndarray
Right-hand side vector.
tol : float
Threshold for treating values as zero (numerical tolerance).
Returns
-------
in_range : bool
True iff rank(A) == rank([A|b]) under Gaussian elimination w/o pivoting.
ranks : tuple[int, int]
(rank_A, rank_Ab) computed from the row-echelon form obtained w/o pivoting.
UAb : ndarray
The upper-triangular (row-echelon-like) matrix after elimination on [A|b]
(useful for debugging/inspection).
Notes
-----
- No row swaps (no pivoting) are used, per the requirement.
- This procedure is less numerically robust than pivoted elimination.
- Rank is computed as the number of nonzero rows (by `tol`) in the echelon form.
"""
tol = A.dtype.type(tol)
if b.ndim == 1:
b = b[:, None]
if A.shape[0] != b.shape[0]:
raise ValueError("A and b must have the same number of rows.")
if A.dtype != b.dtype:
raise ValueError("A and b must have the same dtype.")
# Form augmented matrix [A | b]
UAb = np.concatenate([A, b], axis=1)
m, n_aug = UAb.shape
n = n_aug - 1 # number of columns in A portion
# Gaussian elimination without pivoting
# (Equivalent to LU factorization steps without P; we only keep the U-like result.)
for k in range(min(m, n)):
pivot = UAb[k, k]
if abs(pivot) <= tol:
# No row swap allowed; skip elimination for this column
continue
for i in range(k + 1, m):
factor = UAb[i, k] / pivot
# subtract factor * row k from row i (only on the trailing part for efficiency)
UAb[i, k:n_aug] -= factor * UAb[k, k:n_aug]
# Helper: count nonzero rows under tolerance
def rank_from_row_echelon(M, tol):
# A row is nonzero if any absolute entry exceeds tol
return int(np.sum(np.any(np.abs(M) > tol, axis=1)))
# Rank of A: evaluate on the left block after the same row ops
rank_A = rank_from_row_echelon(UAb[:, :n], tol)
# Rank of augmented matrix
rank_Ab = rank_from_row_echelon(UAb, tol)
return (rank_A == rank_Ab), (rank_A, rank_Ab), UAb
================================================
FILE: newton/_src/solvers/kamino/_src/models/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Predefined models for testing and demonstration of Kamino.
This module provides a collection of model builders and relevant utilities
for testing and demonstrating the features of the Kamino physics solver.
These include:
- A set of utility functions to retrieve paths to USD asset directories
- A set of 'example' models in the form of USD assets.\n
This directory currently provides a symbolic link to `newton-assets/disneyreasearch`.
- A set of 'basic' models used for demonstrating fundamental features of Kamino and for testing purposes.
These are provided both in the form of USD assets as well as manually constructed model builders.
- A set of `testing` models that are used to almost exclusively for unit testing, and include:
- supported geometric shapes, e.g. boxes, spheres, capsules, etc.
- supported joint types,e.g. revolute, prismatic, spherical, etc.
"""
import os
from .builders import basics, basics_newton, testing, utils
__all__ = [
"basics",
"basics_newton",
"builders",
"get_basics_usd_assets_path",
"get_testing_usd_assets_path",
"testing",
"utils",
]
###
# Asset path utilities
###
def get_basics_usd_assets_path() -> str:
"""
Returns the path to the USD assets for basic models.
"""
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), "assets/basics")
if not os.path.exists(path):
raise FileNotFoundError(f"The USD assets path for basic models does not exist: {path}")
return path
def get_testing_usd_assets_path() -> str:
"""
Returns the path to the USD assets for testing models.
"""
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), "assets/testing")
if not os.path.exists(path):
raise FileNotFoundError(f"The USD assets path for testing models does not exist: {path}")
return path
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/basics/box_on_plane.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "box_on_plane"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "box_on_plane"
{
def Scope "RigidBodies"
{
def Xform "box_body" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0, 0.0, 0.1)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (0.006666666828095913, 0.006666666828095913, 0.006666666828095913)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box_geom" (
# NOTE: "MaterialBindingAPI" is required for the material binding to work.
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI", "MaterialBindingAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.1, 0.1, 0.1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
color3f[] primvars:displayColor = [(0.7, 0.0, 1.0)]
}
}
}
}
def Scope "Joints"
{
}
def Scope "StaticGeometry"
{
def Cube "plane" (
# NOTE: "MaterialBindingAPI" is required for the material binding to work.
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI", "MaterialBindingAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (1.0, 1.0, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, -0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
color3f[] primvars:displayColor = [(0.8, 0.8, 0.8)]
}
}
def Scope "Meshes"
{
}
def Scope "Materials"
{
# NOTE: This provides an example for how to override the default material.
def Material "Default" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
customData = {bool overrideDefault = 1}
)
{
# Color Properties
color3f inputs:diffuseColor = (0.5, 0.5, 0.5)
float inputs:roughness = 0.0
float metallic = 0.0
# Intrinsic Properties
double physics:density = 0.0
# Extrinsics Properties
float physics:restitution = 0.0
float physics:staticFriction = 0.9
float physics:dynamicFriction = 0.9
}
def Material "Steel" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
)
{
# Color Properties
color3f inputs:diffuseColor = (0.95, 0.95, 0.95)
float inputs:roughness = 0.0
float metallic = 1.0
# Intrinsic Properties
double physics:density = 7850.0
# Extrinsics Properties
float physics:restitution = 0.56
float physics:staticFriction = 0.78
float physics:dynamicFriction = 0.42
}
def Material "Concrete" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
)
{
# Color Properties
color3f inputs:diffuseColor = (0.8, 0.8, 0.8)
float inputs:roughness = 0.1
float metallic = 0.0
# Intrinsic Properties
double physics:density = 2500.0
# Extrinsics Properties
float physics:restitution = 0.0
float physics:staticFriction = 0.5
float physics:dynamicFriction = 0.5
}
}
def PhysicsCollisionGroup "Collisions"
{
rel collection:colliders:includes = [
,
,
]
rel physics:filteredGroups =
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/basics/box_pendulum.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "box_pendulum"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "box_pendulum"
{
def Scope "RigidBodies"
{
def Xform "body" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.25, 0.0, 0.75)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (0.0016666667070239782, 0.021666666492819786, 0.021666666492819786)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box" (
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.25, 0.05, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
def Scope "Joints"
{
def PhysicsRevoluteJoint "grounding" (
prepend apiSchemas = ["PhysicsDriveAPI:angular"]
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
uniform token physics:axis = "Y"
point3f physics:localPos0 = (0.0, 0.0, 0.75)
point3f physics:localPos1 = (-0.25, 0.0, 0.0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 1
float physics:lowerLimit = -inf
float physics:upperLimit = inf
float drive:angular:physics:maxForce = inf
float drive:angular:physics:targetPosition = 0.0
float drive:angular:physics:targetVelocity = 0.0
float drive:angular:physics:stiffness = 0.0
float drive:angular:physics:damping = 0.0
uniform token drive:angular:physics:type = "force"
}
}
def Scope "StaticGeometry"
{
def Cube "plane" (
# NOTE: "MaterialBindingAPI" is required for the material binding to work.
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI", "MaterialBindingAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (1.0, 1.0, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, -0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
color3f[] primvars:displayColor = [(0.8, 0.8, 0.8)]
}
}
def Scope "Meshes"
{
}
def Scope "Materials"
{
}
def PhysicsCollisionGroup "Collisions"
{
rel collection:colliders:includes = [
,
]
rel physics:filteredGroups =
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/basics/boxes_fourbar.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "boxes_fourbar"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "boxes_fourbar"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0, 0.0, 0.004999999888241291)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
point3f physics:centerOfMass = (0, 0, 0)
float3 physics:diagonalInertia = (1.6666666851961054e-05, 0.0008416666532866657, 0.0008416666532866657)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box_1" (
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.05, 0.005, 0.005)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
def Xform "body_2" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0560000017285347, 0.0, 0.061000000685453415)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
point3f physics:centerOfMass = (0, 0, 0)
float3 physics:diagonalInertia = (0.0008416666532866657, 0.0008416666532866657, 1.6666666851961054e-05)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box_2" (
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.005, 0.005, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
def Xform "body_3" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0, 0.0, 0.11700000613927841)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
point3f physics:centerOfMass = (0, 0, 0)
float3 physics:diagonalInertia = (1.6666666851961054e-05, 0.0008416666532866657, 0.0008416666532866657)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box_3" (
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.05, 0.005, 0.005)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
def Xform "body_4" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0560000017285347, 0.0, 0.061000000685453415)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
point3f physics:centerOfMass = (0, 0, 0)
float3 physics:diagonalInertia = (0.0008416666532866657, 0.0008416666532866657, 1.6666666851961054e-05)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box_4" (
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.005, 0.005, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
def Scope "Joints"
{
def PhysicsRevoluteJoint "joint_1" (
prepend apiSchemas = ["PhysicsDriveAPI:angular"]
)
{
uniform token physics:axis = "Y"
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0560000017285347, 0.0, 0.0)
point3f physics:localPos1 = (0.0, 0.0, -0.0560000017285347)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
float physics:lowerLimit = -45.0
float physics:upperLimit = 45
float drive:angular:physics:maxForce = inf
float drive:angular:physics:stiffness = 17.453292 # ~1000.0 in Nm/rad
float drive:angular:physics:damping = 0.3490658 # ~20.0 in Nms/rad
uniform token drive:angular:physics:type = "force"
}
def PhysicsRevoluteJoint "joint_2"
{
uniform token physics:axis = "Y"
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0, 0.0, 0.056000005453825)
point3f physics:localPos1 = (0.0560000017285347, 0.0, 0.0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
float physics:lowerLimit = -45.0
float physics:upperLimit = 45
}
def PhysicsRevoluteJoint "joint_3" (
prepend apiSchemas = ["PhysicsDriveAPI:angular"]
)
{
uniform token physics:axis = "Y"
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (-0.0560000017285347, 0.0, 0.0)
point3f physics:localPos1 = (0.0, 0.0, 0.056000005453825)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
float physics:lowerLimit = -45.0
float physics:upperLimit = 45
float drive:angular:physics:maxForce = inf
uniform token drive:angular:physics:type = "force"
}
def PhysicsRevoluteJoint "joint_4"
{
uniform token physics:axis = "Y"
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0, 0.0, -0.0560000017285347)
point3f physics:localPos1 = (-0.0560000017285347, 0.0, 0.0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
float physics:lowerLimit = -45.0
float physics:upperLimit = 45
}
}
def Scope "StaticGeometry"
{
def Cube "plane" (
# NOTE: "MaterialBindingAPI" is required for the material binding to work.
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI", "MaterialBindingAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.5, 0.5, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, -0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
color3f[] primvars:displayColor = [(0.8, 0.8, 0.8)]
}
}
def Scope "Meshes"
{
}
def Scope "Materials"
{
}
def PhysicsCollisionGroup "Collisions"
{
rel collection:colliders:includes = [
,
,
,
,
]
rel physics:filteredGroups =
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/basics/boxes_hinged.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "boxes_hinged"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "boxes_hinged"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.25, -0.05, 0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (0.0016666667070239782, 0.021666666492819786, 0.021666666492819786)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Scope "Primary"
{
def Cube "box_1" (
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI", "MaterialBindingAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.25, 0.05, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
}
}
}
}
def Xform "body_2" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.75, 0.05, 0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (0.0016666667070239782, 0.021666666492819786, 0.021666666492819786)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Scope "Primary"
{
def Cube "box_2" (
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI", "MaterialBindingAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.25, 0.05, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
}
}
}
}
}
def Scope "Joints"
{
def PhysicsRevoluteJoint "joint_1" (
prepend apiSchemas = ["PhysicsDriveAPI:angular"]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
uniform token physics:axis = "Y"
point3f physics:localPos0 = (0.25, 0.05, 0.0)
point3f physics:localPos1 = (-0.25, -0.05, 0.0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
float physics:lowerLimit = -inf
float physics:upperLimit = inf
float drive:angular:physics:maxForce = inf
float drive:angular:physics:targetPosition = 0.0
float drive:angular:physics:targetVelocity = 0.0
float drive:angular:physics:stiffness = 0.0
float drive:angular:physics:damping = 0.0
uniform token drive:angular:physics:type = "force"
}
}
def Scope "StaticGeometry"
{
def Cube "plane" (
# NOTE: "MaterialBindingAPI" is required for the material binding to work.
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI", "MaterialBindingAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (2.0, 2.0, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, -0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
color3f[] primvars:displayColor = [(0.8, 0.8, 0.8)]
}
}
def Scope "Meshes"
{
}
def Scope "Materials"
{
# NOTE: This provides an example for how to override the default material.
def Material "Default" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
customData = {bool overrideDefault = 1}
)
{
# Color Properties
color3f inputs:diffuseColor = (0.5, 0.5, 0.5)
float inputs:roughness = 0.0
float metallic = 0.0
# Intrinsic Properties
double physics:density = 0.0
# Extrinsics Properties
float physics:restitution = 0.0
float physics:staticFriction = 0.9
float physics:dynamicFriction = 0.9
}
def Material "Steel" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
)
{
# Color Properties
color3f inputs:diffuseColor = (0.95, 0.95, 0.95)
float inputs:roughness = 0.0
float metallic = 1.0
# Intrinsic Properties
double physics:density = 7850.0
# Extrinsics Properties
float physics:restitution = 0.56
float physics:staticFriction = 0.78
float physics:dynamicFriction = 0.42
}
def Material "Concrete" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
)
{
# Color Properties
color3f inputs:diffuseColor = (0.8, 0.8, 0.8)
float inputs:roughness = 0.1
float metallic = 0.0
# Intrinsic Properties
double physics:density = 2500.0
# Extrinsics Properties
float physics:restitution = 0.0
float physics:staticFriction = 0.5
float physics:dynamicFriction = 0.5
}
}
def PhysicsCollisionGroup "Collisions"
{
rel collection:colliders:includes = [
,
,
,
]
rel physics:filteredGroups =
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/basics/boxes_nunchaku.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "boxes_nunchaku"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "boxes_nunchaku"
{
# Model Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.25, 0.0, 0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (0.0016666667070239782, 0.021666666492819786, 0.021666666492819786)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box_1" (
prepend apiSchemas = [
"PhysicsCollisionAPI",
"PhysicsMeshCollisionAPI",
"PhysicsFilteredPairsAPI",
"MaterialBindingAPI",
]
)
{
color3f[] primvars:displayColor = [(0.2784314, 0.4117647, 1)]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.25, 0.05, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
def Xform "body_2" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.55, 0.0, 0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (0.001, 0.001, 0.001)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Sphere "sphere_2" (
prepend apiSchemas = [
"PhysicsCollisionAPI",
"PhysicsMeshCollisionAPI",
"PhysicsFilteredPairsAPI",
"MaterialBindingAPI",
]
)
{
color3f[] primvars:displayColor = [(0.2784314, 0.4117647, 1)]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
double radius = 0.05
float3 xformOp:scale = (1.0, 1.0, 1.0)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
def Xform "body_3" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.85, 0.0, 0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (0.0016666667070239782, 0.021666666492819786, 0.021666666492819786)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box_3" (
prepend apiSchemas = [
"PhysicsCollisionAPI",
"PhysicsMeshCollisionAPI",
"PhysicsFilteredPairsAPI",
"MaterialBindingAPI",
]
)
{
color3f[] primvars:displayColor = [(0.2784314, 0.4117647, 1)]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.25, 0.05, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
def Scope "Joints"
{
def PhysicsSphericalJoint "joint_1"
{
uniform token physics:axis = "X"
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.25, 0.0, 0.0)
point3f physics:localPos1 = (-0.05, 0.0, 0.0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
float3 physics:lowerLimit = (-inf, -inf, -inf)
float3 physics:upperLimit = (inf, inf, inf)
}
def PhysicsSphericalJoint "joint_2"
{
uniform token physics:axis = "X"
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.05, 0.0, 0.0)
point3f physics:localPos1 = (-0.25, 0.0, 0.0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
float3 physics:lowerLimit = (-inf, -inf, -inf)
float3 physics:upperLimit = (inf, inf, inf)
}
}
def Scope "StaticGeometry"
{
def Cube "plane" (
# NOTE: "MaterialBindingAPI" is required for the material binding to work.
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI", "MaterialBindingAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (2.0, 2.0, 0.05)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, -0.05)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
rel material:binding:physics = (
bindMaterialAs = "weakerThanDescendants"
)
color3f[] primvars:displayColor = [(0.8, 0.8, 0.8)]
}
}
def Scope "Meshes"
{
}
def Scope "Materials"
{
# NOTE: This provides an example for how to override the default material.
def Material "Default" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
customData = {bool overrideDefault = 1}
)
{
# Color Properties
color3f inputs:diffuseColor = (0.5, 0.5, 0.5)
float inputs:roughness = 0.0
float metallic = 0.0
# Intrinsic Properties
double physics:density = 0.0
# Extrinsics Properties
float physics:restitution = 0.0
float physics:staticFriction = 0.9
float physics:dynamicFriction = 0.9
}
def Material "Steel" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
)
{
# Color Properties
color3f inputs:diffuseColor = (0.95, 0.95, 0.95)
float inputs:roughness = 0.0
float metallic = 1.0
# Intrinsic Properties
double physics:density = 7850.0
# Extrinsics Properties
float physics:restitution = 0.56
float physics:staticFriction = 0.78
float physics:dynamicFriction = 0.42
}
def Material "Concrete" (
prepend apiSchemas = ["PhysicsMaterialAPI"]
)
{
# Color Properties
color3f inputs:diffuseColor = (0.8, 0.8, 0.8)
float inputs:roughness = 0.1
float metallic = 0.0
# Intrinsic Properties
double physics:density = 2500.0
# Extrinsics Properties
float physics:restitution = 0.0
float physics:staticFriction = 0.5
float physics:dynamicFriction = 0.5
}
}
def Scope "Collisions"
{
def PhysicsCollisionGroup "World"
{
rel collection:colliders:includes = [
,
]
}
def PhysicsCollisionGroup "Body1"
{
rel collection:colliders:includes = [
,
,
]
}
def PhysicsCollisionGroup "Body2"
{
rel collection:colliders:includes = [
,
,
]
}
def PhysicsCollisionGroup "Body3"
{
rel collection:colliders:includes = [
,
,
]
}
def PhysicsCollisionGroup "Internal"
{
rel collection:colliders:includes = [
,
,
]
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/basics/cartpole.usda
================================================
#usda 1.0
(
doc = """Generated by Disney Research for testing purposes."""
defaultPrim = "cartpole"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "cartpole"
{
# Model Frame
quatd xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
def Scope "RigidBodies"
{
def Xform "cart" (
apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatd xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
float3 physics:diagonalInertia = (0.024166666, 0.006666667, 0.024166666)
def Scope "Geometry"
{
def Cube "box" (
apiSchemas = ["PhysicsCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
quatd xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
double3 xformOp:scale = (0.1, 0.25, 0.1)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
def Xform "pole" (
apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatd xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.125, 0, 0.375)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.2
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
float3 physics:diagonalInertia = (0.009416667, 0.009416667, 0.00008333333)
def Scope "Geometry"
{
def Cube "box" (
apiSchemas = ["PhysicsCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
quatd xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
double3 xformOp:scale = (0.025, 0.025, 0.375)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
def Scope "Joints"
{
def PhysicsPrismaticJoint "rail_to_cart" (
apiSchemas = ["PhysicsDriveAPI:linear"]
)
{
bool physics:jointEnabled = 1
bool physics:collisionEnabled = 0
float drive:linear:physics:maxForce = 1000
uniform token drive:linear:physics:type = "force"
uniform token physics:axis = "Y"
rel physics:body1 =
float physics:breakForce = 3.4028235e38
float physics:breakTorque = 3.4028235e38
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (0, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
float physics:lowerLimit = -4
float physics:upperLimit = 4
}
def PhysicsRevoluteJoint "cart_to_pole"
{
bool physics:jointEnabled = 1
bool physics:collisionEnabled = 0
uniform token physics:axis = "X"
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.1, 0, 0)
point3f physics:localPos1 = (-0.025, 0, -0.375)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
float physics:lowerLimit = -inf
float physics:upperLimit = inf
}
}
def Scope "Meshes"
{
}
def Scope "Materials"
{
}
def Scope "StaticGeometry"
{
def Cube "rail" (
apiSchemas = ["PhysicsCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
quatd xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
double3 xformOp:scale = (0.015, 4.0, 0.015)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
def Scope "Collisions"
{
def PhysicsCollisionGroup "Rail"
{
rel collection:colliders:includes = [
,
]
}
def PhysicsCollisionGroup "Cart"
{
rel collection:colliders:includes = [
,
]
}
def PhysicsCollisionGroup "Pole"
{
rel collection:colliders:includes = [
,
]
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/geoms/test_geom_box.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_geom_box"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_geom_box"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
# Body Geometry
def Scope "Geometry"
{
def Cube "collision_box" (
# NOTE: We add these only for collidable geometries
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
customData = {int maxContacts = 10}
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.11, 0.22, 0.33)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
def Cube "visual_box"
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.111, 0.222, 0.333)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/geoms/test_geom_capsule.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_geom_capsule"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_geom_capsule"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
# Body Geometry
def Scope "Geometry"
{
def Capsule "collision_capsule" (
# NOTE: We add these only for collidable geometries
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
customData = {int maxContacts = 10}
)
{
double radius = 0.1
double height = 2.2
float3 xformOp:scale = (1, 1, 1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
def Capsule "visual_capsule"
{
double radius = 0.2
double height = 3.3
float3 xformOp:scale = (1, 1, 1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/geoms/test_geom_cone.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_geom_cone"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_geom_cone"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
# Body Geometry
def Scope "Geometry"
{
def Cone "collision_cone" (
# NOTE: We add these only for collidable geometries
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
customData = {int maxContacts = 10}
)
{
double radius = 0.1
double height = 2.2
float3 xformOp:scale = (1, 1, 1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
def Cone "visual_cone"
{
double radius = 0.2
double height = 3.3
float3 xformOp:scale = (1, 1, 1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/geoms/test_geom_cylinder.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_geom_cylinder"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_geom_cylinder"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
# Body Geometry
def Scope "Geometry"
{
def Cylinder "collision_cylinder" (
# NOTE: We add these only for collidable geometries
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
customData = {int maxContacts = 10}
)
{
double radius = 0.1
double height = 2.2
float3 xformOp:scale = (1, 1, 1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
def Cylinder "visual_cylinder"
{
double radius = 0.2
double height = 3.3
float3 xformOp:scale = (1, 1, 1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/geoms/test_geom_ellipsoid.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_geom_ellipsoid"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_geom_ellipsoid"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
# Body Geometry
def Scope "Geometry"
{
def Sphere "collision_ellipsoid" (
# NOTE: We add these only for collidable geometries
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
customData = {int maxContacts = 10}
)
{
double radius = 1.0
float3 xformOp:scale = (0.11, 0.22, 0.33)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
def Sphere "visual_ellipsoid"
{
double radius = 1.0
float3 xformOp:scale = (0.22, 0.33, 0.44)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/geoms/test_geom_sphere.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_geom_sphere"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_geom_sphere"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
# Body Geometry
def Scope "Geometry"
{
def Sphere "collision_sphere" (
# NOTE: We add these only for collidable geometries
prepend apiSchemas = ["PhysicsCollisionAPI", "PhysicsMeshCollisionAPI"]
customData = {int maxContacts = 10}
)
{
double radius = 0.11
float3 xformOp:scale = (1, 1, 1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
def Sphere "visual_sphere"
{
double radius = 0.22
float3 xformOp:scale = (1, 1, 1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_cartesian_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_cartesian_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_cartesian_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0456378, -0.00627737, -0.00975077)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.192816
float3 physics:diagonalInertia = (1.29682e-05, 7.14078e-05, 7.18623e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.891551, -0.241196, -0.380002, -0.0505894)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0277918, -0.0131508, 0.0258479)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.192816
float3 physics:diagonalInertia = (1.29682e-05, 7.14078e-05, 7.18623e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.891551, -0.241196, -0.380002, -0.0505894)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_cartesian_joint" (
customData = {string dofs = "cartesian"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:transX",
"PhysicsDriveAPI:transY",
"PhysicsDriveAPI:transZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0350228, 0.00638189, 0.0334306)
point3f physics:localPos1 = (-0.0384068, 0.0132553, -0.00216804)
quatf physics:localRot0 = (-3.99312e-06, 0.904622, 0.158661, -0.395583)
quatf physics:localRot1 = (-3.99312e-06, 0.904622, 0.158661, -0.395583)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cartesian joint
float limit:transX:physics:low = -10
float limit:transX:physics:high = 10
# transY is the second DoF of the cartesian joint
float limit:transY:physics:low = -20
float limit:transY:physics:high = 20
# transZ is the third DoF of the cartesian joint
float limit:transZ:physics:low = -30
float limit:transZ:physics:high = 30
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:transX:physics:maxForce = 100.0
uniform token drive:transX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:transY:physics:maxForce = 200.0
uniform token drive:transY:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the third DoF
float drive:transZ:physics:maxForce = 300.0
uniform token drive:transZ:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_cartesian_actuated_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_cartesian_actuated_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_cartesian_actuated_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0277918, -0.0131508, 0.0258479)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.192816
float3 physics:diagonalInertia = (1.29682e-05, 7.14078e-05, 7.18623e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.891551, -0.241196, -0.380002, -0.0505894)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_cartesian_joint" (
customData = {string dofs = "cartesian"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:transX",
"PhysicsDriveAPI:transY",
"PhysicsDriveAPI:transZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
point3f physics:localPos0 = (-0.0106149, 0.000104522, 0.0236799)
point3f physics:localPos1 = (-0.0384068, 0.0132553, -0.00216804)
quatf physics:localRot0 = (-3.99312e-06, 0.904622, 0.158661, -0.395583)
quatf physics:localRot1 = (-3.99312e-06, 0.904622, 0.158661, -0.395583)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cartesian joint
float limit:transX:physics:low = -10
float limit:transX:physics:high = 10
# transY is the second DoF of the cartesian joint
float limit:transY:physics:low = -20
float limit:transY:physics:high = 20
# transZ is the third DoF of the cartesian joint
float limit:transZ:physics:low = -30
float limit:transZ:physics:high = 30
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:transX:physics:maxForce = 100.0
uniform token drive:transX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:transY:physics:maxForce = 200.0
uniform token drive:transY:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the third DoF
float drive:transZ:physics:maxForce = 300.0
uniform token drive:transZ:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_cartesian_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_cartesian_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_cartesian_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0456378, -0.00627737, -0.00975077)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.192816
float3 physics:diagonalInertia = (1.29682e-05, 7.14078e-05, 7.18623e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.891551, -0.241196, -0.380002, -0.0505894)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0277918, -0.0131508, 0.0258479)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.192816
float3 physics:diagonalInertia = (1.29682e-05, 7.14078e-05, 7.18623e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.891551, -0.241196, -0.380002, -0.0505894)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_cartesian_joint" (
customData = {string dofs = "cartesian"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0350228, 0.00638189, 0.0334306)
point3f physics:localPos1 = (-0.0384068, 0.0132553, -0.00216804)
quatf physics:localRot0 = (-3.99312e-06, 0.904622, 0.158661, -0.395583)
quatf physics:localRot1 = (-3.99312e-06, 0.904622, 0.158661, -0.395583)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cartesian joint
float limit:transX:physics:low = -10
float limit:transX:physics:high = 10
# transY is the second DoF of the cartesian joint
float limit:transY:physics:low = -20
float limit:transY:physics:high = 20
# transZ is the third DoF of the cartesian joint
float limit:transZ:physics:low = -30
float limit:transZ:physics:high = 30
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_cartesian_passive_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_cartesian_passive_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_cartesian_passive_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0277918, -0.0131508, 0.0258479)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.192816
float3 physics:diagonalInertia = (1.29682e-05, 7.14078e-05, 7.18623e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.891551, -0.241196, -0.380002, -0.0505894)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_cartesian_joint" (
customData = {string dofs = "cartesian"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
point3f physics:localPos0 = (-0.0106149, 0.000104522, 0.0236799)
point3f physics:localPos1 = (-0.0384068, 0.0132553, -0.00216804)
quatf physics:localRot0 = (-3.99312e-06, 0.904622, 0.158661, -0.395583)
quatf physics:localRot1 = (-3.99312e-06, 0.904622, 0.158661, -0.395583)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cartesian joint
float limit:transX:physics:low = -10
float limit:transX:physics:high = 10
# transY is the second DoF of the cartesian joint
float limit:transY:physics:low = -20
float limit:transY:physics:high = 20
# transZ is the third DoF of the cartesian joint
float limit:transZ:physics:low = -30
float limit:transZ:physics:high = 30
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_cylindrical_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_cylindrical_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_cylindrical_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0942732, -0.0140893, 0.0190708)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.239474
float3 physics:diagonalInertia = (1.99916e-05, 0.000149804, 0.000154583)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.707106, -0.707106, -0.000926471, -0.000926471)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0206314, -0.0396436, -0.000402213)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.320133, -0.029964, 0.010131, 0.946845)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_cylindrical_joint" (
customData = {string dofs = "cylindrical"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:transX",
"PhysicsDriveAPI:rotX",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0401714, 0, -0.000170496)
point3f physics:localPos1 = (-0.0334704, 0.0255543, 0.0193026)
quatf physics:localRot0 = (-0.5, 0.5, -0.5, -0.5)
quatf physics:localRot1 = (-0.5, 0.5, -0.5, -0.5)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cylindrical joint
float limit:transX:physics:low = -1
float limit:transX:physics:high = 1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the second DoF of the cylindrical joint
float limit:rotX:physics:low = -inf
float limit:rotX:physics:high = inf
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:transX:physics:maxForce = 100.0
uniform token drive:transX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:rotX:physics:maxForce = 200.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_cylindrical_actuated_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_cylindrical_actuated_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_cylindrical_actuated_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0206314, -0.0396436, -0.000402213)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.320133, -0.029964, 0.010131, 0.946845)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_cylindrical_joint" (
customData = {string dofs = "cylindrical"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:transX",
"PhysicsDriveAPI:rotX",
]
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
point3f physics:localPos0 = (-0.0541018, -0.0140893, 0.0189004)
point3f physics:localPos1 = (-0.0334704, 0.0255543, 0.0193026)
quatf physics:localRot0 = (-0.5, 0.5, -0.5, -0.5)
quatf physics:localRot1 = (-0.5, 0.5, -0.5, -0.5)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cylindrical joint
float limit:transX:physics:low = -1
float limit:transX:physics:high = 1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the second DoF of the cylindrical joint
float limit:rotX:physics:low = -inf
float limit:rotX:physics:high = inf
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:transX:physics:maxForce = 100.0
uniform token drive:transX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:rotX:physics:maxForce = 200.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_cylindrical_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_cylindrical_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_cylindrical_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0942732, -0.0140893, 0.0190708)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.239474
float3 physics:diagonalInertia = (1.99916e-05, 0.000149804, 0.000154583)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.707106, -0.707106, -0.000926471, -0.000926471)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0206314, -0.0396436, -0.000402213)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.320133, -0.029964, 0.010131, 0.946845)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_cylindrical_joint" (
customData = {string dofs = "cylindrical"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0401714, 0, -0.000170496)
point3f physics:localPos1 = (-0.0334704, 0.0255543, 0.0193026)
quatf physics:localRot0 = (-0.5, 0.5, -0.5, -0.5)
quatf physics:localRot1 = (-0.5, 0.5, -0.5, -0.5)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cylindrical joint
float limit:transX:physics:low = -1
float limit:transX:physics:high = 1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the second DoF of the cylindrical joint
float limit:rotX:physics:low = -inf
float limit:rotX:physics:high = inf
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_cylindrical_passive_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_cylindrical_passive_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_cylindrical_passive_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0206314, -0.0396436, -0.000402213)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.320133, -0.029964, 0.010131, 0.946845)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_cylindrical_joint" (
customData = {string dofs = "cylindrical"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
point3f physics:localPos0 = (-0.0541018, -0.0140893, 0.0189004)
point3f physics:localPos1 = (-0.0334704, 0.0255543, 0.0193026)
quatf physics:localRot0 = (-0.5, 0.5, -0.5, -0.5)
quatf physics:localRot1 = (-0.5, 0.5, -0.5, -0.5)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cylindrical joint
float limit:transX:physics:low = -1
float limit:transX:physics:high = 1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the second DoF of the cylindrical joint
float limit:rotX:physics:low = -inf
float limit:rotX:physics:high = inf
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_6dof_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_6d_6dof_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_6d_6dof_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_d6_6dof_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:transX",
"PhysicsDriveAPI:transY",
"PhysicsDriveAPI:transZ",
"PhysicsDriveAPI:rotX",
"PhysicsDriveAPI:rotY",
"PhysicsDriveAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
float limit:transX:physics:low = -1
float limit:transX:physics:high = 1
float limit:transY:physics:low = 2
float limit:transY:physics:high = -2
float limit:transZ:physics:low = -3
float limit:transZ:physics:high = 3
float limit:rotX:physics:low = -40
float limit:rotX:physics:high = 40
float limit:rotY:physics:low = -50
float limit:rotY:physics:high = 50
float limit:rotZ:physics:low = -60
float limit:rotZ:physics:high = 60
float drive:transX:physics:maxForce = 100.0
float drive:transX:physics:targetPosition = -1.0
float drive:transX:physics:targetVelocity = 1.0
float drive:transX:physics:stiffness = 10.0
float drive:transX:physics:damping = 1.0
uniform token drive:transX:physics:type = "force" # can be in {"force", "acceleration"}
float drive:transY:physics:maxForce = 100.0
float drive:transY:physics:targetPosition = -1.0
float drive:transY:physics:targetVelocity = 1.0
float drive:transY:physics:stiffness = 10.0
float drive:transY:physics:damping = 1.0
uniform token drive:transY:physics:type = "force" # can be in {"force", "acceleration"}
float drive:transZ:physics:maxForce = 100.0
float drive:transZ:physics:targetPosition = -1.0
float drive:transZ:physics:targetVelocity = 1.0
float drive:transZ:physics:stiffness = 10.0
float drive:transZ:physics:damping = 1.0
uniform token drive:transZ:physics:type = "force" # can be in {"force", "acceleration"}
float drive:rotX:physics:maxForce = 100.0
float drive:rotX:physics:targetPosition = -1.0
float drive:rotX:physics:targetVelocity = 1.0
float drive:rotX:physics:stiffness = 10.0
float drive:rotX:physics:damping = 1.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
float drive:rotX:physics:maxForce = 100.0
float drive:rotX:physics:targetPosition = -1.0
float drive:rotX:physics:targetVelocity = 1.0
float drive:rotX:physics:stiffness = 10.0
float drive:rotX:physics:damping = 1.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_6dof_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_6d_6dof_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_6d_6dof_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_d6_6dof_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
float limit:transX:physics:low = -1
float limit:transX:physics:high = 1
float limit:transY:physics:low = 2
float limit:transY:physics:high = -2
float limit:transZ:physics:low = -3
float limit:transZ:physics:high = 3
float limit:rotX:physics:low = -40
float limit:rotX:physics:high = 40
float limit:rotY:physics:low = -50
float limit:rotY:physics:high = 50
float limit:rotZ:physics:low = -60
float limit:rotZ:physics:high = 60
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_cartesian_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_cartesian_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_cartesian_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_d6_cartesian_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:transX",
"PhysicsDriveAPI:transY",
"PhysicsDriveAPI:transZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cartesian joint
float limit:transX:physics:low = -10
float limit:transX:physics:high = 10
# transY is the second DoF of the cartesian joint
float limit:transY:physics:low = -20
float limit:transY:physics:high = 20
# transZ is the third DoF of the cartesian joint
float limit:transZ:physics:low = -30
float limit:transZ:physics:high = 30
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:transX:physics:maxForce = 100.0
uniform token drive:transX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:transY:physics:maxForce = 200.0
uniform token drive:transY:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the third DoF
float drive:transZ:physics:maxForce = 300.0
uniform token drive:transZ:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_cartesian_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_cartesian_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_cartesian_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_d6_cartesian_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cartesian joint
float limit:transX:physics:low = -10
float limit:transX:physics:high = 10
# transY is the second DoF of the cartesian joint
float limit:transY:physics:low = -20
float limit:transY:physics:high = 20
# transZ is the third DoF of the cartesian joint
float limit:transZ:physics:low = -30
float limit:transZ:physics:high = 30
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_cylindrical_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_cylindrical_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_cylindrical_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_d6_cylindrical_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:transX",
"PhysicsDriveAPI:rotX",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cylindrical joint
float limit:transX:physics:low = -1
float limit:transX:physics:high = 1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the second DoF of the cylindrical joint
float limit:rotX:physics:low = -inf
float limit:rotX:physics:high = inf
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:transX:physics:maxForce = 100.0
uniform token drive:transX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:rotX:physics:maxForce = 200.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_cylindrical_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_cylindrical_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_cylindrical_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_d6_cylindrical_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# transX is the first DoF of the cylindrical joint
float limit:transX:physics:low = -1
float limit:transX:physics:high = 1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the second DoF of the cylindrical joint
float limit:rotX:physics:low = -inf
float limit:rotX:physics:high = inf
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_prismatic_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_prismatic_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_prismatic_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_d6_prismatic_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:transX",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# transX is the DoF of the prismatic joint
float limit:transX:physics:low = -10
float limit:transX:physics:high = 10
# low > high makes rotX a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the transX DoF
float drive:transX:physics:maxForce = 100.0
uniform token drive:transX:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_prismatic_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_prismatic_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_prismatic_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_d6_prismatic_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# transX is the DoF of the prismatic joint
float limit:transX:physics:low = -10
float limit:transX:physics:high = 10
# low > high makes rotX a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# low > high makes rotY a constraint axis
float limit:rotY:physics:low = 1
float limit:rotY:physics:high = -1
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_revolute_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_revolute_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_revolute_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_d6_revolute_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:rotY",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# rotY is the DoF of the revolute joint
float limit:rotY:physics:low = -180
float limit:rotY:physics:high = 180
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the rotY DoF
float drive:rotY:physics:maxForce = 100.0
uniform token drive:rotY:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_revolute_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_revolute_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_revolute_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_d6_revolute_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# low > high makes rotX a constraint axis
float limit:rotX:physics:low = 1
float limit:rotX:physics:high = -1
# rotY is the DoF of the revolute joint
float limit:rotY:physics:low = -180
float limit:rotY:physics:high = 180
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_spherical_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_spherical_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_spherical_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_d6_spherical_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:rotX",
"PhysicsDriveAPI:rotY",
"PhysicsDriveAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the first DoF of the spherical joint
float limit:rotX:physics:low = -180
float limit:rotX:physics:high = 180
# rotY is the second DoF of the spherical joint
float limit:rotY:physics:low = -180
float limit:rotY:physics:high = 180
# rotZ is the third DoF of the spherical joint
float limit:rotZ:physics:low = -180
float limit:rotZ:physics:high = 180
# Drive of the rotX DoF
float drive:rotX:physics:maxForce = 100.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the rotY DoF
float drive:rotY:physics:maxForce = 200.0
uniform token drive:rotY:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the rotZ DoF
float drive:rotZ:physics:maxForce = 300.0
uniform token drive:rotZ:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_spherical_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_spherical_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_spherical_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_d6_spherical_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes rotX a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the first DoF of the spherical joint
float limit:rotX:physics:low = -180
float limit:rotX:physics:high = 180
# rotY is the second DoF of the spherical joint
float limit:rotY:physics:low = -180
float limit:rotY:physics:high = 180
# rotZ is the third DoF of the spherical joint
float limit:rotZ:physics:low = -180
float limit:rotZ:physics:high = 180
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_universal_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_universal_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_universal_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_d6_universal_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:rotX",
"PhysicsDriveAPI:rotY",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the first DoF of the universal joint
float limit:rotX:physics:low = -90
float limit:rotX:physics:high = 90
# rotY is the second DoF of the universal joint
float limit:rotY:physics:low = -90
float limit:rotY:physics:high = 90
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:rotX:physics:maxForce = 100.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:rotY:physics:maxForce = 200.0
uniform token drive:rotY:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_d6_universal_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_d6_universal_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_d6_universal_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (1, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (1, 1, 1)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_d6_universal_joint" (
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0, 0, 0)
point3f physics:localPos1 = (-0.5, 0, 0)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the first DoF of the universal joint
float limit:rotX:physics:low = -90
float limit:rotX:physics:high = 90
# rotY is the second DoF of the universal joint
float limit:rotY:physics:low = -90
float limit:rotY:physics:high = 90
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_fixed.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_fixed"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_fixed"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.05, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.1872
float3 physics:diagonalInertia = (1.248e-05, 6.24e-05, 6.24e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.859903, -0.281874, -0.404403, 0.132562)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.00889736, 0.0229802, -0.0126245)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.1872
float3 physics:diagonalInertia = (1.248e-05, 6.24e-05, 6.24e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.797479, 0.546428, 0.144599, -0.211034)
}
}
def Scope "Joints"
{
def PhysicsFixedJoint "passive_fixed_joint"
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
uniform token physics:axis = "X"
point3f physics:localPos0 = (0.0318885, 0.0227982, 0.0310381)
point3f physics:localPos1 = (-0.0270088, -0.00018202, 0.0436626)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_fixed_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_fixed_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_fixed_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.00889736, 0.0229802, -0.0126245)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.1872
float3 physics:diagonalInertia = (1.248e-05, 6.24e-05, 6.24e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.797479, 0.546428, 0.144599, -0.211034)
}
}
def Scope "Joints"
{
def PhysicsFixedJoint "passive_fixed_joint"
{
bool physics:jointEnabled = 1
rel physics:body1 =
uniform token physics:axis = "X"
point3f physics:localPos0 = (-0.0181115, 0.0227982, 0.0310381)
point3f physics:localPos1 = (-0.0270088, -0.00018202, 0.0436626)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_prismatic_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_prismatic_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_prismatic_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0155601, -0.0848459, -0.00574446)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.2262
float3 physics:diagonalInertia = (1.44825e-05, 0.000123861, 0.000124459)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.181822, -0.340445, 0.916765, -0.102857)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0413202, -0.0281047, 0.0230962)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.2262
float3 physics:diagonalInertia = (1.44825e-05, 0.000123861, 0.000124459)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.181822, -0.340445, 0.916765, -0.102857)
}
}
def Scope "Joints"
{
def PhysicsPrismaticJoint "actuated_prismatic_joint" (
prepend apiSchemas = ["PhysicsDriveAPI:linear"] # NOTE: This enables the UsdPhysics.JointDrive(...) attribute
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
uniform token physics:axis = "X"
point3f physics:localPos0 = (0.0284401, 0.0283706, 0.0144203)
point3f physics:localPos1 = (-0.0284401, -0.0283706, -0.0144203)
quatf physics:localRot0 = (0.910634, -0.0921648, -0.21039, 0.343494)
quatf physics:localRot1 = (0.910634, -0.0921648, -0.21039, 0.343494)
float physics:lowerLimit = -1 # NOTE: The presence of these properties enables the UsdPhysics.JointLimit(...) attribute
float physics:upperLimit = 1
bool physics:collisionEnabled = 0
float drive:linear:physics:maxForce = 100.0
float drive:linear:physics:targetPosition = -1.0
float drive:linear:physics:targetVelocity = 1.0
float drive:linear:physics:stiffness = 10.0
float drive:linear:physics:damping = 1.0
uniform token drive:linear:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_prismatic_actuated_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_prismatic_actuated_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_prismatic_actuated_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0413202, -0.0281047, 0.0230962)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.2262
float3 physics:diagonalInertia = (1.44825e-05, 0.000123861, 0.000124459)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.181822, -0.340445, 0.916765, -0.102857)
}
}
def Scope "Joints"
{
def PhysicsPrismaticJoint "actuated_prismatic_joint" (
prepend apiSchemas = ["PhysicsDriveAPI:linear"] # NOTE: This enables the UsdPhysics.JointDrive(...) attribute
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
uniform token physics:axis = "X"
point3f physics:localPos0 = (0.0128801, -0.0564753, 0.00867585)
point3f physics:localPos1 = (-0.0284401, -0.0283706, -0.0144203)
quatf physics:localRot0 = (0.910634, -0.0921648, -0.21039, 0.343494)
quatf physics:localRot1 = (0.910634, -0.0921648, -0.21039, 0.343494)
float physics:lowerLimit = -1 # NOTE: The presence of these properties enables the UsdPhysics.JointLimit(...) attribute
float physics:upperLimit = 1
bool physics:collisionEnabled = 0
float drive:linear:physics:maxForce = 100.0
float drive:linear:physics:targetPosition = -1.0
float drive:linear:physics:targetVelocity = 1.0
float drive:linear:physics:stiffness = 10.0
float drive:linear:physics:damping = 1.0
uniform token drive:linear:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_prismatic_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_prismatic_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_prismatic_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0155601, -0.0848459, -0.00574446)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.2262
float3 physics:diagonalInertia = (1.44825e-05, 0.000123861, 0.000124459)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.181822, -0.340445, 0.916765, -0.102857)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0413202, -0.0281047, 0.0230962)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.2262
float3 physics:diagonalInertia = (1.44825e-05, 0.000123861, 0.000124459)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.181822, -0.340445, 0.916765, -0.102857)
}
}
def Scope "Joints"
{
def PhysicsPrismaticJoint "passive_prismatic_joint"
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
uniform token physics:axis = "X"
point3f physics:localPos0 = (0.0284401, 0.0283706, 0.0144203)
point3f physics:localPos1 = (-0.0284401, -0.0283706, -0.0144203)
quatf physics:localRot0 = (0.910634, -0.0921648, -0.21039, 0.343494)
quatf physics:localRot1 = (0.910634, -0.0921648, -0.21039, 0.343494)
float physics:lowerLimit = -1 # NOTE: The presence of these properties enables the UsdPhysics.JointLimit(...) attribute
float physics:upperLimit = 1
bool physics:collisionEnabled = 0
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_prismatic_passive_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_prismatic_passive_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_prismatic_passive_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0413202, -0.0281047, 0.0230962)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.2262
float3 physics:diagonalInertia = (1.44825e-05, 0.000123861, 0.000124459)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.181822, -0.340445, 0.916765, -0.102857)
}
}
def Scope "Joints"
{
def PhysicsPrismaticJoint "passive_prismatic_joint"
{
bool physics:jointEnabled = 1
rel physics:body1 =
uniform token physics:axis = "X"
point3f physics:localPos0 = (0.0128801, -0.0564753, 0.00867585)
point3f physics:localPos1 = (-0.0284401, -0.0283706, -0.0144203)
quatf physics:localRot0 = (0.910634, -0.0921648, -0.21039, 0.343494)
quatf physics:localRot1 = (0.910634, -0.0921648, -0.21039, 0.343494)
float physics:lowerLimit = -1 # NOTE: The presence of these properties enables the UsdPhysics.JointLimit(...) attribute
float physics:upperLimit = 1
bool physics:collisionEnabled = 0
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_revolute_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_revolute_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_revolute_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0580082, 0.00433066, 0.00151059)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.216622, -0.365389, 0.904163, 0.0453333)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.00841418, 0.0170561, 0.0250432)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.962133, 0.122309, -0.172458, -0.172042)
}
}
def Scope "Joints"
{
def PhysicsRevoluteJoint "actuated_revolute_joint" (
prepend apiSchemas = ["PhysicsDriveAPI:angular"] # NOTE: This enables the UsdPhysics.JointDrive(...) attribute
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
uniform token physics:axis = "Y"
point3f physics:localPos0 = (0.0281016, 0.0277628, 0.0146162)
point3f physics:localPos1 = (-0.0383208, 0.0150373, -0.00891642)
quatf physics:localRot0 = (0.510989, 0.283121, -0.776741, 0.235387)
quatf physics:localRot1 = (0.510989, 0.283121, -0.776741, 0.235387)
float physics:lowerLimit = -90 # NOTE: The presence of these properties enables the UsdPhysics.JointLimit(...) attribute
float physics:upperLimit = 90
bool physics:collisionEnabled = 0
float drive:angular:physics:maxForce = 100.0
float drive:angular:physics:targetPosition = -1.0
float drive:angular:physics:targetVelocity = 1.0
float drive:angular:physics:stiffness = 10.0
float drive:angular:physics:damping = 1.0
uniform token drive:angular:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_revolute_actuated_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_revolute_actuated_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_revolute_actuated_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.00841418, 0.0170561, 0.0250432)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.962133, 0.122309, -0.172458, -0.172042)
}
}
def Scope "Joints"
{
def PhysicsRevoluteJoint "actuated_revolute_joint" (
prepend apiSchemas = ["PhysicsDriveAPI:angular"] # NOTE: This enables the UsdPhysics.JointDrive(...) attribute
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
uniform token physics:axis = "Y"
point3f physics:localPos0 = (-0.0299066, 0.0320934, 0.0161268)
point3f physics:localPos1 = (-0.0383208, 0.0150373, -0.00891642)
quatf physics:localRot0 = (0.510989, 0.283121, -0.776741, 0.235387)
quatf physics:localRot1 = (0.510989, 0.283121, -0.776741, 0.235387)
float physics:lowerLimit = -90 # NOTE: The presence of these properties enables the UsdPhysics.JointLimit(...) attribute
float physics:upperLimit = 90
bool physics:collisionEnabled = 0
float drive:angular:physics:maxForce = 100.0
float drive:angular:physics:targetPosition = -1.0
float drive:angular:physics:targetVelocity = 1.0
float drive:angular:physics:stiffness = 10.0
float drive:angular:physics:damping = 1.0
uniform token drive:angular:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_revolute_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_revolute_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_revolute_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0580082, 0.00433066, 0.00151059)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.216622, -0.365389, 0.904163, 0.0453333)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.00841418, 0.0170561, 0.0250432)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.962133, 0.122309, -0.172458, -0.172042)
}
}
def Scope "Joints"
{
def PhysicsRevoluteJoint "passive_revolute_joint"
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
uniform token physics:axis = "Y"
point3f physics:localPos0 = (0.0281016, 0.0277628, 0.0146162)
point3f physics:localPos1 = (-0.0383208, 0.0150373, -0.00891642)
quatf physics:localRot0 = (0.510989, 0.283121, -0.776741, 0.235387)
quatf physics:localRot1 = (0.510989, 0.283121, -0.776741, 0.235387)
float physics:lowerLimit = -90 # NOTE: The presence of these properties enables the UsdPhysics.JointLimit(...) attribute
float physics:upperLimit = 90
bool physics:collisionEnabled = 0
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_revolute_passive_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_revolute_passive_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_revolute_passive_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.00841418, 0.0170561, 0.0250432)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.228447
float3 physics:diagonalInertia = (1.45422e-05, 0.000130947, 0.000131568)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (0.962133, 0.122309, -0.172458, -0.172042)
}
}
def Scope "Joints"
{
def PhysicsRevoluteJoint "passive_revolute_joint"
{
bool physics:jointEnabled = 1
rel physics:body1 =
uniform token physics:axis = "Y"
point3f physics:localPos0 = (-0.0299066, 0.0320934, 0.0161268)
point3f physics:localPos1 = (-0.0383208, 0.0150373, -0.00891642)
quatf physics:localRot0 = (0.510989, 0.283121, -0.776741, 0.235387)
quatf physics:localRot1 = (0.510989, 0.283121, -0.776741, 0.235387)
float physics:lowerLimit = -90 # NOTE: The presence of these properties enables the UsdPhysics.JointLimit(...) attribute
float physics:upperLimit = 90
bool physics:collisionEnabled = 0
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_spherical.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_spherical"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_spherical"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0469994, -0.00580511, -0.000663223)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.192959
float3 physics:diagonalInertia = (1.25101e-05, 7.28088e-05, 7.28088e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.0545067, 0.957285, 0.283504, 0.0161424)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0221895, 0.0480183, -0.0262788)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.193009
float3 physics:diagonalInertia = (1.25415e-05, 7.25474e-05, 7.25474e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.421016, 0.17955, 0.348782, 0.817837)
}
}
def Scope "Joints"
{
def PhysicsSphericalJoint "passive_spherical_joint"
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
uniform token physics:axis = "X"
point3f physics:localPos0 = (0.0408711, 0.0263642, 0.00301206)
point3f physics:localPos1 = (-0.0283177, -0.0274592, 0.0286276)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_spherical_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_spherical_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_spherical_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0221895, 0.0480183, -0.0262788)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.193009
float3 physics:diagonalInertia = (1.25415e-05, 7.25474e-05, 7.25474e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.421016, 0.17955, 0.348782, 0.817837)
}
}
def Scope "Joints"
{
def PhysicsSphericalJoint "passive_spherical_joint"
{
bool physics:jointEnabled = 1
rel physics:body1 =
uniform token physics:axis = "X"
point3f physics:localPos0 = (-0.00612828, 0.0205591, 0.00234884)
point3f physics:localPos1 = (-0.0283177, -0.0274592, 0.0286276)
quatf physics:localRot0 = (1, 0, 0, 0)
quatf physics:localRot1 = (1, 0, 0, 0)
bool physics:collisionEnabled = 0
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_universal_actuated.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_universal_actuated"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_universal_actuated"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0443459, -0.00564078, -0.00325617)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.202599
float3 physics:diagonalInertia = (1.31518e-05, 8.59479e-05, 8.6375e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.0439297, -0.36854, 0.743602, 0.556152)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0193489, 0.0554506, -0.00224473)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.202599
float3 physics:diagonalInertia = (1.31518e-05, 8.59479e-05, 8.6375e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.0356189, -0.427367, 0.849557, -0.307151)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_universal_joint" (
customData = {string dofs = "universal"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:rotX",
"PhysicsDriveAPI:rotY",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0340146, 0.0280267, 0.0161785)
point3f physics:localPos1 = (-0.0296803, -0.0330647, 0.0151671)
quatf physics:localRot0 = (-0.220973, 0.448949, 0.477656, 0.722122)
quatf physics:localRot1 = (-0.220973, 0.448949, 0.477656, 0.722122)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the first DoF of the universal joint
float limit:rotX:physics:low = -90
float limit:rotX:physics:high = 90
# rotY is the second DoF of the universal joint
float limit:rotY:physics:low = -90
float limit:rotY:physics:high = 90
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:rotX:physics:maxForce = 100.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:rotY:physics:maxForce = 200.0
uniform token drive:rotY:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_universal_actuated_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_universal_actuated_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_universal_actuated_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0193489, 0.0554506, -0.00224473)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.202599
float3 physics:diagonalInertia = (1.31518e-05, 8.59479e-05, 8.6375e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.0356189, -0.427367, 0.849557, -0.307151)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "actuated_universal_joint" (
customData = {string dofs = "universal"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
# # NOTE: These enable the UsdPhysics.JointDrive(...) attributes
"PhysicsDriveAPI:rotX",
"PhysicsDriveAPI:rotY",
]
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
point3f physics:localPos0 = (-0.0103314, 0.0223859, 0.0129223)
point3f physics:localPos1 = (-0.0296803, -0.0330647, 0.0151671)
quatf physics:localRot0 = (-0.220973, 0.448949, 0.477656, 0.722122)
quatf physics:localRot1 = (-0.220973, 0.448949, 0.477656, 0.722122)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the first DoF of the universal joint
float limit:rotX:physics:low = -90
float limit:rotX:physics:high = 90
# rotY is the second DoF of the universal joint
float limit:rotY:physics:low = -90
float limit:rotY:physics:high = 90
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
# Drive of the first DoF
float drive:rotX:physics:maxForce = 100.0
uniform token drive:rotX:physics:type = "force" # can be in {"force", "acceleration"}
# Drive of the second DoF
float drive:rotY:physics:maxForce = 200.0
uniform token drive:rotY:physics:type = "force" # can be in {"force", "acceleration"}
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_universal_passive.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_universal_passive"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_universal_passive"
{
def Scope "RigidBodies"
{
def Xform "body_0" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (-0.0443459, -0.00564078, -0.00325617)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.202599
float3 physics:diagonalInertia = (1.31518e-05, 8.59479e-05, 8.6375e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.0439297, -0.36854, 0.743602, 0.556152)
}
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0193489, 0.0554506, -0.00224473)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.202599
float3 physics:diagonalInertia = (1.31518e-05, 8.59479e-05, 8.6375e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.0356189, -0.427367, 0.849557, -0.307151)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_universal_joint" (
customData = {string dofs = "universal"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body0 =
rel physics:body1 =
point3f physics:localPos0 = (0.0340146, 0.0280267, 0.0161785)
point3f physics:localPos1 = (-0.0296803, -0.0330647, 0.0151671)
quatf physics:localRot0 = (-0.220973, 0.448949, 0.477656, 0.722122)
quatf physics:localRot1 = (-0.220973, 0.448949, 0.477656, 0.722122)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the first DoF of the universal joint
float limit:rotX:physics:low = -90
float limit:rotX:physics:high = 90
# rotY is the second DoF of the universal joint
float limit:rotY:physics:low = -90
float limit:rotY:physics:high = 90
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/assets/testing/joints/test_joint_universal_passive_unary.usda
================================================
#usda 1.0
(
doc = """Authored manually for testing purposes."""
defaultPrim = "test_joint_universal_passive_unary"
timeCodesPerSecond = 1000
kilogramsPerUnit = 1
metersPerUnit = 1
upAxis = "Z"
)
def PhysicsScene "World"
{
float physics:gravityMagnitude = 9.8067
vector3f physics:gravityDirection = (0, 0, -1)
}
def Xform "test_joint_universal_passive_unary"
{
def Scope "RigidBodies"
{
def Xform "body_1" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0193489, 0.0554506, -0.00224473)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 0.202599
float3 physics:diagonalInertia = (1.31518e-05, 8.59479e-05, 8.6375e-05)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (-0.0356189, -0.427367, 0.849557, -0.307151)
}
}
def Scope "Joints"
{
# NOTE: The `PhysicsD6Joint` type does not work, D6 joints are specified using `PhysicsJoint`
def PhysicsJoint "passive_universal_joint" (
customData = {string dofs = "universal"}
prepend apiSchemas = [
# NOTE: These enable the UsdPhysics.JointLimit(...) attributes
"PhysicsLimitAPI:transX",
"PhysicsLimitAPI:transY",
"PhysicsLimitAPI:transZ",
"PhysicsLimitAPI:rotX",
"PhysicsLimitAPI:rotY",
"PhysicsLimitAPI:rotZ",
]
)
{
bool physics:jointEnabled = 1
rel physics:body1 =
point3f physics:localPos0 = (-0.0103314, 0.0223859, 0.0129223)
point3f physics:localPos1 = (-0.0296803, -0.0330647, 0.0151671)
quatf physics:localRot0 = (-0.220973, 0.448949, 0.477656, 0.722122)
quatf physics:localRot1 = (-0.220973, 0.448949, 0.477656, 0.722122)
bool physics:collisionEnabled = 0
# low > high makes transX a constraint axis
float limit:transX:physics:low = 1
float limit:transX:physics:high = -1
# low > high makes transY a constraint axis
float limit:transY:physics:low = 1
float limit:transY:physics:high = -1
# low > high makes transZ a constraint axis
float limit:transZ:physics:low = 1
float limit:transZ:physics:high = -1
# rotX is the first DoF of the universal joint
float limit:rotX:physics:low = -90
float limit:rotX:physics:high = 90
# rotY is the second DoF of the universal joint
float limit:rotY:physics:low = -90
float limit:rotY:physics:high = 90
# low > high makes rotZ a constraint axis
float limit:rotZ:physics:low = 1
float limit:rotZ:physics:high = -1
}
}
}
================================================
FILE: newton/_src/solvers/kamino/_src/models/builders/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Model building utilities and predefined builders.
This module provides a collection of model builders
for common scenarios and testing purposes, as well
as utility functions for composing and manipulating
model builders.
"""
from . import basics, testing, utils
__all__ = [
"basics",
"testing",
"utils",
]
================================================
FILE: newton/_src/solvers/kamino/_src/models/builders/basics.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Factory methods for building 'basic' models.
This module provides a set of functions to create simple mechanical assemblies
using the ModelBuilderKamino interface. These include fundamental configurations such
as a box on a plane, a box pendulum, a cartpole, and various linked box systems.
Each function constructs a specific model by adding rigid bodies, joints,
and collision geometries to a ModelBuilderKamino instance. The models are designed
to serve as foundational examples for testing and demonstration purposes,
and each features a certain subset of ill-conditioned dynamics.
"""
import math
import warp as wp
from ...core import ModelBuilderKamino, inertia
from ...core.joints import JointActuationType, JointDoFType
from ...core.math import FLOAT32_MAX, FLOAT32_MIN, I_3
from ...core.shapes import BoxShape, SphereShape
from ...core.types import Axis, transformf, vec3f, vec6f
###
# Module interface
###
__all__ = [
"build_box_on_plane",
"build_box_pendulum",
"build_box_pendulum_vertical",
"build_boxes_fourbar",
"build_boxes_hinged",
"build_boxes_nunchaku",
"build_boxes_nunchaku_vertical",
]
###
# Functions
###
def build_box_on_plane(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
ground: bool = True,
new_world: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Constructs a basic model of a free-floating 'box' body and a ground box geom.
Args:
builder (ModelBuilderKamino | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `False`, the model is added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.\n
world_index (int):
The index of the world to which the model should be added if `new_world` is False.\n
If `new_world` is True, this argument is ignored.\n
If the value does not correspond to an existing world, an error will be raised.\n
Defaults to `0`.
Returns:
ModelBuilderKamino: The populated model builder.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="box_on_plane")
# Add first body
bid0 = _builder.add_rigid_body(
m_i=1.0,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(1.0, 0.2, 0.2, 0.2),
q_i_0=transformf(0.0, 0.0, 0.1 + z_offset, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add collision geometries
_builder.add_geometry(body=bid0, shape=BoxShape(0.1, 0.1, 0.1), world_index=world_index)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the lists of element indices
return _builder
def build_box_pendulum(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.7,
ground: bool = True,
new_world: bool = True,
dynamic_joints: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Constructs a basic model of a single box pendulum body with a unary revolute joint.
This version initializes the pendulum in a horizontal configuration.
Args:
builder (ModelBuilderKamino | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `False`, the model is added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.\n
world_index (int):
The index of the world to which the model should be added if `new_world` is False.\n
If `new_world` is True, this argument is ignored.\n
If the value does not correspond to an existing world, an error will be raised.\n
Defaults to `0`.
Returns:
ModelBuilderKamino: The populated model builder.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="box_pendulum")
# Model constants
m = 1.0
d = 0.5
w = 0.1
h = 0.1
z_0 = z_offset # Initial z offset for the body
# Add box pendulum body
bid0 = _builder.add_rigid_body(
name="pendulum",
m_i=m,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m, d, w, h),
q_i_0=transformf(0.5 * d, 0.0, 0.5 * h + z_0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add a joint between the two bodies
_builder.add_joint(
name="world_to_pendulum",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=-1,
bid_F=bid0,
B_r_Bj=vec3f(0.0, 0.0, 0.5 * h + z_0),
F_r_Fj=vec3f(-0.5 * d, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
a_j=1.0 if dynamic_joints else None,
b_j=0.1 if dynamic_joints else None,
k_p_j=100.0 if implicit_pd else None,
k_d_j=1.0 if implicit_pd else None,
world_index=world_index,
)
# Add collision geometries
_builder.add_geometry(
name="box",
body=bid0,
shape=BoxShape(0.5 * d, 0.5 * w, 0.5 * h),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
name="ground",
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the lists of element indices
return _builder
def build_box_pendulum_vertical(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.7,
ground: bool = True,
new_world: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Constructs a basic model of a single box pendulum body with a unary revolute joint.
This version initializes the pendulum in a vertical configuration.
Args:
builder (ModelBuilderKamino | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `False`, the model is added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.\n
world_index (int):
The index of the world to which the model should be added if `new_world` is False.\n
If `new_world` is True, this argument is ignored.\n
If the value does not correspond to an existing world, an error will be raised.\n
Defaults to `0`.
Returns:
ModelBuilderKamino: The populated model builder.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="box_pendulum_vertical")
# Model constants
m = 1.0
d = 0.1
w = 0.1
h = 0.5
z_0 = z_offset # Initial z offset for the body
# Add box pendulum body
bid0 = _builder.add_rigid_body(
name="pendulum",
m_i=m,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m, d, w, h),
q_i_0=transformf(0.0, 0.0, -0.5 * h + z_0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add a joint between the two bodies
_builder.add_joint(
name="world_to_pendulum",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.FORCE,
bid_B=-1,
bid_F=bid0,
B_r_Bj=vec3f(0.0, 0.0, 0.0 + z_0),
F_r_Fj=vec3f(0.0, 0.0, 0.5 * h),
X_j=Axis.Y.to_mat33(),
world_index=world_index,
)
# Add collision geometries
_builder.add_geometry(
name="box",
body=bid0,
shape=BoxShape(0.5 * d, 0.5 * w, 0.5 * h),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
name="ground",
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the lists of element indices
return _builder
def build_cartpole(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
ground: bool = True,
new_world: bool = True,
limits: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Constructs a basic model of a cartpole mounted onto a rail.
Args:
builder (ModelBuilderKamino | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `False`, the model is added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.\n
world_index (int):
The index of the world to which the model should be added if `new_world` is False.\n
If `new_world` is True, this argument is ignored.\n
If the value does not correspond to an existing world, an error will be raised.\n
Defaults to `0`.
Returns:
ModelBuilderKamino: The populated model builder.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="cartpole")
# Model constants
m_cart = 1.0
m_pole = 0.2
dims_rail = (0.03, 8.0, 0.03) # full dimensions (used for inertia, positions)
dims_cart = (0.2, 0.5, 0.2)
dims_pole = (0.05, 0.05, 0.75)
half_dims_rail = (0.5 * dims_rail[0], 0.5 * dims_rail[1], 0.5 * dims_rail[2])
half_dims_cart = (0.5 * dims_cart[0], 0.5 * dims_cart[1], 0.5 * dims_cart[2])
half_dims_pole = (0.5 * dims_pole[0], 0.5 * dims_pole[1], 0.5 * dims_pole[2])
# Add box cart body
bid0 = _builder.add_rigid_body(
name="cart",
m_i=m_cart,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m_cart, *dims_cart),
q_i_0=transformf(0.0, 0.0, z_offset, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add box pole body
x_0_pole = 0.5 * dims_pole[0] + 0.5 * dims_cart[0]
z_0_pole = 0.5 * dims_pole[2] + z_offset
bid1 = _builder.add_rigid_body(
name="pole",
m_i=m_pole,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m_pole, *dims_pole),
q_i_0=transformf(x_0_pole, 0.0, z_0_pole, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add a prismatic joint for the cart
_builder.add_joint(
name="rail_to_cart",
dof_type=JointDoFType.PRISMATIC,
act_type=JointActuationType.FORCE,
bid_B=-1,
bid_F=bid0,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
q_j_min=[-4.0] if limits else [float(FLOAT32_MIN)],
q_j_max=[4.0] if limits else [float(FLOAT32_MAX)],
tau_j_max=[1000.0],
world_index=world_index,
)
# Add a revolute joint for the pendulum
_builder.add_joint(
name="cart_to_pole",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.PASSIVE,
bid_B=bid0,
bid_F=bid1,
B_r_Bj=vec3f(0.5 * dims_cart[0], 0.0, 0.0),
F_r_Fj=vec3f(-0.5 * dims_pole[0], 0.0, -0.5 * dims_pole[2]),
X_j=Axis.X.to_mat33(),
world_index=world_index,
)
# Add collision geometries
_builder.add_geometry(
name="cart",
body=bid0,
shape=BoxShape(*half_dims_cart),
group=2,
collides=2,
world_index=world_index,
)
_builder.add_geometry(
name="pole",
body=bid1,
shape=BoxShape(*half_dims_pole),
group=3,
collides=3,
world_index=world_index,
)
_builder.add_geometry(
name="rail",
body=-1,
shape=BoxShape(*half_dims_rail),
offset=transformf(0.0, 0.0, z_offset, 0.0, 0.0, 0.0, 1.0),
group=1,
collides=1,
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
name="ground",
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.0 + z_offset, 0.0, 0.0, 0.0, 1.0),
group=1,
collides=1,
world_index=world_index,
)
# Return the lists of element indices
return _builder
def build_boxes_hinged(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
ground: bool = True,
dynamic_joints: bool = False,
implicit_pd: bool = False,
new_world: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Constructs a basic model of a two floating boxes connected via revolute joint.
Args:
builder (ModelBuilderKamino | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `False`, the model is added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.\n
world_index (int):
The index of the world to which the model should be added if `new_world` is False.\n
If `new_world` is True, this argument is ignored.\n
If the value does not correspond to an existing world, an error will be raised.\n
Defaults to `0`.
Returns:
ModelBuilderKamino: The populated model builder.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="boxes_hinged")
# Model constants
m_0 = 1.0
m_1 = 1.0
d = 0.5
w = 0.1
h = 0.1
z0 = z_offset # Initial z offset for the bodies
# Add first body
bid0 = _builder.add_rigid_body(
name="base",
m_i=m_0,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m_0, d, w, h),
q_i_0=transformf(0.25, -0.05, 0.05 + z0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add second body
bid1 = _builder.add_rigid_body(
name="follower",
m_i=m_1,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m_1, d, w, h),
q_i_0=transformf(0.75, 0.05, 0.05 + z0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add a joint between the two bodies
_builder.add_joint(
name="hinge",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=bid0,
bid_F=bid1,
B_r_Bj=vec3f(0.25, 0.05, 0.0),
F_r_Fj=vec3f(-0.25, -0.05, 0.0),
X_j=Axis.Y.to_mat33(),
a_j=1.0 if dynamic_joints else None,
b_j=0.1 if dynamic_joints else None,
k_p_j=100.0 if implicit_pd else None,
k_d_j=1.0 if implicit_pd else None,
world_index=world_index,
)
# Add collision geometries
_builder.add_geometry(
name="base/box",
body=bid0,
shape=BoxShape(0.5 * d, 0.5 * w, 0.5 * h),
group=2,
collides=3,
world_index=world_index,
)
_builder.add_geometry(
name="follower/box",
body=bid1,
shape=BoxShape(0.5 * d, 0.5 * w, 0.5 * h),
group=3,
collides=5,
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
name="ground",
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 1.0),
group=1,
collides=7,
world_index=world_index,
)
# Return the lists of element indices
return _builder
def build_boxes_nunchaku(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
ground: bool = True,
new_world: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Constructs a basic model of a faux nunchaku consisting of
two boxes and one sphere connected via spherical joints.
This version initializes the nunchaku in a horizontal configuration.
Args:
builder (ModelBuilderKamino | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `False`, the model is added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.\n
world_index (int):
The index of the world to which the model should be added if `new_world` is False.\n
If `new_world` is True, this argument is ignored.\n
If the value does not correspond to an existing world, an error will be raised.\n
Defaults to `0`.
Returns:
ModelBuilderKamino: The populated model builder.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="boxes_nunchaku")
# Model constants
m_0 = 1.0
m_1 = 1.0
m_2 = 1.0
d = 0.5
w = 0.1
h = 0.1
r = 0.05
# Constant to set an initial z offset for the bodies
# NOTE: for testing purposes, recommend values are {0.0, -0.001}
z_0 = z_offset
# Add first body
bid0 = _builder.add_rigid_body(
name="box_bottom",
m_i=m_0,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m_0, d, w, h),
q_i_0=transformf(0.5 * d, 0.0, 0.5 * h + z_0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add second body
bid1 = _builder.add_rigid_body(
name="sphere_middle",
m_i=m_1,
i_I_i=inertia.solid_sphere_body_moment_of_inertia(m_1, r),
q_i_0=transformf(r + d, 0.0, r + z_0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add third body
bid2 = _builder.add_rigid_body(
name="box_top",
m_i=m_2,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m_2, d, w, h),
q_i_0=transformf(1.5 * d + 2.0 * r, 0.0, 0.5 * h + z_0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add a joint between the first and second body
_builder.add_joint(
name="box_bottom_to_sphere_middle",
dof_type=JointDoFType.SPHERICAL,
act_type=JointActuationType.PASSIVE,
bid_B=bid0,
bid_F=bid1,
B_r_Bj=vec3f(0.5 * d, 0.0, 0.0),
F_r_Fj=vec3f(-r, 0.0, 0.0),
X_j=I_3,
world_index=world_index,
)
# Add a joint between the second and third body
_builder.add_joint(
name="sphere_middle_to_box_top",
dof_type=JointDoFType.SPHERICAL,
act_type=JointActuationType.PASSIVE,
bid_B=bid1,
bid_F=bid2,
B_r_Bj=vec3f(r, 0.0, 0.0),
F_r_Fj=vec3f(-0.5 * d, 0.0, 0.0),
X_j=I_3,
world_index=world_index,
)
# Add collision geometries
_builder.add_geometry(
name="box_bottom",
body=bid0,
shape=BoxShape(0.5 * d, 0.5 * w, 0.5 * h),
group=2,
collides=3,
world_index=world_index,
)
_builder.add_geometry(
name="sphere_middle", body=bid1, shape=SphereShape(r), group=3, collides=5, world_index=world_index
)
_builder.add_geometry(
name="box_top",
body=bid2,
shape=BoxShape(0.5 * d, 0.5 * w, 0.5 * h),
group=2,
collides=3,
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
name="ground",
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 1.0),
group=1,
collides=7,
world_index=world_index,
)
# Return the lists of element indices
return _builder
def build_boxes_nunchaku_vertical(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
ground: bool = True,
new_world: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Constructs a basic model of a faux nunchaku consisting of
two boxes and one sphere connected via spherical joints.
This version initializes the nunchaku in a vertical configuration.
Args:
builder (ModelBuilderKamino | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `False`, the model is added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.\n
world_index (int):
The index of the world to which the model should be added if `new_world` is False.\n
If `new_world` is True, this argument is ignored.\n
If the value does not correspond to an existing world, an error will be raised.\n
Defaults to `0`.
Returns:
ModelBuilderKamino: The populated model builder.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="boxes_nunchaku_vertical")
# Model constants
m_0 = 1.0
m_1 = 1.0
m_2 = 1.0
d = 0.1
w = 0.1
h = 0.5
r = 0.05
# Constant to set an initial z offset for the bodies
# NOTE: for testing purposes, recommend values are {0.0, -0.001}
z_0 = z_offset
# Add first body
bid0 = _builder.add_rigid_body(
name="box_bottom",
m_i=m_0,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m_0, d, w, h),
q_i_0=transformf(0.0, 0.0, 0.5 * h + z_0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add second body
bid1 = _builder.add_rigid_body(
name="sphere_middle",
m_i=m_1,
i_I_i=inertia.solid_sphere_body_moment_of_inertia(m_1, r),
q_i_0=transformf(0.0, 0.0, h + r + z_0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add third body
bid2 = _builder.add_rigid_body(
name="box_top",
m_i=m_2,
i_I_i=inertia.solid_cuboid_body_moment_of_inertia(m_2, d, w, h),
q_i_0=transformf(0.0, 0.0, 1.5 * h + 2.0 * r + z_0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
# Add a joint between the first and second body
_builder.add_joint(
name="box_bottom_to_sphere_middle",
dof_type=JointDoFType.SPHERICAL,
act_type=JointActuationType.PASSIVE,
bid_B=bid0,
bid_F=bid1,
B_r_Bj=vec3f(0.0, 0.0, 0.5 * h),
F_r_Fj=vec3f(0.0, 0.0, -r),
X_j=I_3,
world_index=world_index,
)
# Add a joint between the second and third body
_builder.add_joint(
name="sphere_middle_to_box_top",
dof_type=JointDoFType.SPHERICAL,
act_type=JointActuationType.PASSIVE,
bid_B=bid1,
bid_F=bid2,
B_r_Bj=vec3f(0.0, 0.0, r),
F_r_Fj=vec3f(0.0, 0.0, -0.5 * h),
X_j=I_3,
world_index=world_index,
)
# Add collision geometries
_builder.add_geometry(
name="box_bottom",
body=bid0,
shape=BoxShape(0.5 * d, 0.5 * w, 0.5 * h),
group=2,
collides=3,
world_index=world_index,
)
_builder.add_geometry(
name="sphere_middle", body=bid1, shape=SphereShape(r), group=3, collides=5, world_index=world_index
)
_builder.add_geometry(
name="box_top",
body=bid2,
shape=BoxShape(0.5 * d, 0.5 * w, 0.5 * h),
group=2,
collides=3,
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
name="ground",
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 1.0),
group=1,
collides=7,
world_index=world_index,
)
# Return the lists of element indices
return _builder
def build_boxes_fourbar(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
fixedbase: bool = False,
floatingbase: bool = False,
limits: bool = True,
ground: bool = True,
dynamic_joints: bool = False,
implicit_pd: bool = False,
verbose: bool = False,
new_world: bool = True,
world_index: int = 0,
actuator_ids: list[int] | None = None,
) -> ModelBuilderKamino:
"""
Constructs a basic model of a four-bar linkage.
Args:
builder (ModelBuilderKamino | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `False`, the model is added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.\n
world_index (int):
The index of the world to which the model should be added if `new_world` is False.\n
If `new_world` is True, this argument is ignored.\n
If the value does not correspond to an existing world, an error will be raised.\n
Defaults to `0`.
Returns:
ModelBuilderKamino: A model builder containing the four-bar linkage.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="boxes_fourbar")
# Set default actuator IDs if none are provided
if actuator_ids is None:
actuator_ids = [1, 3]
elif not isinstance(actuator_ids, list):
raise TypeError("actuator_ids, if specified, must be provided as a list of integers.")
###
# Base Parameters
###
# Constant to set an initial z offset for the bodies
# NOTE: for testing purposes, recommend values are {0.0, -0.001}
z_0 = z_offset
# Box dimensions
d = 0.01
w = 0.01
h = 0.1
# Margins
mj = 0.001
dj = 0.5 * d + mj
###
# Body parameters
###
# Box dimensions
d_1 = h
w_1 = w
h_1 = d
d_2 = d
w_2 = w
h_2 = h
d_3 = h
w_3 = w
h_3 = d
d_4 = d
w_4 = w
h_4 = h
# Inertial properties
m_i = 1.0
i_I_i_1 = inertia.solid_cuboid_body_moment_of_inertia(m_i, d_1, w_1, h_1)
i_I_i_2 = inertia.solid_cuboid_body_moment_of_inertia(m_i, d_2, w_2, h_2)
i_I_i_3 = inertia.solid_cuboid_body_moment_of_inertia(m_i, d_3, w_3, h_3)
i_I_i_4 = inertia.solid_cuboid_body_moment_of_inertia(m_i, d_4, w_4, h_4)
if verbose:
print(f"i_I_i_1:\n{i_I_i_1}")
print(f"i_I_i_2:\n{i_I_i_2}")
print(f"i_I_i_3:\n{i_I_i_3}")
print(f"i_I_i_4:\n{i_I_i_4}")
# Initial body positions
r_0 = vec3f(0.0, 0.0, z_0)
dr_b1 = vec3f(0.0, 0.0, 0.5 * d)
dr_b2 = vec3f(0.5 * h + dj, 0.0, 0.5 * h + dj)
dr_b3 = vec3f(0.0, 0.0, 0.5 * d + h + dj + mj)
dr_b4 = vec3f(-0.5 * h - dj, 0.0, 0.5 * h + dj)
# Initial positions of the bodies
r_b1 = r_0 + dr_b1
r_b2 = r_b1 + dr_b2
r_b3 = r_b1 + dr_b3
r_b4 = r_b1 + dr_b4
if verbose:
print(f"r_b1: {r_b1}")
print(f"r_b2: {r_b2}")
print(f"r_b3: {r_b3}")
print(f"r_b4: {r_b4}")
# Initial body poses
q_i_1 = transformf(r_b1, wp.quat_identity())
q_i_2 = transformf(r_b2, wp.quat_identity())
q_i_3 = transformf(r_b3, wp.quat_identity())
q_i_4 = transformf(r_b4, wp.quat_identity())
# Initial joint positions
r_j1 = vec3f(r_b2.x, 0.0, r_b1.z)
r_j2 = vec3f(r_b2.x, 0.0, r_b3.z)
r_j3 = vec3f(r_b4.x, 0.0, r_b3.z)
r_j4 = vec3f(r_b4.x, 0.0, r_b1.z)
if verbose:
print(f"r_j1: {r_j1}")
print(f"r_j2: {r_j2}")
print(f"r_j3: {r_j3}")
print(f"r_j4: {r_j4}")
# Joint axes matrix
X_j = Axis.Y.to_mat33()
###
# Bodies
###
bid1 = _builder.add_rigid_body(
name="link_1",
m_i=m_i,
i_I_i=i_I_i_1,
q_i_0=q_i_1,
u_i_0=vec6f(0.0),
world_index=world_index,
)
bid2 = _builder.add_rigid_body(
name="link_2",
m_i=m_i,
i_I_i=i_I_i_2,
q_i_0=q_i_2,
u_i_0=vec6f(0.0),
world_index=world_index,
)
bid3 = _builder.add_rigid_body(
name="link_3",
m_i=m_i,
i_I_i=i_I_i_3,
q_i_0=q_i_3,
u_i_0=vec6f(0.0),
world_index=world_index,
)
bid4 = _builder.add_rigid_body(
name="link_4",
m_i=m_i,
i_I_i=i_I_i_4,
q_i_0=q_i_4,
u_i_0=vec6f(0.0),
world_index=world_index,
)
###
# Joints
###
if limits:
qmin = -0.25 * math.pi
qmax = 0.25 * math.pi
else:
qmin = float(FLOAT32_MIN)
qmax = float(FLOAT32_MAX)
if fixedbase:
_builder.add_joint(
name="world_to_link1",
dof_type=JointDoFType.FIXED,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid1,
B_r_Bj=vec3f(0.0),
F_r_Fj=-r_b1,
X_j=I_3,
world_index=world_index,
)
if floatingbase:
_builder.add_joint(
name="world_to_link1",
dof_type=JointDoFType.FREE,
act_type=JointActuationType.FORCE if 0 in actuator_ids else JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid1,
B_r_Bj=vec3f(0.0),
F_r_Fj=vec3f(0.0),
X_j=I_3,
world_index=world_index,
)
joint_1_type_if_implicit_pd = JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE
joint_1_type = joint_1_type_if_implicit_pd if 1 in actuator_ids else JointActuationType.PASSIVE
_builder.add_joint(
name="link1_to_link2",
dof_type=JointDoFType.REVOLUTE,
act_type=joint_1_type,
bid_B=bid1,
bid_F=bid2,
B_r_Bj=r_j1 - r_b1,
F_r_Fj=r_j1 - r_b2,
X_j=X_j,
q_j_min=[qmin],
q_j_max=[qmax],
a_j=0.1 if dynamic_joints else None,
b_j=0.001 if dynamic_joints else None,
k_p_j=1000.0 if implicit_pd else None,
k_d_j=20.0 if implicit_pd else None,
world_index=world_index,
)
_builder.add_joint(
name="link2_to_link3",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.FORCE if 2 in actuator_ids else JointActuationType.PASSIVE,
bid_B=bid2,
bid_F=bid3,
B_r_Bj=r_j2 - r_b2,
F_r_Fj=r_j2 - r_b3,
X_j=X_j,
q_j_min=[qmin],
q_j_max=[qmax],
world_index=world_index,
)
_builder.add_joint(
name="link3_to_link4",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.FORCE if 3 in actuator_ids else JointActuationType.PASSIVE,
bid_B=bid3,
bid_F=bid4,
B_r_Bj=r_j3 - r_b3,
F_r_Fj=r_j3 - r_b4,
X_j=X_j,
q_j_min=[qmin],
q_j_max=[qmax],
world_index=world_index,
)
_builder.add_joint(
name="link4_to_link1",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.FORCE if 4 in actuator_ids else JointActuationType.PASSIVE,
bid_B=bid4,
bid_F=bid1,
B_r_Bj=r_j4 - r_b4,
F_r_Fj=r_j4 - r_b1,
X_j=X_j,
q_j_min=[qmin],
q_j_max=[qmax],
world_index=world_index,
)
###
# Geometries
###
# Add collision geometries
_builder.add_geometry(
name="box_1", body=bid1, shape=BoxShape(0.5 * d_1, 0.5 * w_1, 0.5 * h_1), world_index=world_index
)
_builder.add_geometry(
name="box_2", body=bid2, shape=BoxShape(0.5 * d_2, 0.5 * w_2, 0.5 * h_2), world_index=world_index
)
_builder.add_geometry(
name="box_3", body=bid3, shape=BoxShape(0.5 * d_3, 0.5 * w_3, 0.5 * h_3), world_index=world_index
)
_builder.add_geometry(
name="box_4", body=bid4, shape=BoxShape(0.5 * d_4, 0.5 * w_4, 0.5 * h_4), world_index=world_index
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
name="ground",
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the lists of element indices
return _builder
def make_basics_heterogeneous_builder(
ground: bool = True,
dynamic_joints: bool = False,
implicit_pd: bool = False,
) -> ModelBuilderKamino:
"""
Creates a multi-world builder with different worlds in each model.
This function constructs a model builder containing all basic models.
Returns:
ModelBuilderKamino: The constructed model builder.
"""
builder = ModelBuilderKamino(default_world=False)
builder.add_builder(build_boxes_fourbar(ground=ground, dynamic_joints=dynamic_joints, implicit_pd=implicit_pd))
builder.add_builder(build_boxes_nunchaku(ground=ground))
builder.add_builder(build_boxes_hinged(ground=ground, dynamic_joints=dynamic_joints, implicit_pd=implicit_pd))
builder.add_builder(build_box_pendulum(ground=ground, dynamic_joints=dynamic_joints, implicit_pd=implicit_pd))
builder.add_builder(build_box_on_plane(ground=ground))
builder.add_builder(build_cartpole(z_offset=0.5, ground=ground))
return builder
================================================
FILE: newton/_src/solvers/kamino/_src/models/builders/basics_newton.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Factory methods for building 'basic' models using :class:`newton.ModelBuilder`.
This module provides a set of functions to create simple mechanical assemblies using the
:class:`newton.ModelBuilder` interface. These include fundamental configurations such as
a box on a plane, a box pendulum, a cartpole, and various linked box systems.
Each function constructs a specific model by adding rigid bodies, joints, and collision
geometries to a :class:`newton.ModelBuilder` instance. The models are designed to serve
as foundational examples for testing and demonstration purposes, and each features a
certain subset of ill-conditioned dynamics.
"""
import math
import warp as wp
from ......core import Axis
from ......sim import JointTargetMode, ModelBuilder
from ...core import inertia
from ...core.joints import JOINT_QMAX, JOINT_QMIN
###
# Module interface
###
__all__ = [
"build_boxes_fourbar",
"build_boxes_nunchaku",
]
###
# Functions
###
def build_boxes_fourbar(
builder: ModelBuilder | None = None,
z_offset: float = 0.0,
fixedbase: bool = False,
floatingbase: bool = True,
limits: bool = True,
ground: bool = True,
dynamic_joints: bool = False,
implicit_pd: bool = False,
verbose: bool = False,
new_world: bool = True,
actuator_ids: list[int] | None = None,
) -> ModelBuilder:
"""
Constructs a basic model of a four-bar linkage.
Args:
builder (ModelBuilder | None):
An optional existing model builder to populate.\n
If `None`, a new builder is created.
z_offset (float):
A vertical offset to apply to the initial position of the box.
ground (bool):
Whether to add a static ground plane to the model.
new_world (bool):
Whether to create a new world in the builder for this model.\n
If `True`, a new world is created and added to the builder.
Returns:
ModelBuilder: A model builder containing the four-bar linkage.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilder()
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
_builder.begin_world()
# Set default actuator IDs if none are provided
if actuator_ids is None:
actuator_ids = [1, 3]
elif not isinstance(actuator_ids, list):
raise TypeError("actuator_ids, if specified, must be provided as a list of integers.")
###
# Base Parameters
###
# Constant to set an initial z offset for the bodies
# NOTE: for testing purposes, recommend values are {0.0, -0.001}
z_0 = z_offset
# Box dimensions
d = 0.01
w = 0.01
h = 0.1
# Margins
mj = 0.001
dj = 0.5 * d + mj
###
# Body parameters
###
# Box dimensions
d_1 = h
w_1 = w
h_1 = d
d_2 = d
w_2 = w
h_2 = h
d_3 = h
w_3 = w
h_3 = d
d_4 = d
w_4 = w
h_4 = h
# Inertial properties
m_i = 1.0
i_I_i_1 = inertia.solid_cuboid_body_moment_of_inertia(m_i, d_1, w_1, h_1)
i_I_i_2 = inertia.solid_cuboid_body_moment_of_inertia(m_i, d_2, w_2, h_2)
i_I_i_3 = inertia.solid_cuboid_body_moment_of_inertia(m_i, d_3, w_3, h_3)
i_I_i_4 = inertia.solid_cuboid_body_moment_of_inertia(m_i, d_4, w_4, h_4)
if verbose:
print(f"i_I_i_1:\n{i_I_i_1}")
print(f"i_I_i_2:\n{i_I_i_2}")
print(f"i_I_i_3:\n{i_I_i_3}")
print(f"i_I_i_4:\n{i_I_i_4}")
# Initial body positions
r_0 = wp.vec3f(0.0, 0.0, z_0)
dr_b1 = wp.vec3f(0.0, 0.0, 0.5 * d)
dr_b2 = wp.vec3f(0.5 * h + dj, 0.0, 0.5 * h + dj)
dr_b3 = wp.vec3f(0.0, 0.0, 0.5 * d + h + dj + mj)
dr_b4 = wp.vec3f(-0.5 * h - dj, 0.0, 0.5 * h + dj)
# Initial positions of the bodies
r_b1 = r_0 + dr_b1
r_b2 = r_b1 + dr_b2
r_b3 = r_b1 + dr_b3
r_b4 = r_b1 + dr_b4
if verbose:
print(f"r_b1: {r_b1}")
print(f"r_b2: {r_b2}")
print(f"r_b3: {r_b3}")
print(f"r_b4: {r_b4}")
# Initial body poses
q_i_1 = wp.transformf(r_b1, wp.quat_identity(dtype=wp.float32))
q_i_2 = wp.transformf(r_b2, wp.quat_identity(dtype=wp.float32))
q_i_3 = wp.transformf(r_b3, wp.quat_identity(dtype=wp.float32))
q_i_4 = wp.transformf(r_b4, wp.quat_identity(dtype=wp.float32))
# Initial joint positions
r_j1 = wp.vec3f(r_b2.x, 0.0, r_b1.z)
r_j2 = wp.vec3f(r_b2.x, 0.0, r_b3.z)
r_j3 = wp.vec3f(r_b4.x, 0.0, r_b3.z)
r_j4 = wp.vec3f(r_b4.x, 0.0, r_b1.z)
if verbose:
print(f"r_j1: {r_j1}")
print(f"r_j2: {r_j2}")
print(f"r_j3: {r_j3}")
print(f"r_j4: {r_j4}")
###
# Bodies
###
bid1 = _builder.add_link(
label="link_1",
mass=m_i,
inertia=i_I_i_1,
xform=q_i_1,
lock_inertia=True,
)
bid2 = _builder.add_link(
label="link_2",
mass=m_i,
inertia=i_I_i_2,
xform=q_i_2,
lock_inertia=True,
)
bid3 = _builder.add_link(
label="link_3",
mass=m_i,
inertia=i_I_i_3,
xform=q_i_3,
lock_inertia=True,
)
bid4 = _builder.add_link(
label="link_4",
mass=m_i,
inertia=i_I_i_4,
xform=q_i_4,
lock_inertia=True,
)
###
# Geometries
###
_builder.add_shape_box(
label="box_1",
body=bid1,
hx=0.5 * d_1,
hy=0.5 * w_1,
hz=0.5 * h_1,
cfg=ModelBuilder.ShapeConfig(margin=0.0, gap=0.0),
)
_builder.add_shape_box(
label="box_2",
body=bid2,
hx=0.5 * d_2,
hy=0.5 * w_2,
hz=0.5 * h_2,
cfg=ModelBuilder.ShapeConfig(margin=0.0, gap=0.0),
)
_builder.add_shape_box(
label="box_3",
body=bid3,
hx=0.5 * d_3,
hy=0.5 * w_3,
hz=0.5 * h_3,
cfg=ModelBuilder.ShapeConfig(margin=0.0, gap=0.0),
)
_builder.add_shape_box(
label="box_4",
body=bid4,
hx=0.5 * d_4,
hy=0.5 * w_4,
hz=0.5 * h_4,
cfg=ModelBuilder.ShapeConfig(margin=0.0, gap=0.0),
)
# Add a static collision layer and geometry for the plane
if ground:
_builder.add_shape_box(
label="ground",
body=-1,
hx=10.0,
hy=10.0,
hz=0.5,
xform=wp.transformf(0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 1.0),
cfg=ModelBuilder.ShapeConfig(margin=0.0, gap=0.0),
)
###
# Joints
###
if limits:
qmin = -0.25 * math.pi
qmax = 0.25 * math.pi
else:
qmin = float(JOINT_QMIN)
qmax = float(JOINT_QMAX)
if fixedbase:
_builder.add_joint_fixed(
label="world_to_link1",
parent=-1,
child=bid1,
parent_xform=wp.transform_identity(dtype=wp.float32),
child_xform=wp.transformf(-r_b1, wp.quat_identity(dtype=wp.float32)),
)
if floatingbase:
_builder.add_joint_free(
label="world_to_link1",
parent=-1,
child=bid1,
parent_xform=wp.transform_identity(dtype=wp.float32),
child_xform=wp.transform_identity(dtype=wp.float32),
)
passive_joint_dof_config = ModelBuilder.JointDofConfig(
axis=Axis.Y,
actuator_mode=JointTargetMode.NONE,
limit_lower=qmin,
limit_upper=qmax,
)
effort_joint_dof_config = ModelBuilder.JointDofConfig(
axis=Axis.Y,
actuator_mode=JointTargetMode.EFFORT,
limit_lower=qmin,
limit_upper=qmax,
armature=0.1 if dynamic_joints else 0.0,
friction=0.001 if dynamic_joints else 0.0,
)
pd_joint_dof_config = ModelBuilder.JointDofConfig(
axis=Axis.Y,
actuator_mode=JointTargetMode.POSITION_VELOCITY,
armature=0.1 if dynamic_joints else 0.0,
friction=0.001 if dynamic_joints else 0.0,
target_ke=1000.0,
target_kd=20.0,
limit_lower=qmin,
limit_upper=qmax,
)
joint_1_config_if_implicit_pd = pd_joint_dof_config if implicit_pd else effort_joint_dof_config
joint_1_config_if_actuated = joint_1_config_if_implicit_pd if 1 in actuator_ids else passive_joint_dof_config
_builder.add_joint_revolute(
label="link1_to_link2",
parent=bid1,
child=bid2,
axis=joint_1_config_if_actuated if 1 in actuator_ids else passive_joint_dof_config,
parent_xform=wp.transformf(r_j1 - r_b1, wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(r_j1 - r_b2, wp.quat_identity(dtype=wp.float32)),
)
_builder.add_joint_revolute(
label="link2_to_link3",
parent=bid2,
child=bid3,
axis=effort_joint_dof_config if 2 in actuator_ids else passive_joint_dof_config,
parent_xform=wp.transformf(r_j2 - r_b2, wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(r_j2 - r_b3, wp.quat_identity(dtype=wp.float32)),
)
_builder.add_joint_revolute(
label="link3_to_link4",
parent=bid3,
child=bid4,
axis=effort_joint_dof_config if 3 in actuator_ids else passive_joint_dof_config,
parent_xform=wp.transformf(r_j3 - r_b3, wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(r_j3 - r_b4, wp.quat_identity(dtype=wp.float32)),
)
_builder.add_joint_revolute(
label="link4_to_link1",
parent=bid4,
child=bid1,
axis=effort_joint_dof_config if 4 in actuator_ids else passive_joint_dof_config,
parent_xform=wp.transformf(r_j4 - r_b4, wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(r_j4 - r_b1, wp.quat_identity(dtype=wp.float32)),
)
# Signal the end of setting-up the new world
if new_world or builder is None:
_builder.end_world()
# Return the lists of element indices
return _builder
def build_boxes_nunchaku(
builder: ModelBuilder | None = None,
ground: bool = True,
) -> ModelBuilder:
"""
Constructs a nunchaku model: two boxes connected by a sphere via ball joints.
Three bodies (two boxes + one sphere) connected by spherical joints,
optionally resting on a ground plane. Produces 9 contacts with the
ground (4 per box + 1 sphere).
Args:
builder: An optional existing model builder to populate.
If ``None``, a new builder is created.
ground: Whether to add a static ground plane.
Returns:
The populated :class:`ModelBuilder`.
"""
if builder is None:
builder = ModelBuilder()
d, w, h, r = 0.5, 0.1, 0.1, 0.05
no_gap = ModelBuilder.ShapeConfig(gap=0.0)
b0 = builder.add_link()
builder.add_shape_box(b0, hx=d / 2, hy=w / 2, hz=h / 2, cfg=no_gap)
b1 = builder.add_link()
builder.add_shape_sphere(b1, radius=r, cfg=no_gap)
b2 = builder.add_link()
builder.add_shape_box(b2, hx=d / 2, hy=w / 2, hz=h / 2, cfg=no_gap)
j0 = builder.add_joint_ball(
parent=-1,
child=b0,
parent_xform=wp.transform(p=wp.vec3(d / 2, 0.0, h / 2), q=wp.quat_identity()),
child_xform=wp.transform(p=wp.vec3(0.0, 0.0, 0.0), q=wp.quat_identity()),
)
j1 = builder.add_joint_ball(
parent=b0,
child=b1,
parent_xform=wp.transform(p=wp.vec3(d / 2, 0.0, 0.0), q=wp.quat_identity()),
child_xform=wp.transform(p=wp.vec3(-r, 0.0, 0.0), q=wp.quat_identity()),
)
j2 = builder.add_joint_ball(
parent=b1,
child=b2,
parent_xform=wp.transform(p=wp.vec3(r, 0.0, 0.0), q=wp.quat_identity()),
child_xform=wp.transform(p=wp.vec3(-d / 2, 0.0, 0.0), q=wp.quat_identity()),
)
builder.add_articulation([j0, j1, j2])
if ground:
builder.add_ground_plane()
return builder
================================================
FILE: newton/_src/solvers/kamino/_src/models/builders/testing.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides builders for testing supported joint and geometry types.
This module defines a set of functions for creating
model builders to test and demonstrate all the types
of joints and geometries supported by Kamino.
"""
import math
import numpy as np
import warp as wp
from ...core import ModelBuilderKamino
from ...core.joints import JointActuationType, JointDoFType
from ...core.math import I_3, quat_from_euler_xyz
from ...core.shapes import (
BoxShape,
CapsuleShape,
ConeShape,
CylinderShape,
EllipsoidShape,
GeoType,
PlaneShape,
ShapeDescriptorType,
SphereShape,
)
from ...core.types import Axis, mat33f, transformf, vec3f, vec6f
from ...utils import logger as msg
from . import utils
###
# Module interface
###
__all__ = [
"build_binary_cartesian_joint_test",
"build_binary_cylindrical_joint_test",
"build_binary_gimbal_joint_test",
"build_binary_prismatic_joint_test",
"build_binary_revolute_joint_test",
"build_binary_spherical_joint_test",
"build_binary_universal_joint_test",
"build_free_joint_test",
"build_unary_cartesian_joint_test",
"build_unary_cylindrical_joint_test",
"build_unary_gimbal_joint_test",
"build_unary_prismatic_joint_test",
"build_unary_revolute_joint_test",
"build_unary_spherical_joint_test",
"build_unary_universal_joint_test",
"make_shape_pairs_builder",
"make_single_shape_pair_builder",
]
###
# Builders - Joint Tests
###
def build_free_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test free joints.
This world consists of a single rigid body connected to the world via a unary
free joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="unary_free_joint_test")
# Define test system
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(0.0, 0.0, z_offset, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_follower_free",
dof_type=JointDoFType.FREE,
act_type=JointActuationType.FORCE,
bid_B=-1,
bid_F=bid_F,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=I_3,
q_j_min=[-2.0, -2.0, -2.0, -0.6 * math.pi, -0.6 * math.pi, -0.6 * math.pi] if limits else None,
q_j_max=[2.0, 2.0, 2.0, 0.6 * math.pi, 0.6 * math.pi, 0.6 * math.pi] if limits else None,
tau_j_max=[100.0, 100.0, 100.0, 100.0, 100.0, 100.0] if limits else None,
world_index=world_index,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.5, 0.5, 0.5),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_unary_revolute_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
dynamic: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test unary revolute joints.
This world consists of a single rigid body connected to the world via a unary
revolute joint, with optional limits applied to the joint degree of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degree of freedom.
ground (bool): Whether to include a ground plane in the world.
dynamic (bool): Whether to enable dynamic properties for the joint.
implicit_pd (bool): Whether to enable implicit PD control for the joint.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="unary_revolute_joint_test")
# Define test system
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, -0.25, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_follower_revolute",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=-1,
bid_F=bid_F,
B_r_Bj=vec3f(0.0, -0.15, z_offset),
F_r_Fj=vec3f(-0.5, 0.1, 0.0),
X_j=Axis.Y.to_mat33(),
q_j_min=[-0.25 * math.pi] if limits else None,
q_j_max=[0.25 * math.pi] if limits else None,
a_j=0.1 if dynamic else None,
b_j=0.01 if dynamic else None,
k_p_j=10.0 if implicit_pd else None,
k_d_j=0.01 if implicit_pd else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=-1,
shape=BoxShape(0.15, 0.15, 0.15),
world_index=world_index,
group=2,
collides=2,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.5, 0.1, 0.1),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_binary_revolute_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
dynamic: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test binary revolute joints.
This world consists of two rigid bodies connected via a binary revolute
joint, with optional limits applied to the joint degree of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degree of freedom.
ground (bool): Whether to include a ground plane in the world.
dynamic (bool): Whether to set the joint to be dynamic, with non-zero armature and damping.
implicit_pd (bool): Whether to use implicit PD control for the joint.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="binary_revolute_joint_test")
# Define test system
bid_B = _builder.add_rigid_body(
name="base",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, -0.25, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_base",
dof_type=JointDoFType.FIXED,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid_B,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
world_index=world_index,
)
_builder.add_joint(
name="base_to_follower_revolute",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=vec3f(0.0, -0.15, z_offset),
F_r_Fj=vec3f(-0.5, 0.1, 0.0),
X_j=Axis.Y.to_mat33(),
q_j_min=[-0.25 * math.pi] if limits else None,
q_j_max=[0.25 * math.pi] if limits else None,
a_j=0.1 if dynamic else None,
b_j=0.01 if dynamic else None,
k_p_j=10.0 if implicit_pd else None,
k_d_j=0.01 if implicit_pd else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=bid_B,
shape=BoxShape(0.15, 0.15, 0.15),
world_index=world_index,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.5, 0.1, 0.1),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_unary_prismatic_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
dynamic: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test unary prismatic joints.
This world consists of a single rigid body connected to the world via a unary
prismatic joint, with optional limits applied to the joint degree of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degree of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="unary_prismatic_joint_test")
# Define test system
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_follower_prismatic",
dof_type=JointDoFType.PRISMATIC,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=-1,
bid_F=bid_F,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Z.to_mat33(),
q_j_min=[-0.5] if limits else None,
q_j_max=[0.5] if limits else None,
a_j=0.1 if dynamic else None,
b_j=0.01 if dynamic else None,
k_p_j=10.0 if implicit_pd else None,
k_d_j=0.01 if implicit_pd else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=-1,
shape=BoxShape(0.025, 0.025, 0.5),
world_index=world_index,
group=2,
collides=2,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.05, 0.05, 0.05),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_binary_prismatic_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
dynamic: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test binary prismatic joints.
This world consists of two rigid bodies connected via a binary prismatic
joint, with optional limits applied to the joint degree of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degree of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="binary_prismatic_joint_test")
# Define test system
bid_B = _builder.add_rigid_body(
name="base",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_base",
dof_type=JointDoFType.FIXED,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid_B,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
world_index=world_index,
)
_builder.add_joint(
name="base_to_follower_prismatic",
dof_type=JointDoFType.PRISMATIC,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Z.to_mat33(),
q_j_min=[-0.5] if limits else None,
q_j_max=[0.5] if limits else None,
a_j=0.1 if dynamic else None,
b_j=0.01 if dynamic else None,
k_p_j=10.0 if implicit_pd else None,
k_d_j=0.01 if implicit_pd else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=bid_B,
shape=BoxShape(0.025, 0.025, 0.5),
world_index=world_index,
group=2,
collides=2,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.05, 0.05, 0.05),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_unary_cylindrical_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
dynamic: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test unary cylindrical joints.
This world consists of a single rigid body connected to the world via a unary
cylindrical joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
ground (bool): Whether to include a ground plane in the world.
dynamic (bool): Whether to enable dynamic properties for the joint.
implicit_pd (bool): Whether to enable implicit PD control for the joint.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="unary_cylindrical_joint_test")
# Define test system
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_follower_cylindrical",
dof_type=JointDoFType.CYLINDRICAL,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=-1,
bid_F=bid_F,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Z.to_mat33(),
q_j_min=[-0.5, -0.6 * math.pi] if limits else None,
q_j_max=[0.5, 0.6 * math.pi] if limits else None,
a_j=[0.1, 0.2] if dynamic else None,
b_j=[0.01, 0.02] if dynamic else None,
k_p_j=[10.0, 20.0] if implicit_pd else None,
k_d_j=[0.01, 0.02] if implicit_pd else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/cylinder",
body=-1,
shape=CylinderShape(0.025, 0.5),
world_index=world_index,
group=2,
collides=2,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.05, 0.05, 0.05),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_binary_cylindrical_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
dynamic: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test binary cylindrical joints.
This world consists of two rigid bodies connected via a binary cylindrical
joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
ground (bool): Whether to include a ground plane in the world.
dynamic (bool): Whether to enable dynamic properties for the joint.
implicit_pd (bool): Whether to enable implicit PD control for the joint.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="binary_cylindrical_joint_test")
# Define test system
bid_B = _builder.add_rigid_body(
name="base",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_base",
dof_type=JointDoFType.FIXED,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid_B,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
world_index=world_index,
)
_builder.add_joint(
name="base_to_follower_cylindrical",
dof_type=JointDoFType.CYLINDRICAL,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Z.to_mat33(),
q_j_min=[-0.5, -0.6 * math.pi] if limits else None,
q_j_max=[0.5, 0.6 * math.pi] if limits else None,
a_j=[0.1, 0.2] if dynamic else None,
b_j=[0.01, 0.02] if dynamic else None,
k_p_j=[10.0, 20.0] if implicit_pd else None,
k_d_j=[0.01, 0.02] if implicit_pd else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/cylinder",
body=bid_B,
shape=CylinderShape(0.025, 0.5),
world_index=world_index,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.05, 0.05, 0.05),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_unary_universal_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test unary universal joints.
This world consists of a single rigid body connected to the world via a unary
universal joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="unary_universal_joint_test")
# Define test system
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_follower_universal",
dof_type=JointDoFType.UNIVERSAL,
act_type=JointActuationType.FORCE,
bid_B=-1,
bid_F=bid_F,
B_r_Bj=vec3f(0.25, -0.25, -0.25),
F_r_Fj=vec3f(-0.25, -0.25, -0.25),
X_j=Axis.X.to_mat33(),
q_j_min=[-0.6 * math.pi, -0.6 * math.pi] if limits else None,
q_j_max=[0.6 * math.pi, 0.6 * math.pi] if limits else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=-1,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
group=2,
collides=2,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_binary_universal_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test binary universal joints.
This world consists of two rigid bodies connected via a binary universal
joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="binary_cylindrical_joint_test")
# Define test system
bid_B = _builder.add_rigid_body(
name="base",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_base",
dof_type=JointDoFType.FIXED,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid_B,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
world_index=world_index,
)
_builder.add_joint(
name="base_to_follower_universal",
dof_type=JointDoFType.UNIVERSAL,
act_type=JointActuationType.FORCE,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=vec3f(0.25, -0.25, -0.25),
F_r_Fj=vec3f(-0.25, -0.25, -0.25),
X_j=Axis.X.to_mat33(),
q_j_min=[-0.6 * math.pi, -0.6 * math.pi] if limits else None,
q_j_max=[0.6 * math.pi, 0.6 * math.pi] if limits else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=bid_B,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_unary_spherical_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test unary spherical joints.
This world consists of a single rigid body connected to the world via a unary
spherical joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="unary_spherical_joint_test")
# Define test system
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_follower_spherical",
dof_type=JointDoFType.SPHERICAL,
act_type=JointActuationType.FORCE,
bid_B=-1,
bid_F=bid_F,
B_r_Bj=vec3f(0.25, -0.25, -0.25),
F_r_Fj=vec3f(-0.25, -0.25, -0.25),
X_j=Axis.X.to_mat33(),
q_j_min=[-0.6 * math.pi, -0.6 * math.pi, -0.6 * math.pi] if limits else None,
q_j_max=[0.6 * math.pi, 0.6 * math.pi, 0.6 * math.pi] if limits else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=-1,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
group=2,
collides=2,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_binary_spherical_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test binary spherical joints.
This world consists of two rigid bodies connected via a binary spherical
joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="binary_spherical_joint_test")
# Define test system
bid_B = _builder.add_rigid_body(
name="base",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_base",
dof_type=JointDoFType.FIXED,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid_B,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
world_index=world_index,
)
_builder.add_joint(
name="base_to_follower_spherical",
dof_type=JointDoFType.SPHERICAL,
act_type=JointActuationType.FORCE,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=vec3f(0.25, -0.25, -0.25),
F_r_Fj=vec3f(-0.25, -0.25, -0.25),
X_j=Axis.X.to_mat33(),
q_j_min=[-0.6 * math.pi, -0.6 * math.pi, -0.6 * math.pi] if limits else None,
q_j_max=[0.6 * math.pi, 0.6 * math.pi, 0.6 * math.pi] if limits else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=bid_B,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_unary_gimbal_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test unary gimbal joints.
This world consists of a single rigid body connected to the world via a unary
gimbal joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="unary_gimbal_joint_test")
# Define test system
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_follower_gimbal",
dof_type=JointDoFType.GIMBAL,
act_type=JointActuationType.FORCE,
bid_B=-1,
bid_F=bid_F,
B_r_Bj=vec3f(0.25, -0.25, -0.25),
F_r_Fj=vec3f(-0.25, -0.25, -0.25),
X_j=Axis.X.to_mat33(),
# q_j_min=[-0.4 * math.pi, -0.4 * math.pi, -0.4 * math.pi] if limits else None,
# q_j_max=[0.4 * math.pi, 0.4 * math.pi, 0.4 * math.pi] if limits else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=-1,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
group=2,
collides=2,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_binary_gimbal_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test binary gimbal joints.
This world consists of two rigid bodies connected via a binary gimbal
joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
ground (bool): Whether to include a ground plane in the world.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="binary_gimbal_joint_test")
# Define test system
bid_B = _builder.add_rigid_body(
name="base",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_base",
dof_type=JointDoFType.FIXED,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid_B,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
world_index=world_index,
)
_builder.add_joint(
name="base_to_follower_gimbal",
dof_type=JointDoFType.GIMBAL,
act_type=JointActuationType.FORCE,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=vec3f(0.25, -0.25, -0.25),
F_r_Fj=vec3f(-0.25, -0.25, -0.25),
X_j=Axis.X.to_mat33(),
# q_j_min=[-0.4 * math.pi, -0.4 * math.pi, -0.4 * math.pi] if limits else None,
# q_j_max=[0.4 * math.pi, 0.4 * math.pi, 0.4 * math.pi] if limits else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=bid_B,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_unary_cartesian_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
dynamic: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test unary cartesian joints.
This world consists of a single rigid body connected to the world via a unary
cartesian joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
ground (bool): Whether to include a ground plane in the world.
dynamic (bool): Whether to enable dynamic properties for the joint.
implicit_pd (bool): Whether to enable implicit PD control for the joint.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="unary_cartesian_joint_test")
# Define test system
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_follower_cartesian",
dof_type=JointDoFType.CARTESIAN,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=-1,
bid_F=bid_F,
B_r_Bj=vec3f(0.25, -0.25, -0.25),
F_r_Fj=vec3f(-0.25, -0.25, -0.25),
X_j=Axis.X.to_mat33(),
q_j_min=[-1.0, -1.0, -1.0] if limits else None,
q_j_max=[1.0, 1.0, 1.0] if limits else None,
a_j=[0.1, 0.2, 0.3] if dynamic else None,
b_j=[0.01, 0.02, 0.03] if dynamic else None,
k_p_j=[10.0, 20.0, 30.0] if implicit_pd else None,
k_d_j=[0.01, 0.02, 0.03] if implicit_pd else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=-1,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
group=2,
collides=2,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_binary_cartesian_joint_test(
builder: ModelBuilderKamino | None = None,
z_offset: float = 0.0,
new_world: bool = True,
limits: bool = True,
ground: bool = True,
dynamic: bool = False,
implicit_pd: bool = False,
world_index: int = 0,
) -> ModelBuilderKamino:
"""
Builds a world to test binary cartesian joints.
This world consists of two rigid bodies connected via a binary cartesian
joint, with optional limits applied to the joint degrees of freedom.
Args:
builder (ModelBuilderKamino | None): An optional existing ModelBuilderKamino to which the entities will be added.
z_offset (float): A vertical offset to apply to the rigid body position.
new_world (bool): Whether to create a new world in the builder, to which entities will be added.\n
If `False`, the contents are added to the existing world specified by `world_index`.\n
If `True`, a new world is created and added to the builder. In this case the `world_index`
argument is ignored, and the index of the newly created world will be used instead.
limits (bool): Whether to enable limits on the joint degrees of freedom.
ground (bool): Whether to include a ground plane in the world.
dynamic (bool): Whether to enable dynamic properties for the joint.
implicit_pd (bool): Whether to enable implicit PD control for the joint.
world_index (int): The index of the world in the builder where the test model should be added.
"""
# Create a new builder if none is provided
if builder is None:
_builder = ModelBuilderKamino(default_world=False)
else:
_builder = builder
# Create a new world in the builder if requested or if a new builder was created
if new_world or builder is None:
world_index = _builder.add_world(name="binary_gimbal_joint_test")
# Define test system
bid_B = _builder.add_rigid_body(
name="base",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
bid_F = _builder.add_rigid_body(
name="follower",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.5, 0.0, z_offset), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
world_index=world_index,
)
_builder.add_joint(
name="world_to_base",
dof_type=JointDoFType.FIXED,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=bid_B,
B_r_Bj=vec3f(0.0, 0.0, z_offset),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=Axis.Y.to_mat33(),
world_index=world_index,
)
_builder.add_joint(
name="base_to_follower_cartesian",
dof_type=JointDoFType.CARTESIAN,
act_type=JointActuationType.POSITION_VELOCITY if implicit_pd else JointActuationType.FORCE,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=vec3f(0.25, -0.25, -0.25),
F_r_Fj=vec3f(-0.25, -0.25, -0.25),
X_j=Axis.X.to_mat33(),
q_j_min=[-1.0, -1.0, -1.0] if limits else None,
q_j_max=[1.0, 1.0, 1.0] if limits else None,
a_j=[0.1, 0.2, 0.3] if dynamic else None,
b_j=[0.01, 0.02, 0.03] if dynamic else None,
k_p_j=[10.0, 20.0, 30.0] if implicit_pd else None,
k_d_j=[0.01, 0.02, 0.03] if implicit_pd else None,
world_index=world_index,
)
_builder.add_geometry(
name="base/box",
body=bid_B,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
_builder.add_geometry(
name="follower/box",
body=bid_F,
shape=BoxShape(0.25, 0.25, 0.25),
world_index=world_index,
)
# Add a static collision geometry for the plane
if ground:
_builder.add_geometry(
body=-1,
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -1.5, 0.0, 0.0, 0.0, 1.0),
world_index=world_index,
)
# Return the populated builder
return _builder
def build_all_joints_test_model(
z_offset: float = 0.0,
ground: bool = False,
) -> ModelBuilderKamino:
"""
Constructs a model builder containing a world for each joint type.
Args:
z_offset (float): A vertical offset to apply to the initial position of the box.
ground (bool): Whether to add a static ground plane to the model.
Returns:
ModelBuilderKamino: The populated model builder.
"""
# Create a new builder to populate
_builder = ModelBuilderKamino(default_world=False)
# Add a new world for each joint type
_builder.add_builder(build_free_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_unary_revolute_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_binary_revolute_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_unary_prismatic_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_binary_prismatic_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_unary_cylindrical_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_binary_cylindrical_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_unary_universal_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_binary_universal_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_unary_spherical_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_binary_spherical_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_unary_gimbal_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_binary_gimbal_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_unary_cartesian_joint_test(z_offset=z_offset, ground=ground))
_builder.add_builder(build_binary_cartesian_joint_test(z_offset=z_offset, ground=ground))
# Return the lists of element indices
return _builder
###
# Builders - Geometry Tests
###
shape_name_to_type: dict[str, GeoType] = {
"sphere": GeoType.SPHERE,
"cylinder": GeoType.CYLINDER,
"cone": GeoType.CONE,
"capsule": GeoType.CAPSULE,
"box": GeoType.BOX,
"ellipsoid": GeoType.ELLIPSOID,
"plane": GeoType.PLANE,
}
"""Mapping from shape name to GeoType enum."""
shape_type_to_descriptor: dict[GeoType, ShapeDescriptorType] = {
GeoType.SPHERE: SphereShape,
GeoType.CYLINDER: CylinderShape,
GeoType.CONE: ConeShape,
GeoType.CAPSULE: CapsuleShape,
GeoType.BOX: BoxShape,
GeoType.ELLIPSOID: EllipsoidShape,
GeoType.PLANE: PlaneShape,
}
"""Mapping from GeoType enum to corresponding ShapeDescriptorType."""
shape_default_dims: dict[GeoType, tuple] = {
GeoType.SPHERE: (0.5,),
GeoType.CYLINDER: (0.5, 0.5),
GeoType.CONE: (0.5, 0.5),
GeoType.CAPSULE: (0.5, 0.5),
GeoType.BOX: (0.5, 0.5, 0.5),
GeoType.ELLIPSOID: (1.0, 1.0, 0.5),
GeoType.PLANE: (0.0, 0.0, 1.0, 0.0),
}
"""Mapping from GeoType enum to default dimensions (Newton convention: half-extents)."""
def make_shape_initial_position(name: str, dims: tuple, is_top: bool = True) -> vec3f:
"""
Computes the initial position along the z-axis for a given shape.
This function calculates the position required to place a shape just above
(or below) the origin along the z-axis, based on its type and dimensions.
Args:
name (str):
The name of the shape (e.g., "sphere", "box", "capsule", etc.).
dims (tuple):
The dimensions of the shape. The expected format depends on the shape type.
is_top (bool):
If True, computes the position for a top shape (above the origin).
If False, computes the position for a bottom shape (below the origin).
Returns:
vec3f:
The computed position vector along the z-axis.
"""
# Retrieve and check the shape type
shape_type = shape_name_to_type.get(name)
if shape_type is None:
raise ValueError(f"Unsupported shape name: {name}")
expected_len = {
GeoType.SPHERE: 1,
GeoType.CYLINDER: 2,
GeoType.CONE: 2,
GeoType.CAPSULE: 2,
GeoType.BOX: 3,
GeoType.ELLIPSOID: 3,
GeoType.PLANE: 4,
}.get(shape_type)
if expected_len is not None and len(dims) != expected_len:
raise ValueError(f"Invalid dimensions for shape '{name}': expected {expected_len} values, got {len(dims)}")
# Compute the initial position along z-axis that places the shape just above.
# Dimensions use Newton convention (half-extents, half-heights).
if shape_type == GeoType.SPHERE:
r = vec3f(0.0, 0.0, dims[0])
elif shape_type == GeoType.BOX:
r = vec3f(0.0, 0.0, dims[2])
elif shape_type == GeoType.CAPSULE:
r = vec3f(0.0, 0.0, dims[1] + dims[0])
elif shape_type == GeoType.CYLINDER:
r = vec3f(0.0, 0.0, dims[1])
elif shape_type == GeoType.CONE:
r = vec3f(0.0, 0.0, dims[1])
elif shape_type == GeoType.ELLIPSOID:
r = vec3f(0.0, 0.0, dims[2])
elif shape_type == GeoType.PLANE:
r = vec3f(0.0, 0.0, dims[3])
else:
raise ValueError(f"Unsupported shape type: {shape_type}")
# Invert the position if it's the bottom shape
if not is_top:
r = -r
# Return the computed position
return r
def get_shape_bottom_position(center: vec3f, shape: ShapeDescriptorType) -> vec3f:
"""
Computes the position of the bottom along the z-axis for a given shape.
Args:
center (vec3f):
The center position of the shape.
shape (ShapeDescriptorType):
The shape descriptor instance.
Returns:
vec3f:
The computed bottom position of the shape along the z-axis.
"""
# Compute and return the bottom position along z-axis.
# Shape params use Newton convention (half-extents, half-heights).
r_bottom = vec3f(0.0)
if shape.type == GeoType.SPHERE:
r_bottom = center - vec3f(0.0, 0.0, shape.params)
elif shape.type == GeoType.BOX:
r_bottom = center - vec3f(0.0, 0.0, shape.params[2])
elif shape.type == GeoType.CAPSULE:
r_bottom = center - vec3f(0.0, 0.0, shape.params[1] + shape.params[0])
elif shape.type == GeoType.CYLINDER:
r_bottom = center - vec3f(0.0, 0.0, shape.params[1])
elif shape.type == GeoType.CONE:
r_bottom = center - vec3f(0.0, 0.0, shape.params[1])
elif shape.type == GeoType.ELLIPSOID:
r_bottom = center - vec3f(0.0, 0.0, shape.params[2])
elif shape.type == GeoType.PLANE:
r_bottom = center - vec3f(0.0, 0.0, shape.params[3])
else:
raise ValueError(f"Unsupported shape type: {shape.type}")
# Return the bottom position of the given shape
return r_bottom
def make_single_shape_pair_builder(
shapes: tuple[str, str],
bottom_dims: tuple | None = None,
bottom_xyz: tuple | None = None,
bottom_rpy: tuple | None = None,
top_dims: tuple | None = None,
top_xyz: tuple | None = None,
top_rpy: tuple | None = None,
distance: float = 0.0,
ground_box: bool = False,
ground_plane: bool = False,
ground_z: float | None = None,
) -> ModelBuilderKamino:
"""
Generates a ModelBuilderKamino for a given shape combination with specified parameters.
The first shape in the combination is placed below the second shape along
the z-axis, effectively generating a "shape[0] atop shape[1]" configuration.
Args:
shapes (tuple[str, str]):
A tuple specifying the names of the bottom and top shapes (e.g., ("box", "sphere")).
bottom_dims (tuple | None):
Dimensions for the bottom shape. If None, defaults are used.
bottom_xyz (tuple | None):
Position (x, y, z) for the bottom shape. If None, defaults to (0, 0, 0).
bottom_rpy (tuple | None):
Orientation (roll, pitch, yaw) for the bottom shape. If None, defaults to (0, 0, 0).
top_dims (tuple | None):
Dimensions for the top shape. If None, defaults are used.
top_xyz (tuple | None):
Position (x, y, z) for the top shape. If None, defaults to (0, 0, 0).
top_rpy (tuple | None):
Orientation (roll, pitch, yaw) for the top shape. If None, defaults to (0, 0, 0).
distance (float):
Mutual distance along the z-axis between the two shapes.\n
If zero, the shapes are exactly touching.\n
If positive, they are separated by that distance.\n
If negative, they are penetrating by that distance.
Returns:
ModelBuilderKamino:
The constructed ModelBuilderKamino with the specified shape combination.
"""
# Check that the shape combination is tuple of strings
if not (isinstance(shapes, tuple) and len(shapes) == 2 and all(isinstance(s, str) for s in shapes)):
raise ValueError(f"Shape combination must be a tuple of two strings: {shapes}")
# Check that each shape name is valid
for shape_name in shapes:
if shape_name not in shape_name_to_type:
raise ValueError(f"Unsupported shape name: {shape_name}")
# Define bottom and top shapes
top = shapes[0]
bottom = shapes[1]
# Retrieve shape types
top_type = shape_name_to_type[top]
bottom_type = shape_name_to_type[bottom]
# Define default arguments for those not provided
if bottom_dims is None:
bottom_dims = shape_default_dims[bottom_type]
if bottom_xyz is None:
bottom_xyz = make_shape_initial_position(shapes[1], bottom_dims, is_top=False)
if bottom_rpy is None:
bottom_rpy = (0.0, 0.0, 0.0)
if top_dims is None:
top_dims = shape_default_dims[top_type]
if top_xyz is None:
top_xyz = make_shape_initial_position(shapes[0], top_dims, is_top=True)
if top_rpy is None:
top_rpy = (0.0, 0.0, 0.0)
# Retrieve the shape type
bottom_descriptor = shape_type_to_descriptor[bottom_type]
top_descriptor = shape_type_to_descriptor[top_type]
# Define the mutual separation along z-axis
r_dz = vec3f(0.0, 0.0, 0.5 * distance)
# Compute bottom box position and orientation
r_b = vec3f(bottom_xyz) - r_dz
q_b = quat_from_euler_xyz(vec3f(*bottom_rpy))
# Compute top sphere position and orientation
r_t = vec3f(top_xyz) + r_dz
q_t = quat_from_euler_xyz(vec3f(*top_rpy))
# Create the shape descriptors for bottom and top shapes
# with special handling for PlaneShape
if bottom_type == GeoType.PLANE:
bottom_shape = bottom_descriptor(vec3f(*bottom_dims[0:3]), bottom_dims[3])
else:
bottom_shape = bottom_descriptor(*bottom_dims)
if top_type == GeoType.PLANE:
top_shape = top_descriptor(vec3f(*top_dims[0:3]), top_dims[3])
else:
top_shape = top_descriptor(*top_dims)
# Create model builder and add corresponding bodies and their collision geometries
builder: ModelBuilderKamino = ModelBuilderKamino(default_world=True)
bid0 = builder.add_rigid_body(
name="bottom_" + bottom,
m_i=1.0,
i_I_i=mat33f(np.eye(3, dtype=np.float32)),
q_i_0=transformf(r_b, q_b),
)
bid1 = builder.add_rigid_body(
name="top_" + top,
m_i=1.0,
i_I_i=mat33f(np.eye(3, dtype=np.float32)),
q_i_0=transformf(r_t, q_t),
)
builder.add_geometry(body=bid0, name="bottom_" + bottom, shape=bottom_shape)
builder.add_geometry(body=bid1, name="top_" + top, shape=top_shape)
# Optionally add a ground geom below the bottom shape
if ground_box or ground_plane:
if ground_z is not None:
z_g_offset = ground_z
else:
z_g_offset = float(get_shape_bottom_position(r_b, bottom_shape).z - r_dz.z)
if ground_box:
utils.add_ground_box(builder, z_offset=z_g_offset)
if ground_plane:
utils.add_ground_plane(builder, z_offset=z_g_offset)
# Debug output
msg.debug(
"[%s]:\nBODIES:\n%s\nGEOMS:\n%s\n",
shapes,
builder.bodies,
builder.geoms,
)
# Return the constructed builder
return builder
def make_shape_pairs_builder(
shape_pairs: list[tuple[str, str]],
per_shape_pair_args: dict | None = None,
distance: float | None = None,
ground_box: bool = False,
ground_plane: bool = False,
ground_z: float | None = None,
) -> ModelBuilderKamino:
"""
Generates a builder containing a world for each specified shape combination.
Args:
shape_pairs (list[tuple[str, str]]):
A list of tuples specifying the names of the bottom and top shapes
for each combination (e.g., [("box", "sphere"), ("cylinder", "cone")]).
**kwargs:
Additional keyword arguments to be passed to `make_single_shape_pair_builder`.
Returns:
ModelBuilderKamino
A ModelBuilderKamino containing a world for each specified shape combination.
"""
# Create an empty ModelBuilderKamino to hold all shape pair worlds
builder = ModelBuilderKamino(default_world=False)
# Iterate over each shape pair and add its builder to the main builder
for shapes in shape_pairs:
# Set shape-pair-specific arguments if provided
if per_shape_pair_args is not None:
# Check if per_shape_pair_args contains arguments for this shape pair
shape_pair_args = per_shape_pair_args.get(shapes, {})
else:
shape_pair_args = {}
# Override distance if specified
if distance is not None:
shape_pair_args["distance"] = distance
# Create the single shape pair builder and add it to the main builder
single_pair_builder = make_single_shape_pair_builder(
shapes, ground_box=ground_box, ground_plane=ground_plane, ground_z=ground_z, **shape_pair_args
)
builder.add_builder(single_pair_builder)
# Return the combined builder
return builder
================================================
FILE: newton/_src/solvers/kamino/_src/models/builders/utils.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides utility functions for model
builder composition and manipulation.
This module includes functions to add common
modifiers to model builders, such as ground
planes, as well as factory functions to create
homogeneous multi-world builders and import
USD models.
"""
from collections.abc import Callable
import warp as wp
from ...core.builder import ModelBuilderKamino
from ...core.shapes import BoxShape, PlaneShape
from ...core.types import transformf, vec3f, vec6f
from ...utils.io.usd import USDImporter
###
# Module interface
###
__all__ = [
"add_ground_box",
"add_ground_plane",
"build_usd",
"make_homogeneous_builder",
"set_uniform_body_pose_offset",
"set_uniform_body_twist_offset",
]
###
# Modifiers
###
def add_ground_plane(
builder: ModelBuilderKamino,
group: int = 1,
collides: int = 1,
world_index: int = 0,
z_offset: float = 0.0,
) -> int:
"""
Adds a static plane geometry to a given builder to represent a flat ground with infinite dimensions.
Args:
builder (ModelBuilderKamino):
The model builder to which the ground plane should be added.
group (int):
The collision group for the ground geometry.\n
Defaults to `1`.
collides (int):
The collision mask for the ground geometry.\n
Defaults to `1`.
world_index (int):
The index of the world in the builder where the ground geometry should be added.\n
If the value does not correspond to an existing world an error will be raised.\n
Defaults to `0`.
z_offset (float):
The vertical offset of the ground plane along the Z axis.\n
Defaults to `0.0`.
Returns:
int: The ID of the added ground geometry.
"""
return builder.add_geometry(
shape=PlaneShape(vec3f(0.0, 0.0, 1.0), 0.0),
offset=transformf(0.0, 0.0, z_offset, 0.0, 0.0, 0.0, 1.0),
name="ground",
group=group,
collides=collides,
world_index=world_index,
)
def add_ground_box(
builder: ModelBuilderKamino,
group: int = 1,
collides: int = 1,
world_index: int = 0,
z_offset: float = 0.0,
) -> int:
"""
Adds a static box geometry to a given builder to represent a flat ground with finite dimensions.
Args:
builder (ModelBuilderKamino):
The model builder to which the ground box should be added.
group (int):
The collision group for the ground geometry.\n
Defaults to `1`.
collides (int):
The collision mask for the ground geometry.\n
Defaults to `1`.
world_index (int):
The index of the world in the builder where the ground geometry should be added.\n
If the value does not correspond to an existing world an error will be raised.\n
Defaults to `0`.
z_offset (float):
The vertical offset of the ground box along the Z axis.\n
Defaults to `0.0`.
Returns:
int: The ID of the added ground geometry.
"""
return builder.add_geometry(
shape=BoxShape(10.0, 10.0, 0.5),
offset=transformf(0.0, 0.0, -0.5 + z_offset, 0.0, 0.0, 0.0, 1.0),
name="ground",
group=group,
collides=collides,
world_index=world_index,
)
def set_uniform_body_pose_offset(builder: ModelBuilderKamino, offset: transformf):
"""
Offsets the initial poses of all rigid bodies existing in the builder uniformly by the specified offset.
Args:
builder (ModelBuilderKamino): The model builder containing the bodies to offset.
offset (transformf): The pose offset to apply to each body in the builder in the form of a :class:`transformf`.
"""
for i in range(builder.num_bodies):
builder.bodies[i].q_i_0 = wp.mul(offset, builder.bodies[i].q_i_0)
def set_uniform_body_twist_offset(builder: ModelBuilderKamino, offset: vec6f):
"""
Offsets the initial twists of all rigid bodies existing in the builder uniformly by the specified offset.
Args:
builder (ModelBuilderKamino): The model builder containing the bodies to offset.
offset (vec6f): The twist offset to apply to each body in the builder in the form of a :class:`vec6f`.
"""
for i in range(builder.num_bodies):
builder.bodies[i].u_i_0 += offset
###
# Builder utilities
###
def build_usd(
source: str,
load_drive_dynamics: bool = True,
load_static_geometry: bool = True,
ground: bool = True,
) -> ModelBuilderKamino:
"""
Imports a USD model and optionally adds a ground plane.
Each call creates a new world with the USD model and optional ground plane.
Args:
source: Path to USD file
load_drive_dynamics: Whether to load drive parameters from USD. Necessary for using implicit PD
load_static_geometry: Whether to load static geometry from USD
ground: Whether to add a ground plane
Returns:
ModelBuilderKamino with imported USD model and optional ground plane
"""
# Import the USD model
importer = USDImporter()
_builder = importer.import_from(
source=source,
load_drive_dynamics=load_drive_dynamics,
load_static_geometry=load_static_geometry,
)
# Optionally add ground geometry
if ground:
add_ground_box(builder=_builder, group=1, collides=1)
# Return the builder constructed from the USD model
return _builder
def make_homogeneous_builder(num_worlds: int, build_fn: Callable, **kwargs) -> ModelBuilderKamino:
"""
Utility factory function to create a multi-world builder with identical worlds replicated across the model.
Args:
num_worlds (int): The number of worlds to create.
build_fn (callable): The model builder function to use.
**kwargs: Additional keyword arguments to pass to the builder function.
Returns:
ModelBuilderKamino: The constructed model builder.
"""
# First build a single world
# NOTE: We want to do this first to avoid re-constructing the same model multiple
# times especially if the construction is expensive such as importing from USD.
single = build_fn(**kwargs)
# Then replicate it across the specified number of worlds
builder = ModelBuilderKamino(default_world=False)
for _ in range(num_worlds):
builder.add_builder(single)
return builder
================================================
FILE: newton/_src/solvers/kamino/_src/solver_kamino_impl.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Defines the :class:`SolverKaminoImpl` class, providing a physics backend for
simulating constrained multi-body systems for arbitrary mechanical assemblies.
"""
from __future__ import annotations
from collections.abc import Callable
import warp as wp
# Newton imports
from ....core.types import override
from ....sim import Contacts, State
from ...solver import SolverBase
# Kamino imports
from ..solver_kamino import SolverKamino
from .core.bodies import update_body_inertias, update_body_wrenches
from .core.control import ControlKamino
from .core.data import DataKamino
from .core.joints import JointCorrectionMode
from .core.model import ModelKamino
from .core.state import StateKamino
from .core.time import advance_time
from .core.types import float32, int32, transformf, vec6f
from .dynamics.dual import DualProblem
from .dynamics.wrenches import (
compute_constraint_body_wrenches,
compute_joint_dof_body_wrenches,
)
from .geometry.contacts import ContactsKamino
from .geometry.detector import CollisionDetector
from .integrators import IntegratorEuler, IntegratorMoreauJean
from .kinematics.constraints import (
make_unilateral_constraints_info,
unpack_constraint_solutions,
update_constraints_info,
)
from .kinematics.jacobians import DenseSystemJacobians, SparseSystemJacobians
from .kinematics.joints import (
compute_joints_data,
extract_actuators_state_from_joints,
extract_joints_state_from_actuators,
)
from .kinematics.limits import LimitsKamino
from .kinematics.resets import (
reset_body_net_wrenches,
reset_joint_constraint_reactions,
reset_state_from_base_state,
reset_state_from_bodies_state,
reset_state_to_model_default,
reset_time,
)
from .linalg import ConjugateResidualSolver, IterativeSolver, LinearSolverNameToType
from .solvers.fk import ForwardKinematicsSolver
from .solvers.metrics import SolutionMetrics
from .solvers.padmm import PADMMSolver, PADMMWarmStartMode
from .solvers.warmstart import WarmstarterContacts, WarmstarterLimits
from .utils import logger as msg
###
# Module interface
###
__all__ = [
"SolverKaminoImpl",
]
###
# Interfaces
###
class SolverKaminoImpl(SolverBase):
"""
The :class:`SolverKaminoImpl` class implements the core Kamino physics solver.
This class currently holds the actual implementation of the solver, and is wrapped
by the upper-level :class:`SolverKamino` class which serves as the main user-facing
API for now. In the future, a complete refactoring of Kamino will integrate the solver
and underlying components with Newton end-to-end and completely. At that point, the
:class:`SolverKaminoImpl` class will be removed and its implementation merged into
the :class:`SolverKamino` class.
"""
Config = SolverKamino.Config
"""
Defines a type alias of the PADMM solver configurations container, including convergence
criteria, maximum iterations, and options for the linear solver and preconditioning.
See :class:`PADMMSolverConfig` for the full list of configuration options and their descriptions.
"""
ResetCallbackType = Callable[["SolverKaminoImpl", StateKamino], None]
"""Defines the type signature for reset callback functions."""
StepCallbackType = Callable[["SolverKaminoImpl", StateKamino, StateKamino, ControlKamino, ContactsKamino], None]
"""Defines the type signature for step callback functions."""
def __init__(
self,
model: ModelKamino,
contacts: ContactsKamino | None = None,
config: SolverKaminoImpl.Config | None = None,
):
"""
Initializes the Kamino physics solver for the given set of multi-body systems
defined in `model`, and the total contact allocations defined in `contacts`.
Explicit solver config may be provided through the `config` argument. If no
config is provided, a default config will be used.
Args:
model (ModelKamino): The multi-body systems model to simulate.
contacts (ContactsKamino): The contact data container for the simulation.
config (SolverKaminoImpl.Config | None): Optional solver config.
"""
# Ensure the input containers are valid
if not isinstance(model, ModelKamino):
raise TypeError(f"Invalid model container: Expected a `ModelKamino` instance, but got {type(model)}.")
if contacts is not None and not isinstance(contacts, ContactsKamino):
raise TypeError(
f"Invalid contacts container: Expected a `ContactsKamino` instance, but got {type(contacts)}."
)
if config is not None and not isinstance(config, SolverKaminoImpl.Config):
raise TypeError(
f"Invalid solver config: Expected a `SolverKaminoImpl.Config` instance, but got {type(config)}."
)
# First initialize the base solver
# NOTE: Although we pass the model here, we will re-assign it below
# since currently Kamino defines its own :class`ModelKamino` class.
super().__init__(model=model)
self._model = model
# If no explicit config is provided, attempt to create a config
# from the model attributes (e.g. if imported from USD assets).
# NOTE: `Config.from_model` will default-initialize if no relevant custom attributes were
# found on the model, so `self._config` will always be fully initialized after this step.
if config is None:
config = self.Config.from_model(model._model)
# Validate the solver configurations and raise errors early if invalid
config.validate()
# Cache the solver config and parse relevant options for internal use
self._config: SolverKaminoImpl.Config = config
self._warmstart_mode: PADMMWarmStartMode = PADMMWarmStartMode.from_string(config.padmm.warmstart_mode)
self._rotation_correction: JointCorrectionMode = JointCorrectionMode.from_string(config.rotation_correction)
# ---------------------------------------------------------------------------
# TODO: Migrate this entire section into the constructor of `DualProblem`
# Convert the linear solver type from the config literal to the concrete class, raising an error if invalid
linear_solver_type = LinearSolverNameToType.get(self._config.dynamics.linear_solver_type, None)
if linear_solver_type is None:
raise ValueError(
"Invalid linear solver type: Expected one of "
f"{list(LinearSolverNameToType.keys())}, got '{linear_solver_type}'."
)
# Override the linear solver type to an iterative solver if
# sparsity is enabled but the provided solver is not iterative
if self._config.sparse_dynamics and not issubclass(linear_solver_type, IterativeSolver):
msg.warning(
f"Sparse dynamics requires an iterative solver, but got '{linear_solver_type.__name__}'."
" Defaulting to 'ConjugateResidualSolver' as the PADMM linear solver."
)
linear_solver_type = ConjugateResidualSolver
# If graph conditionals are disabled in the PADMM solver, ensure that they
# are also disabled in the linear solver if it is an iterative solver.
linear_solver_kwargs = dict(self._config.dynamics.linear_solver_kwargs)
if not self._config.padmm.use_graph_conditionals and issubclass(linear_solver_type, IterativeSolver):
linear_solver_kwargs.setdefault("use_graph_conditionals", False)
# Bundle both constraint stabilization and forward-
# dynamics problem configurations into a single object
problem_fd_config = DualProblem.Config(
constraints=self._config.constraints,
dynamics=self._config.dynamics,
# TODO: linear_solver_type=linear_solver_type,
# TODO: linear_solver_kwargs=linear_solver_kwargs,
# TODO: sparse=bool(self._config.sparse_dynamics),
)
# ---------------------------------------------------------------------------
# Allocate internal time-varying solver data
self._data = self._model.data()
# Allocate a joint-limits interface
self._limits = LimitsKamino(model=self._model, device=self._model.device)
# Construct the unilateral constraints members in the model info
make_unilateral_constraints_info(model=self._model, data=self._data, limits=self._limits, contacts=contacts)
# Allocate Jacobians data on the device
if self._config.sparse_jacobian:
self._jacobians = SparseSystemJacobians(
model=self._model,
limits=self._limits,
contacts=contacts,
device=self._model.device,
)
else:
self._jacobians = DenseSystemJacobians(
model=self._model,
limits=self._limits,
contacts=contacts,
device=self._model.device,
)
# Allocate the dual problem data on the device
self._problem_fd = DualProblem(
model=self._model,
data=self._data,
limits=self._limits,
contacts=contacts,
config=problem_fd_config,
solver=linear_solver_type,
solver_kwargs=linear_solver_kwargs,
sparse=self._config.sparse_dynamics,
device=self._model.device,
)
# Allocate the forward dynamics solver on the device
self._solver_fd = PADMMSolver(
model=self._model,
config=self._config.padmm,
warmstart=self._warmstart_mode,
use_acceleration=self._config.padmm.use_acceleration,
use_graph_conditionals=self._config.padmm.use_graph_conditionals,
collect_info=self._config.collect_solver_info,
device=self._model.device,
)
# Allocate the forward kinematics solver on the device
self._solver_fk = None
if self._config.use_fk_solver:
self._solver_fk = ForwardKinematicsSolver(model=self._model, config=self._config.fk)
# Create the time-integrator instance based on the config
if self._config.integrator == "euler":
self._integrator = IntegratorEuler(model=self._model)
elif self._config.integrator == "moreau":
self._integrator = IntegratorMoreauJean(model=self._model)
else:
raise ValueError(
f"Unsupported integrator type: Expected 'euler' or 'moreau', but got {self._config.integrator}."
)
# Allocate additional internal data for reset operations
with wp.ScopedDevice(self._model.device):
self._all_worlds_mask = wp.ones(shape=(self._model.size.num_worlds,), dtype=int32)
self._base_q = wp.zeros(shape=(self._model.size.num_worlds,), dtype=transformf)
self._base_u = wp.zeros(shape=(self._model.size.num_worlds,), dtype=vec6f)
self._bodies_u_zeros = wp.zeros(shape=(self._model.size.sum_of_num_bodies,), dtype=vec6f)
self._actuators_q = wp.zeros(shape=(self._model.size.sum_of_num_actuated_joint_coords,), dtype=float32)
self._actuators_u = wp.zeros(shape=(self._model.size.sum_of_num_actuated_joint_dofs,), dtype=float32)
# Allocate the contacts warmstarter if enabled
self._ws_limits: WarmstarterLimits | None = None
self._ws_contacts: WarmstarterContacts | None = None
if self._warmstart_mode == PADMMWarmStartMode.CONTAINERS:
self._ws_limits = WarmstarterLimits(limits=self._limits)
self._ws_contacts = WarmstarterContacts(
contacts=contacts,
method=WarmstarterContacts.Method.from_string(self._config.padmm.contact_warmstart_method),
)
# Allocate the solution metrics evaluator if enabled
self._metrics: SolutionMetrics | None = None
if self._config.compute_solution_metrics:
self._metrics = SolutionMetrics(model=self._model)
# Initialize callbacks
self._pre_reset_cb: SolverKaminoImpl.ResetCallbackType | None = None
self._post_reset_cb: SolverKaminoImpl.ResetCallbackType | None = None
self._pre_step_cb: SolverKaminoImpl.StepCallbackType | None = None
self._mid_step_cb: SolverKaminoImpl.StepCallbackType | None = None
self._post_step_cb: SolverKaminoImpl.StepCallbackType | None = None
# Initialize all internal solver data
with wp.ScopedDevice(self._model.device):
self._reset()
###
# Properties
###
@property
def config(self) -> SolverKaminoImpl.Config:
"""
Returns the host-side cache of high-level solver config.
"""
return self._config
@property
def device(self) -> wp.DeviceLike:
"""
Returns the device where the solver data is allocated.
"""
return self._model.device
@property
def data(self) -> DataKamino:
"""
Returns the internal solver data container.
"""
return self._data
@property
def problem_fd(self) -> DualProblem:
"""
Returns the dual forward dynamics problem.
"""
return self._problem_fd
@property
def solver_fd(self) -> PADMMSolver:
"""
Returns the forward dynamics solver.
"""
return self._solver_fd
@property
def solver_fk(self) -> ForwardKinematicsSolver | None:
"""
Returns the forward kinematics solver backend, if it was initialized.
"""
return self._solver_fk
@property
def metrics(self) -> SolutionMetrics | None:
"""
Returns the solution metrics evaluator, if enabled.
"""
return self._metrics
###
# Configurations
###
def set_pre_reset_callback(self, callback: ResetCallbackType):
"""
Set a reset callback to be called at the beginning of each call to `reset_*()` methods.
"""
self._pre_reset_cb = callback
def set_post_reset_callback(self, callback: ResetCallbackType):
"""
Set a reset callback to be called at the end of each call to to `reset_*()` methods.
"""
self._post_reset_cb = callback
def set_pre_step_callback(self, callback: StepCallbackType):
"""
Sets a callback to be called before forward dynamics solve.
"""
self._pre_step_cb = callback
def set_mid_step_callback(self, callback: StepCallbackType):
"""
Sets a callback to be called between forward dynamics solver and state integration.
"""
self._mid_step_cb = callback
def set_post_step_callback(self, callback: StepCallbackType):
"""
Sets a callback to be called after state integration.
"""
self._post_step_cb = callback
###
# Solver API
###
def reset(
self,
state_out: StateKamino,
world_mask: wp.array | None = None,
actuator_q: wp.array | None = None,
actuator_u: wp.array | None = None,
joint_q: wp.array | None = None,
joint_u: wp.array | None = None,
base_q: wp.array | None = None,
base_u: wp.array | None = None,
bodies_q: wp.array | None = None,
bodies_u: wp.array | None = None,
):
"""
Resets the simulation state given a combination of desired base body
and joint states, as well as an optional per-world mask array indicating
which worlds should be reset. The reset state is written to `state_out`.
For resets given absolute quantities like base body poses, the
`state_out` must initially contain the current state of the simulation.
Args:
state_out (StateKamino):
The output state container to which the reset state data is written.
world_mask (wp.array, optional):
Optional array of per-world masks indicating which worlds should be reset.\n
Shape of `(num_worlds,)` and type :class:`wp.int8 | wp.bool`
actuator_q (wp.array, optional):
Optional array of target actuated joint coordinates.\n
Shape of `(num_actuated_joint_coords,)` and type :class:`wp.float32`
actuator_u (wp.array, optional):
Optional array of target actuated joint DoF velocities.\n
Shape of `(num_actuated_joint_dofs,)` and type :class:`wp.float32`
joint_q (wp.array, optional):
Optional array of target joint coordinates.\n
Shape of `(num_joint_coords,)` and type :class:`wp.float32`
joint_u (wp.array, optional):
Optional array of target joint DoF velocities.\n
Shape of `(num_joint_dofs,)` and type :class:`wp.float32`
base_q (wp.array, optional):
Optional array of target base body poses.\n
Shape of `(num_worlds,)` and type :class:`wp.transformf`
base_u (wp.array, optional):
Optional array of target base body twists.\n
Shape of `(num_worlds,)` and type :class:`wp.spatial_vectorf`
bodies_q (wp.array, optional):
Optional array of target body poses.\n
Shape of `(num_bodies,)` and type :class:`wp.transformf`
bodies_u (wp.array, optional):
Optional array of target body twists.\n
Shape of `(num_bodies,)` and type :class:`wp.spatial_vectorf`
"""
# Ensure the input reset targets are valid
def _check_length(data: wp.array, name: str, expected: int):
if data is not None and data.shape[0] != expected:
raise ValueError(f"Invalid {name} shape: Expected ({expected},), but got {data.shape}.")
_check_length(joint_q, "joint_q", self._model.size.sum_of_num_joint_coords)
_check_length(joint_u, "joint_u", self._model.size.sum_of_num_joint_dofs)
_check_length(actuator_q, "actuator_q", self._model.size.sum_of_num_actuated_joint_coords)
_check_length(actuator_u, "actuator_u", self._model.size.sum_of_num_actuated_joint_dofs)
_check_length(base_q, "base_q", self._model.size.num_worlds)
_check_length(base_u, "base_u", self._model.size.num_worlds)
_check_length(bodies_q, "bodies_q", self._model.size.sum_of_num_bodies)
_check_length(bodies_u, "bodies_u", self._model.size.sum_of_num_bodies)
_check_length(world_mask, "world_mask", self._model.size.num_worlds)
# Ensure that only joint or actuator targets are provided
if (joint_q is not None or joint_u is not None) and (actuator_q is not None or actuator_u is not None):
raise ValueError("Combined joint and actuator targets are not supported. Only one type may be provided.")
# Ensure that joint/actuator velocity-only resets are prevented
if (joint_q is None and joint_u is not None) or (actuator_q is None and actuator_u is not None):
raise ValueError("Velocity-only joint or actuator resets are not supported.")
# Run the pre-reset callback if it has been set
self._run_pre_reset_callback(state_out=state_out)
# Determine the effective world mask to use for the reset operation
_world_mask = world_mask if world_mask is not None else self._all_worlds_mask
# Detect mode
base_reset = base_q is not None or base_u is not None
joint_reset = joint_q is not None or actuator_q is not None
bodies_reset = bodies_q is not None or bodies_u is not None
# If no reset targets are provided, reset all bodies to the model default state
if not base_reset and not joint_reset and not bodies_reset:
self._reset_to_default_state(
state_out=state_out,
world_mask=_world_mask,
)
# If only base targets are provided, uniformly reset all bodies to the given base states
elif base_reset and not joint_reset and not bodies_reset:
self._reset_to_base_state(
state_out=state_out,
world_mask=_world_mask,
base_q=base_q,
base_u=base_u,
)
# If a joint target is provided, use the FK solver to reset the bodies accordingly
elif joint_reset and not bodies_reset:
self._reset_with_fk_solve(
state_out=state_out,
world_mask=_world_mask,
actuator_q=actuator_q,
actuator_u=actuator_u,
joint_q=joint_q,
joint_u=joint_u,
base_q=base_q,
base_u=base_u,
)
# If body targets are provided, reset bodies directly
elif not base_reset and not joint_reset and bodies_reset:
self._reset_to_bodies_state(
state_out=state_out,
world_mask=_world_mask,
bodies_q=bodies_q,
bodies_u=bodies_u,
)
# If no valid combination of reset targets is provided, raise an error
else:
raise ValueError(
"Unsupported reset combination with: "
f" actuator_q: {actuator_q is not None}, actuator_u: {actuator_u is not None},"
f" joint_q: {joint_q is not None}, joint_u: {joint_u is not None},"
f" base_q: {base_q is not None}, base_u: {base_u is not None}."
f" bodies_q: {bodies_q is not None}, bodies_u: {bodies_u is not None}."
)
# Post-process the reset operation
self._reset_post_process(world_mask=_world_mask)
# Run the post-reset callback if it has been set
self._run_post_reset_callback(state_out=state_out)
@override
def step(
self,
state_in: StateKamino,
state_out: StateKamino,
control: ControlKamino,
contacts: ContactsKamino | None = None,
detector: CollisionDetector | None = None,
dt: float | None = None,
):
"""
Progresses the simulation by a single time-step `dt` given the current
state `state_in`, control inputs `control`, and set of active contacts
`contacts`. The updated state is written to `state_out`.
Args:
state_in (StateKamino):
The input current state of the simulation.
state_out (StateKamino):
The output next state after time integration.
control (ControlKamino):
The input controls applied to the system.
contacts (ContactsKamino, optional):
The set of active contacts.
detector (CollisionDetector, optional):
An optional collision detector to use for generating contacts at the current state.\n
If `None`, the `contacts` data will be used as the current set of active contacts.
dt (float, optional):
A uniform time-step to apply uniformly to all worlds of the simulation.
"""
# If specified, configure the internal per-world solver time-step uniformly from the input argument
if dt is not None:
self._model.time.set_uniform_timestep(dt)
# Copy the new input state and control to the internal solver data
self._read_step_inputs(state_in=state_in, control_in=control)
# Execute state integration:
# - Optionally calls limit and contact detection to generate unilateral constraints
# - Solves the forward dynamics sub-problem to compute constraint reactions
# - Integrates the state forward in time
self._integrator.integrate(
forward=self._solve_forward_dynamics,
model=self._model,
data=self._data,
state_in=state_in,
state_out=state_out,
control=control,
limits=self._limits,
contacts=contacts,
detector=detector,
)
# Update the internal joint states from the
# updated body states after time-integration
self._update_joints_data()
# Compute solver solution metrics if enabled
self._compute_metrics(state_in=state_in, contacts=contacts)
# Update time-keeping (i.e. physical time and discrete steps)
self._advance_time()
# Run the post-step callback if it has been set
self._run_poststep_callback(state_in, state_out, control, contacts)
# Copy the updated internal solver state to the output state
self._write_step_output(state_out=state_out)
@override
def notify_model_changed(self, flags: int) -> None:
pass # TODO: Migrate implementation when we fully integrate with Newton
@override
def update_contacts(self, contacts: Contacts, state: State | None = None) -> None:
pass # TODO: Migrate implementation when we fully integrate with Newton
@override
@classmethod
def register_custom_attributes(cls, flags: int):
pass # TODO: Migrate implementation when we fully integrate with Newton
###
# Internals - Callback Operations
###
def _run_pre_reset_callback(self, state_out: StateKamino):
"""
Runs the pre-reset callback if it has been set.
"""
if self._pre_reset_cb is not None:
self._pre_reset_cb(self, state_out)
def _run_post_reset_callback(self, state_out: StateKamino):
"""
Runs the post-reset callback if it has been set.
"""
if self._post_reset_cb is not None:
self._post_reset_cb(self, state_out)
def _run_prestep_callback(
self, state_in: StateKamino, state_out: StateKamino, control: ControlKamino, contacts: ContactsKamino
):
"""
Runs the pre-step callback if it has been set.
"""
if self._pre_step_cb is not None:
self._pre_step_cb(self, state_in, state_out, control, contacts)
def _run_midstep_callback(
self, state_in: StateKamino, state_out: StateKamino, control: ControlKamino, contacts: ContactsKamino
):
"""
Runs the mid-step callback if it has been set.
"""
if self._mid_step_cb is not None:
self._mid_step_cb(self, state_in, state_out, control, contacts)
def _run_poststep_callback(
self, state_in: StateKamino, state_out: StateKamino, control: ControlKamino, contacts: ContactsKamino
):
"""
Executes the post-step callback if it has been set.
"""
if self._post_step_cb is not None:
self._post_step_cb(self, state_in, state_out, control, contacts)
###
# Internals - Input/Output Operations
###
def _read_step_inputs(self, state_in: StateKamino, control_in: ControlKamino):
"""
Updates the internal solver data from the input state and control.
"""
# TODO: Remove corresponding data copies
# by directly using the input containers
wp.copy(self._data.bodies.q_i, state_in.q_i)
wp.copy(self._data.bodies.u_i, state_in.u_i)
wp.copy(self._data.bodies.w_i, state_in.w_i)
wp.copy(self._data.bodies.w_e_i, state_in.w_i_e)
wp.copy(self._data.joints.q_j, state_in.q_j)
wp.copy(self._data.joints.q_j_p, state_in.q_j_p)
wp.copy(self._data.joints.dq_j, state_in.dq_j)
wp.copy(self._data.joints.lambda_j, state_in.lambda_j)
wp.copy(self._data.joints.tau_j, control_in.tau_j)
wp.copy(self._data.joints.q_j_ref, control_in.q_j_ref)
wp.copy(self._data.joints.dq_j_ref, control_in.dq_j_ref)
wp.copy(self._data.joints.tau_j_ref, control_in.tau_j_ref)
def _write_step_output(self, state_out: StateKamino):
"""
Updates the output state from the internal solver data.
"""
# TODO: Remove corresponding data copies
# by directly using the input containers
wp.copy(state_out.q_i, self._data.bodies.q_i)
wp.copy(state_out.u_i, self._data.bodies.u_i)
wp.copy(state_out.w_i, self._data.bodies.w_i)
wp.copy(state_out.w_i_e, self._data.bodies.w_e_i)
wp.copy(state_out.q_j, self._data.joints.q_j)
wp.copy(state_out.q_j_p, self._data.joints.q_j_p)
wp.copy(state_out.dq_j, self._data.joints.dq_j)
wp.copy(state_out.lambda_j, self._data.joints.lambda_j)
###
# Internals - Reset Operations
###
def _reset(self):
"""
Performs a hard-reset of all solver internal data.
"""
# Reset internal time-keeping data
self._data.time.reset()
# Reset all bodies to their model default states
self._data.bodies.clear_all_wrenches()
wp.copy(self._data.bodies.q_i, self._model.bodies.q_i_0)
wp.copy(self._data.bodies.u_i, self._model.bodies.u_i_0)
update_body_inertias(model=self._model.bodies, data=self._data.bodies)
# Reset all joints to their model default states
self._data.joints.reset_state(q_j_0=self._model.joints.q_j_0)
self._data.joints.clear_all()
# Reset the joint-limits interface
self._limits.reset()
# Initialize the constraint state info
self._data.info.num_limits.zero_()
self._data.info.num_contacts.zero_()
update_constraints_info(model=self._model, data=self._data)
# Initialize the system Jacobians so that they may be available after reset
# NOTE: This is not strictly necessary, but serves advanced users who may
# want to query Jacobians in controllers immediately after a reset operation.
self._jacobians.build(
model=self._model,
data=self._data,
limits=None,
contacts=None,
reset_to_zero=True,
)
# Reset the forward dynamics solver
self._solver_fd.reset()
def _reset_to_default_state(self, state_out: StateKamino, world_mask: wp.array):
"""
Resets the simulation to the default state defined in the model.
"""
reset_state_to_model_default(
model=self._model,
state_out=state_out,
world_mask=world_mask,
)
def _reset_to_base_state(
self,
state_out: StateKamino,
world_mask: wp.array,
base_q: wp.array,
base_u: wp.array | None = None,
):
"""
Resets the simulation to the given base body states by
uniformly applying the necessary transform across all bodies.
"""
# Ensure that the base pose reset targets are valid
if base_q is None:
raise ValueError("Base pose targets must be provided for base state resets.")
if base_q.shape[0] != self._model.size.num_worlds:
raise ValueError(
f"Invalid base_q shape: Expected ({self._model.size.num_worlds},), but got {base_q.shape}."
)
# Determine the effective base twists to use
_base_u = base_u if base_u is not None else self._base_u
# Uniformly reset all bodies according to the transform between the given
# base state and the existing body states contained in `state_out`
reset_state_from_base_state(
model=self._model,
state_out=state_out,
world_mask=world_mask,
base_q=base_q,
base_u=_base_u,
)
def _reset_to_bodies_state(
self,
state_out: StateKamino,
world_mask: wp.array,
bodies_q: wp.array | None = None,
bodies_u: wp.array | None = None,
):
"""
Resets the simulation to the given rigid body states.
There is no check that the provided states satisfy any kinematic constraints.
"""
# use initial model poses if not provided
_bodies_q = bodies_q if bodies_q is not None else self._model.bodies.q_i_0
# use zero body velocities if not provided
_bodies_u = bodies_u if bodies_u is not None else self._bodies_u_zeros
reset_state_from_bodies_state(
model=self._model,
state_out=state_out,
world_mask=world_mask,
bodies_q=_bodies_q,
bodies_u=_bodies_u,
)
def _reset_with_fk_solve(
self,
state_out: StateKamino,
world_mask: wp.array,
joint_q: wp.array | None = None,
joint_u: wp.array | None = None,
actuator_q: wp.array | None = None,
actuator_u: wp.array | None = None,
base_q: wp.array | None = None,
base_u: wp.array | None = None,
):
"""
Resets the simulation to the given joint states by solving
the forward kinematics to compute the corresponding body states.
"""
# Check that the FK solver was initialized
if self._solver_fk is None:
raise RuntimeError("The FK solver must be enabled to use resets from joint angles.")
# Detect if joint or actuator targets are provided
with_joint_targets = joint_q is not None and (actuator_q is None and actuator_u is None)
# Unpack the actuated joint states from the input joint states
if with_joint_targets:
extract_actuators_state_from_joints(
model=self._model,
world_mask=world_mask,
joint_q=joint_q,
joint_u=joint_u if joint_u is not None else state_out.dq_j,
actuator_q=self._actuators_q,
actuator_u=self._actuators_u,
)
# Determine the actuator state arrays to use for the FK solve
_actuator_q = actuator_q if actuator_q is not None else self._actuators_q
_actuator_u = actuator_u if actuator_u is not None else self._actuators_u
# TODO: We need a graph-capturable mechanism to detect solver errors
# Solve the forward kinematics to compute the body states
self._solver_fk.run_fk_solve(
world_mask=world_mask,
bodies_q=state_out.q_i,
bodies_u=state_out.u_i if joint_u is not None or actuator_u is not None else None,
actuators_q=_actuator_q,
actuators_u=_actuator_u,
base_q=base_q,
base_u=base_u,
)
# Reset net body wrenches and joint constraint reactions to zero
# NOTE: This is necessary to ensure proper solver behavior after resets
reset_body_net_wrenches(model=self._model, body_w=state_out.w_i, world_mask=world_mask)
reset_joint_constraint_reactions(model=self._model, lambda_j=state_out.lambda_j, world_mask=world_mask)
# If joint targets were provided, copy them to the output state
if with_joint_targets:
# Copy the joint states to the output state
wp.copy(state_out.q_j_p, joint_q)
wp.copy(state_out.q_j, joint_q)
if joint_u is not None:
wp.copy(state_out.dq_j, joint_u)
# Otherwise, extract the joint states from the actuators
else:
extract_joints_state_from_actuators(
model=self._model,
world_mask=world_mask,
actuator_q=_actuator_q,
actuator_u=_actuator_u,
joint_q=state_out.q_j,
joint_u=state_out.dq_j,
)
wp.copy(state_out.q_j_p, state_out.q_j)
def _reset_post_process(self, world_mask: wp.array | None = None):
"""
Resets solver internal data and calls reset callbacks.
This is a common operation that must be called after resetting bodies and joints,
that ensures that all state and control data are synchronized with the internal
solver state, and that intermediate quantities are updated accordingly.
"""
# Reset the solver-internal time-keeping data
reset_time(
model=self._model,
world_mask=world_mask,
time=self._data.time.time,
steps=self._data.time.steps,
)
# Reset the forward dynamics solver to clear internal state
# NOTE: This will cause the solver to perform a cold-start
# on the first call to `step()`
self._solver_fd.reset(problem=self._problem_fd, world_mask=world_mask)
# TODO: Enable this when world-masking is implemented
# Reset the warm-starting caches if enabled
# if self._warmstart_mode == PADMMWarmStartMode.CONTAINERS:
# self._ws_limits.reset()
# self._ws_contacts.reset()
###
# Internals - Step Operations
###
def _update_joints_data(self, q_j_p: wp.array | None = None):
"""
Updates the joint states based on the current body states.
"""
# Use the provided previous joint states if given,
# otherwise use the internal cached joint states
if q_j_p is not None:
_q_j_p = q_j_p
else:
wp.copy(self._data.joints.q_j_p, self._data.joints.q_j)
_q_j_p = self._data.joints.q_j_p
# Update the joint states based on the updated body states
# NOTE: We use the previous state `state_p` for post-processing
# purposes, e.g. account for roll-over of revolute joints etc
compute_joints_data(
model=self._model,
data=self._data,
q_j_p=_q_j_p,
correction=self._rotation_correction,
)
def _update_intermediates(self, state_in: StateKamino):
"""
Updates intermediate quantities required for the forward dynamics solve.
"""
self._update_joints_data(q_j_p=state_in.q_j_p)
update_body_inertias(model=self._model.bodies, data=self._data.bodies)
def _update_limits(self):
"""
Runs limit detection to generate active joint limits.
"""
self._limits.detect(self._model, self._data)
def _update_constraint_info(self):
"""
Updates the state info with the set of active constraints resulting from limit and collision detection.
"""
update_constraints_info(model=self._model, data=self._data)
def _update_jacobians(self, contacts: ContactsKamino | None = None):
"""
Updates the forward kinematics by building the system Jacobians (of actuation and
constraints) based on the current state of the system and set of active constraints.
"""
self._jacobians.build(
model=self._model,
data=self._data,
limits=self._limits,
contacts=contacts,
reset_to_zero=True,
)
def _update_actuation_wrenches(self):
"""
Updates the actuation wrenches based on the current control inputs.
"""
compute_joint_dof_body_wrenches(self._model, self._data, self._jacobians)
def _update_dynamics(self, contacts: ContactsKamino | None = None):
"""
Constructs the forward dynamics problem quantities based on the current state of
the system, the set of active constraints, and the updated system Jacobians.
"""
self._problem_fd.build(
model=self._model,
data=self._data,
limits=self._limits,
contacts=contacts,
jacobians=self._jacobians,
reset_to_zero=True,
)
def _update_constraints(self, contacts: ContactsKamino | None = None):
"""
Solves the forward dynamics sub-problem to compute constraint
reactions and body wrenches effected through constraints.
"""
# If warm-starting is enabled, initialize unilateral
# constraints containers from the current solver data
if self._warmstart_mode > PADMMWarmStartMode.NONE:
if self._warmstart_mode == PADMMWarmStartMode.CONTAINERS:
self._ws_limits.warmstart(self._limits)
self._ws_contacts.warmstart(self._model, self._data, contacts)
self._solver_fd.warmstart(
problem=self._problem_fd,
model=self._model,
data=self._data,
limits=self._limits,
contacts=contacts,
)
# Otherwise, perform a cold-start of the dynamics solver
else:
self._solver_fd.coldstart()
# Solve the dual problem to compute the constraint reactions
self._solver_fd.solve(problem=self._problem_fd)
# Compute the effective body wrenches applied by the set of
# active constraints from the respective reaction multipliers
compute_constraint_body_wrenches(
model=self._model,
data=self._data,
limits=self._limits,
contacts=contacts,
jacobians=self._jacobians,
lambdas_offsets=self._problem_fd.data.vio,
lambdas_data=self._solver_fd.data.solution.lambdas,
)
# Unpack the computed constraint multipliers to the respective joint-limit
# and contact data for post-processing and optional solver warm-starting
unpack_constraint_solutions(
lambdas=self._solver_fd.data.solution.lambdas,
v_plus=self._solver_fd.data.solution.v_plus,
model=self._model,
data=self._data,
limits=self._limits,
contacts=contacts,
)
# If warmstarting is enabled, update the limits and contacts caches
# with the constraint reactions generated by the dynamics solver
# NOTE: This needs to happen after unpacking the multipliers
if self._warmstart_mode == PADMMWarmStartMode.CONTAINERS:
self._ws_limits.update(self._limits)
self._ws_contacts.update(contacts)
def _update_wrenches(self):
"""
Computes the total (i.e. net) body wrenches by summing up all individual contributions,
from joint actuation, joint limits, contacts, and purely external effects.
"""
update_body_wrenches(self._model.bodies, self._data.bodies)
def _forward(self, contacts: ContactsKamino | None = None):
"""
Solves the forward dynamics sub-problem to compute constraint reactions
and total effective body wrenches applied to each body of the system.
"""
# Update the dynamics
self._update_dynamics(contacts=contacts)
# Compute constraint reactions
self._update_constraints(contacts=contacts)
# Post-processing
self._update_wrenches()
def _solve_forward_dynamics(
self,
state_in: StateKamino,
state_out: StateKamino,
control: ControlKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
detector: CollisionDetector | None = None,
):
"""
Solves the forward dynamics sub-problem to compute constraint reactions
and total effective body wrenches applied to each body of the system.
Args:
state_in (`StateKamino`):
State of the system at the current time-step.
state_out (`StateKamino`):
State of the system at the next time-step.
control (`ControlKamino`):
Input controls applied to the system.
limits (`LimitsKamino`, optional):
Optional container for joint limits.
If `None`, joint limit handling is skipped.
contacts (`ContactsKamino`, optional):
Optional container of active contacts.
If `None`, the solver will use the internal collision detector
if the model admits contacts, or skip contact handling if not.
detector (`CollisionDetector`, optional):
Optional collision detector.
If `None`, collision detection is skipped.
"""
# Update intermediate quantities of the bodies and joints
# NOTE: We update the intermediate joint and body data here
# to ensure that they consistent with the current state.
# This is to handle cases when the forward dynamics may be
# evaluated at intermediate points of the discrete time-step
# (and potentially multiple times). The intermediate data is
# then used to perform limit and contact detection, as well
# as to evaluate kinematics and dynamics quantities such as
# the system Jacobians and generalized mass matrix.
self._update_intermediates(state_in=state_in)
# If a collision detector is provided, use it to generate
# update the set of active contacts at the current state
if detector is not None:
detector.collide(data=self._data, state=state_in, contacts=contacts)
# If a limits container/detector is provided, run joint-limit
# detection to generate active joint limits at the current state
if limits is not None:
limits.detect(self._model, self._data)
# Update the constraint state info
self._update_constraint_info()
# Update the differential forward kinematics to compute system Jacobians
self._update_jacobians(contacts=contacts)
# Compute the body actuation wrenches based on the current control inputs
self._update_actuation_wrenches()
# Run the pre-step callback if it has been set
self._run_prestep_callback(state_in, state_out, control, contacts)
# Solve the forward dynamics sub-problem to compute constraint reactions and body wrenches
self._forward(contacts=contacts)
# Run the mid-step callback if it has been set
self._run_midstep_callback(state_in, state_out, control, contacts)
def _compute_metrics(self, state_in: StateKamino, contacts: ContactsKamino | None = None):
"""
Computes performance metrics measuring the physical fidelity of the dynamics solver solution.
"""
if self._config.compute_solution_metrics:
self.metrics.reset()
self._metrics.evaluate(
sigma=self._solver_fd.data.state.sigma,
lambdas=self._solver_fd.data.solution.lambdas,
v_plus=self._solver_fd.data.solution.v_plus,
model=self._model,
data=self._data,
state_p=state_in,
problem=self._problem_fd,
jacobians=self._jacobians,
limits=self._limits,
contacts=contacts,
)
def _advance_time(self):
"""
Updates simulation time-keeping (i.e. physical time and discrete steps).
"""
advance_time(self._model.time, self._data.time)
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Numerical Solvers for Constraint Rigid Multi-Body Kinematics & Dynamics"""
from .fk import ForwardKinematicsSolver
from .padmm import PADMMSolver, PADMMWarmStartMode
###
# Module interface
###
__all__ = [
"ForwardKinematicsSolver",
"PADMMSolver",
"PADMMWarmStartMode",
]
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/fk.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides a solver for forward kinematics, i.e. computing body poses given
joint coordinates and base pose, by solving the kinematic constraints with
a Gauss-Newton method. This is used as a building block in the main Kamino
solver, but can also be used standalone (e.g., for visualization purposes).
"""
from __future__ import annotations
import sys
from dataclasses import dataclass
from enum import IntEnum
from functools import cache
import numpy as np
import warp as wp
from ...config import ForwardKinematicsSolverConfig
from ..core.joints import JointActuationType, JointDoFType
from ..core.math import (
G_of,
quat_left_jacobian_inverse,
quat_log,
unit_quat_apply,
unit_quat_apply_jacobian,
unit_quat_conj_apply,
unit_quat_conj_apply_jacobian,
unit_quat_conj_to_rotation_matrix,
)
from ..core.model import ModelKamino
from ..core.types import vec6f
from ..linalg.blas import (
block_sparse_ATA_blockwise_3_4_inv_diagonal_2d,
block_sparse_ATA_inv_diagonal_2d,
get_blockwise_diag_3_4_gemv_2d,
)
from ..linalg.conjugate import BatchedLinearOperator, CGSolver
from ..linalg.factorize.llt_blocked_semi_sparse import SemiSparseBlockCholeskySolverBatched
from ..linalg.sparse_matrix import BlockDType, BlockSparseMatrices
from ..linalg.sparse_operator import BlockSparseLinearOperators
###
# Module interface
###
__all__ = ["ForwardKinematicsSolver"]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
class block_type(BlockDType(dtype=wp.float32, shape=(7,)).warp_type):
pass
###
# Functions
###
@wp.func
def read_quat_from_array(array: wp.array(dtype=wp.float32), offset: int) -> wp.quatf:
"""
Utility function to read a quaternion from a flat array
"""
return wp.quatf(array[offset], array[offset + 1], array[offset + 2], array[offset + 3])
###
# Kernels
###
@wp.kernel
def _reset_state(
# Inputs
num_bodies: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
bodies_q_0_flat: wp.array(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
bodies_q_flat: wp.array(dtype=wp.float32),
):
"""
A kernel resetting the fk state (body poses) to the reference state
Inputs:
num_bodies: Num bodies per world
first_body_id: First body id per world
bodies_q_0_flat: Reference state, flattened
world_mask: Per-world flag to perform the operation (0 = skip)
Outputs:
bodies_q_flat: State to reset, flattened
"""
wd_id, state_id_loc = wp.tid() # Thread indices (= world index, state index)
rb_id_loc = state_id_loc // 7
if wd_id < num_bodies.shape[0] and world_mask[wd_id] != 0 and rb_id_loc < num_bodies[wd_id]:
state_id_tot = 7 * first_body_id[wd_id] + state_id_loc
bodies_q_flat[state_id_tot] = bodies_q_0_flat[state_id_tot]
@wp.kernel
def _reset_state_base_q(
# Inputs
base_joint_id: wp.array(dtype=wp.int32),
base_q: wp.array(dtype=wp.transformf),
joints_X: wp.array(dtype=wp.mat33f),
joints_B_r_B: wp.array(dtype=wp.vec3f),
num_bodies: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
bodies_q_0: wp.array(dtype=wp.transformf),
world_mask: wp.array(dtype=wp.int32),
# Outputs
bodies_q: wp.array(dtype=wp.transformf),
):
"""
A kernel resetting the fk state (body poses) to a rigid transformation of the reference state,
computed so that the base body is aligned on its prescribed pose.
Inputs:
base_joint_id: Base joint id per world (-1 = None)
base_q: Base body pose per world, in base joint coordinates
joints_X: Joint frame (local axes, valid both on base and follower)
joints_B_r_B: Joint local position on base body
num_bodies: Num bodies per world
first_body_id: First body id per world
bodies_q_0: Reference body poses
world_mask: Per-world flag to perform the operation (0 = skip)
Outputs:
bodies_q: Body poses to reset
"""
wd_id, rb_id_loc = wp.tid() # Thread indices (= world index, body index)
if wd_id < num_bodies.shape[0] and world_mask[wd_id] != 0 and rb_id_loc < num_bodies[wd_id]:
# Worlds without base joint: just copy the reference pose
rb_id_tot = first_body_id[wd_id] + rb_id_loc
base_jt_id = base_joint_id[wd_id]
body_q_0 = bodies_q_0[rb_id_tot]
if base_jt_id < 0:
bodies_q[rb_id_tot] = body_q_0
return
# Read memory
base_q_wd = base_q[wd_id]
X = joints_X[base_jt_id]
x = joints_B_r_B[base_jt_id]
# Compute transformation that maps the reference pose of the base body (follower of the base joint)
# to the pose corresponding by base_q. Note: we make use of the fact that initial body orientations
# are the identity (a more complex formula would needed otherwise)
t_jt = wp.transform_get_translation(base_q_wd)
q_jt = wp.transform_get_rotation(base_q_wd)
q_X = wp.quat_from_matrix(X)
q_tot = q_X * q_jt * wp.quat_inverse(q_X)
t_tot = x - wp.quat_rotate(q_tot, x) + wp.quat_rotate(q_X, t_jt)
transform_tot = wp.transformf(t_tot, q_tot)
# Apply to body pose
bodies_q[rb_id_tot] = wp.transform_multiply(transform_tot, body_q_0)
@wp.kernel
def _eval_fk_actuated_dofs_or_coords(
# Inputs
model_base_dofs: wp.array(dtype=wp.float32),
model_actuated_dofs: wp.array(dtype=wp.float32),
actuated_dofs_map: wp.array(dtype=wp.int32),
# Outputs
fk_actuated_dofs: wp.array(dtype=wp.float32),
):
"""
A kernel mapping actuated and base dofs/coordinates of the main model to actuated dofs/coordinates of the fk model,
which has a modified version of the joints, notably actuated free joints to control floating bases.
This uses a map from fk to model dofs/cords, that has >= 0 indices for fk dofs/coords that correspond to
main model actuated dofs/coords, and negative indices for base dofs/coords (base dof/coord i is stored as -i - 1)
Inputs:
model_base_dofs:
Base dofs or coordinates of the main model (as a flat vector with 6 dofs or 7 coordinates per world)
model_actuated_dofs:
Actuated dofs/coords of the main model
actuated_dofs_map:
Map of fk to main model actuated/base dofs/coords
Outputs:
fk_actuated_dofs: Actuated dofs or coordinates of the fk model
"""
# Retrieve the thread index (= fk actuated dof or coordinate index)
# Note: we use "dof" in variables naming to mean either dof or coordinate
fk_dof_id = wp.tid()
if fk_dof_id < fk_actuated_dofs.shape[0]:
model_dof_id = actuated_dofs_map[fk_dof_id]
if model_dof_id >= 0:
fk_actuated_dofs[fk_dof_id] = model_actuated_dofs[model_dof_id]
else: # Base dofs/coordinates are encoded as negative indices
base_dof_id = -(model_dof_id + 1) # Recover base dof/coord id
fk_actuated_dofs[fk_dof_id] = model_base_dofs[base_dof_id]
@wp.kernel
def _eval_position_control_transformations(
# Inputs
joints_dof_type: wp.array(dtype=wp.int32),
joints_act_type: wp.array(dtype=wp.int32),
actuated_coords_offset: wp.array(dtype=wp.int32),
joints_X: wp.array(dtype=wp.mat33f),
actuators_q: wp.array(dtype=wp.float32),
# Outputs
pos_control_transforms: wp.array(dtype=wp.transformf),
):
"""
A kernel computing a transformation per joint corresponding to position-control parameters
More specifically, this is the identity (no translation, no rotation) for passive joints
and a transformation corresponding to joint generalized coordinates for actuators
The translation part is expressed in joint frame (e.g., translation is along [1,0,0] for a prismatic joint)
The rotation part is expressed in body frame (e.g., rotation is about X[:,0] for a revolute joint)
Inputs:
joints_dof_type: Joint dof type (i.e. revolute, spherical, ...)
joints_act_type: Joint actuation type (i.e. passive or actuated)
actuated_coords_offset: Joint first actuated coordinate id, among all actuated coordinates in all worlds
joints_X: Joint frame (local axes, valid both on base and follower)
actuators_q: Actuated coordinates
Outputs:
pos_control_transforms: Joint position-control transformation
"""
# Retrieve the thread index (= joint index)
jt_id = wp.tid()
if jt_id < joints_dof_type.shape[0]:
# Retrieve the joint model data
dof_type_j = joints_dof_type[jt_id]
act_type_j = joints_act_type[jt_id]
X = joints_X[jt_id]
# Initialize transform to identity (already covers the passive case)
t = wp.vec3f(0.0, 0.0, 0.0)
q = wp.quatf(0.0, 0.0, 0.0, 1.0)
# In the actuated case, set translation/rotation as per joint generalized coordinates
if act_type_j == JointActuationType.FORCE:
offset_q_j = actuated_coords_offset[jt_id]
if dof_type_j == JointDoFType.CARTESIAN:
t[0] = actuators_q[offset_q_j]
t[1] = actuators_q[offset_q_j + 1]
t[2] = actuators_q[offset_q_j + 2]
elif dof_type_j == JointDoFType.CYLINDRICAL:
t[0] = actuators_q[offset_q_j]
q = wp.quat_from_axis_angle(wp.vec3f(X[0, 0], X[1, 0], X[2, 0]), actuators_q[offset_q_j + 1])
elif dof_type_j == JointDoFType.FIXED:
pass # No dofs to apply
elif dof_type_j == JointDoFType.FREE:
t[0] = actuators_q[offset_q_j]
t[1] = actuators_q[offset_q_j + 1]
t[2] = actuators_q[offset_q_j + 2]
q_X = wp.quat_from_matrix(X)
q_loc = read_quat_from_array(actuators_q, offset_q_j + 3)
q = q_X * q_loc * wp.quat_inverse(q_X)
elif dof_type_j == JointDoFType.PRISMATIC:
t[0] = actuators_q[offset_q_j]
elif dof_type_j == JointDoFType.REVOLUTE:
q = wp.quat_from_axis_angle(wp.vec3f(X[0, 0], X[1, 0], X[2, 0]), actuators_q[offset_q_j])
elif dof_type_j == JointDoFType.SPHERICAL:
q_X = wp.quat_from_matrix(X)
q_loc = read_quat_from_array(actuators_q, offset_q_j)
q = q_X * q_loc * wp.quat_inverse(q_X)
elif dof_type_j == JointDoFType.UNIVERSAL:
q_x = wp.quat_from_axis_angle(wp.vec3f(X[0, 0], X[1, 0], X[2, 0]), actuators_q[offset_q_j])
q_y = wp.quat_from_axis_angle(wp.vec3f(X[0, 1], X[1, 1], X[2, 1]), actuators_q[offset_q_j + 1])
q = q_x * q_y
# Write out transformation
pos_control_transforms[jt_id] = wp.transformf(t, q)
@wp.kernel
def _eval_unit_quaternion_constraints(
# Inputs
num_bodies: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
bodies_q: wp.array(dtype=wp.transformf),
world_mask: wp.array(dtype=wp.int32),
# Outputs
constraints: wp.array2d(dtype=wp.float32),
):
"""
A kernel computing unit norm quaternion constraints for each body, written at the top of the constraints vector
Inputs:
num_bodies: Num bodies per world
first_body_id: First body id per world
bodies_q: Body poses
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
constraints: Constraint vector per world
):
"""
# Retrieve the thread indices (= world index, body index)
wd_id, rb_id_loc = wp.tid()
if wd_id < num_bodies.shape[0] and world_mask[wd_id] != 0 and rb_id_loc < num_bodies[wd_id]:
# Get overall body id
rb_id_tot = first_body_id[wd_id] + rb_id_loc
# Evaluate unit quaternion constraint
q = wp.transform_get_rotation(bodies_q[rb_id_tot])
constraints[wd_id, rb_id_loc] = wp.dot(q, q) - 1.0
@cache
def create_eval_joint_constraints_kernel(has_universal_joints: bool):
"""
Returns the joint constraints evaluation kernel, statically baking in whether there are universal joints
or not (these joints need a separate handling)
"""
@wp.kernel
def _eval_joint_constraints(
# Inputs
num_joints: wp.array(dtype=wp.int32),
first_joint_id: wp.array(dtype=wp.int32),
joints_dof_type: wp.array(dtype=wp.int32),
joints_act_type: wp.array(dtype=wp.int32),
joints_bid_B: wp.array(dtype=wp.int32),
joints_bid_F: wp.array(dtype=wp.int32),
joints_X: wp.array(dtype=wp.mat33f),
joints_B_r_B: wp.array(dtype=wp.vec3f),
joints_F_r_F: wp.array(dtype=wp.vec3f),
bodies_q: wp.array(dtype=wp.transformf),
pos_control_transforms: wp.array(dtype=wp.transformf),
ct_full_to_red_map: wp.array(dtype=wp.int32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
constraints: wp.array2d(dtype=wp.float32),
):
"""
A kernel computing joint constraints with the log map formulation, first computing 6 constraints per
joint (treating it as a fixed joint), then writing out the relevant subset of constraints (only along
relevant directions) using a precomputed full to reduced map.
Note: the log map formulation doesn't allow to formulate passive universal joints. If such joints are
present, the right number of (incorrect) constraints is first written with the log map, then the result
is overwritten in a second pass with the correct constraints.
Inputs:
num_joints: Num joints per world
first_joint_id: First joint id per world
joints_dof_type: Joint dof type (i.e. revolute, spherical, ...)
joints_act_type: Joint actuation type (i.e. passive or actuated)
joints_bid_B: Joint base body id
joints_bid_F: Joint follower body id
joints_X: Joint frame (local axes, valid both on base and follower)
joints_B_r_B: Joint local position on base body
joints_F_r_F: Joint local position on follower body
bodies_q: Body poses
pos_control_transforms: Joint position-control transformation
ct_full_to_red_map: Map from full to reduced constraint id
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
constraints: Constraint vector per world
"""
# Retrieve the thread indices (= world index, joint index)
wd_id, jt_id_loc = wp.tid()
if wd_id < num_joints.shape[0] and world_mask[wd_id] != 0 and jt_id_loc < num_joints[wd_id]:
# Get overall joint id
jt_id_tot = first_joint_id[wd_id] + jt_id_loc
# Get reduced constraint ids (-1 meaning constraint is not used)
first_ct_id_full = 6 * jt_id_tot
trans_ct_ids_red = wp.vec3i(
ct_full_to_red_map[first_ct_id_full],
ct_full_to_red_map[first_ct_id_full + 1],
ct_full_to_red_map[first_ct_id_full + 2],
)
rot_ct_ids_red = wp.vec3i(
ct_full_to_red_map[first_ct_id_full + 3],
ct_full_to_red_map[first_ct_id_full + 4],
ct_full_to_red_map[first_ct_id_full + 5],
)
# Get joint local positions and orientation
x_base = joints_B_r_B[jt_id_tot]
x_follower = joints_F_r_F[jt_id_tot]
X_T = wp.transpose(joints_X[jt_id_tot])
# Get base and follower transformations
base_id = joints_bid_B[jt_id_tot]
if base_id < 0:
c_base = wp.vec3f(0.0, 0.0, 0.0)
q_base = wp.quatf(0.0, 0.0, 0.0, 1.0)
else:
c_base = wp.transform_get_translation(bodies_q[base_id])
q_base = wp.transform_get_rotation(bodies_q[base_id])
follower_id = joints_bid_F[jt_id_tot]
c_follower = wp.transform_get_translation(bodies_q[follower_id])
q_follower = wp.transform_get_rotation(bodies_q[follower_id])
# Get position control transformation, in joint/body frame for translation/rotation part
t_control_joint = wp.transform_get_translation(pos_control_transforms[jt_id_tot])
q_control_body = wp.transform_get_rotation(pos_control_transforms[jt_id_tot])
# Translation constraints: compute "error" translation, in joint frame
pos_follower_world = unit_quat_apply(q_follower, x_follower) + c_follower
pos_follower_base = unit_quat_conj_apply(q_base, pos_follower_world - c_base)
pos_rel_base = (
pos_follower_base - x_base
) # Relative position on base body (should match translation from controls)
t_error = X_T * pos_rel_base - t_control_joint # Error in joint frame
# Rotation constraints: compute "error" rotation with the log map, in joint frame
q_error_base = wp.quat_inverse(q_base) * q_follower * wp.quat_inverse(q_control_body)
rot_error = X_T * quat_log(q_error_base)
# Write out constraint
for i in range(3):
if trans_ct_ids_red[i] >= 0:
constraints[wd_id, trans_ct_ids_red[i]] = t_error[i]
if rot_ct_ids_red[i] >= 0:
constraints[wd_id, rot_ct_ids_red[i]] = rot_error[i]
# Correct constraints for passive universal joints
if wp.static(has_universal_joints):
# Check for a passive universal joint
dof_type_j = joints_dof_type[jt_id_tot]
act_type_j = joints_act_type[jt_id_tot]
if dof_type_j != int(JointDoFType.UNIVERSAL) or act_type_j != int(JointActuationType.PASSIVE):
return
# Compute constraint (dot product between x axis on base and y axis on follower)
a_x = X_T[0]
a_y = X_T[1]
a_x_base = unit_quat_apply(q_base, a_x)
a_y_follower = unit_quat_apply(q_follower, a_y)
ct = -wp.dot(a_x_base, a_y_follower)
# Set constraint in output (at a location corresponding to z rotational constraint)
constraints[wd_id, rot_ct_ids_red[2]] = ct
return _eval_joint_constraints
@wp.kernel
def _eval_unit_quaternion_constraints_jacobian(
# Inputs
num_bodies: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
bodies_q: wp.array(dtype=wp.transformf),
world_mask: wp.array(dtype=wp.int32),
# Outputs
constraints_jacobian: wp.array3d(dtype=wp.float32),
):
"""
A kernel computing the Jacobian of unit norm quaternion constraints for each body, written at the top of the
constraints Jacobian
Inputs:
num_bodies: Num bodies per world
first_body_id: First body id per world
bodies_q: Body poses
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
constraints_jacobian: Constraints Jacobian per world
"""
# Retrieve the thread indices (= world index, body index)
wd_id, rb_id_loc = wp.tid()
if wd_id < num_bodies.shape[0] and world_mask[wd_id] != 0 and rb_id_loc < num_bodies[wd_id]:
# Get overall body id
rb_id_tot = first_body_id[wd_id] + rb_id_loc
# Evaluate constraint Jacobian
q = wp.transform_get_rotation(bodies_q[rb_id_tot])
state_offset = 7 * rb_id_loc + 3
constraints_jacobian[wd_id, rb_id_loc, state_offset] = 2.0 * q.x
constraints_jacobian[wd_id, rb_id_loc, state_offset + 1] = 2.0 * q.y
constraints_jacobian[wd_id, rb_id_loc, state_offset + 2] = 2.0 * q.z
constraints_jacobian[wd_id, rb_id_loc, state_offset + 3] = 2.0 * q.w
@wp.kernel
def _eval_unit_quaternion_constraints_sparse_jacobian(
# Inputs
num_bodies: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
bodies_q: wp.array(dtype=wp.transformf),
rb_nzb_id: wp.array(dtype=wp.int32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
jacobian_nzb: wp.array(dtype=block_type),
):
"""
A kernel computing the sparse Jacobian of unit norm quaternion constraints for each body, written at the top of the
constraints Jacobian
Inputs:
num_bodies: Num bodies per world
first_body_id: First body id per world
bodies_q: Body poses
rb_nzb_id: Id of the nzb corresponding to the constraint per body
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
jacobian_nzb: Non-zero blocks of the sparse Jacobian
"""
# Retrieve the thread indices (= world index, body index)
wd_id, rb_id_loc = wp.tid()
if wd_id < num_bodies.shape[0] and world_mask[wd_id] != 0 and rb_id_loc < num_bodies[wd_id]:
# Get overall body id
rb_id_tot = first_body_id[wd_id] + rb_id_loc
# Evaluate constraint Jacobian
q = wp.transform_get_rotation(bodies_q[rb_id_tot])
nzb_id = rb_nzb_id[rb_id_tot]
jacobian_nzb[nzb_id][3] = 2.0 * q.x
jacobian_nzb[nzb_id][4] = 2.0 * q.y
jacobian_nzb[nzb_id][5] = 2.0 * q.z
jacobian_nzb[nzb_id][6] = 2.0 * q.w
@cache
def create_eval_joint_constraints_jacobian_kernel(has_universal_joints: bool):
"""
Returns the joint constraints Jacobian evaluation kernel, statically baking in whether there are universal joints
or not (these joints need a separate handling)
"""
@wp.kernel
def _eval_joint_constraints_jacobian(
# Inputs
num_joints: wp.array(dtype=wp.int32),
first_joint_id: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
joints_dof_type: wp.array(dtype=wp.int32),
joints_act_type: wp.array(dtype=wp.int32),
joints_bid_B: wp.array(dtype=wp.int32),
joints_bid_F: wp.array(dtype=wp.int32),
joints_X: wp.array(dtype=wp.mat33f),
joints_B_r_B: wp.array(dtype=wp.vec3f),
joints_F_r_F: wp.array(dtype=wp.vec3f),
bodies_q: wp.array(dtype=wp.transformf),
pos_control_transforms: wp.array(dtype=wp.transformf),
ct_full_to_red_map: wp.array(dtype=wp.int32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
constraints_jacobian: wp.array3d(dtype=wp.float32),
):
"""
A kernel computing the Jacobian of the joint constraints.
The Jacobian is assumed to have already been filled with zeros, at least in the coefficients that
are always zero due to joint connectivity.
Inputs:
num_joints: Num joints per world
first_joint_id: First joint id per world
first_body_id: First body id per world
joints_dof_type: Joint dof type (i.e. revolute, spherical, ...)
joints_act_type: Joint actuation type (i.e. passive or actuated)
joints_bid_B: Joint base body id
joints_bid_F: Joint follower body id
joints_X: Joint frame (local axes, valid both on base and follower)
joints_B_r_B: Joint local position on base body
joints_F_r_F: Joint local position on follower body
bodies_q: Body poses
pos_control_transforms: Joint position-control transformation
ct_full_to_red_map: Map from full to reduced constraint id
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
constraints_jacobian: Constraint Jacobian per world
"""
# Retrieve the thread indices (= world index, joint index)
wd_id, jt_id_loc = wp.tid()
if wd_id < num_joints.shape[0] and world_mask[wd_id] != 0 and jt_id_loc < num_joints[wd_id]:
# Get overall joint id
jt_id_tot = first_joint_id[wd_id] + jt_id_loc
# Get reduced constraint ids (-1 meaning constraint is not used)
first_ct_id_full = 6 * jt_id_tot
trans_ct_ids_red = wp.vec3i(
ct_full_to_red_map[first_ct_id_full],
ct_full_to_red_map[first_ct_id_full + 1],
ct_full_to_red_map[first_ct_id_full + 2],
)
rot_ct_ids_red = wp.vec3i(
ct_full_to_red_map[first_ct_id_full + 3],
ct_full_to_red_map[first_ct_id_full + 4],
ct_full_to_red_map[first_ct_id_full + 5],
)
# Get joint local positions and orientation
x_follower = joints_F_r_F[jt_id_tot]
X_T = wp.transpose(joints_X[jt_id_tot])
# Get base and follower transformations
base_id_tot = joints_bid_B[jt_id_tot]
if base_id_tot < 0:
c_base = wp.vec3f(0.0, 0.0, 0.0)
q_base = wp.quatf(0.0, 0.0, 0.0, 1.0)
else:
c_base = wp.transform_get_translation(bodies_q[base_id_tot])
q_base = wp.transform_get_rotation(bodies_q[base_id_tot])
follower_id_tot = joints_bid_F[jt_id_tot]
c_follower = wp.transform_get_translation(bodies_q[follower_id_tot])
q_follower = wp.transform_get_rotation(bodies_q[follower_id_tot])
base_id_loc = base_id_tot - first_body_id[wd_id]
follower_id_loc = follower_id_tot - first_body_id[wd_id]
# Get position control transformation (rotation part only, as translation part doesn't affect the Jacobian)
q_control_body = wp.transform_get_rotation(pos_control_transforms[jt_id_tot])
# Translation constraints
X_T_R_base_T = X_T * unit_quat_conj_to_rotation_matrix(q_base)
if base_id_tot >= 0:
jac_trans_c_base = -X_T_R_base_T
delta_pos = unit_quat_apply(q_follower, x_follower) + c_follower - c_base
jac_trans_q_base = X_T * unit_quat_conj_apply_jacobian(q_base, delta_pos)
jac_trans_c_follower = X_T_R_base_T
jac_trans_q_follower = X_T_R_base_T * unit_quat_apply_jacobian(q_follower, x_follower)
# Rotation constraints
q_base_sq_norm = wp.dot(q_base, q_base)
q_follower_sq_norm = wp.dot(q_follower, q_follower)
R_base_T = unit_quat_conj_to_rotation_matrix(q_base / wp.sqrt(q_base_sq_norm))
q_rel = q_follower * wp.quat_inverse(q_control_body) * wp.quat_inverse(q_base)
temp = X_T * R_base_T * quat_left_jacobian_inverse(q_rel)
if base_id_tot >= 0:
jac_rot_q_base = (-2.0 / q_base_sq_norm) * temp * G_of(q_base)
jac_rot_q_follower = (2.0 / q_follower_sq_norm) * temp * G_of(q_follower)
# Note: we need X^T * R_base^T both for translation and rotation constraints, but to get the correct
# derivatives for non-unit quaternions (which may be encountered before convergence) we end up needing
# to use a separate formula to evaluate R_base in either case
# Write out Jacobian
base_offset = 7 * base_id_loc
follower_offset = 7 * follower_id_loc
for i in range(3):
trans_ct_id_red = trans_ct_ids_red[i]
if trans_ct_id_red >= 0:
for j in range(3):
if base_id_tot >= 0:
constraints_jacobian[wd_id, trans_ct_id_red, base_offset + j] = jac_trans_c_base[i, j]
constraints_jacobian[wd_id, trans_ct_id_red, follower_offset + j] = jac_trans_c_follower[i, j]
for j in range(4):
if base_id_tot >= 0:
constraints_jacobian[wd_id, trans_ct_id_red, base_offset + 3 + j] = jac_trans_q_base[i, j]
constraints_jacobian[wd_id, trans_ct_id_red, follower_offset + 3 + j] = jac_trans_q_follower[
i, j
]
rot_ct_id_red = rot_ct_ids_red[i]
if rot_ct_id_red >= 0:
for j in range(4):
if base_id_tot >= 0:
constraints_jacobian[wd_id, rot_ct_id_red, base_offset + 3 + j] = jac_rot_q_base[i, j]
constraints_jacobian[wd_id, rot_ct_id_red, follower_offset + 3 + j] = jac_rot_q_follower[i, j]
# Correct Jacobian for passive universal joints
if wp.static(has_universal_joints):
# Check for a passive universal joint
dof_type_j = joints_dof_type[jt_id_tot]
act_type_j = joints_act_type[jt_id_tot]
if dof_type_j != int(JointDoFType.UNIVERSAL) or act_type_j != int(JointActuationType.PASSIVE):
return
# Compute constraint Jacobian (cross product between x axis on base and y axis on follower)
a_x = X_T[0]
a_y = X_T[1]
if base_id_tot >= 0:
a_y_follower = unit_quat_apply(q_follower, a_y)
jac_q_base = -a_y_follower * unit_quat_apply_jacobian(q_base, a_x)
a_x_base = unit_quat_apply(q_base, a_x)
jac_q_follower = -a_x_base * unit_quat_apply_jacobian(q_follower, a_y)
# Write out Jacobian
for i in range(4):
rot_ct_id_red = rot_ct_ids_red[2]
if base_id_tot >= 0:
constraints_jacobian[wd_id, rot_ct_id_red, base_offset + 3 + i] = jac_q_base[i]
constraints_jacobian[wd_id, rot_ct_id_red, follower_offset + 3 + i] = jac_q_follower[i]
return _eval_joint_constraints_jacobian
@cache
def create_eval_joint_constraints_sparse_jacobian_kernel(has_universal_joints: bool):
"""
Returns the joint constraints sparse Jacobian evaluation kernel,
statically baking in whether there are universal joints or not
(these joints need a separate handling)
"""
@wp.kernel
def _eval_joint_constraints_sparse_jacobian(
# Inputs
num_joints: wp.array(dtype=wp.int32),
first_joint_id: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
joints_dof_type: wp.array(dtype=wp.int32),
joints_act_type: wp.array(dtype=wp.int32),
joints_bid_B: wp.array(dtype=wp.int32),
joints_bid_F: wp.array(dtype=wp.int32),
joints_X: wp.array(dtype=wp.mat33f),
joints_B_r_B: wp.array(dtype=wp.vec3f),
joints_F_r_F: wp.array(dtype=wp.vec3f),
bodies_q: wp.array(dtype=wp.transformf),
pos_control_transforms: wp.array(dtype=wp.transformf),
ct_nzb_id_base: wp.array(dtype=wp.int32),
ct_nzb_id_follower: wp.array(dtype=wp.int32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
jacobian_nzb: wp.array(dtype=block_type),
):
"""
A kernel computing the Jacobian of the joint constraints.
The Jacobian is assumed to have already been filled with zeros, at least in the coefficients that
are always zero due to joint connectivity.
Inputs:
num_joints: Num joints per world
first_joint_id: First joint id per world
first_body_id: First body id per world
joints_dof_type: Joint dof type (i.e. revolute, spherical, ...)
joints_act_type: Joint actuation type (i.e. passive or actuated)
joints_bid_B: Joint base body id
joints_bid_F: Joint follower body id
joints_X: Joint frame (local axes, valid both on base and follower)
joints_B_r_B: Joint local position on base body
joints_F_r_F: Joint local position on follower body
bodies_q: Body poses
pos_control_transforms: Joint position-control transformation
ct_nzb_id_base: Map from full constraint id to nzb id, for the base body blocks
ct_nzb_id_base: Map from full constraint id to nzb id, for the follower body blocks
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
jacobian_nzb: Non-zero blocks of the sparse Jacobian
"""
# Retrieve the thread indices (= world index, joint index)
wd_id, jt_id_loc = wp.tid()
if wd_id < num_joints.shape[0] and world_mask[wd_id] != 0 and jt_id_loc < num_joints[wd_id]:
# Get overall joint id
jt_id_tot = first_joint_id[wd_id] + jt_id_loc
# Get nzb ids (-1 meaning constraint is not used)
start = 6 * jt_id_tot
end = start + 6
nzb_ids_base = ct_nzb_id_base[start:end]
nzb_ids_follower = ct_nzb_id_follower[start:end]
# Get joint local positions and orientation
x_follower = joints_F_r_F[jt_id_tot]
X_T = wp.transpose(joints_X[jt_id_tot])
# Get base and follower transformations
base_id = joints_bid_B[jt_id_tot]
if base_id < 0:
c_base = wp.vec3f(0.0, 0.0, 0.0)
q_base = wp.quatf(0.0, 0.0, 0.0, 1.0)
else:
c_base = wp.transform_get_translation(bodies_q[base_id])
q_base = wp.transform_get_rotation(bodies_q[base_id])
follower_id = joints_bid_F[jt_id_tot]
c_follower = wp.transform_get_translation(bodies_q[follower_id])
q_follower = wp.transform_get_rotation(bodies_q[follower_id])
# Get position control transformation (rotation part only, as translation part doesn't affect the Jacobian)
q_control_body = wp.transform_get_rotation(pos_control_transforms[jt_id_tot])
# Translation constraints
X_T_R_base_T = X_T * unit_quat_conj_to_rotation_matrix(q_base)
if base_id >= 0:
jac_trans_c_base = -X_T_R_base_T
delta_pos = unit_quat_apply(q_follower, x_follower) + c_follower - c_base
jac_trans_q_base = X_T * unit_quat_conj_apply_jacobian(q_base, delta_pos)
jac_trans_c_follower = X_T_R_base_T
jac_trans_q_follower = X_T_R_base_T * unit_quat_apply_jacobian(q_follower, x_follower)
# Rotation constraints
q_base_sq_norm = wp.dot(q_base, q_base)
q_follower_sq_norm = wp.dot(q_follower, q_follower)
R_base_T = unit_quat_conj_to_rotation_matrix(q_base / wp.sqrt(q_base_sq_norm))
q_rel = q_follower * wp.quat_inverse(q_control_body) * wp.quat_inverse(q_base)
temp = X_T * R_base_T * quat_left_jacobian_inverse(q_rel)
if base_id >= 0:
jac_rot_q_base = (-2.0 / q_base_sq_norm) * temp * G_of(q_base)
jac_rot_q_follower = (2.0 / q_follower_sq_norm) * temp * G_of(q_follower)
# Note: we need X^T * R_base^T both for translation and rotation constraints, but to get the correct
# derivatives for non-unit quaternions (which may be encountered before convergence) we end up needing
# to use a separate formula to evaluate R_base in either case
# Write out Jacobian
if base_id >= 0:
for i in range(3):
nzb_id = nzb_ids_base[i]
if nzb_id >= 0:
for j in range(3):
jacobian_nzb[nzb_id][j] = jac_trans_c_base[i, j]
for j in range(4):
jacobian_nzb[nzb_id][3 + j] = jac_trans_q_base[i, j]
for i in range(3):
nzb_id = nzb_ids_base[i + 3]
if nzb_id >= 0:
for j in range(4):
jacobian_nzb[nzb_id][3 + j] = jac_rot_q_base[i, j]
for i in range(3):
nzb_id = nzb_ids_follower[i]
if nzb_id >= 0:
for j in range(3):
jacobian_nzb[nzb_id][j] = jac_trans_c_follower[i, j]
for j in range(4):
jacobian_nzb[nzb_id][3 + j] = jac_trans_q_follower[i, j]
for i in range(3):
nzb_id = nzb_ids_follower[i + 3]
if nzb_id >= 0:
for j in range(4):
jacobian_nzb[nzb_id][3 + j] = jac_rot_q_follower[i, j]
# Correct Jacobian for passive universal joints
if wp.static(has_universal_joints):
# Check for a passive universal joint
dof_type_j = joints_dof_type[jt_id_tot]
act_type_j = joints_act_type[jt_id_tot]
if dof_type_j != int(JointDoFType.UNIVERSAL) or act_type_j != int(JointActuationType.PASSIVE):
return
# Compute constraint Jacobian (cross product between x axis on base and y axis on follower)
a_x = X_T[0]
a_y = X_T[1]
if base_id >= 0:
a_y_follower = unit_quat_apply(q_follower, a_y)
jac_q_base = -a_y_follower * unit_quat_apply_jacobian(q_base, a_x)
a_x_base = unit_quat_apply(q_base, a_x)
jac_q_follower = -a_x_base * unit_quat_apply_jacobian(q_follower, a_y)
# Write out Jacobian
if base_id >= 0:
nzb_id = nzb_ids_base[5]
for j in range(4):
jacobian_nzb[nzb_id][3 + j] = jac_q_base[j]
nzb_id = nzb_ids_follower[5]
for j in range(4):
jacobian_nzb[nzb_id][3 + j] = jac_q_follower[j]
return _eval_joint_constraints_sparse_jacobian
@cache
def create_tile_based_kernels(TILE_SIZE_CTS: wp.int32, TILE_SIZE_VRS: wp.int32):
"""
Generates and returns all tile-based kernels in this module, given the tile size to use along the constraints
and variables (i.e. body poses) dimensions in the constraint vector, Jacobian, step vector etc.
These are _eval_pattern_T_pattern, _eval_max_constraint, _eval_jacobian_T_jacobian, eval_jacobian_T_constraints,
_eval_merit_function, _eval_merit_function_gradient (returned in this order)
"""
@wp.func
def clip_to_one(x: wp.float32):
"""
Clips an number to 1 if it is above
"""
return wp.min(x, 1.0)
@wp.kernel
def _eval_pattern_T_pattern(
# Inputs
sparsity_pattern: wp.array3d(dtype=wp.float32),
# Outputs
pattern_T_pattern: wp.array3d(dtype=wp.float32),
):
"""
A kernel computing the sparsity pattern of J^T * J given that of J, in each world
More specifically, given an integer matrix of zeros and ones representing a sparsity pattern, multiply it by
its transpose and clip values to [0, 1] to get the sparsity pattern of J^T * J
Note: mostly redundant with _eval_jacobian_T_jacobian apart from the clipping, could possibly be removed
(was initially written to take int32, but float32 is actually faster)
Inputs:
sparsity_pattern: Jacobian sparsity pattern per world
Outputs:
pattern_T_pattern: Jacobian^T * Jacobian sparsity pattern per world
"""
wd_id, i, j = wp.tid() # Thread indices (= world index, output tile indices)
if (
wd_id < pattern_T_pattern.shape[0]
and i * TILE_SIZE_VRS < pattern_T_pattern.shape[1]
and j * TILE_SIZE_VRS < pattern_T_pattern.shape[2]
):
tile_out = wp.tile_zeros(shape=(TILE_SIZE_VRS, TILE_SIZE_VRS), dtype=wp.float32)
num_cts = sparsity_pattern.shape[1]
num_tiles_K = (num_cts + TILE_SIZE_CTS - 1) // TILE_SIZE_CTS # Equivalent to ceil(num_cts / TILE_SIZE_CTS)
for k in range(num_tiles_K):
tile_i_3d = wp.tile_load(
sparsity_pattern,
shape=(1, TILE_SIZE_CTS, TILE_SIZE_VRS),
offset=(wd_id, k * TILE_SIZE_CTS, i * TILE_SIZE_VRS),
)
tile_i = wp.tile_reshape(tile_i_3d, (TILE_SIZE_CTS, TILE_SIZE_VRS))
tile_i_T = wp.tile_transpose(tile_i)
tile_j_3d = wp.tile_load(
sparsity_pattern,
shape=(1, TILE_SIZE_CTS, TILE_SIZE_VRS),
offset=(wd_id, k * TILE_SIZE_CTS, j * TILE_SIZE_VRS),
)
tile_j = wp.tile_reshape(tile_j_3d, (TILE_SIZE_CTS, TILE_SIZE_VRS))
wp.tile_matmul(tile_i_T, tile_j, tile_out)
tile_out_3d = wp.tile_reshape(tile_out, (1, TILE_SIZE_VRS, TILE_SIZE_VRS))
tile_out_3d_clipped = wp.tile_map(clip_to_one, tile_out_3d)
wp.tile_store(pattern_T_pattern, tile_out_3d_clipped, offset=(wd_id, i * TILE_SIZE_VRS, j * TILE_SIZE_VRS))
@wp.func
def _isnan(x: wp.float32) -> wp.int32:
"""Calls wp.isnan and converts the result to int32"""
return wp.int32(wp.isnan(x))
@wp.kernel
def _eval_max_constraint(
# Inputs
constraints: wp.array2d(dtype=wp.float32),
# Outputs
max_constraint: wp.array(dtype=wp.float32),
):
"""
A kernel computing the max absolute constraint from the constraints vector, in each world.
Inputs:
constraints: Constraint vector per world
Outputs:
max_constraint: Max absolute constraint per world; must be zero-initialized
"""
wd_id, i, tid = wp.tid() # Thread indices (= world index, input tile index, thread index in block)
if wd_id < constraints.shape[0] and i * TILE_SIZE_CTS < constraints.shape[1]:
segment = wp.tile_load(constraints, shape=(1, TILE_SIZE_CTS), offset=(wd_id, i * TILE_SIZE_CTS))
segment_max = wp.tile_max(wp.tile_map(wp.abs, segment))[0]
segment_has_nan = wp.tile_max(wp.tile_map(_isnan, segment))[0]
if tid == 0:
if segment_has_nan:
# Write NaN in max (non-atomically, as this will overwrite any non-NaN value)
max_constraint[wd_id] = wp.nan
else:
# Atomically update the max, only if it is not yet NaN (in CUDA, the max() operation only
# considers non-NaN values, so the NaN value would get overwritten by a non-NaN otherwise)
while True:
curr_val = max_constraint[wd_id]
if wp.isnan(curr_val):
break
check_val = wp.atomic_cas(max_constraint, wd_id, curr_val, wp.max(curr_val, segment_max))
if check_val == curr_val:
break
@wp.kernel
def _eval_jacobian_T_jacobian(
# Inputs
constraints_jacobian: wp.array3d(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
jacobian_T_jacobian: wp.array3d(dtype=wp.float32),
):
"""
A kernel computing the matrix product J^T * J given the Jacobian J, in each world
Inputs:
constraints_jacobian: Constraint Jacobian per world
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
jacobian_T_jacobian: Jacobian^T * Jacobian per world
"""
wd_id, i, j = wp.tid() # Thread indices (= world index, output tile indices)
if (
wd_id < jacobian_T_jacobian.shape[0]
and world_mask[wd_id] != 0
and i * TILE_SIZE_VRS < jacobian_T_jacobian.shape[1]
and j * TILE_SIZE_VRS < jacobian_T_jacobian.shape[2]
):
tile_out = wp.tile_zeros(shape=(TILE_SIZE_VRS, TILE_SIZE_VRS), dtype=wp.float32)
num_cts = constraints_jacobian.shape[1]
num_tiles_K = (num_cts + TILE_SIZE_CTS - 1) // TILE_SIZE_CTS # Equivalent to ceil(num_cts / TILE_SIZE_CTS)
for k in range(num_tiles_K):
tile_i_3d = wp.tile_load(
constraints_jacobian,
shape=(1, TILE_SIZE_CTS, TILE_SIZE_VRS),
offset=(wd_id, k * TILE_SIZE_CTS, i * TILE_SIZE_VRS),
)
tile_i = wp.tile_reshape(tile_i_3d, (TILE_SIZE_CTS, TILE_SIZE_VRS))
tile_i_T = wp.tile_transpose(tile_i)
tile_j_3d = wp.tile_load(
constraints_jacobian,
shape=(1, TILE_SIZE_CTS, TILE_SIZE_VRS),
offset=(wd_id, k * TILE_SIZE_CTS, j * TILE_SIZE_VRS),
)
tile_j = wp.tile_reshape(tile_j_3d, (TILE_SIZE_CTS, TILE_SIZE_VRS))
wp.tile_matmul(tile_i_T, tile_j, tile_out)
tile_out_3d = wp.tile_reshape(tile_out, (1, TILE_SIZE_VRS, TILE_SIZE_VRS))
wp.tile_store(jacobian_T_jacobian, tile_out_3d, offset=(wd_id, i * TILE_SIZE_VRS, j * TILE_SIZE_VRS))
@wp.kernel
def _eval_jacobian_T_constraints(
# Inputs
constraints_jacobian: wp.array3d(dtype=wp.float32),
constraints: wp.array2d(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
jacobian_T_constraints: wp.array2d(dtype=wp.float32),
):
"""
A kernel computing the matrix product J^T * C given the Jacobian J and the constraints vector C, in each world
Inputs:
constraints_jacobian: Constraint Jacobian per world
constraints: Constraint vector per world
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
jacobian_T_constraints: Jacobian^T * Constraints per world
"""
wd_id, i = wp.tid() # Thread indices (= world index, output tile index)
if (
wd_id < jacobian_T_constraints.shape[0]
and world_mask[wd_id] != 0
and i * TILE_SIZE_VRS < jacobian_T_constraints.shape[1]
):
segment_out = wp.tile_zeros(shape=(TILE_SIZE_VRS, 1), dtype=wp.float32)
num_cts = constraints_jacobian.shape[1]
num_tiles_K = (num_cts + TILE_SIZE_CTS - 1) // TILE_SIZE_CTS # Equivalent to ceil(num_cts / TILE_SIZE_CTS)
for k in range(num_tiles_K):
tile_i_3d = wp.tile_load(
constraints_jacobian,
shape=(1, TILE_SIZE_CTS, TILE_SIZE_VRS),
offset=(wd_id, k * TILE_SIZE_CTS, i * TILE_SIZE_VRS),
)
tile_i = wp.tile_reshape(tile_i_3d, (TILE_SIZE_CTS, TILE_SIZE_VRS))
tile_i_T = wp.tile_transpose(tile_i)
segment_k_2d = wp.tile_load(constraints, shape=(1, TILE_SIZE_CTS), offset=(wd_id, k * TILE_SIZE_CTS))
segment_k = wp.tile_reshape(segment_k_2d, (TILE_SIZE_CTS, 1)) # Technically still 2d...
wp.tile_matmul(tile_i_T, segment_k, segment_out)
segment_out_2d = wp.tile_reshape(
segment_out,
(
1,
TILE_SIZE_VRS,
),
)
wp.tile_store(
jacobian_T_constraints,
segment_out_2d,
offset=(
wd_id,
i * TILE_SIZE_VRS,
),
)
@wp.kernel
def _eval_merit_function(
# Inputs
constraints: wp.array2d(dtype=wp.float32),
# Outputs
merit_function_val: wp.array(dtype=wp.float32),
):
"""
A kernel computing the merit function, i.e. the least-squares error 1/2 * ||C||^2, from the constraints
vector C, in each world
Inputs:
constraints: Constraint vector per world
Outputs:
merit_function_val: Merit function value per world; must be zero-initialized
"""
wd_id, i, tid = wp.tid() # Thread indices (= world index, input tile index, thread index in block)
if wd_id < constraints.shape[0] and i * TILE_SIZE_CTS < constraints.shape[1]:
segment = wp.tile_load(constraints, shape=(1, TILE_SIZE_CTS), offset=(wd_id, i * TILE_SIZE_CTS))
segment_error = 0.5 * wp.tile_sum(wp.tile_map(wp.mul, segment, segment))[0]
if tid == 0:
wp.atomic_add(merit_function_val, wd_id, segment_error)
@wp.kernel
def _eval_merit_function_gradient(
# Inputs
step: wp.array2d(dtype=wp.float32),
grad: wp.array2d(dtype=wp.float32),
# Outputs
merit_function_grad: wp.array(dtype=wp.float32),
):
"""
A kernel computing the merit function gradient w.r.t. line search step size, from the step direction
and the gradient in state space (= dC_ds^T * C). This is simply the dot product between these two vectors.
Inputs:
step: Step in variables per world
grad: Gradient w.r.t. state (i.e. body poses) per world
Outputs:
merit_function_grad: Merit function gradient per world; must be zero-initialized
"""
wd_id, i, tid = wp.tid() # Thread indices (= world index, input tile index, thread index in block)
if wd_id < step.shape[0] and i * TILE_SIZE_VRS < step.shape[1]:
step_segment = wp.tile_load(step, shape=(1, TILE_SIZE_VRS), offset=(wd_id, i * TILE_SIZE_VRS))
grad_segment = wp.tile_load(grad, shape=(1, TILE_SIZE_VRS), offset=(wd_id, i * TILE_SIZE_VRS))
tile_dot_prod = wp.tile_sum(wp.tile_map(wp.mul, step_segment, grad_segment))[0]
if tid == 0:
wp.atomic_add(merit_function_grad, wd_id, tile_dot_prod)
return (
_eval_pattern_T_pattern,
_eval_max_constraint,
_eval_jacobian_T_jacobian,
_eval_jacobian_T_constraints,
_eval_merit_function,
_eval_merit_function_gradient,
)
@wp.kernel
def _eval_rhs(
# Inputs
grad: wp.array2d(dtype=wp.float32),
# Outputs
rhs: wp.array2d(dtype=wp.float32),
):
"""
A kernel computing rhs := -grad (where rhs has shape (num_worlds, num_states_max, 1))
Inputs:
grad: Merit function gradient w.r.t. state (i.e. body poses) per world
Outputs:
rhs: Gauss-Newton right-hand side per world
"""
wd_id, state_id_loc = wp.tid() # Thread indices (= world index, state index)
if wd_id < grad.shape[0] and state_id_loc < grad.shape[1]:
rhs[wd_id, state_id_loc] = -grad[wd_id, state_id_loc]
@wp.kernel
def _eval_linear_combination(
# Inputs
alpha: wp.float32,
x: wp.array2d(dtype=wp.float32),
beta: wp.float32,
y: wp.array2d(dtype=wp.float32),
num_rows: wp.array(dtype=wp.int32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
z: wp.array2d(dtype=wp.float32),
):
"""
A kernel computing z := alpha * x + beta * y
Inputs:
alpha: Scalar coefficient
x: Stack of vectors (one per world) to be multiplied by alpha
beta: Scalar coefficient
y: Stack of vectors (one per world) to be multiplied by beta
num_rows: Active size of the vectors (x, y and z) per world
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
z: Output stack of vectors
"""
wd_id, row_id = wp.tid() # Thread indices (= world index, row index)
if wd_id < num_rows.shape[0] and row_id < num_rows[wd_id]:
z[wd_id, row_id] = alpha * x[wd_id, row_id] + beta * y[wd_id, row_id]
@wp.kernel
def _eval_stepped_state(
# Inputs
num_bodies: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
bodies_q_0_flat: wp.array(dtype=wp.float32),
alpha: wp.array(dtype=wp.float32),
step: wp.array2d(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
bodies_q_alpha_flat: wp.array(dtype=wp.float32),
):
"""
A kernel computing states_alpha := states_0 + alpha * step
Inputs:
num_bodies: Num bodies per world
first_body_id: First body id per world
bodies_q_0_flat: Previous state (for step size 0), flattened
alpha: Step size per world
step: Step direction per world
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
bodies_q_alpha_flat: New state (for step size alpha), flattened
"""
wd_id, state_id_loc = wp.tid() # Thread indices (= world index, state index)
rb_id_loc = state_id_loc // 7
if wd_id < num_bodies.shape[0] and world_mask[wd_id] != 0 and rb_id_loc < num_bodies[wd_id]:
state_id_tot = 7 * first_body_id[wd_id] + state_id_loc
bodies_q_alpha_flat[state_id_tot] = bodies_q_0_flat[state_id_tot] + alpha[wd_id] * step[wd_id, state_id_loc]
@wp.kernel
def _apply_line_search_step(
# Inputs
num_bodies: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
bodies_q_alpha: wp.array(dtype=wp.transformf),
line_search_success: wp.array(dtype=wp.int32),
# Outputs
bodies_q: wp.array(dtype=wp.transformf),
):
"""
A kernel replacing the state with the line search result, in worlds where line search succeeded
Note: relies on the fact that the success flag is left at zero for worlds that don't run line search
(otherwise would also need to check against line search mask)
Inputs
num_bodies: Num bodies per world
first_body_id: First body id per world
bodies_q_alpha: Stepped states (line search result)
line_search_success: Per-world line search success flag
Outputs
bodies_q: Output state (rigid body poses)
"""
wd_id, rb_id_loc = wp.tid() # Thread indices (= world index, body index)
if wd_id < num_bodies.shape[0] and line_search_success[wd_id] != 0 and rb_id_loc < num_bodies[wd_id]:
rb_id_tot = first_body_id[wd_id] + rb_id_loc
bodies_q[rb_id_tot] = bodies_q_alpha[rb_id_tot]
@wp.kernel
def _line_search_check(
# Inputs
val_0: wp.array(dtype=wp.float32),
grad_0: wp.array(dtype=wp.float32),
alpha: wp.array(dtype=wp.float32),
val_alpha: wp.array(dtype=wp.float32),
iteration: wp.array(dtype=wp.int32),
max_iterations: wp.array(dtype=wp.int32, shape=(1,)),
# Outputs
line_search_success: wp.array(dtype=wp.int32),
line_search_mask: wp.array(dtype=wp.int32),
line_search_loop_condition: wp.array(dtype=wp.int32, shape=(1,)),
):
"""
A kernel checking the sufficient decrease condition in line search in each world, and updating the looping
condition (zero if max iterations reached, or all worlds successful)
Inputs:
val_0: Merit function value at 0, per world
grad_0: Merit function gradient at 0, per world
alpha: Step size per world (in/out)
val_alpha: Merit function value at alpha, per world
iteration: Iteration count, per world
max_iterations: Max iterations
Outputs:
line_search_success: Convergence per world
line_search_mask: Per-world flag to continue line search (0 = skip)
line_search_loop_condition: Loop condition; must be zero-initialized
"""
wd_id = wp.tid() # Thread index (= world index)
if wd_id < val_0.shape[0] and line_search_mask[wd_id] != 0:
iteration[wd_id] += 1
line_search_success[wd_id] = int(
wp.isfinite(val_alpha[wd_id]) and val_alpha[wd_id] <= val_0[wd_id] + 1e-4 * alpha[wd_id] * grad_0[wd_id]
)
continue_loop_world = iteration[wd_id] < max_iterations[0] and not line_search_success[wd_id]
line_search_mask[wd_id] = int(continue_loop_world)
if continue_loop_world:
alpha[wd_id] *= 0.5
wp.atomic_max(line_search_loop_condition, 0, int(continue_loop_world))
@wp.kernel
def _newton_check(
# Inputs
max_constraint: wp.array(dtype=wp.float32),
tolerance: wp.array(dtype=wp.float32, shape=(1,)),
iteration: wp.array(dtype=wp.int32),
max_iterations: wp.array(dtype=wp.int32, shape=(1,)),
line_search_success: wp.array(dtype=wp.int32),
# Outputs
newton_success: wp.array(dtype=wp.int32),
newton_mask: wp.array(dtype=wp.int32),
newton_loop_condition: wp.array(dtype=wp.int32, shape=(1,)),
):
"""
A kernel checking the convergence (max constraint vs tolerance) in each world, and updating the looping
condition (zero if max iterations reached, or all worlds successful)
Inputs
max_constraint: Max absolute constraint per world
tolerance: Tolerance on max constraint
iteration: Iteration count, per world
max_iterations: Max iterations
line_search_success: Per-world line search success flag
Outputs
newton_success: Convergence per world
newton_mask: Flag to keep iterating per world
newton_loop_condition: Loop condition; must be zero-initialized
"""
wd_id = wp.tid() # Thread index (= world index)
if wd_id < max_constraint.shape[0] and newton_mask[wd_id] != 0:
iteration[wd_id] += 1
max_constraint_wd = max_constraint[wd_id]
is_finite = wp.isfinite(max_constraint_wd)
newton_success[wd_id] = int(is_finite and max_constraint_wd <= tolerance[0])
newton_continue_world = int(
iteration[wd_id] < max_iterations[0]
and not newton_success[wd_id]
and is_finite # Abort when encountering NaN / Inf values
and line_search_success[wd_id] # Abort in case of line search failure
)
newton_mask[wd_id] = newton_continue_world
wp.atomic_max(newton_loop_condition, 0, newton_continue_world)
@wp.kernel
def _eval_target_constraint_velocities(
# Inputs
num_joints: wp.array(dtype=wp.int32),
first_joint_id: wp.array(dtype=wp.int32),
joints_dof_type: wp.array(dtype=wp.int32),
joints_act_type: wp.array(dtype=wp.int32),
actuated_dofs_offset: wp.array(dtype=wp.int32),
ct_full_to_red_map: wp.array(dtype=wp.int32),
actuators_u: wp.array(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
target_cts_u: wp.array2d(dtype=wp.float32),
):
"""
A kernel computing the target constraint velocities, i.e. zero for passive constraints
and the prescribed dof velocity for actuated constraints.
Inputs:
num_joints: Num joints per world
first_joint_id: First joint id per world
joints_dof_type: Joint dof type (i.e. revolute, spherical, ...)
joints_act_type: Joint actuation type (i.e. passive or actuated)
actuated_dofs_offset: Joint first actuated dof id, among all actuated dofs in all worlds
ct_full_to_red_map: Map from full to reduced constraint id
actuators_u: Actuated joint velocities
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
target_cts_u: Target constraint velocities (assumed to be zero-initialized)
"""
# Retrieve the thread indices (= world index, joint index)
wd_id, jt_id_loc = wp.tid()
if wd_id < world_mask.shape[0] and world_mask[wd_id] != 0 and jt_id_loc < num_joints[wd_id]:
# Retrieve the joint model data
jt_id_tot = first_joint_id[wd_id] + jt_id_loc
if joints_act_type[jt_id_tot] != JointActuationType.FORCE:
return
dof_type_j = joints_dof_type[jt_id_tot]
offset_u_j = actuated_dofs_offset[jt_id_tot]
offset_cts_j = ct_full_to_red_map[6 * jt_id_tot]
if dof_type_j == JointDoFType.CARTESIAN:
target_cts_u[wd_id, offset_cts_j] = actuators_u[offset_u_j]
target_cts_u[wd_id, offset_cts_j + 1] = actuators_u[offset_u_j + 1]
target_cts_u[wd_id, offset_cts_j + 2] = actuators_u[offset_u_j + 2]
elif dof_type_j == JointDoFType.CYLINDRICAL:
target_cts_u[wd_id, offset_cts_j] = actuators_u[offset_u_j]
target_cts_u[wd_id, offset_cts_j + 3] = actuators_u[offset_u_j + 1]
elif dof_type_j == JointDoFType.FIXED:
pass # No dofs to apply
elif dof_type_j == JointDoFType.FREE:
target_cts_u[wd_id, offset_cts_j] = actuators_u[offset_u_j]
target_cts_u[wd_id, offset_cts_j + 1] = actuators_u[offset_u_j + 1]
target_cts_u[wd_id, offset_cts_j + 2] = actuators_u[offset_u_j + 2]
target_cts_u[wd_id, offset_cts_j + 3] = actuators_u[offset_u_j + 3]
target_cts_u[wd_id, offset_cts_j + 4] = actuators_u[offset_u_j + 4]
target_cts_u[wd_id, offset_cts_j + 5] = actuators_u[offset_u_j + 5]
elif dof_type_j == JointDoFType.PRISMATIC:
target_cts_u[wd_id, offset_cts_j] = actuators_u[offset_u_j]
elif dof_type_j == JointDoFType.REVOLUTE:
target_cts_u[wd_id, offset_cts_j + 3] = actuators_u[offset_u_j]
elif dof_type_j == JointDoFType.SPHERICAL:
target_cts_u[wd_id, offset_cts_j + 3] = actuators_u[offset_u_j]
target_cts_u[wd_id, offset_cts_j + 4] = actuators_u[offset_u_j + 1]
target_cts_u[wd_id, offset_cts_j + 5] = actuators_u[offset_u_j + 2]
elif dof_type_j == JointDoFType.UNIVERSAL:
target_cts_u[wd_id, offset_cts_j + 3] = actuators_u[offset_u_j]
target_cts_u[wd_id, offset_cts_j + 4] = actuators_u[offset_u_j + 1]
@wp.kernel
def _eval_body_velocities(
# Inputs
num_bodies: wp.array(dtype=wp.int32),
first_body_id: wp.array(dtype=wp.int32),
bodies_q: wp.array(dtype=wp.transformf),
bodies_q_dot: wp.array2d(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
# Outputs
bodies_u: wp.array(dtype=vec6f),
):
"""
A kernel computing the body velocities (twists) from the time derivative of body poses,
computing in particular angular velocities omega = G(q)q_dot
Inputs:
num_bodies: Number of bodies per world
first_body_id: First body id per world
bodies_q: Body poses
bodies_q_dot: Time derivative of body poses
world_mask: Per-world flag to perform the computation (0 = skip)
Outputs:
bodies_u: Body velocities (twists)
"""
wd_id, rb_id_loc = wp.tid() # Thread indices (= world index, body index)
if wd_id < world_mask.shape[0] and world_mask[wd_id] != 0 and rb_id_loc < num_bodies[wd_id]:
# Indices / offsets
rb_id_tot = first_body_id[wd_id] + rb_id_loc
offset_q_dot = 7 * rb_id_loc
# Copy linear velocity
bodies_u[rb_id_tot][0] = bodies_q_dot[wd_id, offset_q_dot]
bodies_u[rb_id_tot][1] = bodies_q_dot[wd_id, offset_q_dot + 1]
bodies_u[rb_id_tot][2] = bodies_q_dot[wd_id, offset_q_dot + 2]
# Compute angular velocities
q = wp.transform_get_rotation(bodies_q[rb_id_tot])
q_dot = wp.vec4f(
bodies_q_dot[wd_id, offset_q_dot + 3],
bodies_q_dot[wd_id, offset_q_dot + 4],
bodies_q_dot[wd_id, offset_q_dot + 5],
bodies_q_dot[wd_id, offset_q_dot + 6],
)
omega = 2.0 * (G_of(q) * q_dot)
bodies_u[rb_id_tot][3] = omega[0]
bodies_u[rb_id_tot][4] = omega[1]
bodies_u[rb_id_tot][5] = omega[2]
@wp.kernel
def _update_cg_tolerance_kernel(
# Input
max_constraint: wp.array(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
# Output
atol: wp.array(dtype=wp.float32),
rtol: wp.array(dtype=wp.float32),
):
"""
A kernel heuristically adapting the CG tolerance based on the current constraint residual
(starting with a loose tolerance, and tightening it as we converge)
Note: needs to be refined, until then we are still using a fixed tolerance
"""
wd_id = wp.tid()
if wd_id >= world_mask.shape[0] or world_mask[wd_id] == 0:
return
tol = wp.max(1e-8, wp.min(1e-5, 1e-3 * max_constraint[wd_id]))
atol[wd_id] = tol
rtol[wd_id] = tol
###
# Interfaces
###
class ForwardKinematicsSolver:
"""
Forward Kinematics solver class
"""
class PreconditionerType(IntEnum):
"""Conjugate gradient preconditioning options of the FK solver, if sparsity is enabled."""
NONE = 0
"""No preconditioning"""
JACOBI_DIAGONAL = 1
"""Diagonal Jacobi preconditioner"""
JACOBI_BLOCK_DIAGONAL = 2
"""Blockwise-diagonal Jacobi preconditioner, alternating blocks of size 3 and 4 along the diagonal,
corresponding to the position and orientation (quaternion) of individual rigid bodies."""
@classmethod
def from_string(cls, s: str) -> ForwardKinematicsSolver.PreconditionerType:
"""Converts a string to a ForwardKinematicsSolver.PreconditionerType enum value."""
try:
return cls[s.upper()]
except KeyError as e:
raise ValueError(
f"Invalid ForwardKinematicsSolver.PreconditionerType: {s}."
f"Valid options are: {[e.name for e in cls]}"
) from e
Config = ForwardKinematicsSolverConfig
"""
Defines a type alias of the FK solver configurations container, including convergence
criteria, maximum iterations, and options for the linear solver and preconditioning.
See :class:`ForwardKinematicsSolverConfig` for the full
list of configuration options and their descriptions.
"""
@dataclass
class Status:
"""
Container holding detailed information on the success/failure status of a forward kinematics solve.
"""
success: np.ndarray(dtype=np.int32)
"""
Solver success flag per world, as an integer array (0 = failure, 1 = success).\n
Shape `(num_worlds,)` and type :class:`np.int32`.
Note that in some cases the solver may fail to converge within the maximum number
of iterations, but still produce a solution with a reasonable constraint residual.
In such cases, the success flag will be set to 0, but the `max_constraints` field
can be inspected to check the actual constraint residuals and determine if the
solution is acceptable for the intended application.
"""
iterations: np.ndarray(dtype=np.int32)
"""
Number of Gauss-Newton iterations executed per world.\n
Shape `(num_worlds,)` and type :class:`np.int32`.
"""
max_constraints: np.ndarray(dtype=np.float32)
"""
Maximal absolute kinematic constraint residual at the final solution, per world.\n
Shape `(num_worlds,)` and type :class:`np.float32`.
"""
def __init__(self, model: ModelKamino | None = None, config: ForwardKinematicsSolver.Config | None = None):
"""
Initializes the solver to solve forward kinematics for a given model.
Parameters
----------
model : ModelKamino, optional
ModelKamino for which to solve forward kinematics. If not provided, the finalize() method
must be called at a later time for deferred initialization (default: None).
config : ForwardKinematicsSolver.Config, optional
Solver config. If not provided, the default config will be used (default: None).
"""
self.model: ModelKamino | None = None
"""Underlying model"""
self.device: wp.DeviceLike = None
"""Device for data allocations"""
self.config: ForwardKinematicsSolver.Config = ForwardKinematicsSolver.Config()
"""Solver config"""
self.graph: wp.Graph | None = None
"""Cuda graph for the convenience function with verbosity options"""
# Note: there are many other internal data members below, which are not documented here
# Set model and config, and finalize if model was provided
self.model = model
if config is not None:
self.config = config
if model is not None:
self.finalize()
def finalize(self, model: ModelKamino | None = None, config: ForwardKinematicsSolver.Config | None = None):
"""
Finishes the solver initialization, performing necessary allocations and precomputations.
This method only needs to be called manually if a model was not provided in the constructor,
or to reset the solver for a new model.
Parameters
----------
model : ModelKamino, optional
ModelKamino for which to solve forward kinematics. If not provided, the model given to the
constructor will be used. Must be provided if not given to the constructor (default: None).
config : ForwardKinematicsSolver.Config, optional
Solver config. If not provided, the config given to the constructor, or if not, the
default config will be used (default: None).
"""
# Initialize the model and config if provided
if model is not None:
self.model = model
if config is not None:
self.config = config
if self.model is None:
raise ValueError("ForwardKinematicsSolver: error, provided model is None.")
# Initialize device
self.device = self.model.device
# Retrieve / compute dimensions - Worlds
self.num_worlds = self.model.size.num_worlds # For convenience
# Convert preconditioner type
self._preconditioner_type = ForwardKinematicsSolver.PreconditionerType.from_string(self.config.preconditioner)
# Retrieve / compute dimensions - Bodies
num_bodies = self.model.info.num_bodies.numpy() # Number of bodies per world
first_body_id = np.concatenate(([0], num_bodies.cumsum())) # Index of first body per world
self.num_bodies_max = self.model.size.max_of_num_bodies # Max number of bodies across worlds
# Retrieve / compute dimensions - States (i.e., body poses)
num_states = 7 * num_bodies # Number of state dimensions per world
self.num_states_tot = 7 * self.model.size.sum_of_num_bodies # State dimensions for the whole model
self.num_states_max = 7 * self.num_bodies_max # Max state dimension across worlds
# Retrieve / compute dimensions - Joints (main model)
num_joints_prev = self.model.info.num_joints.numpy().copy() # Number of joints per world
first_joint_id_prev = np.concatenate(([0], num_joints_prev.cumsum())) # Index of first joint per world
# Retrieve / compute dimensions - Actuated coordinates (main model)
num_actuated_coords_prev = (
self.model.info.num_actuated_joint_coords.numpy().copy()
) # Number of actuated joint coordinates per world
first_actuated_coord_prev = np.concatenate(
([0], num_actuated_coords_prev.cumsum())
) # Index of first actuated coordinate per world
actuated_coord_offsets_prev = (
self.model.joints.actuated_coords_offset.numpy().copy()
) # Index of first joint actuated coordinate, among actuated coordinates of a single world
for wd_id in range(self.num_worlds): # Convert into offsets among all actuated coordinates
actuated_coord_offsets_prev[first_joint_id_prev[wd_id] : first_joint_id_prev[wd_id + 1]] += (
first_actuated_coord_prev[wd_id]
)
# Note: will currently produce garbage for passive joints (because for these the offsets are set to -1)
# but we won't read these values below anyway.
# Retrieve / compute dimensions - Actuated dofs (main model)
num_actuated_dofs_prev = (
self.model.info.num_actuated_joint_dofs.numpy().copy()
) # Number of actuated joint dofs per world
first_actuated_dof_prev = np.concatenate(
([0], num_actuated_dofs_prev.cumsum())
) # Index of first actuated dof per world
actuated_dof_offsets_prev = (
self.model.joints.actuated_dofs_offset.numpy().copy()
) # Index of first joind actuated dof, among actuated dofs of a single world
for wd_id in range(self.num_worlds): # Convert into offsets among all actuated dofs
actuated_dof_offsets_prev[first_joint_id_prev[wd_id] : first_joint_id_prev[wd_id + 1]] += (
first_actuated_dof_prev[wd_id]
)
# Note: will currently produce garbage for passive joints (because for these the offsets are set to -1)
# but we won't read these values below anyway.
# Create a copy of the model's joints with added actuated
# free joints as needed to reset the base position/orientation
joints_dof_type_prev = self.model.joints.dof_type.numpy().copy()
joints_act_type_prev = self.model.joints.act_type.numpy().copy()
joints_bid_B_prev = self.model.joints.bid_B.numpy().copy()
joints_bid_F_prev = self.model.joints.bid_F.numpy().copy()
joints_B_r_Bj_prev = self.model.joints.B_r_Bj.numpy().copy()
joints_F_r_Fj_prev = self.model.joints.F_r_Fj.numpy().copy()
joints_X_j_prev = self.model.joints.X_j.numpy().copy()
joints_num_coords_prev = self.model.joints.num_coords.numpy().copy()
joints_num_dofs_prev = self.model.joints.num_dofs.numpy().copy()
joints_dof_type = []
joints_act_type = []
joints_bid_B = []
joints_bid_F = []
joints_B_r_Bj = []
joints_F_r_Fj = []
joints_X_j = []
joints_num_actuated_coords = [] # Number of actuated coordinates per joint (0 for passive joints)
joints_num_actuated_dofs = [] # Number of actuated dofs per joint (0 for passive joints)
num_joints = np.zeros(self.num_worlds, dtype=np.int32) # Number of joints per world
self.num_joints_tot = 0 # Number of joints for all worlds
actuated_coords_map = [] # Map of new actuated coordinates to these in the model or to the base coordinates
actuated_dofs_map = [] # Map of new actuated dofs to these in the model or to the base dofs
base_q_default = np.zeros(7 * self.num_worlds, dtype=np.float32) # Default base pose
bodies_q_0 = self.model.bodies.q_i_0.numpy()
base_joint_ids = self.num_worlds * [-1] # Base joint id per world
base_joint_ids_input = self.model.info.base_joint_index.numpy().tolist()
base_body_ids_input = self.model.info.base_body_index.numpy().tolist()
for wd_id in range(self.num_worlds):
# Retrieve base joint id
base_joint_id = base_joint_ids_input[wd_id]
# Copy data for all kept joints
world_joint_ids = [
i for i in range(first_joint_id_prev[wd_id], first_joint_id_prev[wd_id + 1]) if i != base_joint_id
]
for jt_id_prev in world_joint_ids:
joints_dof_type.append(joints_dof_type_prev[jt_id_prev])
joints_act_type.append(joints_act_type_prev[jt_id_prev])
joints_bid_B.append(joints_bid_B_prev[jt_id_prev])
joints_bid_F.append(joints_bid_F_prev[jt_id_prev])
joints_B_r_Bj.append(joints_B_r_Bj_prev[jt_id_prev])
joints_F_r_Fj.append(joints_F_r_Fj_prev[jt_id_prev])
joints_X_j.append(joints_X_j_prev[jt_id_prev])
if joints_act_type[-1] == JointActuationType.FORCE:
num_coords_jt = joints_num_coords_prev[jt_id_prev]
joints_num_actuated_coords.append(num_coords_jt)
coord_offset = actuated_coord_offsets_prev[jt_id_prev]
actuated_coords_map.extend(range(coord_offset, coord_offset + num_coords_jt))
num_dofs_jt = joints_num_dofs_prev[jt_id_prev]
joints_num_actuated_dofs.append(num_dofs_jt)
dof_offset = actuated_dof_offsets_prev[jt_id_prev]
actuated_dofs_map.extend(range(dof_offset, dof_offset + num_dofs_jt))
else:
joints_num_actuated_coords.append(0)
joints_num_actuated_dofs.append(0)
# Add joint for base joint / base body
if base_joint_id >= 0: # Replace base joint with an actuated free joint
joints_dof_type.append(JointDoFType.FREE)
joints_act_type.append(JointActuationType.FORCE)
joints_bid_B.append(-1)
joints_bid_F.append(joints_bid_F_prev[base_joint_id])
joints_B_r_Bj.append(joints_B_r_Bj_prev[base_joint_id])
joints_F_r_Fj.append(joints_F_r_Fj_prev[base_joint_id])
joints_X_j.append(joints_X_j_prev[base_joint_id])
joints_num_actuated_coords.append(7)
coord_offset = -7 * wd_id - 1 # We encode offsets in base_q negatively with i -> -i - 1
actuated_coords_map.extend(range(coord_offset, coord_offset - 7, -1))
base_q_default[7 * wd_id : 7 * wd_id + 7] = [
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
1.0,
] # Default to zero of free joint
joints_num_actuated_dofs.append(6)
dof_offset = -6 * wd_id - 1 # We encode offsets in base_u negatively with i -> -i - 1
actuated_dofs_map.extend(range(dof_offset, dof_offset - 6, -1))
base_joint_ids[wd_id] = len(joints_dof_type) - 1
elif base_body_ids_input[wd_id] >= 0: # Add an actuated free joint to the base body
base_body_id = base_body_ids_input[wd_id]
joints_dof_type.append(JointDoFType.FREE)
joints_act_type.append(JointActuationType.FORCE)
joints_bid_B.append(-1)
joints_bid_F.append(base_body_id)
joints_B_r_Bj.append(np.zeros(3, dtype=np.float32))
joints_F_r_Fj.append(np.zeros(3, dtype=np.float32))
joints_X_j.append(np.eye(3, 3, dtype=np.float32))
joints_num_actuated_coords.append(7)
# Note: we rely on the initial body orientations being identity
# Only then will the corresponding joint coordinates be interpretable as
# specifying the absolute base position and orientation
coord_offset = -7 * wd_id - 1 # We encode offsets in base_q negatively with i -> -i - 1
actuated_coords_map.extend(range(coord_offset, coord_offset - 7, -1))
base_q_default[7 * wd_id : 7 * wd_id + 7] = bodies_q_0[base_body_id] # Default to initial body pose
joints_num_actuated_dofs.append(6)
dof_offset = -6 * wd_id - 1 # We encode offsets in base_u negatively with i -> -i - 1
actuated_dofs_map.extend(range(dof_offset, dof_offset - 6, -1))
base_joint_ids[wd_id] = len(joints_dof_type) - 1
# Record number of joints
num_joints_world = len(joints_dof_type) - self.num_joints_tot
self.num_joints_tot += num_joints_world
num_joints[wd_id] = num_joints_world
# Retrieve / compute dimensions - Joints (FK model)
first_joint_id = np.concatenate(([0], num_joints.cumsum())) # Index of first joint per world
self.num_joints_max = max(num_joints) # Max number of joints across worlds
# Retrieve / compute dimensions - Actuated coordinates (FK model)
joints_num_actuated_coords = np.array(joints_num_actuated_coords) # Number of actuated coordinates per joint
actuated_coord_offsets = np.concatenate(
([0], joints_num_actuated_coords.cumsum())
) # First actuated coordinate offset per joint, among all actuated coordinates
self.num_actuated_coords = actuated_coord_offsets[-1]
# Retrieve / compute dimensions - Actuated dofs (FK model)
joints_num_actuated_dofs = np.array(joints_num_actuated_dofs) # Number of actuated dofs per joint
actuated_dof_offsets = np.concatenate(
([0], joints_num_actuated_dofs.cumsum())
) # First actuated dof offset per joint, among all actuated dofs
self.num_actuated_dofs = actuated_dof_offsets[-1]
# Retrieve / compute dimensions - Constraints
num_constraints = num_bodies.copy() # Number of kinematic constraints per world (unit quat. + joints)
has_universal_joints = False # Whether the model has a least one passive universal joint
constraint_full_to_red_map = np.full(6 * self.num_joints_tot, -1, dtype=np.int32)
for wd_id in range(self.num_worlds):
ct_count = num_constraints[wd_id]
for jt_id_loc in range(num_joints[wd_id]):
jt_id_tot = first_joint_id[wd_id] + jt_id_loc # Joint id among all joints
act_type = joints_act_type[jt_id_tot]
if act_type == JointActuationType.FORCE: # Actuator: select all six constraints
for i in range(6):
constraint_full_to_red_map[6 * jt_id_tot + i] = ct_count + i
ct_count += 6
else:
dof_type = joints_dof_type[jt_id_tot]
if dof_type == JointDoFType.CARTESIAN:
for i in range(3):
constraint_full_to_red_map[6 * jt_id_tot + 3 + i] = ct_count + i
ct_count += 3
elif dof_type == JointDoFType.CYLINDRICAL:
constraint_full_to_red_map[6 * jt_id_tot + 1] = ct_count
constraint_full_to_red_map[6 * jt_id_tot + 2] = ct_count + 1
constraint_full_to_red_map[6 * jt_id_tot + 4] = ct_count + 2
constraint_full_to_red_map[6 * jt_id_tot + 5] = ct_count + 3
ct_count += 4
elif dof_type == JointDoFType.FIXED:
for i in range(6):
constraint_full_to_red_map[6 * jt_id_tot + i] = ct_count + i
ct_count += 6
elif dof_type == JointDoFType.FREE:
pass
elif dof_type == JointDoFType.PRISMATIC:
constraint_full_to_red_map[6 * jt_id_tot + 1] = ct_count
constraint_full_to_red_map[6 * jt_id_tot + 2] = ct_count + 1
for i in range(3):
constraint_full_to_red_map[6 * jt_id_tot + 3 + i] = ct_count + 2 + i
ct_count += 5
elif dof_type == JointDoFType.REVOLUTE:
for i in range(3):
constraint_full_to_red_map[6 * jt_id_tot + i] = ct_count + i
constraint_full_to_red_map[6 * jt_id_tot + 4] = ct_count + 3
constraint_full_to_red_map[6 * jt_id_tot + 5] = ct_count + 4
ct_count += 5
elif dof_type == JointDoFType.SPHERICAL:
for i in range(3):
constraint_full_to_red_map[6 * jt_id_tot + i] = ct_count + i
ct_count += 3
elif dof_type == JointDoFType.UNIVERSAL:
for i in range(3):
constraint_full_to_red_map[6 * jt_id_tot + i] = ct_count + i
constraint_full_to_red_map[6 * jt_id_tot + 5] = ct_count + 3
ct_count += 4
has_universal_joints = True
else:
raise RuntimeError("Unknown joint dof type")
num_constraints[wd_id] = ct_count
self.num_constraints_max = np.max(num_constraints)
# Retrieve / compute dimensions - Number of tiles (for kernels using Tile API)
self.num_tiles_constraints = (
self.num_constraints_max + self.config.TILE_SIZE_CTS - 1
) // self.config.TILE_SIZE_CTS
self.num_tiles_states = (self.num_states_max + self.config.TILE_SIZE_VRS - 1) // self.config.TILE_SIZE_VRS
# Data allocation or transfer from numpy to warp
with wp.ScopedDevice(self.device):
# Dimensions
self.first_body_id = wp.from_numpy(first_body_id, dtype=wp.int32)
self.num_joints = wp.from_numpy(num_joints, dtype=wp.int32)
self.first_joint_id = wp.from_numpy(first_joint_id, dtype=wp.int32)
self.actuated_coord_offsets = wp.from_numpy(actuated_coord_offsets, dtype=wp.int32)
self.actuated_coords_map = wp.from_numpy(np.array(actuated_coords_map), dtype=wp.int32)
self.actuated_dof_offsets = wp.from_numpy(actuated_dof_offsets, dtype=wp.int32)
self.actuated_dofs_map = wp.from_numpy(np.array(actuated_dofs_map), dtype=wp.int32)
self.num_states = wp.from_numpy(num_states, dtype=wp.int32)
self.num_constraints = wp.from_numpy(num_constraints, dtype=wp.int32)
self.constraint_full_to_red_map = wp.from_numpy(constraint_full_to_red_map, dtype=wp.int32)
# Modified joints
self.joints_dof_type = wp.from_numpy(joints_dof_type, dtype=wp.int32)
self.joints_act_type = wp.from_numpy(joints_act_type, dtype=wp.int32)
self.joints_bid_B = wp.from_numpy(joints_bid_B, dtype=wp.int32)
self.joints_bid_F = wp.from_numpy(joints_bid_F, dtype=wp.int32)
self.joints_B_r_Bj = wp.from_numpy(joints_B_r_Bj, dtype=wp.vec3f)
self.joints_F_r_Fj = wp.from_numpy(joints_F_r_Fj, dtype=wp.vec3f)
self.joints_X_j = wp.from_numpy(joints_X_j, dtype=wp.mat33f)
self.base_joint_id = wp.from_numpy(base_joint_ids, dtype=wp.int32)
# Default base state
self.base_q_default = wp.from_numpy(base_q_default, dtype=wp.transformf)
self.base_u_default = wp.zeros(shape=(self.num_worlds,), dtype=vec6f)
# Line search
self.max_line_search_iterations = wp.array(dtype=wp.int32, shape=(1,)) # Max iterations
self.max_line_search_iterations.fill_(self.config.max_line_search_iterations)
self.line_search_iteration = wp.array(dtype=wp.int32, shape=(self.num_worlds,)) # Iteration count
self.line_search_loop_condition = wp.array(dtype=wp.int32, shape=(1,)) # Loop condition
self.line_search_success = wp.array(dtype=wp.int32, shape=(self.num_worlds,)) # Convergence, per world
self.line_search_mask = wp.array(
dtype=wp.int32, shape=(self.num_worlds,)
) # Flag to keep iterating per world
self.val_0 = wp.array(dtype=wp.float32, shape=(self.num_worlds,)) # Merit function value at 0, per world
self.grad_0 = wp.array(
dtype=wp.float32, shape=(self.num_worlds,)
) # Merit function gradient at 0, per world
self.alpha = wp.array(dtype=wp.float32, shape=(self.num_worlds,)) # Step size, per world
self.bodies_q_alpha = wp.array(dtype=wp.transformf, shape=(self.model.size.sum_of_num_bodies,)) # New state
self.val_alpha = wp.array(dtype=wp.float32, shape=(self.num_worlds,)) # New merit function value, per world
# Gauss-Newton
self.max_newton_iterations = wp.array(dtype=wp.int32, shape=(1,)) # Max iterations
self.max_newton_iterations.fill_(self.config.max_newton_iterations)
self.newton_iteration = wp.array(dtype=wp.int32, shape=(self.num_worlds,)) # Iteration count
self.newton_loop_condition = wp.array(dtype=wp.int32, shape=(1,)) # Loop condition
self.newton_success = wp.array(dtype=wp.int32, shape=(self.num_worlds,)) # Convergence per world
self.newton_mask = wp.array(dtype=wp.int32, shape=(self.num_worlds,)) # Flag to keep iterating per world
self.tolerance = wp.array(dtype=wp.float32, shape=(1,)) # Tolerance on max constraint
self.tolerance.fill_(self.config.tolerance)
self.actuators_q = wp.array(dtype=wp.float32, shape=(self.num_actuated_coords,)) # Actuated coordinates
self.pos_control_transforms = wp.array(
dtype=wp.transformf, shape=(self.num_joints_tot,)
) # Position-control transformations at joints
self.constraints = wp.zeros(
dtype=wp.float32,
shape=(
self.num_worlds,
self.num_constraints_max,
),
) # Constraints vector per world
self.max_constraint = wp.array(dtype=wp.float32, shape=(self.num_worlds,)) # Maximal constraint per world
self.jacobian = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_constraints_max, self.num_states_max)
) # Constraints Jacobian per world
if not self.config.use_sparsity:
self.lhs = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_states_max, self.num_states_max)
) # Gauss-Newton left-hand side per world
self.grad = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_states_max)
) # Merit function gradient w.r.t. state per world
self.rhs = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_states_max)
) # Gauss-Newton right-hand side per world (=-grad)
self.step = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_states_max)
) # Step in state variables per world
self.jacobian_times_vector = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_constraints_max)
) # Intermediary vector when computing J^T * (J * x)
self.lhs_times_vector = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_states_max)
) # Intermediary vector when computing J^T * (J * x)
# Velocity solver
self.actuators_u = wp.array(
dtype=wp.float32, shape=(self.num_actuated_dofs,)
) # Velocities for actuated dofs of fk model
self.target_cts_u = wp.zeros(
dtype=wp.float32,
shape=(
self.num_worlds,
self.num_constraints_max,
),
) # Target velocity per constraint
self.bodies_q_dot = wp.array(
ptr=self.step.ptr, dtype=wp.float32, shape=(self.num_worlds, self.num_states_max), copy=False
) # Time derivative of body poses (alias of self.step for data re-use)
# Note: we also re-use self.jacobian, self.lhs and self.rhs for the velocity solver
# Initialize kernels that depend on static values
self._eval_joint_constraints_kernel = create_eval_joint_constraints_kernel(has_universal_joints)
self._eval_joint_constraints_jacobian_kernel = create_eval_joint_constraints_jacobian_kernel(
has_universal_joints
)
(
self._eval_pattern_T_pattern_kernel,
self._eval_max_constraint_kernel,
self._eval_jacobian_T_jacobian_kernel,
self._eval_jacobian_T_constraints_kernel,
self._eval_merit_function_kernel,
self._eval_merit_function_gradient_kernel,
) = create_tile_based_kernels(self.config.TILE_SIZE_CTS, self.config.TILE_SIZE_VRS)
# Compute sparsity pattern and initialize linear solver for dense (semi-sparse) case
if not self.config.use_sparsity:
# Jacobian sparsity pattern
sparsity_pattern = np.zeros((self.num_worlds, self.num_constraints_max, self.num_states_max), dtype=int)
for wd_id in range(self.num_worlds):
for rb_id_loc in range(num_bodies[wd_id]):
sparsity_pattern[wd_id, rb_id_loc, 7 * rb_id_loc + 3 : 7 * rb_id_loc + 7] = 1
for jt_id_loc in range(num_joints[wd_id]):
jt_id_tot = first_joint_id[wd_id] + jt_id_loc
base_id_tot = joints_bid_B[jt_id_tot]
follower_id_tot = joints_bid_F[jt_id_tot]
rb_ids_tot = [base_id_tot, follower_id_tot] if base_id_tot >= 0 else [follower_id_tot]
for rb_id_tot in rb_ids_tot:
rb_id_loc = rb_id_tot - first_body_id[wd_id]
state_offset = 7 * rb_id_loc
for i in range(3):
ct_offset = constraint_full_to_red_map[6 * jt_id_tot + i] # ith translation constraint
if ct_offset >= 0:
sparsity_pattern[wd_id, ct_offset, state_offset : state_offset + 7] = 1
ct_offset = constraint_full_to_red_map[6 * jt_id_tot + 3 + i] # ith rotation constraint
if ct_offset >= 0:
sparsity_pattern[wd_id, ct_offset, state_offset + 3 : state_offset + 7] = 1
# Jacobian^T * Jacobian sparsity pattern
sparsity_pattern_wp = wp.from_numpy(sparsity_pattern, dtype=wp.float32, device=self.device)
sparsity_pattern_lhs_wp = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_states_max, self.num_states_max), device=self.device
)
wp.launch_tiled(
self._eval_pattern_T_pattern_kernel,
dim=(self.num_worlds, self.num_tiles_states, self.num_tiles_states),
inputs=[sparsity_pattern_wp, sparsity_pattern_lhs_wp],
block_dim=64,
device=self.device,
)
sparsity_pattern_lhs = sparsity_pattern_lhs_wp.numpy().astype("int32")
# Initialize linear solver (semi-sparse LLT)
self.linear_solver_llt = SemiSparseBlockCholeskySolverBatched(
self.num_worlds,
self.num_states_max,
block_size=16, # TODO: optimize this (e.g. 14 ?)
device=self.device,
enable_reordering=True,
)
self.linear_solver_llt.capture_sparsity_pattern(sparsity_pattern_lhs, num_states)
# Compute sparsity pattern and initialize linear solver for sparse case
if self.config.use_sparsity:
self.sparse_jacobian = BlockSparseMatrices(
device=self.device, nzb_dtype=BlockDType(dtype=wp.float32, shape=(7,)), num_matrices=self.num_worlds
)
jacobian_dims = list(zip(num_constraints.tolist(), (7 * num_bodies).tolist(), strict=True))
# Determine number of nzb, per world and in total
num_nzb = num_bodies.copy() # nzb due to rigid body unit quaternion constraints
jt_num_constraints = (constraint_full_to_red_map.reshape((-1, 6)) >= 0).sum(axis=1)
jt_num_bodies = np.array([1 if joints_bid_B[i] < 0 else 2 for i in range(self.num_joints_tot)])
for wd_id in range(self.num_worlds): # nzb due to joint constraints
start = first_joint_id[wd_id]
end = start + num_joints[wd_id]
num_nzb[wd_id] += (jt_num_constraints[start:end] * jt_num_bodies[start:end]).sum()
first_nzb = np.concatenate(([0], num_nzb.cumsum()))
num_nzb_tot = num_nzb.sum()
# Symbolic assembly
nzb_row = np.empty(num_nzb_tot, dtype=np.int32)
nzb_col = np.empty(num_nzb_tot, dtype=np.int32)
rb_nzb_id = np.empty(self.model.size.sum_of_num_bodies, dtype=np.int32)
ct_nzb_id_base = np.full(6 * self.num_joints_tot, -1, dtype=np.int32)
ct_nzb_id_follower = np.full(6 * self.num_joints_tot, -1, dtype=np.int32)
for wd_id in range(self.num_worlds):
start_nzb = first_nzb[wd_id]
# Compute index, row and column of rigid body nzb
start_rb = first_body_id[wd_id]
size_rb = num_bodies[wd_id]
rb_ids = np.arange(size_rb)
rb_nzb_id[start_rb : start_rb + size_rb] = start_nzb + rb_ids
nzb_row[start_nzb : start_nzb + size_rb] = rb_ids
nzb_col[start_nzb : start_nzb + size_rb] = 7 * rb_ids
# Compute index, row and column of constraint nzb
start_nzb += size_rb
for jt_id_loc in range(num_joints[wd_id]):
jt_id_tot = jt_id_loc + first_joint_id[wd_id]
has_base = joints_bid_B[jt_id_tot] >= 0
row_ids_full = constraint_full_to_red_map[6 * jt_id_tot : 6 * jt_id_tot + 6]
row_ids_red = [i for i in row_ids_full if i >= 0]
num_cts = len(row_ids_red)
if has_base:
nzb_id_base = ct_nzb_id_base[6 * jt_id_tot : 6 * jt_id_tot + 6]
nzb_id_base[row_ids_full >= 0] = np.arange(start_nzb, start_nzb + num_cts)
nzb_row[start_nzb : start_nzb + num_cts] = row_ids_red
base_id_loc = joints_bid_B[jt_id_tot] - first_body_id[wd_id]
nzb_col[start_nzb : start_nzb + num_cts] = 7 * base_id_loc
start_nzb += num_cts
nzb_id_follower = ct_nzb_id_follower[6 * jt_id_tot : 6 * jt_id_tot + 6]
nzb_id_follower[row_ids_full >= 0] = np.arange(start_nzb, start_nzb + num_cts)
nzb_row[start_nzb : start_nzb + num_cts] = row_ids_red
follower_id_loc = joints_bid_F[jt_id_tot] - first_body_id[wd_id]
nzb_col[start_nzb : start_nzb + num_cts] = 7 * follower_id_loc
start_nzb += num_cts
# Transfer data to GPU
self.sparse_jacobian.finalize(jacobian_dims, num_nzb.tolist())
self.sparse_jacobian.dims.assign(jacobian_dims)
self.sparse_jacobian.num_nzb.assign(num_nzb)
self.sparse_jacobian.nzb_coords.assign(np.stack((nzb_row, nzb_col)).T.flatten())
with wp.ScopedDevice(self.device):
self.rb_nzb_id = wp.from_numpy(rb_nzb_id, dtype=wp.int32)
self.ct_nzb_id_base = wp.from_numpy(ct_nzb_id_base, dtype=wp.int32)
self.ct_nzb_id_follower = wp.from_numpy(ct_nzb_id_follower, dtype=wp.int32)
# Initialize Jacobian assembly kernel
self._eval_joint_constraints_sparse_jacobian_kernel = create_eval_joint_constraints_sparse_jacobian_kernel(
has_universal_joints
)
# Initialize Jacobian linear operator
self.sparse_jacobian_op = BlockSparseLinearOperators(self.sparse_jacobian)
# Initialize preconditioner
if self._preconditioner_type == ForwardKinematicsSolver.PreconditionerType.JACOBI_DIAGONAL:
self.jacobian_diag_inv = wp.array(
dtype=wp.float32, device=self.device, shape=(self.num_worlds, self.num_states_max)
)
preconditioner_op = BatchedLinearOperator.from_diagonal(self.jacobian_diag_inv, self.num_states)
elif self._preconditioner_type == ForwardKinematicsSolver.PreconditionerType.JACOBI_BLOCK_DIAGONAL:
self.inv_blocks_3 = wp.array(
dtype=wp.mat33f, shape=(self.num_worlds, self.num_bodies_max), device=self.device
)
self.inv_blocks_4 = wp.array(
dtype=wp.mat44f, shape=(self.num_worlds, self.num_bodies_max), device=self.device
)
preconditioner_op = BatchedLinearOperator(
gemv_fn=get_blockwise_diag_3_4_gemv_2d(self.inv_blocks_3, self.inv_blocks_4, self.num_states),
n_worlds=self.num_worlds,
max_dim=self.num_states_max,
active_dims=self.num_states,
device=self.device,
dtype=wp.float32,
)
else:
preconditioner_op = None
# Initialize CG solver
cg_op = BatchedLinearOperator(
n_worlds=self.num_worlds,
max_dim=self.num_states_max,
active_dims=self.num_states,
dtype=wp.float32,
device=self.device,
gemv_fn=self._eval_lhs_gemv,
)
self.cg_atol = wp.array(dtype=wp.float32, shape=self.num_worlds, device=self.device)
self.cg_rtol = wp.array(dtype=wp.float32, shape=self.num_worlds, device=self.device)
self.cg_max_iter = wp.from_numpy(2 * self.num_states.numpy(), dtype=wp.int32, device=self.device)
self.linear_solver_cg = CGSolver(
A=cg_op,
active_dims=self.num_states,
Mi=preconditioner_op,
atol=self.cg_atol,
rtol=self.cg_rtol,
maxiter=self.cg_max_iter,
)
###
# Internal evaluators (graph-capturable functions working on pre-allocated data)
###
def _reset_state(
self,
bodies_q: wp.array(dtype=wp.transformf),
world_mask: wp.array(dtype=wp.int32),
):
"""
Internal function resetting the bodies state to the reference state stored in the model.
"""
wp.launch(
_reset_state,
dim=(self.num_worlds, self.num_states_max),
inputs=[
self.model.info.num_bodies,
self.first_body_id,
wp.array(
ptr=self.model.bodies.q_i_0.ptr,
dtype=wp.float32,
shape=(self.num_states_tot,),
device=self.device,
copy=False,
),
world_mask,
wp.array(
ptr=bodies_q.ptr, dtype=wp.float32, shape=(self.num_states_tot,), device=self.device, copy=False
),
],
device=self.device,
)
def _reset_state_base_q(
self,
bodies_q: wp.array(dtype=wp.transformf),
base_q: wp.array(dtype=wp.transformf),
world_mask: wp.array(dtype=wp.int32),
):
"""
Internal function resetting the bodies state to a rigid transformation of the reference state,
computed so that the base body is aligned on its prescribed pose.
"""
wp.launch(
_reset_state_base_q,
dim=(self.num_worlds, self.num_bodies_max),
inputs=[
self.base_joint_id,
base_q,
self.joints_X_j,
self.joints_B_r_Bj,
self.model.info.num_bodies,
self.first_body_id,
self.model.bodies.q_i_0,
world_mask,
bodies_q,
],
device=self.device,
)
def _eval_position_control_transformations(
self,
base_q: wp.array(dtype=wp.transformf),
actuators_q: wp.array(dtype=wp.float32),
pos_control_transforms: wp.array(dtype=wp.transformf),
):
"""
Internal evaluator for position control transformations, from actuated & base coordinates of the main model.
"""
# Compute actuators_q of fk model with modified joints
wp.launch(
_eval_fk_actuated_dofs_or_coords,
dim=(self.num_actuated_coords,),
inputs=[
wp.array(
ptr=base_q.ptr, dtype=wp.float32, shape=(7 * self.num_worlds,), device=self.device, copy=False
),
actuators_q,
self.actuated_coords_map,
self.actuators_q,
],
device=self.device,
)
# Compute position control transformations
wp.launch(
_eval_position_control_transformations,
dim=(self.num_joints_tot,),
inputs=[
self.joints_dof_type,
self.joints_act_type,
self.actuated_coord_offsets,
self.joints_X_j,
self.actuators_q,
pos_control_transforms,
],
device=self.device,
)
def _eval_kinematic_constraints(
self,
bodies_q: wp.array(dtype=wp.transformf),
pos_control_transforms: wp.array(dtype=wp.transformf),
world_mask: wp.array(dtype=wp.int32),
constraints: wp.array2d(dtype=wp.float32),
):
"""
Internal evaluator for the kinematic constraints vector, from body poses and position-control transformations
"""
# Evaluate unit norm quaternion constraints
wp.launch(
_eval_unit_quaternion_constraints,
dim=(self.num_worlds, self.num_bodies_max),
inputs=[self.model.info.num_bodies, self.first_body_id, bodies_q, world_mask, constraints],
device=self.device,
)
# Evaluate joint constraints
wp.launch(
self._eval_joint_constraints_kernel,
dim=(self.num_worlds, self.num_joints_max),
inputs=[
self.num_joints,
self.first_joint_id,
self.joints_dof_type,
self.joints_act_type,
self.joints_bid_B,
self.joints_bid_F,
self.joints_X_j,
self.joints_B_r_Bj,
self.joints_F_r_Fj,
bodies_q,
pos_control_transforms,
self.constraint_full_to_red_map,
world_mask,
constraints,
],
device=self.device,
)
def _eval_max_constraint(
self, constraints: wp.array2d(dtype=wp.float32), max_constraint: wp.array(dtype=wp.float32)
):
"""
Internal evaluator for the maximal absolute constraint, from the constraints vector, in each world
"""
max_constraint.zero_()
wp.launch_tiled(
self._eval_max_constraint_kernel,
dim=(self.num_worlds, self.num_tiles_constraints),
inputs=[constraints, max_constraint],
block_dim=64,
device=self.device,
)
def _eval_kinematic_constraints_jacobian(
self,
bodies_q: wp.array(dtype=wp.transformf),
pos_control_transforms: wp.array(dtype=wp.transformf),
world_mask: wp.array(dtype=wp.int32),
constraints_jacobian: wp.array3d(dtype=wp.float32),
):
"""
Internal evaluator for the kinematic constraints Jacobian with respect to body poses, from body poses
and position-control transformations
"""
# Evaluate unit norm quaternion constraints Jacobian
wp.launch(
_eval_unit_quaternion_constraints_jacobian,
dim=(self.num_worlds, self.num_bodies_max),
inputs=[self.model.info.num_bodies, self.first_body_id, bodies_q, world_mask, constraints_jacobian],
device=self.device,
)
# Evaluate joint constraints Jacobian
wp.launch(
self._eval_joint_constraints_jacobian_kernel,
dim=(self.num_worlds, self.num_joints_max),
inputs=[
self.num_joints,
self.first_joint_id,
self.first_body_id,
self.joints_dof_type,
self.joints_act_type,
self.joints_bid_B,
self.joints_bid_F,
self.joints_X_j,
self.joints_B_r_Bj,
self.joints_F_r_Fj,
bodies_q,
pos_control_transforms,
self.constraint_full_to_red_map,
world_mask,
constraints_jacobian,
],
device=self.device,
)
def _assemble_sparse_jacobian(
self,
bodies_q: wp.array(dtype=wp.transformf),
pos_control_transforms: wp.array(dtype=wp.transformf),
world_mask: wp.array(dtype=wp.int32),
):
"""
Internal evaluator for the sparse kinematic constraints Jacobian with respect to body poses, from body poses
and position-control transformations
"""
self.sparse_jacobian.zero()
# Evaluate unit norm quaternion constraints Jacobian
wp.launch(
_eval_unit_quaternion_constraints_sparse_jacobian,
dim=(self.num_worlds, self.num_bodies_max),
inputs=[
self.model.info.num_bodies,
self.first_body_id,
bodies_q,
self.rb_nzb_id,
world_mask,
self.sparse_jacobian.nzb_values,
],
device=self.device,
)
# Evaluate joint constraints Jacobian
wp.launch(
self._eval_joint_constraints_sparse_jacobian_kernel,
dim=(self.num_worlds, self.num_joints_max),
inputs=[
self.num_joints,
self.first_joint_id,
self.first_body_id,
self.joints_dof_type,
self.joints_act_type,
self.joints_bid_B,
self.joints_bid_F,
self.joints_X_j,
self.joints_B_r_Bj,
self.joints_F_r_Fj,
bodies_q,
pos_control_transforms,
self.ct_nzb_id_base,
self.ct_nzb_id_follower,
world_mask,
self.sparse_jacobian.nzb_values,
],
device=self.device,
)
def _eval_lhs_gemv(
self,
x: wp.array2d(dtype=wp.float32),
y: wp.array2d(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
alpha: wp.float32,
beta: wp.float32,
):
"""
Internal evaluator for y = alpha * J^T * J * x + beta * y, using the assembled sparse Jacobian J
"""
self.sparse_jacobian_op.matvec(x, self.jacobian_times_vector, world_mask)
self.sparse_jacobian_op.matvec_transpose(self.jacobian_times_vector, self.lhs_times_vector, world_mask)
wp.launch(
_eval_linear_combination,
dim=(self.num_worlds, self.num_states_max),
inputs=[alpha, self.lhs_times_vector, beta, y, self.num_constraints, world_mask, y],
device=self.device,
)
def _eval_merit_function(self, constraints: wp.array2d(dtype=wp.float32), error: wp.array(dtype=wp.float32)):
"""
Internal evaluator for the line search merit function, i.e. the least-squares error 1/2 * ||C||^2,
from the constraints vector C, in each world
"""
error.zero_()
wp.launch_tiled(
self._eval_merit_function_kernel,
dim=(self.num_worlds, self.num_tiles_constraints),
inputs=[constraints, error],
block_dim=64,
device=self.device,
)
def _eval_merit_function_gradient(
self,
step: wp.array2d(dtype=wp.float32),
grad: wp.array2d(dtype=wp.float32),
error_grad: wp.array(dtype=wp.float32),
):
"""
Internal evaluator for the merit function gradient w.r.t. line search step size, from the step direction
and the gradient in state space (= dC_ds^T * C). This is simply the dot product between these two vectors.
"""
error_grad.zero_()
wp.launch_tiled(
self._eval_merit_function_gradient_kernel,
dim=(self.num_worlds, self.num_tiles_states),
inputs=[step, grad, error_grad],
block_dim=64,
device=self.device,
)
def _run_line_search_iteration(self, bodies_q: wp.array(dtype=wp.transformf)):
"""
Internal function running one iteration of line search, checking the Armijo sufficient descent condition
"""
# Eval stepped state
wp.launch(
_eval_stepped_state,
dim=(self.num_worlds, self.num_states_max),
inputs=[
self.model.info.num_bodies,
self.first_body_id,
wp.array(
ptr=bodies_q.ptr, dtype=wp.float32, shape=(self.num_states_tot,), device=self.device, copy=False
),
self.alpha,
self.step,
self.line_search_mask,
wp.array(
ptr=self.bodies_q_alpha.ptr,
dtype=wp.float32,
shape=(self.num_states_tot,),
device=self.device,
copy=False,
),
],
device=self.device,
)
# Evaluate new constraints and merit function (least squares norm of constraints)
self._eval_kinematic_constraints(
self.bodies_q_alpha, self.pos_control_transforms, self.line_search_mask, self.constraints
)
self._eval_merit_function(self.constraints, self.val_alpha)
# Check decrease and update step
self.line_search_loop_condition.zero_()
wp.launch(
_line_search_check,
dim=(self.num_worlds,),
inputs=[
self.val_0,
self.grad_0,
self.alpha,
self.val_alpha,
self.line_search_iteration,
self.max_line_search_iterations,
self.line_search_success,
self.line_search_mask,
self.line_search_loop_condition,
],
device=self.device,
)
def _update_cg_tolerance(
self,
residual_norm: wp.array(dtype=wp.float32),
world_mask: wp.array(dtype=wp.int32),
):
"""
Internal function heuristically adapting the CG tolerance based on the current constraint residual
(starting with a loose tolerance, and tightening it as we converge)
Note: needs to be refined, until then we are still using a fixed tolerance
"""
wp.launch(
_update_cg_tolerance_kernel,
dim=(self.num_worlds,),
inputs=[residual_norm, world_mask, self.cg_atol, self.cg_rtol],
device=self.device,
)
def _run_newton_iteration(self, bodies_q: wp.array(dtype=wp.transformf)):
"""
Internal function running one iteration of Gauss-Newton. Assumes the constraints vector to be already
up-to-date (because we will already have checked convergence before the first loop iteration)
"""
# Evaluate constraints Jacobian
if self.config.use_sparsity:
self._assemble_sparse_jacobian(bodies_q, self.pos_control_transforms, self.newton_mask)
else:
self._eval_kinematic_constraints_jacobian(
bodies_q, self.pos_control_transforms, self.newton_mask, self.jacobian
)
# Evaluate Gauss-Newton left-hand side (J^T * J) if needed, and right-hand side (-J^T * C)
if self.config.use_sparsity:
self.sparse_jacobian_op.matvec_transpose(self.constraints, self.grad, self.newton_mask)
else:
wp.launch_tiled(
self._eval_jacobian_T_jacobian_kernel,
dim=(self.num_worlds, self.num_tiles_states, self.num_tiles_states),
inputs=[self.jacobian, self.newton_mask, self.lhs],
block_dim=64,
device=self.device,
)
wp.launch_tiled(
self._eval_jacobian_T_constraints_kernel,
dim=(self.num_worlds, self.num_tiles_states),
inputs=[self.jacobian, self.constraints, self.newton_mask, self.grad],
block_dim=64,
device=self.device,
)
wp.launch(
_eval_rhs,
dim=(self.num_worlds, self.num_states_max),
inputs=[self.grad, self.rhs],
device=self.device,
)
# Compute step (system solve)
if self.config.use_sparsity:
if self._preconditioner_type == ForwardKinematicsSolver.PreconditionerType.JACOBI_DIAGONAL:
block_sparse_ATA_inv_diagonal_2d(self.sparse_jacobian, self.jacobian_diag_inv, self.newton_mask)
elif self._preconditioner_type == ForwardKinematicsSolver.PreconditionerType.JACOBI_BLOCK_DIAGONAL:
block_sparse_ATA_blockwise_3_4_inv_diagonal_2d(
self.sparse_jacobian, self.inv_blocks_3, self.inv_blocks_4, self.newton_mask
)
self.step.zero_()
if self.config.use_adaptive_cg_tolerance:
self._update_cg_tolerance(self.max_constraint, self.newton_mask)
else:
self.cg_atol.fill_(1e-8)
self.cg_rtol.fill_(1e-8)
self.linear_solver_cg.solve(self.rhs, self.step, world_active=self.newton_mask)
else:
self.linear_solver_llt.factorize(self.lhs, self.num_states, self.newton_mask)
self.linear_solver_llt.solve(
self.rhs.reshape((self.num_worlds, self.num_states_max, 1)),
self.step.reshape((self.num_worlds, self.num_states_max, 1)),
self.newton_mask,
)
# Line search
self.line_search_iteration.zero_()
self.line_search_success.zero_()
wp.copy(self.line_search_mask, self.newton_mask)
self.line_search_loop_condition.fill_(1)
self._eval_merit_function(self.constraints, self.val_0)
self._eval_merit_function_gradient(self.step, self.grad, self.grad_0)
self.alpha.fill_(1.0)
wp.capture_while(self.line_search_loop_condition, lambda: self._run_line_search_iteration(bodies_q))
# Apply line search step and update max constraint
wp.launch(
_apply_line_search_step,
dim=(self.num_worlds, self.num_bodies_max),
inputs=[
self.model.info.num_bodies,
self.first_body_id,
self.bodies_q_alpha,
self.line_search_success,
bodies_q,
],
device=self.device,
)
self._eval_max_constraint(self.constraints, self.max_constraint)
# Check convergence
self.newton_loop_condition.zero_()
wp.launch(
_newton_check,
dim=(self.num_worlds,),
inputs=[
self.max_constraint,
self.tolerance,
self.newton_iteration,
self.max_newton_iterations,
self.line_search_success,
self.newton_success,
self.newton_mask,
self.newton_loop_condition,
],
device=self.device,
)
def _solve_for_body_velocities(
self,
pos_control_transforms: wp.array(dtype=wp.transformf),
base_u: wp.array(dtype=vec6f),
actuators_u: wp.array(dtype=wp.float32),
bodies_q: wp.array(dtype=wp.transformf),
bodies_u: wp.array(dtype=vec6f),
world_mask: wp.array(dtype=wp.int32),
):
"""
Internal function solving for body velocities, so that constraint velocities are zero,
except at actuated dofs and at the base joint, where they must match prescribed velocities.
"""
# Compute actuators_u of fk model with modified joints
wp.launch(
_eval_fk_actuated_dofs_or_coords,
dim=(self.num_actuated_dofs,),
inputs=[
wp.array(
ptr=base_u.ptr, dtype=wp.float32, shape=(6 * self.num_worlds,), device=self.device, copy=False
),
actuators_u,
self.actuated_dofs_map,
self.actuators_u,
],
device=self.device,
)
# Compute target constraint velocities (prescribed for actuated dofs, zero for passive constraints)
self.target_cts_u.zero_()
wp.launch(
_eval_target_constraint_velocities,
dim=(
self.num_worlds,
self.num_joints_max,
),
inputs=[
self.num_joints,
self.first_joint_id,
self.joints_dof_type,
self.joints_act_type,
self.actuated_dof_offsets,
self.constraint_full_to_red_map,
self.actuators_u,
world_mask,
self.target_cts_u,
],
device=self.device,
)
# Update constraints Jacobian
if self.config.use_sparsity:
self._assemble_sparse_jacobian(bodies_q, pos_control_transforms, world_mask)
else:
self._eval_kinematic_constraints_jacobian(bodies_q, pos_control_transforms, world_mask, self.jacobian)
# Evaluate system left-hand side (J^T * J) if needed, and right-hand side (J^T * targets_cts_u)
if self.config.use_sparsity:
self.sparse_jacobian_op.matvec_transpose(self.target_cts_u, self.rhs, world_mask)
else:
wp.launch_tiled(
self._eval_jacobian_T_jacobian_kernel,
dim=(self.num_worlds, self.num_tiles_states, self.num_tiles_states),
inputs=[self.jacobian, world_mask, self.lhs],
block_dim=64,
device=self.device,
)
wp.launch_tiled(
self._eval_jacobian_T_constraints_kernel,
dim=(self.num_worlds, self.num_tiles_states),
inputs=[self.jacobian, self.target_cts_u, world_mask, self.rhs],
block_dim=64,
device=self.device,
)
# Compute body velocities (system solve)
if self.config.use_sparsity:
if self._preconditioner_type == ForwardKinematicsSolver.PreconditionerType.JACOBI_DIAGONAL:
block_sparse_ATA_inv_diagonal_2d(self.sparse_jacobian, self.jacobian_diag_inv, world_mask)
elif self._preconditioner_type == ForwardKinematicsSolver.PreconditionerType.JACOBI_BLOCK_DIAGONAL:
block_sparse_ATA_blockwise_3_4_inv_diagonal_2d(
self.sparse_jacobian, self.inv_blocks_3, self.inv_blocks_4, world_mask
)
self.bodies_q_dot.zero_()
self.cg_atol.fill_(1e-8)
self.cg_rtol.fill_(1e-8)
self.linear_solver_cg.solve(self.rhs, self.bodies_q_dot, world_active=world_mask)
else:
self.linear_solver_llt.factorize(self.lhs, self.num_states, world_mask)
self.linear_solver_llt.solve(
self.rhs.reshape((self.num_worlds, self.num_states_max, 1)),
self.bodies_q_dot.reshape((self.num_worlds, self.num_states_max, 1)),
world_mask,
)
wp.launch(
_eval_body_velocities,
dim=(self.num_worlds, self.num_bodies_max),
inputs=[self.model.info.num_bodies, self.first_body_id, bodies_q, self.bodies_q_dot, world_mask, bodies_u],
device=self.device,
)
###
# Exposed functions (overall solve_fk() function + constraints (Jacobian) evaluators for debugging)
###
def eval_position_control_transformations(
self, actuators_q: wp.array(dtype=wp.float32), base_q: wp.array(dtype=wp.transformf) | None = None
):
"""
Evaluates and returns position control transformations (an intermediary quantity needed for the
kinematic constraints/Jacobian evaluation) for a model given actuated coordinates, and optionally
the base pose (the default base pose is used if not provided).
"""
assert base_q is None or base_q.device == self.device
assert actuators_q.device == self.device
if base_q is None:
base_q = self.base_q_default
pos_control_transforms = wp.array(dtype=wp.transformf, shape=(self.num_joints_tot,), device=self.device)
self._eval_position_control_transformations(base_q, actuators_q, pos_control_transforms)
return pos_control_transforms
def eval_kinematic_constraints(
self, bodies_q: wp.array(dtype=wp.transformf), pos_control_transforms: wp.array(dtype=wp.transformf)
):
"""
Evaluates and returns the kinematic constraints vector given the body poses and the position
control transformations.
"""
assert bodies_q.device == self.device
assert pos_control_transforms.device == self.device
constraints = wp.zeros(
dtype=wp.float32,
shape=(
self.num_worlds,
self.num_constraints_max,
),
device=self.device,
)
world_mask = wp.ones(dtype=wp.int32, shape=(self.num_worlds,), device=self.device)
self._eval_kinematic_constraints(bodies_q, pos_control_transforms, world_mask, constraints)
return constraints
def eval_kinematic_constraints_jacobian(
self, bodies_q: wp.array(dtype=wp.transformf), pos_control_transforms: wp.array(dtype=wp.transformf)
):
"""
Evaluates and returns the kinematic constraints Jacobian (w.r.t. body poses) given the body poses
and the position control transformations.
"""
assert bodies_q.device == self.device
assert pos_control_transforms.device == self.device
constraints_jacobian = wp.zeros(
dtype=wp.float32, shape=(self.num_worlds, self.num_constraints_max, self.num_states_max), device=self.device
)
world_mask = wp.ones(dtype=wp.int32, shape=(self.num_worlds,), device=self.device)
self._eval_kinematic_constraints_jacobian(bodies_q, pos_control_transforms, world_mask, constraints_jacobian)
return constraints_jacobian
def assemble_sparse_jacobian(
self, bodies_q: wp.array(dtype=wp.transformf), pos_control_transforms: wp.array(dtype=wp.transformf)
):
"""
Assembles the sparse Jacobian (under self.sparse_jacobian) given input body poses and control transforms.
Note: only safe to call if this object was finalized with sparsity enabled in the config.
"""
assert bodies_q.device == self.device
assert pos_control_transforms.device == self.device
world_mask = wp.ones(dtype=wp.int32, shape=(self.num_worlds,), device=self.device)
self._assemble_sparse_jacobian(bodies_q, pos_control_transforms, world_mask)
def solve_for_body_velocities(
self,
pos_control_transforms: wp.array(dtype=wp.transformf),
actuators_u: wp.array(dtype=wp.float32),
bodies_q: wp.array(dtype=wp.transformf),
bodies_u: wp.array(dtype=vec6f),
base_u: wp.array(dtype=vec6f) | None = None,
world_mask: wp.array(dtype=wp.int32) | None = None,
):
"""
Graph-capturable function solving for body velocities as a post-processing to the FK solve.
More specifically, solves for body twists yielding zero constraint velocities, except at
actuated dofs and at the base joint, where velocities must match prescribed velocities.
Parameters
----------
pos_control_transforms : wp.array
Array of position-control transforms, encoding actuated coordinates and base pose.
Expects shape of ``(num_fk_joints,)`` and type :class:`transform`
actuators_u : wp.array
Array of actuated joint velocities.
Expects shape of ``(sum_of_num_actuated_joint_dofs,)`` and type :class:`float`.
bodies_q : wp.array
Array of rigid body poses. Must be the solution of FK given the position-control transforms.
Expects shape of ``(num_bodies,)`` and type :class:`transform`.
bodies_u : wp.array
Array of rigid body velocities (twists), written out by the solver.
Expects shape of ``(num_bodies,)`` and type :class:`vec6`.
base_u : wp.array, optional
Velocity (twist) of the base body for each world, in the frame of the base joint if it was set, or
absolute otherwise.
If not provided, will default to zero. Ignored if no base body or joint was set for this model.
If this function is captured in a graph, must be either always or never provided.
Expects shape of ``(num_worlds,)`` and type :class:`vec6`.
world_mask : wp.array, optional
Array of per-world flags that indicate which worlds should be processed (0 = leave that world unchanged).
If not provided, all worlds will be processed.
If this function is captured in a graph, must be either always or never provided.
"""
assert pos_control_transforms.device == self.device
assert actuators_u.device == self.device
assert bodies_q.device == self.device
assert bodies_u.device == self.device
assert base_u is None or base_u.device == self.device
assert world_mask is None or world_mask.device == self.device
# Use default base velocity if not provided
if base_u is None:
base_u = self.base_u_default
# Compute velocities
self._solve_for_body_velocities(pos_control_transforms, base_u, actuators_u, bodies_q, bodies_u, world_mask)
def run_fk_solve(
self,
actuators_q: wp.array(dtype=wp.float32),
bodies_q: wp.array(dtype=wp.transformf),
base_q: wp.array(dtype=wp.transformf) | None = None,
actuators_u: wp.array(dtype=wp.float32) | None = None,
base_u: wp.array(dtype=vec6f) | None = None,
bodies_u: wp.array(dtype=vec6f) | None = None,
world_mask: wp.array(dtype=wp.int32) | None = None,
):
"""
Graph-capturable function solving forward kinematics with Gauss-Newton.
More specifically, solves for the rigid body poses satisfying
kinematic constraints, given actuated joint coordinates and
base pose. Optionally also solves for rigid body velocities
given actuator and base body velocities.
Parameters
----------
actuators_q : wp.array
Array of actuated joint coordinates.
Expects shape of ``(sum_of_num_actuated_joint_coords,)`` and type :class:`float`.
bodies_q : wp.array
Array of rigid body poses, written out by the solver and read in as initial guess if the reset_state
solver setting is False.
Expects shape of ``(num_bodies,)`` and type :class:`transform`.
base_q : wp.array, optional
Pose of the base body for each world, in the frame of the base joint if it was set, or absolute otherwise.
If not provided, will default to zero coordinates of the base joint, or the initial pose of the base body.
If no base body or joint was set for this model, will be ignored.
If this function is captured in a graph, must be either always or never provided.
Expects shape of ``(num_worlds,)`` and type :class:`transform`.
actuators_u : wp.array, optional
Array of actuated joint velocities.
Must be provided when solving for body velocities, i.e. if bodies_u is provided.
If this function is captured in a graph, must be either always or never provided.
Expects shape of ``(sum_of_num_actuated_joint_dofs,)`` and type :class:`float`.
base_u : wp.array, optional
Velocity (twist) of the base body for each world, in the frame of the base joint if it was set, or
absolute otherwise.
If not provided, will default to zero. Ignored if no base body or joint was set for this model.
If this function is captured in a graph, must be either always or never provided.
Expects shape of ``(num_worlds,)`` and type :class:`vec6`.
bodies_u : wp.array, optional
Array of rigid body velocities (twists), written out by the solver if provided.
If this function is captured in a graph, must be either always or never provided.
Expects shape of ``(num_bodies,)`` and type :class:`vec6`.
world_mask : wp.array, optional
Array of per-world flags that indicate which worlds should be processed (0 = leave that world unchanged).
If not provided, all worlds will be processed.
If this function is captured in a graph, must be either always or never provided.
"""
# Check that actuators_u are provided if we need to solve for bodies_u
if bodies_u is not None and actuators_u is None:
raise ValueError(
"run_fk_solve: actuators_u must be provided to solve for velocities (i.e. if bodies_u is provided)."
)
# Use default base state if not provided
if base_q is None:
base_q = self.base_q_default
if bodies_u is not None and base_u is None:
base_u = self.base_u_default
# Compute position control transforms (independent of body poses, depends on controls only)
self._eval_position_control_transformations(base_q, actuators_q, self.pos_control_transforms)
# Reset iteration count and success/continuation flags
self.newton_iteration.fill_(-1) # The initial loop condition check will increment this to zero
self.newton_success.zero_()
if world_mask is not None:
self.newton_mask.assign(world_mask)
else:
self.newton_mask.fill_(1)
# Optionally reset state
if self.config.reset_state:
if base_q is None:
self._reset_state(bodies_q, self.newton_mask)
else:
self._reset_state_base_q(bodies_q, base_q, self.newton_mask)
# Evaluate constraints, and initialize loop condition (might not even need to loop)
self._eval_kinematic_constraints(bodies_q, self.pos_control_transforms, self.newton_mask, self.constraints)
self._eval_max_constraint(self.constraints, self.max_constraint)
self.newton_loop_condition.zero_()
wp.copy(self.line_search_success, self.newton_mask) # Newton check will abort in case of line search failure
wp.launch(
_newton_check,
dim=(self.num_worlds,),
inputs=[
self.max_constraint,
self.tolerance,
self.newton_iteration,
self.max_newton_iterations,
self.line_search_success,
self.newton_success,
self.newton_mask,
self.newton_loop_condition,
],
device=self.device,
)
# Main loop
wp.capture_while(self.newton_loop_condition, lambda: self._run_newton_iteration(bodies_q))
# Velocity solve, for worlds where FK ran and was successful
if bodies_u is not None:
self._solve_for_body_velocities(
self.pos_control_transforms, base_u, actuators_u, bodies_q, bodies_u, self.newton_success
)
def solve_fk(
self,
actuators_q: wp.array(dtype=wp.float32),
bodies_q: wp.array(dtype=wp.transformf),
base_q: wp.array(dtype=wp.transformf) | None = None,
actuators_u: wp.array(dtype=wp.float32) | None = None,
base_u: wp.array(dtype=vec6f) | None = None,
bodies_u: wp.array(dtype=vec6f) | None = None,
world_mask: wp.array(dtype=wp.int32) | None = None,
verbose: bool = False,
return_status: bool = False,
use_graph: bool = True,
):
"""
Convenience function with verbosity options (non graph-capturable), solving
forward kinematics with Gauss-Newton. More specifically, it solves for the
rigid body poses satisfying kinematic constraints, given actuated joint
coordinates and base pose. Optionally also solves for rigid body velocities
given actuator and base body velocities.
Parameters
----------
actuators_q : wp.array
Array of actuated joint coordinates.
Expects shape of ``(sum_of_num_actuated_joint_coords,)`` and type :class:`float`.
bodies_q : wp.array
Array of rigid body poses, written out by the solver and read in as initial guess if the reset_state
solver setting is False.
Expects shape of ``(num_bodies,)`` and type :class:`transform`.
base_q : wp.array, optional
Pose of the base body for each world, in the frame of the base joint if it was set, or absolute otherwise.
If not provided, will default to zero coordinates of the base joint, or the initial pose of the base body.
If no base body or joint was set for this model, will be ignored.
Expects shape of ``(num_worlds,)`` and type :class:`transform`.
actuators_u : wp.array, optional
Array of actuated joint velocities.
Must be provided when solving for body velocities, i.e. if bodies_u is provided.
Expects shape of ``(sum_of_num_actuated_joint_dofs,)`` and type :class:`float`.
base_u : wp.array, optional
Velocity (twist) of the base body for each world, in the frame of the base joint if it was set, or
absolute otherwise.
If not provided, will default to zero. Ignored if no base body or joint was set for this model.
Expects shape of ``(num_worlds,)`` and type :class:`vec6`.
bodies_u : wp.array, optional
Array of rigid body velocities (twists), written out by the solver if provided.
Expects shape of ``(num_bodies,)`` and type :class:`vec6`.
world_mask : wp.array, optional
Array of per-world flags that indicate which worlds should be processed (0 = leave that world unchanged).
If not provided, all worlds will be processed.
verbose : bool, optional
whether to write a status message at the end (default: False)
return_status : bool, optional
whether to return the detailed solver status (default: False)
use_graph : bool, optional
whether to use graph capture internally to accelerate multiple calls to this function. Can be turned
off for profiling individual kernels (default: True)
Returns
-------
solver_status : ForwardKinematicsSolverStatus, optional
the detailed solver status with success flag, number of iterations and constraint residual per world
"""
assert base_q is None or base_q.device == self.device
assert actuators_q.device == self.device
assert bodies_q.device == self.device
assert base_u is None or base_u.device == self.device
assert actuators_u is None or actuators_u.device == self.device
assert bodies_u is None or bodies_u.device == self.device
# Run solve (with or without graph)
if use_graph:
if self.graph is None:
wp.capture_begin(self.device)
self.run_fk_solve(actuators_q, bodies_q, base_q, actuators_u, base_u, bodies_u, world_mask)
self.graph = wp.capture_end()
wp.capture_launch(self.graph)
else:
self.run_fk_solve(actuators_q, bodies_q, base_q, actuators_u, base_u, bodies_u, world_mask)
# Status message
if verbose or return_status:
success = self.newton_success.numpy().copy()
iterations = self.newton_iteration.numpy().copy()
max_constraints = self.max_constraint.numpy().copy()
num_active_worlds = self.num_worlds if world_mask is None else world_mask.numpy().sum()
if verbose:
sys.__stdout__.write(f"Newton success for {success.sum()}/{num_active_worlds} worlds; ")
sys.__stdout__.write(f"num iterations={iterations.max()}; ")
sys.__stdout__.write(f"max constraint={max_constraints.max()}\n")
# Return solver status
if return_status:
return ForwardKinematicsSolver.Status(
iterations=iterations, max_constraints=max_constraints, success=success
)
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/metrics.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides a utilities to compute performance metrics that
assess physical plausibility and accuracy of simulations.
The :class:`SolutionMetricsData` class defines the set of simulation solver performance metrics that
provide quantitative measures of constraint satisfaction and physical plausibility of the computed
solution to the dual forward-dynamics Nonlinear Complementarity Problem (NCP).
The :class:`SolutionMetrics` class provides a high-level interface to manage all relevant data
allocations as well as provide the operations to compute the various performance metrics.
Usage
----
A typical example for using this module is:
# Import all relevant types from Kamino
from newton._src.solvers.kamino.core import ModelBuilderKamino
from newton._src.solvers.kamino._src.geometry import ContactsKamino
from newton._src.solvers.kamino._src.kinematics import LimitsKamino
from newton._src.solvers.kamino._src.kinematics import DenseSystemJacobians
from newton._src.solvers.kamino._src.dynamics import DualProblem
from newton._src.solvers.kamino.solvers import PADMMSolver
# Create a model builder and add bodies, joints, geoms, etc.
builder = ModelBuilderKamino()
...
# Create a model from the builder and construct additional
# containers to hold joint-limits, contacts, Jacobians
model = builder.finalize()
state_p = model.state()
data = model.data()
limits = LimitsKamino(model)
contacts = ContactsKamino(builder)
jacobians = DenseSystemJacobians(model, limits, contacts)
# Build the Jacobians for the model and active limits and contacts
jacobians.build(model, data, limits, contacts)
...
# Create a forward-dynamics DualProblem to be solved
dual = DualProblem(model, limits, contacts)
dual.build(model, data, limits, contacts, jacobians)
# Create a forward-dynamics PADMM solver
solver = PADMMSolver(model, limits, contacts)
# Solve the dual forward dynamics problem
solver.coldstart()
solver.solve(problem=dual)
# Create a SolutionMetrics container
metrics = SolutionMetrics(model)
# Compute the solution metrics after solving the dual problem
metrics.reset()
metrics.evaluate(
solver.state.sigma,
solver.solution.lambdas,
solver.solution.v_plus,
model,
data,
state_p,
dual,
jacobians,
limits,
contacts,
)
"""
from dataclasses import dataclass
import warp as wp
from ..core.data import DataKamino
from ..core.math import screw, screw_angular, screw_linear
from ..core.model import ModelKamino
from ..core.state import StateKamino
from ..core.types import float32, int32, int64, mat33f, uint32, vec2f, vec3f, vec4f, vec6f
from ..dynamics.dual import DualProblem
from ..geometry.contacts import ContactsKamino
from ..geometry.keying import build_pair_key2
from ..kinematics.jacobians import DenseSystemJacobians, SparseSystemJacobians
from ..kinematics.limits import LimitsKamino
from ..solvers.padmm.math import (
compute_desaxce_corrections,
compute_dot_product,
compute_double_dot_product,
compute_ncp_complementarity_residual,
compute_ncp_dual_residual,
compute_ncp_natural_map_residual,
compute_ncp_primal_residual,
compute_vector_sum,
)
###
# Module interface
###
__all__ = [
"SolutionMetrics",
"SolutionMetricsData",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
@dataclass
class SolutionMetricsData:
"""
Defines a container to hold performance metrics that assess physical plausibility and accuracy
of the computed solution to the dual forward-dynamics Nonlinear Complementarity Problem (NCP).
Attributes:
r_eom (wp.array):
The largest residual across all DoF dimensions of the Equations-of-Motion (EoM).
r_kinematics (wp.array):
The largest residual across all kinematic bilateral (i.e. equality) constraints.
r_cts_joints (wp.array):
The largest constraint violation residual across all bilateral kinematic joint constraints.
r_cts_limits (wp.array):
The largest constraint violation residual across all unilateral joint-limit constraints.
r_cts_contacts (wp.array):
The largest constraint violation residual across all contact constraints.
r_ncp_primal (wp.array):
The NCP primal residual representing the violation of set-valued constraint reactions.
r_ncp_dual (wp.array):
The NCP dual residual representing the violation of set-valued augmented constraint velocities.
r_ncp_compl (wp.array):
The NCP complementarity residual representing the violation of complementarity conditions.
r_vi_natmap (wp.array):
The Variational Inequality (VI) natural-map residual representing the proximity
of the constraint reactions to a true solution of the VI defined by the NCP.
"""
r_eom: wp.array | None = None
"""
The largest residual across all DoF dimensions of the Equations-of-Motion (EoM).
Measures how well the computed post-event generalized velocity `u^+` and constraint reactions
`lambdas` (i.e. Lagrange multipliers) satisfy the impulse-velocity form of the equations-of-motion.
Computed as the maximum absolute value (i.e. infinity-norm) over the residual of:
`r_eom := || M @ (u^+ - u^-) - dt * (h + J_dofs.T @ tau) - J_cts.T @ lambdas||_inf`,
where:
- `M` is the generalized mass matrix
- `u^+` and `u^-` are the post- and pre-event generalized velocities
- `dt` is the time-step
- `h` is the generalized impulse vector
- `J_cts` is the constraint Jacobian matrix
- `lambdas` is the vector of all constraint reactions (i.e. Lagrange multipliers)
- `J_dofs` is the actuation Jacobian matrix
- `tau` is the vector of generalized actuation forces
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_eom_argmax: wp.array | None = None
"""
The index pair key computed from the body index and
degree-of-freedom (DoF) with the largest EoM residual.
Shape of ``(num_worlds,)`` and type :class:`int64`.
"""
r_kinematics: wp.array | None = None
"""
The largest residual across all kinematic bilateral (i.e. equality) constraints.
Measures how well the computed post-event generalized velocity
`u^+` satisfies the velocity-level kinematic equality constraints.
Computed as the maximum absolute value (i.e. infinity-norm) over velocity-level kinematics constraints:
`r_kinematics := || J_cts_joints @ u^+ ||_inf`,
where:
- `J_cts_joints` is the constraint Jacobian matrix for bilateral joint constraints
- `u^+` is the post-event generalized velocity
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_kinematics_argmax: wp.array | None = None
"""
The index pair key computed from the joint index and
bilateral kinematic constraint with the largest residual.
Shape of ``(num_worlds,)`` and type :class:`int64`.
"""
r_cts_joints: wp.array | None = None
"""
The largest constraint violation residual across all bilateral joint constraints.
Computed as the maximum absolute value (i.e. infinity-norm) over joint constraint residuals.
Equivalent to `r_cts_joints := || r_j ||_inf`, where `r_j` is the
array of joint constraint residuals defined in :class:`JointsData`.
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_cts_joints_argmax: wp.array | None = None
"""
The index pair key computed from the joint index and bilateral
kinematic joint constraint with the largest residual.
Shape of ``(num_worlds,)`` and type :class:`int64`.
"""
r_cts_limits: wp.array | None = None
"""
The largest constraint violation residual across all unilateral joint-limit constraints.
Computed as the maximum absolute value (i.e. infinity-norm) over joint-limit constraint residuals.
Equivalent to `r_cts_limits := || r_l ||_inf`, where `r_l` would be an array of joint-limit
constraint residuals constructed from the collection of `r_q` elements defined in :class:`LimitsKaminoData`.
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_cts_limits_argmax: wp.array | None = None
"""
The index pair key computed from the joint index and degree-of-freedom
(DoF) with the largest unilateral joint-limit constraint residual.
Shape of ``(num_worlds,)`` and type :class:`int64`.
"""
r_cts_contacts: wp.array | None = None
"""
The largest constraint violation residual across all contact constraints.
Computed as the maximum absolute value (i.e. infinity-norm) over contact constraint residuals.
Equivalent to `r_cts_contacts := || d_k ||_inf`, where `d_k` would be an array of
contact penetrations extracted from the `gapfunc` elements of :class:`ContactsKaminoData`.
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_cts_contacts_argmax: wp.array | None = None
"""
The indexir of the contact with the largest unilateral contact constraint residual.
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
r_v_plus: wp.array | None = None
"""
The largest error in the estimation of the post-event constraint-space velocity.
Measures how well the computed post-event constraint-space velocity `v^+` was estimated.
Computed as the maximum absolute value (i.e. infinity-norm) over velocity-level kinematics constraints:
`r_v_plus := || v_plus_est - v_plus_true ||_inf`,
where:
- `v_plus_est` is the estimated post-event constraint-space velocity
- v_plus_true` is the true post-event constraint-space velocity computed as
`v_plus_true = v_f + D @ lambdas`, where `v_f` is the unconstrained constraint-space velocity,
`D` is the Delassus operator, and `lambdas` is the vector of all constraint reactions (i.e. Lagrange multipliers).
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_v_plus_argmax: wp.array | None = None
"""
The index of the constraint with the largest post-event constraint-space velocity estimation error.
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
r_ncp_primal: wp.array | None = None
"""
The NCP primal residual representing the violation of set-valued constraint reactions.
Measures the feasibility of constraint reactions w.r.t the feasible-set cone `K`
defined as the Cartesian product over all positive-orthants for joint-limits and
Coulomb friction cones for contacts:
`K = R^{n_l}_{+} x Π_{k=1}^{n_c} K_{mu_k}`,
Computed as the maximum absolute value (i.e. infinity-norm) over the residual:
`r_ncp_primal(lambda) = || lambda - P_K(lambda) ||_inf`, where `P_K()` is the
Euclidean projection, i.e. proximal operator, onto K, and `lambda` is the
vector of all constraint reactions (i.e. Lagrange multipliers).
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_ncp_primal_argmax: wp.array | None = None
"""
The index of the constraint with the largest NCP primal residual.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
r_ncp_dual: wp.array | None = None
"""
The NCP dual residual representing the violation of set-valued augmented constraint velocities.
Measures the feasibility of augmented constraint-space velocities w.r.t
the dual cone `K*`, the Lagrange dual of the feasible-set cone `K`.
Computed as the maximum absolute value (i.e. infinity-norm) over the residual:
`r_ncp_dual(v_hat^+) = || v_hat^+ - P_K*(v_hat^+) ||_inf`, where `P_K*()` is
the Euclidean projection, i.e. proximal operator, onto K*, and `v_hat^+` is
the so-called augmented constraint-space velocity. The latter is defined as
`v_hat^+ = v^+ + Gamma(v^+)`, where `v^+ := v_f D @ lambda` is the post-event
constraint-space velocity, and `Gamma(v^+)` is the De Saxce correction term.
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_ncp_dual_argmax: wp.array | None = None
"""
The index of the constraint with the largest NCP dual residual.
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
r_ncp_compl: wp.array | None = None
"""
The NCP complementarity residual representing the violation of complementarity conditions.
Measures the complementarity between constraint reactions and the augmented constraint-space
velocities, as defined by the velocity-level Signorini (i.e. complementarity) conditions
and positive orthants for joint-limits and Coulomb friction cones for contacts.
Computed as the maximum absolute value (i.e. infinity-norm) over the residual:
`r_ncp_compl(lambda) = || lambda.T @ v_hat^+ ||_inf`,
where `lambda` is the vector of all constraint reactions (i.e. Lagrange multipliers),
and `v_hat^+` is the augmented constraint-space velocity defined above.
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_ncp_compl_argmax: wp.array | None = None
"""
The index of the constraint with the largest NCP complementarity residual.
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
r_vi_natmap: wp.array | None = None
"""
The Variational Inequality (VI) natural-map residual representing the proximity
of the constraint reactions to a true solution of the VI defined by the NCP.
Measures the how well the given constraint reactions solve the `NCP(D, v_f, K)`,
by simultaneously combining the effects of the primal, dual and complementarity
residuals into a single value, providing a convenient short-hand for both solver
convergence and solution validity.
Computed as the maximum absolute value (i.e. infinity-norm) over the residual:
`r_vi_natmapv_hat(lambda) = || lambda - P_K*(lambda - v_hat^+(lambda)) ||_inf`,
where `P_K*()` is the Euclidean projection, i.e. proximal operator, onto K*,
`lambda` is the vector of all constraint reactions (i.e. Lagrange multipliers),
and `v_hat^+(lambda)` is the augmented constraint-space velocity defined above.
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
r_vi_natmap_argmax: wp.array | None = None
"""
The index of the constraint with the largest VI natural-map residual.
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
f_ncp: wp.array | None = None
"""
Evaluation of the NCP energy dissipation objective function.
Represents only the energy dissipated through friction.
Computed as as:
`f_ncp(lambda) := 0.5 * lambda.T @ D @ lambda + lambda.T @ v_f + lambda.T @ s`,
where `D` is the Delassus operator, `v_f` is the unconstrained constraint-space
velocity and `s := Gamma(v^+)` is the De Saxce correction term.
It is also equivalently computed as:
`f_ncp(lambda) = f_ccp(lambda) + lambda.T @ s`,
where `f_ccp` is the CCP energy dissipation objective.
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
f_ccp: wp.array | None = None
"""
Evaluation of the CCP energy dissipation objective function.
Represents only the energy dissipated through friction.
Computed as as:
`f_ccp(lambda) := 0.5 * lambda.T @ D @ lambda + v_f.T @ lambda`,
where `lambda` is the vector of all constraint reactions (i.e. Lagrange multipliers),
`D` is the Delassus operator and `v_f` is the unconstrained constraint-space velocity.
It is also equivalently computed as:
`f_ccp(lambda) := 0.5 * lambda.T @ (v+ + v_f)`,
where `v+ = v_f + D @ lambda` is the post-event constraint-space velocity.
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
def clear(self):
"""
Clears all metric-argmax indices to -1.
"""
self.r_eom_argmax.fill_(-1)
self.r_kinematics_argmax.fill_(-1)
self.r_cts_joints_argmax.fill_(-1)
self.r_cts_limits_argmax.fill_(-1)
self.r_cts_contacts_argmax.fill_(-1)
self.r_v_plus_argmax.fill_(-1)
self.r_ncp_primal_argmax.fill_(-1)
self.r_ncp_dual_argmax.fill_(-1)
self.r_ncp_compl_argmax.fill_(-1)
self.r_vi_natmap_argmax.fill_(-1)
def zero(self):
"""
Resets all metrics to zeros.
"""
self.r_eom.zero_()
self.r_kinematics.zero_()
self.r_cts_joints.zero_()
self.r_cts_limits.zero_()
self.r_cts_contacts.zero_()
self.r_v_plus.zero_()
self.r_ncp_primal.zero_()
self.r_ncp_dual.zero_()
self.r_ncp_compl.zero_()
self.r_vi_natmap.zero_()
self.f_ncp.zero_()
self.f_ccp.zero_()
def reset(self):
"""
Resets all metrics and argmax indices.
"""
self.clear()
self.zero()
###
# Functions
###
@wp.func
def compute_v_plus(
dim: int32,
vio: int32,
mio: int32,
sigma: float32,
P: wp.array(dtype=float32),
D_p: wp.array(dtype=float32),
v_f_p: wp.array(dtype=float32),
lambdas: wp.array(dtype=float32),
v_plus: wp.array(dtype=float32),
):
"""
Computes the post-event constraint-space velocity as:
`v_plus = P^-1 @ (v_f + (D_p - sigma * I_n) @ (P^-1 @ lambdas))`.
Where `P` is the diagonal preconditioning matrix, `D_p` is the
preconditioned Delassus matrix, `v_f_p` is the preconditioned
unconstrained constraint-space velocity, `lambdas` is the vector
of constraint reactions (i.e. Lagrange multipliers), and `I_n`
is the identity matrix of size `n x n`.
The preconditioned Delassus matrix `D_p` is stored using row-major order in a
flat array with allocation size `maxdim x maxdim`, starting from the matrix
index offset `mio`. The active dimensions of the `D_p` are `dim x dim`, where
`dim` is the number of active rows and columns.
The vectors `v_f_p, lambdas, v_plus` are stored in flat arrays
with dimensions `dim`, starting from the vector index offset `vio`.
Args:
maxdim (int32): The maximum dimension of the matrix `A`.
dim (int32): The active dimension of the matrix `A` and the vectors `x, b, c`.
vio (int32): The vector index offset (i.e. start index) for the vectors `x, b, c`.
mio (int32): The matrix index offset (i.e. start index) for the matrix `A`.
D_p (wp.array(dtype=float32)):
Input preconditioned Delassus matrix stored in row-major order.
v_f_p (wp.array(dtype=float32)):
Input preconditioned unconstrained constraint-space velocity vector.
lambdas (wp.array(dtype=float32)):
Input constraint reactions (i.e. Lagrange multipliers) vector.
v_plus (wp.array(dtype=float32)):
Output array to store the post-event constraint-space velocity vector.
"""
v_f_p_i = float(0.0)
lambdas_j = float(0.0)
for i in range(dim):
v_i = vio + i
m_i = mio + dim * i
v_f_p_i = v_f_p[v_i]
inv_P_i = 1.0 / P[v_i]
lambdas_i = lambdas[v_i]
v_f_p_i -= sigma * inv_P_i * lambdas_i
for j in range(dim):
v_j = vio + j
inv_P_j = 1.0 / P[v_j]
lambdas_j = lambdas[v_j]
v_f_p_i += D_p[m_i + j] * inv_P_j * lambdas_j
v_plus[v_i] = inv_P_i * v_f_p_i
@wp.func
def compute_v_plus_sparse(
dim: int32,
vio: int32,
P: wp.array(dtype=float32),
v_f_p: wp.array(dtype=float32),
D_p_lambdas: wp.array(dtype=float32),
v_plus: wp.array(dtype=float32),
):
"""
Computes the post-event constraint-space velocity as:
`v_plus = P^-1 @ v_f_p + D_p @ lambdas`.
Where `P` is the diagonal preconditioning matrix, `D_p` is the Delassus
matrix (without preconditioning and regularization), `v_f_p` is the
preconditioned unconstrained constraint-space velocity, `lambdas` is the
vector of constraint reactions (i.e. Lagrange multipliers). `D_p @ lambdas`
is passed into the function precomputed as `D_p_lambdas`.
All vectors are stored in flat arrays with dimensions `dim`, starting from
the vector index offset `vio`.
Args:
dim (int32): The active dimension of the matrix `A` and the vectors `x, b, c`.
vio (int32): The vector index offset (i.e. start index) for the vectors `x, b, c`.
v_f_p (wp.array(dtype=float32)):
Input preconditioned unconstrained constraint-space velocity vector.
D_p_lambdas (wp.array(dtype=float32)):
Product of the Delassus matrix with the input constraint reactions
(i.e. Lagrange multipliers) vector.
v_plus (wp.array(dtype=float32)):
Output array to store the post-event constraint-space velocity vector.
"""
for i in range(dim):
v_i = vio + i
v_plus[v_i] = v_f_p[v_i] / P[v_i] + D_p_lambdas[v_i]
@wp.func
def compute_vector_difference_infnorm(
dim: int32,
vio: int32,
x: wp.array(dtype=float32),
y: wp.array(dtype=float32),
) -> tuple[float32, int32]:
"""
Computes the sum of two vectors `x` and `y` and stores the result in vector `z`.\n
All vectors are stored in flat arrays, with dimension `dim` and starting from the vector index offset `vio`.
Args:
dim (int32): The dimension (i.e. size) of the vectors.
vio (int32): The vector index offset (i.e. start index).
x (wp.array(dtype=float32)): The first vector.
y (wp.array(dtype=float32)): The second vector.
z (wp.array(dtype=float32)): The output vector where the sum is stored.
Returns:
None: The result is stored in the output vector `z`.
"""
max = float(0.0)
argmax = int32(-1)
for i in range(dim):
v_i = vio + i
err = wp.abs(x[v_i] - y[v_i])
max = wp.max(max, err)
if err == max:
argmax = i
return max, argmax
###
# Kernels
###
@wp.kernel
def _compute_eom_residual(
# Inputs
model_time_dt: wp.array(dtype=float32),
model_gravity: wp.array(dtype=vec4f),
model_bodies_wid: wp.array(dtype=int32),
model_bodies_m_i: wp.array(dtype=float32),
state_bodies_I_i: wp.array(dtype=mat33f),
state_bodies_w_i: wp.array(dtype=vec6f),
state_bodies_u_i: wp.array(dtype=vec6f),
state_bodies_u_i_p: wp.array(dtype=vec6f),
# Outputs
metric_r_eom: wp.array(dtype=float32),
metric_r_eom_argmax: wp.array(dtype=int64),
):
# Retrieve the thread index as the body index
bid = wp.tid()
# Retrieve the body data
wid = model_bodies_wid[bid]
m_i = model_bodies_m_i[bid]
I_i = state_bodies_I_i[bid]
w_i = state_bodies_w_i[bid]
u_i = state_bodies_u_i[bid]
u_i_p = state_bodies_u_i_p[bid]
# Retrieve the time step
dt = model_time_dt[wid]
gravity = model_gravity[wid]
g = gravity.w * vec3f(gravity.x, gravity.y, gravity.z)
# Decompose into linear and angular parts
f_i = screw_linear(w_i)
v_i = screw_linear(u_i)
v_i_p = screw_linear(u_i_p)
tau_i = screw_angular(w_i)
omega_i = screw_angular(u_i)
omega_i_p = screw_angular(u_i_p)
S_i = wp.skew(omega_i_p)
# Compute the per-body EoM residual over linear and angular parts
r_linear_i = wp.abs(m_i * (v_i - v_i_p) - dt * (m_i * g + f_i))
r_angular_i = wp.abs(I_i @ (omega_i - omega_i_p) - dt * (tau_i - S_i @ (I_i @ omega_i_p)))
r_i = screw(r_linear_i, r_angular_i)
# Compute the per-body maximum residual and argmax index
r_eom_i = wp.max(r_i)
r_eom_argmax_i = int32(wp.argmax(r_i))
# Update the per-world maximum residual and argmax index
previous_max = wp.atomic_max(metric_r_eom, wid, r_eom_i)
if r_eom_i >= previous_max:
argmax_key = int64(build_pair_key2(uint32(bid), uint32(r_eom_argmax_i)))
wp.atomic_exch(metric_r_eom_argmax, int32(wid), argmax_key)
@wp.kernel
def _compute_joint_kinematics_residual_dense(
# Inputs:
model_info_num_body_dofs: wp.array(dtype=int32),
model_info_bodies_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_joint_wid: wp.array(dtype=int32),
model_joint_num_kinematic_cts: wp.array(dtype=int32),
model_joint_kinematic_cts_offset: wp.array(dtype=int32),
model_joint_bid_B: wp.array(dtype=int32),
model_joint_bid_F: wp.array(dtype=int32),
data_bodies_u_i: wp.array(dtype=vec6f),
jacobian_cts_offset: wp.array(dtype=int32),
jacobian_cts_data: wp.array(dtype=float32),
# Outputs:
metric_r_kinematics: wp.array(dtype=float32),
metric_r_kinematics_argmax: wp.array(dtype=int64),
):
# Retrieve the joint index from the thread index
jid = wp.tid()
# Retrieve the world index of the joint
wid = model_joint_wid[jid]
# Retrieve the body indices of the joint
# NOTE: these indices are w.r.t the model
bid_F_j = model_joint_bid_F[jid]
bid_B_j = model_joint_bid_B[jid]
# Retrieve the size and index offset of the joint constraint
num_cts_j = model_joint_num_kinematic_cts[jid]
cts_offset_j = model_joint_kinematic_cts_offset[jid]
# Retrieve the world-specific info
nbd = model_info_num_body_dofs[wid]
bio = model_info_bodies_offset[wid]
kgo = model_info_joint_kinematic_cts_group_offset[wid]
mio = jacobian_cts_offset[wid]
# Compute the per-joint constraint Jacobian matrix-vector product
j_v_j = vec6f(0.0)
u_i_F = data_bodies_u_i[bid_F_j]
dio_F = 6 * (bid_F_j - bio)
for j in range(num_cts_j):
mio_j = mio + nbd * (kgo + cts_offset_j + j) + dio_F
for i in range(6):
j_v_j[j] += jacobian_cts_data[mio_j + i] * u_i_F[i]
if bid_B_j >= 0:
u_i_B = data_bodies_u_i[bid_B_j]
dio_B = 6 * (bid_B_j - bio)
for j in range(num_cts_j):
mio_j = mio + nbd * (kgo + cts_offset_j + j) + dio_B
for i in range(6):
j_v_j[j] += jacobian_cts_data[mio_j + i] * u_i_B[i]
# Compute the per-joint kinematics residual
r_kinematics_j = wp.max(wp.abs(j_v_j))
# Update the per-world maximum residual and argmax index
previous_max = wp.atomic_max(metric_r_kinematics, wid, r_kinematics_j)
if r_kinematics_j >= previous_max:
argmax_key = int64(build_pair_key2(uint32(jid), uint32(cts_offset_j)))
wp.atomic_exch(metric_r_kinematics_argmax, wid, argmax_key)
@wp.kernel
def _compute_joint_kinematics_residual_sparse(
# Inputs:
model_joint_wid: wp.array(dtype=int32),
model_joint_num_dynamic_cts: wp.array(dtype=int32),
model_joint_num_kinematic_cts: wp.array(dtype=int32),
model_joint_kinematic_cts_offset: wp.array(dtype=int32),
model_joint_bid_B: wp.array(dtype=int32),
model_joint_bid_F: wp.array(dtype=int32),
data_bodies_u_i: wp.array(dtype=vec6f),
jac_nzb_values: wp.array(dtype=vec6f),
jac_joint_nzb_offsets: wp.array(dtype=int32),
# Outputs:
metric_r_kinematics: wp.array(dtype=float32),
metric_r_kinematics_argmax: wp.array(dtype=int64),
):
# Retrieve the joint index from the thread index
jid = wp.tid()
# Retrieve the world index of the joint
wid = model_joint_wid[jid]
# Retrieve the body indices of the joint
# NOTE: these indices are w.r.t the model
bid_F_j = model_joint_bid_F[jid]
bid_B_j = model_joint_bid_B[jid]
# Retrieve the size and index offset of the joint constraint
num_dyn_cts_j = model_joint_num_dynamic_cts[jid]
num_kin_cts_j = model_joint_num_kinematic_cts[jid]
kin_cts_offset_j = model_joint_kinematic_cts_offset[jid]
# Retrieve the starting index for the non-zero blocks for the current joint
jac_j_nzb_start = jac_joint_nzb_offsets[jid] + (2 * num_dyn_cts_j if bid_B_j >= 0 else num_dyn_cts_j)
# Compute the per-joint constraint Jacobian matrix-vector product
j_v_j = vec6f(0.0)
u_i_F = data_bodies_u_i[bid_F_j]
for j in range(num_kin_cts_j):
jac_block = jac_nzb_values[jac_j_nzb_start + j]
j_v_j[j] += wp.dot(jac_block, u_i_F)
if bid_B_j >= 0:
u_i_B = data_bodies_u_i[bid_B_j]
for j in range(num_kin_cts_j):
jac_block = jac_nzb_values[jac_j_nzb_start + num_kin_cts_j + j]
j_v_j[j] += wp.dot(jac_block, u_i_B)
# Compute the per-joint kinematics residual
r_kinematics_j = wp.max(wp.abs(j_v_j))
# Update the per-world maximum residual and argmax index
previous_max = wp.atomic_max(metric_r_kinematics, wid, r_kinematics_j)
if r_kinematics_j >= previous_max:
argmax_key = int64(build_pair_key2(uint32(jid), uint32(kin_cts_offset_j)))
wp.atomic_exch(metric_r_kinematics_argmax, wid, argmax_key)
@wp.kernel
def _compute_cts_joints_residual(
# Inputs:
model_info_joints_cts_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
model_joint_wid: wp.array(dtype=int32),
model_joint_num_kinematic_cts: wp.array(dtype=int32),
model_joint_kinematic_cts_offset: wp.array(dtype=int32),
data_joints_r_j: wp.array(dtype=float32),
# Outputs:
metric_r_cts_joints: wp.array(dtype=float32),
metric_r_cts_joints_argmax: wp.array(dtype=int64),
):
# Retrieve the joint index from the thread index
jid = wp.tid()
# Retrieve the joint-specific model data
wid = model_joint_wid[jid]
num_cts_j = model_joint_num_kinematic_cts[jid]
cts_offset_j = model_joint_kinematic_cts_offset[jid]
# Retrieve the joint-block constraint index offset of the world
world_joint_cts_offset = model_info_joints_cts_offset[wid]
world_joint_kinematic_cts_group_offset = model_info_joint_kinematic_cts_group_offset[wid]
# Compute the global constraint index offset for the specific joint
cio_j = world_joint_cts_offset + world_joint_kinematic_cts_group_offset + cts_offset_j
# Compute the per-joint constraint residual (infinity-norm)
r_cts_joints_j = float32(0.0)
for j in range(num_cts_j):
r_cts_joints_j = wp.max(r_cts_joints_j, wp.abs(data_joints_r_j[cio_j + j]))
# Update the per-world maximum residual and argmax index
previous_max = wp.atomic_max(metric_r_cts_joints, wid, r_cts_joints_j)
if r_cts_joints_j >= previous_max:
argmax_key = int64(build_pair_key2(uint32(jid), uint32(cts_offset_j)))
wp.atomic_exch(metric_r_cts_joints_argmax, wid, argmax_key)
@wp.kernel
def _compute_cts_limits_residual(
# Inputs:
limit_model_num_limits: wp.array(dtype=int32),
limit_wid: wp.array(dtype=int32),
limit_lid: wp.array(dtype=int32),
limit_dof: wp.array(dtype=int32),
limit_r_q: wp.array(dtype=float32),
# Outputs:
metric_r_cts_limits: wp.array(dtype=float32),
metric_r_cts_limits_argmax: wp.array(dtype=int64),
):
# Retrieve the thread index as the limit index
lid = wp.tid()
# Retrieve the number of limits active in the model
model_nl = limit_model_num_limits[0]
# Skip if lid is greater than the number of limits active in the model
if lid >= model_nl:
return
# Retrieve the world index and the world-relative limit index for this limit
wid = limit_wid[lid]
wlid = limit_lid[lid]
dof = limit_dof[lid]
# Compute the per-limit constraint residual (infinity-norm)
r_cts_limits_l = wp.abs(limit_r_q[lid])
# Update the per-world maximum residual
previous_max = wp.atomic_max(metric_r_cts_limits, wid, r_cts_limits_l)
if r_cts_limits_l >= previous_max:
argmax_key = int64(build_pair_key2(uint32(wlid), uint32(dof)))
wp.atomic_exch(metric_r_cts_limits_argmax, wid, argmax_key)
@wp.kernel
def _compute_cts_contacts_residual(
# Inputs:
contact_model_num_contacts: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_gapfunc: wp.array(dtype=vec4f),
# Outputs:
metric_r_cts_contacts: wp.array(dtype=float32),
metric_r_cts_contacts_argmax: wp.array(dtype=int32),
):
# Retrieve the thread index as the contact index
cid = wp.tid()
# Retrieve the number of contacts active in the model
model_nc = contact_model_num_contacts[0]
# Skip if cid is greater than the number of contacts active in the model
if cid >= model_nc:
return
# Retrieve the per-contact data
wid = contact_wid[cid]
wcid = contact_cid[cid]
gapfunc = contact_gapfunc[cid]
# Compute the per-contact constraint residual (infinity-norm)
r_cts_contacts_k = wp.abs(gapfunc[3])
# Update the per-world maximum residual and argmax index
previous_max = wp.atomic_max(metric_r_cts_contacts, wid, r_cts_contacts_k)
if r_cts_contacts_k >= previous_max:
wp.atomic_exch(metric_r_cts_contacts_argmax, wid, wcid)
@wp.kernel
def _compute_dual_problem_metrics(
# Inputs:
problem_nl: wp.array(dtype=int32),
problem_nc: wp.array(dtype=int32),
problem_cio: wp.array(dtype=int32),
problem_lcgo: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_mio: wp.array(dtype=int32),
problem_mu: wp.array(dtype=float32),
problem_v_f: wp.array(dtype=float32),
problem_D: wp.array(dtype=float32),
problem_P: wp.array(dtype=float32),
solution_sigma: wp.array(dtype=vec2f),
solution_lambdas: wp.array(dtype=float32),
solution_v_plus: wp.array(dtype=float32),
# Buffers:
buffer_s: wp.array(dtype=float32),
buffer_v: wp.array(dtype=float32),
# Outputs:
metric_r_v_plus: wp.array(dtype=float32),
metric_r_v_plus_argmax: wp.array(dtype=int32),
metric_r_ncp_primal: wp.array(dtype=float32),
metric_r_ncp_primal_argmax: wp.array(dtype=int32),
metric_r_ncp_dual: wp.array(dtype=float32),
metric_r_ncp_dual_argmax: wp.array(dtype=int32),
metric_r_ncp_compl: wp.array(dtype=float32),
metric_r_ncp_compl_argmax: wp.array(dtype=int32),
metric_r_vi_natmap: wp.array(dtype=float32),
metric_r_vi_natmap_argmax: wp.array(dtype=int32),
metric_f_ncp: wp.array(dtype=float32),
metric_f_ccp: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index
wid = wp.tid()
# Retrieve the world-specific data
nl = problem_nl[wid]
nc = problem_nc[wid]
ncts = problem_dim[wid]
cio = problem_cio[wid]
lcgo = problem_lcgo[wid]
ccgo = problem_ccgo[wid]
vio = problem_vio[wid]
mio = problem_mio[wid]
sigma = solution_sigma[wid]
# Compute additional info
njc = ncts - (nl + 3 * nc)
# Compute the post-event constraint-space velocity from the current solution: v_plus = v_f + D @ lambda
# NOTE: We assume the dual problem linear terms `D` and `v_f` have already been preconditioned in-place using `P`
compute_v_plus(ncts, vio, mio, sigma[0], problem_P, problem_D, problem_v_f, solution_lambdas, buffer_v)
# Compute the post-event constraint-space velocity error as: r_v_plus = || v_plus_est - v_plus_true ||_inf
r_v_plus, r_v_plus_argmax = compute_vector_difference_infnorm(ncts, vio, solution_v_plus, buffer_v)
# Compute the De Saxce correction for each contact as: s = G(v_plus)
compute_desaxce_corrections(nc, cio, vio, ccgo, problem_mu, buffer_v, buffer_s)
# Compute the CCP optimization objective as: f_ccp = 0.5 * lambda.dot(v_plus + v_f)
f_ccp = 0.5 * compute_double_dot_product(ncts, vio, solution_lambdas, buffer_v, problem_v_f)
# Compute the NCP optimization objective as: f_ncp = f_ccp + lambda.dot(s)
f_ncp = compute_dot_product(ncts, vio, solution_lambdas, buffer_s)
f_ncp += f_ccp
# Compute the augmented post-event constraint-space velocity as: v_aug = v_plus + s
compute_vector_sum(ncts, vio, buffer_v, buffer_s, buffer_v)
# Compute the NCP primal residual as: r_p := || lambda - proj_K(lambda) ||_inf
r_ncp_p, r_ncp_p_argmax = compute_ncp_primal_residual(nl, nc, vio, lcgo, ccgo, cio, problem_mu, solution_lambdas)
# Compute the NCP dual residual as: r_d := || v_plus + s - proj_dual_K(v_plus + s) ||_inf
r_ncp_d, r_ncp_d_argmax = compute_ncp_dual_residual(njc, nl, nc, vio, lcgo, ccgo, cio, problem_mu, buffer_v)
# Compute the NCP complementarity (lambda _|_ (v_plus + s)) residual as r_c := || lambda.dot(v_plus + s) ||_inf
r_ncp_c, r_ncp_c_argmax = compute_ncp_complementarity_residual(nl, nc, vio, lcgo, ccgo, buffer_v, solution_lambdas)
# Compute the natural-map residuals as: r_natmap = || lambda - proj_K(lambda - (v + s)) ||_inf
r_ncp_natmap, r_ncp_natmap_argmax = compute_ncp_natural_map_residual(
nl, nc, vio, lcgo, ccgo, cio, problem_mu, buffer_v, solution_lambdas
)
# Store the computed metrics in the output arrays
metric_r_v_plus[wid] = r_v_plus
metric_r_v_plus_argmax[wid] = r_v_plus_argmax
metric_r_ncp_primal[wid] = r_ncp_p
metric_r_ncp_primal_argmax[wid] = r_ncp_p_argmax
metric_r_ncp_dual[wid] = r_ncp_d
metric_r_ncp_dual_argmax[wid] = r_ncp_d_argmax
metric_r_ncp_compl[wid] = r_ncp_c
metric_r_ncp_compl_argmax[wid] = r_ncp_c_argmax
metric_r_vi_natmap[wid] = r_ncp_natmap
metric_r_vi_natmap_argmax[wid] = r_ncp_natmap_argmax
metric_f_ncp[wid] = f_ncp
metric_f_ccp[wid] = f_ccp
@wp.kernel
def _compute_dual_problem_metrics_sparse(
# Inputs:
problem_nl: wp.array(dtype=int32),
problem_nc: wp.array(dtype=int32),
problem_cio: wp.array(dtype=int32),
problem_lcgo: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_mu: wp.array(dtype=float32),
problem_v_f: wp.array(dtype=float32),
problem_P: wp.array(dtype=float32),
solution_lambdas: wp.array(dtype=float32),
solution_v_plus: wp.array(dtype=float32),
# Buffers:
buffer_s: wp.array(dtype=float32),
buffer_v: wp.array(dtype=float32),
# Outputs:
metric_r_v_plus: wp.array(dtype=float32),
metric_r_v_plus_argmax: wp.array(dtype=int32),
metric_r_ncp_primal: wp.array(dtype=float32),
metric_r_ncp_primal_argmax: wp.array(dtype=int32),
metric_r_ncp_dual: wp.array(dtype=float32),
metric_r_ncp_dual_argmax: wp.array(dtype=int32),
metric_r_ncp_compl: wp.array(dtype=float32),
metric_r_ncp_compl_argmax: wp.array(dtype=int32),
metric_r_vi_natmap: wp.array(dtype=float32),
metric_r_vi_natmap_argmax: wp.array(dtype=int32),
metric_f_ncp: wp.array(dtype=float32),
metric_f_ccp: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index
wid = wp.tid()
# Retrieve the world-specific data
nl = problem_nl[wid]
nc = problem_nc[wid]
ncts = problem_dim[wid]
cio = problem_cio[wid]
lcgo = problem_lcgo[wid]
ccgo = problem_ccgo[wid]
vio = problem_vio[wid]
# Compute additional info
njc = ncts - (nl + 3 * nc)
# Compute the post-event constraint-space velocity from the current solution: v_plus = v_f + D @ lambda
# NOTE: We assume the dual problem term `v_f` has already been preconditioned in-place using `P`, and
# D @ lambdas is already provided in `buffer_v`
compute_v_plus_sparse(ncts, vio, problem_P, problem_v_f, buffer_v, buffer_v)
# Compute the post-event constraint-space velocity error as: r_v_plus = || v_plus_est - v_plus_true ||_inf
r_v_plus, r_v_plus_argmax = compute_vector_difference_infnorm(ncts, vio, solution_v_plus, buffer_v)
# Compute the De Saxce correction for each contact as: s = G(v_plus)
compute_desaxce_corrections(nc, cio, vio, ccgo, problem_mu, buffer_v, buffer_s)
# Compute the CCP optimization objective as: f_ccp = 0.5 * lambda.dot(v_plus + v_f)
f_ccp = 0.5 * compute_double_dot_product(ncts, vio, solution_lambdas, buffer_v, problem_v_f)
# Compute the NCP optimization objective as: f_ncp = f_ccp + lambda.dot(s)
f_ncp = compute_dot_product(ncts, vio, solution_lambdas, buffer_s)
f_ncp += f_ccp
# Compute the augmented post-event constraint-space velocity as: v_aug = v_plus + s
compute_vector_sum(ncts, vio, buffer_v, buffer_s, buffer_v)
# Compute the NCP primal residual as: r_p := || lambda - proj_K(lambda) ||_inf
r_ncp_p, r_ncp_p_argmax = compute_ncp_primal_residual(nl, nc, vio, lcgo, ccgo, cio, problem_mu, solution_lambdas)
# Compute the NCP dual residual as: r_d := || v_plus + s - proj_dual_K(v_plus + s) ||_inf
r_ncp_d, r_ncp_d_argmax = compute_ncp_dual_residual(njc, nl, nc, vio, lcgo, ccgo, cio, problem_mu, buffer_v)
# Compute the NCP complementarity (lambda _|_ (v_plus + s)) residual as r_c := || lambda.dot(v_plus + s) ||_inf
r_ncp_c, r_ncp_c_argmax = compute_ncp_complementarity_residual(nl, nc, vio, lcgo, ccgo, buffer_v, solution_lambdas)
# Compute the natural-map residuals as: r_natmap = || lambda - proj_K(lambda - (v + s)) ||_inf
r_ncp_natmap, r_ncp_natmap_argmax = compute_ncp_natural_map_residual(
nl, nc, vio, lcgo, ccgo, cio, problem_mu, buffer_v, solution_lambdas
)
# Store the computed metrics in the output arrays
metric_r_v_plus[wid] = r_v_plus
metric_r_v_plus_argmax[wid] = r_v_plus_argmax
metric_r_ncp_primal[wid] = r_ncp_p
metric_r_ncp_primal_argmax[wid] = r_ncp_p_argmax
metric_r_ncp_dual[wid] = r_ncp_d
metric_r_ncp_dual_argmax[wid] = r_ncp_d_argmax
metric_r_ncp_compl[wid] = r_ncp_c
metric_r_ncp_compl_argmax[wid] = r_ncp_c_argmax
metric_r_vi_natmap[wid] = r_ncp_natmap
metric_r_vi_natmap_argmax[wid] = r_ncp_natmap_argmax
metric_f_ncp[wid] = f_ncp
metric_f_ccp[wid] = f_ccp
###
# Interfaces
###
class SolutionMetrics:
"""
Provides a high-level interface to compute a set of simulation solver performance metrics
that provide quantitative measures of constraint satisfaction and physical plausibility of
the computed solution to the dual forward-dynamics Nonlinear Complementarity Problem (NCP).
This class is therefore responsible for managing the lifetime of all metrics data memory
allocations, as well as provide an easy-to-use public API of the relevant operations.
Internally, it holds an instance of the :class:`SolutionMetricsData` class. For more details
about the specific metrics computed, please refer to the documentation of that class.
"""
def __init__(self, model: ModelKamino | None = None, device: wp.DeviceLike = None):
"""
Initializes the solution metrics evaluator.
Args:
model (ModelKamino):
The model containing the time-invariant data of the simulation.
device (wp.DeviceLike, optional):
The device where the metrics data should be allocated.\n
If not specified, the model's device will be used by default.
"""
# Declare and initialize the target device
# NOTE: This can be overridden during a
# later call to `finalize()` if needed
self._device: wp.DeviceLike = device
# Declare the metrics data container
self._data: SolutionMetricsData | None = None
# Declare data buffers for metrics computations
self._buffer_s: wp.array | None = None
self._buffer_v: wp.array | None = None
# If a model is provided, finalize the metrics data allocations
if model is not None:
self.finalize(model, device)
def finalize(self, model: ModelKamino, device: wp.DeviceLike = None):
"""
Finalizes the metrics data allocations on the specified device.
Args:
model (ModelKamino):
The model containing the time-invariant data of the simulation.
device (wp.DeviceLike, optional):
The device where the metrics data should be allocated.\n
If not specified, the model's device will be used by default.
"""
# Ensure the model is valid
if not isinstance(model, ModelKamino):
raise TypeError("Expected 'model' to be of type ModelKamino.")
# Set the target device for metrics data allocation and execution
# If no device is specified, use the model's device by default
self._device = device
if self._device is None:
self._device = model.device
# Allocate metrics data on the target device
with wp.ScopedDevice(self._device):
# Allocate reusable buffers for metrics computations
self._buffer_v = wp.zeros(model.size.sum_of_max_total_cts, dtype=float32)
self._buffer_s = wp.zeros(model.size.sum_of_max_total_cts, dtype=float32)
# Allocate the metrics container data arrays
self._data = SolutionMetricsData(
r_eom=wp.zeros(model.size.num_worlds, dtype=float32),
r_eom_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int64),
r_kinematics=wp.zeros(model.size.num_worlds, dtype=float32),
r_kinematics_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int64),
r_cts_joints=wp.zeros(model.size.num_worlds, dtype=float32),
r_cts_joints_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int64),
r_cts_limits=wp.zeros(model.size.num_worlds, dtype=float32),
r_cts_limits_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int64),
r_cts_contacts=wp.zeros(model.size.num_worlds, dtype=float32),
r_cts_contacts_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int32),
r_v_plus=wp.zeros(model.size.num_worlds, dtype=float32),
r_v_plus_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int32),
r_ncp_primal=wp.zeros(model.size.num_worlds, dtype=float32),
r_ncp_primal_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int32),
r_ncp_dual=wp.zeros(model.size.num_worlds, dtype=float32),
r_ncp_dual_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int32),
r_ncp_compl=wp.zeros(model.size.num_worlds, dtype=float32),
r_ncp_compl_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int32),
r_vi_natmap=wp.zeros(model.size.num_worlds, dtype=float32),
r_vi_natmap_argmax=wp.full(model.size.num_worlds, value=-1, dtype=int32),
f_ncp=wp.zeros(model.size.num_worlds, dtype=float32),
f_ccp=wp.zeros(model.size.num_worlds, dtype=float32),
)
###
# Properties
###
@property
def device(self) -> wp.DeviceLike:
"""
Returns the device where the metrics data is allocated.
"""
return self._device
@property
def data(self) -> SolutionMetricsData:
"""
Returns the metrics data container.
"""
self._assert_has_data()
return self._data
###
# Public Operations
###
def reset(self):
"""
Resets all metrics to zeros.
"""
self._data.reset()
def evaluate(
self,
sigma: wp.array,
lambdas: wp.array,
v_plus: wp.array,
model: ModelKamino,
data: DataKamino,
state_p: StateKamino,
problem: DualProblem,
jacobians: DenseSystemJacobians | SparseSystemJacobians,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
):
"""
Evaluates all solution performance metrics.
Args:
model (ModelKamino):
The model containing the time-invariant data of the simulation.
data (DataKamino):
The model data containing the time-variant data of the simulation.
state_p (StateKamino):
The previous state of the simulation.
limits (LimitsKamino):
The joint-limits data describing active limit constraints.
contacts (ContactsKamino):
The contact data describing active contact constraints.
problem (DualProblem):
The dual forward dynamics problem of the current time-step.
jacobians (DenseSystemJacobians | SparseSystemJacobians):
The system Jacobians of the current time-step.
sigma (wp.array):
The array diagonal regularization applied to the Delassus matrix of the current dual problem.
lambdas (wp.array):
The array of constraint reactions (i.e. Lagrange multipliers) of the current dual problem solution.
v_plus (wp.array):
The array of post-event constraint-space velocities of the current dual problem solution.
"""
self._assert_has_data()
self._evaluate_constraint_violations_perf(model, data, limits, contacts)
self._evaluate_primal_problem_perf(model, data, state_p, jacobians)
self._evaluate_dual_problem_perf(sigma, lambdas, v_plus, problem)
###
# Internals
###
def _assert_has_data(self):
"""
Asserts that the metrics data has been finalized and is available.
Raises:
RuntimeError: If the data is not available.
"""
if self._data is None:
raise RuntimeError("SolutionMetrics data has not been finalized. Call finalize() first.")
def _evaluate_constraint_violations_perf(
self,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
):
"""
Evaluates the constraint-violation performance metrics.
Args:
model (ModelKamino):
The model containing the time-invariant data of the simulation.
data (DataKamino):
The model data containing the time-variant data of the simulation.
limits (LimitsKamino):
The joint-limits data describing active limit constraints.
contacts (ContactsKamino):
The contact data describing active contact constraints.
"""
# Ensure metrics data is available
self._assert_has_data()
# Compute the largest configuration-level joint constraint residuals (i.e. violations)
if model.size.sum_of_num_joints > 0:
wp.launch(
kernel=_compute_cts_joints_residual,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.joint_cts_offset,
model.info.joint_kinematic_cts_group_offset,
model.joints.wid,
model.joints.num_kinematic_cts,
model.joints.kinematic_cts_offset,
data.joints.r_j,
# Outputs:
self._data.r_cts_joints,
self._data.r_cts_joints_argmax,
],
device=model.device,
)
# Compute the largest joint-limit constraint residuals (i.e. penetrations)
if limits is not None and limits.model_max_limits_host > 0:
wp.launch(
kernel=_compute_cts_limits_residual,
dim=limits.data.model_max_limits_host,
inputs=[
# Inputs:
limits.data.model_active_limits,
limits.data.wid,
limits.data.lid,
limits.data.dof,
limits.data.r_q,
# Outputs:
self._data.r_cts_limits,
self._data.r_cts_limits_argmax,
],
device=model.device,
)
# Compute the largest contact constraint residuals (i.e. penetrations)
if contacts is not None and contacts.model_max_contacts_host > 0:
wp.launch(
kernel=_compute_cts_contacts_residual,
dim=contacts.data.model_max_contacts_host,
inputs=[
# Inputs:
contacts.data.model_active_contacts,
contacts.data.wid,
contacts.data.cid,
contacts.data.gapfunc,
# Outputs:
self._data.r_cts_contacts,
self._data.r_cts_contacts_argmax,
],
device=model.device,
)
def _evaluate_primal_problem_perf(
self,
model: ModelKamino,
data: DataKamino,
state_p: StateKamino,
jacobians: DenseSystemJacobians | SparseSystemJacobians,
):
"""
Evaluates the primal problem performance metrics.
Args:
model (ModelKamino):
The model containing the time-invariant data of the simulation.
data (DataKamino):
The model data containing the time-variant data of the simulation.
state_p (StateKamino):
The previous state of the simulation.
jacobians (DenseSystemJacobians | SparseSystemJacobians):
The system Jacobians of the current time-step.
"""
# Ensure metrics data is available
self._assert_has_data()
# Compute the equations-of-motion residuals
wp.launch(
kernel=_compute_eom_residual,
dim=model.size.sum_of_num_bodies,
inputs=[
# Inputs:
model.time.dt,
model.gravity.vector,
model.bodies.wid,
model.bodies.m_i,
data.bodies.I_i,
data.bodies.w_i,
data.bodies.u_i,
state_p.u_i,
# Outputs:
self._data.r_eom,
self._data.r_eom_argmax,
],
device=model.device,
)
# Compute the kinematics constraint residuals,
# i.e. velocity-level joint constraint equations
if model.size.sum_of_num_joints > 0:
if isinstance(jacobians, DenseSystemJacobians):
wp.launch(
kernel=_compute_joint_kinematics_residual_dense,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.info.num_body_dofs,
model.info.bodies_offset,
model.info.joint_kinematic_cts_group_offset,
model.joints.wid,
model.joints.num_kinematic_cts,
model.joints.kinematic_cts_offset,
model.joints.bid_B,
model.joints.bid_F,
data.bodies.u_i,
jacobians.data.J_cts_offsets,
jacobians.data.J_cts_data,
# Outputs:
self._data.r_kinematics,
self._data.r_kinematics_argmax,
],
device=model.device,
)
else:
J_cts = jacobians._J_cts.bsm
wp.launch(
kernel=_compute_joint_kinematics_residual_sparse,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.joints.wid,
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.kinematic_cts_offset,
model.joints.bid_B,
model.joints.bid_F,
data.bodies.u_i,
J_cts.nzb_values,
jacobians._J_cts_joint_nzb_offsets,
# Outputs:
self._data.r_kinematics,
self._data.r_kinematics_argmax,
],
device=model.device,
)
def _evaluate_dual_problem_perf(
self,
sigma: wp.array,
lambdas: wp.array,
v_plus: wp.array,
problem: DualProblem,
):
"""
Evaluates the dual problem performance metrics.
Args:
problem (DualProblem):
The dual problem containing the time-invariant and time-variant data of the simulation.
sigma (wp.array):
The array of sigma values for the dual problem.
lambdas (wp.array):
The array of lambda values for the dual problem.
v_plus (wp.array):
The array of v_plus values for the dual problem.
"""
# Ensure metrics data is available
self._assert_has_data()
# Compute the dual problem NCP/VI performance metrics
if problem.sparse:
# Compute post-event constraint-space velocity from solution: v_plus = v_f + D @ lambda
# Store it in buffer for further processing in dual problem metrics computation
delassus_reg_prev = problem.delassus._eta
delassus_pre_prev = problem.delassus._preconditioner
problem.delassus.set_regularization(None)
problem.delassus.set_preconditioner(None)
problem.delassus.matvec(
x=lambdas,
y=self._buffer_v,
world_mask=wp.ones((problem.data.num_worlds,), dtype=wp.int32, device=self.device),
)
problem.delassus.set_regularization(delassus_reg_prev)
problem.delassus.set_preconditioner(delassus_pre_prev)
wp.launch(
kernel=_compute_dual_problem_metrics_sparse,
dim=problem.size.num_worlds,
inputs=[
# Inputs:
problem.data.nl,
problem.data.nc,
problem.data.cio,
problem.data.lcgo,
problem.data.ccgo,
problem.data.dim,
problem.data.vio,
problem.data.mu,
problem.data.v_f,
problem.data.P,
lambdas,
v_plus,
# Buffers:
self._buffer_s,
self._buffer_v,
# Outputs:
self._data.r_v_plus,
self._data.r_v_plus_argmax,
self._data.r_ncp_primal,
self._data.r_ncp_primal_argmax,
self._data.r_ncp_dual,
self._data.r_ncp_dual_argmax,
self._data.r_ncp_compl,
self._data.r_ncp_compl_argmax,
self._data.r_vi_natmap,
self._data.r_vi_natmap_argmax,
self._data.f_ncp,
self._data.f_ccp,
],
device=problem.device,
)
else:
wp.launch(
kernel=_compute_dual_problem_metrics,
dim=problem.size.num_worlds,
inputs=[
# Inputs:
problem.data.nl,
problem.data.nc,
problem.data.cio,
problem.data.lcgo,
problem.data.ccgo,
problem.data.dim,
problem.data.vio,
problem.data.mio,
problem.data.mu,
problem.data.v_f,
problem.data.D,
problem.data.P,
sigma,
lambdas,
v_plus,
# Buffers:
self._buffer_s,
self._buffer_v,
# Outputs:
self._data.r_v_plus,
self._data.r_v_plus_argmax,
self._data.r_ncp_primal,
self._data.r_ncp_primal_argmax,
self._data.r_ncp_dual,
self._data.r_ncp_dual_argmax,
self._data.r_ncp_compl,
self._data.r_ncp_compl_argmax,
self._data.r_vi_natmap,
self._data.r_vi_natmap_argmax,
self._data.f_ncp,
self._data.f_ccp,
],
device=problem.device,
)
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/padmm/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Proximal-ADMM Solver
Provides a forward dynamics solver for constrained rigid multi-body systems using
the Alternating Direction Method of Multipliers (ADMM). This implementation realizes
the Proximal-ADMM algorithm described in [1] and is based on the work of J. Carpentier
et al in [2]. It solves the Lagrange dual of the constrained forward dynamics problem
in constraint reactions (i.e. impulses) and post-event constraint-space velocities. The
diagonal preconditioner strategy described in [3] is also implemented to improve
numerical conditioning. This version also incorporates Nesterov-style gradient
acceleration with adaptive restarts based on the work of O'Donoghue and Candes in [4].
Notes
----
- ADMM is based on the Augmented Lagrangian Method (ALM) for dealing with set-inclusion
(i.e. inequality) constraints, but introduces an alternating primal-dual descent/ascent scheme.
- Proximal-ADMM introduces an additional proximal regularization term to the optimization objective.
- Uses (optional) Nesterov-style gradient acceleration with adaptive restarts.
- Uses (optional) adaptive penalty updates based on primal-dual residual balancing.
References
----
- [1] https://arxiv.org/abs/2504.19771
- [2] https://arxiv.org/pdf/2405.17020
- [3] https://onlinelibrary.wiley.com/doi/full/10.1002/nme.6693
- [4] https://epubs.siam.org/doi/abs/10.1137/120896219
Usage
----
A typical example for using this module is:
# Import all relevant types from Kamino
from newton._src.solvers.kamino.core import ModelBuilderKamino
from newton._src.solvers.kamino._src.geometry import ContactsKamino
from newton._src.solvers.kamino._src.kinematics import LimitsKamino
from newton._src.solvers.kamino._src.kinematics import DenseSystemJacobians
from newton._src.solvers.kamino._src.dynamics import DualProblem
from newton._src.solvers.kamino.solvers import PADMMSolver
# Create a model builder and add bodies, joints, geoms, etc.
builder = ModelBuilderKamino()
...
# Create a model from the builder and construct additional
# containers to hold joint-limits, contacts, Jacobians
model = builder.finalize()
data = model.data()
limits = LimitsKamino(model)
contacts = ContactsKamino(builder)
jacobians = DenseSystemJacobians(model, limits, contacts)
# Build the Jacobians for the model and active limits and contacts
jacobians.build(model, data, limits, contacts)
...
# Create a forward-dynamics DualProblem to be solved
dual = DualProblem(model, limits, contacts)
dual.build(model, data, limits, contacts, jacobians)
# Create a forward-dynamics PADMM solver
solver = PADMMSolver(model, limits, contacts)
# Solve the dual forward dynamics problem
solver.coldstart()
solver.solve(problem=dual)
# Extract the resulting constraint reactions
multipliers = solver.solution.lambdas
"""
from .solver import PADMMSolver
from .types import PADMMPenaltyUpdate, PADMMWarmStartMode
###
# Module interface
###
__all__ = [
"PADMMPenaltyUpdate",
"PADMMSolver",
"PADMMWarmStartMode",
]
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/padmm/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Defines the Warp kernels used by the Proximal-ADMM solver."""
import functools
from typing import Any
import warp as wp
from ...core.math import FLOAT32_EPS, FLOAT32_MAX
from ...core.types import float32, int32, vec2f, vec3f
from .math import (
compute_cwise_vec_div,
compute_cwise_vec_mul,
compute_desaxce_corrections,
compute_dot_product,
compute_double_dot_product,
compute_gemv,
compute_inverse_preconditioned_iterate_residual,
compute_l2_norm,
compute_ncp_complementarity_residual,
compute_ncp_dual_residual,
compute_ncp_natural_map_residual,
compute_ncp_primal_residual,
compute_preconditioned_iterate_residual,
compute_vector_sum,
project_to_coulomb_cone,
)
from .types import PADMMConfigStruct, PADMMPenalty, PADMMPenaltyUpdate, PADMMStatus
###
# Module interface
###
__all__ = [
"_apply_dual_preconditioner_to_solution",
"_apply_dual_preconditioner_to_state",
"_compute_complementarity_residuals",
"_compute_desaxce_correction",
"_compute_final_desaxce_correction",
"_compute_projection_argument",
"_compute_solution_vectors",
"_compute_velocity_bias",
"_make_compute_infnorm_residuals_accel_kernel",
"_make_compute_infnorm_residuals_kernel",
"_project_to_feasible_cone",
"_reset_solver_data",
"_update_delassus_proximal_regularization",
"_update_delassus_proximal_regularization_sparse",
"_update_state_with_acceleration",
"_warmstart_contact_constraints",
"_warmstart_desaxce_correction",
"_warmstart_joint_constraints",
"_warmstart_limit_constraints",
"make_collect_solver_info_kernel",
"make_collect_solver_info_kernel_sparse",
"make_initialize_solver_kernel",
"make_update_dual_variables_and_compute_primal_dual_residuals",
"make_update_proximal_regularization_kernel",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _reset_solver_data(
# Outputs:
world_mask: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_maxdim: wp.array(dtype=int32),
lambdas: wp.array(dtype=float32),
v_plus: wp.array(dtype=float32),
):
# Retrieve the world and constraint indices from the 2D thread grid
wid, tid = wp.tid()
# Retrieve the maximum number of constraints in the world
maxncts = problem_maxdim[wid]
# Skip operation if the world is masked out
if world_mask[wid] == 0 or tid >= maxncts:
return
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Compute the index offset of the vector block of the world
thread_offset = vio + tid
# Reset the solver state variables to zero
lambdas[thread_offset] = 0.0
v_plus[thread_offset] = 0.0
@wp.kernel
def _warmstart_desaxce_correction(
problem_nc: wp.array(dtype=int32),
problem_cio: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_mu: wp.array(dtype=float32),
# Outputs:
solver_z: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, cid = wp.tid()
# Retrieve the number of contact active in the world
nc = problem_nc[wid]
# Retrieve the limit constraint group offset of the world
ccgo = problem_ccgo[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if cid >= nc:
return
# Retrieve the index offset of the vector block of the world
cio = problem_cio[wid]
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Retrieve the friction coefficient for this contact
mu = problem_mu[cio + cid]
# Compute the vector index offset of the corresponding contact constraint
ccio_k = vio + ccgo + 3 * cid
# Compute the norm of the tangential components, where:
# s = G(v_plus)
# v_plus = z - s => z = v_plus + s
vtx = solver_z[ccio_k]
vty = solver_z[ccio_k + 1]
vn = solver_z[ccio_k + 2]
vt_norm = wp.sqrt(vtx * vtx + vty * vty)
# Store De Saxce correction for this block
solver_z[ccio_k + 2] = vn + mu * vt_norm
@wp.kernel
def _warmstart_joint_constraints(
# Inputs:
model_time_dt: wp.array(dtype=float32),
model_info_joint_cts_offset: wp.array(dtype=int32),
model_info_total_cts_offset: wp.array(dtype=int32),
model_info_joint_dynamic_cts_group_offset: wp.array(dtype=int32),
model_info_joint_kinematic_cts_group_offset: wp.array(dtype=int32),
joint_wid: wp.array(dtype=int32),
joint_num_dynamic_cts: wp.array(dtype=int32),
joint_num_kinematic_cts: wp.array(dtype=int32),
joint_dynamic_cts_offset: wp.array(dtype=int32),
joint_kinematic_cts_offset: wp.array(dtype=int32),
joint_lambda_j: wp.array(dtype=float32),
problem_P: wp.array(dtype=float32),
# Outputs:
x_0: wp.array(dtype=float32),
y_0: wp.array(dtype=float32),
z_0: wp.array(dtype=float32),
):
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the joint-specific model info
wid_j = joint_wid[jid]
num_dynamic_cts_j = joint_num_dynamic_cts[jid]
num_kinematic_cts_j = joint_num_kinematic_cts[jid]
dynamic_cts_start_j = joint_dynamic_cts_offset[jid]
kinematic_cts_start_j = joint_kinematic_cts_offset[jid]
# Retrieve the world-specific info
dt = model_time_dt[wid_j]
world_joint_cts_start = model_info_joint_cts_offset[wid_j]
world_total_cts_start = model_info_total_cts_offset[wid_j]
world_joint_dynamic_cts_group_start = model_info_joint_dynamic_cts_group_offset[wid_j]
world_joint_kinematic_cts_group_start = model_info_joint_kinematic_cts_group_offset[wid_j]
# Compute block offsets of the joint's constraints within
# the joint-only constraints and total constraints arrays
joint_dyn_cts_start = world_joint_cts_start + world_joint_dynamic_cts_group_start + dynamic_cts_start_j
dyn_cts_row_start_j = world_total_cts_start + world_joint_dynamic_cts_group_start + dynamic_cts_start_j
joint_kin_cts_start = world_joint_cts_start + world_joint_kinematic_cts_group_start + kinematic_cts_start_j
kin_cts_row_start_j = world_total_cts_start + world_joint_kinematic_cts_group_start + kinematic_cts_start_j
# For each joint constraint, scale the constraint force by the time-step and
# the preconditioner and initialize the solver state variables accordingly
for j in range(num_dynamic_cts_j):
P_j = problem_P[dyn_cts_row_start_j + j]
lambda_j = (dt / P_j) * joint_lambda_j[joint_dyn_cts_start + j]
x_0[dyn_cts_row_start_j + j] = lambda_j
y_0[dyn_cts_row_start_j + j] = lambda_j
z_0[dyn_cts_row_start_j + j] = 0.0
for j in range(num_kinematic_cts_j):
P_j = problem_P[kin_cts_row_start_j + j]
lambda_j = (dt / P_j) * joint_lambda_j[joint_kin_cts_start + j]
x_0[kin_cts_row_start_j + j] = lambda_j
y_0[kin_cts_row_start_j + j] = lambda_j
z_0[kin_cts_row_start_j + j] = 0.0
@wp.kernel
def _warmstart_limit_constraints(
# Inputs:
model_time_dt: wp.array(dtype=float32),
model_info_total_cts_offset: wp.array(dtype=int32),
data_info_limit_cts_group_offset: wp.array(dtype=int32),
limit_model_num_active: wp.array(dtype=int32),
limit_wid: wp.array(dtype=int32),
limit_lid: wp.array(dtype=int32),
limit_reaction: wp.array(dtype=float32),
limit_velocity: wp.array(dtype=float32),
problem_P: wp.array(dtype=float32),
# Outputs:
x_0: wp.array(dtype=float32),
y_0: wp.array(dtype=float32),
z_0: wp.array(dtype=float32),
):
# Retrieve the thread index as the limit index
lid = wp.tid()
# Retrieve the number of limits active in the model
model_nl = limit_model_num_active[0]
# Skip if lid is greater than the number of limits active in the model
if lid >= model_nl:
return
# Retrieve the limit-specific data
wid = limit_wid[lid]
lid_l = limit_lid[lid]
lambda_l = limit_reaction[lid]
v_plus_l = limit_velocity[lid]
# Retrieve the world-specific info
dt = model_time_dt[wid]
total_cts_offset = model_info_total_cts_offset[wid]
limit_cts_offset = data_info_limit_cts_group_offset[wid]
# Compute block offsets of the limit constraints within
# the limit-only constraints and total constraints arrays
vio_l = total_cts_offset + limit_cts_offset + lid_l
# Load the diagonal preconditioner for the limit constraints
# NOTE: We only need to load the first element since by necessity
# the preconditioner is constant across the 3 constraint dimensions
P_l = problem_P[vio_l]
# Scale the limit force by the time-step to
# render an impulse and by the preconditioner
lambda_l *= dt / P_l
# Scale the limit velocity by the preconditioner
v_plus_l *= P_l
# Compute and store the limit-constraint reaction forces
x_0[vio_l] = lambda_l
y_0[vio_l] = lambda_l
z_0[vio_l] = v_plus_l
@wp.kernel
def _warmstart_contact_constraints(
# Inputs:
model_time_dt: wp.array(dtype=float32),
model_info_total_cts_offset: wp.array(dtype=int32),
data_info_contact_cts_group_offset: wp.array(dtype=int32),
contact_model_num_contacts: wp.array(dtype=int32),
contact_wid: wp.array(dtype=int32),
contact_cid: wp.array(dtype=int32),
contact_material: wp.array(dtype=vec2f),
contact_reaction: wp.array(dtype=vec3f),
contact_velocity: wp.array(dtype=vec3f),
problem_P: wp.array(dtype=float32),
# Outputs:
x_0: wp.array(dtype=float32),
y_0: wp.array(dtype=float32),
z_0: wp.array(dtype=float32),
):
# Retrieve the thread index as the contact index
cid = wp.tid()
# Retrieve the number of contacts active in the model
model_nc = contact_model_num_contacts[0]
# Skip if cid is greater than the number of contacts active in the model
if cid >= model_nc:
return
# Retrieve the contact-specific data
wid = contact_wid[cid]
cid_k = contact_cid[cid]
material_k = contact_material[cid]
lambda_k = contact_reaction[cid]
v_plus_k = contact_velocity[cid]
# Retrieve the world-specific info
dt = model_time_dt[wid]
total_cts_offset = model_info_total_cts_offset[wid]
contact_cts_offset = data_info_contact_cts_group_offset[wid]
# Compute block offsets of the contact constraints within
# the contact-only constraints and total constraints arrays
vio_k = total_cts_offset + contact_cts_offset + 3 * cid_k
# Load the diagonal preconditioner for the contact constraints
# NOTE: We only need to load the first element since by necessity
# the preconditioner is constant across the 3 constraint dimensions
P_k = problem_P[vio_k]
# Scale the contact force by the time-step to
# render an impulse and by the preconditioner
lambda_k *= dt / P_k
# Scale the contact velocity by the preconditioner
# and apply the De Saxce correction to the post-event
# contact velocity to render solver dual variables
v_plus_k *= P_k
mu_k = material_k[0]
vt_norm = wp.sqrt(v_plus_k.x * v_plus_k.x + v_plus_k.y * v_plus_k.y)
v_plus_k.z += mu_k * vt_norm
# Compute and store the contact-constraint reaction forces
for k in range(3):
x_0[vio_k + k] = lambda_k[k]
for k in range(3):
y_0[vio_k + k] = lambda_k[k]
for k in range(3):
z_0[vio_k + k] = v_plus_k[k]
def make_initialize_solver_kernel(use_acceleration: bool = False):
"""
Creates a kernel to initialize the PADMM solver state, status, and penalty parameters.
Specialized for whether acceleration is enabled to reduce unnecessary overhead and branching.
Args:
use_acceleration (bool, optional): Flag indicating whether acceleration is enabled. Defaults to False.
Returns:
wp.kernel: The kernel function to initialize the PADMM solver.
"""
@wp.kernel
def _initialize_solver(
# Inputs:
solver_config: wp.array(dtype=PADMMConfigStruct),
# Outputs:
solver_status: wp.array(dtype=PADMMStatus),
solver_penalty: wp.array(dtype=PADMMPenalty),
solver_state_sigma: wp.array(dtype=vec2f),
solver_state_a_p: wp.array(dtype=float32),
linear_solver_atol: wp.array(dtype=float32),
):
# Retrieve the world index as thread index
wid = wp.tid()
# Retrieve the per-world solver data
config = solver_config[wid]
status = solver_status[wid]
penalty = solver_penalty[wid]
sigma = solver_state_sigma[wid]
# Initialize solver status
status.iterations = int32(0)
status.converged = int32(0)
status.r_p = float32(0.0)
status.r_d = float32(0.0)
status.r_c = float32(0.0)
# NOTE: We initialize acceleration-related
# entries only if acceleration is enabled
if wp.static(use_acceleration):
status.r_dx = float32(0.0)
status.r_dy = float32(0.0)
status.r_dz = float32(0.0)
status.r_a = FLOAT32_MAX
status.r_a_p = FLOAT32_MAX
status.r_a_pp = FLOAT32_MAX
status.restart = int32(0)
status.num_restarts = int32(0)
# Initialize ALM penalty parameter and relevant meta-data
# NOTE: Currently only fixed penalty is used
penalty.rho = config.rho_0
penalty.rho_p = float32(0.0)
penalty.num_updates = int32(0)
# Initialize the total proximal regularization
sigma[0] = config.eta + config.rho_0
sigma[1] = float32(0.0)
# Store the initialized per-world solver data
solver_status[wid] = status
solver_penalty[wid] = penalty
solver_state_sigma[wid] = sigma
# Initialize the previous acclereration
# variables only if acceleration is used
if wp.static(use_acceleration):
solver_state_a_p[wid] = config.a_0
# Initialize the iterative solver tolerance
if linear_solver_atol:
linear_solver_atol[wid] = wp.where(
config.linear_solver_tolerance > 0.0, config.linear_solver_tolerance, FLOAT32_EPS
)
# Return the initialization kernel
return _initialize_solver
def make_update_proximal_regularization_kernel(method: PADMMPenaltyUpdate):
@wp.kernel
def _update_proximal_regularization(
# Inputs:
solver_config: wp.array(dtype=PADMMConfigStruct),
solver_penalty: wp.array(dtype=PADMMPenalty),
solver_status: wp.array(dtype=PADMMStatus),
# Outputs:
solver_state_sigma: wp.array(dtype=vec2f),
):
# Retrieve the world index from the thread index
wid = wp.tid()
# Retrieve the solver status
status = solver_status[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if status.converged > 0:
return
# Retrieve the solver parameters
cfg = solver_config[wid]
pen = solver_penalty[wid]
# Retrieve the (current, previous) proximal regularization pair
sigma = solver_state_sigma[wid]
# Extract the regularization parameters
rho = pen.rho
eta = cfg.eta
# TODO: Add penalty update methods here
# Update the diagonal proximal regularization
sigma[1] = sigma[0]
sigma[0] = eta + rho
# Return the proximal regularization update kernel
return _update_proximal_regularization
@wp.kernel
def _update_delassus_proximal_regularization(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_mio: wp.array(dtype=int32),
solver_status: wp.array(dtype=PADMMStatus),
solver_state_sigma: wp.array(dtype=vec2f),
# Outputs:
D: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the number of active constraints in the world
ncts = problem_dim[wid]
# Retrieve the solver status
status = solver_status[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if tid >= ncts or status.converged > 0:
return
# Retrieve the matrix index offset of the world
mio = problem_mio[wid]
# Retrieve the (current, previous) proximal regularization pair
sigma = solver_state_sigma[wid]
# Add the proximal regularization to the diagonal of the Delassus matrix
D[mio + ncts * tid + tid] += sigma[0] - sigma[1]
@wp.kernel
def _update_delassus_proximal_regularization_sparse(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
solver_config: wp.array(dtype=PADMMConfigStruct),
solver_penalty: wp.array(dtype=PADMMPenalty),
solver_status: wp.array(dtype=PADMMStatus),
# Outputs:
delassus_eta: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the number of active constraints in the world
ncts = problem_dim[wid]
# Retrieve the solver status
status = solver_status[wid]
# Skip if row index exceed the problem size
if tid >= ncts:
return
# Retrieve the vector index offset of the world
mio = problem_vio[wid]
# Set regularization to 0.0 if the solver has already converged
if status.converged > 0:
delassus_eta[mio + tid] = 0.0
return
# Set the proximal regularization term: eta + rho
delassus_eta[mio + tid] = solver_config[wid].eta + solver_penalty[wid].rho
@wp.kernel
def _compute_desaxce_correction(
# Inputs:
problem_nc: wp.array(dtype=int32),
problem_cio: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_mu: wp.array(dtype=float32),
solver_status: wp.array(dtype=PADMMStatus),
solver_z_p: wp.array(dtype=float32),
# Outputs:
solver_s: wp.array(dtype=float32),
):
# Retrieve the thread index as the contact index
wid, cid = wp.tid()
# Retrieve the number of contact active in the world
nc = problem_nc[wid]
# Retrieve the solver status
status = solver_status[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if cid >= nc or status.converged > 0:
return
# Retrieve the contacts index offset of the world
cio = problem_cio[wid]
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Retrieve the contact constraints group offset of the world
ccgo = problem_ccgo[wid]
# Compute the index offset of the corresponding contact constraint
ccio_k = vio + ccgo + 3 * cid
# Retrieve the contact index w.r.t the model
cio_k = cio + cid
# Compute the norm of the tangential components
vtx = solver_z_p[ccio_k]
vty = solver_z_p[ccio_k + 1]
vt_norm = wp.sqrt(vtx * vtx + vty * vty)
# Store De Saxce correction for this block
solver_s[ccio_k] = 0.0
solver_s[ccio_k + 1] = 0.0
solver_s[ccio_k + 2] = problem_mu[cio_k] * vt_norm
@wp.kernel
def _compute_velocity_bias(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_v_f: wp.array(dtype=float32),
solver_config: wp.array(dtype=PADMMConfigStruct),
solver_penalty: wp.array(dtype=PADMMPenalty),
solver_status: wp.array(dtype=PADMMStatus),
solver_s: wp.array(dtype=float32),
solver_x_p: wp.array(dtype=float32),
solver_y_p: wp.array(dtype=float32),
solver_z_p: wp.array(dtype=float32),
# Outputs:
solver_v: wp.array(dtype=float32),
):
# Retrieve the thread indices as the world and constraint index
wid, tid = wp.tid()
# Retrieve the total number of active constraints in the world
ncts = problem_dim[wid]
# Retrieve the solver status
status = solver_status[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if tid >= ncts or status.converged > 0:
return
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Retrieve solver parameters
eta = solver_config[wid].eta
rho = solver_penalty[wid].rho
# Compute the index offset of the vector block of the world
thread_offset = vio + tid
# Retrieve the solver state
v_f = problem_v_f[thread_offset]
s = solver_s[thread_offset]
x_p = solver_x_p[thread_offset]
y_p = solver_y_p[thread_offset]
z_p = solver_z_p[thread_offset]
# Compute the total velocity bias for the thread_offset-th constraint
solver_v[thread_offset] = -v_f - s + eta * x_p + rho * y_p + z_p
@wp.kernel
def _compute_projection_argument(
# Inputs
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
solver_penalty: wp.array(dtype=PADMMPenalty),
solver_status: wp.array(dtype=PADMMStatus),
solver_z_hat: wp.array(dtype=float32),
solver_x: wp.array(dtype=float32),
# Outputs
solver_y: wp.array(dtype=float32),
):
# Retrieve the thread indices as the world and constraint index
wid, tid = wp.tid()
# Retrieve the total number of active constraints in the world
ncts = problem_dim[wid]
# Retrieve the solver status
status = solver_status[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if tid >= ncts or status.converged > 0:
return
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Capture the ALM penalty
rho = solver_penalty[wid].rho
# Compute the index offset of the vector block of the world
thread_offset = vio + tid
# Retrieve the solver state variables
z_hat = solver_z_hat[thread_offset]
x = solver_x[thread_offset]
# Compute and store the updated values back to the solver state
solver_y[thread_offset] = x - (1.0 / rho) * z_hat
@wp.kernel
def _project_to_feasible_cone(
# Inputs:
problem_nl: wp.array(dtype=int32),
problem_nc: wp.array(dtype=int32),
problem_cio: wp.array(dtype=int32),
problem_lcgo: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_mu: wp.array(dtype=float32),
solver_status: wp.array(dtype=PADMMStatus),
# Outputs:
solver_y: wp.array(dtype=float32),
):
# Retrieve the thread index as the unilateral entity index
wid, uid = wp.tid()
# Retrieve the solver status
status = solver_status[wid]
# Retrieve the number of active limits and contacts in the world
nl = problem_nl[wid]
nc = problem_nc[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if uid >= (nl + nc) or status.converged > 0:
return
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Check if the thread should handle a limit
if nl > 0 and uid < nl:
# Retrieve the limit constraint group offset of the world
lcgo = problem_lcgo[wid]
# Compute the constraint index offset of the limit element
lcio_j = vio + lcgo + uid
# Project to the non-negative orthant
solver_y[lcio_j] = wp.max(solver_y[lcio_j], 0.0)
# Check if the thread should handle a contact
elif nc > 0 and uid >= nl:
# Retrieve the contact index offset of the world
cio = problem_cio[wid]
# Retrieve the limit constraint group offset of the world
ccgo = problem_ccgo[wid]
# Compute the index of the contact element in the unilaterals array
# NOTE: We need to subtract the number of active limits
cid = uid - nl
# Compute the index offset of the contact constraint
ccio_j = vio + ccgo + 3 * cid
# Capture a 3D vector
x = vec3f(solver_y[ccio_j], solver_y[ccio_j + 1], solver_y[ccio_j + 2])
# Project to the coulomb friction cone
y_proj = project_to_coulomb_cone(x, problem_mu[cio + cid])
# Copy vec3 projection into the slack variable array
solver_y[ccio_j] = y_proj[0]
solver_y[ccio_j + 1] = y_proj[1]
solver_y[ccio_j + 2] = y_proj[2]
def make_update_dual_variables_and_compute_primal_dual_residuals(use_acceleration: bool = False):
"""
Creates a kernel to update the dual variables and compute the primal and dual residuals.
Specialized for whether acceleration is enabled to reduce unnecessary overhead and branching.
Args:
use_acceleration (bool, optional): Flag indicating whether acceleration is enabled. Defaults to False.
Returns:
wp.kernel: The kernel function to update dual variables and compute residuals.
"""
@wp.kernel
def _update_dual_variables_and_compute_primal_dual_residuals(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_P: wp.array(dtype=float32),
solver_config: wp.array(dtype=PADMMConfigStruct),
solver_penalty: wp.array(dtype=PADMMPenalty),
solver_status: wp.array(dtype=PADMMStatus),
solver_x: wp.array(dtype=float32),
solver_y: wp.array(dtype=float32),
solver_x_p: wp.array(dtype=float32),
solver_y_p: wp.array(dtype=float32),
solver_z_p: wp.array(dtype=float32),
# Outputs:
solver_z: wp.array(dtype=float32),
solver_r_prim: wp.array(dtype=float32),
solver_r_dual: wp.array(dtype=float32),
solver_r_dx: wp.array(dtype=float32),
solver_r_dy: wp.array(dtype=float32),
solver_r_dz: wp.array(dtype=float32),
):
# Retrieve the thread indices as the world and constraint index
wid, tid = wp.tid()
# Retrieve the total number of active constraints in the world
ncts = problem_dim[wid]
# Retrieve the solver status
status = solver_status[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if tid >= ncts or status.converged > 0:
return
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Capture proximal parameter and the ALM penalty
eta = solver_config[wid].eta
rho = solver_penalty[wid].rho
# Compute the index offset of the vector block of the world
thread_offset = vio + tid
# Retrieve
P_i = problem_P[thread_offset]
# Retrieve the solver state inputs
x = solver_x[thread_offset]
y = solver_y[thread_offset]
x_p = solver_x_p[thread_offset]
y_p = solver_y_p[thread_offset]
z_p = solver_z_p[thread_offset]
# Compute and store the dual variable update
z = z_p + rho * (y - x)
solver_z[thread_offset] = z
# Compute the primal residual as the consensus of the primal and slack variable
solver_r_prim[thread_offset] = P_i * (x - y)
# Compute the dual residual using the ADMM-specific shortcut
solver_r_dual[thread_offset] = (1.0 / P_i) * (eta * (x - x_p) + rho * (y - y_p))
# Compute the individual iterate residuals only if acceleration is enabled
# NOTE: These are used to compute the combined residual
# for checking the acceleration restart criteria
if wp.static(use_acceleration):
solver_r_dx[thread_offset] = P_i * (x - x_p)
solver_r_dy[thread_offset] = P_i * (y - y_p)
solver_r_dz[thread_offset] = (1.0 / P_i) * (z - z_p)
# Return the dual update and residual computation kernel
return _update_dual_variables_and_compute_primal_dual_residuals
@wp.kernel
def _compute_complementarity_residuals(
# Inputs:
problem_nl: wp.array(dtype=int32),
problem_nc: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_uio: wp.array(dtype=int32),
problem_lcgo: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
solver_status: wp.array(dtype=PADMMStatus),
solver_x: wp.array(dtype=float32),
solver_z: wp.array(dtype=float32),
# Outputs:
solver_r_c: wp.array(dtype=float32),
):
# Retrieve the thread index as the unilateral entity index
wid, uid = wp.tid()
# Retrieve the solver status
status = solver_status[wid]
# Retrieve the number of active limits and contacts in the world
nl = problem_nl[wid]
nc = problem_nc[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if uid >= (nl + nc) or status.converged > 0:
return
# Retrieve the index offsets of the unilateral elements
uio = problem_uio[wid]
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Compute the index offset of the vector block of the world
uio_u = uio + uid
# Check if the thread should handle a limit
if nl > 0 and uid < nl:
# Retrieve the limit constraint group offset of the world
lcgo = problem_lcgo[wid]
# Compute the constraint index offset of the limit element
lcio_j = vio + lcgo + uid
# Compute the scalar product of the primal and dual variables
solver_r_c[uio_u] = solver_x[lcio_j] * solver_z[lcio_j]
# Check if the thread should handle a contact
elif nc > 0 and uid >= nl:
# Retrieve the limit constraint group offset of the world
ccgo = problem_ccgo[wid]
# Compute the index of the contact element in the unilaterals array
# NOTE: We need to subtract the number of active limits
cid = uid - nl
# Compute the index offset of the contact constraint
ccio_j = vio + ccgo + 3 * cid
# Capture 3D vectors
x_c = vec3f(solver_x[ccio_j], solver_x[ccio_j + 1], solver_x[ccio_j + 2])
z_c = vec3f(solver_z[ccio_j], solver_z[ccio_j + 1], solver_z[ccio_j + 2])
# Compute the inner product of the primal and dual variables
solver_r_c[uio_u] = wp.dot(x_c, z_c)
@wp.func
def _update_penalty(config: PADMMConfigStruct, pen: PADMMPenalty, iterations: int32, r_p: float32, r_d: float32):
"""Adaptively updates the ADMM penalty parameter based on the configured strategy.
For BALANCED mode, rho is scaled up when the primal residual dominates and
scaled down (to ``config.rho_min``) when the dual residual dominates, every
``config.penalty_update_freq`` iterations. For FIXED mode this is a no-op.
"""
if config.penalty_update_method == int32(PADMMPenaltyUpdate.BALANCED):
freq = config.penalty_update_freq
if freq > 0 and iterations > 0 and iterations % freq == 0:
rho = pen.rho
if r_p > config.alpha * r_d:
rho = rho * config.tau
elif r_d > config.alpha * r_p:
rho = wp.max(rho / config.tau, config.rho_min)
pen.rho = rho
pen.num_updates += int32(1)
return pen
@wp.func
def less_than_op(i: wp.int32, threshold: wp.int32) -> wp.float32:
return 1.0 if i < threshold else 0.0
@wp.func
def mul_mask(mask: Any, value: Any):
"""Return value if mask is positive, else 0"""
return wp.where(mask > type(mask)(0), value, type(value)(0))
@functools.cache
def _make_compute_infnorm_residuals_kernel(tile_size: int, n_cts_max: int, n_u_max: int):
num_tiles_cts = (n_cts_max + tile_size - 1) // tile_size
num_tiles_u = (n_u_max + tile_size - 1) // tile_size
@wp.kernel
def _compute_infnorm_residuals(
# Inputs:
problem_nl: wp.array(dtype=int32),
problem_nc: wp.array(dtype=int32),
problem_uio: wp.array(dtype=int32),
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
solver_config: wp.array(dtype=PADMMConfigStruct),
solver_r_p: wp.array(dtype=float32),
solver_r_d: wp.array(dtype=float32),
solver_r_c: wp.array(dtype=float32),
# Outputs:
solver_state_done: wp.array(dtype=int32),
solver_status: wp.array(dtype=PADMMStatus),
solver_penalty: wp.array(dtype=PADMMPenalty),
linear_solver_atol: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index + thread index within block
wid, tid = wp.tid()
# Retrieve the solver status
status = solver_status[wid]
# Skip this step if already converged
if status.converged:
return
# Update iteration counter
status.iterations += 1
# Capture the size of the residuals arrays
nl = problem_nl[wid]
nc = problem_nc[wid]
ncts = problem_dim[wid]
# Retrieve the solver configurations
config = solver_config[wid]
# Retrieve the index offsets of the vector block and unilateral elements
vio = problem_vio[wid]
uio = problem_uio[wid]
# Extract the solver tolerances
eps_p = config.primal_tolerance
eps_d = config.dual_tolerance
eps_c = config.compl_tolerance
# Extract the maximum number of iterations
maxiters = config.max_iterations
# Compute element-wise max over each residual vector to compute the infinity-norm
r_p_max = float32(0.0)
r_d_max = float32(0.0)
if wp.static(num_tiles_cts > 1):
r_p_max_acc = wp.tile_zeros(num_tiles_cts, dtype=float32, storage="shared")
r_d_max_acc = wp.tile_zeros(num_tiles_cts, dtype=float32, storage="shared")
for tile_id in range(num_tiles_cts):
ct_id_tile = tile_id * tile_size
if ct_id_tile >= ncts:
break
rio_tile = vio + ct_id_tile
# Mask out extra entries in case of heterogenous worlds
need_mask = ct_id_tile > ncts - tile_size
if need_mask:
mask = wp.tile_map(less_than_op, wp.tile_arange(tile_size, dtype=int32), ncts - ct_id_tile)
tile = wp.tile_load(solver_r_p, shape=tile_size, offset=rio_tile)
tile = wp.tile_map(wp.abs, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_cts > 1):
r_p_max_acc[tile_id] = wp.tile_max(tile)[0]
else:
r_p_max = wp.tile_max(tile)[0]
tile = wp.tile_load(solver_r_d, shape=tile_size, offset=rio_tile)
tile = wp.tile_map(wp.abs, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_cts > 1):
r_d_max_acc[tile_id] = wp.tile_max(tile)[0]
else:
r_d_max = wp.tile_max(tile)[0]
if wp.static(num_tiles_cts > 1):
r_p_max = wp.tile_max(r_p_max_acc)[0]
r_d_max = wp.tile_max(r_d_max_acc)[0]
# Compute the infinity-norm of the complementarity residuals
nu = nl + nc
r_c_max = float32(0.0)
if wp.static(num_tiles_u > 1):
r_c_max_acc = wp.tile_zeros(num_tiles_u, dtype=float32, storage="shared")
for tile_id in range(num_tiles_u):
u_id_tile = tile_id * tile_size
if u_id_tile >= nu:
break
uio_tile = uio + u_id_tile
# Mask out extra entries in case of heterogenous worlds
need_mask = u_id_tile > nu - tile_size
if need_mask:
mask = wp.tile_map(less_than_op, wp.tile_arange(tile_size, dtype=int32), nu - u_id_tile)
tile = wp.tile_load(solver_r_c, shape=tile_size, offset=uio_tile)
tile = wp.tile_map(wp.abs, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_u > 1):
r_c_max_acc[tile_id] = wp.tile_max(tile)[0]
else:
r_c_max = wp.tile_max(tile)[0]
if wp.static(num_tiles_u > 1):
r_c_max = wp.tile_max(r_c_max_acc)[0]
if tid == 0:
# Store the scalar metric residuals in the solver status
status.r_p = r_p_max
status.r_d = r_d_max
status.r_c = r_c_max
# Check and store convergence state
if status.iterations > 1 and r_p_max <= eps_p and r_d_max <= eps_d and r_c_max <= eps_c:
status.converged = 1
# If converged or reached max iterations, decrement the number of active worlds
if status.converged or status.iterations >= maxiters:
solver_state_done[0] -= 1
# Store the updated status
solver_status[wid] = status
# Adaptive penalty update
solver_penalty[wid] = _update_penalty(config, solver_penalty[wid], status.iterations, r_p_max, r_d_max)
return _compute_infnorm_residuals
@functools.cache
def _make_compute_infnorm_residuals_accel_kernel(tile_size: int, n_cts_max: int, n_u_max: int):
num_tiles_cts = (n_cts_max + tile_size - 1) // tile_size
num_tiles_u = (n_u_max + tile_size - 1) // tile_size
@wp.kernel
def _compute_infnorm_residuals_accel(
# Inputs:
problem_nl: wp.array(dtype=int32),
problem_nc: wp.array(dtype=int32),
problem_uio: wp.array(dtype=int32),
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
solver_config: wp.array(dtype=PADMMConfigStruct),
solver_r_p: wp.array(dtype=float32),
solver_r_d: wp.array(dtype=float32),
solver_r_c: wp.array(dtype=float32),
solver_r_dx: wp.array(dtype=float32),
solver_r_dy: wp.array(dtype=float32),
solver_r_dz: wp.array(dtype=float32),
solver_state_a_p: wp.array(dtype=float32),
# Outputs:
solver_state_done: wp.array(dtype=int32),
solver_state_a: wp.array(dtype=float32),
solver_status: wp.array(dtype=PADMMStatus),
solver_penalty: wp.array(dtype=PADMMPenalty),
linear_solver_atol: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index + thread index within block
wid, tid = wp.tid()
# Retrieve the solver status
status = solver_status[wid]
# Skip this step if already converged
if status.converged:
return
# Update iteration counter
status.iterations += 1
# Capture the size of the residuals arrays
nl = problem_nl[wid]
nc = problem_nc[wid]
ncts = problem_dim[wid]
# Retrieve the solver configurations
config = solver_config[wid]
# Retrieve the index offsets of the vector block and unilateral elements
vio = problem_vio[wid]
uio = problem_uio[wid]
# Extract the solver tolerances
eps_p = config.primal_tolerance
eps_d = config.dual_tolerance
eps_c = config.compl_tolerance
# Extract the maximum number of iterations
maxiters = config.max_iterations
# Extract the penalty parameters
pen = solver_penalty[wid]
rho = pen.rho
# Compute element-wise max over each residual vector to compute the infinity-norm
r_p_max = float32(0.0)
r_d_max = float32(0.0)
r_dx_l2_sum = float32(0.0)
r_dy_l2_sum = float32(0.0)
r_dz_l2_sum = float32(0.0)
if wp.static(num_tiles_cts > 1):
r_p_max_acc = wp.tile_zeros(num_tiles_cts, dtype=float32, storage="shared")
r_d_max_acc = wp.tile_zeros(num_tiles_cts, dtype=float32, storage="shared")
r_dx_l2_sum_acc = wp.tile_zeros(num_tiles_cts, dtype=float32, storage="shared")
r_dy_l2_sum_acc = wp.tile_zeros(num_tiles_cts, dtype=float32, storage="shared")
r_dz_l2_sum_acc = wp.tile_zeros(num_tiles_cts, dtype=float32, storage="shared")
for tile_id in range(num_tiles_cts):
ct_id_tile = tile_id * tile_size
if ct_id_tile >= ncts:
break
rio_tile = vio + ct_id_tile
# Mask out extra entries in case of heterogenous worlds
need_mask = ct_id_tile > ncts - tile_size
if need_mask:
mask = wp.tile_map(less_than_op, wp.tile_arange(tile_size, dtype=int32), ncts - ct_id_tile)
tile = wp.tile_load(solver_r_p, shape=tile_size, offset=rio_tile)
tile = wp.tile_map(wp.abs, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_cts > 1):
r_p_max_acc[tile_id] = wp.tile_max(tile)[0]
else:
r_p_max = wp.tile_max(tile)[0]
tile = wp.tile_load(solver_r_d, shape=tile_size, offset=rio_tile)
tile = wp.tile_map(wp.abs, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_cts > 1):
r_d_max_acc[tile_id] = wp.tile_max(tile)[0]
else:
r_d_max = wp.tile_max(tile)[0]
tile = wp.tile_load(solver_r_dx, shape=tile_size, offset=rio_tile)
tile = wp.tile_map(wp.mul, tile, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_cts > 1):
r_dx_l2_sum_acc[tile_id] = wp.tile_sum(tile)[0]
else:
r_dx_l2_sum = wp.tile_sum(tile)[0]
tile = wp.tile_load(solver_r_dy, shape=tile_size, offset=rio_tile)
tile = wp.tile_map(wp.mul, tile, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_cts > 1):
r_dy_l2_sum_acc[tile_id] = wp.tile_sum(tile)[0]
else:
r_dy_l2_sum = wp.tile_sum(tile)[0]
tile = wp.tile_load(solver_r_dz, shape=tile_size, offset=rio_tile)
tile = wp.tile_map(wp.mul, tile, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_cts > 1):
r_dz_l2_sum_acc[tile_id] = wp.tile_sum(tile)[0]
else:
r_dz_l2_sum = wp.tile_sum(tile)[0]
if wp.static(num_tiles_cts > 1):
r_p_max = wp.tile_max(r_p_max_acc)[0]
r_d_max = wp.tile_max(r_d_max_acc)[0]
r_dx_l2_sum = wp.tile_sum(r_dx_l2_sum_acc)[0]
r_dy_l2_sum = wp.tile_sum(r_dy_l2_sum_acc)[0]
r_dz_l2_sum = wp.tile_sum(r_dz_l2_sum_acc)[0]
# Compute the infinity-norm of the complementarity residuals
nu = nl + nc
r_c_max = float32(0.0)
if wp.static(num_tiles_u > 1):
r_c_max_acc = wp.tile_zeros(num_tiles_u, dtype=float32, storage="shared")
for tile_id in range(num_tiles_u):
u_id_tile = tile_id * tile_size
if u_id_tile >= nu:
break
uio_tile = uio + u_id_tile
# Mask out extra entries in case of heterogenous worlds
need_mask = u_id_tile > nu - tile_size
if need_mask:
mask = wp.tile_map(less_than_op, wp.tile_arange(tile_size, dtype=int32), nu - u_id_tile)
tile = wp.tile_load(solver_r_c, shape=tile_size, offset=uio_tile)
tile = wp.tile_map(wp.abs, tile)
if need_mask:
tile = wp.tile_map(mul_mask, mask, tile)
if wp.static(num_tiles_u > 1):
r_c_max_acc[tile_id] = wp.tile_max(tile)[0]
else:
r_c_max = wp.tile_max(tile)[0]
if wp.static(num_tiles_u > 1):
r_c_max = wp.tile_max(r_c_max_acc)[0]
if tid == 0:
# Store the scalar metric residuals in the solver status
status.r_p = r_p_max
status.r_d = r_d_max
status.r_c = r_c_max
status.r_dx = wp.sqrt(r_dx_l2_sum)
status.r_dy = wp.sqrt(r_dy_l2_sum)
status.r_dz = wp.sqrt(r_dz_l2_sum)
status.r_a = rho * status.r_dy + (1.0 / rho) * status.r_dz
# Check and store convergence state
if status.iterations > 1 and r_p_max <= eps_p and r_d_max <= eps_d and r_c_max <= eps_c:
status.converged = 1
# If converged or reached max iterations, decrement the number of active worlds
if status.converged or status.iterations >= maxiters:
solver_state_done[0] -= 1
# Restart acceleration if the residuals are not decreasing sufficiently
# TODO: Use a warp function that is wrapped with wp.static for conditionally compiling this
if status.r_a < config.restart_tolerance * status.r_a_p:
status.restart = 0
a_p = solver_state_a_p[wid]
solver_state_a[wid] = (1.0 + wp.sqrt(1.0 + 4.0 * a_p * a_p)) / 2.0
else:
status.restart = 1
status.num_restarts += 1
status.r_a = status.r_a_p / config.restart_tolerance
solver_state_a[wid] = float(config.a_0)
status.r_a_pp = status.r_a_p
status.r_a_p = status.r_a
# Store the updated status
solver_status[wid] = status
# Adaptive penalty update
solver_penalty[wid] = _update_penalty(config, solver_penalty[wid], status.iterations, r_p_max, r_d_max)
return _compute_infnorm_residuals_accel
@wp.kernel
def _update_state_with_acceleration(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
solver_status: wp.array(dtype=PADMMStatus),
solver_state_a: wp.array(dtype=float32),
solver_state_y: wp.array(dtype=float32),
solver_state_z: wp.array(dtype=float32),
solver_state_a_p: wp.array(dtype=float32),
solver_state_y_p: wp.array(dtype=float32),
solver_state_z_p: wp.array(dtype=float32),
# Outputs:
solver_state_y_hat: wp.array(dtype=float32),
solver_state_z_hat: wp.array(dtype=float32),
):
# Retrieve the thread indices as the world and constraint index
wid, tid = wp.tid()
# Retrieve the total number of active constraints in the world
ncts = problem_dim[wid]
# Retrieve the solver status
status = solver_status[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if tid >= ncts or status.converged > 0:
return
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Compute the index offset of the vector block of the world
vid = vio + tid
# Only apply acceleration if not restarting
if status.restart == 0:
# Retrieve the previous and current states
a = solver_state_a[wid]
y = solver_state_y[vid]
z = solver_state_z[vid]
a_p = solver_state_a_p[wid]
y_p = solver_state_y_p[vid]
z_p = solver_state_z_p[vid]
# Compute the current acceleration factor
factor = (a_p - 1.0) / a
# Update the primal and dual variables with Nesterov acceleration
y_hat_new = y + factor * (y - y_p)
z_hat_new = z + factor * (z - z_p)
# Store the updated states
solver_state_y_hat[vid] = y_hat_new
solver_state_z_hat[vid] = z_hat_new
else:
# If restarting, reset the auxiliary primal-dual state to the previous-step values
solver_state_y_hat[vid] = solver_state_y_p[vid]
solver_state_z_hat[vid] = solver_state_z_p[vid]
def make_collect_solver_info_kernel(use_acceleration: bool):
"""
Creates a kernel to collect solver convergence information after each iteration.
Specializes the kernel based on whether acceleration is enabled to reduce unnecessary overhead.
Args:
use_acceleration (bool): Whether acceleration is enabled in the solver.
Returns:
wp.kernel: The kernel function to collect solver convergence information.
"""
@wp.kernel
def _collect_solver_convergence_info(
# Inputs:
problem_nl: wp.array(dtype=int32),
problem_nc: wp.array(dtype=int32),
problem_cio: wp.array(dtype=int32),
problem_lcgo: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_mio: wp.array(dtype=int32),
problem_mu: wp.array(dtype=float32),
problem_v_f: wp.array(dtype=float32),
problem_D: wp.array(dtype=float32),
problem_P: wp.array(dtype=float32),
solver_state_sigma: wp.array(dtype=vec2f),
solver_state_s: wp.array(dtype=float32),
solver_state_x: wp.array(dtype=float32),
solver_state_x_p: wp.array(dtype=float32),
solver_state_y: wp.array(dtype=float32),
solver_state_y_p: wp.array(dtype=float32),
solver_state_z: wp.array(dtype=float32),
solver_state_z_p: wp.array(dtype=float32),
solver_state_a: wp.array(dtype=float32),
solver_penalty: wp.array(dtype=PADMMPenalty),
solver_status: wp.array(dtype=PADMMStatus),
# Outputs:
solver_info_lambdas: wp.array(dtype=float32),
solver_info_v_plus: wp.array(dtype=float32),
solver_info_v_aug: wp.array(dtype=float32),
solver_info_s: wp.array(dtype=float32),
solver_info_offset: wp.array(dtype=int32),
solver_info_num_restarts: wp.array(dtype=int32),
solver_info_num_rho_updates: wp.array(dtype=int32),
solver_info_a: wp.array(dtype=float32),
solver_info_norm_s: wp.array(dtype=float32),
solver_info_norm_x: wp.array(dtype=float32),
solver_info_norm_y: wp.array(dtype=float32),
solver_info_norm_z: wp.array(dtype=float32),
solver_info_f_ccp: wp.array(dtype=float32),
solver_info_f_ncp: wp.array(dtype=float32),
solver_info_r_dx: wp.array(dtype=float32),
solver_info_r_dy: wp.array(dtype=float32),
solver_info_r_dz: wp.array(dtype=float32),
solver_info_r_primal: wp.array(dtype=float32),
solver_info_r_dual: wp.array(dtype=float32),
solver_info_r_compl: wp.array(dtype=float32),
solver_info_r_pd: wp.array(dtype=float32),
solver_info_r_dp: wp.array(dtype=float32),
solver_info_r_comb: wp.array(dtype=float32),
solver_info_r_comb_ratio: wp.array(dtype=float32),
solver_info_r_ncp_primal: wp.array(dtype=float32),
solver_info_r_ncp_dual: wp.array(dtype=float32),
solver_info_r_ncp_compl: wp.array(dtype=float32),
solver_info_r_ncp_natmap: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index
wid = wp.tid()
# Retrieve the world-specific data
nl = problem_nl[wid]
nc = problem_nc[wid]
ncts = problem_dim[wid]
cio = problem_cio[wid]
lcgo = problem_lcgo[wid]
ccgo = problem_ccgo[wid]
vio = problem_vio[wid]
mio = problem_mio[wid]
rio = solver_info_offset[wid]
penalty = solver_penalty[wid]
status = solver_status[wid]
sigma = solver_state_sigma[wid]
# Retrieve parameters
iter = status.iterations - 1
# Compute additional info
njc = ncts - (nl + 3 * nc)
# Compute and store the norms of the current solution state
norm_s = compute_l2_norm(ncts, vio, solver_state_s)
norm_x = compute_l2_norm(ncts, vio, solver_state_x)
norm_y = compute_l2_norm(ncts, vio, solver_state_y)
norm_z = compute_l2_norm(ncts, vio, solver_state_z)
# Compute (division safe) residual ratios
r_pd = status.r_p / (status.r_d + FLOAT32_EPS)
r_dp = status.r_d / (status.r_p + FLOAT32_EPS)
# Remove preconditioning from lambdas
compute_cwise_vec_mul(ncts, vio, problem_P, solver_state_y, solver_info_lambdas)
# Compute the post-event constraint-space velocity from the current solution: v_plus = v_f + D @ lambda
compute_gemv(ncts, vio, mio, sigma[0], problem_P, problem_D, solver_state_y, problem_v_f, solver_info_v_plus)
# Compute the De Saxce correction for each contact as: s = G(v_plus)
compute_desaxce_corrections(nc, cio, vio, ccgo, problem_mu, solver_info_v_plus, solver_info_s)
# Compute the CCP optimization objective as: f_ccp = 0.5 * lambda.dot(v_plus + v_f)
f_ccp = 0.5 * compute_double_dot_product(ncts, vio, solver_info_lambdas, solver_info_v_plus, problem_v_f)
# Compute the NCP optimization objective as: f_ncp = f_ccp + lambda.dot(s)
f_ncp = compute_dot_product(ncts, vio, solver_info_lambdas, solver_info_s)
f_ncp += f_ccp
# Compute the augmented post-event constraint-space velocity as: v_aug = v_plus + s
compute_vector_sum(ncts, vio, solver_info_v_plus, solver_info_s, solver_info_v_aug)
# Compute the NCP primal residual as: r_p := || lambda - proj_K(lambda) ||_inf
r_ncp_p, _ = compute_ncp_primal_residual(nl, nc, vio, lcgo, ccgo, cio, problem_mu, solver_info_lambdas)
# Compute the NCP dual residual as: r_d := || v_plus + s - proj_dual_K(v_plus + s) ||_inf
r_ncp_d, _ = compute_ncp_dual_residual(njc, nl, nc, vio, lcgo, ccgo, cio, problem_mu, solver_info_v_aug)
# Compute the NCP complementarity (lambda _|_ (v_plus + s)) residual as r_c := || lambda.dot(v_plus + s) ||_inf
r_ncp_c, _ = compute_ncp_complementarity_residual(
nl, nc, vio, lcgo, ccgo, solver_info_v_aug, solver_info_lambdas
)
# Compute the natural-map residuals as: r_natmap = || lambda - proj_K(lambda - (v + s)) ||_inf
r_ncp_natmap, _ = compute_ncp_natural_map_residual(
nl, nc, vio, lcgo, ccgo, cio, problem_mu, solver_info_v_aug, solver_info_lambdas
)
# Compute the iterate residual as: r_iter := || y - y_p ||_inf
r_dx = compute_preconditioned_iterate_residual(ncts, vio, problem_P, solver_state_x, solver_state_x_p)
r_dy = compute_preconditioned_iterate_residual(ncts, vio, problem_P, solver_state_y, solver_state_y_p)
r_dz = compute_inverse_preconditioned_iterate_residual(ncts, vio, problem_P, solver_state_z, solver_state_z_p)
# Compute index offset for the info of the current iteration
iio = rio + iter
# Store the convergence information in the solver info arrays
solver_info_num_rho_updates[iio] = penalty.num_updates
solver_info_norm_s[iio] = norm_s
solver_info_norm_x[iio] = norm_x
solver_info_norm_y[iio] = norm_y
solver_info_norm_z[iio] = norm_z
solver_info_r_dx[iio] = r_dx
solver_info_r_dy[iio] = r_dy
solver_info_r_dz[iio] = r_dz
solver_info_r_primal[iio] = status.r_p
solver_info_r_dual[iio] = status.r_d
solver_info_r_compl[iio] = status.r_c
solver_info_r_pd[iio] = r_pd
solver_info_r_dp[iio] = r_dp
solver_info_r_ncp_primal[iio] = r_ncp_p
solver_info_r_ncp_dual[iio] = r_ncp_d
solver_info_r_ncp_compl[iio] = r_ncp_c
solver_info_r_ncp_natmap[iio] = r_ncp_natmap
solver_info_f_ccp[iio] = f_ccp
solver_info_f_ncp[iio] = f_ncp
# Optionally store acceleration-relevant info if acceleration is enabled
# NOTE: This is statically evaluated to avoid unnecessary overhead when acceleration is disabled
if wp.static(use_acceleration):
solver_info_a[iio] = solver_state_a[wid]
solver_info_r_comb[iio] = status.r_a
solver_info_r_comb_ratio[iio] = status.r_a / (status.r_a_pp)
solver_info_num_restarts[iio] = status.num_restarts
# Return the generated kernel
return _collect_solver_convergence_info
def make_collect_solver_info_kernel_sparse(use_acceleration: bool):
"""
Creates a kernel to collect solver convergence information after each iteration.
Specializes the kernel based on whether acceleration is enabled to reduce unnecessary overhead.
Args:
use_acceleration (bool): Whether acceleration is enabled in the solver.
Returns:
wp.kernel: The kernel function to collect solver convergence information.
"""
@wp.kernel
def _collect_solver_convergence_info_sparse(
# Inputs:
problem_nl: wp.array(dtype=int32),
problem_nc: wp.array(dtype=int32),
problem_cio: wp.array(dtype=int32),
problem_lcgo: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_mu: wp.array(dtype=float32),
problem_v_f: wp.array(dtype=float32),
problem_P: wp.array(dtype=float32),
solver_state_s: wp.array(dtype=float32),
solver_state_x: wp.array(dtype=float32),
solver_state_x_p: wp.array(dtype=float32),
solver_state_y: wp.array(dtype=float32),
solver_state_y_p: wp.array(dtype=float32),
solver_state_z: wp.array(dtype=float32),
solver_state_z_p: wp.array(dtype=float32),
solver_state_a: wp.array(dtype=float32),
solver_penalty: wp.array(dtype=PADMMPenalty),
solver_status: wp.array(dtype=PADMMStatus),
# Outputs:
solver_info_lambdas: wp.array(dtype=float32),
solver_info_v_plus: wp.array(dtype=float32),
solver_info_v_aug: wp.array(dtype=float32),
solver_info_s: wp.array(dtype=float32),
solver_info_offset: wp.array(dtype=int32),
solver_info_num_restarts: wp.array(dtype=int32),
solver_info_num_rho_updates: wp.array(dtype=int32),
solver_info_a: wp.array(dtype=float32),
solver_info_norm_s: wp.array(dtype=float32),
solver_info_norm_x: wp.array(dtype=float32),
solver_info_norm_y: wp.array(dtype=float32),
solver_info_norm_z: wp.array(dtype=float32),
solver_info_f_ccp: wp.array(dtype=float32),
solver_info_f_ncp: wp.array(dtype=float32),
solver_info_r_dx: wp.array(dtype=float32),
solver_info_r_dy: wp.array(dtype=float32),
solver_info_r_dz: wp.array(dtype=float32),
solver_info_r_primal: wp.array(dtype=float32),
solver_info_r_dual: wp.array(dtype=float32),
solver_info_r_compl: wp.array(dtype=float32),
solver_info_r_pd: wp.array(dtype=float32),
solver_info_r_dp: wp.array(dtype=float32),
solver_info_r_comb: wp.array(dtype=float32),
solver_info_r_comb_ratio: wp.array(dtype=float32),
solver_info_r_ncp_primal: wp.array(dtype=float32),
solver_info_r_ncp_dual: wp.array(dtype=float32),
solver_info_r_ncp_compl: wp.array(dtype=float32),
solver_info_r_ncp_natmap: wp.array(dtype=float32),
):
# Retrieve the thread index as the world index
wid = wp.tid()
# Retrieve the world-specific data
nl = problem_nl[wid]
nc = problem_nc[wid]
ncts = problem_dim[wid]
cio = problem_cio[wid]
lcgo = problem_lcgo[wid]
ccgo = problem_ccgo[wid]
vio = problem_vio[wid]
rio = solver_info_offset[wid]
penalty = solver_penalty[wid]
status = solver_status[wid]
# Retrieve parameters
iter = status.iterations - 1
# Compute additional info
njc = ncts - (nl + 3 * nc)
# Compute and store the norms of the current solution state
norm_s = compute_l2_norm(ncts, vio, solver_state_s)
norm_x = compute_l2_norm(ncts, vio, solver_state_x)
norm_y = compute_l2_norm(ncts, vio, solver_state_y)
norm_z = compute_l2_norm(ncts, vio, solver_state_z)
# Compute (division safe) residual ratios
r_pd = status.r_p / (status.r_d + FLOAT32_EPS)
r_dp = status.r_d / (status.r_p + FLOAT32_EPS)
# Remove preconditioning from lambdas
compute_cwise_vec_mul(ncts, vio, problem_P, solver_state_y, solver_info_lambdas)
# Remove preconditioning from v_plus
compute_cwise_vec_div(ncts, vio, solver_info_v_plus, problem_P, solver_info_v_plus)
# Compute the De Saxce correction for each contact as: s = G(v_plus)
compute_desaxce_corrections(nc, cio, vio, ccgo, problem_mu, solver_info_v_plus, solver_info_s)
# Compute the CCP optimization objective as: f_ccp = 0.5 * lambda.dot(v_plus + v_f)
f_ccp = 0.5 * compute_double_dot_product(ncts, vio, solver_info_lambdas, solver_info_v_plus, problem_v_f)
# Compute the NCP optimization objective as: f_ncp = f_ccp + lambda.dot(s)
f_ncp = compute_dot_product(ncts, vio, solver_info_lambdas, solver_info_s)
f_ncp += f_ccp
# Compute the augmented post-event constraint-space velocity as: v_aug = v_plus + s
compute_vector_sum(ncts, vio, solver_info_v_plus, solver_info_s, solver_info_v_aug)
# Compute the NCP primal residual as: r_p := || lambda - proj_K(lambda) ||_inf
r_ncp_p, _ = compute_ncp_primal_residual(nl, nc, vio, lcgo, ccgo, cio, problem_mu, solver_info_lambdas)
# Compute the NCP dual residual as: r_d := || v_plus + s - proj_dual_K(v_plus + s) ||_inf
r_ncp_d, _ = compute_ncp_dual_residual(njc, nl, nc, vio, lcgo, ccgo, cio, problem_mu, solver_info_v_aug)
# Compute the NCP complementarity (lambda _|_ (v_plus + s)) residual as r_c := || lambda.dot(v_plus + s) ||_inf
r_ncp_c, _ = compute_ncp_complementarity_residual(
nl, nc, vio, lcgo, ccgo, solver_info_v_aug, solver_info_lambdas
)
# Compute the natural-map residuals as: r_natmap = || lambda - proj_K(lambda - (v + s)) ||_inf
r_ncp_natmap, _ = compute_ncp_natural_map_residual(
nl, nc, vio, lcgo, ccgo, cio, problem_mu, solver_info_v_aug, solver_info_lambdas
)
# Compute the iterate residual as: r_iter := || y - y_p ||_inf
r_dx = compute_preconditioned_iterate_residual(ncts, vio, problem_P, solver_state_x, solver_state_x_p)
r_dy = compute_preconditioned_iterate_residual(ncts, vio, problem_P, solver_state_y, solver_state_y_p)
r_dz = compute_inverse_preconditioned_iterate_residual(ncts, vio, problem_P, solver_state_z, solver_state_z_p)
# Compute index offset for the info of the current iteration
iio = rio + iter
# Store the convergence information in the solver info arrays
solver_info_num_rho_updates[iio] = penalty.num_updates
solver_info_norm_s[iio] = norm_s
solver_info_norm_x[iio] = norm_x
solver_info_norm_y[iio] = norm_y
solver_info_norm_z[iio] = norm_z
solver_info_r_dx[iio] = r_dx
solver_info_r_dy[iio] = r_dy
solver_info_r_dz[iio] = r_dz
solver_info_r_primal[iio] = status.r_p
solver_info_r_dual[iio] = status.r_d
solver_info_r_compl[iio] = status.r_c
solver_info_r_pd[iio] = r_pd
solver_info_r_dp[iio] = r_dp
solver_info_r_ncp_primal[iio] = r_ncp_p
solver_info_r_ncp_dual[iio] = r_ncp_d
solver_info_r_ncp_compl[iio] = r_ncp_c
solver_info_r_ncp_natmap[iio] = r_ncp_natmap
solver_info_f_ccp[iio] = f_ccp
solver_info_f_ncp[iio] = f_ncp
# Optionally store acceleration-relevant info if acceleration is enabled
# NOTE: This is statically evaluated to avoid unnecessary overhead when acceleration is disabled
if wp.static(use_acceleration):
solver_info_a[iio] = solver_state_a[wid]
solver_info_r_comb[iio] = status.r_a
solver_info_r_comb_ratio[iio] = status.r_a / (status.r_a_pp)
solver_info_num_restarts[iio] = status.num_restarts
# Return the generated kernel
return _collect_solver_convergence_info_sparse
@wp.kernel
def _apply_dual_preconditioner_to_state(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_P: wp.array(dtype=float32),
# Outputs:
solver_x: wp.array(dtype=float32),
solver_y: wp.array(dtype=float32),
solver_z: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the number of active constraints in the world
ncts = problem_dim[wid]
# Skip if row index exceed the problem size
if tid >= ncts:
return
# Retrieve the vector index offset of the world
vio = problem_vio[wid]
# Compute the global index of the vector entry
v_i = vio + tid
# Retrieve the i-th entries of the target vectors
x_i = solver_x[v_i]
y_i = solver_y[v_i]
z_i = solver_z[v_i]
# Retrieve the i-th entry of the diagonal preconditioner
P_i = problem_P[v_i]
# Store the preconditioned i-th entry of the vector
solver_x[v_i] = P_i * x_i
solver_y[v_i] = P_i * y_i
solver_z[v_i] = (1.0 / P_i) * z_i
@wp.kernel
def _apply_dual_preconditioner_to_solution(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_P: wp.array(dtype=float32),
# Outputs:
solution_lambdas: wp.array(dtype=float32),
solution_v_plus: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the number of active constraints in the world
ncts = problem_dim[wid]
# Skip if row index exceed the problem size
if tid >= ncts:
return
# Retrieve the vector index offset of the world
vio = problem_vio[wid]
# Compute the global index of the vector entry
v_i = vio + tid
# Retrieve the i-th entries of the target vectors
lambdas_i = solution_lambdas[v_i]
v_plus_i = solution_v_plus[v_i]
# Retrieve the i-th entry of the diagonal preconditioner
P_i = problem_P[v_i]
# Store the preconditioned i-th entry of the vector
solution_lambdas[v_i] = (1.0 / P_i) * lambdas_i
solution_v_plus[v_i] = P_i * v_plus_i
@wp.kernel
def _compute_final_desaxce_correction(
problem_nc: wp.array(dtype=int32),
problem_cio: wp.array(dtype=int32),
problem_ccgo: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
problem_mu: wp.array(dtype=float32),
solver_z: wp.array(dtype=float32),
# Outputs:
solver_s: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, cid = wp.tid()
# Retrieve the number of contact active in the world
nc = problem_nc[wid]
# Retrieve the limit constraint group offset of the world
ccgo = problem_ccgo[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if cid >= nc:
return
# Retrieve the index offset of the vector block of the world
cio = problem_cio[wid]
# Retrieve the index offset of the vector block of the world
vio = problem_vio[wid]
# Compute the vector index offset of the corresponding contact constraint
ccio_k = vio + ccgo + 3 * cid
# Compute the norm of the tangential components
vtx = solver_z[ccio_k]
vty = solver_z[ccio_k + 1]
vt_norm = wp.sqrt(vtx * vtx + vty * vty)
# Store De Saxce correction for this block
solver_s[ccio_k] = 0.0
solver_s[ccio_k + 1] = 0.0
solver_s[ccio_k + 2] = problem_mu[cio + cid] * vt_norm
@wp.kernel
def _compute_solution_vectors(
# Inputs:
problem_dim: wp.array(dtype=int32),
problem_vio: wp.array(dtype=int32),
solver_s: wp.array(dtype=float32),
solver_y: wp.array(dtype=float32),
solver_z: wp.array(dtype=float32),
# Outputs:
solver_v_plus: wp.array(dtype=float32),
solver_lambdas: wp.array(dtype=float32),
):
# Retrieve the thread index
wid, tid = wp.tid()
# Retrieve the total number of active constraints in the world
ncts = problem_dim[wid]
# Skip if row index exceed the problem size or if the solver has already converged
if tid >= ncts:
return
# Retrieve the index offset of the vector block of the world
vector_offset = problem_vio[wid]
# Compute the index offset of the vector block of the world
thread_offset = vector_offset + tid
# Retrieve the solver state
z = solver_z[thread_offset]
s = solver_s[thread_offset]
y = solver_y[thread_offset]
# Update constraint velocity: v_plus = z - s;
solver_v_plus[thread_offset] = z - s
# Update constraint reactions: lambda = y
solver_lambdas[thread_offset] = y
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/padmm/math.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Defines mathematical operations used by the Proximal-ADMM solver."""
import warp as wp
from ...core.types import float32, int32, vec3f
###
# Module interface
###
__all__ = [
"compute_cwise_vec_div",
"compute_cwise_vec_mul",
"compute_desaxce_corrections",
"compute_dot_product",
"compute_double_dot_product",
"compute_gemv",
"compute_inf_norm",
"compute_inverse_preconditioned_iterate_residual",
"compute_l1_norm",
"compute_l2_norm",
"compute_ncp_complementarity_residual",
"compute_ncp_dual_residual",
"compute_ncp_natural_map_residual",
"compute_ncp_primal_residual",
"compute_preconditioned_iterate_residual",
"compute_vector_sum",
"project_to_coulomb_cone",
"project_to_coulomb_dual_cone",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Functions
###
@wp.func
def project_to_coulomb_cone(x: vec3f, mu: float32, epsilon: float32 = 0.0) -> vec3f:
"""
Projects a 3D vector `x` onto an isotropic Coulomb friction cone defined by the friction coefficient `mu`.
Args:
x (vec3f): The input vector to be projected.
mu (float32): The friction coefficient defining the aperture of the cone.
epsilon (float32, optional): A numerical tolerance applied to the cone boundary. Defaults to 0.0.
Returns:
vec3f: The vector projected onto the Coulomb cone.
"""
xn = x[2]
xt_norm = wp.sqrt(x[0] * x[0] + x[1] * x[1])
y = wp.vec3f(0.0)
if mu * xt_norm > -xn + epsilon:
if xt_norm <= mu * xn + epsilon:
y = x
else:
ys = (mu * xt_norm + xn) / (mu * mu + 1.0)
yts = mu * ys / xt_norm
y[0] = yts * x[0]
y[1] = yts * x[1]
y[2] = ys
return y
@wp.func
def project_to_coulomb_dual_cone(x: vec3f, mu: float32, epsilon: float32 = 0.0) -> vec3f:
"""
Projects a 3D vector `x` onto the dual of an isotropic Coulomb
friction cone defined by the friction coefficient `mu`.
Args:
x (vec3f): The input vector to be projected.
mu (float32): The friction coefficient defining the aperture of the cone.
epsilon (float32, optional): A numerical tolerance applied to the cone boundary. Defaults to 0.0.
Returns:
vec3f: The vector projected onto the dual Coulomb cone.
"""
xn = x[2]
xt_norm = wp.sqrt(x[0] * x[0] + x[1] * x[1])
y = wp.vec3f(0.0)
if xt_norm > -mu * xn + epsilon:
if mu * xt_norm <= xn + epsilon:
y = x
else:
ys = (xt_norm + mu * xn) / (mu * mu + 1.0)
yts = ys / xt_norm
y[0] = yts * x[0]
y[1] = yts * x[1]
y[2] = mu * ys
return y
###
# BLAS-like Utility Functions
#
# TODO: All of these should be re-implemented to exploit parallelism
###
@wp.func
def compute_inf_norm(
dim: int32,
vio: int32,
x: wp.array(dtype=float32),
) -> float32:
norm = float(0.0)
for i in range(dim):
norm = wp.max(norm, wp.abs(x[vio + i]))
return norm
@wp.func
def compute_l1_norm(
dim: int32,
vio: int32,
x: wp.array(dtype=float32),
) -> float32:
sum = float(0.0)
for i in range(dim):
sum += wp.abs(x[vio + i])
return sum
@wp.func
def compute_l2_norm(
dim: int32,
vio: int32,
x: wp.array(dtype=float32),
) -> float32:
sum = float(0.0)
for i in range(dim):
x_i = x[vio + i]
sum += x_i * x_i
return wp.sqrt(sum)
@wp.func
def compute_dot_product(
dim: int32,
vio: int32,
x: wp.array(dtype=float32),
y: wp.array(dtype=float32),
) -> float32:
"""
Computes the dot (i.e. inner) product between two vectors `x` and `y` stored in flat arrays.\n
Both vectors are of dimension `dim`, starting from the vector index offset `vio`.
Args:
dim (int32): The dimension (i.e. size) of the vectors.
vio (int32): The vector index offset (i.e. start index).
x (wp.array(dtype=float32)): The first vector.
y (wp.array(dtype=float32)): The second vector.
Returns:
float32: The dot product of the two vectors.
"""
product = float(0.0)
for i in range(dim):
v_i = vio + i
product += x[v_i] * y[v_i]
return product
@wp.func
def compute_double_dot_product(
dim: int32,
vio: int32,
x: wp.array(dtype=float32),
y: wp.array(dtype=float32),
z: wp.array(dtype=float32),
) -> float32:
"""
Computes the the inner product `x.T @ (y + z)` between a vector `x` and the sum of two vectors `y` and `z`.\n
All vectors are stored in flat arrays, with dimension `dim` and starting from the vector index offset `vio`.
Args:
dim (int32): The dimension (i.e. size) of the vectors.
vio (int32): The vector index offset (i.e. start index).
x (wp.array(dtype=float32)): The first vector.
y (wp.array(dtype=float32)): The second vector.
z (wp.array(dtype=float32)): The third vector.
Returns:
float32: The inner product of `x` with the sum of `y` and `z`.
"""
product = float(0.0)
for i in range(dim):
v_i = vio + i
product += x[v_i] * (y[v_i] + z[v_i])
return product
@wp.func
def compute_vector_sum(
dim: int32, vio: int32, x: wp.array(dtype=float32), y: wp.array(dtype=float32), z: wp.array(dtype=float32)
):
"""
Computes the sum of two vectors `x` and `y` and stores the result in vector `z`.\n
All vectors are stored in flat arrays, with dimension `dim` and starting from the vector index offset `vio`.
Args:
dim (int32): The dimension (i.e. size) of the vectors.
vio (int32): The vector index offset (i.e. start index).
x (wp.array(dtype=float32)): The first vector.
y (wp.array(dtype=float32)): The second vector.
z (wp.array(dtype=float32)): The output vector where the sum is stored.
Returns:
None: The result is stored in the output vector `z`.
"""
for i in range(dim):
v_i = vio + i
z[v_i] = x[v_i] + y[v_i]
@wp.func
def compute_cwise_vec_mul(
dim: int32,
vio: int32,
a: wp.array(dtype=float32),
x: wp.array(dtype=float32),
y: wp.array(dtype=float32),
):
"""
Computes the coefficient-wise vector-vector product `y = a * x`.\n
Args:
dim (int32): The dimension (i.e. size) of the vectors.
vio (int32): The vector index offset (i.e. start index).
a (wp.array(dtype=float32)): Input array containing the first set of vectors.
x (wp.array(dtype=float32)): Input array containing the second set of vectors.
y (wp.array(dtype=float32)): Output array where the result is stored.
"""
for i in range(dim):
v_i = vio + i
y[v_i] = a[v_i] * x[v_i]
@wp.func
def compute_cwise_vec_div(
dim: int32,
vio: int32,
a: wp.array(dtype=float32),
x: wp.array(dtype=float32),
y: wp.array(dtype=float32),
):
"""
Computes the coefficient-wise vector-vector division `y = a / x`.\n
Args:
dim (int32): The dimension (i.e. size) of the vectors.
vio (int32): The vector index offset (i.e. start index).
a (wp.array(dtype=float32)): Input array containing the first set of vectors.
x (wp.array(dtype=float32)): Input array containing the second set of vectors.
y (wp.array(dtype=float32)): Output array where the result is stored.
"""
for i in range(dim):
v_i = vio + i
y[v_i] = a[v_i] / x[v_i]
@wp.func
def compute_gemv(
dim: int32,
vio: int32,
mio: int32,
sigma: float32,
P: wp.array(dtype=float32),
A: wp.array(dtype=float32),
x: wp.array(dtype=float32),
b: wp.array(dtype=float32),
c: wp.array(dtype=float32),
):
"""
Computes the generalized matrix-vector product `c = b + (A - sigma * I_n)@ x`.\n
The matrix `A` is stored using row-major order in flat array with allocation size `maxdim x maxdim`,
starting from the matrix index offset `mio`. The active dimensions of the matrix are `dim x dim`,
where `dim` is the number of rows and columns. The vectors `x, b, c` are stored in flat arrays with
dimensions `dim`, starting from the vector index offset `vio`.
Args:
maxdim (int32): The maximum dimension of the matrix `A`.
dim (int32): The active dimension of the matrix `A` and the vectors `x, b, c`.
vio (int32): The vector index offset (i.e. start index) for the vectors `x, b, c`.
mio (int32): The matrix index offset (i.e. start index) for the matrix `A`.
A (wp.array(dtype=float32)):
Input matrix `A` stored in row-major order.
x (wp.array(dtype=float32)):
Input array `x` to be multiplied with the matrix `A`.
b (wp.array(dtype=float32)):
Input array `b` to be added to the product `A @ x`.
c (wp.array(dtype=float32)):
Output array `c` where the result of the operation is stored.
"""
b_i = float(0.0)
x_j = float(0.0)
for i in range(dim):
v_i = vio + i
m_i = mio + dim * i
b_i = b[v_i]
for j in range(dim):
x_j = x[vio + j]
b_i += A[m_i + j] * x_j
b_i -= sigma * x[v_i]
c[v_i] = (1.0 / P[v_i]) * b_i
@wp.func
def compute_desaxce_corrections(
nc: int32,
cio: int32,
vio: int32,
ccgo: int32,
mu: wp.array(dtype=float32),
v_plus: wp.array(dtype=float32),
s: wp.array(dtype=float32),
):
"""
Computes the De Saxce correction for each active contact.
`s = G(v) := [ 0, 0 , mu * || vt ||_2,]^T, where v := [vtx, vty, vn]^T, vt := [vtx, vty]^T`
Args:
nc (int32): The number of active contact constraints.
cio (int32): The contact index offset (i.e. start index) for the contacts.
vio (int32): The vector index offset (i.e. start index)
ccgo (int32): The contact constraint group offset (i.e. start index)
mu (wp.array(dtype=float32)):
The array of friction coefficients for each contact constraint.
v_plus (wp.array(dtype=float32)):
The post-event constraint-space velocities array, which contains the tangential velocities `vtx`
and `vty` for each contact constraint.
s (wp.array(dtype=float32)):
The output array where the De Saxce corrections are stored.\n
The size of this array should be at least `vio + ccgo + 3 * nc`, where `vio` is the vector index offset,
`ccgo` is the contact constraint group offset, and `nc` is the number of active contact constraints.
Returns:
None: The De Saxce corrections are stored in the output array `s`.
"""
# Iterate over each active contact
for k in range(nc):
# Compute the contact index
c_k = cio + k
# Compute the constraint vector index
v_k = vio + ccgo + 3 * k
# Retrieve the friction coefficient for this contact
mu_k = mu[c_k]
# Compute the 2D norm of the tangential velocity
vtx_k = v_plus[v_k]
vty_k = v_plus[v_k + 1]
vt_norm_k = wp.sqrt(vtx_k * vtx_k + vty_k * vty_k)
# Store De Saxce correction for this block
s[v_k] = 0.0
s[v_k + 1] = 0.0
s[v_k + 2] = mu_k * vt_norm_k
@wp.func
def compute_ncp_primal_residual(
nl: int32,
nc: int32,
vio: int32,
lcgo: int32,
ccgo: int32,
cio: int32,
mu: wp.array(dtype=float32),
lambdas: wp.array(dtype=float32),
) -> tuple[float32, int32]:
"""
Computes the NCP primal residual as: `r_p := || lambda - proj_K(lambda) ||_inf`, where:
- `lambda` is the vector of constraint reactions (i.e. impulses)
- `proj_K()` is the projection operator onto the cone `K`
- `K` is the total cone defined by the unilateral constraints such as limits and contacts
- `|| . ||_inf` is the infinity norm (i.e. maximum absolute value of the vector components)
Notes:
- The cone for joint constraints is all of `R^njc`, so projection is a no-op.
- For limit constraints, the cone is defined as `K_l := { lambda | lambda >= 0 }`
- For contact constraints, the cone is defined as `K_c := { lambda | || lambda ||_2 <= mu * || vn ||_2 }`
Args:
nl (int32): The number of active limit constraints.
nc (int32): The number of active contact constraints.
vio (int32): The vector index offset (i.e. start index) for the constraints.
lcgo (int32): The limit constraint group offset (i.e. start index).
ccgo (int32): The contact constraint group offset (i.e. start index).
cio (int32): The contact index offset (i.e. start index) for the contacts.
mu (wp.array(dtype=float32)):
The array of friction coefficients for each contact.
lambdas (wp.array(dtype=float32)):
The array of constraint reactions (i.e. impulses).
Returns:
float32: The maximum primal residual across all constraints, computed as the infinity norm.
int32: The index of the constraint with the maximum primal residual.
"""
# Initialize the primal residual
r_ncp_p = float(0.0)
r_ncp_p_argmax = int32(-1)
# NOTE: We skip the joint constraint reactions are not bounded, the cone is all of R^njc
for lid in range(nl):
# Compute the limit constraint index offset
lcio = vio + lcgo + lid
# Compute the primal residual for the limit constraints
lambda_l = lambdas[lcio]
lambda_l -= wp.max(0.0, lambda_l)
r_l = wp.abs(lambda_l)
r_ncp_p = wp.max(r_ncp_p, r_l)
if r_ncp_p == r_l:
r_ncp_p_argmax = lid
for cid in range(nc):
# Compute the contact constraint index offset
ccio = vio + ccgo + 3 * cid
# Retrieve the friction coefficient for this contact
mu_c = mu[cio + cid]
# Compute the primal residual for the contact constraints
lambda_c = vec3f(lambdas[ccio], lambdas[ccio + 1], lambdas[ccio + 2])
lambda_c -= project_to_coulomb_cone(lambda_c, mu_c)
r_c = wp.max(wp.abs(lambda_c))
r_ncp_p = wp.max(r_ncp_p, r_c)
if r_ncp_p == r_c:
r_ncp_p_argmax = cid
# Return the maximum primal residual
return r_ncp_p, r_ncp_p_argmax
@wp.func
def compute_ncp_dual_residual(
njc: int32,
nl: int32,
nc: int32,
vio: int32,
lcgo: int32,
ccgo: int32,
cio: int32,
mu: wp.array(dtype=float32),
v_aug: wp.array(dtype=float32),
) -> tuple[float32, int32]:
"""
Computes the NCP dual residual as: `r_d := || v_aug - proj_K^*(v_aug) ||_inf`, where:
- `v_aug` is the vector of augmented constraint velocities: v_aug := v_plus + s
- `v_plus` is the post-event constraint-space velocities
- `s` is the De Saxce correction vector
- `proj_K^*()` is the projection operator onto the dual cone `K^*`
- `K^*` is the dual of the total cone defined by the unilateral constraints such as limits and contacts
- `|| . ||_inf` is the infinity norm (i.e. maximum absolute value of the vector components)
Notes:
- The dual cone for joint constraints is the origin point x=0.
- For limit constraints, the cone is defined as `K_l := { lambda | lambda >= 0 }`
- For contact constraints, the cone is defined as `K_c := { lambda | || lambda ||_2 <= mu * || vn ||_2 }`
Args:
njc (int32): The number of joint constraints.
nl (int32): The number of active limit constraints.
nc (int32): The number of active contact constraints.
vio (int32): The vector index offset (i.e. start index) for the constraints.
lcgo (int32): The limit constraint group offset (i.e. start index).
ccgo (int32): The contact constraint group offset (i.e. start index).
cio (int32): The contact index offset (i.e. start index) for the contacts.
mu (wp.array(dtype=float32)):
The array of friction coefficients for each contact constraint.
v_aug (wp.array(dtype=float32)):
The array of augmented constraint velocities.
Returns:
float32: The maximum dual residual across all constraints, computed as the infinity norm.
int32: The index of the constraint with the maximum dual residual.
"""
# Initialize the dual residual
r_ncp_d = float(0.0)
r_ncp_d_argmax = int32(-1)
for jid in range(njc):
# Compute the joint constraint index offset
jcio_j = vio + jid
# Compute the dual residual for the joint constraints
# NOTE #1: Each constraint-space velocity for joint should be zero
# NOTE #2: the dual of R^njc is the origin zero vector
v_j = v_aug[jcio_j]
r_j = wp.abs(v_j)
r_ncp_d = wp.max(r_ncp_d, r_j)
if r_ncp_d == r_j:
r_ncp_d_argmax = jid
for lid in range(nl):
# Compute the limit constraint index offset
lcio_l = vio + lcgo + lid
# Compute the dual residual for the limit constraints
# NOTE: Each constraint-space velocity should be non-negative
v_l = v_aug[lcio_l]
v_l -= wp.max(0.0, v_l)
r_l = wp.abs(v_l)
r_ncp_d = wp.max(r_ncp_d, r_l)
if r_ncp_d == r_l:
r_ncp_d_argmax = lid
for cid in range(nc):
# Compute the contact constraint index offset
ccio_c = vio + ccgo + 3 * cid
# Retrieve the friction coefficient for this contact
mu_c = mu[cio + cid]
# Compute the dual residual for the contact constraints
# NOTE: Each constraint-space velocity should be lie in the dual of the Coulomb friction cone
v_c = vec3f(v_aug[ccio_c], v_aug[ccio_c + 1], v_aug[ccio_c + 2])
v_c -= project_to_coulomb_dual_cone(v_c, mu_c)
r_c = wp.max(wp.abs(v_c))
r_ncp_d = wp.max(r_ncp_d, r_c)
if r_ncp_d == r_c:
r_ncp_d_argmax = cid
# Return the maximum dual residual
return r_ncp_d, r_ncp_d_argmax
@wp.func
def compute_ncp_complementarity_residual(
nl: int32,
nc: int32,
vio: int32,
lcgo: int32,
ccgo: int32,
v_aug: wp.array(dtype=float32),
lambdas: wp.array(dtype=float32),
) -> tuple[float32, int32]:
"""
Computes the NCP complementarity residual as `r_c := || lambda.dot(v_plus + s) ||_inf`
Satisfaction of the complementarity condition `lambda _|_ (v_plus + s))` is measured
using the per-constraint entity inner product, i.e. per limit and per contact. Thus,
for each limit constraint `k`, we compute `v_k * lambda_k` and for each contact
constraint `k`, we compute `v_k.dot(lambda_k)`.
Args:
nl (int32): The number of active limit constraints.
nc (int32): The number of active contact constraints.
vio (int32): The vector index offset (i.e. start index) for the constraints.
lcgo (int32): The limit constraint group offset (i.e. start index).
ccgo (int32): The contact constraint group offset (i.e. start index).
v_aug (wp.array(dtype=float32)):
The array of augmented constraint velocities.
lambdas (wp.array(dtype=float32)):
The array of constraint reactions (i.e. impulses).
Returns:
float32: The maximum complementarity residual across all constraints, computed as the infinity norm.
int32: The index of the constraint with the maximum complementarity residual.
"""
# Initialize the complementarity residual
r_ncp_c = float(0.0)
r_ncp_c_argmax = int32(-1)
for lid in range(nl):
# Compute the limit constraint index offset
lcio = vio + lcgo + lid
# Compute the complementarity residual for the limit constraints
v_l = v_aug[lcio]
lambda_l = lambdas[lcio]
r_l = wp.abs(v_l * lambda_l)
r_ncp_c = wp.max(r_ncp_c, r_l)
if r_ncp_c == r_l:
r_ncp_c_argmax = lid
for cid in range(nc):
# Compute the contact constraint index offset
ccio = vio + ccgo + 3 * cid
# Compute the complementarity residual for the contact constraints
v_c = vec3f(v_aug[ccio], v_aug[ccio + 1], v_aug[ccio + 2])
lambda_c = vec3f(lambdas[ccio], lambdas[ccio + 1], lambdas[ccio + 2])
r_c = wp.abs(wp.dot(v_c, lambda_c))
r_ncp_c = wp.max(r_ncp_c, r_c)
if r_ncp_c == r_c:
r_ncp_c_argmax = cid
# Return the maximum complementarity residual
return r_ncp_c, r_ncp_c_argmax
@wp.func
def compute_ncp_natural_map_residual(
nl: int32,
nc: int32,
vio: int32,
lcgo: int32,
ccgo: int32,
cio: int32,
mu: wp.array(dtype=float32),
v_aug: wp.array(dtype=float32),
lambdas: wp.array(dtype=float32),
) -> tuple[float32, int32]:
"""
Computes the natural-map residuals as: `r_natmap = || lambda - proj_K(lambda - (v + s)) ||_inf`
Args:
nl (int32): The number of active limit constraints.
nc (int32): The number of active contact constraints.
vio (int32): The vector index offset (i.e. start index) for the constraints.
lcgo (int32): The limit constraint group offset (i.e. start index).
ccgo (int32): The contact constraint group offset (i.e. start index).
cio (int32): The contact index offset (i.e. start index) for the contacts.
mu (wp.array(dtype=float32)):
The array of friction coefficients for each contact.
v_aug (wp.array(dtype=float32)):
The array of augmented constraint velocities.
lambdas (wp.array(dtype=float32)):
The array of constraint reactions (i.e. impulses).
Returns:
float32: The maximum natural-map residual across all constraints, computed as the infinity norm.
int32: The index of the constraint with the maximum natural-map residual.
"""
# Initialize the natural-map residual
r_ncp_natmap = float(0.0)
r_ncp_natmap_argmax = int32(-1)
for lid in range(nl):
# Compute the limit constraint index offset
lcio = vio + lcgo + lid
# Compute the natural-map residual for the limit constraints
v_l = v_aug[lcio]
lambda_l = lambdas[lcio]
lambda_l -= wp.max(0.0, lambda_l - v_l)
lambda_l = wp.abs(lambda_l)
r_ncp_natmap = wp.max(r_ncp_natmap, lambda_l)
if r_ncp_natmap == lambda_l:
r_ncp_natmap_argmax = lid
for cid in range(nc):
# Compute the contact constraint index offset
ccio = vio + ccgo + 3 * cid
# Retrieve the friction coefficient for this contact
mu_c = mu[cio + cid]
# Compute the natural-map residual for the contact constraints
v_c = vec3f(v_aug[ccio], v_aug[ccio + 1], v_aug[ccio + 2])
lambda_c = vec3f(lambdas[ccio], lambdas[ccio + 1], lambdas[ccio + 2])
lambda_c -= project_to_coulomb_cone(lambda_c - v_c, mu_c)
lambda_c = wp.abs(lambda_c)
lambda_c_max = wp.max(lambda_c)
r_ncp_natmap = wp.max(r_ncp_natmap, lambda_c_max)
if r_ncp_natmap == lambda_c_max:
r_ncp_natmap_argmax = cid
# Return the maximum natural-map residual
return r_ncp_natmap, r_ncp_natmap_argmax
@wp.func
def compute_preconditioned_iterate_residual(
ncts: int32, vio: int32, P: wp.array(dtype=float32), x: wp.array(dtype=float32), x_p: wp.array(dtype=float32)
) -> float32:
"""
Computes the iterate residual as: `r_dx := || P @ (x - x_p) ||_inf`
Args:
ncts (int32): The number of active constraints in the world.
vio (int32): The vector index offset (i.e. start index) for the constraints.
x (wp.array(dtype=float32)):
The current solution vector.
x_p (wp.array(dtype=float32)):
The previous solution vector.
Returns:
float32: The maximum iterate residual across all active constraints, computed as the infinity norm.
"""
# Initialize the iterate residual
r_dx = float(0.0)
for i in range(ncts):
# Compute the index offset of the vector block of the world
v_i = vio + i
# Update the iterate and proximal-point residuals
r_dx = wp.max(r_dx, P[v_i] * wp.abs(x[v_i] - x_p[v_i]))
# Return the maximum iterate residual
return r_dx
@wp.func
def compute_inverse_preconditioned_iterate_residual(
ncts: int32, vio: int32, P: wp.array(dtype=float32), x: wp.array(dtype=float32), x_p: wp.array(dtype=float32)
) -> float32:
"""
Computes the iterate residual as: `r_dx := || P^{-1} @ (x - x_p) ||_inf`
Args:
ncts (int32): The number of active constraints in the world.
vio (int32): The vector index offset (i.e. start index) for the constraints.
x (wp.array(dtype=float32)):
The current solution vector.
x_p (wp.array(dtype=float32)):
The previous solution vector.
Returns:
float32: The maximum iterate residual across all active constraints, computed as the infinity norm.
"""
# Initialize the iterate residual
r_dx = float(0.0)
for i in range(ncts):
# Compute the index offset of the vector block of the world
v_i = vio + i
# Update the iterate and proximal-point residuals
r_dx = wp.max(r_dx, (1.0 / P[v_i]) * wp.abs(x[v_i] - x_p[v_i]))
# Return the maximum iterate residual
return r_dx
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/padmm/solver.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Defines the Proximal-ADMM solver class.
This is the highest-level interface for the Proximal-ADMM solver for constrained rigid multi-body systems.
See the :mod:`newton._src.solvers.kamino.solvers.padmm` module for a detailed description and usage example.
"""
from __future__ import annotations
import math
import warp as wp
from ....config import PADMMSolverConfig
from ...core.data import DataKamino
from ...core.model import ModelKamino
from ...core.size import SizeKamino
from ...dynamics.dual import DualProblem
from ...geometry.contacts import ContactsKamino
from ...kinematics.limits import LimitsKamino
from .kernels import (
_apply_dual_preconditioner_to_solution,
_apply_dual_preconditioner_to_state,
_compute_complementarity_residuals,
_compute_desaxce_correction,
_compute_final_desaxce_correction,
_compute_projection_argument,
_compute_solution_vectors,
_compute_velocity_bias,
_make_compute_infnorm_residuals_accel_kernel,
_make_compute_infnorm_residuals_kernel,
_project_to_feasible_cone,
_reset_solver_data,
_update_delassus_proximal_regularization,
_update_delassus_proximal_regularization_sparse,
_update_state_with_acceleration,
_warmstart_contact_constraints,
_warmstart_desaxce_correction,
_warmstart_joint_constraints,
_warmstart_limit_constraints,
make_collect_solver_info_kernel,
make_collect_solver_info_kernel_sparse,
make_initialize_solver_kernel,
make_update_dual_variables_and_compute_primal_dual_residuals,
)
from .types import (
PADMMConfigStruct,
PADMMData,
PADMMPenaltyUpdate,
PADMMWarmStartMode,
convert_config_to_struct,
)
###
# Module interface
###
__all__ = ["PADMMSolver"]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Interfaces
###
class PADMMSolver:
"""
The Proximal-ADMM (PADMM) forward dynamics solver for constrained rigid multi-body systems.
This solver implements the Proximal-ADMM algorithm to solve the Lagrange dual of the
constrained forward dynamics problem in constraint reactions (i.e. impulses) and
post-event constraint-space velocities.
Notes:
- is designed to work with the DualProblem formulation.
- operates on the Lagrange dual of the constrained forward dynamics problem.
- is based on the Proximal-ADMM algorithm, which introduces a proximal regularization term
- supports multiple penalty update methods, including fixed, linear, and spectral updates.
- can be configured with various tolerances, penalty parameters, and other settings.
"""
Config = PADMMSolverConfig
"""
Defines a type alias of the PADMM solver configurations container, including convergence
criteria, maximum iterations, and options for the linear solver and preconditioning.
See :class:`PADMMSolverConfig` for the full list of configuration options and their descriptions.
"""
def __init__(
self,
model: ModelKamino | None = None,
config: list[PADMMSolver.Config] | PADMMSolver.Config | None = None,
warmstart: PADMMWarmStartMode = PADMMWarmStartMode.NONE,
use_acceleration: bool = True,
use_graph_conditionals: bool = True,
collect_info: bool = False,
device: wp.DeviceLike = None,
):
"""
Initializes a PADMM solver.
If a model is provided, it will perform all necessary memory allocations on the
target device, otherwise the user must call `finalize()` before using the solver.
Args:
model (ModelKamino | None): The model for which to allocate the solver data.
limits (LimitsKamino | None): The limits container associated with the model.
contacts (ContactsKamino | None): The contacts container associated with the model.
config (list[PADMMSolver.Config] | PADMMSolver.Config | None): The solver config to use.
warmstart (PADMMWarmStartMode): The warm-start mode to use for the solver.\n
use_acceleration (bool): Set to `True` to enable Nesterov acceleration.
use_graph_conditionals (bool): Set to `False` to disable CUDA graph conditional nodes.\n
When disabled, replaces `wp.capture_while` with an unrolled for-loop over max iterations.
collect_info (bool): Set to `True` to enable collection of solver convergence info.\n
This setting is intended only for analysis and debugging purposes, as it
will increase memory consumption and reduce wall-clock time.
device (wp.DeviceLike | None): The target device on which to allocate the solver data.
"""
# Declare the internal solver config cache
self._config: list[PADMMSolver.Config] = []
self._warmstart: PADMMWarmStartMode = PADMMWarmStartMode.NONE
self._use_acceleration: bool = True
self._use_adaptive_penalty: bool = False
self._use_graph_conditionals: bool = True
self._collect_info: bool = False
# Declare the model size cache
self._size: SizeKamino | None = None
# Declare the solver data container
self._data: PADMMData | None = None
# Declare the device cache
self._device: wp.DeviceLike = None
# Perform memory allocations if a model is provided
if model is not None:
self.finalize(
model=model,
config=config,
warmstart=warmstart,
use_acceleration=use_acceleration,
use_graph_conditionals=use_graph_conditionals,
collect_info=collect_info,
device=device,
)
###
# Properties
###
@property
def config(self) -> list[PADMMSolver.Config]:
"""
Returns the host-side cache of the solver config.\n
They are used to construct the warp array of type :class:`PADMMSolver.Config` on the target device.
"""
return self._config
@property
def size(self) -> SizeKamino:
"""
Returns the host-side cache of the solver allocation sizes.
"""
return self._size
@property
def data(self) -> PADMMData:
"""
Returns a reference to the high-level solver data container.
"""
if self._data is None:
raise RuntimeError("Solver data has not been allocated yet. Call `finalize()` first.")
return self._data
@property
def device(self) -> wp.DeviceLike:
"""
Returns the device on which the solver data is allocated.
"""
return self._device
###
# Public API
###
def finalize(
self,
model: ModelKamino | None = None,
config: list[PADMMSolver.Config] | PADMMSolver.Config | None = None,
warmstart: PADMMWarmStartMode = PADMMWarmStartMode.NONE,
use_acceleration: bool = True,
use_graph_conditionals: bool = True,
collect_info: bool = False,
device: wp.DeviceLike = None,
):
"""
Allocates the solver data structures on the specified device.
Args:
model (ModelKamino | None): The model for which to allocate the solver data.
limits (LimitsKamino | None): The limits container associated with the model.
contacts (ContactsKamino | None): The contacts container associated with the model.
config (list[PADMMSolver.Config] | PADMMSolver.Config | None): The solver config to use.
warmstart (PADMMWarmStartMode): The warm-start mode to use for the solver.\n
use_acceleration (bool): Set to `True` to enable Nesterov acceleration.
use_graph_conditionals (bool): Set to `False` to disable CUDA graph conditional nodes.\n
When disabled, replaces `wp.capture_while` with an unrolled for-loop over max iterations.
collect_info (bool): Set to `True` to enable collection of solver convergence info.\n
This setting is intended only for analysis and debugging purposes, as it
will increase memory consumption and reduce wall-clock time.
device (wp.DeviceLike | None): The target device on which to allocate the solver data.
"""
# Ensure the model is valid
if model is None:
raise ValueError("A model of type `ModelKamino` must be provided to allocate the Delassus operator.")
elif not isinstance(model, ModelKamino):
raise ValueError("Invalid model provided. Must be an instance of `ModelKamino`.")
# Cache a reference to the model size meta-data container
self._size = model.size
# Set the target device if specified, otherwise use the model device
self._device = device if device is not None else model.device
# Cache solver configs and validate them against the model size
# NOTE: These are configurations which could potentially be different across
# worlds, so we cache them in a list and write to device memory as an array
if config is not None:
self._config = self._check_config(model, config)
elif len(self._config) == 0:
self._config = self._check_config(model, None)
# Cache high-level solver options shared across all worlds
self._warmstart = warmstart
self._use_acceleration = use_acceleration
self._use_graph_conditionals = use_graph_conditionals
self._collect_info = collect_info
# Check if any world uses adaptive penalty updates (requiring per-step regularization updates)
self._use_adaptive_penalty = any(
PADMMPenaltyUpdate.from_string(c.penalty_update_method) != PADMMPenaltyUpdate.FIXED for c in self._config
)
# Compute the largest max iterations across all worlds
# NOTE: This is needed to allocate the solver
# info arrays if `collect_info` is enabled
max_of_max_iters = max([c.max_iterations for c in self._config])
self._max_of_max_iters = max_of_max_iters
# Allocate memory in device global memory
self._data = PADMMData(
size=self._size,
max_iters=max_of_max_iters,
use_acceleration=self._use_acceleration,
collect_info=self._collect_info,
device=self._device,
)
# Write algorithm configs into device memory
configs = [convert_config_to_struct(c) for c in self._config]
with wp.ScopedDevice(self._device):
self._data.config = wp.array(configs, dtype=PADMMConfigStruct)
# Specialize certain solver kernels depending on whether acceleration is enabled
self._initialize_solver_kernel = make_initialize_solver_kernel(self._use_acceleration)
self._collect_solver_info_kernel = make_collect_solver_info_kernel(self._use_acceleration)
self._collect_solver_info_kernel_sparse = make_collect_solver_info_kernel_sparse(self._use_acceleration)
self._update_dual_variables_and_compute_primal_dual_residuals_kernel = (
make_update_dual_variables_and_compute_primal_dual_residuals(self._use_acceleration)
)
def reset(self, problem: DualProblem | None = None, world_mask: wp.array | None = None):
"""
Resets the all internal solver data to sentinel values.
"""
# Reset the internal solver state
self._data.state.reset(use_acceleration=self._use_acceleration)
# Reset the solution cache, which could be used for internal warm-starting
# If no world mask is provided, reset data of all worlds
if world_mask is None:
self._data.solution.zero()
# Otherwise, only the solution cache of the specified worlds
else:
if problem is None:
raise ValueError("A `DualProblem` instance must be provided when a world mask is used.")
wp.launch(
kernel=_reset_solver_data,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
world_mask,
problem.data.vio,
problem.data.maxdim,
self._data.solution.lambdas,
self._data.solution.v_plus,
],
)
def coldstart(self):
"""
Initializes the internal solver state to perform a cold-start solve.\n
This method sets all solver state variables to zeros.
"""
# Initialize state arrays to zero
self._data.state.reset(use_acceleration=self._use_acceleration)
def warmstart(
self,
problem: DualProblem,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
):
"""
Warm-starts the internal solver state based on the selected warm-start mode.
Supported warm-start modes:
- `PADMMWarmStartMode.NONE`: No warm-starting is performed.
- `PADMMWarmStartMode.INTERNAL`: Warm-starts from the internal solution state.
- `PADMMWarmStartMode.CONTAINERS`: Warm-starts from the provided limits and contacts containers.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.\n
This is needed during warm-starts in order to access the problem preconditioning.
model (ModelKamino): The model associated with the problem.
data (DataKamino): The model data associated with the problem.
limits (LimitsKamino | None): The limits container associated with the model.\n
If `None`, no warm-starting from limits is performed.
contacts (ContactsKamino | None): The contacts container associated with the model.\n
If `None`, no warm-starting from contacts is performed.
"""
# TODO: IS THIS EVEN NECESSARY AT ALL?
# First reset the internal solver state to ensure proper initialization
self._data.state.reset(use_acceleration=self._use_acceleration)
# Warm-start based on the selected mode
match self._warmstart:
case PADMMWarmStartMode.NONE:
return
case PADMMWarmStartMode.INTERNAL:
self._warmstart_from_solution(problem)
case PADMMWarmStartMode.CONTAINERS:
self._warmstart_from_containers(problem, model, data, limits, contacts)
case _:
raise ValueError(f"Invalid warmstart mode: {self._warmstart}")
def solve(self, problem: DualProblem):
"""
Solves the given dual problem using PADMM.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Pass the PADMM-owned tolerance array to the iterative linear solver (if present).
inner = getattr(problem._delassus._solver, "solver", None)
if inner is not None:
inner.atol = self._data.linear_solver_atol
# Initialize the solver status, ALM penalty, and iterative solver tolerance
self._initialize()
# Add the diagonal proximal regularization to the Delassus matrix
# D_{eta,rho} := D + (eta + rho) * I_{ncts}
self._update_regularization(problem)
# Reset the solver info to zero if collection is enabled
if self._collect_info:
self._data.info.zero()
# Iterate until convergence or maximum number of iterations is reached
step_fn = self._step_accel if self._use_acceleration else self._step
if self._use_graph_conditionals:
wp.capture_while(self._data.state.done, while_body=step_fn, problem=problem)
else:
for _ in range(self._max_of_max_iters):
step_fn(problem)
# Update the final solution from the terminal PADMM state
self._update_solution(problem)
###
# Internals - High-Level Operations
###
@staticmethod
def _check_config(
model: ModelKamino | None = None, config: list[PADMMSolver.Config] | PADMMSolver.Config | None = None
) -> list[PADMMSolver.Config]:
"""
Checks and validates the provided solver config, returning a list
of config objects corresponding to each world in the model.
Args:
model: The model for which to validate the config.
config: The solver configurations container to validate.
"""
# If no config is provided, use defaults
if config is None:
# If no model is provided, use a single default config object
if model is None:
config = [PADMMSolver.Config()]
# If a model is provided, create a list of default config
# objects based on the number of worlds in the model
else:
num_worlds = model.info.num_worlds
config = [PADMMSolver.Config()] * num_worlds
# If a single config object is provided, convert it to a list
elif isinstance(config, PADMMSolver.Config):
config = [config] * (model.info.num_worlds if model else 1)
# If a list of configs is provided, ensure it matches the number
# of worlds and that all configs are instances of PADMMSolver.Config
elif isinstance(config, list):
if model is not None and len(config) != model.info.num_worlds:
raise ValueError(f"Expected {model.info.num_worlds} configs, got {len(config)}")
if not all(isinstance(s, PADMMSolver.Config) for s in config):
raise TypeError("All configs must be instances of PADMMSolver.Config")
else:
raise TypeError(f"Expected a single object or list of `PADMMSolver.Config`, got {type(config)}")
# Return the validated config
return config
def _initialize(self):
"""
Launches a kernel to initialize the internal solver state before starting a new solve.\n
The kernel is parallelized over the number of worlds.
"""
# Initialize solver status, penalty parameters, and iterative solver tolerance
wp.launch(
kernel=self._initialize_solver_kernel,
dim=self._size.num_worlds,
inputs=[
self._data.config,
self._data.status,
self._data.penalty,
self._data.state.sigma,
self._data.state.a_p,
self._data.linear_solver_atol,
],
)
# Initialize the global while condition flag
# NOTE: We use a single-element array that is initialized
# to number of worlds and decremented by each world that
# converges or reaches the maximum number of iterations
self._data.state.done.fill_(self._size.num_worlds)
def _update_sparse_regularization(self, problem: DualProblem):
"""Propagate eta + rho to the sparse Delassus diagonal regularization."""
wp.launch(
kernel=_update_delassus_proximal_regularization_sparse,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
problem.data.dim,
problem.data.vio,
self._data.config,
self._data.penalty,
self._data.status,
problem.delassus._eta,
],
)
problem.delassus.set_needs_update()
def _update_regularization(self, problem: DualProblem):
"""
Updates the diagonal regularization of the lhs matrix with the proximal regularization terms.\n
For `DualProblem` solves, the lhs matrix corresponds to the Delassus matrix.\n
The kernel is parallelized over the number of worlds and the maximum number of total constraints.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
if problem.sparse:
self._update_sparse_regularization(problem)
else:
# Update the proximal regularization term in the Delassus matrix
wp.launch(
kernel=_update_delassus_proximal_regularization,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.mio,
self._data.status,
self._data.state.sigma,
# Outputs:
problem.data.D,
],
)
# Compute Cholesky/LDLT factorization of the Delassus matrix
problem._delassus.compute(reset_to_zero=True)
def _step(self, problem: DualProblem):
"""
Performs a single PADMM solver iteration.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Compute De Saxce correction from the previous dual variables
self._update_desaxce_correction(problem, self._data.state.z_p)
# Compute the total velocity bias, i.e. rhs vector of the unconstrained linear system
self._update_velocity_bias(problem, self._data.state.y_p, self._data.state.z_p)
# Compute the unconstrained solution and store in the primal variables
self._update_unconstrained_solution(problem)
# Compute the argument to the projection operator with over-relaxation
self._update_projection_argument(problem, self._data.state.z_p)
# Project the over-relaxed primal variables to the feasible set
self._update_projection_to_feasible_set(problem)
# Update the dual variables and compute residuals from the current state
self._update_dual_variables_and_residuals(problem)
# Compute infinity-norm of all residuals and check for convergence
self._update_convergence_check(problem)
# Update sparse Delassus regularization if penalty was updated adaptively
if problem.sparse and self._use_adaptive_penalty:
self._update_sparse_regularization(problem)
# Optionally record internal solver info
if self._collect_info:
self._update_solver_info(problem)
# Update caches of previous state variables
self._update_previous_state()
def _step_accel(self, problem: DualProblem):
"""
Performs a single PADMM solver iteration with Nesterov acceleration.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Compute De Saxce correction from the previous dual variables
self._update_desaxce_correction(problem, self._data.state.z_hat)
# Compute the total velocity bias, i.e. rhs vector of the unconstrained linear system
self._update_velocity_bias(problem, self._data.state.y_hat, self._data.state.z_hat)
# Compute the unconstrained solution and store in the primal variables
self._update_unconstrained_solution(problem)
# Project the over-relaxed primal variables to the feasible set
self._update_projection_argument(problem, self._data.state.z_hat)
# Project the over-relaxed primal variables to the feasible set
self._update_projection_to_feasible_set(problem)
# Update the dual variables and compute residuals from the current state
self._update_dual_variables_and_residuals_accel(problem)
# Compute infinity-norm of all residuals and check for convergence
self._update_convergence_check_accel(problem)
# Update sparse Delassus regularization if penalty was updated adaptively
if problem.sparse and self._use_adaptive_penalty:
self._update_sparse_regularization(problem)
# Optionally update Nesterov acceleration states from the current iteration
self._update_acceleration(problem)
# Optionally record internal solver info
if self._collect_info:
self._update_solver_info(problem)
# Update caches of previous state variables
self._update_previous_state_accel()
###
# Internals - Warm-starting
###
def _warmstart_desaxce_correction(self, problem: DualProblem, z: wp.array):
"""
Applies the De Saxce correction to the provided post-event constraint-space velocity warm-start.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.\n
This is needed during warm-starts in order to access the problem preconditioning.
z (wp.array): The post-event constraint-space velocity warm-start variable.\n
This can either be `z_p` or `z_hat` depending on whether acceleration is used.
"""
wp.launch(
kernel=_warmstart_desaxce_correction,
dim=(self._size.num_worlds, self._size.max_of_max_contacts),
inputs=[
# Inputs:
problem.data.nc,
problem.data.cio,
problem.data.ccgo,
problem.data.vio,
problem.data.mu,
# Outputs:
z,
],
)
def _warmstart_joint_constraints(
self,
model: ModelKamino,
data: DataKamino,
problem: DualProblem,
x_0: wp.array,
y_0: wp.array,
z_0: wp.array,
):
"""
Warm-starts the bilateral joint constraint variables from the model data container.
Args:
model (ModelKamino): The model associated with the problem.
data (DataKamino): The model data associated with the problem.
problem (DualProblem): The dual forward dynamics problem to be solved.\n
This is needed during warm-starts in order to access the problem preconditioning.
x_0 (wp.array): The output primal variables array to be warm-started.
y_0 (wp.array): The output slack variables array to be warm-started.
z_0 (wp.array): The output dual variables array to be warm-started.
"""
wp.launch(
kernel=_warmstart_joint_constraints,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs:
model.time.dt,
model.info.joint_cts_offset,
model.info.total_cts_offset,
model.info.joint_dynamic_cts_group_offset,
model.info.joint_kinematic_cts_group_offset,
model.joints.wid,
model.joints.num_dynamic_cts,
model.joints.num_kinematic_cts,
model.joints.dynamic_cts_offset,
model.joints.kinematic_cts_offset,
data.joints.lambda_j,
problem.data.P,
# Outputs:
x_0,
y_0,
z_0,
],
)
def _warmstart_limit_constraints(
self,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino,
problem: DualProblem,
x_0: wp.array,
y_0: wp.array,
z_0: wp.array,
):
"""
Warm-starts the unilateral limit constraint variables from the limits data container.
Args:
model (ModelKamino): The model associated with the problem.
data (DataKamino): The model data associated with the problem.
limits (LimitsKamino): The limits container associated with the model.
problem (DualProblem): The dual forward dynamics problem to be solved.\n
This is needed during warm-starts in order to access the problem preconditioning.
x_0 (wp.array): The output primal variables array to be warm-started.
y_0 (wp.array): The output slack variables array to be warm-started.
z_0 (wp.array): The output dual variables array to be warm-started.
"""
wp.launch(
kernel=_warmstart_limit_constraints,
dim=limits.model_max_limits_host,
inputs=[
# Inputs:
model.time.dt,
model.info.total_cts_offset,
data.info.limit_cts_group_offset,
limits.model_active_limits,
limits.wid,
limits.lid,
limits.reaction,
limits.velocity,
problem.data.P,
# Outputs:
x_0,
y_0,
z_0,
],
)
def _warmstart_contact_constraints(
self,
model: ModelKamino,
data: DataKamino,
contacts: ContactsKamino,
problem: DualProblem,
x_0: wp.array,
y_0: wp.array,
z_0: wp.array,
):
"""
Warm-starts the unilateral contact constraint variables from the contacts data container.
Args:
model (ModelKamino): The model associated with the problem.
data (DataKamino): The model data associated with the problem.
contacts (ContactsKamino): The contacts container associated with the model.
problem (DualProblem): The dual forward dynamics problem to be solved.\n
This is needed during warm-starts in order to access the problem preconditioning.
x_0 (wp.array): The output primal variables array to be warm-started.
y_0 (wp.array): The output slack variables array to be warm-started.
z_0 (wp.array): The output dual variables array to be warm-started.
"""
wp.launch(
kernel=_warmstart_contact_constraints,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs:
model.time.dt,
model.info.total_cts_offset,
data.info.contact_cts_group_offset,
contacts.model_active_contacts,
contacts.wid,
contacts.cid,
contacts.material,
contacts.reaction,
contacts.velocity,
problem.data.P,
# Outputs:
x_0,
y_0,
z_0,
],
)
def _warmstart_from_solution(self, problem: DualProblem):
"""
Warm-starts the internal solver state from the stored solution variables.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.\n
This is needed during warm-starts in order to access the problem preconditioning.
"""
# Apply the dual-problem preconditioner to the stored solution
# in order to project to solution space of the PADMM variables
wp.launch(
kernel=_apply_dual_preconditioner_to_solution,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.vio,
problem.data.P,
# Outputs:
self._data.solution.lambdas,
self._data.solution.v_plus,
],
)
# Capture references to the warm-start variables
# depending on whether acceleration is used or not
if self._use_acceleration:
y_0 = self._data.state.y_hat
z_0 = self._data.state.z_hat
else:
y_0 = self._data.state.y_p
z_0 = self._data.state.z_p
# Copy the last solution into the warm-start variables
wp.copy(self._data.state.x_p, self._data.solution.lambdas)
wp.copy(y_0, self._data.solution.lambdas)
wp.copy(z_0, self._data.solution.v_plus)
self._warmstart_desaxce_correction(problem, z=z_0)
def _warmstart_from_containers(
self,
problem: DualProblem,
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino | None = None,
contacts: ContactsKamino | None = None,
):
"""
Warm-starts the internal solver state from the provided model data and limits and contacts containers.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.\n
This is needed during warm-starts in order to access the problem preconditioning.
model (ModelKamino): The model associated with the problem.
data (DataKamino): The model data associated with the problem.
limits (LimitsKamino | None): The limits container associated with the model.\n
If `None`, no warm-starting from limits is performed.
contacts (ContactsKamino | None): The contacts container associated with the model.\n
If `None`, no warm-starting from contacts is performed.
"""
# Capture references to the warm-start variables
# depending on whether acceleration is used or not
x_0 = self._data.state.x_p
if self._use_acceleration:
y_0 = self._data.state.y_hat
z_0 = self._data.state.z_hat
else:
y_0 = self._data.state.y_p
z_0 = self._data.state.z_p
# Warm-start each constraint group from constraint states cached in the data containers
if model.size.sum_of_num_joints > 0:
self._warmstart_joint_constraints(model, data, problem, x_0, y_0, z_0)
if limits is not None and limits.model_max_limits_host > 0:
self._warmstart_limit_constraints(model, data, limits, problem, x_0, y_0, z_0)
if contacts is not None and contacts.model_max_contacts_host > 0:
self._warmstart_contact_constraints(model, data, contacts, problem, x_0, y_0, z_0)
###
# Internals - Per-Step Operations
###
def _update_desaxce_correction(self, problem: DualProblem, z: wp.array):
"""
Launches a kernel to compute the De Saxce correction velocity using the previous dual variables.\n
The kernel is parallelized over the number of worlds and the maximum number of contacts.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
z (wp.array): The dual variable array from the previous iteration.\n
This can either be the acceleration variable `z_hat` or the standard dual variable `z_p`.
"""
wp.launch(
kernel=_compute_desaxce_correction,
dim=(self._size.num_worlds, self._size.max_of_max_contacts),
inputs=[
# Inputs:
problem.data.nc,
problem.data.cio,
problem.data.ccgo,
problem.data.vio,
problem.data.mu,
self._data.status,
z,
# Outputs:
self._data.state.s,
],
)
def _update_velocity_bias(self, problem: DualProblem, y: wp.array, z: wp.array):
"""
Launches a kernel to compute the total bias velocity vector using the previous state variables.\n
The kernel is parallelized over the number of worlds and the maximum number of total constraints.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
y (wp.array): The primal variable array from the previous iteration.\n
This can either be the acceleration variable `y_hat` or the standard primal variable `y_p`.
z (wp.array): The dual variable array from the previous iteration.\n
This can either be the acceleration variable `z_hat` or the standard dual variable `z_p`.
"""
wp.launch(
kernel=_compute_velocity_bias,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.vio,
problem.data.v_f,
self._data.config,
self._data.penalty,
self._data.status,
self._data.state.s,
self._data.state.x_p,
y,
z,
# Outputs:
self._data.state.v,
],
)
def _update_unconstrained_solution(self, problem: DualProblem):
"""
Launches a kernel to solve the unconstrained sub-problem for the primal variables.\n
For `DualProblem` solves, this corresponds to solving a linear system with the Delassus matrix.\n
The kernel is parallelized over the number of worlds and the maximum number of total constraints.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# TODO: We should do this in-place
# wp.copy(self._data.state.x, self._data.state.v)
# problem._delassus.solve_inplace(x=self._data.state.x)
problem._delassus.solve(v=self._data.state.v, x=self._data.state.x)
def _update_projection_argument(self, problem: DualProblem, z: wp.array):
"""
Launches a kernel to compute the argument for the projection operator onto the
feasible set using the accelerated state variables and the unconstrained solution.
The kernel is parallelized over the number of worlds and the maximum number of total constraints.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
z (wp.array): The dual variable array from the previous iteration.\n
This can either be the acceleration variable `z_hat` or the standard dual variable `z
"""
# Apply over-relaxation and compute the argument to the projection operator
wp.launch(
kernel=_compute_projection_argument,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.vio,
self._data.penalty,
self._data.status,
z,
self._data.state.x,
# Outputs:
self._data.state.y,
],
)
def _update_projection_to_feasible_set(self, problem: DualProblem):
"""
Launches a kernel to project the current primal variables
onto the feasible set defined by the constraint cone K.
The kernel is parallelized over the number of worlds and the maximum
number of unilateral constraints, i.e. 1D limits and 3D contacts.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Project to the feasible set defined by the cone K := R^{njd} x R_+^{nld} x K_{mu}^{nc}
wp.launch(
kernel=_project_to_feasible_cone,
dim=(self._size.num_worlds, self._size.max_of_max_unilaterals),
inputs=[
# Inputs:
problem.data.nl,
problem.data.nc,
problem.data.cio,
problem.data.lcgo,
problem.data.ccgo,
problem.data.vio,
problem.data.mu,
self._data.status,
# Outputs:
self._data.state.y,
],
)
def _update_complementarity_residuals(self, problem: DualProblem):
"""
Launches a kernel to compute the complementarity residuals from the current state variables.
The kernel is parallelized over the number of worlds and the maximum number of unilateral constraints.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Compute complementarity residual from the current state
wp.launch(
kernel=_compute_complementarity_residuals,
dim=(self._size.num_worlds, self._size.max_of_max_unilaterals),
inputs=[
# Inputs:
problem.data.nl,
problem.data.nc,
problem.data.vio,
problem.data.uio,
problem.data.lcgo,
problem.data.ccgo,
self._data.status,
self._data.state.x,
self._data.state.z,
# Outputs:
self._data.residuals.r_compl,
],
)
def _update_dual_variables_and_residuals(self, problem: DualProblem):
"""
Launches a kernel to update the dual variables and compute the
PADMM residuals from the current and previous state variables.
The kernel is parallelized over the number of worlds and the maximum number of total constraints.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Update the dual variables and compute primal-dual residuals from the current state
# NOTE: These are combined into a single kernel to reduce kernel launch overhead
wp.launch(
kernel=self._update_dual_variables_and_compute_primal_dual_residuals_kernel,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.vio,
problem.data.P,
self._data.config,
self._data.penalty,
self._data.status,
self._data.state.x,
self._data.state.y,
self._data.state.x_p,
self._data.state.y_p,
self._data.state.z_p,
# Outputs:
self._data.state.z,
self._data.residuals.r_primal,
self._data.residuals.r_dual,
self._data.residuals.r_dx,
self._data.residuals.r_dy,
self._data.residuals.r_dz,
],
)
# Compute complementarity residual from the current state
self._update_complementarity_residuals(problem)
def _update_dual_variables_and_residuals_accel(self, problem: DualProblem):
"""
Launches a kernel to update the dual variables and compute the
PADMM residuals from the current and accelerated state variables.
The kernel is parallelized over the number of worlds and the maximum number of total constraints.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Update the dual variables and compute primal-dual residuals from the current state
# NOTE: These are combined into a single kernel to reduce kernel launch overhead
wp.launch(
kernel=self._update_dual_variables_and_compute_primal_dual_residuals_kernel,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.vio,
problem.data.P,
self._data.config,
self._data.penalty,
self._data.status,
self._data.state.x,
self._data.state.y,
self._data.state.x_p,
self._data.state.y_hat,
self._data.state.z_hat,
# Outputs:
self._data.state.z,
self._data.residuals.r_primal,
self._data.residuals.r_dual,
self._data.residuals.r_dx,
self._data.residuals.r_dy,
self._data.residuals.r_dz,
],
)
# Compute complementarity residual from the current state
self._update_complementarity_residuals(problem)
def _update_convergence_check(self, problem: DualProblem):
"""
Launches a kernel to compute the infinity-norm of the PADMM residuals
using the current and previous state variables and check for convergence.
The kernel is parallelized over the number of worlds.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Compute infinity-norm of all residuals and check for convergence
tile_size = min(2048, 2 ** math.ceil(math.log(self._size.max_of_max_total_cts, 2)))
block_dim = min(256, tile_size // 8)
wp.launch_tiled(
kernel=_make_compute_infnorm_residuals_kernel(
tile_size,
self._size.max_of_max_total_cts,
self._size.max_of_max_limits + 3 * self._size.max_of_max_contacts,
),
dim=self._size.num_worlds,
block_dim=block_dim,
inputs=[
# Inputs:
problem.data.nl,
problem.data.nc,
problem.data.uio,
problem.data.dim,
problem.data.vio,
self._data.config,
self._data.residuals.r_primal,
self._data.residuals.r_dual,
self._data.residuals.r_compl,
# Outputs:
self._data.state.done,
self._data.status,
self._data.penalty,
self._data.linear_solver_atol,
],
)
def _update_convergence_check_accel(self, problem: DualProblem):
"""
Launches a kernel to compute the infinity-norm of the PADMM residuals
using the current and accelerated state variables and check for convergence.
The kernel is parallelized over the number of worlds.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Compute infinity-norm of all residuals and check for convergence
tile_size = min(2048, 2 ** math.ceil(math.log(self._size.max_of_max_total_cts, 2)))
block_dim = min(256, tile_size // 8)
wp.launch_tiled(
kernel=_make_compute_infnorm_residuals_accel_kernel(
tile_size,
self._size.max_of_max_total_cts,
self._size.max_of_max_limits + 3 * self._size.max_of_max_contacts,
),
dim=self._size.num_worlds,
block_dim=block_dim,
inputs=[
# Inputs:
problem.data.nl,
problem.data.nc,
problem.data.uio,
problem.data.dim,
problem.data.vio,
self._data.config,
self._data.residuals.r_primal,
self._data.residuals.r_dual,
self._data.residuals.r_compl,
self._data.residuals.r_dx,
self._data.residuals.r_dy,
self._data.residuals.r_dz,
self._data.state.a_p,
# Outputs:
self._data.state.done,
self._data.state.a,
self._data.status,
self._data.penalty,
self._data.linear_solver_atol,
],
)
def _update_acceleration(self, problem: DualProblem):
"""
Launches a kernel to update gradient acceleration and the accelerated state variables.
The kernel is parallelized over the number of worlds and the maximum number of total constraints.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
wp.launch(
kernel=_update_state_with_acceleration,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.vio,
self._data.status,
self._data.state.a,
self._data.state.y,
self._data.state.z,
self._data.state.a_p,
self._data.state.y_p,
self._data.state.z_p,
# Outputs:
self._data.state.y_hat,
self._data.state.z_hat,
],
)
def _update_solver_info(self, problem: DualProblem):
"""
Launches a kernel to update the solver info history from the current solver data.
The kernel is parallelized over the number of worlds.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# First reset the internal buffer arrays to zero
# to ensure we do not accumulate values across iterations
self.data.info.v_plus.zero_()
self.data.info.v_aug.zero_()
self._data.info.s.zero_()
# Collect convergence information from the current state
if problem.sparse:
# Initialize post-event constraint-space velocity from solution: v_plus = v_f + D @ lambda
wp.copy(self._data.info.v_plus, problem.data.v_f)
delassus_reg_prev = problem.delassus._eta
problem.delassus.set_regularization(None)
problem.delassus.gemv(
x=self._data.state.y,
y=self._data.info.v_plus,
world_mask=wp.ones((problem.data.num_worlds,), dtype=wp.int32, device=self.device),
alpha=1.0,
beta=1.0,
)
problem.delassus.set_regularization(delassus_reg_prev)
wp.launch(
kernel=self._collect_solver_info_kernel_sparse,
dim=self._size.num_worlds,
inputs=[
# Inputs:
problem.data.nl,
problem.data.nc,
problem.data.cio,
problem.data.lcgo,
problem.data.ccgo,
problem.data.dim,
problem.data.vio,
problem.data.mu,
problem.data.v_f,
problem.data.P,
self._data.state.s,
self._data.state.x,
self._data.state.x_p,
self._data.state.y,
self._data.state.y_p,
self._data.state.z,
self._data.state.z_p,
self._data.state.a,
self._data.penalty,
self._data.status,
# Outputs:
self._data.info.lambdas,
self._data.info.v_plus,
self._data.info.v_aug,
self._data.info.s,
self._data.info.offsets,
self._data.info.num_restarts,
self._data.info.num_rho_updates,
self._data.info.a,
self._data.info.norm_s,
self._data.info.norm_x,
self._data.info.norm_y,
self._data.info.norm_z,
self._data.info.f_ccp,
self._data.info.f_ncp,
self._data.info.r_dx,
self._data.info.r_dy,
self._data.info.r_dz,
self._data.info.r_primal,
self._data.info.r_dual,
self._data.info.r_compl,
self._data.info.r_pd,
self._data.info.r_dp,
self._data.info.r_comb,
self._data.info.r_comb_ratio,
self._data.info.r_ncp_primal,
self._data.info.r_ncp_dual,
self._data.info.r_ncp_compl,
self._data.info.r_ncp_natmap,
],
)
else:
wp.launch(
kernel=self._collect_solver_info_kernel,
dim=self._size.num_worlds,
inputs=[
# Inputs:
problem.data.nl,
problem.data.nc,
problem.data.cio,
problem.data.lcgo,
problem.data.ccgo,
problem.data.dim,
problem.data.vio,
problem.data.mio,
problem.data.mu,
problem.data.v_f,
problem.data.D,
problem.data.P,
self._data.state.sigma,
self._data.state.s,
self._data.state.x,
self._data.state.x_p,
self._data.state.y,
self._data.state.y_p,
self._data.state.z,
self._data.state.z_p,
self._data.state.a,
self._data.penalty,
self._data.status,
# Outputs:
self._data.info.lambdas,
self._data.info.v_plus,
self._data.info.v_aug,
self._data.info.s,
self._data.info.offsets,
self._data.info.num_restarts,
self._data.info.num_rho_updates,
self._data.info.a,
self._data.info.norm_s,
self._data.info.norm_x,
self._data.info.norm_y,
self._data.info.norm_z,
self._data.info.f_ccp,
self._data.info.f_ncp,
self._data.info.r_dx,
self._data.info.r_dy,
self._data.info.r_dz,
self._data.info.r_primal,
self._data.info.r_dual,
self._data.info.r_compl,
self._data.info.r_pd,
self._data.info.r_dp,
self._data.info.r_comb,
self._data.info.r_comb_ratio,
self._data.info.r_ncp_primal,
self._data.info.r_ncp_dual,
self._data.info.r_ncp_compl,
self._data.info.r_ncp_natmap,
],
)
def _update_previous_state(self):
"""
Updates the cached previous state variables with the current.
This function uses on-device memory copy operations.
"""
wp.copy(self._data.state.x_p, self._data.state.x)
wp.copy(self._data.state.y_p, self._data.state.y)
wp.copy(self._data.state.z_p, self._data.state.z)
def _update_previous_state_accel(self):
"""
Updates the cached previous acceleration and state variable with the current.
This function uses on-device memory copy operations.
"""
wp.copy(self._data.state.a_p, self._data.state.a)
# Cache previous state variables
self._update_previous_state()
###
# Internals - Post-Solve Operations
###
def _update_solution(self, problem: DualProblem):
"""
Launches a set of kernels to extract and post-process
the final solution from the internal PADMM state data.
Args:
problem (DualProblem): The dual forward dynamics problem to be solved.
"""
# Apply the dual preconditioner to recover the final PADMM state
wp.launch(
kernel=_apply_dual_preconditioner_to_state,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.vio,
problem.data.P,
# Outputs:
self._data.state.x,
self._data.state.y,
self._data.state.z,
],
)
# Update the De Saxce correction from terminal PADMM dual variables
wp.launch(
kernel=_compute_final_desaxce_correction,
dim=(self._size.num_worlds, self._size.max_of_max_contacts),
inputs=[
# Inputs:
problem.data.nc,
problem.data.cio,
problem.data.ccgo,
problem.data.vio,
problem.data.mu,
self._data.state.z,
# Outputs:
self._data.state.s,
],
)
# Update solution vectors from the terminal PADMM state
wp.launch(
kernel=_compute_solution_vectors,
dim=(self._size.num_worlds, self._size.max_of_max_total_cts),
inputs=[
# Inputs:
problem.data.dim,
problem.data.vio,
self._data.state.s,
self._data.state.y,
self._data.state.z,
# Outputs:
self._data.solution.v_plus,
self._data.solution.lambdas,
],
)
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/padmm/types.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Defines data types and containers used by the PADMM solver.
High-level Settings:
- :class:`PADMMPenaltyUpdate`:
Defines the ALM penalty update methods supported by the PADMM solver.
- :class:`PADMMWarmStartMode`:
Defines the warmstart modes supported by the PADMM solver.\n
Warp Structs:
- :class:`PADMMConfigStruct`:
Warp struct for on-device PADMM configurations.
- :class:`PADMMStatus`:
Warp struct for on-device PADMM solver status.
- :class:`PADMMPenalty`:
Warp struct for on-device PADMM penalty state.
Data Containers:
- :class:`PADMMState`:
A data container managing the internal PADMM solver state arrays.
- :class:`PADMMResiduals`:
A data container managing the PADMM solver residuals arrays.
- :class:`PADMMSolution`:
A data container managing the PADMM solver solution arrays.
- :class:`PADMMInfo`:
A data container managing arrays PADMM solver convergence info and performance metrics.
- :class:`PADMMData`:
The highest-level PADMM data container, bundling all other PADMM-related data into a single object.
"""
from __future__ import annotations
from enum import IntEnum
from typing import Any
import numpy as np
import warp as wp
from ....config import PADMMSolverConfig
from ...core.size import SizeKamino
from ...core.types import float32, int32, override, vec2f
###
# Module interface
###
__all__ = [
"PADMMConfigStruct",
"PADMMData",
"PADMMInfo",
"PADMMPenalty",
"PADMMPenaltyUpdate",
"PADMMResiduals",
"PADMMSolution",
"PADMMState",
"PADMMStatus",
"PADMMWarmStartMode",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
class PADMMPenaltyUpdate(IntEnum):
"""
An enumeration of the penalty update methods used in PADMM.
"""
FIXED = 0
"""
Fixed penalty:
`rho` is initialized to `config.rho_0`, remaining constant over the solve.
"""
# TODO: Implement adaptive penalty updates
# LINEAR = 1
# """
# Linear penalty update:
# `rho` is increased by a fixed factor.
# """
# BALANCED = 1
# """
# Balanced-residuals penalty update:
# `rho` is increased in order for the ratio of primal/dual residuals to be close to unity.
# """
# SPECTRAL = 2
# """
# Spectral penalty update:
# `rho` is increased by the spectral radius of the Delassus matrix.
# """
BALANCED = 1
"""
Balanced-residuals penalty update:
`rho` is increased in order for the ratio of primal/dual residuals to be close to unity.
"""
@classmethod
def from_string(cls, s: str) -> PADMMPenaltyUpdate:
"""Converts a string to a PADMMPenaltyUpdate enum value."""
try:
return cls[s.upper()]
except KeyError as e:
raise ValueError(f"Invalid PADMMPenaltyUpdate: {s}. Valid options are: {[e.name for e in cls]}") from e
@override
def __str__(self):
"""Returns a string representation of the PADMMPenaltyUpdate."""
return f"PADMMPenaltyUpdate.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the PADMMPenaltyUpdate."""
return self.__str__()
class PADMMWarmStartMode(IntEnum):
"""
An enumeration of the warmstart modes used in PADMM.
"""
NONE = -1
"""
No warmstart:
The solver does not use any warmstart information and starts from
scratch, i.e. performs a cold-start regardless of any cached state.
"""
INTERNAL = 0
"""
From internally cached solution:
The solver uses its values currently in the solution
container as warmstart information for the current solve.
"""
CONTAINERS = 1
"""
From externally cached solution containers:
The solver uses values from externally provided solution
containers as warmstart information for the current solve.
"""
@classmethod
def from_string(cls, s: str) -> PADMMWarmStartMode:
"""Converts a string to a PADMMWarmStartMode enum value."""
try:
return cls[s.upper()]
except KeyError as e:
raise ValueError(f"Invalid PADMMWarmStartMode: {s}. Valid options are: {[e.name for e in cls]}") from e
@override
def __str__(self):
"""Returns a string representation of the PADMMWarmStartMode."""
return f"PADMMWarmStartMode.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the PADMMWarmStartMode."""
return self.__str__()
@staticmethod
def parse_usd_attribute(value: str, context: dict[str, Any] | None = None) -> str:
"""Parse warmstart option imported from USD, following the KaminoSceneAPI schema."""
if not isinstance(value, str):
raise TypeError("Parser expects input of type 'str'.")
mapping = {"none": "none", "internal": "internal", "containers": "containers"}
lower_value = value.lower().strip()
if lower_value not in mapping:
raise ValueError(f"Warmstart parameter '{value}' is not a valid option.")
return mapping[lower_value]
@wp.struct
class PADMMConfigStruct:
"""
A warp struct to hold PADMM per-world solver configurations on the target device.
Intended to be used as ``dtype`` for warp arrays.
Attributes:
primal_tolerance (float32): The target tolerance on the total primal residual `r_primal`.\n
Must be greater than zero. Defaults to `1e-6`.
dual_tolerance (float32): The target tolerance on the total dual residual `r_dual`.\n
Must be greater than zero. Defaults to `1e-6`.
compl_tolerance (float32): The target tolerance on the total complementarity residual `r_compl`.\n
Must be greater than zero. Defaults to `1e-6`.
restart_tolerance (float32): The tolerance on the total combined primal-dual
residual `r_comb`, for determining when gradient acceleration should be restarted.\n
Must be greater than zero. Defaults to `0.999`.
eta (float32): The proximal regularization parameter.\n
Must be greater than zero. Defaults to `1e-5`.
rho_0 (float32): The initial value of the penalty parameter.\n
Must be greater than zero. Defaults to `1.0`.
a_0 (float32): The initial value of the acceleration parameter.\n
Must be greater than zero. Defaults to `1.0`.
alpha (float32): The threshold on primal-dual residual ratios,
used to determine when penalty updates should occur.\n
Must be greater than `1.0`. Defaults to `10.0`.
tau (float32): The factor by which the penalty is increased/decreased
when the primal-dual residual ratios exceed the threshold `alpha`.\n
Must be greater than `1.0`. Defaults to `1.5`.
max_iterations (int32): The maximum number of solver iterations.\n
Must be greater than zero. Defaults to `200`.
penalty_update_freq (int32): The permitted frequency of penalty updates.\n
If zero, no updates are performed. Otherwise, updates are performed every
`penalty_update_freq` iterations. Defaults to `10`.
penalty_update_method (int32): The penalty update method used to adapt the penalty parameter.\n
Defaults to `PADMMPenaltyUpdate.FIXED`.\n
See :class:`PADMMPenaltyUpdate` for details.
"""
primal_tolerance: float32
"""
The target tolerance on the total primal residual `r_primal`.\n
Must be greater than zero. Defaults to `1e-6`.
"""
dual_tolerance: float32
"""
The target tolerance on the total dual residual `r_dual`.\n
Must be greater than zero. Defaults to `1e-6`.
"""
compl_tolerance: float32
"""
The target tolerance on the total complementarity residual `r_compl`.\n
Must be greater than zero. Defaults to `1e-6`.
"""
restart_tolerance: float32
"""
The tolerance applied on the total combined primal-dual residual `r_comb`,
for determining when gradient acceleration should be restarted.\n
Must be greater than zero. Defaults to `0.999`.
"""
eta: float32
"""
The proximal regularization parameter.\n
Must be greater than zero. Defaults to `1e-5`.
"""
rho_0: float32
"""
The initial value of the ALM penalty parameter.\n
Must be greater than zero. Defaults to `1.0`.
"""
rho_min: float32
"""
The lower-bound applied to the ALM penalty parameter.\n
Must be greater than zero. Defaults to `1e-5`.
"""
a_0: float32
"""
The initial value of the acceleration parameter.\n
Must be greater than zero. Defaults to `1.0`.
"""
alpha: float32
"""
The threshold on primal-dual residual ratios,
used to determine when penalty updates should occur.\n
Must be greater than `1.0`. Defaults to `10.0`.
"""
tau: float32
"""
The factor by which the penalty is increased/decreased
when the primal-dual residual ratios exceed the threshold `alpha`.\n
Must be greater than `1.0`. Defaults to `1.5`.
"""
max_iterations: int32
"""
The maximum number of solver iterations.\n
Must be greater than zero. Defaults to `200`.
"""
penalty_update_freq: int32
"""
The permitted frequency of penalty updates.\n
If zero, no updates are performed. Otherwise, updates are performed every
`penalty_update_freq` iterations. Defaults to `10`.
"""
penalty_update_method: int32
"""
The penalty update method used to adapt the penalty parameter.\n
Defaults to `PADMMPenaltyUpdate.FIXED`.\n
See :class:`PADMMPenaltyUpdate` for details.
"""
linear_solver_tolerance: float32
"""
The default absolute tolerance for the iterative linear solver.\n
When positive, the iterative solver's atol is initialized to this value
at the start of each ADMM solve.\n
When zero, the iterative solver's own tolerance is left unchanged.\n
Must be non-negative. Defaults to `0.0`.
"""
linear_solver_tolerance_ratio: float32
"""
The ratio used to adapt the iterative linear solver tolerance from the ADMM primal residual.\n
When positive, the linear solver absolute tolerance is set to
`ratio * ||r_primal||_2` at each ADMM iteration.\n
When zero, the linear solver tolerance is not adapted (fixed tolerance).\n
Must be non-negative. Defaults to `0.0`.
"""
@wp.struct
class PADMMStatus:
"""
A warp struct to hold the PADMM per-world solver status on the target device.
Intended to be used as ``dtype`` for warp arrays.
Attributes:
converged (int32): A flag indicating whether the solver has converged (`1`) or not (`0`).\n
Used internally to keep track of per-world convergence status,
with `1` being set only when all total residuals have satisfied their
respective tolerances. If by the end of the solve the flag is still `0`,
it indicates that the solve reached the maximum number of iterations.
iterations (int32): The number of iterations performed by the solver.\n
Used internally to keep track of per-world iteration counts.
r_p (float32): The total primal residual.\n
Computed using the L-inf norm as `r_primal := || x - y ||_inf`.\n
r_d (float32): The total dual residual.\n
Computed using the L-inf norm as `r_dual := || eta * (x - x_p) + rho * (y - y_p) ||_inf`.\n
r_c (float32): The total complementarity residual.
Computed using the L-inf norm as `r_compl := || [x_k.T @ z_k] ||_inf`,
with `k` indexing each unilateral constraint set, i.e. 1D limits and 3D contacts.
r_dx (float32): The total primal iterate residual.\n
Computed as the L2-norm `r_dx := || x - x_p ||_2`.
r_dy (float32): The total slack iterate residual.\n
Computed as the L2-norm `r_dy := || y - y_p ||_2`.
r_dz (float32): The total dual iterate residual.\n
Computed as the L2-norm `r_dz := || z - z_p ||_2`.
r_a (float32): The total combined primal-dual residual used for acceleration restart checks.
Computed as `r_a := rho * r_dy + (1.0 / rho) * r_dz`.
r_a_p (float32): The previous total combined primal-dual residual.
r_a_pp (float32): An auxiliary cache of the previous total combined primal-dual residual.
restart (int32): A flag indicating whether gradient acceleration requires a restart (`1`) or not (`0`).\n
Used internally to keep track of per-world acceleration restarts.
num_restarts (int32): The number of acceleration restarts performed during the solve.
"""
converged: int32
"""
A flag indicating whether the solver has converged (`1`) or not (`0`).\n
Used internally to keep track of per-world convergence status,
with `1` being set only when all total residuals have satisfied their
respective tolerances. If by the end of the solve the flag is still `0`,
it indicates that the solve reached the maximum number of iterations.
"""
iterations: int32
"""
The number of iterations performed by the solver.\n
Used internally to keep track of per-world iteration counts.
"""
r_p: float32
"""
The total primal residual.\n
Computed using the L-inf norm as `r_primal := || x - y ||_inf`.\n
"""
r_d: float32
"""
The total dual residual.\n
Computed using the L-inf norm as `r_dual := || eta * (x - x_p) + rho * (y - y_p) ||_inf`.\n
"""
r_c: float32
"""
The total complementarity residual.
Computed using the L-inf norm as `r_compl := || [x_k.T @ z_k] ||_inf`,
with `k` indexing each unilateral constraint set, i.e. 1D limits and 3D contacts.
"""
r_dx: float32
"""
The total primal iterate residual.\n
Computed as the L2-norm `r_dx := || x - x_p ||_2`.
"""
r_dy: float32
"""
The total slack iterate residual.\n
Computed as the L2-norm `r_dy := || y - y_p ||_2`.
"""
r_dz: float32
"""
The total dual iterate residual.\n
Computed as the L2-norm `r_dz := || z - z_p ||_2`.
"""
r_a: float32
"""
The total combined primal-dual residual used for acceleration restart checks.\n
Computed as `r_a := rho * r_dy + (1.0 / rho) * r_dz`.
"""
r_a_p: float32
"""The previous total combined primal-dual residual."""
r_a_pp: float32
"""An auxiliary cache of the previous total combined primal-dual residual."""
restart: int32
"""
A flag indicating whether gradient acceleration requires a restart (`1`) or not (`0`).\n
Used internally to keep track of per-world acceleration restarts.
"""
num_restarts: int32
"""The number of acceleration restarts performed during the solve."""
@wp.struct
class PADMMPenalty:
"""
A warp struct to hold the on-device PADMM solver penalty state.
Intended to be used as ``dtype`` for warp arrays.
Attributes:
num_updates (int32): The number of penalty updates performed during a solve.\n
If a direct linear-system solver is used, this also
equals the number of matrix factorizations performed.
rho (float32): The current value of the ALM penalty parameter.\n
If adaptive penalty scheme is used, this value may change during
solve operations, while being lower-bounded by `config.rho_min`
to ensure numerical stability.
rho_p (float32): The previous value of the ALM penalty parameter.\n
As diagonal regularization of the lhs matrix (e.g. Delassus
operator) is performed in-place, we must keep track of the
previous penalty value to remove the previous regularization
before applying the current penalty value.
"""
num_updates: int32
"""
The number of penalty updates performed during a solve.\n
If a direct linear-system solver is used, this also
equals the number of matrix factorizations performed.
"""
rho: float32
"""
The current value of the ALM penalty parameter.\n
If adaptive penalty scheme is used, this value may change during
solve operations, while being lower-bounded by `config.rho_min`
to ensure numerical stability.
"""
rho_p: float32
"""
The previous value of the ALM penalty parameter.\n
As diagonal regularization of the lhs matrix (e.g. Delassus
operator) is performed in-place, we must keep track of the
previous penalty value to remove the previous regularization
before applying the current penalty value.
"""
###
# Containers
###
class PADMMState:
"""
A data container to bundle the internal PADMM state arrays.
Attributes:
done (wp.array): A single-element array containing the global
flag that indicates whether the solver should terminate.\n
Its value is initialized to ``num_worlds`` at the beginning of each
solve, and decremented by one for each world that has converged.\n
Shape of ``(1,)`` and type :class:`int`.
s (wp.array): The De Saxce correction velocities `s = Gamma(v_plus)`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
v (wp.array): The total bias velocity vector serving as the right-hand-side of the PADMM linear system.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
x (wp.array): The current PADMM primal variables.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
x_p (wp.array): The previous PADMM primal variables.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
y (wp.array): The current PADMM slack variables.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
y_p (wp.array): The previous PADMM slack variables.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
z (wp.array): The current PADMM dual variables.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
z_p (wp.array): The previous PADMM dual variables.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
y_hat (wp.array): The auxiliary PADMM slack variables used with gradient acceleration.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
z_hat (wp.array): The auxiliary PADMM dual variables used with gradient acceleration.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
a (wp.array): The current PADMM acceleration variables.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
a_p (wp.array): The previous PADMM acceleration variables.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
def __init__(self, size: SizeKamino | None = None, use_acceleration: bool = False):
"""
Initializes the PADMM solver state container.
If a model size is provided, allocates the state arrays accordingly.
Args:
size (SizeKamino | None): The model-size utility container holding the dimensionality of the model.
"""
self.done: wp.array | None = None
"""
A single-element array containing the global flag that indicates whether the solver should terminate.\n
Its value is initialized to ``num_worlds`` at the beginning of each
solve, and decremented by one for each world that has converged.\n
Shape of ``(1,)`` and type :class:`int32`.
"""
self.sigma: wp.array | None = None
"""
The scalar diagonal regularization applied uniformly across constraint dimensions.
This is computed as `sigma = eta + rho`, where `eta` is the
additional proximal parameter and `rho` is the ALM penalty.
It is stored as a 2-element vector representing `(sigma, sigma_p)`, where
`sigma` is the current and `sigma_p` is the previous value, used to undo
the prior regularization when the ALM penalty parameter `rho` is updated.
Shape of ``(num_worlds,)`` and type :class:`vec2f`.
"""
self.s: wp.array | None = None
"""
The De Saxce correction velocities `s = Gamma(v_plus)`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.v: wp.array | None = None
"""
The total bias velocity vector serving as the right-hand-side of the PADMM linear system.\n
It is computed from the PADMM state and proximal parameters `eta` and `rho`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.x: wp.array | None = None
"""
The current PADMM primal variables.
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.x_p: wp.array | None = None
"""
The previous PADMM primal variables.
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.y: wp.array | None = None
"""
The current PADMM slack variables.
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.y_p: wp.array | None = None
"""
The previous PADMM slack variables.
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.z: wp.array | None = None
"""
The current PADMM dual variables.
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.z_p: wp.array | None = None
"""
The previous PADMM dual variables.
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.y_hat: wp.array | None = None
"""
The auxiliary PADMM slack variables used with gradient acceleration.\n
Only allocated if acceleration is enabled.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.z_hat: wp.array | None = None
"""
The auxiliary PADMM dual variables used with gradient acceleration.\n
Only allocated if acceleration is enabled.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.a: wp.array | None = None
"""
The current PADMM acceleration variables.\n
Only allocated if acceleration is enabled.\n
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
self.a_p: wp.array | None = None
"""
The previous PADMM acceleration variables.\n
Only allocated if acceleration is enabled.\n
Shape of ``(num_worlds,)`` and type :class:`float32`.
"""
# Perform memory allocations if model size is specified
if size is not None:
self.finalize(size, use_acceleration)
def finalize(self, size: SizeKamino, use_acceleration: bool = False):
"""
Allocates the PADMM solver state arrays based on the model size.
Args:
size (SizeKamino): The model-size utility container holding the dimensionality of the model.
"""
# Allocate per-world solver done flags
self.done = wp.zeros(1, dtype=int32)
# Allocate primary state variables
self.sigma = wp.zeros(size.num_worlds, dtype=vec2f)
self.s = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.v = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.x = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.x_p = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.y = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.y_p = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.z = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.z_p = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
# Allocate auxiliary state variables used with acceleration
if use_acceleration:
self.y_hat = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.z_hat = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.a = wp.zeros(size.num_worlds, dtype=float32)
self.a_p = wp.zeros(size.num_worlds, dtype=float32)
def reset(self, use_acceleration: bool = False):
"""
Resets all PADMM state arrays.
Specifically:
- PADMM state arrays (primal, slack, dual, rhs, De Saxce correction) are set to zeros.
- If acceleration is enabled, the momentum arrays `a_p` and `a` are set to ones.
Args:
use_acceleration (bool):
Whether to reset the acceleration state variables.\n
If `True`, auxiliary state variables and acceleration scales are reset as well.\n
Defaults to `False`.
"""
# Reset primary state variables
self.done.zero_()
self.sigma.zero_()
self.s.zero_()
self.v.zero_()
self.x.zero_()
self.x_p.zero_()
self.y.zero_()
self.y_p.zero_()
self.z.zero_()
self.z_p.zero_()
# Optionally reset acceleration state
if use_acceleration:
# Reset auxiliary state variables
self.y_hat.zero_()
self.z_hat.zero_()
# Reset acceleration scale variables
self.a.fill_(1.0)
self.a_p.fill_(1.0)
class PADMMResiduals:
"""
A data container to bundle the internal PADMM residual arrays.
Attributes:
r_primal (wp.array): The PADMM primal residual vector, computed as `r_primal := x - y`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
r_dual (wp.array): The PADMM dual residual vector, computed as `r_dual := eta * (x - x_p) + rho * (y - y_p)`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
r_compl (wp.array): The PADMM complementarity residual vector, computed as `r_compl := [x_j.dot(z_j)]`,\n
where `j` indexes each unilateral constraint set (i.e. 1D limits and 3D contacts).\n
Shape of ``(sum_of_num_unilateral_cts,)`` and type :class:`float32`.
r_dx (wp.array): The PADMM primal iterate residual vector, computed as `r_dx := x - x_p`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
r_dy (wp.array): The PADMM slack iterate residual vector, computed as `r_dy := y - y_p`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
r_dz (wp.array): The PADMM dual iterate residual vector, computed as `r_dz := z - z_p`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
def __init__(self, size: SizeKamino | None = None, use_acceleration: bool = False):
"""
Initializes the PADMM residuals container.
If a model size is provided, allocates the residuals arrays accordingly.
Args:
size (SizeKamino | None): The model-size utility container holding the dimensionality of the model.
"""
self.r_primal: wp.array | None = None
"""
The PADMM primal residual vector, computed as `r_primal := x - y`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.r_dual: wp.array | None = None
"""
The PADMM dual residual vector, computed as `r_dual := eta * (x - x_p) + rho * (y - y_p)`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.r_compl: wp.array | None = None
"""
The PADMM complementarity residual vector, computed as `r_compl := [x_j.dot(z_j)]`,\n
where `j` indexes each unilateral constraint set (i.e. 1D limits and 3D contacts).\n
Shape of ``(sum_of_num_unilateral_cts,)`` and type :class:`float32`.
"""
self.r_dx: wp.array | None = None
"""
The PADMM primal iterate residual vector, computed as `r_dx := x - x_p`.\n
Only allocated if acceleration is enabled.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.r_dy: wp.array | None = None
"""
The PADMM slack iterate residual vector, computed as `r_dy := y - y_p`.\n
Only allocated if acceleration is enabled.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.r_dz: wp.array | None = None
"""
The PADMM dual iterate residual vector, computed as `r_dz := z - z_p`.\n
Only allocated if acceleration is enabled.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
# Perform memory allocations if model size is specified
if size is not None:
self.finalize(size, use_acceleration)
def finalize(self, size: SizeKamino, use_acceleration: bool = False):
"""
Allocates the residuals arrays based on the model size.
Args:
size (SizeKamino): The model-size utility container holding the dimensionality of the model.
use_acceleration (bool): Flag indicating whether to allocate arrays used with acceleration.
"""
# Allocate the main residuals arrays
self.r_primal = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.r_dual = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.r_compl = wp.zeros(size.sum_of_max_unilaterals, dtype=float32)
# Optionally allocate iterate residuals used when acceleration is enabled
if use_acceleration:
self.r_dx = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.r_dy = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.r_dz = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
def zero(self, use_acceleration: bool = False):
"""
Resets all PADMM residual arrays to zeros.
"""
self.r_primal.zero_()
self.r_dual.zero_()
self.r_compl.zero_()
if use_acceleration:
self.r_dx.zero_()
self.r_dy.zero_()
self.r_dz.zero_()
class PADMMSolution:
"""
An interface container to the PADMM solver solution arrays.
Attributes:
lambdas (wp.array): The constraint reactions (i.e. impulses) solution array.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
v_plus (wp.array): The post-event constraint-space velocities solution array.
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
def __init__(self, size: SizeKamino | None = None):
"""
Initializes the PADMM solution container.
If a model size is provided, allocates the solution arrays accordingly.
Args:
size (SizeKamino | None): The model-size utility container holding the dimensionality of the model.
"""
self.lambdas: wp.array | None = None
"""
The constraint reactions (i.e. impulses) solution array.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.v_plus: wp.array | None = None
"""
The post-event constraint-space velocities solution array.
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
# Perform memory allocations if model size is specified
if size is not None:
self.finalize(size)
def finalize(self, size: SizeKamino):
"""
Allocates the PADMM solution arrays based on the model size.
Args:
size (SizeKamino): The model-size utility container holding the dimensionality of the model.
"""
self.lambdas = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.v_plus = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
def zero(self):
"""
Resets all PADMM solution arrays to zeros.
"""
self.lambdas.zero_()
self.v_plus.zero_()
class PADMMInfo:
"""
An interface container to hold the PADMM solver convergence info arrays.
Attributes:
lambdas (wp.array): The constraint reactions (i.e. impulses) of each world.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
v_plus (wp.array): The post-event constraint-space velocities of each world.\n
This is computed using the current solution as: `v_plus := v_f + D @ lambdas`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
v_aug (wp.array): The post-event augmented constraint-space velocities of each world.\n
This is computed using the current solution as: `v_aug := v_plus + s`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
s (wp.array): The De Saxce correction velocities of each world.\n
This is computed using the current solution as: `s := Gamma(v_plus)`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
offsets (wp.array): The residuals index offset of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
num_restarts (wp.array): History of the number of acceleration restarts performed for each world.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`int32`.
num_rho_updates (wp.array): History of the number of penalty updates performed for each world.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`int32`.
a (wp.array): History of PADMM acceleration variables.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
norm_s (wp.array): History of the L2 norm of De Saxce correction velocities.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
norm_x (wp.array): History of the L2 norm of primal variables.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
norm_y (wp.array): History of the L2 norm of slack variables.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
norm_z (wp.array): History of the L2 norm of dual variables.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
f_ccp (wp.array): History of CCP optimization objectives.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
f_ncp (wp.array): History of the NCP optimization objectives.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_dx (wp.array): History of the total primal iterate residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_dy (wp.array): History of the total slack iterate residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_dz (wp.array): History of the total dual iterate residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_primal (wp.array): History of the total primal residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_dual (wp.array): History of the total dual residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_compl (wp.array): History of the total complementarity residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_comb (wp.array): History of the total combined primal-dual residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_comb_ratio (wp.array): History of the combined primal-dual residual ratio.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_ncp_primal (wp.array): History of NCP primal residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_ncp_dual (wp.array): History of NCP dual residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_ncp_compl (wp.array): History of NCP complementarity residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
r_ncp_natmap (wp.array): History of NCP natural-map residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
Notes:
- The length of the arrays is determined by the maximum number of iterations
and is filled up to the number of iterations performed by the solver on each
solve. This allows for post-solve analysis of the convergence behavior.
- This has a significant impact on solver performance and memory usage, so it
is recommended to only enable this for testing and debugging purposes.
"""
def __init__(
self,
size: SizeKamino | None = None,
max_iters: int | None = None,
use_acceleration: bool = False,
):
"""
Initializes the PADMM solver info container.
If a model size is provided, allocates the solution arrays accordingly.
Args:
size (SizeKamino | None): The model-size utility container holding the dimensionality of the model.
max_iters (int | None): The maximum number of iterations for which to allocate convergence data.
"""
self.lambdas: wp.array | None = None
"""
The constraint reactions (i.e. impulses) of each world.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.v_plus: wp.array | None = None
"""
The post-event constraint-space velocities of each world.\n
This is computed using the current solution as: `v_plus := v_f + D @ lambdas`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.v_aug: wp.array | None = None
"""
The post-event augmented constraint-space velocities of each world.\n
This is computed using the current solution as: `v_aug := v_plus + s`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.s: wp.array | None = None
"""
The De Saxce correction velocities of each world.\n
This is computed using the current solution as: `s := Gamma(v_plus)`.\n
Shape of ``(sum_of_max_total_cts,)`` and type :class:`float32`.
"""
self.offsets: wp.array | None = None
"""
The residuals index offset of each world.\n
Shape of ``(num_worlds,)`` and type :class:`int32`.
"""
self.num_restarts: wp.array | None = None
"""
History of the number of acceleration restarts performed for each world.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`int32`.
"""
self.num_rho_updates: wp.array | None = None
"""
History of the number of penalty updates performed for each world.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`int32`.
"""
self.a: wp.array | None = None
"""
History of PADMM acceleration variables.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.norm_s: wp.array | None = None
"""
History of the L2 norm of De Saxce correction velocities.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.norm_x: wp.array | None = None
"""
History of the L2 norm of primal variables.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.norm_y: wp.array | None = None
"""
History of the L2 norm of slack variables.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.norm_z: wp.array | None = None
"""
History of the L2 norm of dual variables.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.f_ccp: wp.array | None = None
"""
History of CCP optimization objectives.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.f_ncp: wp.array | None = None
"""
History of the NCP optimization objectives.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_dx: wp.array | None = None
"""
History of the total primal iterate residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_dy: wp.array | None = None
"""
History of the total slack iterate residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_dz: wp.array | None = None
"""
History of the total dual iterate residual.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_primal: wp.array | None = None
"""
History of PADMM primal residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_dual: wp.array | None = None
"""
History of PADMM dual residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_compl: wp.array | None = None
"""
History of PADMM complementarity residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_pd: wp.array | None = None
"""
History of PADMM primal-dual residual ratio.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_dp: wp.array | None = None
"""
History of PADMM dual-primal residual ratio.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_comb: wp.array | None = None
"""
History of PADMM combined residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_comb_ratio: wp.array | None = None
"""
History of PADMM combined residuals ratio.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_ncp_primal: wp.array | None = None
"""
History of NCP primal residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_ncp_dual: wp.array | None = None
"""
History of NCP dual residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_ncp_compl: wp.array | None = None
"""
History of NCP complementarity residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
self.r_ncp_natmap: wp.array | None = None
"""
History of NCP natural-map residuals.\n
Shape of ``(num_worlds * max_iters,)`` and type :class:`float32`.
"""
# Perform memory allocations if model size is specified
if size is not None:
self.finalize(size=size, max_iters=max_iters, use_acceleration=use_acceleration)
def finalize(self, size: SizeKamino, max_iters: int, use_acceleration: bool = False):
"""
Allocates the PADMM solver info arrays based on the model size and maximum number of iterations.
Args:
size (SizeKamino): The model-size utility container holding the dimensionality of the model.
max_iters (int): The maximum number of iterations for which to allocate convergence data.
Raises:
ValueError: If either ``size.num_worlds`` or ``max_iters`` are not positive integers.
"""
# Ensure num_worlds is valid
if not isinstance(size.num_worlds, int) or size.num_worlds <= 0:
raise ValueError("num_worlds must be a positive integer specifying the number of worlds.")
# Ensure max_iters is valid
if not isinstance(max_iters, int) or max_iters <= 0:
raise ValueError("max_iters must be a positive integer specifying the maximum number of iterations.")
# Allocate intermediate arrays
self.lambdas = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.v_plus = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.v_aug = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
self.s = wp.zeros(size.sum_of_max_total_cts, dtype=float32)
# Compute the index offsets for the info of each world
maxsize = max_iters * size.num_worlds
offsets = [max_iters * i for i in range(size.num_worlds)]
# Allocate the on-device solver info data arrays
self.offsets = wp.array(offsets, dtype=int32)
self.num_rho_updates = wp.zeros(maxsize, dtype=int32)
self.norm_s = wp.zeros(maxsize, dtype=float32)
self.norm_x = wp.zeros(maxsize, dtype=float32)
self.norm_y = wp.zeros(maxsize, dtype=float32)
self.norm_z = wp.zeros(maxsize, dtype=float32)
self.r_dx = wp.zeros(maxsize, dtype=float32)
self.r_dy = wp.zeros(maxsize, dtype=float32)
self.r_dz = wp.zeros(maxsize, dtype=float32)
self.f_ccp = wp.zeros(maxsize, dtype=float32)
self.f_ncp = wp.zeros(maxsize, dtype=float32)
self.r_primal = wp.zeros(maxsize, dtype=float32)
self.r_dual = wp.zeros(maxsize, dtype=float32)
self.r_compl = wp.zeros(maxsize, dtype=float32)
self.r_pd = wp.zeros(maxsize, dtype=float32)
self.r_dp = wp.zeros(maxsize, dtype=float32)
self.r_ncp_primal = wp.zeros(maxsize, dtype=float32)
self.r_ncp_dual = wp.zeros(maxsize, dtype=float32)
self.r_ncp_compl = wp.zeros(maxsize, dtype=float32)
self.r_ncp_natmap = wp.zeros(maxsize, dtype=float32)
if use_acceleration:
self.num_restarts = wp.zeros(maxsize, dtype=int32)
self.a = wp.zeros(maxsize, dtype=float32)
self.r_comb = wp.zeros(maxsize, dtype=float32)
self.r_comb_ratio = wp.zeros(maxsize, dtype=float32)
def zero(self, use_acceleration: bool = False):
"""
Resets all PADMM solver info arrays to zeros.
"""
self.lambdas.zero_()
self.v_plus.zero_()
self.v_aug.zero_()
self.s.zero_()
self.num_rho_updates.zero_()
self.norm_s.zero_()
self.norm_x.zero_()
self.norm_y.zero_()
self.norm_z.zero_()
self.f_ccp.zero_()
self.f_ncp.zero_()
self.r_dx.zero_()
self.r_dy.zero_()
self.r_dz.zero_()
self.r_primal.zero_()
self.r_dual.zero_()
self.r_compl.zero_()
self.r_pd.zero_()
self.r_dp.zero_()
self.r_ncp_primal.zero_()
self.r_ncp_dual.zero_()
self.r_ncp_compl.zero_()
self.r_ncp_natmap.zero_()
if use_acceleration:
self.num_restarts.zero_()
self.a.zero_()
self.r_comb.zero_()
self.r_comb_ratio.zero_()
class PADMMData:
"""
A high-level container to manage all internal PADMM solver data.
Attributes:
config (wp.array): Array of per-world solver configurations,
of type :class:`PADMMConfigStruct` and shape ``(num_worlds,)``.\n
Each element is the on-device version of :class:`PADMMConfig`.
status (wp.array): Array of per-world solver status,
of type :class:`PADMMStatus` and shape ``(num_worlds,)``.\n
Each element holds the status of the solver on
solving the dynamics of the corresponding world.
penalty (wp.array): Array of per-world ALM penalty states,
of type :class:`PADMMPenalty` and shape ``(num_worlds,)``.\n
Each element holds the current and previous ALM penalty `rho`,
as well as additional meta-data regarding it's adaptation.
state (PADMMState): A container holding the PADMM state variable arrays.
residuals (PADMMResiduals): A container holding the PADMM residuals arrays.
solution (PADMMSolution): A container holding the PADMM solution arrays.
info (PADMMInfo): A container holding the PADMM solver convergence info arrays.
"""
def __init__(
self,
size: SizeKamino | None = None,
max_iters: int = 0,
use_acceleration: bool = False,
collect_info: bool = False,
device: wp.DeviceLike = None,
):
"""
Initializes a PADMM solver data container.
Args:
size (SizeKamino): The model-size utility container holding the dimensionality of the model.
max_iters (int): The maximum number of iterations for which to allocate convergence data.
collect_info (bool): Set to `True` to allocate data for reporting solver convergence info.
device (wp.DeviceLike): The target Warp device on which all data will be allocated.
Raises:
ValueError: If either ``size.num_worlds`` or ``max_iters`` are not positive integers.
"""
self.config: wp.array | None = None
"""
Array of on-device PADMM solver configs.\n
Shape is (num_worlds,) and type :class:`PADMMConfigStruct`.
"""
self.status: wp.array | None = None
"""
Array of PADMM solver status.\n
Shape is (num_worlds,) and type :class:`PADMMStatus`.
"""
self.penalty: wp.array | None = None
"""
Array of PADMM solver penalty parameters.\n
Shape is (num_worlds,) and type :class:`PADMMPenalty`.
"""
self.state: PADMMState | None = None
"""The PADMM internal solver state container."""
self.residuals: PADMMResiduals | None = None
"""The PADMM residuals container."""
self.solution: PADMMSolution | None = None
"""The PADMM solution container."""
self.info: PADMMInfo | None = None
"""The (optional) PADMM solver info container."""
self.linear_solver_atol: wp.array | None = None
"""
Per-world absolute tolerance array for the iterative linear solver.\n
Shape is (num_worlds,) and type :class:`float32`.
"""
# Perform memory allocations if model size is specified
if size is not None:
self.finalize(
size=size,
max_iters=max_iters,
use_acceleration=use_acceleration,
collect_info=collect_info,
device=device,
)
def finalize(
self,
size: SizeKamino,
max_iters: int = 0,
use_acceleration: bool = False,
collect_info: bool = False,
device: wp.DeviceLike = None,
):
"""
Allocates the PADMM solver data based on the model size and maximum number of iterations.
Args:
size (SizeKamino): The model-size utility container holding the dimensionality of the model.
max_iters (int): The maximum number of iterations for which to allocate convergence data.
collect_info (bool): Set to `True` to allocate data for reporting solver convergence info.
device (wp.DeviceLike): The target Warp device on which all data will be allocated.
Raises:
ValueError: If either ``size.num_worlds`` or ``max_iters`` are not positive integers.
"""
with wp.ScopedDevice(device):
self.config = wp.zeros(shape=(size.num_worlds,), dtype=PADMMConfigStruct)
self.status = wp.zeros(shape=(size.num_worlds,), dtype=PADMMStatus)
self.penalty = wp.zeros(shape=(size.num_worlds,), dtype=PADMMPenalty)
self.state = PADMMState(size, use_acceleration)
self.residuals = PADMMResiduals(size, use_acceleration)
self.solution = PADMMSolution(size)
self.linear_solver_atol = wp.full(shape=(size.num_worlds,), value=np.finfo(np.float32).eps, dtype=float32)
if collect_info and max_iters > 0:
self.info = PADMMInfo(size, max_iters, use_acceleration)
###
# Utilities
###
def convert_config_to_struct(config: PADMMSolverConfig) -> PADMMConfigStruct:
"""
Converts the host-side config to the corresponding device-side object.
Returns:
PADMMConfigStruct: The solver config as a warp struct.
"""
config_struct = PADMMConfigStruct()
config_struct.primal_tolerance = config.primal_tolerance
config_struct.dual_tolerance = config.dual_tolerance
config_struct.compl_tolerance = config.compl_tolerance
config_struct.restart_tolerance = config.restart_tolerance
config_struct.eta = config.eta
config_struct.rho_0 = config.rho_0
config_struct.rho_min = config.rho_min
config_struct.a_0 = config.a_0
config_struct.alpha = config.alpha
config_struct.tau = config.tau
config_struct.max_iterations = config.max_iterations
config_struct.penalty_update_freq = config.penalty_update_freq
config_struct.penalty_update_method = PADMMPenaltyUpdate.from_string(config.penalty_update_method)
config_struct.linear_solver_tolerance = config.linear_solver_tolerance
config_struct.linear_solver_tolerance_ratio = config.linear_solver_tolerance_ratio
return config_struct
================================================
FILE: newton/_src/solvers/kamino/_src/solvers/warmstart.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides mechanisms to warm-start constraints.
This module provides warm-starting mechanisms for unilateral limit and contact constraints,
in the form of the `WarmstarterLimits` and `WarmstarterContacts` classes, respectively.
These classes utilize cached constraint data from previous simulation steps to initialize
the current step's constraints, improving solver convergence.
The warm-starting process involves matching current constraints to cached ones using unique keys
(e.g., joint-DoF index pairs for limits and geom-pair keys for contacts) and, for contacts,
also considering contact point positions to ensure accurate matching.
For contacts, if a direct match based on position is not found, optional fallback mechanisms using
the net force/wrench on the associated body CoMs are employed to estimate the warm-started reaction.
See the :class:`WarmstarterLimits` and :class:`WarmstarterContacts` classes for detailed usage.
"""
from __future__ import annotations
from enum import IntEnum
import warp as wp
from ..core.data import DataKamino
from ..core.math import contact_wrench_matrix_from_points
from ..core.model import ModelKamino
from ..core.types import float32, int32, override, quatf, transformf, uint64, vec2f, vec2i, vec3f, vec6f
from ..geometry.contacts import ContactsKamino, ContactsKaminoData
from ..geometry.keying import KeySorter, binary_search_find_range_start, make_bitmask
from ..kinematics.limits import LimitsKamino, LimitsKaminoData
from ..solvers.padmm.math import project_to_coulomb_cone
###
# Module interface
###
__all__ = [
"WarmstarterContacts",
"WarmstarterLimits",
"warmstart_contacts_by_matched_geom_pair_key_and_position",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _warmstart_limits_by_matched_jid_dof_key(
# Inputs - Previous:
sorted_limit_keys_old: wp.array(dtype=uint64),
sorted_to_unsorted_map_old: wp.array(dtype=int32),
num_active_limits_old: wp.array(dtype=int32),
limit_velocity_old: wp.array(dtype=float32),
limit_reaction_old: wp.array(dtype=float32),
# Inputs - Next:
num_active_limits_new: wp.array(dtype=int32),
limit_key_new: wp.array(dtype=uint64),
# Outputs:
limit_reaction_new: wp.array(dtype=float32),
limit_velocity_new: wp.array(dtype=float32),
):
"""
Match current limits to previous timestep limits using joint-DoF index pair keys.
For each current limit, finds a matching limit from the
previous step using a binary-search on sorted keys (O(log n))
"""
# Retrieve the limit index as the thread index
lid = wp.tid()
# Perform early exit if out of active bounds
if lid >= num_active_limits_new[0]:
return
# Retrieve number of active old limits and the target key to search for
num_active_old = num_active_limits_old[0]
target_key = limit_key_new[lid]
# Perform binary search to find the start index of the
# target key - i.e. assuming a joint-DoF index pair key
start = binary_search_find_range_start(0, num_active_old, target_key, sorted_limit_keys_old)
# If key not found, then mark as a new limit and skip further processing
# NOTE: This means that a new limit has become active
if start == -1:
limit_reaction_new[lid] = 0.0
limit_velocity_new[lid] = 0.0
else:
# Retrieve the old limit index from the sorted->unsorted map
lid_old = sorted_to_unsorted_map_old[start]
reaction_old = limit_reaction_old[lid_old]
velocity_old = limit_velocity_old[lid_old]
# Retrieve the matched limit reaction and velocity from
# the old limit and store them to the new limit
limit_reaction_new[lid] = wp.max(reaction_old, 0.0)
limit_velocity_new[lid] = wp.max(velocity_old, 0.0)
@wp.kernel
def _warmstart_contacts_by_matched_geom_pair_key_and_position(
# Inputs - Common:
tolerance: float32,
time_dt: wp.array(dtype=float32),
body_q_i: wp.array(dtype=transformf),
body_u_i: wp.array(dtype=vec6f),
# Inputs - Previous:
sorted_contact_keys_old: wp.array(dtype=uint64),
sorted_to_unsorted_map_old: wp.array(dtype=int32),
num_active_contacts_old: wp.array(dtype=int32),
contact_position_B_old: wp.array(dtype=vec3f),
contact_frame_old: wp.array(dtype=quatf),
contact_reaction_old: wp.array(dtype=vec3f),
contact_velocity_old: wp.array(dtype=vec3f),
# Inputs - Next:
num_active_contacts_new: wp.array(dtype=int32),
contact_key_new: wp.array(dtype=uint64),
contact_wid_new: wp.array(dtype=int32),
contact_bid_AB_new: wp.array(dtype=vec2i),
contact_position_B_new: wp.array(dtype=vec3f),
contact_frame_new: wp.array(dtype=quatf),
# Outputs:
contact_reaction_new: wp.array(dtype=vec3f),
contact_velocity_new: wp.array(dtype=vec3f),
):
"""
Match current contacts to previous timestep contacts using geom-pair keys and relative distance.
For each current contact, finds matching contact from previous step by:
1. Binary search on sorted keys (O(log n))
2. Linear scan through matching keys to find matching contact point positions (O(m))
"""
# Retrieve the contact index as the thread index
cid = wp.tid()
# Perform early exit if out of active bounds
if cid >= num_active_contacts_new[0]:
return
# Retrieve number of active old contacts and the target key to search for
num_active_old = num_active_contacts_old[0]
target_key = contact_key_new[cid]
# Initialize the target reaction and velocity to zero
# to account for the case where no matching contact is found
target_reaction = vec3f(0.0)
target_velocity = vec3f(0.0)
# Perform binary search to find the start index of the target key - i.e. assuming a geom-pair key
start = binary_search_find_range_start(0, num_active_old, target_key, sorted_contact_keys_old)
# If key not found, then mark as a new contact and skip further processing
# NOTE: This means that a new geom-pair collision has occurred
if start == -1:
contact_reaction_new[cid] = target_reaction
contact_velocity_new[cid] = target_velocity
return
# Retrieve the new contact position on the corresponding geom B
# NOTE: We only need to match based on one contact point position
# on body/geom B in order to handle the general case of static bodies
# as geom B is by definition always the non-static body in a contact pair
r_B_target = contact_position_B_new[cid]
R_k_target = wp.quat_to_matrix(contact_frame_new[cid])
# Retrieve the timestep delta time for the contact's associated body
dt = time_dt[contact_wid_new[cid]]
# Retrieve the body indices and states for the contact's associated bodies
bid_AB = contact_bid_AB_new[cid]
r_B = wp.transform_get_translation(body_q_i[bid_AB[1]])
u_B = body_u_i[bid_AB[1]]
# Iterate through all old contacts with the same key and check if contacts match
# based on distance of contact points after accounting for associated body motion
# NOTE: For the comparison, new_idx -> cid, old_idx -> sorted_to_unsorted_map_old[start + k]
k = int32(0)
old_key = sorted_contact_keys_old[start]
while target_key == old_key:
# Retrieve the old contact index from the sorted->unsorted map
cid_old = sorted_to_unsorted_map_old[start + k]
r_k_B_old = contact_position_B_old[cid_old]
W_Bk_T = wp.transpose(contact_wrench_matrix_from_points(r_k_B_old, r_B))
r_B_candidate = r_k_B_old + dt * (W_Bk_T @ u_B)
# Compute and check the distance to the target contact positions
dr_B = wp.length(r_B_candidate - r_B_target)
if dr_B < tolerance:
# When a match is found, retrieve the contact reaction and velocity
# from the old contact and transform them to the new contact frame
q_k_old = contact_frame_old[cid_old]
lambda_k_old = contact_reaction_old[cid_old]
v_k_old = contact_velocity_old[cid_old]
R_k_old = wp.quat_to_matrix(q_k_old)
R_k_old_to_new = wp.transpose(R_k_target) @ R_k_old
target_reaction = R_k_old_to_new @ lambda_k_old
target_velocity = R_k_old_to_new @ v_k_old
break
# Update the current old-key to check in the next iteration
k += 1
old_key = sorted_contact_keys_old[start + k]
# Store the new contact reaction and velocity
# NOTE: These will remain zero if no matching contact is found
contact_reaction_new[cid] = target_reaction
contact_velocity_new[cid] = target_velocity
@wp.kernel
def _warmstart_contacts_from_geom_pair_net_force(
# Inputs - Common:
scaling: float32,
body_q_i: wp.array(dtype=transformf),
body_u_i: wp.array(dtype=vec6f),
# Inputs - Previous:
sorted_contact_keys_old: wp.array(dtype=uint64),
sorted_to_unsorted_map_old: wp.array(dtype=int32),
num_active_contacts_old: wp.array(dtype=int32),
contact_frame_old: wp.array(dtype=quatf),
contact_reaction_old: wp.array(dtype=vec3f),
# Inputs - Next:
num_active_contacts_new: wp.array(dtype=int32),
contact_key_new: wp.array(dtype=uint64),
contact_bid_AB_new: wp.array(dtype=vec2i),
contact_position_A_new: wp.array(dtype=vec3f),
contact_position_B_new: wp.array(dtype=vec3f),
contact_frame_new: wp.array(dtype=quatf),
contact_material_new: wp.array(dtype=vec2f),
# Outputs:
contact_reaction_new: wp.array(dtype=vec3f),
contact_velocity_new: wp.array(dtype=vec3f),
):
"""
Match current contacts to previous timestep contacts using geom-pair keys and relative distance.
For each current contact, finds matching contact from previous step by:
1. Binary search on sorted keys (O(log n))
2. Linear scan through matching keys to find matching contact point positions (O(m))
"""
# Retrieve the contact index as the thread index
cid = wp.tid()
# Perform early exit if out of active bounds
if cid >= num_active_contacts_new[0]:
return
# Retrieve number of active old contacts and the target key to search for
num_active_old = num_active_contacts_old[0]
target_key = contact_key_new[cid]
# Initialize the target reaction and velocity to zero
# to account for the case where no matching contact is found
target_reaction = vec3f(0.0)
target_velocity = vec3f(0.0)
# Perform binary search to find the start index of the target key - i.e. assuming a geom-pair key
start = binary_search_find_range_start(0, num_active_old, target_key, sorted_contact_keys_old)
# If key not found, then mark as a new contact and skip further processing
# NOTE: This means that a new geom-pair collision has occurred
if start == -1:
contact_reaction_new[cid] = target_reaction
contact_velocity_new[cid] = target_velocity
return
# Retrieve the friction coefficient for the contact's associated material
target_material = contact_material_new[cid]
target_mu = target_material[0]
# Retrieve the A/B positions on the corresponding geom-pair contact
r_A_target = contact_position_A_new[cid]
r_B_target = contact_position_B_new[cid]
R_k_target = wp.quat_to_matrix(contact_frame_new[cid])
# Retrieve the body indices and states for the contact's associated bodies
# NOTE: If body A is static, then its velocity contribution is zero by definition
bid_AB = contact_bid_AB_new[cid]
v_Ak = vec3f(0.0)
if bid_AB[0] >= 0:
u_A = body_u_i[bid_AB[0]]
r_A = wp.transform_get_translation(body_q_i[bid_AB[0]])
W_Ak_T = wp.transpose(contact_wrench_matrix_from_points(r_A_target, r_A))
v_Ak = W_Ak_T @ u_A
u_B = body_u_i[bid_AB[1]]
r_B = wp.transform_get_translation(body_q_i[bid_AB[1]])
W_Bk_T = wp.transpose(contact_wrench_matrix_from_points(r_B_target, r_B))
v_Bk = W_Bk_T @ u_B
# Compute the new contact velocity based on the measured body motion
# project to the contact frame and set normal component to zero
# NOTE: We only need to consider tangential velocity for warm-starting
# as the normal velocity should always be non-negative in the local
# contact frame, and positive if the solver computes an opening contact
# thus, for warm-starting we only need to consider the tangential velocity
target_velocity = scaling * wp.transpose(R_k_target) @ (v_Bk - v_Ak)
target_velocity.z = wp.max(target_velocity.z, 0.0)
# Iterate through all old contacts with the same key and accumulate net body-com wrench
# NOTE: We only need body B since it is the body on which the contact reaction acts positively
geom_pair_force_body_B = vec3f(0.0)
k = int32(0)
old_key = sorted_contact_keys_old[start]
while target_key == old_key:
# Retrieve the old contact index from the sorted->unsorted map
cid_old = sorted_to_unsorted_map_old[start + k]
# Load old contact data for the old contact
q_k_old = contact_frame_old[cid_old]
lambda_k_old = contact_reaction_old[cid_old]
# Accumulate the old contact's contribution to the geom-pair net force on body B
geom_pair_force_body_B += wp.quat_to_matrix(q_k_old) @ lambda_k_old
# Update the current old-key to check in the next iteration
k += 1
old_key = sorted_contact_keys_old[start + k]
# TODO: We need to cache this value per geom-pair
# TODO: Replace this with a new cache instead of recomputing every time --- IGNORE ---
num_contacts_gid_AB_new = int32(0)
for i in range(num_active_contacts_new[0]):
if contact_key_new[i] == target_key:
num_contacts_gid_AB_new += 1
# Average the net body-com force over the number of contacts for this geom-pair
contact_force_uniform_new = (1.0 / float32(num_contacts_gid_AB_new)) * geom_pair_force_body_B
# Project to the new contact frame and local
# friction cone to obtain the contact reaction
target_reaction = wp.transpose(R_k_target) @ contact_force_uniform_new
target_reaction = scaling * project_to_coulomb_cone(target_reaction, target_mu)
# Store the new contact reaction and velocity
# NOTE: These will remain zero if no matching contact is found
contact_reaction_new[cid] = target_reaction
contact_velocity_new[cid] = target_velocity
@wp.kernel
def _warmstart_contacts_by_matched_geom_pair_key_and_position_with_net_force_backup(
# Inputs - Common:
tolerance: float32,
scaling: float32,
time_dt: wp.array(dtype=float32),
body_q_i: wp.array(dtype=transformf),
body_u_i: wp.array(dtype=vec6f),
# Inputs - Previous:
sorted_contact_keys_old: wp.array(dtype=uint64),
sorted_to_unsorted_map_old: wp.array(dtype=int32),
num_active_contacts_old: wp.array(dtype=int32),
contact_position_B_old: wp.array(dtype=vec3f),
contact_frame_old: wp.array(dtype=quatf),
contact_reaction_old: wp.array(dtype=vec3f),
contact_velocity_old: wp.array(dtype=vec3f),
# Inputs - Next:
num_active_contacts_new: wp.array(dtype=int32),
contact_key_new: wp.array(dtype=uint64),
contact_wid_new: wp.array(dtype=int32),
contact_bid_AB_new: wp.array(dtype=vec2i),
contact_position_A_new: wp.array(dtype=vec3f),
contact_position_B_new: wp.array(dtype=vec3f),
contact_frame_new: wp.array(dtype=quatf),
contact_material_new: wp.array(dtype=vec2f),
# Outputs:
contact_reaction_new: wp.array(dtype=vec3f),
contact_velocity_new: wp.array(dtype=vec3f),
):
"""
Match current contacts to previous timestep contacts using geom-pair keys and relative distance.
For each current contact, finds matching contact from previous step by:
1. Binary search on sorted keys (O(log n))
2. Linear scan through matching keys to find matching contact point positions (O(m))
"""
# Retrieve the contact index as the thread index
cid = wp.tid()
# Perform early exit if out of active bounds
if cid >= num_active_contacts_new[0]:
return
# Retrieve number of active old contacts and the target key to search for
num_active_old = num_active_contacts_old[0]
target_key = contact_key_new[cid]
# Initialize the target reaction and velocity to zero
# to account for the case where no matching contact is found
target_reaction = vec3f(0.0)
target_velocity = vec3f(0.0)
# Perform binary search to find the start index of the target key - i.e. assuming a geom-pair key
start = binary_search_find_range_start(0, num_active_old, target_key, sorted_contact_keys_old)
# If key not found, then mark as a new contact and skip further processing
# NOTE: This means that a new geom-pair collision has occurred
if start == -1:
contact_reaction_new[cid] = target_reaction
contact_velocity_new[cid] = target_velocity
return
# Retrieve the new contact position on the corresponding geom B
# NOTE: We only need to match based on one contact point position
# on body/geom B in order to handle the general case of static bodies
# as geom B is by definition always the non-static body in a contact pair
r_Bk_target = contact_position_B_new[cid]
R_k_target = wp.quat_to_matrix(contact_frame_new[cid])
# Retrieve the timestep delta time for the contact's associated body
dt = time_dt[contact_wid_new[cid]]
# Retrieve the body indices and states for the contact's associated bodies
bid_AB = contact_bid_AB_new[cid]
r_B = wp.transform_get_translation(body_q_i[bid_AB[1]])
u_B = body_u_i[bid_AB[1]]
# Iterate through all old contacts with the same key and check if contacts match
# based on distance of contact points after accounting for associated body motion
# NOTE: For the comparison, new_idx -> cid, old_idx -> sorted_to_unsorted_map_old[start + k]
k = int32(0)
found_match = int32(0)
old_key = sorted_contact_keys_old[start]
while target_key == old_key:
# Retrieve the old contact index from the sorted->unsorted map
cid_old = sorted_to_unsorted_map_old[start + k]
r_k_B_old = contact_position_B_old[cid_old]
W_Bk_T = wp.transpose(contact_wrench_matrix_from_points(r_k_B_old, r_B))
r_B_candidate = r_k_B_old + dt * (W_Bk_T @ u_B)
# Compute and check the distance to the target contact positions
dr_B = wp.length(r_B_candidate - r_Bk_target)
if dr_B < tolerance:
# When a match is found, retrieve the contact reaction and velocity
# from the old contact and transform them to the new contact frame
q_k_old = contact_frame_old[cid_old]
lambda_k_old = contact_reaction_old[cid_old]
v_k_old = contact_velocity_old[cid_old]
R_k_old = wp.quat_to_matrix(q_k_old)
R_k_old_to_new = wp.transpose(R_k_target) @ R_k_old
target_reaction = R_k_old_to_new @ lambda_k_old
target_velocity = R_k_old_to_new @ v_k_old
found_match = int32(1)
break
# Update the current old-key to check in the next iteration
k += 1
old_key = sorted_contact_keys_old[start + k]
# If no matching contact found by position, fallback to net wrench approach
if found_match == 0:
# Retrieve the friction coefficient for the contact's associated material
target_material = contact_material_new[cid]
target_mu = target_material[0]
# Retrieve the body indices and states for the contact's associated bodies
# NOTE: If body A is static, then its velocity contribution is zero by definition
v_Ak = vec3f(0.0)
if bid_AB[0] >= 0:
u_A = body_u_i[bid_AB[0]]
r_A = wp.transform_get_translation(body_q_i[bid_AB[0]])
r_Ak_target = contact_position_A_new[cid]
W_Ak_T = wp.transpose(contact_wrench_matrix_from_points(r_Ak_target, r_A))
v_Ak = W_Ak_T @ u_A
u_B = body_u_i[bid_AB[1]]
r_B = wp.transform_get_translation(body_q_i[bid_AB[1]])
W_Bk_T = wp.transpose(contact_wrench_matrix_from_points(r_Bk_target, r_B))
v_Bk = W_Bk_T @ u_B
# Compute the new contact velocity based on the measured body motion
# project to the contact frame and set normal component to zero
# NOTE: We only need to consider tangential velocity for warm-starting
# as the normal velocity should always be non-negative in the local
# contact frame, and positive if the solver computes an opening contact
# thus, for warm-starting we only need to consider the tangential velocity
target_velocity = scaling * wp.transpose(R_k_target) @ (v_Bk - v_Ak)
target_velocity.z = wp.max(target_velocity.z, 0.0)
# Iterate through all old contacts with the same key and accumulate net body-com wrench
# NOTE: We only need body B since it is the body on which the contact reaction acts positively
geom_pair_force_body_B = vec3f(0.0)
k = int32(0)
old_key = sorted_contact_keys_old[start]
while target_key == old_key:
# Retrieve the old contact index from the sorted->unsorted map
cid_old = sorted_to_unsorted_map_old[start + k]
# Load old contact data for the old contact
q_k_old = contact_frame_old[cid_old]
lambda_k_old = contact_reaction_old[cid_old]
# Accumulate the old contact's contribution to the geom-pair net force on body B
geom_pair_force_body_B += wp.quat_to_matrix(q_k_old) @ lambda_k_old
# Update the current old-key to check in the next iteration
k += 1
old_key = sorted_contact_keys_old[start + k]
# TODO: We need to cache this value per geom-pair
# TODO: Replace this with a new cache instead of recomputing every time --- IGNORE ---
num_contacts_gid_AB_new = int32(0)
for i in range(num_active_contacts_new[0]):
if contact_key_new[i] == target_key:
num_contacts_gid_AB_new += 1
# Average the net body-com force over the number of contacts for this geom-pair
contact_force_uniform_new = (1.0 / float32(num_contacts_gid_AB_new)) * geom_pair_force_body_B
# Project to the new contact frame and local
# friction cone to obtain the contact reaction
target_reaction = wp.transpose(R_k_target) @ contact_force_uniform_new
target_reaction = scaling * project_to_coulomb_cone(target_reaction, target_mu)
# Store the new contact reaction and velocity
# NOTE: These will remain zero if no matching contact is found
contact_reaction_new[cid] = target_reaction
contact_velocity_new[cid] = target_velocity
###
# Launchers
###
def warmstart_limits_by_matched_jid_dof_key(
sorter: KeySorter,
cache: LimitsKaminoData,
limits: LimitsKaminoData,
):
"""
Warm-starts limits by matching joint-DoF index-pair keys.
Args:
sorter (KeySorter): The key sorter used to sort cached limit keys.
cache (LimitsKaminoData): The cached limits data from the previous simulation step.
limits (LimitsKaminoData): The current limits data to be warm-started.
"""
# First sort the keys of cached limits to facilitate binary search
sorter.sort(num_active_keys=cache.model_active_limits, keys=cache.key)
# Launch kernel to warmstart limits by matching jid keys
wp.launch(
kernel=_warmstart_limits_by_matched_jid_dof_key,
dim=limits.model_max_limits_host,
inputs=[
# Inputs - Previous:
sorter.sorted_keys,
sorter.sorted_to_unsorted_map,
cache.model_active_limits,
cache.velocity,
cache.reaction,
# Inputs - Next:
limits.model_active_limits,
limits.key,
# Outputs:
limits.reaction,
limits.velocity,
],
device=sorter.device,
)
def warmstart_contacts_by_matched_geom_pair_key_and_position(
model: ModelKamino,
data: DataKamino,
sorter: KeySorter,
cache: ContactsKaminoData,
contacts: ContactsKaminoData,
tolerance: float32 | None = None,
):
"""
Warm-starts contacts by matching geom-pair keys and contact point positions.
Args:
model (ModelKamino): The model containing simulation parameters.
data (DataKamino): The model data containing body states.
sorter (KeySorter): The key sorter used to sort cached contact keys.
cache (ContactsKaminoData): The cached contacts data from the previous simulation step.
contacts (ContactsKaminoData): The current contacts data to be warm-started.
"""
# Define tolerance for matching contact points based on distance after accounting for body motion
if tolerance is None:
tolerance = float32(1e-5)
# First sort the keys of cached contacts to facilitate binary search
sorter.sort(num_active_keys=cache.model_active_contacts, keys=cache.key)
# Launch kernel to warmstart contacts by matching geom-pair keys and contact point positions
wp.launch(
kernel=_warmstart_contacts_by_matched_geom_pair_key_and_position,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs - Common:
tolerance,
model.time.dt,
data.bodies.q_i,
data.bodies.u_i,
# Inputs - Previous:
sorter.sorted_keys,
sorter.sorted_to_unsorted_map,
cache.model_active_contacts,
cache.position_B,
cache.frame,
cache.reaction,
cache.velocity,
# Inputs - Next:
contacts.model_active_contacts,
contacts.key,
contacts.wid,
contacts.bid_AB,
contacts.position_B,
contacts.frame,
# Outputs:
contacts.reaction,
contacts.velocity,
],
device=model.device,
)
def warmstart_contacts_from_geom_pair_net_force(
data: DataKamino,
sorter: KeySorter,
cache: ContactsKaminoData,
contacts: ContactsKaminoData,
scaling: float32 | None = None,
):
"""
Warm-starts contacts by matching geom-pair keys and contact point positions.
Args:
model (ModelKamino): The model containing simulation parameters.
data (DataKamino): The model data containing body states.
sorter (KeySorter): The key sorter used to sort cached contact keys.
cache (ContactsKaminoData): The cached contacts data from the previous simulation step.
contacts (ContactsKaminoData): The current contacts data to be warm-started.
"""
# Define scaling for warm-started reactions and velocities
if scaling is None:
scaling = float32(1.0)
# First sort the keys of cached contacts to facilitate binary search
sorter.sort(num_active_keys=cache.model_active_contacts, keys=cache.key)
# Launch kernel to warmstart contacts by matching geom-pair keys and contact point positions
wp.launch(
kernel=_warmstart_contacts_from_geom_pair_net_force,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs - Common:
scaling,
data.bodies.q_i,
data.bodies.u_i,
# Inputs - Previous:
sorter.sorted_keys,
sorter.sorted_to_unsorted_map,
cache.model_active_contacts,
cache.frame,
cache.reaction,
# Inputs - Next:
contacts.model_active_contacts,
contacts.key,
contacts.bid_AB,
contacts.position_A,
contacts.position_B,
contacts.frame,
contacts.material,
# Outputs:
contacts.reaction,
contacts.velocity,
],
device=sorter.device,
)
def warmstart_contacts_by_matched_geom_pair_key_and_position_with_net_force_backup(
model: ModelKamino,
data: DataKamino,
sorter: KeySorter,
cache: ContactsKaminoData,
contacts: ContactsKaminoData,
tolerance: float32 | None = None,
scaling: float32 | None = None,
):
"""
Warm-starts contacts by matching geom-pair keys and contact point positions.
Args:
model (ModelKamino): The model containing simulation parameters.
data (DataKamino): The model data containing body states.
sorter (KeySorter): The key sorter used to sort cached contact keys.
cache (ContactsKaminoData): The cached contacts data from the previous simulation step.
contacts (ContactsKaminoData): The current contacts data to be warm-started.
"""
# Define tolerance for matching contact points based on distance after accounting for body motion
if tolerance is None:
tolerance = float32(1e-5)
# Define scaling for warm-started reactions and velocities
if scaling is None:
scaling = float32(1.0)
# First sort the keys of cached contacts to facilitate binary search
sorter.sort(num_active_keys=cache.model_active_contacts, keys=cache.key)
# Launch kernel to warmstart contacts by matching geom-pair keys and contact point positions
wp.launch(
kernel=_warmstart_contacts_by_matched_geom_pair_key_and_position_with_net_force_backup,
dim=contacts.model_max_contacts_host,
inputs=[
# Inputs - Common:
tolerance,
scaling,
model.time.dt,
data.bodies.q_i,
data.bodies.u_i,
# Inputs - Previous:
sorter.sorted_keys,
sorter.sorted_to_unsorted_map,
cache.model_active_contacts,
cache.position_B,
cache.frame,
cache.reaction,
cache.velocity,
# Inputs - Next:
contacts.model_active_contacts,
contacts.key,
contacts.wid,
contacts.bid_AB,
contacts.position_A,
contacts.position_B,
contacts.frame,
contacts.material,
# Outputs:
contacts.reaction,
contacts.velocity,
],
device=model.device,
)
###
# Interfaces
###
class WarmstarterLimits:
"""
Provides a unified mechanism for warm-starting unilateral limit constraints.
"""
def __init__(self, limits: LimitsKamino | None = None):
"""
Initializes the limits warmstarter using the allocations of the provided limits container.
Args:
limits (LimitsKamino): The limits container whose allocations are used to initialize the warmstarter.
"""
# Store the device of the provided contacts container
self._device: wp.DeviceLike = limits.device if limits is not None else None
# Declare the internal limits cache
self._cache: LimitsKaminoData | None = None
# Check if the limits container has allocations and skip cache allocations if not
if limits is None or (limits is not None and limits.model_max_limits_host <= 0):
return
# Allocate contact data cache based on the those of the provided contacts container
with wp.ScopedDevice(self._device):
self._cache = LimitsKaminoData(
model_max_limits_host=limits.model_max_limits_host,
world_max_limits_host=limits.world_max_limits_host,
model_active_limits=wp.zeros_like(limits.model_active_limits),
key=wp.zeros_like(limits.key),
velocity=wp.zeros_like(limits.velocity),
reaction=wp.zeros_like(limits.reaction),
)
# Create a key sorter that can handle the maximum number of contacts
self._sorter = KeySorter(max_num_keys=limits.model_max_limits_host, device=self._device)
@property
def device(self) -> wp.DeviceLike:
"""
Returns the device on which the warmstarter operates.
"""
return self._device
@property
def cache(self) -> LimitsKaminoData | None:
"""
Returns the internal limits cache data.
"""
return self._cache
def warmstart(self, limits: LimitsKamino):
"""
Warm-starts the provided contacts container using the internal cache.
The current implementation matches contacts based on geom-pair keys and contact point positions.
Args:
model (ModelKamino): The model containing simulation parameters.
data (DataKamino): The model data containing body states.
limits (LimitsKamino): The limits container to warm-start.
"""
# Early exit if no cache is allocated
if self._cache is None:
return
# Otherwise, perform warm-starting using matched jid-dof keys
warmstart_limits_by_matched_jid_dof_key(
sorter=self._sorter,
cache=self._cache,
limits=limits.data,
)
def update(self, limits: LimitsKamino | None = None):
"""
Updates the warmstarter's internal cache with the provided limits data.
Args:
limits (LimitsKamino): The limits container from which to update the cache.
"""
# Early exit if no cache is allocated or no limits data is provided
if self._cache is None or limits is None:
return
# Otherwise, copy over the limits data to the internal cache
wp.copy(self._cache.model_active_limits, limits.model_active_limits)
wp.copy(self._cache.key, limits.key)
wp.copy(self._cache.velocity, limits.velocity)
wp.copy(self._cache.reaction, limits.reaction)
def reset(self):
"""
Resets the warmstarter's internal cache by zeroing out all data.
"""
if self._cache is None:
return
self._cache.model_active_limits.zero_()
self._cache.key.fill_(make_bitmask(63))
self._cache.reaction.zero_()
self._cache.velocity.zero_()
class WarmstarterContacts:
"""
Provides a unified mechanism for warm-starting unilateral contact constraints.
This class supports multiple warm-starting strategies, selectable via the `Method` enum:
- `KEY_AND_POSITION`:
Warm-starts contacts by matching geom-pair keys and contact-point positions.
- `GEOM_PAIR_NET_FORCE`:
Warm-starts contacts using the net body-CoM contact force per geom-pair.
- `GEOM_PAIR_NET_WRENCH`:
Warm-starts contacts using the net body-CoM contact wrench per geom-pair.
- `KEY_AND_POSITION_WITH_NET_FORCE_BACKUP`:
Warm-starts contacts by matching geom-pair keys and contact-point positions,
- with a backup strategy using the net body-CoM contact force per geom-pair.
- `KEY_AND_POSITION_WITH_NET_WRENCH_BACKUP`:
Warm-starts contacts by matching geom-pair keys and contact-point positions,
with a backup strategy using the net body-CoM contact wrench per geom-pair.
Geom-pair keys are unique identifiers for pairs of geometries involved in contacts,
allowing for efficient matching of contacts across simulation steps. This class leverages
the :class:`KeySorter` utility to facilitate rapid searching and matching of contacts
based on these keys using Warp's Radix Sort operations.
"""
class Method(IntEnum):
"""Defines the different warm-starting modes available for contacts."""
KEY_AND_POSITION = 0
"""Warm-starts contacts by matching geom-pair keys and contact-point positions."""
GEOM_PAIR_NET_FORCE = 1
"""Warm-starts contacts using the net body-CoM contact force per geom-pair."""
GEOM_PAIR_NET_WRENCH = 2
"""Warm-starts contacts using the net body-CoM contact wrench per geom-pair."""
KEY_AND_POSITION_WITH_NET_FORCE_BACKUP = 3
"""
Warm-starts contacts by matching geom-pair keys and contact-point positions,
with a backup strategy using the net body-CoM contact force per geom-pair.
"""
KEY_AND_POSITION_WITH_NET_WRENCH_BACKUP = 4
"""
Warm-starts contacts by matching geom-pair keys and contact-point positions,
with a backup strategy using the net body-CoM contact wrench per geom-pair.
"""
@classmethod
def from_string(cls, s: str) -> WarmstarterContacts.Method:
"""Converts a string to a WarmstarterContacts.Method enum value."""
try:
return cls[s.upper()]
except KeyError as e:
raise ValueError(
f"Invalid WarmstarterContacts.Method: {s}. Valid options are: {[e.name for e in cls]}"
) from e
@override
def __str__(self):
"""Returns a string representation of the WarmstarterContacts.Method."""
return f"WarmstarterContacts.Method.{self.name} ({self.value})"
@override
def __repr__(self):
"""Returns a string representation of the WarmstarterContacts.Method."""
return self.__str__()
def __init__(
self,
contacts: ContactsKamino | None = None,
method: Method = Method.KEY_AND_POSITION,
tolerance: float = 1e-5,
scaling: float = 1.0,
):
"""
Initializes the contacts warmstarter using the allocations of the provided contacts container.
Args:
contacts (ContactsKamino): The contacts container whose allocations are used to initialize the warmstarter.
method (WarmstarterContacts.Method): The warm-starting method to use.
tolerance (float): The tolerance used for matching contact point positions.\n
Must be a floating-point value specified in meters, and within the range `[0, +inf)`.\n
Setting this to `0.0` requires exact position matches, effectively disabling position-based matching.
scaling (float): The scaling factor applied to warm-started reactions and velocities.\n
Must be a floating-point value specified in the range `[0, 1.0)`.\n
Setting this to `0.0` effectively disables warm-starting.
"""
# Store the specified warm-starting configurations
self._method: WarmstarterContacts.Method = method
self._tolerance: float32 = float32(tolerance)
self._scaling: float32 = float32(scaling)
# Set the device to use as that of the provided contacts container
self._device: wp.DeviceLike = contacts.device if contacts is not None else None
# Declare the internal contacts cache
self._cache: ContactsKaminoData | None = None
# Check if the contacts container has allocations and skip cache allocations if not
if contacts is None or (contacts is not None and contacts.model_max_contacts_host <= 0):
return
# Allocate contact data cache based on the those of the provided contacts container
with wp.ScopedDevice(self._device):
self._cache = ContactsKaminoData(
model_max_contacts_host=contacts.model_max_contacts_host,
world_max_contacts_host=contacts.world_max_contacts_host,
model_active_contacts=wp.zeros_like(contacts.model_active_contacts),
bid_AB=wp.full_like(contacts.bid_AB, value=vec2i(-1, -1)),
position_A=wp.zeros_like(contacts.position_A),
position_B=wp.zeros_like(contacts.position_B),
frame=wp.zeros_like(contacts.frame),
key=wp.zeros_like(contacts.key),
velocity=wp.zeros_like(contacts.velocity),
reaction=wp.zeros_like(contacts.reaction),
)
# Create a key sorter that can handle the maximum number of contacts
self._sorter = KeySorter(max_num_keys=contacts.model_max_contacts_host, device=self._device)
@property
def device(self) -> wp.DeviceLike:
"""
Returns the device on which the warmstarter operates.
"""
return self._device
@property
def cache(self) -> ContactsKaminoData | None:
"""
Returns the internal contacts cache data.
"""
return self._cache
def warmstart(self, model: ModelKamino, data: DataKamino, contacts: ContactsKamino):
"""
Warm-starts the provided contacts container using the internal cache.
The current implementation matches contacts based on geom-pair keys and contact point positions.
Args:
model (ModelKamino): The model containing simulation parameters.
data (DataKamino): The model data containing body states.
contacts (ContactsKamino): The contacts container to warm-start.
"""
# Early exit if no cache is allocated
if self._cache is None:
return
# Otherwise, perform warm-starting using the selected method
match self._method:
case WarmstarterContacts.Method.KEY_AND_POSITION:
warmstart_contacts_by_matched_geom_pair_key_and_position(
model=model,
data=data,
sorter=self._sorter,
cache=self._cache,
contacts=contacts.data,
tolerance=self._tolerance,
)
case WarmstarterContacts.Method.GEOM_PAIR_NET_FORCE:
warmstart_contacts_from_geom_pair_net_force(
data=data,
sorter=self._sorter,
cache=self._cache,
contacts=contacts.data,
scaling=self._scaling,
)
case WarmstarterContacts.Method.GEOM_PAIR_NET_WRENCH:
raise NotImplementedError("WarmstarterContacts.Method.GEOM_PAIR_NET_WRENCH is not yet implemented.")
case WarmstarterContacts.Method.KEY_AND_POSITION_WITH_NET_FORCE_BACKUP:
warmstart_contacts_by_matched_geom_pair_key_and_position_with_net_force_backup(
model=model,
data=data,
sorter=self._sorter,
cache=self._cache,
contacts=contacts.data,
tolerance=self._tolerance,
scaling=self._scaling,
)
case WarmstarterContacts.Method.KEY_AND_POSITION_WITH_NET_WRENCH_BACKUP:
raise NotImplementedError(
"WarmstarterContacts.Method.KEY_AND_POSITION_WITH_NET_WRENCH_BACKUP is not yet implemented."
)
case _:
raise ValueError(
f"Unknown WarmstarterContacts.Method: {int(self._method)}"
"Supported methods are:"
" - KEY_AND_POSITION (0),"
" - GEOM_PAIR_NET_FORCE (1),"
" - GEOM_PAIR_NET_WRENCH (2),"
" - KEY_AND_POSITION_WITH_NET_FORCE_BACKUP (3),"
" - KEY_AND_POSITION_WITH_NET_WRENCH_BACKUP (4)."
)
def update(self, contacts: ContactsKamino | None = None):
"""
Updates the warmstarter's internal cache with the provided contacts data.
Args:
contacts (ContactsKamino): The contacts container from which to update the cache.
"""
# Early exit if no cache is allocated or no contacts data is provided
if self._cache is None or contacts is None:
return
# Otherwise, copy over the contacts data to the internal cache
wp.copy(self._cache.model_active_contacts, contacts.model_active_contacts)
wp.copy(self._cache.bid_AB, contacts.bid_AB)
wp.copy(self._cache.position_A, contacts.position_A)
wp.copy(self._cache.position_B, contacts.position_B)
wp.copy(self._cache.frame, contacts.frame)
wp.copy(self._cache.key, contacts.key)
wp.copy(self._cache.velocity, contacts.velocity)
wp.copy(self._cache.reaction, contacts.reaction)
def reset(self):
"""
Resets the warmstarter's internal cache by zeroing out all data.
"""
if self._cache is None or self._cache.model_active_contacts is None:
return
self._cache.model_active_contacts.zero_()
self._cache.bid_AB.fill_(vec2i(-1, -1))
self._cache.position_A.zero_()
self._cache.position_B.zero_()
self._cache.frame.zero_()
self._cache.key.fill_(make_bitmask(63))
self._cache.reaction.zero_()
self._cache.velocity.zero_()
================================================
FILE: newton/_src/solvers/kamino/_src/utils/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Utilities"""
from . import device
from . import logger as msg
from .profiles import PerformanceProfile
###
# Module API
###
__all__ = [
"PerformanceProfile",
"device",
"msg",
]
================================================
FILE: newton/_src/solvers/kamino/_src/utils/benchmark/.gitignore
================================================
data/
================================================
FILE: newton/_src/solvers/kamino/_src/utils/benchmark/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Utilities for solver configuration benchmarking."""
from .configs import make_benchmark_configs, make_solver_config_default
from .metrics import (
BenchmarkMetrics,
SolverMetrics,
StatsBinary,
StatsFloat,
StatsInteger,
)
from .problems import (
BenchmarkProblemNameToConfigFn,
CameraConfig,
ControlConfig,
ProblemConfig,
ProblemSet,
make_benchmark_problems,
)
from .render import (
render_solver_configs_table,
render_subcolumn_metrics_table,
)
from .runner import (
BenchmarkSim,
run_single_benchmark,
run_single_benchmark_silent,
run_single_benchmark_with_progress,
run_single_benchmark_with_step_metrics,
run_single_benchmark_with_viewer,
)
###
# Module interface
###
__all__ = [
"BenchmarkMetrics",
"BenchmarkProblemNameToConfigFn",
"BenchmarkSim",
"CameraConfig",
"ControlConfig",
"ProblemConfig",
"ProblemSet",
"SolverMetrics",
"StatsBinary",
"StatsFloat",
"StatsInteger",
"make_benchmark_configs",
"make_benchmark_problems",
"make_solver_config_default",
"render_solver_configs_table",
"render_subcolumn_metrics_table",
"run_single_benchmark",
"run_single_benchmark_silent",
"run_single_benchmark_with_progress",
"run_single_benchmark_with_step_metrics",
"run_single_benchmark_with_viewer",
]
================================================
FILE: newton/_src/solvers/kamino/_src/utils/benchmark/__main__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import datetime
import os
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.benchmark.configs import make_benchmark_configs
from newton._src.solvers.kamino._src.utils.benchmark.metrics import BenchmarkMetrics, CodeInfo
from newton._src.solvers.kamino._src.utils.benchmark.problems import (
BenchmarkProblemNameToConfigFn,
make_benchmark_problems,
)
from newton._src.solvers.kamino._src.utils.benchmark.render import (
render_problem_dimensions_table,
render_solver_configs_table,
)
from newton._src.solvers.kamino._src.utils.benchmark.runner import run_single_benchmark
from newton._src.solvers.kamino._src.utils.device import get_device_spec_info
from newton._src.solvers.kamino._src.utils.sim import Simulator
###
# Constants
###
SUPPORTED_BENCHMARK_RUN_MODES = ["total", "stepstats", "convergence", "accuracy", "import"]
"""
A list of supported benchmark run modes that determine the level of metrics collected during execution.
Each mode includes the metrics of the previous modes, with increasing levels of detail.
The supported modes are as follows:
- "total":
Only collects total runtime and final memory usage of each solver configuration and problem.\n
This mode is intended to be used for a high-level comparison of the overall throughput of
different solver configurations across problems, without detailed step-by-step metrics.
- "stepstats":
Collects detailed timings of each simulation step to compute throughput statistics.\n
This mode lightly impacts overall throughput as it requires synchronizing the device at
each step to measure accurate timings. It is intended to be used for analyzing the step
time distribution and variability across different solver configurations.
- "convergence":
Collects solver performance metrics such as PADMM iterations and residuals.\n
This mode moderately impacts overall throughput as it requires additional computation to
collect and store solver metrics at each step. It is intended to be used for analyzing
solver convergence behavior and its relationship to step time.
- "accuracy":
Collects solver performance metrics that evaluate the physical accuracy of the simulation.
This mode significantly impacts overall throughput as it requires additional computation to
evaluate the physical accuracy metrics at each step. This is intended to be used for in-depth
analysis and to evaluate the trade-off between fast convergence and physical correctness.
- "import":
Generates plots for the collected metrics given an HDF5 file containing benchmark results.\n
NOTE: This mode does not execute any benchmarks and only produces plots from existing data.
"""
SUPPORTED_BENCHMARK_OUTPUT_MODES = ["console", "full"] # TODO: add more modes
"""
A list of supported benchmark outputs that determine the format and detail level of the benchmark results.
- "console": Only prints benchmark results to the console as formatted tables.
- "full": TODO.
"""
###
# Functions
###
def parse_benchmark_arguments():
parser = argparse.ArgumentParser(description="Solver performance benchmark")
# Warp runtime arguments
parser.add_argument("--device", type=str, help="Define the Warp device to operate on, e.g. 'cuda:0' or 'cpu'.")
parser.add_argument(
"--cuda-graph",
action=argparse.BooleanOptionalAction,
default=True,
help="Set to `True` to enable CUDA graph capture (only available on CUDA devices). Defaults to `True`.",
)
parser.add_argument(
"--clear-cache",
action=argparse.BooleanOptionalAction,
default=False,
help="Set to `True` to clear Warp's kernel and LTO caches before execution. Defaults to `False`.",
)
# World configuration arguments
parser.add_argument(
"--num-worlds",
type=int,
default=1,
help="Sets the number of parallel simulation worlds to run. Defaults to `1`.",
)
parser.add_argument(
"--num-steps",
type=int,
default=100,
help="Sets the number of simulation steps to execute. Defaults to `100`.",
)
parser.add_argument(
"--dt",
type=float,
default=0.001,
help="Sets the simulation time step. Defaults to `0.001`.",
)
parser.add_argument(
"--gravity",
action=argparse.BooleanOptionalAction,
default=True,
help="Enables/disables gravity in the simulation. Defaults to `True`.",
)
parser.add_argument(
"--ground",
action=argparse.BooleanOptionalAction,
default=True,
help="Enables/disables ground geometry in the simulation. Defaults to `True`.",
)
parser.add_argument(
"--seed",
type=int,
default=0,
help="Sets the random seed for the simulation. Defaults to `0`.",
)
# Benchmark execution arguments
parser.add_argument(
"--mode",
type=str,
choices=SUPPORTED_BENCHMARK_RUN_MODES,
default="accuracy",
help=f"Defines the benchmark mode to run. Defaults to 'accuracy'.\n{SUPPORTED_BENCHMARK_RUN_MODES}",
)
parser.add_argument(
"--problem",
type=str,
choices=BenchmarkProblemNameToConfigFn.keys(),
default="dr_legs",
help="Defines a single problem to benchmark. Defaults to 'dr_legs'. Ignored if '--problem-set' is provided.",
)
parser.add_argument(
"--problem-set",
nargs="+",
default=list(BenchmarkProblemNameToConfigFn.keys()),
help="Defines the benchmark problem(s) to run. If unspecified, all available problems will be used.",
)
parser.add_argument(
"--output",
type=str,
choices=SUPPORTED_BENCHMARK_OUTPUT_MODES,
default="full",
help=f"Defines the benchmark output mode. Defaults to 'full'.\n{SUPPORTED_BENCHMARK_OUTPUT_MODES}",
)
parser.add_argument(
"--import-path",
type=str,
default=None,
help="Defines the path to the HDF5 benchmark data to import in 'import' mode. Defaults to `None`.",
)
parser.add_argument(
"--viewer",
action=argparse.BooleanOptionalAction,
default=False,
help="Set to `True` to run with the simulation viewer. Defaults to `False`.",
)
parser.add_argument(
"--test",
action=argparse.BooleanOptionalAction,
default=False,
help="Set to `True` to run `newton.example.run` tests. Defaults to `False`.",
)
return parser.parse_args()
def benchmark_run(args: argparse.Namespace):
"""
Executes the benchmark data generation with the provided arguments.
This function performs the following steps:
1. Parses the benchmark arguments to determine the configuration of the run.
2. Sets the Warp device and determines if CUDA graphs can be used.
3. Prints device specification info to the console for reference.
4. Determines the level of metrics to collect based on the specified benchmark mode.
5. Generates the problem set based on the provided problem names and arguments.
6. Constructs the `BenchmarkMetrics` object to store collected data.
7. Iterates over all problem names and settings, executing the benchmark for each combination.
8. Computes final statistics for the collected benchmark results.
9. Saves the collected benchmark data to an HDF5 file for later analysis and plotting.
10. Optionally generates plots from the collected benchmark data.
Args:
args: An `argparse.Namespace` object containing the parsed benchmark arguments.
Returns:
output_path: The path to the hdf5 file created, that contains all collected data.
"""
# First print the benchmark configuration to the console for reference
msg.notif(f"Running benchmark in mode: {args.mode}")
# Print the git commit hash and repository info to the
# console for traceability and reproducibility of benchmark runs
codeinfo = CodeInfo()
msg.notif(f"Benchmark will run with the following repository:\n{codeinfo}\n")
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Print device specification info to console for reference
spec_info = get_device_spec_info(device)
msg.notif("[Device]: %s", spec_info)
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"using_cuda_graph: {use_cuda_graph}")
# Determine the metrics to collect based on the benchmark mode
if args.mode == "total":
collect_step_metrics = False
collect_solver_metrics = False
collect_physics_metrics = False
elif args.mode == "stepstats":
collect_step_metrics = True
collect_solver_metrics = False
collect_physics_metrics = False
elif args.mode == "convergence":
collect_step_metrics = True
collect_solver_metrics = True
collect_physics_metrics = False
elif args.mode == "accuracy":
collect_step_metrics = True
collect_solver_metrics = True
collect_physics_metrics = True
else:
raise ValueError(f"Unsupported benchmark mode '{args.mode}'. Supported modes: {SUPPORTED_BENCHMARK_RUN_MODES}")
msg.info(f"collect_step_metrics: {collect_step_metrics}")
msg.info(f"collect_solver_metrics: {collect_solver_metrics}")
msg.info(f"collect_physics_metrics: {collect_physics_metrics}")
# Determine the problem set from
# the single and list arguments
if len(args.problem_set) == 0:
problem_names = [args.problem]
else:
problem_names = args.problem_set
msg.notif(f"problem_names: {problem_names}")
# Define and create the output directory for the benchmark results
RUN_OUTPUT_PATH = None
if args.output == "full":
DATA_DIR_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), "./data"))
RUN_OUTPUT_NAME = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
RUN_OUTPUT_PATH = f"{DATA_DIR_PATH}/{RUN_OUTPUT_NAME}"
os.makedirs(RUN_OUTPUT_PATH, exist_ok=True)
# Generate a set of solver configurations to benchmark over
configs_set = make_benchmark_configs(include_default=False)
msg.notif(f"config_names: {list(configs_set.keys())}")
render_solver_configs_table(configs=configs_set, groups=["sparse", "linear", "padmm"], to_console=True)
if args.output == "full":
render_solver_configs_table(
configs=configs_set,
path=os.path.join(RUN_OUTPUT_PATH, "solver_configs.txt"),
groups=["cts", "sparse", "linear", "padmm", "warmstart"],
to_console=False,
)
# Generate the problem set based on the
# provided problem names and arguments
problem_set = make_benchmark_problems(
names=problem_names,
num_worlds=args.num_worlds,
gravity=args.gravity,
ground=args.ground,
)
# Construct and initialize the metrics
# object to store benchmark data
metrics = BenchmarkMetrics(
problems=problem_names,
configs=configs_set,
num_steps=args.num_steps,
step_metrics=collect_step_metrics,
solver_metrics=collect_solver_metrics,
physics_metrics=collect_physics_metrics,
)
# Iterator over all problem names and settings and run benchmarks for each
for problem_name, problem_config in problem_set.items():
# Unpack problem configurations
builder, control, camera = problem_config
if not isinstance(builder, ModelBuilderKamino):
builder = builder()
for config_name, configs in configs_set.items():
msg.notif("Running benchmark for problem '%s' with simulation configs '%s'", problem_name, config_name)
# Retrieve problem and config indices
problem_idx = metrics._problem_names.index(problem_name)
config_idx = metrics._config_names.index(config_name)
# Construct simulator configurations based on the solver
# configurations for the current benchmark configuration
sim_configs = Simulator.Config(dt=args.dt, solver=configs)
sim_configs.solver.use_fk_solver = False
# Execute the benchmark for the current problem and settings
run_single_benchmark(
problem_idx=problem_idx,
config_idx=config_idx,
metrics=metrics,
args=args,
builder=builder,
configs=sim_configs,
control=control,
camera=camera,
device=device,
use_cuda_graph=use_cuda_graph,
print_device_info=True,
)
# Print table with problem dimensions
render_problem_dimensions_table(metrics._problem_dims, to_console=True)
if args.output == "full":
render_problem_dimensions_table(
metrics._problem_dims,
path=os.path.join(RUN_OUTPUT_PATH, "problem_dimensions.txt"),
to_console=False,
)
# Compute final statistics for the benchmark results
metrics.compute_stats()
# Export the collected benchmark data to an HDF5 file for later analysis and plotting
if args.output == "full":
msg.info("Saving benchmark data to HDF5...")
RUN_HDF5_OUTPUT_PATH = f"{RUN_OUTPUT_PATH}/metrics.hdf5"
metrics.save_to_hdf5(path=RUN_HDF5_OUTPUT_PATH)
msg.info("Done.")
# Return collected metrics and path to export folder (None if not exported)
return metrics, RUN_OUTPUT_PATH
def load_metrics(data_import_path: str | None):
# If the import path is not specified load the latest created HDF5 file in the output directory
if data_import_path is None:
DATA_DIR_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), "./data"))
all_runs = next(os.walk(DATA_DIR_PATH))[1]
all_runs = sorted(all_runs, key=lambda x: os.stat(os.path.join(DATA_DIR_PATH, x)).st_mtime)
if len(all_runs) == 0:
raise FileNotFoundError(f"No benchmark runs found in output directory '{DATA_DIR_PATH}'.")
latest_run = all_runs[-1]
data_import_path = os.path.join(DATA_DIR_PATH, latest_run, "metrics.hdf5")
msg.notif(f"No import path specified. Loading latest benchmark data from '{data_import_path}'.")
# Ensure that the specified import path exists and is a valid HDF5 file
if not os.path.exists(data_import_path):
raise FileNotFoundError(f"The specified import path '{data_import_path}' does not exist.")
elif not os.path.isfile(data_import_path):
raise ValueError(f"The specified import path '{data_import_path}' is not a file.")
elif not data_import_path.endswith(".hdf5"):
raise ValueError(f"The specified import path '{data_import_path}' is not an HDF5 file.")
# Retrieve the parent directory of the import path to use as the base output path for any generated plots
import_parent_dir = os.path.dirname(data_import_path)
msg.notif(f"Output will be generated in directory '{import_parent_dir}'.")
# Load the benchmark data from the specified HDF5 file into
# a `BenchmarkMetrics` object for analysis and plotting
metrics = BenchmarkMetrics(path=data_import_path)
# Return loaded metrics and the path of the containing folder
return metrics, import_parent_dir
def benchmark_output(metrics: BenchmarkMetrics, export_dir: str | None):
# Compute statistics for the collected benchmark
# data to prepare for plotting and analysis
metrics.compute_stats()
# Print the total performance summary as a formatted table to the console:
# - The columns span the problems, with a sub-column for each
# metric (e.g. total time, total FPS, memory used)
# - The rows span the solver configurations
total_metrics_table_path = None
if export_dir is not None:
total_metrics_table_path = os.path.join(export_dir, "total_metrics.txt")
metrics.render_total_metrics_table(path=total_metrics_table_path)
# For each problem, export a table summarizing the step-time for each solver configuration:
# - A sub-column for each statistic (mean, std, min, max)
# - The rows span the solver configurations
if metrics.step_time is not None:
step_time_summary_path = None
if export_dir is not None:
step_time_summary_path = os.path.join(export_dir, "step_time")
metrics.render_step_time_table(path=step_time_summary_path)
# For each problem, export a table summarizing the PADMM metrics for each solver configuration:
# - The columns span the metrics (e.g. step time, padmm.*, physics.*),
# with a sub-column for each statistic (mean, std, min, max)
# - The rows span the solver configurations
if metrics.solver_metrics is not None:
padmm_metrics_summary_path = None
padmm_metrics_plots_path = None
if export_dir is not None:
padmm_metrics_summary_path = os.path.join(export_dir, "padmm_metrics")
padmm_metrics_plots_path = os.path.join(export_dir, "padmm_metrics")
metrics.render_padmm_metrics_table(path=padmm_metrics_summary_path)
metrics.render_padmm_metrics_plots(path=padmm_metrics_plots_path)
# For each problem, export a table summarizing the PADMM metrics for each solver configuration:
# - The columns span the metrics (e.g. step time, padmm.*, physics.*),
# with a sub-column for each statistic (mean, std, min, max)
# - The rows span the solver configurations
if metrics.physics_metrics is not None:
physics_metrics_summary_path = None
physics_metrics_plots_path = None
if export_dir is not None:
physics_metrics_summary_path = os.path.join(export_dir, "physics_metrics")
physics_metrics_plots_path = os.path.join(export_dir, "physics_metrics")
metrics.render_physics_metrics_table(path=physics_metrics_summary_path)
metrics.render_physics_metrics_plots(path=physics_metrics_plots_path)
###
# Main function
###
if __name__ == "__main__":
# Load benchmark-specific program arguments
args = parse_benchmark_arguments()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=6, threshold=10000, suppress=True) # Suppress scientific notation
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# If the benchmark mode is not "import", first execute the
# benchmark and then produce output from the collected data
if args.mode != "import":
metrics, export_dir = benchmark_run(args)
benchmark_output(metrics=metrics, export_dir=export_dir)
else:
if args.import_path is not None:
msg.notif(f"Loading benchmark data from specified import path '{args.import_path}'.")
metrics, export_dir = load_metrics(args.import_path)
benchmark_output(metrics=metrics, export_dir=export_dir)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/benchmark/configs.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import ast
from ...linalg.linear import LinearSolverNameToType, LinearSolverTypeToName
from ...solver_kamino_impl import SolverKaminoImpl
###
# Module interface
###
__all__ = [
"make_benchmark_configs",
"make_solver_config_default",
"make_solver_config_dense_jacobian_llt_accurate",
"make_solver_config_dense_jacobian_llt_fast",
"make_solver_config_sparse_delassus_cr_accurate",
"make_solver_config_sparse_delassus_cr_fast",
"make_solver_config_sparse_jacobian_llt_accurate",
"make_solver_config_sparse_jacobian_llt_fast",
]
###
# Solver configurations
###
def make_solver_config_default() -> tuple[str, SolverKaminoImpl.Config]:
# ------------------------------------------------------------------------------
name = "Default"
# ------------------------------------------------------------------------------
config = SolverKaminoImpl.Config()
# ------------------------------------------------------------------------------
# Constraint stabilization
config.constraints.alpha = 0.1
# ------------------------------------------------------------------------------
# Jacobian representation
config.sparse_jacobian = False
config.sparse_dynamics = False
# ------------------------------------------------------------------------------
# Linear system solver
config.dynamics.linear_solver_type = LinearSolverNameToType["LLTB"]
config.dynamics.linear_solver_kwargs = {}
# ------------------------------------------------------------------------------
# PADMM
config.padmm.max_iterations = 200
config.padmm.primal_tolerance = 1e-6
config.padmm.dual_tolerance = 1e-6
config.padmm.compl_tolerance = 1e-6
config.padmm.restart_tolerance = 0.999
config.padmm.eta = 1e-5
config.padmm.rho_0 = 1.0
config.padmm.rho_min = 1e-5
config.padmm.penalty_update_method = "fixed"
config.padmm.penalty_update_freq = 1
config.padmm.use_acceleration = True
config.padmm.use_graph_conditionals = False
# ------------------------------------------------------------------------------
# Warm-starting
config.padmm.warmstart_mode = "containers"
config.padmm.contact_warmstart_method = "geom_pair_net_force"
# ------------------------------------------------------------------------------
return name, config
def make_solver_config_dense_jacobian_llt_accurate() -> tuple[str, SolverKaminoImpl.Config]:
# ------------------------------------------------------------------------------
name = "Dense Jacobian LLT accurate"
# ------------------------------------------------------------------------------
config = SolverKaminoImpl.Config()
# ------------------------------------------------------------------------------
# Constraint stabilization
config.constraints.alpha = 0.1
# ------------------------------------------------------------------------------
# Jacobian representation
config.sparse_dynamics = False
config.sparse_jacobian = False
# ------------------------------------------------------------------------------
# Linear system solver
config.dynamics.linear_solver_type = LinearSolverNameToType["LLTB"]
config.dynamics.linear_solver_kwargs = {}
# ------------------------------------------------------------------------------
# PADMM
config.padmm.max_iterations = 200
config.padmm.primal_tolerance = 1e-6
config.padmm.dual_tolerance = 1e-6
config.padmm.compl_tolerance = 1e-6
config.padmm.restart_tolerance = 0.999
config.padmm.eta = 1e-5
config.padmm.rho_0 = 0.1
config.padmm.rho_min = 1e-5
config.padmm.penalty_update_method = "fixed"
config.padmm.penalty_update_freq = 1
config.padmm.use_acceleration = True
config.padmm.use_graph_conditionals = False
# ------------------------------------------------------------------------------
# Warm-starting
config.padmm.warmstart_mode = "containers"
config.padmm.contact_warmstart_method = "geom_pair_net_force"
# ------------------------------------------------------------------------------
return name, config
def make_solver_config_dense_jacobian_llt_fast() -> tuple[str, SolverKaminoImpl.Config]:
# ------------------------------------------------------------------------------
name = "Dense Jacobian LLT fast"
# ------------------------------------------------------------------------------
config = SolverKaminoImpl.Config()
# ------------------------------------------------------------------------------
# Constraint stabilization
config.constraints.alpha = 0.1
# ------------------------------------------------------------------------------
# Jacobian representation
config.sparse_dynamics = False
config.sparse_jacobian = False
# ------------------------------------------------------------------------------
# Linear system solver
config.dynamics.linear_solver_type = LinearSolverNameToType["LLTB"]
config.dynamics.linear_solver_kwargs = {}
# ------------------------------------------------------------------------------
# PADMM
config.padmm.max_iterations = 100
config.padmm.primal_tolerance = 1e-4
config.padmm.dual_tolerance = 1e-4
config.padmm.compl_tolerance = 1e-4
config.padmm.restart_tolerance = 0.999
config.padmm.eta = 1e-5
config.padmm.rho_0 = 0.02
config.padmm.rho_min = 1e-5
config.padmm.penalty_update_method = "fixed"
config.padmm.penalty_update_freq = 1
config.padmm.use_acceleration = True
config.padmm.use_graph_conditionals = False
# ------------------------------------------------------------------------------
# Warm-starting
config.padmm.warmstart_mode = "containers"
config.padmm.contact_warmstart_method = "geom_pair_net_force"
# ------------------------------------------------------------------------------
return name, config
def make_solver_config_sparse_jacobian_llt_accurate() -> tuple[str, SolverKaminoImpl.Config]:
# ------------------------------------------------------------------------------
name = "Sparse Jacobian LLT accurate"
# ------------------------------------------------------------------------------
config = SolverKaminoImpl.Config()
# ------------------------------------------------------------------------------
# Constraint stabilization
config.constraints.alpha = 0.1
# ------------------------------------------------------------------------------
# Jacobian representation
config.sparse_dynamics = False
config.sparse_jacobian = True
# ------------------------------------------------------------------------------
# Linear system solver
config.dynamics.linear_solver_type = LinearSolverNameToType["LLTB"]
config.dynamics.linear_solver_kwargs = {}
# ------------------------------------------------------------------------------
# PADMM
config.padmm.max_iterations = 200
config.padmm.primal_tolerance = 1e-6
config.padmm.dual_tolerance = 1e-6
config.padmm.compl_tolerance = 1e-6
config.padmm.restart_tolerance = 0.999
config.padmm.eta = 1e-5
config.padmm.rho_0 = 0.1
config.padmm.rho_min = 1e-5
config.padmm.penalty_update_method = "fixed"
config.padmm.penalty_update_freq = 1
config.padmm.use_acceleration = True
config.padmm.use_graph_conditionals = False
# ------------------------------------------------------------------------------
# Warm-starting
config.padmm.warmstart_mode = "containers"
config.padmm.contact_warmstart_method = "geom_pair_net_force"
# ------------------------------------------------------------------------------
return name, config
def make_solver_config_sparse_jacobian_llt_fast() -> tuple[str, SolverKaminoImpl.Config]:
# ------------------------------------------------------------------------------
name = "Sparse Jacobian LLT fast"
# ------------------------------------------------------------------------------
config = SolverKaminoImpl.Config()
# ------------------------------------------------------------------------------
# Constraint stabilization
config.constraints.alpha = 0.1
# ------------------------------------------------------------------------------
# Jacobian representation
config.sparse_dynamics = False
config.sparse_jacobian = True
# ------------------------------------------------------------------------------
# Linear system solver
config.dynamics.linear_solver_type = LinearSolverNameToType["LLTB"]
config.dynamics.linear_solver_kwargs = {}
# ------------------------------------------------------------------------------
# PADMM
config.padmm.max_iterations = 100
config.padmm.primal_tolerance = 1e-4
config.padmm.dual_tolerance = 1e-4
config.padmm.compl_tolerance = 1e-4
config.padmm.restart_tolerance = 0.999
config.padmm.eta = 1e-5
config.padmm.rho_0 = 0.02
config.padmm.rho_min = 1e-5
config.padmm.penalty_update_method = "fixed"
config.padmm.penalty_update_freq = 1
config.padmm.use_acceleration = True
config.padmm.use_graph_conditionals = False
# ------------------------------------------------------------------------------
# Warm-starting
config.padmm.warmstart_mode = "containers"
config.padmm.contact_warmstart_method = "geom_pair_net_force"
# ------------------------------------------------------------------------------
return name, config
def make_solver_config_sparse_delassus_cr_accurate() -> tuple[str, SolverKaminoImpl.Config]:
# ------------------------------------------------------------------------------
name = "Sparse Delassus CR accurate"
# ------------------------------------------------------------------------------
config = SolverKaminoImpl.Config()
# ------------------------------------------------------------------------------
# Constraint stabilization
config.constraints.alpha = 0.1
# ------------------------------------------------------------------------------
# Jacobian representation
config.sparse_dynamics = True
config.sparse_jacobian = True
# ------------------------------------------------------------------------------
# Linear system solver
config.dynamics.linear_solver_type = LinearSolverNameToType["CR"]
config.dynamics.linear_solver_kwargs = {"maxiter": 30}
# ------------------------------------------------------------------------------
# PADMM
config.padmm.max_iterations = 200
config.padmm.primal_tolerance = 1e-6
config.padmm.dual_tolerance = 1e-6
config.padmm.compl_tolerance = 1e-6
config.padmm.restart_tolerance = 0.999
config.padmm.eta = 1e-5
config.padmm.rho_0 = 0.1
config.padmm.rho_min = 1e-5
config.padmm.penalty_update_method = "fixed"
config.padmm.penalty_update_freq = 1
config.padmm.use_acceleration = True
config.padmm.use_graph_conditionals = False
# ------------------------------------------------------------------------------
# Warm-starting
config.padmm.warmstart_mode = "containers"
config.padmm.contact_warmstart_method = "geom_pair_net_force"
# ------------------------------------------------------------------------------
return name, config
def make_solver_config_sparse_delassus_cr_fast() -> tuple[str, SolverKaminoImpl.Config]:
# ------------------------------------------------------------------------------
name = "Sparse Delassus CR fast"
# ------------------------------------------------------------------------------
config = SolverKaminoImpl.Config()
# ------------------------------------------------------------------------------
# Jacobian representation
config.sparse_dynamics = True
config.sparse_jacobian = True
# ------------------------------------------------------------------------------
# Constraint stabilization
config.constraints.alpha = 0.1
# ------------------------------------------------------------------------------
# Linear system solver
config.dynamics.linear_solver_type = LinearSolverNameToType["CR"]
config.dynamics.linear_solver_kwargs = {"maxiter": 9}
# ------------------------------------------------------------------------------
# PADMM
config.padmm.max_iterations = 100
config.padmm.primal_tolerance = 1e-4
config.padmm.dual_tolerance = 1e-4
config.padmm.compl_tolerance = 1e-4
config.padmm.restart_tolerance = 0.999
config.padmm.eta = 1e-5
config.padmm.rho_0 = 0.02
config.padmm.rho_min = 1e-5
config.padmm.penalty_update_method = "fixed"
config.padmm.penalty_update_freq = 1
config.padmm.use_acceleration = True
config.padmm.use_graph_conditionals = False
# ------------------------------------------------------------------------------
# Warm-starting
config.padmm.warmstart_mode = "containers"
config.padmm.contact_warmstart_method = "geom_pair_net_force"
# ------------------------------------------------------------------------------
return name, config
###
# Utilities
###
def make_benchmark_configs(include_default: bool = True) -> dict[str, SolverKaminoImpl.Config]:
if include_default:
generators = [make_solver_config_default]
else:
generators = []
generators.extend(
[
make_solver_config_dense_jacobian_llt_accurate,
make_solver_config_dense_jacobian_llt_fast,
make_solver_config_sparse_jacobian_llt_accurate,
make_solver_config_sparse_jacobian_llt_fast,
make_solver_config_sparse_delassus_cr_accurate,
make_solver_config_sparse_delassus_cr_fast,
]
)
solver_configs: dict[str, SolverKaminoImpl.Config] = {}
for gen in generators:
name, config = gen()
solver_configs[name] = config
return solver_configs
###
# Functions
###
def save_solver_configs_to_hdf5(configs: dict[str, SolverKaminoImpl.Config], datafile):
for config_name, config in configs.items():
scope = f"Solver/{config_name}"
# ------------------------------------------------------------------------------
datafile[f"{scope}/sparse_jacobian"] = config.sparse_jacobian
datafile[f"{scope}/sparse_dynamics"] = config.sparse_dynamics
# ------------------------------------------------------------------------------
datafile[f"{scope}/constraints/alpha"] = config.constraints.alpha
datafile[f"{scope}/constraints/beta"] = config.constraints.beta
datafile[f"{scope}/constraints/gamma"] = config.constraints.gamma
datafile[f"{scope}/constraints/delta"] = config.constraints.delta
# ------------------------------------------------------------------------------
solver_name = LinearSolverTypeToName[config.dynamics.linear_solver_type]
datafile[f"{scope}/dynamics/preconditioning"] = config.dynamics.preconditioning
datafile[f"{scope}/dynamics/linear_solver/type"] = str(solver_name)
datafile[f"{scope}/dynamics/linear_solver/args"] = f"{config.dynamics.linear_solver_kwargs}"
# ------------------------------------------------------------------------------
datafile[f"{scope}/padmm/max_iterations"] = config.padmm.max_iterations
datafile[f"{scope}/padmm/primal_tolerance"] = config.padmm.primal_tolerance
datafile[f"{scope}/padmm/dual_tolerance"] = config.padmm.dual_tolerance
datafile[f"{scope}/padmm/compl_tolerance"] = config.padmm.compl_tolerance
datafile[f"{scope}/padmm/restart_tolerance"] = config.padmm.restart_tolerance
datafile[f"{scope}/padmm/eta"] = config.padmm.eta
datafile[f"{scope}/padmm/rho_0"] = config.padmm.rho_0
datafile[f"{scope}/padmm/rho_min"] = config.padmm.rho_min
datafile[f"{scope}/padmm/a_0"] = config.padmm.a_0
datafile[f"{scope}/padmm/alpha"] = config.padmm.alpha
datafile[f"{scope}/padmm/tau"] = config.padmm.tau
datafile[f"{scope}/padmm/penalty_update_method"] = config.padmm.penalty_update_method
datafile[f"{scope}/padmm/penalty_update_freq"] = config.padmm.penalty_update_freq
datafile[f"{scope}/padmm/linear_solver_tolerance"] = config.padmm.linear_solver_tolerance
datafile[f"{scope}/padmm/linear_solver_tolerance_ratio"] = config.padmm.linear_solver_tolerance_ratio
datafile[f"{scope}/padmm/use_acceleration"] = config.padmm.use_acceleration
datafile[f"{scope}/padmm/use_graph_conditionals"] = config.padmm.use_graph_conditionals
# ------------------------------------------------------------------------------
datafile[f"{scope}/warmstarting/warmstart_mode"] = config.padmm.warmstart_mode
datafile[f"{scope}/warmstarting/contact_warmstart_method"] = config.padmm.contact_warmstart_method
def load_solver_configs_to_hdf5(datafile) -> dict[str, SolverKaminoImpl.Config]:
configs = {}
for config_name in datafile["Solver"].keys():
config = SolverKaminoImpl.Config()
# ------------------------------------------------------------------------------
config.sparse_jacobian = bool(datafile[f"Solver/{config_name}/sparse_jacobian"][()])
config.sparse_dynamics = bool(datafile[f"Solver/{config_name}/sparse_dynamics"][()])
# ------------------------------------------------------------------------------
config.constraints.alpha = float(datafile[f"Solver/{config_name}/constraints/alpha"][()])
config.constraints.beta = float(datafile[f"Solver/{config_name}/constraints/beta"][()])
config.constraints.gamma = float(datafile[f"Solver/{config_name}/constraints/gamma"][()])
config.constraints.delta = float(datafile[f"Solver/{config_name}/constraints/delta"][()])
# ------------------------------------------------------------------------------
config.dynamics.preconditioning = bool(datafile[f"Solver/{config_name}/dynamics/preconditioning"][()])
config.dynamics.linear_solver_type = LinearSolverNameToType[
datafile[f"Solver/{config_name}/dynamics/linear_solver/type"][()].decode("utf-8")
]
config.dynamics.linear_solver_kwargs = ast.literal_eval(
datafile[f"Solver/{config_name}/dynamics/linear_solver/args"][()].decode("utf-8")
)
# ------------------------------------------------------------------------------
config.padmm.max_iterations = float(datafile[f"Solver/{config_name}/padmm/max_iterations"][()])
config.padmm.primal_tolerance = float(datafile[f"Solver/{config_name}/padmm/primal_tolerance"][()])
config.padmm.dual_tolerance = float(datafile[f"Solver/{config_name}/padmm/dual_tolerance"][()])
config.padmm.compl_tolerance = float(datafile[f"Solver/{config_name}/padmm/compl_tolerance"][()])
config.padmm.restart_tolerance = float(datafile[f"Solver/{config_name}/padmm/restart_tolerance"][()])
config.padmm.eta = float(datafile[f"Solver/{config_name}/padmm/eta"][()])
config.padmm.rho_0 = float(datafile[f"Solver/{config_name}/padmm/rho_0"][()])
config.padmm.rho_min = float(datafile[f"Solver/{config_name}/padmm/rho_min"][()])
config.padmm.a_0 = float(datafile[f"Solver/{config_name}/padmm/a_0"][()])
config.padmm.alpha = float(datafile[f"Solver/{config_name}/padmm/alpha"][()])
config.padmm.tau = float(datafile[f"Solver/{config_name}/padmm/tau"][()])
config.padmm.penalty_update_method = str(datafile[f"Solver/{config_name}/padmm/penalty_update_method"][()])
config.padmm.penalty_update_freq = int(datafile[f"Solver/{config_name}/padmm/penalty_update_freq"][()])
config.padmm.linear_solver_tolerance = float(
datafile[f"Solver/{config_name}/padmm/linear_solver_tolerance"][()]
)
config.padmm.linear_solver_tolerance_ratio = float(
datafile[f"Solver/{config_name}/padmm/linear_solver_tolerance_ratio"][()]
)
config.padmm.use_acceleration = bool(datafile[f"Solver/{config_name}/padmm/use_acceleration"][()])
config.padmm.use_graph_conditionals = bool(datafile[f"Solver/{config_name}/padmm/use_graph_conditionals"][()])
# ------------------------------------------------------------------------------
config.padmm.warmstart_mode = str(datafile[f"Solver/{config_name}/warmstarting/warmstart_mode"][()])
config.padmm.contact_warmstart_method = str(
datafile[f"Solver/{config_name}/warmstarting/contact_warmstart_method"][()]
)
# ------------------------------------------------------------------------------
configs[config_name] = config
return configs
================================================
FILE: newton/_src/solvers/kamino/_src/utils/benchmark/metrics.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import os
from typing import Any, Literal
import numpy as np
from ......core.types import override
from ...solver_kamino_impl import SolverKaminoImpl
from .configs import load_solver_configs_to_hdf5, save_solver_configs_to_hdf5
from .problems import ProblemDimensions, save_problem_dimensions_to_hdf5
from .render import (
ColumnGroup,
render_subcolumn_metrics_table,
render_subcolumn_table,
)
###
# Module interface
###
__all__ = [
"BenchmarkMetrics",
"SolverMetrics",
"StatsBinary",
"StatsFloat",
"StatsInteger",
]
###
# Types - Meta-Data
###
class CodeInfo:
"""
A utility container to encapsulate information about the code
repository, such as the remote URL, branch, and commit hash.
"""
def __init__(self, path: str | None = None, empty: bool = False):
"""
Initialize a CodeInfo object.
Args:
path (str | None):
The path to the git repository. If None, the current working directory is used.
Raises:
RuntimeError: If there is an error retrieving git repository info from the specified path.
"""
# TODO: Consider using a silent warning and allowing the CodeInfo
# to be initialized with None values instead of raising an error
# Attempt to import git first, and warn user
# if the necessary package is not installed
try:
import git
except ImportError as e:
raise ImportError(
"The GitPython package is required for downloading git folders. Install it with: pip install gitpython"
) from e
# Declare git repository info attributes
self.repo: git.Repo | None = None
self.path: str | None = None
self.remote: str | None = None
self.branch: str | None = None
self.commit: str | None = None
self.diff: str | None = None
# If a path is provided, attempt to retrieve git repository info from
# that, otherwise attempt to retrieve from the current working directory
if path is not None:
_path = path
elif not empty:
_path = str(os.path.dirname(__file__))
else:
# If empty is True, skip retrieving git repository info and leave all attributes as None
return
# Attempt to retrieve git repository info from the specified path;
# if any error occurs, raise a RuntimeError with the error message
try:
self.repo = git.Repo(path=_path, search_parent_directories=True)
self.path = self.repo.working_tree_dir
self.remote = self.repo.remote().url if self.repo.remotes else None
self.branch = str(self.repo.active_branch)
self.commit = str(self.repo.head.object.hexsha)
self.diff = str(self.repo.git.diff())
except Exception as e:
raise RuntimeError(f"Error retrieving git repository info: {e}") from e
def __repr__(self):
"""Returns a human-readable string representation of the CodeInfo."""
return (
f"CodeInfo(\n"
f" path: {self.path}\n"
f" remote: {self.remote}\n"
f" branch: {self.branch}\n"
f" commit: {self.commit}\n"
f")"
)
def __str__(self):
"""Returns a human-readable string representation of the CodeInfo (same as __repr__)."""
return self.__repr__()
def as_dict(self) -> dict:
"""Returns a dictionary representation of the CodeInfo."""
return {
"path": self.path,
"remote": self.remote,
"branch": self.branch,
"commit": self.commit,
}
###
# Types - Statistics
###
class StatsFloat:
"""A utility class to compute statistics for floating-point data arrays, such as step times and residuals."""
def __init__(self, data: np.ndarray, name: str | None = None):
"""
Initialize a StatsFloat object.
Args:
data (np.ndarray): A floating-point data array.
name (str | None): An optional name for the statistics object.
Raises:
ValueError: If the data array is not of a floating-point type.
"""
if not np.issubdtype(data.dtype, np.floating):
raise ValueError("StatsFloat requires a floating-point data array.")
self.name: str | None = name
# Declare statistics arrays
self.median: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.mean: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.std: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.min: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.max: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
# Compute float stats of each problem (i.e. along axis=2)
self.median[:, :] = np.median(data, axis=2)
self.mean[:, :] = np.mean(data, axis=2)
self.std[:, :] = np.std(data, axis=2)
self.min[:, :] = np.min(data, axis=2)
self.max[:, :] = np.max(data, axis=2)
@override
def __repr__(self):
"""Returns a human-readable string representation of the StatsFloat."""
return (
f"StatsFloat[{self.name or '-'}](\n"
f"median:\n{self.median},\n"
f"mean:\n{self.mean},\n"
f"std:\n{self.std},\n"
f"min:\n{self.min},\n"
f"max:\n{self.max}\n"
)
class StatsInteger:
"""A utility class to compute statistics for integer data arrays, such as counts and distributions."""
def __init__(self, data: np.ndarray, num_bins: int = 20, name: str | None = None):
"""
Initialize a StatsInteger object.
Args:
data (np.ndarray): An integer data array.
num_bins (int): Number of bins for histogram (default: 20).
name (str | None): An optional name for the statistics object.
Raises:
ValueError: If the data array is not of an integer type.
"""
if not np.issubdtype(data.dtype, np.integer):
raise ValueError("StatsInteger requires an integer data array.")
self.name: str | None = name
# Declare statistics arrays
self.median: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.mean: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.std: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.min: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.max: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
# Compute integer stats of each problem (i.e. along axis=2)
self.median[:, :] = np.median(data.astype(np.float32), axis=2)
self.mean[:, :] = np.mean(data.astype(np.float32), axis=2)
self.std[:, :] = np.std(data.astype(np.float32), axis=2)
self.min[:, :] = np.min(data.astype(np.float32), axis=2)
self.max[:, :] = np.max(data.astype(np.float32), axis=2)
@override
def __repr__(self):
"""Returns a human-readable string representation of the StatsInteger."""
return (
f"StatsInteger[{self.name or '-'}](\n"
f"median:\n{self.median},\n"
f"mean:\n{self.mean},\n"
f"std:\n{self.std},\n"
f"min:\n{self.min},\n"
f"max:\n{self.max}\n"
)
class StatsBinary:
"""A utility class to compute statistics for binary (boolean) data arrays, such as counts of zeros and ones."""
def __init__(self, data: np.ndarray, name: str | None = None):
"""
Initialize a StatsBinary object.
Args:
data (np.ndarray): A binary (boolean) data array.
name (str | None): An optional name for the statistics object.
Raises:
ValueError: If the data array is not of a binary (boolean) type.
"""
if not np.issubdtype(data.dtype, np.integer) or not np.array_equal(data, data.astype(bool)):
raise ValueError("StatsBinary requires a binary (boolean) data array.")
self.name: str | None = name
# Declare Binary statistics arrays
self.count_zeros: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
self.count_ones: np.ndarray = np.zeros((data.shape[0], data.shape[1]), dtype=np.float32)
# Compute binary stats of each problem (i.e. along axis=2)
self.count_zeros[:, :] = np.sum(data == 0, axis=2)
self.count_ones[:, :] = np.sum(data == 1, axis=2)
@override
def __repr__(self):
"""Returns a human-readable string representation of the StatsBinary."""
return f"StatsBinary[{self.name or '-'}](\ncount_zeros:\n{self.count_zeros},\ncount_ones:\n{self.count_ones}\n"
###
# Types - Metrics
###
class SolverMetrics:
def __init__(self, num_problems: int, num_configs: int, num_steps: int):
# Solver-specific metrics
self.padmm_converged: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.int32)
self.padmm_iters: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.int32)
self.padmm_r_p: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.padmm_r_d: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.padmm_r_c: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
# Linear solver metrics (placeholders for now)
self.linear_solver_iters: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.linear_solver_r_error: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
# Stats (computed after data collection)
self.padmm_success_stats: StatsBinary | None = None
self.padmm_iters_stats: StatsInteger | None = None
self.padmm_r_p_stats: StatsFloat | None = None
self.padmm_r_d_stats: StatsFloat | None = None
self.padmm_r_c_stats: StatsFloat | None = None
self.linear_solver_iters_stats: StatsInteger | None = None
self.linear_solver_r_error_stats: StatsFloat | None = None
def compute_stats(self):
self.padmm_success_stats = StatsBinary(self.padmm_converged, name="padmm_converged")
self.padmm_iters_stats = StatsInteger(self.padmm_iters, name="padmm_iters")
self.padmm_r_p_stats = StatsFloat(self.padmm_r_p, name="padmm_r_p")
self.padmm_r_d_stats = StatsFloat(self.padmm_r_d, name="padmm_r_d")
self.padmm_r_c_stats = StatsFloat(self.padmm_r_c, name="padmm_r_c")
# TODO: self.linear_solver_iters_stats = StatsInteger(self.linear_solver_iters, name="linear_solver_iters")
# TODO: self.linear_solver_r_error_stats = StatsFloat(self.linear_solver_r_error, name="linear_solver_r_error")
class PhysicsMetrics:
def __init__(self, num_problems: int, num_configs: int, num_steps: int):
# Physics-specific metrics
self.r_eom: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_kinematics: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_cts_joints: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_cts_limits: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_cts_contacts: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_v_plus: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_ncp_primal: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_ncp_dual: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_ncp_compl: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.r_vi_natmap: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.f_ncp: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
self.f_ccp: np.ndarray = np.zeros((num_problems, num_configs, num_steps), dtype=np.float32)
# Stats (computed after data collection)
self.r_eom_stats: StatsFloat | None = None
self.r_kinematics_stats: StatsFloat | None = None
self.r_cts_joints_stats: StatsFloat | None = None
self.r_cts_limits_stats: StatsFloat | None = None
self.r_cts_contacts_stats: StatsFloat | None = None
self.r_v_plus_stats: StatsFloat | None = None
self.r_ncp_primal_stats: StatsFloat | None = None
self.r_ncp_dual_stats: StatsFloat | None = None
self.r_ncp_compl_stats: StatsFloat | None = None
self.r_vi_natmap_stats: StatsFloat | None = None
self.f_ncp_stats: StatsFloat | None = None
self.f_ccp_stats: StatsFloat | None = None
def compute_stats(self):
self.r_eom_stats = StatsFloat(self.r_eom, name="r_eom")
self.r_kinematics_stats = StatsFloat(self.r_kinematics, name="r_kinematics")
self.r_cts_joints_stats = StatsFloat(self.r_cts_joints, name="r_cts_joints")
self.r_cts_limits_stats = StatsFloat(self.r_cts_limits, name="r_cts_limits")
self.r_cts_contacts_stats = StatsFloat(self.r_cts_contacts, name="r_cts_contacts")
self.r_v_plus_stats = StatsFloat(self.r_v_plus, name="r_v_plus")
self.r_ncp_primal_stats = StatsFloat(self.r_ncp_primal, name="r_ncp_primal")
self.r_ncp_dual_stats = StatsFloat(self.r_ncp_dual, name="r_ncp_dual")
self.r_ncp_compl_stats = StatsFloat(self.r_ncp_compl, name="r_ncp_compl")
self.r_vi_natmap_stats = StatsFloat(self.r_vi_natmap, name="r_vi_natmap")
self.f_ncp_stats = StatsFloat(self.f_ncp, name="f_ncp")
self.f_ccp_stats = StatsFloat(self.f_ccp, name="f_ccp")
class BenchmarkMetrics:
def __init__(
self,
problems: list[str] | None = None,
configs: dict[str, SolverKaminoImpl.Config] | None = None,
num_steps: int | None = None,
step_metrics: bool = False,
solver_metrics: bool = False,
physics_metrics: bool = False,
path: str | None = None,
):
# Declare data-set dimensions
self._problem_names: list[str] | None = None
self._config_names: list[str] | None = None
self._num_steps: int | None = None
# Declare problem dimensions
self._problem_dims: dict[str, ProblemDimensions] = {}
# Declare cache of the solver configurations used in the
# benchmark for easy reference when analyzing results
self._configs: dict[str, SolverKaminoImpl.Config] | None = None
# One-time metrics
self.memory_used: np.ndarray | None = None
self.total_time: np.ndarray | None = None
self.total_fps: np.ndarray | None = None
# Per-step metrics
self.step_time: np.ndarray | None = None
self.step_time_stats: StatsFloat | None = None
# Optional solver-specific metrics
self.solver_metrics: SolverMetrics | None = None
# Optional physics-specific metrics
self.physics_metrics: PhysicsMetrics | None = None
# Meta-data about the code repository at the time of the
# benchmark run for traceability and reproducibility
self.codeinfo: CodeInfo | None = None
# Initialize metrics data structures if problem
# names, config names, and num_steps are provided,
# otherwise load from HDF5 if path is provided
if problems is not None and configs is not None:
self.finalize(
problems=problems,
configs=configs,
num_steps=num_steps,
step_metrics=step_metrics,
solver_metrics=solver_metrics,
physics_metrics=physics_metrics,
)
elif path is not None:
self.load_from_hdf5(path=path)
@property
def num_problems(self) -> int:
if self._problem_names is None:
raise ValueError("BenchmarkMetrics: problem names not set. Call finalize() first.")
return len(self._problem_names)
@property
def num_configs(self) -> int:
if self._config_names is None:
raise ValueError("BenchmarkMetrics: config names not set. Call finalize() first.")
return len(self._config_names)
@property
def num_steps(self) -> int:
if self._num_steps is None:
raise ValueError("BenchmarkMetrics: num_steps not set. Call finalize() first.")
return self._num_steps
def finalize(
self,
problems: list[str],
configs: dict[str, SolverKaminoImpl.Config],
num_steps: int | None = None,
step_metrics: bool = False,
solver_metrics: bool = False,
physics_metrics: bool = False,
):
# Cache run problem and config names as well as total step counts
self._problem_names = problems
self._config_names = list(configs.keys())
self._configs = configs
self._num_steps = num_steps if num_steps is not None else 1
# Allocate arrays for one-time total run metrics
self.memory_used = np.zeros((self.num_problems, self.num_configs), dtype=np.float32)
self.total_time = np.zeros((self.num_problems, self.num_configs), dtype=np.float32)
self.total_fps = np.zeros((self.num_problems, self.num_configs), dtype=np.float32)
# Allocate per-step metrics arrays if enabled
if step_metrics:
self.step_time = np.zeros((self.num_problems, self.num_configs, self._num_steps), dtype=np.float32)
if solver_metrics:
self.solver_metrics = SolverMetrics(self.num_problems, self.num_configs, self._num_steps)
if physics_metrics:
self.physics_metrics = PhysicsMetrics(self.num_problems, self.num_configs, self._num_steps)
# Generate meta-data to record git repository info
self.codeinfo = CodeInfo()
def record_step(
self,
problem_idx: int,
config_idx: int,
step_idx: int,
step_time: float,
solver: SolverKaminoImpl | None = None,
):
if self.step_time is None:
raise ValueError(
"BenchmarkMetrics: step_time array not initialized. Call finalize() with step_metrics=True first."
)
self.step_time[problem_idx, config_idx, step_idx] = step_time
if self.solver_metrics is not None and solver is not None:
# Extract PADMM solver status info - this is multiworld
solver_status_np = solver._solver_fd.data.status.numpy()
solver_status_np = {name: solver_status_np[name].max() for name in solver_status_np.dtype.names}
self.solver_metrics.padmm_converged[problem_idx, config_idx, step_idx] = solver_status_np["converged"]
self.solver_metrics.padmm_iters[problem_idx, config_idx, step_idx] = solver_status_np["iterations"]
self.solver_metrics.padmm_r_p[problem_idx, config_idx, step_idx] = solver_status_np["r_p"]
self.solver_metrics.padmm_r_d[problem_idx, config_idx, step_idx] = solver_status_np["r_d"]
self.solver_metrics.padmm_r_c[problem_idx, config_idx, step_idx] = solver_status_np["r_c"]
if self.physics_metrics is not None and solver is not None and solver.metrics is not None:
r_eom_np = solver.metrics.data.r_eom.numpy().max(axis=0)
r_kinematics_np = solver.metrics.data.r_kinematics.numpy().max(axis=0)
r_cts_joints_np = solver.metrics.data.r_cts_joints.numpy().max(axis=0)
r_cts_limits_np = solver.metrics.data.r_cts_limits.numpy().max(axis=0)
r_cts_contacts_np = solver.metrics.data.r_cts_contacts.numpy().max(axis=0)
r_v_plus_np = solver.metrics.data.r_v_plus.numpy().max(axis=0)
r_ncp_primal_np = solver.metrics.data.r_ncp_primal.numpy().max(axis=0)
r_ncp_dual_np = solver.metrics.data.r_ncp_dual.numpy().max(axis=0)
r_ncp_compl_np = solver.metrics.data.r_ncp_compl.numpy().max(axis=0)
r_vi_natmap_np = solver.metrics.data.r_vi_natmap.numpy().max(axis=0)
f_ncp_np = solver.metrics.data.f_ncp.numpy().max(axis=0)
f_ccp_np = solver.metrics.data.f_ccp.numpy().max(axis=0)
self.physics_metrics.r_eom[problem_idx, config_idx, step_idx] = r_eom_np
self.physics_metrics.r_kinematics[problem_idx, config_idx, step_idx] = r_kinematics_np
self.physics_metrics.r_cts_joints[problem_idx, config_idx, step_idx] = r_cts_joints_np
self.physics_metrics.r_cts_limits[problem_idx, config_idx, step_idx] = r_cts_limits_np
self.physics_metrics.r_cts_contacts[problem_idx, config_idx, step_idx] = r_cts_contacts_np
self.physics_metrics.r_v_plus[problem_idx, config_idx, step_idx] = r_v_plus_np
self.physics_metrics.r_ncp_primal[problem_idx, config_idx, step_idx] = r_ncp_primal_np
self.physics_metrics.r_ncp_dual[problem_idx, config_idx, step_idx] = r_ncp_dual_np
self.physics_metrics.r_ncp_compl[problem_idx, config_idx, step_idx] = r_ncp_compl_np
self.physics_metrics.r_vi_natmap[problem_idx, config_idx, step_idx] = r_vi_natmap_np
self.physics_metrics.f_ncp[problem_idx, config_idx, step_idx] = f_ncp_np
self.physics_metrics.f_ccp[problem_idx, config_idx, step_idx] = f_ccp_np
def record_total(
self,
problem_idx: int,
config_idx: int,
total_steps: int,
total_time: float,
memory_used: float,
):
self.memory_used[problem_idx, config_idx] = memory_used
self.total_time[problem_idx, config_idx] = total_time
self.total_fps[problem_idx, config_idx] = float(total_steps) / total_time if total_time > 0.0 else 0.0
def compute_stats(self):
if self.step_time is not None:
self.step_time_stats = StatsFloat(self.step_time, name="step_time")
if self.solver_metrics is not None:
self.solver_metrics.compute_stats()
if self.physics_metrics is not None:
self.physics_metrics.compute_stats()
def save_to_hdf5(self, path: str):
# Attempt to import h5py first, and warn user
# if the necessary package is not installed
try:
import h5py # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `h5py` package is required for saving to HDF5. Install it with: pip install h5py"
) from e
# Ensure that there is in fact data to save before attempting to write to HDF5
if self._problem_names is None or self._config_names is None or self._num_steps is None:
raise ValueError("BenchmarkMetrics: problem names, config names, and num_steps must be set before saving.")
# Open an HDF5 file for writing and save all data arrays along with meta-data about
# the benchmark run and code repository info for traceability and reproducibility
with h5py.File(path, "w") as datafile:
# Info about the project at the time of the benchmark run
datafile["Info/code/path"] = self.codeinfo.path
datafile["Info/code/remote"] = self.codeinfo.remote
datafile["Info/code/branch"] = self.codeinfo.branch
datafile["Info/code/commit"] = self.codeinfo.commit
datafile["Info/code/diff"] = self.codeinfo.diff
# Problem dimensions
save_problem_dimensions_to_hdf5(self._problem_dims, datafile)
# Save solver configuration parameters
save_solver_configs_to_hdf5(self._configs, datafile)
# Info about the benchmark data
datafile["Info/problem_names"] = self._problem_names
datafile["Info/config_names"] = self._config_names
datafile["Info/num_steps"] = self._num_steps
datafile["Info/has_step_metrics"] = self.step_time is not None
datafile["Info/has_solver_metrics"] = self.solver_metrics is not None
datafile["Info/has_physics_metrics"] = self.physics_metrics is not None
# Basic run metrics
datafile["Data/total/memory_used"] = self.memory_used
datafile["Data/total/total_time"] = self.total_time
datafile["Data/total/total_fps"] = self.total_fps
if self.step_time is not None:
datafile["Data/perstep/step_time"] = self.step_time
if self.solver_metrics is not None:
datafile["Data/perstep/padmm/converged"] = self.solver_metrics.padmm_converged
datafile["Data/perstep/padmm/iterations"] = self.solver_metrics.padmm_iters
datafile["Data/perstep/padmm/r_p"] = self.solver_metrics.padmm_r_p
datafile["Data/perstep/padmm/r_d"] = self.solver_metrics.padmm_r_d
datafile["Data/perstep/padmm/r_c"] = self.solver_metrics.padmm_r_c
if self.physics_metrics is not None:
datafile["Data/perstep/physics/r_eom"] = self.physics_metrics.r_eom
datafile["Data/perstep/physics/r_kinematics"] = self.physics_metrics.r_kinematics
datafile["Data/perstep/physics/r_cts_joints"] = self.physics_metrics.r_cts_joints
datafile["Data/perstep/physics/r_cts_limits"] = self.physics_metrics.r_cts_limits
datafile["Data/perstep/physics/r_cts_contacts"] = self.physics_metrics.r_cts_contacts
datafile["Data/perstep/physics/r_v_plus"] = self.physics_metrics.r_v_plus
datafile["Data/perstep/physics/r_ncp_primal"] = self.physics_metrics.r_ncp_primal
datafile["Data/perstep/physics/r_ncp_dual"] = self.physics_metrics.r_ncp_dual
datafile["Data/perstep/physics/r_ncp_compl"] = self.physics_metrics.r_ncp_compl
datafile["Data/perstep/physics/r_vi_natmap"] = self.physics_metrics.r_vi_natmap
datafile["Data/perstep/physics/f_ncp"] = self.physics_metrics.f_ncp
datafile["Data/perstep/physics/f_ccp"] = self.physics_metrics.f_ccp
def load_from_hdf5(self, path: str):
# Attempt to import h5py first, and warn user
# if the necessary package is not installed
try:
import h5py # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `h5py` package is required for saving to HDF5. Install it with: pip install h5py"
) from e
"""Load raw data arrays from the HDF5 file into the BenchmarkMetrics instance"""
with h5py.File(path, "r") as datafile:
# First load the info group to get the dimensions and initialize the data arrays
self._problem_names = datafile["Info/problem_names"][:].astype(str).tolist()
self._config_names = datafile["Info/config_names"][:].astype(str).tolist()
self._num_steps = int(datafile["Info/num_steps"][()])
has_step_metrics = bool(datafile["Info/has_step_metrics"][()])
has_solver_metrics = bool(datafile["Info/has_solver_metrics"][()])
has_physics_metrics = bool(datafile["Info/has_physics_metrics"][()])
# Load code state info for traceability and reproducibility
self.codeinfo = CodeInfo(empty=True)
self.codeinfo.path = datafile["Info/code/path"][()]
self.codeinfo.remote = datafile["Info/code/remote"][()]
self.codeinfo.branch = datafile["Info/code/branch"][()]
self.codeinfo.commit = datafile["Info/code/commit"][()]
self.codeinfo.diff = datafile["Info/code/diff"][()]
# Load solver configurations into the cache for reference
self._configs = load_solver_configs_to_hdf5(datafile)
# Load raw data directly into the corresponding array attributes
self.memory_used = datafile["Data/total/memory_used"][:, :].astype(np.int32)
self.total_time = datafile["Data/total/total_time"][:, :].astype(np.float32)
self.total_fps = datafile["Data/total/total_fps"][:, :].astype(np.float32)
if has_step_metrics:
self.step_time = datafile["Data/perstep/step_time"][:, :, :].astype(np.float32)
if has_solver_metrics:
solv_ns = "Data/perstep/padmm/"
self.solver_metrics = SolverMetrics(1, 1, 1) # Placeholder initialization to create the object
self.solver_metrics.padmm_converged = datafile[f"{solv_ns}converged"][:, :, :].astype(np.int32)
self.solver_metrics.padmm_iters = datafile[f"{solv_ns}iterations"][:, :, :].astype(np.int32)
self.solver_metrics.padmm_r_p = datafile[f"{solv_ns}r_p"][:, :, :].astype(np.float32)
self.solver_metrics.padmm_r_d = datafile[f"{solv_ns}r_d"][:, :, :].astype(np.float32)
self.solver_metrics.padmm_r_c = datafile[f"{solv_ns}r_c"][:, :, :].astype(np.float32)
if has_physics_metrics:
phys_ns = "Data/perstep/physics/"
self.physics_metrics = PhysicsMetrics(1, 1, 1) # Placeholder initialization to create the object
self.physics_metrics.r_eom = datafile[f"{phys_ns}r_eom"][:, :, :].astype(np.float32)
self.physics_metrics.r_kinematics = datafile[f"{phys_ns}r_kinematics"][:, :, :].astype(np.float32)
self.physics_metrics.r_cts_joints = datafile[f"{phys_ns}r_cts_joints"][:, :, :].astype(np.float32)
self.physics_metrics.r_cts_limits = datafile[f"{phys_ns}r_cts_limits"][:, :, :].astype(np.float32)
self.physics_metrics.r_cts_contacts = datafile[f"{phys_ns}r_cts_contacts"][:, :, :].astype(np.float32)
self.physics_metrics.r_v_plus = datafile[f"{phys_ns}r_v_plus"][:, :, :].astype(np.float32)
self.physics_metrics.r_ncp_primal = datafile[f"{phys_ns}r_ncp_primal"][:, :, :].astype(np.float32)
self.physics_metrics.r_ncp_dual = datafile[f"{phys_ns}r_ncp_dual"][:, :, :].astype(np.float32)
self.physics_metrics.r_ncp_compl = datafile[f"{phys_ns}r_ncp_compl"][:, :, :].astype(np.float32)
self.physics_metrics.r_vi_natmap = datafile[f"{phys_ns}r_vi_natmap"][:, :, :].astype(np.float32)
self.physics_metrics.f_ncp = datafile[f"{phys_ns}f_ncp"][:, :, :].astype(np.float32)
self.physics_metrics.f_ccp = datafile[f"{phys_ns}f_ccp"][:, :, :].astype(np.float32)
def render_total_metrics_table(self, path: str | None = None):
"""
Outputs a formatted table summarizing the total metrics
(memory used, total time, total FPS) for each solver
configuration and problem, and optionally saves the
table to a text file at the specified path.
Args:
path (`str`):
File path to save the table as a text file.
Raises:
ValueError: If the total metrics (memory used, total time, total FPS) are not available.
"""
# Generate the table string for the total metrics summary and print it to the console;
total_metric_data = [self.memory_used, self.total_time, self.total_fps]
total_metric_names = ["Memory (MB)", "Total Time (s)", "Total FPS (Hz)"]
total_metric_formats = [lambda x: f"{x / (1024 * 1024):.2f}", ".2f", ".2f"]
render_subcolumn_metrics_table(
title="Solver Benchmark: Total Metrics",
row_header="Solver Configuration",
row_titles=self._config_names,
col_titles=self._problem_names,
subcol_titles=total_metric_names,
subcol_data=total_metric_data,
subcol_formats=total_metric_formats,
path=path,
to_console=True,
)
def render_step_time_table(self, path: str | None = None, units: Literal["s", "ms", "us"] = "ms"):
"""
Outputs a formatted table for each problem summarizing the per-step time
metrics for each solver configuration and problem, and optionally saves
the table to a text file at the specified path.
Args:
path (`str`):
File path to save the table as a text file.
Raises:
ValueError: If the step time metrics are not available.
"""
if self.step_time_stats is None:
raise ValueError("Step time metrics are not available in this BenchmarkMetrics instance.")
# For each problem, generate the table string for the step time metrics summary and print it to the console;
units_scaling = {"s": 1.0, "ms": 1e3, "us": 1e6}[units]
for prob_idx, prob_name in enumerate(self._problem_names):
problem_table_path = f"{path}_{prob_name}.txt" if path is not None else None
cols: list[ColumnGroup] = []
cols.append(
ColumnGroup(
header="Solver Configuration",
subheaders=["Name"],
justify="left",
color="white",
)
)
cols.append(
ColumnGroup(
header=f"Step Time ({units})",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3f", ".3f", ".3f", ".3f"],
justify="left",
color="cyan",
)
)
rows: list[list[Any]] = []
for config_idx, config_name in enumerate(self._config_names):
rows.append(
[
[config_name],
[
self.step_time_stats.median[prob_idx, config_idx] * units_scaling,
self.step_time_stats.mean[prob_idx, config_idx] * units_scaling,
self.step_time_stats.max[prob_idx, config_idx] * units_scaling,
self.step_time_stats.min[prob_idx, config_idx] * units_scaling,
],
]
)
render_subcolumn_table(
title=f"Solver Benchmark: Step Time - {prob_name}",
cols=cols,
rows=rows,
max_width=300,
path=problem_table_path,
to_console=True,
)
def render_padmm_metrics_table(self, path: str | None = None):
"""
Outputs a formatted table for each problem summarizing the PADMM
solver metrics (convergence, iterations, residuals) for each solver
configuration and problem, and optionally saves the table to a text
file at the specified path.
Args:
path (`str`):
File path to save the table as a text file.
Raises:
ValueError: If the PADMM solver metrics are not available.
"""
if self.solver_metrics is None:
raise ValueError("PADMM solver metrics are not available in this BenchmarkMetrics instance.")
# For each problem, generate the table string for the PADMM solver metrics summary and print it to the console;
for prob_idx, prob_name in enumerate(self._problem_names):
problem_table_path = f"{path}_{prob_name}.txt" if path is not None else None
cols: list[ColumnGroup] = []
cols.append(
ColumnGroup(
header="Solver Configuration",
subheaders=["Name"],
justify="left",
color="white",
)
)
cols.append(
ColumnGroup(
header="Converged",
subheaders=["Count", "Rate"],
subfmt=["d", ".2%"],
justify="left",
color="magenta",
)
)
cols.append(
ColumnGroup(
header="Iterations",
subheaders=["median", "mean", "max", "min"],
subfmt=[".0f", ".0f", ".0f", ".0f"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="Primal Residual (r_p)",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="red",
)
)
cols.append(
ColumnGroup(
header="Dual Residual (r_d)",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="blue",
)
)
cols.append(
ColumnGroup(
header="Complementarity Residual (r_c)",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="green",
)
)
rows: list[list[Any]] = []
for config_idx, config_name in enumerate(self._config_names):
success_count = self.solver_metrics.padmm_success_stats.count_ones[prob_idx, config_idx]
fail_count = self.solver_metrics.padmm_success_stats.count_zeros[prob_idx, config_idx]
total_count = success_count + fail_count
success_rate = success_count / total_count if total_count > 0 else 0.0
rows.append(
[
[config_name],
[success_count, success_rate],
[
self.solver_metrics.padmm_iters_stats.median[prob_idx, config_idx],
self.solver_metrics.padmm_iters_stats.mean[prob_idx, config_idx],
self.solver_metrics.padmm_iters_stats.max[prob_idx, config_idx],
self.solver_metrics.padmm_iters_stats.min[prob_idx, config_idx],
],
[
self.solver_metrics.padmm_r_p_stats.median[prob_idx, config_idx],
self.solver_metrics.padmm_r_p_stats.mean[prob_idx, config_idx],
self.solver_metrics.padmm_r_p_stats.max[prob_idx, config_idx],
self.solver_metrics.padmm_r_p_stats.min[prob_idx, config_idx],
],
[
self.solver_metrics.padmm_r_d_stats.median[prob_idx, config_idx],
self.solver_metrics.padmm_r_d_stats.mean[prob_idx, config_idx],
self.solver_metrics.padmm_r_d_stats.max[prob_idx, config_idx],
self.solver_metrics.padmm_r_d_stats.min[prob_idx, config_idx],
],
[
self.solver_metrics.padmm_r_c_stats.median[prob_idx, config_idx],
self.solver_metrics.padmm_r_c_stats.mean[prob_idx, config_idx],
self.solver_metrics.padmm_r_c_stats.max[prob_idx, config_idx],
self.solver_metrics.padmm_r_c_stats.min[prob_idx, config_idx],
],
],
)
render_subcolumn_table(
title=f"Solver Benchmark: PADMM Convergence - {prob_name}",
cols=cols,
rows=rows,
max_width=300,
path=problem_table_path,
)
def render_physics_metrics_table(self, path: str | None = None):
"""
Outputs a formatted table for each problem summarizing the physics
metrics for each solver configuration and problem, and optionally
saves the table to a text file at the specified path.
Args:
path (`str`):
File path to save the table as a text file.
Raises:
ValueError: If the physics metrics are not available.
"""
if self.physics_metrics is None:
raise ValueError("Physics metrics are not available in this BenchmarkMetrics instance.")
# For each problem, generate the table string for the physics metrics summary and print it to the console;
for prob_idx, prob_name in enumerate(self._problem_names):
problem_table_path = f"{path}_{prob_name}.txt" if path is not None else None
cols: list[ColumnGroup] = []
cols.append(
ColumnGroup(
header="Solver Configuration",
subheaders=["Name"],
justify="left",
color="white",
)
)
cols.append(
ColumnGroup(
header="r_eom",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_kinematics",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_cts_joints",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_cts_limits",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_cts_contacts",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_v_plus",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_ncp_primal",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_ncp_dual",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_ncp_compl",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="r_vi_natmap",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="f_ncp",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
cols.append(
ColumnGroup(
header="f_ccp",
subheaders=["median", "mean", "max", "min"],
subfmt=[".3e", ".3e", ".3e", ".3e"],
justify="left",
color="cyan",
)
)
rows: list[list[Any]] = []
for config_idx, config_name in enumerate(self._config_names):
rows.append(
[
[config_name],
[
self.physics_metrics.r_eom_stats.median[prob_idx, config_idx],
self.physics_metrics.r_eom_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_eom_stats.max[prob_idx, config_idx],
self.physics_metrics.r_eom_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_kinematics_stats.median[prob_idx, config_idx],
self.physics_metrics.r_kinematics_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_kinematics_stats.max[prob_idx, config_idx],
self.physics_metrics.r_kinematics_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_cts_joints_stats.median[prob_idx, config_idx],
self.physics_metrics.r_cts_joints_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_cts_joints_stats.max[prob_idx, config_idx],
self.physics_metrics.r_cts_joints_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_cts_limits_stats.median[prob_idx, config_idx],
self.physics_metrics.r_cts_limits_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_cts_limits_stats.max[prob_idx, config_idx],
self.physics_metrics.r_cts_limits_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_cts_contacts_stats.median[prob_idx, config_idx],
self.physics_metrics.r_cts_contacts_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_cts_contacts_stats.max[prob_idx, config_idx],
self.physics_metrics.r_cts_contacts_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_v_plus_stats.median[prob_idx, config_idx],
self.physics_metrics.r_v_plus_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_v_plus_stats.max[prob_idx, config_idx],
self.physics_metrics.r_v_plus_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_ncp_primal_stats.median[prob_idx, config_idx],
self.physics_metrics.r_ncp_primal_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_ncp_primal_stats.max[prob_idx, config_idx],
self.physics_metrics.r_ncp_primal_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_ncp_dual_stats.median[prob_idx, config_idx],
self.physics_metrics.r_ncp_dual_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_ncp_dual_stats.max[prob_idx, config_idx],
self.physics_metrics.r_ncp_dual_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_ncp_compl_stats.median[prob_idx, config_idx],
self.physics_metrics.r_ncp_compl_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_ncp_compl_stats.max[prob_idx, config_idx],
self.physics_metrics.r_ncp_compl_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.r_vi_natmap_stats.median[prob_idx, config_idx],
self.physics_metrics.r_vi_natmap_stats.mean[prob_idx, config_idx],
self.physics_metrics.r_vi_natmap_stats.max[prob_idx, config_idx],
self.physics_metrics.r_vi_natmap_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.f_ncp_stats.median[prob_idx, config_idx],
self.physics_metrics.f_ncp_stats.mean[prob_idx, config_idx],
self.physics_metrics.f_ncp_stats.max[prob_idx, config_idx],
self.physics_metrics.f_ncp_stats.min[prob_idx, config_idx],
],
[
self.physics_metrics.f_ccp_stats.median[prob_idx, config_idx],
self.physics_metrics.f_ccp_stats.mean[prob_idx, config_idx],
self.physics_metrics.f_ccp_stats.max[prob_idx, config_idx],
self.physics_metrics.f_ccp_stats.min[prob_idx, config_idx],
],
]
)
render_subcolumn_table(
title=f"Solver Benchmark: Physics Metrics - {prob_name}",
cols=cols,
rows=rows,
max_width=650,
path=problem_table_path,
)
def render_padmm_metrics_plots(self, path: str):
"""
Generates time-series plots of the PADMM solver metrics
(convergence, iterations, residuals) across the simulation
steps for each solver configuration and problem, and
optionally saves the plots to a file at the specified path.
Args:
path (`str`):
Target file path of the generated plot image.\n
Raises:
ValueError: If the PADMM solver metrics are not available.
"""
# Ensure that the PADMM solver metrics are available before attempting to render the plots
if self.solver_metrics is None:
raise ValueError("PADMM solver metrics are not available in this BenchmarkMetrics instance.")
# Attempt to import matplotlib for plotting, and raise an informative error if it's not installed
try:
import matplotlib.pyplot as plt
except Exception as e:
raise ImportError(
"matplotlib is required to render PADMM metrics plots. Please install matplotlib and try again."
) from e
# Generate time-series plots of the PADMM solver metrics across the simulation steps of each problem:
# - For each problem we create a figure
# - Each figure has a subplot for each PADMM metric in (iterations, r_p, r_d, r_c)
# - Within each subplot we plot a metric curve for each solver configuration
for prob_idx, prob_name in enumerate(self._problem_names):
fig, axs = plt.subplots(2, 2, figsize=(16, 12))
fig.suptitle(f"PADMM Metrics vs Simulation Steps - {prob_name}", fontsize=16)
titles = [
"PADMM Iterations",
"PADMM Primal Residual",
"PADMM Dual Residual",
"PADMM Complementary Residual",
]
names = ["iterations", "r_p", "r_d", "r_c"]
data = [
self.solver_metrics.padmm_iters[prob_idx, :, :],
self.solver_metrics.padmm_r_p[prob_idx, :, :],
self.solver_metrics.padmm_r_d[prob_idx, :, :],
self.solver_metrics.padmm_r_c[prob_idx, :, :],
]
for metric_idx, (title, name, array) in enumerate(zip(titles, names, data, strict=True)):
ax = axs[metric_idx // 2, metric_idx % 2]
for config_idx, config_name in enumerate(self._config_names):
ax.plot(
np.arange(self.num_steps),
array[config_idx, :],
label=config_name,
marker="o",
markersize=4,
)
ax.set_title(title)
ax.set_xlabel("Simulation Step")
ax.set_ylabel(name)
ax.grid()
plt.tight_layout(rect=[0, 0.03, 1, 0.95])
# Get handles/labels from any one subplot (since they are identical)
handles, labels = axs.flat[0].get_legend_handles_labels()
# Add ONE legend for the whole figure, centered at the bottom
fig.legend(
handles,
labels,
loc="lower center",
ncol=len(labels), # put entries in one row
bbox_to_anchor=(0.5, 0.02), # (x, y) in figure coordinates
frameon=False,
)
# Make room at the bottom for the legend
fig.subplots_adjust(bottom=0.15)
# If a path is provided, also save the plot to an image file at the specified path
if path is not None:
# Check if the directory for the specified path exists, and if not, create it
path_dir = os.path.dirname(path)
if path_dir and not os.path.exists(path_dir):
raise ValueError(
f"Directory for path '{path}' does not exist. "
"Please create the directory before saving the plot."
)
fig_path = path + f"_{prob_name}.pdf"
plt.savefig(fig_path, format="pdf", dpi=300, bbox_inches="tight")
# Close the figure to free up memory after saving
# (or if not saving) before the next iteration
plt.close(fig)
def render_physics_metrics_plots(self, path: str):
"""
Generates time-series plots of the physics metrics (e.g.,
constraint violation etc) across the simulation steps for
each solver configuration and problem, and optionally
saves the plots to a file at the specified path.
Args:
path (`str`):
Target file path of the generated plot image.\n
Raises:
ValueError: If the physics metrics are not available.
"""
# Ensure that the physics metrics are available before attempting to render the plots
if self.physics_metrics is None:
raise ValueError("Physics metrics are not available in this BenchmarkMetrics instance.")
# Attempt to import matplotlib for plotting, and raise an informative error if it's not installed
try:
import matplotlib.pyplot as plt
except Exception as e:
raise ImportError(
"matplotlib is required to render physics metrics plots. Please install matplotlib and try again."
) from e
# Generate time-series plots of the physics solver metrics across the simulation steps of each problem:
# - For each problem we create a figure
# - Each figure has a subplot for each physics metric in (constraint violation, energy, etc.)
# - Within each subplot we plot a metric curve for each solver configuration
for prob_idx, prob_name in enumerate(self._problem_names):
fig, axs = plt.subplots(3, 4, figsize=(20, 15))
fig.suptitle(f"Physics Metrics vs Simulation Steps - {prob_name}", fontsize=16)
equations = [
"$\\Vert \\, M \\, (u^+ - u^-) - dt \\, (h + J_a^T \\, \\tau) - J^T \\, \\lambda \\, \\Vert_\\infty $",
"$\\Vert \\, J_j \\cdot u^+ \\, \\Vert_\\infty $",
"$\\Vert \\, f_j(q) \\, \\Vert_\\infty $",
"$\\Vert \\, f_l(q) \\, \\Vert_\\infty $",
"$\\Vert \\, f_{c,N}(q) \\, \\Vert_\\infty $",
"$\\Vert \\, v^+ - D \\cdot \\lambda - v_f \\, \\Vert_\\infty $",
"$\\Vert \\, \\lambda - P_K(\\lambda) \\, \\Vert_\\infty $",
"$\\Vert \\, v_a^+ - P_{K^*}(v_a^+) \\, \\Vert_\\infty $",
"$\\Vert \\, \\lambda^T \\, v_a^+ \\, \\Vert_\\infty $",
"$\\Vert \\, \\lambda - P_{K^*}(\\lambda - v_a^+(\\lambda)) \\, \\Vert_\\infty $",
"$ 0.5 \\, \\lambda^T \\, D \\, \\lambda + \\lambda^T \\, (v_f + s) $",
"$ 0.5 \\, \\lambda^T \\, D \\, \\lambda + v_f^T \\, \\lambda $",
]
titles = [
f"Equations-of-Motion Residual \n ({equations[0]})",
f"Joint Kinematics Constraint Residual \n ({equations[1]})",
f"Joints Constraint Residual \n ({equations[2]})",
f"Limits Constraint Residual \n ({equations[3]})",
f"Contacts Constraint Residual \n ({equations[4]})",
f"Post-Event Constraint Velocity Residual \n ({equations[5]})",
f"NCP Primal Residual \n ({equations[6]})",
f"NCP Dual Residual\n ({equations[7]})",
f"NCP Complementary Residual\n ({equations[8]})",
f"VI Natural-Map Residual\n ({equations[9]})",
f"NCP Objective \n ({equations[10]})",
f"CCP Objective \n ({equations[11]})",
]
names = [
"r_eom",
"r_kinematics",
"r_cts_joints",
"r_cts_limits",
"r_cts_contacts",
"r_v_plus",
"r_ncp_primal",
"r_ncp_dual",
"r_ncp_compl",
"r_vi_natmap",
"f_ncp",
"f_ccp",
]
data = [
self.physics_metrics.r_eom[prob_idx, :, :],
self.physics_metrics.r_kinematics[prob_idx, :, :],
self.physics_metrics.r_cts_joints[prob_idx, :, :],
self.physics_metrics.r_cts_limits[prob_idx, :, :],
self.physics_metrics.r_cts_contacts[prob_idx, :, :],
self.physics_metrics.r_v_plus[prob_idx, :, :],
self.physics_metrics.r_ncp_primal[prob_idx, :, :],
self.physics_metrics.r_ncp_dual[prob_idx, :, :],
self.physics_metrics.r_ncp_compl[prob_idx, :, :],
self.physics_metrics.r_vi_natmap[prob_idx, :, :],
self.physics_metrics.f_ncp[prob_idx, :, :],
self.physics_metrics.f_ccp[prob_idx, :, :],
]
for metric_idx, (title, name, array) in enumerate(zip(titles, names, data, strict=True)):
ax = axs[metric_idx // 4, metric_idx % 4]
for config_idx, config_name in enumerate(self._config_names):
ax.plot(
np.arange(self.num_steps),
array[config_idx, :],
label=config_name,
marker="o",
markersize=4,
)
ax.set_title(title)
ax.set_xlabel("Simulation Step")
ax.set_ylabel(name)
ax.grid()
plt.tight_layout(rect=[0, 0.03, 1, 0.95], h_pad=3.0, w_pad=2.0)
# Get handles/labels from any one subplot (since they are identical)
handles, labels = axs.flat[0].get_legend_handles_labels()
# Add ONE legend for the whole figure, centered at the bottom
fig.legend(
handles,
labels,
loc="lower center",
ncol=len(labels), # put entries in one row
bbox_to_anchor=(0.5, 0.02), # (x, y) in figure coordinates
frameon=False,
)
# Make room at the bottom for the legend
fig.subplots_adjust(bottom=0.15)
# If a path is provided, also save the plot to an image file at the specified path
if path is not None:
# Check if the directory for the specified path exists, and if not, create it
path_dir = os.path.dirname(path)
if path_dir and not os.path.exists(path_dir):
raise ValueError(
f"Directory for path '{path}' does not exist. "
"Please create the directory before saving the plot."
)
fig_path = path + f"_{prob_name}.pdf"
plt.savefig(fig_path, format="pdf", dpi=300, bbox_inches="tight")
# Close the figure to free up memory after saving
# (or if not saving) before the next iteration
plt.close(fig)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/benchmark/problems.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from collections.abc import Callable
from dataclasses import dataclass
import warp as wp
import newton.utils
from ...core.builder import ModelBuilderKamino
from ...models import basics
from ...models.builders.utils import (
add_ground_box,
make_homogeneous_builder,
set_uniform_body_pose_offset,
)
from ...utils import logger as msg
from ..io.usd import USDImporter
###
# Module interface
###
__all__ = [
"BenchmarkProblemNameToConfigFn",
"CameraConfig",
"ControlConfig",
"ProblemConfig",
"ProblemDimensions",
"ProblemSet",
"make_benchmark_problems",
"save_problem_dimensions_to_hdf5",
]
###
# Types
###
@dataclass
class ProblemDimensions:
num_body_dofs: int = -1
num_joint_dofs: int = -1
min_delassus_dim: int = -1
max_delassus_dim: int = -1
@dataclass
class ControlConfig:
disable_controller: bool = False
decimation: int | list[int] | None = None
scale: float | list[float] | None = None
# TODO: Use set_camera_lookat params instead
@dataclass
class CameraConfig:
position: tuple[float, float, float]
pitch: float
yaw: float
ProblemConfig = tuple[ModelBuilderKamino | Callable, ControlConfig | None, CameraConfig | None]
"""
Defines the configurations for a single benchmark problem.
This contains:
- A model builder that constructs the simulation worlds for the benchmark problem, or a callable
taking no arguments returning such a builder (for deferred loading of the problem assets).
- Optional control configurations for perturbing the benchmark problem.
- Optional camera configurations for visualizing the benchmark problem.
"""
ProblemSet = dict[str, ProblemConfig]
"""
Defines a set of benchmark problems, indexed by a string name.
Each entry contains the configurations for a single
benchmark problem, including the model builder and
optional camera configurations for visualization.
"""
###
# Problem Definitions
###
def make_benchmark_problem_fourbar(
num_worlds: int = 1,
gravity: bool = True,
ground: bool = True,
) -> ProblemConfig:
def builder_fn():
builder = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=basics.build_boxes_fourbar,
ground=ground,
)
for w in range(num_worlds):
builder.gravity[w].enabled = gravity
return builder
control = ControlConfig(decimation=20, scale=10.0)
camera = CameraConfig(
position=(-0.2, -0.5, 0.1),
pitch=-5.0,
yaw=70.0,
)
return builder_fn, control, camera
def make_benchmark_problem_dr_legs(
num_worlds: int = 1,
gravity: bool = True,
ground: bool = True,
) -> ProblemConfig:
# Set the path to the external USD assets
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_legs/usd" / "dr_legs_with_meshes_and_boxes.usda")
def builder_fn():
# Create a model builder from the imported USD
msg.notif("Constructing builder from imported USD ...")
importer = USDImporter()
builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=importer.import_from,
load_static_geometry=True,
source=asset_file,
load_drive_dynamics=True,
use_angular_drive_scaling=True,
)
# Offset the model to place it above the ground
# NOTE: The USD model is centered at the origin
offset = wp.transformf(0.0, 0.0, 0.265, 0.0, 0.0, 0.0, 1.0)
set_uniform_body_pose_offset(builder=builder, offset=offset)
# Add a static collision geometry for the plane
if ground:
for w in range(num_worlds):
add_ground_box(builder, world_index=w)
# Set gravity
for w in range(builder.num_worlds):
builder.gravity[w].enabled = gravity
return builder
# Set control configurations
control = ControlConfig(decimation=20, scale=5.0)
# Set the camera configuration for better visualization of the system
camera = CameraConfig(
position=(0.6, 0.6, 0.3),
pitch=-10.0,
yaw=225.0,
)
return builder_fn, control, camera
###
# Problem Set Generator
###
BenchmarkProblemNameToConfigFn: dict[str, Callable[..., ProblemConfig]] = {
"fourbar": make_benchmark_problem_fourbar,
"dr_legs": make_benchmark_problem_dr_legs,
}
"""
Defines a mapping from benchmark problem names to their
corresponding problem configuration generator functions.
"""
def make_benchmark_problems(
names: list[str],
num_worlds: int = 1,
gravity: bool = True,
ground: bool = True,
) -> ProblemSet:
# Ensure that problem names are provided and valid
if names is None:
raise ValueError("Problem names must be provided as a list of strings.")
# Define common generator kwargs for all problems to avoid repetition
generator_kwargs = {"num_worlds": num_worlds, "gravity": gravity, "ground": ground}
# Generate the problem configurations for each specified problem name
problems: ProblemSet = {}
for name in names:
if name not in BenchmarkProblemNameToConfigFn.keys():
raise ValueError(
f"Unsupported problem name: {name}.\nSupported names are: {list(BenchmarkProblemNameToConfigFn.keys())}"
)
problems[name] = BenchmarkProblemNameToConfigFn[name](**generator_kwargs)
return problems
def save_problem_dimensions_to_hdf5(problem_dims: dict[str, ProblemDimensions], datafile):
for problem_name, dims in problem_dims.items():
scope = f"Problems/{problem_name}"
datafile[f"{scope}/num_body_dofs"] = dims.num_body_dofs
datafile[f"{scope}/num_joint_dofs"] = dims.num_joint_dofs
datafile[f"{scope}/min_delassus_dim"] = dims.min_delassus_dim
datafile[f"{scope}/max_delassus_dim"] = dims.max_delassus_dim
================================================
FILE: newton/_src/solvers/kamino/_src/utils/benchmark/render.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import os
from collections.abc import Callable
from dataclasses import dataclass
from typing import Any
import numpy as np
from ...linalg.linear import LinearSolverTypeToName
from ...solver_kamino_impl import SolverKaminoImpl
from .problems import ProblemDimensions
###
# Module interface
###
__all__ = [
"render_problem_dimensions_table",
"render_solver_configs_table",
"render_subcolumn_metrics_table",
"render_subcolumn_table",
"render_table",
]
###
# Internals
###
def _add_table_column_group(
table,
group_name: str,
subcol_headers: list[str],
justify: str = "left",
color: str | None = None,
) -> None:
# Attempt to import rich first, and warn user
# if the necessary package is not installed
try:
from rich.text import Text # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `rich` package is required for rendering tables. Install it with: pip install rich"
) from e
for i, sub in enumerate(subcol_headers):
header = Text(justify="left")
if i == 0:
header.append(group_name, style=f"bold {color}" if color else "bold")
header.append("\n")
header.append(sub, style=f"dim {color}" if color else "dim")
col_justify = "center" if justify == "center" else justify
table.add_column(header=header, justify=col_justify, no_wrap=True)
def _render_table_to_console_and_file(
table,
path: str | None = None,
to_console: bool = False,
max_width: int | None = None,
):
# Attempt to import rich first, and warn user
# if the necessary package is not installed
try:
from rich.console import Console # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `rich` package is required for rendering tables. Install it with: pip install rich"
) from e
if path is not None:
path_dir = os.path.dirname(path)
if path_dir and not os.path.exists(path_dir):
raise ValueError(
f"Directory for path '{path}' does not exist. Please create the directory before exporting the table."
)
with open(path, "w", encoding="utf-8") as f:
console = Console(file=f, width=9999999)
console.print(table, crop=False)
if to_console:
console = Console(width=max_width)
console.rule()
console.print(table, crop=False)
console.rule()
def _format_cell(x, fmt):
if callable(fmt):
return fmt(x)
if isinstance(fmt, str):
try:
return format(x, fmt)
except Exception:
return str(x)
if isinstance(x, (bool, np.bool_)):
return str(x)
if isinstance(x, (int, np.integer)):
return str(x)
if isinstance(x, (float, np.floating)):
return f"{float(x):.4g}"
return str(x)
@dataclass
class ColumnGroup:
header: str
subheaders: list[str]
subfmt: list[str | Callable | None] | None = None
justify: str = "left"
color: str | None = None
###
# Renderers
###
def render_table(
title: str,
col_headers: list[str],
col_colors: list[str],
col_fmt: list[str | Callable | None] | None,
data: list[Any] | np.ndarray,
transposed: bool = False,
max_width: int | None = None,
path: str | None = None,
to_console: bool = False,
):
# Attempt to import rich first, and warn user
# if the necessary package is not installed
try:
from rich import box # noqa: PLC0415
from rich.table import Table # noqa: PLC0415
from rich.text import Text # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `rich` package is required for rendering tables. Install it with: pip install rich"
) from e
# Initialize the table with appropriate columns and styling
table = Table(
title=title,
show_header=True,
box=box.SIMPLE_HEAVY,
show_lines=True,
pad_edge=True,
)
# Add groups of columns based on the specified groups to include in the table
for header, color in zip(col_headers, col_colors, strict=True):
cheader = Text(justify="left")
cheader.append(header, style=f"bold {color}" if color else "bold")
table.add_column(header=cheader, justify="left", no_wrap=True)
# Add data rows
# If the data is transposed we need to extract from per-column format
if transposed:
ncols = len(data)
nrows = len(data[0])
for r in range(nrows):
rowdata = []
for c in range(ncols):
rowdata.append(_format_cell(data[c][r], col_fmt[c] if col_fmt else None))
table.add_row(*rowdata)
else:
for row in data:
rowdata = []
for rc in range(len(col_headers)):
rowdata.append(_format_cell(row[rc], col_fmt[rc] if col_fmt else None))
table.add_row(*rowdata)
# Render the table to the console and/or save to file
_render_table_to_console_and_file(table, path=path, to_console=to_console, max_width=max_width)
def render_subcolumn_table(
title: str,
cols: list[ColumnGroup],
rows: list[list[Any]] | np.ndarray,
max_width: int | None = None,
path: str | None = None,
to_console: bool = False,
):
# Attempt to import rich first, and warn user
# if the necessary package is not installed
try:
from rich import box # noqa: PLC0415
from rich.table import Table # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `rich` package is required for rendering tables. Install it with: pip install rich"
) from e
# Initialize the table with appropriate columns and styling
table = Table(
title=title,
show_header=True,
box=box.SIMPLE_HEAVY,
show_lines=True,
pad_edge=True,
)
# Add groups of columns based on the specified groups to include in the table
for cgroup in cols:
_add_table_column_group(table, cgroup.header, cgroup.subheaders, color=cgroup.color, justify=cgroup.justify)
# Add data rows
for row in rows:
rowdata = []
for rc in range(len(cols)):
for rsc in range(len(cols[rc].subheaders)):
rowdata.append(_format_cell(row[rc][rsc], cols[rc].subfmt[rsc] if cols[rc].subfmt else None))
table.add_row(*rowdata)
# Render the table to the console and/or save to file
_render_table_to_console_and_file(table, path=path, to_console=to_console, max_width=max_width)
def render_subcolumn_metrics_table(
title: str,
row_header: str,
col_titles: list[str],
row_titles: list[str],
subcol_titles: list[str],
subcol_data: list[np.ndarray],
subcol_formats: list[str | Callable] | None = None,
max_width: int | None = None,
path: str | None = None,
to_console: bool = False,
):
# Attempt to import rich first, and warn user
# if the necessary package is not installed
try:
from rich import box # noqa: PLC0415
from rich.table import Table # noqa: PLC0415
from rich.text import Text # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `rich` package is required for rendering tables. Install it with: pip install rich"
) from e
n_metrics = len(subcol_data)
n_problems = len(col_titles)
n_solvers = len(row_titles)
if len(subcol_titles) != n_metrics:
raise ValueError("subcol_titles length must match number of metric arrays")
for i, arr in enumerate(subcol_data):
if arr.shape != (n_problems, n_solvers):
raise ValueError(f"subcol_data[{i}] has shape {arr.shape}, expected {(n_problems, n_solvers)}")
if subcol_formats is None:
subcol_formats = [None] * n_metrics
if len(subcol_formats) != n_metrics:
raise ValueError("subcol_formats length must match number of metrics")
def format_value(x, fmt):
if callable(fmt):
return fmt(x)
if isinstance(fmt, str):
try:
return format(x, fmt)
except Exception:
return str(x)
if isinstance(x, (int, np.integer)):
return f"{int(x)}"
if isinstance(x, (float, np.floating)):
return f"{float(x):.4g}"
return str(x)
table = Table(
title=title,
header_style="bold cyan",
box=box.SIMPLE_HEAVY,
show_lines=False,
pad_edge=True,
)
# Solver column
table.add_column(row_header, style="bold", no_wrap=True, justify="left")
# Metric columns: problem shown only on first subcolumn in each block
# Header is a Text object with justify="left" (works on rich 14.0.0)
for p_name in col_titles:
for m_idx, m_name in enumerate(subcol_titles):
top = p_name if m_idx == 0 else ""
header = Text(justify="left")
if top:
header.append(top, style="bold")
header.append("\n")
header.append(m_name, style="dim")
table.add_column(
header=header,
justify="right", # numeric cells
no_wrap=True,
)
# Data rows
for s_idx, solver in enumerate(row_titles):
row = [solver]
for p_idx in range(n_problems):
for m_idx in range(n_metrics):
value = subcol_data[m_idx][p_idx, s_idx]
row.append(format_value(value, subcol_formats[m_idx]))
table.add_row(*row)
# Render the table to the console and/or save to file
_render_table_to_console_and_file(table, path=path, to_console=to_console, max_width=max_width)
###
# Solver Configs
###
def render_solver_configs_table(
configs: dict[str, SolverKaminoImpl.Config],
path: str | None = None,
groups: list[str] | None = None,
to_console: bool = False,
):
"""
Renders a rich table summarizing the solver configurations.
Args:
configs (dict[str, SolverKaminoImpl.Config]):
A dictionary mapping configuration names to SolverKaminoImpl.Config objects.
path (str, optional):
The file path to save the rendered table as a text file. If None, the table is not saved to a file.
groups (list[str], optional):
A list of groups to include in the table. If None, "sparse", "linear" and "padmm" are used.\n
Supported groups include:
- "cts": Constraint parameters (alpha, beta, gamma, delta, preconditioning)
- "sparse": Sparse representation settings (sparse, sparse_jacobian)
- "linear": Linear solver settings (type, kwargs)
- "padmm": PADMM settings (max_iterations, primal_tol, dual_tol, etc)
- "warmstart": Warmstarting settings (mode, contact_method)
to_console (bool, optional):
If True, also prints the table to the console.
Raises:
ValueError:
If the configs dictionary is empty or if any of the configuration objects are missing required attributes.
IOError:
If there is an error writing the table to the specified file path.
"""
# Attempt to import rich first, and warn user
# if the necessary package is not installed
try:
from rich import box # noqa: PLC0415
from rich.table import Table # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `rich` package is required for rendering tables. Install it with: pip install rich"
) from e
# Initialize the table with appropriate columns and styling
table = Table(
title="Solver Configurations Summary",
show_header=True,
box=box.SIMPLE_HEAVY,
show_lines=True,
pad_edge=True,
)
# If no groups are specified, default to showing sparsity, linear solver and PADMM settings
if groups is None:
groups = ["sparse", "linear", "padmm"]
# Add the first column for configuration names
_add_table_column_group(table, "Solver Configuration", ["Name"], color="white", justify="left")
# Add groups of columns based on the specified groups to include in the table
if "cts" in groups:
_add_table_column_group(table, "Constraints", ["alpha", "beta", "gamma", "delta", "precond"], color="green")
if "sparse" in groups:
_add_table_column_group(table, "Representation", ["sparse_jacobian", "sparse_dynamics"], color="yellow")
if "linear" in groups:
_add_table_column_group(table, "Linear Solver", ["type", "kwargs"], color="magenta")
if "padmm" in groups:
_add_table_column_group(
table,
"PADMM",
[
"max_iterations",
"primal_tol",
"dual_tol",
"compl_tol",
"restart_tol",
"eta",
"rho_0",
"rho_min",
"penalty_update",
"penalty_freq",
"accel",
],
color="cyan",
)
if "warmstart" in groups:
_add_table_column_group(table, "Warmstarting", ["mode", "contact_method"], color="blue")
# Add rows for each configuration
for name, cfg in configs.items():
cfg_row = []
if "cts" in groups:
cfg_row.extend(
[
f"{cfg.constraints.alpha}",
f"{cfg.constraints.beta}",
f"{cfg.constraints.gamma}",
f"{cfg.constraints.delta}",
str(cfg.dynamics.preconditioning),
]
)
if "sparse" in groups:
cfg_row.extend([str(cfg.sparse_jacobian), str(cfg.sparse_dynamics)])
if "linear" in groups:
cfg_row.extend(
[
str(LinearSolverTypeToName[cfg.dynamics.linear_solver_type]),
str(cfg.dynamics.linear_solver_kwargs),
]
)
if "padmm" in groups:
cfg_row.extend(
[
str(cfg.padmm.max_iterations),
f"{cfg.padmm.primal_tolerance:.0e}",
f"{cfg.padmm.dual_tolerance:.0e}",
f"{cfg.padmm.compl_tolerance:.0e}",
f"{cfg.padmm.restart_tolerance:.0e}",
f"{cfg.padmm.eta:.0e}",
f"{cfg.padmm.rho_0}",
f"{cfg.padmm.rho_min}",
cfg.padmm.penalty_update_method,
str(cfg.padmm.penalty_update_freq),
str(cfg.padmm.use_acceleration),
]
)
if "warmstart" in groups:
cfg_row.extend([cfg.padmm.warmstart_mode, cfg.padmm.contact_warmstart_method])
table.add_row(name, *cfg_row)
# Render the table to the console and/or save to file
_render_table_to_console_and_file(table, path=path, to_console=to_console, max_width=None)
###
# Problem Dimensions
###
def render_problem_dimensions_table(
problem_dims: dict[str, ProblemDimensions],
path: str | None = None,
to_console: bool = False,
):
"""
Renders a rich table summarizing the problem dimensions.
Args:
configs (dict[str, SolverKaminoImpl.Config]):
A dictionary mapping configuration names to problem dimensions.
path (str, optional):
The file path to save the rendered table as a text file. If None, the table is not saved to a file.
to_console (bool, optional):
If True, also prints the table to the console.
Raises:
ValueError:
If the configs dictionary is empty or if any of the configuration objects are missing required attributes.
IOError:
If there is an error writing the table to the specified file path.
"""
# Attempt to import rich first, and warn user
# if the necessary package is not installed
try:
from rich import box # noqa: PLC0415
from rich.table import Table # noqa: PLC0415
except ImportError as e:
raise ImportError(
"The `rich` package is required for rendering tables. Install it with: pip install rich"
) from e
# Initialize the table with appropriate columns and styling
table = Table(
title="Problem Dimensions Summary",
show_header=True,
box=box.SIMPLE_HEAVY,
show_lines=True,
pad_edge=True,
)
# Table header
_add_table_column_group(
table,
"",
["Problem", "# Body Dofs", "# Joint Dofs", "Min Delassus Dim", "Max Delassus Dim"],
color="white",
justify="left",
)
# Add row for each problem
for name, dims in problem_dims.items():
row = [
str(dims.num_body_dofs),
str(dims.num_joint_dofs),
str(dims.min_delassus_dim),
str(dims.max_delassus_dim),
]
table.add_row(name, *row)
# Render the table to the console and/or save to file
_render_table_to_console_and_file(table, path=path, to_console=to_console, max_width=None)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/benchmark/runner.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import gc
import time
import warp as wp
import newton
import newton.examples
from ....examples import print_progress_bar
from ...core.builder import ModelBuilderKamino
from ...utils import logger as msg
from ...utils.control.rand import RandomJointController
from ...utils.device import get_device_malloc_info
from ...utils.sim import SimulationLogger, Simulator, ViewerKamino
from .metrics import BenchmarkMetrics
from .problems import CameraConfig, ControlConfig, ProblemDimensions
###
# Types
###
class BenchmarkSim:
def __init__(
self,
builder: ModelBuilderKamino,
configs: Simulator.Config,
control: ControlConfig | None = None,
camera: CameraConfig | None = None,
device: wp.DeviceLike = None,
use_cuda_graph: bool = False,
max_steps: int = 1000,
seed: int = 0,
viewer: bool = False,
logging: bool = False,
physics_metrics: bool = False,
):
# Cache the device and other internal flags
self.builder: ModelBuilderKamino = builder
self.device: wp.DeviceLike = device
self.use_cuda_graph: bool = use_cuda_graph
self.max_steps: int = max_steps
# Override the default compute_solution_metrics toggle in the
# simulator configs based on the benchmark configuration
configs.solver.compute_solution_metrics = physics_metrics
# Create a simulator
msg.info("Building the simulator...")
self.sim = Simulator(builder=builder, config=configs, device=device)
if control is None or not control.disable_controller:
# Create a random-action controller for the model
self.ctlr = RandomJointController(
model=self.sim.model,
seed=seed,
decimation=control.decimation if control else None,
scale=control.scale if control else None,
)
# Define a callback function to wrap the execution of the controller
def control_callback(simulator: Simulator):
self.ctlr.compute(
time=simulator.solver.data.time,
control=simulator.control,
)
# Set the control callbacks into the simulator
self.sim.set_control_callback(control_callback)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if logging:
msg.info("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, builder)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if viewer:
msg.info("Creating the 3D viewer...")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
)
if hasattr(self.viewer, "set_camera") and camera is not None:
self.viewer.set_camera(wp.vec3(*camera.position), camera.pitch, camera.yaw)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
# Capture CUDA graph if requested and available
self._capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.info("Warming up simulator...")
self.step_once()
self.reset()
###
# Operations
###
def reset(self):
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logger:
self.logger.reset()
self.logger.log()
def step(self):
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logger:
self.logger.log()
def step_once(self):
self.step()
def render(self):
if self.viewer:
self.viewer.render_frame()
def test(self):
pass
def plot(self, path: str | None = None, show: bool = False):
if self.logger:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
###
# Internals
###
def _capture(self):
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
else:
msg.info("Running with kernels...")
###
# Functions
###
def run_single_benchmark_with_viewer(
args: argparse.Namespace,
simulator: BenchmarkSim,
) -> tuple[float, float]:
start_time = time.time()
newton.examples.run(simulator, args)
stop_time = time.time()
return start_time, stop_time
def run_single_benchmark_with_progress(
simulator: BenchmarkSim,
) -> tuple[float, float]:
start_time = time.time()
for step_idx in range(simulator.max_steps):
simulator.step_once()
wp.synchronize()
print_progress_bar(step_idx + 1, simulator.max_steps, start_time, prefix="Progress", suffix="")
stop_time = time.time()
return start_time, stop_time
def run_single_benchmark_silent(
simulator: BenchmarkSim,
) -> tuple[float, float]:
start_time = time.time()
for _s in range(simulator.max_steps):
simulator.step_once()
wp.synchronize()
stop_time = time.time()
return start_time, stop_time
def run_single_benchmark_with_step_metrics(
problem_idx: int,
config_idx: int,
simulator: BenchmarkSim,
metrics: BenchmarkMetrics,
) -> tuple[float, float]:
start_time = time.time()
step_start_time = float(start_time)
for step_idx in range(simulator.max_steps):
simulator.step_once()
wp.synchronize()
step_stop_time = time.time()
metrics.record_step(problem_idx, config_idx, step_idx, step_stop_time - step_start_time, simulator.sim.solver)
step_start_time = float(step_stop_time)
stop_time = time.time()
return start_time, stop_time
def run_single_benchmark(
problem_idx: int,
config_idx: int,
metrics: BenchmarkMetrics,
args: argparse.Namespace,
builder: ModelBuilderKamino,
configs: Simulator.Config,
control: ControlConfig | None = None,
camera: CameraConfig | None = None,
device: wp.DeviceLike = None,
use_cuda_graph: bool = True,
print_device_info: bool = False,
progress: bool = False,
):
# Create example instance
simulator = BenchmarkSim(
builder=builder,
configs=configs,
control=control,
camera=camera,
device=device,
use_cuda_graph=use_cuda_graph,
max_steps=args.num_steps,
seed=args.seed,
viewer=args.viewer,
physics_metrics=metrics.physics_metrics is not None,
)
msg.info("Starting benchmark run...")
if simulator.viewer:
msg.info("Running in Viewer mode...")
start_time, stop_time = run_single_benchmark_with_viewer(args, simulator)
else:
msg.info(f"Running for {simulator.max_steps} steps...")
if metrics.step_time is not None:
start_time, stop_time = run_single_benchmark_with_step_metrics(problem_idx, config_idx, simulator, metrics)
elif progress:
start_time, stop_time = run_single_benchmark_with_progress(simulator)
else:
start_time, stop_time = run_single_benchmark_silent(simulator)
msg.info("Finished benchmark run.")
# Record final metrics for the benchmark run
metrics.record_total(
problem_idx=problem_idx,
config_idx=config_idx,
total_time=stop_time - start_time,
total_steps=int(simulator.sim.solver.data.time.steps.numpy()[0]),
memory_used=float(wp.get_mempool_used_mem_current(device) if device.is_cuda else 0.0),
)
# Record problem dimensions
problem_name = metrics._problem_names[problem_idx]
if problem_name not in metrics._problem_dims:
metrics._problem_dims[problem_name] = ProblemDimensions(
num_body_dofs=simulator.sim.model.size.max_of_num_body_dofs,
num_joint_dofs=simulator.sim.model.size.max_of_num_joint_dofs,
min_delassus_dim=simulator.sim.model.size.max_of_num_kinematic_joint_cts
+ simulator.sim.model.size.max_of_num_dynamic_joint_cts,
max_delassus_dim=simulator.sim.model.size.max_of_max_total_cts,
)
# Optionally also print the total device memory allocated during the benchmark run
if print_device_info:
mem_info = get_device_malloc_info(simulator.device)
msg.info("[Device malloc info]: %s", mem_info)
# Deallocate simulator to ensure accurate memory consumption measure for the next run
del simulator
gc.collect()
wp.synchronize()
================================================
FILE: newton/_src/solvers/kamino/_src/utils/control/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Utilities for reference motion generation and feedback control."""
from .animation import AnimationJointReference, AnimationJointReferenceData
from .pid import JointSpacePIDController, PIDControllerData
###
# Module interface
###
__all__ = [
"AnimationJointReference",
"AnimationJointReferenceData",
"JointSpacePIDController",
"PIDControllerData",
]
================================================
FILE: newton/_src/solvers/kamino/_src/utils/control/animation.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Containers and interfaces for animation reference tracking."""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
import warp as wp
from ...core.model import ModelKamino
from ...core.time import TimeData
from ...core.types import float32, int32
###
# Module interface
###
__all__ = [
"AnimationJointReference",
"AnimationJointReferenceData",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
@dataclass
class AnimationJointReferenceData:
"""
Container of animation references for actuated joints.
By default, the animation reference is allocated such that all worlds share
the same reference data, but can progress and/or loop independently.
The animation references are organized as 2D arrays where each column corresponds to
an actuated joint DoF and each row corresponds to a frame in the animation sequence.
Progression through the animation sequence is controlled via the ``frame`` attribute,
which indicates the current frame index that is active for each world, from which
actuator joint coordinates and velocities are extracted. In addition, to control the
progression along the reference sequence, each world has its own ``rate`` and
``decimation`` attributes. The ``rate`` attribute (defaults to 1) determines the number
of frames by which to advance the active frame at each step, while the ``decimation``
attribute determines how many steps to wait until the frame index should be updated.
These attributes effectively allow for both slowing down and speeding up the
animation's progression relative to the simulation time-step. Finally, the ``loop``
attribute specifies whether the animation should restart from the beginning after
reaching the end, or stop at the last frame.
Attributes:
num_actuated_joint_dofs: wp.array
The number of actuated joint DoFs per world.
actuated_joint_dofs_offset: wp.array
The offset indices for the actuated joint DoFs per world.
q_j_ref: wp.array
The reference actuator joint positions.
dq_j_ref: wp.array
The reference actuator joint velocities.
loop: wp.array
Flag indicating whether the animation should loop.
rate: wp.array
The rate at which to progress the animation sequence.
decimation: wp.array
The decimation factor for extracting references from the animation sequence.
length: wp.array
The length of the animation sequence.
frame: wp.array
The current frame index in the animation sequence that is active.
"""
num_actuated_joint_dofs: wp.array | None = None
"""
Number of actuated joint DoFs per world.\n
Shape is ``(num_worlds,)`` and dtype is :class:`int32`.
"""
actuated_joint_dofs_offset: wp.array | None = None
"""
Offset indices for the actuated joint DoFs per world.\n
Shape is ``(num_worlds,)`` and dtype is :class:`int32`.
"""
q_j_ref: wp.array | None = None
"""
Sequence of reference joint actuator positions.\n
Shape is ``(max_of_num_actuated_joint_coords, sequence_length)`` and dtype is :class:`float32`.
"""
dq_j_ref: wp.array | None = None
"""
Sequence of reference joint actuator velocities.\n
Shape is ``(max_of_num_actuated_joint_dofs, sequence_length)`` and dtype is :class:`float32`.
"""
length: wp.array | None = None
"""
Integer length of the animation sequence.\n
Shape is ``(num_worlds,)`` and dtype is :class:`int32`.
"""
decimation: wp.array | None = None
"""
Integer decimation factor by which references are extracted from the animation sequence.\n
Shape is ``(num_worlds,)`` and dtype is :class:`int32`.
"""
rate: wp.array | None = None
"""
Integer rate by which to progress the active frame of the animation sequence at each step.\n
Shape is ``(num_worlds,)`` and dtype is :class:`int32`.
"""
loop: wp.array | None = None
"""
Integer flag to indicate if the animation should loop.\n
Shape is ``(num_worlds,)`` and dtype is :class:`int32`.\n
If `1`, the animation will restart from the beginning after reaching the end.\n
If `0`, the animation will stop at the last frame.
"""
frame: wp.array | None = None
"""
Integer index indicating the active frame of the animation sequence.\n
Shape is ``(num_worlds,)`` and dtype is :class:`int32`.
"""
###
# Kernels
###
@wp.kernel
def _advance_animation_frame(
# Inputs
time_steps: wp.array(dtype=int32),
animation_length: wp.array(dtype=int32),
animation_decimation: wp.array(dtype=int32),
animation_rate: wp.array(dtype=int32),
animation_loop: wp.array(dtype=int32),
# Outputs
animation_frame: wp.array(dtype=int32),
):
"""
A kernel to advance the animation frame index for each world
based on time steps, decimation, rate, and loop settings.
"""
# Retrieve the the world index from the thread indices
wid = wp.tid()
# Retrieve the animation sequence info
length = animation_length[wid]
decimation = animation_decimation[wid]
rate = animation_rate[wid]
loop = animation_loop[wid]
# Retrieve the current step (i.e. discrete-time index) for this world
step = time_steps[wid]
# Check if we need to advance the animation frame
if step % decimation != 0:
return
# Retrieve the current frame index for this world
frame = animation_frame[wid]
# Advance the frame index
frame += rate
# If looping is enabled, wrap the frame index around
if loop:
frame %= length
# Otherwise, clamp the frame index to the last frame
else:
if frame >= length:
frame = length - 1
# Update the active reference arrays
animation_frame[wid] = frame
# TODO: Make the 2D arrays as flattened 1D arrays to handle arbitrary layouts
@wp.kernel
def _extract_animation_references(
# Inputs
num_actuated_joint_dofs: wp.array(dtype=int32),
actuated_joint_dofs_offset: wp.array(dtype=int32),
animation_frame: wp.array(dtype=int32),
animation_q_j_ref: wp.array2d(dtype=float32),
animation_dq_j_ref: wp.array2d(dtype=float32),
# Outputs
q_j_ref_active: wp.array(dtype=float32),
dq_j_ref_active: wp.array(dtype=float32),
):
"""
A kernel to extract the active joint-space references from the animation data.
"""
# Retrieve the the world and DoF index from the thread indices
wid, qid = wp.tid()
# Retrieve the number of actuated DoFs and offset for this world
num_ajq = num_actuated_joint_dofs[wid]
ajq_offset = actuated_joint_dofs_offset[wid]
# Ensure we are within the valid range of actuated DoFs for this world
if qid >= num_ajq:
return
# Retrieve the current step index for this world
frame = animation_frame[wid]
# Compute the global DoF index
actuator_dof_index = ajq_offset + qid
# Update the active reference arrays
q_j_ref_active[actuator_dof_index] = animation_q_j_ref[frame, qid]
dq_j_ref_active[actuator_dof_index] = animation_dq_j_ref[frame, qid]
###
# Interfaces
###
class AnimationJointReference:
"""
A module for managing and operating joint-space references from an animation.
"""
def __init__(
self,
model: ModelKamino | None = None,
data: np.ndarray | None = None,
data_dt: float | None = None,
target_dt: float | None = None,
decimation: int | list[int] = 1,
rate: int | list[int] = 1,
loop: bool | list[bool] = True,
use_fd: bool = False,
device: wp.DeviceLike = None,
):
"""
Initialize the animation joint reference interface.
Args:
model (ModelKamino | None): The model container used to determine the required allocation sizes.
If None, calling ``finalize()`` later can be used for deferred allocation.
data (np.ndarray | None): The input animation reference data as a 2D numpy array.
If None, calling ``finalize()`` later can be used for deferred allocation.
data_dt (float | None): The time-step between frames in the input data.
target_dt (float | None): The desired time-step between frames in the animation reference.
If None, defaults to ``data_dt``.
decimation (int | list[int]): Decimation factor(s) defining the rate at which the animation
frame index is updated w.r.t the simulation step. If a list of integers, then frame
progression can proceed independently in each world. Defaults to 1 for all worlds.
rate (int | list[int]): Rate(s) by which to progress the animation frame index each time
the simulation step matches the set decimation. Defaults to 1 for all worlds.
loop (bool | list[bool]): Flag(s) indicating whether the animation should loop.
use_fd (bool): Whether to compute finite-difference velocities from the input coordinates.
device (wp.DeviceLike | None): Device to use for allocations and execution.
"""
# Cache the device
self._device: wp.DeviceLike = device
# Declare the model dimensions meta-data
self._num_worlds: int = 0
self._max_of_num_actuated_dofs: int = 0
self._sequence_length: int = 0
# Declare the internal controller data
self._data: AnimationJointReferenceData | None = None
# If a model is provided, allocate the controller data
if model is not None:
self.finalize(
model=model,
data=data,
data_dt=data_dt,
target_dt=target_dt,
decimation=decimation,
rate=rate,
loop=loop,
use_fd=use_fd,
device=device,
)
###
# Properties
###
@property
def device(self) -> wp.DeviceLike | None:
"""The device used for allocations and execution."""
return self._device
@property
def sequence_length(self) -> int:
"""The length of the animation sequence."""
return self._sequence_length
@property
def data(self) -> AnimationJointReferenceData:
"""The internal animation reference data."""
self._assert_has_data()
return self._data
###
# Internals
###
def _assert_has_data(self) -> None:
"""Check if the internal animation data has been allocated."""
if self._data is None:
raise ValueError("Animation reference data is not allocated. Call finalize() first.")
@staticmethod
def _upsample_reference_coordinates(
q_ref: np.ndarray,
dt_in: float,
dt_out: float,
t0: float = 0.0,
t_start: float | None = None,
t_end: float | None = None,
extrapolate: bool = False,
) -> np.ndarray:
"""
Upsample the given reference joint coordinates from the input time-step to the output time-step.
Args:
q_ref (np.ndarray): Reference joint positions of shape (sequence_length, num_actuated_dofs).
dt_in (float): Input time step between frames.
dt_out (float): Output time step between frames.
t0 (float): Initial time corresponding to the first frame.
t_start (float | None): Start time for the up-sampled reference. If None, uses t0.
t_end (float | None): End time for the up-sampled reference. If None, uses the last input frame time.
extrapolate (bool): Whether to allow extrapolation beyond the input time range.
Returns:
np.ndarray: Up-sampled reference joint positions of shape (new_sequence_length, num_actuated_dofs).
"""
# Attempt to import the required interpolation function from scipy,
# and raise an informative error if scipy is not installed
try:
from scipy.interpolate import interp1d
except ImportError as e:
raise ImportError(
"`scipy` is required for up-sampling reference coordinates. Please install with: `pip install scipy`"
) from e
# Extract the number of samples
num_samples, _ = q_ref.shape
if t_start is None:
t_start = t0
if t_end is None:
t_end = t0 + (num_samples - 1) * dt_in
# Construct time-sample sequences for the original and new references
t_original = t0 + dt_in * np.arange(num_samples)
num_samples_new = int(np.floor((t_end - t_start) / dt_out)) + 1
t_new = t_start + dt_out * np.arange(num_samples_new)
# Create the up-sampling interpolation function
upsample_func = interp1d(
t_original,
q_ref,
axis=0,
kind="linear",
bounds_error=False,
fill_value=("extrapolate" if extrapolate else (q_ref[0], q_ref[-1])),
)
# Evaluate the up-sampling function at the new time samples
# to compute the up-sampled joint coordinate references
return upsample_func(t_new)
@staticmethod
def _compute_finite_difference_velocities(q_ref: np.ndarray, dt: float) -> np.ndarray:
"""
Compute finite-difference velocities for the given reference positions.
Args:
q_ref (np.ndarray): Reference joint positions of shape (sequence_length, num_actuated_dofs).
dt (float): Time step between frames.
Returns:
np.ndarray: Reference joint velocities of shape (sequence_length, num_actuated_dofs).
"""
# TODO: Try this instead (it might be more robust):
# _compute_finite_difference_velocities = staticmethod(lambda q_ref_np, dt: np.gradient(q_ref_np, dt, axis=0))
# First allocate and initialize the output array
dq_j_ref = np.zeros_like(q_ref)
# Guard against single-frame animations (constant set-points)
num_samples = q_ref.shape[0]
if num_samples < 2:
return dq_j_ref
# Compute forward finite-difference velocities for the reference positions
dq_j_ref[1:] = np.diff(q_ref, axis=0) / dt
# Set the first velocity to match the second
dq_j_ref[0] = dq_j_ref[1]
# Apply a simple moving average filter to smooth out the velocities
kernel_size = 5
kernel = np.ones(kernel_size) / kernel_size
for i in range(q_ref.shape[1]):
dq_j_ref[:, i] = np.convolve(dq_j_ref[:, i], kernel, mode="same")
# Return the computed reference joint velocities
return dq_j_ref
###
# Operations
###
def finalize(
self,
model: ModelKamino,
data: np.ndarray,
data_dt: float,
target_dt: float | None = None,
decimation: int | list[int] = 1,
rate: int | list[int] = 1,
loop: bool | list[bool] = True,
use_fd: bool = False,
device: wp.DeviceLike = None,
) -> None:
"""
Allocate the animation joint reference data.
Args:
model (ModelKamino): The model container used to determine the required allocation sizes.
data (np.ndarray): The input animation reference data as a 2D numpy array.
data_dt (float): The time-step between frames in the input data.
target_dt (float | None): The desired time-step between frames in the animation reference.
If None, defaults to ``data_dt``.
decimation (int | list[int]): Decimation factor(s) defining the rate at which the animation
frame index is updated w.r.t the simulation step. If a list of integers, then frame
progression can proceed independently in each world. Defaults to 1 for all worlds.
rate (int | list[int]): Rate(s) by which to progress the animation frame index each time
the simulation step matches the set decimation. Defaults to 1 for all worlds.
loop (bool | list[bool]): Flag(s) indicating whether the animation should loop.
use_fd (bool): Whether to compute finite-difference velocities from the input coordinates.
device (wp.DeviceLike | None): Device to use for allocations and execution.
Raises:
ValueError: If the model is not valid or actuated DoFs are not properly configured.
ValueError: If the input data is not a valid 2D numpy array.
Note:
The model must have only 1-DoF actuated joints for this controller to be compatible.
"""
# Ensure the model is valid
if model is None or model.size is None:
raise ValueError("Model is not valid. Cannot allocate controller data.")
# Retrieve the shape of the input data
if data is None:
raise ValueError("Input data must be provided for allocation.")
# Ensure input array is valid
if not isinstance(data, np.ndarray):
raise ValueError("Input data must be a numpy array.")
if data.ndim != 2:
raise ValueError("Input data must be a 2D numpy array.")
# Get the number of actuated coordinates and DoFs
total_num_actuated_coords = model.size.sum_of_num_actuated_joint_coords
total_num_actuated_dofs = model.size.sum_of_num_actuated_joint_dofs
max_num_actuated_dofs = model.size.max_of_num_actuated_joint_dofs
# Check if there are any actuated DoFs
if total_num_actuated_dofs == 0:
raise ValueError("Model has no actuated DoFs.")
# Ensure the model has only 1-DoF actuated joints
if total_num_actuated_coords != total_num_actuated_dofs:
raise ValueError(
f"Model has {total_num_actuated_coords} actuated coordinates but {total_num_actuated_dofs} actuated "
"DoFs. AnimationJointReference is currently incompatible with multi-DoF actuated joints."
)
# Check that input data matches the number of actuated DoFs
if data.shape[1] != max_num_actuated_dofs and data.shape[0] != max_num_actuated_dofs:
raise ValueError(
f"Input data has shape {data.shape} which does not match the "
f"per-world number of actuated DoFs ({max_num_actuated_dofs})."
)
# We assume the input is organized as (sequence_length, num_actuated_dofs)
# Transpose the input if necessary in order to match the assumed shape
if data.shape[0] == max_num_actuated_dofs or data.shape[0] == 2 * max_num_actuated_dofs:
data = data.T
# Ensure the target time-step is valid
if data_dt <= 0.0:
raise ValueError("Target time-step must be positive.")
# Check the target time-step input and set it to the animation dt if not provided
if target_dt is None:
target_dt = data_dt
# Ensure decimation, rate, and loop are lists matching the number of worlds
if isinstance(decimation, int):
decimation = [decimation] * model.size.num_worlds
if isinstance(rate, int):
rate = [rate] * model.size.num_worlds
if isinstance(loop, bool):
loop = [loop] * model.size.num_worlds
# Optionally upsample the input data with linearly-interpolation to match the target time-step
if target_dt < data_dt:
data = self._upsample_reference_coordinates(
q_ref=data,
dt_in=data_dt,
dt_out=target_dt,
extrapolate=False,
)
# Cache the model dimensions meta-data
self._num_worlds = model.size.num_worlds
self._max_of_num_actuated_dofs = max_num_actuated_dofs
self._sequence_length = data.shape[0]
# Extract the reference joint positions and velocities
q_j_ref_np = data[:, :max_num_actuated_dofs].astype(np.float32)
if data.shape[1] >= 2 * max_num_actuated_dofs:
dq_j_ref_np = data[:, max_num_actuated_dofs : 2 * max_num_actuated_dofs].astype(np.float32)
else:
# Optionally use finite-differences to estimate velocities if requested
if use_fd:
dq_j_ref_np = self._compute_finite_difference_velocities(q_j_ref_np, target_dt)
# Otherwise, default to zero velocities
else:
dq_j_ref_np = np.zeros_like(q_j_ref_np)
# Create the rate and loop arrays
length_np = np.array([q_j_ref_np.shape[0]] * self._num_worlds, dtype=np.int32)
decimation_np = np.array(decimation, dtype=np.int32)
rate_np = np.array(rate, dtype=np.int32)
loop_np = np.array([1 if _l else 0 for _l in loop], dtype=np.int32)
# Override the device if provided
if device is not None:
self._device = device
# Allocate the controller data
with wp.ScopedDevice(self._device):
self._data = AnimationJointReferenceData(
num_actuated_joint_dofs=model.info.num_actuated_joint_dofs,
actuated_joint_dofs_offset=model.info.joint_actuated_dofs_offset,
q_j_ref=wp.array(q_j_ref_np, dtype=float32),
dq_j_ref=wp.array(dq_j_ref_np, dtype=float32),
length=wp.array(length_np, dtype=int32),
decimation=wp.array(decimation_np, dtype=int32),
rate=wp.array(rate_np, dtype=int32),
loop=wp.array(loop_np, dtype=int32),
frame=wp.zeros(self._num_worlds, dtype=int32),
)
def plot(self, path: str | None = None, show: bool = False) -> None:
from matplotlib import pyplot as plt
# Extract numpy arrays for plotting
q_j_ref_np = self._data.q_j_ref.numpy()
dq_j_ref_np = self._data.dq_j_ref.numpy()
# Plot the input data for verification
_, axs = plt.subplots(2, 1, figsize=(10, 6))
for i in range(self._max_of_num_actuated_dofs):
axs[0].plot(q_j_ref_np[:, i], label=f"Joint {i}")
axs[1].plot(dq_j_ref_np[:, i], label=f"Joint {i}")
axs[0].set_title("Reference Joint Positions")
axs[0].set_xlabel("Frame")
axs[0].set_ylabel("Position")
axs[0].legend()
axs[1].set_title("Reference Joint Velocities")
axs[1].set_xlabel("Frame")
axs[1].set_ylabel("Velocity")
axs[1].legend()
plt.tight_layout()
# Save the figure if a path is provided
if path is not None:
plt.savefig(path, dpi=300)
# Show the figure if requested
# NOTE: This will block execution until the plot window is closed
if show:
plt.show()
# Close the plot to free up resources
plt.close()
def loop(self, enabled: bool | list[bool] = True) -> None:
"""
Enable or disable looping of the animation sequence.
Args:
enabled (bool | list[bool]): If True, enable looping. If False, disable looping.
"""
# Ensure the animation data container is allocated
if self._data is None:
raise ValueError("Controller data is not allocated. Call finalize() first.")
# Check if a single value or list is provided and set the loop flags accordingly
if isinstance(enabled, list):
if len(enabled) != self._num_worlds:
raise ValueError("Length of 'enabled' list must match the number of worlds.")
enabled_array = np.array([1 if e else 0 for e in enabled], dtype=np.int32)
self._data.loop.assign(enabled_array)
else:
self._data.loop = wp.array([1 if enabled else 0] * self._num_worlds, dtype=int32)
def advance(self, time: TimeData) -> None:
"""
Advances the animation sequence frame index according to the configured
decimation and rate, in accordance with the current simulation time-step.
Args:
time (TimeData): The time data container holding the current simulation time.
"""
self._assert_has_data()
wp.launch(
_advance_animation_frame,
dim=self._num_worlds,
inputs=[
# Inputs:
time.steps,
self._data.length,
self._data.decimation,
self._data.rate,
self._data.loop,
# Outputs:
self._data.frame,
],
device=self._device,
)
def extract(self, q_j_ref_out: wp.array, dq_j_ref_out: wp.array) -> None:
"""
Extract the reference arrays from the animation sequence at the current frame index.
Args:
q_j_ref_out (wp.array): Output array for the reference joint positions.
dq_j_ref_out (wp.array): Output array for the reference joint velocities.
"""
self._assert_has_data()
wp.launch(
_extract_animation_references,
dim=(self._num_worlds, self._max_of_num_actuated_dofs),
inputs=[
# Inputs:
self._data.num_actuated_joint_dofs,
self._data.actuated_joint_dofs_offset,
self._data.frame,
self._data.q_j_ref,
self._data.dq_j_ref,
# Outputs:
q_j_ref_out,
dq_j_ref_out,
],
device=self._device,
)
def reset(self, q_j_ref_out: wp.array, dq_j_ref_out: wp.array) -> None:
"""
Reset the active frame index of the animation sequence to zero
and extracts the initial references into the output arrays.
Args:
q_j_ref_out (wp.array): Output array for the reference joint positions.
dq_j_ref_out (wp.array): Output array for the reference joint velocities.
"""
self._assert_has_data()
self._data.frame.fill_(0)
self.extract(q_j_ref_out, dq_j_ref_out)
def step(self, time: TimeData, q_j_ref_out: wp.array, dq_j_ref_out: wp.array) -> None:
"""
Advances the animation sequence by the configured decimation and
rate, and extracts the reference arrays at the active frame index.
This is a convenience method that effectively combines
``advance()`` and ``extract()`` into a single operation.
Args:
time (TimeData): The time data container holding the current simulation time.
q_j_ref_out (wp.array): Output array for the reference joint positions.
dq_j_ref_out (wp.array): Output array for the reference joint velocities.
"""
self.advance(time)
self.extract(q_j_ref_out, dq_j_ref_out)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/control/pid.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""PID Controller Interfaces"""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
import warp as wp
from ...core.control import ControlKamino
from ...core.joints import JointActuationType
from ...core.model import ModelKamino
from ...core.state import StateKamino
from ...core.time import TimeData
from ...core.types import FloatArrayLike, IntArrayLike, float32, int32
###
# Module interface
###
__all__ = [
"JointSpacePIDController",
"PIDControllerData",
"compute_jointspace_pid_control",
"reset_jointspace_pid_references",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
@dataclass
class PIDControllerData:
"""A data container for joint-space PID controller parameters and state."""
q_j_ref: wp.array | None = None
"""The reference actuator joint positions."""
dq_j_ref: wp.array | None = None
"""The reference actuator joint velocities."""
tau_j_ref: wp.array | None = None
"""The feedforward actuator joint torques."""
K_p: wp.array | None = None
"""The proportional gains."""
K_i: wp.array | None = None
"""The integral gains."""
K_d: wp.array | None = None
"""The derivative gains."""
integrator: wp.array | None = None
"""Integrator of joint-space position tracking error."""
decimation: wp.array | None = None
"""The control decimation for each world expressed as a multiple of simulation steps."""
###
# Kernels
###
@wp.kernel
def _reset_jointspace_pid_references(
# Inputs
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_actuated_dofs_offset: wp.array(dtype=int32),
model_joints_wid: wp.array(dtype=int32),
model_joints_act_type: wp.array(dtype=int32),
model_joints_num_dofs: wp.array(dtype=int32),
model_joints_dofs_offset: wp.array(dtype=int32),
model_joints_actuated_dofs_offset: wp.array(dtype=int32),
state_joints_q_j: wp.array(dtype=float32),
state_joints_dq_j: wp.array(dtype=float32),
# Outputs
controller_q_j_ref: wp.array(dtype=float32),
controller_dq_j_ref: wp.array(dtype=float32),
):
"""
A kernel to reset motion references of the joint-space controller.
"""
# Retrieve the the joint index from the thread indices
jid = wp.tid()
# Retrieve the joint actuation type
act_type = model_joints_act_type[jid]
# Retrieve the world index from the thread indices
wid = model_joints_wid[jid]
# Only proceed for force actuated joints and at
# simulation steps matching the control decimation
if act_type != JointActuationType.FORCE:
return
# Retrieve the offset of the world's joints in the global DoF vector
world_dof_offset = model_info_joint_dofs_offset[wid]
world_actuated_dof_offset = model_info_joint_actuated_dofs_offset[wid]
# Retrieve the number of DoFs and offset of the joint
num_dofs = model_joints_num_dofs[jid]
dofs_offset = model_joints_dofs_offset[jid]
actuated_dofs_offset = model_joints_actuated_dofs_offset[jid]
# Compute the global DoF offset of the joint
dofs_offset += world_dof_offset
actuated_dofs_offset += world_actuated_dof_offset
# Iterate over the DoFs of the joint
for dof in range(num_dofs):
# Compute the DoF index in the global DoF vector
dof_index = dofs_offset + dof
# Compute the actuator index in the controller vectors
actuator_dof_index = actuated_dofs_offset + dof
# Retrieve the current joint state
q_j = state_joints_q_j[dof_index]
dq_j = state_joints_dq_j[dof_index]
# Retrieve the current controller references
controller_q_j_ref[actuator_dof_index] = q_j
controller_dq_j_ref[actuator_dof_index] = dq_j
@wp.kernel
def _compute_jointspace_pid_control(
# Inputs
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_actuated_dofs_offset: wp.array(dtype=int32),
model_joints_wid: wp.array(dtype=int32),
model_joints_act_type: wp.array(dtype=int32),
model_joints_num_dofs: wp.array(dtype=int32),
model_joints_dofs_offset: wp.array(dtype=int32),
model_joints_actuated_dofs_offset: wp.array(dtype=int32),
model_joints_tau_j_max: wp.array(dtype=float32),
model_time_dt: wp.array(dtype=float32),
state_time_steps: wp.array(dtype=int32),
state_joints_q_j: wp.array(dtype=float32),
state_joints_dq_j: wp.array(dtype=float32),
controller_q_j_ref: wp.array(dtype=float32),
controller_dq_j_ref: wp.array(dtype=float32),
controller_tau_j_ref: wp.array(dtype=float32),
controller_K_p: wp.array(dtype=float32),
controller_K_i: wp.array(dtype=float32),
controller_K_d: wp.array(dtype=float32),
controller_integrator: wp.array(dtype=float32),
controller_decimation: wp.array(dtype=int32),
# Outputs
control_tau_j: wp.array(dtype=float32),
):
"""
A kernel to compute joint-space PID control outputs for force-actuated joints.
"""
# Retrieve the the joint index from the thread indices
jid = wp.tid()
# Retrieve the joint actuation type
act_type = model_joints_act_type[jid]
# Retrieve the world index from the thread indices
wid = model_joints_wid[jid]
# Retrieve the current simulation step
step = state_time_steps[wid]
# Retrieve the control decimation for the world
decimation = controller_decimation[wid]
# Only proceed for force actuated joints and at
# simulation steps matching the control decimation
if act_type != JointActuationType.FORCE or step % decimation != 0:
return
# Retrieve the time step and current time
dt = model_time_dt[wid]
# Decimate the simulation time-step by the control
# decimation to get the effective control time-step
dt *= float32(decimation)
# Retrieve the offset of the world's joints in the global DoF vector
world_dof_offset = model_info_joint_dofs_offset[wid]
world_actuated_dof_offset = model_info_joint_actuated_dofs_offset[wid]
# Retrieve the number of DoFs and offset of the joint
num_dofs = model_joints_num_dofs[jid]
dofs_offset = model_joints_dofs_offset[jid]
actuated_dofs_offset = model_joints_actuated_dofs_offset[jid]
# Compute the global DoF offset of the joint
dofs_offset += world_dof_offset
actuated_dofs_offset += world_actuated_dof_offset
# Iterate over the DoFs of the joint
for dof in range(num_dofs):
# Compute the DoF index in the global DoF vector
joint_dof_index = dofs_offset + dof
# Compute the actuator index in the controller vectors
actuator_dof_index = actuated_dofs_offset + dof
# Retrieve the maximum limit of the generalized actuator forces
tau_j_max = model_joints_tau_j_max[joint_dof_index]
# Retrieve the current joint state
q_j = state_joints_q_j[joint_dof_index]
dq_j = state_joints_dq_j[joint_dof_index]
# Retrieve the current controller references
q_j_ref = controller_q_j_ref[actuator_dof_index]
dq_j_ref = controller_dq_j_ref[actuator_dof_index]
tau_j_ref = controller_tau_j_ref[actuator_dof_index]
# Retrieve the controller gains and integrator state
K_p = controller_K_p[actuator_dof_index]
K_i = controller_K_i[actuator_dof_index]
K_d = controller_K_d[actuator_dof_index]
integrator = controller_integrator[actuator_dof_index]
# Compute integral limit
integrator_max = 0.0
if K_i > 0.0:
integrator_max = tau_j_max / K_i # Overflow is not a concern. wp.clamp will work as expected with inf
# Compute tracking errors
q_j_err = q_j_ref - q_j
dq_j_err = dq_j_ref - dq_j
# Update the integrator state with anti-windup clamping
integrator += q_j_err * dt
integrator = wp.clamp(integrator, -integrator_max, integrator_max)
# Compute the Feed-Forward + PID control generalized forces
# NOTE: We also clamp the final control forces to avoid exceeding actuator limits
tau_j_c = tau_j_ref + K_p * q_j_err + K_d * dq_j_err + K_i * integrator
tau_j_c = wp.clamp(tau_j_c, -tau_j_max, tau_j_max)
# Store the updated integrator state and actuator control forces
controller_integrator[actuator_dof_index] = integrator
control_tau_j[joint_dof_index] = tau_j_c
###
# Launchers
###
def reset_jointspace_pid_references(
# Inputs:
model: ModelKamino,
state: StateKamino,
# Outputs:
controller: PIDControllerData,
) -> None:
"""
A kernel launcher to reset joint-space PID controller motion references.
"""
wp.launch(
_reset_jointspace_pid_references,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs
model.info.joint_dofs_offset,
model.info.joint_actuated_dofs_offset,
model.joints.wid,
model.joints.act_type,
model.joints.num_dofs,
model.joints.dofs_offset,
model.joints.actuated_dofs_offset,
state.q_j,
state.dq_j,
# Outputs
controller.q_j_ref,
controller.dq_j_ref,
],
)
def compute_jointspace_pid_control(
# Inputs:
model: ModelKamino,
state: StateKamino,
time: TimeData,
controller: PIDControllerData,
# Outputs:
control: ControlKamino,
) -> None:
"""
A kernel launcher to compute joint-space PID control outputs for force-actuated joints.
"""
wp.launch(
_compute_jointspace_pid_control,
dim=model.size.sum_of_num_joints,
inputs=[
# Inputs
model.info.joint_dofs_offset,
model.info.joint_actuated_dofs_offset,
model.joints.wid,
model.joints.act_type,
model.joints.num_dofs,
model.joints.dofs_offset,
model.joints.actuated_dofs_offset,
model.joints.tau_j_max,
model.time.dt,
time.steps,
state.q_j,
state.dq_j,
controller.q_j_ref,
controller.dq_j_ref,
controller.tau_j_ref,
controller.K_p,
controller.K_i,
controller.K_d,
controller.integrator,
controller.decimation,
# Outputs
control.tau_j,
],
)
###
# Interfaces
###
class JointSpacePIDController:
"""
A simple PID controller in joint space.
This controller currently only supports single-DoF force-actuated joints.
"""
def __init__(
self,
model: ModelKamino | None = None,
K_p: FloatArrayLike | None = None,
K_i: FloatArrayLike | None = None,
K_d: FloatArrayLike | None = None,
decimation: IntArrayLike | None = None,
device: wp.DeviceLike = None,
):
"""
A simple PID controller in joint space.
Args:
model (ModelKamino | None): The model container describing the system to be simulated.
If None, call ``finalize()`` later.
K_p (FloatArrayLike | None): Proportional gains per actuated joint DoF.
K_i (FloatArrayLike | None): Integral gains per actuated joint DoF.
K_d (FloatArrayLike | None): Derivative gains per actuated joint DoF.
decimation (IntArrayLike | None): Control decimation for each world
expressed as a multiple of simulation steps.
device (wp.DeviceLike | None): Device to use for allocations and execution.
"""
# Cache the device
self._device: wp.DeviceLike = device
# Declare the internal controller data
self._data: PIDControllerData | None = None
# If a model is provided, allocate the controller data
if model is not None:
self.finalize(model, K_p, K_i, K_d, decimation, device)
###
# Properties
###
@property
def data(self) -> PIDControllerData:
"""The internal controller data."""
if self._data is None:
raise RuntimeError("Controller data is not allocated. Call finalize() first.")
return self._data
@property
def device(self) -> wp.DeviceLike:
"""The device used for allocations and execution."""
return self._device
###
# Operations
###
def finalize(
self,
model: ModelKamino,
K_p: FloatArrayLike,
K_i: FloatArrayLike,
K_d: FloatArrayLike,
decimation: IntArrayLike | None = None,
device: wp.DeviceLike = None,
) -> None:
"""
Allocates all internal data arrays of the controller.
Args:
model (ModelKamino): The model container describing the system to be simulated.
K_p (FloatArrayLike): Proportional gains per actuated joint DoF.
K_i (FloatArrayLike): Integral gains per actuated joint DoF.
K_d (FloatArrayLike): Derivative gains per actuated joint DoF.
decimation (IntArrayLike | None): Control decimation for each world expressed
as a multiple of simulation steps. Defaults to 1 for all worlds if None.
device (wp.DeviceLike | None): Device to use for allocations and execution.
Raises:
ValueError: If the model has no actuated DoFs.
ValueError: If the model has multi-DoF actuated joints.
ValueError: If the length of any gain array does not match the number of actuated DoFs.
ValueError: If the length of the decimation array does not match the number of worlds.
"""
# Get the number of actuated coordinates and DoFs
num_actuated_coords = model.size.sum_of_num_actuated_joint_coords
num_actuated_dofs = model.size.sum_of_num_actuated_joint_dofs
# Check if there are any actuated DoFs
if num_actuated_dofs == 0:
raise ValueError("Model has no actuated DoFs.")
# Ensure the model has only 1-DoF actuated joints
if num_actuated_coords != num_actuated_dofs:
raise ValueError(
f"Model has {num_actuated_coords} actuated coordinates but {num_actuated_dofs} actuated DoFs. "
"Joint-space PID control is currently incompatible with multi-DoF actuated joints."
)
# Check length of gain arrays
if K_p is not None and len(K_p) != num_actuated_dofs:
raise ValueError(f"K_p must have length {num_actuated_dofs}, but has length {len(K_p)}")
if K_i is not None and len(K_i) != num_actuated_dofs:
raise ValueError(f"K_i must have length {num_actuated_dofs}, but has length {len(K_i)}")
if K_d is not None and len(K_d) != num_actuated_dofs:
raise ValueError(f"K_d must have length {num_actuated_dofs}, but has length {len(K_d)}")
if decimation is not None and len(decimation) != model.size.num_worlds:
raise ValueError(f"decimation must have length {model.size.num_worlds}, but has length {len(decimation)}")
# Override the device if provided
if device is not None:
self._device = device
# Set default decimation if not provided
if decimation is None:
decimation = np.ones(model.size.num_worlds, dtype=np.int32)
# Allocate the controller data
with wp.ScopedDevice(self._device):
self._data = PIDControllerData(
q_j_ref=wp.zeros(num_actuated_dofs, dtype=float32),
dq_j_ref=wp.zeros(num_actuated_dofs, dtype=float32),
tau_j_ref=wp.zeros(num_actuated_dofs, dtype=float32),
K_p=wp.array(K_p if K_p is not None else np.zeros(num_actuated_dofs), dtype=float32),
K_i=wp.array(K_i if K_i is not None else np.zeros(num_actuated_dofs), dtype=float32),
K_d=wp.array(K_d if K_d is not None else np.zeros(num_actuated_dofs), dtype=float32),
integrator=wp.zeros(num_actuated_dofs, dtype=float32),
decimation=wp.array(decimation, dtype=int32),
)
def reset(self, model: ModelKamino, state: StateKamino) -> None:
"""
Reset the internal state of the controller.
The motion references are reset to the current generalized
joint states `q_j` and `dq_j`, while feedforward generalized
forces `tau_j` and the integrator are set to zeros.
Args:
model (ModelKamino): The model container holding the time-invariant parameters of the simulation.
state (StateKamino): The current state of the system to which the references will be reset.
"""
# First reset the references to the current state
reset_jointspace_pid_references(
model=model,
state=state,
controller=self._data,
)
# Then zero the integrator and feedforward torques
self._data.tau_j_ref.zero_()
self._data.integrator.zero_()
def set_references(
self, q_j_ref: FloatArrayLike, dq_j_ref: FloatArrayLike | None = None, tau_j_ref: FloatArrayLike | None = None
) -> None:
"""
Set the controller reference trajectories.
Args:
q_j_ref (FloatArrayLike): The reference generalized actuator positions.
dq_j_ref (FloatArrayLike | None): The reference generalized actuator velocities.
tau_j_ref (FloatArrayLike | None): The feedforward generalized actuator forces.
"""
if len(q_j_ref) != len(self._data.q_j_ref):
raise ValueError(f"q_j_ref must have length {len(self._data.q_j_ref)}, but has length {len(q_j_ref)}")
self._data.q_j_ref.assign(q_j_ref)
if dq_j_ref is not None:
if len(dq_j_ref) != len(self._data.dq_j_ref):
raise ValueError(
f"dq_j_ref must have length {len(self._data.dq_j_ref)}, but has length {len(dq_j_ref)}"
)
self._data.dq_j_ref.assign(dq_j_ref)
if tau_j_ref is not None:
if len(tau_j_ref) != len(self._data.tau_j_ref):
raise ValueError(
f"tau_j_ref must have length {len(self._data.tau_j_ref)}, but has length {len(tau_j_ref)}"
)
self._data.tau_j_ref.assign(tau_j_ref)
def compute(
self,
model: ModelKamino,
state: StateKamino,
time: TimeData,
control: ControlKamino,
) -> None:
"""
Compute the control torques.
Args:
model (ModelKamino): The input model container holding the time-invariant parameters of the simulation.
state (StateKamino): The input state container holding the current state of the simulation.
time (TimeData): The input time data container holding the current simulation time and steps.
control (ControlKamino): The output control container where the computed control torques will be stored.
"""
compute_jointspace_pid_control(
model=model,
state=state,
time=time,
controller=self._data,
control=control,
)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/control/rand.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Provides utilities for generating random control
inputs for testing and benchmarking purposes.
See this link for relevant details:
https://nvidia.github.io/warp/user_guide/runtime.html#random-number-generation
"""
from __future__ import annotations
from dataclasses import dataclass
from typing import get_args
import numpy as np
import warp as wp
from ...core.control import ControlKamino
from ...core.joints import JointActuationType
from ...core.math import FLOAT32_MAX
from ...core.model import ModelKamino
from ...core.time import TimeData
from ...core.types import FloatArrayLike, IntArrayLike, float32, int32
###
# Module interface
###
__all__ = [
"RandomJointController",
"RandomJointControllerData",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
@dataclass
class RandomJointControllerData:
"""Data container for randomized control reference."""
seed: int = 0
"""Seed for random number generation."""
scale: wp.array | None = None
"""
Scaling applied to randomly generated control inputs.
Shape of `(sum_of_num_actuated_joint_dofs,)` and dtype of :class:`float32`.
"""
decimation: wp.array | None = None
"""
Control decimation of each world expressed as a multiple of simulation steps.
Values greater than `1` result in a zero-order hold of the control
inputs, meaning that they will change only every `decimation` steps.
Shape of `(num_worlds,)` and dtype of :class:`int32`.
"""
###
# Kernels
###
@wp.kernel
def _generate_random_control_inputs(
# Inputs
controller_seed: int,
controller_decimation: wp.array(dtype=int32),
controller_scale: wp.array(dtype=float32),
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_joints_wid: wp.array(dtype=int32),
model_joints_act_type: wp.array(dtype=int32),
model_joints_num_dofs: wp.array(dtype=int32),
model_joints_dofs_offset: wp.array(dtype=int32),
model_joints_tau_j_max: wp.array(dtype=float32),
state_time_steps: wp.array(dtype=int32),
# Outputs
# TODO: Add support for other control types
# (e.g. position and velocity targets)
control_tau_j: wp.array(dtype=float32),
):
"""
A kernel to generate random control inputs for testing and benchmarking purposes.
"""
# Retrieve the the joint index from the thread indices
jid = wp.tid()
# Retrieve the total number of joints from the size of the input arrays
num_joints = model_joints_act_type.shape[0]
# Retrieve the joint actuation type
act_type = model_joints_act_type[jid]
# Retrieve the world index from the thread indices
wid = model_joints_wid[jid]
# Retrieve the current simulation step
step = state_time_steps[wid]
# Retrieve the control decimation for the world
decimation = controller_decimation[wid]
# Only proceed for force actuated joints and at
# simulation steps matching the control decimation
if act_type == JointActuationType.PASSIVE or step % decimation != 0:
return
# Retrieve the offset of the world's joints in the global DoF vector
world_dof_offset = model_info_joint_dofs_offset[wid]
# Retrieve the number of DoFs and offset of the joint
num_dofs_j = model_joints_num_dofs[jid]
dofs_offset_j = model_joints_dofs_offset[jid]
# Compute the global DoF offset of the joint
dofs_start = world_dof_offset + dofs_offset_j
# Iterate over the DoFs of the joint
for dof in range(num_dofs_j):
# Compute the DoF index in the global DoF vector
joint_dof_index = dofs_start + dof
# Retrieve the maximum limit of the generalized actuator forces
tau_j_max = model_joints_tau_j_max[joint_dof_index]
if tau_j_max == FLOAT32_MAX:
tau_j_max = 1.0
# Retrieve the scaling factor for the joint DoF
scale_j = controller_scale[joint_dof_index]
# Initialize a random number generator based on the
# seed, current step, joint index, and DoF index
rng_j_dof = wp.rand_init(controller_seed, (step + 1) * (num_joints * jid + dof))
# Generate a random control input for the joint DoF
tau_j_c = scale_j * wp.randf(rng_j_dof, -1.0, 1.0)
# Clamp the control input to the maximum limits of the actuator
tau_j_c = wp.clamp(tau_j_c, -tau_j_max, tau_j_max)
# Store the updated integrator state and actuator control forces
control_tau_j[joint_dof_index] = tau_j_c
###
# Interfaces
###
class RandomJointController:
"""
Provides a simple interface for generating random
control inputs for testing and benchmarking purposes.
"""
def __init__(
self,
model: ModelKamino | None = None,
device: wp.DeviceLike = None,
decimation: int | IntArrayLike | None = None,
scale: float | FloatArrayLike | None = None,
seed: int | None = None,
):
"""
Instantiates a new `RandomJointController` and allocates
on-device data arrays if a model instance is provided.
Args:
model (`ModelKamino`, optional):
The model container describing the system to be simulated.\n
If `None`, a call to ``finalize()`` must be made later.
device (`wp.DeviceLike`, optional):
Device to use for allocations and execution.\n
Defaults to `None`, in which case the model device is used.
decimation (`int` or `IntArrayLike`, optional):
Control decimation for each world expressed as a multiple of simulation steps.\n
Defaults to `1` for all worlds if `None`.
scale (`float` or `FloatArrayLike`, optional):
Scaling applied to randomly generated control inputs.\n
Can be specified per-DoF as an array of shape `(sum_of_num_actuated_joint_dofs,)`
and dtype of `float32`, or as a single float value applied uniformly across all DoFs.\n
Defaults to `1.0` if `None`.
seed (`int`, optional):
Seed for random number generation. If `None`, it will default to `0`.
"""
# Declare a local reference to the model
# for which this controller is created
self._model: ModelKamino | None = None
# Cache constructor arguments for potential later
self._device: wp.DeviceLike = device
self._decimation: int | IntArrayLike | None = decimation
self._scale: float | FloatArrayLike | None = scale
self._seed: int = seed
# Declare the internal controller data
self._data: RandomJointControllerData | None = None
# If a model is provided, allocate the controller data
if model is not None:
self.finalize(model=model, seed=seed, decimation=decimation, scale=scale, device=device)
###
# Properties
###
@property
def device(self) -> wp.DeviceLike:
"""The device used for allocations and execution."""
if self._data is None:
raise RuntimeError("Controller data is not allocated. Call finalize() first.")
return self._data.decimation.device
@property
def seed(self) -> int:
"""The seed used for random number generation."""
if self._data is None:
raise RuntimeError("Controller data is not allocated. Call finalize() first.")
return self._data.seed
@seed.setter
def seed(self, s: int):
"""Sets the seed used for random number generation."""
if self._data is None:
raise RuntimeError("Controller data is not allocated. Call finalize() first.")
self._data.seed = s
@property
def model(self) -> ModelKamino:
"""The model for which this controller is created."""
if self._model is None:
raise RuntimeError("Controller is not finalized with a model. Call finalize() first.")
return self._model
@property
def data(self) -> RandomJointControllerData:
"""The internal controller data."""
if self._data is None:
raise RuntimeError("Controller data is not allocated. Call finalize() first.")
return self._data
###
# Operations
###
def finalize(
self,
model: ModelKamino,
seed: int | None = None,
decimation: int | IntArrayLike | None = None,
scale: float | FloatArrayLike | None = None,
device: wp.DeviceLike = None,
):
"""
Finalizes the random controller by allocating
on-device data arrays based on the provided model.
Args:
model (`ModelKamino`):
The model container describing the system to be simulated.
device (`wp.DeviceLike`, optional):
Device to use for allocations and execution.\n
Defaults to `None`, in which case the model device is used.
decimation (`int` or `IntArrayLike`, optional):
Control decimation for each world expressed as a multiple of simulation steps.\n
Defaults to `1` for all worlds if `None`.
scale (`float` or `FloatArrayLike`, optional):
Scaling applied to randomly generated control inputs.\n
Can be specified per-DoF as an array of shape `(sum_of_num_actuated_joint_dofs,)`
and dtype of `float32`, or as a single float value applied uniformly across all DoFs.\n
Defaults to `1.0` if `None`.
seed (`int`, optional):
Seed for random number generation. If `None`, it will default to `0`.
Raises:
ValueError: If the model has no actuated DoFs.
ValueError: If the length of the decimation array does not match the number of worlds.
"""
# Ensure the model is valid and assign it to the controller
if model is None:
raise ValueError("ModelKamino must be provided to finalize the controller.")
elif not isinstance(model, ModelKamino):
raise ValueError(f"Expected model to be of type ModelKamino, but got {type(model)}.")
# Cache the model reference for use in the compute function
self._model = model
# Check that the model has joint DoFs
num_joint_dofs = model.size.sum_of_num_joint_dofs
if num_joint_dofs == 0:
raise ValueError("The provided model has no joint DoFs to generate control inputs for.")
# Validate and process the constructor arguments
self._decimation, self._scale, self._seed = self._validate_arguments(
num_worlds=model.size.num_worlds,
num_joint_dofs=num_joint_dofs,
decimation=decimation if decimation is not None else self._decimation,
scale=scale if scale is not None else self._scale,
seed=seed if seed is not None else self._seed,
)
# Override the device if provided, otherwise use the model device
if device is not None:
self._device = device
else:
self._device = model.device
# Allocate the controller data
with wp.ScopedDevice(self._device):
self._data = RandomJointControllerData(
seed=self._seed,
decimation=wp.array(self._decimation, dtype=int32),
scale=wp.array(self._scale, dtype=float32),
)
def compute(self, time: TimeData, control: ControlKamino):
"""
Generate randomized generalized control forces to apply to the system.
Each random values is generated based on the seed, current simulation step,
joint index, and local DoF index to ensure reproducibility across runs.
Args:
model (ModelKamino):
The input model container holding the time-invariant parameters of the simulation.
time (TimeData):
The input time data container holding the current simulation time and steps.
control (ControlKamino):
The output control container where the computed control torques will be stored.
"""
# Ensure a model has been assigned and finalized
if self._model is None or self._data is None:
raise RuntimeError("Controller is not finalized with a model. Call finalize() first.")
# Launch the kernel to compute the random control inputs
wp.launch(
_generate_random_control_inputs,
dim=self._model.size.sum_of_num_joints,
inputs=[
# Inputs
self._data.seed,
self._data.decimation,
self._data.scale,
self._model.info.joint_dofs_offset,
self._model.joints.wid,
self._model.joints.act_type,
self._model.joints.num_dofs,
self._model.joints.dofs_offset,
self._model.joints.tau_j_max,
time.steps,
# Outputs
# TODO: Add support for other control types
# (e.g. position and velocity targets)
control.tau_j,
],
)
###
# Internals
###
def _validate_arguments(
self,
num_worlds: int,
num_joint_dofs: int,
decimation: int | IntArrayLike | None,
scale: float | FloatArrayLike | None,
seed: int | None,
):
# Check if the decimation argument is specified, and validate it accordingly
if decimation is not None:
if isinstance(decimation, int):
_decimation = np.full(num_worlds, decimation, dtype=np.int32)
elif isinstance(decimation, get_args(IntArrayLike)):
decsize = len(decimation)
if decsize != num_worlds:
raise ValueError(f"Expected decimation `IntArrayLike` of length {num_worlds}, but has {decsize}.")
_decimation = np.array(decimation, dtype=np.int32)
else:
raise ValueError(f"Expected decimation of type `int` or `IntArrayLike`, but got {type(decimation)}.")
# Otherwise, set it to the default value of 1 for all worlds
else:
_decimation = np.ones(num_worlds, dtype=np.int32)
# Check if the scale argument is specified, and validate it accordingly
if scale is not None:
if isinstance(scale, (int, float)):
_scale = np.full(num_joint_dofs, float(scale), dtype=np.float32)
elif isinstance(scale, get_args(FloatArrayLike)):
if len(scale) != num_joint_dofs:
raise ValueError(
f"Expected scale `FloatArrayLike` of length {num_joint_dofs}, but has {len(scale)}"
)
_scale = np.array(scale, dtype=np.float32)
else:
raise ValueError(f"Expected scale of type `float` or `FloatArrayLike`, but got {type(scale)}.")
# Otherwise, set it to the default value of 1.0 for all DoFs
else:
_scale = np.full(num_joint_dofs, 1.0, dtype=np.float32)
# Check if the seed argument is specified, and set it accordingly
if seed is not None:
if not isinstance(seed, int):
raise ValueError(f"Expected seed of type `int`, but got {type(seed)}.")
_seed = int(seed)
else:
_seed = int(0)
# Return the validated and processed arguments
return _decimation, _scale, _seed
================================================
FILE: newton/_src/solvers/kamino/_src/utils/device.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""KAMINO: Utilities: CPU/GPU Warp Device Info"""
from typing import Literal
import warp as wp
###
# Module interface
###
__all__ = [
"get_device_malloc_info",
"get_device_spec_info",
]
###
# Functions
###
def _fmt_bytes(bytes: int) -> str:
"""
Helper function to format a byte value into a human-readable string with appropriate units.
Args:
bytes (int):
The number of bytes to format.
Returns:
str:
A formatted string representing the byte value in appropriate units (bytes, KB, MB, GB, etc.).
"""
if bytes < 1024:
return f"{bytes} bytes"
elif bytes < 1024**2:
return f"{bytes / 1024:.3f} KB"
elif bytes < 1024**3:
return f"{bytes / (1024**2):.3f} MB"
elif bytes < 1024**4:
return f"{bytes / (1024**3):.3f} GB"
else:
return f"{bytes / (1024**4):.3f} TB"
def get_device_spec_info(device: wp.DeviceLike) -> str:
"""
Retrieves detailed specifications of a given Warp device as a formatted string.
Args:
device (wp.DeviceLike):
The device for which to retrieve specifications.
Returns:
str:
A formatted string containing detailed specifications for the specified device.
"""
spec_info: str = f"[device: `{device}`]:\n"
spec_info += f" name: {device.name}\n"
spec_info += f" alias: {device.alias}\n"
spec_info += f" arch: {device.arch}\n"
spec_info += f" uuid: {device.uuid}\n"
spec_info += f" ordinal: {device.ordinal}\n"
spec_info += f" pci_bus_id: {device.pci_bus_id}\n"
spec_info += f" is_uva: {device.is_uva}\n"
spec_info += f" is_primary: {device.is_primary}\n"
spec_info += f" is_cubin_supported: {device.is_cubin_supported}\n"
spec_info += f"is_mempool_supported: {device.is_mempool_supported}\n"
spec_info += f" is_mempool_enabled: {device.is_mempool_enabled}\n"
spec_info += f" is_ipc_supported: {device.is_ipc_supported}\n"
spec_info += f" is_cpu: {device.is_cpu}\n"
spec_info += f" is_cuda: {device.is_cuda}\n"
spec_info += f" is_capturing: {device.is_capturing}\n"
spec_info += f" has_context: {device.has_context}\n"
spec_info += f" context: {device.context}\n"
spec_info += f" has_stream: {device.has_stream}\n"
spec_info += f" sm_count: {device.sm_count}\n"
spec_info += f" total_memory: {device.total_memory} (~{_fmt_bytes(device.total_memory)})\n"
spec_info += f" free_memory: {device.free_memory} (~{_fmt_bytes(device.free_memory)})\n"
return spec_info
def get_device_malloc_info(
device: wp.DeviceLike,
usage: Literal["current", "high"] = "current",
resolution: Literal["auto", "bytes", "MB", "GB"] = "auto",
) -> str:
"""
Retrieves memory allocation information for the specified device as a formatted string.
Args:
device (wp.DeviceLike):
The device for which to retrieve memory allocation information.
Returns:
str:
A formatted string containing memory allocation information for the specified device.
"""
# Initialize the info string
malloc_info: str = f"[device: `{device}`][{usage}]: "
# Check resolution argument validity
if resolution not in ["auto", "bytes", "MB", "GB"]:
raise ValueError(f"Invalid resolution `{resolution}`. Must be one of 'auto', 'bytes', 'MB', or 'GB'.")
# Get memory allocation info if a CUDA device
if device.is_cuda:
if usage == "current":
mem_used_bytes = wp.get_mempool_used_mem_current(device)
elif usage == "high":
mem_used_bytes = wp.get_mempool_used_mem_high(device)
else:
raise ValueError(f"Invalid usage `{usage}`. Must be one of 'current' or 'high'.")
mem_used_mb = float(mem_used_bytes) / (1024**2)
mem_used_gb = float(mem_used_bytes) / (1024**3)
if resolution == "bytes":
malloc_info += f"{mem_used_bytes} bytes"
elif resolution == "MB":
malloc_info += f"{mem_used_mb:.3f} MB"
elif resolution == "GB":
malloc_info += f"{mem_used_gb:.3f} GB"
else: # resolution == "auto"
malloc_info += f"{_fmt_bytes(mem_used_bytes)}"
else:
malloc_info: str = f"[device: `{device}`][{usage}]: ERROR: Device does not support CUDA"
# Return the formatted memory allocation info
return malloc_info
================================================
FILE: newton/_src/solvers/kamino/_src/utils/io/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
================================================
FILE: newton/_src/solvers/kamino/_src/utils/io/usd.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Provides mechanisms to import OpenUSD Physics models."""
import uuid
from collections.abc import Iterable
from pathlib import Path
from typing import Any
import numpy as np
import warp as wp
from ......geometry.flags import ShapeFlags
from ......usd import utils as usd_utils
from ......utils.topology import topological_sort_undirected
from ...core.bodies import RigidBodyDescriptor
from ...core.builder import ModelBuilderKamino
from ...core.geometry import GeometryDescriptor
from ...core.gravity import GravityDescriptor
from ...core.joints import (
JOINT_QMAX,
JOINT_QMIN,
JOINT_TAUMAX,
JointActuationType,
JointDescriptor,
JointDoFType,
)
from ...core.materials import (
DEFAULT_DENSITY,
DEFAULT_FRICTION,
DEFAULT_RESTITUTION,
MaterialDescriptor,
MaterialPairProperties,
)
from ...core.math import I_3, screw
from ...core.shapes import (
BoxShape,
CapsuleShape,
ConeShape,
CylinderShape,
EllipsoidShape,
MeshShape,
PlaneShape,
SphereShape,
)
from ...core.types import Axis, AxisType, Transform, quatf, transformf, vec3f
from ...utils import logger as msg
###
# Helper Functions
###
__axis_rotations = {}
def quat_between_axes(*axes: AxisType) -> quatf:
"""
Returns a quaternion that represents the rotations between the given sequence of axes.
Args:
axes (AxisType): The axes between to rotate.
Returns:
wp.quat: The rotation quaternion.
"""
q = wp.quat_identity()
for i in range(len(axes) - 1):
src = Axis.from_any(axes[i])
dst = Axis.from_any(axes[i + 1])
if (src.value, dst.value) in __axis_rotations:
dq = __axis_rotations[(src.value, dst.value)]
else:
dq = wp.quat_between_vectors(src.to_vec3(), dst.to_vec3())
__axis_rotations[(src.value, dst.value)] = dq
q *= dq
return q
###
# Importer
###
class USDImporter:
"""
A class to parse OpenUSD files and extract relevant data.
"""
# Class-level variable to hold the imported modules
Sdf = None
Usd = None
UsdGeom = None
UsdPhysics = None
@classmethod
def _load_pxr_openusd(cls):
"""
Attempts to import the necessary USD modules.
Raises ImportError if the modules cannot be imported.
"""
if cls.Sdf is None:
try:
from pxr import Sdf, Usd, UsdGeom, UsdPhysics
cls.Sdf = Sdf
cls.Usd = Usd
cls.UsdGeom = UsdGeom
cls.UsdPhysics = UsdPhysics
except ImportError as e:
raise ImportError("Failed to import pxr. Please install USD (e.g. via `pip install usd-core`).") from e
def __init__(self):
# Load the necessary USD modules
self._load_pxr_openusd()
self._loaded_pxr: bool = True
self._invert_rotations: bool = False
# Define the axis mapping from USD
self.usd_axis_to_axis = {
self.UsdPhysics.Axis.X: Axis.X,
self.UsdPhysics.Axis.Y: Axis.Y,
self.UsdPhysics.Axis.Z: Axis.Z,
}
# Define the axis mapping from USD
self.usd_dofs_to_axis = {
self.UsdPhysics.JointDOF.TransX: Axis.X,
self.UsdPhysics.JointDOF.TransY: Axis.Y,
self.UsdPhysics.JointDOF.TransZ: Axis.Z,
self.UsdPhysics.JointDOF.RotX: Axis.X,
self.UsdPhysics.JointDOF.RotY: Axis.Y,
self.UsdPhysics.JointDOF.RotZ: Axis.Z,
}
# Define the joint DoF axes for translations and rotations
self._usd_trans_axes = (
self.UsdPhysics.JointDOF.TransX,
self.UsdPhysics.JointDOF.TransY,
self.UsdPhysics.JointDOF.TransZ,
)
self._usd_rot_axes = (
self.UsdPhysics.JointDOF.RotX,
self.UsdPhysics.JointDOF.RotY,
self.UsdPhysics.JointDOF.RotZ,
)
# Define the supported USD joint types
self.supported_usd_joint_types = (
self.UsdPhysics.ObjectType.FixedJoint,
self.UsdPhysics.ObjectType.RevoluteJoint,
self.UsdPhysics.ObjectType.PrismaticJoint,
self.UsdPhysics.ObjectType.SphericalJoint,
self.UsdPhysics.ObjectType.D6Joint,
)
self.supported_usd_joint_type_names = (
"PhysicsFixedJoint",
"PhysicsRevoluteJoint",
"PhysicsPrismaticJoint",
"PhysicsSphericalJoint",
"PhysicsJoint",
)
# Define the supported UsdPhysics shape types
self.supported_usd_physics_shape_types = (
self.UsdPhysics.ObjectType.CapsuleShape,
self.UsdPhysics.ObjectType.Capsule1Shape,
self.UsdPhysics.ObjectType.ConeShape,
self.UsdPhysics.ObjectType.CubeShape,
self.UsdPhysics.ObjectType.CylinderShape,
self.UsdPhysics.ObjectType.Cylinder1Shape,
self.UsdPhysics.ObjectType.PlaneShape,
self.UsdPhysics.ObjectType.SphereShape,
self.UsdPhysics.ObjectType.MeshShape,
)
self.supported_usd_physics_shape_type_names = (
"Capsule",
"Capsule1",
"Cone",
"Cube",
"Cylinder",
"Cylinder1",
"Plane",
"Sphere",
"Mesh",
)
# Define the supported UsdPhysics shape types
self.supported_usd_geom_types = (
self.UsdGeom.Capsule,
self.UsdGeom.Capsule_1,
self.UsdGeom.Cone,
self.UsdGeom.Cube,
self.UsdGeom.Cylinder,
self.UsdGeom.Cylinder_1,
self.UsdGeom.Plane,
self.UsdGeom.Sphere,
self.UsdGeom.Mesh,
)
self.supported_usd_geom_type_names = (
"Capsule",
"Capsule1",
"Cone",
"Cube",
"Cylinder",
"Cylinder1",
"Plane",
"Sphere",
"Mesh",
)
###
# Back-end Functions
###
@staticmethod
def _get_leaf_name(name: str) -> str:
"""Retrieves the name of the prim from its path."""
return Path(name).name
@staticmethod
def _get_prim_path(prim) -> str:
"""Retrieves the name of the prim from its path."""
return str(prim.GetPath())
@staticmethod
def _get_prim_name(prim) -> str:
"""Retrieves the name of the prim from its path."""
return str(prim.GetPath())[len(str(prim.GetParent().GetPath())) :].lstrip("/")
@staticmethod
def _get_prim_uid(prim) -> str:
"""Queries the custom data for a unique identifier (UID)."""
uid = None
cdata = prim.GetCustomData()
if cdata is not None:
uid = cdata.get("uuid", None)
return uid if uid is not None else str(uuid.uuid4())
@staticmethod
def _get_prim_layer(prim) -> str | None:
"""Queries the custom data for a unique identifier (UID)."""
layer = None
cdata = prim.GetCustomData()
if cdata is not None:
layer = cdata.get("layer", None)
return layer
@staticmethod
def _get_prim_parent_body(prim):
if prim is None:
return None
parent = prim.GetParent()
if not parent:
return None
if "PhysicsRigidBodyAPI" in parent.GetAppliedSchemas():
return parent
return USDImporter._get_prim_parent_body(parent)
@staticmethod
def _prim_is_rigid_body(prim) -> bool:
if prim is None:
return False
if "PhysicsRigidBodyAPI" in prim.GetAppliedSchemas():
return True
return False
@staticmethod
def _get_material_default_override(prim) -> bool:
"""Queries the custom data to detect if the prim should override the default material."""
override_default = False
cdata = prim.GetCustomData()
if cdata is not None:
override_default = cdata.get("overrideDefault", False)
return override_default
@staticmethod
def _align_geom_to_axis(axis: Axis, q: wp.quatf) -> wp.quatf:
R_g = wp.quat_to_matrix(q)
match axis:
case Axis.X:
R_g = R_g @ wp.mat33f(0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0)
case Axis.Y:
R_g = R_g @ wp.mat33f(0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0)
case Axis.Z:
pass # No rotation needed
case _:
raise ValueError(f"Unsupported axis: {axis}. Supported axes are: X, Y, Z.")
return wp.quat_from_matrix(R_g)
@staticmethod
def _make_faces_from_counts(indices: np.ndarray, counts: Iterable[int], prim_path: str) -> np.ndarray:
faces = []
face_id = 0
for count in counts:
if count == 3:
faces.append(indices[face_id : face_id + 3])
elif count == 4:
faces.append(indices[face_id : face_id + 3])
faces.append(indices[[face_id, face_id + 2, face_id + 3]])
else:
msg.error(
f"Error while parsing USD mesh {prim_path}: "
f"encountered polygon with {count} vertices, but only triangles and quads are supported."
)
continue
face_id += count
return np.array(faces, dtype=np.int32).flatten()
def _get_attribute(self, prim, name) -> Any:
return prim.GetAttribute(name)
def _has_attribute(self, prim, name) -> bool:
attr = self._get_attribute(prim, name)
return attr.IsValid() and attr.HasAuthoredValue()
def _get_translation(self, prim, local: bool = True) -> wp.vec3f:
xform = self.UsdGeom.Xform(prim)
if local:
mat = np.array(xform.GetLocalTransformation(), dtype=np.float32)
else:
mat = np.array(xform.GetWorldTransformation(), dtype=np.float32)
pos = mat[3, :3]
return wp.vec3f(*pos)
def _get_rotation(self, prim, local: bool = True, invert_rotation: bool = True) -> wp.quatf:
xform = self.UsdGeom.Xform(prim)
if local:
mat = np.array(xform.GetLocalTransformation(), dtype=np.float32)
else:
mat = np.array(xform.GetWorldTransformation(), dtype=np.float32)
if invert_rotation:
rot = wp.quat_from_matrix(wp.mat33(mat[:3, :3].T.flatten()))
else:
rot = wp.quat_from_matrix(wp.mat33(mat[:3, :3].flatten()))
return wp.quatf(*rot)
def _get_transform(self, prim, local: bool = True, invert_rotation: bool = True) -> wp.transformf:
xform = self.UsdGeom.Xform(prim)
if local:
mat = np.array(xform.GetLocalTransformation(), dtype=np.float32)
else:
mat = np.array(xform.GetWorldTransformation(), dtype=np.float32)
if invert_rotation:
rot = wp.quat_from_matrix(wp.mat33(mat[:3, :3].T.flatten()))
else:
rot = wp.quat_from_matrix(wp.mat33(mat[:3, :3].flatten()))
pos = mat[3, :3]
return wp.transform(pos, rot)
def _get_scale(self, prim) -> wp.vec3f:
# first get local transform matrix
local_mat = np.array(self.UsdGeom.Xform(prim).GetLocalTransformation(), dtype=np.float32)
# then get scale from the matrix
scale = np.sqrt(np.sum(local_mat[:3, :3] ** 2, axis=0))
return wp.vec3f(*scale)
def _parse_float(self, prim, name, default=None) -> float | None:
attr = self._get_attribute(prim, name)
if not attr or not attr.HasAuthoredValue():
return default
val = attr.Get()
if np.isfinite(val):
return val
return default
def _parse_float_with_fallback(self, prims: Iterable[Any], name: str, default: float = 0.0) -> float:
ret = default
for prim in prims:
if not prim:
continue
attr = self._get_attribute(prim, name)
if not attr or not attr.HasAuthoredValue():
continue
val = attr.Get()
if np.isfinite(val):
ret = val
break
return ret
@staticmethod
def _from_gfquat(gfquat) -> wp.quatf:
return wp.normalize(wp.quat(*gfquat.imaginary, gfquat.real))
def _parse_quat(self, prim, name, default=None) -> np.ndarray | None:
attr = self._get_attribute(prim, name)
if not attr or not attr.HasAuthoredValue():
return default
val = attr.Get()
if self._invert_rotations:
quat = wp.quat(*val.imaginary, -val.real)
else:
quat = wp.quat(*val.imaginary, val.real)
qn = wp.length(quat)
if np.isfinite(qn) and qn > 0.0:
return quat
return default
def _parse_vec(self, prim, name, default=None) -> np.ndarray | None:
attr = self._get_attribute(prim, name)
if not attr or not attr.HasAuthoredValue():
return default
val = attr.Get()
if np.isfinite(val).all():
return np.array(val, dtype=np.float32)
return default
def _parse_generic(self, prim, name, default=None) -> Any | None:
attr = self._get_attribute(prim, name)
if not attr or not attr.HasAuthoredValue():
return default
return attr.Get()
def _parse_xform(self, prim) -> wp.transformf:
xform = self.UsdGeom.Xform(prim)
mat = np.array(xform.GetLocalTransformation(), dtype=np.float32)
if self._invert_rotations:
rot = wp.quat_from_matrix(wp.mat33(mat[:3, :3].T.flatten()))
else:
rot = wp.quat_from_matrix(wp.mat33(mat[:3, :3].flatten()))
pos = mat[3, :3]
return wp.transform(pos, rot)
def _get_geom_max_contacts(self, prim) -> int:
"""Queries the custom data for the max contacts hint."""
max_contacts = None
cdata = prim.GetCustomData()
if cdata is not None:
max_contacts = cdata.get("maxContacts", None)
return int(max_contacts) if max_contacts is not None else 0
@staticmethod
def _warn_invalid_desc(path, descriptor) -> bool:
if not descriptor.isValid:
msg.warning(f'Invalid {type(descriptor).__name__} descriptor for prim at path "{path}".')
return True
return False
@staticmethod
def _material_pair_properties_from(first: MaterialDescriptor, second: MaterialDescriptor) -> MaterialPairProperties:
pair_properties = MaterialPairProperties()
pair_properties.restitution = 0.5 * (first.restitution + second.restitution)
pair_properties.static_friction = 0.5 * (first.static_friction + second.static_friction)
pair_properties.dynamic_friction = 0.5 * (first.dynamic_friction + second.dynamic_friction)
return pair_properties
def _parse_material(
self,
material_prim,
distance_unit: float = 1.0,
mass_unit: float = 1.0,
) -> MaterialDescriptor | None:
"""
Parses a material prim and returns a MaterialDescriptor.
Args:
material_prim: The USD prim representing the material.
material_spec: The UsdPhysicsRigidBodyMaterialDesc entry.
distance_unit: The global unit of distance of the USD stage.
mass_unit: The global unit of mass of the USD stage.
"""
# Retrieve the namespace path of the prim
path = str(material_prim.GetPath())
msg.debug(f"path: {path}")
# Define and check for the required APIs
req_api = ["PhysicsMaterialAPI"]
for api in req_api:
if api not in material_prim.GetAppliedSchemas():
raise ValueError(
f"Required API '{api}' not found on prim '{path}'. "
"Please ensure the prim has the necessary schemas applied."
)
###
# Prim Identifiers
###
# Retrieve the name and UID of the rigid body from the prim
name = self._get_prim_name(material_prim)
uid = self._get_prim_uid(material_prim)
msg.debug(f"name: {name}")
msg.debug(f"uid: {uid}")
###
# Material Properties
###
# Retrieve the USD material properties
density_scale = mass_unit / distance_unit**3
density = (density_scale) * self._parse_float(material_prim, "physics:density", default=DEFAULT_DENSITY)
restitution = self._parse_float(material_prim, "physics:restitution", default=DEFAULT_RESTITUTION)
static_friction = self._parse_float(material_prim, "physics:staticFriction", default=DEFAULT_FRICTION)
dynamic_friction = self._parse_float(material_prim, "physics:dynamicFriction", default=DEFAULT_FRICTION)
msg.debug(f"density: {density}")
msg.debug(f"restitution: {restitution}")
msg.debug(f"static_friction: {static_friction}")
msg.debug(f"dynamic_friction: {dynamic_friction}")
###
# MaterialDescriptor
###
return MaterialDescriptor(
name=name,
uid=uid,
density=density,
restitution=restitution,
static_friction=static_friction,
dynamic_friction=dynamic_friction,
)
def _parse_rigid_body(
self,
rigid_body_prim,
rigid_body_spec,
distance_unit: float = 1.0,
rotation_unit: float = 1.0,
mass_unit: float = 1.0,
offset_xform: wp.transformf | None = None,
only_load_enabled_rigid_bodies: bool = True,
prim_path_names: bool = False,
) -> RigidBodyDescriptor | None:
# Skip this body if it is not enable and we are only loading enabled rigid bodies
if not rigid_body_spec.rigidBodyEnabled and only_load_enabled_rigid_bodies:
return None
# Retrieve the namespace path of the prim
path = str(rigid_body_prim.GetPath())
# Check the applied schemas
has_rigid_body_api = "PhysicsRigidBodyAPI" in rigid_body_prim.GetAppliedSchemas()
has_mass_api = "PhysicsMassAPI" in rigid_body_prim.GetAppliedSchemas()
# If the prim is a rigid body but has no mass,
# skip it and treat it as static geometry
if has_rigid_body_api and not has_mass_api:
msg.critical(f"rigid body prim ({path}) with no mass found; treating as static geometry")
return None
# Define and check for the required APIs
req_api = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
for api in req_api:
if api not in rigid_body_prim.GetAppliedSchemas():
raise ValueError(
f"Required API '{api}' not found on prim '{path}'. "
"Please ensure the prim has the necessary schemas applied."
)
###
# Prim Identifiers
###
# Retrieve the name and UID of the rigid body from the prim
path = self._get_prim_path(rigid_body_prim)
name = self._get_prim_name(rigid_body_prim)
uid = self._get_prim_uid(rigid_body_prim)
# Use the explicit prim path as the geometry name if specified
name = path if prim_path_names else name
msg.debug(f"[Body]: path: {path}")
msg.debug(f"[Body]: uid: {uid}")
msg.debug(f"[Body]: name: {name}")
###
# PhysicsRigidBodyAPI
###
# Retrieve the rigid body origin (i.e. the pose of the body frame)
body_xform = wp.transform(distance_unit * rigid_body_spec.position, self._from_gfquat(rigid_body_spec.rotation))
# Apply an offset transformation to the origin if provided
if offset_xform is not None:
body_xform = wp.mul(distance_unit * offset_xform, body_xform)
# Retrieve the linear and angular velocities
# NOTE: They are transformed to world coordinates since the
# RigidBodyAPI specifies them in local body coordinates
v_i = wp.transform_vector(body_xform, distance_unit * vec3f(rigid_body_spec.linearVelocity))
omega_i = wp.transform_vector(body_xform, rotation_unit * vec3f(rigid_body_spec.angularVelocity))
msg.debug(f"body_xform: {body_xform}")
msg.debug(f"omega_i: {omega_i}")
msg.debug(f"v_i: {v_i}")
###
# PhysicsMassAPI
###
# Define specialized unit scales
inertia_unit = mass_unit * distance_unit * distance_unit
# Define default values for mass properties
# TODO: What are better defaults?
m_i_default = 0.0
i_r_com_i_default = np.zeros(3, dtype=np.float32)
i_I_i_default = np.zeros((3, 3), dtype=np.float32)
# Extract the mass, center of mass, diagonal inertia, and principal axes from the prim
m_i = mass_unit * self._parse_float(rigid_body_prim, "physics:mass", default=m_i_default)
i_r_com_i = distance_unit * self._parse_vec(rigid_body_prim, "physics:centerOfMass", default=i_r_com_i_default)
i_I_i_diag = inertia_unit * self._parse_vec(rigid_body_prim, "physics:diagonalInertia", default=i_I_i_default)
i_q_i_pa = usd_utils.get_quat(rigid_body_prim, "physics:principalAxes", wp.quat_identity())
msg.debug(f"m_i: {m_i}")
msg.debug(f"i_r_com_i: {i_r_com_i}")
msg.debug(f"i_I_i_diag: {i_I_i_diag}")
msg.debug(f"i_q_i_pa: {i_q_i_pa}")
# Check if the required properties are defined
if m_i is None:
raise ValueError(f"Rigid body '{path}' has no mass defined. Please set the mass using 'physics:mass'.")
if i_r_com_i is None:
raise ValueError(
f"Rigid body '{path}' has no center of mass defined. "
"Please set the center of mass using 'physics:centerOfMass'."
)
if i_I_i_diag is None:
raise ValueError(
f"Rigid body '{path}' has no diagonal inertia defined. "
"Please set the diagonal inertia using 'physics:diagonalInertia'."
)
if i_q_i_pa is None:
raise ValueError(
f"Rigid body '{path}' has no principal axes defined. "
"Please set the principal axes using 'physics:principalAxes'."
)
# Check each property to ensure they are valid
# TODO: What should we check?
# TODO: Should we handle massless bodies?
# Compute the moment of inertia matrix (in body-local coordinates) from the diagonal inertia and principal axes
if np.linalg.norm(i_I_i_diag) > 0.0:
R_i_pa = np.array(wp.quat_to_matrix(i_q_i_pa), dtype=np.float32).reshape(3, 3)
i_I_i = R_i_pa @ np.diag(i_I_i_diag) @ R_i_pa.T
i_I_i = wp.mat33(i_I_i)
i_I_i = 0.5 * (i_I_i + wp.transpose(i_I_i)) # Ensure moment of inertia is symmetric
else:
i_I_i = wp.mat33(0.0)
msg.debug(f"i_I_i_diag:\n{i_I_i_diag}")
msg.debug(f"i_q_i_pa: {i_q_i_pa}")
msg.debug(f"i_I_i:\n{i_I_i}")
# Compute the center of mass in world coordinates
r_com_i = wp.transform_point(body_xform, vec3f(i_r_com_i))
msg.debug(f"r_com_i: {r_com_i}")
# Construct the initial pose and twist of the body in world coordinates
q_i_0 = transformf(r_com_i, body_xform.q)
u_i_0 = screw(v_i, omega_i)
msg.debug(f"q_i_0: {q_i_0}")
msg.debug(f"u_i_0: {u_i_0}")
###
# RigidBodyDescriptor
###
# Construct and return the RigidBodyDescriptor
# with the data imported from the USD prim
return RigidBodyDescriptor(
name=name,
uid=uid,
m_i=m_i,
i_r_com_i=i_r_com_i,
i_I_i=i_I_i,
q_i_0=q_i_0,
u_i_0=u_i_0,
)
def _has_joints(self, ret_dict: dict) -> bool:
"""
Check if the ret_dict contains any joints.
"""
for joint_type in self.supported_usd_joint_types:
if joint_type in ret_dict:
return True
return False
def _get_joint_dof_hint(self, prim) -> JointDoFType | None:
"""Queries the custom data for a DoF type hints."""
dofs = None
cdata = prim.GetCustomData()
if cdata is not None:
dofs = cdata.get("dofs", None)
dof_type = None
if dofs == "cylindrical":
dof_type = JointDoFType.CYLINDRICAL
elif dofs == "universal":
dof_type = JointDoFType.UNIVERSAL
elif dofs == "cartesian":
dof_type = JointDoFType.CARTESIAN
return dof_type
def _make_joint_default_limits(self, dof_type: JointDoFType) -> tuple[list[float], list[float], list[float]]:
num_dofs = int(dof_type.num_dofs)
q_j_min = [JOINT_QMIN] * num_dofs
q_j_max = [JOINT_QMAX] * num_dofs
tau_j_max = [JOINT_TAUMAX] * num_dofs
return q_j_min, q_j_max, tau_j_max
def _make_joint_default_dynamics(
self, dof_type: JointDoFType
) -> tuple[list[float], list[float], list[float], list[float]]:
a_j = None
b_j = None
k_p_j = None
k_d_j = None
return a_j, b_j, k_p_j, k_d_j
def _infer_joint_actuation_type(self, stiffness: float, damping: float, drive_enabled: bool) -> JointActuationType:
if not drive_enabled:
return JointActuationType.PASSIVE
elif stiffness > 0.0 and damping > 0.0:
return JointActuationType.POSITION_VELOCITY
elif stiffness > 0.0:
return JointActuationType.POSITION
elif damping > 0.0:
return JointActuationType.VELOCITY
return JointActuationType.FORCE
def _parse_joint_revolute(
self,
joint_spec,
rotation_unit: float = 1.0,
load_drive_dynamics: bool = False,
use_angular_drive_scaling: bool = True,
):
dof_type = JointDoFType.REVOLUTE
act_type = JointActuationType.PASSIVE
X_j = self.usd_axis_to_axis[joint_spec.axis].to_mat33()
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
a_j, b_j, k_p_j, k_d_j = self._make_joint_default_dynamics(dof_type)
if joint_spec.limit.enabled:
q_j_min[0] = max(rotation_unit * joint_spec.limit.lower, JOINT_QMIN)
q_j_max[0] = min(rotation_unit * joint_spec.limit.upper, JOINT_QMAX)
if joint_spec.drive.enabled:
if not joint_spec.drive.acceleration:
tau_j_max[0] = min(joint_spec.drive.forceLimit, JOINT_TAUMAX)
has_pd_gains = joint_spec.drive.stiffness > 0.0 or joint_spec.drive.damping > 0.0
if load_drive_dynamics and has_pd_gains:
a_j = [0.0] * dof_type.num_dofs
b_j = [0.0] * dof_type.num_dofs
scaling = rotation_unit if use_angular_drive_scaling else 1.0
k_p_j = [joint_spec.drive.stiffness / scaling] * dof_type.num_coords
k_d_j = [joint_spec.drive.damping / scaling] * dof_type.num_dofs
act_type = self._infer_joint_actuation_type(
joint_spec.drive.stiffness, joint_spec.drive.damping, joint_spec.drive.enabled
)
else:
act_type = JointActuationType.FORCE
else:
# TODO: Should we handle acceleration drives?
raise ValueError("Revolute acceleration drive actuators are not yet supported.")
return dof_type, act_type, X_j, q_j_min, q_j_max, tau_j_max, a_j, b_j, k_p_j, k_d_j
def _parse_joint_prismatic(self, joint_spec, distance_unit: float = 1.0, load_drive_dynamics: bool = False):
dof_type = JointDoFType.PRISMATIC
act_type = JointActuationType.PASSIVE
X_j = self.usd_axis_to_axis[joint_spec.axis].to_mat33()
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
a_j, b_j, k_p_j, k_d_j = self._make_joint_default_dynamics(dof_type)
if joint_spec.limit.enabled:
q_j_min[0] = max(distance_unit * joint_spec.limit.lower, JOINT_QMIN)
q_j_max[0] = min(distance_unit * joint_spec.limit.upper, JOINT_QMAX)
if joint_spec.drive.enabled:
if not joint_spec.drive.acceleration:
tau_j_max[0] = min(joint_spec.drive.forceLimit, JOINT_TAUMAX)
has_pd_gains = joint_spec.drive.stiffness > 0.0 or joint_spec.drive.damping > 0.0
if load_drive_dynamics and has_pd_gains:
a_j = [0.0] * dof_type.num_dofs
b_j = [0.0] * dof_type.num_dofs
k_p_j = [joint_spec.drive.stiffness] * dof_type.num_coords
k_d_j = [joint_spec.drive.damping] * dof_type.num_dofs
act_type = self._infer_joint_actuation_type(
joint_spec.drive.stiffness, joint_spec.drive.damping, joint_spec.drive.enabled
)
else:
act_type = JointActuationType.FORCE
else:
# TODO: Should we handle acceleration drives?
raise ValueError("Prismatic acceleration drive actuators are not yet supported.")
return dof_type, act_type, X_j, q_j_min, q_j_max, tau_j_max, a_j, b_j, k_p_j, k_d_j
def _parse_joint_revolute_from_d6(self, name, joint_prim, joint_spec, joint_dof, rotation_unit: float = 1.0):
dof_type = JointDoFType.REVOLUTE
X_j = self.usd_dofs_to_axis[joint_dof].to_mat33()
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
for limit in joint_spec.jointLimits:
dof = limit.first
if dof == joint_dof:
q_j_min[0] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[0] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
num_drives = len(joint_spec.jointDrives)
if num_drives > 0:
if num_drives != 1:
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) has {num_drives} drives, "
"but revolute joints require exactly one drive."
)
act_type = JointActuationType.FORCE
for drive in joint_spec.jointDrives:
if drive.first == joint_dof:
tau_j_max[0] = min(drive.second.forceLimit, JOINT_TAUMAX)
else:
act_type = JointActuationType.PASSIVE
return dof_type, act_type, X_j, q_j_min, q_j_max, tau_j_max
def _parse_joint_prismatic_from_d6(self, name, joint_prim, joint_spec, joint_dof, distance_unit: float = 1.0):
dof_type = JointDoFType.PRISMATIC
X_j = self.usd_dofs_to_axis[joint_dof].to_mat33()
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
for limit in joint_spec.jointLimits:
dof = limit.first
if dof == joint_dof:
q_j_min[0] = max(distance_unit * limit.second.lower, JOINT_QMIN)
q_j_max[0] = min(distance_unit * limit.second.upper, JOINT_QMAX)
num_drives = len(joint_spec.jointDrives)
if num_drives > 0:
if num_drives != 1:
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) has {num_drives} drives, "
"but prismatic joints require exactly one drive."
)
act_type = JointActuationType.FORCE
for drive in joint_spec.jointDrives:
if drive.first == joint_dof:
tau_j_max[0] = min(drive.second.forceLimit, JOINT_TAUMAX)
else:
act_type = JointActuationType.PASSIVE
return dof_type, act_type, X_j, q_j_min, q_j_max, tau_j_max
def _parse_joint_cylindrical_from_d6(
self, name, joint_prim, joint_spec, distance_unit: float = 1.0, rotation_unit: float = 1.0
):
dof_type = JointDoFType.CYLINDRICAL
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
for limit in joint_spec.jointLimits:
dof = limit.first
if dof == self.UsdPhysics.JointDOF.TransX:
q_j_min[0] = max(distance_unit * limit.second.lower, JOINT_QMIN)
q_j_max[0] = min(distance_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.RotX:
q_j_min[1] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[1] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
num_drives = len(joint_spec.jointDrives)
if num_drives > 0:
if num_drives != JointDoFType.CYLINDRICAL.num_dofs:
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) has {num_drives}"
f"drives, but cylindrical joints require {JointDoFType.CYLINDRICAL.num_dofs} drives. "
)
act_type = JointActuationType.FORCE
for drive in joint_spec.jointDrives:
dof = drive.first
if dof == self.UsdPhysics.JointDOF.TransX:
tau_j_max[0] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.RotX:
tau_j_max[1] = min(drive.second.forceLimit, JOINT_TAUMAX)
else:
act_type = JointActuationType.PASSIVE
return dof_type, act_type, q_j_min, q_j_max, tau_j_max
def _parse_joint_universal_from_d6(self, name, joint_prim, joint_spec, rotation_unit: float = 1.0):
dof_type = JointDoFType.UNIVERSAL
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
for limit in joint_spec.jointLimits:
dof = limit.first
if dof == self.UsdPhysics.JointDOF.RotX:
q_j_min[0] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[0] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.RotY:
q_j_min[1] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[1] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
num_drives = len(joint_spec.jointDrives)
if num_drives > 0:
if num_drives != JointDoFType.UNIVERSAL.num_dofs:
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) has {num_drives}"
f"drives, but universal joints require {JointDoFType.UNIVERSAL.num_dofs} drives. "
)
act_type = JointActuationType.FORCE
for drive in joint_spec.jointDrives:
dof = drive.first
if dof == self.UsdPhysics.JointDOF.RotX:
tau_j_max[0] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.RotY:
tau_j_max[1] = min(drive.second.forceLimit, JOINT_TAUMAX)
else:
act_type = JointActuationType.PASSIVE
return dof_type, act_type, q_j_min, q_j_max, tau_j_max
def _parse_joint_cartesian_from_d6(
self,
name,
joint_prim,
joint_spec,
distance_unit: float = 1.0,
):
dof_type = JointDoFType.CARTESIAN
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
for limit in joint_spec.jointLimits:
dof = limit.first
if dof == self.UsdPhysics.JointDOF.TransX:
q_j_min[0] = max(distance_unit * limit.second.lower, JOINT_QMIN)
q_j_max[0] = min(distance_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.TransY:
q_j_min[1] = max(distance_unit * limit.second.lower, JOINT_QMIN)
q_j_max[1] = min(distance_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.TransZ:
q_j_min[2] = max(distance_unit * limit.second.lower, JOINT_QMIN)
q_j_max[2] = min(distance_unit * limit.second.upper, JOINT_QMAX)
num_drives = len(joint_spec.jointDrives)
if num_drives > 0:
if num_drives != JointDoFType.CARTESIAN.num_dofs:
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) has {num_drives}"
f"drives, but cartesian joints require {JointDoFType.CARTESIAN.num_dofs} drives. "
)
act_type = JointActuationType.FORCE
for drive in joint_spec.jointDrives:
dof = drive.first
if dof == self.UsdPhysics.JointDOF.TransX:
tau_j_max[0] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.TransY:
tau_j_max[1] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.TransZ:
tau_j_max[2] = min(drive.second.forceLimit, JOINT_TAUMAX)
else:
act_type = JointActuationType.PASSIVE
return dof_type, act_type, q_j_min, q_j_max, tau_j_max
def _parse_joint_spherical_from_d6(self, name, joint_prim, joint_spec, rotation_unit: float = 1.0):
dof_type = JointDoFType.SPHERICAL
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
for limit in joint_spec.jointLimits:
dof = limit.first
if dof == self.UsdPhysics.JointDOF.RotX:
q_j_min[0] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[0] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.RotY:
q_j_min[1] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[1] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.RotZ:
q_j_min[2] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[2] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
num_drives = len(joint_spec.jointDrives)
if num_drives > 0:
if num_drives != JointDoFType.SPHERICAL.num_dofs:
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) has {num_drives}"
f"drives, but spherical joints require {JointDoFType.SPHERICAL.num_dofs} drives. "
)
act_type = JointActuationType.FORCE
for drive in joint_spec.jointDrives:
dof = drive.first
if dof == self.UsdPhysics.JointDOF.RotX:
tau_j_max[0] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.RotY:
tau_j_max[1] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.RotZ:
tau_j_max[2] = min(drive.second.forceLimit, JOINT_TAUMAX)
else:
act_type = JointActuationType.PASSIVE
return dof_type, act_type, q_j_min, q_j_max, tau_j_max
def _parse_joint_free_from_d6(
self,
name,
joint_prim,
joint_spec,
distance_unit: float = 1.0,
rotation_unit: float = 1.0,
):
dof_type = JointDoFType.FREE
q_j_min, q_j_max, tau_j_max = self._make_joint_default_limits(dof_type)
for limit in joint_spec.jointLimits:
dof = limit.first
if dof == self.UsdPhysics.JointDOF.TransX:
q_j_min[0] = max(distance_unit * limit.second.lower, JOINT_QMIN)
q_j_max[0] = min(distance_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.TransY:
q_j_min[1] = max(distance_unit * limit.second.lower, JOINT_QMIN)
q_j_max[1] = min(distance_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.TransZ:
q_j_min[2] = max(distance_unit * limit.second.lower, JOINT_QMIN)
q_j_max[2] = min(distance_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.RotX:
q_j_min[0] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[0] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.RotY:
q_j_min[1] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[1] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
elif dof == self.UsdPhysics.JointDOF.RotZ:
q_j_min[2] = max(rotation_unit * limit.second.lower, JOINT_QMIN)
q_j_max[2] = min(rotation_unit * limit.second.upper, JOINT_QMAX)
num_drives = len(joint_spec.jointDrives)
if num_drives > 0:
if num_drives != JointDoFType.FREE.num_dofs:
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) has {num_drives}"
f"drives, but free joints require {JointDoFType.FREE.num_dofs} drives. "
)
act_type = JointActuationType.FORCE
for drive in joint_spec.jointDrives:
dof = drive.first
if dof == self.UsdPhysics.JointDOF.TransX:
tau_j_max[0] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.TransY:
tau_j_max[1] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.TransZ:
tau_j_max[2] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.RotX:
tau_j_max[0] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.RotY:
tau_j_max[1] = min(drive.second.forceLimit, JOINT_TAUMAX)
elif dof == self.UsdPhysics.JointDOF.RotZ:
tau_j_max[2] = min(drive.second.forceLimit, JOINT_TAUMAX)
else:
act_type = JointActuationType.PASSIVE
return dof_type, act_type, q_j_min, q_j_max, tau_j_max
def _parse_joint(
self,
stage,
joint_prim,
joint_spec,
joint_type,
body_index_map: dict[str, int],
distance_unit: float = 1.0,
rotation_unit: float = 1.0,
only_load_enabled_joints: bool = True,
load_drive_dynamics: bool = False,
prim_path_names: bool = False,
use_angular_drive_scaling: bool = False,
) -> JointDescriptor | None:
# Skip this body if it is not enable and we are only loading enabled rigid bodies
if not joint_spec.jointEnabled and only_load_enabled_joints:
return None
###
# Prim Identifiers
###
# Retrieve the name and UID of the rigid body from the prim
path = self._get_prim_path(joint_prim)
name = self._get_prim_name(joint_prim)
uid = self._get_prim_uid(joint_prim)
# Use the explicit prim path as the geometry name if specified
name = path if prim_path_names else name
msg.debug(f"[Joint]: path: {path}")
msg.debug(f"[Joint]: uid: {uid}")
msg.debug(f"[Joint]: name: {name}")
###
# PhysicsJoint Common Properties
###
# Check if body0 and body1 are specified
if (not joint_spec.body0) and (not joint_spec.body1):
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) does not specify bodies. "
"Specify the joint bodies using 'physics:body0' and 'physics:body1'."
)
# Extract the relative poses of the joint
B_r_Bj = distance_unit * vec3f(joint_spec.localPose0Position)
F_r_Fj = distance_unit * vec3f(joint_spec.localPose1Position)
B_q_Bj = self._from_gfquat(joint_spec.localPose0Orientation)
F_q_Fj = self._from_gfquat(joint_spec.localPose1Orientation)
msg.debug(f"B_r_Bj (before COM correction): {B_r_Bj}")
msg.debug(f"F_r_Fj (before COM correction): {F_r_Fj}")
msg.debug(f"B_q_Bj: {B_q_Bj}")
msg.debug(f"F_q_Fj: {F_q_Fj}")
# Correct for COM offset
if joint_spec.body0:
body_B_prim = stage.GetPrimAtPath(joint_spec.body0)
i_r_com_B = distance_unit * self._parse_vec(
body_B_prim, "physics:centerOfMass", default=np.zeros(3, dtype=np.float32)
)
B_r_Bj = B_r_Bj - vec3f(i_r_com_B)
msg.debug(f"i_r_com_B: {i_r_com_B}")
msg.debug(f"B_r_Bj (after COM correction): {B_r_Bj}")
if joint_spec.body1:
body_F_prim = stage.GetPrimAtPath(joint_spec.body1)
i_r_com_F = distance_unit * self._parse_vec(
body_F_prim, "physics:centerOfMass", default=np.zeros(3, dtype=np.float32)
)
F_r_Fj = F_r_Fj - vec3f(i_r_com_F)
msg.debug(f"i_r_com_F: {i_r_com_F}")
msg.debug(f"F_r_Fj (after COM correction): {F_r_Fj}")
# Check if body0 is specified
if (not joint_spec.body0) and joint_spec.body1:
# body0 is unspecified, and (0,1) are mapped to (B,F)
bid_F = body_index_map[str(joint_spec.body1)]
bid_B = -1
elif joint_spec.body0 and (not joint_spec.body1):
# body1 is unspecified, and (0,1) are mapped to (F,B)
bid_F = body_index_map[str(joint_spec.body0)]
bid_B = -1
B_r_Bj, F_r_Fj = F_r_Fj, B_r_Bj
B_q_Bj, F_q_Fj = F_q_Fj, B_q_Bj
else:
# Both bodies are specified, and (0,1) are mapped to (B,F)
bid_B = body_index_map[str(joint_spec.body0)]
bid_F = body_index_map[str(joint_spec.body1)]
msg.debug(f"bid_B: {bid_B}")
msg.debug(f"bid_F: {bid_F}")
# Skip constructing this joint if both body indices are -1
# (i.e. indicating they are part of the world)
if bid_B == -1 and bid_F == -1:
return None
###
# PhysicsJoint Specific Properties
###
X_j = I_3
dof_type = None
act_type = None
q_j_min = None
q_j_max = None
tau_j_max = None
a_j = None
b_j = None
k_p_j = None
k_d_j = None
if joint_type == self.UsdPhysics.ObjectType.FixedJoint:
dof_type = JointDoFType.FIXED
act_type = JointActuationType.PASSIVE
elif joint_type == self.UsdPhysics.ObjectType.RevoluteJoint:
dof_type, act_type, X_j, q_j_min, q_j_max, tau_j_max, a_j, b_j, k_p_j, k_d_j = self._parse_joint_revolute(
joint_spec,
rotation_unit=rotation_unit,
load_drive_dynamics=load_drive_dynamics,
use_angular_drive_scaling=use_angular_drive_scaling,
)
elif joint_type == self.UsdPhysics.ObjectType.PrismaticJoint:
dof_type, act_type, X_j, q_j_min, q_j_max, tau_j_max, a_j, b_j, k_p_j, k_d_j = self._parse_joint_prismatic(
joint_spec, distance_unit=distance_unit, load_drive_dynamics=load_drive_dynamics
)
elif joint_type == self.UsdPhysics.ObjectType.SphericalJoint:
dof_type = JointDoFType.SPHERICAL
act_type = JointActuationType.PASSIVE
X_j = self.usd_axis_to_axis[joint_spec.axis].to_mat33()
elif joint_type == self.UsdPhysics.ObjectType.DistanceJoint:
raise NotImplementedError("Distance joints are not yet supported.")
elif joint_type == self.UsdPhysics.ObjectType.D6Joint:
# First check if the joint contains a DoF type hint in the custom data
# NOTE: The hint allows us to skip the extensive D6 joint parsing
custom_dof_type = self._get_joint_dof_hint(joint_prim)
if custom_dof_type:
if custom_dof_type == JointDoFType.CYLINDRICAL:
dof_type, act_type, q_j_min, q_j_max, tau_j_max = self._parse_joint_cylindrical_from_d6(
name, joint_prim, joint_spec, distance_unit, rotation_unit
)
elif custom_dof_type == JointDoFType.UNIVERSAL:
dof_type, act_type, q_j_min, q_j_max, tau_j_max = self._parse_joint_universal_from_d6(
name, joint_prim, joint_spec, rotation_unit
)
elif custom_dof_type == JointDoFType.CARTESIAN:
dof_type, act_type, q_j_min, q_j_max, tau_j_max = self._parse_joint_cartesian_from_d6(
name, joint_prim, joint_spec, distance_unit
)
else:
raise ValueError(
f"Unsupported custom DoF type hint '{custom_dof_type}' for joint '{joint_prim.GetPath()}'. "
"Supported hints are: {'cylindrical', 'universal', 'cartesian'}."
)
# If no custom DoF type hint is provided, we parse the D6 joint limits and drives
else:
# Parse the joint limits to determine the DoF type
dofs = []
cts = []
for limit in joint_spec.jointLimits:
upper = limit.second.upper
lower = limit.second.lower
axis_is_free = lower < upper
axis = limit.first
if axis_is_free:
dofs.append(axis)
else:
cts.append(axis)
# Attempt to detect the type of the joint based on the limits
if len(dofs) == 0:
dof_type = JointDoFType.FIXED
act_type = JointActuationType.PASSIVE
elif len(dofs) == 1:
if dofs[0] in self._usd_rot_axes:
dof_type, act_type, X_j, q_j_min, q_j_max, tau_j_max = self._parse_joint_revolute_from_d6(
name, joint_prim, joint_spec, dofs[0], rotation_unit
)
if dofs[0] in self._usd_trans_axes:
dof_type, act_type, X_j, q_j_min, q_j_max, tau_j_max = self._parse_joint_prismatic_from_d6(
name, joint_prim, joint_spec, dofs[0], distance_unit
)
elif len(dofs) == 2:
if all(dof in self._usd_rot_axes for dof in dofs):
dof_type, act_type, q_j_min, q_j_max, tau_j_max = self._parse_joint_universal_from_d6(
name, joint_prim, joint_spec, rotation_unit
)
if dofs[0] in self._usd_trans_axes and dofs[1] in self._usd_rot_axes:
dof_type, act_type, q_j_min, q_j_max, tau_j_max = self._parse_joint_cylindrical_from_d6(
name, joint_prim, joint_spec, distance_unit, rotation_unit
)
elif len(dofs) == 3:
if all(dof in self._usd_rot_axes for dof in dofs):
dof_type, act_type, q_j_min, q_j_max, tau_j_max = self._parse_joint_spherical_from_d6(
name, joint_prim, joint_spec, rotation_unit
)
elif all(dof in self._usd_trans_axes for dof in dofs):
dof_type, act_type, q_j_min, q_j_max, tau_j_max = self._parse_joint_cartesian_from_d6(
name, joint_prim, joint_spec, distance_unit
)
elif len(dofs) == 6:
dof_type, act_type, q_j_min, q_j_max, tau_j_max = self._parse_joint_free_from_d6(
name, joint_prim, joint_spec, distance_unit, rotation_unit
)
else:
raise ValueError(
f"Joint '{name}' ({joint_prim.GetPath()}) has {len(dofs)} free axes, "
"but D6 joints are only supported up to 3 DoFs. "
)
elif joint_type == self.UsdPhysics.ObjectType.CustomJoint:
raise NotImplementedError("Custom joints are not yet supported.")
else:
raise ValueError(
f"Unsupported joint type: {joint_type}. Supported types are: {self.supported_usd_joint_types}."
)
msg.debug(f"dof_type: {dof_type}")
msg.debug(f"act_type: {act_type}")
msg.debug(f"X_j:\n{X_j}")
msg.debug(f"q_j_min: {q_j_min}")
msg.debug(f"q_j_max: {q_j_max}")
msg.debug(f"tau_j_max: {tau_j_max}")
msg.debug(f"a_j: {a_j}")
msg.debug(f"b_j: {b_j}")
msg.debug(f"k_p_j: {k_p_j}")
msg.debug(f"k_d_j: {k_d_j}")
###
# JointDescriptor
###
# Construct and return the RigidBodyDescriptor
# with the data imported from the USD prim
return JointDescriptor(
name=name,
uid=uid,
act_type=act_type,
dof_type=dof_type,
bid_B=bid_B,
bid_F=bid_F,
B_r_Bj=B_r_Bj,
F_r_Fj=F_r_Fj,
X_j=wp.quat_to_matrix(B_q_Bj) @ X_j,
q_j_min=q_j_min,
q_j_max=q_j_max,
tau_j_max=tau_j_max,
a_j=a_j,
b_j=b_j,
k_p_j=k_p_j,
k_d_j=k_d_j,
)
def _parse_visual_geom(
self,
geom_prim,
geom_type,
body_index_map: dict[str, int],
distance_unit: float = 1.0,
prim_path_names: bool = False,
) -> GeometryDescriptor | None:
"""
Parses a UsdGeom geometry prim and returns a GeometryDescriptor.
"""
###
# Prim Identifiers
###
# Retrieve the name and UID of the geometry from the prim
path = self._get_prim_path(geom_prim)
name = self._get_prim_name(geom_prim)
uid = self._get_prim_uid(geom_prim)
msg.debug(f"[Geom]: path: {path}")
msg.debug(f"[Geom]: name: {name}")
msg.debug(f"[Geom]: uid: {uid}")
# Attempt to identify the parent rigid body of the geometry
body_prim = self._get_prim_parent_body(geom_prim)
body_name = None
if body_prim is not None:
msg.debug(f"[Geom]: Found parent rigid body prim: {body_prim.GetPath()}")
body_index = body_index_map.get(str(body_prim.GetPath()), -1)
body_name = self._get_prim_name(body_prim)
else:
msg.debug("[Geom]: No parent rigid body prim found.")
body_index = -1
body_name = "world"
msg.debug(f"[Geom]: body_index: {body_index}")
msg.debug(f"[Geom]: body_name: {body_name}")
# Attempt to get the layer the geometry belongs to
layer = self._get_prim_layer(geom_prim)
layer = layer if layer is not None else ("primary" if body_index > -1 else "world")
msg.debug(f"[Geom]: layer: {layer}")
# Use the explicit prim path as the geometry name if specified
if prim_path_names:
name = path
# Otherwise define the a condensed name based on the body and geometry layer
else:
name = f"{self._get_leaf_name(body_name)}/visual/{layer}/{name}"
msg.debug(f"[Geom]: name: {name}")
###
# PhysicsGeom Common Properties
###
i_q_ig = self._get_rotation(geom_prim)
i_r_ig = distance_unit * self._get_translation(geom_prim)
# # Extract the relative poses of the geom w.r.t the rigid body frame
msg.debug(f"[{name}]: i_q_ig: {i_q_ig}")
msg.debug(f"[{name}]: i_r_ig (before COM correction): {i_r_ig}")
# Correct for COM offset
if body_prim and body_index > -1:
i_r_com = distance_unit * self._parse_vec(
body_prim, "physics:centerOfMass", default=np.zeros(3, dtype=np.float32)
)
i_r_ig = i_r_ig - vec3f(i_r_com)
msg.debug(f"[{name}]: i_r_com: {i_r_com}")
msg.debug(f"[{name}]: i_r_ig (after COM correction): {i_r_ig}")
# Construct the transform descriptor
i_T_ig = transformf(i_r_ig, i_q_ig)
msg.debug(f"[{name}]: i_T_ig: {i_T_ig}")
###
# PhysicsGeom Shape Properties
###
# Retrieve the geom scale
scale = self._get_scale(geom_prim)
msg.debug(f"[{name}]: scale: {scale}")
# Construct the shape descriptor based on the geometry type
shape = None
if geom_type == self.UsdGeom.Capsule:
capsule = self.UsdGeom.Capsule(geom_prim)
height = distance_unit * capsule.GetHeightAttr().Get()
radius = distance_unit * capsule.GetRadiusAttr().Get()
axis = Axis.from_string(capsule.GetAxisAttr().Get())
i_q_ig = self._align_geom_to_axis(axis, i_q_ig)
i_T_ig = transformf(i_r_ig, i_q_ig)
shape = CapsuleShape(radius=radius, half_height=0.5 * height)
elif geom_type == self.UsdGeom.Capsule_1:
raise NotImplementedError("Capsule1 UsdGeom is not yet supported.")
elif geom_type == self.UsdGeom.Cone:
cone = self.UsdGeom.Cone(geom_prim)
height = distance_unit * cone.GetHeightAttr().Get()
radius = distance_unit * cone.GetRadiusAttr().Get()
axis = Axis.from_string(cone.GetAxisAttr().Get())
i_q_ig = self._align_geom_to_axis(axis, i_q_ig)
i_T_ig = transformf(i_r_ig, i_q_ig)
shape = ConeShape(radius=radius, half_height=0.5 * height)
elif geom_type == self.UsdGeom.Cube:
d, w, h = 2.0 * distance_unit * scale
shape = BoxShape(hx=0.5 * d, hy=0.5 * w, hz=0.5 * h)
elif geom_type == self.UsdGeom.Cylinder:
cylinder = self.UsdGeom.Cylinder(geom_prim)
height = distance_unit * cylinder.GetHeightAttr().Get()
radius = distance_unit * cylinder.GetRadiusAttr().Get()
axis = Axis.from_string(cylinder.GetAxisAttr().Get())
i_q_ig = self._align_geom_to_axis(axis, i_q_ig)
i_T_ig = transformf(i_r_ig, i_q_ig)
shape = CylinderShape(radius=radius, half_height=0.5 * height)
elif geom_type == self.UsdGeom.Cylinder_1:
raise NotImplementedError("Cylinder1 UsdGeom is not yet supported.")
elif geom_type == self.UsdGeom.Plane:
plane = self.UsdGeom.Plane(geom_prim)
axis = Axis.from_string(plane.GetAxisAttr().Get())
shape = PlaneShape(normal=axis.to_vec3(), distance=0.0)
elif geom_type == self.UsdGeom.Sphere:
sphere = self.UsdGeom.Sphere(geom_prim)
radius = distance_unit * sphere.GetRadiusAttr().Get()
scale = np.array(scale, dtype=np.float32)
if np.all(scale[0:] == scale[0]):
shape = SphereShape(radius=radius)
else:
a, b, c = distance_unit * scale * radius
shape = EllipsoidShape(a=a, b=b, c=c)
elif geom_type == self.UsdGeom.Mesh:
# Retrieve the mesh data from the USD mesh prim
usd_mesh = self.UsdGeom.Mesh(geom_prim)
usd_mesh_path = usd_mesh.GetPath()
# Extract mandatory mesh attributes
points = np.array(usd_mesh.GetPointsAttr().Get(), dtype=np.float32)
indices = np.array(usd_mesh.GetFaceVertexIndicesAttr().Get(), dtype=np.float32)
counts = usd_mesh.GetFaceVertexCountsAttr().Get()
# Extract optional normals attribute if defined
normals = (
np.array(usd_mesh.GetNormalsAttr().Get(), dtype=np.float32)
if usd_mesh.GetNormalsAttr().IsDefined()
else None
)
# Extract triangle face indices from the mesh data
# NOTE: This handles both triangle and quad meshes
faces = self._make_faces_from_counts(indices, counts, usd_mesh_path)
# Create the mesh shape (i.e. wrapper around newton.geometry.Mesh)
shape = MeshShape(vertices=points, indices=faces, normals=normals)
else:
raise ValueError(
f"Unsupported UsdGeom type: {geom_type}. Supported types: {self.supported_usd_geom_types}."
)
msg.debug(f"[{name}]: shape: {shape}")
###
# GeometryDescriptor
###
# Construct and return the GeometryDescriptor
# with the data imported from the USD prim
return GeometryDescriptor(
name=name,
uid=uid,
body=body_index,
offset=i_T_ig,
shape=shape,
group=0,
collides=0,
flags=ShapeFlags.VISIBLE,
)
def _parse_physics_geom(
self,
stage,
geom_prim,
geom_type,
geom_spec,
body_index_map: dict[str, int],
cgroup_index_map: dict[str, int],
material_index_map: dict[str, int],
distance_unit: float = 1.0,
meshes_are_collidable: bool = False,
force_show_colliders: bool = False,
prim_path_names: bool = False,
) -> GeometryDescriptor | None:
"""
Parses a UsdPhysics geometry prim and returns a GeometryDescriptor.
"""
###
# Prim Identifiers
###
# Retrieve the name and UID of the geometry from the prim
path = self._get_prim_path(geom_prim)
name = self._get_prim_name(geom_prim)
uid = self._get_prim_uid(geom_prim)
msg.debug(f"[Geom]: path: {path}")
msg.debug(f"[Geom]: name: {name}")
msg.debug(f"[Geom]: uid: {uid}")
# Retrieve the name and index of the rigid body associated with the geom
# NOTE: If a rigid body is not associated with the geom, the body index (bid) is
# set to `-1` indicating that the geom belongs to the world, i.e. it is a static
body_index = body_index_map.get(str(geom_spec.rigidBody), -1)
body_name = str(geom_spec.rigidBody) if body_index > -1 else "world"
msg.debug(f"[Geom]: body_name: {body_name}")
msg.debug(f"[Geom]: body_index: {body_index}")
# Attempt to get the layer the geometry belongs to
layer = self._get_prim_layer(geom_prim)
layer = layer if layer is not None else ("primary" if body_index > -1 else "world")
msg.debug(f"[Geom]: layer: {layer}")
# Use the explicit prim path as the geometry name if specified
if prim_path_names:
name = path
# Otherwise define the a condensed name based on the body and geometry layer
else:
name = f"{self._get_leaf_name(body_name)}/physics/{layer}/{name}"
msg.debug(f"[Geom]: name: {name}")
###
# PhysicsGeom Common Properties
###
# Extract the relative poses of the geom w.r.t the rigid body frame
i_r_ig = distance_unit * vec3f(geom_spec.localPos)
i_q_ig = self._from_gfquat(geom_spec.localRot)
msg.debug(f"[{name}]: i_r_ig (before COM correction): {i_r_ig}")
msg.debug(f"[{name}]: i_q_ig: {i_q_ig}")
# Correct for COM offset
if geom_spec.rigidBody and body_index > -1:
body_prim = stage.GetPrimAtPath(geom_spec.rigidBody)
i_r_com = distance_unit * self._parse_vec(
body_prim, "physics:centerOfMass", default=np.zeros(3, dtype=np.float32)
)
i_r_ig = i_r_ig - vec3f(i_r_com)
msg.debug(f"[{name}]: i_r_com: {i_r_com}")
msg.debug(f"[{name}]: i_r_ig (after COM correction): {i_r_ig}")
# Construct the transform descriptor
i_T_ig = transformf(i_r_ig, i_q_ig)
###
# PhysicsGeom Shape Properties
###
# Retrieve the geom scale
# TODO: materials = geom_spec.materials
scale = np.array(geom_spec.localScale)
msg.debug(f"[{name}]: scale: {scale}")
# Construct the shape descriptor based on the geometry type
shape = None
is_mesh_shape = False
if geom_type == self.UsdPhysics.ObjectType.CapsuleShape:
# TODO: axis = geom_spec.axis, how can we use this?
shape = CapsuleShape(
radius=distance_unit * geom_spec.radius,
half_height=distance_unit * geom_spec.halfHeight,
)
elif geom_type == self.UsdPhysics.ObjectType.Capsule1Shape:
raise NotImplementedError("Capsule1Shape is not yet supported.")
elif geom_type == self.UsdPhysics.ObjectType.ConeShape:
# TODO: axis = geom_spec.axis, how can we use this?
shape = ConeShape(
radius=distance_unit * geom_spec.radius,
half_height=distance_unit * geom_spec.halfHeight,
)
elif geom_type == self.UsdPhysics.ObjectType.CubeShape:
he = distance_unit * vec3f(geom_spec.halfExtents)
shape = BoxShape(hx=he[0], hy=he[1], hz=he[2])
elif geom_type == self.UsdPhysics.ObjectType.CylinderShape:
# TODO: axis = geom_spec.axis, how can we use this?
shape = CylinderShape(
radius=distance_unit * geom_spec.radius,
half_height=distance_unit * geom_spec.halfHeight,
)
elif geom_type == self.UsdPhysics.ObjectType.Cylinder1Shape:
raise NotImplementedError("Cylinder1Shape is not yet supported.")
elif geom_type == self.UsdPhysics.ObjectType.PlaneShape:
# TODO: get distance from geom position
shape = PlaneShape(normal=self.usd_axis_to_axis[geom_spec.axis].to_vec3f(), distance=0.0)
elif geom_type == self.UsdPhysics.ObjectType.SphereShape:
if np.all(scale[0:] == scale[0]):
shape = SphereShape(radius=distance_unit * geom_spec.radius)
else:
a, b, c = distance_unit * scale
shape = EllipsoidShape(a=a, b=b, c=c)
elif geom_type == self.UsdPhysics.ObjectType.MeshShape:
# Retrieve the mesh data from the USD mesh prim
usd_mesh = self.UsdGeom.Mesh(geom_prim)
usd_mesh_path = usd_mesh.GetPath()
# Extract mandatory mesh attributes
points = np.array(usd_mesh.GetPointsAttr().Get(), dtype=np.float32)
indices = np.array(usd_mesh.GetFaceVertexIndicesAttr().Get(), dtype=np.float32)
counts = usd_mesh.GetFaceVertexCountsAttr().Get()
# Extract optional normals attribute if defined
normals = (
np.array(usd_mesh.GetNormalsAttr().Get(), dtype=np.float32)
if usd_mesh.GetNormalsAttr().IsDefined()
else None
)
# Extract triangle face indices from the mesh data
# NOTE: This handles both triangle and quad meshes
faces = self._make_faces_from_counts(indices, counts, usd_mesh_path)
# Create the mesh shape (i.e. wrapper around newton.geometry.Mesh)
shape = MeshShape(vertices=points, indices=faces, normals=normals)
is_mesh_shape = True
else:
raise ValueError(
f"Unsupported UsdPhysics shape type: {geom_type}. "
f"Supported types: {self.supported_usd_physics_shape_types}."
)
msg.debug(f"[{name}]: shape: {shape}")
###
# Collision Properties
###
# Promote the GeometryDescriptor to a CollisionGeometryDescriptor if it's collidable
geom_max_contacts: int = 0
geom_group: int = 0
geom_collides: int = 0
geom_material: str | None = None
geom_material_index: int = 0
geom_flags = 0
if geom_spec.collisionEnabled and ((meshes_are_collidable and is_mesh_shape) or not is_mesh_shape):
# Query the geom prim for the maximum number of contacts hint
geom_max_contacts = self._get_geom_max_contacts(geom_prim)
# Enable collision for this geom by setting the appropriate flags
geom_flags = geom_flags | ShapeFlags.COLLIDE_SHAPES | ShapeFlags.COLLIDE_PARTICLES
# Assign a material if specified
# NOTE: Only the first material is considered for now
materials = geom_spec.materials
if len(materials) > 0:
geom_material = str(materials[0])
geom_material_index = material_index_map.get(str(materials[0]), 0)
msg.debug(f"[{name}]: material_index_map: {material_index_map}")
msg.debug(f"[{name}]: materials: {materials}")
msg.debug(f"[{name}]: geom_material: {geom_material}")
msg.debug(f"[{name}]: geom_material_index: {geom_material_index}")
# Assign collision group/filters if specified
collision_group_paths = geom_spec.collisionGroups
filtered_collisions_paths = geom_spec.filteredCollisions
msg.debug(f"[{name}]: collision_group_paths: {collision_group_paths}")
msg.debug(f"[{name}]: filtered_collisions_paths: {filtered_collisions_paths}")
collision_groups = []
for collision_group_path in collision_group_paths:
collision_groups.append(cgroup_index_map.get(str(collision_group_path), 0))
geom_group = min(collision_groups) if len(collision_groups) > 0 else 1
msg.debug(f"[{name}]: collision_groups: {collision_groups}")
msg.debug(f"[{name}]: geom_group: {geom_group}")
geom_collides = geom_group
for cgroup in collision_groups:
if cgroup != geom_group:
geom_collides += cgroup
msg.debug(f"[{name}]: geom_collides: {geom_collides}")
# Set the geom to be visible if it is a non-collidable mesh and we are forcing show colliders
if force_show_colliders:
geom_flags = geom_flags | ShapeFlags.VISIBLE
###
# GeometryDescriptor
###
# Construct and return the GeometryDescriptor
# with the data imported from the USD prim
return GeometryDescriptor(
name=name,
uid=uid,
body=body_index,
shape=shape,
offset=i_T_ig,
material=geom_material,
mid=geom_material_index,
group=geom_group,
collides=geom_collides,
max_contacts=geom_max_contacts,
flags=geom_flags,
)
###
# Public API
###
def import_from(
self,
source: str,
root_path: str = "/",
xform: Transform | None = None,
ignore_paths: list[str] | None = None,
builder: ModelBuilderKamino | None = None,
apply_up_axis_from_stage: bool = True,
only_load_enabled_rigid_bodies: bool = True,
only_load_enabled_joints: bool = True,
retain_joint_ordering: bool = True,
retain_geom_ordering: bool = True,
load_drive_dynamics: bool = False,
load_static_geometry: bool = True,
load_materials: bool = True,
meshes_are_collidable: bool = False,
force_show_colliders: bool = False,
use_prim_path_names: bool = False,
use_articulation_root_name: bool = True,
use_angular_drive_scaling: bool = True,
) -> ModelBuilderKamino:
"""
Parses an OpenUSD file.
"""
# Check if the source is a valid USD file path or an existing stage
if isinstance(source, str):
stage = self.Usd.Stage.Open(source, self.Usd.Stage.LoadAll)
# TODO: When does this case happen?
else:
stage = source
# Retrieve the default prim name to assign as the name of the world
if stage.HasDefaultPrim():
default_prim = stage.GetDefaultPrim()
default_prim_name = default_prim.GetName()
else:
default_prim_name = Path(source).name if isinstance(source, str) else "world"
msg.debug(f"default_prim_path: {default_prim.GetPath() if stage.HasDefaultPrim() else 'N/A'}")
msg.debug(f"default_prim_name: {default_prim_name}")
###
# Units
###
# Load the global distance, rotation and mass units from the stage
rotation_unit = np.pi / 180
distance_unit = 1.0
mass_unit = 1.0
try:
if self.UsdGeom.StageHasAuthoredMetersPerUnit(stage):
distance_unit = self.UsdGeom.GetStageMetersPerUnit(stage)
except Exception as e:
msg.error(f"Failed to get linear unit: {e}")
try:
if self.UsdPhysics.StageHasAuthoredKilogramsPerUnit(stage):
mass_unit = self.UsdPhysics.GetStageKilogramsPerUnit(stage)
except Exception as e:
msg.error(f"Failed to get mass unit: {e}")
msg.debug(f"distance_unit: {distance_unit}")
msg.debug(f"rotation_unit: {rotation_unit}")
msg.debug(f"mass_unit: {mass_unit}")
###
# Preparation
###
# Initialize the ignore paths as an empty list if it is None
# NOTE: This is required by the LoadUsdPhysicsFromRange method
if ignore_paths is None:
ignore_paths = []
# Load the USD file into an object dictionary
ret_dict = self.UsdPhysics.LoadUsdPhysicsFromRange(stage, [root_path], excludePaths=ignore_paths)
# Create a new ModelBuilderKamino if not provided
if builder is None:
builder = ModelBuilderKamino(default_world=False)
builder.add_world(name=default_prim_name)
###
# World
###
# Initialize the world properties
gravity = GravityDescriptor()
# Parse for PhysicsScene prims
if self.UsdPhysics.ObjectType.Scene in ret_dict:
# Retrieve the phusics sene path and description
paths, scene_descs = ret_dict[self.UsdPhysics.ObjectType.Scene]
path, scene_desc = paths[0], scene_descs[0]
msg.debug(f"Found PhysicsScene at {path}")
if len(paths) > 1:
msg.error("Multiple PhysicsScene prims found in the USD file. Only the first prim will be considered.")
# Extract the world gravity from the physics scene
gravity.acceleration = distance_unit * scene_desc.gravityMagnitude
gravity.direction = vec3f(scene_desc.gravityDirection)
builder.set_gravity(gravity)
msg.debug(f"World gravity: {gravity}")
# Set the world up-axis based on the gravity direction
up_axis = Axis.from_any(int(np.argmax(np.abs(scene_desc.gravityDirection))))
else:
# NOTE: Gravity is left with default values
up_axis = Axis.from_string(str(self.UsdGeom.GetStageUpAxis(stage)))
# Determine the up-axis transformation
if apply_up_axis_from_stage:
builder.set_up_axis(up_axis)
axis_xform = wp.transform_identity()
msg.debug(f"Using stage up axis {up_axis} as builder up axis")
else:
axis_xform = wp.transform(vec3f(0.0), quat_between_axes(up_axis, builder.up_axes[0]))
msg.debug(f"Rotating stage to align its up axis {up_axis} with builder up axis {builder.up_axes[0]}")
# Set the world offset transform based on the provided xform
if xform is None:
world_xform = axis_xform
else:
world_xform = wp.transform(*xform) * axis_xform
msg.debug(f"World offset transform: {world_xform}")
###
# Materials
###
# Initialize an empty materials map
material_index_map = {}
# Load materials only if requested
if load_materials:
# TODO: mechanism to detect multiple default overrides
# Parse for and import UsdPhysicsRigidBodyMaterialDesc entries
if self.UsdPhysics.ObjectType.RigidBodyMaterial in ret_dict:
prim_paths, rigid_body_material_specs = ret_dict[self.UsdPhysics.ObjectType.RigidBodyMaterial]
for prim_path, material_spec in zip(prim_paths, rigid_body_material_specs, strict=False):
msg.debug(f"Parsing material @'{prim_path}': {material_spec}")
material_desc = self._parse_material(
material_prim=stage.GetPrimAtPath(prim_path),
distance_unit=distance_unit,
mass_unit=mass_unit,
)
if material_desc is not None:
has_default_override = self._get_material_default_override(stage.GetPrimAtPath(prim_path))
if has_default_override:
msg.debug(f"Overriding default material with:\n{material_desc}\n")
builder.set_default_material(material=material_desc)
material_index_map[str(prim_path)] = 0
else:
msg.debug(f"Adding material '{builder.num_materials}':\n{material_desc}\n")
material_index_map[str(prim_path)] = builder.add_material(material=material_desc)
msg.debug(f"material_index_map: {material_index_map}")
# Generate material pair properties for each combination
# NOTE: This applies the OpenUSD convention of using the average of the two properties
for i, first in enumerate(builder.materials):
for j, second in enumerate(builder.materials):
if i <= j: # Avoid duplicate pairs
msg.debug(f"Generating material pair properties for '{first.name}' and '{second.name}'")
material_pair = self._material_pair_properties_from(first, second)
msg.debug(f"material_pair: {material_pair}")
builder.set_material_pair(
first=first.name,
second=second.name,
material_pair=material_pair,
)
###
# Collision Groups
###
# Parse for and import UsdPhysicsCollisionGroup prims
cgroup_count = 0
cgroup_index_map = {}
if self.UsdPhysics.ObjectType.CollisionGroup in ret_dict:
prim_paths, collision_group_specs = ret_dict[self.UsdPhysics.ObjectType.CollisionGroup]
for prim_path, collision_group_spec in zip(prim_paths, collision_group_specs, strict=False):
msg.debug(f"Parsing collision group @'{prim_path}': {collision_group_spec}")
cgroup_index_map[str(prim_path)] = cgroup_count + 1
cgroup_count += 1
msg.debug(f"cgroup_count: {cgroup_count}")
msg.debug(f"cgroup_index_map: {cgroup_index_map}")
###
# Articulations
###
# Construct a list of articulation root bodies to create FREE
# joints if not already defined explicitly in the USD file
articulation_root_body_paths = []
if self.UsdPhysics.ObjectType.Articulation in ret_dict:
paths, articulation_descs = ret_dict[self.UsdPhysics.ObjectType.Articulation]
for path, _desc in zip(paths, articulation_descs, strict=False):
articulation_prim = stage.GetPrimAtPath(path)
try:
parent_prim = articulation_prim.GetParent()
except Exception:
parent_prim = None
if articulation_prim.HasAPI(self.UsdPhysics.ArticulationRootAPI):
if self._prim_is_rigid_body(articulation_prim):
msg.debug(f"Adding articulation_prim at '{path}' as articulation root body")
articulation_root_body_paths.append(articulation_prim.GetPath())
elif (
parent_prim is not None
and parent_prim.IsValid()
and parent_prim.HasAPI(self.UsdPhysics.ArticulationRootAPI)
):
if self._prim_is_rigid_body(parent_prim):
msg.debug(f"Adding parent_prim at '{parent_prim.GetPath()}' as articulation root body")
articulation_root_body_paths.append(parent_prim.GetPath())
msg.debug(f"articulation_root_body_paths: {articulation_root_body_paths}")
###
# Bodies
###
# Define a mapping from prim paths to body indices
# NOTE: This can be used for both rigid and flexible bodies
body_index_map = {}
body_path_map = {}
# Parse for and import UsdPhysicsRigidBody prims
if self.UsdPhysics.ObjectType.RigidBody in ret_dict:
prim_paths, rigid_body_specs = ret_dict[self.UsdPhysics.ObjectType.RigidBody]
for prim_path, rigid_body_spec in zip(prim_paths, rigid_body_specs, strict=False):
msg.debug(f"Parsing rigid body @'{prim_path}'")
rigid_body_desc = self._parse_rigid_body(
only_load_enabled_rigid_bodies=only_load_enabled_rigid_bodies,
rigid_body_prim=stage.GetPrimAtPath(prim_path),
rigid_body_spec=rigid_body_spec,
offset_xform=world_xform,
distance_unit=distance_unit,
rotation_unit=rotation_unit,
mass_unit=mass_unit,
prim_path_names=use_prim_path_names,
)
if rigid_body_desc is not None:
msg.debug(f"Adding body '{builder.num_bodies}':\n{rigid_body_desc}\n")
body_index = builder.add_rigid_body_descriptor(body=rigid_body_desc)
body_index_map[str(prim_path)] = body_index
body_path_map[body_index] = str(prim_path)
else:
msg.debug(f"Rigid body @'{prim_path}' not loaded. Will be treated as static geometry.")
body_index_map[str(prim_path)] = -1 # Body not loaded, is statically part of the world
msg.debug(f"body_index_map: {body_index_map}")
msg.debug(f"body_path_map: {body_path_map}")
###
# Joints
###
# Define a list to hold all joint descriptors to be added to the builder after sorting
joint_descriptors: list[JointDescriptor] = []
articulation_root_joints: list[JointDescriptor] = []
# If retaining joint ordering, first construct lists of joint prim paths and their
# types that retain the order of the joints as specified in the USD file, then iterate
# over each pair of prim path and joint type-name to parse the joint specifications
if retain_joint_ordering:
# First construct lists of joint prim paths and their types that
# retain the order of the joints as specified in the USD file.
joint_prim_paths = []
joint_type_names = []
for prim in stage.Traverse():
if prim.GetTypeName() in self.supported_usd_joint_type_names:
joint_type_names.append(prim.GetTypeName())
joint_prim_paths.append(prim.GetPath())
msg.debug(f"joint_prim_paths: {joint_prim_paths}")
msg.debug(f"joint_type_names: {joint_type_names}")
# Then iterate over each pair of prim path and joint type-name to parse the joint specifications
for joint_prim_path, joint_type_name in zip(joint_prim_paths, joint_type_names, strict=False):
joint_type = self.supported_usd_joint_types[self.supported_usd_joint_type_names.index(joint_type_name)]
joint_paths, joint_specs = ret_dict[joint_type]
for prim_path, joint_spec in zip(joint_paths, joint_specs, strict=False):
if prim_path == joint_prim_path:
msg.debug(f"Parsing joint @'{prim_path}' of type '{joint_type_name}'")
joint_desc = self._parse_joint(
stage=stage,
only_load_enabled_joints=only_load_enabled_joints,
joint_prim=stage.GetPrimAtPath(prim_path),
joint_spec=joint_spec,
joint_type=joint_type,
body_index_map=body_index_map,
distance_unit=distance_unit,
rotation_unit=rotation_unit,
load_drive_dynamics=load_drive_dynamics,
prim_path_names=use_prim_path_names,
use_angular_drive_scaling=use_angular_drive_scaling,
)
if joint_desc is not None:
msg.debug(f"Adding joint '{builder.num_joints}':\n{joint_desc}\n")
joint_descriptors.append(joint_desc)
# Check if the joint's Follower body is the articulation root
if body_path_map[joint_desc.bid_F] in articulation_root_body_paths:
articulation_root_joints.append(joint_desc)
else:
msg.debug(f"Joint @'{prim_path}' not loaded. Will be ignored.")
break # Stop after the first match
# If not retaining joint ordering, simply iterate over the joint types in any order and parse the joints
# NOTE: This has been added only to be able to reproduce the behavior of the newton.ModelBuilder
# TODO: Once the newton.ModelBuilder is updated to retain USD joint ordering, this branch can be removed
else:
# Collect joint specifications grouped by their USD-native joint type
joint_specifications = {}
for key, value in ret_dict.items():
if key in self.supported_usd_joint_types:
paths, joint_specs = value
for path, joint_spec in zip(paths, joint_specs, strict=False):
joint_specifications[str(path)] = (joint_spec, key)
# Then iterate over each pair of prim path and joint type-name to parse the joint specifications
for prim_path, (joint_spec, joint_type_name) in joint_specifications.items():
joint_type = self.supported_usd_joint_types[self.supported_usd_joint_types.index(joint_type_name)]
msg.debug(f"Parsing joint @'{prim_path}' of type '{joint_type_name}'")
joint_desc = self._parse_joint(
stage=stage,
only_load_enabled_joints=only_load_enabled_joints,
joint_prim=stage.GetPrimAtPath(prim_path),
joint_spec=joint_spec,
joint_type=joint_type,
body_index_map=body_index_map,
distance_unit=distance_unit,
rotation_unit=rotation_unit,
load_drive_dynamics=load_drive_dynamics,
prim_path_names=use_prim_path_names,
use_angular_drive_scaling=use_angular_drive_scaling,
)
if joint_desc is not None:
msg.debug(f"Adding joint '{builder.num_joints}':\n{joint_desc}\n")
joint_descriptors.append(joint_desc)
# Check if the joint's Follower body is the articulation root
if body_path_map[joint_desc.bid_F] in articulation_root_body_paths:
articulation_root_joints.append(joint_desc)
else:
msg.debug(f"Joint @'{prim_path}' not loaded. Will be ignored.")
# For each articulation root body that does not have an explicit joint
# defined in the USD file, add a FREE joint to attach it to the world
if len(articulation_root_joints) != len(articulation_root_body_paths):
for root_body_path in articulation_root_body_paths:
# Check if the root body already has a joint defined in the USD file
root_body_index = int(body_index_map[str(root_body_path)])
has_joint = False
for joint_desc in articulation_root_joints:
if joint_desc.has_follower_body(root_body_index):
has_joint = True
break
if has_joint:
msg.debug(
f"Articulation root body '{root_body_path}' already has a joint defined. Skipping FREE joint."
)
continue
# If not, create a FREE joint descriptor to attach the root body to the
# world and insert it at the beginning of the joint descriptors list
root_body_name = builder.bodies[root_body_index].name
joint_desc = JointDescriptor(
name=f"world_to_{root_body_name}"
if use_articulation_root_name
else f"joint_{builder.num_joints + 1}",
dof_type=JointDoFType.FREE,
act_type=JointActuationType.PASSIVE,
bid_B=-1,
bid_F=root_body_index,
X_j=Axis.X.to_mat33(),
)
msg.debug(
f"Adding FREE joint '{joint_desc.name}' to attach articulation "
f"root body '{root_body_path}' to the world:\n{joint_desc}\n"
)
joint_descriptors.insert(0, joint_desc)
# If an articulation is present, sort joint indices according
# to DFS to produce a minimum-depth kinematic tree ordering
if len(articulation_root_body_paths) > 0 and len(joint_descriptors) > 0:
# Create a list of body-pair indices (B, F) for each joint
joint_body_pairs = [(joint_desc.bid_B, joint_desc.bid_F) for joint_desc in joint_descriptors]
# Perform a topological sort of the joints based on their body-pair indices
joint_indices, reversed_joints = topological_sort_undirected(joints=joint_body_pairs, use_dfs=True)
# Reverse the order of the joints that were reversed during the topological
# sort to maintain the original joint directionality as much as possible
for i in reversed_joints:
joint_desc = joint_descriptors[i]
joint_desc.bid_B, joint_desc.bid_F = joint_desc.bid_F, joint_desc.bid_B
joint_desc.B_r_Bj, joint_desc.F_r_Fj = joint_desc.F_r_Fj, joint_desc.B_r_Bj
# Reorder the joint descriptors based on the topological sort
joint_descriptors = [joint_descriptors[i] for i in joint_indices]
# Add all descriptors to the builder
for joint_desc in joint_descriptors:
builder.add_joint_descriptor(joint=joint_desc)
###
# Geometry
###
# Define a list to hold all geometry prims found in the
# stage, including those nested within instances
path_geom_prim_map = {}
visual_geom_prims = []
collision_geom_prims = []
# Define a function to check if a given prim has an enabled collider
def _is_enabled_collider(prim) -> bool:
if collider := self.UsdPhysics.CollisionAPI(prim):
return collider.GetCollisionEnabledAttr().Get()
return False
# Define a recursive function to traverse the stage and collect
# all UsdGeom prims, including those nested within instances
def _collect_geom_prims(prim, colliders=True, visuals=True):
if prim.IsA(self.UsdGeom.Gprim):
msg.debug(f"Found UsdGeom prim: {prim.GetPath()}, type: {prim.GetTypeName()}")
path = str(prim.GetPath())
if path not in path_geom_prim_map:
is_collider = _is_enabled_collider(prim)
if is_collider and colliders:
collision_geom_prims.append(prim)
path_geom_prim_map[path] = prim
if not is_collider and visuals:
visual_geom_prims.append(prim)
path_geom_prim_map[path] = prim
elif prim.IsInstance():
proto = prim.GetPrototype()
for child in proto.GetChildren():
inst_child = stage.GetPrimAtPath(child.GetPath().ReplacePrefix(proto.GetPath(), prim.GetPath()))
_collect_geom_prims(inst_child, colliders=colliders, visuals=visuals)
# If enabled, traverse the stage to collect geometry
# prims in the order they are defined in the USD file
if retain_geom_ordering:
# Traverse the stage to collect geometry prims
for prim in stage.Traverse():
_collect_geom_prims(prim=prim)
# Otherwise, simply retrieve the geometry prim paths and descriptors from the physics shape
else:
# First traverse only visuals
for prim in stage.Traverse():
_collect_geom_prims(prim=prim, colliders=False)
# Then iterate through physics-only shapes
for key, value in ret_dict.items():
if key in self.supported_usd_physics_shape_types:
paths, shape_specs = value
for xpath, shape_spec in zip(paths, shape_specs, strict=False):
if self._warn_invalid_desc(xpath, shape_spec):
continue
_collect_geom_prims(prim=stage.GetPrimAtPath(str(xpath)), visuals=False)
# Define separate lists to hold geometry descriptors for visual and physics geometry
visual_geoms: list[GeometryDescriptor] = []
physics_geoms: list[GeometryDescriptor] = []
# Define a function to process each geometry prim and construct geometry descriptors based on whether
# they are marked for physics simulation or not. The geometry descriptors are then added to the
# corresponding list to be added to the builder at the end of the process.
def _process_geom_prim(prim):
# Extract UsdGeom prim information
geom_prim_path = prim.GetPath()
typename = prim.GetTypeName()
schemas = prim.GetAppliedSchemas()
has_physics_schemas = "PhysicsCollisionAPI" in schemas or "PhysicsMeshCollisionAPI" in schemas
msg.debug(f"Geom prim: {geom_prim_path}, typename: {typename}, has_physics: {has_physics_schemas}")
# Parse the geometry based on whether it is a UsdPhysics shape or a standard UsdGeom
# In either case, check that the geometry type is supported and retrieve the
# corresponding type to then parse the UsdGeom and constrict a geometry descriptor
geom_type = None
geom_desc = None
if has_physics_schemas:
if typename in self.supported_usd_physics_shape_type_names:
geom_type_index = self.supported_usd_physics_shape_type_names.index(typename)
geom_type = self.supported_usd_physics_shape_types[geom_type_index]
msg.debug(f"Processing UsdPhysics shape prim '{geom_prim_path}' of type '{typename}'")
# Check that the geometry type exists in the UsdPhysics descriptors dictionary
if geom_type in ret_dict:
# Extract the list of physics prim paths and descriptors for the given type
geom_paths, geom_specs = ret_dict[geom_type]
msg.debug(f"Found {len(geom_paths)} geometry descriptors of type '{typename}'")
else:
msg.critical(f"No UsdPhysics shape descriptors found that match prim type '{typename}'")
return
# Iterate over physics geom descriptors until a match to the target geom prims is found
for geom_path, geom_spec in zip(geom_paths, geom_specs, strict=False):
if geom_path == geom_prim_path:
# Parse the UsdPhysics geom descriptor to construct a corresponding sim geometry descriptor
msg.debug(f"Parsing UsdPhysics shape @'{geom_path}' of type '{typename}'")
geom_desc = self._parse_physics_geom(
stage=stage,
geom_prim=prim,
geom_spec=geom_spec,
geom_type=geom_type,
body_index_map=body_index_map,
cgroup_index_map=cgroup_index_map,
material_index_map=material_index_map,
distance_unit=distance_unit,
meshes_are_collidable=meshes_are_collidable,
force_show_colliders=force_show_colliders,
prim_path_names=use_prim_path_names,
)
break # Stop after the first match
else:
msg.warning(f"Skipping unsupported physics geom prim: {geom_prim_path} of type {typename}")
return
else:
if typename in self.supported_usd_geom_type_names:
geom_type_index = self.supported_usd_geom_type_names.index(typename)
geom_type = self.supported_usd_geom_types[geom_type_index]
msg.debug(f"Parsing UsdGeom @'{geom_prim_path}' of type '{typename}'")
geom_desc = self._parse_visual_geom(
geom_prim=prim,
geom_type=geom_type,
body_index_map=body_index_map,
distance_unit=distance_unit,
prim_path_names=use_prim_path_names,
)
else:
msg.warning(f"Skipping unsupported geom prim: {geom_prim_path} of type {typename}")
return
# If construction succeeded, append it to the model builder
if geom_desc is not None:
# Skip static geometry if not requested
if geom_desc.body == -1 and not load_static_geometry:
return
# Append geometry descriptor to appropriate entity
if type(geom_desc) is GeometryDescriptor:
if has_physics_schemas:
msg.debug("Adding physics geom '%d':\n%s\n", builder.num_geoms, geom_desc)
physics_geoms.append(geom_desc)
else:
msg.debug("Adding visual geom '%d':\n%s\n", builder.num_geoms, geom_desc)
visual_geoms.append(geom_desc)
# Indicate to user that a UsdGeom has potentially not been marked for physics simulation
else:
msg.critical("Failed to parse geom prim '%s'", geom_prim_path)
# Process each geometry prim to construct geometry descriptors
for geom_prim in visual_geom_prims + collision_geom_prims:
_process_geom_prim(geom_prim)
# Add all geoms grouped by whether they belong to the physics scene or not
for geom_desc in visual_geoms + physics_geoms:
builder.add_geometry_descriptor(geom=geom_desc)
###
# Summary
###
msg.debug("Builder: Rigid Bodies:\n%s\n", builder.bodies)
msg.debug("Builder: Joints:\n%s\n", builder.joints)
msg.debug("Builder: Geoms:\n%s\n", builder.geoms)
msg.debug("Builder: Materials:\n%s\n", builder.materials)
# Return the ModelBuilderKamino populated from the parsed USD file
return builder
================================================
FILE: newton/_src/solvers/kamino/_src/utils/logger.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Utilities: Message Logging
"""
import logging
from enum import IntEnum
from typing import ClassVar
class LogLevel(IntEnum):
"""Enumeration for log levels."""
DEBUG = logging.DEBUG
INFO = logging.INFO
NOTIF = logging.INFO + 5
WARNING = logging.WARNING
ERROR = logging.ERROR
CRITICAL = logging.CRITICAL
class Logger(logging.Formatter):
"""Base logger with color highlighting for log levels."""
HEADER = "[KAMINO]"
HEADERCOL = "\x1b[38;5;13m"
WHITE = "\x1b[37m"
RED = "\x1b[31;20m"
BOLD_RED = "\x1b[31;1m"
BLUE = "\x1b[34;20m"
BOLD_BLUE = "\x1b[34;1m"
GREEN = "\x1b[32;20m"
BOLD_GREEN = "\x1b[32;1m"
YELLOW = "\x1b[33;20m"
BOLD_YELLOW = "\x1b[33;1m"
RESET = "\x1b[0m"
LINE_FORMAT = "[%(asctime)s][%(filename)s:%(lineno)d][%(levelname)s]: %(message)s"
"""Line format for the log messages, including timestamp, filename, line number, log level, and message."""
FORMATS: ClassVar[dict[int, str]] = {
LogLevel.DEBUG: HEADERCOL + HEADER + RESET + BLUE + LINE_FORMAT + RESET,
LogLevel.INFO: HEADERCOL + HEADER + RESET + WHITE + LINE_FORMAT + RESET,
LogLevel.NOTIF: HEADERCOL + HEADER + RESET + GREEN + LINE_FORMAT + RESET,
LogLevel.WARNING: HEADERCOL + HEADER + RESET + YELLOW + LINE_FORMAT + RESET,
LogLevel.ERROR: HEADERCOL + HEADER + RESET + RED + LINE_FORMAT + RESET,
LogLevel.CRITICAL: HEADERCOL + HEADER + RESET + BOLD_RED + LINE_FORMAT + RESET,
}
"""Dictionary mapping log levels to their respective formats."""
def __init__(self):
"""Initialize the Logger with a stream handler and set the default logging level."""
super().__init__()
# Create a stream handler with the custom logger format.
self._streamhandler = logging.StreamHandler()
self._streamhandler.setFormatter(self)
# Add custom level NOTIF to the logging module
logging.addLevelName(LogLevel.NOTIF, "NOTIF")
# Set the default logging level to DEBUG
logging.basicConfig(
handlers=[self._streamhandler],
level=LogLevel.NOTIF, # Default level set to NOTIF
)
def format(self, record):
"""Format the log record with the appropriate color based on the log level."""
log_fmt = self.FORMATS.get(record.levelno)
formatter = logging.Formatter(log_fmt)
return formatter.format(record)
def get(self):
"""Get the global logger instance."""
return logging.getLogger()
def notif(self, msg, *args, **kwargs):
"""
Log 'msg % args' with severity 'NOTIF'.
To pass exception information, use the keyword argument exc_info with
a true value, e.g.
logger.notif("Houston, we have a %s", "thorny problem", exc_info=1)
"""
self.get().log(LogLevel.NOTIF, msg, *args, **kwargs)
###
# Globals
###
LOGGER: Logger | None = None
"""Global logger instance for the application."""
###
# Configurations
###
def get_default_logger() -> logging.Logger:
"""Initialize the global logger instance."""
global LOGGER # noqa: PLW0603
if LOGGER is None:
LOGGER = Logger()
return LOGGER.get()
def set_log_level(level: LogLevel):
"""Set the logging level for the default logger."""
get_default_logger().setLevel(level)
get_default_logger().debug(f"Log level set to: {logging.getLevelName(level)}")
def reset_log_level():
"""Reset the logging level for the default logger to WARNING."""
get_default_logger().setLevel(LogLevel.NOTIF)
get_default_logger().debug(f"Log level reset to: {logging.getLevelName(LogLevel.NOTIF)}")
def set_log_header(header: str):
"""Set the header for the logger."""
Logger.HEADER = header
###
# Logging
###
def debug(msg: str, *args, **kwargs):
"""Log a debug message."""
get_default_logger().debug(msg, *args, **kwargs, stacklevel=2)
def info(msg: str, *args, **kwargs):
"""Log an info message."""
get_default_logger().info(msg, *args, **kwargs, stacklevel=2)
def notif(msg: str, *args, **kwargs):
"""Log a notification message."""
get_default_logger().log(LogLevel.NOTIF, msg, *args, **kwargs, stacklevel=2)
def warning(msg: str, *args, **kwargs):
"""Log a warning message."""
get_default_logger().warning(msg, *args, **kwargs, stacklevel=2)
def error(msg: str, *args, **kwargs):
"""Log an error message."""
get_default_logger().error(msg, *args, **kwargs, stacklevel=2)
def critical(msg: str, *args, **kwargs):
"""Log a critical message."""
get_default_logger().critical(msg, *args, **kwargs, stacklevel=2)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/profiles.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
This module provides utilities for computing and plotting performance profiles [1,2,3],
which are useful for benchmarking and comparing the performance of different
solvers across a set of problems.
References
----------
- Dolan, E. D., & Moré, J. J. (2002).
Benchmarking Optimization Software with Performance Profiles.
Mathematical Programming, 91, 201-213.
https://doi.org/10.1007/s101070100263
- Dingle, N. J., & Higham, N. J. (2013).
Reducing the Influence of Tiny Normwise Relative Errors on Performance Profiles.
ACM Transactions on Mathematical Software, 39(4), 24:1-24:11.
https://doi.org/10.1145/2491491.2491494
- J. J. Moré, and S. M Wild,
Benchmarking derivative-free optimization algorithms.
SIAM Journal on Optimization, 20(1), 172-191, 2009
https://doi.org/10.1137/080724083
Example
-------
.. code-block:: python
import numpy as np
from newton._src.solvers.kamino._src.utils.profiles import PerformanceProfile
data = np.array([[1.0, 2.0, 4.0], [1.5, 1.5, 8.0]])
pp = PerformanceProfile(data)
pp.plot(names=["S1", "S2"])
"""
import numpy as np
from . import logger as msg
###
# Internals
###
# Local numeric constants (analogs of the C++ constants)
EPSILON: float = float(np.finfo(float).eps)
EPSILON_2: float = float(0.5 * EPSILON)
class _nullcontext:
"""Simple nullcontext for optional style management (avoid importing contextlib at module import time)"""
def __init__(self, *_, **__):
pass
def __enter__(self):
return self
def __exit__(self, *exc):
return False
###
# Types
###
class PerformanceProfile:
"""
Compute performance profiles for a set of num_solvers solvers across a set of np problems.
Given performance measurements t_{p,s} for each solver s in S and problem p in P,
this class computes performance ratios r_{p,s} and the performance profile
rho_s(τ), defined as:
rho_s(τ) = (1 / |P|) * |{ p ∈ P : r_{p,s} ≤ τ }|.
On construction, the input is optionally refined and the profile data are computed
and cached for later use.
Args:
data (np.ndarray): Array of shape (num_solvers, num_problems) with measurements for num_solvers
solvers across num_problems problems. Each entry typically represents a cost such as time,
iterations, or error.
success (np.ndarray | None): Optional boolean or integer array of shape
(num_solvers, num_problems) indicating successful runs (nonzero/True means success). Failed
runs are excluded from the profile. If None, all runs are treated as
successful.
taumax (float): Maximum performance ratio τ for which the profile is
generated. If set to float('inf'), τ_max is chosen as 2x (max observed ratio).
ppfixmax (float): Upper threshold used by the Dingle-Higham refinement when
ppfix is True (useful when the data represent relative errors).
ppfixmin (float): Lower floor used by the Dingle-Higham refinement when
ppfix is True to prevent zero or near-zero values.
ppfix (bool): If True, apply Dingle-Higham style refinement to the input
data before computing ratios.
Attributes:
_t_p_min (np.ndarray | None): Per-problem best (minimal) measurement across
solvers, used to form performance ratios.
_r_ps (np.ndarray | None): Array of performance ratios r_{p,s}.
_rho_s (list[np.ndarray]): Empirical distribution functions rho_s(τ) for each
solver over the profile domain.
_r_min (float): Minimum observed performance ratio.
_r_max (float): Maximum observed performance ratio.
_tau_max (float): Effective maximum τ used for the profile domain.
_valid (bool): True if the profile was successfully computed given the input
data and success mask; False otherwise.
Notes:
- If taumax is float('inf'), the profile domain upper bound is set to twice
the maximum observed ratio to capture the right tail of the distributions.
- The Dingle-Higham refinement (controlled by ppfix, ppfixmin, ppfixmax) is
intended for stabilizing relative-error data by flooring very small values
and capping extreme ratios before computing performance profiles.
"""
def __init__(
self,
data: np.ndarray | None = None,
success: np.ndarray | None = None,
taumax: float = np.inf,
ppfixmax: float = EPSILON_2,
ppfixmin: float = 1e-18,
ppfix: bool = False,
) -> None:
self._t_p_min: np.ndarray | None = None
"""
The per-problem best performance amongst all solvers\n
For every p in P : t_p_min = min{s in S : t_ps}\n
Dimensions are: |P|
"""
self._r_ps: np.ndarray | None = None
"""
The per-sample performance ratios\n
For every s in S and p in P : r_ps = t_ps / min{s in S : t_ps}\n
Dimensions are: |P| x |S|
"""
self._rho_s: list[np.ndarray] = []
"""
The per-solver cumulative distributions\n
For every s in S : rho_s(tau) = (1/np) * size{p in P : r_ps <= tau}\n
Dimensions are: |S| x |Rho|
"""
self._r_min: float = np.inf
"""
The smallest performance ratio present in the data\n
r in [r_min, r_max]
"""
self._r_max: float = np.inf
"""
The largest performance ratio present in the data\n
r in [r_min, r_max]
"""
self._tau_max: float = np.inf
"""
The largest performance ratio considered for generating the performance profile\n
tau in [1, tau_max]
"""
self._valid: bool = False
"""
Flag to indicate if last call to 'compute' was valid\n
This is also useful to check construction-time generation was successful.
"""
# Compute the performance profile if data is provided
if data is not None:
self._valid = self.compute(
data=data,
success=success,
taumax=taumax,
ppfixmax=ppfixmax,
ppfixmin=ppfixmin,
ppfix=ppfix,
)
###
# Properties
###
@property
def is_valid(self) -> bool:
return self._valid
@property
def performance_minima(self) -> np.ndarray:
if self._t_p_min is None:
raise RuntimeError("Performance profile not computed yet.")
return self._t_p_min
@property
def performance_ratios(self) -> np.ndarray:
if self._r_ps is None:
raise RuntimeError("Performance profile not computed yet.")
return self._r_ps
@property
def largest_performance_ratio(self) -> float:
return self._r_max
@property
def cumulative_distributions(self) -> list[np.ndarray]:
return self._rho_s
###
# Operations
###
def compute(
self,
data: np.ndarray,
success: np.ndarray | None = None,
taumax: float = np.inf,
ppfixmax: float = EPSILON_2,
ppfixmin: float = 1e-18,
ppfix: bool = False,
) -> bool:
# Validate input
valid = True
if data is None or data.size == 0:
msg.error("`data` argument is empty!")
valid = False
if data.ndim != 2:
msg.error("`data` must be 2D (num_solvers x np)!")
valid = False
else:
num_solvers, num_problems = data.shape
if num_solvers < 2:
msg.error("`data` must contain at least two solver-wise entries (rows >= 2)!")
valid = False
if num_problems < 1:
msg.error("`data` must contain at least one problem-wise entry (cols >= 1)!")
valid = False
if success is not None and success.size > 0:
if success.shape != data.shape:
msg.error("`success` flags do not match the `data` dimensions!")
valid = False
if not valid:
self._valid = False
return False
# Dimensions
num_solvers, num_problems = data.shape
# Success flags default to ones
success_ps = (
success.astype(float) if success is not None and success.size > 0 else np.ones_like(data, dtype=float)
)
# Work on a copy of data
samples = data.astype(float).copy()
# Optional Dingle & Higham refinement (useful for relative errors)
if ppfix:
ppfixscaling = (ppfixmax - ppfixmin) / ppfixmax
mask = samples < ppfixmax
samples[mask] = ppfixmin + samples[mask] * ppfixscaling
# Per-problem minima across solvers
self._t_p_min = samples.min(axis=0)
# Performance ratios r_ps = t_ps / min_s t_ps when successful, else inf
with np.errstate(divide="ignore", invalid="ignore"):
r_ps = np.where(success_ps > 0.0, samples / self._t_p_min[None, :], np.inf)
# Minimal ratio (before filtering non-finite)
self._r_min = float(np.nanmin(r_ps))
# Replace non-finite values with zeros; negatives are left as-is and pruned later
r_ps = np.where(np.isfinite(r_ps), r_ps, 0.0)
# Maximal ratio observed
self._r_max = float(np.nanmax(r_ps))
# tau max for plotting/domain
self._tau_max = float(taumax) if np.isfinite(taumax) else 2.0 * self._r_max
if self._tau_max == 1.0:
self._tau_max += EPSILON
# Build cumulative distributions per solver
rho_s: list[np.ndarray] = []
for s in range(num_solvers):
tauvec = r_ps[s, :].astype(float)
# unique sorted values
utaus = np.unique(tauvec)
# prune < 1.0 (these include marker zeros)
utaus = utaus[utaus >= 1.0]
# prune > tau_max
utaus = utaus[utaus <= self._tau_max]
# counts for each unique tau value
counts: list[int] = []
for tau in utaus:
counts.append(int(np.count_nonzero(tauvec == tau)))
# cumulative distribution (normalized by number of problems)
rhos = np.cumsum(np.array(counts, dtype=float)) / float(num_problems)
# ensure starting at tau = 1.0 with rho = 0.0
if utaus.size == 0 or (utaus.size > 0 and utaus[0] >= 1.0 + EPSILON):
utaus = np.insert(utaus, 0, 1.0)
rhos = np.insert(rhos, 0, 0.0)
# ensure ending at tau = tau_max with flat tail
if utaus.size <= 1 or (utaus[-1] < self._tau_max - EPSILON):
utaus = np.append(utaus, self._tau_max)
tail = rhos[-1] if rhos.size > 0 else 0.0
rhos = np.append(rhos, tail)
# store as 2xK array: first row taus, second row rhos
rho = np.vstack([utaus, rhos])
rho_s.append(rho)
self._r_ps = r_ps
self._rho_s = rho_s
self._valid = True
return True
def rankings(self) -> dict[str, tuple[np.ndarray, np.ndarray, bool]]:
"""
Compute solver rankings at tau=1.0 and at rho=1.0.
Returns:
tuple[np.ndarray, np.ndarray]: A pair of 1D integer arrays with length equal to the number of solvers.
- First array: rankings based on rho at tau=1.0 (higher is better).
- Second array: rankings based on the smallest tau at which rho=1.0 (lower is better).
Notes:
Rankings are dense: solvers with the same value share the same rank, and
the next rank is incremented accordingly. Solvers that do not achieve the
target value are ranked as -1.
"""
# Ensure a valid profile has been computed before summarizing
if not self._valid:
return "Data is invalid: cannot generate rankings."
# Names default to indices
num_solvers = len(self._rho_s)
# Extract the indices of the best and worst solvers at tau = 1.0 and tau = tau_max
rankings_tau_1 = np.full((num_solvers,), -1, dtype=int) # Best solver at tau = 1.0
rankings_rho_1 = np.full((num_solvers,), -1, dtype=int) # First solver to reach rho = 1.0
# rho_s is stored as a 2xK array: [taus; rhos]
# Collect rho(1.0) for each solver
# (fraction of problems where it performed best)
rhos_at_tau_1 = np.empty((num_solvers,), dtype=float)
for s in range(num_solvers):
taus = self._rho_s[s][0, :]
rhos = self._rho_s[s][1, :]
rhos_at_tau_1[s] = float(rhos[0])
# Dense ranking: higher is better; rank 1 = best
unique_vals = np.unique(rhos_at_tau_1)[::-1]
for rank, val in enumerate(unique_vals, start=1):
rankings_tau_1[rhos_at_tau_1 == val] = rank
# Collect tau where rho first reaches 1.0 for each solver
# (the factor by which it is close to the best solver on all problems)
taus_at_rho_1 = np.empty((num_solvers,), dtype=float)
for s in range(num_solvers):
taus = self._rho_s[s][0, :]
rhos = self._rho_s[s][1, :]
idx = np.where(np.isclose(rhos, 1.0, atol=EPSILON, rtol=0.0))[0]
taus_at_rho_1[s] = float(taus[idx[0]]) if idx.size else np.inf
# Dense ranking: lower is better; rank 1 = best
unique_vals = np.unique(taus_at_rho_1)
for rank, val in enumerate(unique_vals, start=1):
rankings_rho_1[taus_at_rho_1 == val] = rank
# Return the computed rankings together with the associated values
return {"rho@tau=1": (rankings_tau_1, rhos_at_tau_1, False), "tau@rho=1": (rankings_rho_1, taus_at_rho_1, True)}
def plot(
self,
names: list[str] | None = None,
title: str = "Performance Profile",
xtitle: str = "Performance Ratio",
ytitle: str = "Proportion of Problems",
xlim: tuple[float, float] | None = None,
ylim: tuple[float, float] | None = None,
xscale: str = "log2",
yscale: str = "linear",
style: str | None = None,
show: bool = True,
path: str | None = None,
) -> None:
"""
Generates a 2D plot to visualize the performance profile using Matplotlib.
Args:
names (str, optional): A vector of solver names used to generate plot labels.
title (str, optional): The title of the plot.
xtitle (str, optional): The label for the x-axis.
ytitle (str, optional): The label for the y-axis.
xlim (tuple, optional): The limits for the x-axis as (xmin, xmax). If None, defaults to (1.0, tau_max).
ylim (tuple, optional): The limits for the y-axis as (ymin, ymax). If None, defaults to (0.0, 1.1).
xscale (str, optional): The scale for the x-axis. Options are 'linear', 'log2', or 'log10'.
yscale (str, optional): The scale for the y-axis. Options are 'linear', 'log2', or 'log10'.
legend_loc (str, optional): The location of the legend on the plot.
style (str, optional): An optional Matplotlib style to apply to the plot.
"""
# Ensure a valid profile has been computed before plotting
if not self._valid:
msg.error("Data is invalid: aborting plot operation")
return
# Attempt to import matplotlib
try:
import matplotlib.pyplot as plt
from cycler import cycler # noqa: PLC0415
except Exception as exc: # pragma: no cover - optional dependency
msg.error(f"`matplotlib` is required to plot profiles: {exc}")
return
# Names default to indices
num_solvers = len(self._rho_s)
solver_names = list(names) if names is not None else [str(i) for i in range(num_solvers)]
if len(solver_names) < num_solvers:
solver_names += [str(i) for i in range(len(solver_names), num_solvers)]
# Apply an optional style
context_mgr = plt.style.context(style) if style else _nullcontext()
with context_mgr:
# 60 styles = 10 colors x 6 dash styles
colors = list(plt.cm.tab10.colors) # 10 high-contrast colors
dash_styles = ["-", "--", "-.", ":", (0, (3, 1, 1, 1)), (0, (1, 5))] # 6 clear linestyles
prop = cycler(linestyle=dash_styles) * cycler(color=colors)
plt.rcParams["lines.linewidth"] = 1.8 # consistent, readable width
# Create the figure and axis
fig, ax = plt.subplots(figsize=(8, 8))
# Set up a prop cycle with distinct colors and linestyles
ax.set_prop_cycle(prop)
# Plot step profiles
lines = []
for s in range(num_solvers):
x = self._rho_s[s][0, :]
y = self._rho_s[s][1, :]
(line,) = ax.step(x, y, where="post", label=solver_names[s])
lines.append(line)
# Axes scales and limits
if xscale == "log2":
ax.set_xscale("log", base=2)
elif xscale == "log10":
ax.set_xscale("log", base=10)
else:
ax.set_xscale("linear")
if yscale == "log2":
ax.set_yscale("log", base=2)
elif yscale == "log10":
ax.set_yscale("log", base=10)
else:
ax.set_yscale("linear")
xlim = (1.0, self._tau_max) if xlim is None else xlim
ylim = (0.0, 1.1) if ylim is None else ylim
ax.set_xlim(*xlim)
ax.set_ylim(*ylim)
ax.set_xlabel(xtitle)
ax.set_ylabel(ytitle)
ax.set_title(title)
ax.grid(True, which="both", linestyle=":", linewidth=0.8, alpha=0.25)
legend = ax.legend(bbox_to_anchor=(1.05, 0.5), loc="center left", fancybox=True, shadow=True)
map_legend_to_ax = {} # Will map legend lines to original lines.
pickradius = 5 # Points (Pt). How close the click needs to be to trigger an event.
for legend_line, ax_line in zip(legend.get_lines(), lines, strict=False):
legend_line.set_picker(pickradius) # Enable picking on the legend line.
map_legend_to_ax[legend_line] = ax_line
def on_pick(event):
# On the pick event, find the original line corresponding to the legend
# proxy line, and toggle its visibility.
legend_line = event.artist
# Do nothing if the source of the event is not a legend line.
if legend_line not in map_legend_to_ax:
return
ax_line = map_legend_to_ax[legend_line]
visible = not ax_line.get_visible()
ax_line.set_visible(visible)
# Change the alpha on the line in the legend, so we can see what lines
# have been toggled.
legend_line.set_alpha(1.0 if visible else 0.2)
fig.canvas.draw()
# Works even if the legend is draggable. This is independent from picking legend lines.
fig.canvas.mpl_connect("pick_event", on_pick)
legend.set_draggable(True)
plt.tight_layout()
if path is not None:
plt.savefig(path, bbox_inches="tight", dpi=300)
msg.debug(f"Profile plot saved to: {path}")
if show:
plt.show()
plt.close(fig)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/sim/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Simulation Module
"""
from .datalog import SimulationLogger
from .simulator import Simulator, SimulatorData
from .viewer import ViewerKamino
###
# Module interface
###
__all__ = ["SimulationLogger", "Simulator", "SimulatorData", "ViewerKamino"]
================================================
FILE: newton/_src/solvers/kamino/_src/utils/sim/datalog.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Utilities for simulation data logging and plotting."""
import os
import numpy as np
from ...core.builder import ModelBuilderKamino
from .. import logger as msg
from ..control import JointSpacePIDController
from .simulator import Simulator
###
# Module interface
###
__all__ = [
"SimulationLogger",
]
###
# Interfaces
###
class SimulationLogger:
"""
TODO
"""
plt = None
"""Class-level variable to hold the imported module"""
@classmethod
def initialize_plt(cls):
"""TODO"""
if cls.plt is None: # Only import if not already imported
# Attempt to import matplotlib for plotting
try:
import matplotlib.pyplot as plt
cls.plt = plt
except ImportError:
return # matplotlib is not available so we skip plotting
def __init__(
self,
max_frames: int,
sim: Simulator,
builder: ModelBuilderKamino,
controller: JointSpacePIDController | None = None,
):
"""
TODO
"""
# Check if the simulation builder, and controller instances are valid
if not isinstance(sim, Simulator):
raise TypeError("'simulator' must be an instance of `Simulator`.")
if not isinstance(builder, ModelBuilderKamino):
raise TypeError("'builder' must be an instance of `ModelBuilderKamino`.")
if controller is not None:
if not isinstance(controller, JointSpacePIDController):
raise TypeError("'controller' must be an instance of `JointSpacePIDController` or `None`.")
# Warn if multiple worlds are present
if sim.model.size.num_worlds > 1:
msg.warning("SimulationLogger currently only records data from the first world.")
# Attempt to initialize matplotlib for plotting
self.initialize_plt()
# Initialize internals
self._frames: int = 0
self._max_frames: int = max_frames
self._sim: Simulator = sim
self._builder: ModelBuilderKamino = builder
self._ctrl: JointSpacePIDController | None = controller
# Allocate logging arrays for solver convergence info
self.log_num_limits = np.zeros(self._max_frames, dtype=np.int32)
self.log_num_contacts = np.zeros(self._max_frames, dtype=np.int32)
self.log_padmm_iters = np.zeros((self._max_frames,), dtype=np.int32)
self.log_padmm_r_p = np.zeros((self._max_frames,), dtype=np.float32)
self.log_padmm_r_d = np.zeros((self._max_frames,), dtype=np.float32)
self.log_padmm_r_c = np.zeros((self._max_frames,), dtype=np.float32)
# Extract actuated DOF indices
self._nja: int = self._sim.model.size.sum_of_num_actuated_joints
self._njaq: int = self._sim.model.size.sum_of_num_actuated_joint_coords
self._actuated_dofs: list[int] = []
if self._njaq != self._nja:
self._nja = 0
self._njaq = 0
msg.warning(
f"Number of actuated joint coordinates ({self._njaq}) does not match "
f"number of actuated joints ({self._nja}), skipping joint logging."
)
else:
dof_offset = 0
for joint in self._builder.joints:
if joint.is_actuated:
for dof in range(joint.num_dofs):
self._actuated_dofs.append(dof_offset + dof)
dof_offset += joint.num_dofs
# Allocate actuated joint logging arrays if applicable
if self._njaq > 0 and self._nja > 0:
self.log_q_j = np.zeros((self._max_frames, self._nja), dtype=np.float32)
self.log_dq_j = np.zeros((self._max_frames, self._nja), dtype=np.float32)
self.log_tau_j = np.zeros((self._max_frames, self._nja), dtype=np.float32)
if self._ctrl is not None:
self.log_q_j_ref = np.zeros((self._max_frames, self._nja), dtype=np.float32)
self.log_dq_j_ref = np.zeros((self._max_frames, self._nja), dtype=np.float32)
# Allocate logging arrays for solution metrics
if self._sim.metrics is not None:
# self.log_kinetic_energy = np.zeros((self._max_frames,), dtype=np.float32)
# self.log_potential_energy = np.zeros((self._max_frames,), dtype=np.float32)
# self.log_total_energy = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_eom = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_eom_argmax = np.full((self._max_frames, 2), fill_value=(-1, -1), dtype=np.int32)
self.log_r_kinematics = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_kinematics_argmax = np.full((self._max_frames, 2), fill_value=(-1, -1), dtype=np.int32)
self.log_r_cts_joints = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_cts_joints_argmax = np.full((self._max_frames, 2), fill_value=(-1, -1), dtype=np.int32)
self.log_r_cts_limits = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_cts_limits_argmax = np.full((self._max_frames, 2), fill_value=(-1, -1), dtype=np.int32)
self.log_r_cts_contacts = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_cts_contacts_argmax = np.full((self._max_frames,), fill_value=-1, dtype=np.int32)
self.log_r_v_plus = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_v_plus_argmax = np.full((self._max_frames,), fill_value=-1, dtype=np.int32)
self.log_r_ncp_primal = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_ncp_primal_argmax = np.full((self._max_frames,), fill_value=-1, dtype=np.int32)
self.log_r_ncp_dual = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_ncp_dual_argmax = np.full((self._max_frames,), fill_value=-1, dtype=np.int32)
self.log_r_ncp_compl = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_ncp_compl_argmax = np.full((self._max_frames,), fill_value=-1, dtype=np.int32)
self.log_r_vi_natmap = np.zeros((self._max_frames,), dtype=np.float32)
self.log_r_vi_natmap_argmax = np.full((self._max_frames,), fill_value=-1, dtype=np.int32)
self.log_f_ncp = np.zeros((self._max_frames,), dtype=np.float32)
self.log_f_ccp = np.zeros((self._max_frames,), dtype=np.float32)
def reset(self):
"""
Resets the logging frame counter to zero.
"""
self._frames = 0
def log(self):
"""
TODO
"""
if self._frames >= self._max_frames:
msg.warning("Maximum logging frames reached, skipping data logging.")
return
# Log unilateral constraints info
if self._sim.limits.model_max_limits_host > 0:
self.log_num_limits[self._frames] = self._sim.limits.model_active_limits.numpy()[0]
if self._sim.contacts.model_max_contacts_host > 0:
self.log_num_contacts[self._frames] = self._sim.contacts.data.model_active_contacts.numpy()[0]
# Log PADMM solver info
self.log_padmm_iters[self._frames] = self._sim.solver.solver_fd.data.status.numpy()[0][1]
self.log_padmm_r_p[self._frames] = self._sim.solver.solver_fd.data.status.numpy()[0][2]
self.log_padmm_r_d[self._frames] = self._sim.solver.solver_fd.data.status.numpy()[0][3]
self.log_padmm_r_c[self._frames] = self._sim.solver.solver_fd.data.status.numpy()[0][4]
# Log joint actuator info if available
if self._njaq > 0 and self._nja > 0 and self._njaq == self._nja:
self.log_q_j[self._frames, :] = self._sim.state.q_j.numpy()[self._actuated_dofs]
self.log_dq_j[self._frames, :] = self._sim.state.dq_j.numpy()[self._actuated_dofs]
self.log_tau_j[self._frames, :] = self._sim.control.tau_j.numpy()[self._actuated_dofs]
# Log controller references if available
if self._ctrl is not None:
self.log_q_j_ref[self._frames, :] = self._ctrl.data.q_j_ref.numpy()
self.log_dq_j_ref[self._frames, :] = self._ctrl.data.dq_j_ref.numpy()
# Log solution metrics if available
if self._sim.metrics is not None:
metrics = self._sim.metrics.data
# self.log_kinetic_energy[self._frames] = metrics.kinetic_energy.numpy()[0]
# self.log_potential_energy[self._frames] = metrics.potential_energy.numpy()[0]
# self.log_total_energy[self._frames] = metrics.total_energy.numpy()[0]
self.log_r_eom[self._frames] = metrics.r_eom.numpy()[0]
self.log_r_eom_argmax[self._frames, :] = self._unpack_key(metrics.r_eom_argmax.numpy()[0])
self.log_r_kinematics[self._frames] = metrics.r_kinematics.numpy()[0]
self.log_r_kinematics_argmax[self._frames, :] = self._unpack_key(metrics.r_kinematics_argmax.numpy()[0])
self.log_r_cts_joints[self._frames] = metrics.r_cts_joints.numpy()[0]
self.log_r_cts_joints_argmax[self._frames, :] = self._unpack_key(metrics.r_cts_joints_argmax.numpy()[0])
self.log_r_cts_limits[self._frames] = metrics.r_cts_limits.numpy()[0]
self.log_r_cts_limits_argmax[self._frames, :] = self._unpack_key(metrics.r_cts_limits_argmax.numpy()[0])
self.log_r_cts_contacts[self._frames] = metrics.r_cts_contacts.numpy()[0]
self.log_r_cts_contacts_argmax[self._frames] = metrics.r_cts_contacts_argmax.numpy()[0]
self.log_r_v_plus[self._frames] = metrics.r_v_plus.numpy()[0]
self.log_r_v_plus_argmax[self._frames] = metrics.r_v_plus_argmax.numpy()[0]
self.log_r_ncp_primal[self._frames] = metrics.r_ncp_primal.numpy()[0]
self.log_r_ncp_primal_argmax[self._frames] = metrics.r_ncp_primal_argmax.numpy()[0]
self.log_r_ncp_dual[self._frames] = metrics.r_ncp_dual.numpy()[0]
self.log_r_ncp_dual_argmax[self._frames] = metrics.r_ncp_dual_argmax.numpy()[0]
self.log_r_ncp_compl[self._frames] = metrics.r_ncp_compl.numpy()[0]
self.log_r_ncp_compl_argmax[self._frames] = metrics.r_ncp_compl_argmax.numpy()[0]
self.log_r_vi_natmap[self._frames] = metrics.r_vi_natmap.numpy()[0]
self.log_r_vi_natmap_argmax[self._frames] = metrics.r_vi_natmap_argmax.numpy()[0]
self.log_f_ncp[self._frames] = metrics.f_ncp.numpy()[0]
self.log_f_ccp[self._frames] = metrics.f_ccp.numpy()[0]
# Progress the frame counter
self._frames += 1
def plot_solver_info(self, path: str | None = None, show: bool = False):
"""
TODO
"""
# Attempt to initialize matplotlib
if self.plt is None:
msg.warning("matplotlib is not available, skipping plotting.")
return
# Check if there are any logged frames
if self._frames == 0:
msg.warning("No logged frames to plot, skipping solver info plotting.")
return
# Create an array for time logging
# TODO: Handle array-valued time-steps
dt = self._sim.config.dt
time = np.arange(0, self._frames, dtype=np.float32) * dt
# Plot the PADMM convergence information
padmm_iters_path = os.path.join(path, "padmm_status.png") if path is not None else None
_, axs = self.plt.subplots(4, 1, figsize=(10, 10), sharex=True)
# Plot the PADMM iterations
axs[0].step(time, self.log_padmm_iters[: self._frames], label="PADMM Iterations", color="blue")
axs[0].set_title("PADMM Solver Iterations")
axs[0].set_ylabel("Iterations")
axs[0].set_xlabel("Time (s)")
axs[0].legend()
axs[0].grid()
# Plot the PADMM primal residuals
eps_p = self._sim.config.solver.padmm.primal_tolerance
axs[1].step(time, self.log_padmm_r_p[: self._frames], label="PADMM Primal Residual", color="orange")
axs[1].axhline(eps_p, color="black", linestyle="--", linewidth=1.0, label=f"eps_p={eps_p:.1e}")
axs[1].set_title("PADMM Primal Residual")
axs[1].set_ylabel("Primal Residual")
axs[1].set_xlabel("Time (s)")
axs[1].legend()
axs[1].grid()
# Plot the PADMM dual residuals
eps_d = self._sim.config.solver.padmm.dual_tolerance
axs[2].step(time, self.log_padmm_r_d[: self._frames], label="PADMM Dual Residual", color="green")
axs[2].axhline(eps_d, color="black", linestyle="--", linewidth=1.0, label=f"eps_d={eps_d:.1e}")
axs[2].set_title("PADMM Dual Residual")
axs[2].set_ylabel("Dual Residual")
axs[2].set_xlabel("Time (s)")
axs[2].legend()
axs[2].grid()
# Plot the PADMM complementarity residuals
eps_c = self._sim.config.solver.padmm.compl_tolerance
axs[3].step(time, self.log_padmm_r_c[: self._frames], label="PADMM Complementarity Residual", color="red")
axs[3].axhline(eps_c, color="black", linestyle="--", linewidth=1.0, label=f"eps_c={eps_c:.1e}")
axs[3].set_title("PADMM Complementarity Residual")
axs[3].set_ylabel("Complementarity Residual")
axs[3].set_xlabel("Time (s)")
axs[3].legend()
axs[3].grid()
# Adjust layout
self.plt.tight_layout()
# Save the figure if a path is provided
if padmm_iters_path is not None:
self.plt.savefig(padmm_iters_path, dpi=300)
# Show the figure if requested
# NOTE: This will block execution until the plot window is closed
if show:
self.plt.show()
# Close the current figure to free memory
self.plt.close()
# Plot histogram
padmm_iters_hist_path = os.path.join(path, "padmm_iterations_histogram.png") if path is not None else None
self.plt.rcParams["axes.axisbelow"] = True
self.plt.grid(True, which="major", linestyle="--", linewidth=0.5)
self.plt.grid(True, which="minor", linestyle=":", linewidth=0.25)
self.plt.hist(self.log_padmm_iters[: self._frames], bins=50)
self.plt.yscale("log") # Make Y-axis logarithmic
self.plt.title("Histogram of PADMM Solver Iterations")
self.plt.xlabel("Iterations")
self.plt.ylabel("Frequency")
# Save the figure if a path is provided
if padmm_iters_hist_path is not None:
self.plt.savefig(padmm_iters_hist_path, dpi=300)
# Show the figure if requested
# NOTE: This will block execution until the plot window is closed
if show:
self.plt.show()
# Close the current figure to free memory
self.plt.close()
self.plt.rcParams["axes.axisbelow"] = False
def plot_joint_tracking(self, path: str | None = None, show: bool = False):
"""
TODO
"""
# Attempt to initialize matplotlib
if self.plt is None:
msg.warning("matplotlib is not available, skipping plotting.")
return
# Check if there are any logged frames
if self._frames == 0:
msg.warning("No logged frames to plot, skipping solver info plotting.")
return
# Ensure that joint logging data is available
if self._njaq == 0 or self._nja == 0:
msg.warning("No actuated joints to plot, skipping joint-tracking plotting.")
return
# Create an array for time logging
dt = self._sim.config.dt
time = np.arange(0, self._frames, dtype=np.float32) * dt
# Then plot the joint tracking results
for j in range(len(self._actuated_dofs)):
# Set the output path for the current joint
tracking_path = os.path.join(path, f"tracking_joint_{j}.png") if path is not None else None
# Plot logged data after the viewer is closed
_, axs = self.plt.subplots(3, 1, figsize=(10, 10), sharex=True)
# Plot the measured vs reference joint positions
axs[0].step(time, self.log_q_j[: self._frames, j], label="Measured")
if self._ctrl:
axs[0].step(time, self.log_q_j_ref[: self._frames, j], label="Reference", linestyle="--")
axs[0].set_title(f"Actuator DoF {j} Position Tracking")
axs[0].set_ylabel("Actuator Position (rad)")
axs[0].legend()
axs[0].grid()
# Plot the measured vs reference joint velocities
axs[1].step(time, self.log_dq_j[: self._frames, j], label="Measured")
if self._ctrl:
axs[1].step(time, self.log_dq_j_ref[: self._frames, j], label="Reference", linestyle="--")
axs[1].set_title(f"Actuator DoF {j} Velocity Tracking")
axs[1].set_ylabel("Actuator Velocity (rad/s)")
axs[1].legend()
axs[1].grid()
# Plot the control torques
axs[2].step(time, self.log_tau_j[: self._frames, j], label="Control Torque")
axs[2].set_title(f"Actuator DoF {j} Control Torque")
axs[2].set_ylabel("Torque (Nm)")
axs[2].set_xlabel("Time (s)")
axs[2].legend()
axs[2].grid()
# Adjust layout
self.plt.tight_layout()
# Save the figure if a path is provided
if tracking_path is not None:
self.plt.savefig(tracking_path, dpi=300)
# Show the figure if requested
# NOTE: This will block execution until the plot window is closed
if show:
self.plt.show()
# Close the current figure to free memory
self.plt.close()
def plot_solution_metrics(self, path: str | None = None, show: bool = False):
"""
TODO
"""
# Attempt to initialize matplotlib
if self.plt is None:
msg.warning("matplotlib is not available, skipping plotting.")
return
# Check if there are any logged frames
if self._frames == 0:
msg.warning("No logged frames to plot, skipping solution metrics plotting.")
return
# Ensure that solution metrics data is available
if self._sim.metrics is None:
msg.warning("No solution metrics to plot, skipping solution metrics plotting.")
return
# Create an array for time logging
dt = self._sim.config.dt
time = np.arange(0, self._frames, dtype=np.float32) * dt
# Plot the solution metrics
metrics_path = os.path.join(path, "solution_metrics.png") if path is not None else None
_, axs = self.plt.subplots(6, 2, figsize=(15, 20), sharex=True)
# Plot each metric
metric_titles = [
"EoM Residual (r_eom)",
"Kinematics Residual (r_kinematics)",
"Joint Constraints Residual (r_cts_joints)",
"Limit Constraints Residual (r_cts_limits)",
"Contact Constraints Residual (r_cts_contacts)",
"Post-Event Constraint Velocity Residual (r_v_plus)",
"NCP Primal Residual (r_ncp_primal)",
"NCP Dual Residual (r_ncp_dual)",
"NCP Complementarity Residual (r_ncp_compl)",
"VI Natural-Map Residual (r_vi_natmap)",
"NCP Objective (f_ncp)",
"CCP Objective (f_ccp)",
]
metric_names = [
"r_eom",
"r_kinematics",
"r_cts_joints",
"r_cts_limits",
"r_cts_contacts",
"r_v_plus",
"r_ncp_primal",
"r_ncp_dual",
"r_ncp_compl",
"r_vi_natmap",
"f_ncp",
"f_ccp",
]
log_attrs = [
self.log_r_eom,
self.log_r_kinematics,
self.log_r_cts_joints,
self.log_r_cts_limits,
self.log_r_cts_contacts,
self.log_r_v_plus,
self.log_r_ncp_primal,
self.log_r_ncp_dual,
self.log_r_ncp_compl,
self.log_r_vi_natmap,
self.log_f_ncp,
self.log_f_ccp,
]
for i, (title, name, log_attr) in enumerate(zip(metric_titles, metric_names, log_attrs, strict=False)):
ax = axs[i // 2, i % 2]
ax.step(time, log_attr[: self._frames], label=name)
ax.set_title(title)
ax.set_ylabel(name)
ax.set_xlabel("Time (s)")
ax.legend()
ax.grid()
# Adjust layout
self.plt.tight_layout()
# Save the figure if a path is provided
if metrics_path is not None:
self.plt.savefig(metrics_path, dpi=300)
# Show the figure if requested
# NOTE: This will block execution until the plot window is closed
if show:
self.plt.show()
# Close the current figure to free memory
self.plt.close()
@staticmethod
def _unpack_key(key: np.uint64) -> tuple[int, int]:
"""
TODO
"""
index1 = (key >> 32) & 0x7FFFFFFF
index2 = key & 0x7FFFFFFF
return int(index1), int(index2)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/sim/runner.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import time
import warp as wp
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.control import JointSpacePIDController
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import print_progress_bar
###
# Interfaces
###
class SimulationRunner:
def __init__(
self,
builder: ModelBuilderKamino,
simulator: Simulator,
controller: JointSpacePIDController | None = None,
device: wp.DeviceLike = None,
max_steps: int = 0,
max_time: float = 0.0,
viewer_fps: float = 60.0,
sim_dt: float = 0.001,
gravity: bool = True,
ground: bool = True,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
"""TODO"""
# Initialize target frames per second and corresponding time-steps
self.fps = viewer_fps
self.sim_dt = sim_dt
self.frame_dt = 1.0 / self.fps
self.substeps = max(1, round(self.frame_dt / self.sim_dt))
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = False
self.logging: bool = logging
# TODO
self.builder = builder
self.sim = simulator
self.ctrl = controller
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(max_steps, self.builder, self.sim, self.ctrl)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
# Capture CUDA graph if requested and available
self._capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulation...")
self.step()
self.reset()
###
# Simulation API
###
def reset(self):
"""TODO"""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self._run_reset()
if self.logging:
self.logger.reset()
self.logger.log()
def step(self):
"""TODO"""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self._run_step()
if self.logging:
self.logger.log()
def render(self):
"""TODO"""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""TODO"""
pass
def export(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""TODO"""
pass
def run(self, num_frames: int = 0, progress: bool = True):
"""TODO"""
msg.notif(f"Running for {num_frames} frames...")
start_time = time.time()
for i in range(num_frames):
self.step()
wp.synchronize()
if progress:
print_progress_bar(i + 1, num_frames, start_time, prefix="Progress", suffix="")
###
# Internals
###
def _capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self._run_reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self._run_step()
self.step_graph = step_capture.graph
else:
msg.info("Running with kernels...")
def _run_reset(self):
"""TODO"""
self.sim.reset()
def _run_step(self):
"""TODO"""
for _i in range(self.substeps):
self.sim.step()
================================================
FILE: newton/_src/solvers/kamino/_src/utils/sim/simulator.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Provides a high-level interface for physics simulation."""
from __future__ import annotations
from collections.abc import Callable
from dataclasses import dataclass, field
import warp as wp
from ...core.builder import ModelBuilderKamino
from ...core.control import ControlKamino
from ...core.model import ModelKamino
from ...core.state import StateKamino
from ...core.types import FloatArrayLike
from ...geometry import CollisionDetector
from ...solver_kamino_impl import SolverKaminoImpl
###
# Module interface
###
__all__ = [
"Simulator",
"SimulatorData",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Types
###
class SimulatorData:
"""
Holds the time-varying data for the simulation.
Attributes:
state_p (StateKamino):
The previous state data of the simulation
state_n (StateKamino):
The current state data of the simulation, computed from the previous step as:
``state_n = f(state_p, control)``, where ``f()`` is the system dynamics function.
control (ControlKamino):
The control data, computed at each step as:
``control = g(state_n, state_p, control)``, where ``g()`` is the control function.
"""
def __init__(self, model: ModelKamino):
"""
Initializes the simulator data for the given model on the specified device.
"""
self.state_p: StateKamino = model.state(device=model.device)
self.state_n: StateKamino = model.state(device=model.device)
self.control: ControlKamino = model.control(device=model.device)
def cache_state(self):
"""
Updates the previous-step caches of the state and control data from the next-step.
"""
self.state_p.copy_from(self.state_n)
###
# Interfaces
###
class Simulator:
"""
A high-level interface for executing physics simulations using Kamino.
The Simulator class encapsulates the entire simulation pipeline, including model definition,
state management, collision detection, constraint handling, and time integration.
A Simulator is typically instantiated from a :class:`ModelBuilderKamino` that defines the model
to be simulated. The simulator manages the time-stepping loop, invoking callbacks at various
stages of the simulation step, and provides access to the current state and control inputs.
Example:
```python
# Create a model builder and define the model
builder = ModelBuilderKamino()
# Define the model components (e.g., bodies, joints, collision geometries etc.)
builder.add_rigid_body(...)
builder.add_joint(...)
builder.add_geometry(...)
# Create the simulator from the builder
simulator = Simulator(builder)
# Run the simulation for a specified number of steps
for _i in range(num_steps):
simulator.step()
```
"""
@dataclass
class Config:
"""
Holds the configuration for the simulator.
"""
dt: float | FloatArrayLike = 0.001
"""
The time-step to be used for the simulation.\n
Defaults to `0.001` seconds.
"""
collision_detector: CollisionDetector.Config = field(default_factory=CollisionDetector.Config)
"""
The config for the collision detector.
See :class:`CollisionDetector.Config` for more details.
"""
solver: SolverKaminoImpl.Config = field(default_factory=SolverKaminoImpl.Config)
"""
The config for the dynamics solver.\n
See :class:`SolverKaminoImpl.Config` for more details.
"""
def validate(self) -> None:
"""
Validates the simulator configurations.
"""
# First check the time-step
if isinstance(self.dt, float):
if self.dt != self.dt:
raise ValueError("Invalid time-step: cannot be NaN.")
if self.dt <= 0.0:
raise ValueError(f"Invalid time-step: got {self.dt}, but must be a positive value.")
elif isinstance(self.dt, FloatArrayLike):
if len(self.dt) == 0:
raise ValueError("Invalid time-step array: cannot be empty.")
elif any(dt <= 0.0 or dt != dt for dt in self.dt):
raise ValueError("Invalid time-step array: all values must be positive and non-NaN.")
elif not all(isinstance(dt, float) for dt in self.dt):
raise TypeError("Invalid time-step array: all values must be of type float.")
else:
raise TypeError("Invalid time-step: must be a `float` or a `FloatArrayLike`.`")
# Ensure nested configs are properly created
if not isinstance(self.collision_detector, CollisionDetector.Config):
raise TypeError(f"Invalid type for collision_detector config: {type(self.collision_detector)}")
if not isinstance(self.solver, SolverKaminoImpl.Config):
raise TypeError(f"Invalid type for solver config: {type(self.solver)}")
# Then check the nested config values
self.collision_detector.validate()
self.solver.validate()
def __post_init__(self):
"""Post-initialization processing to ensure nested configs are properly created."""
self.validate()
SimCallbackType = Callable[["Simulator"], None]
"""Defines a common type signature for all simulator callback functions."""
def __init__(
self,
builder: ModelBuilderKamino,
config: Simulator.Config = None,
device: wp.DeviceLike = None,
):
"""
Initializes the simulator with the given model builder, time-step, and device.
Args:
builder (ModelBuilderKamino): The model builder defining the model to be simulated.
config (Simulator.Config, optional): The simulator config to use. If None, the default config are used.
device (wp.DeviceLike, optional): The device to run the simulation on. If None, the default device is used.
"""
# Cache simulator config: If no config is provided, use default configs
if config is None:
config = Simulator.Config()
config.validate()
self._config: Simulator.Config = config
# Cache the target device use for the simulation
self._device: wp.DeviceLike = device
# Pass collision detector config to builder before finalization
if self._config.collision_detector.max_contacts_per_pair is not None:
builder.max_contacts_per_pair = self._config.collision_detector.max_contacts_per_pair
# Finalize the model from the builder on the specified
# device, allocating all necessary model data structures
self._model = builder.finalize(device=self._device)
# Configure model time-steps across all worlds
if isinstance(self._config.dt, float):
self._model.time.set_uniform_timestep(self._config.dt)
elif isinstance(self._config.dt, FloatArrayLike):
self._model.time.set_timesteps(self._config.dt)
# Allocate time-varying simulation data
self._data = SimulatorData(model=self._model)
# Allocate collision detection and contacts interface
self._collision_detector = CollisionDetector(
model=self._model,
config=self._config.collision_detector,
)
# Capture a reference to the contacts manager
self._contacts = self._collision_detector.contacts
# Define a physics solver for time-stepping
self._solver = SolverKaminoImpl(
model=self._model,
contacts=self._contacts,
config=self._config.solver,
)
# Initialize callbacks
self._pre_reset_cb: Simulator.SimCallbackType = None
self._post_reset_cb: Simulator.SimCallbackType = None
self._control_cb: Simulator.SimCallbackType = None
# Initialize the simulation state
with wp.ScopedDevice(self._device):
self.reset()
###
# Properties
###
@property
def config(self) -> Simulator.Config:
"""
Returns the simulator config.
"""
return self._config
@property
def model(self) -> ModelKamino:
"""
Returns the time-invariant simulation model data.
"""
return self._model
@property
def data(self) -> SimulatorData:
"""
Returns the simulation data container.
"""
return self._data
@property
def state(self) -> StateKamino:
"""
Returns the current state of the simulation.
"""
return self._data.state_n
@property
def state_previous(self) -> StateKamino:
"""
Returns the previous state of the simulation.
"""
return self._data.state_p
@property
def control(self) -> ControlKamino:
"""
Returns the current control inputs of the simulation.
"""
return self._data.control
@property
def limits(self):
"""
Returns the limits container of the simulation.
"""
return self._solver._limits
@property
def contacts(self):
"""
Returns the contacts container of the simulation.
"""
return self._contacts
@property
def metrics(self):
"""
Returns the current simulation metrics.
"""
return self._solver.metrics
@property
def collision_detector(self) -> CollisionDetector:
"""
Returns the collision detector.
"""
return self._collision_detector
@property
def solver(self) -> SolverKaminoImpl:
"""
Returns the physics step solver.
"""
return self._solver
###
# Configurations - Callbacks
###
def set_pre_reset_callback(self, callback: SimCallbackType):
"""
Sets a reset callback to be called at the beginning of each call to `reset_*()` methods.
"""
self._pre_reset_cb = callback
def set_post_reset_callback(self, callback: SimCallbackType):
"""
Sets a reset callback to be called at the end of each call to to `reset_*()` methods.
"""
self._post_reset_cb = callback
def set_control_callback(self, callback: SimCallbackType):
"""
Sets a control callback to be called at the beginning of the step, that
should populate `data.control`, i.e. the control inputs for the current
step, based on the current and previous states and controls.
"""
self._control_cb = callback
###
# Operations
###
def reset(
self,
world_mask: wp.array | None = None,
actuator_q: wp.array | None = None,
actuator_u: wp.array | None = None,
joint_q: wp.array | None = None,
joint_u: wp.array | None = None,
base_q: wp.array | None = None,
base_u: wp.array | None = None,
bodies_q: wp.array | None = None,
bodies_u: wp.array | None = None,
):
"""
Resets the simulation state given a combination of desired base body
and joint states, as well as an optional per-world mask array indicating
which worlds should be reset.
Args:
world_mask (wp.array, optional):
Optional array of per-world masks indicating which worlds should be reset.
Shape of `(num_worlds,)` and type :class:`wp.int8 | wp.bool`
joint_q (wp.array, optional):
Optional array of target joint coordinates.
Shape of `(num_joint_coords,)` and type :class:`wp.float32`
joint_qd (wp.array, optional):
Optional array of target joint DoF velocities.
Shape of `(num_joint_dofs,)` and type :class:`wp.float32`
base_q (wp.array, optional):
Optional array of target base body poses.
Shape of `(num_worlds,)` and type :class:`wp.transformf`
base_qd (wp.array, optional):
Optional array of target base body twists.
Shape of `(num_worlds,)` and type :class:`wp.spatial_vectorf`
"""
# Run the pre-reset callback if it has been set
if self._pre_reset_cb is not None:
self._pre_reset_cb(self)
# Step the physics solver
self._solver.reset(
state_out=self._data.state_n,
world_mask=world_mask,
actuator_q=actuator_q,
actuator_u=actuator_u,
joint_q=joint_q,
joint_u=joint_u,
base_q=base_q,
base_u=base_u,
bodies_q=bodies_q,
bodies_u=bodies_u,
)
# Cache the current state as the previous state for the next step
self._data.cache_state()
# Run the post-reset callback if it has been set
if self._post_reset_cb is not None:
self._post_reset_cb(self)
def step(self):
"""
Advances the simulation by a single time-step.
"""
# Run the control callback if it has been set
if self._control_cb is not None:
self._control_cb(self)
# Cache the current state as the previous state for the next step
self._data.cache_state()
# Step the physics solver
self._solver.step(
state_in=self._data.state_p,
state_out=self._data.state_n,
control=self._data.control,
contacts=self._contacts,
detector=self._collision_detector,
)
================================================
FILE: newton/_src/solvers/kamino/_src/utils/sim/viewer.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""The customized debug viewer of Kamino"""
# Python
import glob
import os
import threading
from typing import ClassVar
# Thirdparty
import warp as wp
from ......geometry.types import GeoType
from ......viewer import ViewerGL
from ...core.builder import ModelBuilderKamino
from ...core.geometry import GeometryDescriptor
from ...core.types import vec3f
from ...core.world import WorldDescriptor
from ...geometry.contacts import ContactMode
from ...utils import logger as msg
from .simulator import Simulator
###
# Kernels
###
@wp.kernel
def compute_contact_box_transforms(
# Kamino contact data
position_A: wp.array(dtype=wp.vec3), # Contact position on body A
position_B: wp.array(dtype=wp.vec3), # Contact position on body B
frame: wp.array(dtype=wp.quatf), # Contact frames
mode: wp.array(dtype=wp.int32), # Contact modes
wid: wp.array(dtype=wp.int32),
num_contacts: int,
world_spacing: wp.vec3,
box_size: wp.vec3, # Box dimensions
# Output buffers
transforms: wp.array(dtype=wp.transform),
scales: wp.array(dtype=wp.vec3),
colors: wp.array(dtype=wp.vec3),
):
"""
Compute transforms, scales, and colors for contact frame boxes.
"""
i = wp.tid()
# Hide contacts beyond the active count or with INACTIVE mode
contact_mode = mode[i]
if i >= num_contacts or contact_mode == wp.int32(ContactMode.INACTIVE):
scales[i] = wp.vec3(0.0, 0.0, 0.0)
colors[i] = wp.vec3(0.0, 0.0, 0.0)
transforms[i] = wp.transform(wp.vec3(0.0, 0.0, 0.0), wp.quat_identity())
return
# Contact position - we could also use the midpoint?
contact_pos = position_B[i]
# Apply world spacing
world_id = float(wid[i])
contact_pos = contact_pos + world_spacing * world_id
# Contact frame rotation
q = frame[i]
# Set transform
transforms[i] = wp.transform(contact_pos, q)
# Set scale
scales[i] = box_size
# Set color based on contact mode
if contact_mode == wp.int32(ContactMode.OPENING):
# White
colors[i] = wp.vec3(1.0, 1.0, 1.0)
elif contact_mode == wp.int32(ContactMode.STICKING):
# Black
colors[i] = wp.vec3(0.1, 0.1, 0.1)
elif contact_mode == wp.int32(ContactMode.SLIDING):
# Blue
colors[i] = wp.vec3(0.404, 0.647, 0.953)
else:
# Unknown mode: Gray
colors[i] = wp.vec3(0.5, 0.5, 0.5)
@wp.kernel
def compute_contact_force_arrows(
# Kamino contact data
position_A: wp.array(dtype=wp.vec3),
position_B: wp.array(dtype=wp.vec3),
frame: wp.array(dtype=wp.quatf), # Contact frames
reaction: wp.array(dtype=wp.vec3), # Contact forces in respective local contact frame
mode: wp.array(dtype=wp.int32), # Contact modes
wid: wp.array(dtype=wp.int32),
num_contacts: int,
world_spacing: wp.vec3,
force_scale: float,
force_threshold: float, # Minimum force to display
# Output buffers
line_starts: wp.array(dtype=wp.vec3),
line_ends: wp.array(dtype=wp.vec3),
line_colors: wp.array(dtype=wp.vec3),
line_widths: wp.array(dtype=float),
):
"""
Compute line segments for visualizing contact forces as arrows.
"""
i = wp.tid()
if i >= num_contacts:
return
# Skip inactive contacts
if mode[i] == wp.int32(ContactMode.INACTIVE):
line_starts[i] = wp.vec3(0.0, 0.0, 0.0)
line_ends[i] = wp.vec3(0.0, 0.0, 0.0)
line_widths[i] = 0.0
return
# Contact position - we could also use the midpoint?
contact_pos = position_B[i]
# Apply world spacing
world_id = float(wid[i])
contact_pos = contact_pos + world_spacing * world_id
# Transform force from contact frame to world frame
# reaction is in contact frame, need to rotate by frame quaternion
q = frame[i]
C = wp.quat_to_matrix(q)
f_world = C * reaction[i]
# Compute force magnitude
f_mag = wp.length(f_world)
# Only render if force is above threshold
if f_mag < force_threshold:
line_starts[i] = wp.vec3(0.0, 0.0, 0.0)
line_ends[i] = wp.vec3(0.0, 0.0, 0.0)
line_widths[i] = 0.0
return
# linear - Nonlinear scaling # todo make this be an option?
scaled_length = force_scale * f_mag
# scaled_length = force_scale * wp.sqrt(f_mag)
# Force direction
f_dir = f_world / f_mag
# Arrow from contact point along force direction
line_starts[i] = contact_pos
line_ends[i] = contact_pos + f_dir * scaled_length
# Magenta color for forces
line_colors[i] = wp.vec3(1.0, 0.0, 1.0)
# Line width proportional to force magnitude but clipped, requires modification in viewer_gl to work properly
# We could also use this to actually visualize something meaningful
line_widths[i] = wp.clamp(1.0 + 0.1 * f_mag, 1.0, 5.0)
###
# Interfaces
###
class ViewerKamino(ViewerGL):
"""
A customized debug viewer for Kamino.
"""
# Define a static set of colors for different bodies
body_colors: ClassVar[list[wp.array]] = [
wp.array([wp.vec3(0.9, 0.1, 0.3)], dtype=wp.vec3), # Crimson Red
wp.array([wp.vec3(0.1, 0.7, 0.9)], dtype=wp.vec3), # Cyan Blue
wp.array([wp.vec3(1.0, 0.5, 0.0)], dtype=wp.vec3), # Orange
wp.array([wp.vec3(0.6, 0.2, 0.8)], dtype=wp.vec3), # Purple
wp.array([wp.vec3(0.2, 0.8, 0.2)], dtype=wp.vec3), # Green
wp.array([wp.vec3(0.8, 0.8, 0.2)], dtype=wp.vec3), # Yellow
wp.array([wp.vec3(0.8, 0.2, 0.8)], dtype=wp.vec3), # Magenta
wp.array([wp.vec3(0.5, 0.5, 0.5)], dtype=wp.vec3), # Gray
]
# Define a static world spacing offset for multiple worlds
world_spacing: ClassVar[vec3f] = vec3f(-2.0, 0.0, 0.0)
def __init__(
self,
builder: ModelBuilderKamino,
simulator: Simulator,
width: int = 1920,
height: int = 1080,
vsync: bool = False,
headless: bool = False,
show_contacts: bool = False,
record_video: bool = False,
video_folder: str | None = None,
skip_img_idx: int = 0,
async_save: bool = False,
):
"""
Initialize the Kamino viewer.
Args:
builder: Model builder.
simulator: The simulator instance to visualize.
width: Window width in pixels.
height: Window height in pixels.
vsync: Enable vertical sync.
headless: Run without displaying a window.
show_contacts: Enable contact point visualization (default: False).
record_video: Enable frame recording to disk.
video_folder: Directory to save recorded frames (default: "./frames").
skip_img_idx: Number of initial frames to skip before recording.
async_save: Save frames asynchronously in background threads.
"""
# Initialize the base viewer
super().__init__(width=width, height=height, vsync=vsync, headless=headless)
# Cache references to the simulator
self._simulator = simulator
# Declare and initialize geometry info cache
self._worlds: list[WorldDescriptor] = builder.worlds
self._geometry: list[GeometryDescriptor] = builder.geoms
# Initialize video recording settings
self._record_video = record_video
self._video_folder = video_folder or "./frames"
self._async_save = async_save
self._skip_img_idx = skip_img_idx
self._img_idx = 0
self._frame_buffer = None
# Contact visualization settings
self._show_contacts = show_contacts
if self._record_video:
os.makedirs(self._video_folder, exist_ok=True)
def render_geometry(self, body_poses: wp.array, geom: GeometryDescriptor, scope: str):
# TODO: Fix this
bid = geom.body + self._worlds[geom.wid].bodies_idx_offset if geom.body >= 0 else -1
# Handle the case of static geometry (bid < 0)
if bid < 0:
body_transform = wp.transform_identity()
else:
body_transform = wp.transform(*body_poses[bid])
# Retrieve the geometry ID as a float
wid = float(geom.wid)
# Apply world spacing based on world ID
offset_transform = wp.transform(self.world_spacing * wid, wp.quat_identity())
# Combine body and offset transforms
geom_transform = wp.transform_multiply(body_transform, geom.offset)
geom_transform = wp.transform_multiply(offset_transform, geom_transform)
# Choose color based on body ID
color = self.body_colors[geom.body % len(self.body_colors)]
# Shape params are already in Newton convention (half-extents, half-heights)
params = geom.shape.params
# Update the geometry data
self.log_shapes(
name=f"/world_{geom.wid}/body_{geom.body}/{scope}/{geom.gid}-{geom.name}",
geo_type=geom.shape.type,
geo_scale=params,
xforms=wp.array([geom_transform], dtype=wp.transform),
geo_is_solid=geom.shape.is_solid,
colors=color,
geo_src=geom.shape.data,
)
def render_frame(self, stop_recording: bool = False):
# Begin a new frame
self.begin_frame(self.time)
# Extract body poses from the kamino simulator
body_poses = self._simulator.state.q_i.numpy()
# Render each collision geom
for geom in self._geometry:
if geom.shape.type == GeoType.NONE:
continue
self.render_geometry(body_poses, geom, scope="geoms")
# Render contacts if they exist and visualization is enabled
if hasattr(self._simulator, "contacts") and self._simulator.contacts is not None:
self.render_contacts_kamino(self._simulator.contacts)
# End the new frame
self.end_frame()
# Capture frame if recording is enabled and not stopped
if self._record_video and not stop_recording:
# todo : think about if we should continue to step the _img_idx even when not recording
self._capture_frame()
def render_contacts_kamino(self, contacts):
"""
Render contact points, frames, and forces for contacts.
Visualizations include:
- Small oriented boxes showing contact frame by mode
- Force arrows showing contact force magnitude and direction
"""
if not self._show_contacts:
# Hide all contact visualizations
if hasattr(self, "_contact_box_mesh_created"):
self.log_instances("/contact_boxes", "/contact_box_mesh", None, None, None, materials=None, hidden=True)
self.log_lines("/contact_forces", None, None, None)
return
# Get number of active contacts
num_contacts = contacts.model_active_contacts.numpy()[0]
max_contacts = contacts.model_max_contacts_host
if False: # Debug: Always print contact info
print(f"[VIEWER] Frame {getattr(self, '_frame', 0)}: num_contacts={num_contacts} (max={max_contacts})")
# Print all contact slots
modes = contacts.mode.numpy()[:max_contacts]
positions = contacts.position_B.numpy()[:max_contacts]
velocities = contacts.velocity.numpy()[:max_contacts]
reactions = contacts.reaction.numpy()[:max_contacts]
for i in range(max_contacts):
active = "ACTIVE" if i < num_contacts else "STALE"
print(
f" [{active}] Contact[{i}]: mode={modes[i]} (INACTIVE={ContactMode.INACTIVE}), "
f"pos={positions[i]}, vel={velocities[i]}, reaction={reactions[i]}"
)
self._frame = getattr(self, "_frame", 0) + 1
# ======================================================================
# Render Contact Frame Boxes
# ======================================================================
# Allocate buffers for box transforms
if not hasattr(self, "_contact_box_transforms"):
self._contact_box_transforms = wp.zeros(max_contacts, dtype=wp.transform, device=self.device)
self._contact_box_scales = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
self._contact_box_colors = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
# Render boxes as instanced meshes
if not hasattr(self, "_contact_box_mesh_created"):
# Unit box mesh
points, indices_wp = self._create_box_mesh_simple(1.0, 1.0, 1.0)
self.log_mesh(
"/contact_box_mesh",
points,
indices_wp,
normals=None,
hidden=True,
)
self._contact_box_mesh_created = True
# Log instances of the box mesh
if num_contacts > 0:
# small scaled unit box to show frame orientation
box_size = wp.vec3(
0.025, 0.025, 0.025
) # a little bit flat would look better? todo should we have like a viewer config somewhere?
# Compute box transforms, scales, and colors
wp.launch(
kernel=compute_contact_box_transforms,
dim=max_contacts,
inputs=[
contacts.position_A,
contacts.position_B,
contacts.frame,
contacts.mode,
contacts.wid,
num_contacts,
self.world_spacing,
box_size,
],
outputs=[
self._contact_box_transforms,
self._contact_box_scales,
self._contact_box_colors,
],
device=self.device,
)
# Always render all max_contacts instances, not just active ones
# Inactive ones will have zero scale from the kernel
xforms = self._contact_box_transforms
scales = self._contact_box_scales
colors = self._contact_box_colors
self.log_instances(
"/contact_boxes",
"/contact_box_mesh",
xforms,
scales,
colors,
materials=None,
hidden=False,
)
else:
# Hide instances when no contacts
if hasattr(self, "_contact_box_mesh_created"):
self.log_instances(
"/contact_boxes",
"/contact_box_mesh",
None,
None,
None,
materials=None,
hidden=True,
)
# ======================================================================
# Render Contact Force Arrow
# ======================================================================
# Allocate buffers for force arrows
if not hasattr(self, "_contact_force_starts"):
self._contact_force_starts = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
self._contact_force_ends = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
self._contact_force_colors = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
self._contact_force_widths = wp.zeros(max_contacts, dtype=float, device=self.device)
# Compute force arrows
wp.launch(
kernel=compute_contact_force_arrows,
dim=max_contacts,
inputs=[
contacts.position_A,
contacts.position_B,
contacts.frame,
contacts.reaction,
contacts.mode,
contacts.wid,
num_contacts,
self.world_spacing,
0.05, # force_scale # todo move to a cfg file?
1e-4, # force_threshold # todo move to a cfg file?
],
outputs=[
self._contact_force_starts,
self._contact_force_ends,
self._contact_force_colors,
self._contact_force_widths,
],
device=self.device,
)
# Render force arrows as lines
if num_contacts > 0:
self.log_lines(
"/contact_forces",
self._contact_force_starts[:num_contacts],
self._contact_force_ends[:num_contacts],
self._contact_force_colors[:num_contacts],
width=3.0, # todo this assumes we fix the viewer_gl line width issue
)
else:
self.log_lines("/contact_forces", None, None, None)
def set_camera_lookat(self, pos: wp.vec3, target: wp.vec3):
"""
Set the camera position and orient it to face a specific target.
Args:
pos: The camera position.
target: The point the camera should look at.
"""
# Calculate the direction vector from camera to target
dir = wp.normalize(target - pos)
# Calculate camera angles
yaw = wp.degrees(wp.atan2(dir[1], dir[0]))
pitch = wp.degrees(wp.asin(dir[2]))
# Call basic set camera method
self.set_camera(pos, pitch, yaw)
def _create_box_mesh_simple(self, sx, sy, sz):
"""
# todo where should this function go, is it already implemented somewhere else?
Helper to create a simple box mesh for contact visualization using warp.
Returns (vertices, indices) as warp arrays.
"""
# Create vertex array (8 corners of box)
verts = wp.array(
[
wp.vec3(-0.5 * sx, -0.5 * sy, -0.5 * sz), # 0
wp.vec3(0.5 * sx, -0.5 * sy, -0.5 * sz), # 1
wp.vec3(0.5 * sx, 0.5 * sy, -0.5 * sz), # 2
wp.vec3(-0.5 * sx, 0.5 * sy, -0.5 * sz), # 3
wp.vec3(-0.5 * sx, -0.5 * sy, 0.5 * sz), # 4
wp.vec3(0.5 * sx, -0.5 * sy, 0.5 * sz), # 5
wp.vec3(0.5 * sx, 0.5 * sy, 0.5 * sz), # 6
wp.vec3(-0.5 * sx, 0.5 * sy, 0.5 * sz), # 7
],
dtype=wp.vec3,
device=self.device,
)
# Create index array (12 triangles, flattened)
indices = wp.array(
[
# Bottom face
0,
2,
1,
0,
3,
2,
# Top face
4,
5,
6,
4,
6,
7,
# Front face
0,
1,
5,
0,
5,
4,
# Back face
3,
7,
6,
3,
6,
2,
# Left face
0,
4,
7,
0,
7,
3,
# Right face
1,
2,
6,
1,
6,
5,
],
dtype=wp.int32,
device=self.device,
)
return verts, indices
def _capture_frame(self):
"""
Capture and save a single frame from the viewer.
This method retrieves the current rendered frame, converts it to a PIL Image,
and saves it as a PNG file.
"""
# Attempt to import PIL, which is required for image saving
try:
from PIL import Image
except ImportError:
msg.warning("PIL not installed. Frames cannot be saved as images.")
msg.info("Install with: pip install pillow")
return False
# Only capture and save if we've reached the skip threshold
if self._img_idx >= self._skip_img_idx:
# Get frame from viewer as GPU array (height, width, 3) uint8
frame = self.get_frame(target_image=self._frame_buffer)
# Cache buffer for reuse to minimize allocations
if self._frame_buffer is None:
self._frame_buffer = frame
# Convert to numpy on CPU and PIL
frame_np = frame.numpy()
image = Image.fromarray(frame_np, mode="RGB")
# Generate filename with zero-padded frame number # todo : 05d is currently hardcoded
filename = os.path.join(self._video_folder, f"{self._img_idx - self._skip_img_idx:05d}.png")
# Save either asynchronously or synchronously
if self._async_save:
# Use non-daemon thread to save in background
# Each image has its own copy, so thread safety is maintained
threading.Thread(
target=image.save,
args=(filename,),
daemon=False, # make sure the thread completes even if main program exits todo can be challenged
).start()
else:
# Synchronous save - blocks until complete
image.save(filename)
self._img_idx += 1
def generate_video(self, output_filename: str = "recording.mp4", fps: int = 60, keep_frames: bool = True) -> bool:
"""
Generate MP4 video from recorded png frames using imageio-ffmpeg.
Args:
output_filename: Name of output video file (default: "recording.mp4")
fps: Frames per second for video (default: 60)
keep_frames: If True, keep png frames after video creation; if False, delete them (default: True)
"""
# Try to import imageio-ffmpeg (optional dependency)
try:
import imageio_ffmpeg as ffmpeg # noqa: PLC0415
except ImportError:
msg.warning("imageio-ffmpeg not installed. Frames saved but video not generated.")
msg.info("Install with: pip install imageio-ffmpeg")
return False
# Try to import PIL (optional dependency for image loading)
try:
from PIL import Image
except ImportError:
msg.warning("PIL not installed. Frames saved but video not generated.")
msg.info("Install with: pip install pillow")
return False
import numpy as np # noqa: PLC0415
# Check if we have frames to process
if not self._record_video or self._img_idx <= self._skip_img_idx:
msg.warning("No frames recorded, cannot generate video")
return False
# Get sorted list of frame files
frame_files = sorted(glob.glob(os.path.join(self._video_folder, "*.png")))
if not frame_files:
msg.warning(f"No png frames found in {self._video_folder}")
return False
msg.info(f"Generating video from {len(frame_files)} frames...")
try:
# Use imageio-ffmpeg to write video
writer = ffmpeg.write_frames(
output_filename,
size=(self.renderer._screen_width, self.renderer._screen_height),
fps=fps,
codec="libx264",
macro_block_size=8,
quality=5, # set to default quality
)
writer.send(None) # Initialize the writer
# Read each frame and send each frame from and to disk
for frame_path in frame_files:
img = Image.open(frame_path)
frame_array = np.array(img)
writer.send(frame_array)
writer.close()
msg.info(f"Video generated successfully: {output_filename}")
if not keep_frames:
msg.info("Deleting png frames...")
for frame_path in frame_files:
os.remove(frame_path)
msg.info("Frames deleted")
return True
except Exception as e:
msg.warning(f"Failed to generate video: {e}")
return False
================================================
FILE: newton/_src/solvers/kamino/_src/utils/sparse.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: Utilities
"""
import numpy as np
from . import logger as msg
###
# Matrix Sparsity
###
def sparseplot(
matrix: np.ndarray,
title: str = "Matrix Sparsity",
tick_fontsize: int = 5,
max_ticks: int = 20,
grid: bool = False,
path: str | None = None,
):
# Attempt to import matplotlib
try:
import matplotlib.pyplot as plt
from matplotlib.cm import ScalarMappable
from matplotlib.colors import Normalize
except Exception as exc: # pragma: no cover - optional dependency
msg.error(f"`matplotlib` is required to plot profiles: {exc}")
return
"""
Visualize the sparsity pattern of a matrix.
Zero entries are shown in red.
Non-zero entries are shown in grayscale (black to white).
Parameters:
- matrix: 2D numpy array
- title: Title for the plot (used for display or saved image)
- save_path: If provided, saves the image to this file path; otherwise, displays it.
"""
# Check if the input is a 2D NumPy array
if not isinstance(matrix, np.ndarray):
raise ValueError("Input must be a NumPy array.")
# A helper function to compute sparse ticks
def get_sparse_ticks(length, max_ticks):
if length <= max_ticks:
return np.arange(length)
step = max(1, int(np.ceil(length / max_ticks)))
return np.arange(0, length, step)
# Create color image: start with a gray image
color_image = np.zeros((*matrix.shape, 3)) # RGB image
# Normalize non-zero values to 0-1 for grayscale
non_zero_mask = matrix != 0
zero_mask = matrix == 0
if np.any(non_zero_mask):
norm = Normalize(vmin=matrix[non_zero_mask].min(), vmax=matrix[non_zero_mask].max())
gray_values = norm(matrix) # normalized to [0,1]
gray_image = np.stack([gray_values] * 3, axis=-1)
color_image[non_zero_mask] = gray_image[non_zero_mask]
# Set exact zeros to red
color_image[zero_mask] = [1, 0, 0]
# Plot the image
_fig, ax = plt.subplots()
ax.imshow(color_image, origin="upper")
# Configure figure tick labels
xticks = get_sparse_ticks(matrix.shape[1], max_ticks)
yticks = get_sparse_ticks(matrix.shape[0], max_ticks)
ax.set_xticks(xticks)
ax.set_yticks(yticks)
ax.set_xticklabels(xticks, fontsize=tick_fontsize)
ax.set_yticklabels(yticks, fontsize=tick_fontsize)
# Minor ticks for grid alignment (optional)
ax.set_xticks(np.arange(matrix.shape[1] + 1) - 0.5, minor=True)
ax.set_yticks(np.arange(matrix.shape[0] + 1) - 0.5, minor=True)
ax.grid(False)
ax.tick_params(which="minor", size=0)
# Add colorbar only for the non-zero values
if np.any(non_zero_mask):
sm = ScalarMappable(cmap="gray", norm=norm)
sm.set_array([]) # dummy array for colorbar
cbar = plt.colorbar(sm, ax=ax)
cbar.set_label("non-zero values")
# Set title and layout
plt.title(title)
plt.tight_layout()
# Set grid for better visibility
if grid:
plt.grid(True, which="major", color="blue", linestyle="-", linewidth=0.1)
# Save or show the plot
if path:
plt.savefig(path, dpi=300, bbox_inches="tight")
plt.close()
else:
plt.show()
================================================
FILE: newton/_src/solvers/kamino/_src/utils/viewer.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
# Python
import dataclasses
Color3 = tuple[float, float, float]
class MeshColors:
LIGHT = (0.8, 0.8, 0.8)
GREY = (0.45, 0.45, 0.45)
DARK = (0.2, 0.2, 0.2)
BONE = (191 / 255, 190 / 255, 178 / 255)
ORANGE = (206 / 255, 117 / 255, 52 / 255)
RED = (200 / 255, 30 / 255, 30 / 255)
BLUE = (88 / 255, 135 / 255, 171 / 255)
BLUEGREY = (100 / 255, 100 / 255, 130 / 255)
SAGEGREY = (145 / 255, 157 / 255, 132 / 255)
GREEN = (120 / 255, 183.6 / 255, 48 / 255)
YELLOW = (183 / 255, 146 / 255, 76 / 255)
SOFTGREEN = (150 / 255, 200 / 255, 150 / 255)
LIGHTGREEN = (100 / 255, 220 / 255, 100 / 255)
SOFTPINK = (200 / 255, 182 / 255, 203 / 255)
PINK = (200 / 255, 150 / 255, 200 / 255)
@dataclasses.dataclass
class ViewerConfig:
"""Viewer appearance settings for :class:`RigidBodySim`.
All ``None`` defaults leave the standard Newton viewer appearance unchanged.
"""
robot_color: Color3 | None = None
"""Override color for all robot shapes. ``None`` keeps USD material colors."""
diffuse_scale: float | None = None
"""Diffuse light scale (multiplied on top of the base ``* 3.0``). ``None`` keeps default (1.0)."""
specular_scale: float | None = None
"""Specular highlight scale. ``None`` keeps default (1.0)."""
shadow_radius: float | None = None
"""PCF shadow softness radius. Larger = softer edges. ``None`` keeps default (3.0)."""
shadow_extents: float | None = None
"""Shadow map half-size in world units. ``None`` keeps default (10.0)."""
spotlight_enabled: bool | None = None
"""Use cone spotlight (True) or uniform directional light (False). ``None`` keeps default (True)."""
background_brightness_scale: float | None = None
"""Scale factor for ground color and ground plane shape brightness. ``None`` keeps default (1.0)."""
sky_color: Color3 | None = None
"""Override sky color (ambient + background upper). ``None`` keeps default blue."""
light_color: Color3 | None = None
"""Override directional (sun) light color. ``None`` keeps default white ``(1, 1, 1)``."""
render_width: int = 1920
"""Viewer / recording width in pixels."""
render_height: int = 1080
"""Viewer / recording height in pixels."""
================================================
FILE: newton/_src/solvers/kamino/config.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Defines configurations for :class:`SolverKamino`.
"""
from __future__ import annotations
from dataclasses import dataclass, field
from typing import Any, Literal
import warp as wp
from ...core.types import override
from ...sim import Model, ModelBuilder
###
# Module interface
###
__all__ = [
"CollisionDetectorConfig",
"ConfigBase",
"ConstrainedDynamicsConfig",
"ConstraintStabilizationConfig",
"ForwardKinematicsSolverConfig",
"PADMMSolverConfig",
]
###
# Types
###
@dataclass
class ConfigBase:
"""
Defines a base class for configuration containers providing interfaces for
registering custom attributes and parsing configurations from a Newton model.
"""
@staticmethod
def register_custom_attributes(builder: ModelBuilder) -> None:
"""
Registers custom attributes for config type with the given builder.
Args:
builder: The model builder instance with which to register the custom attributes.
"""
pass
@staticmethod
def from_model(model: Model, **kwargs: dict[str, Any]) -> ConfigBase:
"""
Creates a :class:`ConfigBase` by attempting to parse custom attributes from a :class:`Model` if available.
Args:
model: The Newton model from which to parse configurations.
"""
return ConfigBase(**kwargs)
def validate(self) -> None:
"""
Validates the config parameters to ensure they are within acceptable ranges and consistent with each other.
Raises:
ValueError: If any parameter is out of range or if there are inconsistencies between parameters.
TypeError: If any parameter is of an incorrect type.
"""
pass
@dataclass
class CollisionDetectorConfig(ConfigBase):
"""
A container to hold configurations for the internal collision detector used for contact generation.
"""
pipeline: Literal["primitive", "unified"] = "unified"
"""
The type of collision-detection pipeline to use, either `primitive` or `unified`.\n
Defaults to `unified`.
"""
broadphase: Literal["nxn", "sap", "explicit"] = "explicit"
"""
The broad-phase collision-detection to use (`nxn`, `sap`, or `explicit`).\n
Defaults to `explicit`.
"""
bvtype: Literal["aabb", "bs"] = "aabb"
"""
The type of bounding volume to use in the broad-phase.\n
Defaults to `aabb`.
"""
max_contacts: int | None = None
"""
The maximum number of contacts to generate over the entire model.\n
Used to compute the total maximum contacts allocated for the model,
in conjunction with the total number of candidate geom-pairs.\n
Defaults to `DEFAULT_MODEL_MAX_CONTACTS` (`1000`) if unspecified.
"""
max_contacts_per_world: int | None = None
"""
The per-world maximum contacts allocation override.\n
If specified, it will override the per-world maximum number of contacts
computed according to the candidate geom-pairs represented in the model.\n
Defaults to `None`, allowing contact allocations to occur according to the model.
"""
max_contacts_per_pair: int | None = None
"""
The maximum number of contacts to generate per candidate geom-pair.\n
Used to compute the total maximum contacts allocated for the model,
in conjunction with the total number of candidate geom-pairs.\n
Defaults to `DEFAULT_GEOM_PAIR_MAX_CONTACTS` (`12`) if unspecified.
"""
max_triangle_pairs: int | None = None
"""
The maximum number of triangle-primitive shape pairs to consider in the narrow-phase.\n
Used only when the model contains triangle meshes or heightfields.\n
Defaults to `DEFAULT_TRIANGLE_MAX_PAIRS` (`1_000_000`) if unspecified.
"""
default_gap: float | None = None
"""
The default detection gap [m] applied as a floor to per-geometry gaps.\n
Defaults to `DEFAULT_GEOM_PAIR_CONTACT_GAP` (`0.0`) if unspecified.
"""
@override
@staticmethod
def register_custom_attributes(builder: ModelBuilder) -> None:
"""
Registers custom attributes for the CollisionDetector solver config with the given builder.
Note: Currently, this class does not have any custom attributes registered,
as only those supported by the Kamino USD scene API have been included. More
will be added in the future as latter is being developed.
Args:
builder: The model builder instance with which to register the custom attributes.
"""
pass # TODO: Add custom attributes for the CD when supported by the Kamino USD scene API
@override
@staticmethod
def from_model(model: Model, **kwargs: dict[str, Any]) -> CollisionDetectorConfig:
"""
Creates a :class:`CollisionDetectorConfig` by attempting to
parse custom attributes from a :class:`Model` if available.
Args:
model: The Newton model from which to parse configurations.
"""
cfg = CollisionDetectorConfig(**kwargs)
# TODO: Implement these
# Return the fully constructed config with configurations
# parsed from the model's custom attributes if available,
# otherwise using defaults or provided kwargs.
return cfg
@override
def validate(self) -> None:
"""
Validates the current values held by the :class:`CollisionDetectorConfig` instance.
"""
# Import here to avoid module-level imports and circular dependencies
from ._src.geometry import BoundingVolumeType, BroadPhaseType, CollisionPipelineType # noqa: PLC0415
from ._src.geometry.contacts import ( # noqa: PLC0415
DEFAULT_GEOM_PAIR_CONTACT_GAP,
DEFAULT_GEOM_PAIR_MAX_CONTACTS,
DEFAULT_MODEL_MAX_CONTACTS,
DEFAULT_TRIANGLE_MAX_PAIRS,
)
# Check that the string literals provided correspond to supported enum types, and raise an error if not
pipelines_supported = [e.name.lower() for e in CollisionPipelineType]
if self.pipeline not in pipelines_supported:
raise ValueError(f"Invalid CD pipeline type: {self.pipeline}. Valid options are: {pipelines_supported}")
broadphases_supported = [e.name.lower() for e in BroadPhaseType]
if self.broadphase not in broadphases_supported:
raise ValueError(
f"Invalid CD broad-phase type: {self.broadphase}. Valid options are: {broadphases_supported}"
)
bvtypes_supported = [e.name.lower() for e in BoundingVolumeType]
if self.bvtype not in bvtypes_supported:
raise ValueError(f"Invalid CD bounding-volume type: {self.bvtype}. Valid options are: {bvtypes_supported}")
# Ensure that max_contacts, if specified, is non-negative
if self.max_contacts is not None and self.max_contacts < 0:
raise ValueError(f"Invalid max_contacts: {self.max_contacts}. Must be non-negative.")
if self.max_contacts_per_world is not None and self.max_contacts_per_world < 0:
raise ValueError(f"Invalid max_contacts_per_world: {self.max_contacts_per_world}. Must be non-negative.")
if self.max_contacts_per_pair is not None and self.max_contacts_per_pair < 0:
raise ValueError(f"Invalid max_contacts_per_pair: {self.max_contacts_per_pair}. Must be non-negative.")
if self.max_triangle_pairs is not None and self.max_triangle_pairs < 0:
raise ValueError(f"Invalid max_triangle_pairs: {self.max_triangle_pairs}. Must be non-negative.")
# Check if optional arguments are specified and override with defaults if not
if self.max_contacts is None:
self.max_contacts = DEFAULT_MODEL_MAX_CONTACTS
if self.max_contacts_per_pair is None:
self.max_contacts_per_pair = DEFAULT_GEOM_PAIR_MAX_CONTACTS
if self.max_triangle_pairs is None:
self.max_triangle_pairs = DEFAULT_TRIANGLE_MAX_PAIRS
if self.default_gap is None:
self.default_gap = DEFAULT_GEOM_PAIR_CONTACT_GAP
@override
def __post_init__(self):
"""Post-initialization to validate configurations."""
self.validate()
@dataclass
class ConstraintStabilizationConfig(ConfigBase):
"""
A container to hold configurations for global constraint stabilization parameters.
These parameters serve as global defaults/overrides, to be used
in combination with the per-constraint stabilization parameters
specified in the model, if the latter are provided.
"""
alpha: float = 0.01
"""
Global default Baumgarte stabilization parameter for bilateral joint constraints.\n
Must be in range `[0, 1.0]`.\n
Defaults to `0.01`.
"""
beta: float = 0.01
"""
Global default Baumgarte stabilization parameter for unilateral joint-limit constraints.\n
Must be in range `[0, 1.0]`.\n
Defaults to `0.01`.
"""
gamma: float = 0.01
"""
Global default Baumgarte stabilization parameter for unilateral contact constraints.\n
Must be in range `[0, 1.0]`.\n
Defaults to `0.01`.
"""
delta: float = 1.0e-6
"""
Contact penetration margin used for unilateral contact constraints.\n
Must be non-negative.\n
Defaults to `1.0e-6`.
"""
@override
@staticmethod
def register_custom_attributes(builder: ModelBuilder) -> None:
"""
Registers custom attributes for this config with the given builder.
Note: Currently, not all configurations are registered as custom attributes,
as only those supported by the Kamino USD scene API have been included. More
will be added in the future as latter is being developed.
Args:
builder: The model builder instance with which to register the custom attributes.
"""
# Create a default instance of the config to access default values for the attributes
default_cfg = ConstraintStabilizationConfig()
# Register KaminoSceneAPI attributes so the USD importer will store them on the model
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="constraints_alpha",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=default_cfg.alpha,
namespace="kamino",
usd_attribute_name="newton:kamino:constraints:alpha",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="constraints_beta",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=default_cfg.beta,
namespace="kamino",
usd_attribute_name="newton:kamino:constraints:beta",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="constraints_gamma",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=default_cfg.gamma,
namespace="kamino",
usd_attribute_name="newton:kamino:constraints:gamma",
)
)
@override
@staticmethod
def from_model(model: Model, **kwargs: dict[str, Any]) -> ConstraintStabilizationConfig:
"""
Creates a :class:`ConstraintStabilizationConfig` by attempting
to parse custom attributes from a :class:`Model` if available.
Args:
model: The Newton model from which to parse configurations.
"""
cfg = ConstraintStabilizationConfig(**kwargs)
# Parse solver-specific attributes imported from USD
kamino_attrs = getattr(model, "kamino", None)
if kamino_attrs is not None:
if hasattr(kamino_attrs, "constraints_alpha"):
cfg.alpha = float(kamino_attrs.constraints_alpha.numpy()[0])
if hasattr(kamino_attrs, "constraints_beta"):
cfg.beta = float(kamino_attrs.constraints_beta.numpy()[0])
if hasattr(kamino_attrs, "constraints_gamma"):
cfg.gamma = float(kamino_attrs.constraints_gamma.numpy()[0])
# Return the fully constructed config with configurations
# parsed from the model's custom attributes if available,
# otherwise using defaults or provided kwargs.
return cfg
@override
def validate(self) -> None:
"""
Validates the current values held by the :class:`ConstraintStabilizationConfig` instance.
"""
if self.alpha < 0.0 or self.alpha > 1.0:
raise ValueError(f"Invalid alpha: {self.alpha}. Must be in range [0, 1.0].")
if self.beta < 0.0 or self.beta > 1.0:
raise ValueError(f"Invalid beta: {self.beta}. Must be in range [0, 1.0].")
if self.gamma < 0.0 or self.gamma > 1.0:
raise ValueError(f"Invalid gamma: {self.gamma}. Must be in range [0, 1.0].")
if self.delta < 0.0:
raise ValueError(f"Invalid delta: {self.delta}. Must be non-negative.")
@override
def __post_init__(self):
"""Post-initialization to validate configurations."""
self.validate()
@dataclass
class ConstrainedDynamicsConfig(ConfigBase):
"""
A container to hold configurations for the construction of the constrained forward dynamics problem.
"""
preconditioning: bool = True
"""
Set to `True` to enable preconditioning of the dual problem.\n
Defaults to `True`.
"""
linear_solver_type: Literal["LLTB", "CR"] = "LLTB"
"""
The type of linear solver to use for the dynamics problem.\n
See :class:`LinearSolverType` for available options.\n
Defaults to 'LLTB', which will use the :class:`LLTBlockedSolver`.
"""
linear_solver_kwargs: dict[str, Any] = field(default_factory=dict)
"""
Additional keyword arguments to pass to the linear solver.\n
Defaults to an empty dictionary.
"""
@override
@staticmethod
def register_custom_attributes(builder: ModelBuilder) -> None:
"""
Registers custom attributes for the constrained dynamics problem configurations with the given builder.
Note: Currently, not all configurations are registered as custom attributes,
as only those supported by the Kamino USD scene API have been included. More
will be added in the future as latter is being developed.
Args:
builder: The model builder instance with which to register the custom attributes.
"""
# Register KaminoSceneAPI attributes so the USD importer will store them on the model
# TODO: Rename `name` to this to "dynamics_preconditioning" or similar
# TODO: Rename `usd_attribute_name` to "newton:kamino:usePreconditioning" or similar
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="constraints_use_preconditioning",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.bool,
default=True,
namespace="kamino",
usd_attribute_name="newton:kamino:constraints:usePreconditioning",
)
)
@override
@staticmethod
def from_model(model: Model, **kwargs: dict[str, Any]) -> ConstrainedDynamicsConfig:
"""
Creates a :class:`ConstrainedDynamicsConfig` by attempting to
parse custom attributes from a :class:`Model` if available.
Args:
model: The Newton model from which to parse configurations.
"""
cfg = ConstrainedDynamicsConfig(**kwargs)
# Parse solver-specific attributes imported from USD
kamino_attrs = getattr(model, "kamino", None)
if kamino_attrs is not None:
if hasattr(kamino_attrs, "constraints_use_preconditioning"):
cfg.preconditioning = bool(kamino_attrs.constraints_use_preconditioning.numpy()[0])
# Return the fully constructed config with configurations
# parsed from the model's custom attributes if available,
# otherwise using defaults or provided kwargs.
return cfg
@override
def validate(self) -> None:
"""
Validates the current values held by the :class:`ConstrainedDynamicsConfig` instance.
"""
# Import here to avoid module-level imports and circular dependencies
from ._src.linalg import LinearSolverNameToType # noqa: PLC0415
# Ensure that the linear solver type is a valid option
supported_linear_solver_types = LinearSolverNameToType.keys()
if self.linear_solver_type not in supported_linear_solver_types:
raise ValueError(
f"Invalid linear_solver_type: {self.linear_solver_type}. "
f"Must be one of {supported_linear_solver_types}."
)
@override
def __post_init__(self):
"""Post-initialization to validate configurations."""
self.validate()
@dataclass
class PADMMSolverConfig:
"""
A container to hold configurations for the PADMM forward dynamics solver.
"""
primal_tolerance: float = 1e-6
"""
The target tolerance on the total primal residual `r_primal`.\n
Must be greater than zero. Defaults to `1e-6`.
"""
dual_tolerance: float = 1e-6
"""
The target tolerance on the total dual residual `r_dual`.\n
Must be greater than zero. Defaults to `1e-6`.
"""
compl_tolerance: float = 1e-6
"""
The target tolerance on the total complementarity residual `r_compl`.\n
Must be greater than zero. Defaults to `1e-6`.
"""
restart_tolerance: float = 0.999
"""
The tolerance on the total combined primal-dual residual `r_comb`,
for determining when gradient acceleration should be restarted.\n
Must be greater than zero. Defaults to `0.999`.
"""
eta: float = 1e-5
"""
The proximal regularization parameter.\n
Must be greater than zero. Defaults to `1e-5`.
"""
rho_0: float = 1.0
"""
The initial value of the ALM penalty parameter.\n
Must be greater than zero. Defaults to `1.0`.
"""
rho_min: float = 1e-5
"""
The lower-bound applied to the ALM penalty parameter.\n
Used to ensure numerical stability when adaptive penalty updates are used.\n
Must be greater than zero. Defaults to `1e-5`.
"""
a_0: float = 1.0
"""
The initial value of the acceleration parameter.\n
Must be greater than zero. Defaults to `1.0`.
"""
alpha: float = 10.0
"""
The primal-dual residual threshold used to determine when penalty updates are needed.
Must be greater than one. Defaults to `10.0`.
"""
tau: float = 1.5
"""
The factor by which the ALM penalty is increased/decreased when
the primal-dual residual ratios exceed the threshold `alpha`.\n
Must be greater than `1.0`. Defaults to `1.5`.
"""
max_iterations: int = 200
"""
The maximum number of solver iterations.\n
Must be greater than zero. Defaults to `200`.
"""
penalty_update_freq: int = 1
"""
The permitted frequency of penalty updates.\n
If zero, no updates are performed. Otherwise, updates are performed every
`penalty_update_freq` iterations. Defaults to `1`.
"""
penalty_update_method: Literal["fixed", "balanced"] = "fixed"
"""
The penalty update method used to adapt the penalty parameter.\n
Defaults to `fixed`. See :class:`PADMMPenaltyUpdate` for details.
"""
linear_solver_tolerance: float = 0.0
"""
The default absolute tolerance for the iterative linear solver.\n
When zero, the iterative solver's own tolerance is left unchanged.\n
When positive, the iterative solver's atol is initialized
to this value at the start of each ADMM solve.\n
Must be non-negative. Defaults to `0.0`.
"""
linear_solver_tolerance_ratio: float = 0.0
"""
The ratio used to adapt the iterative linear solver tolerance from the ADMM primal residual.\n
When zero, the linear solver tolerance is not adapted (fixed tolerance).\n
When positive, the linear solver absolute tolerance is
set to `ratio * ||r_primal||_2` at each ADMM iteration.\n
Must be non-negative. Defaults to `0.0`.
"""
use_acceleration: bool = True
"""
Enables Nesterov-type acceleration, i.e. use APADMM instead of standard PADMM.\n
Defaults to `True`.
"""
use_graph_conditionals: bool = True
"""
Enables use of CUDA graph conditional nodes in iterative solvers.\n
If `False`, replaces `wp.capture_while` with unrolled for-loops over max iterations.\n
Defaults to `True`.
"""
warmstart_mode: Literal["none", "internal", "containers"] = "containers"
"""
Warmstart mode to be used for the dynamics solver.\n
See :class:`PADMMWarmStartMode` for the available options.\n
Defaults to `containers` to warmstart from the solver data containers.
"""
contact_warmstart_method: Literal[
"key_and_position",
"geom_pair_net_force",
"geom_pair_net_wrench",
"key_and_position_with_net_force_backup",
"key_and_position_with_net_wrench_backup",
] = "key_and_position"
"""
Method to be used for warm-starting contacts.\n
See :class:`WarmstarterContacts.Method` for available options.\n
Defaults to `key_and_position`.
"""
@override
@staticmethod
def register_custom_attributes(builder: ModelBuilder) -> None:
"""
Registers custom attributes for the PADMM solver configurations with the given builder.
Note: Currently, not all configurations are registered as custom attributes,
as only those supported by the Kamino USD scene API have been included. More
will be added in the future as latter is being developed.
Args:
builder: The model builder instance with which to register the custom attributes.
"""
# Import here to avoid module-level imports and circular dependencies
from ._src.solvers.padmm import PADMMWarmStartMode # noqa: PLC0415
# Separately register `newton:maxSolverIterations` from
# `KaminoSceneAPI` so we have access to it through the model.
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="max_solver_iterations",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.int32,
default=-1,
namespace="kamino",
usd_attribute_name="newton:maxSolverIterations",
)
)
# Register KaminoSceneAPI attributes so the USD importer will store them on the model
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="padmm_primal_tolerance",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=1e-6,
namespace="kamino",
usd_attribute_name="newton:kamino:padmm:primalTolerance",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="padmm_dual_tolerance",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=1e-6,
namespace="kamino",
usd_attribute_name="newton:kamino:padmm:dualTolerance",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="padmm_complementarity_tolerance",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=1e-6,
namespace="kamino",
usd_attribute_name="newton:kamino:padmm:complementarityTolerance",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="padmm_use_acceleration",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.bool,
default=True,
namespace="kamino",
usd_attribute_name="newton:kamino:padmm:useAcceleration",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="padmm_warmstarting",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=str,
default="containers",
namespace="kamino",
usd_attribute_name="newton:kamino:padmm:warmstarting",
usd_value_transformer=PADMMWarmStartMode.parse_usd_attribute,
)
)
@override
@staticmethod
def from_model(model: Model, **kwargs: dict[str, Any]) -> PADMMSolverConfig:
"""
Creates a :class:`PADMMSolverConfig` by attempting to
parse custom attributes from a :class:`Model` if available.
Args:
model: The Newton model from which to parse configurations.
"""
cfg = PADMMSolverConfig(**kwargs)
# Parse solver-specific attributes imported from USD
kamino_attrs = getattr(model, "kamino", None)
if kamino_attrs is not None:
if hasattr(kamino_attrs, "max_solver_iterations"):
max_iterations = kamino_attrs.max_solver_iterations.numpy()[0]
if max_iterations >= 0:
cfg.max_iterations = max_iterations
if hasattr(kamino_attrs, "padmm_primal_tolerance"):
cfg.primal_tolerance = float(kamino_attrs.padmm_primal_tolerance.numpy()[0])
if hasattr(kamino_attrs, "padmm_dual_tolerance"):
cfg.dual_tolerance = float(kamino_attrs.padmm_dual_tolerance.numpy()[0])
if hasattr(kamino_attrs, "padmm_complementarity_tolerance"):
cfg.compl_tolerance = float(kamino_attrs.padmm_complementarity_tolerance.numpy()[0])
if hasattr(kamino_attrs, "padmm_warmstarting"):
cfg.warmstart_mode = kamino_attrs.padmm_warmstarting[0]
if hasattr(kamino_attrs, "padmm_use_acceleration"):
cfg.use_acceleration = bool(kamino_attrs.padmm_use_acceleration.numpy()[0])
# Return the fully constructed config with configurations
# parsed from the model's custom attributes if available,
# otherwise using defaults or provided kwargs.
return cfg
@override
def validate(self) -> None:
"""
Validates the current values held by the :class:`PADMMSolverConfig` instance.
"""
# Import here to avoid module-level imports and circular dependencies
from ._src.solvers.padmm import PADMMPenaltyUpdate, PADMMWarmStartMode # noqa: PLC0415
from ._src.solvers.warmstart import WarmstarterContacts # noqa: PLC0415
# Ensure that the scalar parameters are within valid ranges
if self.primal_tolerance < 0.0:
raise ValueError(f"Invalid primal tolerance: {self.primal_tolerance}. Must be non-negative.")
if self.dual_tolerance < 0.0:
raise ValueError(f"Invalid dual tolerance: {self.dual_tolerance}. Must be non-negative.")
if self.compl_tolerance < 0.0:
raise ValueError(f"Invalid complementarity tolerance: {self.compl_tolerance}. Must be non-negative.")
if not (0.0 <= self.restart_tolerance < 1.0):
raise ValueError(f"Invalid restart tolerance: {self.restart_tolerance}. Must be in the range [0.0, 1.0).")
if self.eta <= 0.0:
raise ValueError(f"Invalid proximal parameter: {self.eta}. Must be greater than zero.")
if self.rho_0 <= 0.0:
raise ValueError(f"Invalid initial ALM penalty: {self.rho_0}. Must be greater than zero.")
if self.rho_min <= 0.0:
raise ValueError(f"Invalid minimum ALM penalty: {self.rho_min}. Must be greater than zero.")
if self.a_0 <= 0.0:
raise ValueError(f"Invalid initial acceleration parameter: {self.a_0}. Must be greater than zero.")
if self.alpha <= 1.0:
raise ValueError(f"Invalid penalty threshold: {self.alpha}. Must be greater than one.")
if self.tau <= 1.0:
raise ValueError(f"Invalid penalty increment factor: {self.tau}. Must be greater than one.")
if self.max_iterations <= 0:
raise ValueError(f"Invalid maximum iterations: {self.max_iterations}. Must be a positive integer.")
if self.penalty_update_freq < 0:
raise ValueError(f"Invalid penalty update frequency: {self.penalty_update_freq}. Must be non-negative.")
if self.linear_solver_tolerance < 0.0:
raise ValueError(f"Invalid linear solver tolerance: {self.linear_solver_tolerance}. Must be non-negative.")
if self.linear_solver_tolerance_ratio < 0.0:
raise ValueError(
f"Invalid linear solver tolerance ratio: {self.linear_solver_tolerance_ratio}. Must be non-negative."
)
# Ensure that the enum-valued parameters are valid options
# Conversion to enum-type configs will raise an error
# if the corresponding input string is invalid.
PADMMPenaltyUpdate.from_string(self.penalty_update_method)
PADMMWarmStartMode.from_string(self.warmstart_mode)
WarmstarterContacts.Method.from_string(self.contact_warmstart_method)
@override
def __post_init__(self):
"""Post-initialization to validate configurations."""
self.validate()
@dataclass
class ForwardKinematicsSolverConfig:
"""
A container to hold configurations for the Gauss-Newton forward kinematics solver used for state resets.
"""
preconditioner: Literal["none", "jacobi_diagonal", "jacobi_block_diagonal"] = "jacobi_block_diagonal"
"""
Preconditioner to use for the Conjugate Gradient solver if sparsity is enabled
Changing this setting after the solver's initialization leads to undefined behavior.
Defaults to `jacobi_block_diagonal`.
"""
max_newton_iterations: int = 30
"""
Maximal number of Gauss-Newton iterations.
Changes to this setting after the solver's initialization will have no effect.
Defaults to `30`.
"""
max_line_search_iterations: int = 20
"""
Maximal line search iterations in the inner loop.
Changes to this setting after the solver's initialization will have no effect.
Defaults to `20`.
"""
tolerance: float = 1e-6
"""
Maximal absolute kinematic constraint value that is acceptable at the solution.
Changes to this setting after the solver's initialization will have no effect.
Defaults to `1e-6`.
"""
TILE_SIZE_CTS: int = 8
"""
Tile size for kernels along the dimension of kinematic constraints.
Changes to this setting after the solver's initialization will have no effect.
Defaults to `8`.
"""
TILE_SIZE_VRS: int = 8
"""
Tile size for kernels along the dimension of rigid body pose variables.
Changes to this setting after the solver's initialization will have no effect.
Defaults to `8`.
"""
use_sparsity: bool = False
"""
Whether to use sparse Jacobian and solver; otherwise, dense versions are used.
Changes to this setting after the solver's initialization lead to undefined behavior.
Defaults to `False`.
"""
use_adaptive_cg_tolerance: bool = True
"""
Whether to use an adaptive tolerance strategy for the Conjugate Gradient solver if sparsity
is enabled, which reduces the number of CG iterations in most cases.
Changes to this setting after graph capture will have no effect.
Defaults to `True`.
"""
reset_state: bool = True
"""
Whether to reset the state to initial states, to use as initial guess.
Changes to this setting after graph capture will have no effect.
Defaults to `True`.
"""
@override
@staticmethod
def register_custom_attributes(builder: ModelBuilder) -> None:
"""
Registers custom attributes for the FK solver configurations with the given builder.
Note: Currently, this class does not have any custom attributes registered,
as only those supported by the Kamino USD scene API have been included. More
will be added in the future as latter is being developed.
Args:
builder: The model builder instance with which to register the custom attributes.
"""
pass # TODO: Add custom attributes for the FK solver when supported by the Kamino USD scene API
@override
@staticmethod
def from_model(model: Model, **kwargs: dict[str, Any]) -> ForwardKinematicsSolverConfig:
"""
Creates a :class:`ForwardKinematicsSolverConfig` by attempting
to parse custom attributes from a :class:`Model` if available.
Args:
model: The Newton model from which to parse configurations.
"""
cfg = ForwardKinematicsSolverConfig(**kwargs)
# TODO: Implement these
# Return the fully constructed config with configurations
# parsed from the model's custom attributes if available,
# otherwise using defaults or provided kwargs.
return cfg
@override
def validate(self) -> None:
"""
Validates the current values held by the :class:`ForwardKinematicsSolverConfig` instance.
"""
# Import here to avoid module-level imports and circular dependencies
from ._src.solvers.fk import ForwardKinematicsSolver # noqa: PLC0415
# Ensure that the enum-valued parameters are valid options
ForwardKinematicsSolver.PreconditionerType.from_string(self.preconditioner)
# Ensure that the integer and float parameters are within valid ranges
if self.max_newton_iterations <= 0:
raise ValueError("`max_newton_iterations` must be positive.")
if self.max_line_search_iterations <= 0:
raise ValueError("`max_line_search_iterations` must be positive.")
if self.tolerance <= 0.0:
raise ValueError("`tolerance` must be positive.")
if self.TILE_SIZE_CTS <= 0:
raise ValueError("`TILE_SIZE_CTS` must be positive.")
if self.TILE_SIZE_VRS <= 0:
raise ValueError("`TILE_SIZE_VRS` must be positive.")
@override
def __post_init__(self):
"""Post-initialization to validate configurations."""
self.validate()
================================================
FILE: newton/_src/solvers/kamino/examples/.gitignore
================================================
data/
output/
================================================
FILE: newton/_src/solvers/kamino/examples/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import os
import sys
import time
import warp as wp
from .._src.utils import logger as msg
###
# Example Paths
###
def get_examples_output_path() -> str:
path = os.path.join(os.path.dirname(os.path.realpath(__file__)), "output")
if not os.path.exists(path):
os.makedirs(path)
return path
###
# Utilities
###
def run_headless(example, progress: bool = True):
"""Run the simulation in headless mode for a fixed number of steps."""
msg.notif(f"Running for {example.max_steps} steps...")
start_time = time.time()
for i in range(example.max_steps):
example.step_once()
wp.synchronize()
if progress:
print_progress_bar(i + 1, example.max_steps, start_time, prefix="Progress", suffix="")
msg.notif("Finished headless run")
def print_progress_bar(iteration, total, start_time, length=40, prefix="", suffix=""):
"""
Display a progress bar with ETA and estimated FPS.
Args:
iteration (int) : Current iteration
total (int) : Total iterations
start_time (float) : Start time from time.time()
length (int) : Character length of the bar
prefix (str) : Prefix string
suffix (str) : Suffix string
"""
elapsed = time.time() - start_time
progress = iteration / total
filled_length = int(length * progress)
if sys.stdout.encoding == "cp1252": # Fix for Windows terminal
bar = "x" * filled_length + "-" * (length - filled_length)
else:
bar = "█" * filled_length + "-" * (length - filled_length)
# Estimated Time of Arrival
if iteration > 0 and elapsed > 0:
eta = elapsed / iteration * (total - iteration)
eta_str = time.strftime("%H:%M:%S", time.gmtime(eta))
fps = iteration / elapsed
fps_str = f"{fps:.2f} fps"
else:
eta_str = "Calculating..."
fps_str = "-- fps"
line_reset = " " * 120
sys.stdout.write(f"\r{line_reset}")
sys.stdout.write(f"\r{prefix} |{bar}| {iteration}/{total} ETA: {eta_str} ({fps_str}) {suffix}")
sys.stdout.flush()
if iteration == total:
sys.stdout.write("\n")
================================================
FILE: newton/_src/solvers/kamino/examples/newton/example_anymal_d.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# Example Robot Anymal D
#
# Shows how to simulate Anymal D with multiple worlds using SolverKamino.
#
# Command: python -m newton.examples robot_anymal_d --num-worlds 16
#
###########################################################################
import warp as wp
import newton
import newton.examples
class Example:
def __init__(self, viewer, num_worlds=8, args=None):
# Set simulation run-time configurations
self.fps = 60
self.sim_dt = 0.0025
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.sim_time = 0.0
self.num_worlds = num_worlds
self.viewer = viewer
self.device = wp.get_device()
# Create a single-robot model builder and register the Kamino-specific custom attributes
robot_builder = newton.ModelBuilder(up_axis=newton.Axis.Z)
newton.solvers.SolverKamino.register_custom_attributes(robot_builder)
robot_builder.default_shape_cfg.margin = 0.0
robot_builder.default_shape_cfg.gap = 0.0
# Load the Anymal D USD and add it to the builder
asset_path = newton.utils.download_asset("anybotics_anymal_d")
asset_file = str(asset_path / "usd" / "anymal_d.usda")
robot_builder.add_usd(
asset_file,
force_position_velocity_actuation=True,
collapse_fixed_joints=True,
enable_self_collisions=True,
hide_collision_shapes=True,
)
# Create the multi-world model by duplicating the single-robot
# builder for the specified number of worlds
builder = newton.ModelBuilder(up_axis=newton.Axis.Z)
for _ in range(self.num_worlds):
builder.add_world(robot_builder)
# Add a global ground plane applied to all worlds
builder.add_ground_plane()
# Create the model from the builder
self.model = builder.finalize(skip_validation_joints=True)
# Create the Kamino solver for the given model
self.solver = newton.solvers.SolverKamino(self.model)
# Create state, control, and contacts data containers
self.state_0 = self.model.state()
self.state_1 = self.model.state()
self.control = self.model.control()
self.contacts = self.model.contacts()
# Use Newton's collision pipeline (same as standard Newton examples)
self.collision_pipeline = newton.examples.create_collision_pipeline(self.model, args)
self.contacts = self.model.collide(self.state_0, collision_pipeline=self.collision_pipeline)
# Reset the simulation state to a valid initial configuration above the ground
self.base_q = wp.zeros(shape=(self.num_worlds,), dtype=wp.transformf)
q_b = wp.quat_identity(dtype=wp.float32)
q_base = wp.transformf((0.0, 0.0, 1.0), q_b)
self.base_q.assign([q_base] * self.num_worlds)
self.solver.reset(state_out=self.state_0, base_q=self.base_q)
# Attach the model to the viewer for visualization
self.viewer.set_model(self.model)
# Capture the simulation graph if running on CUDA
# NOTE: This only has an effect on GPU devices
self.capture()
def capture(self):
self.graph = None
if self.device.is_cuda:
with wp.ScopedCapture() as capture:
self.simulate()
self.graph = capture.graph
# simulate() performs one frame's worth of updates
def simulate(self):
for _ in range(self.sim_substeps):
self.state_0.clear_forces()
self.viewer.apply_forces(self.state_0)
self.contacts = self.model.collide(self.state_0, collision_pipeline=self.collision_pipeline)
self.solver.step(self.state_0, self.state_1, self.control, self.contacts, self.sim_dt)
self.state_0, self.state_1 = self.state_1, self.state_0
def step(self):
if self.graph:
wp.capture_launch(self.graph)
else:
self.simulate()
self.sim_time += self.frame_dt
def render(self):
self.viewer.begin_frame(self.sim_time)
self.viewer.log_state(self.state_0)
self.viewer.log_contacts(self.contacts, self.state_0)
self.viewer.end_frame()
def test_final(self):
newton.examples.test_body_state(
self.model,
self.state_0,
"all bodies are above the ground",
lambda q, qd: q[2] > -0.006,
)
# Only check velocities on CUDA where we run 500 frames (enough time to settle)
# On CPU we only run 10 frames and the robot is still falling (~0.65 m/s)
if self.device.is_cuda:
newton.examples.test_body_state(
self.model,
self.state_0,
"body velocities are small",
lambda q, qd: max(abs(qd))
< 0.25, # Relaxed from 0.1 - unified pipeline has residual velocities up to ~0.2
)
if __name__ == "__main__":
parser = newton.examples.create_parser()
parser.add_argument("--num-worlds", type=int, default=1, help="Total number of simulated worlds.")
viewer, args = newton.examples.init(parser)
example = Example(viewer, args.num_worlds, args)
example.viewer._paused = True # Start paused to inspect the initial configuration
# If only a single-world is created, set initial
# camera position for better view of the system
if args.num_worlds == 1 and hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(5.0, 0.0, 2.0)
pitch = -15.0
yaw = -180.0
example.viewer.set_camera(camera_pos, pitch, yaw)
newton.examples.run(example, args)
================================================
FILE: newton/_src/solvers/kamino/examples/newton/example_dr_legs.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# Example Robot DR Legs
#
# Shows how to simulate DR Legs with multiple worlds using SolverKamino.
#
# Command: python -m newton.examples robot_dr_legs --num-worlds 16
#
###########################################################################
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.utils import logger as msg
class Example:
def __init__(self, viewer, num_worlds=8, args=None):
# Set simulation run-time configurations
self.fps = 60
self.sim_dt = 0.01
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.sim_time = 0.0
self.num_worlds = num_worlds
self.viewer = viewer
self.device = wp.get_device()
# Create a single-robot model builder and register the Kamino-specific custom attributes
robot_builder = newton.ModelBuilder(up_axis=newton.Axis.Z)
newton.solvers.SolverKamino.register_custom_attributes(robot_builder)
robot_builder.default_shape_cfg.margin = 1e-6
robot_builder.default_shape_cfg.gap = 0.01
# Load the DR Legs USD and add it to the builder
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_legs/usd" / "dr_legs_with_meshes_and_boxes.usda")
robot_builder.add_usd(
asset_file,
joint_ordering=None,
force_show_colliders=True,
force_position_velocity_actuation=True,
collapse_fixed_joints=False, # TODO @cavemor: Fails when True, investigate (doesn't have fixed joints)
enable_self_collisions=False,
hide_collision_shapes=True,
)
# Create the multi-world model by duplicating the single-robot
# builder for the specified number of worlds
builder = newton.ModelBuilder(up_axis=newton.Axis.Z)
for _ in range(self.num_worlds):
builder.add_world(robot_builder)
# Add a global ground plane applied to all worlds
builder.add_ground_plane()
# Create the model from the builder
self.model = builder.finalize(skip_validation_joints=True)
# TODO @nvtw: This is a temporary fix because `robot_builder.default_shape_cfg`
# is not correctly applied to the shapes when using `add_usd()`,
msg.debug("self.model.shape_margin: %s", self.model.shape_margin)
msg.debug("self.model.shape_gap: %s", self.model.shape_gap)
self.model.shape_margin.fill_(1e-6)
self.model.shape_gap.fill_(0.01)
# Create the Kamino solver for the given model
self.config = newton.solvers.SolverKamino.Config.from_model(self.model)
self.config.use_collision_detector = True
self.config.use_fk_solver = True
self.config.padmm.max_iterations = 200
self.config.padmm.primal_tolerance = 1e-4
self.config.padmm.dual_tolerance = 1e-4
self.config.padmm.compl_tolerance = 1e-4
self.solver = newton.solvers.SolverKamino(self.model, config=self.config)
# Set joint armature and viscous damping for better
# stability of the implicit joint-space PD controller
# TODO: Remove this once we add Newton USD schemas in the model asset
self.solver._solver_kamino._model.joints.a_j.fill_(0.011) # Joint armature
self.solver._solver_kamino._model.joints.b_j.fill_(0.044) # Joint viscous damping
self.solver._solver_kamino._model.joints.k_p_j.fill_(10.0) # Proportional gain
self.solver._solver_kamino._model.joints.k_d_j.fill_(2.0) # Derivative gain
# Create state and control data containers
self.state_0 = self.model.state()
self.state_1 = self.model.state()
self.control = self.model.control()
self.contacts = self.model.contacts()
# Reset the simulation state to a valid initial configuration above the ground
self.base_q = wp.zeros(shape=(self.num_worlds,), dtype=wp.transformf)
q_b = wp.quat_identity(dtype=wp.float32)
q_base = wp.transformf((0.0, 0.0, 0.4), q_b)
self.base_q.assign([q_base] * self.num_worlds)
self.solver.reset(state_out=self.state_0, base_q=self.base_q)
# Attach the model to the viewer for visualization
self.viewer.set_model(self.model)
# Capture the simulation graph if running on CUDA
# NOTE: This only has an effect on GPU devices
self.capture()
def capture(self):
self.graph = None
if self.device.is_cuda:
with wp.ScopedCapture() as capture:
self.simulate()
self.graph = capture.graph
# simulate() performs one frame's worth of updates
def simulate(self):
for _ in range(self.sim_substeps):
self.state_0.clear_forces()
self.viewer.apply_forces(self.state_0)
self.solver.step(self.state_0, self.state_1, self.control, None, self.sim_dt)
self.solver.update_contacts(self.contacts, self.state_0)
self.state_0, self.state_1 = self.state_1, self.state_0
def step(self):
if self.graph:
wp.capture_launch(self.graph)
else:
self.simulate()
self.sim_time += self.frame_dt
def render(self):
self.viewer.begin_frame(self.sim_time)
self.viewer.log_state(self.state_0)
self.viewer.log_contacts(self.contacts, self.state_1)
self.viewer.end_frame()
def test_final(self):
newton.examples.test_body_state(
self.model,
self.state_0,
"all bodies are above the ground",
lambda q, qd: q[2] > -0.006,
)
# Only check velocities on CUDA where we run 500 frames (enough time to settle)
# On CPU we only run 10 frames and the robot is still falling (~0.65 m/s)
if self.device.is_cuda:
newton.examples.test_body_state(
self.model,
self.state_0,
"body velocities are small",
lambda q, qd: max(abs(qd))
< 0.25, # Relaxed from 0.1 - unified pipeline has residual velocities up to ~0.2
)
if __name__ == "__main__":
parser = newton.examples.create_parser()
parser.add_argument("--num-worlds", type=int, default=1, help="Total number of simulated worlds.")
viewer, args = newton.examples.init(parser)
example = Example(viewer, args.num_worlds, args)
example.viewer._paused = True # Start paused to inspect the initial configuration
# If only a single-world is created, set initial
# camera position for better view of the system
if args.num_worlds == 1 and hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(1.34, 0.0, 0.25)
pitch = -7.0
yaw = -180.0
example.viewer.set_camera(camera_pos, pitch, yaw)
msg.notif("Starting the simulation...")
newton.examples.run(example, args)
================================================
FILE: newton/_src/solvers/kamino/examples/newton/example_fourbar.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# Example for basic four-bar mechanism
#
# Shows how to simulate a basic four-bar linkage with multiple worlds using SolverKamino.
#
# Command: python -m newton.examples example_fourbar --num-worlds 16
#
###########################################################################
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.models import get_basics_usd_assets_path
from newton._src.solvers.kamino._src.utils import logger as msg
class Example:
def __init__(self, viewer, num_worlds=1, args=None):
# Set simulation run-time configurations
self.fps = 60
self.sim_dt = 0.0025
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.sim_time = 0.0
self.num_worlds = num_worlds
self.viewer = viewer
self.device = wp.get_device()
# Create a single-robot model builder and register the Kamino-specific custom attributes
msg.notif("Creating and configuring the model builder for Kamino...")
robot_builder = newton.ModelBuilder(up_axis=newton.Axis.Z)
newton.solvers.SolverKamino.register_custom_attributes(robot_builder)
robot_builder.default_shape_cfg.margin = 0.0
robot_builder.default_shape_cfg.gap = 0.0
# Load the basic four-bar USD and add it to the builder
msg.notif("Loading USD asset and adding it to the model builder...")
asset_file = os.path.join(get_basics_usd_assets_path(), "boxes_fourbar.usda")
robot_builder.add_usd(
asset_file,
joint_ordering=None,
force_show_colliders=True,
force_position_velocity_actuation=True,
enable_self_collisions=False,
hide_collision_shapes=False,
)
# Create the multi-world model by duplicating the single-robot
# builder for the specified number of worlds
msg.notif(f"Duplicating the model builder for {self.num_worlds} worlds and finalizing the model...")
builder = newton.ModelBuilder(up_axis=newton.Axis.Z)
for _ in range(self.num_worlds):
builder.add_world(robot_builder)
# Create the model from the builder
msg.notif("Creating the model from the builder...")
self.model = builder.finalize(skip_validation_joints=True)
# Create and configure settings for SolverKamino and the collision detector
solver_config = newton.solvers.SolverKamino.Config.from_model(self.model)
solver_config.use_collision_detector = True
solver_config.use_fk_solver = True
solver_config.collision_detector.pipeline = "unified"
solver_config.collision_detector.max_contacts = 32 * self.num_worlds
solver_config.dynamics.preconditioning = True
solver_config.padmm.primal_tolerance = 1e-4
solver_config.padmm.dual_tolerance = 1e-4
solver_config.padmm.compl_tolerance = 1e-4
solver_config.padmm.max_iterations = 200
solver_config.padmm.rho_0 = 0.1
solver_config.padmm.use_acceleration = True
solver_config.padmm.warmstart_mode = "containers"
solver_config.padmm.contact_warmstart_method = "geom_pair_net_force"
# Create the Kamino solver for the given model
msg.notif("Creating the Kamino solver for the given model...")
self.solver = newton.solvers.SolverKamino(model=self.model, config=solver_config)
# Create state, control, and contacts data containers
self.state_0 = self.model.state()
self.state_1 = self.model.state()
self.control = self.model.control()
self.contacts = self.model.contacts()
# Reset the simulation state to a valid initial configuration above the ground
msg.notif("Resetting the simulation state to a valid initial configuration above the ground...")
self.base_q = wp.zeros(shape=(self.num_worlds,), dtype=wp.transformf)
q_b = wp.quat_identity(dtype=wp.float32)
q_base = wp.transformf((0.0, 0.0, 0.1), q_b)
q_base = np.array(q_base)
q_base = np.tile(q_base, (self.num_worlds, 1))
for w in range(self.num_worlds):
q_base[w, :3] += np.array([0.0, 0.0, 0.2]) * float(w)
self.base_q.assign(q_base)
self.solver.reset(state_out=self.state_0, base_q=self.base_q)
# Attach the model to the viewer for visualization
self.viewer.set_model(self.model)
# Capture the simulation graph if running on CUDA
# NOTE: This only has an effect on GPU devices
self.capture()
def capture(self):
self.graph = None
if self.device.is_cuda:
with wp.ScopedCapture() as capture:
self.simulate()
self.graph = capture.graph
# simulate() performs one frame's worth of updates
def simulate(self):
for _ in range(self.sim_substeps):
self.state_0.clear_forces()
self.viewer.apply_forces(self.state_0)
self.solver.step(self.state_0, self.state_1, self.control, None, self.sim_dt)
self.solver.update_contacts(self.contacts, self.state_0)
self.state_0, self.state_1 = self.state_1, self.state_0
def step(self):
if self.graph:
wp.capture_launch(self.graph)
else:
self.simulate()
self.sim_time += self.frame_dt
def render(self):
self.viewer.begin_frame(self.sim_time)
# Since rendering is called after stepping the simulation, the previous and next
# states correspond to self.state_1 and self.state_0 due to the reference swaps,
# so contacts are rendered with self.state_1 to match the body positions at the
# time of contact generation.
self.viewer.log_state(self.state_0)
self.viewer.log_contacts(self.contacts, self.state_1)
self.viewer.end_frame()
def test_final(self):
newton.examples.test_body_state(
self.model,
self.state_0,
"all bodies are above the ground",
lambda q, qd: q[2] > -0.006,
)
# Only check velocities on CUDA where we run 500 frames (enough time to settle)
# On CPU we only run 10 frames and the robot is still falling (~0.65 m/s)
if self.device.is_cuda:
newton.examples.test_body_state(
self.model,
self.state_0,
"body velocities are small",
lambda q, qd: (
max(abs(qd)) < 0.25
), # Relaxed from 0.1 - unified pipeline has residual velocities up to ~0.2
)
if __name__ == "__main__":
parser = newton.examples.create_parser()
parser.add_argument("--num-worlds", type=int, default=1, help="Total number of simulated worlds.")
viewer, args = newton.examples.init(parser)
example = Example(viewer, args.num_worlds, args)
example.viewer._paused = True # Start paused to inspect the initial configuration
# If only a single-world is created, set initial
# camera position for better view of the system
if args.num_worlds == 1 and hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(-0.5, -1.0, 0.2)
pitch = -5.0
yaw = 70.0
example.viewer.set_camera(camera_pos, pitch, yaw)
msg.notif("Starting the simulation...")
newton.examples.run(example, args)
================================================
FILE: newton/_src/solvers/kamino/examples/reset/example_reset_dr_legs.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
from scipy.spatial.transform import Rotation # noqa: TID253
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.types import float32, int32, transformf, vec6f
from newton._src.solvers.kamino._src.models.builders.utils import (
make_homogeneous_builder,
set_uniform_body_pose_offset,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino._src.utils.sim import ViewerKamino
from newton._src.solvers.kamino._src.utils.sim.simulator import Simulator
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Kernels
###
@wp.kernel
def _test_control_callback(
sim_has_started_resets: wp.array(dtype=wp.bool),
sim_reset_index: wp.array(dtype=wp.int32),
actuated_joint_idx: wp.array(dtype=int32),
state_t: wp.array(dtype=float32),
control_tau_j: wp.array(dtype=float32),
):
"""
An example control callback kernel.
"""
# Skip if no joint is selected for actuation
if not sim_has_started_resets[0]:
return
# Hack to handle negative reset index
joint_reset_index = sim_reset_index[0]
if joint_reset_index < 0:
joint_reset_index = actuated_joint_idx.size - 1
# Define the time window for the active external force profile
t_start = float32(0.0)
t_end = float32(0.5)
# Get the current time
t = state_t[0]
# Ad-hoc torque magnitude based on the selected joint
# because we want higher actuation for the two hip joints
if joint_reset_index == 0 or joint_reset_index == 6:
torque = 0.01
else:
torque = 0.001
# Reverse torque direction for the first leg
if joint_reset_index < 6:
torque = -torque
# Apply a time-dependent external force
if t >= t_start and t < t_end:
control_tau_j[actuated_joint_idx[joint_reset_index]] = torque
else:
control_tau_j[actuated_joint_idx[joint_reset_index]] = 0.0
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
num_worlds: int = 1,
max_steps: int = 1000,
use_cuda_graph: bool = False,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Define internal counters
self.sim_steps = 0
self.sim_reset_mode = 0
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
# Load the DR Legs USD and add it to the builder
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_legs/usd" / "dr_legs_with_meshes_and_boxes.usda")
# Create a model builder from the imported USD
msg.notif("Constructing builder from imported USD ...")
importer = USDImporter()
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=importer.import_from,
load_drive_dynamics=False,
load_static_geometry=True,
source=asset_file,
)
msg.info("total mass: %f", self.builder.worlds[0].mass_total)
msg.info("total diag inertia: %f", self.builder.worlds[0].inertia_total)
# Offset the model to place it above the ground
# NOTE: The USD model is centered at the origin
q_base = wp.transformf((0.0, 0.0, 0.265), wp.quat_identity(dtype=float32))
set_uniform_body_pose_offset(builder=self.builder, offset=q_base)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = False
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.constraints.alpha = 0.1
config.solver.padmm.primal_tolerance = 1e-4
config.solver.padmm.dual_tolerance = 1e-4
config.solver.padmm.compl_tolerance = 1e-4
config.solver.padmm.max_iterations = 100
config.solver.padmm.eta = 1e-5
config.solver.padmm.rho_0 = 0.02
config.solver.padmm.rho_min = 0.01
config.solver.padmm.use_acceleration = True
config.solver.padmm.warmstart_mode = "containers"
config.solver.padmm.contact_warmstart_method = "geom_pair_net_force"
config.solver.collect_solver_info = False
config.solver.compute_solution_metrics = False
config.solver.use_fk_solver = True
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
# Create a list of actuated joint indices from the model and builder
self.actuated_joint_idx_np = np.zeros(shape=(self.sim.model.size.sum_of_num_actuated_joints,), dtype=np.int32)
jidx = 0
for j, joint in enumerate(self.builder.joints):
if joint.is_actuated:
self.actuated_joint_idx_np[jidx] = j
jidx += 1
msg.warning("actuated_joint_idx_np: %s", self.actuated_joint_idx_np)
msg.warning("actuated_joint_names:\n%s", self.builder.worlds[0].actuated_joint_names)
# Allocate utility arrays for resetting
with wp.ScopedDevice(self.device):
self.base_q = wp.zeros(shape=(self.sim.model.size.num_worlds,), dtype=transformf)
self.base_u = wp.zeros(shape=(self.sim.model.size.num_worlds,), dtype=vec6f)
self.joint_q = wp.zeros(shape=(self.sim.model.size.sum_of_num_joint_coords,), dtype=float32)
self.joint_u = wp.zeros(shape=(self.sim.model.size.sum_of_num_joint_dofs,), dtype=float32)
self.actuator_q = wp.zeros(shape=(self.sim.model.size.sum_of_num_actuated_joint_coords,), dtype=float32)
self.actuator_u = wp.zeros(shape=(self.sim.model.size.sum_of_num_actuated_joint_dofs,), dtype=float32)
self.actuated_joint_idx = wp.array(self.actuated_joint_idx_np, dtype=int32)
self.sim_has_started_resets = wp.full(shape=(1,), dtype=wp.bool, value=False)
self.sim_reset_index = wp.full(shape=(1,), dtype=wp.int32, value=-1)
# Define the control callback function that will actuate a single joint
def test_control_callback(sim: Simulator):
wp.launch(
_test_control_callback,
dim=1,
inputs=[
self.sim_has_started_resets,
self.sim_reset_index,
self.actuated_joint_idx,
sim.solver.data.time.time,
sim.data.control.tau_j,
],
)
# Set the test control callback into the simulator
self.sim.set_control_callback(test_control_callback)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "reset_dr_legs/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
self.sim_steps += 1
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
def update_reset_config(self):
"""Update the reset configuration based on the current reset index and mode."""
self.sim_steps = 0
self.sim.data.control.tau_j.zero_()
self.sim_has_started_resets.fill_(True)
joint_reset_index = self.sim_reset_index.numpy()[0]
num_actuated_joints = len(self.actuated_joint_idx_np)
joint_reset_index = (joint_reset_index + 1) % num_actuated_joints
# If all joints have been cycled through, proceed to the next reset mode
if joint_reset_index == num_actuated_joints - 1:
self.sim_reset_mode = (self.sim_reset_mode + 1) % 6
joint_reset_index = -1
self.sim_reset_index.fill_(joint_reset_index)
msg.warning(f"Next joint_reset_index: {joint_reset_index}")
msg.warning(f"Next sim_reset_mode: {self.sim_reset_mode}")
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
self.sim_steps += self.sim_substeps
else:
self.simulate()
# Demo of resetting to the default state defined in the model
if self.sim_steps >= self.max_steps and self.sim_reset_mode == 0:
msg.notif("Resetting to default model state...")
self.update_reset_config()
self.sim.reset()
# Demo of resetting only the base pose
if self.sim_steps >= self.max_steps and self.sim_reset_mode == 1:
msg.notif("Resetting with base pose...")
self.update_reset_config()
R_b = Rotation.from_rotvec(np.pi / 4 * np.array([0, 0, 1]))
q_b = R_b.as_quat() # x, y, z, w
q_base = wp.transformf((0.1, 0.1, 0.3), q_b)
self.base_q.assign([q_base] * self.sim.model.size.num_worlds)
self.sim.reset(base_q=self.base_q)
# Demo of resetting the base pose and twist
if self.sim_steps >= self.max_steps and self.sim_reset_mode == 2:
msg.notif("Resetting with base pose and twist...")
self.update_reset_config()
R_b = Rotation.from_rotvec(np.pi / 4 * np.array([0, 0, 1]))
q_b = R_b.as_quat() # x, y, z, w
q_base = wp.transformf((0.1, 0.1, 0.3), q_b)
u_base = vec6f(0.0, 0.0, 0.05, 0.0, 0.0, 0.3)
self.base_q.assign([q_base] * self.sim.model.size.num_worlds)
self.base_u.assign([u_base] * self.sim.model.size.num_worlds)
self.sim.reset(base_q=self.base_q, base_u=self.base_u)
# Demo of resetting the base state and joint configurations
# NOTE: This will invoke the FK solver to update body poses
if self.sim_steps >= self.max_steps and self.sim_reset_mode == 3:
msg.notif("Resetting with base pose and joint configurations...")
self.update_reset_config()
R_b = Rotation.from_rotvec(np.pi / 4 * np.array([0, 0, 1]))
q_b = R_b.as_quat() # x, y, z, w
q_base = wp.transformf((0.1, 0.1, 0.3), q_b)
u_base = vec6f(0.0, 0.0, 0.05, 0.0, 0.0, -0.3)
self.base_q.assign([q_base] * self.sim.model.size.num_worlds)
self.base_u.assign([u_base] * self.sim.model.size.num_worlds)
joint_q_np = np.zeros(self.sim.model.size.sum_of_num_joint_coords, dtype=np.float32)
self.joint_q.assign(joint_q_np)
self.sim.reset(base_q=self.base_q, base_u=self.base_u, joint_q=self.joint_q, joint_u=self.joint_u)
# Demo of resetting the base state and joint configurations to specific poses
# NOTE: This will invoke the FK solver to update body poses
if self.sim_steps >= self.max_steps and self.sim_reset_mode == 4:
msg.notif("Resetting with base pose and specific joint configurations...")
self.update_reset_config()
joint_reset_index = self.sim_reset_index.numpy()[0]
msg.warning(f"Resetting joint {self.actuated_joint_idx_np[joint_reset_index]}...")
R_b = Rotation.from_rotvec(np.pi / 4 * np.array([0, 0, 1]))
q_b = R_b.as_quat() # x, y, z, w
q_base = wp.transformf((0.1, 0.1, 0.3), q_b)
u_base = vec6f(0.0, 0.0, -0.05, 0.0, 0.0, 0.3)
self.base_q.assign([q_base] * self.sim.model.size.num_worlds)
self.base_u.assign([u_base] * self.sim.model.size.num_worlds)
actuated_joint_config = np.array(
[
np.pi / 12,
np.pi / 12,
np.pi / 12,
np.pi / 12,
np.pi / 12,
np.pi / 12,
-np.pi / 12,
-np.pi / 12,
-np.pi / 12,
-np.pi / 12,
-np.pi / 12,
-np.pi / 12,
],
dtype=np.float32,
)
joint_q_np = np.zeros(self.sim.model.size.sum_of_num_joint_coords, dtype=np.float32)
joint_q_np[self.actuated_joint_idx_np[joint_reset_index]] = actuated_joint_config[joint_reset_index]
self.joint_q.assign(joint_q_np)
self.sim.reset(base_q=self.base_q, base_u=self.base_u, joint_q=self.joint_q, joint_u=self.joint_u)
# Demo of resetting the base state and joint configurations to specific poses
# NOTE: This will invoke the FK solver to update body poses
if self.sim_steps >= self.max_steps and self.sim_reset_mode == 5:
msg.notif("Resetting with base pose and specific actuator configurations...")
self.update_reset_config()
joint_reset_index = self.sim_reset_index.numpy()[0]
msg.warning(f"Resetting joint {self.actuated_joint_idx_np[joint_reset_index]}...")
R_b = Rotation.from_rotvec(np.pi / 4 * np.array([0, 0, 1]))
q_b = R_b.as_quat() # x, y, z, w
q_base = wp.transformf((0.1, 0.1, 0.3), q_b)
u_base = vec6f(0.0, 0.0, -0.05, 0.0, 0.0, -0.3)
self.base_q.assign([q_base] * self.sim.model.size.num_worlds)
self.base_u.assign([u_base] * self.sim.model.size.num_worlds)
actuated_joint_config = np.array(
[
np.pi / 12,
np.pi / 12,
np.pi / 12,
np.pi / 12,
np.pi / 12,
np.pi / 12,
-np.pi / 12,
-np.pi / 12,
-np.pi / 12,
-np.pi / 12,
-np.pi / 12,
-np.pi / 12,
],
dtype=np.float32,
)
actuator_q_np = np.zeros(self.sim.model.size.sum_of_num_actuated_joint_coords, dtype=np.float32)
actuator_q_np[joint_reset_index] = actuated_joint_config[joint_reset_index]
self.actuator_q.assign(actuator_q_np)
self.sim.reset(
base_q=self.base_q, base_u=self.base_u, actuator_q=self.actuator_q, actuator_u=self.actuator_u
)
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="DR Legs simulation example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-worlds", type=int, default=1, help="Number of worlds to simulate in parallel")
parser.add_argument("--num-steps", type=int, default=400, help="Number of steps for headless mode")
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=6, threshold=10000, suppress=True) # Suppress scientific notation
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
num_worlds=args.num_worlds,
max_steps=args.num_steps,
headless=args.headless,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(0.6, 0.6, 0.3)
pitch = -10.0
yaw = 225.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "reset_dr_legs")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH)
================================================
FILE: newton/_src/solvers/kamino/examples/rl/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from newton._src.solvers.kamino.examples.rl.simulation import RigidBodySim
__all__ = ["RigidBodySim"]
================================================
FILE: newton/_src/solvers/kamino/examples/rl/camera_follow.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import numpy as np
import warp as wp
class CameraFollowRobot:
"""Smoothly follow the robot root body with the viewer camera.
The camera maintains a fixed offset from the robot position and uses
exponential smoothing to avoid jerky motion. Call :meth:`update` once
per render frame.
Args:
viewer: Newton viewer instance (must support ``set_camera``).
camera_offset: ``(x, y, z)`` offset from robot root to camera position.
pitch: Camera pitch angle in degrees.
yaw: Camera yaw angle in degrees.
filter_coeff: Exponential smoothing factor in ``(0, 1]``.
Smaller = smoother/slower tracking, larger = snappier.
"""
def __init__(
self,
viewer,
camera_offset: tuple[float, float, float] = (1.5, 1.5, 0.5),
pitch: float = -10.0,
yaw: float = 225.0,
filter_coeff: float = 0.1,
):
self._viewer = viewer
self._offset = np.array(camera_offset, dtype=np.float32)
self._pitch = pitch
self._yaw = yaw
self._filter = filter_coeff
self._cam_pos: np.ndarray | None = None
def update(self, root_pos: np.ndarray):
"""Update camera to follow the given root position.
Args:
root_pos: Robot root position as ``(3,)`` numpy array or similar.
"""
target = np.asarray(root_pos, dtype=np.float32).ravel()[:3]
desired = target + self._offset
if self._cam_pos is None:
self._cam_pos = desired.copy()
else:
self._cam_pos += self._filter * (desired - self._cam_pos)
self._viewer.set_camera(wp.vec3(*self._cam_pos.tolist()), self._pitch, self._yaw)
def set_offset(self, offset: tuple[float, float, float]):
"""Change the camera offset from the robot."""
self._offset = np.array(offset, dtype=np.float32)
def set_pitch(self, pitch: float):
"""Change the camera pitch angle in degrees."""
self._pitch = pitch
def set_yaw(self, yaw: float):
"""Change the camera yaw angle in degrees."""
self._yaw = yaw
def reset(self):
"""Reset smoothing state (e.g. after a simulation reset)."""
self._cam_pos = None
================================================
FILE: newton/_src/solvers/kamino/examples/rl/example_rl_bipedal.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# Example: Bipedal RL policy play-back
#
# Runs a trained RL walking policy on the robot using the
# Kamino solver with implicit PD joint control. Velocity commands come
# from an Xbox gamepad or, when no gamepad is connected, from keyboard
# input via the 3-D viewer.
#
# Usage:
# python example_rl_bipedal.py
###########################################################################
# Python
import argparse
import os
# Thirdparty
import numpy as np
import torch # noqa: TID253
import warp as wp
# Kamino
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.viewer import ViewerConfig
from newton._src.solvers.kamino.examples import run_headless
from newton._src.solvers.kamino.examples.rl.joystick import JoystickController
from newton._src.solvers.kamino.examples.rl.observations import BipedalObservation
from newton._src.solvers.kamino.examples.rl.simulation import RigidBodySim
from newton._src.solvers.kamino.examples.rl.simulation_runner import SimulationRunner
from newton._src.solvers.kamino.examples.rl.utils import _load_policy_checkpoint, quat_to_projected_yaw
# Asset root relative to this file
_ASSETS_DIR = os.path.normpath(
os.path.join(
os.path.dirname(__file__),
"..",
"..",
"..",
"..",
"..",
"..",
"..",
"walking-character-rl",
"walking_rl_kamino",
"assets",
"usd",
)
)
# ---------------------------------------------------------------------------
# Bipedal joint normalization
# ---------------------------------------------------------------------------
# Each entry maps joint name -> (position_offset, position_scale) used to
# normalise joint positions in the observation vector.
_BIPEDAL_JOINT_NORMALIZATION = {
"NECK_FORWARD": (1.23, 0.19),
"NECK_PITCH": (-1.09, 0.44),
"NECK_YAW": (0.0, 0.35),
"NECK_ROLL": (0.0, 0.11),
"RIGHT_HIP_YAW": (0.0, 0.26),
"RIGHT_HIP_ROLL": (0.06, 0.32),
"RIGHT_HIP_PITCH": (0.49, 0.75),
"RIGHT_KNEE_PITCH": (-0.91, 0.61),
"RIGHT_ANKLE_PITCH": (0.22, 0.66),
"LEFT_HIP_YAW": (0.0, 0.26),
"LEFT_HIP_ROLL": (-0.06, 0.32),
"LEFT_HIP_PITCH": (0.49, 0.75),
"LEFT_KNEE_PITCH": (-0.91, 0.61),
"LEFT_ANKLE_PITCH": (0.22, 0.66),
}
_BIPEDAL_JOINT_VELOCITY_SCALE = 5.0
_BIPEDAL_PATH_DEVIATION_SCALE = 0.1
_BIPEDAL_PHASE_EMBEDDING_DIM = 4
def _build_normalization(joint_names: list[str]):
"""Build ordered (offset, scale) lists from simulator joint names."""
offsets: list[float] = []
scales: list[float] = []
for name in joint_names:
if name in _BIPEDAL_JOINT_NORMALIZATION:
o, s = _BIPEDAL_JOINT_NORMALIZATION[name]
else:
msg.warning(f"Joint '{name}' not in BIPEDAL normalization dict -- using identity.")
o, s = 0.0, 1.0
offsets.append(o)
scales.append(s)
return offsets, scales
###########################################################################
# Example class
###########################################################################
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
policy=None,
headless: bool = False,
):
# Timing
self.sim_dt = 0.02
self.control_decimation = 1
num_worlds = 1
self.env_dt = self.sim_dt * self.control_decimation
# USD model path
USD_MODEL_PATH = os.path.join(_ASSETS_DIR, "bipedal", "bipedal_with_textures.usda")
# Create generic articulated body simulator
self.sim_wrapper = RigidBodySim(
usd_model_path=USD_MODEL_PATH,
num_worlds=1,
sim_dt=self.sim_dt,
device=device,
headless=headless,
body_pose_offset=(0.0, 0.0, 0.33, 0.0, 0.0, 0.0, 1.0),
use_cuda_graph=True,
render_config=ViewerConfig(
diffuse_scale=1.0,
specular_scale=0.3,
shadow_radius=10.0,
),
)
# Override PD gains
self.sim_wrapper.sim.model.joints.k_p_j.fill_(15.0)
self.sim_wrapper.sim.model.joints.k_d_j.fill_(0.6)
self.sim_wrapper.sim.model.joints.a_j.fill_(0.004)
self.sim_wrapper.sim.model.joints.b_j.fill_(0.0)
# Build normalization from simulator joint order
joint_pos_offset, joint_pos_scale = _build_normalization(self.sim_wrapper.joint_names)
self.joint_pos_offset = torch.tensor(joint_pos_offset, device=self.torch_device)
self.joint_pos_scale = torch.tensor(joint_pos_scale, device=self.torch_device)
# Observation builder
self.obs = BipedalObservation(
body_sim=self.sim_wrapper,
joint_position_default=joint_pos_offset,
joint_position_range=joint_pos_scale,
joint_velocity_scale=_BIPEDAL_JOINT_VELOCITY_SCALE,
path_deviation_scale=_BIPEDAL_PATH_DEVIATION_SCALE,
phase_embedding_dim=_BIPEDAL_PHASE_EMBEDDING_DIM,
phase_rate_policy_path=PHASE_RATE_POLICY_PATH,
dt=self.env_dt,
num_joints=len(self.joint_pos_offset),
)
msg.info(f"Observation dim: {self.obs.num_observations}")
# Joystick / keyboard command controller
self.joystick = JoystickController(
dt=self.env_dt,
viewer=self.sim_wrapper.viewer,
num_worlds=num_worlds,
device=self.torch_device,
)
# Initialize path to current robot pose
root_pos_2d = self.sim_wrapper.q_i[:, 0, :2]
root_yaw = quat_to_projected_yaw(self.sim_wrapper.q_i[:, 0, 3:])
self.joystick.reset(root_pos_2d=root_pos_2d, root_yaw=root_yaw)
# Action buffer
self.actions = self.sim_wrapper.q_j.clone()
# Pre-allocated command buffers (eliminates per-step torch.tensor())
self._cmd_vel_buf = torch.zeros(1, 2, device=self.torch_device)
self._neck_cmd_buf = torch.zeros(4, device=self.torch_device)
# Policy (None = zero actions)
self.policy = policy
# Convenience accessors for the main block
@property
def torch_device(self) -> str:
return self.sim_wrapper.torch_device
@property
def viewer(self):
return self.sim_wrapper.viewer
def reset(self):
"""Reset the simulation and internal state."""
self.sim_wrapper.reset()
self.obs.reset()
root_pos_2d = self.sim_wrapper.q_i[:, 0, :2]
root_yaw = quat_to_projected_yaw(self.sim_wrapper.q_i[:, 0, 3:])
self.joystick.reset(root_pos_2d=root_pos_2d, root_yaw=root_yaw)
self.actions[:] = self.sim_wrapper.q_j
def step_once(self):
"""Single physics step (used by run_headless warm-up)."""
self.sim_wrapper.step()
def update_input(self):
"""Transfer joystick commands to the observation command tensor."""
cmd = self.obs.command
cmd[:, BipedalObservation.CMD_PATH_HEADING] = self.joystick.path_heading[:, 0]
cmd[:, BipedalObservation.CMD_PATH_POSITION] = self.joystick.path_position
self._cmd_vel_buf[0, 0] = self.joystick.forward_velocity
self._cmd_vel_buf[0, 1] = self.joystick.lateral_velocity
cmd[:, BipedalObservation.CMD_VEL] = self._cmd_vel_buf
cmd[:, BipedalObservation.CMD_YAW_RATE] = self.joystick.angular_velocity
# Head command: head_forward is an up-bias coupled to head pitch
# (looking up also raises the head). head_pitch = forward + pitch.
js = self.joystick
head_forward = max(js.head_pitch, 0.0) * 0.4
head_z_des = max(-1.0, min(head_forward, 0.3))
head_roll_des = 0.0
head_pitch_des = max(-0.6, min(head_forward + js.head_pitch, 1.0))
head_yaw_des = max(-1.0, min(js.head_yaw, 1.0))
self._neck_cmd_buf[0] = head_z_des
self._neck_cmd_buf[1] = head_roll_des
self._neck_cmd_buf[2] = head_pitch_des
self._neck_cmd_buf[3] = head_yaw_des
cmd[:, BipedalObservation.CMD_HEAD] = self._neck_cmd_buf
def sim_step(self):
"""Observations -> policy inference -> actions -> physics step."""
# Compute observation from current state (with previous setpoints)
obs = self.obs.compute(setpoints=self.actions)
# Policy inference (in-place: no clone, no intermediates)
with torch.inference_mode():
raw = self.policy(obs)
torch.mul(raw, self.joint_pos_scale, out=self.actions)
self.actions.add_(self.joint_pos_offset)
# Write action targets to implicit PD controller
self.sim_wrapper.q_j_ref[:] = self.actions
# Step physics
for _ in range(self.control_decimation):
self.sim_wrapper.step()
def step(self):
"""One RL step: commands -> observe -> infer -> apply -> simulate."""
if self.joystick.check_reset():
self.reset()
self.joystick.update(root_pos_2d=self.sim_wrapper.q_i[:, 0, :2])
self.update_input()
self.sim_step()
def render(self):
"""Render the current frame."""
self.sim_wrapper.render()
###########################################################################
# Main
###########################################################################
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Bipedal RL play example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument(
"--headless",
action=argparse.BooleanOptionalAction,
default=False,
help="Run in headless mode",
)
parser.add_argument(
"--mode",
choices=["sync", "async"],
default="sync",
help="Sim loop mode: sync (default) or async",
)
parser.add_argument(
"--render-fps",
type=float,
default=30.0,
help="Target render FPS for async mode (default: 30)",
)
args = parser.parse_args()
np.set_printoptions(linewidth=20000, precision=6, threshold=10000, suppress=True)
msg.set_log_level(msg.LogLevel.INFO)
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
msg.info(f"device: {device}")
# Convert warp device to torch device string for checkpoint loading
torch_device = "cuda" if device.is_cuda else "cpu"
# Load trained policy
POLICY_PATH = os.path.join(_ASSETS_DIR, "bipedal", "model.pt")
PHASE_RATE_POLICY_PATH = os.path.join(_ASSETS_DIR, "bipedal", "phase_rate.pt")
policy = _load_policy_checkpoint(POLICY_PATH, device=torch_device)
msg.info(f"Loaded policy from: {POLICY_PATH}")
example = Example(
device=device,
policy=policy,
headless=args.headless,
)
try:
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
else:
msg.notif(f"Running in Viewer mode ({args.mode})...")
if hasattr(example.viewer, "set_camera"):
example.viewer.set_camera(wp.vec3(0.6, 0.6, 0.3), -10.0, 225.0)
SimulationRunner(example, mode=args.mode, render_fps=args.render_fps).run()
except KeyboardInterrupt:
pass
finally:
example.joystick.close()
================================================
FILE: newton/_src/solvers/kamino/examples/rl/example_rl_drlegs.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# Example: DR Legs walk policy play-back
#
# Runs a trained walk RL policy on the DR Legs robot using the Kamino
# solver with implicit PD joint control. Velocity commands come from an
# Xbox gamepad or keyboard via the 3-D viewer.
#
# The policy expects 94D observations with path-frame integration:
# ori_root_to_path (9D) + path_deviation (2D) + path_dev_heading (2D)
# + path_cmd (3D) + cmd_linvel_in_root (3D) + cmd_angvel_in_root (3D)
# + phase_encoding (4D) + root_linvel_in_root (3D) + root_angvel_in_root (3D)
# + cmd_height (1D) + height_error (1D)
# + joint_positions (36D) + action_history (24D)
#
# Usage:
# python example_rl_drlegs.py --policy path/to/model.pt
# python example_rl_drlegs.py --policy path/to/model.pt --mode async
# python example_rl_drlegs.py --headless --num-steps 200
###########################################################################
import argparse
from pathlib import Path
from typing import ClassVar
import numpy as np
import torch # noqa: TID253
import warp as wp
import yaml
import newton
from newton._src.solvers.kamino._src.core.joints import JointActuationType
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.viewer import MeshColors, ViewerConfig
from newton._src.solvers.kamino.examples import run_headless
from newton._src.solvers.kamino.examples.rl.joystick import JoystickConfig, JoystickController
from newton._src.solvers.kamino.examples.rl.observations import DrlegsBaseObservation
from newton._src.solvers.kamino.examples.rl.simulation import RigidBodySim
from newton._src.solvers.kamino.examples.rl.simulation_runner import SimulationRunner
from newton._src.solvers.kamino.examples.rl.utils import (
_load_policy_checkpoint,
periodic_encoding,
quat_inv_mul,
quat_rotate_inv,
quat_to_projected_yaw,
quat_to_rotation9d,
yaw_apply_2d,
yaw_to_quat,
)
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
# ---------------------------------------------------------------------------
# Walk task config
# ---------------------------------------------------------------------------
_DEFAULTS = {
"action_scale": 0.4,
"contact_duration": 0.3,
"phase_embedding_k": 2,
"vel_cmd_max": 0.3,
"yaw_cmd_max": 0.8,
"pd_kp": 15.0,
"pd_kd": 0.6,
"pd_armature": 0.01,
"path_deviation_scale": 0.1,
"linear_path_error_limit": 0.1,
"standing_height": 0.265,
"height_cmd_min": 0.16,
"height_cmd_max": 0.27,
"height_error_scale": 0.05,
"sim_dt": 0.004,
"control_decimation": 5,
"body_pose_offset_z": 0.265,
"usd_model": "dr_legs/usd/dr_legs_with_meshes_and_boxes.usda",
"policy_file": "drlegs_walk.pt",
}
def _load_drlegs_config(asset_path: Path) -> dict:
"""Load walk config YAML from assets, falling back to built-in defaults."""
cfg = dict(_DEFAULTS)
yaml_path = asset_path / "dr_legs" / "rl_policies" / "drlegs_walk.yaml"
if yaml_path.exists():
with open(yaml_path, encoding="utf-8") as f:
overrides = yaml.safe_load(f) or {}
cfg.update(overrides)
msg.info(f"Loaded config from {yaml_path}")
else:
msg.info("No YAML config found, using built-in defaults")
# Derived constant
cfg["phase_rate"] = 1.0 / (2.0 * cfg["contact_duration"])
return cfg
###
# Example class
###
class Example:
def __init__(
self,
config: dict,
device: wp.DeviceLike = None,
policy=None,
headless: bool = False,
max_steps: int = 10000,
):
self.cfg = config
# Timing
self.sim_dt = config["sim_dt"]
self.control_decimation = config["control_decimation"]
self.env_dt = self.sim_dt * self.control_decimation
self.max_steps = max_steps
num_worlds = 1
# USD model path
asset_path = newton.utils.download_asset("disneyresearch")
usd_model_path = str(asset_path / config["usd_model"])
# Create generic articulated body simulator
self.sim_wrapper = RigidBodySim(
usd_model_path=usd_model_path,
num_worlds=num_worlds,
sim_dt=self.sim_dt,
device=device,
headless=headless,
body_pose_offset=(0.0, 0.0, config["body_pose_offset_z"], 0.0, 0.0, 0.0, 1.0),
use_cuda_graph=True,
render_config=ViewerConfig(
diffuse_scale=1.0,
specular_scale=0.3,
shadow_radius=10.0,
),
)
# Apply per-body-group colors for visual distinction
if not headless and self.sim_wrapper.viewer is not None:
self._apply_body_group_colors()
# Override implicit PD gains to match training config exactly
act_type = wp.to_torch(self.sim_wrapper.sim.model.joints.act_type)
k_p = wp.to_torch(self.sim_wrapper.sim.model.joints.k_p_j)
k_d = wp.to_torch(self.sim_wrapper.sim.model.joints.k_d_j)
a_j = wp.to_torch(self.sim_wrapper.sim.model.joints.a_j)
b_j = wp.to_torch(self.sim_wrapper.sim.model.joints.b_j)
actuated_mask = act_type != JointActuationType.PASSIVE
k_p[actuated_mask] = config["pd_kp"]
k_d[actuated_mask] = config["pd_kd"]
a_j[actuated_mask] = config["pd_armature"]
k_p[~actuated_mask] = 0.0
k_d[~actuated_mask] = 0.0
b_j.fill_(0.0)
# Observation builder (63D base: root_pos(3) + joints(36) + action_hist(24))
self.obs_builder = DrlegsBaseObservation(
body_sim=self.sim_wrapper,
action_scale=config["action_scale"],
)
# Phase clock for gait timing
phase_k = config["phase_embedding_k"]
self._phase = torch.zeros(num_worlds, device=self.torch_device, dtype=torch.float32)
freq_2pi, offset = periodic_encoding(k=phase_k)
self._freq_2pi = torch.from_numpy(freq_2pi).float().to(self.torch_device)
self._offset = torch.from_numpy(offset).float().to(self.torch_device)
self._phase_enc = torch.zeros(num_worlds, phase_k * 2, device=self.torch_device, dtype=torch.float32)
# Path frame state
self._path_heading = torch.zeros(num_worlds, device=self.torch_device, dtype=torch.float32)
self._path_position = torch.zeros(num_worlds, 2, device=self.torch_device, dtype=torch.float32)
# Command velocity buffer (filled by joystick each step)
self._cmd_vel = torch.zeros(num_worlds, 2, device=self.torch_device, dtype=torch.float32)
# Command yaw rate buffer (filled by joystick each step)
self._cmd_yaw_rate = torch.zeros(num_worlds, 1, device=self.torch_device, dtype=torch.float32)
# Height command buffer (default = standing height, adjustable via keyboard Y/N)
self._cmd_height = torch.full(
(num_worlds, 1), config["standing_height"], device=self.torch_device, dtype=torch.float32
)
# Zero column for 2D->3D padding
self._zeros = torch.zeros(num_worlds, 1, device=self.torch_device, dtype=torch.float32)
# Full observation buffer (94D)
# 9 + 2 + 2 + 3 + 3 + 3 + 4 + 3 + 3 + 1 + 1 + 36 + 24 = 94
obs_dim = 94
self._obs_buffer = torch.zeros(num_worlds, obs_dim, device=self.torch_device, dtype=torch.float32)
msg.info(f"Observation dim: {obs_dim}")
# Action buffer (12 actuated joints)
self.actions = torch.zeros(
(num_worlds, self.sim_wrapper.num_actuated),
device=self.torch_device,
dtype=torch.float32,
)
# Joystick for velocity commands
self.joystick = JoystickController(
dt=self.env_dt,
viewer=self.sim_wrapper.viewer,
num_worlds=num_worlds,
device=self.torch_device,
config=JoystickConfig(
forward_velocity_base=config["vel_cmd_max"],
forward_velocity_turbo=0.0,
lateral_velocity_base=config["vel_cmd_max"],
lateral_velocity_turbo=0.0,
angular_velocity_base=config["yaw_cmd_max"],
angular_velocity_turbo=0.0,
),
)
# Policy (None = random actions)
self.policy = policy
# Body name prefix to color mapping
BODY_GROUP_COLORS: ClassVar[dict] = {
"pelvis": MeshColors.BONE,
"hip_servos": MeshColors.DARK,
"upperleg_link": MeshColors.SAGEGREY,
"lowerleg_link": MeshColors.BONE,
"ankle_bracket": MeshColors.SAGEGREY,
"foot": MeshColors.DARK,
"servohorn": MeshColors.DARK,
"upperleg_rod": MeshColors.DARK,
}
def _apply_body_group_colors(self):
"""Color robot shapes by body group for visual distinction."""
model = self.sim_wrapper._newton_model
shape_body = model.shape_body.numpy()
body_labels = model.body_label
color_overrides = {}
for s_idx in range(model.shape_count):
bid = int(shape_body[s_idx])
if bid < 0:
continue
name = body_labels[bid].rsplit("/", 1)[-1]
for prefix, color in self.BODY_GROUP_COLORS.items():
if name.startswith(prefix):
color_overrides[s_idx] = color
break
if color_overrides:
for s_idx, color in color_overrides.items():
model.shape_color[s_idx : s_idx + 1].fill_(wp.vec3(color))
# Convenience accessors
@property
def torch_device(self) -> str:
return self.sim_wrapper.torch_device
@property
def viewer(self):
return self.sim_wrapper.viewer
# Simulation helpers
def _apply_actions(self):
"""Convert policy actions to implicit PD joint position references."""
self.sim_wrapper.q_j_ref.zero_()
self.sim_wrapper.q_j_ref[:, self.sim_wrapper.actuated_dof_indices_tensor] = (
self.cfg["action_scale"] * self.actions
)
self.sim_wrapper.dq_j_ref.zero_()
def _advance_path(self):
"""Integrate path heading and position from velocity commands.
Uses mid-point heading integration.
"""
cmd_yaw = self._cmd_yaw_rate.squeeze(-1) # (N,)
# Mid-point heading for numerical accuracy
mid_heading = self._path_heading + 0.5 * self.env_dt * cmd_yaw
self._path_position += yaw_apply_2d(mid_heading, self._cmd_vel) * self.env_dt
# Heading integration
self._path_heading += cmd_yaw * self.env_dt
# Clip path position to stay near robot (prevent drift)
root_pos_2d = self.sim_wrapper.q_i[:, 0, :2]
diff = self._path_position - root_pos_2d
clipped = diff.renorm(p=2, dim=0, maxnorm=self.cfg["linear_path_error_limit"])
self._path_position[:] = root_pos_2d + clipped
def reset(self):
"""Reset the simulation and internal state."""
self.sim_wrapper.reset()
self.actions.zero_()
self.obs_builder.reset()
self._phase.zero_()
self._cmd_vel.zero_()
self._cmd_yaw_rate.zero_()
self._cmd_height.fill_(self.cfg["standing_height"])
self._path_heading.zero_()
self._path_position[:] = self.sim_wrapper.q_i[:, 0, :2]
self.sim_wrapper.q_j_ref.zero_()
self.sim_wrapper.dq_j_ref.zero_()
self.joystick.reset()
def step_once(self):
"""Single physics step (used by run_headless warm-up)."""
self.sim_wrapper.step()
def update_input(self):
"""Transfer joystick velocity commands and height command to buffers."""
self._cmd_vel[0, 0] = self.joystick.forward_velocity
self._cmd_vel[0, 1] = self.joystick.lateral_velocity
self._cmd_yaw_rate[0, 0] = self.joystick.angular_velocity
# Height command: right stick Y (joystick) or Y/N keys (keyboard)
if self.joystick._mode == "joystick":
pitch = self.joystick.head_pitch # right stick Y, positive = up
if pitch >= 0:
t = min(1.0, pitch / self.joystick._cfg.head_pitch_up)
self._cmd_height[0, 0] = self.cfg["standing_height"] + t * (
self.cfg["height_cmd_max"] - self.cfg["standing_height"]
)
else:
t = min(1.0, -pitch / self.joystick._cfg.head_pitch_down)
self._cmd_height[0, 0] = self.cfg["standing_height"] - t * (
self.cfg["standing_height"] - self.cfg["height_cmd_min"]
)
elif self.viewer is not None and hasattr(self.viewer, "is_key_down"):
if self.viewer.is_key_down("y"):
self._cmd_height[0, 0] = min(self._cmd_height[0, 0].item() + 0.001, self.cfg["height_cmd_max"])
if self.viewer.is_key_down("n"):
self._cmd_height[0, 0] = max(self._cmd_height[0, 0].item() - 0.001, self.cfg["height_cmd_min"])
def sim_step(self):
"""Observations -> policy inference -> actions -> physics step.
Builds 94D path-frame observations matching DrlegsWalkObserver:
ori_root_to_path(9) + path_dev(2) + path_dev_heading(2)
+ path_cmd(3) + cmd_linvel_root(3) + cmd_angvel_root(3)
+ phase_enc(4) + root_linvel_root(3) + root_angvel_root(3)
+ cmd_height(1) + height_error(1)
+ joints(36) + action_hist(24)
"""
# Advance phase clock
self._phase.add_(self.env_dt * self.cfg["phase_rate"]).remainder_(1.0)
# Advance path frame
self._advance_path()
# Base observation (63D: root_pos(3) + joints(36) + action_hist(24))
base_obs = self.obs_builder.compute(actions=self.actions)
base_no_root = base_obs[:, 3:] # 60D (joints + action_history)
# --- Path quaternion from heading ---
path_quat = yaw_to_quat(self._path_heading) # (N, 4)
# --- Root orientation relative to path frame (9D) ---
root_quat = self.sim_wrapper.q_i[:, 0, 3:] # (N, 4)
root_in_path = quat_inv_mul(path_quat, root_quat) # (N, 4)
ori_9d = quat_to_rotation9d(root_in_path) # (N, 9)
# --- Path deviation in path frame (2D, scaled) ---
diff_xy = self.sim_wrapper.q_i[:, 0, :2] - self._path_position # (N, 2)
diff_3d = torch.cat([diff_xy, self._zeros], dim=-1) # (N, 3)
dev_in_path = quat_rotate_inv(path_quat, diff_3d)[:, :2] # (N, 2)
inv_scale = 1.0 / self.cfg["path_deviation_scale"]
path_dev = dev_in_path * inv_scale
# --- Path deviation in heading frame (2D, scaled) ---
root_heading = quat_to_projected_yaw(root_in_path) # (N, 1)
heading_quat = yaw_to_quat(root_heading) # (N, 4)
neg_dev = torch.cat([-dev_in_path, self._zeros], dim=-1) # (N, 3)
dev_in_heading = quat_rotate_inv(heading_quat, neg_dev)[:, :2] # (N, 2)
path_dev_h = dev_in_heading * inv_scale
# --- Path command (3D, local frame) ---
path_cmd = torch.cat([self._cmd_vel, self._cmd_yaw_rate], dim=-1) # (N, 3)
# --- Command velocities in root frame (3D + 3D) ---
cmd_vel_3d = torch.cat([self._cmd_vel, self._zeros], dim=-1) # (N, 3)
cmd_linvel_root = quat_rotate_inv(root_in_path, cmd_vel_3d) # (N, 3)
cmd_angvel_3d = torch.cat([self._zeros, self._zeros, self._cmd_yaw_rate], dim=-1) # (N, 3)
cmd_angvel_root = quat_rotate_inv(root_in_path, cmd_angvel_3d) # (N, 3)
# --- Phase encoding (4D) ---
torch.sin(torch.outer(self._phase, self._freq_2pi).add_(self._offset), out=self._phase_enc)
# --- Actual velocities in root frame (3D + 3D) ---
world_linvel = self.sim_wrapper.u_i[:, 0, :3]
world_angvel = self.sim_wrapper.u_i[:, 0, 3:]
root_linvel = quat_rotate_inv(root_quat, world_linvel) # (N, 3)
root_angvel = quat_rotate_inv(root_quat, world_angvel) # (N, 3)
# --- Pelvis height command and error (2D) ---
actual_height = self.sim_wrapper.q_i[:, 0, 2:3] # (N, 1)
height_error = (actual_height - self._cmd_height) / self.cfg["height_error_scale"] # (N, 1)
# --- Build full 94D observation ---
i = 0
self._obs_buffer[:, i : i + 9] = ori_9d
i += 9
self._obs_buffer[:, i : i + 2] = path_dev
i += 2
self._obs_buffer[:, i : i + 2] = path_dev_h
i += 2
self._obs_buffer[:, i : i + 3] = path_cmd
i += 3
self._obs_buffer[:, i : i + 3] = cmd_linvel_root
i += 3
self._obs_buffer[:, i : i + 3] = cmd_angvel_root
i += 3
self._obs_buffer[:, i : i + 4] = self._phase_enc
i += 4
self._obs_buffer[:, i : i + 3] = root_linvel
i += 3
self._obs_buffer[:, i : i + 3] = root_angvel
i += 3
self._obs_buffer[:, i : i + 1] = self._cmd_height
i += 1
self._obs_buffer[:, i : i + 1] = height_error
i += 1
self._obs_buffer[:, i : i + 60] = base_no_root
# i += 60 → total = 94
# Policy inference
with torch.no_grad():
if self.policy is not None:
self.actions[:] = self.policy(self._obs_buffer)
else:
self.actions[:] = 2.0 * torch.rand_like(self.actions) - 1.0
# Write action targets to implicit PD controller
self._apply_actions()
# Step physics for control_decimation substeps
for _ in range(self.control_decimation):
self.sim_wrapper.step()
def step(self):
"""One RL step: check reset -> joystick -> observe -> infer -> apply -> simulate."""
if self.joystick.check_reset():
self.reset()
self.joystick.update()
self.update_input()
self.sim_step()
def render(self):
"""Render the current frame."""
self.sim_wrapper.render()
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="DR Legs walk policy play example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument(
"--headless",
action=argparse.BooleanOptionalAction,
default=False,
help="Run in headless mode",
)
parser.add_argument("--num-steps", type=int, default=10000, help="Steps for headless mode")
parser.add_argument(
"--control-decimation",
type=int,
default=None,
help="Number of physics substeps per RL step (overrides YAML)",
)
parser.add_argument(
"--sim-dt", type=float, default=None, help="Physics substep duration in seconds (overrides YAML)"
)
parser.add_argument(
"--policy", type=str, default=None, help="Path to an rsl_rl checkpoint .pt file (overrides asset default)"
)
parser.add_argument(
"--mode",
choices=["sync", "async"],
default="sync",
help="Sim loop mode: sync (default) or async",
)
parser.add_argument(
"--render-fps",
type=float,
default=30.0,
help="Target render FPS for async mode (default: 30)",
)
args = parser.parse_args()
np.set_printoptions(linewidth=20000, precision=6, threshold=10000, suppress=True)
msg.set_log_level(msg.LogLevel.INFO)
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
msg.info(f"device: {device}")
# Convert warp device to torch device string
torch_device = "cuda" if device.is_cuda else "cpu"
# Load config from YAML (with hardcoded fallback defaults)
asset_path = newton.utils.download_asset("disneyresearch")
config = _load_drlegs_config(asset_path)
# CLI overrides
if args.sim_dt is not None:
config["sim_dt"] = args.sim_dt
if args.control_decimation is not None:
config["control_decimation"] = args.control_decimation
# Load policy: explicit --policy flag > asset default > random actions
policy = None
if args.policy:
policy = _load_policy_checkpoint(args.policy, device=torch_device)
msg.info(f"Loaded policy from: {args.policy}")
else:
default_policy = asset_path / "dr_legs" / "rl_policies" / config["policy_file"]
if default_policy.exists():
policy = _load_policy_checkpoint(str(default_policy), device=torch_device)
msg.info(f"Loaded default policy from: {default_policy}")
else:
msg.info(f"No policy at {default_policy} -- using random actions")
example = Example(
config=config,
device=device,
policy=policy,
headless=args.headless,
max_steps=args.num_steps,
)
try:
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
else:
msg.notif(f"Running in Viewer mode ({args.mode})...")
if hasattr(example.viewer, "set_camera"):
example.viewer.set_camera(wp.vec3(0.6, 0.6, 0.3), -10.0, 225.0)
SimulationRunner(example, mode=args.mode, render_fps=args.render_fps).run()
except KeyboardInterrupt:
pass
finally:
example.joystick.close()
================================================
FILE: newton/_src/solvers/kamino/examples/rl/joystick.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""General-purpose Xbox gamepad / keyboard controller for RL inference.
Reads Xbox 360/One controller input (or keyboard via the viewer) and
provides velocity commands and head pose deltas. Optionally integrates
a 2-D path (heading + position) from the velocity commands.
Input is selected automatically:
1. Xbox gamepad (if ``xbox360controller`` package is installed and a pad is
connected).
2. Keyboard via the 3D viewer window (if a viewer is provided).
3. No-op — zero commands, robot stands still.
Gamepad mapping::
Left stick Y forward / backward velocity
Left stick X yaw rate (angular velocity)
Triggers (L minus R) lateral velocity
Right stick Y head pitch (look up / down)
Right stick X head yaw (look left / right)
RB (right bumper) turbo (hold for higher velocity limits)
Select / Back reset
Keyboard layout (viewer window must have focus)::
I / K — forward / backward
J / L — strafe left / right
U / O — turn left / right
T / G — head pitch up / down
F / H — head yaw left / right
P — reset
"""
from __future__ import annotations
# Python
import dataclasses
# Thirdparty
import torch # noqa: TID253
from newton._src.solvers.kamino.examples.rl.utils import (
RateLimitedValue,
_deadband,
_LowPassFilter,
_scale_asym,
yaw_apply_2d,
)
@dataclasses.dataclass
class JoystickConfig:
"""Velocity limits and turbo parameters for :class:`JoystickController`.
Each velocity axis has a *base* value (no turbo) and a *turbo* delta
that is blended in as turbo_alpha ramps from 0 → 1::
effective_max = base + turbo_alpha * turbo
"""
# Velocity limits
forward_velocity_base: float = 0.3
forward_velocity_turbo: float = 0.3
lateral_velocity_base: float = 0.15
lateral_velocity_turbo: float = 0.15
angular_velocity_base: float = 1.0
angular_velocity_turbo: float = 0.75
# Head limits
head_pitch_up: float = 1.0
head_pitch_down: float = 0.6
head_yaw_max: float = 0.9
# Input processing
axis_deadband: float = 0.2
trigger_deadband: float = 0.2
cutoff_hz: float = 10.0
# Path integration
path_deviation_max: float = 0.1
# Turbo ramp rate
turbo_rate: float = 2.0
def forward_velocity_max(self, turbo_alpha: float) -> float:
return self.forward_velocity_base + turbo_alpha * self.forward_velocity_turbo
def lateral_velocity_max(self, turbo_alpha: float) -> float:
return self.lateral_velocity_base + turbo_alpha * self.lateral_velocity_turbo
def angular_velocity_max(self, turbo_alpha: float) -> float:
return self.angular_velocity_base + turbo_alpha * self.angular_velocity_turbo
class JoystickController:
"""General-purpose Xbox gamepad / keyboard controller for RL inference.
Reads gamepad axes or keyboard keys and exposes command outputs as
attributes. Optionally integrates a 2-D path (heading + position)
from the velocity commands.
Gamepad mapping:
Left stick Y -> forward velocity
Left stick X -> yaw rate (angular velocity)
Triggers (L minus R) -> lateral velocity
Right stick Y -> neck pitch
Right stick X -> neck yaw
RB (right bumper) -> turbo (hold)
Select / Back -> reset
Keyboard mapping (see module docstring for layout).
Output attributes (updated each :meth:`update` call):
``forward_velocity`` Forward velocity (positive = forward)
``lateral_velocity`` Lateral velocity (positive = strafe left)
``angular_velocity`` Angular velocity (positive = turn left)
``head_pitch`` Head pitch command (positive = look up)
``head_yaw`` Head yaw command (positive = look left)
``turbo_alpha`` Current turbo blend factor (0.0 - 1.0)
Path state (when ``root_pos_2d`` is passed to :meth:`update`):
``path_heading`` Integrated heading ``(num_worlds, 1)``
``path_position`` Integrated position ``(num_worlds, 2)``
"""
def __init__(
self,
dt: float,
viewer=None,
num_worlds: int = 1,
device: str = "cuda:0",
config: JoystickConfig | None = None,
) -> None:
cfg = config or JoystickConfig()
self._cfg = cfg
self._dt = dt
self._viewer = viewer
self._num_worlds = num_worlds
self._device = device
# Low-pass filters (named by semantic axis)
hz = cfg.cutoff_hz
self._forward_filter = _LowPassFilter(hz, dt)
self._lateral_filter = _LowPassFilter(hz, dt)
self._angular_filter = _LowPassFilter(hz, dt)
self._head_pitch_filter = _LowPassFilter(hz, dt)
self._head_yaw_filter = _LowPassFilter(hz, dt)
# Turbo ramp (rate-limited 0→1 blend)
self._turbo = RateLimitedValue(cfg.turbo_rate, dt)
# Path state (per-world)
self.path_heading = torch.zeros(num_worlds, 1, device=device)
self.path_position = torch.zeros(num_worlds, 2, device=device)
# Command outputs (updated by update())
self.forward_velocity: float = 0.0
self.lateral_velocity: float = 0.0
self.angular_velocity: float = 0.0
self.head_pitch: float = 0.0
self.head_yaw: float = 0.0
self.turbo_alpha: float = 0.0
# Pre-allocated command velocity buffer (eliminates per-step torch.tensor())
self._cmd_vel_buf = torch.zeros(1, 2, device=device)
# Reset edge-detection state
self._reset_prev = False
# --- Input mode detection ---
self._controller = None
self._mode: str | None = None # "joystick", "keyboard", or None
try:
# Thirdparty
from xbox360controller import Xbox360Controller # noqa: PLC0415
self._controller = Xbox360Controller(0, axis_threshold=0.015)
self._mode = "joystick"
print("Joystick connected.")
except Exception:
if viewer is not None and hasattr(viewer, "is_key_down"):
self._mode = "keyboard"
print(
"No joystick found. Using keyboard controls:\n"
" I/K — forward/backward J/L — strafe left/right\n"
" U/O — turn left/right T/G — look up/down\n"
" F/H — look left/right\n"
" P — reset"
)
else:
print("No joystick or keyboard available. Commands will be zero.")
def _read_input(self) -> tuple[float, float, float, float, float]:
"""Read controller input as semantic axes.
Returns:
``(forward, lateral, angular, head_pitch, head_yaw)``
Sign convention — positive means:
forward : walk forward
lateral : strafe left
angular : turn left (CCW)
head_pitch: look up
head_yaw : look left
"""
if self._mode == "joystick":
c = self._controller
return (
-c.axis_l.y, # forward (negate: HW up is negative)
c.trigger_l.value - c.trigger_r.value, # lateral (L trigger = strafe left)
-c.axis_l.x, # angular (negate: HW left is negative)
-c.axis_r.y, # head pitch (negate: HW up is negative)
-c.axis_r.x, # head yaw (negate: HW left is negative)
)
# Keyboard fallback
v = self._viewer
def _axis(neg_key: str, pos_key: str) -> float:
val = 0.0
if v.is_key_down(neg_key):
val -= 1.0
if v.is_key_down(pos_key):
val += 1.0
return val
return (
_axis("k", "i"), # forward: I = forward(+), K = backward(-)
_axis("l", "j"), # lateral: J = left(+), L = right(-)
_axis("o", "u"), # angular: U = left(+), O = right(-)
_axis("t", "g"), # head pitch: T = up(+), G = down(-)
_axis("h", "f"), # head yaw: F = left(+), H = right(-)
)
def _read_turbo(self) -> float:
"""Return 1.0 if turbo is engaged, 0.0 otherwise."""
if self._mode == "joystick":
return 1.0 if self._controller.button_trigger_r.is_pressed else 0.0
return 0.0
def update(self, root_pos_2d: torch.Tensor | None = None) -> None:
"""Read input, compute commands, and optionally advance the path.
Args:
root_pos_2d: Current robot XY position ``(num_worlds, 2)`` for
path deviation clipping. When ``None``, path integration
is skipped.
"""
if self._mode is None:
return
cfg = self._cfg
# --- Read & filter ---
fwd_raw, lat_raw, ang_raw, npitch_raw, nyaw_raw = self._read_input()
fwd = _deadband(self._forward_filter.update(fwd_raw), cfg.axis_deadband)
lat = _deadband(self._lateral_filter.update(lat_raw), cfg.trigger_deadband)
ang = _deadband(self._angular_filter.update(ang_raw), cfg.axis_deadband)
npitch = _deadband(self._head_pitch_filter.update(npitch_raw), cfg.axis_deadband)
nyaw = _deadband(self._head_yaw_filter.update(nyaw_raw), cfg.axis_deadband)
# --- Turbo ---
self.turbo_alpha = self._turbo.update(self._read_turbo())
# --- Scale to physical units ---
self.forward_velocity = fwd * cfg.forward_velocity_max(self.turbo_alpha)
self.lateral_velocity = lat * cfg.lateral_velocity_max(self.turbo_alpha)
self.angular_velocity = ang * cfg.angular_velocity_max(self.turbo_alpha)
self.head_pitch = _scale_asym(npitch, cfg.head_pitch_down, cfg.head_pitch_up)
self.head_yaw = nyaw * cfg.head_yaw_max
# --- Path integration ---
if root_pos_2d is not None:
dt = self._dt
self._cmd_vel_buf[0, 0] = self.forward_velocity
self._cmd_vel_buf[0, 1] = self.lateral_velocity
# Mid-point heading integration
mid_heading = self.path_heading + 0.5 * dt * self.angular_velocity
self.path_position += yaw_apply_2d(mid_heading, self._cmd_vel_buf) * dt
# Update heading
self.path_heading += self.angular_velocity * dt
# Clip path deviation to root position
diff = self.path_position - root_pos_2d
clipped = diff.renorm(p=2, dim=0, maxnorm=cfg.path_deviation_max)
self.path_position[:] = root_pos_2d + clipped
def close(self) -> None:
"""Release gamepad resources so the process can exit cleanly."""
if self._controller is not None:
try:
self._controller.close()
except Exception:
pass
self._controller = None
def check_reset(self) -> bool:
"""Return True on the rising edge of the reset input.
Gamepad: Select/Back button. Keyboard: ``p`` key.
"""
pressed = False
if self._mode == "joystick":
pressed = bool(self._controller.button_select.is_pressed)
# Also allow keyboard 'p' when a gamepad is connected
if not pressed and self._viewer is not None and hasattr(self._viewer, "is_key_down"):
pressed = bool(self._viewer.is_key_down("p"))
elif self._mode == "keyboard" and self._viewer is not None:
pressed = bool(self._viewer.is_key_down("p"))
triggered = pressed and not self._reset_prev
self._reset_prev = pressed
return triggered
def reset(self, root_pos_2d: torch.Tensor | None = None, root_yaw: torch.Tensor | None = None) -> None:
"""Reset path state and filters.
Args:
root_pos_2d: Current robot XY position ``(num_worlds, 2)``.
root_yaw: Current robot yaw angle ``(num_worlds, 1)``.
"""
if root_yaw is not None:
self.path_heading[:] = root_yaw
if root_pos_2d is not None:
self.path_position[:] = root_pos_2d
self._forward_filter.reset()
self._lateral_filter.reset()
self._angular_filter.reset()
self._head_pitch_filter.reset()
self._head_yaw_filter.reset()
self._turbo.reset()
def set_dt(self, dt: float) -> None:
"""Change the timestep used for path integration and filtering."""
self._dt = dt
hz = self._cfg.cutoff_hz
self._forward_filter = _LowPassFilter(hz, dt)
self._lateral_filter = _LowPassFilter(hz, dt)
self._angular_filter = _LowPassFilter(hz, dt)
self._head_pitch_filter = _LowPassFilter(hz, dt)
self._head_yaw_filter = _LowPassFilter(hz, dt)
self._turbo.dt = dt
================================================
FILE: newton/_src/solvers/kamino/examples/rl/observations.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
# Python
from abc import ABC, abstractmethod
# Thirdparty
import torch # noqa: TID253
import warp as wp
from newton._src.solvers.kamino._src.utils.sim import Simulator
from newton._src.solvers.kamino.examples.rl.simulation import RigidBodySim
from newton._src.solvers.kamino.examples.rl.utils import StackedIndices, periodic_encoding
# ---------------------------------------------------------------------------
# Warp helpers for BipedalObservation
# ---------------------------------------------------------------------------
_Z_AXIS = wp.constant(wp.vec3(0.0, 0.0, 1.0))
@wp.func
def _projected_yaw(q: wp.quat) -> float:
"""Extract the yaw angle from a quaternion (no warp builtin equivalent)."""
qx = q[0]
qy = q[1]
qz = q[2]
qw = q[3]
return wp.atan2(2.0 * (qz * qw + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
@wp.func
def _write_vec3(obs: wp.array(dtype=wp.float32), idx: int, v: wp.vec3):
obs[idx + 0] = v[0]
obs[idx + 1] = v[1]
obs[idx + 2] = v[2]
# Observation group indices into the offsets array (must match _OBS_NAMES order).
_OBS_ORI = wp.constant(0)
_OBS_PATH_DEV = wp.constant(1)
_OBS_PATH_DEV_H = wp.constant(2)
_OBS_PATH_CMD = wp.constant(3)
_OBS_PATH_LIN_VEL = wp.constant(4)
_OBS_PATH_ANG_VEL = wp.constant(5)
_OBS_PHASE_ENC = wp.constant(6)
_OBS_NECK = wp.constant(7)
_OBS_ROOT_LIN_VEL = wp.constant(8)
_OBS_ROOT_ANG_VEL = wp.constant(9)
_OBS_JOINT_POS = wp.constant(10)
_OBS_JOINT_VEL = wp.constant(11)
# Python-side list matching the constant order above.
_OBS_NAMES = [
"orientation_root_to_path",
"path_deviation",
"path_deviation_in_heading",
"path_cmd",
"path_lin_vel_in_root",
"path_ang_vel_in_root",
"phase_encoding",
"neck_cmd",
"root_lin_vel_in_root",
"root_ang_vel_in_root",
"normalized_joint_positions",
"normalized_joint_velocities",
]
@wp.kernel
def _compute_bipedal_obs_core(
obs: wp.array(dtype=wp.float32),
q_i: wp.array(dtype=wp.float32),
u_i: wp.array(dtype=wp.float32),
q_j: wp.array(dtype=wp.float32),
dq_j: wp.array(dtype=wp.float32),
command: wp.array(dtype=wp.float32),
phase: wp.array(dtype=wp.float32),
freq_2pi: wp.array(dtype=wp.float32),
offset_enc: wp.array(dtype=wp.float32),
joint_default: wp.array(dtype=wp.float32),
joint_range: wp.array(dtype=wp.float32),
obs_offsets: wp.array(dtype=wp.int32),
num_bodies: int,
num_joint_coords: int,
num_joint_dofs: int,
num_obs: int,
cmd_dim: int,
inv_path_dev_scale: float,
inv_joint_vel_scale: float,
phase_enc_dim: int,
num_joints: int,
):
w = wp.tid()
# Flat array strides
qi_base = w * num_bodies * 7
ui_base = w * num_bodies * 6
qj_base = w * num_joint_coords
dqj_base = w * num_joint_dofs
cmd_base = w * cmd_dim
o = w * num_obs
# Root pose & velocities (body 0)
root_quat = wp.quat(q_i[qi_base + 3], q_i[qi_base + 4], q_i[qi_base + 5], q_i[qi_base + 6])
root_lin_vel = wp.vec3(u_i[ui_base + 0], u_i[ui_base + 1], u_i[ui_base + 2])
root_ang_vel = wp.vec3(u_i[ui_base + 3], u_i[ui_base + 4], u_i[ui_base + 5])
# Commands
path_heading = command[cmd_base + 0]
cmd_vel = wp.vec3(command[cmd_base + 3], command[cmd_base + 4], 0.0)
cmd_yaw_rate = command[cmd_base + 5]
# root_orientation_in_path = inv(path_quat) * root_quat
path_quat = wp.quat_from_axis_angle(_Z_AXIS, path_heading)
root_in_path = wp.mul(wp.quat_inverse(path_quat), root_quat)
# Orientation as flattened 3x3 rotation matrix
rot = wp.quat_to_matrix(root_in_path)
oi = o + obs_offsets[_OBS_ORI]
for i in range(3):
for j in range(3):
obs[oi + i * 3 + j] = rot[i, j]
# Path deviation in path frame (scaled)
diff = wp.vec3(q_i[qi_base + 0] - command[cmd_base + 1], q_i[qi_base + 1] - command[cmd_base + 2], 0.0)
rtp = wp.quat_rotate_inv(path_quat, diff)
obs[o + obs_offsets[_OBS_PATH_DEV] + 0] = rtp[0] * inv_path_dev_scale
obs[o + obs_offsets[_OBS_PATH_DEV] + 1] = rtp[1] * inv_path_dev_scale
# Path deviation in heading frame (scaled)
root_heading = _projected_yaw(root_in_path)
heading_quat = wp.quat_from_axis_angle(_Z_AXIS, root_heading)
pth = wp.quat_rotate_inv(heading_quat, wp.vec3(-rtp[0], -rtp[1], 0.0))
obs[o + obs_offsets[_OBS_PATH_DEV_H] + 0] = pth[0] * inv_path_dev_scale
obs[o + obs_offsets[_OBS_PATH_DEV_H] + 1] = pth[1] * inv_path_dev_scale
# Path command
_write_vec3(obs, o + obs_offsets[_OBS_PATH_CMD], wp.vec3(cmd_vel[0], cmd_vel[1], cmd_yaw_rate))
# Path velocities in root frame
_write_vec3(obs, o + obs_offsets[_OBS_PATH_LIN_VEL], wp.quat_rotate_inv(root_in_path, cmd_vel))
_write_vec3(
obs, o + obs_offsets[_OBS_PATH_ANG_VEL], wp.quat_rotate_inv(root_in_path, wp.vec3(0.0, 0.0, cmd_yaw_rate))
)
# Phase encoding
p = phase[w]
op = o + obs_offsets[_OBS_PHASE_ENC]
for i in range(phase_enc_dim):
obs[op + i] = wp.sin(p * freq_2pi[i] + offset_enc[i])
# Neck command
on = o + obs_offsets[_OBS_NECK]
for i in range(4):
obs[on + i] = command[cmd_base + 6 + i]
# Root velocities in root frame
_write_vec3(obs, o + obs_offsets[_OBS_ROOT_LIN_VEL], wp.quat_rotate_inv(root_quat, root_lin_vel))
_write_vec3(obs, o + obs_offsets[_OBS_ROOT_ANG_VEL], wp.quat_rotate_inv(root_quat, root_ang_vel))
# Normalized joint positions & velocities
ojp = o + obs_offsets[_OBS_JOINT_POS]
ojv = o + obs_offsets[_OBS_JOINT_VEL]
for j in range(num_joints):
obs[ojp + j] = (q_j[qj_base + j] - joint_default[j]) / joint_range[j]
obs[ojv + j] = dq_j[dqj_base + j] * inv_joint_vel_scale
class PhaseRate(torch.nn.Module):
"""
Defines the mapping between robot measurements and a pretrained phase rate.
"""
def __init__(self, path, obs_cmd_range) -> None:
super().__init__()
self.obs_cmd_idx = list(obs_cmd_range)
# Load pre-trained model
model = torch.load(path, weights_only=False)
model.eval()
# Turn off gradients of the pretrained parameters
for param in model.parameters():
param.requires_grad_(False)
super().add_module("_phase_rate", model)
def forward(self, input: torch.Tensor) -> torch.Tensor:
path_cmd = input[:, self.obs_cmd_idx]
return self._phase_rate(path_cmd)
# ---------------------------------------------------------------------------
# Warp helpers for BipedalObservation
# ---------------------------------------------------------------------------
_Z_AXIS = wp.constant(wp.vec3(0.0, 0.0, 1.0))
@wp.func
def _projected_yaw(q: wp.quat) -> float:
"""Extract the yaw angle from a quaternion (no warp builtin equivalent)."""
qx = q[0]
qy = q[1]
qz = q[2]
qw = q[3]
return wp.atan2(2.0 * (qz * qw + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
@wp.func
def _write_vec3(obs: wp.array(dtype=wp.float32), idx: int, v: wp.vec3):
obs[idx + 0] = v[0]
obs[idx + 1] = v[1]
obs[idx + 2] = v[2]
# Observation group indices into the offsets array (must match _OBS_NAMES order).
_OBS_ORI = wp.constant(0)
_OBS_PATH_DEV = wp.constant(1)
_OBS_PATH_DEV_H = wp.constant(2)
_OBS_PATH_CMD = wp.constant(3)
_OBS_PATH_LIN_VEL = wp.constant(4)
_OBS_PATH_ANG_VEL = wp.constant(5)
_OBS_PHASE_ENC = wp.constant(6)
_OBS_NECK = wp.constant(7)
_OBS_ROOT_LIN_VEL = wp.constant(8)
_OBS_ROOT_ANG_VEL = wp.constant(9)
_OBS_JOINT_POS = wp.constant(10)
_OBS_JOINT_VEL = wp.constant(11)
# Python-side list matching the constant order above.
_OBS_NAMES = [
"orientation_root_to_path",
"path_deviation",
"path_deviation_in_heading",
"path_cmd",
"path_lin_vel_in_root",
"path_ang_vel_in_root",
"phase_encoding",
"neck_cmd",
"root_lin_vel_in_root",
"root_ang_vel_in_root",
"normalized_joint_positions",
"normalized_joint_velocities",
]
@wp.kernel
def _compute_bipedal_obs_core(
obs: wp.array(dtype=wp.float32),
q_i: wp.array(dtype=wp.float32),
u_i: wp.array(dtype=wp.float32),
q_j: wp.array(dtype=wp.float32),
dq_j: wp.array(dtype=wp.float32),
command: wp.array(dtype=wp.float32),
phase: wp.array(dtype=wp.float32),
freq_2pi: wp.array(dtype=wp.float32),
offset_enc: wp.array(dtype=wp.float32),
joint_default: wp.array(dtype=wp.float32),
joint_range: wp.array(dtype=wp.float32),
obs_offsets: wp.array(dtype=wp.int32),
num_bodies: int,
num_joint_coords: int,
num_joint_dofs: int,
num_obs: int,
cmd_dim: int,
inv_path_dev_scale: float,
inv_joint_vel_scale: float,
phase_enc_dim: int,
num_joints: int,
):
w = wp.tid()
# Flat array strides
qi_base = w * num_bodies * 7
ui_base = w * num_bodies * 6
qj_base = w * num_joint_coords
dqj_base = w * num_joint_dofs
cmd_base = w * cmd_dim
o = w * num_obs
# Root pose & velocities (body 0)
root_quat = wp.quat(q_i[qi_base + 3], q_i[qi_base + 4], q_i[qi_base + 5], q_i[qi_base + 6])
root_lin_vel = wp.vec3(u_i[ui_base + 0], u_i[ui_base + 1], u_i[ui_base + 2])
root_ang_vel = wp.vec3(u_i[ui_base + 3], u_i[ui_base + 4], u_i[ui_base + 5])
# Commands
path_heading = command[cmd_base + 0]
cmd_vel = wp.vec3(command[cmd_base + 3], command[cmd_base + 4], 0.0)
cmd_yaw_rate = command[cmd_base + 5]
# root_orientation_in_path = inv(path_quat) * root_quat
path_quat = wp.quat_from_axis_angle(_Z_AXIS, path_heading)
root_in_path = wp.mul(wp.quat_inverse(path_quat), root_quat)
# Orientation as flattened 3x3 rotation matrix
rot = wp.quat_to_matrix(root_in_path)
oi = o + obs_offsets[_OBS_ORI]
for i in range(3):
for j in range(3):
obs[oi + i * 3 + j] = rot[i, j]
# Path deviation in path frame (scaled)
diff = wp.vec3(q_i[qi_base + 0] - command[cmd_base + 1], q_i[qi_base + 1] - command[cmd_base + 2], 0.0)
rtp = wp.quat_rotate_inv(path_quat, diff)
obs[o + obs_offsets[_OBS_PATH_DEV] + 0] = rtp[0] * inv_path_dev_scale
obs[o + obs_offsets[_OBS_PATH_DEV] + 1] = rtp[1] * inv_path_dev_scale
# Path deviation in heading frame (scaled)
root_heading = _projected_yaw(root_in_path)
heading_quat = wp.quat_from_axis_angle(_Z_AXIS, root_heading)
pth = wp.quat_rotate_inv(heading_quat, wp.vec3(-rtp[0], -rtp[1], 0.0))
obs[o + obs_offsets[_OBS_PATH_DEV_H] + 0] = pth[0] * inv_path_dev_scale
obs[o + obs_offsets[_OBS_PATH_DEV_H] + 1] = pth[1] * inv_path_dev_scale
# Path command
_write_vec3(obs, o + obs_offsets[_OBS_PATH_CMD], wp.vec3(cmd_vel[0], cmd_vel[1], cmd_yaw_rate))
# Path velocities in root frame
_write_vec3(obs, o + obs_offsets[_OBS_PATH_LIN_VEL], wp.quat_rotate_inv(root_in_path, cmd_vel))
_write_vec3(
obs, o + obs_offsets[_OBS_PATH_ANG_VEL], wp.quat_rotate_inv(root_in_path, wp.vec3(0.0, 0.0, cmd_yaw_rate))
)
# Phase encoding
p = phase[w]
op = o + obs_offsets[_OBS_PHASE_ENC]
for i in range(phase_enc_dim):
obs[op + i] = wp.sin(p * freq_2pi[i] + offset_enc[i])
# Neck command
on = o + obs_offsets[_OBS_NECK]
for i in range(4):
obs[on + i] = command[cmd_base + 6 + i]
# Root velocities in root frame
_write_vec3(obs, o + obs_offsets[_OBS_ROOT_LIN_VEL], wp.quat_rotate_inv(root_quat, root_lin_vel))
_write_vec3(obs, o + obs_offsets[_OBS_ROOT_ANG_VEL], wp.quat_rotate_inv(root_quat, root_ang_vel))
# Normalized joint positions & velocities
ojp = o + obs_offsets[_OBS_JOINT_POS]
ojv = o + obs_offsets[_OBS_JOINT_VEL]
for j in range(num_joints):
obs[ojp + j] = (q_j[qj_base + j] - joint_default[j]) / joint_range[j]
obs[ojv + j] = dq_j[dqj_base + j] * inv_joint_vel_scale
class PhaseRate(torch.nn.Module):
"""
Defines the mapping between robot measurements and a pretrained phase rate.
"""
def __init__(self, path, obs_cmd_range) -> None:
super().__init__()
self.obs_cmd_idx = list(obs_cmd_range)
# Load pre-trained model
model = torch.load(path, weights_only=False)
model.eval()
# Turn off gradients of the pretrained parameters
for param in model.parameters():
param.requires_grad_(False)
super().add_module("_phase_rate", model)
def forward(self, input: torch.Tensor) -> torch.Tensor:
path_cmd = input[:, self.obs_cmd_idx]
return self._phase_rate(path_cmd)
class ObservationBuilder(ABC):
"""Base class for building observation tensors from a Kamino Simulator.
Subclasses define which signals to extract and concatenate. The builder
maintains internal buffers (e.g. action history) and provides a uniform
``compute()`` interface suitable for inference loops.
Args:
sim: A Kamino ``Simulator`` instance.
num_worlds: Number of parallel simulation worlds.
device: Torch device string (e.g. ``"cuda:0"``).
command_dim: Dimensionality of the external command vector
(0 means no command input; todo reserved for future joystick / keyboard velocity commands).
"""
def __init__(
self,
sim: Simulator,
num_worlds: int,
device: str,
command_dim: int = 0,
) -> None:
self._sim = sim
self._num_worlds = num_worlds
self._device = device
self._command_dim = command_dim
# Pre-allocated command tensor (empty if command_dim == 0).
self._command: torch.Tensor = torch.zeros(
(num_worlds, max(command_dim, 0)),
device=device,
dtype=torch.float32,
)
@property
@abstractmethod
def num_observations(self) -> int:
"""Total observation dimensionality (per environment)."""
...
@property
def command_dim(self) -> int:
"""Dimensionality of the external command vector."""
return self._command_dim
@property
def command(self) -> torch.Tensor:
"""Current command tensor, shape ``(num_worlds, command_dim)``.
External code (keyboard handler, joystick) writes into this tensor
so that ``compute()`` can include it in the observation.
"""
return self._command
@command.setter
def command(self, value: torch.Tensor) -> None:
self._command = value
@abstractmethod
def compute(self, actions: torch.Tensor | None = None) -> torch.Tensor:
"""Build the observation tensor from the current simulator state.
Args:
actions: The most recent actions applied, shape
``(num_worlds, num_actions)``. Pass ``None`` on the very
first step (before any action has been applied).
Returns:
Observation tensor of shape ``(num_worlds, num_observations)``.
"""
...
@abstractmethod
def reset(self, env_ids: torch.Tensor | None = None) -> None:
"""Reset internal buffers (e.g. action history) for given envs.
Args:
env_ids: Which environments to reset. ``None`` resets all.
"""
...
def _get_joint_positions(self) -> torch.Tensor:
"""Joint positions as a PyTorch tensor ``(num_worlds, num_joint_coords)``.
Zero-copy view of the underlying Warp array.
"""
num_joint_coords = self._sim.model.size.max_of_num_joint_coords
return wp.to_torch(self._sim.state.q_j).reshape(self._num_worlds, num_joint_coords)
def _get_joint_velocities(self) -> torch.Tensor:
"""Joint velocities as a PyTorch tensor ``(num_worlds, num_joint_dofs)``.
Zero-copy view of the underlying Warp array.
"""
num_joint_dofs = self._sim.model.size.max_of_num_joint_dofs
return wp.to_torch(self._sim.state.dq_j).reshape(self._num_worlds, num_joint_dofs)
def _get_root_positions(self) -> torch.Tensor:
"""Root body positions as a PyTorch tensor ``(num_worlds, 3)``.
Zero-copy view of the underlying Warp array (body 0, xyz).
"""
num_bodies = self._sim.model.size.max_of_num_bodies
q_i = wp.to_torch(self._sim.state.q_i).reshape(self._num_worlds, num_bodies, 7)
return q_i[:, 0, :3]
class DrlegsBaseObservation(ObservationBuilder):
"""Base observation builder for DR Legs.
Observation vector (63D):
* root position (3D — pelvis xyz)
* DOF positions (36D — all joints, including passive linkages)
* action history t-0 (12D — actuated joints, current step)
* action history t-1 (12D — actuated joints, previous step)
Args:
body_sim: A :class:`RigidBodySim` instance.
action_scale: Scale applied to raw actions before storing in history.
"""
def __init__(
self,
body_sim: RigidBodySim,
action_scale: float = 0.25,
) -> None:
super().__init__(
sim=body_sim.sim,
num_worlds=body_sim.num_worlds,
device=body_sim.torch_device,
command_dim=0,
)
self._body_sim = body_sim
self._num_actions = body_sim.num_actuated
self._num_dofs = body_sim.num_joint_coords
self._action_scale = action_scale
# Action history buffers (actuated joints only).
self._action_history: torch.Tensor = torch.zeros(
(body_sim.num_worlds, self._num_actions),
device=body_sim.torch_device,
dtype=torch.float32,
)
self._action_history_prev: torch.Tensor = torch.zeros(
(body_sim.num_worlds, self._num_actions),
device=body_sim.torch_device,
dtype=torch.float32,
)
# Pre-allocated observation buffer (eliminates torch.cat)
self._obs_buffer = torch.zeros(
(body_sim.num_worlds, 3 + self._num_dofs + 2 * self._num_actions),
device=body_sim.torch_device,
dtype=torch.float32,
)
@property
def num_observations(self) -> int:
return 3 + self._num_dofs + self._num_actions + self._num_actions # 63
def compute(self, actions: torch.Tensor | None = None) -> torch.Tensor:
if actions is not None:
self._action_history_prev[:] = self._action_history
self._action_history[:] = self._action_scale * actions
root_pos = self._get_root_positions()
q_j = self._get_joint_positions()
d = self._num_dofs
a = self._num_actions
self._obs_buffer[:, :3] = root_pos
self._obs_buffer[:, 3 : 3 + d] = q_j
self._obs_buffer[:, 3 + d : 3 + d + a] = self._action_history
self._obs_buffer[:, 3 + d + a :] = self._action_history_prev
return self._obs_buffer
def reset(self, env_ids: torch.Tensor | None = None) -> None:
if env_ids is None:
self._action_history.zero_()
self._action_history_prev.zero_()
else:
self._action_history[env_ids] = 0.0
self._action_history_prev[env_ids] = 0.0
# ---------------------------------------------------------------------------
# BipedalObservation — standalone Bipedal observation builder
# ---------------------------------------------------------------------------
class BipedalObservation(ObservationBuilder, torch.nn.Module):
"""Bipedal observation builder for inference.
Reads commands from :pyattr:`command` (shape ``(num_worlds, 10)``),
simulator state from a :class:`RigidBodySim`, and maintains action
history and gait phase internally.
Command tensor layout (10 dims)::
[0] path_heading (1)
[1:3] path_position_2d (2)
[3:5] cmd_vel_xy (2)
[5] cmd_yaw_rate (1)
[6:10] neck_cmd (4)
Phase is managed internally via a pretrained :class:`PhaseRate` model
that predicts the gait frequency from the path command.
"""
# -- Command tensor indices --
CMD_DIM = 10
CMD_PATH_HEADING = 0
CMD_PATH_POSITION = slice(1, 3)
CMD_VEL = slice(3, 5)
CMD_YAW_RATE = 5
CMD_HEAD = slice(6, 10)
def __init__(
self,
body_sim: RigidBodySim,
joint_position_default: list[float],
joint_position_range: list[float],
joint_velocity_scale: float,
path_deviation_scale: float,
phase_embedding_dim: int,
phase_rate_policy_path: str,
dt: float = 0.02,
num_joints: int = 14,
) -> None:
torch.nn.Module.__init__(self)
ObservationBuilder.__init__(
self,
sim=body_sim.sim,
num_worlds=body_sim.num_worlds,
device=body_sim.torch_device,
command_dim=self.CMD_DIM,
)
self._body_sim = body_sim
self._dt = dt
self.joint_velocity_scale = joint_velocity_scale
self.path_deviation_scale = path_deviation_scale
# Joint normalization
self.register_buffer(
"_joint_position_default",
torch.tensor(joint_position_default, dtype=torch.float),
)
self.register_buffer(
"_joint_position_range",
torch.tensor(joint_position_range, dtype=torch.float),
)
# Phase encoding
freq_2pi, offset = periodic_encoding(k=phase_embedding_dim // 2)
self.register_buffer("_freq_2pi", torch.from_numpy(freq_2pi).to(dtype=torch.float))
self.register_buffer("_offset", torch.from_numpy(offset).to(dtype=torch.float))
phase_encoding_size = len(freq_2pi)
# History size
self.history_size = num_joints * 2
# Observation structure (matches the GTC training layout)
self.obs_idx = StackedIndices(
[
("orientation_root_to_path", 9),
("path_deviation", 2),
("path_deviation_in_heading", 2),
("path_cmd", 3),
("path_lin_vel_in_root", 3),
("path_ang_vel_in_root", 3),
("phase_encoding", phase_encoding_size),
("neck_cmd", 4),
("root_lin_vel_in_root", 3),
("root_ang_vel_in_root", 3),
("normalized_joint_positions", num_joints),
("normalized_joint_velocities", num_joints),
("history", self.history_size),
]
)
self.num_obs = len(self.obs_idx)
# Action history (normalised joint-position setpoints)
self._action_hist_0 = torch.zeros(self._num_worlds, num_joints, device=self._device, dtype=torch.float)
self._action_hist_1 = torch.zeros(self._num_worlds, num_joints, device=self._device, dtype=torch.float)
# Internal gait phase state
self._phase = torch.zeros(self._num_worlds, device=self._device, dtype=torch.float)
self._phase_rate = PhaseRate(phase_rate_policy_path, self.obs_idx.path_cmd)
# Cached intermediates (populated by compute, used by subclasses
# for privileged observations)
self._root_orientation_in_path: torch.Tensor | None = None
self._root_lin_vel_in_root: torch.Tensor | None = None
self._root_ang_vel_in_root: torch.Tensor | None = None
self._skip_obs = torch.empty((self._num_worlds, 0), dtype=torch.float, device=self._device)
# Move registered buffers to device
self.to(self._device)
# -- Pre-allocated observation buffer --
self._obs_buffer = torch.zeros(self._num_worlds, self.num_obs, device=self._device, dtype=torch.float)
self._wp_obs = wp.from_torch(self._obs_buffer.reshape(-1))
# Phase rate
self._phase_rate_input = torch.zeros(self._num_worlds, 3, device=self._device, dtype=torch.float)
# Warp views of simulator state
self._wp_q_i = wp.from_torch(body_sim.q_i.reshape(-1))
self._wp_u_i = wp.from_torch(body_sim.u_i.reshape(-1))
self._wp_q_j = wp.from_torch(body_sim.q_j.reshape(-1))
self._wp_dq_j = wp.from_torch(body_sim.dq_j.reshape(-1))
self._wp_command = wp.from_torch(self._command.reshape(-1))
self._wp_phase = wp.from_torch(self._phase)
self._wp_freq_2pi = wp.from_torch(self._freq_2pi)
self._wp_offset = wp.from_torch(self._offset)
self._wp_joint_default = wp.from_torch(self._joint_position_default)
self._wp_joint_range = wp.from_torch(self._joint_position_range)
# Observation group offsets from StackedIndices (matches _OBS_NAMES order)
obs_offsets = torch.tensor(
[self.obs_idx[name].start for name in _OBS_NAMES],
dtype=torch.int32,
device=self._device,
)
self._wp_obs_offsets = wp.from_torch(obs_offsets)
# Stride constants for kernel
self._num_bodies = body_sim.num_bodies
self._num_joint_coords = body_sim.num_joint_coords
self._num_joint_dofs = body_sim.num_joint_dofs
self._num_joints = num_joints
self._phase_enc_dim = phase_encoding_size
self._wp_device = body_sim.device
# Pre-computed inverse scales
self._inv_path_dev_scale = 1.0 / path_deviation_scale
self._inv_joint_vel_scale = 1.0 / joint_velocity_scale
# Action history slice indices
self._hist_start = self.obs_idx.history.start
self._hist_mid = self._hist_start + num_joints
# Indices for cached velocity views
self._root_lin_vel_start = self.obs_idx.root_lin_vel_in_root.start
self._root_ang_vel_start = self.obs_idx.root_ang_vel_in_root.start
def get_feature_module(self) -> BipedalObservation:
return self
@property
def num_observations(self) -> int:
return self.num_obs
def compute(self, setpoints: torch.Tensor | None = None) -> torch.Tensor:
"""Build the observation tensor from current simulator state.
Args:
setpoints: Latest joint-position setpoints (raw, un-normalised),
shape ``(num_worlds, num_joints)``. ``None`` on the very
first step before any action has been applied.
"""
nw = self._num_worlds
# -- Phase advance --
self._phase_rate_input[:, :2] = self._command[:, self.CMD_VEL]
self._phase_rate_input[:, 2] = self._command[:, self.CMD_YAW_RATE]
with torch.inference_mode():
rate = self._phase_rate._phase_rate(self._phase_rate_input).squeeze(-1)
self._phase.add_(rate * self._dt).remainder_(1.0)
# -- Warp kernel: obs[0:hist_start] --
wp.launch(
_compute_bipedal_obs_core,
dim=nw,
inputs=[
self._wp_obs,
self._wp_q_i,
self._wp_u_i,
self._wp_q_j,
self._wp_dq_j,
self._wp_command,
self._wp_phase,
self._wp_freq_2pi,
self._wp_offset,
self._wp_joint_default,
self._wp_joint_range,
self._wp_obs_offsets,
self._num_bodies,
self._num_joint_coords,
self._num_joint_dofs,
self.num_obs,
self.CMD_DIM,
self._inv_path_dev_scale,
self._inv_joint_vel_scale,
self._phase_enc_dim,
self._num_joints,
],
device=self._wp_device,
)
# -- Action history: pointer swap (no copy) then overwrite --
self._action_hist_0, self._action_hist_1 = self._action_hist_1, self._action_hist_0
if setpoints is not None:
torch.sub(setpoints, self._joint_position_default, out=self._action_hist_0)
self._action_hist_0.div_(self._joint_position_range)
# -- Write action history into pre-allocated buffer --
self._obs_buffer[:, self._hist_start : self._hist_mid] = self._action_hist_0
self._obs_buffer[:, self._hist_mid : self._hist_start + self.history_size] = self._action_hist_1
# -- Cache velocity views for subclasses --
s = self._root_lin_vel_start
self._root_lin_vel_in_root = self._obs_buffer[:, s : s + 3]
s = self._root_ang_vel_start
self._root_ang_vel_in_root = self._obs_buffer[:, s : s + 3]
return self._obs_buffer
def reset(self, env_ids: torch.Tensor | None = None) -> None:
"""Reset action history and phase for the given environments."""
if env_ids is None:
normalized = (self._body_sim.q_j - self._joint_position_default) / self._joint_position_range
self._action_hist_0[:] = normalized
self._action_hist_1[:] = normalized
self._phase.zero_()
else:
normalized = (self._body_sim.q_j[env_ids] - self._joint_position_default) / self._joint_position_range
self._action_hist_0[env_ids] = normalized
self._action_hist_1[env_ids] = normalized
self._phase[env_ids] = 0.0
================================================
FILE: newton/_src/solvers/kamino/examples/rl/simulation.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# RigidBodySim: Generic Kamino rigid body simulator for RL
#
# Consolidates common simulation infrastructure (model building, solver
# setup, state/control tensor wiring, contact aggregation, selective
# resets, CUDA graphs) into a single reusable class. Any robot RL
# example can use this instead of duplicating ~300 lines of boilerplate.
###########################################################################
from __future__ import annotations
# Python
import glob
import os
import threading
# Thirdparty
import torch # noqa: TID253
import warp as wp
# Kamino
import newton
from newton._src.solvers.kamino._src.core.bodies import convert_body_com_to_origin
from newton._src.solvers.kamino._src.core.control import ControlKamino
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.core.state import StateKamino
from newton._src.solvers.kamino._src.core.types import transformf, vec6f
from newton._src.solvers.kamino._src.geometry import CollisionDetector
from newton._src.solvers.kamino._src.geometry.aggregation import ContactAggregation
from newton._src.solvers.kamino._src.solver_kamino_impl import SolverKaminoImpl
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.sim import Simulator
from newton._src.solvers.kamino._src.utils.viewer import Color3, ViewerConfig
from newton._src.viewer import ViewerGL
class SimulatorFromNewton:
"""Kamino :class:`Simulator`-like wrapper initialized from a Newton :class:`~newton.Model`.
Mirrors the core API of the Kamino ``Simulator`` class but accepts an
already-finalized :class:`newton.Model` instead of a ``ModelBuilderKamino``.
Internally uses :meth:`ModelKamino.from_newton` to obtain Kamino-native
model, state, control, and solver objects.
"""
def __init__(
self,
newton_model: newton.Model,
config: Simulator.Config | None = None,
device: wp.DeviceLike = None,
):
self._device = wp.get_device(device)
if config is None:
config = Simulator.Config()
self._config = config
# Create Kamino model from Newton model
self._model: ModelKamino = ModelKamino.from_newton(newton_model)
if isinstance(config.dt, float):
self._model.time.set_uniform_timestep(config.dt)
# Allocate state and control
self._state_p: StateKamino = self._model.state(device=self._device)
self._state_n: StateKamino = self._model.state(device=self._device)
self._control: ControlKamino = self._model.control(device=self._device)
# Collision detection
self._collision_detector = CollisionDetector(
model=self._model,
config=config.collision_detector,
)
self._contacts = self._collision_detector.contacts
# Solver
self._solver = SolverKaminoImpl(
model=self._model,
contacts=self._contacts,
config=config.solver,
)
# Initialize state
self._solver.reset(state_out=self._state_n)
self._state_p.copy_from(self._state_n)
@property
def model(self) -> ModelKamino:
return self._model
@property
def state(self) -> StateKamino:
"""Current (next-step) state."""
return self._state_n
@property
def state_previous(self) -> StateKamino:
"""Previous-step state."""
return self._state_p
@property
def control(self) -> ControlKamino:
return self._control
@property
def contacts(self):
return self._contacts
@property
def collision_detector(self) -> CollisionDetector:
return self._collision_detector
@property
def solver(self) -> SolverKaminoImpl:
return self._solver
def step(self):
"""Advance the simulation by one timestep.
``q_i`` is kept in COM-frame throughout (matching ``q_i_0`` and
the internal solver). The COM→body-frame conversion is done
only for rendering via :meth:`RigidBodySim.render`.
"""
self._state_p.copy_from(self._state_n)
self._solver.step(
state_in=self._state_p,
state_out=self._state_n,
control=self._control,
contacts=self._contacts,
detector=self._collision_detector,
)
def reset(self, **kwargs):
"""Reset the simulation state.
Keyword arguments are forwarded to :meth:`SolverKaminoImpl.reset`
(e.g. ``world_mask``, ``joint_q``, ``joint_u``, ``base_q``, ``base_u``).
"""
self._solver.reset(state_out=self._state_n, **kwargs)
self._state_p.copy_from(self._state_n)
class RigidBodySim:
"""Generic Kamino rigid body simulator for RL.
Features:
* USD model loading via ``newton.ModelBuilder.add_usd``
* ``ModelKamino.from_newton`` for Kamino-native state/control layout
* Configurable solver settings with sensible RL defaults
* Zero-copy PyTorch views of state, control and contact arrays
* Automatic extraction of actuated joint metadata
* Selective per-world reset infrastructure (world mask + deferred buffers)
* Optional CUDA graph capture for step and reset
* Optional Newton ViewerGL
Args:
usd_model_path: Full filesystem path to the USD model file.
num_worlds: Number of parallel simulation worlds.
sim_dt: Physics timestep in seconds.
device: Warp device (e.g. ``"cuda:0"``). ``None`` → preferred device.
headless: If ``True``, skip viewer creation.
body_pose_offset: Optional ``(x, y, z, qx, qy, qz, qw)`` tuple to
offset every body's initial pose (e.g. to place the robot above
the ground plane).
add_ground: Add a ground-plane to each world.
enable_gravity: Enable gravity in every world.
settings: Simulator settings. ``None`` uses ``default_settings(sim_dt)``.
use_cuda_graph: Capture CUDA graphs for step and reset (requires
CUDA device with memory pool enabled).
render_config: Viewer appearance settings. ``None`` uses defaults.
"""
def __init__(
self,
usd_model_path: str,
num_worlds: int = 1,
sim_dt: float = 0.01,
device: wp.DeviceLike = None,
headless: bool = False,
body_pose_offset: tuple | None = None,
add_ground: bool = True,
enable_gravity: bool = True,
settings: Simulator.Config | None = None,
use_cuda_graph: bool = False,
record_video: bool = False,
video_folder: str | None = None,
async_save: bool = True,
max_contacts_per_pair: int | None = None,
render_config: ViewerConfig | None = None,
collapse_fixed_joints: bool = False,
):
# ----- Device setup -----
self._device = wp.get_device(device)
self._torch_device: str = "cuda" if self._device.is_cuda else "cpu"
self._use_cuda_graph = use_cuda_graph
self._sim_dt = sim_dt
# ----- Video recording -----
self._record_video = record_video
self._video_folder = video_folder or "./frames"
self._async_save = async_save
self._frame_buffer = None
self._img_idx = 0
if self._record_video:
os.makedirs(self._video_folder, exist_ok=True)
# Resolve settings (subclass override via default_settings)
if settings is None:
settings = self.default_settings(sim_dt)
# ----- Build Newton model (needed for ViewerGL) -----
msg.notif("Constructing builder from imported USD ...")
robot_builder = newton.ModelBuilder(up_axis=newton.Axis.Z)
newton.solvers.SolverKamino.register_custom_attributes(robot_builder)
robot_builder.default_shape_cfg.margin = 0.0
robot_builder.default_shape_cfg.gap = 0.0
robot_builder.add_usd(
usd_model_path,
joint_ordering=None,
force_show_colliders=True,
force_position_velocity_actuation=True,
collapse_fixed_joints=collapse_fixed_joints,
enable_self_collisions=False,
hide_collision_shapes=True,
)
# Create the multi-world model by duplicating the single-robot
# builder for the specified number of worlds
builder = newton.ModelBuilder(up_axis=newton.Axis.Z)
for _ in range(num_worlds):
builder.add_world(robot_builder)
# Add a global ground plane applied to all worlds
builder.add_ground_plane()
# Create the model from the builder
self._newton_model = builder.finalize(skip_validation_joints=True)
# TODO remove after fixing bug
self._newton_model.shape_margin.fill_(0.0)
self._newton_model.shape_gap.fill_(1e-5)
if enable_gravity:
self._newton_model.set_gravity((0.0, 0.0, -9.81))
# ----- Create Kamino simulator from Newton model -----
msg.notif("Building Kamino simulator ...")
# Cap per-pair contact count to limit Delassus matrix size
if max_contacts_per_pair is not None:
settings.collision_detector.max_contacts_per_pair = max_contacts_per_pair
self.sim = SimulatorFromNewton(
newton_model=self._newton_model,
config=settings,
device=self._device,
)
self.model: ModelKamino = self.sim.model
msg.info(f"Model size: {self.sim.model.size}")
msg.info(f"Contacts capacity: {self.sim.contacts.model_max_contacts_host}")
# Apply body pose offset to initial body poses (affects all resets).
# Kamino q_i stores COM positions, so we offset the COM directly.
if body_pose_offset is not None:
offset_z = body_pose_offset[2]
q_i_0 = self.sim.model.bodies.q_i_0
q_i_0_np = q_i_0.numpy().copy()
q_i_0_np[:, 2] += offset_z
q_i_0.assign(q_i_0_np)
# Re-initialize state from the offset initial poses
self.sim.reset()
# ----- Wire RL interface (zero-copy tensors) -----
self._make_rl_interface()
# ----- Extract metadata -----
self._extract_metadata()
# ----- Viewer -----
self.viewer: ViewerGL | None = None
self._newton_state: newton.State | None = None
self._render_config = render_config or ViewerConfig()
if not headless:
msg.notif("Creating the 3D viewer ...")
self.viewer = ViewerGL()
self.viewer.set_model(self._newton_model)
# Newton state used only for rendering (body_q synced from Kamino each frame)
self._newton_state = self._newton_model.state()
self._apply_render_config(self._render_config)
# ----- CUDA graphs -----
self._reset_graph = None
self._step_graph = None
self._capture_graphs()
# ----- Warm-up (compiles Warp kernels) -----
msg.notif("Warming up simulator ...")
self.step()
self.reset()
# ------------------------------------------------------------------
# Viewer appearance
# ------------------------------------------------------------------
def _apply_render_config(self, cfg: ViewerConfig):
"""Apply render configuration to the viewer."""
viewer = self.viewer
renderer = viewer.renderer
model = self._newton_model
def apply_shape_colors(shape_colors: dict[int, Color3]):
for shape_idx, color in shape_colors.items():
model.shape_color[shape_idx : shape_idx + 1].fill_(wp.vec3(color))
# Shape colors (robot only)
if cfg.robot_color is not None:
shape_body = model.shape_body.numpy()
color_overrides: dict[int, Color3] = {}
for s in range(model.shape_count):
if int(shape_body[s]) >= 0:
color_overrides[s] = cfg.robot_color
if color_overrides:
apply_shape_colors(color_overrides)
# Lighting settings
if cfg.diffuse_scale is not None:
renderer.diffuse_scale = cfg.diffuse_scale
if cfg.specular_scale is not None:
renderer.specular_scale = cfg.specular_scale
if cfg.shadow_radius is not None:
renderer.shadow_radius = cfg.shadow_radius
if cfg.shadow_extents is not None:
renderer.shadow_extents = cfg.shadow_extents
if cfg.spotlight_enabled is not None:
renderer.spotlight_enabled = cfg.spotlight_enabled
# Sky color (renderer.sky_upper = "Sky Color" in viewer GUI)
if cfg.sky_color is not None:
renderer.sky_upper = cfg.sky_color
# Directional light color
if cfg.light_color is not None:
renderer._light_color = cfg.light_color
# Background brightness — scales ground color (renderer.sky_lower) and ground plane shapes
if cfg.background_brightness_scale is not None:
s = cfg.background_brightness_scale
renderer.sky_lower = tuple(min(c * s, 1.0) for c in renderer.sky_lower)
# Also brighten ground plane shape colors
shape_body = model.shape_body.numpy()
shape_colors = model.shape_color.numpy()
ground_colors: dict[int, Color3] = {}
for s_idx in range(model.shape_count):
if int(shape_body[s_idx]) < 0:
cur = shape_colors[s_idx]
ground_colors[s_idx] = tuple(min(float(c) * s, 1.0) for c in cur)
if ground_colors:
apply_shape_colors(ground_colors)
# ------------------------------------------------------------------
# RL interface wiring
# ------------------------------------------------------------------
def _make_rl_interface(self):
"""Create zero-copy PyTorch views of simulator state, control and contact arrays."""
nw = self.sim.model.size.num_worlds
njd = self.sim.model.size.max_of_num_joint_dofs
nb = self.sim.model.size.max_of_num_bodies
# State tensors (read-only views into simulator)
self._q_j = wp.to_torch(self.sim.state.q_j).reshape(nw, njd)
self._dq_j = wp.to_torch(self.sim.state.dq_j).reshape(nw, njd)
self._q_i = wp.to_torch(self.sim.state.q_i).reshape(nw, nb, 7)
self._u_i = wp.to_torch(self.sim.state.u_i).reshape(nw, nb, 6)
# Control tensors (writable views)
self._q_j_ref = wp.to_torch(self.sim.control.q_j_ref).reshape(nw, njd)
self._dq_j_ref = wp.to_torch(self.sim.control.dq_j_ref).reshape(nw, njd)
self._tau_j_ref = wp.to_torch(self.sim.control.tau_j_ref).reshape(nw, njd)
# World mask for selective resets
self._world_mask_wp = wp.zeros((nw,), dtype=wp.int32, device=self._device)
self._world_mask = wp.to_torch(self._world_mask_wp)
# Reset buffers
self._reset_base_q_wp = wp.zeros(nw, dtype=transformf, device=self._device)
self._reset_base_u_wp = wp.zeros(nw, dtype=vec6f, device=self._device)
self._reset_q_j = torch.zeros((nw, njd), device=self._torch_device)
self._reset_dq_j = torch.zeros((nw, njd), device=self._torch_device)
self._reset_base_q = wp.to_torch(self._reset_base_q_wp).reshape(nw, 7)
self._reset_base_u = wp.to_torch(self._reset_base_u_wp).reshape(nw, 6)
# Reset flags
self._update_q_j = False
self._update_dq_j = False
self._update_base_q = False
self._update_base_u = False
# Contact aggregation
self._contact_aggregation = ContactAggregation(
model=self.sim.model, contacts=self.sim.contacts, device=self._device
)
self._contact_flags = wp.to_torch(self._contact_aggregation.body_contact_flag).reshape(nw, nb)
self._ground_contact_flags = wp.to_torch(self._contact_aggregation.body_static_contact_flag).reshape(nw, nb)
self._net_contact_forces = wp.to_torch(self._contact_aggregation.body_net_force).reshape(nw, nb, 3)
self._body_pair_contact_flag: torch.Tensor | None = None # Set via set_body_pair_contact_filter()
# Default joint positions (cloned from initial state)
self._default_q_j = self._q_j.clone()
# Environment origins for multi-env offsets
self._env_origins = torch.zeros((nw, 3), device=self._torch_device)
# External wrenches (zero-copy view)
self._w_e_i = wp.to_torch(self.sim.solver.data.bodies.w_e_i).reshape(nw, nb, 6)
# Body masses (zero-copy view)
self._mass = wp.to_torch(self.sim.model.bodies.m_i).reshape(nw, nb)
# ------------------------------------------------------------------
# Metadata extraction
# ------------------------------------------------------------------
def _extract_metadata(self):
"""Extract joint/body names, actuated DOF indices, and joint limits from the Kamino model."""
max_joints = self.sim.model.size.max_of_num_joints
max_bodies = self.sim.model.size.max_of_num_bodies
# Read per-joint metadata from the Kamino model (first world only)
joint_labels = [lbl.rsplit("/", 1)[-1] for lbl in self.sim.model.joints.label[:max_joints]]
joint_num_dofs = wp.to_torch(self.sim.model.joints.num_dofs)[:max_joints].tolist()
joint_act_type = wp.to_torch(self.sim.model.joints.act_type)[:max_joints].tolist()
joint_q_j_min = wp.to_torch(self.sim.model.joints.q_j_min)
joint_q_j_max = wp.to_torch(self.sim.model.joints.q_j_max)
# Joint names and actuated indices
self._joint_names: list[str] = []
self._actuated_joint_names: list[str] = []
self._actuated_dof_indices: list[int] = []
dof_offset = 0
for j in range(max_joints):
ndofs = int(joint_num_dofs[j])
self._joint_names.append(joint_labels[j])
if int(joint_act_type[j]) > 0: # act_type > PASSIVE means actuated
self._actuated_joint_names.append(joint_labels[j])
for dof_idx in range(ndofs):
self._actuated_dof_indices.append(dof_offset + dof_idx)
dof_offset += ndofs
self._actuated_dof_indices_tensor = torch.tensor(
self._actuated_dof_indices, device=self._torch_device, dtype=torch.long
)
msg.info(f"Actuated joints ({self.num_actuated}): {self._actuated_joint_names}")
# Body names
self._body_names: list[str] = [lbl.rsplit("/", 1)[-1] for lbl in self.sim.model.bodies.label[:max_bodies]]
# Joint limits (per-DOF, first world only)
num_dofs_total = self.sim.model.size.max_of_num_joint_dofs
self._joint_limits: list[list[float]] = []
for d in range(num_dofs_total):
lower = float(joint_q_j_min[d])
upper = float(joint_q_j_max[d])
self._joint_limits.append([lower, upper])
# ------------------------------------------------------------------
# CUDA graph capture
# ------------------------------------------------------------------
def _capture_graphs(self):
"""Capture CUDA graphs for step and reset if requested and available."""
if not self._use_cuda_graph:
return
if not (self._device.is_cuda and wp.is_mempool_enabled(self._device)):
msg.warning("CUDA graphs requested but not available (need CUDA device with mempool). Using kernels.")
return
msg.notif("Capturing CUDA graphs ...")
with wp.ScopedCapture(device=self._device) as reset_capture:
self._reset_worlds()
self._reset_graph = reset_capture.graph
with wp.ScopedCapture(device=self._device) as step_capture:
self.sim.step()
self._contact_aggregation.compute()
self._step_graph = step_capture.graph
# ------------------------------------------------------------------
# Core operations
# ------------------------------------------------------------------
def step(self):
"""Execute a single physics step (simulator + contact aggregation).
Flushes any pending resets via :meth:`apply_resets` first, then runs
the physics step. Uses CUDA graph replay if available.
"""
self.apply_resets()
if self._step_graph:
wp.capture_launch(self._step_graph)
else:
self.sim.step()
self._contact_aggregation.compute()
def reset(self):
"""Full reset of all worlds to initial state."""
self._world_mask.fill_(1)
self._reset_worlds()
self._world_mask.zero_()
def apply_resets(self):
"""Apply pending selective resets staged via :meth:`set_dof` / :meth:`set_root`.
Applies resets for worlds marked in :attr:`world_mask`, then clears
the mask and all update flags. Call this before :meth:`step` when
using deferred resets.
"""
if self._reset_graph:
wp.capture_launch(self._reset_graph)
else:
self._reset_worlds()
self._world_mask.zero_()
self._update_q_j = False
self._update_dq_j = False
self._update_base_q = False
self._update_base_u = False
def _reset_worlds(self):
"""Reset selected worlds based on world_mask."""
self.sim.reset(
world_mask=self._world_mask_wp,
joint_q=wp.from_torch(self._reset_q_j.view(-1)) if self._update_q_j else None,
joint_u=wp.from_torch(self._reset_dq_j.view(-1)) if self._update_dq_j else None,
base_q=wp.from_torch(self._reset_base_q.view(-1, 7)) if self._update_base_q else None,
base_u=wp.from_torch(self._reset_base_u.view(-1, 6)) if self._update_base_u else None,
)
def render(self):
"""Render the current frame if viewer exists."""
if self.viewer is not None:
self.viewer.begin_frame(self.time)
# Kamino q_i is COM-frame; ViewerGL expects body-frame-origin poses.
convert_body_com_to_origin(
body_com=self.sim.model.bodies.i_r_com_i,
body_q_com=self.sim.state.q_i,
body_q=self._newton_state.body_q,
)
self.viewer.log_state(self._newton_state)
self.viewer.end_frame()
if self._record_video:
self._capture_frame()
def _capture_frame(self):
"""Capture and save the current rendered frame as a PNG.
If ``render_width``/``render_height`` in the render config differ from
the viewer's native resolution, the renderer is temporarily resized to
the target resolution, re-rendered, captured, then restored — giving
true high-res capture without affecting the display window.
"""
try:
# Thirdparty
from PIL import Image
except ImportError:
msg.warning("PIL not installed. Install with: pip install pillow")
return
renderer = self.viewer.renderer
target_w = self._render_config.render_width
target_h = self._render_config.render_height
native_w = renderer._screen_width
native_h = renderer._screen_height
needs_hires = target_w != native_w or target_h != native_h
if needs_hires:
# Resize FBOs to target resolution and re-render the scene
renderer.resize(target_w, target_h)
renderer.render(self.viewer.camera, self.viewer.objects, self.viewer.lines)
# Invalidate PBO and cached buffer so they get reallocated at new size
self.viewer._pbo = None
self.viewer._wp_pbo = None
self._frame_buffer = None
frame = self.viewer.get_frame(target_image=self._frame_buffer)
if self._frame_buffer is None:
self._frame_buffer = frame
if needs_hires:
# Restore native (window) resolution and invalidate PBO again
renderer.resize()
self.viewer._pbo = None
self.viewer._wp_pbo = None
frame_np = frame.numpy()
image = Image.fromarray(frame_np, mode="RGB")
filename = os.path.join(self._video_folder, f"{self._img_idx:05d}.png")
if self._async_save:
threading.Thread(target=image.save, args=(filename,), daemon=False).start()
else:
image.save(filename)
self._img_idx += 1
def generate_video(self, output_filename: str = "recording.mp4", fps: int = 60, keep_frames: bool = True) -> bool:
"""Generate MP4 video from recorded PNG frames using imageio-ffmpeg.
Args:
output_filename: Name of output video file.
fps: Frames per second for video.
keep_frames: If ``True``, keep PNG frames after video creation.
"""
try:
# Thirdparty
import imageio_ffmpeg as ffmpeg # noqa: PLC0415
except ImportError:
msg.warning("imageio-ffmpeg not installed. Install with: pip install imageio-ffmpeg")
return False
try:
# Thirdparty
from PIL import Image
except ImportError:
msg.warning("PIL not installed. Install with: pip install pillow")
return False
# Thirdparty
import numpy as np # noqa: PLC0415
frame_files = sorted(glob.glob(os.path.join(self._video_folder, "*.png")))
if not frame_files:
msg.warning(f"No PNG frames found in {self._video_folder}")
return False
# Read first frame to get dimensions; ensure even for libx264 yuv420p
first_img = Image.open(frame_files[0])
w, h = first_img.size
# libx264 with yuv420p requires even width and height
even_w = w if w % 2 == 0 else w + 1
even_h = h if h % 2 == 0 else h + 1
needs_pad = even_w != w or even_h != h
size = (even_w, even_h)
msg.info(f"Generating video from {len(frame_files)} frames at {even_w}x{even_h}...")
try:
writer = ffmpeg.write_frames(
output_filename,
size=size,
fps=fps,
codec="libx264",
macro_block_size=1,
quality=5,
)
writer.send(None)
for frame_path in frame_files:
img = Image.open(frame_path)
frame = np.array(img)
if needs_pad:
padded = np.zeros((even_h, even_w, frame.shape[2]), dtype=frame.dtype)
padded[:h, :w] = frame
frame = padded
writer.send(frame)
writer.close()
msg.info(f"Video generated: {output_filename}")
if not keep_frames:
for frame_path in frame_files:
os.remove(frame_path)
msg.info("Frames deleted")
return True
except Exception as e:
msg.warning(f"Failed to generate video: {e}")
return False
@property
def time(self) -> float:
"""Current simulation time."""
return getattr(self, "_sim_time", 0.0)
def is_running(self) -> bool:
"""Check if the viewer is still running (always ``True`` in headless mode)."""
if self.viewer is None:
return True
return self.viewer.is_running()
# ------------------------------------------------------------------
# Deferred reset staging
# ------------------------------------------------------------------
def set_dof(
self,
dof_positions: torch.Tensor | None = None,
dof_velocities: torch.Tensor | None = None,
env_ids: torch.Tensor | list[int] | None = None,
):
"""Stage joint state for deferred reset.
The actual reset happens on the next call to :meth:`apply_resets`.
Args:
dof_positions: Joint positions ``(len(env_ids), num_joint_dofs)``.
dof_velocities: Joint velocities ``(len(env_ids), num_joint_dofs)``.
env_ids: Which worlds to reset. ``None`` resets all.
"""
if env_ids is None:
self._world_mask.fill_(1)
ids = slice(None)
else:
self._world_mask[env_ids] = 1
ids = env_ids
if dof_positions is not None:
self._update_q_j = True
self._reset_q_j[ids] = dof_positions
if dof_velocities is not None:
self._update_dq_j = True
self._reset_dq_j[ids] = dof_velocities
def set_root(
self,
root_positions: torch.Tensor | None = None,
root_orientations: torch.Tensor | None = None,
root_linear_velocities: torch.Tensor | None = None,
root_angular_velocities: torch.Tensor | None = None,
env_ids: torch.Tensor | list[int] | None = None,
):
"""Stage root body state for deferred reset.
The actual reset happens on the next call to :meth:`apply_resets`.
Args:
root_positions: Root positions ``(len(env_ids), 3)``.
root_orientations: Root orientations ``(len(env_ids), 4)`` (quaternion).
root_linear_velocities: Root linear velocities ``(len(env_ids), 3)``.
root_angular_velocities: Root angular velocities ``(len(env_ids), 3)``.
env_ids: Which worlds to reset. ``None`` resets all.
"""
if env_ids is None:
self._world_mask.fill_(1)
ids = slice(None)
else:
self._world_mask[env_ids] = 1
ids = env_ids
if root_positions is not None or root_orientations is not None:
self._update_base_q = True
# Copy current state as baseline
self._reset_base_q[ids] = self._q_i[ids, 0, :7]
if root_positions is not None:
self._reset_base_q[ids, :3] = root_positions
if root_orientations is not None:
self._reset_base_q[ids, 3:] = root_orientations
if root_linear_velocities is not None or root_angular_velocities is not None:
self._update_base_u = True
self._reset_base_u[ids] = self._u_i[ids, 0, :6]
if root_linear_velocities is not None:
self._reset_base_u[ids, :3] = root_linear_velocities
if root_angular_velocities is not None:
self._reset_base_u[ids, 3:] = root_angular_velocities
# ------------------------------------------------------------------
# State properties (zero-copy torch views)
# ------------------------------------------------------------------
@property
def q_j(self) -> torch.Tensor:
"""Joint positions ``(num_worlds, num_joint_coords)``."""
return self._q_j
@property
def dq_j(self) -> torch.Tensor:
"""Joint velocities ``(num_worlds, num_joint_dofs)``."""
return self._dq_j
@property
def q_i(self) -> torch.Tensor:
"""Body poses ``(num_worlds, num_bodies, 7)`` — position + quaternion."""
return self._q_i
@property
def u_i(self) -> torch.Tensor:
"""Body twists ``(num_worlds, num_bodies, 6)`` — linear + angular velocity."""
return self._u_i
# ------------------------------------------------------------------
# Control properties (zero-copy torch views)
# ------------------------------------------------------------------
@property
def q_j_ref(self) -> torch.Tensor:
"""Joint position reference ``(num_worlds, num_joint_coords)`` for implicit PD."""
return self._q_j_ref
@property
def dq_j_ref(self) -> torch.Tensor:
"""Joint velocity reference ``(num_worlds, num_joint_dofs)`` for implicit PD."""
return self._dq_j_ref
@property
def tau_j_ref(self) -> torch.Tensor:
"""Joint torque reference ``(num_worlds, num_joint_dofs)`` for feed-forward control."""
return self._tau_j_ref
# ------------------------------------------------------------------
# Contact properties
# ------------------------------------------------------------------
@property
def contact_flags(self) -> torch.Tensor:
"""Per-body contact flags ``(num_worlds, num_bodies)``."""
return self._contact_flags
@property
def ground_contact_flags(self) -> torch.Tensor:
"""Per-body ground contact flags ``(num_worlds, num_bodies)``."""
return self._ground_contact_flags
@property
def net_contact_forces(self) -> torch.Tensor:
"""Net contact forces ``(num_worlds, num_bodies, 3)``."""
return self._net_contact_forces
@property
def body_pair_contact_flag(self) -> torch.Tensor:
"""Per-world body-pair contact flag ``(num_worlds,)``."""
return self._body_pair_contact_flag
def set_body_pair_contact_filter(self, body_a_name: str, body_b_name: str) -> None:
"""Configure detection of contacts between two named bodies.
Must be called after construction. The detection runs outside the
CUDA graph via :meth:`compute_body_pair_contacts`.
Args:
body_a_name: Name of the first body.
body_b_name: Name of the second body.
"""
a_idx = self.find_body_index(body_a_name)
b_idx = self.find_body_index(body_b_name)
self._contact_aggregation.set_body_pair_filter(a_idx, b_idx)
self._body_pair_contact_flag = wp.to_torch(self._contact_aggregation.body_pair_contact_flag)
def compute_body_pair_contacts(self) -> None:
"""Run body-pair contact detection (call after physics step)."""
self._contact_aggregation.compute_body_pair_contact()
# ------------------------------------------------------------------
# Metadata properties
# ------------------------------------------------------------------
@property
def num_worlds(self) -> int:
return self.sim.model.size.num_worlds
@property
def num_joint_coords(self) -> int:
return self.sim.model.size.max_of_num_joint_coords
@property
def num_joint_dofs(self) -> int:
return self.sim.model.size.max_of_num_joint_dofs
@property
def num_bodies(self) -> int:
return self.sim.model.size.max_of_num_bodies
@property
def joint_names(self) -> list[str]:
return self._joint_names
@property
def body_names(self) -> list[str]:
return self._body_names
@property
def actuated_joint_names(self) -> list[str]:
return self._actuated_joint_names
@property
def actuated_dof_indices(self) -> list[int]:
return self._actuated_dof_indices
@property
def actuated_dof_indices_tensor(self) -> torch.Tensor:
"""Actuated DOF indices as a ``torch.long`` tensor on the simulation device."""
return self._actuated_dof_indices_tensor
@property
def num_actuated(self) -> int:
return len(self._actuated_dof_indices)
@property
def env_origins(self) -> torch.Tensor:
"""Environment origins ``(num_worlds, 3)``."""
return self._env_origins
@property
def external_wrenches(self) -> torch.Tensor:
"""External wrenches ``(num_worlds, num_bodies, 6)``."""
return self._w_e_i
@property
def body_masses(self) -> torch.Tensor:
"""Body masses ``(num_worlds, num_bodies)``."""
return self._mass
@property
def default_q_j(self) -> torch.Tensor:
"""Default joint positions ``(num_worlds, num_joint_coords)`` cloned at init."""
return self._default_q_j
@property
def joint_limits(self) -> list[list[float]]:
"""Per-joint ``[lower, upper]`` limits."""
return self._joint_limits
@property
def torch_device(self) -> str:
"""Torch device string (``"cuda"`` or ``"cpu"``)."""
return self._torch_device
@property
def device(self):
"""Warp device."""
return self._device
@property
def sim_dt(self) -> float:
return self._sim_dt
@property
def world_mask(self) -> torch.Tensor:
"""World mask ``(num_worlds,)`` int32 for selective resets."""
return self._world_mask
# ------------------------------------------------------------------
# Name lookup helpers
# ------------------------------------------------------------------
def find_body_index(self, name: str) -> int:
"""Return the index of the body with the given *name*.
Raises ``ValueError`` if not found.
"""
try:
return self._body_names.index(name)
except ValueError:
raise ValueError(f"Body '{name}' not found. Available: {self._body_names}") from None
def find_body_indices(self, names: list[str]) -> list[int]:
"""Return indices for a list of body *names*."""
return [self.find_body_index(n) for n in names]
# ------------------------------------------------------------------
# Default solver settings
# ------------------------------------------------------------------
@staticmethod
def default_settings(sim_dt: float = 0.01) -> Simulator.Config:
"""Return sensible default solver settings for RL."""
settings = Simulator.Config()
settings.dt = sim_dt
settings.solver.sparse_jacobian = True
settings.solver.use_fk_solver = False
settings.solver.use_collision_detector = True
settings.collision_detector.pipeline = "unified"
settings.collision_detector.max_contacts_per_pair = 8
settings.solver.integrator = "moreau"
settings.solver.constraints.alpha = 0.1
settings.solver.padmm.primal_tolerance = 1e-4
settings.solver.padmm.dual_tolerance = 1e-4
settings.solver.padmm.compl_tolerance = 1e-4
settings.solver.padmm.max_iterations = 200
settings.solver.padmm.eta = 1e-5
settings.solver.padmm.rho_0 = 0.05
settings.solver.padmm.use_acceleration = True
settings.solver.padmm.warmstart_mode = "containers"
settings.solver.padmm.contact_warmstart_method = "geom_pair_net_force"
settings.solver.collect_solver_info = False
settings.solver.compute_solution_metrics = False
settings.solver.padmm.use_graph_conditionals = False
return settings
================================================
FILE: newton/_src/solvers/kamino/examples/rl/simulation_runner.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Sync / async simulation loop for RL examples.
In **sync** mode the viewer and physics run in lockstep on the main thread
(the current default behavior).
In **async** mode the GPU physics + policy inference run as fast as possible
on a background thread while the main thread handles viewer rendering and
joystick polling at fixed rates. OpenGL must stay on the thread that created
the context, so rendering always happens on the main thread.
"""
from __future__ import annotations
# Python
import threading
import time
class SimulationRunner:
"""Run an RL example in sync or async mode.
Args:
example: An ``Example`` instance (must expose ``step``, ``sim_step``,
``update_input``, ``reset``, ``render``, ``joystick``, and
``sim_wrapper``).
mode: ``"sync"`` (default) or ``"async"``.
render_fps: Target rendering rate in Hz (async mode only).
joystick_hz: Target joystick polling rate in Hz (async mode only).
"""
def __init__(
self,
example,
mode: str = "sync",
render_fps: float = 30.0,
joystick_hz: float = 30.0,
) -> None:
if mode not in ("sync", "async"):
raise ValueError(f"mode must be 'sync' or 'async', got {mode!r}")
self._example = example
self._mode = mode
self._render_period = 1.0 / render_fps
self._joystick_period = 1.0 / joystick_hz
# Threading primitives (used only in async mode)
self._lock = threading.Lock()
self._reset_event = threading.Event()
self._stop_event = threading.Event()
# ------------------------------------------------------------------
# Public entry point
# ------------------------------------------------------------------
def run(self) -> None:
"""Run the loop until the viewer is closed or KeyboardInterrupt."""
if self._mode == "sync":
self._run_sync()
else:
self._run_async()
# ------------------------------------------------------------------
# Sync mode
# ------------------------------------------------------------------
def _run_sync(self) -> None:
ex = self._example
while ex.sim_wrapper.is_running():
ex.step()
ex.render()
# ------------------------------------------------------------------
# Async mode
# ------------------------------------------------------------------
def _run_async(self) -> None:
# In async mode the joystick is polled at joystick_hz, not every
# sim step, so its dt (used for path integration and filters)
# must match the actual polling period.
self._example.joystick.set_dt(self._joystick_period)
sim_thread = threading.Thread(target=self._sim_thread_fn, daemon=True)
sim_thread.start()
try:
self._main_thread_loop()
finally:
self._stop_event.set()
sim_thread.join(timeout=2.0)
# Restore dt so sync mode works if run() is called again.
self._example.joystick.set_dt(self._example.env_dt)
def _main_thread_loop(self) -> None:
ex = self._example
next_render = time.monotonic()
next_joystick = time.monotonic()
while ex.sim_wrapper.is_running():
now = time.monotonic()
acted = False
# --- Joystick polling ---
if now >= next_joystick:
next_joystick += self._joystick_period
if next_joystick < now:
next_joystick = now + self._joystick_period
if ex.joystick.check_reset():
self._reset_event.set()
# Single lock acquisition: snapshot root pos, run joystick
# filter + path integration, and write commands to obs.
with self._lock:
root_pos_2d = ex.sim_wrapper.q_i[:, 0, :2].clone()
ex.joystick.update(root_pos_2d=root_pos_2d)
ex.update_input()
acted = True
# --- Rendering ---
if now >= next_render:
next_render += self._render_period
if next_render < now:
next_render = now + self._render_period
with self._lock:
ex.render()
acted = True
if not acted:
sleep_until = min(next_render, next_joystick)
remaining = sleep_until - time.monotonic()
if remaining > 0:
time.sleep(min(remaining, 0.001))
def _sim_thread_fn(self) -> None:
ex = self._example
sim_period = ex.env_dt
next_step = time.monotonic()
while not self._stop_event.is_set():
if self._reset_event.is_set():
with self._lock:
ex.reset()
self._reset_event.clear()
next_step = time.monotonic()
with self._lock:
ex.sim_step()
next_step += sim_period
now = time.monotonic()
if next_step > now:
time.sleep(next_step - now)
elif next_step < now - sim_period:
# Fell too far behind, reset the clock
next_step = now
================================================
FILE: newton/_src/solvers/kamino/examples/rl/test_multi_env_dr_legs.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
###########################################################################
# Quick smoke test: DR Legs with many parallel environments
#
# Usage:
# python test_multi_env_dr_legs.py # default 2048 envs
# python test_multi_env_dr_legs.py --num-worlds 4096
# python test_multi_env_dr_legs.py --num-worlds 1024 2048 4096 # sweep
###########################################################################
import argparse
import time
import warp as wp
import newton
from newton._src.solvers.kamino._src.models.builders.utils import (
build_usd,
make_homogeneous_builder,
set_uniform_body_pose_offset,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.sim import Simulator
wp.set_module_options({"enable_backward": False})
def make_settings(sim_dt: float = 0.004) -> Simulator.Config:
settings = Simulator.Config()
settings.dt = sim_dt
settings.solver.integrator = "moreau"
settings.solver.constraints.alpha = 0.1
settings.solver.padmm.primal_tolerance = 1e-6
settings.solver.padmm.dual_tolerance = 1e-6
settings.solver.padmm.compl_tolerance = 1e-6
settings.solver.padmm.max_iterations = 200
settings.solver.padmm.rho_0 = 0.1
settings.solver.padmm.use_acceleration = True
settings.solver.padmm.warmstart_mode = "containers"
settings.solver.collect_solver_info = False
settings.solver.compute_solution_metrics = False
return settings
def run_test(num_worlds: int, num_steps: int, device):
asset_path = newton.utils.download_asset("disneyresearch")
usd_path = str(asset_path / "dr_legs/usd/dr_legs_with_boxes.usda")
msg.notif(f"--- Testing {num_worlds} environments ---")
# Build model
msg.info("Building model...")
builder = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=build_usd,
source=usd_path,
load_static_geometry=True,
load_drive_dynamics=True,
ground=True,
)
builder.max_contacts_per_pair = 8 # Cap contact budget to avoid Warp tile API shared memory bug
offset = wp.transformf(0.0, 0.0, 0.265, 0.0, 0.0, 0.0, 1.0)
set_uniform_body_pose_offset(builder=builder, offset=offset)
for w in range(builder.num_worlds):
builder.gravity[w].enabled = True
# Create simulator
msg.info("Creating simulator...")
settings = make_settings(0.004)
sim = Simulator(builder=builder, config=settings, device=device)
sim.set_control_callback(lambda _: None)
msg.info(f"Model size: {sim.model.size}")
msg.info(f"Contacts capacity: {sim.contacts.model_max_contacts_host}")
# Warm-up: step without CUDA graphs to compile kernels
msg.info("Stepping (warmup)...")
try:
sim.step()
wp.synchronize()
msg.notif("Warmup step 1 OK")
except Exception as e:
msg.error(f"Warmup step 1 FAILED: {e}")
return False
# Reset
msg.info("Resetting...")
sim.reset()
wp.synchronize()
msg.notif("Reset OK")
# Step multiple times
msg.info(f"Stepping {num_steps} times...")
t0 = time.perf_counter()
for _i in range(num_steps):
sim.step()
wp.synchronize()
t1 = time.perf_counter()
msg.notif(f"OK: {num_worlds} envs x {num_steps} steps in {t1 - t0:.2f}s ({num_steps / (t1 - t0):.0f} steps/s)")
return True
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Multi-env smoke test for DR Legs")
parser.add_argument("--device", type=str, default=None)
parser.add_argument("--num-worlds", type=int, nargs="+", default=[2048])
parser.add_argument("--num-steps", type=int, default=10)
args = parser.parse_args()
msg.set_log_level(msg.LogLevel.INFO)
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
for nw in args.num_worlds:
ok = run_test(nw, args.num_steps, device)
if not ok:
msg.error(f"FAILED at {nw} envs")
break
msg.notif("Done!")
================================================
FILE: newton/_src/solvers/kamino/examples/rl/utils.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Standalone math utilities for RL observation builders.
Provides batched torch rotation operations, stacked-index bookkeeping,
and periodic phase encoding — no external RL-framework dependency.
"""
from __future__ import annotations
# Python
import math
# Thirdparty
import numpy as np
import torch # noqa: TID253
import warp as wp
# ---------------------------------------------------------------------------
# Rotation helpers (xyzw convention, warp kernels with torch tensor wrappers)
# ---------------------------------------------------------------------------
_Z_AXIS = wp.constant(wp.vec3(0.0, 0.0, 1.0))
@wp.kernel
def _quat_to_projected_yaw_kernel(
q: wp.array(dtype=wp.float32),
yaw: wp.array(dtype=wp.float32),
):
i = wp.tid()
base = i * 4
qx = q[base + 0]
qy = q[base + 1]
qz = q[base + 2]
qw = q[base + 3]
yaw[i] = wp.atan2(2.0 * (qz * qw + qx * qy), qw * qw + qx * qx - qy * qy - qz * qz)
def quat_to_projected_yaw(q: torch.Tensor) -> torch.Tensor:
"""Extract yaw angle from quaternion. Returns shape ``(-1, 1)``."""
q_flat = q.reshape(-1, 4).contiguous()
n = q_flat.shape[0]
yaw = torch.empty(n, dtype=torch.float32, device=q.device)
wp.launch(
_quat_to_projected_yaw_kernel,
dim=n,
inputs=[wp.from_torch(q_flat.reshape(-1)), wp.from_torch(yaw)],
device=str(q.device),
)
return yaw.view(-1, 1)
@wp.kernel
def _yaw_apply_2d_kernel(
yaw: wp.array(dtype=wp.float32),
v: wp.array(dtype=wp.float32),
out: wp.array(dtype=wp.float32),
):
i = wp.tid()
q = wp.quat_from_axis_angle(_Z_AXIS, yaw[i])
base = i * 2
r = wp.quat_rotate(q, wp.vec3(v[base], v[base + 1], 0.0))
out[base] = r[0]
out[base + 1] = r[1]
def yaw_apply_2d(yaw: torch.Tensor, v: torch.Tensor) -> torch.Tensor:
"""Forward yaw rotation of a 2-D vector."""
yaw_flat = yaw.reshape(-1).contiguous()
v_flat = v.reshape(-1, 2).contiguous()
n = yaw_flat.shape[0]
out = torch.empty_like(v_flat)
wp.launch(
_yaw_apply_2d_kernel,
dim=n,
inputs=[wp.from_torch(yaw_flat), wp.from_torch(v_flat.reshape(-1)), wp.from_torch(out.reshape(-1))],
device=str(yaw.device),
)
return out.view(-1, 2)
def yaw_to_quat(yaw: torch.Tensor) -> torch.Tensor:
"""Build quaternion (xyzw) from yaw angle. Returns shape ``(-1, 4)``."""
yaw = yaw.reshape(-1, 1)
half = yaw * 0.5
zeros = torch.zeros_like(half)
return torch.cat([zeros, zeros, torch.sin(half), torch.cos(half)], dim=-1)
def quat_inv_mul(a: torch.Tensor, b: torch.Tensor) -> torch.Tensor:
"""Compute ``inv(a) * b`` for quaternions (xyzw convention).
Returns shape ``(-1, 4)``.
"""
a = a.reshape(-1, 4)
b = b.reshape(-1, 4)
# inv(a) = conjugate(a) for unit quaternions (negate xyz)
ax, ay, az, aw = -a[:, 0], -a[:, 1], -a[:, 2], a[:, 3]
bx, by, bz, bw = b[:, 0], b[:, 1], b[:, 2], b[:, 3]
return torch.stack(
[
aw * bx + ax * bw + ay * bz - az * by,
aw * by - ax * bz + ay * bw + az * bx,
aw * bz + ax * by - ay * bx + az * bw,
aw * bw - ax * bx - ay * by - az * bz,
],
dim=-1,
)
def quat_rotate_inv(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor:
"""Rotate 3-D vector by the inverse of quaternion ``q`` (xyzw).
Returns shape ``(-1, 3)``.
"""
q = q.reshape(-1, 4)
v = v.reshape(-1, 3)
qx, qy, qz, qw = q[:, 0], q[:, 1], q[:, 2], q[:, 3]
# t = 2 * cross(q_xyz, v) — but for inverse we negate q_xyz
nqx, nqy, nqz = -qx, -qy, -qz
tx = 2.0 * (nqy * v[:, 2] - nqz * v[:, 1])
ty = 2.0 * (nqz * v[:, 0] - nqx * v[:, 2])
tz = 2.0 * (nqx * v[:, 1] - nqy * v[:, 0])
return torch.stack(
[
v[:, 0] + qw * tx + nqy * tz - nqz * ty,
v[:, 1] + qw * ty + nqz * tx - nqx * tz,
v[:, 2] + qw * tz + nqx * ty - nqy * tx,
],
dim=-1,
)
def quat_to_rotation9d(q: torch.Tensor) -> torch.Tensor:
"""Convert quaternion (xyzw) to a flattened 3x3 rotation matrix (9D).
Returns shape ``(-1, 9)`` with row-major layout.
"""
q = q.reshape(-1, 4)
x, y, z, w = q[:, 0], q[:, 1], q[:, 2], q[:, 3]
xx, yy, zz = x * x, y * y, z * z
xy, xz, yz = x * y, x * z, y * z
wx, wy, wz = w * x, w * y, w * z
return torch.stack(
[
1.0 - 2.0 * (yy + zz),
2.0 * (xy - wz),
2.0 * (xz + wy),
2.0 * (xy + wz),
1.0 - 2.0 * (xx + zz),
2.0 * (yz - wx),
2.0 * (xz - wy),
2.0 * (yz + wx),
1.0 - 2.0 * (xx + yy),
],
dim=-1,
)
# ---------------------------------------------------------------------------
# StackedIndices — lightweight named-range bookkeeping
# ---------------------------------------------------------------------------
class StackedIndices:
"""Named ranges for indexing into a flat (stacked) vector.
Example::
idx = StackedIndices([("pos", 3), ("vel", 3)])
idx.pos # range(0, 3)
idx.vel_idx # 3 (scalar start, only for size-1 entries)
len(idx) # 6
"""
def __init__(self, names_and_sizes: list[tuple[str, int]]) -> None:
start: int = 0
self.names: list[str] = []
for name, size in names_and_sizes:
if hasattr(self, name):
raise ValueError(f"Duplicate entry '{name}'.")
if size <= 0:
continue
setattr(self, name, range(start, start + size))
setattr(self, name + "_slice", slice(start, start + size))
if size == 1:
setattr(self, name + "_idx", start)
start += size
self.names.append(name)
self.size: int = start
def names_and_sizes(self) -> list[tuple[str, int]]:
return [(n, len(self[n])) for n in self.names]
def __getitem__(self, key: str | list[str] | tuple[str, ...]) -> range | list[int]:
if isinstance(key, str):
return getattr(self, key)
elif isinstance(key, (list, tuple)):
return [i for k in key for i in getattr(self, k)]
raise TypeError(f"Invalid key type: {type(key)}")
def __len__(self) -> int:
return self.size
# ---------------------------------------------------------------------------
# Phase encoding
# ---------------------------------------------------------------------------
def periodic_encoding(k: int) -> tuple[np.ndarray, np.ndarray]:
"""Compute sin/cos phase-encoding frequencies and offsets.
Returns ``(freq_2pi, offset)`` arrays of length ``2*k``. Each pair
encodes ``[cos(n·2π·φ), sin(n·2π·φ)]`` for ``n = 1 … k``.
"""
dim = k * 2
freq_2pi = np.zeros((dim,))
offset = np.zeros((dim,))
for i in range(k):
freq = 2.0 * np.pi * (1 + i)
freq_2pi[2 * i] = freq
freq_2pi[2 * i + 1] = freq
offset[2 * i] = 0.5 * np.pi
return freq_2pi, offset
# ---------------------------------------------------------------------------
# Checkpoint loading helpers
# ---------------------------------------------------------------------------
def _build_mlp_from_state_dict(sd: dict, prefix: str, activation: str = "elu") -> torch.nn.Sequential:
"""Reconstruct a Sequential MLP from a state dict with numbered layers."""
act_map = {"elu": torch.nn.ELU, "relu": torch.nn.ReLU, "tanh": torch.nn.Tanh}
act_cls = act_map.get(activation, torch.nn.ELU)
# Collect linear layer indices (keys like "actor.0.weight", "actor.2.weight", ...)
indices = sorted({int(k.split(".")[1]) for k in sd if k.startswith(prefix + ".")})
layers: list[torch.nn.Module] = []
for i, idx in enumerate(indices):
w = sd[f"{prefix}.{idx}.weight"]
b = sd[f"{prefix}.{idx}.bias"]
lin = torch.nn.Linear(w.shape[1], w.shape[0])
lin.weight.data.copy_(w)
lin.bias.data.copy_(b)
layers.append(lin)
if i < len(indices) - 1: # activation after every layer except the last
layers.append(act_cls())
return torch.nn.Sequential(*layers)
def _load_policy_checkpoint(path: str, device: str) -> callable:
"""Load a raw rsl_rl training checkpoint and return a callable policy.
Handles both TorchScript (.pt exported via torch.jit.save) and raw
training checkpoints (saved via torch.save with model_state_dict).
Args:
path: Path to the checkpoint file.
device: Torch device string (e.g. ``"cuda"`` or ``"cpu"``).
"""
try:
return torch.jit.load(path, map_location=device)
except RuntimeError:
pass
ckpt = torch.load(path, map_location=device, weights_only=False)
model_sd = ckpt["model_state_dict"]
actor = _build_mlp_from_state_dict(model_sd, "actor").to(device)
actor.eval()
# Observation normalizer (if present)
obs_norm_sd = ckpt.get("obs_norm_state_dict")
if obs_norm_sd is not None:
mean = obs_norm_sd["_mean"].to(device)
std = obs_norm_sd["_std"].to(device)
eps = 1e-2
def policy(obs: torch.Tensor) -> torch.Tensor:
return actor((obs - mean) / (std + eps))
else:
def policy(obs: torch.Tensor) -> torch.Tensor:
return actor(obs)
return policy
# ---------------------------------------------------------------------------
# Joystick controller
# ---------------------------------------------------------------------------
def _deadband(value: float, threshold: float) -> float:
"""Remove dead zone and rescale to full range."""
if abs(value) < threshold:
return 0.0
sign = 1.0 if value > 0.0 else -1.0
return sign * (abs(value) - threshold) / (1.0 - threshold)
class _LowPassFilter:
"""Scalar backward-Euler low-pass filter."""
def __init__(self, cutoff_hz: float, dt: float) -> None:
omega = cutoff_hz * 2.0 * math.pi
self.alpha = omega * dt / (omega * dt + 1.0)
self.value: float | None = None
def update(self, x: float) -> float:
if self.value is None:
self.value = x
else:
self.value = (1.0 - self.alpha) * self.value + self.alpha * x
return self.value
def reset(self) -> None:
self.value = None
class RateLimitedValue:
"""Scalar rate limiter — clamps the rate of change to ±rate_limit/s."""
def __init__(self, rate_limit: float, dt: float) -> None:
self.rate_limit = rate_limit
self.dt = dt
self.value: float = 0.0
self._initialized = False
def update(self, target: float) -> float:
if not self._initialized:
self._initialized = True
self.value = target
else:
max_delta = self.rate_limit * self.dt
delta = max(-max_delta, min(target - self.value, max_delta))
self.value += delta
return self.value
def reset(self) -> None:
self.value = 0.0
self._initialized = False
def _scale_asym(value: float, neg_scale: float, pos_scale: float) -> float:
"""Asymmetric scaling around zero."""
return value * neg_scale if value < 0.0 else value * pos_scale
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_basics_all_heterogeneous.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.models.builders import basics
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _test_control_callback(
state_t: wp.array(dtype=float32),
control_tau_j: wp.array(dtype=float32),
):
"""
An example control callback kernel.
"""
# Set world index
wid = int(0)
# Define the time window for the active external force profile
t_start = float32(1.0)
t_end = float32(3.0)
# Get the current time
t = state_t[wid]
# Apply a time-dependent external force
if t > t_start and t < t_end:
control_tau_j[0] = 1.0
else:
control_tau_j[0] = 0.0
###
# Launchers
###
def test_control_callback(sim: Simulator):
"""
A control callback function
"""
wp.launch(
_test_control_callback,
dim=1,
inputs=[
sim.data.solver.time.time,
sim.data.control_n.tau_j,
],
)
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
max_steps: int = 1000,
use_cuda_graph: bool = False,
gravity: bool = True,
ground: bool = True,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
# Construct model builder
msg.notif("Constructing builder using model generator ...")
self.builder: ModelBuilderKamino = basics.make_basics_heterogeneous_builder(ground=ground)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.dynamics.preconditioning = True
config.solver.padmm.primal_tolerance = 1e-6
config.solver.padmm.dual_tolerance = 1e-6
config.solver.padmm.compl_tolerance = 1e-6
config.solver.padmm.max_iterations = 200
config.solver.padmm.rho_0 = 0.1
config.solver.compute_solution_metrics = logging and not use_cuda_graph
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "all_heterogeneous/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logging:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logging:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="A demo of all supported joint types.")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-steps", type=int, default=1000, help="Number of steps for headless mode")
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument(
"--ground", action=argparse.BooleanOptionalAction, default=True, help="Adds a ground plane to the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
max_steps=args.num_steps,
gravity=args.gravity,
ground=args.ground,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(-6.4, -11.0, 1.5)
pitch = -1.5
yaw = 92.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "all_heterogeneous")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_basics_box_on_plane.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.types import float32, int32, vec6f
from newton._src.solvers.kamino._src.models import get_basics_usd_assets_path
from newton._src.solvers.kamino._src.models.builders.basics import build_box_on_plane
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _control_callback(
model_body_wid: wp.array(dtype=int32),
contact_world_num_active: wp.array(dtype=int32),
data_t: wp.array(dtype=float32),
state_w_i_e: wp.array(dtype=vec6f),
):
"""
An example control callback kernel.
"""
# Retrieve the body index from the thread index
bid = wp.tid()
# Retrieve world index of the body
wid = model_body_wid[bid]
# Retrieve number of active contacts in the world
wnc = contact_world_num_active[wid]
# Define the time window for the active external force profile
t_start = float32(0.0)
t_end = float32(6.0)
# Get the current time
t = data_t[wid]
# Apply a time-dependent external force
if t > t_start and t < t_end and wnc > 0:
m = float32(1.0) # Mass of the box
g = float32(9.8067) # Gravitational acceleration
mu = float32(0.9) # Friction coefficient
f_ext = 1.1 * m * g * mu # Magnitude of the external force
state_w_i_e[bid] = vec6f(f_ext, 0.0, 0.0, 0.0, 0.0, 0.0)
else:
state_w_i_e[bid] = vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
###
# Launchers
###
def control_callback(sim: Simulator):
"""
A control callback function
"""
wp.launch(
_control_callback,
dim=sim.model.size.sum_of_num_bodies,
inputs=[
sim.model.bodies.bid,
sim.contacts.data.world_active_contacts,
sim.solver.data.time.time,
sim.state.w_i_e,
],
)
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
num_worlds: int = 1,
max_steps: int = 1000,
use_cuda_graph: bool = False,
load_from_usd: bool = False,
gravity: bool = True,
ground: bool = True,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
# Construct model builder
if load_from_usd:
msg.notif("Constructing builder from imported USD ...")
USD_MODEL_PATH = os.path.join(get_basics_usd_assets_path(), "box_on_plane.usda")
importer = USDImporter()
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=importer.import_from,
source=USD_MODEL_PATH,
load_static_geometry=ground,
)
else:
msg.notif("Constructing builder using model generator ...")
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds, build_fn=build_box_on_plane, ground=ground
)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.dynamics.preconditioning = True
config.solver.padmm.primal_tolerance = 1e-6
config.solver.padmm.dual_tolerance = 1e-6
config.solver.padmm.compl_tolerance = 1e-6
config.solver.padmm.max_iterations = 200
config.solver.padmm.rho_0 = 0.1
config.solver.padmm.use_acceleration = True
config.solver.padmm.warmstart_mode = "containers"
config.solver.padmm.contact_warmstart_method = "key_and_position_with_net_force_backup"
config.solver.collect_solver_info = True
config.solver.compute_solution_metrics = logging and not use_cuda_graph
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
self.sim.set_control_callback(control_callback)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "box_on_plane/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
show_contacts=True,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logging:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logging:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Box-on-Plane simulation example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-worlds", type=int, default=1, help="Number of worlds to simulate in parallel")
parser.add_argument("--num-steps", type=int, default=2000, help="Number of steps for headless mode")
parser.add_argument(
"--load-from-usd", action=argparse.BooleanOptionalAction, default=True, help="Load model from USD file"
)
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument(
"--ground", action=argparse.BooleanOptionalAction, default=True, help="Adds a ground plane to the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
load_from_usd=args.load_from_usd,
num_worlds=args.num_worlds,
max_steps=args.num_steps,
gravity=args.gravity,
ground=args.ground,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(2.0, 2.0, 0.5)
pitch = -5.0
yaw = 180.0 + 45.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "box_on_plane")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_basics_box_pendulum.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.models import get_basics_usd_assets_path
from newton._src.solvers.kamino._src.models.builders.basics import build_box_pendulum_vertical
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.control import JointSpacePIDController
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
num_worlds: int = 1,
max_steps: int = 1000,
use_cuda_graph: bool = False,
load_from_usd: bool = False,
gravity: bool = True,
ground: bool = True,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
# Construct model builder
if load_from_usd:
msg.notif("Constructing builder from imported USD ...")
USD_MODEL_PATH = os.path.join(get_basics_usd_assets_path(), "box_pendulum.usda")
importer = USDImporter()
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=importer.import_from,
source=USD_MODEL_PATH,
load_static_geometry=ground,
)
else:
msg.notif("Constructing builder using model generator ...")
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds, build_fn=build_box_pendulum_vertical, ground=ground
)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.dynamics.preconditioning = True
config.solver.padmm.primal_tolerance = 1e-6
config.solver.padmm.dual_tolerance = 1e-6
config.solver.padmm.compl_tolerance = 1e-6
config.solver.padmm.rho_0 = 0.1
config.solver.rotation_correction = "continuous"
config.solver.compute_solution_metrics = logging and not use_cuda_graph
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
# Create a joint-space PID controller
njq = self.sim.model.size.sum_of_num_joint_dofs
K_p = 0.0 * np.ones(njq, dtype=np.float32)
K_i = 0.0 * np.ones(njq, dtype=np.float32)
K_d = 60.0 * np.ones(njq, dtype=np.float32)
decimation = 1 * np.ones(self.sim.model.size.num_worlds, dtype=np.int32) # Control on every step
self.controller = JointSpacePIDController(
model=self.sim.model, K_p=K_p, K_i=K_i, K_d=K_d, decimation=decimation, device=device
)
self.controller.reset(model=self.sim.model, state=self.sim.data.state_n)
q_j_ref = np.zeros(njq, dtype=np.float32)
dq_j_ref = np.full(njq, 1.0, dtype=np.float32)
self.controller.set_references(q_j_ref=q_j_ref, dq_j_ref=dq_j_ref)
# Define a callback function to wrap the execution of the controller
def jointspace_pid_control_callback(simulator: Simulator):
self.controller.compute(
model=simulator.model,
state=simulator.data.state_n,
time=simulator.solver.data.time,
control=simulator.control,
)
# Set the reference tracking generation & control callbacks into the simulator
self.sim.set_control_callback(jointspace_pid_control_callback)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder, self.controller)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "box_pendulum/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logging:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
msg.info("q_j: %s", self.sim.state.q_j)
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
msg.info("q_j: %s", self.sim.state.q_j)
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logging:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Box-Pendulum simulation example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-worlds", type=int, default=1, help="Number of worlds to simulate in parallel")
parser.add_argument("--num-steps", type=int, default=10000, help="Number of steps for headless mode")
parser.add_argument(
"--load-from-usd", action=argparse.BooleanOptionalAction, default=True, help="Load model from USD file"
)
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument(
"--ground", action=argparse.BooleanOptionalAction, default=True, help="Adds a ground plane to the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
load_from_usd=args.load_from_usd,
num_worlds=args.num_worlds,
max_steps=args.num_steps,
gravity=args.gravity,
ground=args.ground,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(0.0, -2.0, 1.0)
pitch = -10.0
yaw = 90
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "box_pendulum")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_basics_boxes_fourbar.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.models import get_basics_usd_assets_path
from newton._src.solvers.kamino._src.models.builders.basics import build_boxes_fourbar
from newton._src.solvers.kamino._src.models.builders.utils import (
make_homogeneous_builder,
set_uniform_body_pose_offset,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _pd_control_callback(
state_t: wp.array(dtype=float32),
control_q_j_ref: wp.array(dtype=float32),
control_dq_j_ref: wp.array(dtype=float32),
control_tau_j_ref: wp.array(dtype=float32),
):
"""
An example control callback kernel.
"""
# Set world index
wid = int(0)
jid = int(0)
# Define the time window for the active external force profile
t_start = float32(3.0)
t_window = float32(3.0)
t_0 = t_start + t_window
t_1 = t_0 + t_window
t_2 = t_1 + t_window
t_3 = t_2 + t_window
t_4 = t_3 + t_window
t_5 = t_4 + t_window
# Get the current time
t = state_t[wid]
# Apply a time-dependent joint references
if t > t_start and t < t_0:
control_q_j_ref[jid] = 0.1
control_dq_j_ref[jid] = 0.0
control_tau_j_ref[jid] = 0.0
elif t > t_0 and t < t_1:
control_q_j_ref[jid] = -0.1
control_dq_j_ref[jid] = 0.0
control_tau_j_ref[jid] = 0.0
elif t > t_1 and t < t_2:
control_q_j_ref[jid] = 0.2
control_dq_j_ref[jid] = 0.0
control_tau_j_ref[jid] = 0.0
elif t > t_2 and t < t_3:
control_q_j_ref[jid] = -0.2
control_dq_j_ref[jid] = 0.0
control_tau_j_ref[jid] = 0.0
elif t > t_3 and t < t_4:
control_q_j_ref[jid] = 0.3
control_dq_j_ref[jid] = 0.0
control_tau_j_ref[jid] = 0.0
elif t > t_4 and t < t_5:
control_q_j_ref[jid] = -0.3
control_dq_j_ref[jid] = 0.0
control_tau_j_ref[jid] = 0.0
else:
control_q_j_ref[jid] = 0.0
control_dq_j_ref[jid] = 0.0
control_tau_j_ref[jid] = 0.0
@wp.kernel
def _torque_control_callback(
state_t: wp.array(dtype=float32),
control_tau_j: wp.array(dtype=float32),
):
"""
An example control callback kernel.
"""
# Set world index
wid = int(0)
jid = int(0)
# Define the time window for the active external force profile
t_start = float32(2.0)
t_end = float32(2.5)
# Get the current time
t = state_t[wid]
# Apply a time-dependent external force
if t > t_start and t < t_end:
control_tau_j[jid] = 0.1
else:
control_tau_j[jid] = 0.0
###
# Launchers
###
def pd_control_callback(sim: Simulator):
"""
A control callback function
"""
wp.launch(
_pd_control_callback,
dim=1,
inputs=[
sim.solver.data.time.time,
sim.control.q_j_ref,
sim.control.dq_j_ref,
sim.control.tau_j_ref,
],
)
def torque_control_callback(sim: Simulator):
"""
A control callback function
"""
wp.launch(
_torque_control_callback,
dim=1,
inputs=[
sim.solver.data.time.time,
sim.control.tau_j,
],
)
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
num_worlds: int = 1,
max_steps: int = 1000,
use_cuda_graph: bool = False,
load_from_usd: bool = False,
implicit_pd: bool = True,
gravity: bool = True,
ground: bool = True,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.01 if implicit_pd else 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
# Construct model builder
if load_from_usd:
msg.notif("Constructing builder from imported USD ...")
USD_MODEL_PATH = os.path.join(get_basics_usd_assets_path(), "boxes_fourbar.usda")
importer = USDImporter()
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=importer.import_from,
source=USD_MODEL_PATH,
load_drive_dynamics=implicit_pd,
load_static_geometry=ground,
use_angular_drive_scaling=True,
)
# Set joint armature and damping because the purely
# UsdPhysics schema does not support these properties yet
if implicit_pd:
for joint in self.builder.joints:
if joint.is_dynamic or joint.is_implicit_pd:
joint.a_j = [0.1]
joint.b_j = [0.001]
else:
msg.notif("Constructing builder using model generator ...")
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=build_boxes_fourbar,
ground=ground,
limits=True,
dynamic_joints=implicit_pd,
implicit_pd=implicit_pd,
)
# Offset the model to place it above the ground
# NOTE: The USD model is centered at the origin
offset = wp.transformf(0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 1.0)
set_uniform_body_pose_offset(builder=self.builder, offset=offset)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.constraints.gamma = 0.1
config.solver.sparse_jacobian = False
config.solver.sparse_dynamics = False
config.solver.integrator = "euler" # Select from {"euler", "moreau"}
config.solver.dynamics.preconditioning = True
config.solver.padmm.primal_tolerance = 1e-4
config.solver.padmm.dual_tolerance = 1e-4
config.solver.padmm.compl_tolerance = 1e-4
config.solver.padmm.max_iterations = 200
config.solver.padmm.rho_0 = 0.1
config.solver.padmm.use_acceleration = True
config.solver.padmm.warmstart_mode = "containers"
config.solver.padmm.contact_warmstart_method = "geom_pair_net_force"
config.solver.collect_solver_info = False
config.solver.compute_solution_metrics = logging and not use_cuda_graph
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
# Set the control callback based on whether implicit PD control is enabled
if implicit_pd:
self.sim.set_control_callback(pd_control_callback)
else:
self.sim.set_control_callback(torque_control_callback)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "boxes_fourbar/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
show_contacts=True,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logging:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logging:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Boxes-Fourbar simulation example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-worlds", type=int, default=1, help="Number of worlds to simulate in parallel")
parser.add_argument("--num-steps", type=int, default=3000, help="Number of steps for headless mode")
parser.add_argument(
"--load-from-usd", action=argparse.BooleanOptionalAction, default=True, help="Load model from USD file"
)
parser.add_argument(
"--implicit-pd",
action=argparse.BooleanOptionalAction,
default=True,
help="Enables implicit PD control of joints",
)
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument(
"--ground", action=argparse.BooleanOptionalAction, default=True, help="Adds a ground plane to the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
load_from_usd=args.load_from_usd,
num_worlds=args.num_worlds,
max_steps=args.num_steps,
implicit_pd=args.implicit_pd,
gravity=args.gravity,
ground=args.ground,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(-0.2, -0.5, 0.1)
pitch = -5.0
yaw = 70.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "boxes_fourbar")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_basics_boxes_hinged.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.models import get_basics_usd_assets_path
from newton._src.solvers.kamino._src.models.builders.basics import build_boxes_hinged
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _control_callback(
state_t: wp.array(dtype=float32),
control_tau_j: wp.array(dtype=float32),
):
"""
An example control callback kernel.
"""
# Set world index
wid = int(0)
jid = int(0)
# Define the time window for the active external force profile
t_start = float32(2.0)
t_end = float32(2.5)
# Get the current time
t = state_t[wid]
# Apply a time-dependent external force
if t > t_start and t < t_end:
control_tau_j[jid] = -3.0
else:
control_tau_j[jid] = 0.0
###
# Launchers
###
def control_callback(sim: Simulator):
"""
A control callback function
"""
wp.launch(
_control_callback,
dim=1,
inputs=[
sim.solver.data.time.time,
sim.control.tau_j,
],
)
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
num_worlds: int = 1,
max_steps: int = 1000,
use_cuda_graph: bool = False,
load_from_usd: bool = False,
gravity: bool = True,
ground: bool = True,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
# Construct model builder
if load_from_usd:
msg.notif("Constructing builder from imported USD ...")
USD_MODEL_PATH = os.path.join(get_basics_usd_assets_path(), "boxes_hinged.usda")
importer = USDImporter()
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=importer.import_from,
source=USD_MODEL_PATH,
load_static_geometry=ground,
)
else:
msg.notif("Constructing builder using model generator ...")
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds, build_fn=build_boxes_hinged, ground=ground
)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.dynamics.preconditioning = True
config.solver.padmm.primal_tolerance = 1e-6
config.solver.padmm.dual_tolerance = 1e-6
config.solver.padmm.compl_tolerance = 1e-6
config.solver.padmm.max_iterations = 200
config.solver.padmm.rho_0 = 0.1
config.solver.padmm.use_acceleration = True
config.solver.padmm.warmstart_mode = "containers"
config.solver.padmm.contact_warmstart_method = "geom_pair_net_force"
config.solver.collect_solver_info = False
config.solver.compute_solution_metrics = logging and not use_cuda_graph
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
self.sim.set_control_callback(control_callback)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "boxes_hinged/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logging:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logging:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Boxes-Hinged simulation example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-worlds", type=int, default=1, help="Number of worlds to simulate in parallel")
parser.add_argument("--num-steps", type=int, default=5000, help="Number of steps for headless mode")
parser.add_argument(
"--load-from-usd", action=argparse.BooleanOptionalAction, default=True, help="Load model from USD file"
)
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument(
"--ground", action=argparse.BooleanOptionalAction, default=True, help="Adds a ground plane to the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
load_from_usd=args.load_from_usd,
num_worlds=args.num_worlds,
max_steps=args.num_steps,
gravity=args.gravity,
ground=args.ground,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(0.5, -1.5, 0.8)
pitch = -15.0
yaw = 90.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "boxes_hinged")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_basics_cartpole.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
from dataclasses import dataclass
import numpy as np
import torch # noqa: TID253
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.types import float32, uint32
from newton._src.solvers.kamino._src.models import get_basics_usd_assets_path
from newton._src.solvers.kamino._src.models.builders.basics import build_cartpole
from newton._src.solvers.kamino._src.models.builders.utils import add_ground_box, make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# RL Interfaces
###
@dataclass
class CartpoleStates:
q_j: torch.Tensor | None = None
dq_j: torch.Tensor | None = None
@dataclass
class CartpoleActions:
tau_j: torch.Tensor | None = None
###
# Kernels
###
@wp.kernel
def _test_control_callback(
state_t: wp.array(dtype=float32),
control_tau_j: wp.array(dtype=float32),
):
"""
An example control callback kernel.
"""
# Retrieve the world index from the thread ID
wid = wp.tid()
# Define the time window for the active external force profile
t_start = float32(1.0)
t_end = float32(3.1)
# Get the current time
t = state_t[wid]
# Apply a time-dependent external force
if t >= 0.0 and t < t_start:
control_tau_j[wid * 2 + 0] = 1.0 * wp.randf(uint32(wid) + uint32(t), -1.0, 1.0)
control_tau_j[wid * 2 + 1] = 0.0
elif t > t_start and t < t_end:
control_tau_j[wid * 2 + 0] = 10.0
control_tau_j[wid * 2 + 1] = 0.0
else:
control_tau_j[wid * 2 + 0] = -10.0
control_tau_j[wid * 2 + 1] = 0.0
###
# Launchers
###
def test_control_callback(sim: Simulator):
"""
A control callback function
"""
wp.launch(
_test_control_callback,
dim=sim.model.size.num_worlds,
inputs=[
sim.solver.data.time.time,
sim.control.tau_j,
],
)
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
num_worlds: int = 1,
max_steps: int = 1000,
use_cuda_graph: bool = False,
load_from_usd: bool = False,
gravity: bool = True,
ground: bool = False,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
self.sim_steps = 0
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
# Construct model builder
if load_from_usd:
msg.notif("Constructing builder from imported USD ...")
USD_MODEL_PATH = os.path.join(get_basics_usd_assets_path(), "cartpole.usda")
importer = USDImporter()
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds, build_fn=importer.import_from, load_static_geometry=True, source=USD_MODEL_PATH
)
if ground:
for w in range(num_worlds):
add_ground_box(self.builder, z_offset=-0.5, world_index=w)
else:
msg.notif("Constructing builder using model generator ...")
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds, build_fn=build_cartpole, ground=ground
)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Demo of printing builder contents in debug logging mode
msg.info("self.builder.gravity:\n%s", self.builder.gravity)
msg.info("self.builder.bodies:\n%s", self.builder.bodies)
msg.info("self.builder.joints:\n%s", self.builder.joints)
msg.info("self.builder.geoms:\n%s", self.builder.geoms)
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.use_fk_solver = True
config.solver.use_collision_detector = True
config.solver.constraints.alpha = 0.1
config.solver.constraints.beta = 0.1
config.solver.padmm.primal_tolerance = 1e-6
config.solver.padmm.dual_tolerance = 1e-6
config.solver.padmm.compl_tolerance = 1e-6
config.solver.padmm.max_iterations = 200
config.solver.padmm.rho_0 = 0.05
config.solver.padmm.use_acceleration = True
config.solver.padmm.warmstart_mode = "containers"
config.solver.collect_solver_info = False
config.solver.compute_solution_metrics = logging and not use_cuda_graph
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
self.sim.set_control_callback(test_control_callback)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if logging and not use_cuda_graph:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "cartpole/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare a PyTorch data interface for the current state and controls data
self.states: CartpoleStates | None = None
self.actions: CartpoleActions | None = None
self.world_mask_wp: wp.array | None = None
self.world_mask_pt: torch.Tensor | None = None
# Set default default reset joint coordinates
_q_j_ref = [0.0, 0.0]
q_j_ref = np.tile(_q_j_ref, reps=self.sim.model.size.num_worlds)
self.q_j_ref: wp.array = wp.array(q_j_ref, dtype=float32, device=self.device)
# Set default default reset joint velocities
_dq_j_ref = [0.0, 0.0]
dq_j_ref = np.tile(_dq_j_ref, reps=self.sim.model.size.num_worlds)
self.dq_j_ref: wp.array = wp.array(dq_j_ref, dtype=float32, device=self.device)
# Initialize RL interfaces
self.make_rl_interface()
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def make_rl_interface(self):
"""
Constructs data interfaces for batched MDP states and actions.
Notes:
- Each torch.Tensor wraps the underlying kamino simulator data arrays without copying.
"""
# Retrieve the batched system dimensions
num_worlds = self.sim.model.size.num_worlds
num_joint_dofs = self.sim.model.size.max_of_num_joint_dofs
# Construct state and action tensors wrapping the underlying simulator data
self.states = CartpoleStates(
q_j=wp.to_torch(self.sim.state.q_j).reshape(num_worlds, num_joint_dofs),
dq_j=wp.to_torch(self.sim.state.dq_j).reshape(num_worlds, num_joint_dofs),
)
self.actions = CartpoleActions(
tau_j=wp.to_torch(self.sim.control.tau_j).reshape(num_worlds, num_joint_dofs),
)
# Create a world mask array+tensor for per-world selective resets
self.world_mask_wp = wp.ones((num_worlds,), dtype=wp.int32, device=self.device)
self.world_mask_pt = wp.to_torch(self.world_mask_wp)
def _reset_worlds(self):
"""Reset selected worlds to reference joint states."""
self.sim.reset(
world_mask=self.world_mask_wp,
joint_q=self.q_j_ref,
# joint_u=self.dq_j_ref,
)
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.notif("Running with CUDA graphs...")
with wp.ScopedCapture(device=self.device) as reset_capture:
self._reset_worlds()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(device=self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(device=self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.notif("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
self.sim_steps += 1
if self.logger:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self._reset_worlds()
if self.logger:
self.logger.log()
self.sim_steps = 0
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
self.sim_steps += 1
if self.logger:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
# DEMO OF PERFORMING A RESET AFTER A FIXED NUMBER OF STEPS
if self.sim_steps > 2000:
msg.warning("Resetting simulation after %d steps", self.sim_steps)
self.reset()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logger:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Cartpole simulation example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-worlds", type=int, default=4, help="Number of worlds to simulate in parallel")
parser.add_argument("--num-steps", type=int, default=5000, help="Number of steps for headless mode")
parser.add_argument(
"--load-from-usd", action=argparse.BooleanOptionalAction, default=True, help="Load model from USD file"
)
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument(
"--ground", action=argparse.BooleanOptionalAction, default=False, help="Adds a ground plane to the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=False, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
load_from_usd=args.load_from_usd,
num_worlds=args.num_worlds,
max_steps=args.num_steps,
gravity=args.gravity,
ground=args.ground,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(5.0, 5.0, 1.5)
pitch = -10.0
yaw = 218.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "cartpole")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_dr_legs.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.joints import JointActuationType
from newton._src.solvers.kamino._src.core.types import float32, int32
from newton._src.solvers.kamino._src.linalg.linear import LinearSolverTypeToName as LinearSolverShorthand
from newton._src.solvers.kamino._src.models.builders.utils import (
add_ground_box,
make_homogeneous_builder,
set_uniform_body_pose_offset,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.control import AnimationJointReference, JointSpacePIDController
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Kernels
###
@wp.kernel
def _pd_control_callback(
# Inputs:
decimation: int32,
model_info_joint_coords_offset: wp.array(dtype=int32),
model_info_joint_dofs_offset: wp.array(dtype=int32),
model_info_joint_actuated_coords_offset: wp.array(dtype=int32),
model_info_joint_actuated_dofs_offset: wp.array(dtype=int32),
model_joints_wid: wp.array(dtype=int32),
model_joints_act_type: wp.array(dtype=int32),
model_joint_num_coords: wp.array(dtype=int32),
model_joint_num_dofs: wp.array(dtype=int32),
model_joint_coords_offset: wp.array(dtype=int32),
model_joint_dofs_offset: wp.array(dtype=int32),
model_joint_actuated_coords_offset: wp.array(dtype=int32),
model_joint_actuated_dofs_offset: wp.array(dtype=int32),
data_time_steps: wp.array(dtype=int32),
animation_frame: wp.array(dtype=int32),
animation_q_j_ref: wp.array2d(dtype=float32),
animation_dq_j_ref: wp.array2d(dtype=float32),
# Outputs:
control_q_j_ref: wp.array(dtype=float32),
control_dq_j_ref: wp.array(dtype=float32),
control_tau_j_ref: wp.array(dtype=float32),
):
"""
A kernel to compute joint-space PID control outputs for force-actuated joints.
"""
# Retrieve the the joint index from the thread indices
jid = wp.tid()
# Retrieve the joint actuation type
act_type = model_joints_act_type[jid]
# Retrieve the world index from the thread indices
wid = model_joints_wid[jid]
# Retrieve the current simulation step
step = data_time_steps[wid]
# Only proceed for force actuated joints and at
# simulation steps matching the control decimation
if act_type != JointActuationType.POSITION_VELOCITY or step % decimation != 0:
return
# Retrieve joint-specific mode info
num_coords_j = model_joint_num_coords[jid]
num_dofs_j = model_joint_num_dofs[jid]
coords_offset_j = model_joint_coords_offset[jid]
dofs_offset_j = model_joint_dofs_offset[jid]
actuated_coords_offset_j = model_joint_actuated_coords_offset[jid]
actuated_dofs_offset_j = model_joint_actuated_dofs_offset[jid]
# Retrieve the offset of the world's joints in the global DoF vector
world_coords_offset = model_info_joint_coords_offset[wid]
world_dofs_offset = model_info_joint_dofs_offset[wid]
world_actuated_coords_offset = model_info_joint_actuated_coords_offset[wid]
world_actuated_dofs_offset = model_info_joint_actuated_dofs_offset[wid]
# Retrieve the current frame of the animation reference for the world
frame = animation_frame[wid]
# Compute the global DoF offset of the joint
coords_offset_j += world_coords_offset
dofs_offset_j += world_dofs_offset
actuated_coords_offset_j += world_actuated_coords_offset
actuated_dofs_offset_j += world_actuated_dofs_offset
# Copy the joint reference coordinates and velocities
# from the animation data to the controller data
for coord in range(num_coords_j):
joint_coord_index = coords_offset_j + coord
actuator_coord_index = actuated_coords_offset_j + coord
control_q_j_ref[joint_coord_index] = animation_q_j_ref[frame, actuator_coord_index]
for dof in range(num_dofs_j):
joint_dof_index = dofs_offset_j + dof
actuator_dof_index = actuated_dofs_offset_j + dof
control_dq_j_ref[joint_dof_index] = animation_dq_j_ref[frame, actuator_dof_index]
control_tau_j_ref[joint_coord_index] = 0.0 # No feed-forward term in this example
###
# Launchers
###
def pd_control_callback(sim: Simulator, animation: AnimationJointReference, decimation: int = 1):
wp.launch(
_pd_control_callback,
dim=sim.model.size.sum_of_num_joints,
inputs=[
# Inputs
int32(decimation),
sim.model.info.joint_coords_offset,
sim.model.info.joint_dofs_offset,
sim.model.info.joint_actuated_coords_offset,
sim.model.info.joint_actuated_dofs_offset,
sim.model.joints.wid,
sim.model.joints.act_type,
sim.model.joints.num_coords,
sim.model.joints.num_dofs,
sim.model.joints.coords_offset,
sim.model.joints.dofs_offset,
sim.model.joints.actuated_coords_offset,
sim.model.joints.actuated_dofs_offset,
sim.solver.data.time.steps,
animation.data.frame,
animation.data.q_j_ref,
animation.data.dq_j_ref,
# Outputs:
sim.control.q_j_ref,
sim.control.dq_j_ref,
sim.control.tau_j_ref,
],
)
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
num_worlds: int = 1,
max_steps: int = 1000,
use_cuda_graph: bool = False,
implicit_pd: bool = False,
gravity: bool = True,
ground: bool = True,
logging: bool = False,
linear_solver: str = "LLTB",
linear_solver_maxiter: int = 0,
use_graph_conditionals: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.01 if implicit_pd else 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
self.implicit_pd: bool = implicit_pd
# Load the DR Legs USD and add it to the builder
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_legs/usd" / "dr_legs_with_meshes_and_boxes.usda")
# Create a model builder from the imported USD
msg.notif("Constructing builder from imported USD ...")
importer = USDImporter()
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=importer.import_from,
load_drive_dynamics=implicit_pd,
load_static_geometry=True,
source=asset_file,
use_angular_drive_scaling=True,
)
msg.info("total mass: %f", self.builder.worlds[0].mass_total)
msg.info("total diag inertia: %f", self.builder.worlds[0].inertia_total)
# Offset the model to place it above the ground
# NOTE: The USD model is centered at the origin
offset = wp.transformf(0.0, 0.0, 0.265, 0.0, 0.0, 0.0, 1.0)
set_uniform_body_pose_offset(builder=self.builder, offset=offset)
# Add a static collision geometry for the plane
if ground:
for w in range(num_worlds):
add_ground_box(self.builder, world_index=w)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Set joint armatures, and verify that correct gains were loaded from the USD file
for joint in self.builder.joints:
if joint.is_dynamic or joint.is_implicit_pd:
joint.a_j = [0.011] # Set joint armature according to Dynamixel XH540-V150 specs
joint.b_j = [0.044] # Set joint damping according to Dynamixel XH540-V150 specs
assert abs(joint.k_p_j[0] - 50.0) < 1e-4
assert abs(joint.k_d_j[0] - 1.0) < 1e-4
# Parse the linear solver max iterations for iterative solvers from the command-line arguments
linear_solver_kwargs = {"maxiter": linear_solver_maxiter} if linear_solver_maxiter > 0 else {}
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.collision_detector.pipeline = "unified" # Select from {"primitive", "unified"}
config.solver.sparse_jacobian = False
config.solver.sparse_dynamics = False
config.solver.integrator = "moreau" # Select from {"euler", "moreau"}
config.solver.constraints.alpha = 0.1
config.solver.constraints.beta = 0.011
config.solver.constraints.gamma = 0.05
config.solver.padmm.primal_tolerance = 1e-4
config.solver.padmm.dual_tolerance = 1e-4
config.solver.padmm.compl_tolerance = 1e-4
config.solver.padmm.max_iterations = 200
config.solver.padmm.eta = 1e-5
config.solver.padmm.rho_0 = 0.02 # try 0.02 for Balanced update
config.solver.padmm.rho_min = 0.05
config.solver.padmm.penalty_update_method = "fixed" # try "balanced"
config.solver.padmm.use_acceleration = True
config.solver.padmm.warmstart_mode = "containers"
config.solver.padmm.contact_warmstart_method = "geom_pair_net_force"
config.solver.collect_solver_info = False
config.solver.compute_solution_metrics = logging and not use_cuda_graph
config.solver.dynamics.linear_solver_type = linear_solver
config.solver.dynamics.linear_solver_kwargs = linear_solver_kwargs
config.solver.padmm.use_graph_conditionals = use_graph_conditionals
config.solver.angular_velocity_damping = 0.0
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
# Load animation data for dr_legs
animation_asset = str(asset_path / "dr_legs/animation" / "dr_legs_animation_100fps.npy")
animation_np = np.load(animation_asset, allow_pickle=True)
msg.debug("animation_np (shape={%s}):\n{%s}\n", animation_np.shape, animation_np)
# Compute animation time step and rate
animation_dt = 0.01 # 100 fps
animation_rate = round(animation_dt / config.dt)
msg.info(f"animation_dt: {animation_dt}")
msg.info(f"animation_rate: {animation_rate}")
# Create a joint-space animation reference generator
self.animation = AnimationJointReference(
model=self.sim.model,
data=animation_np,
data_dt=animation_dt,
target_dt=config.dt,
decimation=1,
rate=1,
loop=False,
use_fd=True,
device=device,
)
# Create a joint-space PID controller
njaq = self.sim.model.size.sum_of_num_actuated_joint_dofs
K_p = 80.0 * np.ones(njaq, dtype=np.float32)
K_d = 0.1 * np.ones(njaq, dtype=np.float32)
K_i = 0.01 * np.ones(njaq, dtype=np.float32)
decimation = 1 * np.ones(self.sim.model.size.num_worlds, dtype=np.int32)
self.controller = JointSpacePIDController(
model=self.sim.model, K_p=K_p, K_i=K_i, K_d=K_d, decimation=decimation, device=device
)
# Define a callback function to reset the controller
def reset_jointspace_pid_control_callback(simulator: Simulator):
self.animation.reset(q_j_ref_out=self.controller.data.q_j_ref, dq_j_ref_out=self.controller.data.dq_j_ref)
self.controller.reset(model=simulator.model, state=simulator.state)
# Define a callback function to wrap the execution of the controller
def compute_jointspace_pid_control_callback(simulator: Simulator):
if self.implicit_pd:
self.animation.advance(time=simulator.solver.data.time)
pd_control_callback(sim=simulator, animation=self.animation, decimation=decimation[0])
else:
self.animation.step(
time=simulator.solver.data.time,
q_j_ref_out=self.controller.data.q_j_ref,
dq_j_ref_out=self.controller.data.dq_j_ref,
)
self.controller.compute(
model=simulator.model,
state=simulator.state,
time=simulator.solver.data.time,
control=simulator.control,
)
# Set the reference tracking generation & control callbacks into the simulator
self.sim.set_post_reset_callback(reset_jointspace_pid_control_callback)
self.sim.set_control_callback(compute_jointspace_pid_control_callback)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder, self.controller)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "dr_legs/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logging:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Plot the animation sequence references
animation_path = os.path.join(path, "animation_references.png") if path is not None else None
self.animation.plot(path=animation_path, show=show)
# Optionally plot the logged simulation data
if self.logging:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="DR Legs simulation example")
parser.add_argument("--device", type=str, default=None, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-worlds", type=int, default=1, help="Number of worlds to simulate in parallel")
parser.add_argument("--num-steps", type=int, default=1000, help="Number of steps for headless mode")
parser.add_argument(
"--implicit-pd",
action=argparse.BooleanOptionalAction,
default=True,
help="Enables implicit PD control of joints",
)
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument(
"--ground", action=argparse.BooleanOptionalAction, default=True, help="Adds a ground plane to the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
parser.add_argument(
"--linear-solver",
default="LLTB",
choices=LinearSolverShorthand.values(),
type=str.upper,
help="Linear solver to use",
)
parser.add_argument(
"--linear-solver-maxiter", default=0, type=int, help="Max number of iterations for iterative linear solvers"
)
parser.add_argument(
"--use-graph-conditionals",
action=argparse.BooleanOptionalAction,
default=True,
help="Use CUDA graph conditional nodes in iterative solvers",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=6, threshold=10000, suppress=True) # Suppress scientific notation
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
num_worlds=args.num_worlds,
linear_solver=args.linear_solver,
linear_solver_maxiter=args.linear_solver_maxiter,
use_graph_conditionals=args.use_graph_conditionals,
max_steps=args.num_steps,
implicit_pd=args.implicit_pd,
gravity=args.gravity,
ground=args.ground,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(0.6, 0.6, 0.3)
pitch = -10.0
yaw = 225.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "dr_legs")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_dr_testmech.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
num_worlds: int = 1,
max_steps: int = 1000,
use_cuda_graph: bool = False,
gravity: bool = True,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
# Load the DR Legs USD and add it to the builder
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_testmech/usd" / "dr_testmech.usda")
# Create a single-instance system (always load from USD for DR Test Mechanism)
msg.notif("Constructing builder from imported USD ...")
importer = USDImporter()
self.builder: ModelBuilderKamino = make_homogeneous_builder(
num_worlds=num_worlds, build_fn=importer.import_from, load_static_geometry=True, source=asset_file
)
msg.info("total mass: %f", self.builder.worlds[0].mass_total)
msg.info("total diag inertia: %f", self.builder.worlds[0].inertia_total)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.use_fk_solver = True
config.solver.constraints.alpha = 0.1
config.solver.padmm.primal_tolerance = 1e-6
config.solver.padmm.dual_tolerance = 1e-6
config.solver.padmm.compl_tolerance = 1e-6
config.solver.padmm.max_iterations = 200
config.solver.padmm.rho_0 = 0.1
config.solver.compute_solution_metrics = logging and not use_cuda_graph
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "dr_testmech/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logging:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logging:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="DR Test Mechanism simulation example")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-worlds", type=int, default=1, help="Number of worlds to simulate in parallel")
parser.add_argument("--num-steps", type=int, default=1000, help="Number of steps for headless mode")
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
num_worlds=args.num_worlds,
max_steps=args.num_steps,
gravity=args.gravity,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(0.2, 0.2, 0.15)
pitch = -20.0
yaw = 215.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "dr_testmech")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_test_all_geoms.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.shapes import GeoType
from newton._src.solvers.kamino._src.geometry.primitive.broadphase import PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES
from newton._src.solvers.kamino._src.geometry.primitive.narrowphase import PRIMITIVE_NARROWPHASE_SUPPORTED_SHAPE_PAIRS
from newton._src.solvers.kamino._src.models.builders import testing
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike,
max_steps: int = 1000,
use_cuda_graph: bool = False,
pipeline_name: str = "primitive",
headless: bool = False,
logging: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
self.logging: bool = logging
# Define excluded shape types for broadphase / narrowphase (temporary)
excluded_types = [
GeoType.NONE, # NOTE: Need to skip empty shapes
GeoType.PLANE, # NOTE: Currently not supported well by the viewer
GeoType.ELLIPSOID, # NOTE: Currently not supported well by the viewer
GeoType.MESH, # NOTE: Currently not supported any pipeline
GeoType.CONVEX_MESH, # NOTE: Currently not supported any pipeline
GeoType.HFIELD, # NOTE: Currently not supported any pipeline
GeoType.GAUSSIAN, # NOTE: Render-only, no collision shape pairs
]
# Generate a list of all supported shape-pair combinations for the configured pipeline
supported_shape_pairs: list[tuple[str, str]] = []
if pipeline_name == "unified":
supported_shape_types = [st.value for st in GeoType]
for shape_bottom in supported_shape_types:
shape_bottom_name = GeoType(shape_bottom).name.lower()
for shape_top in supported_shape_types:
shape_top_name = GeoType(shape_top).name.lower()
if shape_top in excluded_types or shape_bottom in excluded_types:
continue
supported_shape_pairs.append((shape_top_name, shape_bottom_name))
elif pipeline_name == "primitive":
excluded_types.extend([GeoType.CYLINDER])
supported_shape_types = list(PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES)
supported_type_pairs = list(PRIMITIVE_NARROWPHASE_SUPPORTED_SHAPE_PAIRS)
supported_type_pairs_reversed = [(b, a) for (a, b) in supported_type_pairs]
supported_type_pairs.extend(supported_type_pairs_reversed)
for shape_bottom in supported_shape_types:
shape_bottom_name = shape_bottom.name.lower()
for shape_top in supported_shape_types:
shape_top_name = shape_top.name.lower()
if shape_top in excluded_types or shape_bottom in excluded_types:
continue
if (shape_top, shape_bottom) in supported_type_pairs:
supported_shape_pairs.append((shape_top_name, shape_bottom_name))
else:
raise ValueError(f"Unsupported collision pipeline type: {pipeline_name}")
msg.notif(f"Supported shape pairs for pipeline '{pipeline_name}': {supported_shape_pairs}")
# Construct model builder containing all shape-pair combinations supported by the configured pipeline
msg.info("Constructing builder using model generator ...")
self.builder: ModelBuilderKamino = testing.make_shape_pairs_builder(
shape_pairs=supported_shape_pairs,
distance=0.0,
ground_box=True,
ground_z=-2.0,
)
# Set solver config
config = Simulator.Config()
config.dt = 0.001
config.solver.padmm.rho_0 = 0.1
config.collision_detector.pipeline = pipeline_name
# Create a simulator
msg.info("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
# Initialize the data logger
self.logger: SimulationLogger | None = None
if self.logging:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(self.max_steps, self.sim, self.builder)
# Initialize the viewer
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "test_all_geoms/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
show_contacts=True,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
else:
self.viewer = None
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if not self.use_cuda_graph and self.logging:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if not self.use_cuda_graph and self.logging:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logging:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer is not None and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="A demo of all supported geometry types and CD pipelines.")
parser.add_argument("--num-steps", type=int, default=1000, help="Number of steps for headless mode")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--pipeline-name",
type=str,
choices=["primitive", "unified"],
default="unified",
help="Collision detection pipeline name ('primitive' or 'unified')",
)
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
max_steps=args.num_steps,
headless=args.headless,
pipeline_name=args.pipeline_name,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(8.7, -26.0, 1.0)
pitch = 2.0
yaw = 140.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "test_all_geoms")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/examples/sim/example_sim_test_all_joints.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import numpy as np
import warp as wp
import newton
import newton.examples
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.models.builders.testing import build_all_joints_test_model
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.sim import SimulationLogger, Simulator, ViewerKamino
from newton._src.solvers.kamino.examples import get_examples_output_path, run_headless
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Example class
###
class Example:
def __init__(
self,
device: wp.DeviceLike = None,
max_steps: int = 1000,
use_cuda_graph: bool = False,
gravity: bool = True,
ground: bool = False,
logging: bool = False,
headless: bool = False,
record_video: bool = False,
async_save: bool = False,
):
# Initialize target frames per second and corresponding time-steps
self.fps = 60
self.sim_dt = 0.001
self.frame_dt = 1.0 / self.fps
self.sim_substeps = max(1, round(self.frame_dt / self.sim_dt))
self.max_steps = max_steps
# Cache the device and other internal flags
self.device = device
self.use_cuda_graph: bool = use_cuda_graph
# Construct model builder
msg.notif("Constructing builder using model generator ...")
self.builder: ModelBuilderKamino = build_all_joints_test_model(ground=ground)
# Set gravity
for w in range(self.builder.num_worlds):
self.builder.gravity[w].enabled = gravity
# Set solver config
config = Simulator.Config()
config.dt = self.sim_dt
config.solver.padmm.primal_tolerance = 1e-6
config.solver.padmm.dual_tolerance = 1e-6
config.solver.padmm.compl_tolerance = 1e-6
config.solver.padmm.rho_0 = 0.1
config.solver.compute_solution_metrics = logging and not use_cuda_graph
# Create a simulator
msg.notif("Building the simulator...")
self.sim = Simulator(builder=self.builder, config=config, device=device)
# # Initialize the data logger
self.logger: SimulationLogger | None = None
if logging and not use_cuda_graph:
msg.notif("Creating the sim data logger...")
self.logger = SimulationLogger(max_frames=self.max_steps, builder=self.builder, sim=self.sim)
# Initialize the 3D viewer
self.viewer: ViewerKamino | None = None
if not headless:
msg.notif("Creating the 3D viewer...")
# Set up video recording folder
video_folder = None
if record_video:
video_folder = os.path.join(get_examples_output_path(), "test_all_joints/frames")
os.makedirs(video_folder, exist_ok=True)
msg.info(f"Frame recording enabled ({'async' if async_save else 'sync'} mode)")
msg.info(f"Frames will be saved to: {video_folder}")
self.viewer = ViewerKamino(
builder=self.builder,
simulator=self.sim,
record_video=record_video,
video_folder=video_folder,
async_save=async_save,
)
# Declare and initialize the optional computation graphs
# NOTE: These are used for most efficient GPU runtime
self.reset_graph = None
self.step_graph = None
self.simulate_graph = None
# Capture CUDA graph if requested and available
self.capture()
# Warm-start the simulator before rendering
# NOTE: This compiles and loads the warp kernels prior to execution
msg.notif("Warming up simulator...")
self.step_once()
self.reset()
def capture(self):
"""Capture CUDA graph if requested and available."""
if self.use_cuda_graph:
msg.info("Running with CUDA graphs...")
with wp.ScopedCapture(self.device) as reset_capture:
self.sim.reset()
self.reset_graph = reset_capture.graph
with wp.ScopedCapture(self.device) as step_capture:
self.sim.step()
self.step_graph = step_capture.graph
with wp.ScopedCapture(self.device) as sim_capture:
self.simulate()
self.simulate_graph = sim_capture.graph
else:
msg.info("Running with kernels...")
def simulate(self):
"""Run simulation substeps."""
for _i in range(self.sim_substeps):
self.sim.step()
if self.logger:
self.logger.log()
def reset(self):
"""Reset the simulation."""
if self.reset_graph:
wp.capture_launch(self.reset_graph)
else:
self.sim.reset()
if self.logger:
self.logger.reset()
self.logger.log()
def step_once(self):
"""Run the simulation for a single time-step."""
if self.step_graph:
wp.capture_launch(self.step_graph)
else:
self.sim.step()
if self.logger:
self.logger.log()
def step(self):
"""Step the simulation."""
if self.simulate_graph:
wp.capture_launch(self.simulate_graph)
else:
self.simulate()
def render(self):
"""Render the current frame."""
if self.viewer:
self.viewer.render_frame()
def test(self):
"""Test function for compatibility."""
pass
def plot(self, path: str | None = None, show: bool = False, keep_frames: bool = False):
"""
Plot logged data and generate video from recorded frames.
Args:
path: Output directory path (uses video_folder if None)
show: If True, display plots after saving
keep_frames: If True, keep PNG frames after video creation
"""
# Optionally plot the logged simulation data
if self.logger:
self.logger.plot_solver_info(path=path, show=show)
self.logger.plot_joint_tracking(path=path, show=show)
self.logger.plot_solution_metrics(path=path, show=show)
# Optionally generate video from recorded frames
if self.viewer and self.viewer._record_video:
output_dir = path if path is not None else self.viewer._video_folder
output_path = os.path.join(output_dir, "recording.mp4")
self.viewer.generate_video(output_filename=output_path, fps=self.fps, keep_frames=keep_frames)
###
# Main function
###
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="A demo of all supported joint types.")
parser.add_argument("--device", type=str, help="The compute device to use")
parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False, help="Run in headless mode")
parser.add_argument("--num-steps", type=int, default=1000, help="Number of steps for headless mode")
parser.add_argument(
"--gravity", action=argparse.BooleanOptionalAction, default=True, help="Enables gravity in the simulation"
)
parser.add_argument(
"--ground", action=argparse.BooleanOptionalAction, default=False, help="Adds a ground plane to the simulation"
)
parser.add_argument("--cuda-graph", action=argparse.BooleanOptionalAction, default=True, help="Use CUDA graphs")
parser.add_argument("--clear-cache", action=argparse.BooleanOptionalAction, default=False, help="Clear warp cache")
parser.add_argument(
"--logging", action=argparse.BooleanOptionalAction, default=True, help="Enable logging of simulation data"
)
parser.add_argument(
"--show-plots", action=argparse.BooleanOptionalAction, default=False, help="Show plots of logging data"
)
parser.add_argument("--test", action=argparse.BooleanOptionalAction, default=False, help="Run tests")
parser.add_argument(
"--record",
type=str,
choices=["sync", "async"],
default=None,
help="Enable frame recording: 'sync' for synchronous, 'async' for asynchronous (non-blocking)",
)
args = parser.parse_args()
# Set global numpy configurations
np.set_printoptions(linewidth=20000, precision=10, threshold=10000, suppress=True)
# Clear warp cache if requested
if args.clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# TODO: Make optional
# Set the verbosity of the global message logger
msg.set_log_level(msg.LogLevel.INFO)
# Set device if specified, otherwise use Warp's default
if args.device:
device = wp.get_device(args.device)
wp.set_device(device)
else:
device = wp.get_preferred_device()
# Determine if CUDA graphs should be used for execution
can_use_cuda_graph = device.is_cuda and wp.is_mempool_enabled(device)
use_cuda_graph = can_use_cuda_graph and args.cuda_graph
msg.info(f"can_use_cuda_graph: {can_use_cuda_graph}")
msg.info(f"use_cuda_graph: {use_cuda_graph}")
msg.info(f"device: {device}")
# Create example instance
example = Example(
device=device,
use_cuda_graph=use_cuda_graph,
max_steps=args.num_steps,
gravity=args.gravity,
ground=args.ground,
headless=args.headless,
logging=args.logging,
record_video=args.record is not None and not args.headless,
async_save=args.record == "async",
)
# Run a brute-force simulation loop if headless
if args.headless:
msg.notif("Running in headless mode...")
run_headless(example, progress=True)
# Otherwise launch using a debug viewer
else:
msg.notif("Running in Viewer mode...")
# Set initial camera position for better view of the system
if hasattr(example.viewer, "set_camera"):
camera_pos = wp.vec3(-17.0, -25.0, 0.0)
pitch = 0.0
yaw = 90.0
example.viewer.set_camera(camera_pos, pitch, yaw)
# Launch the example using Newton's built-in runtime
newton.examples.run(example, args)
# Plot logged data after the viewer is closed
if args.logging or args.record:
OUTPUT_PLOT_PATH = os.path.join(get_examples_output_path(), "test_all_joints")
os.makedirs(OUTPUT_PLOT_PATH, exist_ok=True)
example.plot(path=OUTPUT_PLOT_PATH, show=args.show_plots)
================================================
FILE: newton/_src/solvers/kamino/solver_kamino.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Defines the :class:`SolverKamino` class, providing a physics backend for
simulating constrained multi-body systems for arbitrary mechanical assemblies.
"""
from __future__ import annotations
import warnings
from dataclasses import dataclass
from typing import TYPE_CHECKING, Any, Literal
import warp as wp
from ...core.types import override
from ...sim import (
Contacts,
Control,
JointType,
Model,
ModelBuilder,
State,
)
from ..flags import SolverNotifyFlags
from ..solver import SolverBase
if TYPE_CHECKING:
from .config import (
CollisionDetectorConfig,
ConfigBase,
ConstrainedDynamicsConfig,
ConstraintStabilizationConfig,
ForwardKinematicsSolverConfig,
PADMMSolverConfig,
)
###
# Module interface
###
__all__ = ["SolverKamino"]
###
# Interfaces
###
class SolverKamino(SolverBase):
"""
A physics solver for simulating constrained multi-body systems containing kinematic loops,
under-/overactuation, joint-limits, hard frictional contacts and restitutive impacts.
This solver uses the Proximal-ADMM algorithm to solve the forward dynamics formulated
as a Nonlinear Complementarity Problem (NCP) over the set of bilateral kinematic joint
constraints and unilateral constraints that include joint-limits and contacts.
.. note::
Currently still in `Beta`, so we do not recommend using this solver for
production use cases yet, as we expect many things to change in future releases.
This includes both the public API and internal implementation; adding support for
more simulation features (e.g. joints, constraints, actuators), performance
optimizations, and bug fixes.
References:
- Tsounis, Vassilios, Ruben Grandia, and Moritz Bächer.
On Solving the Dynamics of Constrained Rigid Multi-Body Systems with Kinematic Loops.
arXiv preprint arXiv:2504.19771 (2025).
https://doi.org/10.48550/arXiv.2504.19771
- Carpentier, Justin, Quentin Le Lidec, and Louis Montaut.
From Compliant to Rigid Contact Simulation: a Unified and Efficient Approach.
20th edition of the “Robotics: Science and Systems”(RSS) Conference. 2024.
https://roboticsproceedings.org/rss20/p108.pdf
- Tasora, A., Mangoni, D., Benatti, S., & Garziera, R. (2021).
Solving variational inequalities and cone complementarity problems in
nonsmooth dynamics using the alternating direction method of multipliers.
International Journal for Numerical Methods in Engineering, 122(16), 4093-4113.
https://onlinelibrary.wiley.com/doi/full/10.1002/nme.6693
After constructing :class:`ModelKamino`, :class:`StateKamino`, :class:`ControlKamino` and :class:`ContactsKamino`
objects, this physics solver may be used to advance the simulation state forward in time.
Example
-------
.. code-block:: python
config = newton.solvers.SolverKamino.Config()
solver = newton.solvers.SolverKamino(model, config)
# simulation loop
for i in range(100):
solver.step(state_in, state_out, control, contacts, dt)
state_in, state_out = state_out, state_in
"""
@dataclass
class Config:
"""
A container to hold all configurations of the :class:`SolverKamino` solver.
"""
sparse_jacobian: bool = False
"""
Flag to indicate whether the solver should use sparse data representations for the Jacobian.
"""
sparse_dynamics: bool = False
"""
Flag to indicate whether the solver should use sparse data representations for the dynamics.
"""
use_collision_detector: bool = False
"""
Flag to indicate whether the Kamino-provided collision detector should be used.
"""
use_fk_solver: bool = False
"""
Flag to indicate whether the Kamino-provided FK solver should be enabled.\n
The FK solver is used for computing consistent initial states given input
joint positions, joint velocities and optional base body poses and twists.
It is specifically designed to handle the presence of:
- kinematic loops
- passive joints
- over/under-actuation
"""
collision_detector: CollisionDetectorConfig | None = None
"""
Configurations for the collision detector.\n
See :class:`CollisionDetectorConfig` for more details.\n
If `None`, the default configuration will be used.
"""
constraints: ConstraintStabilizationConfig | None = None
"""
Configurations for the constraint stabilization parameters.\n
See :class:`ConstraintStabilizationConfig` for more details.\n
If `None`, default values will be used.
"""
dynamics: ConstrainedDynamicsConfig | None = None
"""
Configurations for the constrained dynamics problem.\n
See :class:`ConstrainedDynamicsConfig` for more details.\n
If `None`, default values will be used.
"""
padmm: PADMMSolverConfig | None = None
"""
Configurations for the dynamics solver.\n
See :class:`PADMMSolverConfig` for more details.\n
If `None`, default values will be used.
"""
fk: ForwardKinematicsSolverConfig | None = None
"""
Configurations for the forward kinematics solver.\n
See :class:`ForwardKinematicsSolverConfig` for more details.\n
If `None`, default values will be used.
"""
rotation_correction: Literal["twopi", "continuous", "none"] = "twopi"
"""
The rotation correction mode to use for rotational DoFs.\n
See :class:`JointCorrectionMode` for available options.
Defaults to `twopi`.
"""
integrator: Literal["euler", "moreau"] = "euler"
"""
The time-integrator to use for state integration.\n
See available options in the `integrators` module.\n
Defaults to `"euler"`.
"""
angular_velocity_damping: float = 0.0
"""
A damping factor applied to the angular velocity of bodies during state integration.\n
This can help stabilize simulations with large time steps or high angular velocities.\n
Defaults to `0.0` (i.e. no damping).
"""
collect_solver_info: bool = False
"""
Enables/disables collection of solver convergence and performance info at each simulation step.\n
Enabling this option as it will significantly increase the runtime of the solver.\n
Defaults to `False`.
"""
compute_solution_metrics: bool = False
"""
Enables/disables computation of solution metrics at each simulation step.\n
Enabling this option as it will significantly increase the runtime of the solver.\n
Defaults to `False`.
"""
@staticmethod
def register_custom_attributes(builder: ModelBuilder) -> None:
"""
Register custom attributes for the :class:`SolverKamino.Config` configurations.
Note: Currently, not all configurations are registered as custom attributes,
as only those supported by the Kamino USD scene API have been included. More
will be added in the future as latter is being developed.
Args:
builder: The model builder instance with which to register the custom attributes.
"""
# Import here to avoid module-level imports and circular dependencies
from . import config # noqa: PLC0415
from ._src.core.joints import JointCorrectionMode # noqa: PLC0415
# Register KaminoSceneAPI custom attributes for each sub-configuration container
config.ForwardKinematicsSolverConfig.register_custom_attributes(builder)
config.ConstraintStabilizationConfig.register_custom_attributes(builder)
config.ConstrainedDynamicsConfig.register_custom_attributes(builder)
config.CollisionDetectorConfig.register_custom_attributes(builder)
config.PADMMSolverConfig.register_custom_attributes(builder)
# Register KaminoSceneAPI custom attributes for each individual solver-level configurations
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="joint_correction",
frequency=Model.AttributeFrequency.ONCE,
assignment=Model.AttributeAssignment.MODEL,
dtype=str,
default="twopi",
namespace="kamino",
usd_attribute_name="newton:kamino:jointCorrection",
usd_value_transformer=JointCorrectionMode.parse_usd_attribute,
)
)
@staticmethod
def from_model(model: Model, **kwargs: dict[str, Any]) -> SolverKamino.Config:
"""
Creates a configuration container by attempting to parse
custom attributes from a :class:`Model` if available.
Note: If the model was imported from USD and contains custom attributes defined
by the KaminoSceneAPI, those attributes will be parsed and used to populate
the configuration container. Additionally, any sub-configurations that are
provided as keyword arguments will also be used to populate the corresponding
sections of the configuration, allowing for a combination of model-imported
and explicit user-provided configurations. If certain configurations are not
provided either via the model's custom attributes or as keyword arguments,
then default values will be used.
Args:
model: The Newton model from which to parse configurations.
"""
# Import here to avoid module-level imports and circular dependencies
from . import config # noqa: PLC0415
# Create a base config with default values and
# user-provided provided kwarg overrides
cfg = SolverKamino.Config(**kwargs)
# Parse solver-specific attributes imported from USD
kamino_attrs = getattr(model, "kamino", None)
if kamino_attrs is not None:
if hasattr(kamino_attrs, "joint_correction"):
cfg.rotation_correction = kamino_attrs.joint_correction[0]
# Parse sub-configurations from the provided kwargs, if available, otherwise use defaults
subconfigs: dict[str, ConfigBase] = {
"collision_detector": config.CollisionDetectorConfig,
"constraints": config.ConstraintStabilizationConfig,
"dynamics": config.ConstrainedDynamicsConfig,
"padmm": config.PADMMSolverConfig,
"fk": config.ForwardKinematicsSolverConfig,
}
for attr_name, config_cls in subconfigs.items():
nested_config = kwargs.get(attr_name, None)
nested_kwargs = nested_config.__dict__ if nested_config is not None else {}
setattr(cfg, attr_name, config_cls.from_model(model, **nested_kwargs))
# Return the fully constructed config with sub-configurations
# parsed from the model's custom attributes if available,
# otherwise using defaults or provided kwargs.
return cfg
@override
def validate(self) -> None:
"""
Validates the current values held by the :class:`SolverKamino.Config` instance.
"""
# Import here to avoid module-level imports and circular dependencies
from ._src.core.joints import JointCorrectionMode # noqa: PLC0415
# Ensure that the sparsity settings are compatible with each other
if self.sparse_dynamics and not self.sparse_jacobian:
raise ValueError(
"Sparsity setting mismatch: `sparse_dynamics` solver "
"option requires that `sparse_jacobian` is set to `True`."
)
# Ensure that all mandatory configurations are not None.
if self.constraints is None:
raise ValueError("Constraint stabilization config cannot be None.")
elif self.dynamics is None:
raise ValueError("Constrained dynamics config cannot be None.")
elif self.padmm is None:
raise ValueError("PADMM solver config cannot be None.")
# Validate specialized sub-configurations
# using their own built-in validations
if self.collision_detector is not None:
self.collision_detector.validate()
if self.fk is not None:
self.fk.validate()
self.constraints.validate()
self.dynamics.validate()
self.padmm.validate()
# Conversion to JointCorrectionMode will raise an error if the input string is invalid.
JointCorrectionMode.from_string(self.rotation_correction)
# Ensure the integrator choice is valid
supported_integrators = {"euler", "moreau"}
if self.integrator not in supported_integrators:
raise ValueError(f"Invalid integrator: {self.integrator}. Must be one of {supported_integrators}.")
# Ensure the angular velocity damping factor is non-negative
if self.angular_velocity_damping < 0.0 or self.angular_velocity_damping > 1.0:
raise ValueError(
f"Invalid angular velocity damping factor: {self.angular_velocity_damping}. "
"Must be in the range [0.0, 1.0]."
)
@override
def __post_init__(self):
"""
Post-initialization to default-initialize empty configurations and validate those specified by the user.
"""
# Import here to avoid module-level imports and circular dependencies
from . import config # noqa: PLC0415
# Default-initialize any sub-configurations that were not explicitly provided by the user
if self.collision_detector is None and self.use_collision_detector:
self.collision_detector = config.CollisionDetectorConfig()
if self.fk is None and self.use_fk_solver:
self.fk = config.ForwardKinematicsSolverConfig()
if self.constraints is None:
self.constraints = config.ConstraintStabilizationConfig()
if self.dynamics is None:
self.dynamics = config.ConstrainedDynamicsConfig()
if self.padmm is None:
self.padmm = config.PADMMSolverConfig()
# Validate the config values after all default-initialization is done
# to ensure that any inter-dependent parameters are properly checked.
self.validate()
_kamino = None
"""
Class variable storing the imported Kamino module.\n
The module is imported and cached on the first instantiation of
the solver to avoid import overhead if the solver is not used.
"""
def __init__(
self,
model: Model,
config: Config | None = None,
):
"""
Constructs a Kamino solver for the given model and optional configurations.
Args:
model:
The Newton model for which to create the Kamino solver instance.
config:
Explicit user-provided configurations for the Kamino solver.\n
If `None`, configurations will be parsed from the Newton model's
custom attributes using :meth:`SolverKamino.Config.from_model`,
e.g. to be loaded from USD assets. If that also fails, then
default configurations will be used.
"""
# Initialize the base solver
super().__init__(model=model)
# Import all Kamino dependencies and cache them
# as class variables if not already done
self._import_kamino()
# Validate that the model does not contain unsupported components
self._validate_model_compatibility(model)
# Cache configurations; either from the user-provided config or from the model's custom attributes
# NOTE: `Config.from_model` will default-initialize if no relevant custom attributes were
# found on the model, so `self._config` will always be fully initialized after this step.
if config is None:
config = self.Config.from_model(model)
self._config = config
# Create a Kamino model from the Newton model
self._model_kamino = self._kamino.ModelKamino.from_newton(model)
# Create a collision detector if enabled in the config, otherwise
# set to `None` to disable internal collision detection in Kamino
self._collision_detector_kamino = None
if self._config.use_collision_detector:
self._collision_detector_kamino = self._kamino.CollisionDetector(
model=self._model_kamino,
config=self._config.collision_detector,
)
# Capture a reference to the contacts container
self._contacts_kamino = None
if self._collision_detector_kamino is not None:
self._contacts_kamino = self._collision_detector_kamino.contacts
else:
# If collision detector is disabled allocate contacts manually
# TODO: We need to fix this logic to properly handle the case where the collision
# detector is disabled but contacts are still provided by Newton's collision pipeline.
if self.model.rigid_contact_max == 0:
world_max_contacts = self._model_kamino.geoms.world_minimum_contacts
else:
world_max_contacts = [model.rigid_contact_max // self.model.world_count] * self.model.world_count
self._contacts_kamino = self._kamino.ContactsKamino(capacity=world_max_contacts, device=self.model.device)
# Initialize the internal Kamino solver
self._solver_kamino = self._kamino.SolverKaminoImpl(
model=self._model_kamino,
contacts=self._contacts_kamino,
config=self._config,
)
def reset(
self,
state_out: State,
world_mask: wp.array | None = None,
actuator_q: wp.array | None = None,
actuator_u: wp.array | None = None,
joint_q: wp.array | None = None,
joint_u: wp.array | None = None,
base_q: wp.array | None = None,
base_u: wp.array | None = None,
):
"""
Resets the simulation state given a combination of desired base body
and joint states, as well as an optional per-world mask array indicating
which worlds should be reset. The reset state is written to `state_out`.
For resets given absolute quantities like base body poses, the
`state_out` must initially contain the current state of the simulation.
Args:
state_out: The output state container to which the reset state data is written.
world_mask: Optional array of per-world masks indicating which worlds should be reset.\n
Shape of `(num_worlds,)` and type :class:`wp.int8 | wp.bool`
actuator_q: Optional array of target actuated joint coordinates.\n
Shape of `(num_actuated_joint_coords,)` and type :class:`wp.float32`
actuator_u: Optional array of target actuated joint DoF velocities.\n
Shape of `(num_actuated_joint_dofs,)` and type :class:`wp.float32`
joint_q: Optional array of target joint coordinates.\n
Shape of `(num_joint_coords,)` and type :class:`wp.float32`
joint_u: Optional array of target joint DoF velocities.\n
Shape of `(num_joint_dofs,)` and type :class:`wp.float32`
base_q: Optional array of target base body poses.\n
Shape of `(num_worlds,)` and type :class:`wp.transformf`
base_u: Optional array of target base body twists.\n
Shape of `(num_worlds,)` and type :class:`wp.spatial_vectorf`
"""
# Convert base pose from body-origin to COM frame
if base_q is not None:
base_q_com = wp.zeros_like(base_q)
self._kamino.convert_base_origin_to_com(
base_body_index=self._model_kamino.info.base_body_index,
body_com=self._model_kamino.bodies.i_r_com_i,
base_q=base_q,
base_q_com=base_q_com,
)
base_q = base_q_com
# TODO: fix brittle in-place update of arrays after conversion
# Create a zer-copy view of the input state_out as a StateKamino
# to interface with the Kamino solver's reset operation
state_out_kamino = self._kamino.StateKamino.from_newton(self._model_kamino.size, self.model, state_out)
# Execute the reset operation of the Kamino solver,
# to write the reset state to `state_out_kamino`
self._solver_kamino.reset(
state_out=state_out_kamino,
world_mask=world_mask,
actuator_q=actuator_q,
actuator_u=actuator_u,
joint_q=joint_q,
joint_u=joint_u,
base_q=base_q,
base_u=base_u,
)
# Convert com-frame poses from Kamino reset to body-origin frame
self._kamino.convert_body_com_to_origin(
body_com=self._model_kamino.bodies.i_r_com_i,
body_q_com=state_out_kamino.q_i,
body_q=state_out_kamino.q_i,
world_mask=world_mask,
body_wid=self._model_kamino.bodies.wid,
)
@override
def step(self, state_in: State, state_out: State, control: Control | None, contacts: Contacts | None, dt: float):
"""
Simulate the model for a given time step using the given control input.
When ``contacts`` is not ``None`` (i.e. produced by :meth:`~newton.Model.collide`),
those contacts are converted to Kamino's internal format and used directly,
bypassing Kamino's own collision detector. When ``contacts`` is ``None``,
Kamino's internal collision pipeline runs as a fallback.
Args:
state_in: The input state.
state_out: The output state.
control: The control input.
Defaults to `None` which means the control values from the
:class:`Model` are used.
contacts: The contact information from Newton's collision
pipeline, or ``None`` to use Kamino's internal collision detector.
dt: The time step (typically in seconds).
"""
# Interface the input state containers to Kamino's equivalents
# NOTE: These should produce zero-copy views/references
# to the arrays of the source Newton containers.
state_in_kamino = self._kamino.StateKamino.from_newton(self._model_kamino.size, self.model, state_in)
state_out_kamino = self._kamino.StateKamino.from_newton(self._model_kamino.size, self.model, state_out)
# Handle the control input, defaulting to the model's
# internal control arrays if None is provided.
if control is None:
control = self.model.control(clone_variables=False)
control_kamino = self._kamino.ControlKamino.from_newton(control)
# If contacts are provided, use them directly, bypassing Kamino's collision detector
if contacts is not None:
self._kamino.convert_contacts_newton_to_kamino(self.model, state_in, contacts, self._contacts_kamino)
_detector = None
# Otherwise, use Kamino's internal collision detector to generate contacts
else:
_detector = self._collision_detector_kamino
# Convert Newton body-frame poses to Kamino CoM-frame poses using
# Kamino's corrected body-com offsets (can differ from Newton model data).
self._kamino.convert_body_origin_to_com(
body_com=self._model_kamino.bodies.i_r_com_i,
body_q=state_in_kamino.q_i,
body_q_com=state_in_kamino.q_i,
)
# Step the physics solver
self._solver_kamino.step(
state_in=state_in_kamino,
state_out=state_out_kamino,
control=control_kamino,
contacts=self._contacts_kamino,
detector=_detector,
dt=dt,
)
# Convert back from Kamino CoM-frame to Newton body-frame poses using
# the same corrected body-com offsets as the forward conversion.
self._kamino.convert_body_com_to_origin(
body_com=self._model_kamino.bodies.i_r_com_i,
body_q_com=state_in_kamino.q_i,
body_q=state_in_kamino.q_i,
)
self._kamino.convert_body_com_to_origin(
body_com=self._model_kamino.bodies.i_r_com_i,
body_q_com=state_out_kamino.q_i,
body_q=state_out_kamino.q_i,
)
@override
def notify_model_changed(self, flags: int) -> None:
"""Propagate Newton model property changes to Kamino's internal ModelKamino.
Args:
flags: Bitmask of :class:`SolverNotifyFlags` indicating which properties changed.
"""
if flags & SolverNotifyFlags.MODEL_PROPERTIES:
self._update_gravity()
if flags & SolverNotifyFlags.BODY_PROPERTIES:
pass # TODO: convert to CoM-frame if body_q_i_0 is changed at runtime?
if flags & SolverNotifyFlags.BODY_INERTIAL_PROPERTIES:
# Kamino's RigidBodiesModel references Newton's arrays directly
# (m_i, inv_m_i, i_I_i, inv_i_I_i, i_r_com_i), so no copy needed.
pass
if flags & SolverNotifyFlags.SHAPE_PROPERTIES:
pass # TODO: ???
if flags & SolverNotifyFlags.JOINT_PROPERTIES:
self._update_joint_transforms()
if flags & SolverNotifyFlags.JOINT_DOF_PROPERTIES:
# Joint limits (q_j_min, q_j_max, dq_j_max, tau_j_max) are direct
# references to Newton's arrays, so no copy needed.
pass
if flags & SolverNotifyFlags.ACTUATOR_PROPERTIES:
pass # TODO: ???
if flags & SolverNotifyFlags.CONSTRAINT_PROPERTIES:
pass # TODO: ???
unsupported = flags & ~(
SolverNotifyFlags.MODEL_PROPERTIES
| SolverNotifyFlags.BODY_INERTIAL_PROPERTIES
| SolverNotifyFlags.JOINT_PROPERTIES
| SolverNotifyFlags.JOINT_DOF_PROPERTIES
)
if unsupported:
self._kamino.msg.warning(
"SolverKamino.notify_model_changed: flags 0x%x not yet supported",
unsupported,
)
@override
def update_contacts(self, contacts: Contacts, state: State | None = None) -> None:
"""
Converts Kamino contacts to Newton's Contacts format.
Args:
contacts: The Newton Contacts object to populate.
state: Simulation state providing ``body_q`` for converting
world-space contact positions to body-local frame.
"""
# Ensure the containers are not None and of the correct shape
if contacts is None:
raise ValueError("contacts cannot be None when calling SolverKamino.update_contacts")
elif not isinstance(contacts, Contacts):
raise TypeError(f"contacts must be of type Contacts, got {type(contacts)}")
if state is None:
raise ValueError("state cannot be None when calling SolverKamino.update_contacts")
elif not isinstance(state, State):
raise TypeError(f"state must be of type State, got {type(state)}")
# Skip the conversion if contacts have not been allocated
if self._contacts_kamino is None or self._contacts_kamino._data.model_max_contacts_host == 0:
return
# Ensure the output contacts containers has sufficient size to hold the contact data from Kamino
if self._contacts_kamino._data.model_max_contacts_host > contacts.rigid_contact_max:
raise ValueError(
f"Contacts container has insufficient capacity for Kamino contacts: "
f"model_max_contacts={self._contacts_kamino._data.model_max_contacts_host} > "
f"contacts.rigid_contact_max={contacts.rigid_contact_max}"
)
# If all checks pass, proceed to convert contacts from Kamino to Newton format
self._kamino.convert_contacts_kamino_to_newton(self.model, state, self._contacts_kamino, contacts)
@override
@staticmethod
def register_custom_attributes(builder: ModelBuilder) -> None:
"""
Register custom attributes for SolverKamino.
Args:
builder: The model builder to register the custom attributes to.
"""
# Register State attributes
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="body_f_total",
assignment=Model.AttributeAssignment.STATE,
frequency=Model.AttributeFrequency.BODY,
dtype=wp.spatial_vectorf,
default=wp.spatial_vectorf(0.0),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="joint_q_prev",
assignment=Model.AttributeAssignment.STATE,
frequency=Model.AttributeFrequency.JOINT_COORD,
dtype=wp.float32,
default=0.0,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="joint_lambdas",
assignment=Model.AttributeAssignment.STATE,
frequency=Model.AttributeFrequency.JOINT_CONSTRAINT,
dtype=wp.float32,
default=0.0,
)
)
# Register KaminoSceneAPI attributes so the USD importer will store them on the model
SolverKamino.Config.register_custom_attributes(builder)
###
# Internals
###
@classmethod
def _import_kamino(cls):
"""Import the Kamino dependencies and cache them as class variables."""
if cls._kamino is None:
try:
with warnings.catch_warnings():
# Set a filter to make all ImportWarnings "always" appear
# This is useful to debug import errors on Windows, for example
warnings.simplefilter("always", category=ImportWarning)
from . import _src as kamino # noqa: PLC0415
cls._kamino = kamino
except ImportError as e:
raise ImportError("Kamino backend not found.") from e
@staticmethod
def _validate_model_compatibility(model: Model):
"""
Validates that the model does not contain components unsupported by SolverKamino:
- particles
- springs
- triangles, edges, tetrahedra
- muscles
- equality constraints
- distance, cable, or gimbal joints
Args:
model: The Newton model to validate.
Raises:
ValueError: If the model contains unsupported components.
"""
unsupported_features = []
if model.particle_count > 0:
unsupported_features.append(f"particles (found {model.particle_count})")
if model.spring_count > 0:
unsupported_features.append(f"springs (found {model.spring_count})")
if model.tri_count > 0:
unsupported_features.append(f"triangle elements (found {model.tri_count})")
if model.edge_count > 0:
unsupported_features.append(f"edge elements (found {model.edge_count})")
if model.tet_count > 0:
unsupported_features.append(f"tetrahedral elements (found {model.tet_count})")
if model.muscle_count > 0:
unsupported_features.append(f"muscles (found {model.muscle_count})")
if model.equality_constraint_count > 0:
unsupported_features.append(f"equality constraints (found {model.equality_constraint_count})")
# Check for unsupported joint types
if model.joint_count > 0:
joint_type_np = model.joint_type.numpy()
joint_dof_dim_np = model.joint_dof_dim.numpy()
joint_q_start_np = model.joint_q_start.numpy()
joint_qd_start_np = model.joint_qd_start.numpy()
unsupported_joint_types = {}
for j in range(model.joint_count):
joint_type = int(joint_type_np[j])
dof_dim = (int(joint_dof_dim_np[j][0]), int(joint_dof_dim_np[j][1]))
q_count = int(joint_q_start_np[j + 1] - joint_q_start_np[j])
qd_count = int(joint_qd_start_np[j + 1] - joint_qd_start_np[j])
# Check for explicitly unsupported joint types
if joint_type == JointType.DISTANCE:
unsupported_joint_types["DISTANCE"] = unsupported_joint_types.get("DISTANCE", 0) + 1
elif joint_type == JointType.CABLE:
unsupported_joint_types["CABLE"] = unsupported_joint_types.get("CABLE", 0) + 1
# Check for GIMBAL configuration (3 coords, 3 DoFs, 0 linear/3 angular)
elif joint_type == JointType.D6 and q_count == 3 and qd_count == 3 and dof_dim == (0, 3):
unsupported_joint_types["D6 (GIMBAL)"] = unsupported_joint_types.get("D6 (GIMBAL)", 0) + 1
if len(unsupported_joint_types) > 0:
joint_desc = [f"{name} ({count} instances)" for name, count in unsupported_joint_types.items()]
unsupported_features.append("joint types: " + ", ".join(joint_desc))
# If any unsupported features were found, raise an error
if len(unsupported_features) > 0:
error_msg = "SolverKamino cannot simulate this model due to unsupported features:"
for feature in unsupported_features:
error_msg += "\n - " + feature
raise ValueError(error_msg)
def _update_gravity(self):
"""
Updates Kamino's :class:`GravityModel` from Newton's model.gravity.
Called when :data:`SolverNotifyFlags.MODEL_PROPERTIES` is raised,
indicating that ``model.gravity`` may have changed at runtime.
"""
self._kamino.convert_model_gravity(self.model, self._model_kamino.gravity)
def _update_joint_transforms(self):
"""
Re-derive Kamino joint anchors and axes from Newton's joint_X_p / joint_X_c.
Called when :data:`SolverNotifyFlags.JOINT_PROPERTIES` is raised,
indicating that ``model.joint_X_p`` or ``model.joint_X_c`` may have
changed at runtime (e.g. animated root transforms).
"""
self._kamino.convert_model_joint_transforms(self.model, self._model_kamino.joints)
================================================
FILE: newton/_src/solvers/kamino/tests/.gitignore
================================================
output/
================================================
FILE: newton/_src/solvers/kamino/tests/README.md
================================================
# Running Unit Tests
## CLI
1. Enable the python environment (`conda`, `virtualenv`, `uv`), e.g. for `pyenv` + `virtualenv`:
```bash
pyenv activate newton-dev
```
```bash
cd /newton/newton/_src/solvers/kamino/tests
```
```bash
python -m unittest discover -s . -p 'test_*.py'
```
## VS Code (& Cursor)
We will use the built-in unit-test discovery system of the VS Code IDE described [here](https://code.visualstudio.com/docs/python/testing).
0. Install Newton in editable mode (*can be skip if already installed like this*):
```bash
cd /newton
pip install -e .[dev,data,docs]
```
1. Open `newton/newton/_src/solvers/kamino` folder in VSCode or Cursor:
```bash
cd /newton/newton/_src/solvers/kamino
code .
```
3. Create a `.vscode/settings.json` in `newton/newton/_src/solvers/kamino` with contents:
```json
{
"python.testing.unittestArgs": [
"-v",
"-s",
"./tests",
"-p",
"test_*.py"
],
"python.testing.pytestEnabled": false,
"python.testing.unittestEnabled": true
}
```
4. Run test discovery via `ctrl/cmd + shift + P` and typing `Testing: Focus on Test Explorer View`.
5. The play buttons displayed to the right of the test hierarchy can be used to automatically launch test sets.
----
================================================
FILE: newton/_src/solvers/kamino/tests/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from dataclasses import dataclass
from pathlib import Path
import numpy as np
import warp as wp
###
# Module interface
###
__all__ = ["setup_tests", "test_context"]
###
# Global test context
###
@dataclass
class TestContext:
setup_done: bool = False
"""Whether the global test setup has already run """
verbose: bool = False
"""Global default verbosity flag to be used by unit tests """
device: wp.DeviceLike | None = None
"""Global default device to be used by unit tests """
output_path: Path | None = None
"""Global cache directory for tests to use, if any."""
test_context = TestContext()
""" Global object shared across unit tests, containing status & settings regarding test execution. """
###
# Functions
###
def setup_tests(verbose: bool = False, device: wp.DeviceLike | str | None = None, clear_cache: bool = True):
# Numpy configuration
np.set_printoptions(
linewidth=999999, edgeitems=999999, threshold=999999, precision=10, suppress=True
) # Suppress scientific notation
# Warp configuration
wp.init()
wp.config.mode = "release"
wp.config.enable_backward = False
wp.config.verbose = False
wp.config.verify_fp = False
wp.config.verify_cuda = False
# Clear cache
if clear_cache:
wp.clear_kernel_cache()
wp.clear_lto_cache()
# Update test context
test_context.verbose = verbose
test_context.device = wp.get_device(device)
test_context.setup_done = True
# Set the cache directory for optional test output, if any
# Data directory (contains perfprof.csv)
test_context.output_path = Path(__file__).parent / "output"
test_context.output_path.mkdir(parents=True, exist_ok=True)
================================================
FILE: newton/_src/solvers/kamino/tests/__main__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import argparse
import os
import unittest
from newton._src.solvers.kamino.tests import setup_tests
###
# Utilities
###
# Overload of TextTestResult printing a header for each new test module
class ModuleHeaderTestResult(unittest.TextTestResult):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self._current_module = None
def startTest(self, test):
module = test.__class__.__module__
if module != self._current_module:
self._current_module = module
filename = module.replace(".", "/") + ".py"
# Print spacing + header
self.stream.write("\n\n")
self.stream.write(f"=== Running tests in: {filename} ===\n")
self.stream.write("\n")
self.stream.flush()
super().startTest(test)
# Overload of TextTestRunner printing a header for each new test module
class ModuleHeaderTestRunner(unittest.TextTestRunner):
resultclass = ModuleHeaderTestResult
###
# Test execution
###
if __name__ == "__main__":
# Parse command-line arguments
parser = argparse.ArgumentParser(description="Runs all unit tests in Kamino.")
parser.add_argument(
"--device",
type=str,
default="cuda", # Edit to change device (if not running in command line)
help="The compute device to use.",
)
parser.add_argument(
"--clear-cache",
default=True, # Edit to enable/disable cache clear (if not running in command line)
action=argparse.BooleanOptionalAction,
help="Whether to clear the warp cache before running tests.",
)
parser.add_argument(
"--verbose",
default=False, # Edit to change verbosity (if not running in command line)
action=argparse.BooleanOptionalAction,
help="Whether to print detailed information during tests execution.",
)
args = parser.parse_args()
# Perform global setup
setup_tests(verbose=args.verbose, device=args.device, clear_cache=args.clear_cache)
# Detect all unit tests
test_folder = os.path.dirname(os.path.abspath(__file__))
tests = unittest.defaultTestLoader.discover(test_folder, pattern="test_*.py")
# Run tests
ModuleHeaderTestRunner(verbosity=2).run(tests)
================================================
FILE: newton/_src/solvers/kamino/tests/test_core_builder.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: UNIT TESTS: CORE: BUILDER
"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.bodies import RigidBodyDescriptor
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.geometry import GeometryDescriptor
from newton._src.solvers.kamino._src.core.gravity import (
GRAVITY_ACCEL_DEFAULT,
GRAVITY_DIREC_DEFAULT,
GRAVITY_NAME_DEFAULT,
)
from newton._src.solvers.kamino._src.core.joints import JointActuationType, JointDescriptor, JointDoFType
from newton._src.solvers.kamino._src.core.materials import MaterialDescriptor
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.core.shapes import SphereShape
from newton._src.solvers.kamino._src.core.types import Axis, mat33f, transformf, vec6f
from newton._src.solvers.kamino._src.models.builders import basics
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Utilities
###
def assert_model_matches_builder(test: unittest.TestCase, builder: ModelBuilderKamino, model: ModelKamino):
"""
Assert that a constructed model matches the specifications of the given builder.
Args:
test: The test case instance.
builder: The model builder instance.
model: The constructed model instance.
"""
# Check model sizes and offsets
for w, world in enumerate(builder.worlds):
test.assertEqual(world.wid, w)
test.assertEqual(model.info.num_bodies.numpy()[w], world.num_bodies)
test.assertEqual(model.info.num_joints.numpy()[w], world.num_joints)
test.assertEqual(model.info.num_passive_joints.numpy()[w], world.num_passive_joints)
test.assertEqual(model.info.num_actuated_joints.numpy()[w], world.num_actuated_joints)
test.assertEqual(model.info.num_dynamic_joints.numpy()[w], world.num_dynamic_joints)
test.assertEqual(model.info.num_geoms.numpy()[w], world.num_geoms)
test.assertEqual(model.info.num_body_dofs.numpy()[w], world.num_body_dofs)
test.assertEqual(model.info.num_joint_coords.numpy()[w], world.num_joint_coords)
test.assertEqual(model.info.num_joint_dofs.numpy()[w], world.num_joint_dofs)
test.assertEqual(model.info.num_passive_joint_coords.numpy()[w], world.num_passive_joint_coords)
test.assertEqual(model.info.num_passive_joint_dofs.numpy()[w], world.num_passive_joint_dofs)
test.assertEqual(model.info.num_actuated_joint_coords.numpy()[w], world.num_actuated_joint_coords)
test.assertEqual(model.info.num_actuated_joint_dofs.numpy()[w], world.num_actuated_joint_dofs)
test.assertEqual(model.info.num_joint_cts.numpy()[w], world.num_joint_cts)
test.assertEqual(model.info.num_joint_dynamic_cts.numpy()[w], world.num_dynamic_joint_cts)
test.assertEqual(model.info.num_joint_kinematic_cts.numpy()[w], world.num_kinematic_joint_cts)
test.assertEqual(model.info.bodies_offset.numpy()[w], world.bodies_idx_offset)
test.assertEqual(model.info.joints_offset.numpy()[w], world.joints_idx_offset)
test.assertEqual(model.info.geoms_offset.numpy()[w], world.geoms_idx_offset)
test.assertEqual(model.info.body_dofs_offset.numpy()[w], world.body_dofs_idx_offset)
test.assertEqual(model.info.joint_coords_offset.numpy()[w], world.joint_coords_idx_offset)
test.assertEqual(model.info.joint_dofs_offset.numpy()[w], world.joint_dofs_idx_offset)
test.assertEqual(model.info.joint_passive_coords_offset.numpy()[w], world.joint_passive_coords_idx_offset)
test.assertEqual(model.info.joint_passive_dofs_offset.numpy()[w], world.joint_passive_dofs_idx_offset)
test.assertEqual(model.info.joint_actuated_coords_offset.numpy()[w], world.joint_actuated_coords_idx_offset)
test.assertEqual(model.info.joint_actuated_dofs_offset.numpy()[w], world.joint_actuated_dofs_idx_offset)
# TODO: test.assertEqual(model.info.joint_cts_offset.numpy()[w], world.joint_cts_idx_offset)
test.assertEqual(model.info.joint_dynamic_cts_offset.numpy()[w], world.joint_dynamic_cts_idx_offset)
test.assertEqual(model.info.joint_kinematic_cts_offset.numpy()[w], world.joint_kinematic_cts_idx_offset)
for i in range(model.size.sum_of_num_bodies):
test.assertEqual(model.bodies.wid.numpy()[i], builder.bodies[i].wid)
test.assertEqual(model.bodies.bid.numpy()[i], builder.bodies[i].bid)
for i in range(model.size.sum_of_num_joints):
wid = builder.joints[i].wid
bid_offset = builder.worlds[wid].bodies_idx_offset
test.assertEqual(model.joints.wid.numpy()[i], builder.joints[i].wid)
test.assertEqual(model.joints.jid.numpy()[i], builder.joints[i].jid)
test.assertEqual(
model.joints.bid_B.numpy()[i], builder.joints[i].bid_B + bid_offset if builder.joints[i].bid_B >= 0 else -1
)
test.assertEqual(
model.joints.bid_F.numpy()[i], builder.joints[i].bid_F + bid_offset if builder.joints[i].bid_F >= 0 else -1
)
for i in range(model.size.sum_of_num_geoms):
wid = builder.geoms[i].wid
bid_offset = builder.worlds[wid].bodies_idx_offset
test.assertEqual(model.geoms.wid.numpy()[i], builder.geoms[i].wid)
test.assertEqual(model.geoms.gid.numpy()[i], builder.geoms[i].gid)
test.assertEqual(
model.geoms.bid.numpy()[i],
builder.geoms[i].body + bid_offset if builder.geoms[i].body >= 0 else -1,
)
# Optional printout for debugging
msg.info("model.bodies.wid: %s", model.bodies.wid)
msg.info("model.bodies.bid: %s\n", model.bodies.bid)
msg.info("model.joints.wid: %s", model.joints.wid)
msg.info("model.joints.jid: %s", model.joints.jid)
msg.info("model.joints.bid_B: %s", model.joints.bid_B)
msg.info("model.joints.bid_F: %s\n", model.joints.bid_F)
msg.info("model.geoms.wid: %s", model.geoms.wid)
msg.info("model.geoms.gid: %s", model.geoms.gid)
msg.info("model.geoms.bid: %s", model.geoms.bid)
msg.info("model.info.bodies_offset: %s", model.info.bodies_offset)
msg.info("model.info.joints_offset: %s", model.info.joints_offset)
msg.info("model.info.body_dofs_offset: %s", model.info.body_dofs_offset)
msg.info("model.info.joint_coords_offset: %s", model.info.joint_coords_offset)
msg.info("model.info.joint_dofs_offset: %s", model.info.joint_dofs_offset)
msg.info("model.info.joint_cts_offset: %s\n", model.info.joint_cts_offset)
msg.info("model.info.joint_dynamic_cts_offset: %s\n", model.info.joint_dynamic_cts_offset)
msg.info("model.info.joint_kinematic_cts_offset: %s\n", model.info.joint_kinematic_cts_offset)
msg.info("model.info.joint_passive_coords_offset: %s", model.info.joint_passive_coords_offset)
msg.info("model.info.joint_passive_dofs_offset: %s", model.info.joint_passive_dofs_offset)
msg.info("model.info.joint_actuated_coords_offset: %s", model.info.joint_actuated_coords_offset)
msg.info("model.info.joint_actuated_dofs_offset: %s\n", model.info.joint_actuated_dofs_offset)
msg.info("model.info.base_body_index: %s", model.info.base_body_index)
msg.info("model.info.base_joint_index: %s", model.info.base_joint_index)
###
# Tests
###
class TestModelBuilder(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True to enable verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default(self):
builder = ModelBuilderKamino()
self.assertEqual(builder.num_worlds, 0)
self.assertEqual(builder.num_bodies, 0)
self.assertEqual(builder.num_joints, 0)
self.assertEqual(builder.num_geoms, 0)
self.assertEqual(builder.num_materials, 1) # Default material is always created
self.assertEqual(builder.num_body_dofs, 0)
self.assertEqual(builder.num_joint_dofs, 0)
self.assertEqual(builder.num_passive_joint_dofs, 0)
self.assertEqual(builder.num_actuated_joint_dofs, 0)
self.assertEqual(builder.num_joint_cts, 0)
self.assertEqual(builder.num_dynamic_joint_cts, 0)
self.assertEqual(builder.num_kinematic_joint_cts, 0)
self.assertEqual(len(builder.bodies), 0)
self.assertEqual(len(builder.joints), 0)
self.assertEqual(len(builder.geoms), 0)
self.assertEqual(len(builder.materials), 1) # Default material is always created
def test_01_make_default_with_world(self):
builder = ModelBuilderKamino(default_world=True)
self.assertEqual(builder.num_worlds, 1)
self.assertEqual(builder.num_bodies, 0)
self.assertEqual(builder.num_joints, 0)
self.assertEqual(builder.num_geoms, 0)
self.assertEqual(builder.num_materials, 1) # Default material is always created
self.assertEqual(builder.num_body_dofs, 0)
self.assertEqual(builder.num_joint_dofs, 0)
self.assertEqual(builder.num_passive_joint_dofs, 0)
self.assertEqual(builder.num_actuated_joint_dofs, 0)
self.assertEqual(builder.num_joint_cts, 0)
self.assertEqual(len(builder.bodies), 0)
self.assertEqual(len(builder.joints), 0)
self.assertEqual(len(builder.geoms), 0)
self.assertEqual(len(builder.materials), 1) # Default material is always created
def test_02_add_world(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Y)
self.assertEqual(wid, 0)
self.assertEqual(builder.num_worlds, 1)
self.assertEqual(builder.worlds[wid].wid, wid)
self.assertEqual(builder.worlds[wid].name, "test_world")
self.assertEqual(builder.up_axes[wid], Axis.Y)
self.assertEqual(builder.gravity[wid].name, GRAVITY_NAME_DEFAULT)
self.assertEqual(builder.gravity[wid].acceleration, GRAVITY_ACCEL_DEFAULT)
np.testing.assert_array_equal(builder.gravity[wid].direction, np.array(GRAVITY_DIREC_DEFAULT, dtype=np.float32))
def test_03_add_rigid_body(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
bid = builder.add_rigid_body(
name="test_rigid_body",
m_i=1.0,
i_I_i=mat33f(np.eye(3, dtype=np.float32)),
q_i_0=transformf(),
u_i_0=vec6f(),
world_index=wid,
)
self.assertEqual(builder.num_bodies, 1)
self.assertEqual(bid, 0)
self.assertEqual(bid, builder.bodies[bid].bid)
self.assertEqual(builder.bodies[bid].name, "test_rigid_body")
self.assertEqual(builder.bodies[bid].wid, wid)
self.assertEqual(builder.bodies[bid].m_i, 1.0)
np.testing.assert_array_equal(builder.bodies[bid].i_I_i, np.eye(3, dtype=np.float32).flatten())
np.testing.assert_array_equal(builder.bodies[bid].q_i_0, np.array(transformf(), dtype=np.float32))
np.testing.assert_array_equal(builder.bodies[bid].u_i_0, np.zeros(6, dtype=np.float32))
def test_04_add_rigid_body_descriptor(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
body = RigidBodyDescriptor(
name="test_rigid_body",
m_i=2.0,
i_I_i=mat33f(2.0 * np.eye(3, dtype=np.float32)),
q_i_0=transformf(),
u_i_0=vec6f(),
)
bid = builder.add_rigid_body_descriptor(body, world_index=wid)
self.assertEqual(builder.num_bodies, 1)
self.assertEqual(bid, 0)
self.assertEqual(bid, builder.bodies[bid].bid)
self.assertEqual(builder.bodies[bid].name, "test_rigid_body")
self.assertEqual(builder.bodies[bid].wid, wid)
self.assertEqual(builder.bodies[bid].m_i, 2.0)
np.testing.assert_array_equal(builder.bodies[bid].i_I_i, 2.0 * np.eye(3, dtype=np.float32).flatten())
np.testing.assert_array_equal(builder.bodies[bid].q_i_0, np.array(transformf(), dtype=np.float32))
np.testing.assert_array_equal(builder.bodies[bid].u_i_0, np.zeros(6, dtype=np.float32))
def test_05_add_duplicate_rigid_body(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
body_0 = RigidBodyDescriptor(
name="test_rigid_body",
m_i=2.0,
i_I_i=mat33f(2.0 * np.eye(3, dtype=np.float32)),
q_i_0=transformf(),
u_i_0=vec6f(),
)
builder.add_rigid_body_descriptor(body_0, world_index=wid)
# Attempt to add the same body again and expect an error
self.assertRaises(ValueError, builder.add_rigid_body_descriptor, body_0, world_index=wid)
def test_06_add_joint(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
# Define two rigid bodies to connect with a joint
body_0 = RigidBodyDescriptor(
name="test_rigid_body_0",
m_i=2.0,
i_I_i=mat33f(2.0 * np.eye(3, dtype=np.float32)),
q_i_0=transformf(),
u_i_0=vec6f(),
)
body_1 = RigidBodyDescriptor(
name="test_rigid_body_1",
m_i=1.0,
i_I_i=mat33f(1.0 * np.eye(3, dtype=np.float32)),
q_i_0=transformf(),
u_i_0=vec6f(),
)
bid_0 = builder.add_rigid_body_descriptor(body_0, world_index=wid)
bid_1 = builder.add_rigid_body_descriptor(body_1, world_index=wid)
# Define a joint descriptor
joint = JointDescriptor(
name="test_joint",
bid_B=bid_0,
bid_F=bid_1,
dof_type=JointDoFType.PRISMATIC,
act_type=JointActuationType.FORCE,
a_j=1.0,
b_j=1.0,
)
jid = builder.add_joint_descriptor(joint, world_index=wid)
self.assertEqual(builder.num_bodies, 2)
self.assertEqual(builder.num_joints, 1)
self.assertEqual(jid, 0)
self.assertEqual(jid, builder.joints[jid].jid)
self.assertEqual(builder.joints[jid].name, "test_joint")
self.assertEqual(builder.joints[jid].wid, wid)
self.assertEqual(builder.joints[jid].bid_B, bid_0)
self.assertEqual(builder.joints[jid].bid_F, bid_1)
self.assertEqual(builder.joints[jid].dof_type, JointDoFType.PRISMATIC)
self.assertEqual(builder.joints[jid].act_type, JointActuationType.FORCE)
self.assertEqual(builder.joints[jid].a_j, [1.0])
self.assertEqual(builder.joints[jid].b_j, [1.0])
self.assertTrue(builder.joints[jid].is_dynamic)
self.assertTrue(builder.joints[jid].num_kinematic_cts, 5)
self.assertTrue(builder.joints[jid].num_dynamic_cts, 1)
def test_07_add_duplicate_joint(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
# Define two rigid bodies to connect with a joint
body_0 = RigidBodyDescriptor(
name="test_rigid_body_0",
m_i=2.0,
i_I_i=mat33f(2.0 * np.eye(3, dtype=np.float32)),
q_i_0=transformf(),
u_i_0=vec6f(),
)
body_1 = RigidBodyDescriptor(
name="test_rigid_body_1",
m_i=1.0,
i_I_i=mat33f(1.0 * np.eye(3, dtype=np.float32)),
q_i_0=transformf(),
u_i_0=vec6f(),
)
bid_0 = builder.add_rigid_body_descriptor(body_0, world_index=wid)
bid_1 = builder.add_rigid_body_descriptor(body_1, world_index=wid)
# Define a joint descriptor
joint = JointDescriptor(
name="test_joint",
bid_B=bid_0,
bid_F=bid_1,
dof_type=JointDoFType.PRISMATIC,
act_type=JointActuationType.FORCE,
)
builder.add_joint_descriptor(joint, world_index=wid)
# Attempt to add the same joint again and expect an error
self.assertRaises(ValueError, builder.add_joint_descriptor, joint, world_index=wid)
def test_08_add_invalid_joint(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
# Define a joint descriptor
joint = JointDescriptor(
name="test_joint",
dof_type=JointDoFType.PRISMATIC,
act_type=JointActuationType.FORCE,
)
# Attempt to add a joint without specifying bodies and expect an error
self.assertRaises(ValueError, builder.add_joint_descriptor, joint, world_index=wid)
def test_09_add_geometry(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
self.assertTrue(builder.num_geoms == 0)
# Create a collision geometry descriptor from the geometry descriptor
gid = builder.add_geometry(
name="test_geom",
shape=SphereShape(radius=1.0),
world_index=wid,
)
self.assertEqual(builder.num_geoms, 1)
self.assertEqual(gid, 0)
self.assertEqual(gid, builder.geoms[gid].gid)
self.assertEqual(builder.geoms[gid].name, "test_geom")
self.assertEqual(builder.geoms[gid].body, -1)
self.assertEqual(builder.geoms[gid].wid, wid)
self.assertEqual(builder.geoms[gid].mid, 0)
def test_10_add_geometry_descriptors(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
self.assertTrue(builder.num_geoms == 0)
# Define a geometry descriptor
geom = GeometryDescriptor(name="test_geom", shape=SphereShape(radius=1.0))
gid = builder.add_geometry_descriptor(geom, world_index=wid)
self.assertEqual(builder.num_geoms, 1)
self.assertEqual(gid, 0)
self.assertEqual(gid, builder.geoms[gid].gid)
self.assertEqual(builder.geoms[gid].name, "test_geom")
self.assertEqual(builder.geoms[gid].body, -1)
self.assertEqual(builder.geoms[gid].wid, wid)
self.assertEqual(builder.geoms[gid].mid, -1)
def test_11_add_material(self):
builder = ModelBuilderKamino()
wid = builder.add_world(name="test_world", up_axis=Axis.Z)
self.assertEqual(builder.num_materials, 1) # Default material exists
material = MaterialDescriptor(
name="test_material", density=500.0, restitution=0.8, static_friction=0.6, dynamic_friction=0.4
)
mid = builder.add_material(material=material)
self.assertEqual(builder.num_materials, 2)
self.assertEqual(mid, 1)
self.assertEqual(mid, builder.materials[mid].mid)
self.assertEqual(builder.materials[mid].name, "test_material")
self.assertEqual(builder.materials[mid].wid, wid)
self.assertEqual(builder.materials[mid].density, 500.0)
self.assertEqual(builder.materials[mid].restitution, 0.8)
self.assertEqual(builder.materials[mid].static_friction, 0.6)
self.assertEqual(builder.materials[mid].dynamic_friction, 0.4)
def test_12_make_builder_box_on_plane(self):
# Construct box-on-plane model
builder = basics.build_box_on_plane()
self.assertEqual(builder.num_worlds, 1)
self.assertEqual(builder.num_bodies, 1)
self.assertEqual(builder.num_joints, 0)
self.assertEqual(builder.num_geoms, 2)
self.assertEqual(builder.worlds[0].name, "box_on_plane")
self.assertEqual(builder.worlds[0].wid, 0)
# Extract the IDs of bodies, joints, and collision geometries
bids = [body.bid for body in builder.bodies]
jids = [joint.jid for joint in builder.joints]
gids = [geom.gid for geom in builder.geoms]
# Check the number of bodies, joints, and collision geometries
for i, bid in enumerate(bids):
self.assertEqual(bid, i)
self.assertEqual(bid, builder.bodies[i].bid)
for i, jid in enumerate(jids):
self.assertEqual(jid, i)
self.assertEqual(jid, builder.joints[i].jid)
for i, gid in enumerate(gids):
self.assertEqual(gid, i)
self.assertEqual(gid, builder.geoms[i].gid)
# Build the model
model = builder.finalize(self.default_device)
self.assertEqual(model.size.num_worlds, 1)
self.assertEqual(model.size.sum_of_num_bodies, 1)
self.assertEqual(model.size.sum_of_num_joints, 0)
self.assertEqual(model.size.sum_of_num_geoms, 2)
self.assertEqual(model.device, self.default_device)
def test_13_make_builder_box_pendulum(self):
# Construct box-pendulum model
builder = basics.build_box_pendulum()
self.assertEqual(builder.num_worlds, 1)
self.assertEqual(builder.num_bodies, 1)
self.assertEqual(builder.num_joints, 1)
self.assertEqual(builder.num_geoms, 2)
self.assertEqual(builder.worlds[0].name, "box_pendulum")
self.assertEqual(builder.worlds[0].wid, 0)
# Extract the IDs of bodies, joints, and collision geometries
bids = [body.bid for body in builder.bodies]
jids = [joint.jid for joint in builder.joints]
gids = [geom.gid for geom in builder.geoms]
# Check the number of bodies, joints, and collision geometries
for i, bid in enumerate(bids):
self.assertEqual(bid, i)
self.assertEqual(bid, builder.bodies[i].bid)
for i, jid in enumerate(jids):
self.assertEqual(jid, i)
self.assertEqual(jid, builder.joints[i].jid)
for i, gid in enumerate(gids):
self.assertEqual(gid, i)
self.assertEqual(gid, builder.geoms[i].gid)
# Build the model
model = builder.finalize(self.default_device)
self.assertEqual(model.size.num_worlds, 1)
self.assertEqual(model.size.sum_of_num_bodies, 1)
self.assertEqual(model.size.sum_of_num_joints, 1)
self.assertEqual(model.size.sum_of_num_geoms, 2)
self.assertEqual(model.device, self.default_device)
def test_14_make_builder_boxes_hinged(self):
# Construct boxes-hinged model
builder = basics.build_boxes_hinged()
self.assertEqual(builder.num_worlds, 1)
self.assertEqual(builder.num_bodies, 2)
self.assertEqual(builder.num_joints, 1)
self.assertEqual(builder.num_geoms, 3)
self.assertEqual(builder.worlds[0].name, "boxes_hinged")
self.assertEqual(builder.worlds[0].wid, 0)
# Extract the IDs of bodies, joints, and collision geometries
bids = [body.bid for body in builder.bodies]
jids = [joint.jid for joint in builder.joints]
gids = [geom.gid for geom in builder.geoms]
# Check the number of bodies, joints, and collision geometries
for i, bid in enumerate(bids):
self.assertEqual(bid, i)
self.assertEqual(bid, builder.bodies[i].bid)
for i, jid in enumerate(jids):
self.assertEqual(jid, i)
self.assertEqual(jid, builder.joints[i].jid)
for i, gid in enumerate(gids):
self.assertEqual(gid, i)
self.assertEqual(gid, builder.geoms[i].gid)
# Build the model
model = builder.finalize(self.default_device)
self.assertEqual(model.size.num_worlds, 1)
self.assertEqual(model.size.sum_of_num_bodies, 2)
self.assertEqual(model.size.sum_of_num_joints, 1)
self.assertEqual(model.size.sum_of_num_geoms, 3)
self.assertEqual(model.device, self.default_device)
def test_15_make_builder_boxes_nunchaku(self):
# Construct boxes-nunchaku model
builder = basics.build_boxes_nunchaku()
self.assertEqual(builder.num_worlds, 1)
self.assertEqual(builder.num_bodies, 3)
self.assertEqual(builder.num_joints, 2)
self.assertEqual(builder.num_geoms, 4)
self.assertEqual(builder.worlds[0].name, "boxes_nunchaku")
self.assertEqual(builder.worlds[0].wid, 0)
# Extract the IDs of bodies, joints, and collision geometries
bids = [body.bid for body in builder.bodies]
jids = [joint.jid for joint in builder.joints]
gids = [geom.gid for geom in builder.geoms]
# Check the number of bodies, joints, and collision geometries
for i, bid in enumerate(bids):
self.assertEqual(bid, i)
self.assertEqual(bid, builder.bodies[i].bid)
for i, jid in enumerate(jids):
self.assertEqual(jid, i)
self.assertEqual(jid, builder.joints[i].jid)
for i, gid in enumerate(gids):
self.assertEqual(gid, i)
self.assertEqual(gid, builder.geoms[i].gid)
# Build the model
model = builder.finalize(self.default_device)
self.assertEqual(model.size.sum_of_num_bodies, 3)
self.assertEqual(model.size.sum_of_num_joints, 2)
self.assertEqual(model.size.sum_of_num_geoms, 4)
self.assertEqual(model.device, self.default_device)
def test_16_make_builder_boxes_fourbar(self):
# Construct boxes-fourbar model
builder = basics.build_boxes_fourbar()
self.assertEqual(builder.num_worlds, 1)
self.assertEqual(builder.num_bodies, 4)
self.assertEqual(builder.num_joints, 4)
self.assertEqual(builder.num_geoms, 5)
self.assertEqual(builder.worlds[0].name, "boxes_fourbar")
self.assertEqual(builder.worlds[0].wid, 0)
# Extract the IDs of bodies, joints, and collision geometries
bids = [body.bid for body in builder.bodies]
jids = [joint.jid for joint in builder.joints]
gids = [geom.gid for geom in builder.geoms]
# Check the number of bodies, joints, and collision geometries
for i, bid in enumerate(bids):
self.assertEqual(bid, i)
self.assertEqual(bid, builder.bodies[i].bid)
for i, jid in enumerate(jids):
self.assertEqual(jid, i)
self.assertEqual(jid, builder.joints[i].jid)
for i, gid in enumerate(gids):
self.assertEqual(gid, i)
self.assertEqual(gid, builder.geoms[i].gid)
# Generate meta-data for collision detection and contacts allocation
model_candidate_pairs = builder.make_collision_candidate_pairs(allow_neighbors=False)
model_excluded_pairs = builder.make_collision_excluded_pairs(allow_neighbors=False)
world_num_collidables, model_num_collidables = builder.compute_num_collidable_geoms(model_candidate_pairs)
model_min_contacts, world_min_contacts = builder.compute_required_contact_capacity(model_candidate_pairs)
# Optional printouts for debugging
msg.info("model_candidate_pairs: %s", model_candidate_pairs)
msg.info("model_candidate_pairs_count: %s", len(model_candidate_pairs))
msg.info("model_excluded_pairs: %s", model_excluded_pairs)
msg.info("model_excluded_pairs_count: %s", len(model_excluded_pairs))
msg.info("world_num_collidables: %s", world_num_collidables)
msg.info("model_num_collidables: %s", model_num_collidables)
msg.info("model_min_contacts: %s", model_min_contacts)
msg.info("world_min_contacts: %s", world_min_contacts)
# Check that the generated meta-data matches expected values for this model
expected_contacts_per_world = 2 * len(model_candidate_pairs) * 12 # 12 is the max contacts per pair
self.assertEqual(world_num_collidables[0], 5)
self.assertEqual(model_num_collidables, 5)
self.assertEqual(len(model_candidate_pairs), 6)
self.assertEqual(len(model_excluded_pairs), 4)
self.assertEqual(model_min_contacts, expected_contacts_per_world)
self.assertEqual(world_min_contacts[0], expected_contacts_per_world)
# Build the model
model = builder.finalize(self.default_device)
self.assertEqual(model.size.sum_of_num_bodies, 4)
self.assertEqual(model.size.sum_of_num_joints, 4)
self.assertEqual(model.size.sum_of_num_geoms, 5)
self.assertEqual(model.device, self.default_device)
def test_17_make_builder_cartpole(self):
# Construct cartpole model
builder = basics.build_cartpole()
self.assertEqual(builder.num_worlds, 1)
self.assertEqual(builder.num_bodies, 2)
self.assertEqual(builder.num_joints, 2)
self.assertEqual(builder.num_geoms, 4)
self.assertEqual(builder.worlds[0].name, "cartpole")
self.assertEqual(builder.worlds[0].wid, 0)
# Extract the IDs of bodies, joints, and collision geometries
bids = [body.bid for body in builder.bodies]
jids = [joint.jid for joint in builder.joints]
gids = [geom.gid for geom in builder.geoms]
# Check the number of bodies, joints, and collision geometries
for i, bid in enumerate(bids):
self.assertEqual(bid, i)
self.assertEqual(bid, builder.bodies[i].bid)
for i, jid in enumerate(jids):
self.assertEqual(jid, i)
self.assertEqual(jid, builder.joints[i].jid)
for i, gid in enumerate(gids):
self.assertEqual(gid, i)
self.assertEqual(gid, builder.geoms[i].gid)
# Generate meta-data for collision detection and contacts allocation
model_candidate_pairs = builder.make_collision_candidate_pairs(allow_neighbors=False)
model_excluded_pairs = builder.make_collision_excluded_pairs(allow_neighbors=False)
world_num_collidables, model_num_collidables = builder.compute_num_collidable_geoms(model_candidate_pairs)
model_min_contacts, world_min_contacts = builder.compute_required_contact_capacity(model_candidate_pairs)
# Optional printouts for debugging
msg.info("model_candidate_pairs: %s", model_candidate_pairs)
msg.info("model_candidate_pairs_count: %s", len(model_candidate_pairs))
msg.info("model_excluded_pairs: %s", model_excluded_pairs)
msg.info("model_excluded_pairs_count: %s", len(model_excluded_pairs))
msg.info("world_num_collidables: %s", world_num_collidables)
msg.info("model_num_collidables: %s", model_num_collidables)
msg.info("model_min_contacts: %s", model_min_contacts)
msg.info("world_min_contacts: %s", world_min_contacts)
# Check that the generated meta-data matches expected values for this model
expected_contacts_per_world = 2 * len(model_candidate_pairs) * 12 # 12 is the max contacts per pair
self.assertEqual(world_num_collidables[0], 3)
self.assertEqual(model_num_collidables, 3)
self.assertEqual(len(model_candidate_pairs), 2)
self.assertEqual(len(model_excluded_pairs), 4)
self.assertEqual(model_min_contacts, expected_contacts_per_world)
self.assertEqual(world_min_contacts[0], expected_contacts_per_world)
# Build the model
model = builder.finalize(self.default_device)
self.assertEqual(model.size.sum_of_num_bodies, 2)
self.assertEqual(model.size.sum_of_num_joints, 2)
self.assertEqual(model.size.sum_of_num_geoms, 4)
self.assertEqual(model.device, self.default_device)
def test_18_add_two_cartpole_worlds_to_builder(self):
# Construct cartpole model
builder = ModelBuilderKamino(default_world=False)
builder = basics.build_cartpole(builder=builder, new_world=True)
builder = basics.build_cartpole(builder=builder, new_world=True)
builder = basics.build_cartpole(builder=builder, new_world=True)
self.assertEqual(builder.num_worlds, 3)
self.assertEqual(builder.num_bodies, 6)
self.assertEqual(builder.num_joints, 6)
self.assertEqual(builder.num_geoms, 12)
# Build the model
model = builder.finalize(self.default_device)
# Verify that the contents of the model matches those of the combined builder
assert_model_matches_builder(self, builder, model)
def test_19_add_two_cartpole_builders(self):
# Construct cartpole model
builder0 = basics.build_cartpole()
builder1 = basics.build_cartpole()
builder2 = basics.build_cartpole()
# Combine two builders into one with two worlds
builder0.add_builder(builder1)
builder0.add_builder(builder2)
self.assertEqual(builder0.num_worlds, 3)
self.assertEqual(builder0.num_bodies, 6)
self.assertEqual(builder0.num_joints, 6)
self.assertEqual(builder0.num_geoms, 12)
# Build the model
model = builder0.finalize(self.default_device)
# Verify that the contents of the model matches those of the combined builder
assert_model_matches_builder(self, builder0, model)
def test_20_make_homogeneous_multi_cartpole_builder(self):
# Construct cartpole model
builder = make_homogeneous_builder(num_worlds=3, build_fn=basics.build_cartpole)
self.assertEqual(builder.num_worlds, 3)
self.assertEqual(builder.num_bodies, 6)
self.assertEqual(builder.num_joints, 6)
self.assertEqual(builder.num_geoms, 12)
# Build the model
model = builder.finalize(self.default_device)
# Verify that the contents of the model matches those of the combined builder
assert_model_matches_builder(self, builder, model)
def test_21_make_homogeneous_multi_fourbar_builder(self):
# Construct fourbar model
builder = make_homogeneous_builder(num_worlds=3, build_fn=basics.build_boxes_fourbar)
self.assertEqual(builder.num_worlds, 3)
self.assertEqual(builder.num_bodies, 12)
self.assertEqual(builder.num_joints, 12)
self.assertEqual(builder.num_geoms, 15)
# Generate meta-data for collision detection and contacts allocation
model_candidate_pairs = builder.make_collision_candidate_pairs(allow_neighbors=False)
model_excluded_pairs = builder.make_collision_excluded_pairs(allow_neighbors=False)
world_num_collidables, model_num_collidables = builder.compute_num_collidable_geoms(model_candidate_pairs)
model_min_contacts, world_min_contacts = builder.compute_required_contact_capacity(model_candidate_pairs)
# Optional printouts for debugging
msg.info("model_candidate_pairs: %s", model_candidate_pairs)
msg.info("model_candidate_pairs_count: %s", len(model_candidate_pairs))
msg.info("model_excluded_pairs: %s", model_excluded_pairs)
msg.info("model_excluded_pairs_count: %s", len(model_excluded_pairs))
msg.info("world_num_collidables: %s", world_num_collidables)
msg.info("model_num_collidables: %s", model_num_collidables)
msg.info("model_min_contacts: %s", model_min_contacts)
msg.info("world_min_contacts: %s", world_min_contacts)
# Check that the generated meta-data matches expected values for this model
expected_contacts_per_world = 2 * 6 * 12 # 12 is the max contacts per pair
self.assertEqual(model_num_collidables, 5 * builder.num_worlds)
self.assertEqual(world_num_collidables, [5] * builder.num_worlds)
self.assertEqual(len(model_candidate_pairs), 6 * builder.num_worlds)
self.assertEqual(len(model_excluded_pairs), 4 * builder.num_worlds)
self.assertEqual(model_min_contacts, expected_contacts_per_world * builder.num_worlds)
self.assertEqual(world_min_contacts, [expected_contacts_per_world] * builder.num_worlds)
# Build the model
model = builder.finalize(self.default_device)
# Verify that the contents of the model matches those of the combined builder
assert_model_matches_builder(self, builder, model)
def test_22_make_heterogeneous_test_builder(self):
# Construct cartpole model
builder = basics.make_basics_heterogeneous_builder(ground=True)
self.assertEqual(builder.num_worlds, 6)
self.assertEqual(builder.num_bodies, 13)
self.assertEqual(builder.num_joints, 10)
self.assertEqual(builder.num_geoms, 20)
# Build the model
model = builder.finalize(self.default_device)
# Verify that the contents of the model matches those of the combined builder
assert_model_matches_builder(self, builder, model)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_core_geometry.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for core geometry containers and operations"""
import unittest
import numpy as np
import warp as wp
from newton._src.geometry.types import GeoType
from newton._src.solvers.kamino._src.core.geometry import GeometryDescriptor
from newton._src.solvers.kamino._src.core.shapes import (
MeshShape,
SphereShape,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestGeometryDescriptor(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_primitive_shape_geom(self):
# Create a geometry descriptor with a sphere shape
geom = GeometryDescriptor(
name="sphere_geom",
shape=SphereShape(radius=0.42),
material="concrete",
group=3,
max_contacts=10,
gap=0.01,
margin=0.02,
)
# Check default values
self.assertEqual(geom.name, "sphere_geom")
self.assertEqual(geom.body, -1)
self.assertEqual(geom.shape.type, GeoType.SPHERE)
self.assertEqual(geom.shape.params, 0.42)
self.assertEqual(geom.shape.name, "sphere")
self.assertEqual(geom.offset, wp.transform_identity(dtype=wp.float32))
self.assertEqual(geom.material, "concrete")
self.assertEqual(geom.group, 3)
self.assertEqual(geom.max_contacts, 10)
self.assertEqual(geom.gap, 0.01)
self.assertEqual(geom.margin, 0.02)
self.assertEqual(geom.wid, -1)
self.assertEqual(geom.gid, -1)
self.assertEqual(geom.mid, -1)
self.assertEqual(geom.flags, 7)
self.assertIsInstance(geom.shape.uid, str)
# Check hash function
geom_hash = hash(geom)
geom_hash2 = hash(geom)
shape_hash = hash(geom.shape)
self.assertEqual(geom_hash, geom_hash2)
self.assertEqual(shape_hash, shape_hash)
msg.info(f"geom_hash: {geom_hash}")
msg.info(f"geom_hash2: {geom_hash2}")
msg.info(f"shape_hash: {shape_hash}")
def test_01_mesh_shape_geom(self):
# Create a geometry descriptor with a mesh shape
vertices = [(0, 0, 0), (1, 0, 0), (0, 1, 0)]
indices = [(0, 1, 2)]
geom = GeometryDescriptor(
name="mesh_geom",
shape=MeshShape(vertices, indices),
material="steel",
group=3,
max_contacts=10,
gap=0.01,
margin=0.02,
)
# Check default values
self.assertEqual(geom.name, "mesh_geom")
self.assertEqual(geom.body, -1)
self.assertEqual(geom.shape.type, GeoType.MESH)
self.assertEqual(geom.shape.params, (1.0, 1.0, 1.0))
self.assertEqual(geom.shape.name, "mesh")
self.assertEqual(geom.offset, wp.transform_identity(dtype=wp.float32))
self.assertEqual(geom.material, "steel")
self.assertEqual(geom.group, 3)
self.assertEqual(geom.max_contacts, 10)
self.assertEqual(geom.gap, 0.01)
self.assertEqual(geom.margin, 0.02)
self.assertEqual(geom.wid, -1)
self.assertEqual(geom.gid, -1)
self.assertEqual(geom.mid, -1)
self.assertEqual(geom.flags, 7)
self.assertIsInstance(geom.shape.uid, str)
self.assertTrue(np.array_equal(geom.shape.vertices, np.array(vertices)))
self.assertTrue(np.array_equal(geom.shape.indices, np.array(indices).flatten()))
# Check hash function
geom_hash = hash(geom)
geom_hash2 = hash(geom)
shape_hash = hash(geom.shape)
self.assertEqual(geom_hash, geom_hash2)
self.assertEqual(shape_hash, shape_hash)
msg.info(f"geom_hash: {geom_hash}")
msg.info(f"geom_hash2: {geom_hash2}")
msg.info(f"shape_hash: {shape_hash}")
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_core_joints.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the `kamino.core.joints` module"""
import unittest
import warp as wp
from newton._src.solvers.kamino._src.core.joints import JointDoFType
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestCoreJoints(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True to enable verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_joint_dof_type_enum(self):
doftype = JointDoFType.REVOLUTE
# Optional verbose output
msg.info(f"doftype: {doftype}")
msg.info(f"doftype.value: {doftype.value}")
msg.info(f"doftype.name: {doftype.name}")
msg.info(f"doftype.num_cts: {doftype.num_cts}")
msg.info(f"doftype.num_dofs: {doftype.num_dofs}")
msg.info(f"doftype.cts_axes: {doftype.cts_axes}")
msg.info(f"doftype.dofs_axes: {doftype.dofs_axes}")
# Check the enum values
self.assertEqual(doftype.value, JointDoFType.REVOLUTE)
self.assertEqual(doftype.name, "REVOLUTE")
self.assertEqual(doftype.num_cts, 5)
self.assertEqual(doftype.num_dofs, 1)
self.assertEqual(doftype.cts_axes, (0, 1, 2, 4, 5))
self.assertEqual(doftype.dofs_axes, (3,))
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_core_materials.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the `core/materials.py` module."""
import unittest
# Module to be tested
from newton._src.solvers.kamino._src.core.materials import (
DEFAULT_FRICTION,
DEFAULT_RESTITUTION,
MaterialDescriptor,
MaterialManager,
MaterialPairProperties,
)
# Test utilities
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Utilities
###
def tril_index(i: int, j: int) -> int:
if i < j:
i, j = j, i
return (i * (i + 1)) // 2 + j
###
# Tests
###
class TestMaterials(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for verbose output
def test_00_default_material_properties(self):
# Create a default-constructed surface material
material = MaterialDescriptor(name="default")
# Check default values
self.assertEqual(material.restitution, DEFAULT_RESTITUTION)
self.assertEqual(material.static_friction, DEFAULT_FRICTION)
self.assertEqual(material.dynamic_friction, DEFAULT_FRICTION)
def test_00_default_material_pair_properties(self):
# Create a default-constructed surface material
material_pair = MaterialPairProperties()
# Check default values
self.assertEqual(material_pair.restitution, DEFAULT_RESTITUTION)
self.assertEqual(material_pair.static_friction, DEFAULT_FRICTION)
self.assertEqual(material_pair.dynamic_friction, DEFAULT_FRICTION)
def test_01_material_manager_default_material(self):
# Create a default-constructed material manager
manager = MaterialManager()
self.assertEqual(manager.num_materials, 1)
# Create a default-constructed material descriptor
dm = manager.default
# Check initial default material values
self.assertIsInstance(dm, MaterialDescriptor)
self.assertEqual(dm.name, "default")
self.assertEqual(type(dm.uid), str)
# Check initial material-pair properties
mp = manager.pairs
self.assertEqual(len(mp), 1)
self.assertEqual(len(mp[0]), 1)
self.assertEqual(mp[0][0].restitution, DEFAULT_RESTITUTION)
self.assertEqual(mp[0][0].static_friction, DEFAULT_FRICTION)
self.assertEqual(mp[0][0].dynamic_friction, DEFAULT_FRICTION)
# Check restitution matrix of the default material
drm = manager.restitution_matrix()
self.assertEqual(drm.shape, (1,))
self.assertEqual(drm[0], DEFAULT_RESTITUTION)
# Check the static friction matrix of the default material
dsfm = manager.static_friction_matrix()
self.assertEqual(dsfm.shape, (1,))
self.assertEqual(dsfm[0], DEFAULT_FRICTION)
# Check the dynamic friction matrix of the default material
ddfm = manager.dynamic_friction_matrix()
self.assertEqual(ddfm.shape, (1,))
self.assertEqual(ddfm[0], DEFAULT_FRICTION)
# Modify the default material properties
manager.configure_pair(
first="default",
second="default",
material_pair=MaterialPairProperties(restitution=0.5, static_friction=0.5, dynamic_friction=0.5),
)
# Check modified material-pair properties
mp = manager.pairs
self.assertEqual(len(mp), 1)
self.assertEqual(len(mp[0]), 1)
self.assertEqual(mp[0][0].restitution, 0.5)
self.assertEqual(mp[0][0].static_friction, 0.5)
self.assertEqual(mp[0][0].dynamic_friction, 0.5)
# Check restitution matrix of the default material
drm = manager.restitution_matrix()
self.assertEqual(drm.shape, (1,))
self.assertEqual(drm[0], 0.5)
# Check friction matrix of the default material
dsfm = manager.static_friction_matrix()
self.assertEqual(dsfm.shape, (1,))
self.assertEqual(dsfm[0], 0.5)
# Check dynamic friction matrix of the default material
ddfm = manager.dynamic_friction_matrix()
self.assertEqual(ddfm.shape, (1,))
self.assertEqual(ddfm[0], 0.5)
def test_02_material_manager_register_material(self):
# Create a default-constructed material manager
manager = MaterialManager()
# Define a new material
steel = MaterialDescriptor("steel")
# Add a new material
mid = manager.register(steel)
self.assertEqual(mid, 1)
self.assertEqual(manager.num_materials, 2)
self.assertEqual(manager.index("steel"), mid)
self.assertIsInstance(manager["steel"], MaterialDescriptor)
self.assertIsInstance(manager[mid], MaterialDescriptor)
self.assertEqual(manager[mid].name, "steel")
self.assertEqual(manager[mid].uid, steel.uid)
# Check the material-pair properties
mp = manager.pairs
self.assertEqual(len(mp), 2)
self.assertEqual(len(mp[1]), 2)
self.assertEqual(mp[1][0], None)
self.assertEqual(mp[0][1], None)
self.assertEqual(mp[1][1], None)
# Define material pair properties for the new material
steel_on_steel = MaterialPairProperties(restitution=0.2, static_friction=0.1, dynamic_friction=0.1)
default_on_steel = MaterialPairProperties(restitution=1.0, static_friction=1.0, dynamic_friction=1.0)
# Register properties for the new material
manager.register_pair(steel, steel, steel_on_steel)
manager.register_pair(manager.default, steel, default_on_steel)
# Check the material-pair properties
mp = manager.pairs
self.assertEqual(len(mp), 2)
self.assertEqual(len(mp[1]), 2)
self.assertEqual(mp[1][0].restitution, 1.0)
self.assertEqual(mp[1][0].static_friction, 1.0)
self.assertEqual(mp[1][0].dynamic_friction, 1.0)
self.assertEqual(mp[1][1].restitution, 0.2)
self.assertEqual(mp[1][1].static_friction, 0.1)
self.assertEqual(mp[1][1].dynamic_friction, 0.1)
# Check the restitution matrix
rm = manager.restitution_matrix()
self.assertEqual(rm.shape, (manager.num_material_pairs,))
self.assertEqual(rm[tril_index(0, 0)], DEFAULT_RESTITUTION)
self.assertEqual(rm[tril_index(0, 1)], 1.0)
self.assertEqual(rm[tril_index(1, 0)], 1.0)
self.assertEqual(rm[tril_index(1, 1)], 0.2)
# Check the friction matrix
sfm = manager.static_friction_matrix()
self.assertEqual(sfm.shape, (manager.num_material_pairs,))
self.assertEqual(sfm[tril_index(0, 0)], DEFAULT_FRICTION)
self.assertEqual(sfm[tril_index(0, 1)], 1.0)
self.assertEqual(sfm[tril_index(1, 0)], 1.0)
self.assertEqual(sfm[tril_index(1, 1)], 0.1)
# Check the dynamic friction matrix
dfm = manager.dynamic_friction_matrix()
self.assertEqual(dfm.shape, (manager.num_material_pairs,))
self.assertEqual(dfm[tril_index(0, 0)], DEFAULT_FRICTION)
self.assertEqual(dfm[tril_index(0, 1)], 1.0)
self.assertEqual(dfm[tril_index(1, 0)], 1.0)
self.assertEqual(dfm[tril_index(1, 1)], 0.1)
# Configure the material pair
manager.configure_pair(
first="default",
second="steel",
material_pair=MaterialPairProperties(restitution=0.5, static_friction=0.5, dynamic_friction=0.5),
)
# Check the material-pair properties
mp = manager.pairs
self.assertEqual(mp[1][0].restitution, 0.5)
self.assertEqual(mp[1][0].static_friction, 0.5)
self.assertEqual(mp[1][0].dynamic_friction, 0.5)
self.assertEqual(mp[1][1].restitution, 0.2)
self.assertEqual(mp[1][1].static_friction, 0.1)
self.assertEqual(mp[1][1].dynamic_friction, 0.1)
# Check the updated restitution matrix
rm = manager.restitution_matrix()
self.assertEqual(rm.shape, (manager.num_material_pairs,))
self.assertEqual(rm[tril_index(0, 0)], DEFAULT_RESTITUTION)
self.assertEqual(rm[tril_index(0, 1)], 0.5)
self.assertEqual(rm[tril_index(1, 0)], 0.5)
self.assertEqual(rm[tril_index(1, 1)], 0.2)
# Check the updated friction matrix
sfm = manager.static_friction_matrix()
self.assertEqual(sfm.shape, (manager.num_material_pairs,))
self.assertEqual(sfm[tril_index(0, 0)], DEFAULT_FRICTION)
self.assertEqual(sfm[tril_index(0, 1)], 0.5)
self.assertEqual(sfm[tril_index(1, 0)], 0.5)
self.assertEqual(sfm[tril_index(1, 1)], 0.1)
# Check the updated dynamic friction matrix
dfm = manager.dynamic_friction_matrix()
self.assertEqual(dfm.shape, (manager.num_material_pairs,))
self.assertEqual(dfm[tril_index(0, 0)], DEFAULT_FRICTION)
self.assertEqual(dfm[tril_index(0, 1)], 0.5)
self.assertEqual(dfm[tril_index(1, 0)], 0.5)
self.assertEqual(dfm[tril_index(1, 1)], 0.1)
def test_03_material_manager_register_pair(self):
# Create a default-constructed material manager
manager = MaterialManager()
# Define two new materials
steel = MaterialDescriptor("steel")
rubber = MaterialDescriptor("rubber")
# Register the new materials
manager.register(steel)
manager.register(rubber)
self.assertEqual(manager.num_materials, 3)
self.assertEqual(manager.index("steel"), 1)
self.assertEqual(manager.index("rubber"), 2)
# Define material pair properties
steel_on_steel = MaterialPairProperties(restitution=0.2, static_friction=0.1, dynamic_friction=0.1)
rubber_on_rubber = MaterialPairProperties(restitution=0.4, static_friction=0.3, dynamic_friction=0.3)
rubber_on_steel = MaterialPairProperties(restitution=0.6, static_friction=0.5, dynamic_friction=0.5)
default_on_steel = MaterialPairProperties(restitution=0.8, static_friction=0.7, dynamic_friction=0.7)
default_on_rubber = MaterialPairProperties(restitution=1.0, static_friction=0.9, dynamic_friction=0.9)
# Register the material pair
manager.register_pair(steel, steel, steel_on_steel)
manager.register_pair(rubber, rubber, rubber_on_rubber)
manager.register_pair(rubber, steel, rubber_on_steel)
manager.register_pair(manager.default, steel, default_on_steel)
manager.register_pair(manager.default, rubber, default_on_rubber)
# Check the material-pair properties
mp = manager.pairs
self.assertEqual(len(mp), 3)
self.assertEqual(len(mp[1]), 3)
self.assertEqual(len(mp[2]), 3)
self.assertEqual(mp[1][0].restitution, 0.8)
self.assertEqual(mp[1][0].static_friction, 0.7)
self.assertEqual(mp[1][0].dynamic_friction, 0.7)
self.assertEqual(mp[1][1].restitution, 0.2)
self.assertEqual(mp[1][1].static_friction, 0.1)
self.assertEqual(mp[1][1].dynamic_friction, 0.1)
self.assertEqual(mp[1][2].restitution, 0.6)
self.assertEqual(mp[1][2].static_friction, 0.5)
self.assertEqual(mp[1][2].dynamic_friction, 0.5)
self.assertEqual(mp[2][0].restitution, 1.0)
self.assertEqual(mp[2][0].static_friction, 0.9)
self.assertEqual(mp[2][0].dynamic_friction, 0.9)
self.assertEqual(mp[2][1].restitution, 0.6)
self.assertEqual(mp[2][1].static_friction, 0.5)
self.assertEqual(mp[2][1].dynamic_friction, 0.5)
self.assertEqual(mp[2][2].restitution, 0.4)
self.assertEqual(mp[2][2].static_friction, 0.3)
self.assertEqual(mp[2][2].dynamic_friction, 0.3)
# Check the restitution matrix
rm = manager.restitution_matrix()
self.assertEqual(rm.shape, (manager.num_material_pairs,))
self.assertEqual(rm[tril_index(0, 0)], DEFAULT_RESTITUTION)
self.assertEqual(rm[tril_index(0, 1)], 0.8)
self.assertEqual(rm[tril_index(0, 2)], 1.0)
self.assertEqual(rm[tril_index(1, 0)], 0.8)
self.assertEqual(rm[tril_index(1, 1)], 0.2)
self.assertEqual(rm[tril_index(1, 2)], 0.6)
self.assertEqual(rm[tril_index(2, 0)], 1.0)
self.assertEqual(rm[tril_index(2, 1)], 0.6)
self.assertEqual(rm[tril_index(2, 2)], 0.4)
# Check the static friction matrix
fm = manager.static_friction_matrix()
self.assertEqual(fm.shape, (manager.num_material_pairs,))
self.assertEqual(fm[tril_index(0, 0)], DEFAULT_FRICTION)
self.assertEqual(fm[tril_index(0, 1)], 0.7)
self.assertEqual(fm[tril_index(0, 2)], 0.9)
self.assertEqual(fm[tril_index(1, 0)], 0.7)
self.assertEqual(fm[tril_index(1, 1)], 0.1)
self.assertEqual(fm[tril_index(1, 2)], 0.5)
self.assertEqual(fm[tril_index(2, 0)], 0.9)
self.assertEqual(fm[tril_index(2, 1)], 0.5)
self.assertEqual(fm[tril_index(2, 2)], 0.3)
# Check the dynamic friction matrix
dym = manager.dynamic_friction_matrix()
self.assertEqual(dym.shape, (manager.num_material_pairs,))
self.assertEqual(dym[tril_index(0, 0)], DEFAULT_FRICTION)
self.assertEqual(dym[tril_index(0, 1)], 0.7)
self.assertEqual(dym[tril_index(0, 2)], 0.9)
self.assertEqual(dym[tril_index(1, 0)], 0.7)
self.assertEqual(dym[tril_index(1, 1)], 0.1)
self.assertEqual(dym[tril_index(1, 2)], 0.5)
self.assertEqual(dym[tril_index(2, 0)], 0.9)
self.assertEqual(dym[tril_index(2, 1)], 0.5)
self.assertEqual(dym[tril_index(2, 2)], 0.3)
# Optional verbose output
if self.verbose:
print(f"\nRestitution Matrix:\n{rm}")
print(f"\nStatic Friction Matrix:\n{fm}")
print(f"\nDynamic Friction Matrix:\n{dym}")
def test_04_material_manager_merge(self):
# Create a two material managers
manager0 = MaterialManager()
manager1 = MaterialManager()
# Define two new materials
steel = MaterialDescriptor("steel")
rubber = MaterialDescriptor("rubber")
wood = MaterialDescriptor("wood")
plastic = MaterialDescriptor("plastic")
glass = MaterialDescriptor("glass")
# Register the first set of materials with manager0
manager0.register(steel)
manager0.register(rubber)
self.assertEqual(manager0.num_materials, 3)
self.assertEqual(manager0.index("steel"), 1)
self.assertEqual(manager0.index("rubber"), 2)
# Register the second set of materials with manager1
manager1.register(wood)
manager1.register(plastic)
manager1.register(glass)
self.assertEqual(manager1.num_materials, 4)
self.assertEqual(manager1.index("wood"), 1)
self.assertEqual(manager1.index("plastic"), 2)
self.assertEqual(manager1.index("glass"), 3)
# Define material pair properties
steel_on_steel = MaterialPairProperties(restitution=0.2, static_friction=0.1, dynamic_friction=0.1)
steel_on_rubber = MaterialPairProperties(restitution=0.6, static_friction=0.5, dynamic_friction=0.5)
steel_on_wood = MaterialPairProperties(restitution=0.4, static_friction=0.8, dynamic_friction=0.3)
steel_on_plastic = MaterialPairProperties(restitution=0.8, static_friction=0.4, dynamic_friction=0.4)
steel_on_glass = MaterialPairProperties(restitution=0.7, static_friction=0.6, dynamic_friction=0.6)
rubber_on_rubber = MaterialPairProperties(restitution=0.0, static_friction=0.9, dynamic_friction=0.9)
rubber_on_wood = MaterialPairProperties(restitution=0.3, static_friction=0.9, dynamic_friction=0.8)
rubber_on_plastic = MaterialPairProperties(restitution=0.1, static_friction=0.8, dynamic_friction=0.7)
rubber_on_glass = MaterialPairProperties(restitution=0.6, static_friction=0.4, dynamic_friction=0.5)
wood_on_wood = MaterialPairProperties(restitution=0.3, static_friction=0.2, dynamic_friction=0.2)
wood_on_plastic = MaterialPairProperties(restitution=0.4, static_friction=0.3, dynamic_friction=0.3)
wood_on_glass = MaterialPairProperties(restitution=0.2, static_friction=0.5, dynamic_friction=0.4)
plastic_on_plastic = MaterialPairProperties(restitution=0.4, static_friction=0.3, dynamic_friction=0.1)
plastic_on_glass = MaterialPairProperties(restitution=0.5, static_friction=0.4, dynamic_friction=0.4)
glass_on_glass = MaterialPairProperties(restitution=0.6, static_friction=0.4, dynamic_friction=0.3)
default_on_steel = MaterialPairProperties(restitution=0.8, static_friction=0.7, dynamic_friction=0.7)
default_on_rubber = MaterialPairProperties(restitution=1.0, static_friction=0.9, dynamic_friction=0.9)
default_on_wood = MaterialPairProperties(restitution=0.7, static_friction=0.8, dynamic_friction=0.8)
default_on_plastic = MaterialPairProperties(restitution=0.7, static_friction=0.5, dynamic_friction=0.5)
default_on_glass = MaterialPairProperties(restitution=0.9, static_friction=0.8, dynamic_friction=0.8)
# Register the material pairs of the first set with manager0
manager0.register_pair(steel, steel, steel_on_steel)
manager0.register_pair(rubber, steel, steel_on_rubber)
manager0.register_pair(rubber, rubber, rubber_on_rubber)
manager0.register_pair(manager0.default, steel, default_on_steel)
manager0.register_pair(manager0.default, rubber, default_on_rubber)
# Register the material pairs of the second set with manager1
manager1.register_pair(wood, wood, wood_on_wood)
manager1.register_pair(wood, plastic, wood_on_plastic)
manager1.register_pair(wood, glass, wood_on_glass)
manager1.register_pair(plastic, plastic, plastic_on_plastic)
manager1.register_pair(plastic, glass, plastic_on_glass)
manager1.register_pair(glass, glass, glass_on_glass)
manager1.register_pair(manager1.default, wood, default_on_wood)
manager1.register_pair(manager1.default, plastic, default_on_plastic)
manager1.register_pair(manager1.default, glass, default_on_glass)
# Check the material-pair properties of the first manager
mp0 = manager0.pairs
self.assertEqual(len(mp0), 3)
self.assertEqual(len(mp0[1]), 3)
self.assertEqual(len(mp0[2]), 3)
self.assertEqual(mp0[1][0], default_on_steel)
self.assertEqual(mp0[1][1], steel_on_steel)
self.assertEqual(mp0[1][2], steel_on_rubber)
self.assertEqual(mp0[2][0], default_on_rubber)
self.assertEqual(mp0[2][1], steel_on_rubber)
self.assertEqual(mp0[2][2], rubber_on_rubber)
# Check the material-pair properties of the second manager
mp1 = manager1.pairs
self.assertEqual(len(mp1), 4)
self.assertEqual(len(mp1[1]), 4)
self.assertEqual(len(mp1[2]), 4)
self.assertEqual(len(mp1[3]), 4)
self.assertEqual(mp1[1][0], default_on_wood)
self.assertEqual(mp1[1][1], wood_on_wood)
self.assertEqual(mp1[1][2], wood_on_plastic)
self.assertEqual(mp1[1][3], wood_on_glass)
self.assertEqual(mp1[2][0], default_on_plastic)
self.assertEqual(mp1[2][1], wood_on_plastic)
self.assertEqual(mp1[2][2], plastic_on_plastic)
self.assertEqual(mp1[2][3], plastic_on_glass)
self.assertEqual(mp1[3][0], default_on_glass)
self.assertEqual(mp1[3][1], wood_on_glass)
self.assertEqual(mp1[3][2], plastic_on_glass)
self.assertEqual(mp1[3][3], glass_on_glass)
# Merge manager1 into manager0
manager0.merge(manager1)
self.assertEqual(manager0.num_materials, 6)
self.assertEqual(manager0.index("default"), 0)
self.assertEqual(manager0.index("steel"), 1)
self.assertEqual(manager0.index("rubber"), 2)
self.assertEqual(manager0.index("wood"), 3)
self.assertEqual(manager0.index("plastic"), 4)
self.assertEqual(manager0.index("glass"), 5)
# Check the material-pair properties of the merged manager before registering missing pairs
mpm = manager0.pairs
self.assertEqual(len(mpm), 6)
self.assertEqual(len(mpm[0]), 6)
self.assertEqual(len(mpm[1]), 6)
self.assertEqual(len(mpm[2]), 6)
self.assertEqual(len(mpm[3]), 6)
self.assertEqual(len(mpm[4]), 6)
self.assertEqual(len(mpm[5]), 6)
self.assertEqual(mpm[1][0], default_on_steel)
self.assertEqual(mpm[2][0], default_on_rubber)
self.assertEqual(mpm[3][0], default_on_wood)
self.assertEqual(mpm[4][0], default_on_plastic)
self.assertEqual(mpm[5][0], default_on_glass)
self.assertEqual(mpm[1][1], steel_on_steel)
self.assertEqual(mpm[1][2], steel_on_rubber)
self.assertEqual(mpm[1][3], None)
self.assertEqual(mpm[1][4], None)
self.assertEqual(mpm[1][5], None)
self.assertEqual(mpm[2][1], steel_on_rubber)
self.assertEqual(mpm[2][2], rubber_on_rubber)
self.assertEqual(mpm[2][3], None)
self.assertEqual(mpm[2][4], None)
self.assertEqual(mpm[2][5], None)
self.assertEqual(mpm[3][1], None)
self.assertEqual(mpm[3][2], None)
self.assertEqual(mpm[3][3], wood_on_wood)
self.assertEqual(mpm[3][4], wood_on_plastic)
self.assertEqual(mpm[3][5], wood_on_glass)
self.assertEqual(mpm[4][1], None)
self.assertEqual(mpm[4][2], None)
self.assertEqual(mpm[4][3], wood_on_plastic)
self.assertEqual(mpm[4][4], plastic_on_plastic)
self.assertEqual(mpm[4][5], plastic_on_glass)
self.assertEqual(mpm[5][1], None)
self.assertEqual(mpm[5][2], None)
self.assertEqual(mpm[5][3], wood_on_glass)
self.assertEqual(mpm[5][4], plastic_on_glass)
self.assertEqual(mpm[5][5], glass_on_glass)
# Register missing material pairs between the two original sets
manager0.register_pair(steel, wood, steel_on_wood)
manager0.register_pair(steel, plastic, steel_on_plastic)
manager0.register_pair(steel, glass, steel_on_glass)
manager0.register_pair(rubber, wood, rubber_on_wood)
manager0.register_pair(rubber, plastic, rubber_on_plastic)
manager0.register_pair(rubber, glass, rubber_on_glass)
# Check the material-pair properties of the merged manager after registering missing pairs
mpm = manager0.pairs
self.assertEqual(mpm[0][0], MaterialPairProperties())
self.assertEqual(mpm[0][1], default_on_steel)
self.assertEqual(mpm[0][2], default_on_rubber)
self.assertEqual(mpm[0][3], default_on_wood)
self.assertEqual(mpm[0][4], default_on_plastic)
self.assertEqual(mpm[0][5], default_on_glass)
self.assertEqual(mpm[1][0], default_on_steel)
self.assertEqual(mpm[1][1], steel_on_steel)
self.assertEqual(mpm[1][2], steel_on_rubber)
self.assertEqual(mpm[1][3], steel_on_wood)
self.assertEqual(mpm[1][4], steel_on_plastic)
self.assertEqual(mpm[1][5], steel_on_glass)
self.assertEqual(mpm[2][0], default_on_rubber)
self.assertEqual(mpm[2][1], steel_on_rubber)
self.assertEqual(mpm[2][2], rubber_on_rubber)
self.assertEqual(mpm[2][3], rubber_on_wood)
self.assertEqual(mpm[2][4], rubber_on_plastic)
self.assertEqual(mpm[2][5], rubber_on_glass)
self.assertEqual(mpm[3][0], default_on_wood)
self.assertEqual(mpm[3][1], steel_on_wood)
self.assertEqual(mpm[3][2], rubber_on_wood)
self.assertEqual(mpm[3][3], wood_on_wood)
self.assertEqual(mpm[3][4], wood_on_plastic)
self.assertEqual(mpm[3][5], wood_on_glass)
self.assertEqual(mpm[4][0], default_on_plastic)
self.assertEqual(mpm[4][1], steel_on_plastic)
self.assertEqual(mpm[4][2], rubber_on_plastic)
self.assertEqual(mpm[4][3], wood_on_plastic)
self.assertEqual(mpm[4][4], plastic_on_plastic)
self.assertEqual(mpm[4][5], plastic_on_glass)
self.assertEqual(mpm[5][0], default_on_glass)
self.assertEqual(mpm[5][1], steel_on_glass)
self.assertEqual(mpm[5][2], rubber_on_glass)
self.assertEqual(mpm[5][3], wood_on_glass)
self.assertEqual(mpm[5][4], plastic_on_glass)
self.assertEqual(mpm[5][5], glass_on_glass)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_core_model.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Unit tests for the :class:`ModelKamino` class and related functionality.
"""
import copy
import os
import unittest
import numpy as np
import warp as wp
import newton
import newton._src.solvers.kamino.tests.utils.checks as test_util_checks
from newton._src.sim import Control, Model, ModelBuilder, State
from newton._src.solvers.kamino._src.core.bodies import convert_body_com_to_origin, convert_body_origin_to_com
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.control import ControlKamino
from newton._src.solvers.kamino._src.core.model import MaterialDescriptor, ModelKamino
from newton._src.solvers.kamino._src.core.state import StateKamino
from newton._src.solvers.kamino._src.models import basics as basics_kamino
from newton._src.solvers.kamino._src.models import basics_newton, get_basics_usd_assets_path
from newton._src.solvers.kamino._src.models.builders import utils as model_utils
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino.solver_kamino import SolverKamino
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils import print as print_utils
###
# Tests
###
class TestModel(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_single_model(self):
# Create a model builder
builder = basics_kamino.build_boxes_hinged()
# Finalize the model
model: ModelKamino = builder.finalize(self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print_utils.print_model_info(model)
# Create a model state
state = model.data()
if self.verbose:
print("") # Add a newline for better readability
print_utils.print_data_info(state)
# Check the model info entries
self.assertEqual(model.size.sum_of_num_bodies, builder.num_bodies)
self.assertEqual(model.size.sum_of_num_joints, builder.num_joints)
self.assertEqual(model.size.sum_of_num_geoms, builder.num_geoms)
self.assertEqual(model.device, self.default_device)
def test_02_double_model(self):
# Create a model builder
builder1 = basics_kamino.build_boxes_hinged()
builder2 = basics_kamino.build_boxes_nunchaku()
# Compute the total number of elements from the two builders
total_nb = builder1.num_bodies + builder2.num_bodies
total_nj = builder1.num_joints + builder2.num_joints
total_ng = builder1.num_geoms + builder2.num_geoms
# Add the second builder to the first one
builder1.add_builder(builder2)
# Finalize the model
model: ModelKamino = builder1.finalize(self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print_utils.print_model_info(model)
# Create a model state
data = model.data()
if self.verbose:
print("") # Add a newline for better readability
print_utils.print_data_info(data)
# Check the model info entries
self.assertEqual(model.size.sum_of_num_bodies, total_nb)
self.assertEqual(model.size.sum_of_num_joints, total_nj)
self.assertEqual(model.size.sum_of_num_geoms, total_ng)
def test_03_homogeneous_model(self):
# Constants
num_worlds = 4
# Create a model builder
builder = model_utils.make_homogeneous_builder(num_worlds=num_worlds, build_fn=basics_kamino.build_boxes_hinged)
# Finalize the model
model: ModelKamino = builder.finalize(self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print_utils.print_model_info(model)
# Create a model state
state = model.data()
if self.verbose:
print("") # Add a newline for better readability
print_utils.print_data_info(state)
# Check the model info entries
self.assertEqual(model.size.sum_of_num_bodies, num_worlds * 2)
self.assertEqual(model.size.sum_of_num_joints, num_worlds * 1)
self.assertEqual(model.size.sum_of_num_geoms, num_worlds * 3)
self.assertEqual(model.device, self.default_device)
def test_04_hetereogeneous_model(self):
# Create a model builder
builder = basics_kamino.make_basics_heterogeneous_builder()
num_worlds = builder.num_worlds
# Finalize the model
model: ModelKamino = builder.finalize(self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print_utils.print_model_info(model)
print("") # Add a newline for better readability
print_utils.print_model_bodies(model)
print("") # Add a newline for better readability
print_utils.print_model_joints(model)
# Create a model state
state = model.data()
if self.verbose:
print("") # Add a newline for better readability
print_utils.print_data_info(state)
# Check the model info entries
self.assertEqual(model.info.num_worlds, num_worlds)
class TestModelConversions(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True to enable verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO) # TODO @nvtw: set this to DEBUG when investigating noted issues
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_model_conversions_fourbar_from_builder(self):
"""
Test the conversion operations between newton.Model and kamino.ModelKamino
on a simple fourbar model created explicitly using the builder.
"""
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0: ModelBuilder = basics_newton.build_boxes_fourbar(
builder=builder_0,
z_offset=0.0,
fixedbase=False,
floatingbase=True,
limits=True,
ground=True,
dynamic_joints=False,
implicit_pd=False,
new_world=True,
actuator_ids=[1, 3],
)
# Overwriting mu = 0.7 to match Kamino's default material properties
builder_0.shape_material_mu = [0.7] * len(builder_0.shape_material_mu)
# Duplicate the world to test multi-world handling
builder_0.begin_world()
builder_0.add_builder(copy.deepcopy(builder_0))
builder_0.end_world()
# Create a fourbar using Kamino's ModelBuilderKamino
builder_1: ModelBuilderKamino = basics_kamino.build_boxes_fourbar(
builder=None,
z_offset=0.0,
fixedbase=False,
floatingbase=True,
limits=True,
ground=True,
dynamic_joints=False,
implicit_pd=False,
new_world=True,
actuator_ids=[1, 3],
)
# Duplicate the world to test multi-world handling
builder_1.add_builder(copy.deepcopy(builder_1))
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize(skip_validation_joints=True)
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
test_util_checks.assert_model_equal(self, model_2, model_1)
def test_01_model_conversions_fourbar_from_usd(self):
"""
Test the conversion operations between newton.Model and kamino.ModelKamino
on a simple fourbar model loaded from USD.
"""
# Define the path to the USD file for the fourbar model
asset_file = os.path.join(get_basics_usd_assets_path(), "boxes_fourbar.usda")
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0.begin_world()
builder_0.add_usd(
source=asset_file,
joint_ordering=None,
force_show_colliders=True,
force_position_velocity_actuation=True,
)
builder_0.end_world()
# Overwriting mu = 0.7 to match Kamino's default material properties
builder_0.shape_material_mu = [0.7] * len(builder_0.shape_material_mu)
# Import the same fourbar using Kamino's USDImporter and ModelBuilderKamino
importer = USDImporter()
builder_1: ModelBuilderKamino = importer.import_from(
source=asset_file,
load_drive_dynamics=True,
load_static_geometry=True,
force_show_colliders=True,
use_prim_path_names=True,
use_angular_drive_scaling=True,
)
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize(skip_validation_joints=True)
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
test_util_checks.assert_model_equal(self, model_2, model_1)
# TODO: IMPLEMENT THIS CHECK: We wanna see if the both generate
# the same data containers and unilateral constraint info
# data_1: DataKamino = model_1.data()
# data_2: DataKamino = model_2.data()
# make_unilateral_constraints_info(model=model_1, data=data_1)
# make_unilateral_constraints_info(model=model_2, data=data_2)
# test_util_checks.assert_model_equal(self, model_2, model_1)
# test_util_checks.assert_data_equal(self, data_2, data_1)
def test_02_model_conversions_dr_testmech_from_usd(self):
"""
Test the conversion operations between newton.Model and kamino.ModelKamino
on the DR testmechanism model loaded from USD.
"""
# Define the path to the USD file for the DR testmechanism model
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_testmech" / "usd" / "dr_testmech.usda")
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0.begin_world()
builder_0.add_usd(
source=asset_file,
joint_ordering=None,
force_show_colliders=True,
)
builder_0.end_world()
# Overwriting mu = 0.7 to match Kamino's default material properties
builder_0.shape_material_mu = [0.7] * len(builder_0.shape_material_mu)
# Import the same fourbar using Kamino's USDImporter and ModelBuilderKamino
importer = USDImporter()
builder_1: ModelBuilderKamino = importer.import_from(
source=asset_file,
load_static_geometry=True,
retain_joint_ordering=False,
meshes_are_collidable=True,
force_show_colliders=True,
use_prim_path_names=True,
)
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize(skip_validation_joints=True)
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
# NOTE: We don't check:
# - mesh geometry pointers since they have been loaded separately
# Inversion of the inertia matrix amplifies small floating-point differences,
# so inv_i_I_i needs a somewhat higher tolerance.
rtol = {"inv_i_I_i": 1e-5}
atol = {"inv_i_I_i": 1e-6}
test_util_checks.assert_model_equal(self, model_2, model_1, excluded=["ptr"], rtol=rtol, atol=atol)
def test_03_model_conversions_dr_legs_from_usd(self):
"""
Test the conversion operations between newton.Model and kamino.ModelKamino
on the DR legs model loaded from USD.
"""
# Define the path to the USD file for the DR legs model
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_meshes_and_boxes.usda")
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0.begin_world()
builder_0.add_usd(
source=asset_file,
joint_ordering=None,
force_show_colliders=True,
force_position_velocity_actuation=True,
)
builder_0.end_world()
# Overwriting mu = 0.7 to match Kamino's default material properties
builder_0.shape_material_mu = [0.7] * len(builder_0.shape_material_mu)
# Import the same fourbar using Kamino's USDImporter and ModelBuilderKamino
importer = USDImporter()
builder_1: ModelBuilderKamino = importer.import_from(
source=asset_file,
load_drive_dynamics=True,
force_show_colliders=True,
use_prim_path_names=True,
use_angular_drive_scaling=True,
)
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize(skip_validation_joints=True)
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
# NOTE: We don't check:
# - mesh geometry pointers since they have been loaded separately
# - the shape contact group (TODO @nvtw: investigate why) because newton.ModelBuilder
# sets it to `1` even for non-collidable visual shapes
# - shape gap since newton.ModelBuilder sets it to `0.001` for all shapes even if
# the default shape config has gap=0.0
# - excluded/filtered collision pairs since newton.ModelBuilder preemptively adds
# geom-pairs of joint neighbours to `shape_collision_filter_pairs` regardless of
# whether they are actually collidable or not, which leads to differences in the
# number of excluded pairs and their contents
excluded = ["ptr", "group", "gap", "num_excluded_pairs", "excluded_pairs"]
rtol = {"inv_i_I_i": 1e-5}
atol = {"inv_i_I_i": 1e-6}
test_util_checks.assert_model_equal(self, model_2, model_1, excluded=excluded, rtol=rtol, atol=atol)
def test_04_model_conversions_anymal_d_from_usd(self):
"""
Test the conversion operations between newton.Model and kamino.ModelKamino
on the Anymal D model loaded from USD.
"""
# Define the path to the USD file for the Anymal D model
asset_path = newton.utils.download_asset("anybotics_anymal_d")
asset_file = str(asset_path / "usd" / "anymal_d.usda")
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0.begin_world()
builder_0.add_usd(
source=asset_file,
collapse_fixed_joints=False,
enable_self_collisions=False,
force_show_colliders=True,
)
builder_0.end_world()
# Overwriting mu = 0.7 to match Kamino's default material properties
builder_0.shape_material_mu = [0.7] * len(builder_0.shape_material_mu)
# Import the same fourbar using Kamino's USDImporter and ModelBuilderKamino
importer = USDImporter()
builder_1: ModelBuilderKamino = importer.import_from(
source=asset_file,
load_static_geometry=True,
retain_geom_ordering=False,
use_articulation_root_name=False,
force_show_colliders=True,
use_prim_path_names=True,
use_angular_drive_scaling=True,
)
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize()
# TODO @nvtw: Why are shape_collision_group[i] values for
# visual shapes set to `=1` since they are not collidable?
msg.error(f"model_0.shape_collision_group:\n{model_0.shape_collision_group}\n")
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
# NOTE: We don't check mesh geometry pointers since they have been loaded separately
excluded = [
"i_r_com_i", # TODO: Investigate if the difference is expected or not
"i_I_i", # TODO: Investigate if the difference is expected or not
"inv_i_I_i", # TODO: Investigate if the difference is expected or not
"q_i_0", # TODO: Investigate if the difference is expected or not
"B_r_Bj", # TODO: Investigate if the difference is expected or not
"F_r_Fj", # TODO: Investigate if the difference is expected or not
"X_j", # TODO: Investigate if the difference is expected or not
"q_j_0", # TODO: Investigate if the difference is expected or not
"num_collidable_pairs", # TODO: newton.ModelBuilder preemptively adding geom-pairs to shape_collision_filter_pairs
"num_excluded_pairs", # TODO: newton.ModelBuilder preemptively adding geom-pairs to shape_collision_filter_pairs
"model_minimum_contacts", # TODO: Investigate
"world_minimum_contacts", # TODO: Investigate
"offset", # TODO: Investigate if the difference is expected or not
"group", # TODO: newton.ModelBuilder setting shape_collision_group=1 for all shapes even non-collidable ones
"gap", # TODO: newton.ModelBuilder setting shape gap to 0.001 for all shapes even if default shape config has gap=0.0
"ptr", # Exclude geometry pointers since they have been loaded separately
"collidable_pairs", # TODO @nvtw: not sure why these are different
"excluded_pairs", # TODO: newton.ModelBuilder preemptively adding geom-pairs to shape_collision_filter_pairs
]
test_util_checks.assert_model_equal(self, model_2, model_1, excluded=excluded)
def test_05_model_conversions_arbitrary_axis(self):
"""
Test that Newton→Kamino conversion succeeds for a revolute joint
with an arbitrary (non-canonical) axis, e.g. ``(1, 1, 0)``.
"""
builder: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder)
builder.default_shape_cfg.margin = 0.0
builder.default_shape_cfg.gap = 0.0
builder.begin_world()
# Parent body at origin
bid0 = builder.add_link(
label="base",
mass=1.0,
xform=wp.transformf(wp.vec3f(0.0, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
lock_inertia=True,
)
builder.add_shape_box(label="box_base", body=bid0, hx=0.05, hy=0.05, hz=0.05)
# Child body offset along z
bid1 = builder.add_link(
label="pendulum",
mass=1.0,
xform=wp.transformf(wp.vec3f(0.0, 0.0, 0.5), wp.quat_identity(dtype=wp.float32)),
lock_inertia=True,
)
builder.add_shape_box(label="box_pend", body=bid1, hx=0.05, hy=0.05, hz=0.25)
# Fix the base to the world
builder.add_joint_fixed(
label="world_to_base",
parent=-1,
child=bid0,
parent_xform=wp.transform_identity(dtype=wp.float32),
child_xform=wp.transform_identity(dtype=wp.float32),
)
# Diagonal revolute axis (non-canonical)
axis_vec = wp.vec3(1.0, 1.0, 0.0)
builder.add_joint_revolute(
label="base_to_pendulum",
parent=bid0,
child=bid1,
axis=axis_vec,
parent_xform=wp.transformf(wp.vec3f(0.0, 0.0, 0.25), wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(wp.vec3f(0.0, 0.0, -0.25), wp.quat_identity(dtype=wp.float32)),
)
builder.end_world()
model: Model = builder.finalize(skip_validation_joints=True)
# Conversion must succeed (previously raised ValueError)
kamino_model: ModelKamino = ModelKamino.from_newton(model)
# Verify X_j first column is aligned with the expected axis direction
X_j = kamino_model.joints.X_j.numpy()
# X_j has shape (num_joints, 3, 3); the revolute joint is the second one (index 1)
R = X_j[1] # 3x3 rotation matrix
ax_col = R[:, 0] # first column = joint axis direction
expected_ax = np.array([1.0, 1.0, 0.0])
expected_ax = expected_ax / np.linalg.norm(expected_ax)
np.testing.assert_allclose(ax_col, expected_ax, atol=1e-6)
def test_06_model_conversions_q_i_0_com_frame(self):
"""
Test that ``q_i_0`` stores COM world poses (not body-origin poses)
after Newton→Kamino conversion for bodies with non-zero COM offsets.
"""
builder: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder)
builder.default_shape_cfg.margin = 0.0
builder.default_shape_cfg.gap = 0.0
builder.begin_world()
# Body 0: at origin, identity rotation, COM offset along x
bid0 = builder.add_link(
label="body0",
mass=1.0,
xform=wp.transformf(wp.vec3f(0.0, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
com=wp.vec3f(0.1, 0.0, 0.0),
lock_inertia=True,
)
builder.add_shape_box(label="box0", body=bid0, hx=0.05, hy=0.05, hz=0.05)
# Body 1: at (0,0,1), rotated 90° about z-axis, single-axis COM offset
rot_90z = wp.quat_from_axis_angle(wp.vec3f(0.0, 0.0, 1.0), np.pi / 2.0)
bid1 = builder.add_link(
label="body1",
mass=1.0,
xform=wp.transformf(wp.vec3f(0.0, 0.0, 1.0), rot_90z),
com=wp.vec3f(0.1, 0.0, 0.0),
lock_inertia=True,
)
builder.add_shape_box(label="box1", body=bid1, hx=0.05, hy=0.05, hz=0.05)
# Body 2: at (1,0,0), rotated 90° about x-axis, 3D COM offset
rot_90x = wp.quat_from_axis_angle(wp.vec3f(1.0, 0.0, 0.0), np.pi / 2.0)
bid2 = builder.add_link(
label="body2",
mass=1.0,
xform=wp.transformf(wp.vec3f(1.0, 0.0, 0.0), rot_90x),
com=wp.vec3f(0.1, 0.2, 0.3),
lock_inertia=True,
)
builder.add_shape_box(label="box2", body=bid2, hx=0.05, hy=0.05, hz=0.05)
# Fix body 0 to world
builder.add_joint_fixed(
label="world_to_body0",
parent=-1,
child=bid0,
parent_xform=wp.transform_identity(dtype=wp.float32),
child_xform=wp.transform_identity(dtype=wp.float32),
)
# Revolute joint: body 0 → body 1
builder.add_joint_revolute(
label="body0_to_body1",
parent=bid0,
child=bid1,
axis=wp.vec3(0.0, 1.0, 0.0),
parent_xform=wp.transformf(wp.vec3f(0.0, 0.0, 0.5), wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(wp.vec3f(0.0, 0.0, -0.5), wp.quat_identity(dtype=wp.float32)),
)
# Revolute joint: body 1 → body 2
builder.add_joint_revolute(
label="body1_to_body2",
parent=bid1,
child=bid2,
axis=wp.vec3(0.0, 1.0, 0.0),
parent_xform=wp.transformf(wp.vec3f(0.5, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(wp.vec3f(-0.5, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
)
builder.end_world()
model: Model = builder.finalize(skip_validation_joints=True)
kamino_model: ModelKamino = ModelKamino.from_newton(model)
q_i_0_np = kamino_model.bodies.q_i_0.numpy() # shape (N, 7)
body_q_np = model.body_q.numpy()
# Body 0: identity rotation, origin (0,0,0), COM (0.1,0,0) → world (0.1, 0, 0)
np.testing.assert_allclose(q_i_0_np[0, :3], [0.1, 0.0, 0.0], atol=1e-6)
np.testing.assert_allclose(q_i_0_np[0, 3:7], body_q_np[0, 3:7], atol=1e-6)
# Body 1: 90° z-rotation maps local (0.1,0,0) → world (0, 0.1, 0), plus origin (0,0,1)
np.testing.assert_allclose(q_i_0_np[1, :3], [0.0, 0.1, 1.0], atol=1e-6)
np.testing.assert_allclose(q_i_0_np[1, 3:7], body_q_np[1, 3:7], atol=1e-6)
# Body 2: 90° x-rotation maps local (0.1, 0.2, 0.3) → world (0.1, -0.3, 0.2),
# plus origin (1,0,0) → (1.1, -0.3, 0.2)
np.testing.assert_allclose(q_i_0_np[2, :3], [1.1, -0.3, 0.2], atol=1e-6)
np.testing.assert_allclose(q_i_0_np[2, 3:7], body_q_np[2, 3:7], atol=1e-6)
def _build_com_offset_model(self):
"""Build a 3-body chain with non-zero COM offsets for reset tests."""
builder: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder)
builder.default_shape_cfg.margin = 0.0
builder.default_shape_cfg.gap = 0.0
builder.begin_world()
# Body 0: at origin, identity rotation, COM offset along x
bid0 = builder.add_link(
label="body0",
mass=1.0,
xform=wp.transformf(wp.vec3f(0.0, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
com=wp.vec3f(0.1, 0.0, 0.0),
lock_inertia=True,
)
builder.add_shape_box(label="box0", body=bid0, hx=0.05, hy=0.05, hz=0.05)
# Body 1: at (0,0,1), rotated 90° about z-axis, single-axis COM offset
rot_90z = wp.quat_from_axis_angle(wp.vec3f(0.0, 0.0, 1.0), np.pi / 2.0)
bid1 = builder.add_link(
label="body1",
mass=1.0,
xform=wp.transformf(wp.vec3f(0.0, 0.0, 1.0), rot_90z),
com=wp.vec3f(0.1, 0.0, 0.0),
lock_inertia=True,
)
builder.add_shape_box(label="box1", body=bid1, hx=0.05, hy=0.05, hz=0.05)
# Body 2: at (1,0,0), rotated 90° about x-axis, 3D COM offset
rot_90x = wp.quat_from_axis_angle(wp.vec3f(1.0, 0.0, 0.0), np.pi / 2.0)
bid2 = builder.add_link(
label="body2",
mass=1.0,
xform=wp.transformf(wp.vec3f(1.0, 0.0, 0.0), rot_90x),
com=wp.vec3f(0.1, 0.2, 0.3),
lock_inertia=True,
)
builder.add_shape_box(label="box2", body=bid2, hx=0.05, hy=0.05, hz=0.05)
# Fix body 0 to world
builder.add_joint_fixed(
label="world_to_body0",
parent=-1,
child=bid0,
parent_xform=wp.transform_identity(dtype=wp.float32),
child_xform=wp.transform_identity(dtype=wp.float32),
)
# Revolute joint: body 0 -> body 1
builder.add_joint_revolute(
label="body0_to_body1",
parent=bid0,
child=bid1,
axis=wp.vec3(0.0, 1.0, 0.0),
parent_xform=wp.transformf(wp.vec3f(0.0, 0.0, 0.5), wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(wp.vec3f(0.0, 0.0, -0.5), wp.quat_identity(dtype=wp.float32)),
)
# Revolute joint: body 1 -> body 2
builder.add_joint_revolute(
label="body1_to_body2",
parent=bid1,
child=bid2,
axis=wp.vec3(0.0, 1.0, 0.0),
parent_xform=wp.transformf(wp.vec3f(0.5, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
child_xform=wp.transformf(wp.vec3f(-0.5, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
)
builder.end_world()
return builder.finalize(skip_validation_joints=True)
def test_07_reset_produces_body_origin_frame(self):
"""
Test that ``SolverKamino.reset()`` writes body-origin frame poses
into ``state.body_q``, not COM-frame poses, for bodies with non-zero
COM offsets.
"""
model = self._build_com_offset_model()
body_q_expected = model.body_q.numpy().copy()
solver = SolverKamino(model)
# Default reset (no args) should restore body-origin poses
state_out: State = model.state()
solver.reset(state_out=state_out)
body_q_after = state_out.body_q.numpy()
for i in range(model.body_count):
np.testing.assert_allclose(
body_q_after[i],
body_q_expected[i],
atol=1e-6,
err_msg=f"Default reset: body {i} pose is not in body-origin frame",
)
# Velocities should be zero after default reset
body_qd_after = state_out.body_qd.numpy()
np.testing.assert_allclose(
body_qd_after,
0.0,
atol=1e-6,
err_msg="Default reset: body velocities should be zero",
)
def test_08_base_reset_produces_body_origin_frame(self):
"""
Test that ``SolverKamino.reset(base_q=..., base_u=...)`` writes
body-origin frame poses and velocities into ``state.body_q`` and
``state.body_qd`` for bodies with non-zero COM offsets.
"""
model = self._build_com_offset_model()
body_q_expected = model.body_q.numpy().copy()
solver = SolverKamino(model)
# --- Base reset with identity base pose should restore body-origin poses ---
state_out: State = model.state()
base_q = wp.array(
[wp.transformf(wp.vec3f(0.0, 0.0, 0.0), wp.quat_identity(dtype=wp.float32))],
dtype=wp.transformf,
)
base_u = wp.zeros(1, dtype=wp.spatial_vectorf)
solver.reset(state_out=state_out, base_q=base_q, base_u=base_u)
body_q_after = state_out.body_q.numpy()
for i in range(model.body_count):
np.testing.assert_allclose(
body_q_after[i],
body_q_expected[i],
atol=1e-6,
err_msg=f"Base reset (identity): body {i} pose is not in body-origin frame",
)
# Velocities should be zero with zero base twist
body_qd_after = state_out.body_qd.numpy()
np.testing.assert_allclose(
body_qd_after,
0.0,
atol=1e-6,
err_msg="Base reset (identity): body velocities should be zero",
)
# --- Base reset with a translated base pose ---
offset = np.array([2.0, 3.0, 5.0])
base_q_shifted = wp.array(
[wp.transformf(wp.vec3f(*offset), wp.quat_identity(dtype=wp.float32))],
dtype=wp.transformf,
)
solver.reset(state_out=state_out, base_q=base_q_shifted, base_u=base_u)
body_q_shifted = state_out.body_q.numpy()
for i in range(model.body_count):
np.testing.assert_allclose(
body_q_shifted[i, :3],
body_q_expected[i, :3] + offset,
atol=1e-6,
err_msg=f"Base reset (translated): body {i} position mismatch",
)
np.testing.assert_allclose(
body_q_shifted[i, 3:7],
body_q_expected[i, 3:7],
atol=1e-6,
err_msg=f"Base reset (translated): body {i} rotation mismatch",
)
def test_09_model_conversions_shape_offset_com_relative(self):
"""
Test that ``geoms.offset`` stores COM-relative shape positions
after Newton→Kamino conversion, while ground shapes are unchanged.
"""
builder: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder)
builder.default_shape_cfg.margin = 0.0
builder.default_shape_cfg.gap = 0.0
builder.begin_world()
# Body with COM=(0.1, 0.2, 0.0), shape at (0.5, 0.0, 0.0)
bid = builder.add_link(
label="body0",
mass=1.0,
xform=wp.transformf(wp.vec3f(0.0, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
com=wp.vec3f(0.1, 0.2, 0.0),
lock_inertia=True,
)
builder.add_shape_box(
label="box0",
body=bid,
hx=0.05,
hy=0.05,
hz=0.05,
xform=wp.transformf(wp.vec3f(0.5, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
)
# Ground shape (bid=-1) — should be left unchanged
builder.add_shape_box(
label="ground_box",
body=-1,
hx=1.0,
hy=1.0,
hz=0.01,
xform=wp.transformf(wp.vec3f(1.0, 0.0, 0.0), wp.quat_identity(dtype=wp.float32)),
)
builder.add_joint_fixed(
label="fix",
parent=-1,
child=bid,
parent_xform=wp.transform_identity(dtype=wp.float32),
child_xform=wp.transform_identity(dtype=wp.float32),
)
builder.end_world()
model: Model = builder.finalize(skip_validation_joints=True)
kamino_model: ModelKamino = ModelKamino.from_newton(model)
offset_np = kamino_model.geoms.offset.numpy()
# Shape on body: pos should be (0.5-0.1, 0.0-0.2, 0.0) = (0.4, -0.2, 0.0)
np.testing.assert_allclose(offset_np[0, :3], [0.4, -0.2, 0.0], atol=1e-6)
# Ground shape: pos unchanged at (1.0, 0.0, 0.0)
np.testing.assert_allclose(offset_np[1, :3], [1.0, 0.0, 0.0], atol=1e-6)
def test_10_origin_com_roundtrip(self):
"""
Test that origin→COM→origin is the identity on body_q.
"""
model = self._build_com_offset_model()
body_q = wp.clone(model.body_q)
q_orig = body_q.numpy().copy()
convert_body_origin_to_com(model.body_com, body_q, body_q)
convert_body_com_to_origin(model.body_com, body_q, body_q)
np.testing.assert_allclose(body_q.numpy(), q_orig, atol=1e-6, err_msg="body_q roundtrip failed")
def test_11_model_conversions_material_fourbar_from_builder(self):
"""
Test the conversion operations between newton.Model and kamino.ModelKamino
on a simple fourbar model with different materials, created explicitly using the builder.
"""
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0: ModelBuilder = basics_newton.build_boxes_fourbar(
builder=builder_0,
z_offset=0.0,
fixedbase=False,
floatingbase=True,
limits=True,
ground=True,
dynamic_joints=False,
implicit_pd=False,
new_world=True,
actuator_ids=[1, 3],
)
# Setting material properties
restitution = [0.1, 0.2, 0.3, 0.4, 0.5]
mu = [0.5, 0.6, 0.7, 0.8, 0.9]
builder_0.shape_material_restitution = list(restitution)
builder_0.shape_material_mu = list(mu)
# Duplicate the world to test multi-world handling
builder_0.begin_world()
builder_0.add_builder(copy.deepcopy(builder_0))
builder_0.end_world()
# Create a fourbar using Kamino's ModelBuilderKamino
builder_1: ModelBuilderKamino = basics_kamino.build_boxes_fourbar(
builder=None,
z_offset=0.0,
fixedbase=False,
floatingbase=True,
limits=True,
ground=True,
dynamic_joints=False,
implicit_pd=False,
new_world=True,
actuator_ids=[1, 3],
)
# Setting material properties
for i in range(len(mu)):
mid = builder_1.add_material(
MaterialDescriptor(
name=f"mat{i}",
restitution=restitution[i],
static_friction=mu[i],
dynamic_friction=mu[i],
)
)
builder_1.geoms[i].material = mid
builder_1.geoms[i].mid = mid
# Duplicate the world to test multi-world handling
builder_1.add_builder(copy.deepcopy(builder_1))
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize(skip_validation_joints=True)
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
test_util_checks.assert_model_equal(self, model_2, model_1)
def test_12_model_conversions_material_box_on_plane_from_usd(self):
"""
Test the conversion operations between newton.Model and kamino.ModelKamino
on a simple box on plane model loaded from USD, containing different materials.
"""
# Define the path to the USD file for the fourbar model
asset_file = os.path.join(get_basics_usd_assets_path(), "box_on_plane.usda")
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0.begin_world()
builder_0.add_usd(
source=asset_file,
joint_ordering=None,
force_show_colliders=True,
force_position_velocity_actuation=True,
)
builder_0.end_world()
# Duplicate the world to test multi-world handling
builder_0.begin_world()
builder_0.add_builder(copy.deepcopy(builder_0))
builder_0.end_world()
# Import the same fourbar using Kamino's USDImporter and ModelBuilderKamino
importer = USDImporter()
builder_1: ModelBuilderKamino = importer.import_from(
source=asset_file,
load_drive_dynamics=True,
load_static_geometry=True,
force_show_colliders=True,
use_prim_path_names=True,
)
# Resetting default material parameters, since the Newton USD importer does not import a
# default material and therefore does not have a non-standard default material
builder_1.materials[0].dynamic_friction = 0.7
# Overwriting dynamic friction with static friction, since the Newton USD importer only
# imports static friction and the Kamino conversion uses this to initialize both parameters
for mat in builder_1.materials:
mat.static_friction = mat.dynamic_friction
# Duplicate the world to test multi-world handling
builder_1.add_builder(copy.deepcopy(builder_1))
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize(skip_validation_joints=True)
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
msg.warning(f"{model_1.materials.restitution}")
msg.warning(f"{model_2.materials.restitution}")
msg.warning(f"{model_1.material_pairs.restitution}")
msg.warning(f"{model_2.material_pairs.restitution}")
test_util_checks.assert_model_geoms_equal(self, model_2.geoms, model_1.geoms)
test_util_checks.assert_model_materials_equal(self, model_2.materials, model_1.materials)
# TODO: Material pairs are currently not checked. The Kamino USD importer will set material
# pair properties based on the list of materials, using the average of the material
# properties. The Newton-to-Kamino conversion will leave the material pair properties
# uninitialized, leaving the choice of how to combine materials for a pair to the
# runtime material resolution system (see :class:`MaterialMuxMode`).
# test_util_checks.assert_model_material_pairs_equal(self, model_2.material_pairs, model_1.material_pairs)
def test_20_state_conversions(self):
"""
Test the conversion operations between newton.State and kamino.StateKamino.
"""
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0: ModelBuilder = basics_newton.build_boxes_fourbar(
builder=builder_0,
z_offset=0.0,
fixedbase=False,
floatingbase=True,
limits=True,
ground=True,
new_world=True,
actuator_ids=[2, 4],
)
# Overwriting mu = 0.7 to match Kamino's default material properties
builder_0.shape_material_mu = [0.7] * len(builder_0.shape_material_mu)
# Duplicate the world to test multi-world handling
builder_0.begin_world()
builder_0.add_builder(copy.deepcopy(builder_0))
builder_0.end_world()
# Create a fourbar using Kamino's ModelBuilderKamino
builder_1: ModelBuilderKamino = basics_kamino.build_boxes_fourbar(
builder=None,
z_offset=0.0,
fixedbase=False,
floatingbase=True,
limits=True,
ground=True,
new_world=True,
actuator_ids=[2, 4],
)
# Duplicate the world to test multi-world handling
builder_1.add_builder(copy.deepcopy(builder_1))
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize(skip_validation_joints=True)
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
test_util_checks.assert_model_equal(self, model_1, model_2)
# Create a Newton state container
state_0: State = model_0.state()
self.assertIsInstance(state_0.body_q, wp.array)
self.assertEqual(state_0.body_q.size, model_0.body_count)
self.assertIsNotNone(state_0.joint_q_prev)
self.assertEqual(state_0.joint_q_prev.size, model_0.joint_coord_count)
self.assertIsNotNone(state_0.joint_lambdas)
self.assertEqual(state_0.joint_lambdas.size, model_0.joint_constraint_count)
# Create a Kamino state container
state_1: StateKamino = model_1.state()
self.assertIsInstance(state_1.q_i, wp.array)
self.assertEqual(state_1.q_i.size, model_1.size.sum_of_num_bodies)
state_2: StateKamino = StateKamino.from_newton(model_2.size, model_0, state_0, True, False)
self.assertIsInstance(state_2.q_i, wp.array)
self.assertEqual(state_2.q_i.size, model_1.size.sum_of_num_bodies)
# NOTE: Check ptr due to conversion from wp.spatial_vectorf
self.assertIs(state_2.u_i.ptr, state_0.body_qd.ptr)
self.assertIs(state_2.w_i_e.ptr, state_0.body_f.ptr)
self.assertIs(state_2.w_i.ptr, state_0.body_f_total.ptr)
# NOTE: Check that the same arrays because these should be pure references
self.assertIs(state_2.q_i, state_0.body_q)
self.assertIs(state_2.q_j, state_0.joint_q)
self.assertIs(state_2.dq_j, state_0.joint_qd)
self.assertIs(state_2.q_j_p, state_0.joint_q_prev)
self.assertIs(state_2.lambda_j, state_0.joint_lambdas)
test_util_checks.assert_state_equal(self, state_2, state_1)
state_3: State = StateKamino.to_newton(model_0, state_2)
self.assertIsInstance(state_3.body_q, wp.array)
self.assertEqual(state_3.body_q.size, model_0.body_count)
# NOTE: Check ptr due to conversion from vec6f
self.assertIs(state_3.body_qd.ptr, state_2.u_i.ptr)
self.assertIs(state_3.body_f.ptr, state_2.w_i_e.ptr)
self.assertIs(state_3.body_f_total.ptr, state_2.w_i.ptr)
# NOTE: Check that the same arrays because these should be pure references
self.assertIs(state_3.body_q, state_2.q_i)
self.assertIs(state_3.joint_q, state_2.q_j)
self.assertIs(state_3.joint_qd, state_2.dq_j)
self.assertIs(state_3.joint_q_prev, state_2.q_j_p)
self.assertIs(state_3.joint_lambdas, state_2.lambda_j)
def test_30_control_conversions(self):
"""
Test the conversions between newton.Control and kamino.ControlKamino.
"""
# Create a fourbar using Newton's ModelBuilder and
# register Kamino-specific custom attributes
builder_0: ModelBuilder = ModelBuilder()
SolverKamino.register_custom_attributes(builder_0)
builder_0.default_shape_cfg.margin = 0.0
builder_0.default_shape_cfg.gap = 0.0
# Create a fourbar using Newton's ModelBuilder
builder_0: ModelBuilder = basics_newton.build_boxes_fourbar(
builder=builder_0,
z_offset=0.0,
fixedbase=False,
floatingbase=True,
# dynamic_joints=True,
# implicit_pd=True,
limits=True,
ground=True,
new_world=True,
actuator_ids=[1, 2, 3, 4],
)
# Overwriting mu = 0.7 to match Kamino's default material properties
builder_0.shape_material_mu = [0.7] * len(builder_0.shape_material_mu)
# Duplicate the world to test multi-world handling
builder_0.begin_world()
builder_0.add_builder(copy.deepcopy(builder_0))
builder_0.end_world()
# Create a fourbar using Kamino's ModelBuilderKamino
builder_1: ModelBuilderKamino = basics_kamino.build_boxes_fourbar(
builder=None,
z_offset=0.0,
fixedbase=False,
floatingbase=True,
# dynamic_joints=True,
# implicit_pd=True,
limits=True,
ground=True,
new_world=True,
actuator_ids=[1, 2, 3, 4],
)
# Duplicate the world to test multi-world handling
builder_1.add_builder(copy.deepcopy(builder_1))
# Create models from the builders and conversion operations, and check for consistency
model_0: Model = builder_0.finalize(skip_validation_joints=True)
model_1: ModelKamino = builder_1.finalize()
model_2: ModelKamino = ModelKamino.from_newton(model_0)
test_util_checks.assert_model_equal(self, model_2, model_1)
# Create a Newton control container
control_0: Control = model_0.control()
self.assertIsInstance(control_0.joint_f, wp.array)
self.assertEqual(control_0.joint_f.size, model_0.joint_dof_count)
# Create a Kamino control container
control_1: ControlKamino = model_1.control()
self.assertIsInstance(control_1.tau_j, wp.array)
self.assertEqual(control_1.tau_j.size, model_1.size.sum_of_num_joint_dofs)
# Create a Kamino control container
control_2: ControlKamino = ControlKamino.from_newton(control_0)
self.assertIsInstance(control_2.tau_j, wp.array)
self.assertIs(control_2.tau_j, control_0.joint_f)
self.assertEqual(control_2.tau_j.size, model_0.joint_dof_count)
test_util_checks.assert_control_equal(self, control_2, control_1)
# Convert back to a Newton control container
control_3: Control = ControlKamino.to_newton(control_2)
self.assertIsInstance(control_3.joint_f, wp.array)
self.assertIs(control_3.joint_f, control_2.tau_j)
self.assertEqual(control_3.joint_f.size, model_0.joint_dof_count)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_core_shapes.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: UNIT TESTS: CORE: SHAPES
"""
import unittest
import numpy as np
import warp as wp
from newton._src.geometry.types import GeoType
from newton._src.solvers.kamino._src.core.shapes import (
BoxShape,
CapsuleShape,
ConeShape,
CylinderShape,
EllipsoidShape,
EmptyShape,
MeshShape,
PlaneShape,
SphereShape,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestShapeDescriptors(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True to enable verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_empty_shape(self):
# Create a default-constructed surface material
shape = EmptyShape()
# Check default values
self.assertEqual(shape.type, GeoType.NONE)
self.assertEqual(shape.params, None)
self.assertEqual(shape.name, "empty")
self.assertIsInstance(shape.uid, str)
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(EmptyShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertEqual(shape_hash, base_hash)
def test_01_sphere_shape(self):
# Create a sphere shape
radius = 1.0
shape = SphereShape(radius)
# Check default values
self.assertEqual(shape.name, "sphere")
self.assertEqual(shape.type, GeoType.SPHERE)
self.assertEqual(shape.params, radius)
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(SphereShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertEqual(shape_hash, base_hash)
def test_02_cylinder_shape(self):
# Create a cylinder shape (Newton convention: half-height)
radius = 0.5
half_height = 1.0
shape = CylinderShape(radius, half_height)
# Check default values
self.assertEqual(shape.name, "cylinder")
self.assertEqual(shape.type, GeoType.CYLINDER)
self.assertEqual(shape.params, (radius, half_height))
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(CylinderShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertEqual(shape_hash, base_hash)
def test_03_cone_shape(self):
# Create a cone shape (Newton convention: half-height)
radius = 0.5
half_height = 1.0
shape = ConeShape(radius, half_height)
# Check default values
self.assertEqual(shape.name, "cone")
self.assertEqual(shape.type, GeoType.CONE)
self.assertEqual(shape.params, (radius, half_height))
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(ConeShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertEqual(shape_hash, base_hash)
def test_04_capsule_shape(self):
# Create a capsule shape (Newton convention: half-height)
radius = 0.5
half_height = 1.0
shape = CapsuleShape(radius, half_height)
# Check default values
self.assertEqual(shape.name, "capsule")
self.assertEqual(shape.type, GeoType.CAPSULE)
self.assertEqual(shape.params, (radius, half_height))
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(CapsuleShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertEqual(shape_hash, base_hash)
def test_05_box_shape(self):
# Create a box shape (Newton convention: half-extents)
half_extents = (0.5, 1.0, 1.5)
shape = BoxShape(*half_extents)
# Check default values
self.assertEqual(shape.name, "box")
self.assertEqual(shape.type, GeoType.BOX)
self.assertEqual(shape.params, half_extents)
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(BoxShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertEqual(shape_hash, base_hash)
def test_06_ellipsoid_shape(self):
# Create an ellipsoid shape
radii = (1.0, 2.0, 3.0)
shape = EllipsoidShape(*radii)
# Check default values
self.assertEqual(shape.name, "ellipsoid")
self.assertEqual(shape.type, GeoType.ELLIPSOID)
self.assertEqual(shape.params, radii)
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(EllipsoidShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertEqual(shape_hash, base_hash)
def test_07_plane_shape(self):
# Create a plane shape
normal = (0.0, 1.0, 0.0)
distance = 0.5
shape = PlaneShape(normal, distance)
# Check default values
self.assertEqual(shape.name, "plane")
self.assertEqual(shape.type, GeoType.PLANE)
self.assertEqual(shape.params, (*normal, distance))
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(PlaneShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertEqual(shape_hash, base_hash)
def test_08_mesh_shape(self):
# Create a mesh shape
vertices = [(0, 0, 0), (1, 0, 0), (0, 1, 0)]
indices = [(0, 1, 2)]
shape = MeshShape(vertices, indices)
# Check default values
self.assertEqual(shape.name, "mesh")
self.assertEqual(shape.type, GeoType.MESH)
self.assertEqual(shape.params, (1.0, 1.0, 1.0))
self.assertTrue(np.array_equal(shape.vertices, np.array(vertices)))
self.assertTrue(np.array_equal(shape.indices, np.array(indices).flatten()))
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(MeshShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertNotEqual(shape_hash, base_hash)
def test_09_convex_shape(self):
# Create a convex mesh shape
vertices = [(0, 0, 0), (1, 0, 0), (0, 1, 0)]
indices = [(0, 1, 2)]
shape = MeshShape(vertices, indices, is_convex=True)
# Check default values
self.assertEqual(shape.name, "convex")
self.assertEqual(shape.type, GeoType.CONVEX_MESH)
self.assertEqual(shape.params, (1.0, 1.0, 1.0))
self.assertTrue(np.array_equal(shape.vertices, np.array(vertices)))
self.assertTrue(np.array_equal(shape.indices, np.array(indices).flatten()))
# Check hash function
shape_hash = hash(shape)
shape_hash2 = hash(shape)
base_hash = super(MeshShape, shape).__hash__()
self.assertEqual(shape_hash, shape_hash2)
self.assertNotEqual(shape_hash, base_hash)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_core_world.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the WorldDescriptor container in Kamino"""
import math
import unittest
import numpy as np
import warp as wp
from newton._src.geometry.types import GeoType
from newton._src.solvers.kamino._src.core.bodies import RigidBodyDescriptor
from newton._src.solvers.kamino._src.core.geometry import GeometryDescriptor
from newton._src.solvers.kamino._src.core.gravity import (
GRAVITY_ACCEL_DEFAULT,
GRAVITY_DIREC_DEFAULT,
GRAVITY_NAME_DEFAULT,
GravityDescriptor,
)
from newton._src.solvers.kamino._src.core.joints import (
JOINT_DQMAX,
JOINT_QMAX,
JOINT_QMIN,
JOINT_TAUMAX,
JointActuationType,
JointDescriptor,
JointDoFType,
)
from newton._src.solvers.kamino._src.core.materials import (
DEFAULT_DENSITY,
DEFAULT_FRICTION,
DEFAULT_RESTITUTION,
MaterialDescriptor,
)
from newton._src.solvers.kamino._src.core.shapes import SphereShape
from newton._src.solvers.kamino._src.core.world import WorldDescriptor
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestGravityDescriptor(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_default_construction(self):
gravity = GravityDescriptor()
msg.info(f"gravity: {gravity}")
self.assertIsInstance(gravity, GravityDescriptor)
self.assertEqual(gravity.name, GRAVITY_NAME_DEFAULT)
self.assertEqual(gravity.enabled, True)
self.assertEqual(gravity.acceleration, GRAVITY_ACCEL_DEFAULT)
expected_direction = np.array(GRAVITY_DIREC_DEFAULT, dtype=np.float32)
expected_dir_accel = np.array([*GRAVITY_DIREC_DEFAULT, GRAVITY_ACCEL_DEFAULT], dtype=np.float32)
expected_vector = np.array([0.0, 0.0, -GRAVITY_ACCEL_DEFAULT, 1.0], dtype=np.float32)
np.testing.assert_array_equal(gravity.direction, expected_direction)
np.testing.assert_array_equal(gravity.dir_accel(), expected_dir_accel)
np.testing.assert_array_equal(gravity.vector(), expected_vector)
def test_01_with_parameters_and_dir_as_list(self):
gravity = GravityDescriptor(name="test_gravity", enabled=False, acceleration=15.0, direction=[1.0, 0.0, 0.0])
msg.info(f"gravity: {gravity}")
self.assertIsInstance(gravity, GravityDescriptor)
self.assertEqual(gravity.name, "test_gravity")
self.assertEqual(gravity.enabled, False)
self.assertEqual(gravity.acceleration, 15.0)
np.testing.assert_array_equal(gravity.direction, np.array([1.0, 0.0, 0.0], dtype=np.float32))
np.testing.assert_array_equal(gravity.dir_accel(), np.array([1.0, 0.0, 0.0, 15.0], dtype=np.float32))
np.testing.assert_array_equal(gravity.vector(), np.array([15.0, 0.0, 0.0, 0.0], dtype=np.float32))
def test_02_with_parameters_and_dir_as_tuple(self):
gravity = GravityDescriptor(name="test_gravity", enabled=False, acceleration=9.0, direction=(1.0, 0.0, 0.0))
msg.info(f"gravity: {gravity}")
self.assertIsInstance(gravity, GravityDescriptor)
self.assertEqual(gravity.name, "test_gravity")
self.assertEqual(gravity.enabled, False)
self.assertEqual(gravity.acceleration, 9.0)
np.testing.assert_array_equal(gravity.direction, np.array([1.0, 0.0, 0.0], dtype=np.float32))
np.testing.assert_array_equal(gravity.dir_accel(), np.array([1.0, 0.0, 0.0, 9.0], dtype=np.float32))
np.testing.assert_array_equal(gravity.vector(), np.array([9.0, 0.0, 0.0, 0.0], dtype=np.float32))
def test_03_with_parameters_and_dir_as_nparray(self):
direction = np.array([1.0, 0.0, 0.0], dtype=np.float32)
gravity = GravityDescriptor(name="test_gravity", enabled=False, acceleration=12.0, direction=direction)
msg.info(f"gravity: {gravity}")
self.assertIsInstance(gravity, GravityDescriptor)
self.assertEqual(gravity.name, "test_gravity")
self.assertEqual(gravity.enabled, False)
self.assertEqual(gravity.acceleration, 12.0)
np.testing.assert_array_equal(gravity.direction, np.array([1.0, 0.0, 0.0], dtype=np.float32))
np.testing.assert_array_equal(gravity.dir_accel(), np.array([1.0, 0.0, 0.0, 12.0], dtype=np.float32))
np.testing.assert_array_equal(gravity.vector(), np.array([12.0, 0.0, 0.0, 0.0], dtype=np.float32))
class TestBodyDescriptor(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_default_construction(self):
body = RigidBodyDescriptor(name="test_body")
self.assertIsInstance(body, RigidBodyDescriptor)
msg.info(f"body: {body}")
self.assertEqual(body.name, "test_body")
self.assertEqual(body.m_i, 0.0)
np.testing.assert_array_equal(body.i_I_i, np.zeros(9, dtype=np.float32))
np.testing.assert_array_equal(body.q_i_0, np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float32))
np.testing.assert_array_equal(body.u_i_0, np.zeros(6, dtype=np.float32))
self.assertEqual(body.wid, -1)
self.assertEqual(body.bid, -1)
class TestJointDescriptor(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_default_construction(self):
joint = JointDescriptor(name="test_joint")
self.assertIsInstance(joint, JointDescriptor)
msg.info(f"joint: {joint}")
self.assertEqual(joint.name, "test_joint")
self.assertIsInstance(joint.dof_type, JointDoFType)
self.assertIsInstance(joint.act_type, JointActuationType)
self.assertEqual(joint.dof_type, JointDoFType.FREE)
self.assertEqual(joint.act_type, JointActuationType.PASSIVE)
self.assertEqual(joint.bid_B, -1)
self.assertEqual(joint.bid_F, -1)
self.assertEqual(joint.num_coords, 7)
self.assertEqual(joint.num_dofs, 6)
self.assertEqual(joint.num_dynamic_cts, 0)
self.assertEqual(joint.num_kinematic_cts, 0)
np.testing.assert_array_equal(joint.B_r_Bj, np.zeros(3, dtype=np.float32))
np.testing.assert_array_equal(joint.F_r_Fj, np.zeros(3, dtype=np.float32))
np.testing.assert_array_equal(joint.X_j, np.zeros(9, dtype=np.float32))
np.testing.assert_array_equal(joint.q_j_min, np.full(6, JOINT_QMIN, dtype=np.float32))
np.testing.assert_array_equal(joint.q_j_max, np.full(6, JOINT_QMAX, dtype=np.float32))
np.testing.assert_array_equal(joint.dq_j_max, np.full(6, JOINT_DQMAX, dtype=np.float32))
np.testing.assert_array_equal(joint.tau_j_max, np.full(6, JOINT_TAUMAX, dtype=np.float32))
np.testing.assert_array_equal(joint.a_j, np.zeros(6, dtype=np.float32))
np.testing.assert_array_equal(joint.b_j, np.zeros(6, dtype=np.float32))
np.testing.assert_array_equal(joint.k_p_j, np.zeros(6, dtype=np.float32))
np.testing.assert_array_equal(joint.k_d_j, np.zeros(6, dtype=np.float32))
self.assertEqual(joint.wid, -1)
self.assertEqual(joint.jid, -1)
self.assertEqual(joint.coords_offset, -1)
self.assertEqual(joint.dofs_offset, -1)
self.assertEqual(joint.passive_coords_offset, -1)
self.assertEqual(joint.passive_dofs_offset, -1)
self.assertEqual(joint.actuated_coords_offset, -1)
self.assertEqual(joint.actuated_dofs_offset, -1)
self.assertEqual(joint.kinematic_cts_offset, -1)
self.assertEqual(joint.dynamic_cts_offset, -1)
# Check property methods
self.assertEqual(joint.is_actuated, False)
self.assertEqual(joint.is_passive, True)
self.assertEqual(joint.is_binary, False)
self.assertEqual(joint.is_unary, True)
self.assertEqual(joint.is_dynamic, False)
def test_01_actuated_revolute_joint_with_effort_dynamics(self):
joint = JointDescriptor(
name="test_joint_revolute_dynamic",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.FORCE,
bid_B=0,
bid_F=1,
a_j=1.0,
b_j=1.0,
)
msg.info(f"joint: {joint}")
# Check values
self.assertIsInstance(joint, JointDescriptor)
self.assertEqual(joint.name, "test_joint_revolute_dynamic")
self.assertIsInstance(joint.dof_type, JointDoFType)
self.assertIsInstance(joint.act_type, JointActuationType)
self.assertEqual(joint.dof_type, JointDoFType.REVOLUTE)
self.assertEqual(joint.act_type, JointActuationType.FORCE)
self.assertEqual(joint.bid_B, 0)
self.assertEqual(joint.bid_F, 1)
self.assertEqual(joint.num_coords, 1)
self.assertEqual(joint.num_dofs, 1)
self.assertEqual(joint.num_dynamic_cts, 1)
self.assertEqual(joint.num_kinematic_cts, 5)
np.testing.assert_array_equal(joint.B_r_Bj, np.zeros(3, dtype=np.float32))
np.testing.assert_array_equal(joint.F_r_Fj, np.zeros(3, dtype=np.float32))
np.testing.assert_array_equal(joint.X_j, np.zeros(9, dtype=np.float32))
np.testing.assert_array_equal(joint.q_j_min, float(JOINT_QMIN))
np.testing.assert_array_equal(joint.q_j_max, float(JOINT_QMAX))
np.testing.assert_array_equal(joint.dq_j_max, float(JOINT_DQMAX))
np.testing.assert_array_equal(joint.tau_j_max, float(JOINT_TAUMAX))
np.testing.assert_array_equal(joint.a_j, 1.0)
np.testing.assert_array_equal(joint.b_j, 1.0)
np.testing.assert_array_equal(joint.k_p_j, 0.0)
np.testing.assert_array_equal(joint.k_d_j, 0.0)
self.assertEqual(joint.wid, -1)
self.assertEqual(joint.jid, -1)
self.assertEqual(joint.coords_offset, -1)
self.assertEqual(joint.dofs_offset, -1)
self.assertEqual(joint.passive_coords_offset, -1)
self.assertEqual(joint.passive_dofs_offset, -1)
self.assertEqual(joint.actuated_coords_offset, -1)
self.assertEqual(joint.actuated_dofs_offset, -1)
self.assertEqual(joint.kinematic_cts_offset, -1)
self.assertEqual(joint.dynamic_cts_offset, -1)
# Check property methods
self.assertEqual(joint.is_actuated, True)
self.assertEqual(joint.is_passive, False)
self.assertEqual(joint.is_binary, True)
self.assertEqual(joint.is_unary, False)
self.assertEqual(joint.is_dynamic, True)
class TestGeometryDescriptor(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_default_construction(self):
geom = GeometryDescriptor(name="test_geom", body=0)
msg.info(f"geom: {geom}")
self.assertIsInstance(geom, GeometryDescriptor)
self.assertEqual(geom.name, "test_geom")
self.assertEqual(geom.body, 0)
self.assertEqual(geom.shape, None)
self.assertEqual(geom.wid, -1)
self.assertEqual(geom.gid, -1)
self.assertEqual(geom.material, None)
self.assertEqual(geom.group, 1)
self.assertEqual(geom.collides, 1)
self.assertEqual(geom.max_contacts, 0)
self.assertEqual(geom.margin, 0.0)
self.assertEqual(geom.mid, -1)
def test_01_with_shape(self):
cgeom = GeometryDescriptor(name="test_geom", body=0, shape=SphereShape(radius=1.0, name="test_sphere"))
msg.info(f"cgeom: {cgeom}")
self.assertIsInstance(cgeom, GeometryDescriptor)
self.assertEqual(cgeom.name, "test_geom")
self.assertEqual(cgeom.body, 0)
self.assertEqual(cgeom.shape.type, GeoType.SPHERE)
self.assertEqual(cgeom.shape.radius, 1.0)
self.assertEqual(cgeom.wid, -1)
self.assertEqual(cgeom.gid, -1)
self.assertEqual(cgeom.material, None)
self.assertEqual(cgeom.group, 1)
self.assertEqual(cgeom.collides, 1)
self.assertEqual(cgeom.max_contacts, 0)
self.assertEqual(cgeom.margin, 0.0)
self.assertEqual(cgeom.mid, -1)
def test_02_with_shape_and_material(self):
test_material = MaterialDescriptor(name="test_material")
cgeom = GeometryDescriptor(
name="test_geom",
body=0,
shape=SphereShape(radius=1.0, name="test_sphere"),
material=test_material.name,
)
msg.info(f"cgeom: {cgeom}")
self.assertIsInstance(cgeom, GeometryDescriptor)
self.assertEqual(cgeom.name, "test_geom")
self.assertEqual(cgeom.body, 0)
self.assertEqual(cgeom.shape.type, GeoType.SPHERE)
self.assertEqual(cgeom.shape.radius, 1.0)
self.assertEqual(cgeom.wid, -1)
self.assertEqual(cgeom.gid, -1)
self.assertEqual(cgeom.material, test_material.name)
self.assertEqual(cgeom.group, 1)
self.assertEqual(cgeom.collides, 1)
self.assertEqual(cgeom.max_contacts, 0)
self.assertEqual(cgeom.margin, 0.0)
self.assertEqual(cgeom.mid, -1)
def test_03_from_base_geometry(self):
geom = GeometryDescriptor(name="test_geom", body=0, shape=SphereShape(radius=1.0, name="test_sphere"))
msg.info(f"geom: {geom}")
self.assertIsInstance(geom, GeometryDescriptor)
self.assertEqual(geom.name, "test_geom")
self.assertEqual(geom.body, 0)
self.assertEqual(geom.shape.type, GeoType.SPHERE)
self.assertEqual(geom.shape.radius, 1.0)
self.assertEqual(geom.wid, -1)
self.assertEqual(geom.gid, -1)
self.assertEqual(geom.material, None)
self.assertEqual(geom.group, 1)
self.assertEqual(geom.collides, 1)
self.assertEqual(geom.max_contacts, 0)
self.assertEqual(geom.margin, 0.0)
self.assertEqual(geom.mid, -1)
class TestMaterialDescriptor(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_default_construction(self):
mat = MaterialDescriptor(name="test_mat")
msg.info(f"mat: {mat}")
self.assertIsInstance(mat, MaterialDescriptor)
self.assertEqual(mat.name, "test_mat")
self.assertEqual(mat.density, DEFAULT_DENSITY)
self.assertEqual(mat.restitution, DEFAULT_RESTITUTION)
self.assertEqual(mat.static_friction, DEFAULT_FRICTION)
self.assertEqual(mat.dynamic_friction, DEFAULT_FRICTION)
self.assertEqual(mat.wid, -1)
self.assertEqual(mat.mid, -1)
def test_01_with_properties(self):
mat = MaterialDescriptor(
name="test_mat",
density=500.0,
restitution=0.5,
static_friction=0.6,
dynamic_friction=0.4,
)
msg.info(f"mat: {mat}")
self.assertIsInstance(mat, MaterialDescriptor)
self.assertEqual(mat.name, "test_mat")
self.assertEqual(mat.density, 500.0)
self.assertEqual(mat.restitution, 0.5)
self.assertEqual(mat.static_friction, 0.6)
self.assertEqual(mat.dynamic_friction, 0.4)
self.assertEqual(mat.wid, -1)
self.assertEqual(mat.mid, -1)
class TestWorldDescriptor(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_default_construction(self):
world = WorldDescriptor(name="test_world")
self.assertIsInstance(world, WorldDescriptor)
msg.info(f"world.name: {world.name}")
msg.info(f"world.uid: {world.uid}")
msg.info(f"world.num_bodies: {world.num_bodies}")
msg.info(f"world.num_joints: {world.num_joints}")
msg.info(f"world.body_names: {world.body_names}")
msg.info(f"world.joint_names: {world.joint_names}")
msg.info(f"world.mass_min: {world.mass_min}")
msg.info(f"world.mass_max: {world.mass_max}")
msg.info(f"world.mass_total: {world.mass_total}")
msg.info(f"world.inertia_total: {world.inertia_total}")
self.assertEqual(world.name, "test_world")
self.assertEqual(world.num_bodies, 0)
self.assertEqual(world.num_joints, 0)
self.assertEqual(world.num_geoms, 0)
self.assertEqual(len(world.body_names), 0)
self.assertEqual(len(world.joint_names), 0)
self.assertEqual(len(world.geom_names), 0)
self.assertEqual(world.mass_min, math.inf)
self.assertEqual(world.mass_max, 0.0)
self.assertEqual(world.mass_total, 0.0)
self.assertEqual(world.inertia_total, 0.0)
def test_10_add_body(self):
world = WorldDescriptor(name="test_world", wid=37)
msg.info(f"world.name: {world.name}")
msg.info(f"world.uid: {world.uid}")
# Add two bodies to the world
body_0 = RigidBodyDescriptor(name="body_0", m_i=1.0)
world.add_body(body_0)
msg.info(f"body_0: {body_0}")
self.assertEqual(body_0.bid, 0)
self.assertEqual(body_0.wid, world.wid)
body_1 = RigidBodyDescriptor(name="body_1", m_i=0.5)
world.add_body(body_1)
msg.info(f"body_1: {body_1}")
self.assertEqual(body_1.bid, 1)
self.assertEqual(body_1.wid, world.wid)
# Verify world properties
self.assertEqual(world.num_bodies, 2)
self.assertIn(body_0.name, world.body_names)
self.assertIn(body_1.name, world.body_names)
self.assertEqual(world.mass_min, 0.5)
self.assertEqual(world.mass_max, 1.0)
self.assertEqual(world.mass_total, 1.5)
self.assertEqual(world.inertia_total, 4.5)
def test_20_add_joint_revolute_passive(self):
world = WorldDescriptor(name="test_world", wid=42)
msg.info(f"world.name: {world.name}")
msg.info(f"world.uid: {world.uid}")
# Add two bodies to the world
body_0 = RigidBodyDescriptor(name="body_0", m_i=1.0)
world.add_body(body_0)
msg.info(f"body_0: {body_0}")
self.assertEqual(body_0.bid, 0)
self.assertEqual(body_0.wid, world.wid)
body_1 = RigidBodyDescriptor(name="body_1", m_i=0.5)
world.add_body(body_1)
msg.info(f"body_1: {body_1}")
self.assertEqual(body_1.bid, 1)
self.assertEqual(body_1.wid, world.wid)
# Define a joint between two bodies
joint_0 = JointDescriptor(
name="body_0_to_1",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.PASSIVE,
bid_B=body_0.bid,
bid_F=body_1.bid,
)
world.add_joint(joint_0)
msg.info(f"joint_0: {joint_0}")
self.assertEqual(joint_0.jid, 0)
self.assertEqual(joint_0.wid, world.wid)
self.assertFalse(joint_0.is_actuated)
self.assertTrue(joint_0.is_binary)
self.assertTrue(joint_0.is_connected_to_body(body_0.bid))
self.assertTrue(joint_0.is_connected_to_body(body_1.bid))
# Verify world properties
self.assertEqual(world.num_bodies, 2)
self.assertEqual(world.num_joints, 1)
self.assertIn(body_0.name, world.body_names)
self.assertIn(body_1.name, world.body_names)
self.assertIn(joint_0.name, world.joint_names)
self.assertIn(joint_0.name, world.passive_joint_names)
self.assertEqual(world.mass_min, 0.5)
self.assertEqual(world.mass_max, 1.0)
self.assertEqual(world.mass_total, 1.5)
self.assertEqual(world.inertia_total, 4.5)
def test_21_add_joint_revolute_actuated_dynamic(self):
world = WorldDescriptor(name="test_world", wid=42)
msg.info(f"world.name: {world.name}")
msg.info(f"world.uid: {world.uid}")
# Add two bodies to the world
body_0 = RigidBodyDescriptor(name="body_0", m_i=1.0)
world.add_body(body_0)
msg.info(f"body_0: {body_0}")
self.assertEqual(body_0.bid, 0)
self.assertEqual(body_0.wid, world.wid)
body_1 = RigidBodyDescriptor(name="body_1", m_i=0.5)
world.add_body(body_1)
msg.info(f"body_1: {body_1}")
self.assertEqual(body_1.bid, 1)
self.assertEqual(body_1.wid, world.wid)
# Define a joint between two bodies
joint_0 = JointDescriptor(
name="body_0_to_1",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.PASSIVE,
bid_B=body_0.bid,
bid_F=body_1.bid,
a_j=1.0,
b_j=1.0,
)
world.add_joint(joint_0)
msg.info(f"joint_0: {joint_0}")
self.assertEqual(joint_0.jid, 0)
self.assertEqual(joint_0.wid, world.wid)
self.assertFalse(joint_0.is_actuated)
self.assertTrue(joint_0.is_binary)
self.assertTrue(joint_0.is_dynamic)
self.assertTrue(joint_0.is_connected_to_body(body_0.bid))
self.assertTrue(joint_0.is_connected_to_body(body_1.bid))
# Verify world properties
self.assertEqual(world.num_bodies, 2)
self.assertEqual(world.num_joints, 1)
self.assertIn(body_0.name, world.body_names)
self.assertIn(body_1.name, world.body_names)
self.assertIn(joint_0.name, world.joint_names)
self.assertIn(joint_0.name, world.passive_joint_names)
self.assertTrue(world.has_passive_dofs)
self.assertFalse(world.has_actuated_dofs)
self.assertTrue(world.has_implicit_dofs)
def test_30_add_geometry(self):
world = WorldDescriptor(name="test_world", wid=42)
msg.info(f"world.name: {world.name}")
msg.info(f"world.uid: {world.uid}")
# Add two bodies to the world
body_0 = RigidBodyDescriptor(name="body_0", m_i=1.0)
world.add_body(body_0)
msg.info(f"body_0: {body_0}")
self.assertEqual(body_0.bid, 0)
self.assertEqual(body_0.wid, world.wid)
# Add physical geometry to body_0
geom = GeometryDescriptor(name="test_geom", body=body_0.bid, shape=SphereShape(radius=1.0, name="test_sphere"))
world.add_geometry(geom)
msg.info(f"geom: {geom}")
self.assertEqual(geom.name, "test_geom")
self.assertEqual(geom.body, body_0.bid)
self.assertEqual(geom.shape.type, GeoType.SPHERE)
self.assertEqual(geom.shape.radius, 1.0)
self.assertEqual(geom.wid, world.wid)
self.assertEqual(geom.gid, 0)
# Verify world properties
self.assertEqual(world.num_geoms, 1)
self.assertIn(body_0.name, world.body_names)
self.assertIn(geom.name, world.geom_names)
def test_40_add_material(self):
world = WorldDescriptor(name="test_world", wid=42)
msg.info(f"world.name: {world.name}")
msg.info(f"world.uid: {world.uid}")
# Add a material to the world
mat = MaterialDescriptor(name="test_mat")
world.add_material(mat)
msg.info(f"mat: {mat}")
self.assertEqual(mat.name, "test_mat")
self.assertEqual(mat.wid, world.wid)
self.assertEqual(mat.mid, 0)
# Verify world properties
self.assertEqual(world.num_materials, 1)
self.assertIn(mat.name, world.material_names)
self.assertIn(mat.uid, world.material_uids)
def test_50_set_base_body(self):
world = WorldDescriptor(name="test_world", wid=42)
msg.info(f"world.name: {world.name}")
msg.info(f"world.uid: {world.uid}")
# Add some bodies to the world
body_0 = RigidBodyDescriptor(name="body_0", m_i=1.0)
world.add_body(body_0)
msg.info(f"body_0: {body_0}")
self.assertEqual(body_0.bid, 0)
self.assertEqual(body_0.wid, world.wid)
body_1 = RigidBodyDescriptor(name="body_1", m_i=0.5)
world.add_body(body_1)
msg.info(f"body_1: {body_1}")
self.assertEqual(body_1.bid, 1)
self.assertEqual(body_1.wid, world.wid)
body_2 = RigidBodyDescriptor(name="body_2", m_i=0.25)
world.add_body(body_2)
msg.info(f"body_2: {body_2}")
self.assertEqual(body_2.bid, 2)
self.assertEqual(body_2.wid, world.wid)
# Set body_0 as the base body
world.set_base_body(2)
self.assertEqual(world.base_body_idx, body_2.bid)
self.assertTrue(world.has_base_body)
# Attempt to set an invalid body as the base body
self.assertRaises(ValueError, world.set_base_body, 3)
def test_51_set_base_joint(self):
world = WorldDescriptor(name="test_world", wid=42)
msg.info(f"world.name: {world.name}")
msg.info(f"world.uid: {world.uid}")
# Add some bodies to the world
body_0 = RigidBodyDescriptor(name="body_0", m_i=1.0)
world.add_body(body_0)
msg.info(f"body_0: {body_0}")
self.assertEqual(body_0.bid, 0)
self.assertEqual(body_0.wid, world.wid)
body_1 = RigidBodyDescriptor(name="body_1", m_i=0.5)
world.add_body(body_1)
msg.info(f"body_1: {body_1}")
self.assertEqual(body_1.bid, 1)
self.assertEqual(body_1.wid, world.wid)
body_2 = RigidBodyDescriptor(name="body_2", m_i=0.25)
world.add_body(body_2)
msg.info(f"body_2: {body_2}")
self.assertEqual(body_2.bid, 2)
self.assertEqual(body_2.wid, world.wid)
# Add some joints to the world
joint_0 = JointDescriptor(
name="body_0_to_1",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.PASSIVE,
bid_B=body_0.bid,
bid_F=body_1.bid,
)
world.add_joint(joint_0)
msg.info(f"joint_0: {joint_0}")
self.assertEqual(joint_0.jid, 0)
self.assertEqual(joint_0.wid, world.wid)
joint_1 = JointDescriptor(
name="body_1_to_2",
dof_type=JointDoFType.REVOLUTE,
act_type=JointActuationType.PASSIVE,
bid_F=body_2.bid,
)
world.add_joint(joint_1)
msg.info(f"joint_1: {joint_1}")
self.assertEqual(joint_1.jid, 1)
self.assertEqual(joint_1.wid, world.wid)
# Set joint_1 as the base joint
world.set_base_joint(1)
self.assertEqual(world.base_joint_idx, joint_1.jid)
self.assertTrue(world.has_base_joint)
# Attempt to set an invalid joint as the base joint
self.assertRaises(ValueError, world.set_base_joint, 2)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_dynamics_delassus.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the DelassusOperator class"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.data import DataKamino
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.dynamics.delassus import BlockSparseMatrixFreeDelassusOperator, DelassusOperator
from newton._src.solvers.kamino._src.geometry.contacts import ContactsKamino
from newton._src.solvers.kamino._src.kinematics.constraints import get_max_constraints_per_world
from newton._src.solvers.kamino._src.kinematics.jacobians import SparseSystemJacobians
from newton._src.solvers.kamino._src.kinematics.limits import LimitsKamino
from newton._src.solvers.kamino._src.linalg import LLTSequentialSolver
from newton._src.solvers.kamino._src.models.builders.basics import (
build_boxes_fourbar,
build_boxes_nunchaku,
make_basics_heterogeneous_builder,
)
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import (
extract_active_constraint_dims,
extract_cts_jacobians,
extract_delassus,
extract_delassus_sparse,
extract_problem_vector,
)
from newton._src.solvers.kamino.tests.utils.make import (
make_containers,
make_inverse_generalized_mass_matrices,
update_containers,
)
from newton._src.solvers.kamino.tests.utils.print import print_error_stats
from newton._src.solvers.kamino.tests.utils.rand import random_rhs_for_matrix
###
# Helper functions
###
def check_delassus_allocations(
fixture: unittest.TestCase,
model: ModelKamino,
limits: LimitsKamino,
contacts: ContactsKamino,
delassus: DelassusOperator,
) -> None:
# Compute expected and allocated dimensions and sizes
expected_max_constraint_dims = get_max_constraints_per_world(model, limits, contacts)
num_worlds = len(expected_max_constraint_dims)
expected_D_sizes = [expected_max_constraint_dims[i] * expected_max_constraint_dims[i] for i in range(num_worlds)]
delassus_maxdim_np = delassus.info.maxdim.numpy()
fixture.assertEqual(
len(delassus_maxdim_np), num_worlds, "Number of Delassus operator blocks does not match the number of worlds"
)
D_maxdims = [int(delassus_maxdim_np[i]) for i in range(num_worlds)]
D_sizes = [D_maxdims[i] * D_maxdims[i] for i in range(num_worlds)]
D_sizes_sum = sum(D_sizes)
for i in range(num_worlds):
fixture.assertEqual(
D_maxdims[i],
expected_max_constraint_dims[i],
f"Delassus operator block {i} maxdim does not match expected maximum constraint dimension",
)
fixture.assertEqual(
D_sizes[i], expected_D_sizes[i], f"Delassus operator block {i} max size does not match expected max size"
)
# Check Delassus operator data sizes
fixture.assertEqual(delassus.info.maxdim.size, num_worlds)
fixture.assertEqual(delassus.info.dim.size, num_worlds)
fixture.assertEqual(delassus.info.mio.size, num_worlds)
fixture.assertEqual(delassus.info.vio.size, num_worlds)
fixture.assertEqual(delassus.D.size, D_sizes_sum)
# Check if the factorizer info data to the same as the Delassus info data
fixture.assertEqual(delassus.info.maxdim.ptr, delassus.solver.operator.info.maxdim.ptr)
fixture.assertEqual(delassus.info.dim.ptr, delassus.solver.operator.info.dim.ptr)
fixture.assertEqual(delassus.info.mio.ptr, delassus.solver.operator.info.mio.ptr)
fixture.assertEqual(delassus.info.vio.ptr, delassus.solver.operator.info.vio.ptr)
def print_delassus_info(delassus: DelassusOperator) -> None:
print(f"delassus.info.maxdim: {delassus.info.maxdim}")
print(f"delassus.info.dim: {delassus.info.dim}")
print(f"delassus.info.mio: {delassus.info.mio}")
print(f"delassus.info.vio: {delassus.info.vio}")
print(f"delassus.D: {delassus.D.shape}")
###
# Tests
###
class TestDelassusOperator(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for detailed output
self.default_device = wp.get_device(test_context.device)
def tearDown(self):
self.default_device = None
def test_01_allocate_single_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
builder = build_boxes_nunchaku()
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Compare expected to allocated dimensions and sizes
check_delassus_allocations(self, model, limits, detector.contacts, delassus)
# Optional verbose output
if self.verbose:
print("") # Print a newline for better readability
print_delassus_info(delassus)
def test_02_allocate_homogeneous_delassus_operator(self):
# Model constants
num_worlds = 3
max_world_contacts = 12
# Construct a homogeneous model description using model builders
builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_nunchaku)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Compare expected to allocated dimensions and sizes
check_delassus_allocations(self, model, limits, detector.contacts, delassus)
# Optional verbose output
if self.verbose:
print("") # Print a newline for better readability
print_delassus_info(delassus)
def test_03_allocate_heterogeneous_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder()
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Compare expected to allocated dimensions and sizes
check_delassus_allocations(self, model, limits, detector.contacts, delassus)
# Optional verbose output
if self.verbose:
print("") # Print a newline for better readability
print_delassus_info(delassus)
def test_04_build_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
# builder = build_boxes_hinged(z_offset=0.0, ground=False)
builder = build_boxes_fourbar(z_offset=0.0, ground=False, dynamic_joints=True, implicit_pd=True)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder,
max_world_contacts=max_world_contacts,
device=self.default_device,
sparse=True,
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Build the Delassus operator from the current data
delassus.build(model=model, data=data, jacobians=jacobians, reset_to_zero=True)
# Extract the active constraint dimensions
active_dims = extract_active_constraint_dims(data)
active_size = [dims * dims for dims in active_dims]
# Extract Jacobians as numpy arrays
J_cts_np = extract_cts_jacobians(model, limits, detector.contacts, jacobians, only_active_cts=True)
# Extract Delassus data as numpy arrays
D_np = extract_delassus(delassus, only_active_dims=True)
# Construct a list of generalized inverse mass matrices of each world
invM_np = make_inverse_generalized_mass_matrices(model, data)
# Construct the joint armature regularization term for each world
njdcts = model.info.num_joint_dynamic_cts.numpy()
jdcts_start = model.info.joint_dynamic_cts_offset.numpy()
inv_M_q_np = [np.zeros(shape=(dim,), dtype=np.float32) for dim in active_dims]
if np.any(njdcts):
inv_m_j_np = data.joints.inv_m_j.numpy()
for w in range(delassus.num_worlds):
inv_M_q_np[w][: njdcts[w]] = inv_m_j_np[jdcts_start[w] : jdcts_start[w] + njdcts[w]]
inv_M_q_np[w] = np.diag(inv_M_q_np[w])
msg.info(f"[{w}]: inv_M_q_np (shape={inv_M_q_np[w].shape}):\n{inv_M_q_np[w]}\n\n")
# For each world, compute the Delassus matrix using numpy and
# compare it with the one from the Delassus operator class
for w in range(delassus.num_worlds):
# Compute the Delassus matrix using the inverse mass matrix and the Jacobian
D_w = (J_cts_np[w] @ invM_np[w]) @ J_cts_np[w].T + inv_M_q_np[w]
# Compare the computed Delassus matrix with the one from the dual problem
is_D_close = np.allclose(D_np[w], D_w, rtol=1e-3, atol=1e-4)
if not is_D_close or self.verbose:
msg.warning(f"[{w}]: D_w (shape={D_w.shape}):\n{D_w}")
msg.warning(f"[{w}]: D_np (shape={D_np[w].shape}):\n{D_np[w]}")
print_error_stats(f"D[{w}]", D_np[w], D_w, active_size[w], show_errors=True)
self.assertTrue(is_D_close)
def test_05_build_homogeneous_delassus_operator(self):
# Model constants
num_worlds = 3
max_world_contacts = 12
# Construct a homogeneous model description using model builders
builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_nunchaku)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Build the Delassus operator from the current data
delassus.build(model=model, data=data, jacobians=jacobians, reset_to_zero=True)
# Extract the active constraint dimensions
active_dims = extract_active_constraint_dims(data)
# Extract Jacobians as numpy arrays
J_cts_np = extract_cts_jacobians(model, limits, detector.contacts, jacobians, only_active_cts=True)
# Extract Delassus data as numpy arrays
D_np = extract_delassus(delassus, only_active_dims=True)
# Construct a list of generalized inverse mass matrices of each world
invM_np = make_inverse_generalized_mass_matrices(model, data)
# Construct the joint armature regularization term for each world
njdcts = model.info.num_joint_dynamic_cts.numpy()
jdcts_start = model.info.joint_dynamic_cts_offset.numpy()
inv_M_q_np = [np.zeros(shape=(dim,), dtype=np.float32) for dim in active_dims]
if np.any(njdcts):
inv_m_j_np = data.joints.inv_m_j.numpy()
for w in range(delassus.num_worlds):
inv_M_q_np[w][: njdcts[w]] = inv_m_j_np[jdcts_start[w] : jdcts_start[w] + njdcts[w]]
inv_M_q_np[w] = np.diag(inv_M_q_np[w])
print(f"[{w}]: inv_M_q_np (shape={inv_M_q_np[w].shape}):\n{inv_M_q_np[w]}\n\n")
# Optional verbose output
if self.verbose:
print("") # Print a newline for better readability
for i in range(len(active_dims)):
print(f"[{i}]: active_dims: {active_dims[i]}")
for i in range(len(J_cts_np)):
print(f"[{i}]: J_cts_np (shape={J_cts_np[i].shape}):\n{J_cts_np[i]}")
for i in range(len(D_np)):
print(f"[{i}]: D_np (shape={D_np[i].shape}):\n{D_np[i]}")
for i in range(len(invM_np)):
print(f"[{i}]: invM_np (shape={invM_np[i].shape}):\n{invM_np[i]}")
print("") # Add a newline for better readability
print_delassus_info(delassus)
print("") # Add a newline for better readability
# For each world, compute the Delassus matrix using numpy and
# compare it with the one from the Delassus operator class
for w in range(delassus.num_worlds):
# Compute the Delassus matrix using the inverse mass matrix and the Jacobian
D_w = (J_cts_np[w] @ invM_np[w]) @ J_cts_np[w].T + inv_M_q_np[w]
# Compare the computed Delassus matrix with the one from the dual problem
is_D_close = np.allclose(D_np[w], D_w, atol=1e-3, rtol=1e-4)
if not is_D_close or self.verbose:
print(f"[{w}]: D_w (shape={D_w.shape}):\n{D_w}")
print(f"[{w}]: D_np (shape={D_np[w].shape}):\n{D_np[w]}")
print_error_stats(f"D[{w}]", D_np[w], D_w, active_dims[w])
self.assertTrue(is_D_close)
def test_06_build_heterogeneous_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder()
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Build the Delassus operator from the current data
delassus.build(model=model, data=data, jacobians=jacobians, reset_to_zero=True)
# Extract the active constraint dimensions
active_dims = extract_active_constraint_dims(data)
# Extract Jacobians as numpy arrays
J_cts_np = extract_cts_jacobians(model, limits, detector.contacts, jacobians, only_active_cts=True)
# Extract Delassus data as numpy arrays
D_np = extract_delassus(delassus, only_active_dims=True)
# Construct a list of generalized inverse mass matrices of each world
invM_np = make_inverse_generalized_mass_matrices(model, data)
# Optional verbose output
if self.verbose:
print("") # Print a newline for better readability
for i in range(len(active_dims)):
print(f"[{i}]: active_dims: {active_dims[i]}")
for i in range(len(J_cts_np)):
print(f"[{i}]: J_cts_np (shape={J_cts_np[i].shape}):\n{J_cts_np[i]}")
for i in range(len(D_np)):
print(f"[{i}]: D_np (shape={D_np[i].shape}):\n{D_np[i]}")
for i in range(len(invM_np)):
print(f"[{i}]: invM_np (shape={invM_np[i].shape}):\n{invM_np[i]}")
print("") # Add a newline for better readability
print_delassus_info(delassus)
print("") # Add a newline for better readability
# For each world, compute the Delassus matrix using numpy and
# compare it with the one from the Delassus operator class
for w in range(delassus.num_worlds):
# Compute the Delassus matrix using the inverse mass matrix and the Jacobian
D_w = (J_cts_np[w] @ invM_np[w]) @ J_cts_np[w].T
# Compare the computed Delassus matrix with the one from the dual problem
is_D_close = np.allclose(D_np[w], D_w, atol=1e-3, rtol=1e-4)
if not is_D_close or self.verbose:
print(f"[{w}]: D_w (shape={D_w.shape}):\n{D_w}")
print(f"[{w}]: D_np (shape={D_np[w].shape}):\n{D_np[w]}")
print_error_stats(f"D[{w}]", D_np[w], D_w, active_dims[w])
self.assertTrue(is_D_close)
def test_07_regularize_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder()
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Build the Delassus operator from the current data
delassus.build(model=model, data=data, jacobians=jacobians, reset_to_zero=True)
# Extract the active constraint dimensions
active_dims = extract_active_constraint_dims(data)
# Now we reset the Delassus operator to zero and use diagonal regularization to set the diagonal entries to 1.0
eta_wp = wp.full(
shape=(delassus._model_maxdims,), value=wp.float32(1.0), dtype=wp.float32, device=self.default_device
)
delassus.zero()
delassus.regularize(eta_wp)
# Extract Delassus data as numpy arrays
D_np = extract_delassus(delassus, only_active_dims=True)
# Optional verbose output
if self.verbose:
print("\n")
for i in range(len(active_dims)):
print(f"[{i}]: active_dims: {active_dims[i]}")
for i in range(len(D_np)):
print(f"[{i}]: D_np (shape={D_np[i].shape}):\n{D_np[i]}")
print("") # Add a newline for better readability
print_delassus_info(delassus)
print("") # Add a newline for better readability
# For each world, compute the Delassus matrix using numpy and
# compare it with the one from the Delassus operator class
num_worlds = delassus.num_worlds
for w in range(num_worlds):
# Create reference
D_w = np.eye(active_dims[w], dtype=np.float32)
# Compare the computed Delassus matrix with the one from the dual problem
is_D_close = np.allclose(D_np[w], D_w, atol=1e-3, rtol=1e-4)
if not is_D_close or self.verbose:
print(f"[{w}]: D_w (shape={D_w.shape}):\n{D_w}")
print(f"[{w}]: D_np (shape={D_np[w].shape}):\n{D_np[w]}")
print_error_stats(f"D[{w}]", D_np[w], D_w, active_dims[w])
self.assertTrue(is_D_close)
def test_08_delassus_operator_factorize_and_solve_with_sequential_cholesky(self):
"""
Tests the factorization of a Delassus matrix and solving linear
systems with randomly generated right-hand-side vectors.
"""
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder()
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
if self.verbose:
print("") # Print a newline for better readability
print(f"model.info.num_joint_cts: {model.info.num_joint_cts}")
print(f"limits.data.world_max_limits: {limits.data.world_max_limits}")
print(f"limits.data.world_active_limits: {limits.data.world_active_limits}")
print(f"contacts.data.world_max_contacts: {detector.contacts.data.world_max_contacts}")
print(f"contacts.data.world_active_contacts: {detector.contacts.data.world_active_contacts}")
print(f"data.info.num_total_cts: {data.info.num_total_cts}")
print("") # Print a newline for better readability
# Create the Delassus operator
delassus = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Build the Delassus operator from the current data
delassus.build(model=model, data=data, jacobians=jacobians, reset_to_zero=True)
# Extract the active constraint dimensions
active_dims = extract_active_constraint_dims(data)
# Add some regularization to the Delassus matrix to ensure it is positive definite
eta = 10.0 # TODO: investigate why this has to be so large
eta_wp = wp.full(
shape=(delassus._model_maxdims,), value=wp.float32(eta), dtype=wp.float32, device=self.default_device
)
delassus.regularize(eta=eta_wp)
# Factorize the Delassus matrix
delassus.compute(reset_to_zero=True)
# Extract Delassus data as numpy arrays
D_np = extract_delassus(delassus, only_active_dims=True)
# For each world, generate a random right-hand side vector
num_worlds = delassus.num_worlds
vio_np = delassus.info.vio.numpy()
v_f_np = np.zeros(shape=(delassus._model_maxdims,), dtype=np.float32)
for w in range(num_worlds):
v_f_w = random_rhs_for_matrix(D_np[w])
v_f_np[vio_np[w] : vio_np[w] + v_f_w.size] = v_f_w
# Construct a warp array for the free-velocity and solution vectors
v_f_wp = wp.array(v_f_np, dtype=wp.float32, device=self.default_device)
x_wp = wp.zeros(shape=(delassus._model_maxdims,), dtype=wp.float32, device=self.default_device)
# Solve the linear system using the Delassus operator
delassus.solve(v=v_f_wp, x=x_wp)
# Extract free-velocity and solution vectors lists of numpy arrays
v_f_np = extract_problem_vector(delassus, vector=v_f_wp.numpy(), only_active_dims=True)
x_wp_np = extract_problem_vector(delassus, vector=x_wp.numpy(), only_active_dims=True)
# For each world, solve the linear system using numpy
x_np: list[np.ndarray] = []
for w in range(num_worlds):
x_np.append(np.linalg.solve(D_np[w][: active_dims[w], : active_dims[w]], v_f_np[w]))
# Optional verbose output
if self.verbose:
for i in range(len(active_dims)):
print(f"[{i}]: active_dims: {active_dims[i]}")
for i in range(len(D_np)):
print(f"[{i}]: D_np (shape={D_np[i].shape}):\n{D_np[i]}")
for w in range(num_worlds):
print(f"[{w}]: v_f_np: {v_f_np[w]}")
for w in range(num_worlds):
print(f"[{w}]: x_np: {x_np[w]}")
print(f"[{w}]: x_wp: {x_wp_np[w]}")
print("") # Add a newline for better readability
print_delassus_info(delassus)
print("") # Add a newline for better readability
# For each world, compare the numpy and DelassusOperator solutions
for w in range(num_worlds):
# Compare the reconstructed solution vector with the one computed using numpy
is_x_close = np.allclose(x_wp_np[w], x_np[w], atol=1e-3, rtol=1e-4)
if not is_x_close or self.verbose:
print_error_stats(f"x[{w}]", x_wp_np[w], x_np[w], active_dims[w])
self.assertTrue(is_x_close)
def test_09_compare_dense_sparse_delassus_operator_assembly(self):
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
# builder = build_boxes_hinged(z_offset=0.0, ground=False)
builder = build_boxes_fourbar(z_offset=0.0, ground=False)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians_dense = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=False
)
jacobians_sparse = SparseSystemJacobians(
model=model, limits=limits, contacts=detector.contacts, device=self.default_device
)
# Update the containers
update_containers(model, data, state, limits, detector, jacobians_dense)
update_containers(model, data, state, limits, detector, jacobians_sparse)
# Create the Delassus operator
delassus_dense = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
delassus_sparse = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Build the Delassus operator from the current data
delassus_dense.build(model=model, data=data, jacobians=jacobians_dense, reset_to_zero=True)
delassus_sparse.build(model=model, data=data, jacobians=jacobians_sparse, reset_to_zero=True)
# Extract the active constraint dimensions
active_dims = extract_active_constraint_dims(data)
active_size = [dims * dims for dims in active_dims]
# Extract Delassus data as numpy arrays
D_dense_np = extract_delassus(delassus_dense, only_active_dims=True)
D_sparse_np = extract_delassus(delassus_sparse, only_active_dims=True)
# For each world, compare the Delassus matrix
for w in range(delassus_dense.num_worlds):
# Compare the computed Delassus matrix with the one from the dual problem
is_D_close = np.allclose(D_dense_np[w], D_sparse_np[w], rtol=1e-3, atol=1e-4)
if not is_D_close or self.verbose:
print(f"[{w}]: D_dense_np (shape={D_dense_np[w].shape}):\n{D_dense_np[w]}")
print(f"[{w}]: D_sparse_np (shape={D_sparse_np[w].shape}):\n{D_sparse_np[w]}")
print_error_stats(f"D[{w}]", D_dense_np[w], D_sparse_np[w], active_size[w], show_errors=True)
self.assertTrue(is_D_close)
def test_10_compare_dense_sparse_homogeneous_delassus_operator_assembly(self):
# Model constants
num_worlds = 3
max_world_contacts = 12
# Construct a homogeneous model description using model builders
builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_nunchaku)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians_dense = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=False
)
jacobians_sparse = SparseSystemJacobians(
model=model, limits=limits, contacts=detector.contacts, device=self.default_device
)
# Update the containers
update_containers(model, data, state, limits, detector, jacobians_dense)
update_containers(model, data, state, limits, detector, jacobians_sparse)
# Create the Delassus operator
delassus_dense = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
delassus_sparse = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Build the Delassus operator from the current data
delassus_dense.build(model=model, data=data, jacobians=jacobians_dense, reset_to_zero=True)
delassus_sparse.build(model=model, data=data, jacobians=jacobians_sparse, reset_to_zero=True)
# Extract the active constraint dimensions
active_dims = extract_active_constraint_dims(data)
# Extract Delassus data as numpy arrays
D_dense_np = extract_delassus(delassus_dense, only_active_dims=True)
D_sparse_np = extract_delassus(delassus_sparse, only_active_dims=True)
# For each world, compare the Delassus matrix
for w in range(delassus_dense.num_worlds):
# Compare the computed Delassus matrix with the one from the dual problem
is_D_close = np.allclose(D_dense_np[w], D_sparse_np[w], rtol=1e-3, atol=1e-4)
if not is_D_close or self.verbose:
print(f"[{w}]: D_dense_np (shape={D_dense_np[w].shape}):\n{D_dense_np[w]}")
print(f"[{w}]: D_sparse_np (shape={D_sparse_np[w].shape}):\n{D_sparse_np[w]}")
print_error_stats(f"D[{w}]", D_dense_np[w], D_sparse_np[w], active_dims[w] * active_dims[w])
self.assertTrue(is_D_close)
def test_11_compare_dense_sparse_heterogeneous_delassus_operator_assembly(self):
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder()
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians_dense = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=False
)
jacobians_sparse = SparseSystemJacobians(
model=model, limits=limits, contacts=detector.contacts, device=self.default_device
)
# Update the containers
update_containers(model, data, state, limits, detector, jacobians_dense)
update_containers(model, data, state, limits, detector, jacobians_sparse)
# Create the Delassus operator
delassus_dense = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
delassus_sparse = DelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
solver=LLTSequentialSolver,
)
# Build the Delassus operator from the current data
delassus_dense.build(model=model, data=data, jacobians=jacobians_dense, reset_to_zero=True)
delassus_sparse.build(model=model, data=data, jacobians=jacobians_sparse, reset_to_zero=True)
# Extract the active constraint dimensions
active_dims = extract_active_constraint_dims(data)
# Extract Delassus data as numpy arrays
D_dense_np = extract_delassus(delassus_dense, only_active_dims=True)
D_sparse_np = extract_delassus(delassus_sparse, only_active_dims=True)
# For each world, compare the Delassus matrix
for w in range(delassus_dense.num_worlds):
# Compare the computed Delassus matrix with the one from the dual problem
is_D_close = np.allclose(D_dense_np[w], D_sparse_np[w], rtol=1e-3, atol=1e-4)
if not is_D_close or self.verbose:
print(f"[{w}]: D_dense_np (shape={D_dense_np[w].shape}):\n{D_dense_np[w]}")
print(f"[{w}]: D_sparse_np (shape={D_sparse_np[w].shape}):\n{D_sparse_np[w]}")
print_error_stats(f"D[{w}]", D_dense_np[w], D_sparse_np[w], active_dims[w] * active_dims[w])
self.assertTrue(is_D_close)
class TestDelassusOperatorSparse(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for detailed output
self.default_device = wp.get_device(test_context.device)
self.seed = 42
self.dynamic_joints = True
def tearDown(self):
self.default_device = None
###
# Helpers
###
def _check_sparse_delassus_allocations(
self,
model: ModelKamino,
delassus: BlockSparseMatrixFreeDelassusOperator,
):
"""Checks the allocation of a sparse Delassus operator."""
self.assertEqual(delassus._model, model)
self.assertIsNotNone(delassus._data)
self.assertIsNone(delassus._preconditioner)
self.assertIsNone(delassus._eta)
# Check that body space temp vector is initialized
self.assertEqual(delassus._vec_temp_body_space.shape, (model.size.sum_of_num_body_dofs,))
rng = np.random.default_rng(seed=self.seed)
regularization_np = rng.standard_normal((model.size.sum_of_max_total_cts,), dtype=np.float32)
regularization = wp.from_numpy(regularization_np, dtype=wp.float32, device=self.default_device)
delassus.set_regularization(regularization)
# Check that setting regularization works
self.assertEqual(delassus._eta, regularization)
preconditioner_np = rng.standard_normal((model.size.sum_of_max_total_cts,), dtype=np.float32)
preconditioner = wp.from_numpy(preconditioner_np, dtype=wp.float32, device=self.default_device)
delassus.set_preconditioner(preconditioner)
delassus.update()
# Check that setting preconditioner works
self.assertEqual(delassus._preconditioner, preconditioner)
def _check_delassus_matrix(
self,
model: ModelKamino,
data: DataKamino,
delassus: BlockSparseMatrixFreeDelassusOperator,
jacobians: SparseSystemJacobians,
):
"""Checks that a sparse Delassus operator represents the correct matrix."""
rng = np.random.default_rng(seed=self.seed)
def run_check(use_regularization: bool, use_preconditioner: bool):
# Extract Jacobians as numpy arrays
J_cts_np = jacobians._J_cts.bsm.numpy()
# Add regularization
if use_regularization:
regularization_list: list[np.ndarray] = []
regularization_np = np.zeros((model.size.sum_of_max_total_cts,), dtype=np.float32)
delassus_sizes = data.info.num_total_cts.numpy()
jac_row_start = jacobians._J_cts.bsm.row_start.numpy()
for w in range(model.info.num_worlds):
d_size = delassus_sizes[w]
regularization_list.append(rng.standard_normal((d_size,), dtype=np.float32))
regularization_np[jac_row_start[w] : jac_row_start[w] + d_size] = regularization_list[-1]
regularization = wp.from_numpy(regularization_np, dtype=wp.float32, device=self.default_device)
delassus.set_regularization(regularization)
else:
delassus.set_regularization(None)
# Add preconditioner
if use_preconditioner:
preconditioner_list: list[np.ndarray] = []
preconditioner_np = np.zeros((model.size.sum_of_max_total_cts,), dtype=np.float32)
delassus_sizes = data.info.num_total_cts.numpy()
jac_row_start = jacobians._J_cts.bsm.row_start.numpy()
for w in range(model.info.num_worlds):
d_size = delassus_sizes[w]
preconditioner_list.append(rng.standard_normal((d_size,), dtype=np.float32))
preconditioner_np[jac_row_start[w] : jac_row_start[w] + d_size] = preconditioner_list[-1]
preconditioner = wp.from_numpy(preconditioner_np, dtype=wp.float32, device=self.default_device)
delassus.set_preconditioner(preconditioner)
else:
delassus.set_preconditioner(None)
delassus.update()
# Extract Delassus matrices as numpy arrays
D_np = extract_delassus_sparse(delassus, only_active_dims=True)
# Construct a list of generalized inverse mass matrices of each world
invM_np = make_inverse_generalized_mass_matrices(model, data)
# Construct the joint armature regularization term for each world
active_dims = data.info.num_total_cts.numpy()
num_joint_dynamic_cts = model.info.num_joint_dynamic_cts.numpy()
joint_dynamic_cts_offset = model.info.joint_dynamic_cts_offset.numpy()
inv_M_q_np = [np.zeros(shape=(dim,), dtype=np.float32) for dim in active_dims]
if np.any(num_joint_dynamic_cts):
inv_m_j_np = data.joints.inv_m_j.numpy()
for w in range(model.info.num_worlds):
inv_M_q_np[w][: num_joint_dynamic_cts[w]] = inv_m_j_np[
joint_dynamic_cts_offset[w] : joint_dynamic_cts_offset[w] + num_joint_dynamic_cts[w]
]
# Optional verbose output
if self.verbose:
print("") # Print a newline for better readability
for i in range(len(J_cts_np)):
print(f"[{i}]: J_cts_np (shape={J_cts_np[i].shape}):\n{J_cts_np[i]}")
for i in range(len(invM_np)):
print(f"[{i}]: invM_np (shape={invM_np[i].shape}):\n{invM_np[i]}")
for i in range(len(D_np)):
print(f"[{i}]: D_np (shape={D_np[i].shape}):\n{D_np[i]}")
print("") # Add a newline for better readability
# For each world, compute the Delassus matrix using numpy and compare it with the matrix
# represented by the Delassus operator.
for w in range(model.info.num_worlds):
# Compute the Delassus matrix using the inverse mass matrix and the Jacobian
D_w = (J_cts_np[w] @ invM_np[w]) @ J_cts_np[w].T + np.diag(inv_M_q_np[w])
if use_preconditioner:
D_w = np.diag(preconditioner_list[w]) @ D_w @ np.diag(preconditioner_list[w])
if use_regularization:
D_w = D_w + np.diag(regularization_list[w])
is_D_close = np.allclose(D_np[w], D_w, atol=1e-3, rtol=1e-4)
if not is_D_close or self.verbose:
print(f"[{w}]: D_w (shape={D_w.shape}):\n{D_w}")
print(f"[{w}]: D_np (shape={D_np[w].shape}):\n{D_np[w]}")
print_error_stats(f"D[{w}]", D_np[w], D_w, D_w.shape[0])
self.assertTrue(is_D_close)
run_check(use_regularization=False, use_preconditioner=False)
run_check(use_regularization=True, use_preconditioner=False)
run_check(use_regularization=False, use_preconditioner=True)
run_check(use_regularization=True, use_preconditioner=True)
def _check_delassus_matrix_vector_product(
self,
model: ModelKamino,
data: DataKamino,
delassus: BlockSparseMatrixFreeDelassusOperator,
jacobians: SparseSystemJacobians,
):
"""Checks the different matrix-vector products provided by the sparse Delassus operator."""
rng = np.random.default_rng(seed=self.seed)
def run_check(use_regularization: bool, use_preconditioner: bool, mask_worlds: bool):
delassus_sizes = data.info.num_total_cts.numpy()
jac_row_start = jacobians._J_cts.bsm.row_start.numpy()
# Add regularization
if use_regularization:
regularization_list: list[np.ndarray] = []
regularization_np = np.zeros((model.size.sum_of_max_total_cts,), dtype=np.float32)
for w in range(model.info.num_worlds):
d_size = delassus_sizes[w]
regularization_list.append(rng.standard_normal((d_size,), dtype=np.float32))
regularization_np[jac_row_start[w] : jac_row_start[w] + d_size] = regularization_list[-1]
regularization = wp.from_numpy(regularization_np, dtype=wp.float32, device=self.default_device)
delassus.set_regularization(regularization)
else:
delassus.set_regularization(None)
# Add preconditioner
if use_preconditioner:
preconditioner_list: list[np.ndarray] = []
preconditioner_np = np.zeros((model.size.sum_of_max_total_cts,), dtype=np.float32)
for w in range(model.info.num_worlds):
d_size = delassus_sizes[w]
preconditioner_list.append(rng.standard_normal((d_size,), dtype=np.float32))
preconditioner_np[jac_row_start[w] : jac_row_start[w] + d_size] = preconditioner_list[-1]
preconditioner = wp.from_numpy(preconditioner_np, dtype=wp.float32, device=self.default_device)
delassus.set_preconditioner(preconditioner)
else:
delassus.set_preconditioner(None)
delassus.update()
# Generate vectors for multiplication
alpha = float(rng.standard_normal((1,))[0])
beta = float(rng.standard_normal((1,))[0])
input_vec_list: list[np.ndarray] = []
offset_vec_list: list[np.ndarray] = []
input_vec_np = np.zeros((model.size.sum_of_max_total_cts,), dtype=np.float32)
offset_vec_np = np.zeros((model.size.sum_of_max_total_cts,), dtype=np.float32)
for w in range(model.info.num_worlds):
d_size = delassus_sizes[w]
input_vec_list.append(rng.standard_normal((d_size,), dtype=np.float32))
offset_vec_list.append(rng.standard_normal((d_size,), dtype=np.float32))
input_vec_np[jac_row_start[w] : jac_row_start[w] + d_size] = input_vec_list[-1]
offset_vec_np[jac_row_start[w] : jac_row_start[w] + d_size] = offset_vec_list[-1]
input_vec = wp.from_numpy(input_vec_np, dtype=wp.float32, device=self.default_device)
output_vec_matmul = wp.zeros_like(input_vec)
output_vec_gemv = wp.from_numpy(offset_vec_np, dtype=wp.float32, device=self.default_device)
output_vec_gemv_zero = wp.from_numpy(offset_vec_np, dtype=wp.float32, device=self.default_device)
mask_np = np.ones((model.size.num_worlds,), dtype=np.int32)
if mask_worlds:
mask_np[::2] = 0
world_mask = wp.from_numpy(mask_np, dtype=wp.int32, device=self.default_device)
# Compute different products (simple matvec, gemv, and gemv with beta = 0.0)
delassus.matvec(input_vec, output_vec_matmul, world_mask)
delassus.gemv(input_vec, output_vec_gemv, world_mask, alpha, beta)
delassus.gemv(input_vec, output_vec_gemv_zero, world_mask, alpha, 0.0)
output_vec_matmul_np = output_vec_matmul.numpy()
output_vec_gemv_np = output_vec_gemv.numpy()
output_vec_gemv_zero_np = output_vec_gemv_zero.numpy()
# Extract Jacobians as numpy arrays
J_cts_np = jacobians._J_cts.bsm.numpy()
# Construct a list of generalized inverse mass matrices of each world
invM_np = make_inverse_generalized_mass_matrices(model, data)
# Construct the joint armature regularization term for each world
active_dims = data.info.num_total_cts.numpy()
num_joint_dynamic_cts = model.info.num_joint_dynamic_cts.numpy()
joint_dynamic_cts_offset = model.info.joint_dynamic_cts_offset.numpy()
inv_M_q_np = [np.zeros(shape=(dim,), dtype=np.float32) for dim in active_dims]
if np.any(num_joint_dynamic_cts):
inv_m_j_np = data.joints.inv_m_j.numpy()
for w in range(model.info.num_worlds):
inv_M_q_np[w][: num_joint_dynamic_cts[w]] = inv_m_j_np[
joint_dynamic_cts_offset[w] : joint_dynamic_cts_offset[w] + num_joint_dynamic_cts[w]
]
# For each world, compute the Delassus matrix-vector product using numpy and compare it
# with the one from the Delassus operator class
for w in range(model.info.num_worlds):
vec_matmul_w = output_vec_matmul_np[jac_row_start[w] : jac_row_start[w] + delassus_sizes[w]]
vec_gemv_w = output_vec_gemv_np[jac_row_start[w] : jac_row_start[w] + delassus_sizes[w]]
vec_gemv_zero_w = output_vec_gemv_zero_np[jac_row_start[w] : jac_row_start[w] + delassus_sizes[w]]
if mask_np[w] == 0:
self.assertEqual(np.max(np.abs(vec_matmul_w)), 0.0)
self.assertTrue((vec_gemv_w == offset_vec_list[w]).all())
self.assertTrue((vec_gemv_zero_w == offset_vec_list[w]).all())
else:
# Compute the Delassus matrix using the inverse mass matrix and the Jacobian
D_w = (J_cts_np[w] @ invM_np[w]) @ J_cts_np[w].T + np.diag(inv_M_q_np[w])
if use_preconditioner:
D_w = np.diag(preconditioner_list[w]) @ D_w @ np.diag(preconditioner_list[w])
if use_regularization:
D_w = D_w + np.diag(regularization_list[w])
vec_matmul_ref = D_w @ input_vec_list[w]
vec_gemv_ref = alpha * (D_w @ input_vec_list[w]) + beta * offset_vec_list[w]
vec_gemv_zero_ref = alpha * (D_w @ input_vec_list[w])
# Compare the computed Delassus matrix with the one from the dual problem
is_matmul_close = np.allclose(vec_matmul_ref, vec_matmul_w, atol=1e-3, rtol=1e-4)
if not is_matmul_close or self.verbose:
print(f"[{w}]: vec_matmul_ref (shape={vec_matmul_ref.shape}):\n{vec_matmul_ref}")
print(f"[{w}]: vec_matmul_w (shape={vec_matmul_w.shape}):\n{vec_matmul_w}")
print_error_stats(
f"vec_gemv_zero[{w}]",
vec_matmul_ref,
vec_matmul_w,
vec_matmul_w.shape[0],
)
self.assertTrue(is_matmul_close)
is_gemv_close = np.allclose(vec_gemv_ref, vec_gemv_w, atol=1e-3, rtol=1e-4)
if not is_gemv_close or self.verbose:
print(f"[{w}]: vec_gemv_ref (shape={vec_gemv_ref.shape}):\n{vec_gemv_ref}")
print(f"[{w}]: vec_gemv_w (shape={vec_gemv_w.shape}):\n{vec_gemv_w}")
print_error_stats(
f"vec_gemv_zero[{w}]",
vec_gemv_ref,
vec_gemv_w,
vec_gemv_w.shape[0],
)
self.assertTrue(is_gemv_close)
is_gemv_zero_close = np.allclose(vec_gemv_zero_ref, vec_gemv_zero_w, atol=1e-3, rtol=1e-4)
if not is_gemv_zero_close or self.verbose:
print(f"[{w}]: vec_gemv_zero_ref (shape={vec_gemv_zero_ref.shape}):\n{vec_gemv_zero_ref}")
print(f"[{w}]: vec_gemv_zero_w (shape={vec_gemv_zero_w.shape}):\n{vec_gemv_zero_w}")
print_error_stats(
f"vec_gemv_zero[{w}]",
vec_gemv_zero_ref,
vec_gemv_zero_w,
vec_gemv_zero_w.shape[0],
)
self.assertTrue(is_gemv_zero_close)
run_check(use_regularization=False, use_preconditioner=False, mask_worlds=False)
run_check(use_regularization=True, use_preconditioner=False, mask_worlds=False)
run_check(use_regularization=False, use_preconditioner=True, mask_worlds=False)
run_check(use_regularization=True, use_preconditioner=True, mask_worlds=False)
run_check(use_regularization=False, use_preconditioner=False, mask_worlds=True)
run_check(use_regularization=True, use_preconditioner=False, mask_worlds=True)
run_check(use_regularization=False, use_preconditioner=True, mask_worlds=True)
run_check(use_regularization=True, use_preconditioner=True, mask_worlds=True)
def _check_delassus_diagonal(
self,
model: ModelKamino,
data: DataKamino,
delassus: BlockSparseMatrixFreeDelassusOperator,
jacobians: SparseSystemJacobians,
):
"""Check the diagonal extraction routine of the sparse Delassus operator."""
# Extract Jacobians as numpy arrays
J_cts_np = jacobians._J_cts.bsm.numpy()
# Get diagonals from the sparse Delassus operator and split the single array into separate
# numpy vectors
D_diag = wp.zeros((model.size.sum_of_max_total_cts,), dtype=wp.float32, device=self.default_device)
delassus.diagonal(D_diag)
D_diag_np = D_diag.numpy()
row_start_np = jacobians._J_cts.bsm.row_start.numpy()
num_total_cts_np = data.info.num_total_cts.numpy()
max_total_cts_np = model.info.max_total_cts.numpy()
D_diag_list = []
for w in range(model.info.num_worlds):
D_diag_list.append(D_diag_np[row_start_np[w] : row_start_np[w] + num_total_cts_np[w]])
if max_total_cts_np[w] > num_total_cts_np[w]:
# Check that unused entries of the diagonal vector are zero
diag_unused = D_diag_np[row_start_np[w] + num_total_cts_np[w] : row_start_np[w] + max_total_cts_np[w]]
self.assertEqual(np.max(np.abs(diag_unused)), 0)
# Construct a list of generalized inverse mass matrices of each world
invM_np = make_inverse_generalized_mass_matrices(model, data)
# Construct the joint armature regularization term for each world
active_dims = data.info.num_total_cts.numpy()
num_joint_dynamic_cts = model.info.num_joint_dynamic_cts.numpy()
joint_dynamic_cts_offset = model.info.joint_dynamic_cts_offset.numpy()
inv_M_q_np = [np.zeros(shape=(dim,), dtype=np.float32) for dim in active_dims]
if np.any(num_joint_dynamic_cts):
inv_m_j_np = data.joints.inv_m_j.numpy()
for w in range(model.info.num_worlds):
inv_M_q_np[w][: num_joint_dynamic_cts[w]] = inv_m_j_np[
joint_dynamic_cts_offset[w] : joint_dynamic_cts_offset[w] + num_joint_dynamic_cts[w]
]
# Optional verbose output
if self.verbose:
print("") # Print a newline for better readability
for i in range(len(J_cts_np)):
print(f"[{i}]: J_cts_np (shape={J_cts_np[i].shape}):\n{J_cts_np[i]}")
for i in range(len(invM_np)):
print(f"[{i}]: invM_np (shape={invM_np[i].shape}):\n{invM_np[i]}")
for i in range(len(D_diag_list)):
print(f"[{i}]: D_diag (shape={D_diag_list[i].shape}):\n{D_diag_list[i]}")
print("") # Add a newline for better readability
# For each world, compute the Delassus matrix diagonal using numpy and compare it with the
# one from the Delassus operator class
for w in range(model.info.num_worlds):
# Compute the Delassus matrix diagonal using the inverse mass matrix and the Jacobian
D_w = (J_cts_np[w] @ invM_np[w]) @ J_cts_np[w].T + np.diag(inv_M_q_np[w])
D_diag_ref = np.diag(D_w)
# Compare the computed Delassus matrix diagonals
is_D_diag_close = np.allclose(D_diag_list[w], D_diag_ref, atol=1e-3, rtol=1e-4)
if not is_D_diag_close or self.verbose:
print(f"[{w}]: D_diag_ref (shape={D_diag_ref.shape}):\n{D_diag_ref}")
print(f"[{w}]: D_diag (shape={D_diag_list[w].shape}):\n{D_diag_list[w]}")
print_error_stats(f"D_diag[{w}]", D_diag_list[w], D_diag_ref, D_diag_ref.shape[0])
self.assertTrue(is_D_diag_close)
###
# Allocation
###
def test_01_allocate_single_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
builder = build_boxes_nunchaku()
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the sparse Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
)
# Compare expected to allocated dimensions and sizes
self._check_sparse_delassus_allocations(model, delassus)
def test_02_allocate_homogeneous_delassus_operator(self):
# Model constants
num_worlds = 3
max_world_contacts = 12
# Construct a homogeneous model description using model builders
builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_nunchaku)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
)
# Compare expected to allocated dimensions and sizes
self._check_sparse_delassus_allocations(model, delassus)
def test_03_allocate_heterogeneous_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder()
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
device=self.default_device,
)
# Compare expected to allocated dimensions and sizes
self._check_sparse_delassus_allocations(model, delassus)
def test_04_build_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
builder = build_boxes_fourbar(
z_offset=0.0, ground=False, dynamic_joints=self.dynamic_joints, implicit_pd=self.dynamic_joints
)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_matrix(model, data, delassus, jacobians)
def test_05_build_homogeneous_delassus_operator(self):
# Model constants
num_worlds = 3
max_world_contacts = 12
# Construct a homogeneous model description using model builders
builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_nunchaku)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_matrix(model, data, delassus, jacobians)
def test_06_build_heterogeneous_delassus_operator(self):
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder(dynamic_joints=self.dynamic_joints, implicit_pd=self.dynamic_joints)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_matrix(model, data, delassus, jacobians)
def test_07_extract_delassus_diagonal(self):
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
builder = build_boxes_fourbar(
z_offset=0.0, ground=False, dynamic_joints=self.dynamic_joints, implicit_pd=self.dynamic_joints
)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_diagonal(model, data, delassus, jacobians)
def test_08_extract_delassus_diagonal_homogeneous(self):
# Model constants
num_worlds = 3
max_world_contacts = 12
# Construct a homogeneous model description using model builders
builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_nunchaku)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_diagonal(model, data, delassus, jacobians)
def test_09_extract_delassus_diagonal_heterogeneous(self):
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder(dynamic_joints=self.dynamic_joints, implicit_pd=self.dynamic_joints)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_diagonal(model, data, delassus, jacobians)
def test_10_delassus_operator_vector_product(self):
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
builder = build_boxes_fourbar(
z_offset=0.0, ground=False, dynamic_joints=self.dynamic_joints, implicit_pd=self.dynamic_joints
)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_matrix_vector_product(model, data, delassus, jacobians)
def test_11_homogeneous_delassus_operator_vector_product(self):
# Model constants
num_worlds = 3
max_world_contacts = 12
# Construct a homogeneous model description using model builders
builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_nunchaku)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_matrix_vector_product(model, data, delassus, jacobians)
def test_12_heterogeneous_delassus_operator_vector_product(self):
# Model constants
max_world_contacts = 12
# Create a heterogeneous model description using model builders
builder = make_basics_heterogeneous_builder(dynamic_joints=self.dynamic_joints, implicit_pd=self.dynamic_joints)
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device, sparse=True
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
# Create the Delassus operator
delassus = BlockSparseMatrixFreeDelassusOperator(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
jacobians=jacobians,
device=self.default_device,
)
# Check that the Delassus operator represents the actual Delassus matrix
self._check_delassus_matrix_vector_product(model, data, delassus, jacobians)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_dynamics_dual.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: UNIT TESTS: DYNAMICS: DUAL PROBLEM
"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.dynamics.dual import DualProblem
from newton._src.solvers.kamino._src.linalg import ConjugateGradientSolver
from newton._src.solvers.kamino._src.models.builders.basics import make_basics_heterogeneous_builder
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import extract_problem_vector
from newton._src.solvers.kamino.tests.utils.make import make_containers, update_containers
from newton._src.solvers.kamino.tests.utils.print import print_model_info
###
# Tests
###
class TestDualProblem(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for detailed output
self.default_device = wp.get_device(test_context.device)
def tearDown(self):
self.default_device = None
def test_01_allocate_dual_problem(self):
"""
Tests the allocation of a DualProblem data members.
"""
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
builder = make_basics_heterogeneous_builder()
# Create the model and containers from the builder
model, data, _state, limits, detector, _jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Create the Delassus operator
problem = DualProblem(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
solver=ConjugateGradientSolver,
device=self.default_device,
)
# Optional verbose output
if self.verbose:
print("\n") # Print a blank line for better readability
print(f"problem.data.config: {problem.data.config}")
print(f"problem.data.maxdim: {problem.data.maxdim}")
print(f"problem.data.dim: {problem.data.dim}")
print(f"problem.data.mio: {problem.data.mio}")
print(f"problem.data.vio: {problem.data.vio}")
print(f"problem.data.u_f (shape): {problem.data.u_f.shape}")
print(f"problem.data.v_b (shape): {problem.data.v_b.shape}")
print(f"problem.data.v_i (shape): {problem.data.v_i.shape}")
print(f"problem.data.v_f (shape): {problem.data.v_f.shape}")
print(f"problem.data.mu (shape): {problem.data.mu.shape}")
print(f"problem.data.D (shape): {problem.data.D.shape}")
# Extract expected allocation sizes
nw = model.info.num_worlds
nb = model.size.sum_of_num_bodies
maxnl = limits.model_max_limits_host
maxnc = detector.contacts.model_max_contacts_host
maxdims = model.size.sum_of_num_joint_cts + maxnl + 3 * maxnc
# Check allocations
self.assertEqual(problem.data.config.size, nw)
self.assertEqual(problem.data.maxdim.size, nw)
self.assertEqual(problem.data.dim.size, nw)
if not problem.sparse:
self.assertEqual(problem.data.mio.size, nw)
self.assertEqual(problem.data.vio.size, nw)
self.assertEqual(problem.data.u_f.size, nb)
self.assertEqual(problem.data.v_b.size, maxdims)
self.assertEqual(problem.data.v_i.size, maxdims)
self.assertEqual(problem.data.v_f.size, maxdims)
self.assertEqual(problem.data.mu.size, maxnc)
maxdim_np = problem.data.maxdim.numpy()
self.assertEqual(int(np.sum(maxdim_np)), maxdims)
dim_np = problem.data.dim.numpy()
self.assertEqual(int(np.sum(dim_np)), 46) # Total number of active constraints in the default state
def test_02_dual_problem_build(self):
"""
Tests building the dual problem from time-varying data.
"""
# Model constants
max_world_contacts = 12
# Construct the model description using model builders for different systems
builder = make_basics_heterogeneous_builder()
num_worlds = builder.num_worlds
# Create the model and containers from the builder
model, data, state, limits, detector, jacobians = make_containers(
builder=builder, max_world_contacts=max_world_contacts, device=self.default_device
)
# Update the containers
update_containers(model=model, data=data, state=state, limits=limits, detector=detector, jacobians=jacobians)
if self.verbose:
print_model_info(model)
# Create the Delassus operator
problem = DualProblem(
model=model,
data=data,
limits=limits,
contacts=detector.contacts,
solver=ConjugateGradientSolver,
device=self.default_device,
)
# Build the dual problem
problem.build(model=model, data=data, limits=limits, contacts=detector.contacts, jacobians=jacobians)
# Extract numpy arrays from the problem data
v_b_wp_np = problem.data.v_b.numpy()
v_i_wp_np = problem.data.v_i.numpy()
v_f_wp_np = problem.data.v_f.numpy()
# Extract free-velocity and solution vectors lists of numpy arrays
v_b_np = extract_problem_vector(problem.delassus, vector=v_b_wp_np, only_active_dims=True)
v_i_np = extract_problem_vector(problem.delassus, vector=v_i_wp_np, only_active_dims=True)
v_f_np = extract_problem_vector(problem.delassus, vector=v_f_wp_np, only_active_dims=True)
# Optional verbose output
if self.verbose:
print("") # Print a blank line for better readability
print(f"problem.data.maxdim: {problem.data.maxdim}")
print(f"problem.data.dim: {problem.data.dim}")
print(f"problem.data.mio: {problem.data.mio}")
print(f"problem.data.vio: {problem.data.vio}")
print(f"problem.data.D: {problem.data.D.shape}")
print(f"problem.data.u_f:\n{problem.data.u_f}")
print(f"problem.data.v_b:\n{problem.data.v_b}")
print(f"problem.data.v_i:\n{problem.data.v_i}")
print(f"problem.data.v_f:\n{problem.data.v_f}")
print(f"problem.data.mu:\n{problem.data.mu}")
for w in range(num_worlds):
print(f"problem.data.v_b[{w}]:\n{v_b_np[w]}")
for w in range(num_worlds):
print(f"problem.data.v_i[{w}]:\n{v_i_np[w]}")
for w in range(num_worlds):
print(f"problem.data.v_f[{w}]:\n{v_f_np[w]}")
# Check the problem data
# TODO
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_dynamics_wrenches.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Unit tests for `dynamics/wrenches.py`.
"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.data import DataKamino
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.dynamics.wrenches import (
compute_constraint_body_wrenches_dense,
compute_constraint_body_wrenches_sparse,
compute_joint_dof_body_wrenches_dense,
compute_joint_dof_body_wrenches_sparse,
)
from newton._src.solvers.kamino._src.geometry.contacts import ContactsKamino
from newton._src.solvers.kamino._src.kinematics.jacobians import DenseSystemJacobians, SparseSystemJacobians
from newton._src.solvers.kamino._src.kinematics.limits import LimitsKamino
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import (
extract_active_constraint_vectors,
extract_actuation_forces,
extract_cts_jacobians,
extract_dofs_jacobians,
)
from newton._src.solvers.kamino.tests.utils.make import (
make_constraint_multiplier_arrays,
make_test_problem_fourbar,
make_test_problem_heterogeneous,
)
###
# Constants
###
test_jacobian_rtol = 1e-7
test_jacobian_atol = 1e-7
# TODO: FIX THIS: sparse-dense differences are larger than expected,
# likely due to the sparse implementation not fully matching the dense
test_wrench_rtol = 1e-4 # TODO: Should be 1e-6
test_wrench_atol = 1e-4 # TODO: Should be 1e-6
###
# Helper Functions
###
def compute_and_compare_dense_sparse_jacobian_wrenches(
model: ModelKamino,
data: DataKamino,
limits: LimitsKamino,
contacts: ContactsKamino,
):
# Create the Jacobians container
jacobians_dense = DenseSystemJacobians(model=model, limits=limits, contacts=contacts)
jacobians_sparse = SparseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the system Jacobians
jacobians_dense.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
jacobians_sparse.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Create arrays for the constraint multipliers and initialize them
lambdas_start, lambdas = make_constraint_multiplier_arrays(model)
lambdas.fill_(1.0)
# Initialize the generalized joint actuation forces
data.joints.tau_j.fill_(1.0)
# Compute the wrenches using the dense Jacobians
compute_joint_dof_body_wrenches_dense(
model=model,
data=data,
jacobians=jacobians_dense,
reset_to_zero=True,
)
compute_constraint_body_wrenches_dense(
model=model,
data=data,
jacobians=jacobians_dense,
lambdas_offsets=lambdas_start,
lambdas_data=lambdas,
limits=limits.data,
contacts=contacts.data,
reset_to_zero=True,
)
wp.synchronize()
w_a_i_dense_np = data.bodies.w_a_i.numpy().copy()
w_j_i_dense_np = data.bodies.w_j_i.numpy().copy()
w_l_i_dense_np = data.bodies.w_l_i.numpy().copy()
w_c_i_dense_np = data.bodies.w_c_i.numpy().copy()
# Compute the wrenches using the sparse Jacobians
compute_joint_dof_body_wrenches_sparse(
model=model,
data=data,
jacobians=jacobians_sparse,
reset_to_zero=True,
)
compute_constraint_body_wrenches_sparse(
model=model,
data=data,
jacobians=jacobians_sparse,
lambdas_offsets=lambdas_start,
lambdas_data=lambdas,
reset_to_zero=True,
)
wp.synchronize()
w_a_i_sparse_np = data.bodies.w_a_i.numpy().copy()
w_j_i_sparse_np = data.bodies.w_j_i.numpy().copy()
w_l_i_sparse_np = data.bodies.w_l_i.numpy().copy()
w_c_i_sparse_np = data.bodies.w_c_i.numpy().copy()
# TODO
np.set_printoptions(precision=12, suppress=True, linewidth=20000, threshold=20000)
# Extract the number of bodies and constraints for each world
num_bodies_np = model.info.num_bodies.numpy().astype(int).tolist()
num_joint_cts_np = model.info.num_joint_cts.numpy().astype(int).tolist()
num_limit_cts_np = data.info.num_limit_cts.numpy().astype(int).tolist()
num_contact_cts_np = data.info.num_contact_cts.numpy().astype(int).tolist()
num_total_cts_np = data.info.num_total_cts.numpy().astype(int).tolist()
msg.info("num_bodies_np: %s", num_bodies_np)
msg.info("num_joint_cts_np: %s", num_joint_cts_np)
msg.info("num_limit_cts_np: %s", num_limit_cts_np)
msg.info("num_contact_cts_np: %s", num_contact_cts_np)
msg.info("num_total_cts_np: %s\n", num_total_cts_np)
# Extract the Jacobians and constraint multipliers as lists of numpy arrays (i.e. per world)
J_cts_dense = extract_cts_jacobians(model, limits, contacts, jacobians_dense, only_active_cts=True)
J_dofs_dense = extract_dofs_jacobians(model, jacobians_dense)
J_cts_sparse = jacobians_sparse._J_cts.bsm.numpy()
J_dofs_sparse = jacobians_sparse._J_dofs.bsm.numpy()
lambdas_np = extract_active_constraint_vectors(model, data, lambdas)
tau_j_np = extract_actuation_forces(model, data)
for w in range(model.size.num_worlds):
msg.info("[world='%d']: J_cts_dense:\n%s", w, J_cts_dense[w])
msg.info("[world='%d']: J_cts_sparse:\n%s\n", w, J_cts_sparse[w])
msg.info("[world='%d']: lambdas_np:\n%s\n\n", w, lambdas_np[w])
msg.info("[world='%d']: J_dofs_dense:\n%s", w, J_dofs_dense[w])
msg.info("[world='%d']: J_dofs_sparse:\n%s\n", w, J_dofs_sparse[w])
msg.info("[world='%d']: tau_j_np:\n%s\n", w, tau_j_np[w])
# Compute the wrenches manually using the extracted Jacobians and multipliers/forces for additional verification
inv_dt_np = model.time.inv_dt.numpy().tolist()
w_a_i_ref_np = [np.zeros((num_bodies_np[w], 6), dtype=np.float32) for w in range(model.size.num_worlds)]
w_j_i_ref_np = [np.zeros((num_bodies_np[w], 6), dtype=np.float32) for w in range(model.size.num_worlds)]
w_l_i_ref_np = [np.zeros((num_bodies_np[w], 6), dtype=np.float32) for w in range(model.size.num_worlds)]
w_c_i_ref_np = [np.zeros((num_bodies_np[w], 6), dtype=np.float32) for w in range(model.size.num_worlds)]
for w in range(model.size.num_worlds):
joint_cts_start_w = 0
joint_cts_end_w = num_joint_cts_np[w]
limit_cts_start_w = joint_cts_end_w
limit_cts_end_w = limit_cts_start_w + num_limit_cts_np[w]
contact_cts_start_w = limit_cts_end_w
contact_cts_end_w = contact_cts_start_w + num_contact_cts_np[w]
J_cts_j = J_cts_dense[w][joint_cts_start_w:joint_cts_end_w, :]
J_cts_l = J_cts_dense[w][limit_cts_start_w:limit_cts_end_w, :]
J_cts_c = J_cts_dense[w][contact_cts_start_w:contact_cts_end_w, :]
lambdas_j = lambdas_np[w][joint_cts_start_w:joint_cts_end_w]
lambdas_l = lambdas_np[w][limit_cts_start_w:limit_cts_end_w]
lambdas_c = lambdas_np[w][contact_cts_start_w:contact_cts_end_w]
w_a_i_ref_np[w][:, :] = (J_dofs_dense[w].T @ tau_j_np[w]).reshape(num_bodies_np[w], 6)
w_j_i_ref_np[w][:, :] = inv_dt_np[w] * (J_cts_j.T @ lambdas_j).reshape(num_bodies_np[w], 6)
w_l_i_ref_np[w][:, :] = inv_dt_np[w] * (J_cts_l.T @ lambdas_l).reshape(num_bodies_np[w], 6)
w_c_i_ref_np[w][:, :] = inv_dt_np[w] * (J_cts_c.T @ lambdas_c).reshape(num_bodies_np[w], 6)
w_a_i_ref_np = wp.array(np.concatenate(w_a_i_ref_np, axis=0), device="cpu")
w_j_i_ref_np = wp.array(np.concatenate(w_j_i_ref_np, axis=0), device="cpu")
w_l_i_ref_np = wp.array(np.concatenate(w_l_i_ref_np, axis=0), device="cpu")
w_c_i_ref_np = wp.array(np.concatenate(w_c_i_ref_np, axis=0), device="cpu")
# Debug output
msg.info("w_a_i_ref_np:\n%s", w_a_i_ref_np)
msg.info("w_a_i_dense_np:\n%s", w_a_i_dense_np)
msg.info("w_a_i_sparse_np:\n%s\n", w_a_i_sparse_np)
msg.info("w_j_i_ref_np:\n%s", w_j_i_ref_np)
msg.info("w_j_i_dense_np:\n%s", w_j_i_dense_np)
msg.info("w_j_i_sparse_np:\n%s\n", w_j_i_sparse_np)
msg.info("w_l_i_ref_np:\n%s", w_l_i_ref_np)
msg.info("w_l_i_dense_np:\n%s", w_l_i_dense_np)
msg.info("w_l_i_sparse_np:\n%s\n", w_l_i_sparse_np)
msg.info("w_c_i_ref_np:\n%s", w_c_i_ref_np)
msg.info("w_c_i_dense_np:\n%s", w_c_i_dense_np)
msg.info("w_c_i_sparse_np:\n%s\n\n", w_c_i_sparse_np)
# Check that the Jacobians computed using the dense and sparse implementations are close
for w in range(model.size.num_worlds):
np.testing.assert_allclose(J_cts_sparse[w], J_cts_dense[w], rtol=test_jacobian_rtol, atol=test_jacobian_atol)
np.testing.assert_allclose(J_dofs_sparse[w], J_dofs_dense[w], rtol=test_jacobian_rtol, atol=test_jacobian_atol)
# Check that the wrenches computed using the dense Jacobians match the reference wrenches
np.testing.assert_allclose(w_a_i_dense_np, w_a_i_ref_np, rtol=test_wrench_rtol, atol=test_wrench_atol)
np.testing.assert_allclose(w_j_i_dense_np, w_j_i_ref_np, rtol=test_wrench_rtol, atol=test_wrench_atol)
np.testing.assert_allclose(w_l_i_dense_np, w_l_i_ref_np, rtol=test_wrench_rtol, atol=test_wrench_atol)
np.testing.assert_allclose(w_c_i_dense_np, w_c_i_ref_np, rtol=test_wrench_rtol, atol=test_wrench_atol)
# Check that the wrenches computed using the dense and sparse Jacobians are close
np.testing.assert_allclose(w_a_i_sparse_np, w_a_i_dense_np, rtol=test_wrench_rtol, atol=test_wrench_atol)
np.testing.assert_allclose(w_j_i_sparse_np, w_j_i_dense_np, rtol=test_wrench_rtol, atol=test_wrench_atol)
np.testing.assert_allclose(w_l_i_sparse_np, w_l_i_dense_np, rtol=test_wrench_rtol, atol=test_wrench_atol)
np.testing.assert_allclose(w_c_i_sparse_np, w_c_i_dense_np, rtol=test_wrench_rtol, atol=test_wrench_atol)
###
# Tests
###
class TestDynamicsWrenches(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set info-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_compute_wrenches_for_single_fourbar_with_limits_and_contacts(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=True,
with_contacts=True,
with_implicit_joints=True,
verbose=False, # TODO
)
# Compute and compare the wrenches using the dense and sparse Jacobians
compute_and_compare_dense_sparse_jacobian_wrenches(
model=model,
data=data,
limits=limits,
contacts=contacts,
)
def test_02_compute_wrenches_for_multiple_fourbars_with_limits_and_contacts(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=3,
with_limits=True,
with_contacts=True,
with_implicit_joints=True,
verbose=False,
)
# Compute and compare the wrenches using the dense and sparse Jacobians
compute_and_compare_dense_sparse_jacobian_wrenches(
model=model,
data=data,
limits=limits,
contacts=contacts,
)
def test_03_compute_wrenches_heterogeneous_model_with_limits_and_contacts(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_heterogeneous(
device=self.default_device,
max_world_contacts=12,
with_limits=True,
with_contacts=True,
with_implicit_joints=True,
verbose=False,
)
# Compute and compare the wrenches using the dense and sparse Jacobians
compute_and_compare_dense_sparse_jacobian_wrenches(
model=model,
data=data,
limits=limits,
contacts=contacts,
)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_geometry_aggregation.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for `geometry/aggregation.py`"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.geometry import CollisionDetector, ContactAggregation, ContactMode
from newton._src.solvers.kamino._src.models.builders import basics
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestContactAggregation(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
# self.verbose = test_context.verbose # Set to True for detailed output
self.verbose = True # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
self.build_func = basics.build_boxes_nunchaku
self.expected_contacts = 9 # NOTE: specialized to build_boxes_nunchaku
msg.debug(f"build_func: {self.build_func.__name__}")
msg.debug(f"expected_contacts: {self.expected_contacts}")
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_contact_aggregation(self):
"""
Test the collision detector with the primitive pipeline
on multiple worlds containing boxes_nunchaku model.
"""
# Create and set up a model builder
builder = make_homogeneous_builder(num_worlds=3, build_fn=self.build_func)
model = builder.finalize(self.default_device)
data = model.data()
state = model.state()
# Create a collision detector with primitive pipeline
config = CollisionDetector.Config(
pipeline="primitive",
broadphase="explicit",
bvtype="aabb",
)
detector = CollisionDetector(model=model, config=config)
self.assertIs(detector.device, self.default_device)
# Create a contacts aggregator
aggregator = ContactAggregation(model=model, contacts=detector.contacts)
# Run collision detection and aggregate per-body and per-geom contacts
detector.collide(data, state)
# Set contact reactions to known values for testing aggregation results
model_active_nc = int(detector.contacts.model_active_contacts.numpy()[0])
world_body_start = model.info.bodies_offset.numpy()
wid_np = detector.contacts.wid.numpy()
bid_AB_np = detector.contacts.bid_AB.numpy()
contact_reaction_np = detector.contacts.reaction.numpy().copy()
for c in range(model_active_nc):
cwid = int(wid_np[c])
bid_A = max(-1, int(bid_AB_np[c, 0]) - world_body_start[cwid])
bid_B = max(-1, int(bid_AB_np[c, 1]) - world_body_start[cwid])
msg.info(f"Contact {c}: wid={cwid}, bid_A={bid_A}, bid_B={bid_B}")
# First nunchaku box body
if bid_B == 0 and bid_A == -1:
contact_reaction_np[c, :] = np.array([1.0, 0.0, 0.0]) # Force in +x direction
# Second nunchaku box body
elif bid_B == 1 and bid_A == -1:
contact_reaction_np[c, :] = np.array([0.0, 1.0, 0.0]) # Force in +y direction
# Third nunchaku box body
elif bid_B == 2 and bid_A == -1:
contact_reaction_np[c, :] = np.array([0.0, 0.0, 1.0]) # Force in +z direction
detector.contacts.reaction.assign(contact_reaction_np)
msg.info("contacts.reaction:\n%s", detector.contacts.reaction.numpy())
# Enable the mode of active contacts to ensure all contacts are processed by the aggregator
contact_mode_np = detector.contacts.mode.numpy().copy()
contact_mode_np[:model_active_nc] = ContactMode.STICKING # Set mode to STICKING (1) for all active contacts
detector.contacts.mode.assign(contact_mode_np)
# Aggregate per-body and per-geom contacts
aggregator.compute()
# Optional debug output
msg.info("aggregator.body_net_force:\n%s", aggregator.body_net_force)
msg.info("aggregator.body_contact_flag:\n%s", aggregator.body_contact_flag)
msg.info("aggregator.body_static_contact_flag:\n%s", aggregator.body_static_contact_flag)
msg.info("aggregator.geom_net_force:\n%s", aggregator.geom_net_force)
msg.info("aggregator.geom_contact_flag:\n%s", aggregator.geom_contact_flag)
# Test results: check that the aggregated net forces on bodies and
# geoms match expected values based on the contact reactions we set
body_net_force_np = aggregator.body_net_force.numpy()
body_contact_flag_np = aggregator.body_contact_flag.numpy()
geom_net_force_np = aggregator.geom_net_force.numpy()
geom_contact_flag_np = aggregator.geom_contact_flag.numpy()
for w in range(model.info.num_worlds):
for b in range(model.size.max_of_num_bodies):
if b == 0:
expected_force = np.array([4.0, 0.0, 0.0]) # First box body should have +x force
expected_flag = 1
elif b == 1:
expected_force = np.array([0.0, 1.0, 0.0]) # Second box body should have +y force
expected_flag = 1
elif b == 2:
expected_force = np.array([0.0, 0.0, 4.0]) # Third box body should have +z force
expected_flag = 1
np.testing.assert_allclose(
actual=body_net_force_np[w, b],
desired=expected_force,
err_msg=f"World {w} Body {b} net force mismatch",
)
self.assertEqual(
body_contact_flag_np[w, b], expected_flag, msg=f"World {w} Body {b} contact flag mismatch"
)
for g in range(model.size.max_of_num_geoms):
if g == 0:
expected_force = np.array([4.0, 0.0, 0.0]) # First box body should have +x force
expected_flag = 1
elif g == 1:
expected_force = np.array([0.0, 1.0, 0.0]) # Second box body should have +y force
expected_flag = 1
elif g == 2:
expected_force = np.array([0.0, 0.0, 4.0]) # Third box body should have +z force
expected_flag = 1
else:
continue # Skip world (i.e. static) geoms
np.testing.assert_allclose(
actual=geom_net_force_np[w, g],
desired=expected_force,
err_msg=f"World {w} Geom {g} net force mismatch",
)
self.assertEqual(
geom_contact_flag_np[w, g], expected_flag, msg=f"World {w} Geom {g} contact flag mismatch"
)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_geometry_contacts.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Unit tests for `geometry/contacts.py`.
Tests all components of the ContactsKamino data types and operations.
"""
import unittest
import numpy as np
import warp as wp
import newton
from newton._src.sim import ModelBuilder
from newton._src.sim.contacts import Contacts
from newton._src.solvers.kamino._src.core.types import int32, mat33f, vec3f
from newton._src.solvers.kamino._src.geometry.contacts import (
ContactMode,
ContactsKamino,
convert_contacts_kamino_to_newton,
convert_contacts_newton_to_kamino,
make_contact_frame_xnorm,
make_contact_frame_znorm,
)
from newton._src.solvers.kamino._src.models.builders.basics_newton import build_boxes_nunchaku
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Kernels
###
@wp.kernel
def _compute_contact_frame_znorm(
# Inputs:
normal: wp.array(dtype=vec3f),
# Outputs:
frame: wp.array(dtype=mat33f),
):
tid = wp.tid()
frame[tid] = make_contact_frame_znorm(normal[tid])
@wp.kernel
def _compute_contact_frame_xnorm(
# Inputs:
normal: wp.array(dtype=vec3f),
# Outputs:
frame: wp.array(dtype=mat33f),
):
tid = wp.tid()
frame[tid] = make_contact_frame_xnorm(normal[tid])
@wp.kernel
def _compute_contact_mode(
# Inputs:
velocity: wp.array(dtype=vec3f),
# Outputs:
mode: wp.array(dtype=int32),
):
tid = wp.tid()
mode[tid] = wp.static(ContactMode.make_compute_mode_func())(velocity[tid])
###
# Launchers
###
def compute_contact_frame_znorm(normal: wp.array, frame: wp.array, num_threads: int = 1):
wp.launch(
_compute_contact_frame_znorm,
dim=num_threads,
inputs=[normal],
outputs=[frame],
)
def compute_contact_frame_xnorm(normal: wp.array, frame: wp.array, num_threads: int = 1):
wp.launch(
_compute_contact_frame_xnorm,
dim=num_threads,
inputs=[normal],
outputs=[frame],
)
def compute_contact_mode(velocity: wp.array, mode: wp.array, num_threads: int = 1):
wp.launch(
_compute_contact_mode,
dim=num_threads,
inputs=[velocity],
outputs=[mode],
)
###
# Tests
###
class TestGeometryContactFrames(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.info("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_make_contact_frame_znorm(self):
# Create a normal vectors
test_normals: list[vec3f] = []
# Add normals for which to test contact frame creation
test_normals.append(vec3f(1.0, 0.0, 0.0))
test_normals.append(vec3f(0.0, 1.0, 0.0))
test_normals.append(vec3f(0.0, 0.0, 1.0))
test_normals.append(vec3f(-1.0, 0.0, 0.0))
test_normals.append(vec3f(0.0, -1.0, 0.0))
test_normals.append(vec3f(0.0, 0.0, -1.0))
# Create the input output arrays
normals = wp.array(test_normals, dtype=vec3f)
frames = wp.zeros(shape=(len(test_normals),), dtype=mat33f)
# Compute the contact frames
compute_contact_frame_znorm(normal=normals, frame=frames, num_threads=len(test_normals))
if self.verbose:
print(f"normals:\n{normals}\n")
print(f"frames:\n{frames}\n")
# Extract numpy arrays for comparison
frames_np = frames.numpy()
# Check determinants of each frame
for i in range(len(test_normals)):
det = np.linalg.det(frames_np[i])
self.assertTrue(np.isclose(det, 1.0, atol=1e-6))
# Check each primitive frame
self.assertTrue(
np.allclose(frames_np[0], np.array([[0.0, 0.0, 1.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]), atol=1e-6)
)
self.assertTrue(
np.allclose(frames_np[1], np.array([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, -1.0, 0.0]]), atol=1e-6)
)
self.assertTrue(
np.allclose(frames_np[2], np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]), atol=1e-6)
)
self.assertTrue(
np.allclose(frames_np[3], np.array([[0.0, 0.0, -1.0], [1.0, 0.0, 0.0], [0.0, -1.0, 0.0]]), atol=1e-6)
)
self.assertTrue(
np.allclose(frames_np[4], np.array([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]), atol=1e-6)
)
self.assertTrue(
np.allclose(frames_np[5], np.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]), atol=1e-6)
)
def test_02_make_contact_frame_xnorm(self):
# Create a normal vectors
test_normals: list[vec3f] = []
# Add normals for which to test contact frame creation
test_normals.append(vec3f(1.0, 0.0, 0.0))
test_normals.append(vec3f(0.0, 1.0, 0.0))
test_normals.append(vec3f(0.0, 0.0, 1.0))
test_normals.append(vec3f(-1.0, 0.0, 0.0))
test_normals.append(vec3f(0.0, -1.0, 0.0))
test_normals.append(vec3f(0.0, 0.0, -1.0))
# Create the input output arrays
normals = wp.array(test_normals, dtype=vec3f)
frames = wp.zeros(shape=(len(test_normals),), dtype=mat33f)
# Compute the contact frames
compute_contact_frame_xnorm(normal=normals, frame=frames, num_threads=len(test_normals))
if self.verbose:
print(f"normals:\n{normals}\n")
print(f"frames:\n{frames}\n")
# Extract numpy arrays for comparison
frames_np = frames.numpy()
# Check determinants of each frame
for i in range(len(test_normals)):
det = np.linalg.det(frames_np[i])
self.assertTrue(np.isclose(det, 1.0, atol=1e-6))
class TestGeometryContactMode(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.info("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_contact_mode_opening(self):
v_input = wp.array([vec3f(0.0, 0.0, 0.01)], dtype=vec3f)
mode_output = wp.zeros(shape=(1,), dtype=int32)
compute_contact_mode(velocity=v_input, mode=mode_output, num_threads=1)
mode_int32 = mode_output.numpy()[0]
mode = ContactMode(int(mode_int32))
msg.info(f"mode: {mode} (int: {int(mode_int32)})")
self.assertEqual(mode, ContactMode.OPENING)
def test_02_contact_mode_sticking(self):
v_input = wp.array([vec3f(0.0, 0.0, 1e-7)], dtype=vec3f)
mode_output = wp.zeros(shape=(1,), dtype=int32)
compute_contact_mode(velocity=v_input, mode=mode_output, num_threads=1)
mode_int32 = mode_output.numpy()[0]
mode = ContactMode(int(mode_int32))
msg.info(f"mode: {mode} (int: {int(mode_int32)})")
self.assertEqual(mode, ContactMode.STICKING)
def test_03_contact_mode_slipping(self):
v_input = wp.array([vec3f(0.1, 0.0, 0.0)], dtype=vec3f)
mode_output = wp.zeros(shape=(1,), dtype=int32)
compute_contact_mode(velocity=v_input, mode=mode_output, num_threads=1)
mode_int32 = mode_output.numpy()[0]
mode = ContactMode(int(mode_int32))
msg.info(f"mode: {mode} (int: {int(mode_int32)})")
self.assertEqual(mode, ContactMode.SLIDING)
class TestGeometryContacts(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.info("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_single_default_allocation(self):
contacts = ContactsKamino(capacity=0, device=self.default_device)
self.assertEqual(contacts.model_max_contacts_host, contacts.default_max_world_contacts)
self.assertEqual(contacts.world_max_contacts_host[0], contacts.default_max_world_contacts)
self.assertEqual(len(contacts.model_max_contacts), 1)
self.assertEqual(len(contacts.model_active_contacts), 1)
self.assertEqual(len(contacts.world_max_contacts), 1)
self.assertEqual(len(contacts.world_active_contacts), 1)
self.assertEqual(contacts.model_max_contacts.numpy()[0], contacts.default_max_world_contacts)
self.assertEqual(contacts.model_active_contacts.numpy()[0], 0)
self.assertEqual(contacts.world_max_contacts.numpy()[0], contacts.default_max_world_contacts)
self.assertEqual(contacts.world_active_contacts.numpy()[0], 0)
self.assertEqual(len(contacts.wid), contacts.default_max_world_contacts)
self.assertEqual(len(contacts.cid), contacts.default_max_world_contacts)
self.assertEqual(len(contacts.gid_AB), contacts.default_max_world_contacts)
self.assertEqual(len(contacts.bid_AB), contacts.default_max_world_contacts)
self.assertEqual(len(contacts.position_A), contacts.default_max_world_contacts)
self.assertEqual(len(contacts.position_B), contacts.default_max_world_contacts)
self.assertEqual(len(contacts.gapfunc), contacts.default_max_world_contacts)
self.assertEqual(len(contacts.frame), contacts.default_max_world_contacts)
self.assertEqual(len(contacts.material), contacts.default_max_world_contacts)
def test_multiple_default_allocations(self):
num_worlds = 10
capacities = [0] * num_worlds
contacts = ContactsKamino(capacity=capacities, device=self.default_device)
model_max_contacts = contacts.model_max_contacts.numpy()
model_active_contacts = contacts.model_active_contacts.numpy()
self.assertEqual(len(contacts.model_max_contacts), 1)
self.assertEqual(len(contacts.model_active_contacts), 1)
self.assertEqual(model_max_contacts[0], num_worlds * contacts.default_max_world_contacts)
self.assertEqual(model_active_contacts[0], 0)
world_max_contacts = contacts.world_max_contacts.numpy()
world_active_contacts = contacts.world_active_contacts.numpy()
self.assertEqual(len(contacts.world_max_contacts), num_worlds)
self.assertEqual(len(contacts.world_active_contacts), num_worlds)
for i in range(num_worlds):
self.assertEqual(world_max_contacts[i], contacts.default_max_world_contacts)
self.assertEqual(world_active_contacts[i], 0)
self.assertEqual(len(contacts.wid), num_worlds * contacts.default_max_world_contacts)
self.assertEqual(len(contacts.cid), num_worlds * contacts.default_max_world_contacts)
self.assertEqual(len(contacts.gid_AB), num_worlds * contacts.default_max_world_contacts)
self.assertEqual(len(contacts.bid_AB), num_worlds * contacts.default_max_world_contacts)
self.assertEqual(len(contacts.position_A), num_worlds * contacts.default_max_world_contacts)
self.assertEqual(len(contacts.position_B), num_worlds * contacts.default_max_world_contacts)
self.assertEqual(len(contacts.gapfunc), num_worlds * contacts.default_max_world_contacts)
self.assertEqual(len(contacts.frame), num_worlds * contacts.default_max_world_contacts)
self.assertEqual(len(contacts.material), num_worlds * contacts.default_max_world_contacts)
def test_multiple_custom_allocations(self):
capacities = [10, 20, 30, 40, 50, 60]
contacts = ContactsKamino(capacity=capacities, device=self.default_device)
num_worlds = len(capacities)
model_max_contacts = contacts.model_max_contacts.numpy()
model_active_contacts = contacts.model_active_contacts.numpy()
self.assertEqual(len(contacts.model_max_contacts), 1)
self.assertEqual(len(contacts.model_active_contacts), 1)
self.assertEqual(model_max_contacts[0], sum(capacities))
self.assertEqual(model_active_contacts[0], 0)
world_max_contacts = contacts.world_max_contacts.numpy()
world_active_contacts = contacts.world_active_contacts.numpy()
self.assertEqual(len(contacts.world_max_contacts), num_worlds)
self.assertEqual(len(contacts.world_active_contacts), num_worlds)
for i in range(num_worlds):
self.assertEqual(world_max_contacts[i], capacities[i])
self.assertEqual(world_active_contacts[i], 0)
maxnc = sum(capacities)
self.assertEqual(len(contacts.wid), maxnc)
self.assertEqual(len(contacts.cid), maxnc)
self.assertEqual(len(contacts.gid_AB), maxnc)
self.assertEqual(len(contacts.bid_AB), maxnc)
self.assertEqual(len(contacts.position_A), maxnc)
self.assertEqual(len(contacts.position_B), maxnc)
self.assertEqual(len(contacts.gapfunc), maxnc)
self.assertEqual(len(contacts.frame), maxnc)
self.assertEqual(len(contacts.material), maxnc)
class TestGeometryContactConversions(unittest.TestCase):
"""Tests for Newton <-> Kamino contact conversion functions."""
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose
if self.verbose:
msg.info("\n")
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
@staticmethod
def _build_nunchaku_newton() -> ModelBuilder:
"""Build a nunchaku scene using the shared builder in basics_newton."""
return build_boxes_nunchaku()
def _setup_newton_scene(self):
"""Finalize the nunchaku model and return (newton_model, newton_state, newton_contacts).
For single-world models, Newton assigns ``shape_world = -1`` (global)
to all shapes. The conversion kernels require world-0 assignment, so
we normalize ``shape_world`` to match what ``ModelKamino.from_newton``
does internally.
"""
builder = self._build_nunchaku_newton()
model = builder.finalize()
if model.world_count == 1:
sw = model.shape_world.numpy()
if np.any(sw < 0):
sw[sw < 0] = 0
model.shape_world.assign(sw)
state = model.state()
newton.eval_fk(model, model.joint_q, model.joint_qd, state)
contacts = model.collide(state)
return model, state, contacts
def test_01_newton_to_kamino(self):
"""Newton contacts converted to Kamino must preserve count, A/B convention, and properties."""
model, state, newton_contacts = self._setup_newton_scene()
nc = int(newton_contacts.rigid_contact_count.numpy()[0])
self.assertGreater(nc, 0, "Newton collision detection must produce contacts")
kamino_out = ContactsKamino(capacity=nc + 16, device=self.default_device)
convert_contacts_newton_to_kamino(model, state, newton_contacts, kamino_out)
wp.synchronize()
nc_kamino = int(kamino_out.model_active_contacts.numpy()[0])
self.assertGreater(nc_kamino, 0, "Conversion must produce Kamino contacts")
# Verify A/B convention: bid_B must be >= 0 for all active contacts
bid_AB = kamino_out.bid_AB.numpy()[:nc_kamino]
for i in range(nc_kamino):
self.assertGreaterEqual(int(bid_AB[i, 1]), 0, f"Contact {i}: bid_B must be >= 0 (Kamino convention)")
# Verify gapfunc normals are unit vectors
gapfunc = kamino_out.gapfunc.numpy()[:nc_kamino]
for i in range(nc_kamino):
n = gapfunc[i, :3]
norm = np.linalg.norm(n)
self.assertTrue(np.isclose(norm, 1.0, atol=1e-5), f"Contact {i}: normal not unit ({norm})")
# Verify signed distance is non-positive (only penetrating contacts are kept)
for i in range(nc_kamino):
self.assertLessEqual(gapfunc[i, 3], 0.0, f"Contact {i}: distance must be <= 0")
# Verify material properties are non-negative
material = kamino_out.material.numpy()[:nc_kamino]
for i in range(nc_kamino):
self.assertGreaterEqual(material[i, 0], 0.0, f"Contact {i}: friction must be >= 0")
self.assertGreaterEqual(material[i, 1], 0.0, f"Contact {i}: restitution must be >= 0")
if self.verbose:
msg.debug("Newton -> Kamino: %d contacts converted", nc_kamino)
msg.debug("bid_AB:\n%s", bid_AB)
msg.debug("gapfunc:\n%s", gapfunc)
def test_02_kamino_to_newton(self):
"""Kamino contacts converted to Newton must have valid shape indices and body-local points."""
model, state, newton_contacts_orig = self._setup_newton_scene()
nc = int(newton_contacts_orig.rigid_contact_count.numpy()[0])
self.assertGreater(nc, 0)
# Newton -> Kamino first (to populate Kamino contacts from a known source)
kamino_contacts = ContactsKamino(capacity=nc + 16, device=self.default_device)
convert_contacts_newton_to_kamino(model, state, newton_contacts_orig, kamino_contacts)
wp.synchronize()
nc_kamino = int(kamino_contacts.model_active_contacts.numpy()[0])
self.assertGreater(nc_kamino, 0)
# Kamino -> Newton (the function under test)
newton_contacts_out = Contacts(
rigid_contact_max=kamino_contacts.model_max_contacts_host,
soft_contact_max=0,
device=self.default_device,
)
convert_contacts_kamino_to_newton(model, state, kamino_contacts, newton_contacts_out)
wp.synchronize()
nc_newton = int(newton_contacts_out.rigid_contact_count.numpy()[0])
self.assertEqual(nc_newton, nc_kamino)
# Verify shape indices are valid model-global indices
shape_count = model.shape_count
shape0 = newton_contacts_out.rigid_contact_shape0.numpy()[:nc_newton]
shape1 = newton_contacts_out.rigid_contact_shape1.numpy()[:nc_newton]
for i in range(nc_newton):
self.assertGreaterEqual(int(shape0[i]), 0)
self.assertLess(int(shape0[i]), shape_count, f"Contact {i}: shape0 out of range")
self.assertGreaterEqual(int(shape1[i]), 0)
self.assertLess(int(shape1[i]), shape_count, f"Contact {i}: shape1 out of range")
# Verify normals are unit vectors
normals = newton_contacts_out.rigid_contact_normal.numpy()[:nc_newton]
for i in range(nc_newton):
norm = np.linalg.norm(normals[i])
self.assertTrue(np.isclose(norm, 1.0, atol=1e-5), f"Contact {i}: normal not unit ({norm})")
# Verify body-local points: transforming back to world should match Kamino positions
body_q = state.body_q.numpy()
shape_body = model.shape_body.numpy()
point0 = newton_contacts_out.rigid_contact_point0.numpy()[:nc_newton]
point1 = newton_contacts_out.rigid_contact_point1.numpy()[:nc_newton]
kamino_pos_A = kamino_contacts.position_A.numpy()[:nc_kamino]
kamino_pos_B = kamino_contacts.position_B.numpy()[:nc_kamino]
for i in range(nc_newton):
b0 = int(shape_body[int(shape0[i])])
b1 = int(shape_body[int(shape1[i])])
if b0 >= 0:
p0_world = _transform_point(body_q[b0], point0[i])
else:
p0_world = point0[i]
if b1 >= 0:
p1_world = _transform_point(body_q[b1], point1[i])
else:
p1_world = point1[i]
np.testing.assert_allclose(
p0_world,
kamino_pos_A[i],
atol=1e-4,
err_msg=f"Contact {i}: point0 world mismatch",
)
np.testing.assert_allclose(
p1_world,
kamino_pos_B[i],
atol=1e-4,
err_msg=f"Contact {i}: point1 world mismatch",
)
if self.verbose:
msg.debug("Kamino -> Newton: %d contacts converted", nc_newton)
msg.debug("shape0: %s", shape0)
msg.debug("shape1: %s", shape1)
def test_03_roundtrip_newton_kamino_newton(self):
"""Round-trip Newton -> Kamino -> Newton preserves world-space contact geometry.
N->K may filter non-penetrating contacts, so the round-tripped count
can be smaller than the original Newton count. The test verifies that
the Kamino intermediate world-space positions match the K->N
round-tripped positions exactly, and that shape *pairs* (as unordered
sets) are preserved.
"""
model, state, newton_contacts_orig = self._setup_newton_scene()
nc_orig = int(newton_contacts_orig.rigid_contact_count.numpy()[0])
self.assertGreater(nc_orig, 0)
# Step 1: Newton -> Kamino
kamino_out = ContactsKamino(capacity=nc_orig + 16, device=self.default_device)
convert_contacts_newton_to_kamino(model, state, newton_contacts_orig, kamino_out)
wp.synchronize()
nc_kamino = int(kamino_out.model_active_contacts.numpy()[0])
self.assertGreater(nc_kamino, 0)
self.assertLessEqual(nc_kamino, nc_orig, "N->K must not create more contacts than Newton")
# Capture Kamino world-space positions as ground truth
kamino_pos_A = kamino_out.position_A.numpy()[:nc_kamino]
kamino_pos_B = kamino_out.position_B.numpy()[:nc_kamino]
# Step 2: Kamino -> Newton (round-trip back)
newton_contacts_rt = Contacts(
rigid_contact_max=kamino_out.model_max_contacts_host,
soft_contact_max=0,
device=self.default_device,
)
convert_contacts_kamino_to_newton(model, state, kamino_out, newton_contacts_rt)
wp.synchronize()
nc_rt = int(newton_contacts_rt.rigid_contact_count.numpy()[0])
self.assertEqual(nc_rt, nc_kamino)
# Verify all shape indices are valid
shape_count = model.shape_count
body_q = state.body_q.numpy()
shape_body = model.shape_body.numpy()
shape0_rt = newton_contacts_rt.rigid_contact_shape0.numpy()[:nc_rt]
shape1_rt = newton_contacts_rt.rigid_contact_shape1.numpy()[:nc_rt]
point0_rt = newton_contacts_rt.rigid_contact_point0.numpy()[:nc_rt]
point1_rt = newton_contacts_rt.rigid_contact_point1.numpy()[:nc_rt]
normal_rt = newton_contacts_rt.rigid_contact_normal.numpy()[:nc_rt]
for i in range(nc_rt):
self.assertGreaterEqual(int(shape0_rt[i]), 0)
self.assertLess(int(shape0_rt[i]), shape_count)
self.assertGreaterEqual(int(shape1_rt[i]), 0)
self.assertLess(int(shape1_rt[i]), shape_count)
norm = np.linalg.norm(normal_rt[i])
self.assertTrue(np.isclose(norm, 1.0, atol=1e-5), f"Contact {i}: normal not unit")
# Verify round-tripped body-local points transform back to Kamino world positions
for i in range(nc_rt):
b0 = int(shape_body[int(shape0_rt[i])])
b1 = int(shape_body[int(shape1_rt[i])])
p0w = _transform_point(body_q[b0], point0_rt[i]) if b0 >= 0 else point0_rt[i]
p1w = _transform_point(body_q[b1], point1_rt[i]) if b1 >= 0 else point1_rt[i]
np.testing.assert_allclose(
p0w,
kamino_pos_A[i],
atol=1e-4,
err_msg=f"Contact {i}: round-trip point0 world mismatch",
)
np.testing.assert_allclose(
p1w,
kamino_pos_B[i],
atol=1e-4,
err_msg=f"Contact {i}: round-trip point1 world mismatch",
)
if self.verbose:
msg.debug("Round-trip: %d -> %d -> %d contacts", nc_orig, nc_kamino, nc_rt)
def test_04_nunchaku_full_pipeline(self):
"""Full nunchaku pipeline: Newton collide -> Kamino -> Newton, verifying each step."""
model, state, newton_contacts = self._setup_newton_scene()
nc_newton = int(newton_contacts.rigid_contact_count.numpy()[0])
self.assertGreater(nc_newton, 0, "Newton collision detection must produce contacts")
# Step 1: Newton -> Kamino
kamino_contacts = ContactsKamino(capacity=nc_newton + 16, device=self.default_device)
convert_contacts_newton_to_kamino(model, state, newton_contacts, kamino_contacts)
wp.synchronize()
nc_kamino = int(kamino_contacts.model_active_contacts.numpy()[0])
self.assertGreater(nc_kamino, 0)
# Verify Kamino contacts properties
gapfunc = kamino_contacts.gapfunc.numpy()[:nc_kamino]
bid_AB = kamino_contacts.bid_AB.numpy()[:nc_kamino]
kamino_pos_A = kamino_contacts.position_A.numpy()[:nc_kamino]
kamino_pos_B = kamino_contacts.position_B.numpy()[:nc_kamino]
for i in range(nc_kamino):
self.assertGreaterEqual(int(bid_AB[i, 1]), 0, f"Contact {i}: bid_B must be >= 0")
n = gapfunc[i, :3]
norm = np.linalg.norm(n)
self.assertTrue(np.isclose(norm, 1.0, atol=1e-5), f"Contact {i}: normal not unit ({norm})")
self.assertLessEqual(gapfunc[i, 3], 0.0, f"Contact {i}: distance must be <= 0")
# Step 2: Kamino -> Newton
newton_contacts_2 = Contacts(
rigid_contact_max=kamino_contacts.model_max_contacts_host,
soft_contact_max=0,
device=self.default_device,
)
convert_contacts_kamino_to_newton(model, state, kamino_contacts, newton_contacts_2)
wp.synchronize()
nc_newton_2 = int(newton_contacts_2.rigid_contact_count.numpy()[0])
self.assertEqual(nc_newton_2, nc_kamino)
# Verify Newton contacts are valid
shape_count = model.shape_count
shape0 = newton_contacts_2.rigid_contact_shape0.numpy()[:nc_newton_2]
shape1 = newton_contacts_2.rigid_contact_shape1.numpy()[:nc_newton_2]
normals = newton_contacts_2.rigid_contact_normal.numpy()[:nc_newton_2]
body_q = state.body_q.numpy()
shape_body = model.shape_body.numpy()
point0 = newton_contacts_2.rigid_contact_point0.numpy()[:nc_newton_2]
point1 = newton_contacts_2.rigid_contact_point1.numpy()[:nc_newton_2]
for i in range(nc_newton_2):
self.assertGreaterEqual(int(shape0[i]), 0)
self.assertLess(int(shape0[i]), shape_count, f"Contact {i}: shape0 out of range")
self.assertGreaterEqual(int(shape1[i]), 0)
self.assertLess(int(shape1[i]), shape_count, f"Contact {i}: shape1 out of range")
norm = np.linalg.norm(normals[i])
self.assertTrue(np.isclose(norm, 1.0, atol=1e-5), f"Contact {i}: normal not unit")
# Verify K->N body-local points transform back to the Kamino world positions
for i in range(nc_newton_2):
b0 = int(shape_body[int(shape0[i])])
b1 = int(shape_body[int(shape1[i])])
if b0 >= 0:
p0_world = _transform_point(body_q[b0], point0[i])
else:
p0_world = point0[i]
if b1 >= 0:
p1_world = _transform_point(body_q[b1], point1[i])
else:
p1_world = point1[i]
np.testing.assert_allclose(
p0_world,
kamino_pos_A[i],
atol=1e-4,
err_msg=f"Contact {i}: K->N point0 world mismatch with Kamino pos_A",
)
np.testing.assert_allclose(
p1_world,
kamino_pos_B[i],
atol=1e-4,
err_msg=f"Contact {i}: K->N point1 world mismatch with Kamino pos_B",
)
if self.verbose:
msg.debug("Nunchaku pipeline: %d -> %d -> %d contacts", nc_newton, nc_kamino, nc_newton_2)
def test_05_multi_world_roundtrip(self):
"""N->K->N round-trip with heterogeneous worlds and a shared ground plane.
Scene layout (ground plane added first, shared across all worlds):
- World 0: nunchaku (2 boxes + 1 sphere) -> 9 contacts (4+4+1)
- World 1: nunchaku (2 boxes + 1 sphere) -> 9 contacts (4+4+1)
- World 2: single box -> 4 contacts
The ground plane is global (shape_world == -1) and precedes all
per-world shapes, exercising the plane-first ordering path.
Expected total: 22 contacts.
"""
nunchaku_blueprint = build_boxes_nunchaku(ground=False)
box_blueprint = ModelBuilder()
b = box_blueprint.add_link()
no_gap = ModelBuilder.ShapeConfig(gap=0.0)
box_blueprint.add_shape_box(b, hx=0.25, hy=0.25, hz=0.25, cfg=no_gap)
j = box_blueprint.add_joint_free(
parent=-1,
child=b,
parent_xform=wp.transform(p=wp.vec3(0.0, 0.0, 0.25), q=wp.quat_identity()),
child_xform=wp.transform_identity(),
)
box_blueprint.add_articulation([j])
scene = ModelBuilder()
scene.add_ground_plane()
scene.add_world(nunchaku_blueprint, xform=wp.transform(p=wp.vec3(0.0, 0.0, 0.0)))
scene.add_world(nunchaku_blueprint, xform=wp.transform(p=wp.vec3(5.0, 0.0, 0.0)))
scene.add_world(box_blueprint, xform=wp.transform(p=wp.vec3(10.0, 0.0, 0.0)))
model = scene.finalize()
self.assertEqual(model.world_count, 3)
sws = model.shape_world_start.numpy()
self.assertGreater(int(sws[1]), 0, "World 1 must have nonzero shape_world_start")
state = model.state()
newton.eval_fk(model, model.joint_q, model.joint_qd, state)
contacts = model.collide(state)
nc_orig = int(contacts.rigid_contact_count.numpy()[0])
self.assertGreater(nc_orig, 0)
expected_contacts = 9 + 9 + 4
self.assertEqual(
nc_orig,
expected_contacts,
f"Expected {expected_contacts} contacts (9+9+4), got {nc_orig}",
)
kamino_out = ContactsKamino(capacity=nc_orig + 32, device=self.default_device)
convert_contacts_newton_to_kamino(model, state, contacts, kamino_out)
wp.synchronize()
nc_kamino = int(kamino_out.model_active_contacts.numpy()[0])
self.assertGreater(nc_kamino, 0)
newton_rt = Contacts(
rigid_contact_max=kamino_out.model_max_contacts_host,
soft_contact_max=0,
device=self.default_device,
)
convert_contacts_kamino_to_newton(model, state, kamino_out, newton_rt)
wp.synchronize()
nc_rt = int(newton_rt.rigid_contact_count.numpy()[0])
self.assertEqual(nc_rt, nc_kamino)
shape_count = model.shape_count
shape_body = model.shape_body.numpy()
body_q = state.body_q.numpy()
shape0 = newton_rt.rigid_contact_shape0.numpy()[:nc_rt]
shape1 = newton_rt.rigid_contact_shape1.numpy()[:nc_rt]
point0 = newton_rt.rigid_contact_point0.numpy()[:nc_rt]
point1 = newton_rt.rigid_contact_point1.numpy()[:nc_rt]
kamino_pos_A = kamino_out.position_A.numpy()[:nc_kamino]
kamino_pos_B = kamino_out.position_B.numpy()[:nc_kamino]
for i in range(nc_rt):
self.assertGreaterEqual(int(shape0[i]), 0)
self.assertLess(int(shape0[i]), shape_count, f"Contact {i}: shape0 out of range")
self.assertGreaterEqual(int(shape1[i]), 0)
self.assertLess(int(shape1[i]), shape_count, f"Contact {i}: shape1 out of range")
b0 = int(shape_body[int(shape0[i])])
b1 = int(shape_body[int(shape1[i])])
p0w = _transform_point(body_q[b0], point0[i]) if b0 >= 0 else point0[i]
p1w = _transform_point(body_q[b1], point1[i]) if b1 >= 0 else point1[i]
np.testing.assert_allclose(p0w, kamino_pos_A[i], atol=1e-4, err_msg=f"Contact {i}: pos_A mismatch")
np.testing.assert_allclose(p1w, kamino_pos_B[i], atol=1e-4, err_msg=f"Contact {i}: pos_B mismatch")
if self.verbose:
msg.debug(
"Multi-world round-trip: %d -> %d -> %d contacts (3 worlds: nunchaku, nunchaku, box)",
nc_orig,
nc_kamino,
nc_rt,
)
###
# Helpers
###
def _quat_rotate(q: np.ndarray, v: np.ndarray) -> np.ndarray:
"""Rotate vector v by quaternion q (x, y, z, w convention from Warp transforms)."""
qx, qy, qz, qw = q[0], q[1], q[2], q[3]
t = 2.0 * np.cross(np.array([qx, qy, qz]), v)
return v + qw * t + np.cross(np.array([qx, qy, qz]), t)
def _transform_point(xform: np.ndarray, point: np.ndarray) -> np.ndarray:
"""Apply a Warp transform (p[0:3], q[3:7]) to a point."""
return xform[:3] + _quat_rotate(xform[3:], point)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_geometry_detector.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for `geometry/detector.py`"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.geometry import (
CollisionDetector,
)
from newton._src.solvers.kamino._src.models.builders import basics
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.test_geometry_primitive import check_contacts
###
# Tests
###
class TestCollisionDetectorConfig(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for detailed output
self.default_device = wp.get_device(test_context.device)
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.info("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default(self):
"""Test making default collision detector config."""
config = CollisionDetector.Config()
self.assertEqual(config.pipeline, "unified")
self.assertEqual(config.broadphase, "explicit")
self.assertEqual(config.bvtype, "aabb")
def test_01_make_with_string_args(self):
"""Test making collision detector config with string arguments."""
config = CollisionDetector.Config(pipeline="primitive", broadphase="explicit", bvtype="aabb")
self.assertEqual(config.pipeline, "primitive")
self.assertEqual(config.broadphase, "explicit")
self.assertEqual(config.bvtype, "aabb")
class TestGeometryCollisionDetector(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
self.build_func = basics.build_boxes_nunchaku
self.expected_contacts = 9 # NOTE: specialized to build_boxes_nunchaku
msg.debug(f"build_func: {self.build_func.__name__}")
msg.debug(f"expected_contacts: {self.expected_contacts}")
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_primitive_pipeline(self):
"""
Test the collision detector with the primitive pipeline
on multiple worlds containing boxes_nunchaku model.
"""
# Create and set up a model builder
builder = make_homogeneous_builder(num_worlds=3, build_fn=self.build_func)
model = builder.finalize(self.default_device)
data = model.data()
state = model.state()
# Create a collision detector with primitive pipeline
config = CollisionDetector.Config(
pipeline="primitive",
broadphase="explicit",
bvtype="aabb",
)
detector = CollisionDetector(model=model, config=config)
self.assertIs(detector.device, self.default_device)
# Run collision detection
detector.collide(data, state)
# Create a list of expected number of contacts per shape pair
expected_world_contacts: list[int] = [self.expected_contacts] * builder.num_worlds
msg.debug("expected_world_contacts:\n%s\n", expected_world_contacts)
# Define expected contacts dictionary
expected = {
"model_active_contacts": sum(expected_world_contacts),
"world_active_contacts": np.array(expected_world_contacts, dtype=np.int32),
}
# Check results
check_contacts(
detector.contacts,
expected,
case="boxes_nunchaku",
header="primitive pipeline",
)
def test_02_unified_pipeline(self):
"""
Test the collision detector with the unified pipeline
on multiple worlds containing boxes_nunchaku model.
"""
# Create and set up a model builder
builder = make_homogeneous_builder(num_worlds=3, build_fn=self.build_func)
model = builder.finalize(self.default_device)
data = model.data()
state = model.state()
# Create a collision detector with unified pipeline
config = CollisionDetector.Config(
pipeline="unified",
broadphase="explicit",
bvtype="aabb",
)
detector = CollisionDetector(model=model, config=config)
self.assertIs(detector.device, self.default_device)
# Run collision detection
detector.collide(data, state)
# Create a list of expected number of contacts per shape pair
expected_world_contacts: list[int] = [self.expected_contacts] * builder.num_worlds
msg.debug("expected_world_contacts:\n%s\n", expected_world_contacts)
# Define expected contacts dictionary
expected = {
"model_active_contacts": sum(expected_world_contacts),
"world_active_contacts": np.array(expected_world_contacts, dtype=np.int32),
}
# Check results
check_contacts(
detector.contacts,
expected,
case="boxes_nunchaku",
header="unified pipeline",
)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_geometry_keying.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for `geometry/keying.py`"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.types import int32, uint32, uint64
from newton._src.solvers.kamino._src.geometry.keying import (
KeySorter,
binary_search_find_pair,
binary_search_find_range_start,
build_pair_key2,
make_bitmask,
make_build_pair_key3_func,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Helper functions
###
###
# Tests
###
class TestPairKeyOps(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
# Global numpy configurations
np.set_printoptions(linewidth=10000, precision=10, threshold=10000, suppress=True)
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_bitmask(self):
"""Test make_bitmask function for various bit sizes."""
# Test make_bitmask for out-of-bounds inputs
self.assertRaises(ValueError, make_bitmask, 0)
self.assertRaises(ValueError, make_bitmask, 65)
# Test make_bitmask for valid inputs
bitmask_1_Bits = make_bitmask(1)
msg.info(f"bitmask_1_Bits: {bitmask_1_Bits:#0{4}x} ({bitmask_1_Bits})")
self.assertEqual(bitmask_1_Bits, 0x1)
bitmask_3_Bits = make_bitmask(3)
msg.info(f"bitmask_3_Bits: {bitmask_3_Bits:#0{4}x} ({bitmask_3_Bits})")
self.assertEqual(bitmask_3_Bits, 0x7)
bitmask_7_Bits = make_bitmask(7)
msg.info(f"bitmask_7_Bits: {bitmask_7_Bits:#0{4}x} ({bitmask_7_Bits})")
self.assertEqual(bitmask_7_Bits, 0x7F)
bitmask_8_Bits = make_bitmask(8)
msg.info(f"bitmask_8_Bits: {bitmask_8_Bits:#0{4}x} ({bitmask_8_Bits})")
self.assertEqual(bitmask_8_Bits, 0xFF)
bitmask_23_Bits = make_bitmask(23)
msg.info(f"bitmask_23_Bits: {bitmask_23_Bits:#0{8}x} ({bitmask_23_Bits})")
self.assertEqual(bitmask_23_Bits, 0x7FFFFF)
bitmask_32_Bits = make_bitmask(32)
msg.info(f"bitmask_32_Bits: {bitmask_32_Bits:#0{10}x} ({bitmask_32_Bits})")
self.assertEqual(bitmask_32_Bits, 0xFFFFFFFF)
bitmask_64_Bits = make_bitmask(64)
msg.info(f"bitmask_64_Bits: {bitmask_64_Bits:#0{18}x} ({bitmask_64_Bits})")
self.assertEqual(bitmask_64_Bits, 0xFFFFFFFFFFFFFFFF)
def test_01_build_pair_key2(self):
"""Test build_pair_key2 function for various index pairs."""
# Define a Warp kernel to test build_pair_key2
@wp.kernel
def _test_kernel_build_pair_key2(
index_A: wp.array(dtype=uint32), index_B: wp.array(dtype=uint32), key: wp.array(dtype=uint64)
):
tid = wp.tid()
key[tid] = build_pair_key2(index_A[tid], index_B[tid])
# Define test cases for index pairs
test_cases = [
(0, 0),
(1, 1),
(2, 23),
(12345, 67890),
(0x7FFFFFFF, 0xFFFFFFFF),
(0x12345678, 0x9ABCDEF0),
]
num_test_cases = len(test_cases)
msg.info("num_test_cases: %d", num_test_cases)
msg.info("test_cases: %s", test_cases)
# Create Warp arrays for inputs and outputs
index_A = wp.array([index_A for index_A, _ in test_cases], dtype=uint32)
index_B = wp.array([index_B for _, index_B in test_cases], dtype=uint32)
keys = wp.zeros(num_test_cases, dtype=uint64)
msg.info("Inputs: index_A: %s", index_A)
msg.info("Inputs: index_B: %s", index_B)
msg.info("Inputs: keys: %s", keys)
self.assertEqual(index_A.size, num_test_cases)
self.assertEqual(index_B.size, num_test_cases)
self.assertEqual(keys.size, num_test_cases)
# Launch the test kernel to generate keys for the given index pairs
wp.launch(_test_kernel_build_pair_key2, dim=num_test_cases, inputs=[index_A, index_B, keys])
# Verify the generated keys against expected values
keys_np = keys.numpy()
msg.info("Output: keys: %s", keys_np)
for i, (index_A, index_B) in enumerate(test_cases):
expected_key = (index_A << 32) | index_B
msg.info(f"build_pair_key2({index_A}, {index_B}): {keys_np[i]} (expected: {expected_key})")
self.assertEqual(keys_np[i], expected_key)
def test_02_build_pair_key3(self):
"""Test build_pair_key3 function for various index pairs with auxiliary index."""
# Define a Warp kernel to test build_pair_key3
def make_test_kernel_build_pair_key3(main_key_bits: int, aux_key_bits: int):
# Generate the build_pair_key3 function with specified bit widths
build_pair_key3 = make_build_pair_key3_func(main_key_bits, aux_key_bits)
# Generate the test kernel for the specified build_pair_key3 function
@wp.kernel
def _test_kernel_build_pair_key3(
index_A: wp.array(dtype=uint32),
index_B: wp.array(dtype=uint32),
index_C: wp.array(dtype=uint32),
key: wp.array(dtype=uint64),
):
tid = wp.tid()
key[tid] = build_pair_key3(index_A[tid], index_B[tid], index_C[tid])
return _test_kernel_build_pair_key3
# Define test cases for index pairs
test_cases = [
(0, 0, 0),
(1, 1, 1),
(2, 23, 3),
(12345, 67890, 4),
(0xFFFFFFF, 0xFFFFFFF, 5),
(0x2345678, 0xABCDEF0, 6),
]
num_test_cases = len(test_cases)
msg.info("num_test_cases: %d", num_test_cases)
msg.info("test_cases: %s", test_cases)
# Create Warp arrays for inputs and outputs
index_A = wp.array([index_A for index_A, _, _ in test_cases], dtype=uint32)
index_B = wp.array([index_B for _, index_B, _ in test_cases], dtype=uint32)
index_C = wp.array([index_C for *_, index_C in test_cases], dtype=uint32)
keys = wp.zeros(num_test_cases, dtype=uint64)
msg.info("Inputs: index_A: %s", index_A)
msg.info("Inputs: index_B: %s", index_B)
msg.info("Inputs: index_C: %s", index_C)
msg.info("Inputs: keys: %s", keys)
self.assertEqual(index_A.size, num_test_cases)
self.assertEqual(index_B.size, num_test_cases)
self.assertEqual(index_C.size, num_test_cases)
self.assertEqual(keys.size, num_test_cases)
# Test various bit size configurations
# NOTE: main_key_bits * 2 + aux_key_bits must equal 63
test_valid_bitsizes = [(21, 21), (20, 23), (22, 19), (18, 27)]
for main_key_bits, aux_key_bits in test_valid_bitsizes:
msg.info("Testing build_pair_key3 with main_key_bits=%d, aux_key_bits=%d", main_key_bits, aux_key_bits)
_test_kernel_build_pair_key2 = make_test_kernel_build_pair_key3(main_key_bits, aux_key_bits)
# Launch the test kernel to generate keys for the given index pairs
wp.launch(_test_kernel_build_pair_key2, dim=num_test_cases, inputs=[index_A, index_B, index_C, keys])
keys_np = keys.numpy()
msg.info("Output: keys: %s", keys_np)
# Generate bitmasks for expected key computation
MAIN_BITMASK = make_bitmask(main_key_bits)
AUX_BITMASK = make_bitmask(aux_key_bits)
# Verify the generated keys against expected values
for i, (index_A_val, index_B_val, index_C_val) in enumerate(test_cases):
expected_key = (
((index_A_val & MAIN_BITMASK) << (main_key_bits + aux_key_bits))
| ((index_B_val & MAIN_BITMASK) << aux_key_bits)
| (index_C_val & AUX_BITMASK)
)
msg.info(f"expected_key: {expected_key:#0{10}x}")
msg.info(
f"build_pair_key3({index_A_val:#0{10}x}, {index_B_val:#0{10}x}, {index_C_val:#0{10}x}): "
f"{keys_np[i]:#0{10}x}"
)
self.assertEqual(keys_np[i], expected_key)
class TestBinarySearchOps(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
# Global numpy configurations
np.set_printoptions(linewidth=10000, precision=10, threshold=10000, suppress=True)
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_binary_search_find_pair(self):
"""Test binary_search_find_pair function."""
# Define a Warp kernel to test binary_search_find_pair
@wp.kernel
def _test_kernel_binary_search_find_pair(
# Inputs:
num_active_pairs: wp.array(dtype=int32),
all_pairs: wp.array(dtype=wp.vec2i),
target_pair: wp.array(dtype=wp.vec2i),
# Output:
target_index: wp.array(dtype=wp.int32),
):
tid = wp.tid()
target_index[tid] = binary_search_find_pair(num_active_pairs[0], target_pair[tid], all_pairs)
# Define sorted array of unique integer pairs with some inactive dummy pairs at the end
pairs_list = [(1, 1), (1, 2), (3, 1), (5, 6), (5, 5), (7, 8), (9, 10), (0, 0), (0, 0)]
num_all_pairs = len(pairs_list)
pairs = wp.array(pairs_list, dtype=wp.vec2i)
num_active_pairs = wp.array([7], dtype=wp.int32) # Only first 7 pairs are active
msg.info("pairs:\n%s", pairs)
msg.info("num_active_pairs: %s", num_active_pairs)
msg.info("num_all_pairs: %s", num_all_pairs)
# Define target pairs to search for
target_pairs_list = [(1, 1), (3, 1), (7, 8), (0, 0), (9, 10), (11, 12)]
expected_idxs = [0, 2, 5, -1, 6, -1] # Expected indices or -1 if not found
num_target_pairs = len(target_pairs_list)
target_pairs = wp.array(target_pairs_list, dtype=wp.vec2i)
target_idxs = wp.zeros(num_target_pairs, dtype=wp.int32)
msg.info("target_pairs:\n%s", target_pairs)
msg.info("expected_idxs: %s", expected_idxs)
# Launch the test kernel
wp.launch(
_test_kernel_binary_search_find_pair,
dim=num_target_pairs,
inputs=[num_active_pairs, pairs, target_pairs, target_idxs],
)
# Verify results
target_idxs_np = target_idxs.numpy()
msg.info("target_idxs: %s", target_idxs_np)
for i, expected_index in enumerate(expected_idxs):
msg.info(f"target {target_pairs_list[i]} at index: {target_idxs_np[i]} (expected: {expected_index})")
self.assertEqual(target_idxs_np[i], expected_index)
def test_02_binary_search_find_range_start(self):
"""Test binary_search_find_range_start function."""
# Define a Warp kernel to test binary_search_find_range_start
@wp.kernel
def _test_kernel_binary_search_find_range_start(
# Inputs:
num_active_keys: wp.array(dtype=int32),
all_keys: wp.array(dtype=uint64),
target_key: wp.array(dtype=uint64),
# Output:
target_start: wp.array(dtype=int32),
):
tid = wp.tid()
target_start[tid] = binary_search_find_range_start(int32(0), num_active_keys[0], target_key[tid], all_keys)
# Define sorted array of unique integer keys with some inactive dummy keys at the end
keys_list = [0, 1, 1, 3, 5, 5, 9, 11, 11, 0, 0, 0, 0]
num_all_keys = len(keys_list)
keys = wp.array(keys_list, dtype=uint64)
num_active_keys = wp.array([9], dtype=int32) # Only first 9 keys are active
msg.info("keys:\n%s", keys)
msg.info("num_active_keys: %s", num_active_keys)
msg.info("num_all_keys: %s", num_all_keys)
# Define target keys to search for
target_keys_list = [1, 5, 11, 2, 9, 12]
expected_range_start_idxs = [1, 4, 7, -1, 6, -1] # Expected start indices or -1 if not found
num_target_elements = len(target_keys_list)
target_keys = wp.array(target_keys_list, dtype=uint64)
target_start_idxs = wp.zeros(num_target_elements, dtype=int32)
msg.info("target_keys:\n%s", target_keys)
msg.info("expected_range_start_idxs: %s", expected_range_start_idxs)
# Launch the test kernel
wp.launch(
_test_kernel_binary_search_find_range_start,
dim=num_target_elements,
inputs=[num_active_keys, keys, target_keys, target_start_idxs],
)
# Verify results
target_start_idxs_np = target_start_idxs.numpy()
msg.info("target_start_idxs: %s", target_start_idxs_np)
for i, expected in enumerate(expected_range_start_idxs):
msg.info(f"target {target_keys_list[i]} start index: {target_start_idxs_np[i]} (expected: {expected})")
self.assertEqual(target_start_idxs_np[i], expected)
class TestKeySorter(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
# Global numpy configurations
np.set_printoptions(linewidth=10000, precision=10, threshold=10000, suppress=True)
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_key_sorter(self):
"""Test creation of KeySorter instance and relevant memory allocations."""
max_num_keys = 10
sorter = KeySorter(max_num_keys=max_num_keys, device=self.default_device)
msg.info("sorter.sorted_keys: %s", sorter.sorted_keys)
msg.info("sorter.sorted_keys_int64: %s", sorter.sorted_keys_int64)
msg.info("sorter.sorted_to_unsorted_map: %s", sorter.sorted_to_unsorted_map)
self.assertEqual(sorter.sorted_keys.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
self.assertEqual(sorter.sorted_keys_int64.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
self.assertEqual(sorter.sorted_to_unsorted_map.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
def test_01_sort_fixed_keys(self):
"""Test sorting of fixed keys and verification of sorted results."""
max_num_keys = 10
sorter = KeySorter(max_num_keys=max_num_keys, device=self.default_device)
msg.info("sorter.sorted_keys: %s", sorter.sorted_keys)
msg.info("sorter.sorted_keys_int64: %s", sorter.sorted_keys_int64)
msg.info("sorter.sorted_to_unsorted_map: %s", sorter.sorted_to_unsorted_map)
self.assertEqual(sorter.sorted_keys.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
self.assertEqual(sorter.sorted_keys_int64.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
self.assertEqual(sorter.sorted_to_unsorted_map.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
# Generate random keys
sentinel = make_bitmask(64)
num_active_keys_const = 8
num_active_keys = wp.array([num_active_keys_const], dtype=int32)
keys_list = [5, 3, 9, 1, 7, 3, 5, 11, sentinel, sentinel]
keys = wp.array(keys_list, dtype=uint64)
msg.info("num_active_keys: %s", num_active_keys)
msg.info("keys: %s", keys)
# Compute expected results using numpy
expected_sorted_keys = [1, 3, 3, 5, 5, 7, 9, 11]
expected_sorted_to_unsorted_map = [3, 1, 5, 0, 6, 4, 2, 7]
msg.info("expected_sorted_keys: %s", expected_sorted_keys)
msg.info("expected_sorted_to_unsorted_map: %s", expected_sorted_to_unsorted_map)
# Launch sorter to sort the random keys
sorter.sort(num_active_keys=num_active_keys, keys=keys)
# Verify results
sorted_keys_np = sorter.sorted_keys.numpy()[:num_active_keys_const]
sorted_to_unsorted_map_np = sorter.sorted_to_unsorted_map.numpy()[:num_active_keys_const]
msg.info("sorter.sorted_keys: %s", sorted_keys_np)
msg.info("sorter.sorted_to_unsorted_map: %s", sorted_to_unsorted_map_np)
for i in range(num_active_keys_const):
msg.info(f"sorted_keys[{i}]: {sorted_keys_np[i]} (expected: {expected_sorted_keys[i]})")
msg.info(
f"sorted_to_unsorted_map[{i}]: {sorted_to_unsorted_map_np[i]} "
f"(expected: {expected_sorted_to_unsorted_map[i]})"
)
self.assertEqual(sorted_keys_np[i], expected_sorted_keys[i])
self.assertEqual(sorted_to_unsorted_map_np[i], expected_sorted_to_unsorted_map[i])
def test_02_sort_random_keys(self):
"""Test sorting of random keys and verification of sorted results."""
max_num_keys = 10
sorter = KeySorter(max_num_keys=max_num_keys, device=self.default_device)
msg.info("sorter.sorted_keys: %s", sorter.sorted_keys)
msg.info("sorter.sorted_keys_int64: %s", sorter.sorted_keys_int64)
msg.info("sorter.sorted_to_unsorted_map: %s", sorter.sorted_to_unsorted_map)
self.assertEqual(sorter.sorted_keys.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
self.assertEqual(sorter.sorted_keys_int64.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
self.assertEqual(sorter.sorted_to_unsorted_map.size, 2 * max_num_keys) # Factor of 2 for sort algorithm
# Set up random seed for reproducibility
rng = np.random.default_rng(seed=42)
# Generate random keys
num_active_keys_const = 8
num_active_keys = wp.array([num_active_keys_const], dtype=int32)
random_keys_np = rng.integers(low=0, high=100, size=max_num_keys, dtype=np.uint64)
random_keys_np[-2:] = make_bitmask(63) # Set last two keys to sentinel value
random_keys = wp.array(random_keys_np, dtype=uint64)
msg.info("num_active_keys: %s", num_active_keys)
msg.info("random_keys: %s", random_keys)
# Compute expected results using numpy
expected_sorted_keys_np = np.sort(random_keys_np, stable=True)[:num_active_keys_const]
expected_sorted_to_unsorted_map_np = np.argsort(random_keys_np, stable=True)[:num_active_keys_const]
msg.info("expected_sorted_keys: %s", expected_sorted_keys_np.tolist())
msg.info("expected_sorted_to_unsorted_map: %s", expected_sorted_to_unsorted_map_np.tolist())
# Launch sorter to sort the random keys
sorter.sort(num_active_keys=num_active_keys, keys=random_keys)
# Verify results
sorted_keys_np = sorter.sorted_keys.numpy()[:num_active_keys_const]
sorted_to_unsorted_map_np = sorter.sorted_to_unsorted_map.numpy()[:num_active_keys_const]
msg.info("sorter.sorted_keys: %s", sorted_keys_np.tolist())
msg.info("sorter.sorted_to_unsorted_map: %s", sorted_to_unsorted_map_np.tolist())
for i in range(num_active_keys_const):
msg.info(f"sorted_keys[{i}]: {sorted_keys_np[i]} (expected: {expected_sorted_keys_np[i]})")
msg.info(
f"sorted_to_unsorted_map[{i}]: {sorted_to_unsorted_map_np[i]} "
f"(expected: {expected_sorted_to_unsorted_map_np[i]})"
)
self.assertEqual(sorted_keys_np[i], expected_sorted_keys_np[i])
self.assertEqual(sorted_to_unsorted_map_np[i], expected_sorted_to_unsorted_map_np[i])
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_geometry_margin_gap.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Unit tests for margin (rest offset) and gap (detection threshold) semantics.
Margin is a per-shape surface offset (pairwise additive) that defines the
resting separation between shapes. Gap is an additional detection distance
on top of margin; contacts are generated when the surface distance is within
``margin + gap``, but the resting position is controlled solely by margin.
"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.joints import JointActuationType, JointDoFType
from newton._src.solvers.kamino._src.core.math import I_3
from newton._src.solvers.kamino._src.core.shapes import BoxShape, SphereShape
from newton._src.solvers.kamino._src.core.types import mat33f, transformf, vec3f, vec6f
from newton._src.solvers.kamino._src.geometry.contacts import ContactsKamino
from newton._src.solvers.kamino._src.geometry.detector import CollisionDetector
from newton._src.solvers.kamino._src.geometry.primitive import CollisionPipelinePrimitive
from newton._src.solvers.kamino._src.solver_kamino_impl import SolverKaminoImpl
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Helpers
###
def _make_sphere_pair_builder(
distance: float = 0.0,
margin_top: float = 0.0,
margin_bottom: float = 0.0,
gap_top: float = 0.0,
gap_bottom: float = 0.0,
radius: float = 0.5,
) -> ModelBuilderKamino:
"""Build a model with two spheres stacked along z, separated by *distance*."""
builder = ModelBuilderKamino(default_world=True)
bid0 = builder.add_rigid_body(
name="bottom_sphere",
m_i=1.0,
i_I_i=mat33f(np.eye(3, dtype=np.float32)),
q_i_0=transformf(vec3f(0.0, 0.0, -radius - 0.5 * distance), wp.quat_identity()),
)
bid1 = builder.add_rigid_body(
name="top_sphere",
m_i=1.0,
i_I_i=mat33f(np.eye(3, dtype=np.float32)),
q_i_0=transformf(vec3f(0.0, 0.0, radius + 0.5 * distance), wp.quat_identity()),
)
builder.add_geometry(body=bid0, name="bottom", shape=SphereShape(radius), margin=margin_bottom, gap=gap_bottom)
builder.add_geometry(body=bid1, name="top", shape=SphereShape(radius), margin=margin_top, gap=gap_top)
return builder
def _run_primitive_pipeline(builder: ModelBuilderKamino, device, max_contacts_per_pair: int = 4):
"""Run broadphase + narrowphase and return the contacts container."""
model = builder.finalize(device)
data = model.data()
state = model.state()
_, world_req = builder.compute_required_contact_capacity(max_contacts_per_pair=max_contacts_per_pair)
contacts = ContactsKamino(capacity=world_req, device=device)
contacts.clear()
pipeline = CollisionPipelinePrimitive(model=model, bvtype="aabb", default_gap=0.0, device=device)
pipeline.collide(data, state, contacts)
return contacts, model
###
# Tests — Narrowphase
###
class TestMarginGapNarrowphase(unittest.TestCase):
"""Verify that margin and gap are correctly handled in the primitive narrowphase."""
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
if test_context.verbose:
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
msg.reset_log_level()
# ------------------------------------------------------------------
# Margin tests
# ------------------------------------------------------------------
def test_01_margin_shifts_gapfunc_touching(self):
"""Two touching spheres with margin: gapfunc.w = 0 - margin = -margin."""
margin = 0.05
builder = _make_sphere_pair_builder(distance=0.0, margin_top=margin, margin_bottom=0.0)
contacts, _model = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(num_active, 1, "Expected exactly 1 contact for touching spheres with margin")
gapfunc = contacts.gapfunc.numpy()[0]
expected_gapfunc_w = 0.0 - margin
np.testing.assert_allclose(
gapfunc[3],
expected_gapfunc_w,
atol=1e-5,
err_msg="gapfunc.w should be surface_distance - margin for touching spheres",
)
def test_02_margin_symmetric(self):
"""Margin is pairwise additive: margin_A + margin_B."""
margin_a = 0.02
margin_b = 0.03
builder = _make_sphere_pair_builder(distance=0.0, margin_top=margin_a, margin_bottom=margin_b)
contacts, _ = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(num_active, 1)
gapfunc = contacts.gapfunc.numpy()[0]
expected_gapfunc_w = 0.0 - (margin_a + margin_b)
np.testing.assert_allclose(
gapfunc[3],
expected_gapfunc_w,
atol=1e-5,
err_msg="gapfunc.w should be surface_distance - (margin_A + margin_B)",
)
def test_03_margin_penetrating(self):
"""Penetrating spheres with margin: gapfunc.w = -penetration - margin."""
margin = 0.05
penetration = 0.01
builder = _make_sphere_pair_builder(distance=-penetration, margin_top=margin, margin_bottom=0.0)
contacts, _ = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(num_active, 1)
gapfunc = contacts.gapfunc.numpy()[0]
expected_gapfunc_w = -penetration - margin
np.testing.assert_allclose(
gapfunc[3],
expected_gapfunc_w,
atol=1e-5,
err_msg="gapfunc.w should be -penetration - margin",
)
# ------------------------------------------------------------------
# Gap tests
# ------------------------------------------------------------------
def test_04_gap_detects_contacts_before_touch(self):
"""Two spheres separated by a small distance: gap enables early detection."""
separation = 0.02
gap = 0.015
builder = _make_sphere_pair_builder(distance=separation, gap_top=gap, gap_bottom=gap)
contacts, _ = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(
num_active,
1,
f"Expected 1 contact: separation={separation} < gap_A+gap_B={2 * gap}",
)
def test_05_gap_no_contact_beyond_threshold(self):
"""Two spheres separated beyond the gap threshold: no contacts."""
separation = 0.04
gap = 0.015
builder = _make_sphere_pair_builder(distance=separation, gap_top=gap, gap_bottom=gap)
contacts, _ = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(
num_active,
0,
f"Expected 0 contacts: separation={separation} > gap_A+gap_B={2 * gap}",
)
def test_06_gap_does_not_shift_gapfunc(self):
"""Gap should not affect gapfunc.w — only margin shifts it."""
gap = 0.02
builder = _make_sphere_pair_builder(distance=0.0, gap_top=gap, gap_bottom=0.0)
contacts, _ = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(num_active, 1)
gapfunc = contacts.gapfunc.numpy()[0]
np.testing.assert_allclose(
gapfunc[3],
0.0,
atol=1e-5,
err_msg="gapfunc.w should be 0 for touching spheres with gap but no margin",
)
# ------------------------------------------------------------------
# Combined margin + gap tests
# ------------------------------------------------------------------
def test_07_margin_and_gap_combined_threshold(self):
"""Detection threshold = margin + gap; contacts detected within that range."""
margin = 0.03
gap = 0.02
separation = 0.04
builder = _make_sphere_pair_builder(
distance=separation,
margin_top=margin,
margin_bottom=0.0,
gap_top=gap,
gap_bottom=0.0,
)
contacts, _ = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(
num_active,
1,
f"Expected 1 contact: separation={separation} < margin+gap={margin + gap}",
)
def test_08_margin_and_gap_combined_gapfunc(self):
"""gapfunc.w = surface_distance - margin, regardless of gap."""
margin = 0.03
gap = 0.02
separation = 0.01
builder = _make_sphere_pair_builder(
distance=separation,
margin_top=margin,
margin_bottom=0.0,
gap_top=gap,
gap_bottom=0.0,
)
contacts, _ = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(num_active, 1)
gapfunc = contacts.gapfunc.numpy()[0]
expected_gapfunc_w = separation - margin
np.testing.assert_allclose(
gapfunc[3],
expected_gapfunc_w,
atol=1e-5,
err_msg="gapfunc.w should be surface_distance - margin",
)
def test_09_margin_and_gap_beyond_threshold(self):
"""No contacts when separation exceeds margin + gap."""
margin = 0.03
gap = 0.02
separation = 0.06
builder = _make_sphere_pair_builder(
distance=separation,
margin_top=margin,
margin_bottom=0.0,
gap_top=gap,
gap_bottom=0.0,
)
contacts, _ = _run_primitive_pipeline(builder, self.default_device)
num_active = int(contacts.model_active_contacts.numpy()[0])
self.assertEqual(
num_active,
0,
f"Expected 0 contacts: separation={separation} > margin+gap={margin + gap}",
)
###
# Helpers — Solver tests
###
GROUND_HALF_H = 0.5
SPHERE_R = 0.25
def _build_sphere_on_ground(
sphere_z: float,
margin: float = 0.0,
gap: float = 0.0,
) -> ModelBuilderKamino:
"""Build a free sphere above a static ground box, following ``build_free_joint_test`` pattern."""
builder = ModelBuilderKamino(default_world=True)
bid = builder.add_rigid_body(
name="sphere",
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(vec3f(0.0, 0.0, sphere_z), wp.quat_identity()),
u_i_0=vec6f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
)
builder.add_joint(
name="world_to_sphere",
dof_type=JointDoFType.FREE,
act_type=JointActuationType.FORCE,
bid_B=-1,
bid_F=bid,
B_r_Bj=vec3f(0.0, 0.0, sphere_z),
F_r_Fj=vec3f(0.0, 0.0, 0.0),
X_j=I_3,
)
builder.add_geometry(
name="sphere",
body=bid,
shape=SphereShape(SPHERE_R),
margin=margin,
gap=gap,
)
builder.add_geometry(
body=-1,
name="ground",
shape=BoxShape(2.0, 2.0, GROUND_HALF_H),
offset=transformf(vec3f(0.0, 0.0, 0.0), wp.quat_identity()),
margin=margin,
gap=gap,
)
return builder
def _fast_solver_config() -> SolverKaminoImpl.Config:
"""Relaxed solver config suitable for fast unit tests."""
config = SolverKaminoImpl.Config()
config.constraints.alpha = 0.1
config.padmm.primal_tolerance = 1e-3
config.padmm.dual_tolerance = 1e-3
config.padmm.compl_tolerance = 1e-3
config.padmm.max_iterations = 50
config.padmm.rho_0 = 0.05
config.padmm.use_acceleration = True
config.padmm.warmstart_mode = "containers"
config.padmm.use_graph_conditionals = False
config.collect_solver_info = False
config.compute_solution_metrics = False
return config
def _step_solver(builder: ModelBuilderKamino, device, num_steps: int = 5, dt: float = 0.01):
"""Finalize model, create solver + detector, step, return final z-position of the sphere."""
model = builder.finalize(device)
state_p = model.state()
state_n = model.state()
control = model.control()
detector = CollisionDetector(
model=model,
config=CollisionDetector.Config(pipeline="primitive", default_gap=0.0),
)
contacts = detector.contacts
solver = SolverKaminoImpl(model=model, contacts=contacts, config=_fast_solver_config())
for _ in range(num_steps):
solver.step(
state_in=state_p,
state_out=state_n,
control=control,
contacts=contacts,
detector=detector,
dt=dt,
)
wp.synchronize()
state_p.copy_from(state_n)
return float(state_n.q_i.numpy()[0][2])
###
# Tests — Solver rest-distance semantics
###
class TestMarginGapSolver(unittest.TestCase):
"""Verify that the solver treats margin as the rest offset and gap as detection-only.
Uses a single free-joint sphere above a static ground box (same pattern as
``build_free_joint_test``). Only 5 steps at dt=0.005 — enough to observe
the solver's immediate response without waiting for convergence.
"""
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
if test_context.verbose:
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
msg.reset_log_level()
def test_10_penetrating_margin_gets_pushed_out(self):
"""Sphere placed inside the margin envelope should be pushed upward."""
margin = 0.02
rest_z = GROUND_HALF_H + SPHERE_R + 2.0 * margin
start_z = rest_z - 0.01
final_z = _step_solver(
_build_sphere_on_ground(sphere_z=start_z, margin=margin, gap=0.01),
self.default_device,
)
self.assertGreater(
final_z,
start_z,
f"Sphere penetrating margin should be pushed up; z: {start_z:.5f} -> {final_z:.5f}",
)
def test_11_at_rest_with_margin_stays_put(self):
"""Sphere placed at the margin rest position should barely move."""
margin = 0.02
rest_z = GROUND_HALF_H + SPHERE_R + 2.0 * margin
final_z = _step_solver(
_build_sphere_on_ground(sphere_z=rest_z, margin=margin, gap=0.01),
self.default_device,
)
np.testing.assert_allclose(
final_z,
rest_z,
atol=2e-3,
err_msg=f"Sphere at margin rest should stay; moved to z={final_z:.5f}",
)
def test_12_gap_does_not_change_rest_position(self):
"""Rest position should be the same regardless of gap value."""
margin = 0.02
rest_z = GROUND_HALF_H + SPHERE_R + 2.0 * margin
for gap in (0.005, 0.05):
final_z = _step_solver(
_build_sphere_on_ground(sphere_z=rest_z, margin=margin, gap=gap),
self.default_device,
)
np.testing.assert_allclose(
final_z,
rest_z,
atol=2e-3,
err_msg=f"gap={gap}: sphere at rest should stay at z={rest_z:.5f}, got {final_z:.5f}",
)
def test_13_zero_margin_pushes_out_of_surface(self):
"""With zero margin, a penetrating sphere should be pushed toward the geometric surface."""
start_z = GROUND_HALF_H + SPHERE_R - 0.005
final_z = _step_solver(
_build_sphere_on_ground(sphere_z=start_z, margin=0.0, gap=0.01),
self.default_device,
)
self.assertGreater(
final_z,
start_z,
f"Sphere penetrating surface (margin=0) should be pushed up; z: {start_z:.5f} -> {final_z:.5f}",
)
###
# Test execution
###
if __name__ == "__main__":
setup_tests()
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_geometry_primitive.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the collider functions of narrow-phase collision detection"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.data import DataKamino
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.core.state import StateKamino
from newton._src.solvers.kamino._src.core.types import float32, int32, vec6f
from newton._src.solvers.kamino._src.geometry.contacts import DEFAULT_GEOM_PAIR_CONTACT_GAP, ContactsKamino
from newton._src.solvers.kamino._src.geometry.primitive import (
BoundingVolumeType,
CollisionPipelinePrimitive,
)
from newton._src.solvers.kamino._src.geometry.primitive.broadphase import (
PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES,
BoundingVolumesData,
CollisionCandidatesData,
CollisionCandidatesModel,
nxn_broadphase_aabb,
nxn_broadphase_bs,
update_geoms_aabb,
update_geoms_bs,
)
from newton._src.solvers.kamino._src.geometry.primitive.narrowphase import (
PRIMITIVE_NARROWPHASE_SUPPORTED_SHAPE_PAIRS,
primitive_narrowphase,
)
from newton._src.solvers.kamino._src.models.builders import basics, testing
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Constants
###
nominal_expected_contacts_per_shape_pair = {
("sphere", "sphere"): 1,
("sphere", "cylinder"): 1,
("sphere", "capsule"): 1,
("sphere", "box"): 1,
# TODO: ("sphere", "plane"): 1,
("cylinder", "sphere"): 1,
# TODO: ("cylinder", "plane"): 4,
("capsule", "sphere"): 1,
("capsule", "capsule"): 1,
("capsule", "box"): 1,
# TODO: ("capsule", "plane"): 1,
("box", "box"): 4,
# TODO: ("box", "plane"): 4,
# TODO: ("ellipsoid", "plane"): 1,
}
"""
Defines the expected number of contacts for each supported
shape combination under the following idealized conditions:
- all shapes are perfectly stacked along the vertical (z) axis
- all shapes are centered at the origin in the (x,y) plane
- the geoms are perfectly touching (i.e. penetration is exactly zero)
- all contact margins are set to zero
- all shapes are positioned and oriented in configurations
that would would generate a "nominal" number of contacts per shape combination
Notes:
- We refer to these "nominal" expected contacts as those that are neither the worst-case
(i.e. maximum possible contacts) nor the best-case (i.e. minimum possible contacts).
- An example of a "nominal" configuration is a box-on-box arrangement where two boxes are
perfectly aligned and touching face-to-face, generating 4 contact points. The worst-case
would be if the boxes were slightly offset, generating 8 contact points (i.e. full face-face
contact with 4 points on each face). The best-case would be if the boxes were touching at a
single edge or corner, generating only 1 contact point.
"""
###
# Test Scaffolding
###
class PrimitiveBroadPhaseTestBS:
def __init__(self, model: ModelKamino, device: wp.DeviceLike = None):
# Retrieve the number of world
num_worlds = model.size.num_worlds
num_geoms = model.geoms.num_geoms
# Construct collision pairs
world_num_geom_pairs, model_wid = CollisionPipelinePrimitive._assert_shapes_supported(model, True)
# Allocate the collision model data
with wp.ScopedDevice(device):
# Allocate the bounding volumes data
self.bvdata = BoundingVolumesData(radius=wp.zeros(shape=(num_geoms,), dtype=float32))
# Allocate the time-invariant collision candidates model
self._cmodel = CollisionCandidatesModel(
num_model_geom_pairs=model.geoms.num_collidable_pairs,
num_world_geom_pairs=world_num_geom_pairs,
model_num_pairs=wp.array([model.geoms.num_collidable_pairs], dtype=int32),
world_num_pairs=wp.array(world_num_geom_pairs, dtype=int32),
wid=wp.array(model_wid, dtype=int32),
geom_pair=model.geoms.collidable_pairs,
)
# Allocate the time-varying collision candidates data
self._cdata = CollisionCandidatesData(
num_model_geom_pairs=model.geoms.num_collidable_pairs,
model_num_collisions=wp.zeros(shape=(1,), dtype=int32),
world_num_collisions=wp.zeros(shape=(num_worlds,), dtype=int32),
wid=wp.zeros(shape=(model.geoms.num_collidable_pairs,), dtype=int32),
geom_pair=wp.zeros_like(model.geoms.collidable_pairs),
)
def collide(self, model: ModelKamino, data: DataKamino, state: StateKamino, default_gap: float = 0.0):
self._cdata.clear()
update_geoms_bs(state.q_i, model.geoms, data.geoms, self.bvdata, default_gap)
nxn_broadphase_bs(model.geoms, data.geoms, self.bvdata, self._cmodel, self._cdata)
class PrimitiveBroadPhaseTestAABB:
def __init__(self, model: ModelKamino, device: wp.DeviceLike = None):
# Retrieve the number of world
num_worlds = model.size.num_worlds
num_geoms = model.geoms.num_geoms
# Construct collision pairs
world_num_geom_pairs, model_wid = CollisionPipelinePrimitive._assert_shapes_supported(model, True)
# Allocate the collision model data
with wp.ScopedDevice(device):
# Allocate the bounding volumes data
self.bvdata = BoundingVolumesData(aabb=wp.zeros(shape=(num_geoms,), dtype=vec6f))
# Allocate the time-invariant collision candidates model
self._cmodel = CollisionCandidatesModel(
num_model_geom_pairs=model.geoms.num_collidable_pairs,
num_world_geom_pairs=world_num_geom_pairs,
model_num_pairs=wp.array([model.geoms.num_collidable_pairs], dtype=int32),
world_num_pairs=wp.array(world_num_geom_pairs, dtype=int32),
wid=wp.array(model_wid, dtype=int32),
geom_pair=model.geoms.collidable_pairs,
)
# Allocate the time-varying collision candidates data
self._cdata = CollisionCandidatesData(
num_model_geom_pairs=model.geoms.num_collidable_pairs,
model_num_collisions=wp.zeros(shape=(1,), dtype=int32),
world_num_collisions=wp.zeros(shape=(num_worlds,), dtype=int32),
wid=wp.zeros(shape=(model.geoms.num_collidable_pairs,), dtype=int32),
geom_pair=wp.zeros_like(model.geoms.collidable_pairs),
)
def collide(self, model: ModelKamino, data: DataKamino, state: StateKamino, default_gap: float = 0.0):
self._cdata.clear()
update_geoms_aabb(state.q_i, model.geoms, data.geoms, self.bvdata, default_gap)
nxn_broadphase_aabb(model.geoms, self.bvdata, self._cmodel, self._cdata)
PrimitiveBroadPhaseType = PrimitiveBroadPhaseTestBS | PrimitiveBroadPhaseTestAABB
"""Type alias for all primitive broad-phase implementations."""
###
# Testing Operations
###
def check_broadphase_allocations(
testcase: unittest.TestCase,
builder: ModelBuilderKamino,
broadphase: PrimitiveBroadPhaseType,
):
# Calculate the maximum number of geometry pairs
model_candidate_pairs = builder.make_collision_candidate_pairs()
num_candidate_pairs = len(model_candidate_pairs)
# Construct a broad-phase
testcase.assertEqual(broadphase._cmodel.num_model_geom_pairs, num_candidate_pairs)
testcase.assertEqual(sum(broadphase._cmodel.num_world_geom_pairs), num_candidate_pairs)
testcase.assertEqual(broadphase._cmodel.model_num_pairs.size, 1)
testcase.assertEqual(broadphase._cmodel.world_num_pairs.size, builder.num_worlds)
testcase.assertEqual(broadphase._cmodel.wid.size, num_candidate_pairs)
testcase.assertEqual(broadphase._cmodel.geom_pair.size, num_candidate_pairs)
np.testing.assert_array_equal(broadphase._cmodel.geom_pair.numpy(), model_candidate_pairs)
testcase.assertEqual(broadphase._cdata.model_num_collisions.size, 1)
testcase.assertEqual(broadphase._cdata.world_num_collisions.size, builder.num_worlds)
testcase.assertEqual(broadphase._cdata.wid.size, num_candidate_pairs)
testcase.assertEqual(broadphase._cdata.geom_pair.size, num_candidate_pairs)
def test_broadphase(
testcase: unittest.TestCase,
broadphase_type: type[PrimitiveBroadPhaseType],
builder: ModelBuilderKamino,
expected_model_collisions: int,
expected_world_collisions: list[int],
expected_worlds: list[int] | None = None,
gap: float = 0.0,
case_name: str = "",
device: wp.DeviceLike = None,
):
"""
Tests a primitive broad-phase backend on a system specified via a ModelBuilderKamino.
"""
# Create a test model and data
model = builder.finalize(device)
data = model.data()
state = model.state()
# Create a broad-phase backend
broadphase = broadphase_type(model=model, device=device)
check_broadphase_allocations(testcase, builder, broadphase)
# Perform broad-phase collision detection and check results
broadphase.collide(model, data, state, default_gap=gap)
# Check overall collision counts
num_model_collisions = broadphase._cdata.model_num_collisions.numpy()[0]
np.testing.assert_array_equal(
actual=num_model_collisions,
desired=expected_model_collisions,
err_msg=f"\n{broadphase_type.__name__}: Failed `model_num_collisions` check for {case_name}\n",
)
np.testing.assert_array_equal(
actual=broadphase._cdata.world_num_collisions.numpy(),
desired=expected_world_collisions,
err_msg=f"\n{broadphase_type.__name__}: Failed `world_num_collisions` check for {case_name}\n",
)
# Skip per-collision pair checks if there are no active collisions
if num_model_collisions == 0:
return
# Run per-collision checks
if expected_worlds is not None:
# Sort worlds since ordering of result might not be deterministic
expected_worlds_sorted = np.sort(expected_worlds)
actual_worlds_sorted = np.sort(broadphase._cdata.wid.numpy()[:num_model_collisions])
np.testing.assert_array_equal(
actual=actual_worlds_sorted,
desired=expected_worlds_sorted,
err_msg=f"\n{broadphase_type.__name__}: Failed `wid` check for {case_name}\n",
)
def test_broadphase_on_single_pair(
testcase: unittest.TestCase,
broadphase_type: type[PrimitiveBroadPhaseType],
shape_pair: tuple[str, str],
expected_collisions: int,
distance: float = 0.0,
gap: float = 0.0,
device: wp.DeviceLike = None,
):
"""
Tests a primitive broad-phase backend on a single shape pair.
"""
# Create a test model builder, model, and data
builder = testing.make_single_shape_pair_builder(shapes=shape_pair, distance=distance)
# Run the broad-phase test
test_broadphase(
testcase,
broadphase_type,
builder,
expected_collisions,
[expected_collisions],
gap=gap,
case_name=f"shape_pair='{shape_pair}', distance={distance}, gap={gap}",
device=device,
)
def check_contacts(
contacts: ContactsKamino,
expected: dict,
header: str,
case: str,
rtol: float = 1e-6,
atol: float = 0.0,
):
"""
Checks the contents of a ContactsKamino container against expected values.
"""
# Run contact counts checks
if "model_active_contacts" in expected:
np.testing.assert_equal(
actual=int(contacts.model_active_contacts.numpy()[0]),
desired=int(expected["model_active_contacts"]),
err_msg=f"\n{header}: Failed `model_active_contacts` check for `{case}`\n",
)
if "world_active_contacts" in expected:
np.testing.assert_equal(
actual=contacts.world_active_contacts.numpy(),
desired=expected["world_active_contacts"],
err_msg=f"\n{header}: Failed `world_active_contacts` check for `{case}`\n",
)
# Skip per-contact checks if there are no active contacts
num_active = contacts.model_active_contacts.numpy()[0]
if num_active == 0:
return
# Run per-contact assignment checks
if "wid" in expected:
np.testing.assert_equal(
actual=contacts.wid.numpy()[:num_active],
desired=np.zeros((num_active,), dtype=np.int32),
err_msg=f"\n{header}: Failed `wid` check for `{case}`\n",
)
if "cid" in expected:
np.testing.assert_equal(
actual=contacts.cid.numpy()[:num_active],
desired=np.arange(num_active, dtype=np.int32),
err_msg=f"\n{header}: Failed `cid` check for `{case}`\n",
)
# Run per-contact detailed checks
if "gid_AB" in expected:
np.testing.assert_equal(
actual=contacts.gid_AB.numpy()[:num_active],
desired=expected["gid_AB"],
err_msg=f"\n{header}: Failed `gid_AB` check for `{case}`\n",
)
if "bid_AB" in expected:
np.testing.assert_equal(
actual=contacts.bid_AB.numpy()[:num_active],
desired=expected["bid_AB"],
err_msg=f"\n{header}: Failed `bid_AB` check for `{case}`\n",
)
if "position_A" in expected:
np.testing.assert_allclose(
actual=contacts.position_A.numpy()[:num_active],
desired=expected["position_A"],
rtol=rtol,
atol=atol,
err_msg=f"\n{header}: Failed `position_A` check for `{case}`\n",
)
if "position_B" in expected:
np.testing.assert_allclose(
actual=contacts.position_B.numpy()[:num_active],
desired=expected["position_B"],
rtol=rtol,
atol=atol,
err_msg=f"\n{header}: Failed `position_B` check for `{case}`\n",
)
if "gapfunc" in expected:
np.testing.assert_allclose(
actual=contacts.gapfunc.numpy()[:num_active],
desired=expected["gapfunc"],
rtol=rtol,
atol=atol,
err_msg=f"{header}: Failed `gapfunc` check for `{case}`",
)
if "frame" in expected:
np.testing.assert_allclose(
actual=contacts.frame.numpy()[:num_active],
desired=expected["frame"],
rtol=rtol,
atol=atol,
err_msg=f"\n{header}: Failed `frame` check for `{case}`\n",
)
def test_narrowphase(
testcase: unittest.TestCase,
builder: ModelBuilderKamino,
expected: dict,
max_contacts_per_pair: int = 12,
gap: float = 0.0,
rtol: float = 1e-6,
atol: float = 0.0,
case: str = "",
device: wp.DeviceLike = None,
):
"""
Runs the primitive narrow-phase collider using all broad-phase backends
on a system specified via a ModelBuilderKamino and checks the results.
"""
# Run the narrow-phase test over each broad-phase backend
broadphase_types = [PrimitiveBroadPhaseTestAABB, PrimitiveBroadPhaseTestBS]
for bp_type in broadphase_types:
bp_name = bp_type.__name__
msg.info("Running narrow-phase test on '%s' using '%s'", case, bp_name)
# Create a test model and data
model = builder.finalize(device)
data = model.data()
state = model.state()
# Create a broad-phase backend
broadphase = bp_type(model=model, device=device)
broadphase.collide(model, data, state, default_gap=gap)
# Create a contacts container
_, world_req_contacts = builder.compute_required_contact_capacity(max_contacts_per_pair=max_contacts_per_pair)
contacts = ContactsKamino(capacity=world_req_contacts, device=device)
contacts.clear()
# Execute narrowphase for primitive shapes
primitive_narrowphase(model, data, broadphase._cdata, contacts, default_gap=gap)
# Optional verbose output
msg.debug("[%s][%s]: bodies.q_i:\n%s", case, bp_name, data.bodies.q_i)
msg.debug("[%s][%s]: contacts.model_active_contacts: %s", case, bp_name, contacts.model_active_contacts)
msg.debug("[%s][%s]: contacts.world_active_contacts: %s", case, bp_name, contacts.world_active_contacts)
msg.debug("[%s][%s]: contacts.wid: %s", case, bp_name, contacts.wid)
msg.debug("[%s][%s]: contacts.cid: %s", case, bp_name, contacts.cid)
msg.debug("[%s][%s]: contacts.gid_AB:\n%s", case, bp_name, contacts.gid_AB)
msg.debug("[%s][%s]: contacts.bid_AB:\n%s", case, bp_name, contacts.bid_AB)
msg.debug("[%s][%s]: contacts.position_A:\n%s", case, bp_name, contacts.position_A)
msg.debug("[%s][%s]: contacts.position_B:\n%s", case, bp_name, contacts.position_B)
msg.debug("[%s][%s]: contacts.gapfunc:\n%s", case, bp_name, contacts.gapfunc)
msg.debug("[%s][%s]: contacts.frame:\n%s", case, bp_name, contacts.frame)
msg.debug("[%s][%s]: contacts.material:\n%s", case, bp_name, contacts.material)
# Check results
check_contacts(
contacts,
expected,
rtol=rtol,
atol=atol,
case=f"{case} using {bp_name}",
header="primitive narrow-phase",
)
def test_narrowphase_on_shape_pair(
testcase: unittest.TestCase,
shape_pair: tuple[str, str],
expected_contacts: int,
distance: float = 0.0,
gap: float = 0.0,
builder_kwargs: dict | None = None,
):
"""
Tests the primitive narrow-phase collider on a single shape pair.
Note:
This test only checks the number of generated contacts.
"""
# Set default builder kwargs if none provided
if builder_kwargs is None:
builder_kwargs = {}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(shapes=shape_pair, distance=distance, **builder_kwargs)
# Define expected contacts dictionary
expected = {
"model_active_contacts": expected_contacts,
"world_active_contacts": [expected_contacts],
}
# Run the narrow-phase test
test_narrowphase(
testcase=testcase,
builder=builder,
expected=expected,
gap=gap,
case=f"shape_pair='{shape_pair}'",
device=testcase.default_device,
)
###
# Tests
###
class TestPrimitiveBroadPhase(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
# Construct a list of all supported primitive shape pairs
self.supported_shape_pairs: list[tuple[str, str]] = []
for shape_A in PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES:
shape_A_name = shape_A.name.lower()
for shape_B in PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES:
shape_B_name = shape_B.name.lower()
self.supported_shape_pairs.append((shape_A_name, shape_B_name))
msg.debug("supported_shape_pairs:\n%s\n", self.supported_shape_pairs)
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_bspheres_on_each_primitive_shape_pair_exact(self):
# Each shape pair in its own world with
# - zero distance: (i.e., exactly touching)
# - zero gap: no preemption of collisions
for shape_pair in self.supported_shape_pairs:
msg.info("[BS]: testing broadphase with exact boundaries on shape pair: %s", shape_pair)
test_broadphase_on_single_pair(
self,
broadphase_type=PrimitiveBroadPhaseTestBS,
shape_pair=shape_pair,
expected_collisions=1,
distance=0.0,
gap=0.0,
device=self.default_device,
)
def test_02_bspheres_on_each_primitive_shape_pair_apart(self):
# Each shape pair in its own world with
# - positive distance: (i.e., apart)
# - zero gap: no preemption of collisions
for shape_pair in self.supported_shape_pairs:
msg.info("[BS]: testing broadphase with shapes apart on shape pair: %s", shape_pair)
test_broadphase_on_single_pair(
self,
broadphase_type=PrimitiveBroadPhaseTestBS,
shape_pair=shape_pair,
expected_collisions=0,
distance=1.5,
gap=0.0,
device=self.default_device,
)
def test_03_bspheres_on_each_primitive_shape_pair_apart_with_margin(self):
# Each shape pair in its own world with
# - positive distance: (i.e., apart)
# - positive gap: preemption of collisions
for shape_pair in self.supported_shape_pairs:
msg.info("[BS]: testing broadphase with shapes apart but gap on shape pair: %s", shape_pair)
test_broadphase_on_single_pair(
self,
broadphase_type=PrimitiveBroadPhaseTestBS,
shape_pair=shape_pair,
expected_collisions=1,
distance=1.0,
gap=1.0,
device=self.default_device,
)
def test_04_bspheres_on_each_primitive_shape_pair_with_overlap(self):
# Each shape pair in its own world with
# - negative distance: (i.e., overlapping)
# - zero gap: no preemption of collisions
for shape_pair in self.supported_shape_pairs:
msg.info("[BS]: testing broadphase with overlapping shapes on shape pair: %s", shape_pair)
test_broadphase_on_single_pair(
self,
broadphase_type=PrimitiveBroadPhaseTestBS,
shape_pair=shape_pair,
expected_collisions=1,
distance=-0.01,
gap=0.0,
device=self.default_device,
)
def test_05_bspheres_on_all_primitive_shape_pairs(self):
# All shape pairs, but each in its own world with
# - zero distance: (i.e., exactly touching)
# - zero gap: no preemption of collisions
msg.info("[BS]: testing broadphase with overlapping shapes on all shape pairs")
builder = testing.make_shape_pairs_builder(
shape_pairs=self.supported_shape_pairs,
distance=0.0,
)
test_broadphase(
self,
builder=builder,
broadphase_type=PrimitiveBroadPhaseTestBS,
expected_model_collisions=len(self.supported_shape_pairs),
expected_world_collisions=[1] * len(self.supported_shape_pairs),
gap=0.0,
case_name="all shape pairs",
device=self.default_device,
)
def test_06_aabbs_on_each_primitive_shape_pair_exact(self):
# Each shape pair in its own world with
# - zero distance: (i.e., exactly touching)
# - zero gap: no preemption of collisions
for shape_pair in self.supported_shape_pairs:
msg.info("[AABB]: testing broadphase with exact boundaries on shape pair: %s", shape_pair)
test_broadphase_on_single_pair(
self,
broadphase_type=PrimitiveBroadPhaseTestAABB,
shape_pair=shape_pair,
expected_collisions=1,
distance=0.0,
gap=0.0,
device=self.default_device,
)
def test_07_aabbs_on_each_primitive_shape_pair_apart(self):
# Each shape pair in its own world with
# - positive distance: (i.e., apart)
# - zero gap: no preemption of collisions
for shape_pair in self.supported_shape_pairs:
msg.info("[AABB]: testing broadphase with shapes apart on shape pair: %s", shape_pair)
test_broadphase_on_single_pair(
self,
broadphase_type=PrimitiveBroadPhaseTestAABB,
shape_pair=shape_pair,
expected_collisions=0,
distance=1e-6,
gap=0.0,
device=self.default_device,
)
def test_08_aabbs_on_each_primitive_shape_pair_apart_with_margin(self):
# Each shape pair in its own world with
# - positive distance: (i.e., apart)
# - positive gap: preemption of collisions
for shape_pair in self.supported_shape_pairs:
msg.info("[AABB]: testing broadphase with shapes apart but gap on shape pair: %s", shape_pair)
test_broadphase_on_single_pair(
self,
broadphase_type=PrimitiveBroadPhaseTestAABB,
shape_pair=shape_pair,
expected_collisions=1,
distance=1e-6,
gap=1e-6,
device=self.default_device,
)
def test_09_aabbs_on_each_primitive_shape_pair_with_overlap(self):
# Each shape pair in its own world with
# - negative distance: (i.e., overlapping)
# - zero gap: no preemption of collisions
for shape_pair in self.supported_shape_pairs:
msg.info("[AABB]: testing broadphase with overlapping shapes on shape pair: %s", shape_pair)
test_broadphase_on_single_pair(
self,
broadphase_type=PrimitiveBroadPhaseTestAABB,
shape_pair=shape_pair,
expected_collisions=1,
distance=-0.01,
gap=0.0,
device=self.default_device,
)
def test_10_aabbs_on_all_primitive_shape_pairs(self):
# All shape pairs, but each in its own world with
# - zero distance: (i.e., exactly touching)
# - zero gap: no preemption of collisions
msg.info("[AABB]: testing broadphase with overlapping shapes on all shape pairs")
builder = testing.make_shape_pairs_builder(
shape_pairs=self.supported_shape_pairs,
distance=0.0,
)
test_broadphase(
self,
builder=builder,
broadphase_type=PrimitiveBroadPhaseTestAABB,
expected_model_collisions=len(self.supported_shape_pairs),
expected_world_collisions=[1] * len(self.supported_shape_pairs),
expected_worlds=list(range(len(self.supported_shape_pairs))),
gap=0.0,
case_name="all shape pairs",
device=self.default_device,
)
def test_11_bspheres_on_boxes_nunchaku(self):
msg.info("[BS]: testing broadphase on `boxes_nunchaku`")
builder = basics.build_boxes_nunchaku()
test_broadphase(
self,
builder=builder,
broadphase_type=PrimitiveBroadPhaseTestBS,
expected_model_collisions=3,
expected_world_collisions=[3],
expected_worlds=[0, 0, 0],
gap=0.0,
case_name="boxes_nunchaku",
device=self.default_device,
)
def test_12_aabbs_on_boxes_nunchaku(self):
msg.info("[AABB]: testing broadphase on `boxes_nunchaku`")
builder = basics.build_boxes_nunchaku()
test_broadphase(
self,
builder=builder,
broadphase_type=PrimitiveBroadPhaseTestAABB,
expected_model_collisions=3,
expected_world_collisions=[3],
expected_worlds=[0, 0, 0],
gap=0.0,
case_name="boxes_nunchaku",
device=self.default_device,
)
class TestPrimitiveNarrowPhase(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
# Construct a list of all supported primitive shape pairs
self.supported_shape_pairs: list[tuple[str, str]] = []
for shape_A in PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES:
shape_A_name = shape_A.name.lower()
for shape_B in PRIMITIVE_BROADPHASE_SUPPORTED_SHAPES:
shape_B_name = shape_B.name.lower()
if (shape_A, shape_B) in PRIMITIVE_NARROWPHASE_SUPPORTED_SHAPE_PAIRS:
self.supported_shape_pairs.append((shape_A_name, shape_B_name))
msg.debug("supported_shape_pairs:\n%s\n", self.supported_shape_pairs)
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_on_each_primitive_shape_pair_exact(self):
"""
Tests the narrow-phase collision detection for each supported primitive
shape pair when placed exactly at their contact boundaries.
"""
msg.info("Testing narrow-phase tests with exact boundaries")
# Each shape pair in its own world with
# - zero distance: (i.e., exactly touching)
# - zero gap: no preemption of collisions
for shape_pair in nominal_expected_contacts_per_shape_pair.keys():
# Define any special kwargs for specific shape pairs
kwargs = {}
if shape_pair == ("box", "box"):
# NOTE: To asses "nominal" contacts for box-box,
# we need to specify larger box dimensions for
# the bottom box to avoid contacts on edges
kwargs["bottom_dims"] = (2.0, 2.0, 1.0)
# Retrieve the nominal expected contacts for the shape pair
expected_contacts = nominal_expected_contacts_per_shape_pair.get(shape_pair, 0)
# Run the narrow-phase test on the shape pair
test_narrowphase_on_shape_pair(
self,
shape_pair=shape_pair,
expected_contacts=expected_contacts,
gap=0.0, # No contact gap
distance=0.0, # Exactly touching
builder_kwargs=kwargs,
)
def test_02_on_each_primitive_shape_pair_apart(self):
"""
Tests the narrow-phase collision detection for each
supported primitive shape pair when placed apart.
"""
msg.info("Testing narrow-phase tests with shapes apart")
# Each shape pair in its own world with
# - zero distance: (i.e., exactly touching)
# - zero gap: no preemption of collisions
for shape_pair in nominal_expected_contacts_per_shape_pair.keys():
test_narrowphase_on_shape_pair(
self,
shape_pair=shape_pair,
expected_contacts=0,
gap=0.0, # No contact gap
distance=1e-6, # Shapes apart
)
def test_03_on_each_primitive_shape_pair_apart_with_margin(self):
"""
Tests the narrow-phase collision detection for each supported
primitive shape pair when placed apart but with contact gap.
"""
msg.info("Testing narrow-phase tests with shapes apart")
# Each shape pair in its own world with
# - zero distance: (i.e., exactly touching)
# - zero gap: no preemption of collisions
for shape_pair in nominal_expected_contacts_per_shape_pair.keys():
# Define any special kwargs for specific shape pairs
kwargs = {}
if shape_pair == ("box", "box"):
# NOTE: To asses "nominal" contacts for box-box,
# we need to specify larger box dimensions for
# the bottom box to avoid contacts on edges
kwargs["bottom_dims"] = (2.0, 2.0, 1.0)
# Retrieve the nominal expected contacts for the shape pair
expected_contacts = nominal_expected_contacts_per_shape_pair.get(shape_pair, 0)
# Run the narrow-phase test on the shape pair
test_narrowphase_on_shape_pair(
self,
shape_pair=shape_pair,
expected_contacts=expected_contacts,
gap=1e-6, # Contact gap
distance=1e-6, # Shapes apart
builder_kwargs=kwargs,
)
###
# Tests for special cases of shape combinations/configurations
###
def test_04_on_sphere_on_sphere_full(self):
"""
Tests all narrow-phase output data for the case of two spheres
stacked along the vertical (z) axis, centered at the origin
in the (x,y) plane, and slightly penetrating each other.
"""
# NOTE: We set to negative value to move the geoms into each other,
# i.e. move the bottom geom upwards and the top geom downwards.
distance = -0.01
# Define expected contact data
expected = {
"model_active_contacts": 1,
"world_active_contacts": [1],
"gid_AB": np.array([[0, 1]], dtype=np.int32),
"bid_AB": np.array([[0, 1]], dtype=np.int32),
"position_A": np.array([[0.0, 0.0, 0.5 * abs(distance)]], dtype=np.float32),
"position_B": np.array([[0.0, 0.0, -0.5 * abs(distance)]], dtype=np.float32),
"gapfunc": np.array([[0.0, 0.0, 1.0, distance]], dtype=np.float32),
"frame": np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32),
}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(
shapes=("sphere", "sphere"),
distance=distance,
)
# Run the narrow-phase test on the shape pair
test_narrowphase(
self,
builder=builder,
expected=expected,
max_contacts_per_pair=2,
case="sphere_on_sphere_detailed",
device=self.default_device,
)
def test_05_box_on_box_with_four_points(self):
"""
Tests all narrow-phase output data for the case of two boxes
stacked along the vertical (z) axis, centered at the origin
in the (x,y) plane, and slightly penetrating each other.
This test makes the bottom box larger in the (x,y) dimensions
to ensure that only four contact points are generated at the
face of the top box.
"""
# NOTE: We set to negative value to move the geoms into each other,
# i.e. move the bottom geom upwards and the top geom downwards.
distance = -0.01
# Define expected contact data
expected = {
"model_active_contacts": 4,
"world_active_contacts": [4],
"gid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(4, 1)),
"bid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(4, 1)),
"position_A": np.array(
[
[-0.5, -0.5, 0.5 * abs(distance)],
[0.5, -0.5, 0.5 * abs(distance)],
[-0.5, 0.5, 0.5 * abs(distance)],
[0.5, 0.5, 0.5 * abs(distance)],
],
dtype=np.float32,
),
"position_B": np.array(
[
[-0.5, -0.5, -0.5 * abs(distance)],
[0.5, -0.5, -0.5 * abs(distance)],
[-0.5, 0.5, -0.5 * abs(distance)],
[0.5, 0.5, -0.5 * abs(distance)],
],
dtype=np.float32,
),
"gapfunc": np.tile(np.array([0.0, 0.0, 1.0, distance], dtype=np.float32), reps=(4, 1)),
"frame": np.tile(np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), reps=(4, 1)),
}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(
shapes=("box", "box"),
distance=distance,
bottom_dims=(2.0, 2.0, 1.0), # Larger bottom box
)
# Run the narrow-phase test on the shape pair
test_narrowphase(
self,
builder=builder,
expected=expected,
case="box_on_box_four_points",
device=self.default_device,
)
def test_06_box_on_box_eight_points(self):
"""
Tests the narrow-phase collision detection for a special case of
two boxes stacked along the vertical (z) axis, centered at the origin
in the (x,y) plane, and slightly penetrating each other.
"""
# NOTE: We set to negative value to move the geoms into each other,
# i.e. move the bottom geom upwards and the top geom downwards.
distance = -0.01
# Define expected contact data
expected = {
"model_active_contacts": 8,
"world_active_contacts": [8],
"gid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(8, 1)),
"bid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(8, 1)),
"position_A": np.array(
[
[-0.207107, -0.5, 0.5 * abs(distance)],
[0.207107, -0.5, 0.5 * abs(distance)],
[-0.5, -0.207107, 0.5 * abs(distance)],
[-0.5, 0.207107, 0.5 * abs(distance)],
[0.5, 0.207107, 0.5 * abs(distance)],
[0.5, -0.207107, 0.5 * abs(distance)],
[0.207107, 0.5, 0.5 * abs(distance)],
[-0.207107, 0.5, 0.5 * abs(distance)],
],
dtype=np.float32,
),
"position_B": np.array(
[
[-0.207107, -0.5, -0.5 * abs(distance)],
[0.207107, -0.5, -0.5 * abs(distance)],
[-0.5, -0.207107, -0.5 * abs(distance)],
[-0.5, 0.207107, -0.5 * abs(distance)],
[0.5, 0.207107, -0.5 * abs(distance)],
[0.5, -0.207107, -0.5 * abs(distance)],
[0.207107, 0.5, -0.5 * abs(distance)],
[-0.207107, 0.5, -0.5 * abs(distance)],
],
dtype=np.float32,
),
"gapfunc": np.tile(np.array([0.0, 0.0, 1.0, distance], dtype=np.float32), reps=(8, 1)),
"frame": np.tile(np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), reps=(8, 1)),
}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(
shapes=("box", "box"),
distance=distance,
top_rpy=[0.0, 0.0, np.pi / 4],
)
# Run the narrow-phase test on the shape pair
test_narrowphase(
self,
builder=builder,
expected=expected,
case="box_on_box_eight_points",
device=self.default_device,
rtol=1e-5,
atol=1e-6,
)
def test_07_on_box_on_box_one_point(self):
"""
Tests the narrow-phase collision detection for a special case of
two boxes stacked along the vertical (z) axis, centered at the origin
in the (x,y) plane, and the top box rotated so two diagonally opposing corners
lie exactly on the Z-axis. thus the bottom corner of the top box touches the
top face of the bottom box at a single point, slightly penetrating each other.
"""
# NOTE: We set to negative value to move the geoms into each other,
# i.e. move the bottom geom upwards and the top geom downwards.
penetration = -0.01
# Define expected contact data
expected = {
"num_contacts": 1,
"gid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(1, 1)),
"bid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(1, 1)),
"position_A": np.array([[0.0, 0.0, 0.0]], dtype=np.float32),
"position_B": np.array([[0.0, 0.0, -abs(penetration)]], dtype=np.float32),
"gapfunc": np.tile(np.array([0.0, 0.0, 1.0, penetration], dtype=np.float32), reps=(1, 1)),
"frame": np.tile(np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), reps=(1, 1)),
}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(
shapes=("box", "box"),
top_xyz=[0.0, 0.0, 0.5 * np.sqrt(3) + 0.5],
top_rpy=[np.pi / 4, -np.arctan(1.0 / np.sqrt(2)), 0.0],
)
# Run the narrow-phase test on the shape pair
test_narrowphase(
self,
builder=builder,
expected=expected,
case="box_on_box_one_point",
device=self.default_device,
rtol=1e-5,
atol=1e-6,
)
class TestPipelinePrimitive(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default(self):
"""Tests the default constructor of CollisionPipelinePrimitive."""
pipeline = CollisionPipelinePrimitive()
self.assertIsNone(pipeline._device)
self.assertEqual(pipeline._bvtype, BoundingVolumeType.AABB)
self.assertEqual(pipeline._default_gap, DEFAULT_GEOM_PAIR_CONTACT_GAP)
self.assertRaises(RuntimeError, pipeline.collide, ModelKamino(), DataKamino(), ContactsKamino())
def test_02_make_and_collide(self):
"""
Tests the construction and execution
of the CollisionPipelinePrimitive on
all supported primitive shape pairs.
"""
# Create a list of collidable shape pairs and their reversed versions
collidable_shape_pairs = list(nominal_expected_contacts_per_shape_pair.keys())
msg.debug("collidable_shape_pairs:\n%s\n", collidable_shape_pairs)
# Define any special kwargs for specific shape pairs
per_shape_pair_args = {}
per_shape_pair_args[("box", "box")] = {
# NOTE: To asses "nominal" contacts for box-box,
# we need to specify larger box dimensions for
# the bottom box to avoid contacts on edges
"bottom_dims": (2.0, 2.0, 1.0)
}
# Create a builder for all supported shape pairs
builder = testing.make_shape_pairs_builder(
shape_pairs=collidable_shape_pairs, per_shape_pair_args=per_shape_pair_args
)
model = builder.finalize(device=self.default_device)
data = model.data()
state = model.state()
# Create a contacts container
max_contacts_per_pair = 12 # Conservative estimate based on max contacts for any supported shape pair
_, world_req_contacts = builder.compute_required_contact_capacity(max_contacts_per_pair=max_contacts_per_pair)
contacts = ContactsKamino(capacity=world_req_contacts, device=self.default_device)
contacts.clear()
# Create the collision pipeline
pipeline = CollisionPipelinePrimitive(model=model, device=self.default_device)
# Run collision detection
pipeline.collide(data, state, contacts)
# Create a list of expected number of contacts per shape pair
expected_contacts_per_pair: list[int] = list(nominal_expected_contacts_per_shape_pair.values())
msg.debug("expected_contacts_per_pair:\n%s\n", expected_contacts_per_pair)
# Define expected contacts dictionary
expected = {
"model_active_contacts": sum(expected_contacts_per_pair),
"world_active_contacts": np.array(expected_contacts_per_pair, dtype=np.int32),
}
# Check results
check_contacts(
contacts,
expected,
case="all shape pairs",
header="pipeline primitive narrow-phase",
)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_geometry_unified.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Unit tests for `geometry/unified.py`
Tests the unified collision detection pipeline.
"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.data import DataKamino
from newton._src.solvers.kamino._src.core.math import I_3
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.core.shapes import SphereShape
from newton._src.solvers.kamino._src.core.state import StateKamino
from newton._src.solvers.kamino._src.core.types import transformf, vec6f
from newton._src.solvers.kamino._src.geometry.contacts import ContactsKamino
from newton._src.solvers.kamino._src.geometry.unified import CollisionPipelineUnifiedKamino
from newton._src.solvers.kamino._src.models.builders import basics, testing
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.test_geometry_primitive import check_contacts
###
# Constants
###
nominal_expected_contacts_per_shape_pair = {
("sphere", "sphere"): 1,
("sphere", "cylinder"): 1,
("sphere", "cone"): 1,
("sphere", "capsule"): 1,
("sphere", "box"): 1,
("sphere", "ellipsoid"): 1,
("sphere", "plane"): 1,
("cylinder", "sphere"): 1,
("cylinder", "cylinder"): 4,
("cylinder", "cone"): 1,
("cylinder", "capsule"): 1,
("cylinder", "box"): 4,
("cylinder", "ellipsoid"): 1,
("cylinder", "plane"): 3, # GJK manifold on the circular face yields 3 points
("cone", "sphere"): 1,
("cone", "cylinder"): 4,
("cone", "cone"): 1,
("cone", "capsule"): 1,
("cone", "box"): 4,
("cone", "ellipsoid"): 1,
("cone", "plane"): 4,
("capsule", "cone"): 1,
("capsule", "capsule"): 1,
("capsule", "box"): 1,
("capsule", "ellipsoid"): 1,
("capsule", "plane"): 1,
("box", "cone"): 1,
("box", "box"): 4,
("box", "ellipsoid"): 1,
("box", "plane"): 4,
("ellipsoid", "cone"): 1,
("ellipsoid", "ellipsoid"): 1,
("ellipsoid", "plane"): 1,
}
"""
Defines the expected number of contacts for each supported
shape combination under the following idealized conditions:
- all shapes are perfectly stacked along the vertical (z) axis
- all shapes are centered at the origin in the (x,y) plane
- the geoms are perfectly touching (i.e. penetration is exactly zero)
- all contact margins are set to zero
- all shapes are positioned and oriented in configurations
that would would generate a "nominal" number of contacts per shape combination
Notes:
- We refer to these "nominal" expected contacts as those that are neither the worst-case
(i.e. maximum possible contacts) nor the best-case (i.e. minimum possible contacts).
- An example of a "nominal" configuration is a box-on-box arrangement where two boxes are
perfectly aligned and touching face-to-face, generating 4 contact points. The worst-case
would be if the boxes were slightly offset, generating 8 contact points (i.e. full face-face
contact with 4 points on each face). The best-case would be if the boxes were touching at a
single edge or corner, generating only 1 contact point.
"""
###
# Testing Operations
###
def test_unified_pipeline(
builder: ModelBuilderKamino,
expected: dict,
max_contacts_per_pair: int = 12,
margin: float = 0.0,
rtol: float = 1e-6,
atol: float = 0.0,
case: str = "",
broadphase_modes: list[str] | None = None,
device: wp.DeviceLike = None,
):
"""
Runs the unified collision detection pipeline using all broad-phase backends
on a system specified via a ModelBuilderKamino and checks the results.
"""
# Run the narrow-phase test over each broad-phase backend
if broadphase_modes is None:
broadphase_modes = ["nxn", "sap", "explicit"]
for bp_mode in broadphase_modes:
msg.info("Testing unified CD on '%s' using '%s'", case, bp_mode)
# Create a test model and data
model: ModelKamino = builder.finalize(device)
data: DataKamino = model.data()
state: StateKamino = model.state()
# Create a pipeline
pipeline = CollisionPipelineUnifiedKamino(
model=model,
broadphase=bp_mode,
default_gap=margin,
device=device,
)
# Create a contacts container using the worst-case capacity of NxN over model-wise geom pairs
# NOTE: This is required by the unified pipeline when using SAP and NXN broad-phases
capacity = 2 * max_contacts_per_pair * ((builder.num_geoms * (builder.num_geoms - 1)) // 2)
contacts = ContactsKamino(capacity=capacity, device=device)
contacts.clear()
# Execute the unified collision detection pipeline
pipeline.collide(data, state, contacts)
# Optional verbose output
msg.debug("[%s][%s]: bodies.q_i:\n%s", case, bp_mode, data.bodies.q_i)
msg.debug("[%s][%s]: contacts.model_active_contacts: %s", case, bp_mode, contacts.model_active_contacts)
msg.debug("[%s][%s]: contacts.world_active_contacts: %s", case, bp_mode, contacts.world_active_contacts)
msg.debug("[%s][%s]: contacts.wid: %s", case, bp_mode, contacts.wid)
msg.debug("[%s][%s]: contacts.cid: %s", case, bp_mode, contacts.cid)
msg.debug("[%s][%s]: contacts.gid_AB:\n%s", case, bp_mode, contacts.gid_AB)
msg.debug("[%s][%s]: contacts.bid_AB:\n%s", case, bp_mode, contacts.bid_AB)
msg.debug("[%s][%s]: contacts.position_A:\n%s", case, bp_mode, contacts.position_A)
msg.debug("[%s][%s]: contacts.position_B:\n%s", case, bp_mode, contacts.position_B)
msg.debug("[%s][%s]: contacts.gapfunc:\n%s", case, bp_mode, contacts.gapfunc)
msg.debug("[%s][%s]: contacts.frame:\n%s", case, bp_mode, contacts.frame)
msg.debug("[%s][%s]: contacts.material:\n%s", case, bp_mode, contacts.material)
# Check results
check_contacts(
contacts,
expected,
rtol=rtol,
atol=atol,
case=f"{case} using {bp_mode}",
header="unified CD pipeline",
)
def test_unified_pipeline_on_shape_pair(
testcase: unittest.TestCase,
shape_pair: tuple[str, str],
expected_contacts: int,
distance: float = 0.0,
margin: float = 0.0,
builder_kwargs: dict | None = None,
):
"""
Tests the unified collision detection pipeline on a single shape pair.
Note:
This test only checks the number of generated contacts.
"""
# Set default builder kwargs if none provided
if builder_kwargs is None:
builder_kwargs = {}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(shapes=shape_pair, distance=distance, **builder_kwargs)
# Define expected contacts dictionary
expected = {
"model_active_contacts": expected_contacts,
"world_active_contacts": [expected_contacts],
}
# Run the narrow-phase test
test_unified_pipeline(
builder=builder,
expected=expected,
margin=margin,
case=f"shape_pair='{shape_pair}'",
device=testcase.default_device,
)
###
# Tests
###
class TestCollisionPipelineUnified(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
self.skip_buggy_tests = False # Set to True to skip known-buggy tests
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
# Generate a list of supported shape pairs
self.supported_shape_pairs = nominal_expected_contacts_per_shape_pair.keys()
msg.debug("Supported shape pairs for unified pipeline tests:\n%s", self.supported_shape_pairs)
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_on_specific_primitive_shape_pair(self):
"""
Tests the unified collision pipeline on a specific primitive shape pair.
NOTE: This is mainly for debugging purposes, where we can easily test a specific case.
"""
if self.skip_buggy_tests:
self.skipTest("Skipping 'specific_primitive_shape_pair' test")
# Define the specific shape pair to test
shape_pair = ("cylinder", "ellipsoid")
msg.info(f"Testing narrow-phase tests with exact boundaries on {shape_pair}")
# Define any special kwargs for specific shape pairs
kwargs = {
"top_dims": (0.5, 1.0), # radius, height of cylinder
"bottom_dims": (1.0, 1.0, 0.5), # radii(a,b,c) of ellipsoid
}
# Retrieve the nominal expected contacts for the shape pair
# TODO: This should be =1, but generates =4
expected_contacts = 1
# Run the narrow-phase test on the shape pair
test_unified_pipeline_on_shape_pair(
self,
shape_pair=shape_pair,
expected_contacts=expected_contacts,
margin=0.0, # No contact margin
distance=1.0e-8, # Exactly touching
builder_kwargs=kwargs,
)
def test_01_on_each_primitive_shape_pair_touching(self):
"""
Tests the unified collision pipeline on each supported primitive
shape pair when placed exactly at their contact boundaries.
"""
msg.info("Testing unified pipeline tests with exact boundaries")
# Each shape pair in its own world with
for shape_pair in self.supported_shape_pairs:
# Define any special kwargs for specific shape pairs
kwargs = {}
if shape_pair == ("box", "box"):
# NOTE: To asses "nominal" contacts for box-box,
# we need to specify larger box dimensions for
# the bottom box to avoid contacts on edges
kwargs["bottom_dims"] = (2.0, 2.0, 1.0)
# Retrieve the nominal expected contacts for the shape pair
expected_contacts = nominal_expected_contacts_per_shape_pair.get(shape_pair, 0)
# Run the narrow-phase test on the shape pair
test_unified_pipeline_on_shape_pair(
self,
shape_pair=shape_pair,
expected_contacts=expected_contacts,
margin=1.0e-5, # Default contact margin
distance=0.0, # Exactly touching
builder_kwargs=kwargs,
)
def test_02_on_each_primitive_shape_pair_apart(self):
"""
Tests the unified collision pipeline on each
supported primitive shape pair when placed apart.
"""
msg.info("Testing unified pipeline tests with shapes apart")
# Each shape pair in its own world with
for shape_pair in self.supported_shape_pairs:
test_unified_pipeline_on_shape_pair(
self,
shape_pair=shape_pair,
expected_contacts=0,
margin=0.0, # No contact margin
distance=1e-6, # Shapes apart with epsilon distance
)
def test_03_on_each_primitive_shape_pair_apart_with_margin(self):
"""
Tests the unified collision pipeline on each supported
primitive shape pair when placed apart but with contact margin.
"""
msg.info("Testing unified pipeline tests with shapes apart")
# Each shape pair in its own world with
# - zero distance: (i.e., exactly touching)
# - zero margin: no preemption of collisions
for shape_pair in self.supported_shape_pairs:
# Define any special kwargs for specific shape pairs
kwargs = {}
if shape_pair == ("box", "box"):
# NOTE: To asses "nominal" contacts for box-box,
# we need to specify larger box dimensions for
# the bottom box to avoid contacts on edges
kwargs["bottom_dims"] = (2.0, 2.0, 1.0)
# Retrieve the nominal expected contacts for the shape pair
expected_contacts = nominal_expected_contacts_per_shape_pair.get(shape_pair, 0)
# Run the narrow-phase test on the shape pair
test_unified_pipeline_on_shape_pair(
self,
shape_pair=shape_pair,
expected_contacts=expected_contacts,
margin=1e-5, # Contact margin
distance=1e-6, # Shapes apart
builder_kwargs=kwargs,
)
###
# Tests for special cases of shape combinations/configurations
###
def test_04_sphere_on_sphere_detailed(self):
"""
Tests all unified pipeline output data for the case of two spheres
stacked along the vertical (z) axis, centered at the origin
in the (x,y) plane, and slightly penetrating each other.
"""
if self.skip_buggy_tests:
self.skipTest("Skipping `sphere_on_sphere_detailed` test")
# NOTE: We set to negative value to move the geoms into each other,
# i.e. move the bottom geom upwards and the top geom downwards.
distance = 0.0
# Define expected contact data
expected = {
"model_active_contacts": 1,
"world_active_contacts": [1],
"gid_AB": np.array([[0, 1]], dtype=np.int32),
"bid_AB": np.array([[0, 1]], dtype=np.int32),
"position_A": np.array([[0.0, 0.0, 0.5 * abs(distance)]], dtype=np.float32),
"position_B": np.array([[0.0, 0.0, -0.5 * abs(distance)]], dtype=np.float32),
"gapfunc": np.array([[0.0, 0.0, 1.0, distance]], dtype=np.float32),
"frame": np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32),
}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(
shapes=("sphere", "sphere"),
distance=distance,
)
# Run the narrow-phase test on the shape pair
test_unified_pipeline(
builder=builder,
expected=expected,
case="sphere_on_sphere_detailed",
device=self.default_device,
# rtol=0.0,
# atol=1e-5,
)
def test_05_box_on_box_simple(self):
"""
Tests unified pipeline output contacts for the case of two boxes
stacked along the vertical (z) axis, centered at the origin in
the (x,y) plane, and slightly penetrating each other.
This test makes the bottom box larger in the (x,y) dimensions
to ensure that only four contact points are generated at the
face of the top box.
"""
# NOTE: We set to negative value to move the geoms into each other,
# i.e. move the bottom geom upwards and the top geom downwards.
distance = -0.01
# Define expected contact data
expected = {
"model_active_contacts": 4,
"world_active_contacts": [4],
"gid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(4, 1)),
"bid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(4, 1)),
"position_A": np.array(
[
[-0.5, -0.5, 0.5 * abs(distance)],
[0.5, -0.5, 0.5 * abs(distance)],
[0.5, 0.5, 0.5 * abs(distance)],
[-0.5, 0.5, 0.5 * abs(distance)],
],
dtype=np.float32,
),
"position_B": np.array(
[
[-0.5, -0.5, -0.5 * abs(distance)],
[0.5, -0.5, -0.5 * abs(distance)],
[0.5, 0.5, -0.5 * abs(distance)],
[-0.5, 0.5, -0.5 * abs(distance)],
],
dtype=np.float32,
),
"gapfunc": np.tile(np.array([0.0, 0.0, 1.0, distance], dtype=np.float32), reps=(4, 1)),
"frame": np.tile(np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), reps=(4, 1)),
}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(
shapes=("box", "box"),
distance=distance,
bottom_dims=(2.0, 2.0, 1.0), # Larger bottom box
)
# Run the narrow-phase test on the shape pair
test_unified_pipeline(
builder=builder,
expected=expected,
case="box_on_box_simple",
device=self.default_device,
rtol=0.0,
atol=1e-5,
)
def test_07_on_box_on_box_vertex_on_face(self):
"""
Tests the unified pipeline on the special case of two boxes
stacked along the vertical (z) axis, centered at the origin
in the (x,y) plane, and the top box rotated so that one of
its lowest vertex is touching the top face of the bottom box.
"""
# NOTE: We set to negative value to move the geoms into each other,
# i.e. move the bottom geom upwards and the top geom downwards.
penetration = -0.01
# Define expected contact data
expected = {
"num_contacts": 1,
"gid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(1, 1)),
"bid_AB": np.tile(np.array([0, 1], dtype=np.int32), reps=(1, 1)),
"position_A": np.array([[0.0, 0.0, 0.0]], dtype=np.float32),
"position_B": np.array([[0.0, 0.0, -abs(penetration)]], dtype=np.float32),
"gapfunc": np.tile(np.array([0.0, 0.0, 1.0, penetration], dtype=np.float32), reps=(1, 1)),
"frame": np.tile(np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), reps=(1, 1)),
}
# Create a builder for the specified shape pair
builder = testing.make_single_shape_pair_builder(
shapes=("box", "box"),
top_xyz=[0.0, 0.0, 0.5 * np.sqrt(3) + 0.5],
top_rpy=[np.pi / 4, -np.arctan(1.0 / np.sqrt(2)), 0.0],
)
# Run the narrow-phase test on the shape pair
test_unified_pipeline(
builder=builder,
expected=expected,
case="box_on_box_vertex_on_face",
device=self.default_device,
rtol=1e-5,
atol=1e-6,
)
def test_08_on_boxes_nunchaku(self):
"""
Tests the unified collision detection pipeline on the boxes_nunchaku model.
"""
# Define expected contact data
expected = {
"model_active_contacts": 9,
"world_active_contacts": [9],
}
# Create a builder for the specified shape pair
builder = basics.build_boxes_nunchaku()
# Run the narrow-phase test on the shape pair
# Note: Use small margin to handle floating point precision for touching contacts
test_unified_pipeline(
builder=builder,
expected=expected,
case="boxes_nunchaku",
broadphase_modes=["explicit"],
margin=1e-6,
device=self.default_device,
)
class TestUnifiedWriterContactDataRegression(unittest.TestCase):
"""
Regression tests for the unified writer's ContactData API usage.
The writer previously referenced ``thickness_a/b`` fields on
:class:`ContactData` which no longer exist; the correct fields are
``radius_eff_a/b`` and ``margin_a/b``. It also used a max-based
margin threshold instead of additive per-shape gap. These tests
verify the corrected behaviour end-to-end.
"""
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
def tearDown(self):
self.default_device = None
def _run_pipeline(self, builder: ModelBuilderKamino, default_gap=0.0):
model = builder.finalize(self.default_device)
data = model.data()
state = model.state()
pipeline = CollisionPipelineUnifiedKamino(
model=model,
broadphase="explicit",
default_gap=default_gap,
device=self.default_device,
)
n_geoms = builder.num_geoms
capacity = 8 * ((n_geoms * (n_geoms - 1)) // 2)
contacts = ContactsKamino(capacity=max(capacity, 8), device=self.default_device)
contacts.clear()
pipeline.collide(data, state, contacts)
return contacts
def test_00_touching_spheres_produces_contact(self):
"""Two touching spheres must generate exactly one contact with d ≈ 0."""
builder = testing.make_single_shape_pair_builder(
shapes=("sphere", "sphere"),
distance=0.0,
)
contacts = self._run_pipeline(builder, default_gap=1e-5)
active = contacts.model_active_contacts.numpy()[0]
self.assertEqual(active, 1, "Touching spheres should produce one contact")
gapfunc = contacts.gapfunc.numpy()[0]
self.assertAlmostEqual(float(gapfunc[3]), 0.0, places=4, msg="gapfunc.w should be ≈ 0 for touching spheres")
def test_01_penetrating_spheres_negative_distance(self):
"""Penetrating spheres must produce a negative gapfunc.w."""
penetration = -0.02
builder = testing.make_single_shape_pair_builder(
shapes=("sphere", "sphere"),
distance=penetration,
)
contacts = self._run_pipeline(builder, default_gap=1e-5)
active = contacts.model_active_contacts.numpy()[0]
self.assertEqual(active, 1)
gapfunc = contacts.gapfunc.numpy()[0]
self.assertLess(float(gapfunc[3]), 0.0, "gapfunc.w must be negative for penetrating spheres")
def test_02_gap_retains_nearby_contact(self):
"""Contact within detection gap must be retained by the writer."""
separation = 1e-6
builder = testing.make_single_shape_pair_builder(
shapes=("sphere", "sphere"),
distance=separation,
)
for geom in builder.geoms:
geom.gap = 0.01
contacts = self._run_pipeline(builder)
active = contacts.model_active_contacts.numpy()[0]
self.assertEqual(active, 1, "Contact within gap must be retained")
def test_03_gap_rejects_distant_contact(self):
"""ContactsKamino beyond the detection gap must be rejected."""
separation = 0.1
builder = testing.make_single_shape_pair_builder(
shapes=("sphere", "sphere"),
distance=separation,
)
for geom in builder.geoms:
geom.gap = 0.001
contacts = self._run_pipeline(builder)
active = contacts.model_active_contacts.numpy()[0]
self.assertEqual(active, 0, "Contact beyond gap must be rejected")
class TestUnifiedPipelineNxnBroadphase(unittest.TestCase):
"""Tests verifying NXN broadphase correctness with collision radii and filter pairs."""
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
def tearDown(self):
self.default_device = None
def _make_two_sphere_builder(self, group_a=1, collides_a=1, group_b=1, collides_b=1, same_body=False):
"""Helper: build a single-world scene with two spheres near each other."""
builder = ModelBuilderKamino()
builder.add_world()
bid_a = builder.add_rigid_body(
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0),
)
if same_body:
bid_b = bid_a
else:
bid_b = builder.add_rigid_body(
m_i=1.0,
i_I_i=I_3,
q_i_0=transformf(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0),
u_i_0=vec6f(0.0),
)
builder.add_geometry(body=bid_a, shape=SphereShape(radius=0.5), group=group_a, collides=collides_a)
builder.add_geometry(body=bid_b, shape=SphereShape(radius=0.5), group=group_b, collides=collides_b)
return builder
def test_00_nxn_sphere_on_plane_generates_contacts(self):
"""Sphere resting on a plane via NXN broadphase must produce contacts.
Validates that shape_collision_radius is populated correctly for
infinite planes (which need a large bounding-sphere radius for
AABB-based broadphase modes to detect them).
"""
builder = testing.make_single_shape_pair_builder(
shapes=("sphere", "plane"),
distance=0.0,
)
expected = {
"model_active_contacts": 1,
"world_active_contacts": [1],
}
test_unified_pipeline(
builder=builder,
expected=expected,
margin=1e-5,
case="nxn_sphere_on_plane",
broadphase_modes=["nxn"],
device=self.default_device,
)
def test_01_nxn_box_on_plane_generates_contacts(self):
"""Box on a plane via NXN broadphase must produce contacts."""
builder = testing.make_single_shape_pair_builder(
shapes=("box", "plane"),
distance=0.0,
)
expected = {
"model_active_contacts": 4,
"world_active_contacts": [4],
}
test_unified_pipeline(
builder=builder,
expected=expected,
margin=1e-5,
case="nxn_box_on_plane",
broadphase_modes=["nxn"],
device=self.default_device,
)
def test_02_nxn_excludes_non_collidable_pairs(self):
"""NXN broadphase must exclude pairs whose group/collides bitmasks do not overlap.
Creates two spheres in the same world but with non-overlapping
collision groups so that they should never collide.
"""
builder = self._make_two_sphere_builder(group_a=0b01, collides_a=0b01, group_b=0b10, collides_b=0b10)
model = builder.finalize(self.default_device)
data = model.data()
state = model.state()
pipeline = CollisionPipelineUnifiedKamino(
model=model,
broadphase="nxn",
default_gap=1.0,
device=self.default_device,
)
n_geoms = builder.num_geoms
capacity = 12 * ((n_geoms * (n_geoms - 1)) // 2)
contacts = ContactsKamino(capacity=max(capacity, 12), device=self.default_device)
contacts.clear()
pipeline.collide(data, state, contacts)
active = contacts.model_active_contacts.numpy()[0]
self.assertEqual(active, 0, "Non-collidable groups must produce zero contacts via NXN")
def test_03_nxn_same_body_excluded(self):
"""NXN broadphase must exclude same-body shape pairs.
Attaches two collision geometries to the same body and verifies
that no self-collision contacts are produced.
"""
builder = self._make_two_sphere_builder(same_body=True)
model = builder.finalize(self.default_device)
data = model.data()
state = model.state()
pipeline = CollisionPipelineUnifiedKamino(
model=model,
broadphase="nxn",
default_gap=1.0,
device=self.default_device,
)
n_geoms = builder.num_geoms
capacity = 12 * ((n_geoms * (n_geoms - 1)) // 2)
contacts = ContactsKamino(capacity=max(capacity, 12), device=self.default_device)
contacts.clear()
pipeline.collide(data, state, contacts)
active = contacts.model_active_contacts.numpy()[0]
self.assertEqual(active, 0, "Same-body shapes must not collide via NXN broadphase")
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_kinematics_constraints.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: UNIT TESTS: KINEMATICS: CONSTRAINTS
"""
import unittest
import warp as wp
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.geometry.contacts import ContactsKamino
from newton._src.solvers.kamino._src.kinematics.constraints import make_unilateral_constraints_info
from newton._src.solvers.kamino._src.kinematics.limits import LimitsKamino
from newton._src.solvers.kamino._src.models.builders.basics import (
build_boxes_fourbar,
make_basics_heterogeneous_builder,
)
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.print import print_data_info, print_model_constraint_info
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Tests
###
class TestKinematicsConstraints(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_single_model_make_constraints(self):
"""
Tests the population of model info with constraint sizes and offsets for a single-world model.
"""
# Constants
max_world_contacts = 20
# Construct the model description using the ModelBuilderKamino
builder = build_boxes_fourbar(dynamic_joints=True, implicit_pd=True)
# Create the model from the builder
model: ModelKamino = builder.finalize(device=self.default_device)
msg.info(f"model.joints.cts_offset:\n{model.joints.cts_offset}")
msg.info(f"model.joints.dynamic_cts_offset:\n{model.joints.dynamic_cts_offset}")
msg.info(f"model.joints.kinematic_cts_offset:\n{model.joints.kinematic_cts_offset}")
# Create a model data
data = model.data(device=self.default_device)
# Create a limits container
limits = LimitsKamino(model=model, device=self.default_device)
if self.verbose:
print("")
print("limits.model_max_limits_host: ", limits.model_max_limits_host)
print("limits.world_max_limits_host: ", limits.world_max_limits_host)
# Set the contact allocation capacities
required_world_max_contacts = [max_world_contacts] * builder.num_worlds
if self.verbose:
print("required_world_max_contacts: ", required_world_max_contacts)
# Construct and allocate the contacts container
contacts = ContactsKamino(capacity=required_world_max_contacts, device=self.default_device)
if self.verbose:
print("contacts.default_max_world_contacts: ", contacts.default_max_world_contacts)
print("contacts.model_max_contacts_host: ", contacts.model_max_contacts_host)
print("contacts.world_max_contacts_host: ", contacts.world_max_contacts_host)
# Create the constraints info
make_unilateral_constraints_info(
model=model,
data=data,
limits=limits,
contacts=contacts,
device=self.default_device,
)
if self.verbose:
print(f"model.size:\n{model.size}\n\n")
print_model_constraint_info(model)
print_data_info(data)
def test_02_homogeneous_model_make_constraints(self):
"""
Tests the population of model info with constraint sizes and offsets for a homogeneous multi-world model.
"""
# Constants
num_worlds: int = 10
max_world_contacts: int = 20
# Construct the model description using the ModelBuilderKamino
builder = make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=build_boxes_fourbar,
dynamic_joints=True,
implicit_pd=True,
)
# Create the model from the builder
model: ModelKamino = builder.finalize(device=self.default_device)
msg.info(f"model.joints.cts_offset:\n{model.joints.cts_offset}")
msg.info(f"model.joints.dynamic_cts_offset:\n{model.joints.dynamic_cts_offset}")
msg.info(f"model.joints.kinematic_cts_offset:\n{model.joints.kinematic_cts_offset}")
# Create a model data
data = model.data(device=self.default_device)
# Create a limits container
limits = LimitsKamino(model=model, device=self.default_device)
if self.verbose:
print("")
print("limits.model_max_limits_host: ", limits.model_max_limits_host)
print("limits.world_max_limits_host: ", limits.world_max_limits_host)
# Set the contact allocation capacities
required_world_max_contacts = [max_world_contacts] * builder.num_worlds
if self.verbose:
print("required_world_max_contacts: ", required_world_max_contacts)
# Construct and allocate the contacts container
contacts = ContactsKamino(capacity=required_world_max_contacts, device=self.default_device)
if self.verbose:
print("contacts.default_max_world_contacts: ", contacts.default_max_world_contacts)
print("contacts.model_max_contacts_host: ", contacts.model_max_contacts_host)
print("contacts.world_max_contacts_host: ", contacts.world_max_contacts_host)
# Create the constraints info
make_unilateral_constraints_info(
model=model,
data=data,
limits=limits,
contacts=contacts,
device=self.default_device,
)
if self.verbose:
print_model_constraint_info(model)
print_data_info(data)
print("\n===============================================================")
print("data.info.num_limits.ptr: ", data.info.num_limits.ptr)
print("limits.world_active_limits.ptr: ", limits.world_active_limits.ptr)
print("data.info.num_contacts.ptr: ", data.info.num_contacts.ptr)
print("contacts.world_active_contacts.ptr: ", contacts.world_active_contacts.ptr)
# Check if the data info entity counters point to the same arrays as the limits and contacts containers
self.assertTrue(data.info.num_limits.ptr, limits.world_active_limits.ptr)
self.assertTrue(data.info.num_contacts.ptr, contacts.world_active_contacts.ptr)
# Extract numpy arrays from the model info
model_max_limits = model.size.sum_of_max_limits
model_max_contacts = model.size.sum_of_max_contacts
max_limits = model.info.max_limits.numpy()
max_contacts = model.info.max_contacts.numpy()
max_limit_cts = model.info.max_limit_cts.numpy()
max_contact_cts = model.info.max_contact_cts.numpy()
max_total_cts = model.info.max_total_cts.numpy()
limits_offset = model.info.limits_offset.numpy()
contacts_offset = model.info.contacts_offset.numpy()
unilaterals_offset = model.info.unilaterals_offset.numpy()
total_cts_offset = model.info.total_cts_offset.numpy()
# Check the model info entries
nj = 0
njc = 0
nl = 0
nlc = 0
nc = 0
ncc = 0
for i in range(num_worlds):
self.assertEqual(model_max_limits, 4 * num_worlds)
self.assertEqual(model_max_contacts, max_world_contacts * num_worlds)
self.assertEqual(max_limits[i], 4)
self.assertEqual(max_contacts[i], max_world_contacts)
self.assertEqual(max_limit_cts[i], 4)
self.assertEqual(max_contact_cts[i], 3 * max_world_contacts)
self.assertEqual(max_total_cts[i], 21 + 4 + 3 * max_world_contacts)
self.assertEqual(limits_offset[i], nl)
self.assertEqual(contacts_offset[i], nc)
self.assertEqual(unilaterals_offset[i], nl + nc)
self.assertEqual(total_cts_offset[i], njc + nlc + ncc)
nj += 4
njc += 21
nl += 4
nlc += 4
nc += max_world_contacts
ncc += 3 * max_world_contacts
def test_03_heterogeneous_model_make_constraints(self):
"""
Tests the population of model info with constraint sizes and offsets for a heterogeneous multi-world model.
"""
# Constants
max_world_contacts = 20
# Construct the model description using the ModelBuilderKamino
builder = make_basics_heterogeneous_builder()
# Create the model from the builder
model: ModelKamino = builder.finalize(device=self.default_device)
# Create a model data
data = model.data(device=self.default_device)
# Create a limits container
limits = LimitsKamino(model=model, device=self.default_device)
if self.verbose:
print("")
print("limits.model_max_limits_host: ", limits.model_max_limits_host)
print("limits.world_max_limits_host: ", limits.world_max_limits_host)
# Set the contact allocation capacities
required_world_max_contacts = [max_world_contacts] * builder.num_worlds
if self.verbose:
print("required_world_max_contacts: ", required_world_max_contacts)
# Construct and allocate the contacts container
contacts = ContactsKamino(capacity=required_world_max_contacts, device=self.default_device)
if self.verbose:
print("contacts.default_max_world_contacts: ", contacts.default_max_world_contacts)
print("contacts.model_max_contacts_host: ", contacts.model_max_contacts_host)
print("contacts.world_max_contacts_host: ", contacts.world_max_contacts_host)
# Create the constraints info
make_unilateral_constraints_info(
model=model,
data=data,
limits=limits,
contacts=contacts,
device=self.default_device,
)
if self.verbose:
print_model_constraint_info(model)
print_data_info(data)
print("\n===============================================================")
print("data.info.num_limits.ptr: ", data.info.num_limits.ptr)
print("limits.world_active_limits.ptr: ", limits.world_active_limits.ptr)
print("data.info.num_contacts.ptr: ", data.info.num_contacts.ptr)
print("contacts.world_active_contacts.ptr: ", contacts.world_active_contacts.ptr)
# Check if the data info entity counters point to the same arrays as the limits and contacts containers
self.assertTrue(data.info.num_limits.ptr, limits.world_active_limits.ptr)
self.assertTrue(data.info.num_contacts.ptr, contacts.world_active_contacts.ptr)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_kinematics_jacobians.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Unit tests for `kinematics/jacobians.py`.
"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.geometry.contacts import ContactsKamino
from newton._src.solvers.kamino._src.kinematics.constraints import make_unilateral_constraints_info
from newton._src.solvers.kamino._src.kinematics.jacobians import (
ColMajorSparseConstraintJacobians,
DenseSystemJacobians,
SparseSystemJacobians,
)
from newton._src.solvers.kamino._src.kinematics.limits import LimitsKamino
from newton._src.solvers.kamino._src.models.builders.basics import (
build_boxes_fourbar,
make_basics_heterogeneous_builder,
)
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import extract_cts_jacobians, extract_dofs_jacobians
from newton._src.solvers.kamino.tests.utils.make import make_test_problem_fourbar, make_test_problem_heterogeneous
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Tests
###
class TestKinematicsDenseSystemJacobians(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_allocate_single_dense_system_jacobians_only_joints(self):
# Construct the model description using the ModelBuilderKamino
builder = build_boxes_fourbar()
# Create the model from the builder
model = builder.finalize(device=self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print(f"model.size.sum_of_num_bodies: {model.size.sum_of_num_bodies}")
print(f"model.size.sum_of_num_joints: {model.size.sum_of_num_joints}")
print(f"model.size.sum_of_num_joint_cts: {model.size.sum_of_num_joint_cts}")
print(f"model.size.sum_of_num_joint_dofs: {model.size.sum_of_num_joint_dofs}")
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, device=self.default_device)
if self.verbose:
print(f"J_cts_offsets (shape={jacobians.data.J_cts_offsets.shape}): {jacobians.data.J_cts_offsets}")
print(f"J_dofs_offsets (shape={jacobians.data.J_dofs_offsets.shape}): {jacobians.data.J_dofs_offsets}")
print(f"J_cts_data: shape={jacobians.data.J_cts_data.shape}")
print(f"J_dofs_data: shape={jacobians.data.J_dofs_data.shape}")
# Check the allocations of Jacobians
model_num_cts = model.size.sum_of_num_joint_cts
self.assertEqual(jacobians.data.J_dofs_offsets.size, 1)
self.assertEqual(jacobians.data.J_cts_offsets.size, 1)
self.assertEqual(jacobians.data.J_dofs_offsets.numpy()[0], 0)
self.assertEqual(jacobians.data.J_cts_offsets.numpy()[0], 0)
self.assertEqual(
jacobians.data.J_dofs_data.shape, (model.size.sum_of_num_joint_dofs * model.size.sum_of_num_body_dofs,)
)
self.assertEqual(jacobians.data.J_cts_data.shape, (model_num_cts * model.size.sum_of_num_body_dofs,))
def test_02_allocate_single_dense_system_jacobians_with_limits(self):
# Construct the model description using the ModelBuilderKamino
builder = build_boxes_fourbar()
# Create the model from the builder
model = builder.finalize(device=self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print(f"model.size.sum_of_num_bodies: {model.size.sum_of_num_bodies}")
print(f"model.size.sum_of_num_joints: {model.size.sum_of_num_joints}")
print(f"model.size.sum_of_num_joint_cts: {model.size.sum_of_num_joint_cts}")
print(f"model.size.sum_of_num_joint_dofs: {model.size.sum_of_num_joint_dofs}")
# Construct and allocate the limits container
limits = LimitsKamino(model=model, device=self.default_device)
if self.verbose:
print("limits.model_max_limits_host: ", limits.model_max_limits_host)
print("limits.world_max_limits_host: ", limits.world_max_limits_host)
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, limits=limits, device=self.default_device)
if self.verbose:
print(f"J_dofs_offsets (shape={jacobians.data.J_dofs_offsets.shape}): {jacobians.data.J_dofs_offsets}")
print(f"J_cts_offsets (shape={jacobians.data.J_cts_offsets.shape}): {jacobians.data.J_cts_offsets}")
print(f"J_dofs_data: shape={jacobians.data.J_dofs_data.shape}")
print(f"J_cts_data: shape={jacobians.data.J_cts_data.shape}")
# Check the allocations of Jacobians
model_num_cts = model.size.sum_of_num_joint_cts + limits.model_max_limits_host
self.assertEqual(jacobians.data.J_dofs_offsets.size, 1)
self.assertEqual(jacobians.data.J_cts_offsets.size, 1)
self.assertEqual(jacobians.data.J_dofs_offsets.numpy()[0], 0)
self.assertEqual(jacobians.data.J_cts_offsets.numpy()[0], 0)
self.assertEqual(
jacobians.data.J_dofs_data.shape, (model.size.sum_of_num_joint_dofs * model.size.sum_of_num_body_dofs,)
)
self.assertEqual(jacobians.data.J_cts_data.shape, (model_num_cts * model.size.sum_of_num_body_dofs,))
def test_03_allocate_single_dense_system_jacobians_with_contacts(self):
# Problem constants
max_world_contacts = 12
# Construct the model description using the ModelBuilderKamino
builder = build_boxes_fourbar()
# Create the model from the builder
model = builder.finalize(device=self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print(f"model.size.sum_of_num_bodies: {model.size.sum_of_num_bodies}")
print(f"model.size.sum_of_num_joints: {model.size.sum_of_num_joints}")
print(f"model.size.sum_of_num_joint_cts: {model.size.sum_of_num_joint_cts}")
print(f"model.size.sum_of_num_joint_dofs: {model.size.sum_of_num_joint_dofs}")
# Set the contact allocation capacities
required_world_max_contacts = [max_world_contacts] * builder.num_worlds
if self.verbose:
print("required_world_max_contacts: ", required_world_max_contacts)
# Construct and allocate the contacts container
contacts = ContactsKamino(capacity=required_world_max_contacts, device=self.default_device)
if self.verbose:
print("contacts.default_max_world_contacts: ", contacts.default_max_world_contacts)
print("contacts.model_max_contacts_host: ", contacts.model_max_contacts_host)
print("contacts.world_max_contacts_host: ", contacts.world_max_contacts_host)
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, contacts=contacts, device=self.default_device)
if self.verbose:
print(f"J_dofs_offsets (shape={jacobians.data.J_dofs_offsets.shape}): {jacobians.data.J_dofs_offsets}")
print(f"J_cts_offsets (shape={jacobians.data.J_cts_offsets.shape}): {jacobians.data.J_cts_offsets}")
print(f"J_dofs_data: shape={jacobians.data.J_dofs_data.shape}")
print(f"J_cts_data: shape={jacobians.data.J_cts_data.shape}")
# Check the allocations of Jacobians
model_num_cts = model.size.sum_of_num_joint_cts + 3 * contacts.model_max_contacts_host
self.assertEqual(jacobians.data.J_dofs_offsets.size, 1)
self.assertEqual(jacobians.data.J_cts_offsets.size, 1)
self.assertEqual(jacobians.data.J_dofs_offsets.numpy()[0], 0)
self.assertEqual(jacobians.data.J_cts_offsets.numpy()[0], 0)
self.assertEqual(
jacobians.data.J_dofs_data.shape, (model.size.sum_of_num_joint_dofs * model.size.sum_of_num_body_dofs,)
)
self.assertEqual(jacobians.data.J_cts_data.shape, (model_num_cts * model.size.sum_of_num_body_dofs,))
def test_04_allocate_single_dense_system_jacobians_with_limits_and_contacts(self):
# Problem constants
max_world_contacts = 12
# Construct the model description using the ModelBuilderKamino
builder = build_boxes_fourbar()
# Create the model from the builder
model = builder.finalize(device=self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print(f"model.size.sum_of_num_bodies: {model.size.sum_of_num_bodies}")
print(f"model.size.sum_of_num_joints: {model.size.sum_of_num_joints}")
print(f"model.size.sum_of_num_joint_cts: {model.size.sum_of_num_joint_cts}")
print(f"model.size.sum_of_num_joint_dofs: {model.size.sum_of_num_joint_dofs}")
# Construct and allocate the limits container
limits = LimitsKamino(model=model, device=self.default_device)
if self.verbose:
print("limits.model_max_limits_host: ", limits.model_max_limits_host)
print("limits.world_max_limits_host: ", limits.world_max_limits_host)
# Set the contact allocation capacities
required_world_max_contacts = [max_world_contacts] * builder.num_worlds
if self.verbose:
print("required_world_max_contacts: ", required_world_max_contacts)
# Construct and allocate the contacts container
contacts = ContactsKamino(capacity=required_world_max_contacts, device=self.default_device)
if self.verbose:
print("contacts.default_max_world_contacts: ", contacts.default_max_world_contacts)
print("contacts.model_max_contacts_host: ", contacts.model_max_contacts_host)
print("contacts.world_max_contacts_host: ", contacts.world_max_contacts_host)
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, limits=limits, contacts=contacts, device=self.default_device)
if self.verbose:
print(f"J_dofs_offsets (shape={jacobians.data.J_dofs_offsets.shape}): {jacobians.data.J_dofs_offsets}")
print(f"J_cts_offsets (shape={jacobians.data.J_cts_offsets.shape}): {jacobians.data.J_cts_offsets}")
print(f"J_dofs_data: shape={jacobians.data.J_dofs_data.shape}")
print(f"J_cts_data: shape={jacobians.data.J_cts_data.shape}")
# Check the allocations of Jacobians
model_num_cts = (
model.size.sum_of_num_joint_cts + limits.model_max_limits_host + 3 * contacts.model_max_contacts_host
)
self.assertEqual(jacobians.data.J_dofs_offsets.size, 1)
self.assertEqual(jacobians.data.J_cts_offsets.size, 1)
self.assertEqual(jacobians.data.J_dofs_offsets.numpy()[0], 0)
self.assertEqual(jacobians.data.J_cts_offsets.numpy()[0], 0)
self.assertEqual(
jacobians.data.J_dofs_data.shape, (model.size.sum_of_num_joint_dofs * model.size.sum_of_num_body_dofs,)
)
self.assertEqual(jacobians.data.J_cts_data.shape, (model_num_cts * model.size.sum_of_num_body_dofs,))
def test_05_allocate_homogeneous_dense_system_jacobians(self):
# Problem constants
num_worlds = 3
max_world_contacts = 12
# Construct the model description using the ModelBuilderKamino
builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_fourbar)
# Create the model from the builder
model = builder.finalize(device=self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print(f"model.size.sum_of_num_bodies: {model.size.sum_of_num_bodies}")
print(f"model.size.sum_of_num_joints: {model.size.sum_of_num_joints}")
print(f"model.size.sum_of_num_joint_cts: {model.size.sum_of_num_joint_cts}")
print(f"model.size.sum_of_num_joint_dofs: {model.size.sum_of_num_joint_dofs}")
# Construct and allocate the limits container
limits = LimitsKamino(model=model, device=self.default_device)
if self.verbose:
print("limits.model_max_limits_host: ", limits.model_max_limits_host)
print("limits.world_max_limits_host: ", limits.world_max_limits_host)
# Set the contact allocation capacities
required_world_max_contacts = [max_world_contacts] * builder.num_worlds
if self.verbose:
print("required_world_max_contacts: ", required_world_max_contacts)
# Construct and allocate the contacts container
contacts = ContactsKamino(capacity=required_world_max_contacts, device=self.default_device)
if self.verbose:
print("contacts.default_max_world_contacts: ", contacts.default_max_world_contacts)
print("contacts.model_max_contacts_host: ", contacts.model_max_contacts_host)
print("contacts.world_max_contacts_host: ", contacts.world_max_contacts_host)
print("contacts.world_max_contacts_host: ", contacts.world_max_contacts_host)
# Build model info
make_unilateral_constraints_info(model, model.data(), limits, contacts)
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, limits=limits, contacts=contacts, device=self.default_device)
if self.verbose:
print(f"J_dofs_offsets (shape={jacobians.data.J_dofs_offsets.shape}): {jacobians.data.J_dofs_offsets}")
print(f"J_cts_offsets (shape={jacobians.data.J_cts_offsets.shape}): {jacobians.data.J_cts_offsets}")
print(f"J_dofs_data: shape={jacobians.data.J_dofs_data.shape}")
print(f"J_cts_data: shape={jacobians.data.J_cts_data.shape}")
# Compute the total maximum number of constraints
num_body_dofs = model.info.num_body_dofs.numpy().tolist()
num_joint_dofs = model.info.num_joint_dofs.numpy().tolist()
max_total_cts = model.info.max_total_cts.numpy().tolist()
if self.verbose:
print("num_body_dofs: ", num_body_dofs)
print("max_total_cts: ", max_total_cts)
print("num_joint_dofs: ", num_joint_dofs)
# Compute Jacobian sizes
J_dofs_size: list[int] = [0] * num_worlds
J_cts_size: list[int] = [0] * num_worlds
for w in range(num_worlds):
J_dofs_size[w] = num_joint_dofs[w] * num_body_dofs[w]
J_cts_size[w] = max_total_cts[w] * num_body_dofs[w]
# Compute Jacobian offsets
J_dofs_offsets: list[int] = [0] + [sum(J_dofs_size[:w]) for w in range(1, num_worlds)]
J_cts_offsets: list[int] = [0] + [sum(J_cts_size[:w]) for w in range(1, num_worlds)]
# Check the allocations of Jacobians
self.assertEqual(jacobians.data.J_dofs_offsets.size, num_worlds)
self.assertEqual(jacobians.data.J_cts_offsets.size, num_worlds)
J_dofs_mio_np = jacobians.data.J_dofs_offsets.numpy()
J_cts_mio_np = jacobians.data.J_cts_offsets.numpy()
for w in range(num_worlds):
self.assertEqual(J_dofs_mio_np[w], J_dofs_offsets[w])
self.assertEqual(J_cts_mio_np[w], J_cts_offsets[w])
self.assertEqual(jacobians.data.J_dofs_data.size, sum(J_dofs_size))
self.assertEqual(jacobians.data.J_cts_data.size, sum(J_cts_size))
def test_06_allocate_heterogeneous_dense_system_jacobians(self):
# Problem constants
max_world_contacts = 12
# Construct the model description using the ModelBuilderKamino
builder = make_basics_heterogeneous_builder()
num_worlds = builder.num_worlds
# Create the model from the builder
model = builder.finalize(device=self.default_device)
if self.verbose:
print("") # Add a newline for better readability
print(f"model.size.sum_of_num_bodies: {model.size.sum_of_num_bodies}")
print(f"model.size.sum_of_num_joints: {model.size.sum_of_num_joints}")
print(f"model.size.sum_of_num_joint_cts: {model.size.sum_of_num_joint_cts}")
print(f"model.size.sum_of_num_joint_dofs: {model.size.sum_of_num_joint_dofs}")
# Construct and allocate the limits container
limits = LimitsKamino(model=model, device=self.default_device)
if self.verbose:
print("limits.model_max_limits_host: ", limits.model_max_limits_host)
print("limits.world_max_limits_host: ", limits.world_max_limits_host)
# Set the contact allocation capacities
required_world_max_contacts = [max_world_contacts] * builder.num_worlds
if self.verbose:
print("required_world_max_contacts: ", required_world_max_contacts)
# Construct and allocate the contacts container
contacts = ContactsKamino(capacity=required_world_max_contacts, device=self.default_device)
if self.verbose:
print("contacts.default_max_world_contacts: ", contacts.default_max_world_contacts)
print("contacts.model_max_contacts_host: ", contacts.model_max_contacts_host)
print("contacts.world_max_contacts_host: ", contacts.world_max_contacts_host)
# Build model info
make_unilateral_constraints_info(model, model.data(), limits, contacts)
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, limits=limits, contacts=contacts)
if self.verbose:
print(f"J_dofs_offsets (shape={jacobians.data.J_dofs_offsets.shape}): {jacobians.data.J_dofs_offsets}")
print(f"J_cts_offsets (shape={jacobians.data.J_cts_offsets.shape}): {jacobians.data.J_cts_offsets}")
print(f"J_dofs_data: shape={jacobians.data.J_dofs_data.shape}")
print(f"J_cts_data: shape={jacobians.data.J_cts_data.shape}")
# Compute the total maximum number of constraints
num_body_dofs = model.info.num_body_dofs.numpy().tolist()
num_joint_dofs = model.info.num_joint_dofs.numpy().tolist()
max_total_cts = model.info.max_total_cts.numpy().tolist()
if self.verbose:
print("num_body_dofs: ", num_body_dofs)
print("max_total_cts: ", max_total_cts)
print("num_joint_dofs: ", num_joint_dofs)
# Compute Jacobian sizes
J_dofs_size: list[int] = [0] * num_worlds
J_cts_size: list[int] = [0] * num_worlds
for w in range(num_worlds):
J_dofs_size[w] = num_joint_dofs[w] * num_body_dofs[w]
J_cts_size[w] = max_total_cts[w] * num_body_dofs[w]
# Compute Jacobian offsets
J_dofs_offsets: list[int] = [0] + [sum(J_dofs_size[:w]) for w in range(1, num_worlds)]
J_cts_offsets: list[int] = [0] + [sum(J_cts_size[:w]) for w in range(1, num_worlds)]
# Check the allocations of Jacobians
self.assertEqual(jacobians.data.J_dofs_offsets.size, num_worlds)
self.assertEqual(jacobians.data.J_cts_offsets.size, num_worlds)
J_dofs_mio_np = jacobians.data.J_dofs_offsets.numpy()
J_cts_mio_np = jacobians.data.J_cts_offsets.numpy()
for w in range(num_worlds):
self.assertEqual(J_dofs_mio_np[w], J_dofs_offsets[w])
self.assertEqual(J_cts_mio_np[w], J_cts_offsets[w])
self.assertEqual(jacobians.data.J_dofs_data.size, sum(J_dofs_size))
self.assertEqual(jacobians.data.J_cts_data.size, sum(J_cts_size))
def test_07_build_single_dense_system_jacobians(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=True,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the dense system Jacobians
jacobians.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Reshape the flat actuation Jacobian as a matrix
J_dofs_offsets = jacobians.data.J_dofs_offsets.numpy()
J_dofs_flat = jacobians.data.J_dofs_data.numpy()
njd = J_dofs_flat.size // model.size.sum_of_num_body_dofs
J_dofs_mat = J_dofs_flat.reshape((njd, model.size.sum_of_num_body_dofs))
# Reshape the flat constraintJacobian as a matrix
J_cts_offsets = jacobians.data.J_cts_offsets.numpy()
J_cts_flat = jacobians.data.J_cts_data.numpy()
maxncts = J_cts_flat.size // model.size.sum_of_num_body_dofs
J_cts_mat = J_cts_flat.reshape((maxncts, model.size.sum_of_num_body_dofs))
# Check the shapes of the Jacobians
self.assertEqual(J_dofs_offsets.size, 1)
self.assertEqual(J_cts_offsets.size, 1)
self.assertEqual(
maxncts,
model.size.sum_of_num_joint_cts + limits.model_max_limits_host + 3 * contacts.model_max_contacts_host,
)
self.assertEqual(njd, model.size.sum_of_num_joint_dofs)
# Optional verbose output
if self.verbose:
print(f"J_cts_offsets (shape={jacobians.data.J_cts_offsets.shape}): {jacobians.data.J_cts_offsets}")
print(f"J_cts_flat (shape={J_cts_flat.shape}):\n{J_cts_flat}")
print(f"J_cts_mat (shape={J_cts_mat.shape}):\n{J_cts_mat}")
print(f"J_dofs_offsets (shape={jacobians.data.J_dofs_offsets.shape}): {jacobians.data.J_dofs_offsets}")
print(f"J_dofs_flat (shape={J_dofs_flat.shape}):\n{J_dofs_flat}")
print(f"J_dofs_mat (shape={J_dofs_mat.shape}):\n{J_dofs_mat}")
def test_08_build_homogeneous_dense_system_jacobians(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=3,
with_limits=True,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the dense system Jacobians
jacobians.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Extract the Jacobian matrices
J_cts = extract_cts_jacobians(model=model, limits=limits, contacts=contacts, jacobians=jacobians)
J_dofs = extract_dofs_jacobians(model=model, jacobians=jacobians)
for w in range(model.size.num_worlds):
msg.info("[world='%d']: J_cts:\n%s", w, J_cts[w])
msg.info("[world='%d']: J_dofs:\n%s", w, J_dofs[w])
def test_09_build_heterogeneous_dense_system_jacobians(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_heterogeneous(
device=self.default_device,
max_world_contacts=12,
with_limits=True,
with_contacts=True,
with_implicit_joints=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = DenseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the dense system Jacobians
jacobians.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Extract the Jacobian matrices
J_cts = extract_cts_jacobians(model=model, limits=limits, contacts=contacts, jacobians=jacobians)
J_dofs = extract_dofs_jacobians(model=model, jacobians=jacobians)
for w in range(model.size.num_worlds):
msg.info("[world='%d']: J_cts:\n%s", w, J_cts[w])
msg.info("[world='%d']: J_dofs:\n%s", w, J_dofs[w])
class TestKinematicsSparseSystemJacobians(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
self.epsilon = 1e-6 # Threshold for sparse-dense comparison test
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
###
# Helpers
###
def _compare_dense_sparse_jacobians(
self,
model: ModelKamino,
limits: LimitsKamino | None,
contacts: ContactsKamino | None,
jacobians_dense: DenseSystemJacobians,
jacobians_sparse: SparseSystemJacobians,
):
# Reshape the dense Jacobian data as a matrices
J_cts_dense = extract_cts_jacobians(
model=model, limits=limits, contacts=contacts, jacobians=jacobians_dense, verbose=self.verbose
)
J_dofs_dense = extract_dofs_jacobians(model=model, jacobians=jacobians_dense, verbose=self.verbose)
# Get the (dense) numpy version of the sparse Jacobians
J_dofs_sparse = jacobians_sparse._J_dofs.bsm.numpy()
J_cts_sparse = jacobians_sparse._J_cts.bsm.numpy()
self.assertEqual(len(J_cts_dense), len(J_cts_sparse))
self.assertEqual(len(J_dofs_dense), len(J_dofs_sparse))
# Check that Jacobians match
for mat_id in range(len(J_cts_dense)):
if J_dofs_dense[mat_id].size > 0:
diff_J_dofs = J_dofs_dense[mat_id] - J_dofs_sparse[mat_id]
self.assertLess(np.max(np.abs(diff_J_dofs)), self.epsilon)
diff_J_cts = J_cts_dense[mat_id][: J_cts_sparse[mat_id].shape[0], :] - J_cts_sparse[mat_id]
self.assertLess(np.max(np.abs(diff_J_cts)), self.epsilon)
# Extra entries in dense constraint Jacobian need to be zero
if J_cts_dense[mat_id].shape[0] > J_cts_sparse[mat_id].shape[0]:
self.assertEqual(np.max(np.abs(J_cts_dense[mat_id][J_cts_sparse[mat_id].shape[0] :, :])), 0)
def _compare_row_col_major_jacobians(
self,
jacobians: SparseSystemJacobians,
jacobians_col_major: ColMajorSparseConstraintJacobians,
):
# Get the (dense) numpy version of the Jacobians
J_cts_row_major = jacobians._J_cts.bsm.numpy()
J_cts_col_major = jacobians_col_major.bsm.numpy()
self.assertEqual(len(J_cts_row_major), len(J_cts_col_major))
# Check that Jacobians match
for mat_id in range(len(J_cts_row_major)):
diff_J_cts = J_cts_row_major[mat_id] - J_cts_col_major[mat_id]
max_diff = np.max(np.abs(diff_J_cts))
if max_diff > self.epsilon and self.verbose:
msg.warning(f"[{mat_id}] J_cts_row_major:\n{J_cts_row_major[mat_id]}")
msg.warning(f"[{mat_id}] J_cts_col_major:\n{J_cts_col_major[mat_id]}")
self.assertLess(max_diff, self.epsilon)
###
# Construction
###
def test_01_allocate_single_sparse_system_jacobians_only_joints(self):
# Construct the test problem
model, *_ = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=False,
with_contacts=False,
verbose=self.verbose,
)
# Create the sparse Jacobians
jacobians = SparseSystemJacobians(model=model)
self.assertIs(jacobians._J_cts.bsm.device, model.device)
self.assertIs(jacobians._J_cts.device, model.device)
if self.verbose:
print(f"J_cts max_dims (shape={jacobians._J_cts.bsm.max_dims.shape}): {jacobians._J_cts.bsm.max_dims}")
print(f"J_cts dims (shape={jacobians._J_cts.bsm.dims.shape}): {jacobians._J_cts.bsm.dims}")
print(f"J_cts max_nzb (shape={jacobians._J_cts.bsm.max_nzb.shape}): {jacobians._J_cts.bsm.max_nzb}")
print(f"J_dofs max_dims (shape={jacobians._J_dofs.bsm.max_dims.shape}): {jacobians._J_dofs.bsm.max_dims}")
print(f"J_dofs dims (shape={jacobians._J_dofs.bsm.dims.shape}): {jacobians._J_dofs.bsm.dims}")
print(f"J_dofs max_nzb (shape={jacobians._J_dofs.bsm.max_nzb.shape}): {jacobians._J_dofs.bsm.max_nzb}")
# Check the allocation of Jacobians
model_num_cts = model.size.sum_of_num_joint_cts
model_num_dofs = model.size.sum_of_num_joint_dofs
model_num_bodies = model.size.sum_of_num_bodies
self.assertEqual(jacobians._J_cts.bsm.num_matrices, 1)
self.assertEqual(jacobians._J_dofs.bsm.num_matrices, 1)
self.assertTrue((jacobians._J_cts.bsm.max_dims.numpy() == [[model_num_cts, 6 * model_num_bodies]]).all())
self.assertTrue((jacobians._J_dofs.bsm.max_dims.numpy() == [[model_num_dofs, 6 * model_num_bodies]]).all())
self.assertEqual(jacobians._J_cts.bsm.max_nzb.numpy()[0], 2 * model_num_cts)
def test_02_allocate_single_sparse_system_jacobians_with_limits(self):
# Construct the test problem
model, _data, _state, limits, *_ = make_test_problem_fourbar(
device=self.default_device,
num_worlds=1,
with_limits=True,
with_contacts=False,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits)
if self.verbose:
print(f"J_cts max_dims (shape={jacobians._J_cts.bsm.max_dims.shape}): {jacobians._J_cts.bsm.max_dims}")
print(f"J_cts dims (shape={jacobians._J_cts.bsm.dims.shape}): {jacobians._J_cts.bsm.dims}")
print(f"J_cts max_nzb (shape={jacobians._J_cts.bsm.max_nzb.shape}): {jacobians._J_cts.bsm.max_nzb}")
print(f"J_dofs max_dims (shape={jacobians._J_dofs.bsm.max_dims.shape}): {jacobians._J_dofs.bsm.max_dims}")
print(f"J_dofs dims (shape={jacobians._J_dofs.bsm.dims.shape}): {jacobians._J_dofs.bsm.dims}")
print(f"J_dofs max_nzb (shape={jacobians._J_dofs.bsm.max_nzb.shape}): {jacobians._J_dofs.bsm.max_nzb}")
# Check the allocation of Jacobians
model_num_cts = model.size.sum_of_num_joint_cts + limits.model_max_limits_host
model_num_dofs = model.size.sum_of_num_joint_dofs
model_num_bodies = model.size.sum_of_num_bodies
self.assertEqual(jacobians._J_cts.bsm.num_matrices, 1)
self.assertEqual(jacobians._J_dofs.bsm.num_matrices, 1)
self.assertTrue((jacobians._J_cts.bsm.max_dims.numpy() == [[model_num_cts, 6 * model_num_bodies]]).all())
self.assertTrue((jacobians._J_dofs.bsm.max_dims.numpy() == [[model_num_dofs, 6 * model_num_bodies]]).all())
self.assertEqual(jacobians._J_cts.bsm.max_nzb.numpy()[0], 2 * model_num_cts)
def test_03_allocate_single_sparse_system_jacobians_with_contacts(self):
# Construct the test problem
model, _data, _state, _limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=False,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, contacts=contacts)
if self.verbose:
print(f"J_cts max_dims (shape={jacobians._J_cts.bsm.max_dims.shape}): {jacobians._J_cts.bsm.max_dims}")
print(f"J_cts dims (shape={jacobians._J_cts.bsm.dims.shape}): {jacobians._J_cts.bsm.dims}")
print(f"J_cts max_nzb (shape={jacobians._J_cts.bsm.max_nzb.shape}): {jacobians._J_cts.bsm.max_nzb}")
print(f"J_dofs max_dims (shape={jacobians._J_dofs.bsm.max_dims.shape}): {jacobians._J_dofs.bsm.max_dims}")
print(f"J_dofs dims (shape={jacobians._J_dofs.bsm.dims.shape}): {jacobians._J_dofs.bsm.dims}")
print(f"J_dofs max_nzb (shape={jacobians._J_dofs.bsm.max_nzb.shape}): {jacobians._J_dofs.bsm.max_nzb}")
# Check the allocation of Jacobians
model_num_cts = model.size.sum_of_num_joint_cts + 3 * contacts.model_max_contacts_host
model_num_dofs = model.size.sum_of_num_joint_dofs
model_num_bodies = model.size.sum_of_num_bodies
self.assertEqual(jacobians._J_cts.bsm.num_matrices, 1)
self.assertEqual(jacobians._J_dofs.bsm.num_matrices, 1)
self.assertTrue((jacobians._J_cts.bsm.max_dims.numpy() == [[model_num_cts, 6 * model_num_bodies]]).all())
self.assertTrue((jacobians._J_dofs.bsm.max_dims.numpy() == [[model_num_dofs, 6 * model_num_bodies]]).all())
self.assertEqual(jacobians._J_cts.bsm.max_nzb.numpy()[0], 2 * model_num_cts)
def test_04_allocate_single_sparse_system_jacobians_with_limits_and_contacts(self):
# Construct the test problem
model, _data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=True,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=contacts)
if self.verbose:
print(f"J_cts max_dims (shape={jacobians._J_cts.bsm.max_dims.shape}): {jacobians._J_cts.bsm.max_dims}")
print(f"J_cts dims (shape={jacobians._J_cts.bsm.dims.shape}): {jacobians._J_cts.bsm.dims}")
print(f"J_cts max_nzb (shape={jacobians._J_cts.bsm.max_nzb.shape}): {jacobians._J_cts.bsm.max_nzb}")
print(f"J_dofs max_dims (shape={jacobians._J_dofs.bsm.max_dims.shape}): {jacobians._J_dofs.bsm.max_dims}")
print(f"J_dofs dims (shape={jacobians._J_dofs.bsm.dims.shape}): {jacobians._J_dofs.bsm.dims}")
print(f"J_dofs max_nzb (shape={jacobians._J_dofs.bsm.max_nzb.shape}): {jacobians._J_dofs.bsm.max_nzb}")
# Check the allocation of Jacobians
model_num_cts = (
model.size.sum_of_num_joint_cts + limits.model_max_limits_host + 3 * contacts.model_max_contacts_host
)
model_num_dofs = model.size.sum_of_num_joint_dofs
model_num_bodies = model.size.sum_of_num_bodies
self.assertEqual(jacobians._J_cts.bsm.num_matrices, 1)
self.assertEqual(jacobians._J_dofs.bsm.num_matrices, 1)
self.assertTrue((jacobians._J_cts.bsm.max_dims.numpy() == [[model_num_cts, 6 * model_num_bodies]]).all())
self.assertTrue((jacobians._J_dofs.bsm.max_dims.numpy() == [[model_num_dofs, 6 * model_num_bodies]]).all())
self.assertEqual(jacobians._J_cts.bsm.max_nzb.numpy()[0], 2 * model_num_cts)
def test_05_allocate_homogeneous_sparse_system_jacobians(self):
# Construct the test problem
model, _data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=3,
with_limits=True,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=contacts)
if self.verbose:
print(f"J_cts max_dims (shape={jacobians._J_cts.bsm.max_dims.shape}): {jacobians._J_cts.bsm.max_dims}")
print(f"J_cts dims (shape={jacobians._J_cts.bsm.dims.shape}): {jacobians._J_cts.bsm.dims}")
print(f"J_cts max_nzb (shape={jacobians._J_cts.bsm.max_nzb.shape}): {jacobians._J_cts.bsm.max_nzb}")
print(f"J_dofs max_dims (shape={jacobians._J_dofs.bsm.max_dims.shape}): {jacobians._J_dofs.bsm.max_dims}")
print(f"J_dofs dims (shape={jacobians._J_dofs.bsm.dims.shape}): {jacobians._J_dofs.bsm.dims}")
print(f"J_dofs max_nzb (shape={jacobians._J_dofs.bsm.max_nzb.shape}): {jacobians._J_dofs.bsm.max_nzb}")
# Check the allocation of Jacobians
num_body_dofs = model.info.num_body_dofs.numpy().tolist()
num_joint_dofs = model.info.num_joint_dofs.numpy().tolist()
max_total_cts = model.info.max_total_cts.numpy().tolist()
self.assertEqual(jacobians._J_cts.bsm.num_matrices, model.size.num_worlds)
self.assertEqual(jacobians._J_dofs.bsm.num_matrices, model.size.num_worlds)
self.assertTrue(
(
jacobians._J_cts.bsm.max_dims.numpy()
== [[max_total_cts[w], num_body_dofs[w]] for w in range(model.size.num_worlds)]
).all()
)
self.assertTrue(
(
jacobians._J_dofs.bsm.max_dims.numpy()
== [[num_joint_dofs[w], num_body_dofs[w]] for w in range(model.size.num_worlds)]
).all()
)
self.assertTrue(
(jacobians._J_cts.bsm.max_nzb.numpy() == [2 * max_total_cts[w] for w in range(model.size.num_worlds)]).all()
)
def test_06_allocate_heterogeneous_sparse_system_jacobians(self):
# Construct the test problem
model, _data, _state, limits, contacts = make_test_problem_heterogeneous(
device=self.default_device,
max_world_contacts=12,
with_limits=True,
with_contacts=True,
with_implicit_joints=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=contacts, device=self.default_device)
if self.verbose:
print(f"J_cts max_dims (shape={jacobians._J_cts.bsm.max_dims.shape}): {jacobians._J_cts.bsm.max_dims}")
print(f"J_cts dims (shape={jacobians._J_cts.bsm.dims.shape}): {jacobians._J_cts.bsm.dims}")
print(f"J_cts max_nzb (shape={jacobians._J_cts.bsm.max_nzb.shape}): {jacobians._J_cts.bsm.max_nzb}")
print(f"J_dofs max_dims (shape={jacobians._J_dofs.bsm.max_dims.shape}): {jacobians._J_dofs.bsm.max_dims}")
print(f"J_dofs dims (shape={jacobians._J_dofs.bsm.dims.shape}): {jacobians._J_dofs.bsm.dims}")
print(f"J_dofs max_nzb (shape={jacobians._J_dofs.bsm.max_nzb.shape}): {jacobians._J_dofs.bsm.max_nzb}")
# Check the allocation of Jacobians
num_body_dofs = model.info.num_body_dofs.numpy().tolist()
num_joint_dofs = model.info.num_joint_dofs.numpy().tolist()
max_total_cts = model.info.max_total_cts.numpy().tolist()
self.assertEqual(jacobians._J_cts.bsm.num_matrices, model.size.num_worlds)
self.assertEqual(jacobians._J_dofs.bsm.num_matrices, model.size.num_worlds)
self.assertTrue(
(
jacobians._J_cts.bsm.max_dims.numpy()
== [[max_total_cts[w], num_body_dofs[w]] for w in range(model.size.num_worlds)]
).all()
)
self.assertTrue(
(
jacobians._J_dofs.bsm.max_dims.numpy()
== [[num_joint_dofs[w], num_body_dofs[w]] for w in range(model.size.num_worlds)]
).all()
)
def test_07_build_compare_single_system_jacobians(self):
# Construct the test problem
model, data, *_ = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=False,
with_contacts=False,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model)
jacobians_dense = DenseSystemJacobians(model=model)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data)
jacobians_dense.build(model=model, data=data)
wp.synchronize()
# Check that Jacobians match
self._compare_dense_sparse_jacobians(model, None, None, jacobians_dense, jacobians)
def test_08_build_compare_single_system_jacobians_with_limits(self):
# Construct the test problem
model, data, _state, limits, _contacts = make_test_problem_fourbar(
device=self.default_device,
num_worlds=1,
with_limits=True,
with_contacts=False,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits)
jacobians_dense = DenseSystemJacobians(model=model, limits=limits)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, limits=limits.data)
jacobians_dense.build(model=model, data=data, limits=limits.data)
wp.synchronize()
# Check that Jacobians match
self._compare_dense_sparse_jacobians(model, limits, None, jacobians_dense, jacobians)
def test_09_build_compare_single_system_jacobians_with_contacts(self):
# Construct the test problem
model, data, _state, _limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=False,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, contacts=contacts)
jacobians_dense = DenseSystemJacobians(model=model, contacts=contacts)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, contacts=contacts.data)
jacobians_dense.build(model=model, data=data, contacts=contacts.data)
wp.synchronize()
# Check that Jacobians match
self._compare_dense_sparse_jacobians(model, None, contacts, jacobians_dense, jacobians)
def test_10_build_compare_single_system_jacobians_with_limits_and_contacts(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=True,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=contacts)
jacobians_dense = DenseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
jacobians_dense.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Check that Jacobians match
self._compare_dense_sparse_jacobians(model, limits, contacts, jacobians_dense, jacobians)
def test_11_build_compare_homogeneous_system_jacobians(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=3,
with_limits=True,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=contacts)
jacobians_dense = DenseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
jacobians_dense.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Check that Jacobians match
self._compare_dense_sparse_jacobians(model, limits, contacts, jacobians_dense, jacobians)
def test_12_build_compare_heterogeneous_system_jacobians(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_heterogeneous(
device=self.default_device,
max_world_contacts=12,
with_limits=True,
with_contacts=True,
with_implicit_joints=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians_sparse = SparseSystemJacobians(model=model, limits=limits, contacts=contacts)
jacobians_dense = DenseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the system Jacobians
jacobians_sparse.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
jacobians_dense.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Check that Jacobians match
self._compare_dense_sparse_jacobians(model, limits, contacts, jacobians_dense, jacobians_sparse)
def test_13_build_col_major_single_system_jacobians(self):
# Construct the test problem
model, data, *_ = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=False,
with_contacts=False,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data)
wp.synchronize()
# Build column-major constraint Jacobian version
jacobian_col_maj = ColMajorSparseConstraintJacobians(model=model, jacobians=jacobians)
jacobian_col_maj.update(model=model, jacobians=jacobians)
# Check that Jacobians match
self._compare_row_col_major_jacobians(jacobians, jacobian_col_maj)
def test_14_build_col_major_single_system_jacobians_with_limits(self):
# Construct the test problem
model, data, _state, limits, _contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=True,
with_contacts=False,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, limits=limits.data)
wp.synchronize()
# Build column-major constraint Jacobian version
jacobian_col_maj = ColMajorSparseConstraintJacobians(model=model, limits=limits, jacobians=jacobians)
jacobian_col_maj.update(model=model, jacobians=jacobians, limits=limits)
# Check that Jacobians match
self._compare_row_col_major_jacobians(jacobians, jacobian_col_maj)
def test_15_build_col_major_single_system_jacobians_with_contacts(self):
# Construct the test problem
model, data, _state, _limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=False,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, contacts=contacts, device=self.default_device)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, contacts=contacts.data)
wp.synchronize()
# Build column-major constraint Jacobian version
jacobian_col_maj = ColMajorSparseConstraintJacobians(model=model, contacts=contacts, jacobians=jacobians)
jacobian_col_maj.update(model=model, jacobians=jacobians, contacts=contacts)
# Check that Jacobians match
self._compare_row_col_major_jacobians(jacobians, jacobian_col_maj)
def test_16_build_col_major_single_system_jacobians_with_limits_and_contacts(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=1,
with_limits=True,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=contacts, device=self.default_device)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Build column-major constraint Jacobian version
jacobian_col_maj = ColMajorSparseConstraintJacobians(
model=model, limits=limits, contacts=contacts, jacobians=jacobians
)
jacobian_col_maj.update(model=model, jacobians=jacobians, limits=limits, contacts=contacts)
# Check that Jacobians match
self._compare_row_col_major_jacobians(jacobians, jacobian_col_maj)
def test_17_build_col_major_homogeneous_system_jacobians(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_fourbar(
device=self.default_device,
max_world_contacts=12,
num_worlds=3,
with_limits=True,
with_contacts=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Build column-major constraint Jacobian version
jacobian_col_maj = ColMajorSparseConstraintJacobians(
model=model, limits=limits, contacts=contacts, jacobians=jacobians
)
jacobian_col_maj.update(model=model, jacobians=jacobians, limits=limits, contacts=contacts)
# Check that Jacobians match
self._compare_row_col_major_jacobians(jacobians, jacobian_col_maj)
def test_18_build_col_major_heterogeneous_system_jacobians(self):
# Construct the test problem
model, data, _state, limits, contacts = make_test_problem_heterogeneous(
device=self.default_device,
max_world_contacts=12,
with_limits=True,
with_contacts=True,
with_implicit_joints=True,
verbose=self.verbose,
)
# Create the Jacobians container
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=contacts)
wp.synchronize()
# Build the system Jacobians
jacobians.build(model=model, data=data, limits=limits.data, contacts=contacts.data)
wp.synchronize()
# Build column-major constraint Jacobian version
jacobian_col_maj = ColMajorSparseConstraintJacobians(
model=model, limits=limits, contacts=contacts, jacobians=jacobians
)
jacobian_col_maj.update(model=model, jacobians=jacobians, limits=limits, contacts=contacts)
# Check that Jacobians match
self._compare_row_col_major_jacobians(jacobians, jacobian_col_maj)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_kinematics_joints.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the `kamino.kinematics.joints` module"""
import math
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.data import DataKamino
from newton._src.solvers.kamino._src.core.math import quat_exp, screw, screw_angular, screw_linear
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.core.types import float32, int32, mat33f, transformf, vec3f, vec6f
from newton._src.solvers.kamino._src.kinematics.joints import compute_joints_data
from newton._src.solvers.kamino._src.models.builders.testing import build_unary_revolute_joint_test
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
Q_X_J = 0.5 * math.pi
THETA_Y_J = 0.1
THETA_Z_J = -0.2
J_DR_J = vec3f(0.01, 0.02, 0.03)
J_DV_J = vec3f(0.1, -0.2, 0.3)
J_DOMEGA_J = vec3f(-1.0, 0.04, -0.05)
# Compute revolute joint rotational residual: sin(angle) * axis
ROT_RES_VEC = np.array([0.0, THETA_Y_J, THETA_Z_J])
ROT_RES_ANGLE = np.linalg.norm(ROT_RES_VEC)
ROT_RES = (np.sin(ROT_RES_ANGLE) / ROT_RES_ANGLE) * ROT_RES_VEC
###
# Kernels
###
@wp.kernel
def _set_joint_follower_body_state(
model_joint_bid_F: wp.array(dtype=int32),
model_joint_B_r_Bj: wp.array(dtype=vec3f),
model_joint_F_r_Fj: wp.array(dtype=vec3f),
model_joint_X_j: wp.array(dtype=mat33f),
state_body_q_i: wp.array(dtype=transformf),
state_body_u_i: wp.array(dtype=vec6f),
):
"""
Set the state of the bodies to a certain values in order to check computations of joint states.
"""
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the joint parameters
bid_F = model_joint_bid_F[jid]
B_r_Bj = model_joint_B_r_Bj[jid]
F_r_Fj = model_joint_F_r_Fj[jid]
X_j = model_joint_X_j[jid]
# The base body is assumed to be at the origin with no rotation or twist
p_B = transformf(vec3f(0.0), wp.quat_identity())
u_B = vec6f(0.0)
r_B = wp.transform_get_translation(p_B)
q_B = wp.transform_get_rotation(p_B)
R_B = wp.quat_to_matrix(q_B)
v_B = screw_linear(u_B)
omega_B = screw_angular(u_B)
# Define the joint rotation offset
# NOTE: X_j projects quantities into the joint frame
# NOTE: X_j^T projects quantities into the outer frame (world or body)
j_dR_yz_j = vec3f(0.0, THETA_Y_J, THETA_Z_J) # Joint residual as rotation vector
j_dR_x_j = vec3f(Q_X_J, 0.0, 0.0) # Joint dof rotation as rotation vector
q_jq = quat_exp(j_dR_yz_j) * quat_exp(j_dR_x_j) # Total joint offset
R_jq = wp.quat_to_matrix(q_jq) # Joint offset as rotation matrix
# Define the joint translation offset
j_dr_j = J_DR_J
# Define the joint twist offset
j_dv_j = J_DV_J
j_domega_j = J_DOMEGA_J
# Follower body rotation via the Base and joint frames
R_B_X_j = R_B @ X_j
R_F_new = R_B_X_j @ R_jq @ wp.transpose(X_j)
q_F_new = wp.quat_from_matrix(R_F_new)
# Follower body position via the Base and joint frames
r_Fj = R_F_new @ F_r_Fj
r_F_new = r_B + R_B @ B_r_Bj + R_B_X_j @ j_dr_j - r_Fj
# Follower body twist via the Base and joint frames
r_Bj = R_B @ B_r_Bj
r_Fj = R_F_new @ F_r_Fj
omega_F_new = R_B_X_j @ j_domega_j + omega_B
v_F_new = R_B_X_j @ j_dv_j + v_B + wp.cross(omega_B, r_Bj) - wp.cross(omega_F_new, r_Fj)
# Offset the bose of the body by a fixed amount
state_body_q_i[bid_F] = wp.transformation(r_F_new, q_F_new, dtype=float32)
state_body_u_i[bid_F] = screw(v_F_new, omega_F_new)
###
# Launchers
###
def set_joint_follower_body_state(model: ModelKamino, data: DataKamino):
wp.launch(
_set_joint_follower_body_state,
dim=model.size.sum_of_num_joints,
inputs=[
model.joints.bid_F,
model.joints.B_r_Bj,
model.joints.F_r_Fj,
model.joints.X_j,
data.bodies.q_i,
data.bodies.u_i,
],
)
###
# Tests
###
class TestKinematicsJoints(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True to enable verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.info("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_single_revolute_joint(self):
# Construct the model description using the ModelBuilderKamino
builder = build_unary_revolute_joint_test()
# Create the model and state
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
# Set the state of the Follower body to a known state
set_joint_follower_body_state(model, data)
msg.info("data.bodies.q_i: %s", data.bodies.q_i)
msg.info("data.bodies.u_i: %s", data.bodies.u_i)
# Update the state of the joints
compute_joints_data(model=model, data=data, q_j_p=wp.zeros_like(data.joints.q_j))
msg.info("data.joints.p_j: %s", data.joints.p_j)
# Extract joint data
r_j_np = data.joints.r_j.numpy().copy()
dr_j_np = data.joints.dr_j.numpy().copy()
q_j_np = data.joints.q_j.numpy().copy()
dq_j_np = data.joints.dq_j.numpy().copy()
msg.info("[measured]: r_j: %s", r_j_np)
msg.info("[measured]: dr_j: %s", dr_j_np)
msg.info("[measured]: q_j: %s", q_j_np)
msg.info("[measured]: dq_j: %s", dq_j_np)
# Construct expected joint data
r_j_expected = np.array([J_DR_J[0], J_DR_J[1], J_DR_J[2], ROT_RES[1], ROT_RES[2]], dtype=np.float32)
dr_j_expected = np.array([J_DV_J[0], J_DV_J[1], J_DV_J[2], J_DOMEGA_J[1], J_DOMEGA_J[2]], dtype=np.float32)
q_j_expected = np.array([Q_X_J], dtype=np.float32)
dq_j_expected = np.array([J_DOMEGA_J[0]], dtype=np.float32)
msg.info("[expected]: r_j: %s", r_j_expected)
msg.info("[expected]: dr_j: %s", dr_j_expected)
msg.info("[expected]: q_j: %s", q_j_expected)
msg.info("[expected]: dq_j: %s", dq_j_expected)
# Check the joint state values
np.testing.assert_almost_equal(r_j_np, r_j_expected)
np.testing.assert_almost_equal(dr_j_np, dr_j_expected)
np.testing.assert_almost_equal(q_j_np, q_j_expected)
np.testing.assert_almost_equal(dq_j_np, dq_j_expected)
def test_02_multiple_revolute_joints(self):
# Construct the model description using the ModelBuilderKamino
builder = make_homogeneous_builder(num_worlds=4, build_fn=build_unary_revolute_joint_test)
# Create the model and state
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
# Set the state of the Follower body to a known state
set_joint_follower_body_state(model, data)
msg.info("data.bodies.q_i:\n%s", data.bodies.q_i)
msg.info("data.bodies.u_i:\n%s", data.bodies.u_i)
# Update the state of the joints
compute_joints_data(model=model, data=data, q_j_p=wp.zeros_like(data.joints.q_j))
msg.info("data.joints.p_j: %s", data.joints.p_j)
# Extract joint data
r_j_np = data.joints.r_j.numpy().copy()
dr_j_np = data.joints.dr_j.numpy().copy()
q_j_np = data.joints.q_j.numpy().copy()
dq_j_np = data.joints.dq_j.numpy().copy()
msg.info("[measured]: r_j: %s", r_j_np)
msg.info("[measured]: dr_j: %s", dr_j_np)
msg.info("[measured]: q_j: %s", q_j_np)
msg.info("[measured]: dq_j: %s", dq_j_np)
# Construct expected joint data
r_j_expected = np.array([J_DR_J[0], J_DR_J[1], J_DR_J[2], ROT_RES[1], ROT_RES[2]], dtype=np.float32)
dr_j_expected = np.array([J_DV_J[0], J_DV_J[1], J_DV_J[2], J_DOMEGA_J[1], J_DOMEGA_J[2]], dtype=np.float32)
q_j_expected = np.array([Q_X_J], dtype=np.float32)
dq_j_expected = np.array([J_DOMEGA_J[0]], dtype=np.float32)
# Tile expected values for all joints
r_j_expected = np.tile(r_j_expected, builder.num_worlds)
dr_j_expected = np.tile(dr_j_expected, builder.num_worlds)
q_j_expected = np.tile(q_j_expected, builder.num_worlds)
dq_j_expected = np.tile(dq_j_expected, builder.num_worlds)
msg.info("[expected]: r_j: %s", r_j_expected)
msg.info("[expected]: dr_j: %s", dr_j_expected)
msg.info("[expected]: q_j: %s", q_j_expected)
msg.info("[expected]: dq_j: %s", dq_j_expected)
# Check the joint state values
np.testing.assert_almost_equal(r_j_np, r_j_expected)
np.testing.assert_almost_equal(dr_j_np, dr_j_expected)
np.testing.assert_almost_equal(q_j_np, q_j_expected)
np.testing.assert_almost_equal(dq_j_np, dq_j_expected)
def test_03_single_dynamic_revolute_joint(self):
# Construct the model description using the ModelBuilderKamino
builder = build_unary_revolute_joint_test(dynamic=True, implicit_pd=True)
# Create the model and state
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
model.time.set_uniform_timestep(0.01)
# Optionally print model parameters for debugging
msg.info("model.time.dt: %s", model.time.dt)
msg.info("model.joints.a_j: %s", model.joints.a_j)
msg.info("model.joints.b_j: %s", model.joints.b_j)
msg.info("model.joints.k_p_j: %s", model.joints.k_p_j)
msg.info("model.joints.k_d_j: %s\n", model.joints.k_d_j)
msg.info("model.joints.num_cts: %s", model.joints.num_cts)
msg.info("model.joints.num_dynamic_cts: %s", model.joints.num_dynamic_cts)
msg.info("model.joints.num_kinematic_cts: %s", model.joints.num_kinematic_cts)
msg.info("model.joints.dynamic_cts_offset: %s", model.joints.dynamic_cts_offset)
msg.info("model.joints.kinematic_cts_offset: %s\n", model.joints.kinematic_cts_offset)
msg.info("model.info.num_joint_dynamic_cts: %s", model.info.num_joint_dynamic_cts)
msg.info("model.info.joint_dynamic_cts_offset: %s\n", model.info.joint_dynamic_cts_offset)
# Set the state of the Follower body to a known state
set_joint_follower_body_state(model, data)
msg.info("data.bodies.q_i: %s", data.bodies.q_i)
msg.info("data.bodies.u_i: %s\n", data.bodies.u_i)
# Update the state of the joints
compute_joints_data(model=model, data=data, q_j_p=wp.zeros_like(data.joints.q_j))
msg.info("data.joints.p_j: %s\n", data.joints.p_j)
# Extract measured joint data
r_j_np = data.joints.r_j.numpy().copy()
dr_j_np = data.joints.dr_j.numpy().copy()
q_j_np = data.joints.q_j.numpy().copy()
dq_j_np = data.joints.dq_j.numpy().copy()
m_j_np = data.joints.m_j.numpy().copy()
inv_m_j_np = data.joints.inv_m_j.numpy().copy()
dq_b_j_np = data.joints.dq_b_j.numpy().copy()
q_j_ref_np = data.joints.q_j_ref.numpy().copy()
dq_j_ref_np = data.joints.dq_j_ref.numpy().copy()
tau_j_ref_np = data.joints.tau_j_ref.numpy().copy()
msg.info("[measured]: r_j: %s", r_j_np)
msg.info("[measured]: dr_j: %s", dr_j_np)
msg.info("[measured]: q_j: %s", q_j_np)
msg.info("[measured]: dq_j: %s\n", dq_j_np)
msg.info("[measured]: m_j: %s", m_j_np)
msg.info("[measured]: inv_m_j: %s", inv_m_j_np)
msg.info("[measured]: dq_b_j: %s\n", dq_b_j_np)
msg.info("[measured]: q_j_ref: %s", q_j_ref_np)
msg.info("[measured]: dq_j_ref: %s\n", dq_j_ref_np)
msg.info("[measured]: tau_j_ref: %s\n", tau_j_ref_np)
# Compute expected joint dynamics values based on the PD control
# law and the equations of motion for a single revolute joint
dt = model.time.dt.numpy().copy()[0]
a_j_np = model.joints.a_j.numpy().copy()
b_j_np = model.joints.b_j.numpy().copy()
k_p_j_np = model.joints.k_p_j.numpy().copy()
k_d_j_np = model.joints.k_d_j.numpy().copy()
m_j_exp_val = a_j_np[0] + dt * (b_j_np[0] + k_d_j_np[0]) + dt * dt * k_p_j_np[0]
inv_m_j_exp_val = 1.0 / m_j_exp_val
tau_j_exp_val = tau_j_ref_np[0] + k_p_j_np[0] * (q_j_ref_np[0] - q_j_np[0]) + k_d_j_np[0] * dq_j_ref_np[0]
h_j_exp_val = a_j_np[0] * dq_j_np[0] + dt * tau_j_exp_val
dq_b_j_exp_val = inv_m_j_exp_val * h_j_exp_val
# Construct expected joint data
r_j_expected = np.array([J_DR_J[0], J_DR_J[1], J_DR_J[2], ROT_RES[1], ROT_RES[2]], dtype=np.float32)
dr_j_expected = np.array([J_DV_J[0], J_DV_J[1], J_DV_J[2], J_DOMEGA_J[1], J_DOMEGA_J[2]], dtype=np.float32)
q_j_expected = np.array([Q_X_J], dtype=np.float32)
dq_j_expected = np.array([J_DOMEGA_J[0]], dtype=np.float32)
m_j_expected = np.array([m_j_exp_val], dtype=np.float32)
tau_j_expected = np.array([tau_j_exp_val], dtype=np.float32)
h_j_expected = np.array([h_j_exp_val], dtype=np.float32)
inv_m_j_expected = np.array([inv_m_j_exp_val], dtype=np.float32)
dq_b_j_expected = np.array([dq_b_j_exp_val], dtype=np.float32)
msg.info("[expected]: r_j: %s", r_j_expected)
msg.info("[expected]: dr_j: %s", dr_j_expected)
msg.info("[expected]: q_j: %s", q_j_expected)
msg.info("[expected]: dq_j: %s\n", dq_j_expected)
msg.info("[expected]: m_j: %s", m_j_expected)
msg.info("[expected]: tau_j: %s", tau_j_expected)
msg.info("[expected]: h_j: %s", h_j_expected)
msg.info("[expected]: inv_m_j: %s", inv_m_j_expected)
msg.info("[expected]: dq_b_j: %s\n", dq_b_j_expected)
# Check the joint data values
np.testing.assert_almost_equal(r_j_np, r_j_expected)
np.testing.assert_almost_equal(dr_j_np, dr_j_expected)
np.testing.assert_almost_equal(q_j_np, q_j_expected)
np.testing.assert_almost_equal(dq_j_np, dq_j_expected)
np.testing.assert_almost_equal(m_j_np, m_j_expected)
np.testing.assert_almost_equal(inv_m_j_np, inv_m_j_expected)
np.testing.assert_almost_equal(dq_b_j_np, dq_b_j_expected)
def test_04_multiple_dynamic_revolute_joints(self):
# Construct the model description using the ModelBuilderKamino
builder = make_homogeneous_builder(
num_worlds=4, build_fn=build_unary_revolute_joint_test, dynamic=True, implicit_pd=True
)
# Create the model and data
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
model.time.set_uniform_timestep(0.01)
# Optionally print model parameters for debugging
msg.info("model.time.dt: %s", model.time.dt)
msg.info("model.joints.a_j: %s", model.joints.a_j)
msg.info("model.joints.b_j: %s", model.joints.b_j)
msg.info("model.joints.k_p_j: %s", model.joints.k_p_j)
msg.info("model.joints.k_d_j: %s\n", model.joints.k_d_j)
msg.info("model.joints.num_cts: %s", model.joints.num_cts)
msg.info("model.joints.num_dynamic_cts: %s", model.joints.num_dynamic_cts)
msg.info("model.joints.num_kinematic_cts: %s", model.joints.num_kinematic_cts)
msg.info("model.joints.dynamic_cts_offset: %s", model.joints.dynamic_cts_offset)
msg.info("model.joints.kinematic_cts_offset: %s\n", model.joints.kinematic_cts_offset)
msg.info("model.info.num_joint_dynamic_cts: %s", model.info.num_joint_dynamic_cts)
msg.info("model.info.joint_dynamic_cts_offset: %s\n", model.info.joint_dynamic_cts_offset)
# Set the state of the Follower body to a known state
set_joint_follower_body_state(model, data)
msg.info("data.bodies.q_i:\n%s", data.bodies.q_i)
msg.info("data.bodies.u_i:\n%s\n", data.bodies.u_i)
# Update the state of the joints
compute_joints_data(model=model, data=data, q_j_p=wp.zeros_like(data.joints.q_j))
msg.info("data.joints.p_j:\n%s", data.joints.p_j)
# Extract measured joint data
r_j_np = data.joints.r_j.numpy().copy()
dr_j_np = data.joints.dr_j.numpy().copy()
q_j_np = data.joints.q_j.numpy().copy()
dq_j_np = data.joints.dq_j.numpy().copy()
m_j_np = data.joints.m_j.numpy().copy()
inv_m_j_np = data.joints.inv_m_j.numpy().copy()
dq_b_j_np = data.joints.dq_b_j.numpy().copy()
q_j_ref_np = data.joints.q_j_ref.numpy().copy()
dq_j_ref_np = data.joints.dq_j_ref.numpy().copy()
tau_j_ref_np = data.joints.tau_j_ref.numpy().copy()
msg.info("[measured]: r_j: %s", r_j_np)
msg.info("[measured]: dr_j: %s", dr_j_np)
msg.info("[measured]: q_j: %s", q_j_np)
msg.info("[measured]: dq_j: %s\n", dq_j_np)
msg.info("[measured]: m_j: %s", m_j_np)
msg.info("[measured]: inv_m_j: %s", inv_m_j_np)
msg.info("[measured]: dq_b_j: %s\n", dq_b_j_np)
msg.info("[measured]: q_j_ref: %s", q_j_ref_np)
msg.info("[measured]: dq_j_ref: %s\n", dq_j_ref_np)
msg.info("[measured]: tau_j_ref: %s\n", tau_j_ref_np)
# Compute expected joint dynamics values based on the PD control
# law and the equations of motion for a single revolute joint
dt = model.time.dt.numpy().copy()[0]
a_j_np = model.joints.a_j.numpy().copy()
b_j_np = model.joints.b_j.numpy().copy()
k_p_j_np = model.joints.k_p_j.numpy().copy()
k_d_j_np = model.joints.k_d_j.numpy().copy()
m_j_exp_val = a_j_np[0] + dt * (b_j_np[0] + k_d_j_np[0]) + dt * dt * k_p_j_np[0]
inv_m_j_exp_val = 1.0 / m_j_exp_val
tau_j_exp_val = tau_j_ref_np[0] + k_p_j_np[0] * (q_j_ref_np[0] - q_j_np[0]) + k_d_j_np[0] * dq_j_ref_np[0]
h_j_exp_val = a_j_np[0] * dq_j_np[0] + dt * tau_j_exp_val
dq_b_j_exp_val = inv_m_j_exp_val * h_j_exp_val
# Construct expected joint data
r_j_expected = np.array([J_DR_J[0], J_DR_J[1], J_DR_J[2], ROT_RES[1], ROT_RES[2]], dtype=np.float32)
dr_j_expected = np.array([J_DV_J[0], J_DV_J[1], J_DV_J[2], J_DOMEGA_J[1], J_DOMEGA_J[2]], dtype=np.float32)
q_j_expected = np.array([Q_X_J], dtype=np.float32)
dq_j_expected = np.array([J_DOMEGA_J[0]], dtype=np.float32)
m_j_expected = np.array([m_j_exp_val], dtype=np.float32)
h_j_expected = np.array([h_j_exp_val], dtype=np.float32)
inv_m_j_expected = np.array([inv_m_j_exp_val], dtype=np.float32)
dq_b_j_expected = np.array([dq_b_j_exp_val], dtype=np.float32)
# Tile expected values for all joints
r_j_expected = np.tile(r_j_expected, builder.num_worlds)
dr_j_expected = np.tile(dr_j_expected, builder.num_worlds)
q_j_expected = np.tile(q_j_expected, builder.num_worlds)
dq_j_expected = np.tile(dq_j_expected, builder.num_worlds)
m_j_expected = np.tile(m_j_expected, builder.num_worlds)
h_j_expected = np.tile(h_j_expected, builder.num_worlds)
inv_m_j_expected = np.tile(inv_m_j_expected, builder.num_worlds)
dq_b_j_expected = np.tile(dq_b_j_expected, builder.num_worlds)
msg.info("[expected]: r_j: %s", r_j_expected)
msg.info("[expected]: dr_j: %s", dr_j_expected)
msg.info("[expected]: q_j: %s", q_j_expected)
msg.info("[expected]: dq_j: %s\n", dq_j_expected)
msg.info("[expected]: m_j: %s", m_j_expected)
msg.info("[expected]: h_j: %s", h_j_expected)
msg.info("[expected]: inv_m_j: %s", inv_m_j_expected)
msg.info("[expected]: dq_b_j: %s\n", dq_b_j_expected)
# Check the joint data values
np.testing.assert_almost_equal(r_j_np, r_j_expected)
np.testing.assert_almost_equal(dr_j_np, dr_j_expected)
np.testing.assert_almost_equal(q_j_np, q_j_expected)
np.testing.assert_almost_equal(dq_j_np, dq_j_expected)
np.testing.assert_almost_equal(m_j_np, m_j_expected)
np.testing.assert_almost_equal(inv_m_j_np, inv_m_j_expected)
np.testing.assert_almost_equal(dq_b_j_np, dq_b_j_expected)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_kinematics_limits.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: UNIT TESTS: KINEMATICS: LIMITS
"""
import math
import unittest
import warp as wp
from newton._src.solvers.kamino._src.core.data import DataKamino
from newton._src.solvers.kamino._src.core.math import quat_exp, screw, screw_angular, screw_linear
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.core.types import float32, int32, mat33f, transformf, vec3f, vec6f
from newton._src.solvers.kamino._src.kinematics.joints import compute_joints_data
from newton._src.solvers.kamino._src.kinematics.limits import LimitsKamino
from newton._src.solvers.kamino._src.models.builders import basics, testing
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Constants
###
Q_X_J = 0.3 * math.pi
Q_X_J_MAX = 0.25 * math.pi
###
# Kernels
###
@wp.kernel
def _set_joint_follower_body_state(
model_joint_bid_B: wp.array(dtype=int32),
model_joint_bid_F: wp.array(dtype=int32),
model_joint_B_r_Bj: wp.array(dtype=vec3f),
model_joint_F_r_Fj: wp.array(dtype=vec3f),
model_joint_X_j: wp.array(dtype=mat33f),
state_body_q_i: wp.array(dtype=transformf),
state_body_u_i: wp.array(dtype=vec6f),
):
"""
Set the state of the bodies to a certain values in order to check computations of joint states.
"""
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the joint parameters
bid_B = model_joint_bid_B[jid]
bid_F = model_joint_bid_F[jid]
B_r_Bj = model_joint_B_r_Bj[jid]
F_r_Fj = model_joint_F_r_Fj[jid]
X_j = model_joint_X_j[jid]
# Retrieve the current state of the Base body
p_B = state_body_q_i[bid_B]
u_B = state_body_u_i[bid_B]
# Extract the position and orientation of the Base body
r_B = wp.transform_get_translation(p_B)
q_B = wp.transform_get_rotation(p_B)
R_B = wp.quat_to_matrix(q_B)
# Extract the linear and angular velocity of the Base body
v_B = screw_linear(u_B)
omega_B = screw_angular(u_B)
# Define the joint rotation offset
# NOTE: X_j projects quantities into the joint frame
# NOTE: X_j^T projects quantities into the outer frame (world or body)
q_x_j = Q_X_J
theta_y_j = 0.0
theta_z_j = 0.0
j_dR_j = vec3f(q_x_j, theta_y_j, theta_z_j) # Joint offset as rotation vector
q_jq = quat_exp(j_dR_j) # Joint offset as rotation quaternion
R_jq = wp.quat_to_matrix(q_jq) # Joint offset as rotation matrix
# Define the joint translation offset
j_dr_j = vec3f(0.0)
# Define the joint twist offset
j_dv_j = vec3f(0.0)
j_domega_j = vec3f(0.0)
# Follower body rotation via the Base and joint frames
R_B_X_j = R_B @ X_j
R_F_new = R_B_X_j @ R_jq @ wp.transpose(X_j)
q_F_new = wp.quat_from_matrix(R_F_new)
# Follower body position via the Base and joint frames
r_Fj = R_F_new @ F_r_Fj
r_F_new = r_B + R_B @ B_r_Bj + R_B_X_j @ j_dr_j - r_Fj
# Follower body twist via the Base and joint frames
r_Bj = R_B @ B_r_Bj
r_Fj = R_F_new @ F_r_Fj
omega_F_new = R_B_X_j @ j_domega_j + omega_B
v_F_new = R_B_X_j @ j_dv_j + v_B + wp.cross(omega_B, r_Bj) - wp.cross(omega_F_new, r_Fj)
# Offset the bose of the body by a fixed amount
state_body_q_i[bid_F] = wp.transformation(r_F_new, q_F_new, dtype=float32)
state_body_u_i[bid_F] = screw(v_F_new, omega_F_new)
###
# Launchers
###
def set_joint_follower_body_state(model: ModelKamino, data: DataKamino):
wp.launch(
_set_joint_follower_body_state,
dim=model.size.sum_of_num_joints,
inputs=[
model.joints.bid_B,
model.joints.bid_F,
model.joints.B_r_Bj,
model.joints.F_r_Fj,
model.joints.X_j,
data.bodies.q_i,
data.bodies.u_i,
],
)
###
# Tests
###
class TestKinematicsLimits(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True to enable verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
msg.info("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_create_empty_limits_container(self):
"""
Tests the creation of an empty LimitsKamino container (for deferred allocation).
"""
# Create a LimitsKamino container
limits = LimitsKamino(device=self.default_device)
# Check the initial state of the limits
self.assertEqual(limits._data.model_max_limits_host, 0)
self.assertEqual(limits._data.world_max_limits_host, [])
def test_01_allocate_limits_container_from_homogeneous_builder(self):
"""
Tests the allocation of a LimitsKamino container.
"""
# Construct the model description using the ModelBuilderKamino
builder = make_homogeneous_builder(num_worlds=3, build_fn=basics.build_boxes_fourbar)
model = builder.finalize(device=self.default_device)
# Create a LimitsKamino container
limits = LimitsKamino(model=model, device=self.default_device)
# Check the initial state of the limits
self.assertIsNotNone(limits.model_max_limits)
self.assertIsNotNone(limits.model_active_limits)
self.assertIsNotNone(limits.world_max_limits)
self.assertIsNotNone(limits.world_max_limits)
self.assertIsNotNone(limits.wid)
self.assertIsNotNone(limits.lid)
self.assertIsNotNone(limits.jid)
self.assertIsNotNone(limits.bids)
self.assertIsNotNone(limits.dof)
self.assertIsNotNone(limits.side)
self.assertIsNotNone(limits.r_q)
self.assertIsNotNone(limits.key)
self.assertIsNotNone(limits.reaction)
self.assertIsNotNone(limits.velocity)
# Check the shapes of the limits arrays
self.assertEqual(limits.model_max_limits_host, 12)
self.assertEqual(limits.world_max_limits_host, [4, 4, 4])
self.assertEqual(limits.model_active_limits.shape, (1,))
self.assertEqual(limits.model_active_limits.shape, (1,))
self.assertEqual(limits.world_max_limits.shape, (3,))
self.assertEqual(limits.world_active_limits.shape, (3,))
# Optional verbose output
msg.info("limits.model_max_limits_host: %s", limits.model_max_limits_host)
msg.info("limits.world_max_limits_host: %s", limits.world_max_limits_host)
msg.info("limits.model_max_limits: %s", limits.model_max_limits)
msg.info("limits.model_active_limits: %s", limits.model_active_limits)
msg.info("limits.world_max_limits: %s", limits.world_max_limits)
msg.info("limits.world_active_limits: %s", limits.world_active_limits)
msg.info("limits.wid: %s", limits.wid)
msg.info("limits.lid: %s", limits.lid)
msg.info("limits.jid: %s", limits.jid)
msg.info("limits.bids:\n%s", limits.bids)
msg.info("limits.dof: %s", limits.dof)
msg.info("limits.side: %s", limits.side)
msg.info("limits.r_q: %s", limits.r_q)
msg.info("limits.key: %s", limits.key)
msg.info("limits.reaction: %s", limits.reaction)
msg.info("limits.velocity: %s", limits.velocity)
def test_02_check_revolute_joint(self):
# Construct the model description using the ModelBuilderKamino
builder = make_homogeneous_builder(num_worlds=4, build_fn=testing.build_unary_revolute_joint_test)
num_worlds = builder.num_worlds
# Create the model and state
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
# Set the state of the Follower body to a known state
set_joint_follower_body_state(model, data)
# Update the state of the joints
compute_joints_data(model=model, data=data, q_j_p=wp.zeros_like(data.joints.q_j))
# Optional verbose output
msg.info("model.joints.q_j_min: %s", model.joints.q_j_min)
msg.info("model.joints.q_j_max: %s", model.joints.q_j_max)
msg.info("model.joints.dq_j_max: %s", model.joints.dq_j_max)
msg.info("model.joints.tau_j_max: %s", model.joints.tau_j_max)
msg.info("data.bodies.q_i:\n%s", data.bodies.q_i)
msg.info("data.bodies.u_i:\n%s", data.bodies.u_i)
msg.info("data.joints.p_j:\n%s", data.joints.p_j)
msg.info("data.joints.r_j: %s", data.joints.r_j)
msg.info("data.joints.dr_j: %s", data.joints.dr_j)
msg.info("data.joints.q_j: %s", data.joints.q_j)
msg.info("data.joints.dq_j: %s\n\n", data.joints.dq_j)
# Create a LimitsKamino container
limits = LimitsKamino(model=model, device=self.default_device)
# Optional verbose output
msg.info("[before]: limits.model_max_limits_host: %s", limits.model_max_limits_host)
msg.info("[before]: limits.world_max_limits_host: %s", limits.world_max_limits_host)
msg.info("[before]: limits.model_max_limits: %s", limits.model_max_limits)
msg.info("[before]: limits.model_active_limits: %s", limits.model_active_limits)
msg.info("[before]: limits.world_max_limits: %s", limits.world_max_limits)
msg.info("[before]: limits.world_active_limits: %s", limits.world_active_limits)
msg.info("[before]: limits.wid: %s", limits.wid)
msg.info("[before]: limits.lid: %s", limits.lid)
msg.info("[before]: limits.jid: %s", limits.jid)
msg.info("[before]: limits.bids:\n%s", limits.bids)
msg.info("[before]: limits.dof: %s", limits.dof)
msg.info("[before]: limits.side: %s", limits.side)
msg.info("[before]: limits.r_q: %s", limits.r_q)
msg.info("[before]: limits.key: %s", limits.key)
msg.info("[before]: limits.reaction: %s", limits.reaction)
msg.info("[before]: limits.velocity: %s", limits.velocity)
# Check for active joint limits
limits.detect(model, data)
# Optional verbose output
msg.info("[after]: limits.model_max_limits_host: %s", limits.model_max_limits_host)
msg.info("[after]: limits.world_max_limits_host: %s", limits.world_max_limits_host)
msg.info("[after]: limits.model_max_limits: %s", limits.model_max_limits)
msg.info("[after]: limits.model_active_limits: %s", limits.model_active_limits)
msg.info("[after]: limits.world_max_limits: %s", limits.world_max_limits)
msg.info("[after]: limits.world_active_limits: %s", limits.world_active_limits)
msg.info("[after]: limits.wid: %s", limits.wid)
msg.info("[after]: limits.lid: %s", limits.lid)
msg.info("[after]: limits.jid: %s", limits.jid)
msg.info("[after]: limits.bids:\n%s", limits.bids)
msg.info("[after]: limits.dof: %s", limits.dof)
msg.info("[after]: limits.side: %s", limits.side)
msg.info("[after]: limits.r_q: %s", limits.r_q)
msg.info("[after]: limits.key: %s", limits.key)
msg.info("[after]: limits.reaction: %s", limits.reaction)
msg.info("[after]: limits.velocity: %s", limits.velocity)
# Check the limits
limits_num_np = limits.world_active_limits.numpy()
limits_wid_np = limits.wid.numpy()
limits_lid_np = limits.lid.numpy()
limits_jid_np = limits.jid.numpy()
limits_dof_np = limits.dof.numpy()
limits_side_np = limits.side.numpy()
limits_r_q_np = limits.r_q.numpy()
for i in range(num_worlds):
# Check the number of limits for this world
self.assertEqual(limits_num_np[i], 1)
for j in range(limits_num_np[i]):
# Check the limits for this world
self.assertEqual(limits_wid_np[i], i)
self.assertEqual(limits_lid_np[i], j)
self.assertEqual(limits_jid_np[i], i * limits_num_np[i] + j)
self.assertEqual(limits_dof_np[i], j)
self.assertEqual(limits_side_np[i], -1)
self.assertAlmostEqual(limits_r_q_np[i * limits_num_np[i] + j], Q_X_J_MAX - Q_X_J, places=6)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_core.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the base classes in linalg/core.py"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.linalg.core import (
DenseRectangularMultiLinearInfo,
DenseSquareMultiLinearInfo,
make_dtype_tolerance,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestLinAlgCoreMakeTolerance(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_tolerance_from_defaults(self):
tol = make_dtype_tolerance()
msg.debug(f"tol = {tol} (type: {type(tol)})")
self.assertIsInstance(tol, wp.float32)
self.assertAlmostEqual(tol, wp.float32(np.finfo(np.float32).eps), places=7)
def test_01_make_tolerance_default_for_wp_float64(self):
tol = make_dtype_tolerance(dtype=wp.float64)
msg.debug(f"tol = {tol} (type: {type(tol)})")
self.assertIsInstance(tol, wp.float64)
self.assertAlmostEqual(tol, wp.float64(np.finfo(np.float64).eps), places=23)
def test_02_make_tolerance_default_for_wp_float32(self):
tol = make_dtype_tolerance(dtype=wp.float32)
msg.debug(f"tol = {tol} (type: {type(tol)})")
self.assertIsInstance(tol, wp.float32)
self.assertAlmostEqual(tol, wp.float32(np.finfo(np.float32).eps), places=7)
def test_03_make_tolerance_from_np_float64_to_wp_float64(self):
tol = make_dtype_tolerance(tol=np.float64(1e-5), dtype=wp.float64)
msg.debug(f"tol = {tol} (type: {type(tol)})")
self.assertIsInstance(tol, wp.float64)
self.assertAlmostEqual(tol, wp.float64(np.float64(1e-5)), places=23)
def test_04_make_tolerance_from_np_float64_to_wp_float32(self):
tol = make_dtype_tolerance(tol=np.float64(1e-5), dtype=wp.float32)
msg.debug(f"tol = {tol} (type: {type(tol)})")
self.assertIsInstance(tol, wp.float32)
self.assertAlmostEqual(tol, wp.float32(np.float64(1e-5)), places=7)
def test_05_make_tolerance_from_float_to_wp_float32(self):
tol = make_dtype_tolerance(tol=1e-6, dtype=wp.float32)
msg.debug(f"tol = {tol} (type: {type(tol)})")
self.assertIsInstance(tol, wp.float32)
self.assertAlmostEqual(tol, wp.float32(1e-6), places=7)
def test_06_make_tolerance_smaller_than_eps(self):
tol = make_dtype_tolerance(tol=1e-10, dtype=wp.float32)
msg.debug(f"tol = {tol} (type: {type(tol)})")
self.assertIsInstance(tol, wp.float32)
self.assertAlmostEqual(tol, wp.float32(np.finfo(np.float32).eps), places=7)
class TestLinAlgCoreDenseMultiLinearRectangularInfo(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_single_rectangular_default(self):
dims = (3, 4)
info = DenseRectangularMultiLinearInfo()
info.finalize(dimensions=dims)
msg.debug("info:\n%s", info)
msg.debug("info.maxdim: %s", info.maxdim)
msg.debug("info.dim: %s", info.dim)
msg.debug("info.mio: %s", info.mio)
msg.debug("info.rvio: %s", info.rvio)
msg.debug("info.ivio: %s", info.ivio)
self.assertEqual(info.num_blocks, 1)
self.assertEqual(info.dimensions[0], dims)
self.assertEqual(info.max_dimensions, dims)
self.assertEqual(info.total_mat_size, dims[0] * dims[1])
self.assertEqual(info.total_rhs_size, dims[0])
self.assertEqual(info.total_inp_size, dims[1])
self.assertEqual(info.dtype, wp.float32)
self.assertEqual(info.itype, wp.int32)
self.assertEqual(info.maxdim.shape, (1,))
self.assertEqual(info.dim.shape, (1,))
self.assertEqual(info.mio.shape, (1,))
self.assertEqual(info.rvio.shape, (1,))
self.assertEqual(info.ivio.shape, (1,))
self.assertEqual(info.maxdim.numpy()[0][0], 3)
self.assertEqual(info.maxdim.numpy()[0][1], 4)
self.assertEqual(info.dim.numpy()[0][0], 3)
self.assertEqual(info.dim.numpy()[0][1], 4)
self.assertEqual(info.mio.numpy()[0], 0)
self.assertEqual(info.rvio.numpy()[0], 0)
self.assertEqual(info.ivio.numpy()[0], 0)
def test_01_make_single_rectangular_wp_float64_int64(self):
dims = (3, 4)
info = DenseRectangularMultiLinearInfo()
info.finalize(dimensions=dims, dtype=wp.float64, itype=wp.int64)
msg.debug("info:\n%s", info)
msg.debug("info.maxdim: %s", info.maxdim)
msg.debug("info.dim: %s", info.dim)
msg.debug("info.mio: %s", info.mio)
msg.debug("info.rvio: %s", info.rvio)
msg.debug("info.ivio: %s", info.ivio)
self.assertEqual(info.num_blocks, 1)
self.assertEqual(info.dimensions[0], dims)
self.assertEqual(info.max_dimensions, dims)
self.assertEqual(info.total_mat_size, dims[0] * dims[1])
self.assertEqual(info.total_rhs_size, dims[0])
self.assertEqual(info.total_inp_size, dims[1])
self.assertEqual(info.dtype, wp.float64)
self.assertEqual(info.itype, wp.int64)
self.assertEqual(info.maxdim.shape, (1,))
self.assertEqual(info.dim.shape, (1,))
self.assertEqual(info.mio.shape, (1,))
self.assertEqual(info.rvio.shape, (1,))
self.assertEqual(info.ivio.shape, (1,))
self.assertEqual(info.maxdim.numpy()[0][0], 3)
self.assertEqual(info.maxdim.numpy()[0][1], 4)
self.assertEqual(info.dim.numpy()[0][0], 3)
self.assertEqual(info.dim.numpy()[0][1], 4)
self.assertEqual(info.mio.numpy()[0], 0)
self.assertEqual(info.rvio.numpy()[0], 0)
self.assertEqual(info.ivio.numpy()[0], 0)
def test_02_make_multiple_rectangular(self):
dims = [(3, 4), (2, 5), (4, 3)]
info = DenseRectangularMultiLinearInfo()
info.finalize(dimensions=dims)
msg.debug("info:\n%s", info)
msg.debug("info.maxdim: %s", info.maxdim)
msg.debug("info.dim: %s", info.dim)
msg.debug("info.mio: %s", info.mio)
msg.debug("info.rvio: %s", info.rvio)
msg.debug("info.ivio: %s", info.ivio)
self.assertEqual(info.num_blocks, len(dims))
self.assertEqual(info.dimensions, dims)
self.assertEqual(info.max_dimensions, (4, 5))
self.assertEqual(info.total_mat_size, sum(m * n for m, n in dims))
self.assertEqual(info.total_rhs_size, sum(m for m, _ in dims))
self.assertEqual(info.total_inp_size, sum(n for _, n in dims))
self.assertEqual(info.dtype, wp.float32)
self.assertEqual(info.itype, wp.int32)
self.assertEqual(info.maxdim.shape, (len(dims),))
self.assertEqual(info.dim.shape, (len(dims),))
self.assertEqual(info.mio.shape, (len(dims),))
self.assertEqual(info.rvio.shape, (len(dims),))
self.assertEqual(info.ivio.shape, (len(dims),))
for i, (m, n) in enumerate(dims):
self.assertEqual(info.maxdim.numpy()[i][0], m)
self.assertEqual(info.maxdim.numpy()[i][1], n)
self.assertEqual(info.dim.numpy()[i][0], m)
self.assertEqual(info.dim.numpy()[i][1], n)
self.assertEqual(info.mio.numpy()[i], sum(d[0] * d[1] for d in dims[:i]))
self.assertEqual(info.rvio.numpy()[i], sum(d[0] for d in dims[:i]))
self.assertEqual(info.ivio.numpy()[i], sum(d[1] for d in dims[:i]))
class TestLinAlgCoreDenseMultiLinearSquareInfo(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_single_square_default(self):
dims = 4
info = DenseSquareMultiLinearInfo()
info.finalize(dimensions=dims)
msg.debug("info:\n%s", info)
msg.debug("info.maxdim: %s", info.maxdim)
msg.debug("info.dim: %s", info.dim)
msg.debug("info.mio: %s", info.mio)
msg.debug("info.vio: %s", info.vio)
self.assertEqual(info.num_blocks, 1)
self.assertEqual(info.dimensions[0], dims)
self.assertEqual(info.max_dimension, dims)
self.assertEqual(info.total_mat_size, dims * dims)
self.assertEqual(info.total_vec_size, dims)
self.assertEqual(info.dtype, wp.float32)
self.assertEqual(info.itype, wp.int32)
self.assertEqual(info.maxdim.shape, (1,))
self.assertEqual(info.dim.shape, (1,))
self.assertEqual(info.mio.shape, (1,))
self.assertEqual(info.vio.shape, (1,))
self.assertEqual(info.maxdim.numpy()[0], dims)
self.assertEqual(info.dim.numpy()[0], dims)
self.assertEqual(info.mio.numpy()[0], 0)
self.assertEqual(info.vio.numpy()[0], 0)
def test_01_make_single_square_wp_float64_int64(self):
dims = 13
info = DenseSquareMultiLinearInfo()
info.finalize(dimensions=dims, dtype=wp.float64, itype=wp.int64)
msg.debug("info:\n%s", info)
msg.debug("info.maxdim: %s", info.maxdim)
msg.debug("info.dim: %s", info.dim)
msg.debug("info.mio: %s", info.mio)
msg.debug("info.vio: %s", info.vio)
self.assertEqual(info.num_blocks, 1)
self.assertEqual(info.dimensions[0], dims)
self.assertEqual(info.max_dimension, dims)
self.assertEqual(info.total_mat_size, dims * dims)
self.assertEqual(info.total_vec_size, dims)
self.assertEqual(info.dtype, wp.float64)
self.assertEqual(info.itype, wp.int64)
self.assertEqual(info.maxdim.shape, (1,))
self.assertEqual(info.dim.shape, (1,))
self.assertEqual(info.mio.shape, (1,))
self.assertEqual(info.vio.shape, (1,))
self.assertEqual(info.maxdim.numpy()[0], dims)
self.assertEqual(info.dim.numpy()[0], dims)
self.assertEqual(info.mio.numpy()[0], 0)
self.assertEqual(info.vio.numpy()[0], 0)
def test_02_make_multiple_square(self):
dims = [4, 10, 5, 12]
info = DenseSquareMultiLinearInfo()
info.finalize(dimensions=dims)
msg.debug("info:\n%s", info)
msg.debug("info.maxdim: %s", info.maxdim)
msg.debug("info.dim: %s", info.dim)
msg.debug("info.mio: %s", info.mio)
msg.debug("info.vio: %s", info.vio)
self.assertEqual(info.num_blocks, len(dims))
self.assertEqual(info.dimensions, dims)
self.assertEqual(info.max_dimension, max(dims))
self.assertEqual(info.total_mat_size, sum(n * n for n in dims))
self.assertEqual(info.total_vec_size, sum(n for n in dims))
self.assertEqual(info.dtype, wp.float32)
self.assertEqual(info.itype, wp.int32)
self.assertEqual(info.maxdim.shape, (len(dims),))
self.assertEqual(info.dim.shape, (len(dims),))
self.assertEqual(info.mio.shape, (len(dims),))
self.assertEqual(info.vio.shape, (len(dims),))
for i, n in enumerate(dims):
self.assertEqual(info.maxdim.numpy()[i], n)
self.assertEqual(info.dim.numpy()[i], n)
self.assertEqual(info.mio.numpy()[i], sum(d * d for d in dims[:i]))
self.assertEqual(info.vio.numpy()[i], sum(d for d in dims[:i]))
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_factorize_llt_blocked.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the LLTSequentialSolver from linalg/linear.py"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.linalg.core import DenseLinearOperatorData, DenseSquareMultiLinearInfo
from newton._src.solvers.kamino._src.linalg.factorize import (
llt_blocked_factorize,
llt_blocked_solve,
llt_blocked_solve_inplace,
make_llt_blocked_factorize_kernel,
make_llt_blocked_solve_inplace_kernel,
make_llt_blocked_solve_kernel,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import get_matrix_block, get_vector_block
from newton._src.solvers.kamino.tests.utils.print import print_error_stats
from newton._src.solvers.kamino.tests.utils.rand import RandomProblemLLT
###
# Tests
###
class TestLinAlgLLTSequential(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
# Set the tile block size for the blocked LLT kernels
self.block_size = 16
# Pre-create kernels to avoid measuring compilation time during tests
self.factorize_kernel = make_llt_blocked_factorize_kernel(self.block_size)
self.solve_kernel = make_llt_blocked_solve_kernel(self.block_size)
self.solve_inplace_kernel = make_llt_blocked_solve_inplace_kernel(self.block_size)
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_single_problem_dims_all_active(self):
"""
Test the sequential LLT solver on a single small problem.
"""
# Constants
# N = 12 # Use this for visual debugging with small matrices
N = 2000 # Use this for performance testing with large matrices
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
# Optional verbose output
msg.debug("Problem:\n%s\n", problem)
msg.debug("b_np:\n%s\n", problem.b_np[0])
msg.debug("A_np:\n%s\n", problem.A_np[0])
msg.debug("X_np:\n%s\n", problem.X_np[0])
msg.debug("y_np:\n%s\n", problem.y_np[0])
msg.debug("x_np:\n%s\n", problem.x_np[0])
msg.info("A_wp (%s):\n%s\n", problem.A_wp.shape, problem.A_wp.numpy().reshape((N, N)))
msg.info("b_wp (%s):\n%s\n", problem.b_wp.shape, problem.b_wp.numpy().reshape((N,)))
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.dims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat (%s):\n%s\n", operator.mat.shape, operator.mat.numpy().reshape((N, N)))
# Allocate LLT data arrays
L_wp = wp.zeros_like(problem.A_wp, device=self.default_device)
y_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt_blocked_factorize(
kernel=self.factorize_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
A=problem.A_wp,
L=L_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(0, L_wp.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[0], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[0], problem.dims[0])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[0].shape, problem.A_np[0])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[0], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[0], problem.dims[0])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt_blocked_solve(
kernel=self.solve_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
b=problem.b_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_np (%s):\n%s\n", problem.y_np[0].shape, problem.y_np[0])
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[0].shape, problem.x_np[0])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt_blocked_solve_inplace(
kernel=self.solve_inplace_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
def test_02_single_problem_dims_partially_active(self):
"""
Test the sequential LLT solver on a single small problem.
"""
# Constants
# N_max = 16 # Use this for visual debugging with small matrices
# N_act = 11
N_max = 2000 # Use this for performance testing with large matrices
N_act = 1537
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N_act,
maxdims=N_max,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
# Optional verbose output
msg.debug("Problem:\n%s\n", problem)
msg.debug("b_np:\n%s\n", problem.b_np[0])
msg.debug("A_np:\n%s\n", problem.A_np[0])
msg.debug("X_np:\n%s\n", problem.X_np[0])
msg.debug("y_np:\n%s\n", problem.y_np[0])
msg.debug("x_np:\n%s\n", problem.x_np[0])
msg.info("A_wp (%s):\n%s\n", problem.A_wp.shape, problem.A_wp.numpy().reshape((N_max, N_max)))
msg.info("b_wp (%s):\n%s\n", problem.b_wp.shape, problem.b_wp.numpy().reshape((N_max,)))
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.maxdims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat (%s):\n%s\n", operator.mat.shape, operator.mat.numpy().reshape((N_max, N_max)))
# IMPORTANT: Now we set the active dimensions in the operator info
operator.info.dim.fill_(N_act)
# Allocate LLT data arrays
L_wp = wp.zeros_like(problem.A_wp, device=self.default_device)
y_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt_blocked_factorize(
kernel=self.factorize_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
A=problem.A_wp,
L=L_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(0, L_wp.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[0], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[0], problem.dims[0])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[0].shape, problem.A_np[0])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[0], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[0], problem.dims[0])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt_blocked_solve(
kernel=self.solve_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
b=problem.b_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt_blocked_solve_inplace(
kernel=self.solve_inplace_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
def test_03_multiple_problems_dims_all_active(self):
"""
Test the sequential LLT solver on multiple small problems.
"""
# Constants
N = [7, 8, 9, 10, 11]
# N = [16, 64, 128, 512, 1024]
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
msg.debug("Problem:\n%s\n", problem)
# Optional verbose output
for i in range(problem.num_blocks):
A_wp_np = get_matrix_block(i, problem.A_wp.numpy(), problem.dims, problem.maxdims)
b_wp_np = get_vector_block(i, problem.b_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("[%d]: b_np:\n%s\n", i, problem.b_np[i])
msg.debug("[%d]: A_np:\n%s\n", i, problem.A_np[i])
msg.debug("[%d]: X_np:\n%s\n", i, problem.X_np[i])
msg.debug("[%d]: y_np:\n%s\n", i, problem.y_np[i])
msg.debug("[%d]: x_np:\n%s\n", i, problem.x_np[i])
msg.info("[%d]: A_wp_np (%s):\n%s\n", i, A_wp_np.shape, A_wp_np)
msg.info("[%d]: b_wp_np (%s):\n%s\n", i, b_wp_np.shape, b_wp_np)
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.dims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat shape:\n%s\n", operator.mat.shape)
# Allocate LLT data arrays
L_wp = wp.zeros_like(problem.A_wp, device=self.default_device)
y_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt_blocked_factorize(
kernel=self.factorize_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
A=problem.A_wp,
L=L_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(i, L_wp.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
msg.info("X_np (%s):\n%s\n", problem.X_np[i].shape, problem.X_np[i])
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[i], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[i], problem.dims[i])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[i].shape, problem.A_np[i])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[i], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[i], problem.dims[i])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt_blocked_solve(
kernel=self.solve_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
b=problem.b_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt_blocked_solve_inplace(
kernel=self.solve_inplace_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
def test_04_multiple_problems_dims_partially_active(self):
"""
Test the sequential LLT solver on multiple small problems.
"""
# Constants
# N_max = [7, 8, 9, 14, 21] # Use this for visual debugging with small matrices
# N_act = [5, 6, 4, 11, 17]
N_max = [16, 64, 128, 512, 1024] # Use this for performance testing with large matrices
N_act = [11, 51, 101, 376, 999]
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N_act,
maxdims=N_max,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
msg.debug("Problem:\n%s\n", problem)
# Optional verbose output
for i in range(problem.num_blocks):
A_wp_np = get_matrix_block(i, problem.A_wp.numpy(), problem.dims, problem.maxdims)
b_wp_np = get_vector_block(i, problem.b_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("[%d]: b_np:\n%s\n", i, problem.b_np[i])
msg.debug("[%d]: A_np:\n%s\n", i, problem.A_np[i])
msg.debug("[%d]: X_np:\n%s\n", i, problem.X_np[i])
msg.debug("[%d]: y_np:\n%s\n", i, problem.y_np[i])
msg.debug("[%d]: x_np:\n%s\n", i, problem.x_np[i])
msg.info("[%d]: A_wp_np (%s):\n%s\n", i, A_wp_np.shape, A_wp_np)
msg.info("[%d]: b_wp_np (%s):\n%s\n", i, b_wp_np.shape, b_wp_np)
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.maxdims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat shape:\n%s\n", operator.mat.shape)
# Allocate LLT data arrays
L_wp = wp.zeros_like(problem.A_wp, device=self.default_device)
y_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# IMPORTANT: Now we set the active dimensions in the operator info
operator.info.dim.assign(N_act)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt_blocked_factorize(
kernel=self.factorize_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
A=problem.A_wp,
L=L_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(i, L_wp.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
msg.info("X_np (%s):\n%s\n", problem.X_np[i].shape, problem.X_np[i])
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[i], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[i], problem.dims[i])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[i].shape, problem.A_np[i])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[i], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[i], problem.dims[i])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt_blocked_solve(
kernel=self.solve_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
b=problem.b_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt_blocked_solve_inplace(
kernel=self.solve_inplace_kernel,
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# TODO: How can we get these to work?
# Ensure the AOT module is compiled for the current device
# wp.compile_aot_module(module=linear, device=wp.get_preferred_device())
# wp.load_aot_module(module=linear.factorize.llt_sequential, device=wp.get_preferred_device())
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_factorize_llt_sequential.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the LLTSequentialSolver from linalg/linear.py"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.linalg.core import DenseLinearOperatorData, DenseSquareMultiLinearInfo
from newton._src.solvers.kamino._src.linalg.factorize import (
llt_sequential_factorize,
llt_sequential_solve,
llt_sequential_solve_inplace,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import get_matrix_block, get_vector_block
from newton._src.solvers.kamino.tests.utils.print import print_error_stats
from newton._src.solvers.kamino.tests.utils.rand import RandomProblemLLT
###
# Tests
###
class TestLinAlgLLTSequential(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_single_problem_dims_all_active(self):
"""
Test the sequential LLT solver on a single small problem.
"""
# Constants
# N = 12 # Use this for visual debugging with small matrices
N = 2000 # Use this for performance testing with large matrices
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
# Optional verbose output
msg.debug("Problem:\n%s\n", problem)
msg.debug("b_np:\n%s\n", problem.b_np[0])
msg.debug("A_np:\n%s\n", problem.A_np[0])
msg.debug("X_np:\n%s\n", problem.X_np[0])
msg.debug("y_np:\n%s\n", problem.y_np[0])
msg.debug("x_np:\n%s\n", problem.x_np[0])
msg.info("A_wp (%s):\n%s\n", problem.A_wp.shape, problem.A_wp.numpy().reshape((N, N)))
msg.info("b_wp (%s):\n%s\n", problem.b_wp.shape, problem.b_wp.numpy().reshape((N,)))
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.dims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat (%s):\n%s\n", operator.mat.shape, operator.mat.numpy().reshape((N, N)))
# Allocate LLT data arrays
L_wp = wp.zeros_like(problem.A_wp, device=self.default_device)
y_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt_sequential_factorize(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
A=problem.A_wp,
L=L_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(0, L_wp.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[0], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[0], problem.dims[0])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[0].shape, problem.A_np[0])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[0], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[0], problem.dims[0])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt_sequential_solve(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
b=problem.b_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_np (%s):\n%s\n", problem.y_np[0].shape, problem.y_np[0])
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[0].shape, problem.x_np[0])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt_sequential_solve_inplace(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
x=x_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
def test_02_single_problem_dims_partially_active(self):
"""
Test the sequential LLT solver on a single small problem.
"""
# Constants
# N_max = 16 # Use this for visual debugging with small matrices
# N_act = 11
N_max = 2000 # Use this for performance testing with large matrices
N_act = 1537
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N_act,
maxdims=N_max,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
# Optional verbose output
msg.debug("Problem:\n%s\n", problem)
msg.debug("b_np:\n%s\n", problem.b_np[0])
msg.debug("A_np:\n%s\n", problem.A_np[0])
msg.debug("X_np:\n%s\n", problem.X_np[0])
msg.debug("y_np:\n%s\n", problem.y_np[0])
msg.debug("x_np:\n%s\n", problem.x_np[0])
msg.info("A_wp (%s):\n%s\n", problem.A_wp.shape, problem.A_wp.numpy().reshape((N_max, N_max)))
msg.info("b_wp (%s):\n%s\n", problem.b_wp.shape, problem.b_wp.numpy().reshape((N_max,)))
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.maxdims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat (%s):\n%s\n", operator.mat.shape, operator.mat.numpy().reshape((N_max, N_max)))
# IMPORTANT: Now we set the active dimensions in the operator info
operator.info.dim.fill_(N_act)
# Allocate LLT data arrays
L_wp = wp.zeros_like(problem.A_wp, device=self.default_device)
y_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt_sequential_factorize(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
A=problem.A_wp,
L=L_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(0, L_wp.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[0], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[0], problem.dims[0])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[0].shape, problem.A_np[0])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[0], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[0], problem.dims[0])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt_sequential_solve(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
b=problem.b_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt_sequential_solve_inplace(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
x=x_wp,
device=self.default_device,
)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
def test_03_multiple_problems_dims_all_active(self):
"""
Test the sequential LLT solver on multiple small problems.
"""
# Constants
N = [7, 8, 9, 10, 11]
# N = [16, 64, 128, 512, 1024]
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
msg.debug("Problem:\n%s\n", problem)
# Optional verbose output
for i in range(problem.num_blocks):
A_wp_np = get_matrix_block(i, problem.A_wp.numpy(), problem.dims, problem.maxdims)
b_wp_np = get_vector_block(i, problem.b_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("[%d]: b_np:\n%s\n", i, problem.b_np[i])
msg.debug("[%d]: A_np:\n%s\n", i, problem.A_np[i])
msg.debug("[%d]: X_np:\n%s\n", i, problem.X_np[i])
msg.debug("[%d]: y_np:\n%s\n", i, problem.y_np[i])
msg.debug("[%d]: x_np:\n%s\n", i, problem.x_np[i])
msg.info("[%d]: A_wp_np (%s):\n%s\n", i, A_wp_np.shape, A_wp_np)
msg.info("[%d]: b_wp_np (%s):\n%s\n", i, b_wp_np.shape, b_wp_np)
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.dims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat shape:\n%s\n", operator.mat.shape)
# Allocate LLT data arrays
L_wp = wp.zeros_like(problem.A_wp, device=self.default_device)
y_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt_sequential_factorize(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
A=problem.A_wp,
L=L_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(i, L_wp.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
msg.info("X_np (%s):\n%s\n", problem.X_np[i].shape, problem.X_np[i])
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[i], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[i], problem.dims[i])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[i].shape, problem.A_np[i])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[i], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[i], problem.dims[i])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt_sequential_solve(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
b=problem.b_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt_sequential_solve_inplace(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
x=x_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
def test_04_multiple_problems_dims_partially_active(self):
"""
Test the sequential LLT solver on multiple small problems.
"""
# Constants
# N_max = [7, 8, 9, 14, 21] # Use this for visual debugging with small matrices
# N_act = [5, 6, 4, 11, 17]
N_max = [16, 64, 128, 512, 1024] # Use this for performance testing with large matrices
N_act = [11, 51, 101, 376, 999]
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N_act,
maxdims=N_max,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
msg.debug("Problem:\n%s\n", problem)
# Optional verbose output
for i in range(problem.num_blocks):
A_wp_np = get_matrix_block(i, problem.A_wp.numpy(), problem.dims, problem.maxdims)
b_wp_np = get_vector_block(i, problem.b_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("[%d]: b_np:\n%s\n", i, problem.b_np[i])
msg.debug("[%d]: A_np:\n%s\n", i, problem.A_np[i])
msg.debug("[%d]: X_np:\n%s\n", i, problem.X_np[i])
msg.debug("[%d]: y_np:\n%s\n", i, problem.y_np[i])
msg.debug("[%d]: x_np:\n%s\n", i, problem.x_np[i])
msg.info("[%d]: A_wp_np (%s):\n%s\n", i, A_wp_np.shape, A_wp_np)
msg.info("[%d]: b_wp_np (%s):\n%s\n", i, b_wp_np.shape, b_wp_np)
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.maxdims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat shape:\n%s\n", operator.mat.shape)
# Allocate LLT data arrays
L_wp = wp.zeros_like(problem.A_wp, device=self.default_device)
y_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# IMPORTANT: Now we set the active dimensions in the operator info
operator.info.dim.assign(N_act)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt_sequential_factorize(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
A=problem.A_wp,
L=L_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(i, L_wp.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
msg.info("X_np (%s):\n%s\n", problem.X_np[i].shape, problem.X_np[i])
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[i], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[i], problem.dims[i])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[i].shape, problem.A_np[i])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[i], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[i], problem.dims[i])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt_sequential_solve(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
b=problem.b_wp,
y=y_wp,
x=x_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt_sequential_solve_inplace(
num_blocks=problem.num_blocks,
dim=operator.info.dim,
mio=operator.info.mio,
vio=operator.info.vio,
L=L_wp,
x=x_wp,
device=self.default_device,
)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, y_wp.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# TODO: How can we get these to work?
# Ensure the AOT module is compiled for the current device
# wp.compile_aot_module(module=linear, device=wp.get_preferred_device())
# wp.load_aot_module(module=linear.factorize.llt_sequential, device=wp.get_preferred_device())
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_solve_cg.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the CGSolver class from linalg/conjugate.py"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.linalg.conjugate import BatchedLinearOperator, CGSolver, CRSolver
from newton._src.solvers.kamino._src.linalg.core import DenseLinearOperatorData, DenseSquareMultiLinearInfo
from newton._src.solvers.kamino._src.linalg.linear import ConjugateGradientSolver
from newton._src.solvers.kamino._src.linalg.sparse_matrix import (
BlockDType,
BlockSparseMatrices,
allocate_block_sparse_from_dense,
dense_to_block_sparse_copy_values,
)
from newton._src.solvers.kamino._src.linalg.utils.rand import random_spd_matrix
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import get_vector_block
from newton._src.solvers.kamino.tests.utils.print import print_error_stats
from newton._src.solvers.kamino.tests.utils.rand import RandomProblemLLT
class TestLinalgConjugate(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for verbose output
self.seed = 42
def tearDown(self):
pass
def _test_solve(self, solver_cls, problem_params, device):
problem = RandomProblemLLT(
**problem_params,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=device,
)
n_worlds = problem.num_blocks
maxdim = int(problem.maxdims[0])
b_2d = problem.b_wp.reshape((n_worlds, maxdim))
x_wp = wp.zeros_like(b_2d, device=device)
world_active = wp.full(n_worlds, 1, dtype=wp.int32, device=device)
# Create operator - use maxdim for allocation, then set actual dims
info = DenseSquareMultiLinearInfo()
info.finalize(dimensions=[maxdim] * n_worlds, dtype=float32, device=device)
info.dim = problem.dim_wp # Override with actual active dimensions
operator = DenseLinearOperatorData(info=info, mat=problem.A_wp)
A = BatchedLinearOperator.from_dense(operator)
atol = wp.full(n_worlds, 1.0e-4, dtype=problem.wp_dtype, device=device)
rtol = wp.full(n_worlds, 1.0e-5, dtype=problem.wp_dtype, device=device)
maxiter = wp.full(n_worlds, max(3 * maxdim, 50), dtype=int, device=device)
solver = solver_cls(
A=A,
world_active=world_active,
atol=atol,
rtol=rtol,
maxiter=maxiter,
Mi=None,
callback=None,
use_cuda_graph=False,
)
cur_iter, r_norm_sq, atol_sq = solver.solve(b_2d, x_wp)
x_wp_np = x_wp.numpy().reshape(-1)
if self.verbose:
pass
for block_idx, block_act in enumerate(problem.dims):
x_found = get_vector_block(block_idx, x_wp_np, problem.dims, problem.maxdims)[:block_act]
is_x_close = np.allclose(x_found, problem.x_np[block_idx][:block_act], rtol=1e-5, atol=1e-4)
if self.verbose:
print(f"Cur iter: {cur_iter}")
print(f"R norm sq {r_norm_sq}")
print(f"Atol sq: {atol_sq}")
if sum(problem.dims) < 20:
print("x:")
print(x_found)
print("x_goal:")
print(problem.x_np[block_idx])
print_error_stats("x", x_found, problem.x_np[block_idx], problem.dims[block_idx])
self.assertTrue(is_x_close)
@classmethod
def _problem_params(cls):
problems = {
"small_full": {"maxdims": 7, "dims": [4, 7]},
"small_partial": {"maxdims": 23, "dims": [14, 11]},
"large_partial": {"maxdims": 1024, "dims": [11, 51, 101, 376, 999]},
}
return problems
def test_solve_cg_cpu(self):
device = "cpu"
solver_cls = CGSolver
for problem_name, problem_params in self._problem_params().items():
with self.subTest(problem=problem_name, solver=solver_cls.__name__):
self._test_solve(solver_cls, problem_params, device)
def test_solve_cr_cpu(self):
device = "cpu"
solver_cls = CRSolver
for problem_name, problem_params in self._problem_params().items():
with self.subTest(problem=problem_name, solver=solver_cls.__name__):
self._test_solve(solver_cls, problem_params, device)
def test_solve_cg_cuda(self):
if not wp.get_cuda_devices():
self.skipTest("No CUDA devices found")
device = wp.get_cuda_device()
solver_cls = CGSolver
for problem_name, problem_params in self._problem_params().items():
with self.subTest(problem=problem_name, solver=solver_cls.__name__):
self._test_solve(solver_cls, problem_params, device)
def test_solve_cr_cuda(self):
if not wp.get_cuda_devices():
self.skipTest("No CUDA devices found")
device = wp.get_cuda_device()
solver_cls = CRSolver
for problem_name, problem_params in self._problem_params().items():
with self.subTest(problem=problem_name, solver=solver_cls.__name__):
self._test_solve(solver_cls, problem_params, device)
def _test_sparse_solve(self, solver_cls, n_worlds, dim, block_size, device):
"""Test CG/CR with sparse matrices built from random SPD matrices."""
rng = np.random.default_rng(self.seed)
# Pad to block-aligned size
n_blocks_per_dim = (dim + block_size - 1) // block_size
padded_dim = n_blocks_per_dim * block_size
total_blocks = n_blocks_per_dim * n_blocks_per_dim
# Generate random SPD matrices and RHS vectors
A_list, A_padded_list, b_list, x_ref_list = [], [], [], []
for i in range(n_worlds):
A = random_spd_matrix(dim=dim, seed=self.seed + i, dtype=np.float32)
A_padded = np.zeros((padded_dim, padded_dim), dtype=np.float32)
A_padded[:dim, :dim] = A
b = rng.standard_normal(dim).astype(np.float32)
A_list.append(A)
A_padded_list.append(A_padded)
b_list.append(b)
x_ref_list.append(np.linalg.solve(A, b))
# Block coordinates (all blocks, row-major) - same for all worlds
coords = [
(bi * block_size, bj * block_size) for bi in range(n_blocks_per_dim) for bj in range(n_blocks_per_dim)
]
all_coords = np.array(coords * n_worlds, dtype=np.int32)
# Build BlockSparseMatrices
bsm = BlockSparseMatrices()
bsm.finalize(
max_dims=[(padded_dim, padded_dim)] * n_worlds,
capacities=[total_blocks] * n_worlds,
nzb_dtype=BlockDType(float32, (block_size, block_size)),
device=device,
)
bsm.dims.assign(np.array([[padded_dim, padded_dim]] * n_worlds, dtype=np.int32))
bsm.num_nzb.assign(np.array([total_blocks] * n_worlds, dtype=np.int32))
bsm.nzb_coords.assign(all_coords)
bsm.assign(A_padded_list)
# Build dense operator for comparison
A_dense = np.array([A.flatten() for A in A_padded_list], dtype=np.float32)
A_wp = wp.array(A_dense, dtype=float32, device=device)
active_dims = wp.array([dim] * n_worlds, dtype=wp.int32, device=device)
info = DenseSquareMultiLinearInfo()
info.finalize(dimensions=[padded_dim] * n_worlds, dtype=float32, device=device)
info.dim = active_dims
dense_op = BatchedLinearOperator.from_dense(DenseLinearOperatorData(info=info, mat=A_wp))
sparse_op = BatchedLinearOperator.from_block_sparse(bsm, active_dims)
# Prepare RHS
b_2d = np.zeros((n_worlds, padded_dim), dtype=np.float32)
for m, b in enumerate(b_list):
b_2d[m, :dim] = b
b_wp = wp.array(b_2d, dtype=float32, device=device)
world_active = wp.full(n_worlds, 1, dtype=wp.int32, device=device)
atol = wp.full(n_worlds, 1.0e-6, dtype=float32, device=device)
rtol = wp.full(n_worlds, 1.0e-6, dtype=float32, device=device)
# Solve with dense operator
x_dense = wp.zeros((n_worlds, padded_dim), dtype=float32, device=device)
solver_dense = solver_cls(
A=dense_op,
world_active=world_active,
atol=atol,
rtol=rtol,
maxiter=None,
Mi=None,
callback=None,
use_cuda_graph=False,
)
solver_dense.solve(b_wp, x_dense)
# Solve with sparse operator
x_sparse = wp.zeros((n_worlds, padded_dim), dtype=float32, device=device)
solver_sparse = solver_cls(
A=sparse_op,
world_active=world_active,
atol=atol,
rtol=rtol,
maxiter=None,
Mi=None,
callback=None,
use_cuda_graph=False,
)
solver_sparse.solve(b_wp, x_sparse)
# Compare results
x_dense_np = x_dense.numpy()
x_sparse_np = x_sparse.numpy()
for m in range(n_worlds):
x_d = x_dense_np[m, :dim]
x_s = x_sparse_np[m, :dim]
x_ref = x_ref_list[m]
if self.verbose:
print(f"World {m}:")
print_error_stats("x_dense vs ref", x_d, x_ref, dim)
print_error_stats("x_sparse vs ref", x_s, x_ref, dim)
print_error_stats("x_dense vs x_sparse", x_d, x_s, dim)
self.assertTrue(np.allclose(x_d, x_ref, rtol=1e-3, atol=1e-4), "Dense solution differs from reference")
self.assertTrue(np.allclose(x_s, x_ref, rtol=1e-3, atol=1e-4), "Sparse solution differs from reference")
self.assertTrue(np.allclose(x_d, x_s, rtol=1e-5, atol=1e-6), "Dense and sparse solutions differ")
@classmethod
def _sparse_problem_params(cls):
return {
"small_4x4_blocks": {"n_worlds": 2, "dim": 16, "block_size": 4},
"medium_6x6_blocks": {"n_worlds": 3, "dim": 48, "block_size": 6},
}
def test_sparse_solve_cg_cuda(self):
if not wp.get_cuda_devices():
self.skipTest("No CUDA devices found")
device = wp.get_cuda_device()
for problem_name, params in self._sparse_problem_params().items():
with self.subTest(problem=problem_name, solver="CGSolver"):
self._test_sparse_solve(CGSolver, device=device, **params)
def test_sparse_solve_cr_cuda(self):
if not wp.get_cuda_devices():
self.skipTest("No CUDA devices found")
device = wp.get_cuda_device()
for problem_name, params in self._sparse_problem_params().items():
with self.subTest(problem=problem_name, solver="CRSolver"):
self._test_sparse_solve(CRSolver, device=device, **params)
def _build_sparse_operator(self, A: np.ndarray, block_size: int, device):
"""Helper to build a sparse operator from a dense matrix."""
dim = A.shape[0]
n_blocks = dim // block_size
total_blocks = n_blocks * n_blocks
# Set up block coordinates (all blocks, row-major order)
coords = [(bi * block_size, bj * block_size) for bi in range(n_blocks) for bj in range(n_blocks)]
bsm = BlockSparseMatrices()
bsm.finalize(
max_dims=[(dim, dim)],
capacities=[total_blocks],
nzb_dtype=BlockDType(float32, (block_size, block_size)),
device=device,
)
bsm.dims.assign(np.array([[dim, dim]], dtype=np.int32))
bsm.num_nzb.assign(np.array([total_blocks], dtype=np.int32))
bsm.nzb_coords.assign(np.array(coords, dtype=np.int32))
bsm.assign([A])
active_dims = wp.array([dim], dtype=wp.int32, device=device)
return BatchedLinearOperator.from_block_sparse(bsm, active_dims)
def test_sparse_cg_solve_simple(self):
"""Test CG solve with sparse operator on a 16x16 system with 4x4 blocks."""
if not wp.get_cuda_devices():
self.skipTest("No CUDA devices found")
device = wp.get_cuda_device()
dim, block_size = 16, 4
A = random_spd_matrix(dim=dim, seed=self.seed, dtype=np.float32)
b = np.random.default_rng(self.seed).standard_normal(dim).astype(np.float32)
x_ref = np.linalg.solve(A, b)
sparse_op = self._build_sparse_operator(A, block_size, device)
b_wp = wp.array(b.reshape(1, -1), dtype=float32, device=device)
x_wp = wp.zeros((1, dim), dtype=float32, device=device)
world_active = wp.full(1, 1, dtype=wp.int32, device=device)
atol = wp.full(1, 1e-6, dtype=float32, device=device)
rtol = wp.full(1, 1e-6, dtype=float32, device=device)
solver = CGSolver(
A=sparse_op,
world_active=world_active,
atol=atol,
rtol=rtol,
maxiter=None,
Mi=None,
use_cuda_graph=False,
)
solver.solve(b_wp, x_wp)
x_result = x_wp.numpy().flatten()
self.assertTrue(
np.allclose(x_result, x_ref, rtol=1e-3, atol=1e-4),
f"CG solve failed: {x_result} vs {x_ref}, error={np.abs(x_result - x_ref).max():.2e}",
)
def test_dense_to_block_sparse_conversion(self):
"""Test conversion from DenseLinearOperatorData to BlockSparseMatrices and back."""
if not wp.get_cuda_devices():
self.skipTest("No CUDA devices found")
device = wp.get_cuda_device()
rng = np.random.default_rng(self.seed)
n_worlds = 4
block_size = 4
dims = [12, 16, 8, 20] # Different dimensions per world
# Create block-sparse matrices in numpy (some blocks are zero)
original_matrices = []
for dim in dims:
n_blocks = (dim + block_size - 1) // block_size
matrix = np.zeros((dim, dim), dtype=np.float32)
# Fill some blocks with random values, leave others as zero
for bi in range(n_blocks):
for bj in range(n_blocks):
# ~60% chance of non-zero block
if rng.random() < 0.6:
row_start = bi * block_size
col_start = bj * block_size
row_end = min(row_start + block_size, dim)
col_end = min(col_start + block_size, dim)
block_rows = row_end - row_start
block_cols = col_end - col_start
matrix[row_start:row_end, col_start:col_end] = rng.standard_normal(
(block_rows, block_cols)
).astype(np.float32)
original_matrices.append(matrix)
# Create DenseLinearOperatorData using canonical compact storage:
# - Offsets based on maxdim^2 (each world gets maxdim^2 slots)
# - Within each world, only dim*dim elements stored with stride=dim
max_dim = max(dims)
# Allocate with maxdim^2 per world, but only store dim*dim elements compactly
A_flat = np.full(n_worlds * max_dim * max_dim, np.inf, dtype=np.float32)
for w, (dim, matrix) in enumerate(zip(dims, original_matrices, strict=False)):
offset = w * max_dim * max_dim
# Store compactly with dim as stride (canonical format)
A_flat[offset : offset + dim * dim] = matrix.flatten()
A_wp = wp.array(A_flat, dtype=float32, device=device)
info = DenseSquareMultiLinearInfo()
info.finalize(dimensions=[max_dim] * n_worlds, dtype=float32, device=device)
info.dim = wp.array(dims, dtype=wp.int32, device=device)
dense_op = DenseLinearOperatorData(info=info, mat=A_wp)
# Allocate BSM with threshold (allow for all blocks)
bsm = allocate_block_sparse_from_dense(
dense_op=dense_op,
block_size=block_size,
sparsity_threshold=1.0,
device=device,
)
# Convert dense to block sparse
dense_to_block_sparse_copy_values(
dense_op=dense_op,
bsm=bsm,
block_size=block_size,
)
wp.synchronize()
# Convert back to numpy and compare
recovered_matrices = bsm.numpy()
for w, (orig, recovered) in enumerate(zip(original_matrices, recovered_matrices, strict=False)):
dim = dims[w]
orig_trimmed = orig[:dim, :dim].astype(np.float32)
recovered_trimmed = recovered[:dim, :dim].astype(np.float32)
if self.verbose:
print(f"World {w} (dim={dim}):")
print(f" Original non-zeros: {np.count_nonzero(orig_trimmed)}")
print(f" Recovered non-zeros: {np.count_nonzero(recovered_trimmed)}")
max_diff = np.abs(orig_trimmed - recovered_trimmed).max()
print(f" Max abs diff: {max_diff:.2e}")
self.assertTrue(
np.allclose(orig_trimmed, recovered_trimmed, rtol=1e-5, atol=1e-6),
f"World {w}: matrices don't match, max diff={np.abs(orig_trimmed - recovered_trimmed).max():.2e}",
)
def test_cg_solver_discover_sparse(self):
"""Test ConjugateGradientSolver with discover_sparse=True."""
if not wp.get_cuda_devices():
self.skipTest("No CUDA devices found")
device = wp.get_cuda_device()
rng = np.random.default_rng(self.seed)
n_worlds = 3
block_size = 6
maxdim = 24 # Multiple of block_size for clean blocks
# Generate SPD matrices and RHS
A_list, b_list, x_ref_list = [], [], []
for i in range(n_worlds):
A = random_spd_matrix(dim=maxdim, seed=self.seed + i, dtype=np.float32)
b = rng.standard_normal(maxdim).astype(np.float32)
A_list.append(A)
b_list.append(b)
x_ref_list.append(np.linalg.solve(A, b))
# Create dense storage (compact format: dim*dim per world, with maxdim^2 spacing)
A_flat = np.zeros(n_worlds * maxdim * maxdim, dtype=np.float32)
for w, A in enumerate(A_list):
offset = w * maxdim * maxdim
A_flat[offset : offset + maxdim * maxdim] = A.flatten()
A_wp = wp.array(A_flat, dtype=float32, device=device)
# Create DenseLinearOperatorData
info = DenseSquareMultiLinearInfo()
info.finalize(dimensions=[maxdim] * n_worlds, dtype=float32, device=device)
dense_op = DenseLinearOperatorData(info=info, mat=A_wp)
# Create b and x arrays
b_2d = np.array(b_list, dtype=np.float32)
b_wp = wp.array(b_2d.flatten(), dtype=float32, device=device)
x_wp = wp.zeros(n_worlds * maxdim, dtype=float32, device=device)
# Solve with discover_sparse=True
solver = ConjugateGradientSolver(
discover_sparse=True, sparse_block_size=block_size, sparse_threshold=1.0, device=device
)
solver.finalize(dense_op)
solver.compute(A_wp)
solver.solve(b_wp, x_wp)
# Check results
x_np = x_wp.numpy().reshape(n_worlds, maxdim)
for w in range(n_worlds):
x_found = x_np[w]
x_ref = x_ref_list[w]
if self.verbose:
print(f"World {w}: max error = {np.abs(x_found - x_ref).max():.2e}")
self.assertTrue(
np.allclose(x_found, x_ref, rtol=1e-3, atol=1e-4),
f"World {w}: solve failed, max error={np.abs(x_found - x_ref).max():.2e}",
)
# Also solve with discover_sparse=False and compare
x_dense_wp = wp.zeros(n_worlds * maxdim, dtype=float32, device=device)
solver_dense = ConjugateGradientSolver(discover_sparse=False, device=device)
solver_dense.finalize(dense_op)
solver_dense.compute(A_wp)
solver_dense.solve(b_wp, x_dense_wp)
x_sparse = x_wp.numpy()
x_dense = x_dense_wp.numpy()
if self.verbose:
print(f"Sparse vs dense max diff: {np.abs(x_sparse - x_dense).max():.2e}")
self.assertTrue(
np.allclose(x_sparse, x_dense, rtol=1e-5, atol=1e-6),
f"Sparse and dense solutions differ: max diff={np.abs(x_sparse - x_dense).max():.2e}",
)
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_solver_llt_blocked.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the LLTBlockedSolver from linalg/linear.py"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.linalg.core import DenseLinearOperatorData, DenseSquareMultiLinearInfo
from newton._src.solvers.kamino._src.linalg.linear import LLTBlockedSolver
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import get_matrix_block, get_vector_block
from newton._src.solvers.kamino.tests.utils.print import print_error_stats
from newton._src.solvers.kamino.tests.utils.rand import RandomProblemLLT
###
# Tests
###
class TestLinAlgLLTBlockedSolver(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default_solver(self):
"""
Test the default constructor of the LLTBlockedSolver class.
"""
llt = LLTBlockedSolver(device=self.default_device)
self.assertIsNone(llt._operator)
self.assertEqual(llt.dtype, float32)
self.assertEqual(llt.device, self.default_device)
def test_01_single_problem_dims_all_active(self):
"""
Test the blocked LLT solver on a single small problem.
"""
# Constants
# N = 12 # Use this for visual debugging with small matrices
N = 2000 # Use this for performance testing with large matrices
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
# Optional verbose output
msg.debug("Problem:\n%s\n", problem)
msg.debug("b_np:\n%s\n", problem.b_np[0])
msg.debug("A_np:\n%s\n", problem.A_np[0])
msg.debug("X_np:\n%s\n", problem.X_np[0])
msg.debug("y_np:\n%s\n", problem.y_np[0])
msg.debug("x_np:\n%s\n", problem.x_np[0])
msg.info("A_wp (%s):\n%s\n", problem.A_wp.shape, problem.A_wp.numpy().reshape((N, N)))
msg.info("b_wp (%s):\n%s\n", problem.b_wp.shape, problem.b_wp.numpy().reshape((N,)))
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.dims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat (%s):\n%s\n", operator.mat.shape, operator.mat.numpy().reshape((N, N)))
# Create a SequentialCholeskyFactorizer instance
llt = LLTBlockedSolver(operator=operator, device=self.default_device)
self.assertIsNotNone(llt._operator)
self.assertEqual(llt.dtype, problem.wp_dtype)
self.assertEqual(llt.device, self.default_device)
self.assertIsNotNone(llt._L)
self.assertIsNotNone(llt._y)
self.assertEqual(llt.L.size, problem.A_wp.size)
self.assertEqual(llt.y.size, problem.b_wp.size)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt.compute(A=problem.A_wp)
msg.info("llt.L (%s):\n%s\n", llt.L.shape, llt.L.numpy().reshape((N, N)))
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(0, llt.L.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[0], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[0], problem.dims[0])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[0].shape, problem.A_np[0])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[0], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[0], problem.dims[0])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt.solve(b=problem.b_wp, x=x_wp)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_np (%s):\n%s\n", problem.y_np[0].shape, problem.y_np[0])
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[0].shape, problem.x_np[0])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt.solve_inplace(x=x_wp)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
def test_02_single_problem_dims_partially_active(self):
"""
Test the blocked LLT solver on a single small problem.
"""
# Constants
# N_max = 16 # Use this for visual debugging with small matrices
# N_act = 11
N_max = 2000 # Use this for performance testing with large matrices
N_act = 1537
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N_act,
maxdims=N_max,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
# Optional verbose output
msg.debug("Problem:\n%s\n", problem)
msg.debug("b_np:\n%s\n", problem.b_np[0])
msg.debug("A_np:\n%s\n", problem.A_np[0])
msg.debug("X_np:\n%s\n", problem.X_np[0])
msg.debug("y_np:\n%s\n", problem.y_np[0])
msg.debug("x_np:\n%s\n", problem.x_np[0])
msg.info("A_wp (%s):\n%s\n", problem.A_wp.shape, problem.A_wp.numpy().reshape((N_max, N_max)))
msg.info("b_wp (%s):\n%s\n", problem.b_wp.shape, problem.b_wp.numpy().reshape((N_max,)))
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.maxdims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat (%s):\n%s\n", operator.mat.shape, operator.mat.numpy().reshape((N_max, N_max)))
# Create a SequentialCholeskyFactorizer instance
llt = LLTBlockedSolver(operator=operator, device=self.default_device)
self.assertIsNotNone(llt._operator)
self.assertEqual(llt.dtype, problem.wp_dtype)
self.assertEqual(llt.device, self.default_device)
self.assertIsNotNone(llt._L)
self.assertIsNotNone(llt._y)
self.assertEqual(llt.L.size, problem.A_wp.size)
self.assertEqual(llt.y.size, problem.b_wp.size)
# IMPORTANT: Now we set the active dimensions in the operator info
operator.info.dim.fill_(N_act)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt.compute(A=problem.A_wp)
msg.info("llt.L (%s):\n%s\n", llt.L.shape, llt.L.numpy().reshape((N_max, N_max)))
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(0, llt.L.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[0], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[0], problem.dims[0])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[0].shape, problem.A_np[0])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[0], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[0], problem.dims[0])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt.solve(b=problem.b_wp, x=x_wp)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt.solve_inplace(x=x_wp)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
def test_03_multiple_problems_dims_all_active(self):
"""
Test the blocked LLT solver on multiple small problems.
"""
# Constants
N = [7, 8, 9, 10, 11]
# N = [16, 64, 128, 512, 1024]
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
msg.debug("Problem:\n%s\n", problem)
# Optional verbose output
for i in range(problem.num_blocks):
A_wp_np = get_matrix_block(i, problem.A_wp.numpy(), problem.dims, problem.maxdims)
b_wp_np = get_vector_block(i, problem.b_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("[%d]: b_np:\n%s\n", i, problem.b_np[i])
msg.debug("[%d]: A_np:\n%s\n", i, problem.A_np[i])
msg.debug("[%d]: X_np:\n%s\n", i, problem.X_np[i])
msg.debug("[%d]: y_np:\n%s\n", i, problem.y_np[i])
msg.debug("[%d]: x_np:\n%s\n", i, problem.x_np[i])
msg.info("[%d]: A_wp_np (%s):\n%s\n", i, A_wp_np.shape, A_wp_np)
msg.info("[%d]: b_wp_np (%s):\n%s\n", i, b_wp_np.shape, b_wp_np)
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.dims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat shape:\n%s\n", operator.mat.shape)
# Create a SequentialCholeskyFactorizer instance
llt = LLTBlockedSolver(operator=operator, device=self.default_device)
self.assertIsNotNone(llt._operator)
self.assertEqual(llt.dtype, problem.wp_dtype)
self.assertEqual(llt.device, self.default_device)
self.assertIsNotNone(llt._L)
self.assertIsNotNone(llt._y)
self.assertEqual(llt.L.size, problem.A_wp.size)
self.assertEqual(llt.y.size, problem.b_wp.size)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt.compute(A=problem.A_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(i, llt.L.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
msg.info("X_np (%s):\n%s\n", problem.X_np[i].shape, problem.X_np[i])
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[i], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[i], problem.dims[i])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[i].shape, problem.A_np[i])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[i], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[i], problem.dims[i])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt.solve(b=problem.b_wp, x=x_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt.solve_inplace(x=x_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
def test_04_multiple_problems_dims_partially_active(self):
"""
Test the blocked LLT solver on multiple small problems.
"""
# Constants
# N_max = [7, 8, 9, 14, 21] # Use this for visual debugging with small matrices
# N_act = [5, 6, 4, 11, 17]
N_max = [16, 64, 128, 512, 1024] # Use this for performance testing with large matrices
N_act = [11, 51, 101, 376, 999]
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N_act,
maxdims=N_max,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
msg.debug("Problem:\n%s\n", problem)
# Optional verbose output
for i in range(problem.num_blocks):
A_wp_np = get_matrix_block(i, problem.A_wp.numpy(), problem.dims, problem.maxdims)
b_wp_np = get_vector_block(i, problem.b_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("[%d]: b_np:\n%s\n", i, problem.b_np[i])
msg.debug("[%d]: A_np:\n%s\n", i, problem.A_np[i])
msg.debug("[%d]: X_np:\n%s\n", i, problem.X_np[i])
msg.debug("[%d]: y_np:\n%s\n", i, problem.y_np[i])
msg.debug("[%d]: x_np:\n%s\n", i, problem.x_np[i])
msg.info("[%d]: A_wp_np (%s):\n%s\n", i, A_wp_np.shape, A_wp_np)
msg.info("[%d]: b_wp_np (%s):\n%s\n", i, b_wp_np.shape, b_wp_np)
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.maxdims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat shape:\n%s\n", operator.mat.shape)
# Create a SequentialCholeskyFactorizer instance
llt = LLTBlockedSolver(operator=operator, device=self.default_device)
self.assertIsNotNone(llt._operator)
self.assertEqual(llt.dtype, problem.wp_dtype)
self.assertEqual(llt.device, self.default_device)
self.assertIsNotNone(llt._L)
self.assertIsNotNone(llt._y)
self.assertEqual(llt.L.size, problem.A_wp.size)
self.assertEqual(llt.y.size, problem.b_wp.size)
# IMPORTANT: Now we set the active dimensions in the operator info
operator.info.dim.assign(N_act)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt.compute(A=problem.A_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(i, llt.L.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
msg.info("X_np (%s):\n%s\n", problem.X_np[i].shape, problem.X_np[i])
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[i], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[i], problem.dims[i])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[i].shape, problem.A_np[i])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[i], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[i], problem.dims[i])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt.solve(b=problem.b_wp, x=x_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt.solve_inplace(x=x_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# TODO: How can we get these to work?
# Ensure the AOT module is compiled for the current device
# wp.compile_aot_module(module=linear, device=wp.get_preferred_device())
# wp.load_aot_module(module=linear.factorize.llt_sequential, device=wp.get_preferred_device())
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_solver_llt_sequential.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the LLTSequentialSolver from linalg/linear.py"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.linalg.core import DenseLinearOperatorData, DenseSquareMultiLinearInfo
from newton._src.solvers.kamino._src.linalg.linear import LLTSequentialSolver
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import get_matrix_block, get_vector_block
from newton._src.solvers.kamino.tests.utils.print import print_error_stats
from newton._src.solvers.kamino.tests.utils.rand import RandomProblemLLT
###
# Tests
###
class TestLinAlgLLTSequentialSolver(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default_solver(self):
"""
Test the default constructor of the LLTSequentialSolver class.
"""
llt = LLTSequentialSolver(device=self.default_device)
self.assertIsNone(llt._operator)
self.assertEqual(llt.dtype, float32)
self.assertEqual(llt.device, self.default_device)
def test_01_single_problem_dims_all_active(self):
"""
Test the sequential LLT solver on a single small problem.
"""
# Constants
# N = 12 # Use this for visual debugging with small matrices
N = 2000 # Use this for performance testing with large matrices
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
# Optional verbose output
msg.debug("Problem:\n%s\n", problem)
msg.debug("b_np:\n%s\n", problem.b_np[0])
msg.debug("A_np:\n%s\n", problem.A_np[0])
msg.debug("X_np:\n%s\n", problem.X_np[0])
msg.debug("y_np:\n%s\n", problem.y_np[0])
msg.debug("x_np:\n%s\n", problem.x_np[0])
msg.info("A_wp (%s):\n%s\n", problem.A_wp.shape, problem.A_wp.numpy().reshape((N, N)))
msg.info("b_wp (%s):\n%s\n", problem.b_wp.shape, problem.b_wp.numpy().reshape((N,)))
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.dims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat (%s):\n%s\n", operator.mat.shape, operator.mat.numpy().reshape((N, N)))
# Create a SequentialCholeskyFactorizer instance
llt = LLTSequentialSolver(operator=operator, device=self.default_device)
self.assertIsNotNone(llt._operator)
self.assertEqual(llt.dtype, problem.wp_dtype)
self.assertEqual(llt.device, self.default_device)
self.assertIsNotNone(llt._L)
self.assertIsNotNone(llt._y)
self.assertEqual(llt.L.size, problem.A_wp.size)
self.assertEqual(llt.y.size, problem.b_wp.size)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt.compute(A=problem.A_wp)
msg.info("llt.L (%s):\n%s\n", llt.L.shape, llt.L.numpy().reshape((N, N)))
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(0, llt.L.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[0], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[0], problem.dims[0])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[0].shape, problem.A_np[0])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[0], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[0], problem.dims[0])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt.solve(b=problem.b_wp, x=x_wp)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_np (%s):\n%s\n", problem.y_np[0].shape, problem.y_np[0])
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[0].shape, problem.x_np[0])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt.solve_inplace(x=x_wp)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
def test_02_single_problem_dims_partially_active(self):
"""
Test the sequential LLT solver on a single small problem.
"""
# Constants
# N_max = 16 # Use this for visual debugging with small matrices
# N_act = 11
N_max = 2000 # Use this for performance testing with large matrices
N_act = 1537
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N_act,
maxdims=N_max,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
# Optional verbose output
msg.debug("Problem:\n%s\n", problem)
msg.debug("b_np:\n%s\n", problem.b_np[0])
msg.debug("A_np:\n%s\n", problem.A_np[0])
msg.debug("X_np:\n%s\n", problem.X_np[0])
msg.debug("y_np:\n%s\n", problem.y_np[0])
msg.debug("x_np:\n%s\n", problem.x_np[0])
msg.info("A_wp (%s):\n%s\n", problem.A_wp.shape, problem.A_wp.numpy().reshape((N_max, N_max)))
msg.info("b_wp (%s):\n%s\n", problem.b_wp.shape, problem.b_wp.numpy().reshape((N_max,)))
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.maxdims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat (%s):\n%s\n", operator.mat.shape, operator.mat.numpy().reshape((N_max, N_max)))
# Create a SequentialCholeskyFactorizer instance
llt = LLTSequentialSolver(operator=operator, device=self.default_device)
self.assertIsNotNone(llt._operator)
self.assertEqual(llt.dtype, problem.wp_dtype)
self.assertEqual(llt.device, self.default_device)
self.assertIsNotNone(llt._L)
self.assertIsNotNone(llt._y)
self.assertEqual(llt.L.size, problem.A_wp.size)
self.assertEqual(llt.y.size, problem.b_wp.size)
# IMPORTANT: Now we set the active dimensions in the operator info
operator.info.dim.fill_(N_act)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt.compute(A=problem.A_wp)
msg.info("llt.L (%s):\n%s\n", llt.L.shape, llt.L.numpy().reshape((N_max, N_max)))
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(0, llt.L.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[0], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[0], problem.dims[0])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[0].shape, problem.A_np[0])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[0], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[0], problem.dims[0])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt.solve(b=problem.b_wp, x=x_wp)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt.solve_inplace(x=x_wp)
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(0, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(0, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[0], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[0], problem.dims[0])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[0], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[0], problem.dims[0])
self.assertTrue(is_x_close)
def test_03_multiple_problems_dims_all_active(self):
"""
Test the sequential LLT solver on multiple small problems.
"""
# Constants
N = [7, 8, 9, 10, 11]
# N = [16, 64, 128, 512, 1024]
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
msg.debug("Problem:\n%s\n", problem)
# Optional verbose output
for i in range(problem.num_blocks):
A_wp_np = get_matrix_block(i, problem.A_wp.numpy(), problem.dims, problem.maxdims)
b_wp_np = get_vector_block(i, problem.b_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("[%d]: b_np:\n%s\n", i, problem.b_np[i])
msg.debug("[%d]: A_np:\n%s\n", i, problem.A_np[i])
msg.debug("[%d]: X_np:\n%s\n", i, problem.X_np[i])
msg.debug("[%d]: y_np:\n%s\n", i, problem.y_np[i])
msg.debug("[%d]: x_np:\n%s\n", i, problem.x_np[i])
msg.info("[%d]: A_wp_np (%s):\n%s\n", i, A_wp_np.shape, A_wp_np)
msg.info("[%d]: b_wp_np (%s):\n%s\n", i, b_wp_np.shape, b_wp_np)
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.dims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat shape:\n%s\n", operator.mat.shape)
# Create a SequentialCholeskyFactorizer instance
llt = LLTSequentialSolver(operator=operator, device=self.default_device)
self.assertIsNotNone(llt._operator)
self.assertEqual(llt.dtype, problem.wp_dtype)
self.assertEqual(llt.device, self.default_device)
self.assertIsNotNone(llt._L)
self.assertIsNotNone(llt._y)
self.assertEqual(llt.L.size, problem.A_wp.size)
self.assertEqual(llt.y.size, problem.b_wp.size)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt.compute(A=problem.A_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(i, llt.L.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
msg.info("X_np (%s):\n%s\n", problem.X_np[i].shape, problem.X_np[i])
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[i], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[i], problem.dims[i])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[i].shape, problem.A_np[i])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[i], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[i], problem.dims[i])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt.solve(b=problem.b_wp, x=x_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt.solve_inplace(x=x_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
def test_04_multiple_problems_dims_partially_active(self):
"""
Test the sequential LLT solver on multiple small problems.
"""
# Constants
# N_max = [7, 8, 9, 14, 21] # Use this for visual debugging with small matrices
# N_act = [5, 6, 4, 11, 17]
N_max = [16, 64, 128, 512, 1024] # Use this for performance testing with large matrices
N_act = [11, 51, 101, 376, 999]
# Create a single-instance problem
problem = RandomProblemLLT(
dims=N_act,
maxdims=N_max,
seed=self.seed,
np_dtype=np.float32,
wp_dtype=float32,
device=self.default_device,
)
msg.debug("Problem:\n%s\n", problem)
# Optional verbose output
for i in range(problem.num_blocks):
A_wp_np = get_matrix_block(i, problem.A_wp.numpy(), problem.dims, problem.maxdims)
b_wp_np = get_vector_block(i, problem.b_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("[%d]: b_np:\n%s\n", i, problem.b_np[i])
msg.debug("[%d]: A_np:\n%s\n", i, problem.A_np[i])
msg.debug("[%d]: X_np:\n%s\n", i, problem.X_np[i])
msg.debug("[%d]: y_np:\n%s\n", i, problem.y_np[i])
msg.debug("[%d]: x_np:\n%s\n", i, problem.x_np[i])
msg.info("[%d]: A_wp_np (%s):\n%s\n", i, A_wp_np.shape, A_wp_np)
msg.info("[%d]: b_wp_np (%s):\n%s\n", i, b_wp_np.shape, b_wp_np)
# Create the linear operator meta-data
opinfo = DenseSquareMultiLinearInfo()
opinfo.finalize(dimensions=problem.maxdims, dtype=problem.wp_dtype, device=self.default_device)
msg.debug("opinfo:\n%s", opinfo)
# Create the linear operator data structure
operator = DenseLinearOperatorData(info=opinfo, mat=problem.A_wp)
msg.debug("operator.info:\n%s\n", operator.info)
msg.debug("operator.mat shape:\n%s\n", operator.mat.shape)
# Create a SequentialCholeskyFactorizer instance
llt = LLTSequentialSolver(operator=operator, device=self.default_device)
self.assertIsNotNone(llt._operator)
self.assertEqual(llt.dtype, problem.wp_dtype)
self.assertEqual(llt.device, self.default_device)
self.assertIsNotNone(llt._L)
self.assertIsNotNone(llt._y)
self.assertEqual(llt.L.size, problem.A_wp.size)
self.assertEqual(llt.y.size, problem.b_wp.size)
# IMPORTANT: Now we set the active dimensions in the operator info
operator.info.dim.assign(N_act)
###
# Matrix factorization
###
# Factorize the target square-symmetric matrix
llt.compute(A=problem.A_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
L_wp_np = get_matrix_block(i, llt.L.numpy(), problem.dims, problem.maxdims)
msg.info("L_wp_np (%s):\n%s\n", L_wp_np.shape, L_wp_np)
msg.info("X_np (%s):\n%s\n", problem.X_np[i].shape, problem.X_np[i])
# Check matrix factorization against numpy
is_L_close = np.allclose(L_wp_np, problem.X_np[i], rtol=1e-3, atol=1e-4)
if not is_L_close or self.verbose:
print_error_stats("L", L_wp_np, problem.X_np[i], problem.dims[i])
self.assertTrue(is_L_close)
# Reconstruct the original matrix A from the factorization
A_wp_np = L_wp_np @ L_wp_np.T
msg.info("A_np (%s):\n%s\n", problem.A_np[i].shape, problem.A_np[i])
msg.info("A_wp_np (%s):\n%s\n", A_wp_np.shape, A_wp_np)
# Check matrix reconstruction against original matrix
is_A_close = np.allclose(A_wp_np, problem.A_np[i], rtol=1e-3, atol=1e-4)
if not is_A_close or self.verbose:
print_error_stats("A", A_wp_np, problem.A_np[i], problem.dims[i])
self.assertTrue(is_A_close)
###
# Linear system solve
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
# Solve the linear system using the factorization
llt.solve(b=problem.b_wp, x=x_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Linear system solve in-place
###
# Prepare the solution vector x
x_wp = wp.zeros_like(problem.b_wp, device=self.default_device)
wp.copy(x_wp, problem.b_wp)
# Solve the linear system using the factorization
llt.solve_inplace(x=x_wp)
# Iterate over all problems for verification
for i in range(problem.num_blocks):
# Convert the warp array to numpy for verification
y_wp_np = get_vector_block(i, llt.y.numpy(), problem.dims, problem.maxdims)
x_wp_np = get_vector_block(i, x_wp.numpy(), problem.dims, problem.maxdims)
msg.debug("y_wp_np (%s):\n%s\n", y_wp_np.shape, y_wp_np)
msg.debug("y_np (%s):\n%s\n", problem.y_np[i].shape, problem.y_np[i])
msg.debug("x_wp_np (%s):\n%s\n", x_wp_np.shape, x_wp_np)
msg.debug("x_np (%s):\n%s\n", problem.x_np[i].shape, problem.x_np[i])
# Assert the result is as expected
is_y_close = np.allclose(y_wp_np, problem.y_np[i], rtol=1e-3, atol=1e-4)
if not is_y_close or self.verbose:
print_error_stats("y", y_wp_np, problem.y_np[i], problem.dims[i])
self.assertTrue(is_y_close)
# Assert the result is as expected
is_x_close = np.allclose(x_wp_np, problem.x_np[i], rtol=1e-3, atol=1e-4)
if not is_x_close or self.verbose:
print_error_stats("x", x_wp_np, problem.x_np[i], problem.dims[i])
self.assertTrue(is_x_close)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# TODO: How can we get these to work?
# Ensure the AOT module is compiled for the current device
# wp.compile_aot_module(module=linear, device=wp.get_preferred_device())
# wp.load_aot_module(module=linear.factorize.llt_sequential, device=wp.get_preferred_device())
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_sparse.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the base classes in linalg/sparse.py"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.linalg.sparse_matrix import BlockDType, BlockSparseMatrices
from newton._src.solvers.kamino._src.linalg.sparse_operator import BlockSparseLinearOperators
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.sparse import sparseplot
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestBlockDType(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
###
# Construction Tests
###
def test_00_make_block_dtype(self):
# Default construction should fail
self.assertRaises(TypeError, BlockDType)
# Scalar block type, shape should be `()` to match numpy scalar behavior
scalar_block_type_0 = BlockDType(dtype=wp.float32)
self.assertEqual(scalar_block_type_0.dtype, wp.float32)
self.assertEqual(scalar_block_type_0.shape, ())
scalar_block_type_1 = BlockDType(shape=1, dtype=wp.float32)
self.assertEqual(scalar_block_type_1.dtype, wp.float32)
self.assertEqual(scalar_block_type_1.shape, ())
# Vector block types
vector_block_type_0 = BlockDType(shape=2, dtype=wp.float32)
self.assertEqual(vector_block_type_0.dtype, wp.float32)
self.assertEqual(vector_block_type_0.shape, (2,))
vector_block_type_1 = BlockDType(shape=(3,), dtype=wp.float32)
self.assertEqual(vector_block_type_1.dtype, wp.float32)
self.assertEqual(vector_block_type_1.shape, (3,))
# Matrix block types
matrix_block_type_0 = BlockDType(shape=(2, 4), dtype=wp.float32)
self.assertEqual(matrix_block_type_0.dtype, wp.float32)
self.assertEqual(matrix_block_type_0.shape, (2, 4))
# Invalid shape specifications should fail
self.assertRaises(ValueError, BlockDType, shape=0, dtype=wp.float32)
self.assertRaises(ValueError, BlockDType, shape=(-2,), dtype=wp.float32)
self.assertRaises(ValueError, BlockDType, shape=(3, -4), dtype=wp.float32)
self.assertRaises(ValueError, BlockDType, shape=(1, 2, 3), dtype=wp.float32)
# Invalid dtype specifications should fail
self.assertRaises(TypeError, BlockDType, shape=2, dtype=None)
self.assertRaises(TypeError, BlockDType, shape=(2, 2), dtype=str)
def test_01_block_dtype_size(self):
# Scalar block type
scalar_block_type = BlockDType(dtype=wp.float32)
self.assertEqual(scalar_block_type.size, 1)
# Vector block type
vector_block_type = BlockDType(shape=4, dtype=wp.float32)
self.assertEqual(vector_block_type.size, 4)
# Matrix block type
matrix_block_type = BlockDType(shape=(3, 5), dtype=wp.float32)
self.assertEqual(matrix_block_type.size, 15)
def test_02_block_dtype_warp_type(self):
# Scalar block type
scalar_block_type = BlockDType(dtype=wp.float32)
warp_scalar_type = scalar_block_type.warp_type
self.assertEqual(warp_scalar_type, wp.float32)
# Vector block type
vector_block_type = BlockDType(shape=4, dtype=wp.float32)
warp_vector_type = vector_block_type.warp_type
self.assertEqual(warp_vector_type._length_, 4)
self.assertEqual(warp_vector_type._wp_scalar_type_, wp.float32)
# Matrix block type
matrix_block_type = BlockDType(shape=(3, 5), dtype=wp.float32)
warp_matrix_type = matrix_block_type.warp_type
self.assertEqual(warp_matrix_type._shape_, (3, 5))
self.assertEqual(warp_matrix_type._length_, 15)
self.assertEqual(warp_matrix_type._wp_scalar_type_, wp.float32)
class TestBlockSparseMatrices(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
self.plot = test_context.verbose # Set to True to plot sparse matrices
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
###
# Construction Tests
###
def test_00_make_default(self):
bsm = BlockSparseMatrices()
self.assertIsInstance(bsm, BlockSparseMatrices)
# Host-side meta-data should be default-initialized
self.assertIsNone(bsm.device)
self.assertEqual(bsm.num_matrices, 0)
self.assertEqual(bsm.sum_of_num_nzb, 0)
self.assertEqual(bsm.max_of_num_nzb, 0)
self.assertEqual(bsm.max_of_max_dims, (0, 0))
self.assertIsNone(bsm.nzb_dtype)
self.assertIs(bsm.index_dtype, wp.int32)
# On-device data should be None
self.assertIsNone(bsm.max_dims)
self.assertIsNone(bsm.dims)
self.assertIsNone(bsm.row_start)
self.assertIsNone(bsm.col_start)
self.assertIsNone(bsm.max_nzb)
self.assertIsNone(bsm.num_nzb)
self.assertIsNone(bsm.nzb_start)
self.assertIsNone(bsm.nzb_coords)
self.assertIsNone(bsm.nzb_values)
# Finalization should fail since the block size `nzb_size` is not set
self.assertRaises(RuntimeError, bsm.finalize, max_dims=[(0, 0)], capacities=[0])
def test_01_make_single_scalar_block_sparse_matrix(self):
bsm = BlockSparseMatrices(num_matrices=1, nzb_dtype=BlockDType(dtype=wp.float32), device=self.default_device)
bsm.finalize(max_dims=[(1, 1)], capacities=[1])
# Check meta-data
self.assertEqual(bsm.num_matrices, 1)
self.assertEqual(bsm.sum_of_num_nzb, 1)
self.assertEqual(bsm.max_of_num_nzb, 1)
self.assertEqual(bsm.max_of_max_dims, (1, 1))
self.assertEqual(bsm.nzb_dtype.dtype, wp.float32)
self.assertEqual(bsm.nzb_dtype.shape, ())
self.assertIs(bsm.index_dtype, wp.int32)
self.assertEqual(bsm.device, self.default_device)
# Check on-device data shapes
self.assertEqual(bsm.max_dims.shape, (1, 2))
self.assertEqual(bsm.dims.shape, (1, 2))
self.assertEqual(bsm.row_start.shape, (1,))
self.assertEqual(bsm.col_start.shape, (1,))
self.assertEqual(bsm.max_nzb.shape, (1,))
self.assertEqual(bsm.num_nzb.shape, (1,))
self.assertEqual(bsm.nzb_start.shape, (1,))
self.assertEqual(bsm.nzb_coords.shape, (1, 2))
self.assertEqual(bsm.nzb_values.shape, (1,))
self.assertEqual(bsm.nzb_values.size, 1)
self.assertEqual(bsm.nzb_values.view(dtype=wp.float32).size, 1)
def test_02_make_single_vector_block_sparse_matrix(self):
bsm = BlockSparseMatrices(num_matrices=1, nzb_dtype=BlockDType(shape=(6,), dtype=wp.float32))
bsm.finalize(max_dims=[(6, 1)], capacities=[1], device=self.default_device)
# Check meta-data
self.assertEqual(bsm.num_matrices, 1)
self.assertEqual(bsm.sum_of_num_nzb, 1)
self.assertEqual(bsm.max_of_num_nzb, 1)
self.assertEqual(bsm.max_of_max_dims, (6, 1))
self.assertEqual(bsm.nzb_dtype.dtype, wp.float32)
self.assertEqual(bsm.nzb_dtype.shape, (6,))
self.assertIs(bsm.index_dtype, wp.int32)
self.assertEqual(bsm.device, self.default_device)
# Check on-device data shapes
self.assertEqual(bsm.max_dims.shape, (1, 2))
self.assertEqual(bsm.dims.shape, (1, 2))
self.assertEqual(bsm.row_start.shape, (1,))
self.assertEqual(bsm.col_start.shape, (1,))
self.assertEqual(bsm.max_nzb.shape, (1,))
self.assertEqual(bsm.num_nzb.shape, (1,))
self.assertEqual(bsm.nzb_start.shape, (1,))
self.assertEqual(bsm.nzb_coords.shape, (1, 2))
self.assertEqual(bsm.nzb_values.shape, (1,))
self.assertEqual(bsm.nzb_values.size, 1)
self.assertEqual(bsm.nzb_values.view(dtype=wp.float32).size, 6)
def test_03_make_single_matrix_block_sparse_matrix(self):
bsm = BlockSparseMatrices(num_matrices=1, nzb_dtype=BlockDType(shape=(6, 5), dtype=wp.float32))
bsm.finalize(max_dims=[(6, 5)], capacities=[1], device=self.default_device)
# Check meta-data
self.assertEqual(bsm.num_matrices, 1)
self.assertEqual(bsm.sum_of_num_nzb, 1)
self.assertEqual(bsm.max_of_num_nzb, 1)
self.assertEqual(bsm.max_of_max_dims, (6, 5))
self.assertEqual(bsm.nzb_dtype.dtype, wp.float32)
self.assertEqual(bsm.nzb_dtype.shape, (6, 5))
self.assertIs(bsm.index_dtype, wp.int32)
self.assertEqual(bsm.device, self.default_device)
# Check on-device data shapes
self.assertEqual(bsm.max_dims.shape, (1, 2))
self.assertEqual(bsm.dims.shape, (1, 2))
self.assertEqual(bsm.row_start.shape, (1,))
self.assertEqual(bsm.col_start.shape, (1,))
self.assertEqual(bsm.max_nzb.shape, (1,))
self.assertEqual(bsm.num_nzb.shape, (1,))
self.assertEqual(bsm.nzb_start.shape, (1,))
self.assertEqual(bsm.nzb_coords.shape, (1, 2))
self.assertEqual(bsm.nzb_values.shape, (1,))
self.assertEqual(bsm.nzb_values.size, 1)
self.assertEqual(bsm.nzb_values.view(dtype=wp.float32).size, 30)
def test_04_build_multiple_vector_block_matrices(self):
bsm = BlockSparseMatrices(num_matrices=1, nzb_dtype=BlockDType(shape=(6,), dtype=wp.float32))
bsm.finalize(max_dims=[(6, 1), (12, 2), (6, 4)], capacities=[3, 4, 5], device=self.default_device)
# Check meta-data
self.assertEqual(bsm.num_matrices, 3)
self.assertEqual(bsm.sum_of_num_nzb, 12)
self.assertEqual(bsm.max_of_num_nzb, 5)
self.assertEqual(bsm.max_of_max_dims, (12, 4))
self.assertEqual(bsm.nzb_dtype.dtype, wp.float32)
self.assertEqual(bsm.nzb_dtype.shape, (6,))
self.assertIs(bsm.index_dtype, wp.int32)
self.assertEqual(bsm.device, self.default_device)
# Check on-device data shapes
self.assertEqual(bsm.max_dims.shape, (3, 2))
self.assertEqual(bsm.dims.shape, (3, 2))
self.assertEqual(bsm.row_start.shape, (3,))
self.assertEqual(bsm.col_start.shape, (3,))
self.assertEqual(bsm.max_nzb.shape, (3,))
self.assertEqual(bsm.num_nzb.shape, (3,))
self.assertEqual(bsm.nzb_start.shape, (3,))
self.assertEqual(bsm.nzb_coords.shape, (12, 2))
self.assertEqual(bsm.nzb_values.shape, (12,))
self.assertEqual(bsm.nzb_values.size, 12)
self.assertEqual(bsm.nzb_values.view(dtype=wp.float32).size, 72)
###
# Building Tests
###
def test_10_build_multiple_vector_block_sparse_matrices_full(self):
"""
Tests building two fully-filled block-sparse matrices with vector-shaped blocks and same overall shape.
"""
bsm = BlockSparseMatrices(num_matrices=2, nzb_dtype=BlockDType(shape=(6,), dtype=wp.float32))
bsm.finalize(max_dims=[(2, 12), (2, 12)], capacities=[2, 3], device=self.default_device)
# Check meta-data
self.assertEqual(bsm.num_matrices, 2)
self.assertEqual(bsm.sum_of_num_nzb, 5)
self.assertEqual(bsm.max_of_num_nzb, 3)
self.assertEqual(bsm.max_of_max_dims, (2, 12))
self.assertEqual(bsm.nzb_dtype.dtype, wp.float32)
self.assertEqual(bsm.nzb_dtype.shape, (6,))
self.assertIs(bsm.index_dtype, wp.int32)
self.assertEqual(bsm.device, self.default_device)
# Check on-device data shapes
self.assertEqual(bsm.max_dims.shape, (bsm.num_matrices, 2))
self.assertEqual(bsm.dims.shape, (bsm.num_matrices, 2))
self.assertEqual(bsm.row_start.shape, (bsm.num_matrices,))
self.assertEqual(bsm.col_start.shape, (bsm.num_matrices,))
self.assertEqual(bsm.max_nzb.shape, (bsm.num_matrices,))
self.assertEqual(bsm.num_nzb.shape, (bsm.num_matrices,))
self.assertEqual(bsm.nzb_start.shape, (bsm.num_matrices,))
self.assertEqual(bsm.nzb_coords.shape, (bsm.sum_of_num_nzb, 2))
self.assertEqual(bsm.nzb_values.shape, (bsm.sum_of_num_nzb,))
self.assertEqual(bsm.nzb_values.size, bsm.sum_of_num_nzb)
self.assertEqual(bsm.nzb_values.view(dtype=wp.float32).size, bsm.sum_of_num_nzb * bsm.nzb_dtype.size)
# Build each matrix as follows:
# Matrix 0: 2x12 block0diagonal with 2 non-zero blocks at on diagonals (0,0) and (1,6)
# Matrix 1: 2x12 upper-block-triangular with 3 non-zero blocks at on at (0,0), (0,6), and (1,6)
nzb_dims_np = np.array([[2, 12], [2, 12]], dtype=np.int32)
num_nzb_np = np.array([[2], [3]], dtype=np.int32)
nzb_coords_np = np.array([[0, 0], [1, 6], [0, 0], [0, 6], [1, 6]], dtype=np.int32)
nzb_values_np = np.array(
[
[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
[0.1, 0.2, 0.3, 0.4, 0.5, 0.6],
[2.0, 4.0, 6.0, 8.0, 10.0, 12.0],
[3.0, 3.0, 3.0, 3.0, 3.0, 3.0],
[9.0, 9.0, 9.0, 9.0, 9.0, 9.0],
],
dtype=np.float32,
)
bsm.dims.assign(nzb_dims_np)
bsm.num_nzb.assign(num_nzb_np)
bsm.nzb_coords.assign(nzb_coords_np)
bsm.nzb_values.view(dtype=wp.float32).assign(nzb_values_np)
msg.info("bsm.max_of_max_dims:\n%s", bsm.max_of_max_dims)
msg.info("bsm.max_dims:\n%s", bsm.max_dims)
msg.info("bsm.dims:\n%s", bsm.dims)
msg.info("bsm.max_nzb:\n%s", bsm.max_nzb)
msg.info("bsm.num_nzb:\n%s", bsm.num_nzb)
msg.info("bsm.nzb_start:\n%s", bsm.nzb_start)
msg.info("bsm.nzb_coords:\n%s", bsm.nzb_coords)
msg.info("bsm.nzb_values:\n%s", bsm.nzb_values)
# Check host device data
self.assertEqual(bsm.max_of_max_dims, (2, 12))
# Check on-device data shapes again to ensure nothing changed during building
self.assertEqual(bsm.max_dims.shape, (bsm.num_matrices, 2))
self.assertEqual(bsm.dims.shape, (bsm.num_matrices, 2))
self.assertEqual(bsm.row_start.shape, (bsm.num_matrices,))
self.assertEqual(bsm.col_start.shape, (bsm.num_matrices,))
self.assertEqual(bsm.max_nzb.shape, (bsm.num_matrices,))
self.assertEqual(bsm.num_nzb.shape, (bsm.num_matrices,))
self.assertEqual(bsm.nzb_start.shape, (bsm.num_matrices,))
self.assertEqual(bsm.nzb_coords.shape, (bsm.sum_of_num_nzb, 2))
self.assertEqual(bsm.nzb_values.shape, (bsm.sum_of_num_nzb,))
self.assertEqual(bsm.nzb_values.size, bsm.sum_of_num_nzb)
self.assertEqual(bsm.nzb_values.view(dtype=wp.float32).size, bsm.sum_of_num_nzb * bsm.nzb_dtype.size)
# Convert to list of numpy arrays for easier verification
bsm_np = bsm.numpy()
for i in range(bsm.num_matrices):
msg.info("bsm_np[%d]:\n%s", i, bsm_np[i])
if self.plot:
sparseplot(bsm_np[i], title=f"bsm_np[{i}]")
# Assign new values to the dense numpy arrays and set them back to the block-sparse matrices
for i in range(bsm.num_matrices):
bsm_np[i] += 1.0 * (i + 1)
bsm.assign(bsm_np)
# Convert again to list of numpy arrays for easier verification
bsm_np = bsm.numpy()
for i in range(bsm.num_matrices):
msg.info("bsm_np[%d]:\n%s", i, bsm_np[i])
if self.plot:
sparseplot(bsm_np[i], title=f"bsm_np[{i}]")
class TestBlockSparseMatrixOperations(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.epsilon = 1e-5 # Threshold for matvec product test
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
self.plot = test_context.verbose # Set to True to plot sparse matrices
# Random number generation.
self.seed = 42
self.rng = None
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
###
# Test Helpers
###
def _matvec_product_check(self, ops: BlockSparseLinearOperators):
"""Tests the regular matrix-vector product and the generalized matrix-vector product for the
given operator, both in regular and transposed versions."""
bsm = ops.bsm
num_matrices = bsm.num_matrices
matrix_max_dims = bsm.max_dims.numpy()
matrix_dims = bsm.dims.numpy()
row_start_np = bsm.row_start.numpy()
col_start_np = bsm.col_start.numpy()
matrix_max_dims_sum = np.sum(matrix_max_dims, axis=0)
def product_check(transpose: bool, mask_matrices: bool):
input_dim, output_dim = (0, 1) if transpose else (1, 0)
size_input = matrix_max_dims_sum[input_dim]
size_output = matrix_max_dims_sum[output_dim]
input_start, output_start = (row_start_np, col_start_np) if transpose else (col_start_np, row_start_np)
if mask_matrices:
mask_np = np.ones((num_matrices,), dtype=np.int32)
mask_np[::2] = 0
matrix_mask = wp.from_numpy(mask_np, dtype=wp.int32, device=self.default_device)
else:
matrix_mask = wp.ones((num_matrices,), dtype=wp.int32, device=self.default_device)
# Create vectors for matrix-vector multiplications.
alpha = float(self.rng.standard_normal((1,))[0])
beta = float(self.rng.standard_normal((1,))[0])
input_vectors = [self.rng.standard_normal((shape[input_dim],)) for shape in matrix_dims]
offset_vectors = [self.rng.standard_normal((shape[output_dim],)) for shape in matrix_dims]
input_vec_np = np.zeros((size_input,), dtype=np.float32)
offset_vec_np = np.zeros((size_output,), dtype=np.float32)
for mat_id in range(num_matrices):
input_vec_np[input_start[mat_id] : input_start[mat_id] + matrix_dims[mat_id, input_dim]] = (
input_vectors[mat_id]
)
offset_vec_np[output_start[mat_id] : output_start[mat_id] + matrix_dims[mat_id, output_dim]] = (
offset_vectors[mat_id]
)
# Compute matrix-vector product.
input_vec = wp.from_numpy(input_vec_np, dtype=wp.float32, device=self.default_device)
output_vec_matmul = wp.zeros((size_output,), dtype=wp.float32, device=self.default_device)
output_vec_gemv = wp.from_numpy(offset_vec_np, dtype=wp.float32, device=self.default_device)
if transpose:
ops.matvec_transpose(input_vec, output_vec_matmul, matrix_mask)
ops.gemv_transpose(input_vec, output_vec_gemv, matrix_mask, alpha, beta)
else:
ops.matvec(input_vec, output_vec_matmul, matrix_mask)
ops.gemv(input_vec, output_vec_gemv, matrix_mask, alpha, beta)
# Compare result to dense matrix-vector product.
matrices_np = bsm.numpy()
output_vec_matmul_np = output_vec_matmul.numpy()
output_vec_gemv_np = output_vec_gemv.numpy()
matrix_mask_np = matrix_mask.numpy()
for mat_id in range(num_matrices):
if matrix_mask_np[mat_id] == 0:
output_matmul = output_vec_matmul_np[
output_start[mat_id] : output_start[mat_id] + matrix_dims[mat_id, output_dim]
]
self.assertEqual(np.max(np.abs(output_matmul)), 0.0)
diff_gemv = (
offset_vec_np[output_start[mat_id] : output_start[mat_id] + matrix_dims[mat_id, output_dim]]
- output_vec_gemv_np[
output_start[mat_id] : output_start[mat_id] + matrix_dims[mat_id, output_dim]
]
)
self.assertEqual(np.max(np.abs(diff_gemv)), 0.0)
else:
if transpose:
output_vec_matmul_ref = matrices_np[mat_id].T @ input_vectors[mat_id]
else:
output_vec_matmul_ref = matrices_np[mat_id] @ input_vectors[mat_id]
output_vec_gemv_ref = alpha * output_vec_matmul_ref + beta * offset_vectors[mat_id]
diff_matmul = (
output_vec_matmul_ref
- output_vec_matmul_np[
output_start[mat_id] : output_start[mat_id] + matrix_dims[mat_id, output_dim]
]
)
self.assertLess(np.max(np.abs(diff_matmul)), self.epsilon)
diff_gemv = (
output_vec_gemv_ref
- output_vec_gemv_np[
output_start[mat_id] : output_start[mat_id] + matrix_dims[mat_id, output_dim]
]
)
self.assertLess(np.max(np.abs(diff_gemv)), self.epsilon)
product_check(transpose=False, mask_matrices=False)
product_check(transpose=False, mask_matrices=True)
product_check(transpose=True, mask_matrices=False)
product_check(transpose=True, mask_matrices=True)
###
# Matrix-Vector Product Tests
###
def test_00_sparse_matrix_vector_product_full(self):
"""
Tests multiplication of a random dense block matrix with a random vector.
"""
# Test dimensions.
blocks_per_dim = np.array([[4, 4], [6, 4], [4, 6]], dtype=np.int32)
block_dims_array = [(1,), (3,), (2, 2)]
self.rng = np.random.default_rng(seed=self.seed)
for block_dims_short in block_dims_array:
num_matrices = len(blocks_per_dim)
block_dims = (1, block_dims_short[0]) if len(block_dims_short) == 1 else block_dims_short
# Add offsets for max dimensions (in terms of blocks).
max_blocks_per_dim = blocks_per_dim.copy()
for mat_id in range(num_matrices):
max_blocks_per_dim[mat_id, :] += [2 * mat_id + 3, 2 * mat_id + 4]
# Compute matrix dimensions in terms of entries.
matrix_dims = [(int(shape[0] * block_dims[0]), int(shape[1] * block_dims[1])) for shape in blocks_per_dim]
matrix_max_dims = [
(int(shape[0] * block_dims[0]), int(shape[1] * block_dims[1])) for shape in max_blocks_per_dim
]
# Generate random matrix and vector.
matrices = [self.rng.standard_normal((shape[0], shape[1])) for shape in matrix_dims]
bsm = BlockSparseMatrices(
num_matrices=num_matrices, nzb_dtype=BlockDType(shape=block_dims_short, dtype=wp.float32)
)
capacities = np.asarray([shape[0] * shape[1] for shape in max_blocks_per_dim], dtype=np.int32)
num_nzb_np = np.asarray([shape[0] * shape[1] for shape in blocks_per_dim], dtype=np.int32)
bsm.finalize(max_dims=matrix_max_dims, capacities=[int(c) for c in capacities], device=self.default_device)
# Fill in sparse matrix data structure.
nzb_start_np = bsm.nzb_start.numpy()
nzb_coords_np = np.zeros((bsm.sum_of_num_nzb, 2), dtype=np.int32)
nzb_values_np = np.zeros((bsm.sum_of_num_nzb, block_dims[0], block_dims[1]), dtype=np.float32)
for mat_id in range(num_matrices):
for outer_row_id in range(blocks_per_dim[mat_id, 0]):
row_id = outer_row_id * block_dims[0]
for outer_col_id in range(blocks_per_dim[mat_id, 1]):
col_id = outer_col_id * block_dims[1]
global_idx = nzb_start_np[mat_id] + outer_row_id * blocks_per_dim[mat_id, 1] + outer_col_id
nzb_coords_np[global_idx, :] = [row_id, col_id]
nzb_values_np[global_idx, :, :] = matrices[mat_id][
row_id : row_id + block_dims[0], col_id : col_id + block_dims[1]
]
bsm.dims.assign(matrix_dims)
bsm.num_nzb.assign(num_nzb_np)
bsm.nzb_coords.assign(nzb_coords_np)
bsm.nzb_values.view(dtype=wp.float32).assign(nzb_values_np)
# Build operator.
ops = BlockSparseLinearOperators(bsm)
# Run multiplication operator checks.
self._matvec_product_check(ops)
def test_01_sparse_matrix_vector_product_partial(self):
"""
Tests multiplication of a random block sparse matrix with a random vector.
"""
# Test dimensions.
blocks_per_dim = np.array([[7, 7], [17, 11], [13, 19]], dtype=np.int32)
block_dims_array = [(1,), (3,), (2, 2)]
sparse_block_offset = 5 # Every i-th block will be filled.
self.rng = np.random.default_rng(seed=self.seed)
for block_dims_short in block_dims_array:
num_matrices = len(blocks_per_dim)
block_dims = (1, block_dims_short[0]) if len(block_dims_short) == 1 else block_dims_short
# Add offsets for max dimensions (in terms of blocks).
max_blocks_per_dim = blocks_per_dim.copy()
for mat_id in range(num_matrices):
max_blocks_per_dim[mat_id, :] += [2 * mat_id + 3, 2 * mat_id + 4]
# Compute matrix dimensions in terms of entries.
matrix_dims = [(int(shape[0] * block_dims[0]), int(shape[1] * block_dims[1])) for shape in blocks_per_dim]
matrix_max_dims = [
(int(shape[0] * block_dims[0]), int(shape[1] * block_dims[1])) for shape in max_blocks_per_dim
]
# Create sparse matrices by randomly selecting which blocks to populate.
num_nzb_np = np.zeros((num_matrices,), dtype=np.int32)
max_nzb_np = np.zeros((num_matrices,), dtype=np.int32)
nzb_coords_list = []
nzb_values_list = []
for mat_id in range(num_matrices):
# Randomly select blocks to be non-zero.
all_block_indices = [
[row * block_dims[0], col * block_dims[1]]
for col in range(blocks_per_dim[mat_id, 1])
for row in range(blocks_per_dim[mat_id, 0])
]
nzb_coords = all_block_indices[mat_id::sparse_block_offset]
num_nzb_np[mat_id] = len(nzb_coords)
# Create non-zero blocks for matrix.
for _ in nzb_coords:
nzb_values_list.append(self.rng.standard_normal(block_dims, dtype=np.float32))
# Add empty entries.
for _ in range(5):
nzb_coords.append([0, 0])
nzb_values_list.append(np.zeros(block_dims, dtype=np.float32))
max_nzb_np[mat_id] = len(nzb_coords)
nzb_coords_list.extend(nzb_coords)
nzb_coords_np = np.asarray(nzb_coords_list, dtype=np.float32)
nzb_values_np = np.asarray(nzb_values_list, dtype=np.float32)
bsm = BlockSparseMatrices(
num_matrices=num_matrices, nzb_dtype=BlockDType(shape=block_dims_short, dtype=wp.float32)
)
bsm.finalize(max_dims=matrix_max_dims, capacities=[int(c) for c in max_nzb_np], device=self.default_device)
# Fill in sparse matrix data structure.
bsm.dims.assign(matrix_dims)
bsm.num_nzb.assign(num_nzb_np)
bsm.nzb_coords.assign(nzb_coords_np)
bsm.nzb_values.view(dtype=wp.float32).assign(nzb_values_np)
# Build operator.
ops = BlockSparseLinearOperators(bsm)
# Run multiplication operator checks.
self._matvec_product_check(ops)
if self.plot:
matrices = bsm.numpy()
for i in range(num_matrices):
sparseplot(matrices[i], title=f"Matrix {i}")
def test_02_sparse_matrix_vector_product_jacobian(self):
"""
Tests multiplication of a block sparse matrix with a random vector, where the sparse matrix has a Jacobian-like
structure.
"""
# Problem size as number of rigid bodies and number of constraints.
problem_sizes = [(10, 10), (20, 22), (100, 105)]
num_matrices = 5
block_dims_short = (6,)
block_dims = (1, 6)
self.rng = np.random.default_rng(seed=self.seed)
for problem_size in problem_sizes:
num_rigid_bodies = problem_size[0]
num_constraints = problem_size[1]
max_num_contacts = 10
max_num_limits = num_constraints // 2
num_eqs_per_constraint = self.rng.integers(3, 6, num_constraints, endpoint=True, dtype=int)
num_constraint_eqs = np.sum(num_eqs_per_constraint)
# Compute matrix dimensions.
max_blocks_per_dim = (int(num_constraint_eqs) + 3 * max_num_contacts + max_num_limits, num_rigid_bodies)
matrix_max_dims = [
(max_blocks_per_dim[0] * block_dims[0], max_blocks_per_dim[1] * block_dims[1])
] * num_matrices
# Create sparse Jacobian-like matrices by randomly selecting which blocks to populate.
num_nzb_np = np.zeros((num_matrices,), dtype=np.int32)
max_nzb_np = np.zeros((num_matrices,), dtype=np.int32)
nzb_coords_list = []
nzb_values_list = []
matrix_dims = []
for mat_id in range(num_matrices):
row_idx = 0
nzb_coords = []
# Add binary constraints.
for ct_id in range(num_constraints):
rb_id_A, rb_id_B = self.rng.choice(num_rigid_bodies, 2, replace=False)
for i in range(num_eqs_per_constraint[ct_id]):
nzb_coords.append((row_idx + i, block_dims[1] * rb_id_A))
for i in range(num_eqs_per_constraint[ct_id]):
nzb_coords.append((row_idx + i, block_dims[1] * rb_id_B))
row_idx += num_eqs_per_constraint[ct_id]
# Add some numbers of contacts.
for _ in range(max_num_contacts // 3):
rb_id = self.rng.choice(num_rigid_bodies, 1)[0]
for i in range(3):
nzb_coords.append((row_idx + i, block_dims[1] * rb_id))
row_idx += 3
# Add some number of limits.
for _ in range(num_constraints // 3):
rb_id_A, rb_id_B = self.rng.choice(num_rigid_bodies, 2, replace=False)
nzb_coords.append((row_idx, block_dims[1] * rb_id_A))
nzb_coords.append((row_idx, block_dims[1] * rb_id_B))
row_idx += 1
num_nzb_np[mat_id] = len(nzb_coords)
dims = (row_idx, block_dims[1] * num_rigid_bodies)
matrix_dims.append(list(dims))
# Create non-zero blocks for matrix.
for _ in nzb_coords:
nzb_values_list.append(self.rng.standard_normal(block_dims, dtype=np.float32))
# Add empty entries.
for _ in range(5):
nzb_coords.append([0, 0])
nzb_values_list.append(np.zeros(block_dims, dtype=np.float32))
max_nzb_np[mat_id] = len(nzb_coords)
nzb_coords_list.extend(nzb_coords)
matrix_dims = np.asarray(matrix_dims)
nzb_coords_np = np.asarray(nzb_coords_list, dtype=np.float32)
nzb_values_np = np.asarray(nzb_values_list, dtype=np.float32)
bsm = BlockSparseMatrices(
num_matrices=num_matrices, nzb_dtype=BlockDType(shape=block_dims_short, dtype=wp.float32)
)
bsm.finalize(max_dims=matrix_max_dims, capacities=[int(c) for c in max_nzb_np], device=self.default_device)
# Fill in sparse matrix data structure.
bsm.dims.assign(matrix_dims)
bsm.num_nzb.assign(num_nzb_np)
bsm.nzb_coords.assign(nzb_coords_np)
bsm.nzb_values.view(dtype=wp.float32).assign(nzb_values_np)
# Build operator.
ops = BlockSparseLinearOperators(bsm)
# Run multiplication operator checks.
self._matvec_product_check(ops)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_utils_matrix.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for linear algebra matrix analysis utilities"""
import unittest
import numpy as np
import newton._src.solvers.kamino._src.linalg as linalg
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestUtilsLinAlgMatrix(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for verbose output
if self.verbose:
msg.set_log_level(msg.LogLevel.DEBUG)
def tearDown(self):
if self.verbose:
msg.reset_log_level()
def test_01_spd_matrix_properties(self):
A = linalg.utils.rand.random_spd_matrix(dim=10, dtype=np.float32, scale=4.0, seed=42)
A_props = linalg.utils.matrix.SquareSymmetricMatrixProperties(A)
msg.debug(f"A (shape: {A.shape}, dtype: {A.dtype}):\n{A}\n") if self.verbose else None
msg.debug(f"A properties:\n{A_props}\n") if self.verbose else None
msg.debug(f"cond(A): {np.linalg.cond(A)}\n") if self.verbose else None
msg.debug(f"det(A): {np.linalg.det(A)}\n") if self.verbose else None
self.assertAlmostEqual(A_props.cond, np.linalg.cond(A), places=6)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_linalg_utils_rand.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for linalg/utils/rand.py"""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.linalg.utils.matrix import (
SquareSymmetricMatrixProperties,
is_square_matrix,
is_symmetric_matrix,
)
from newton._src.solvers.kamino._src.linalg.utils.rand import (
eigenvalues_from_distribution,
random_rhs_for_matrix,
random_spd_matrix,
random_symmetric_matrix,
)
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestLinAlgUtilsRandomMatrixSymmetric(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_make_small_random_symmetric_matrix_in_fp64(self):
# Generate a small random symmetric matrix in fp64
A = random_symmetric_matrix(dim=10, dtype=np.float64, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (10, 10))
self.assertEqual(A.dtype, np.float64)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
def test_02_make_medium_random_symmetric_matrix_in_fp64(self):
# Generate a small random symmetric matrix in fp64
A = random_symmetric_matrix(dim=100, dtype=np.float64, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (100, 100))
self.assertEqual(A.dtype, np.float64)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
def test_03_make_large_random_symmetric_matrix_in_fp64(self):
# Generate a small random symmetric matrix in fp64
A = random_symmetric_matrix(dim=1000, dtype=np.float64, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (1000, 1000))
self.assertEqual(A.dtype, np.float64)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
def test_04_make_small_random_symmetric_matrix_in_fp32(self):
# Generate a small random symmetric matrix in fp32
A = random_symmetric_matrix(dim=10, dtype=np.float32, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (10, 10))
self.assertEqual(A.dtype, np.float32)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
def test_05_make_medium_random_symmetric_matrix_in_fp32(self):
# Generate a small random symmetric matrix in fp32
A = random_symmetric_matrix(dim=100, dtype=np.float32, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (100, 100))
self.assertEqual(A.dtype, np.float32)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
def test_06_make_large_random_symmetric_matrix_in_fp32(self):
# Generate a small random symmetric matrix in fp32
A = random_symmetric_matrix(dim=1000, dtype=np.float32, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (1000, 1000))
self.assertEqual(A.dtype, np.float32)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
def test_07_make_large_symmetric_matrix_in_fp64_with_eigenvalues_from_distribution(self):
# Set matrix properties
M = 1000
dtype = np.float64
# Generate a distribution of eigenvalues
eigenvalues = eigenvalues_from_distribution(size=M, dtype=dtype, seed=self.seed)
msg.debug("eigenvalues:\n%s\n", eigenvalues)
# Generate a large random symmetric matrix in fp32
A = random_symmetric_matrix(
dim=M,
dtype=dtype,
seed=self.seed,
eigenvalues=eigenvalues,
)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (M, M))
self.assertEqual(A.dtype, dtype)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertAlmostEqual(props_A.lambda_min, np.min(eigenvalues), places=6)
self.assertAlmostEqual(props_A.lambda_max, np.max(eigenvalues), places=6)
def test_08_make_large_symmetric_matrix_in_fp32_with_eigenvalues_from_distribution(self):
# Set matrix properties
M = 1000
dtype = np.float32
# Generate a distribution of eigenvalues
eigenvalues = eigenvalues_from_distribution(size=M, dtype=dtype, seed=self.seed)
msg.debug("eigenvalues:\n%s\n", eigenvalues)
# Generate a large random symmetric matrix in fp32
A = random_symmetric_matrix(
dim=M,
dtype=dtype,
seed=self.seed,
eigenvalues=eigenvalues,
)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (M, M))
self.assertEqual(A.dtype, dtype)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertAlmostEqual(props_A.lambda_min, np.min(eigenvalues), places=5)
self.assertAlmostEqual(props_A.lambda_max, np.max(eigenvalues), places=5)
def test_09_make_large_symmetric_matrix_in_fp64_with_rank(self):
# Set matrix properties
M = 1000
rank = 513
dtype = np.float64
# Generate a large random symmetric matrix in fp32
A = random_symmetric_matrix(
dim=M,
dtype=dtype,
seed=self.seed,
rank=rank,
)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (M, M))
self.assertEqual(A.dtype, dtype)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertAlmostEqual(props_A.rank, rank)
def test_10_make_large_symmetric_matrix_in_fp32_with_rank(self):
# Set matrix properties
M = 1000
rank = 513
dtype = np.float32
# Generate a large random symmetric matrix in fp32
A = random_symmetric_matrix(
dim=M,
dtype=dtype,
seed=self.seed,
rank=rank,
)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (M, M))
self.assertEqual(A.dtype, dtype)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertAlmostEqual(props_A.rank, rank)
class TestLinAlgUtilsRandomMatrixSPD(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_make_small_random_spd_matrix_in_fp64(self):
# Generate a small random SPD matrix in fp64
A = random_spd_matrix(dim=10, dtype=np.float64, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (10, 10))
self.assertEqual(A.dtype, np.float64)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertGreater(props_A.lambda_min, 0.0)
self.assertGreater(props_A.lambda_max, 0.0)
def test_02_make_medium_random_spd_matrix_in_fp64(self):
# Generate a small random SPD matrix in fp64
A = random_spd_matrix(dim=100, dtype=np.float64, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (100, 100))
self.assertEqual(A.dtype, np.float64)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertGreater(props_A.lambda_min, 0.0)
self.assertGreater(props_A.lambda_max, 0.0)
def test_03_make_large_random_spd_matrix_in_fp64(self):
# Generate a small random SPD matrix in fp64
A = random_spd_matrix(dim=1000, dtype=np.float64, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (1000, 1000))
self.assertEqual(A.dtype, np.float64)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertGreater(props_A.lambda_min, 0.0)
self.assertGreater(props_A.lambda_max, 0.0)
def test_04_make_small_random_spd_matrix_in_fp32(self):
# Generate a small random SPD matrix in fp32
A = random_spd_matrix(dim=10, dtype=np.float32, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (10, 10))
self.assertEqual(A.dtype, np.float32)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertGreater(props_A.lambda_min, 0.0)
self.assertGreater(props_A.lambda_max, 0.0)
def test_05_make_medium_random_spd_matrix_in_fp32(self):
# Generate a small random SPD matrix in fp32
A = random_spd_matrix(dim=100, dtype=np.float32, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (100, 100))
self.assertEqual(A.dtype, np.float32)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertGreater(props_A.lambda_min, 0.0)
self.assertGreater(props_A.lambda_max, 0.0)
def test_06_make_large_random_spd_matrix_in_fp32(self):
# Generate a small random SPD matrix in fp32
A = random_spd_matrix(dim=1000, dtype=np.float32, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Check basic matrix properties
self.assertEqual(A.shape, (1000, 1000))
self.assertEqual(A.dtype, np.float32)
self.assertTrue(is_square_matrix(A))
self.assertTrue(is_symmetric_matrix(A))
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check spectral matrix properties
self.assertGreater(props_A.lambda_min, 0.0)
self.assertGreater(props_A.lambda_max, 0.0)
class TestLinAlgUtilsRandomRhsVectors(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_make_rhs_for_small_random_spd_matrix_in_fp64(self):
# Generate a small random SPD matrix in fp64
A = random_spd_matrix(dim=10, dtype=np.float64, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Generate a random rhs vector for A
b, x = random_rhs_for_matrix(A, seed=self.seed, return_source=True)
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check basic vector properties
self.assertEqual(b.shape, (A.shape[0],))
self.assertEqual(b.dtype, A.dtype)
# Check norm properties
norm_b = np.linalg.norm(b, ord=2)
norm_x = np.linalg.norm(x, ord=2)
msg.debug("||b||_2 = %f\n", norm_b)
msg.debug("||x||_2 = %f\n", norm_x)
msg.debug("||A||_2 = %f\n", props_A.lambda_max)
self.assertGreater(norm_b, 0.0)
self.assertGreater(norm_x, 0.0)
self.assertLessEqual(norm_b, props_A.lambda_max * norm_x)
# Check that A*x = b
b_computed = A @ x
error = np.linalg.norm(b - b_computed, ord=2)
msg.debug("Error in A*x = b: ||b - A*x||_2 = %e\n", error)
self.assertAlmostEqual(error, 0.0, places=12)
def test_02_make_rhs_for_large_random_spd_matrix_in_fp64(self):
# Generate a small random SPD matrix in fp64
A = random_spd_matrix(dim=1000, dtype=np.float64, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Generate a random rhs vector for A
b, x = random_rhs_for_matrix(A, seed=self.seed, return_source=True)
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check basic vector properties
self.assertEqual(b.shape, (A.shape[0],))
self.assertEqual(b.dtype, A.dtype)
# Check norm properties
norm_b = np.linalg.norm(b, ord=2)
norm_x = np.linalg.norm(x, ord=2)
msg.debug("||b||_2 = %f\n", norm_b)
msg.debug("||x||_2 = %f\n", norm_x)
msg.debug("||A||_2 = %f\n", props_A.lambda_max)
self.assertGreater(norm_b, 0.0)
self.assertGreater(norm_x, 0.0)
self.assertLessEqual(norm_b, props_A.lambda_max * norm_x)
# Check that A*x = b
b_computed = A @ x
error = np.linalg.norm(b - b_computed, ord=2)
msg.debug("Error in A*x = b: ||b - A*x||_2 = %e\n", error)
self.assertAlmostEqual(error, 0.0, places=12)
def test_03_make_rhs_for_small_random_spd_matrix_in_fp32(self):
# Generate a small random SPD matrix in fp32
A = random_spd_matrix(dim=10, dtype=np.float32, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Generate a random rhs vector for A
b, x = random_rhs_for_matrix(A, seed=self.seed, return_source=True)
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check basic vector properties
self.assertEqual(b.shape, (A.shape[0],))
self.assertEqual(b.dtype, A.dtype)
# Check norm properties
norm_b = np.linalg.norm(b, ord=2)
norm_x = np.linalg.norm(x, ord=2)
msg.debug("||b||_2 = %f\n", norm_b)
msg.debug("||x||_2 = %f\n", norm_x)
msg.debug("||A||_2 = %f\n", props_A.lambda_max)
self.assertGreater(norm_b, 0.0)
self.assertGreater(norm_x, 0.0)
self.assertLessEqual(norm_b, props_A.lambda_max * norm_x)
# Check that A*x = b
b_computed = A @ x
error = np.linalg.norm(b - b_computed, ord=2)
msg.debug("Error in A*x = b: ||b - A*x||_2 = %e\n", error)
self.assertAlmostEqual(error, 0.0, places=12)
def test_02_make_rhs_for_large_random_spd_matrix_in_fp32(self):
# Generate a small random SPD matrix in fp32
A = random_spd_matrix(dim=1000, dtype=np.float32, seed=self.seed)
msg.debug("A:\n%s\n", A)
# Generate a random rhs vector for A
b, x = random_rhs_for_matrix(A, seed=self.seed, return_source=True)
# Compute matrix properties
props_A = SquareSymmetricMatrixProperties(A)
msg.debug("Matrix properties of A:\n%s\n", props_A)
# Check basic vector properties
self.assertEqual(b.shape, (A.shape[0],))
self.assertEqual(b.dtype, A.dtype)
# Check norm properties
norm_b = np.linalg.norm(b, ord=2)
norm_x = np.linalg.norm(x, ord=2)
msg.debug("||b||_2 = %f\n", norm_b)
msg.debug("||x||_2 = %f\n", norm_x)
msg.debug("||A||_2 = %f\n", props_A.lambda_max)
self.assertGreater(norm_b, 0.0)
self.assertGreater(norm_x, 0.0)
self.assertLessEqual(norm_b, props_A.lambda_max * norm_x)
# Check that A*x = b
b_computed = A @ x
error = np.linalg.norm(b - b_computed, ord=2)
msg.debug("Error in A*x = b: ||b - A*x||_2 = %e\n", error)
self.assertAlmostEqual(error, 0.0, places=12)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_solver_kamino.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the :class:`SolverKaminoImpl` class"""
import time
import unittest
import numpy as np
import warp as wp
import newton._src.solvers.kamino.config as kamino_config
from newton._src.solvers.kamino._src.core.control import ControlKamino
from newton._src.solvers.kamino._src.core.data import DataKamino
from newton._src.solvers.kamino._src.core.joints import JointActuationType
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.core.state import StateKamino
from newton._src.solvers.kamino._src.core.types import float32, int32, transformf, vec6f
from newton._src.solvers.kamino._src.dynamics import DualProblem
from newton._src.solvers.kamino._src.geometry.contacts import ContactsKamino
from newton._src.solvers.kamino._src.kinematics.jacobians import DenseSystemJacobians, SparseSystemJacobians
from newton._src.solvers.kamino._src.kinematics.limits import LimitsKamino
from newton._src.solvers.kamino._src.models.builders.basics import build_boxes_fourbar
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.solver_kamino_impl import SolverKaminoImpl
from newton._src.solvers.kamino._src.solvers import PADMMSolver
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.examples import print_progress_bar
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Kernels
###
@wp.kernel
def _test_control_callback(
model_dt: wp.array(dtype=float32),
data_time: wp.array(dtype=float32),
control_tau_j: wp.array(dtype=float32),
):
"""
An example control callback kernel.
"""
# Retrieve the world index from the thread ID
wid = wp.tid()
# Get the fixed time-step and current time
dt = model_dt[wid]
t = data_time[wid]
# Define the time window for the active external force profile
t_start = float32(0.0)
t_end = 10.0 * dt
# Compute the first actuated joint index for the current world
aid = wid * 2 + 0
# Apply a time-dependent external force
if t > t_start and t < t_end:
control_tau_j[aid] = 0.1
else:
control_tau_j[aid] = 0.0
###
# Launchers
###
def test_prestep_callback(
solver: SolverKaminoImpl,
state_in: StateKamino,
state_out: StateKamino,
control: ControlKamino,
contacts: ContactsKamino,
):
"""
A control callback function
"""
wp.launch(
_test_control_callback,
dim=solver._model.size.num_worlds,
inputs=[
solver._model.time.dt,
solver._data.time.time,
control.tau_j,
],
)
###
# Utils
###
rtol = 1e-7
atol = 1e-6
def assert_solver_config(testcase: unittest.TestCase, config: SolverKaminoImpl.Config):
testcase.assertIsInstance(config, SolverKaminoImpl.Config)
testcase.assertIsInstance(config.constraints, kamino_config.ConstraintStabilizationConfig)
testcase.assertIsInstance(config.dynamics, kamino_config.ConstrainedDynamicsConfig)
testcase.assertIsInstance(config.padmm, kamino_config.PADMMSolverConfig)
testcase.assertIsInstance(config.rotation_correction, str)
def assert_solver_components(testcase: unittest.TestCase, solver: SolverKaminoImpl):
testcase.assertIsInstance(solver, SolverKaminoImpl)
testcase.assertIsInstance(solver.config, SolverKaminoImpl.Config)
testcase.assertIsInstance(solver._model, ModelKamino)
testcase.assertIsInstance(solver._data, DataKamino)
testcase.assertIsInstance(solver._limits, LimitsKamino)
if solver._problem_fd.sparse:
testcase.assertIsInstance(solver._jacobians, SparseSystemJacobians)
else:
testcase.assertIsInstance(solver._jacobians, DenseSystemJacobians)
testcase.assertIsInstance(solver._problem_fd, DualProblem)
testcase.assertIsInstance(solver._solver_fd, PADMMSolver)
def assert_states_equal(testcase: unittest.TestCase, state_0: StateKamino, state_1: StateKamino):
testcase.assertIsInstance(state_0, StateKamino)
testcase.assertIsInstance(state_1, StateKamino)
np.testing.assert_array_equal(state_0.q_i.numpy(), state_1.q_i.numpy())
np.testing.assert_array_equal(state_0.u_i.numpy(), state_1.u_i.numpy())
np.testing.assert_array_equal(state_0.w_i.numpy(), state_1.w_i.numpy())
np.testing.assert_array_equal(state_0.q_j.numpy(), state_1.q_j.numpy())
np.testing.assert_array_equal(state_0.q_j_p.numpy(), state_1.q_j_p.numpy())
np.testing.assert_array_equal(state_0.dq_j.numpy(), state_1.dq_j.numpy())
np.testing.assert_array_equal(state_0.lambda_j.numpy(), state_1.lambda_j.numpy())
def assert_states_close(testcase: unittest.TestCase, state_0: StateKamino, state_1: StateKamino):
testcase.assertIsInstance(state_0, StateKamino)
testcase.assertIsInstance(state_1, StateKamino)
np.testing.assert_allclose(state_0.q_i.numpy(), state_1.q_i.numpy(), rtol=rtol, atol=atol)
np.testing.assert_allclose(state_0.u_i.numpy(), state_1.u_i.numpy(), rtol=rtol, atol=atol)
np.testing.assert_allclose(state_0.w_i.numpy(), state_1.w_i.numpy(), rtol=rtol, atol=atol)
np.testing.assert_allclose(state_0.q_j.numpy(), state_1.q_j.numpy(), rtol=rtol, atol=atol)
np.testing.assert_allclose(state_0.q_j_p.numpy(), state_1.q_j_p.numpy(), rtol=rtol, atol=atol)
np.testing.assert_allclose(state_0.dq_j.numpy(), state_1.dq_j.numpy(), rtol=rtol, atol=atol)
np.testing.assert_allclose(state_0.lambda_j.numpy(), state_1.lambda_j.numpy(), rtol=rtol, atol=atol)
def assert_states_close_masked(
testcase: unittest.TestCase,
model: ModelKamino,
state_0: StateKamino,
state_n: StateKamino,
state_n_ref: StateKamino,
world_mask: wp.array,
):
testcase.assertIsInstance(model, ModelKamino)
testcase.assertIsInstance(state_0, StateKamino)
testcase.assertIsInstance(state_n, StateKamino)
num_bodies_per_world = model.size.max_of_num_bodies
num_joint_dofs_per_world = model.size.max_of_num_joint_dofs
num_joint_cts_per_world = model.size.max_of_num_joint_cts
bodies_start = 0
joint_dofs_start = 0
joint_cts_start = 0
world_mask_np = world_mask.numpy().copy()
for wid in range(model.size.num_worlds):
# Select reference state based on world mask
if world_mask_np[wid]:
state_ref = state_0
else:
state_ref = state_n_ref
# Check state attributes for the current world
for attr in ["q_i", "u_i", "w_i"]:
np.testing.assert_allclose(
getattr(state_n, attr).numpy()[bodies_start : bodies_start + num_bodies_per_world],
getattr(state_ref, attr).numpy()[bodies_start : bodies_start + num_bodies_per_world],
rtol=rtol,
atol=atol,
err_msg=f"\nWorld wid={wid}: attribute `{attr}` mismatch:\n",
)
for attr in ["q_j", "q_j_p", "dq_j"]:
np.testing.assert_allclose(
getattr(state_n, attr).numpy()[joint_dofs_start : joint_dofs_start + num_joint_dofs_per_world],
getattr(state_ref, attr).numpy()[joint_dofs_start : joint_dofs_start + num_joint_dofs_per_world],
rtol=rtol,
atol=atol,
err_msg=f"\nWorld wid={wid}: attribute `{attr}` mismatch:\n",
)
for attr in ["lambda_j"]:
np.testing.assert_allclose(
getattr(state_n, attr).numpy()[joint_cts_start : joint_cts_start + num_joint_cts_per_world],
getattr(state_ref, attr).numpy()[joint_cts_start : joint_cts_start + num_joint_cts_per_world],
rtol=rtol,
atol=atol,
err_msg=f"\nWorld wid={wid}: attribute `{attr}` mismatch:\n",
)
bodies_start += num_bodies_per_world
joint_dofs_start += num_joint_dofs_per_world
joint_cts_start += num_joint_cts_per_world
def step_solver(
num_steps: int,
solver: SolverKaminoImpl,
state_p: StateKamino,
state_n: StateKamino,
control: ControlKamino,
contacts: ContactsKamino | None = None,
dt: float = 0.001,
show_progress: bool = False,
):
start_time = time.time()
for step in range(num_steps):
solver.step(state_in=state_p, state_out=state_n, control=control, contacts=contacts, dt=dt)
wp.synchronize()
state_p.copy_from(state_n)
if show_progress:
print_progress_bar(step + 1, num_steps, start_time, prefix="Progress", suffix="")
###
# Tests
###
class TestSolverKaminoConfig(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True to enable verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default(self):
config = SolverKaminoImpl.Config()
assert_solver_config(self, config)
self.assertEqual(config.rotation_correction, "twopi")
self.assertEqual(config.dynamics.linear_solver_type, "LLTB")
self.assertEqual(config.padmm.warmstart_mode, "containers")
def test_01_make_explicit(self):
config = SolverKaminoImpl.Config(
dynamics=kamino_config.ConstrainedDynamicsConfig(linear_solver_type="CR"),
padmm=kamino_config.PADMMSolverConfig(warmstart_mode="internal"),
rotation_correction="continuous",
)
assert_solver_config(self, config)
self.assertEqual(config.rotation_correction, "continuous")
self.assertEqual(config.dynamics.linear_solver_type, "CR")
self.assertEqual(config.padmm.warmstart_mode, "internal")
class TestSolverKaminoImpl(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True to enable verbose output
self.progress = test_context.verbose # Set to True to show progress bars during long tests
self.seed = 42
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
###
# Test Solver Construction
###
def test_00_make_default_invalid(self):
"""
Test that creating a default Kamino solver without a model raises an error.
"""
self.assertRaises(TypeError, lambda: SolverKaminoImpl())
def test_01_make_default_valid_with_limits_and_without_contacts(self):
"""
Test creating a default Kamino solver without support for contacts.
"""
builder = make_homogeneous_builder(num_worlds=1, build_fn=build_boxes_fourbar)
model = builder.finalize(device=self.default_device)
solver = SolverKaminoImpl(model=model)
self.assertIsInstance(solver, SolverKaminoImpl)
assert_solver_components(self, solver)
def test_02_make_default_valid_with_limits_and_with_contacts(self):
"""
Test creating a default Kamino solver with support for contacts.
"""
builder = make_homogeneous_builder(num_worlds=1, build_fn=build_boxes_fourbar)
model = builder.finalize(device=self.default_device)
_, world_max_contacts = builder.compute_required_contact_capacity(max_contacts_per_pair=16)
contacts = ContactsKamino(capacity=world_max_contacts, device=model.device)
solver = SolverKaminoImpl(model=model, contacts=contacts)
self.assertIsInstance(solver, SolverKaminoImpl)
assert_solver_components(self, solver)
def test_03_make_default_valid_without_limits_and_without_contacts(self):
"""
Test creating a default Kamino solver without support for contacts.
"""
builder = make_homogeneous_builder(num_worlds=1, build_fn=build_boxes_fourbar, limits=False)
model = builder.finalize(device=self.default_device)
solver = SolverKaminoImpl(model=model)
self.assertIsInstance(solver, SolverKaminoImpl)
assert_solver_components(self, solver)
self.assertIsNone(solver._limits.data.wid)
def test_04_make_default_valid_without_limits_and_with_contacts(self):
"""
Test creating a default Kamino solver with support for contacts.
"""
builder = make_homogeneous_builder(num_worlds=1, build_fn=build_boxes_fourbar, limits=False)
model = builder.finalize(device=self.default_device)
_, world_max_contacts = builder.compute_required_contact_capacity(max_contacts_per_pair=16)
contacts = ContactsKamino(capacity=world_max_contacts, device=model.device)
solver = SolverKaminoImpl(model=model, contacts=contacts)
self.assertIsInstance(solver, SolverKaminoImpl)
assert_solver_components(self, solver)
self.assertIsNone(solver._limits.data.wid)
###
# Test Reset Operations
###
def test_05_reset_with_invalid_args(self):
"""
Test resetting multiple world solvers to default state defined in the model.
"""
builder = make_homogeneous_builder(num_worlds=3, build_fn=build_boxes_fourbar, limits=False)
model = builder.finalize(device=self.default_device)
solver = SolverKaminoImpl(model=model)
# Create reset argument arrays
state_0 = model.state()
with wp.ScopedDevice(self.default_device):
base_q_0 = wp.zeros(shape=(model.size.num_worlds,), dtype=transformf)
base_u_0 = wp.zeros(shape=(model.size.num_worlds,), dtype=vec6f)
joint_q_0 = wp.zeros(shape=(model.size.sum_of_num_joint_coords,), dtype=float32)
joint_u_0 = wp.zeros(shape=(model.size.sum_of_num_joint_dofs,), dtype=float32)
actuator_q_0 = wp.zeros(shape=(model.size.sum_of_num_actuated_joint_coords,), dtype=float32)
actuator_u_0 = wp.zeros(shape=(model.size.sum_of_num_actuated_joint_dofs,), dtype=float32)
###
# Test invalid argument combinations to ensure errors are correctly raised
###
# Setting only joint or actuator velocities is not supported
self.assertRaises(ValueError, lambda: solver.reset(state_out=state_0, joint_u=joint_u_0))
self.assertRaises(ValueError, lambda: solver.reset(state_out=state_0, actuator_u=actuator_u_0))
# Setting both joint and actuator coordinates or velocities is not supported
self.assertRaises(
ValueError, lambda: solver.reset(state_out=state_0, joint_q=joint_q_0, actuator_q=actuator_q_0)
)
self.assertRaises(
ValueError, lambda: solver.reset(state_out=state_0, joint_q=joint_q_0, actuator_u=actuator_u_0)
)
self.assertRaises(
ValueError, lambda: solver.reset(state_out=state_0, joint_u=joint_u_0, actuator_q=actuator_q_0)
)
self.assertRaises(
ValueError, lambda: solver.reset(state_out=state_0, joint_u=joint_u_0, actuator_u=actuator_u_0)
)
self.assertRaises(
ValueError,
lambda: solver.reset(state_out=state_0, base_q=base_q_0, joint_u=joint_u_0, actuator_u=actuator_u_0),
)
self.assertRaises(
ValueError,
lambda: solver.reset(state_out=state_0, base_u=base_u_0, joint_u=joint_u_0, actuator_u=actuator_u_0),
)
self.assertRaises(
ValueError,
lambda: solver.reset(
state_out=state_0, base_q=base_q_0, base_u=base_u_0, joint_u=joint_u_0, actuator_u=actuator_u_0
),
)
def test_06_reset_to_default_state(self):
"""
Test resetting multiple world solvers to default state defined in the model.
"""
builder = make_homogeneous_builder(num_worlds=3, build_fn=build_boxes_fourbar, limits=False)
model = builder.finalize(device=self.default_device)
solver = SolverKaminoImpl(model=model)
# Set a pre-step control callback to apply external forces
# that will sufficiently perturb the system state
solver.set_pre_step_callback(test_prestep_callback)
# Create a state container to hold the output of the reset
# and a world_mask array to specify which worlds to reset
state_0 = model.state()
state_p = model.state()
state_n = model.state()
control = model.control()
world_mask = wp.array([0, 1, 0], dtype=int32, device=self.default_device)
# Step the solver a few times to change the state
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset all worlds to the initial state
solver.reset(state_out=state_n)
# Check that all worlds were reset
assert_states_equal(self, state_n, state_0)
# Step the solver a few times to change the state
solver._reset()
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Create a copy of the current state before reset
state_n_ref = model.state()
state_n_ref.copy_from(state_n)
# Reset only the specified worlds to the initial state
solver.reset(state_out=state_n, world_mask=world_mask)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Check that only the specified worlds were reset
assert_states_close_masked(self, model, state_0, state_n, state_n_ref, world_mask)
def test_07_reset_to_base_state(self):
"""
Test resetting multiple world solvers to specified base states.
"""
builder = make_homogeneous_builder(num_worlds=3, build_fn=build_boxes_fourbar, limits=False)
model = builder.finalize(device=self.default_device)
solver = SolverKaminoImpl(model=model)
# Set a pre-step control callback to apply external forces
# that will sufficiently perturb the system state
solver.set_pre_step_callback(test_prestep_callback)
# Create a state container to hold the output of the reset
# and a world_mask array to specify which worlds to reset
state_p = model.state()
state_n = model.state()
control = model.control()
world_mask = wp.array([1, 1, 0], dtype=int32, device=self.default_device)
# Define the reset base pose
base_q_0_np = [0.1, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0]
base_q_0_np = np.tile(base_q_0_np, reps=model.size.num_worlds).astype(np.float32)
base_q_0_np = base_q_0_np.reshape(model.size.num_worlds, 7)
base_q_0: wp.array = wp.array(base_q_0_np, dtype=transformf, device=self.default_device)
# Define the reset base twist
base_u_0_np = [0.0, 1.5, 0.0, 0.0, 0.0, 0.0]
base_u_0_np = np.tile(base_u_0_np, reps=model.size.num_worlds).astype(np.float32)
base_u_0_np = base_u_0_np.reshape(model.size.num_worlds, 6)
base_u_0: wp.array = wp.array(base_u_0_np, dtype=vec6f, device=self.default_device)
# Step the solver a few times to change the state
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset all worlds to the specified base poses
solver.reset(
state_out=state_n,
base_q=base_q_0,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Check if the assigned base body was correctly reset
base_body_idx = model.info.base_body_index.numpy().copy()
for wid in range(model.size.num_worlds):
base_idx = base_body_idx[wid]
np.testing.assert_allclose(
state_n.q_i.numpy()[base_idx],
base_q_0_np[wid],
rtol=rtol,
atol=atol,
)
# Step the solver a few times to change the state
solver._reset()
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset all worlds to the specified base states
solver.reset(
state_out=state_n,
base_q=base_q_0,
base_u=base_u_0,
)
# Check if the assigned base body was correctly reset
for wid in range(model.size.num_worlds):
base_idx = base_body_idx[wid]
np.testing.assert_allclose(
state_n.q_i.numpy()[base_idx],
base_q_0_np[wid],
rtol=rtol,
atol=atol,
)
np.testing.assert_allclose(
state_n.u_i.numpy()[base_idx],
base_u_0_np[wid],
rtol=rtol,
atol=atol,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Step the solver a few times to change the state
solver._reset()
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset selected worlds to the specified base states
solver.reset(
state_out=state_n,
world_mask=world_mask,
base_q=base_q_0,
base_u=base_u_0,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Check if the assigned base body was correctly reset
world_mask_np = world_mask.numpy().copy()
for wid in range(model.size.num_worlds):
if world_mask_np[wid]:
base_idx = base_body_idx[wid]
np.testing.assert_allclose(
state_n.q_i.numpy()[base_idx],
base_q_0_np[wid],
rtol=rtol,
atol=atol,
)
np.testing.assert_allclose(
state_n.u_i.numpy()[base_idx],
base_u_0_np[wid],
rtol=rtol,
atol=atol,
)
def test_08_reset_to_joint_state(self):
"""
Test resetting multiple world solvers to specified joint states.
"""
builder = make_homogeneous_builder(num_worlds=3, build_fn=build_boxes_fourbar, limits=False)
model = builder.finalize(device=self.default_device)
config = SolverKaminoImpl.Config(use_fk_solver=True)
solver = SolverKaminoImpl(model=model, config=config)
# Set a pre-step control callback to apply external forces
# that will sufficiently perturb the system state
solver.set_pre_step_callback(test_prestep_callback)
# Create a state container to hold the output of the reset
# and a world_mask array to specify which worlds to reset
state_p = model.state()
state_n = model.state()
control = model.control()
world_mask = wp.array([1, 0, 1], dtype=int32, device=self.default_device)
# Set default default reset joint coordinates
joint_q_0_np = [0.1, 0.1, 0.1, 0.1]
joint_q_0_np = np.tile(joint_q_0_np, reps=model.size.num_worlds).astype(np.float32)
joint_q_0: wp.array = wp.array(joint_q_0_np, dtype=float32, device=self.default_device)
# Set default default reset joint velocities
joint_u_0_np = [0.1, 0.1, 0.1, 0.1]
joint_u_0_np = np.tile(joint_u_0_np, reps=model.size.num_worlds).astype(np.float32)
joint_u_0: wp.array = wp.array(joint_u_0_np, dtype=float32, device=self.default_device)
# Step the solver a few times to change the state
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset all worlds to the specified joint states
solver.reset(
state_out=state_n,
joint_q=joint_q_0,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Check if the assigned joint states were correctly reset
np.testing.assert_allclose(
state_n.q_j.numpy(),
joint_q_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.q_j_p.numpy(),
joint_q_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j_p` does not match joint_q target\n",
)
self.assertTrue(
np.isfinite(state_n.q_i.numpy()).all(),
msg="\n`state_out.q_i` contains non-finite values (Inf/NaN)\n",
)
self.assertTrue(
np.isfinite(state_n.u_i.numpy()).all(),
msg="\n`state_out.u_i` contains non-finite values (Inf/NaN)\n",
)
# Step the solver a few times to change the state
solver._reset()
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset all worlds to the specified joint states
solver.reset(
state_out=state_n,
joint_q=joint_q_0,
joint_u=joint_u_0,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Check if the assigned joint states were correctly reset
np.testing.assert_allclose(
state_n.q_j.numpy(),
joint_q_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.q_j_p.numpy(),
joint_q_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j_p` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.dq_j.numpy(),
joint_u_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.dq_j` does not match joint_u target\n",
)
self.assertTrue(
np.isfinite(state_n.q_i.numpy()).all(),
msg="\n`state_out.q_i` contains non-finite values (Inf/NaN)\n",
)
self.assertTrue(
np.isfinite(state_n.u_i.numpy()).all(),
msg="\n`state_out.u_i` contains non-finite values (Inf/NaN)\n",
)
# Step the solver a few times to change the state
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset selected worlds to the specified joint states
solver.reset(
state_out=state_n,
world_mask=world_mask,
joint_q=joint_q_0,
joint_u=joint_u_0,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Check if the assigned joint states were correctly reset
num_world_coords = model.size.max_of_num_joint_coords
num_world_dofs = model.size.max_of_num_joint_dofs
coords_start = 0
dofs_start = 0
world_mask_np = world_mask.numpy().copy()
for wid in range(model.size.num_worlds):
if world_mask_np[wid]:
np.testing.assert_allclose(
state_n.q_j.numpy()[coords_start : coords_start + num_world_coords],
joint_q_0_np[coords_start : coords_start + num_world_coords],
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.q_j_p.numpy()[coords_start : coords_start + num_world_coords],
joint_q_0_np[coords_start : coords_start + num_world_coords],
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j_p` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.dq_j.numpy()[dofs_start : dofs_start + num_world_dofs],
joint_u_0_np[dofs_start : dofs_start + num_world_dofs],
rtol=rtol,
atol=atol,
err_msg="\n`state_out.dq_j` does not match joint_u target\n",
)
coords_start += num_world_coords
dofs_start += num_world_dofs
self.assertTrue(
np.isfinite(state_n.q_i.numpy()).all(),
msg="\n`state_out.q_i` contains non-finite values (Inf/NaN)\n",
)
self.assertTrue(
np.isfinite(state_n.u_i.numpy()).all(),
msg="\n`state_out.u_i` contains non-finite values (Inf/NaN)\n",
)
def test_09_reset_to_actuator_state(self):
"""
Test resetting multiple world solvers to specified actuator states.
"""
builder = make_homogeneous_builder(num_worlds=3, build_fn=build_boxes_fourbar, limits=False)
model = builder.finalize(device=self.default_device)
config = SolverKaminoImpl.Config(use_fk_solver=True)
solver = SolverKaminoImpl(model=model, config=config)
# Set a pre-step control callback to apply external forces
# that will sufficiently perturb the system state
solver.set_pre_step_callback(test_prestep_callback)
# Create a state container to hold the output of the reset
# and a world_mask array to specify which worlds to reset
state_p = model.state()
state_n = model.state()
control = model.control()
world_mask = wp.array([1, 0, 1], dtype=int32, device=self.default_device)
# Set default default reset joint coordinates
actuator_q_0_np = [0.25, 0.25]
actuator_q_0_np = np.tile(actuator_q_0_np, reps=model.size.num_worlds)
actuator_q_0: wp.array = wp.array(actuator_q_0_np, dtype=float32, device=self.default_device)
# Set default default reset joint velocities
actuator_u_0_np = [-1.0, -1.0]
actuator_u_0_np = np.tile(actuator_u_0_np, reps=model.size.num_worlds)
actuator_u_0: wp.array = wp.array(actuator_u_0_np, dtype=float32, device=self.default_device)
# Step the solver a few times to change the state
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset all worlds to the specified joint states
solver.reset(
state_out=state_n,
actuator_q=actuator_q_0,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Create expanded actuator state arrays matching the full joint state size
joint_q_0_np = state_n.q_j.numpy().copy()
joint_act_type = model.joints.act_type.numpy().copy()
joint_num_coords = model.joints.num_coords.numpy().copy()
jnt_coords_start = 0
act_coords_start = 0
for j in range(model.size.max_of_num_joints):
nq = joint_num_coords[j]
act_type = joint_act_type[j]
if act_type > JointActuationType.PASSIVE:
joint_q_0_np[jnt_coords_start : jnt_coords_start + nq] = actuator_q_0_np[
act_coords_start : act_coords_start + nq
]
act_coords_start += nq
jnt_coords_start += nq
msg.info("state_n.q_q_j:\n%s\n", state_n.q_j)
msg.info("joint_q_0_np:\n%s\n", joint_q_0_np)
# Check if the assigned joint states were correctly reset
np.testing.assert_allclose(
state_n.q_j.numpy(),
joint_q_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.q_j_p.numpy(),
joint_q_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j_p` does not match joint_q target\n",
)
self.assertTrue(
np.isfinite(state_n.q_i.numpy()).all(),
msg="\n`state_out.q_i` contains non-finite values (Inf/NaN)\n",
)
self.assertTrue(
np.isfinite(state_n.u_i.numpy()).all(),
msg="\n`state_out.u_i` contains non-finite values (Inf/NaN)\n",
)
# Step the solver a few times to change the state
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset all worlds to the specified joint states
solver.reset(
state_out=state_n,
actuator_q=actuator_q_0,
actuator_u=actuator_u_0,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Create expanded actuator state arrays matching the full joint state size
joint_q_0_np = state_n.q_j.numpy().copy()
joint_u_0_np = state_n.dq_j.numpy().copy()
joint_num_dofs = model.joints.num_dofs.numpy().copy()
jnt_coords_start = 0
jnt_dofs_start = 0
act_coord_start = 0
act_dof_start = 0
for j in range(model.size.max_of_num_joints):
nq = joint_num_coords[j]
nu = joint_num_dofs[j]
act_type = joint_act_type[j]
if act_type > JointActuationType.PASSIVE:
joint_q_0_np[jnt_coords_start : jnt_coords_start + nq] = actuator_q_0_np[
act_coord_start : act_coord_start + nq
]
joint_u_0_np[jnt_dofs_start : jnt_dofs_start + nu] = actuator_u_0_np[act_dof_start : act_dof_start + nu]
act_coord_start += nq
act_dof_start += nu
jnt_coords_start += nq
jnt_dofs_start += nu
# Check if the assigned joint states were correctly reset
np.testing.assert_allclose(
state_n.q_j.numpy(),
joint_q_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.q_j_p.numpy(),
joint_q_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j_p` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.dq_j.numpy(),
joint_u_0_np,
rtol=rtol,
atol=atol,
err_msg="\n`state_out.dq_j` does not match joint_u target\n",
)
self.assertTrue(
np.isfinite(state_n.q_i.numpy()).all(),
msg="\n`state_out.q_i` contains non-finite values (Inf/NaN)\n",
)
self.assertTrue(
np.isfinite(state_n.u_i.numpy()).all(),
msg="\n`state_out.u_i` contains non-finite values (Inf/NaN)\n",
)
# Step the solver a few times to change the state
step_solver(
num_steps=11,
solver=solver,
state_p=state_p,
state_n=state_n,
control=control,
show_progress=self.progress or self.verbose,
)
# Reset all worlds to the specified joint states
solver.reset(
state_out=state_n,
world_mask=world_mask,
actuator_q=actuator_q_0,
actuator_u=actuator_u_0,
)
# Optionally print the reset state for debugging
msg.info("state_n.q_i:\n%s\n", state_n.q_i)
msg.info("state_n.u_i:\n%s\n", state_n.u_i)
msg.info("state_n.w_i:\n%s\n", state_n.w_i)
msg.info("state_n.q_j:\n%s\n", state_n.q_j)
msg.info("state_n.q_j_p:\n%s\n", state_n.q_j_p)
msg.info("state_n.dq_j:\n%s\n", state_n.dq_j)
msg.info("state_n.lambda_j:\n%s\n", state_n.lambda_j)
# Create expanded actuator state arrays matching the full joint state size
joint_q_0_np = state_n.q_j.numpy().copy()
joint_u_0_np = state_n.dq_j.numpy().copy()
jnt_coords_start = 0
jnt_dofs_start = 0
act_coord_start = 0
act_dof_start = 0
for j in range(model.size.max_of_num_joints):
nq = joint_num_coords[j]
nu = joint_num_dofs[j]
act_type = joint_act_type[j]
if act_type > JointActuationType.PASSIVE:
joint_q_0_np[jnt_coords_start : jnt_coords_start + nq] = actuator_q_0_np[
act_coord_start : act_coord_start + nq
]
joint_u_0_np[jnt_dofs_start : jnt_dofs_start + nu] = actuator_u_0_np[act_dof_start : act_dof_start + nu]
act_coord_start += nq
act_dof_start += nu
jnt_coords_start += nq
jnt_dofs_start += nu
# Check if the assigned joint states were correctly reset
num_world_coords = model.size.max_of_num_joint_coords
num_world_dofs = model.size.max_of_num_joint_dofs
coords_start = 0
dofs_start = 0
world_mask_np = world_mask.numpy().copy()
for wid in range(model.size.num_worlds):
if world_mask_np[wid]:
np.testing.assert_allclose(
state_n.q_j.numpy()[coords_start : coords_start + num_world_coords],
joint_q_0_np[coords_start : coords_start + num_world_coords],
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.q_j_p.numpy()[coords_start : coords_start + num_world_coords],
joint_q_0_np[coords_start : coords_start + num_world_coords],
rtol=rtol,
atol=atol,
err_msg="\n`state_out.q_j_p` does not match joint_q target\n",
)
np.testing.assert_allclose(
state_n.dq_j.numpy()[dofs_start : dofs_start + num_world_dofs],
joint_u_0_np[dofs_start : dofs_start + num_world_dofs],
rtol=rtol,
atol=atol,
err_msg="\n`state_out.dq_j` does not match joint_u target\n",
)
coords_start += num_world_coords
dofs_start += num_world_dofs
self.assertTrue(
np.isfinite(state_n.q_i.numpy()).all(),
msg="\n`state_out.q_i` contains non-finite values (Inf/NaN)\n",
)
self.assertTrue(
np.isfinite(state_n.u_i.numpy()).all(),
msg="\n`state_out.u_i` contains non-finite values (Inf/NaN)\n",
)
###
# Test Step Operations
###
def test_10_step_multiple_worlds_from_initial_state_without_contacts(self):
"""
Test stepping multiple worlds solvers initialized
uniformly from the default initial state multiple times.
"""
# Create a single-instance system
single_builder = build_boxes_fourbar(ground=False)
for i, body in enumerate(single_builder.bodies):
msg.info(f"[single]: [builder]: body {i}: q_i: {body.q_i_0}")
msg.info(f"[single]: [builder]: body {i}: u_i: {body.u_i_0}")
# Create a model and states from the builder
single_model = single_builder.finalize(device=self.default_device)
single_state_p = single_model.state()
single_state_n = single_model.state()
single_control = single_model.control()
self.assertEqual(single_model.size.sum_of_num_bodies, 4)
self.assertEqual(single_model.size.sum_of_num_joints, 4)
for i, body in enumerate(single_builder.bodies):
np.testing.assert_allclose(single_model.bodies.q_i_0.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_model.bodies.u_i_0.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_state_p.q_i.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_state_p.u_i.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_state_n.q_i.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_state_n.u_i.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[single]: [init]: model.size:\n{single_model.size}\n\n")
msg.info(f"[single]: [init]: single_state_p.q_i:\n{single_state_p.q_i}\n\n")
msg.info(f"[single]: [init]: single_state_p.u_i:\n{single_state_p.u_i}\n\n")
msg.info(f"[single]: [init]: single_state_p.w_i:\n{single_state_p.w_i}\n\n")
msg.info(f"[single]: [init]: single_state_p.q_j:\n{single_state_p.q_j}\n\n")
msg.info(f"[single]: [init]: single_state_p.dq_j:\n{single_state_p.dq_j}\n\n")
msg.info(f"[single]: [init]: single_state_p.lambda_j:\n{single_state_p.lambda_j}\n\n")
# Create simulator and check if the initial state is consistent with the contents of the builder
single_solver = SolverKaminoImpl(model=single_model)
self.assertIsInstance(single_solver, SolverKaminoImpl)
assert_solver_components(self, single_solver)
self.assertIs(single_solver._model, single_model)
# Define the total number of sample steps to collect, and the
# total number of execution steps from which to collect them
num_worlds = 42
num_steps = 1000
# Collect the initial states
initial_q_i = single_state_p.q_i.numpy().copy()
initial_u_i = single_state_p.u_i.numpy().copy()
initial_q_j = single_state_p.q_j.numpy().copy()
initial_dq_j = single_state_p.dq_j.numpy().copy()
msg.info(f"[samples]: [single]: [init]: q_i (shape={initial_q_i.shape}):\n{initial_q_i}\n")
msg.info(f"[samples]: [single]: [init]: u_i (shape={initial_u_i.shape}):\n{initial_u_i}\n")
msg.info(f"[samples]: [single]: [init]: w_i (shape={initial_u_i.shape}):\n{initial_u_i}\n")
msg.info(f"[samples]: [single]: [init]: q_j (shape={initial_q_j.shape}):\n{initial_q_j}\n")
msg.info(f"[samples]: [single]: [init]: dq_j (shape={initial_dq_j.shape}):\n{initial_dq_j}\n")
msg.info(f"[samples]: [single]: [init]: lambda_j (shape={initial_dq_j.shape}):\n{initial_dq_j}\n")
# Set a simple control callback that applies control inputs
# NOTE: We use this to disturb the system from its initial state
single_solver.set_pre_step_callback(test_prestep_callback)
# Run the simulation for the specified number of steps
msg.info(f"[single]: Executing {num_steps} single-world steps")
start_time = time.time()
for step in range(num_steps):
# Execute a single simulation step
single_solver.step(state_in=single_state_p, state_out=single_state_n, control=single_control, dt=0.001)
wp.synchronize()
if self.verbose or self.progress:
print_progress_bar(step + 1, num_steps, start_time, prefix="Progress", suffix="")
# Collect the initial and final states
final_q_i = single_state_n.q_i.numpy().copy()
final_u_i = single_state_n.u_i.numpy().copy()
final_w_i = single_state_n.w_i.numpy().copy()
final_q_j = single_state_n.q_j.numpy().copy()
final_dq_j = single_state_n.dq_j.numpy().copy()
final_lambda_j = single_state_n.lambda_j.numpy().copy()
msg.info(f"[samples]: [single]: [final]: q_i (shape={final_q_i.shape}):\n{final_q_i}\n")
msg.info(f"[samples]: [single]: [final]: u_i (shape={final_u_i.shape}):\n{final_u_i}\n")
msg.info(f"[samples]: [single]: [final]: w_i (shape={final_w_i.shape}):\n{final_w_i}\n")
msg.info(f"[samples]: [single]: [final]: q_j (shape={final_q_j.shape}):\n{final_q_j}\n")
msg.info(f"[samples]: [single]: [final]: dq_j (shape={final_dq_j.shape}):\n{final_dq_j}\n")
msg.info(f"[samples]: [single]: [final]: lambda_j (shape={final_lambda_j.shape}):\n{final_lambda_j}\n")
# Tile the collected states for comparison against the multi-instance simulator
multi_init_q_i = np.tile(initial_q_i, (num_worlds, 1))
multi_init_u_i = np.tile(initial_u_i, (num_worlds, 1))
multi_init_q_j = np.tile(initial_q_j, (num_worlds, 1)).reshape(-1)
multi_init_dq_j = np.tile(initial_dq_j, (num_worlds, 1)).reshape(-1)
multi_final_q_i = np.tile(final_q_i, (num_worlds, 1))
multi_final_u_i = np.tile(final_u_i, (num_worlds, 1))
multi_final_q_j = np.tile(final_q_j, (num_worlds, 1)).reshape(-1)
multi_final_dq_j = np.tile(final_dq_j, (num_worlds, 1)).reshape(-1)
msg.info(f"[samples]: [multi] [init]: q_i (shape={multi_init_q_i.shape}):\n{multi_init_q_i}\n")
msg.info(f"[samples]: [multi] [init]: u_i (shape={multi_init_u_i.shape}):\n{multi_init_u_i}\n")
msg.info(f"[samples]: [multi] [init]: q_j (shape={multi_init_q_j.shape}):\n{multi_init_q_j}\n")
msg.info(f"[samples]: [multi] [init]: dq_j (shape={multi_init_dq_j.shape}):\n{multi_init_dq_j}\n")
msg.info(f"[samples]: [multi] [final]: q_i (shape={multi_final_q_i.shape}):\n{multi_final_q_i}\n")
msg.info(f"[samples]: [multi] [final]: u_i (shape={multi_final_u_i.shape}):\n{multi_final_u_i}\n")
msg.info(f"[samples]: [multi] [final]: q_j (shape={multi_final_q_j.shape}):\n{multi_final_q_j}\n")
msg.info(f"[samples]: [multi] [final]: dq_j (shape={multi_final_dq_j.shape}):\n{multi_final_dq_j}\n")
# Create a multi-instance system by replicating the single-instance builder
multi_builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_fourbar, ground=False)
for i, body in enumerate(multi_builder.bodies):
msg.info(f"[multi]: [builder]: body {i}: bid: {body.bid}")
msg.info(f"[multi]: [builder]: body {i}: q_i: {body.q_i_0}")
msg.info(f"[multi]: [builder]: body {i}: u_i: {body.u_i_0}")
# Create a model and states from the builder
multi_model = multi_builder.finalize(device=self.default_device)
multi_state_p = multi_model.state()
multi_state_n = multi_model.state()
multi_control = multi_model.control()
# Create simulator and check if the initial state is consistent with the contents of the builder
multi_solver = SolverKaminoImpl(model=multi_model)
self.assertEqual(multi_model.size.sum_of_num_bodies, single_model.size.sum_of_num_bodies * num_worlds)
self.assertEqual(multi_model.size.sum_of_num_joints, single_model.size.sum_of_num_joints * num_worlds)
for i, body in enumerate(multi_builder.bodies):
np.testing.assert_allclose(multi_model.bodies.q_i_0.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_model.bodies.u_i_0.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.q_i.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.u_i.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.q_i.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.u_i.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [init]: sim.model.size:\n{multi_model.size}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.q_i:\n{multi_state_p.q_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.u_i:\n{multi_state_p.u_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.q_j:\n{multi_state_p.q_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.dq_j:\n{multi_state_p.dq_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.q_i:\n{multi_state_n.q_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.u_i:\n{multi_state_n.u_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.q_j:\n{multi_state_n.q_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.dq_j:\n{multi_state_n.dq_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.control.tau_j:\n{multi_control.tau_j}\n\n")
# Check if the multi-instance simulator has initial states matching the tiled samples
np.testing.assert_allclose(multi_state_p.q_i.numpy(), multi_init_q_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.u_i.numpy(), multi_init_u_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.q_i.numpy(), multi_init_q_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.u_i.numpy(), multi_init_u_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.q_j.numpy(), multi_init_q_j, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.dq_j.numpy(), multi_init_dq_j, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.q_j.numpy(), multi_init_q_j, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.dq_j.numpy(), multi_init_dq_j, rtol=rtol, atol=atol)
# Set a simple control callback that applies control inputs
# NOTE: We use this to disturb the system from its initial state
multi_solver.set_pre_step_callback(test_prestep_callback)
# Step the multi-instance simulator for the same number of steps
msg.info(f"[multi]: Executing {num_steps} multi-world steps")
start_time = time.time()
for step in range(num_steps):
# Execute a single simulation step
multi_solver.step(state_in=multi_state_p, state_out=multi_state_n, control=multi_control, dt=0.001)
wp.synchronize()
if self.verbose or self.progress:
print_progress_bar(step + 1, num_steps, start_time, prefix="Progress", suffix="")
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [final]: multi_state_n.q_i:\n{multi_state_n.q_i}\n\n")
msg.info(f"[multi]: [final]: multi_state_n.u_i:\n{multi_state_n.u_i}\n\n")
msg.info(f"[multi]: [final]: multi_state_n.q_j:\n{multi_state_n.q_j}\n\n")
msg.info(f"[multi]: [final]: multi_state_n.dq_j:\n{multi_state_n.dq_j}\n\n")
# Check that the next states match the collected samples
np.testing.assert_allclose(multi_state_n.q_i.numpy(), multi_final_q_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.u_i.numpy(), multi_final_u_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.q_j.numpy(), multi_final_q_j, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.dq_j.numpy(), multi_final_dq_j, rtol=rtol, atol=atol)
def test_11_step_multiple_worlds_from_initial_state_with_contacts(self):
"""
Test stepping multiple world solvers initialized
uniformly from the default initial state multiple times.
"""
# Create a single-instance system
single_builder = build_boxes_fourbar(ground=True)
for i, body in enumerate(single_builder.bodies):
msg.info(f"[single]: [builder]: body {i}: q_i: {body.q_i_0}")
msg.info(f"[single]: [builder]: body {i}: u_i: {body.u_i_0}")
# Create a model and states from the builder
single_model = single_builder.finalize(device=self.default_device)
single_state_p = single_model.state()
single_state_n = single_model.state()
single_control = single_model.control()
self.assertEqual(single_model.size.sum_of_num_bodies, 4)
self.assertEqual(single_model.size.sum_of_num_joints, 4)
for i, body in enumerate(single_builder.bodies):
np.testing.assert_allclose(single_model.bodies.q_i_0.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_model.bodies.u_i_0.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_state_p.q_i.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_state_p.u_i.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_state_n.q_i.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(single_state_n.u_i.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[single]: [init]: model.size:\n{single_model.size}\n\n")
msg.info(f"[single]: [init]: single_state_p.q_i:\n{single_state_p.q_i}\n\n")
msg.info(f"[single]: [init]: single_state_p.u_i:\n{single_state_p.u_i}\n\n")
msg.info(f"[single]: [init]: single_state_p.w_i:\n{single_state_p.w_i}\n\n")
msg.info(f"[single]: [init]: single_state_p.q_j:\n{single_state_p.q_j}\n\n")
msg.info(f"[single]: [init]: single_state_p.dq_j:\n{single_state_p.dq_j}\n\n")
msg.info(f"[single]: [init]: single_state_p.lambda_j:\n{single_state_p.lambda_j}\n\n")
# Create a contacts container for the single-instance system
_, single_world_max_contacts = single_builder.compute_required_contact_capacity(max_contacts_per_pair=16)
single_contacts = ContactsKamino(capacity=single_world_max_contacts, device=single_model.device)
# Create simulator and check if the initial state is consistent with the contents of the builder
single_solver = SolverKaminoImpl(model=single_model, contacts=single_contacts)
self.assertIsInstance(single_solver, SolverKaminoImpl)
assert_solver_components(self, single_solver)
self.assertIs(single_solver._model, single_model)
# Define the total number of sample steps to collect, and the
# total number of execution steps from which to collect them
num_worlds = 42
num_steps = 1000
# Collect the initial states
initial_q_i = single_state_p.q_i.numpy().copy()
initial_u_i = single_state_p.u_i.numpy().copy()
initial_q_j = single_state_p.q_j.numpy().copy()
initial_dq_j = single_state_p.dq_j.numpy().copy()
msg.info(f"[samples]: [single]: [init]: q_i (shape={initial_q_i.shape}):\n{initial_q_i}\n")
msg.info(f"[samples]: [single]: [init]: u_i (shape={initial_u_i.shape}):\n{initial_u_i}\n")
msg.info(f"[samples]: [single]: [init]: w_i (shape={initial_u_i.shape}):\n{initial_u_i}\n")
msg.info(f"[samples]: [single]: [init]: q_j (shape={initial_q_j.shape}):\n{initial_q_j}\n")
msg.info(f"[samples]: [single]: [init]: dq_j (shape={initial_dq_j.shape}):\n{initial_dq_j}\n")
msg.info(f"[samples]: [single]: [init]: lambda_j (shape={initial_dq_j.shape}):\n{initial_dq_j}\n")
# Set a simple control callback that applies control inputs
# NOTE: We use this to disturb the system from its initial state
single_solver.set_pre_step_callback(test_prestep_callback)
# Run the simulation for the specified number of steps
msg.info(f"[single]: Executing {num_steps} single-world steps")
start_time = time.time()
for step in range(num_steps):
# Execute a single simulation step
single_solver.step(single_state_p, single_state_n, single_control, contacts=single_contacts, dt=0.001)
wp.synchronize()
if self.verbose or self.progress:
print_progress_bar(step + 1, num_steps, start_time, prefix="Progress", suffix="")
# Collect the initial and final states
final_q_i = single_state_n.q_i.numpy().copy()
final_u_i = single_state_n.u_i.numpy().copy()
final_w_i = single_state_n.w_i.numpy().copy()
final_q_j = single_state_n.q_j.numpy().copy()
final_dq_j = single_state_n.dq_j.numpy().copy()
final_lambda_j = single_state_n.lambda_j.numpy().copy()
msg.info(f"[samples]: [single]: [final]: q_i (shape={final_q_i.shape}):\n{final_q_i}\n")
msg.info(f"[samples]: [single]: [final]: u_i (shape={final_u_i.shape}):\n{final_u_i}\n")
msg.info(f"[samples]: [single]: [final]: w_i (shape={final_w_i.shape}):\n{final_w_i}\n")
msg.info(f"[samples]: [single]: [final]: q_j (shape={final_q_j.shape}):\n{final_q_j}\n")
msg.info(f"[samples]: [single]: [final]: dq_j (shape={final_dq_j.shape}):\n{final_dq_j}\n")
msg.info(f"[samples]: [single]: [final]: lambda_j (shape={final_lambda_j.shape}):\n{final_lambda_j}\n")
# Tile the collected states for comparison against the multi-instance simulator
multi_init_q_i = np.tile(initial_q_i, (num_worlds, 1))
multi_init_u_i = np.tile(initial_u_i, (num_worlds, 1))
multi_init_q_j = np.tile(initial_q_j, (num_worlds, 1)).reshape(-1)
multi_init_dq_j = np.tile(initial_dq_j, (num_worlds, 1)).reshape(-1)
multi_final_q_i = np.tile(final_q_i, (num_worlds, 1))
multi_final_u_i = np.tile(final_u_i, (num_worlds, 1))
multi_final_q_j = np.tile(final_q_j, (num_worlds, 1)).reshape(-1)
multi_final_dq_j = np.tile(final_dq_j, (num_worlds, 1)).reshape(-1)
msg.info(f"[samples]: [multi] [init]: q_i (shape={multi_init_q_i.shape}):\n{multi_init_q_i}\n")
msg.info(f"[samples]: [multi] [init]: u_i (shape={multi_init_u_i.shape}):\n{multi_init_u_i}\n")
msg.info(f"[samples]: [multi] [init]: q_j (shape={multi_init_q_j.shape}):\n{multi_init_q_j}\n")
msg.info(f"[samples]: [multi] [init]: dq_j (shape={multi_init_dq_j.shape}):\n{multi_init_dq_j}\n")
msg.info(f"[samples]: [multi] [final]: q_i (shape={multi_final_q_i.shape}):\n{multi_final_q_i}\n")
msg.info(f"[samples]: [multi] [final]: u_i (shape={multi_final_u_i.shape}):\n{multi_final_u_i}\n")
msg.info(f"[samples]: [multi] [final]: q_j (shape={multi_final_q_j.shape}):\n{multi_final_q_j}\n")
msg.info(f"[samples]: [multi] [final]: dq_j (shape={multi_final_dq_j.shape}):\n{multi_final_dq_j}\n")
# Create a multi-instance system by replicating the single-instance builder
multi_builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_boxes_fourbar, ground=True)
for i, body in enumerate(multi_builder.bodies):
msg.info(f"[multi]: [builder]: body {i}: bid: {body.bid}")
msg.info(f"[multi]: [builder]: body {i}: q_i: {body.q_i_0}")
msg.info(f"[multi]: [builder]: body {i}: u_i: {body.u_i_0}")
# Create a model and states from the builder
multi_model = multi_builder.finalize(device=self.default_device)
multi_state_p = multi_model.state()
multi_state_n = multi_model.state()
multi_control = multi_model.control()
# Create a contacts container for the multi-instance system
_, multi_world_max_contacts = multi_builder.compute_required_contact_capacity(max_contacts_per_pair=16)
multi_contacts = ContactsKamino(capacity=multi_world_max_contacts, device=multi_model.device)
# Create simulator and check if the initial state is consistent with the contents of the builder
multi_solver = SolverKaminoImpl(model=multi_model, contacts=multi_contacts)
self.assertEqual(multi_model.size.sum_of_num_bodies, single_model.size.sum_of_num_bodies * num_worlds)
self.assertEqual(multi_model.size.sum_of_num_joints, single_model.size.sum_of_num_joints * num_worlds)
for i, body in enumerate(multi_builder.bodies):
np.testing.assert_allclose(multi_model.bodies.q_i_0.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_model.bodies.u_i_0.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.q_i.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.u_i.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.q_i.numpy()[i], body.q_i_0, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.u_i.numpy()[i], body.u_i_0, rtol=rtol, atol=atol)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [init]: sim.model.size:\n{multi_model.size}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.q_i:\n{multi_state_p.q_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.u_i:\n{multi_state_p.u_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.q_j:\n{multi_state_p.q_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.dq_j:\n{multi_state_p.dq_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.q_i:\n{multi_state_n.q_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.u_i:\n{multi_state_n.u_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.q_j:\n{multi_state_n.q_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.dq_j:\n{multi_state_n.dq_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.control.tau_j:\n{multi_control.tau_j}\n\n")
# Check if the multi-instance simulator has initial states matching the tiled samples
np.testing.assert_allclose(multi_state_p.q_i.numpy(), multi_init_q_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.u_i.numpy(), multi_init_u_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.q_i.numpy(), multi_init_q_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.u_i.numpy(), multi_init_u_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.q_j.numpy(), multi_init_q_j, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_p.dq_j.numpy(), multi_init_dq_j, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.q_j.numpy(), multi_init_q_j, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.dq_j.numpy(), multi_init_dq_j, rtol=rtol, atol=atol)
# Set a simple control callback that applies control inputs
# NOTE: We use this to disturb the system from its initial state
multi_solver.set_pre_step_callback(test_prestep_callback)
# Step the multi-instance simulator for the same number of steps
msg.info(f"[multi]: Executing {num_steps} multi-world steps")
start_time = time.time()
for step in range(num_steps):
# Execute a single simulation step
multi_solver.step(multi_state_p, multi_state_n, multi_control, contacts=multi_contacts, dt=0.001)
wp.synchronize()
if self.verbose or self.progress:
print_progress_bar(step + 1, num_steps, start_time, prefix="Progress", suffix="")
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [final]: multi_state_n.q_i:\n{multi_state_n.q_i}\n\n")
msg.info(f"[multi]: [final]: multi_state_n.u_i:\n{multi_state_n.u_i}\n\n")
msg.info(f"[multi]: [final]: multi_state_n.q_j:\n{multi_state_n.q_j}\n\n")
msg.info(f"[multi]: [final]: multi_state_n.dq_j:\n{multi_state_n.dq_j}\n\n")
# Check that the next states match the collected samples
np.testing.assert_allclose(multi_state_n.q_i.numpy(), multi_final_q_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.u_i.numpy(), multi_final_u_i, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.q_j.numpy(), multi_final_q_j, rtol=rtol, atol=atol)
np.testing.assert_allclose(multi_state_n.dq_j.numpy(), multi_final_dq_j, rtol=rtol, atol=atol)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_solvers_forward_kinematics.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Unit tests for the ForwardKinematicsSolver class of Kamino, in `solvers/fk.py`.
"""
import hashlib
import unittest
import numpy as np
import warp as wp
import newton
from newton._src.solvers.kamino._src.core.joints import JointActuationType, JointCorrectionMode, JointDoFType
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.core.types import vec6f
from newton._src.solvers.kamino._src.kinematics.joints import compute_joints_data
from newton._src.solvers.kamino._src.solvers.fk import ForwardKinematicsSolver
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.diff_check import diff_check, run_test_single_joint_examples
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# Tests
###
class JacobianCheckForwardKinematics(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.has_cuda = self.default_device.is_cuda
def tearDown(self):
self.default_device = None
def test_Jacobian_check(self):
# Initialize RNG
test_name = "Forward Kinematics Jacobian check"
seed = int(hashlib.sha256(test_name.encode("utf8")).hexdigest(), 16)
rng = np.random.default_rng(seed)
def test_function(model: ModelKamino):
assert model.size.num_worlds == 1 # For simplicity we assume a single world
# Generate (random) body poses
bodies_q_np = rng.uniform(-1.0, 1.0, 7 * model.size.sum_of_num_bodies).astype("float32")
bodies_q = wp.from_numpy(bodies_q_np, dtype=wp.transformf, device=model.device)
# Generate (random) actuated coordinates
actuators_q_np = rng.uniform(-1.0, 1.0, model.size.sum_of_num_actuated_joint_coords).astype("float32")
actuators_q = wp.from_numpy(actuators_q_np, dtype=wp.float32, device=model.device)
# Evaluate analytic Jacobian
solver = ForwardKinematicsSolver(model=model)
pos_control_transforms = solver.eval_position_control_transformations(actuators_q, None)
jacobian = solver.eval_kinematic_constraints_jacobian(bodies_q, pos_control_transforms)
# Check against finite differences Jacobian
def eval_constraints(bodies_q_stepped_np):
bodies_q.assign(bodies_q_stepped_np)
constraints = solver.eval_kinematic_constraints(bodies_q, pos_control_transforms)
bodies_q.assign(bodies_q_np) # Reset state
return constraints.numpy()[0]
return diff_check(
eval_constraints,
jacobian.numpy()[0],
bodies_q_np,
epsilon=1e-4,
tolerance_abs=5e-3,
tolerance_rel=5e-3,
)
success = run_test_single_joint_examples(test_function, test_name, device=self.default_device)
self.assertTrue(success)
def get_actuators_q_quaternion_first_ids(model: ModelKamino):
"""Lists the first index of every unit quaternion 4-segment in the model's actuated coordinates."""
act_types = model.joints.act_type.numpy()
dof_types = model.joints.dof_type.numpy()
num_coords = model.joints.num_coords.numpy()
coord_id = 0
quat_ids = []
for jt_id in range(model.size.sum_of_num_joints):
if act_types[jt_id] == JointActuationType.PASSIVE:
continue
if dof_types[jt_id] == JointDoFType.SPHERICAL:
quat_ids.append(coord_id)
elif dof_types[jt_id] == JointDoFType.FREE:
quat_ids.append(coord_id + 3)
coord_id += num_coords[jt_id]
return quat_ids
def compute_actuated_coords_and_dofs_offsets(model: ModelKamino):
"""
Helper function computing the offsets and sizes needed to extract actuated joint coordinates
and dofs from all joint coordinates/dofs
Returns actuated_coords_offsets, actuated_coords_sizes, actuated_dofs_offsets, actuated_dofs_sizes
"""
# Joints
num_joints = model.info.num_joints.numpy() # Num joints per world
first_joint_id = np.concatenate(([0], num_joints.cumsum())) # First joint id per world
# Joint coordinates
num_coords = model.info.num_joint_coords.numpy() # Num coords per world
first_coord = np.concatenate(([0], num_coords.cumsum())) # First coord id per world
coord_offsets = model.joints.coords_offset.numpy().copy() # First coord id per joint within world
for wd_id in range(model.size.num_worlds): # Convert to first coord id per joint globally
coord_offsets[first_joint_id[wd_id] : first_joint_id[wd_id + 1]] += first_coord[wd_id]
joint_num_coords = model.joints.num_coords.numpy() # Num coords per joint
# Joint dofs
num_dofs = model.info.num_joint_dofs.numpy() # Num dofs per world
first_dof = np.concatenate(([0], num_dofs.cumsum())) # First dof id per world
dof_offsets = model.joints.dofs_offset.numpy().copy() # First dof id per joint within world
for wd_id in range(model.size.num_worlds): # Convert to first dof id per joint globally
dof_offsets[first_joint_id[wd_id] : first_joint_id[wd_id + 1]] += first_dof[wd_id]
joint_num_dofs = model.joints.num_dofs.numpy() # Num dofs per joint
# Filter for actuators only
joint_is_actuator = model.joints.act_type.numpy() == JointActuationType.FORCE
actuated_coord_offsets = coord_offsets[joint_is_actuator]
actuated_coords_sizes = joint_num_coords[joint_is_actuator]
actuated_dof_offsets = dof_offsets[joint_is_actuator]
actuated_dofs_sizes = joint_num_dofs[joint_is_actuator]
return actuated_coord_offsets, actuated_coords_sizes, actuated_dof_offsets, actuated_dofs_sizes
def extract_segments(array, offsets, sizes):
"""
Helper function extracting from a flat array the segments with given offsets and sizes
and returning their concatenation
"""
res = []
for i in range(len(offsets)):
res.extend(array[offsets[i] : offsets[i] + sizes[i]])
return np.array(res)
def compute_constraint_residual_mask(model: ModelKamino):
"""
Computes a boolean mask for constraint residuals, True for most constraints but False
for base joints (to filter out residuals for fixed base models if the base is reset
to a different pose) and passive universal joints (residual implementation is currently flawed)
"""
# Precompute constraint offsets
num_joints = model.info.num_joints.numpy() # Num joints per world
first_joint_id = np.concatenate(([0], num_joints.cumsum())) # First joint id per world
num_cts = model.info.num_joint_cts.numpy() # Num joint cts per world
first_ct_id = np.concatenate(([0], num_cts.cumsum())) # First joint ct id per world
first_joint_ct_id = model.joints.cts_offset.numpy().copy() # First ct id per joint within world
for wd_id in range(model.size.num_worlds): # Convert to first ct id per joint globally
first_joint_ct_id[first_joint_id[wd_id] : first_joint_id[wd_id + 1]] += first_ct_id[wd_id]
num_joint_cts = model.joints.num_cts.numpy() # Num cts per joint
mask = np.array(model.size.sum_of_num_joint_cts * [True])
# Exclude base joints
base_joint_index = model.info.base_joint_index.numpy().tolist()
for wd_id in range(model.size.num_worlds):
if base_joint_index[wd_id] < 0:
continue
base_jt_id = base_joint_index[wd_id]
ct_offset = first_joint_ct_id[base_jt_id]
mask[ct_offset : ct_offset + num_joint_cts[base_jt_id]] = False
# Exclude passive universal joints
act_types = model.joints.act_type.numpy()
dof_types = model.joints.dof_type.numpy()
for jt_id in range(model.size.sum_of_num_joints):
if act_types[jt_id] != JointActuationType.PASSIVE or dof_types[jt_id] != JointDoFType.UNIVERSAL:
continue
ct_offset = first_joint_ct_id[jt_id]
mask[ct_offset : ct_offset + num_joint_cts[jt_id]] = False
return mask
def generate_random_inputs_q(
model: ModelKamino,
num_poses: int,
max_base_q: np.ndarray,
max_actuators_q: np.ndarray,
rng: np.random._generator.Generator,
unit_quaternions=True,
):
# Check dimensions
base_q_size = 7 * model.size.num_worlds
actuators_q_size = model.size.sum_of_num_actuated_joint_dofs
assert len(max_base_q) == base_q_size
assert len(max_actuators_q) == actuators_q_size
# Generate (random) base_q, actuators_q
base_q_np = np.zeros((num_poses, base_q_size))
for i in range(base_q_size):
base_q_np[:, i] = rng.uniform(-max_base_q[i], max_base_q[i], num_poses)
actuators_q_np = np.zeros((num_poses, actuators_q_size))
for i in range(actuators_q_size):
actuators_q_np[:, i] = rng.uniform(-max_actuators_q[i], max_actuators_q[i], num_poses)
# Normalize quaternions in base_q, actuators_q
if unit_quaternions:
for i in range(model.size.num_worlds):
base_q_np[:, 7 * i + 3 : 7 * i + 7] /= np.linalg.norm(base_q_np[:, 7 * i + 3 : 7 * i + 7], axis=1)[:, None]
quat_ids = get_actuators_q_quaternion_first_ids(model)
for i in quat_ids:
actuators_q_np[:, i : i + 4] /= np.linalg.norm(actuators_q_np[:, i : i + 4], axis=1)[:, None]
return base_q_np, actuators_q_np
def generate_random_inputs_u(
model: ModelKamino,
num_poses: int,
max_base_u: np.ndarray,
max_actuators_u: np.ndarray,
rng: np.random._generator.Generator,
):
# Check dimensions
base_u_size = 6 * model.size.num_worlds
actuators_u_size = model.size.sum_of_num_actuated_joint_dofs
assert len(max_base_u) == base_u_size
assert len(max_actuators_u) == actuators_u_size
# Generate (random) base_u, actuators_u
base_u_np = np.zeros((num_poses, base_u_size))
for i in range(base_u_size):
base_u_np[:, i] = rng.uniform(-max_base_u[i], max_base_u[i], num_poses)
actuators_u_np = np.zeros((num_poses, actuators_u_size))
for i in range(actuators_u_size):
actuators_u_np[:, i] = rng.uniform(-max_actuators_u[i], max_actuators_u[i], num_poses)
return base_u_np, actuators_u_np
def generate_random_poses(
model: ModelKamino,
num_poses: int,
max_bodies_q: np.ndarray,
rng: np.random._generator.Generator,
unit_quaternions=True,
):
# Check dimensions
bodies_q_size = 7 * model.size.sum_of_num_bodies
assert len(max_bodies_q) == bodies_q_size
# Generate (random) bodies_q
bodies_q_np = np.zeros((num_poses, bodies_q_size))
for i in range(bodies_q_size):
bodies_q_np[:, i] = rng.uniform(-max_bodies_q[i], max_bodies_q[i], num_poses)
# Normalize quaternions in bodies_q
if unit_quaternions:
for i in range(model.size.num_worlds):
bodies_q_np[:, 7 * i + 3 : 7 * i + 7] /= np.linalg.norm(bodies_q_np[:, 7 * i + 3 : 7 * i + 7], axis=1)[
:, None
]
return bodies_q_np
def simulate_random_poses(
model: ModelKamino,
num_poses: int,
max_base_q: np.ndarray,
max_actuators_q: np.ndarray,
max_base_u: np.ndarray,
max_actuators_u: np.ndarray,
rng: np.random._generator.Generator,
use_graph: bool = False,
verbose: bool = False,
):
# Generate random inputs
base_q_np, actuators_q_np = generate_random_inputs_q(model, num_poses, max_base_q, max_actuators_q, rng)
base_u_np, actuators_u_np = generate_random_inputs_u(model, num_poses, max_base_u, max_actuators_u, rng)
# Precompute offset arrays for extracting actuator coordinates/dofs
actuated_coord_offsets, actuated_coords_sizes, actuated_dof_offsets, actuated_dofs_sizes = (
compute_actuated_coords_and_dofs_offsets(model)
)
# Precompute boolean mask for extracting relevant constraint residuals
residual_mask = compute_constraint_residual_mask(model)
# Run forward kinematics on all random poses
config = ForwardKinematicsSolver.Config()
config.reset_state = True
config.use_sparsity = False # Change for sparse/dense solver
config.preconditioner = "jacobi_block_diagonal" # Change to test preconditioners
solver = ForwardKinematicsSolver(model, config)
success_flags = []
with wp.ScopedDevice(model.device):
bodies_q = wp.array(shape=(model.size.sum_of_num_bodies), dtype=wp.transformf)
base_q = wp.array(shape=(model.size.num_worlds), dtype=wp.transformf)
actuators_q = wp.array(shape=(actuators_q_np.shape[1]), dtype=wp.float32)
bodies_u = wp.array(shape=(model.size.sum_of_num_bodies), dtype=vec6f)
base_u = wp.array(shape=(model.size.num_worlds), dtype=vec6f)
actuators_u = wp.array(shape=(actuators_u_np.shape[1]), dtype=wp.float32)
data = model.data(device=model.device)
epsilon = 1e-2
for pose_id in range(num_poses):
# Run FK solve and check convergence
base_q.assign(base_q_np[pose_id])
actuators_q.assign(actuators_q_np[pose_id])
base_u.assign(base_u_np[pose_id])
actuators_u.assign(actuators_u_np[pose_id])
status = solver.solve_fk(
actuators_q,
bodies_q,
base_q=base_q,
base_u=base_u,
actuators_u=actuators_u,
bodies_u=bodies_u,
use_graph=use_graph,
verbose=verbose,
return_status=True,
)
if status.success.min() < 1:
success_flags.append(False)
continue
else:
success_flags.append(True)
# Update joints data from body states for validation
wp.copy(data.bodies.q_i, bodies_q)
wp.copy(data.bodies.u_i, bodies_u)
compute_joints_data(model=model, data=data, q_j_p=model.joints.q_j_0, correction=JointCorrectionMode.CONTINUOUS)
# Validate positions computation
residual_ct_pos = np.max(np.abs(data.joints.r_j.numpy()[residual_mask]))
if residual_ct_pos > epsilon:
print(f"Large constraint residual ({residual_ct_pos}) for pose {pose_id}")
success_flags[-1] = False
actuators_q_check = extract_segments(data.joints.q_j.numpy(), actuated_coord_offsets, actuated_coords_sizes)
residual_actuators_q = np.max(np.abs(actuators_q_check - actuators_q_np[pose_id]))
if residual_actuators_q > epsilon:
print(f"Large error on prescribed actuator coordinates ({residual_actuators_q}) for pose {pose_id}")
success_flags[-1] = False
# Validate velocities computation
residual_ct_vel = np.max(np.abs(data.joints.dr_j.numpy()[residual_mask]))
if residual_ct_vel > epsilon:
print(f"Large constraint velocity residual ({residual_ct_vel}) for pose {pose_id}")
success_flags[-1] = False
actuators_u_check = extract_segments(data.joints.dq_j.numpy(), actuated_dof_offsets, actuated_dofs_sizes)
residual_actuators_u = np.max(np.abs(actuators_u_check - actuators_u_np[pose_id]))
if residual_actuators_u > epsilon:
print(f"Large error on prescribed actuator velocities ({residual_actuators_u}) for pose {pose_id}")
success_flags[-1] = False
success = np.sum(success_flags) == num_poses
if not success:
print(f"Random poses simulation & validation failed, {np.sum(success_flags)}/{num_poses} poses successful")
return success
class DRTestMechanismRandomPosesCheckForwardKinematics(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.has_cuda = self.default_device.is_cuda
self.verbose = test_context.verbose
def tearDown(self):
self.default_device = None
def test_mechanism_FK_random_poses(self):
# Initialize RNG
test_name = "Test mechanism FK random poses check"
seed = int(hashlib.sha256(test_name.encode("utf8")).hexdigest(), 16)
rng = np.random.default_rng(seed)
# Load the DR TestMech model from the `newton-assets` repository
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_testmech" / "usd" / "dr_testmech.usda")
# Load model
builder = USDImporter().import_from(asset_file)
builder.set_base_joint("base")
model = builder.finalize(device=self.default_device, requires_grad=False)
# Simulate random poses
num_poses = 30
base_q_max = np.array(3 * [0.2] + 4 * [1.0])
actuators_q_max = np.radians([95.0])
base_u_max = np.array(3 * [0.1] + 3 * [0.5])
actuators_u_max = np.array([0.5])
success = simulate_random_poses(
model,
num_poses,
base_q_max,
actuators_q_max,
base_u_max,
actuators_u_max,
rng,
self.has_cuda,
self.verbose,
)
self.assertTrue(success)
class DRLegsRandomPosesCheckForwardKinematics(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.has_cuda = self.default_device.is_cuda
self.verbose = test_context.verbose
def tearDown(self):
self.default_device = None
def test_dr_legs_FK_random_poses(self):
# Initialize RNG
test_name = "FK random poses check for dr_legs model"
seed = int(hashlib.sha256(test_name.encode("utf8")).hexdigest(), 16)
rng = np.random.default_rng(seed)
# Load the DR TestMech and DR Legs models from the `newton-assets` repository
asset_path = newton.utils.download_asset("disneyresearch")
asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_boxes.usda")
builder = USDImporter().import_from(asset_file)
builder.set_base_body("pelvis")
model = builder.finalize(device=self.default_device, requires_grad=False)
# Simulate random poses
num_poses = 30
theta_max = np.radians(10.0) # Angles too far from the initial pose lead to singularities
base_q_max = np.array(3 * [0.2] + 4 * [1.0])
actuators_q_max = np.array(model.size.sum_of_num_actuated_joint_coords * [theta_max])
base_u_max = np.array(3 * [0.5] + 3 * [0.5])
actuators_u_max = np.array(model.size.sum_of_num_actuated_joint_dofs * [0.5])
success = simulate_random_poses(
model,
num_poses,
base_q_max,
actuators_q_max,
base_u_max,
actuators_u_max,
rng,
self.has_cuda,
self.verbose,
)
self.assertTrue(success)
class HeterogenousModelRandomPosesCheckForwardKinematics(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.has_cuda = self.default_device.is_cuda
self.verbose = test_context.verbose
def tearDown(self):
self.default_device = None
def test_heterogenous_model_FK_random_poses(self):
# Initialize RNG
test_name = "Heterogeneous model (test mechanism + dr_legs) FK random poses check"
seed = int(hashlib.sha256(test_name.encode("utf8")).hexdigest(), 16)
rng = np.random.default_rng(seed)
# Load the DR TestMech and DR Legs models from the `newton-assets` repository
asset_path = newton.utils.download_asset("disneyresearch")
asset_file_0 = str(asset_path / "dr_testmech" / "usd" / "dr_testmech.usda")
asset_file_1 = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_boxes.usda")
builder = USDImporter().import_from(asset_file_0)
builder.set_base_joint("base")
builder1 = USDImporter().import_from(asset_file_1)
builder1.set_base_body("pelvis")
builder.add_builder(builder1)
model = builder.finalize(device=self.default_device, requires_grad=False)
# Simulate random poses
num_poses = 30
theta_max_test_mech = np.radians(100.0)
theta_max_dr_legs = np.radians(10.0)
base_q_max = np.array(3 * [0.2] + 4 * [1.0] + 3 * [0.2] + 4 * [1.0])
actuators_q_max = np.array([theta_max_test_mech] + builder1.num_actuated_joint_coords * [theta_max_dr_legs])
base_u_max = np.array(3 * [0.1] + 3 * [0.5] + 3 * [0.5] + 3 * [0.5])
actuators_u_max = np.array(model.size.sum_of_num_actuated_joint_dofs * [0.5])
success = simulate_random_poses(
model, num_poses, base_q_max, actuators_q_max, base_u_max, actuators_u_max, rng, self.has_cuda, self.verbose
)
self.assertTrue(success)
class HeterogenousModelSparseJacobianAssemblyCheck(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.has_cuda = self.default_device.is_cuda
self.verbose = test_context.verbose
def tearDown(self):
self.default_device = None
def test_heterogenous_model_FK_random_poses(self):
# Initialize RNG
test_name = "Heterogeneous model (test mechanism + dr_legs) sparse Jacobian assembly check"
seed = int(hashlib.sha256(test_name.encode("utf8")).hexdigest(), 16)
rng = np.random.default_rng(seed)
# Load the DR TestMech and DR Legs models from the `newton-assets` repository
asset_path = newton.utils.download_asset("disneyresearch")
asset_file_0 = str(asset_path / "dr_testmech" / "usd" / "dr_testmech.usda")
asset_file_1 = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_boxes.usda")
builder = USDImporter().import_from(asset_file_0)
builder.set_base_joint("base")
builder1 = USDImporter().import_from(asset_file_1)
builder1.set_base_body("pelvis")
builder.add_builder(builder1)
model = builder.finalize(device=self.default_device, requires_grad=False)
# Generate random poses
num_poses = 30
bodies_q_max = np.array(model.size.sum_of_num_bodies * [0.2, 0.2, 0.2, 1.0, 1.0, 1.0, 1.0])
theta_max_test_mech = np.radians(100.0)
theta_max_dr_legs = np.radians(10.0)
base_q_max = np.array(3 * [0.2] + 4 * [1.0] + 3 * [0.2] + 4 * [1.0])
actuators_q_max = np.array([theta_max_test_mech] + builder1.num_actuated_joint_coords * [theta_max_dr_legs])
bodies_q_np = generate_random_poses(model, num_poses, bodies_q_max, rng, False)
base_q_np, actuators_q_np = generate_random_inputs_q(model, num_poses, base_q_max, actuators_q_max, rng)
# Assemble and compare dense and sparse Jacobian for each pose
solver = ForwardKinematicsSolver(model, config=ForwardKinematicsSolver.Config(use_sparsity=True))
with wp.ScopedDevice(model.device):
bodies_q = wp.array(shape=(model.size.sum_of_num_bodies), dtype=wp.transformf)
base_q = wp.array(shape=(model.size.num_worlds), dtype=wp.transformf)
actuators_q = wp.array(shape=(actuators_q_np.shape[1]), dtype=wp.float32)
dims = solver.sparse_jacobian.dims.numpy()
for pose_id in range(num_poses):
bodies_q.assign(bodies_q_np[pose_id])
base_q.assign(base_q_np[pose_id])
actuators_q.assign(actuators_q_np[pose_id])
transforms = solver.eval_position_control_transformations(actuators_q, base_q)
jac_dense_np = solver.eval_kinematic_constraints_jacobian(bodies_q, transforms).numpy()
solver.assemble_sparse_jacobian(bodies_q, transforms)
jac_sparse_np = solver.sparse_jacobian.numpy()
for wd_id in range(model.size.num_worlds):
rows, cols = int(dims[wd_id][0]), int(dims[wd_id][1])
residual = jac_dense_np[wd_id, :rows, :cols] - jac_sparse_np[wd_id]
self.assertTrue(np.max(np.abs(residual)) < 1e-10)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_solvers_metrics.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for `solvers/metrics.py`."""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.integrators.euler import integrate_euler_semi_implicit
from newton._src.solvers.kamino._src.models.builders.basics import build_box_on_plane, build_boxes_hinged
from newton._src.solvers.kamino._src.solvers.metrics import SolutionMetrics
from newton._src.solvers.kamino._src.solvers.padmm import PADMMSolver
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.test_solvers_padmm import TestSetup
###
# Tests
###
class TestSolverMetrics(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for detailed output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default(self):
"""
Test creating a SolutionMetrics instance with default initialization.
"""
# Creating a default solver metrics evaluator without any model
# should result in an instance without any memory allocation.
metrics = SolutionMetrics()
self.assertIsNone(metrics._device)
self.assertIsNone(metrics._data)
self.assertIsNone(metrics._buffer_s)
self.assertIsNone(metrics._buffer_v)
# Requesting the solver data container when the
# solver has not been finalized should raise an
# error since no allocations have been made.
self.assertRaises(RuntimeError, lambda: metrics.data)
def test_01_finalize_default(self):
"""
Test creating a SolutionMetrics instance with default initialization and then finalizing all memory allocations.
"""
# Create a test setup
test = TestSetup(builder_fn=build_box_on_plane, max_world_contacts=8, device=self.default_device)
# Creating a default solver metrics evaluator without any model
# should result in an instance without any memory allocation.
metrics = SolutionMetrics()
# Finalize the solver with a model
metrics.finalize(test.model)
# Check that the solver has been properly allocated
self.assertIsNotNone(metrics._data)
self.assertIsNotNone(metrics._device)
self.assertIs(metrics._device, test.model.device)
self.assertIsNotNone(metrics._buffer_s)
self.assertIsNotNone(metrics._buffer_v)
# Check allocation sizes
msg.info("num_worlds: %s", test.model.size.num_worlds)
msg.info("sum_of_max_total_cts: %s", test.model.size.sum_of_max_total_cts)
msg.info("buffer_s size: %s", metrics._buffer_s.size)
msg.info("buffer_v size: %s", metrics._buffer_v.size)
self.assertEqual(metrics._buffer_s.size, test.model.size.sum_of_max_total_cts)
self.assertEqual(metrics._buffer_v.size, test.model.size.sum_of_max_total_cts)
self.assertEqual(metrics.data.r_eom.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_eom_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_kinematics.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_kinematics_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_cts_joints.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_cts_joints_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_cts_limits.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_cts_limits_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_cts_contacts.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_cts_contacts_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_v_plus.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_v_plus_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_ncp_primal.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_ncp_primal_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_ncp_dual.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_ncp_dual_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_ncp_compl.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_ncp_compl_argmax.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_vi_natmap.size, test.model.size.num_worlds)
self.assertEqual(metrics.data.r_vi_natmap_argmax.size, test.model.size.num_worlds)
def test_02_evaluate_trivial_solution(self):
"""
Tests evaluating metrics on an all-zeros trivial solution.
"""
# Create the test problem
test = TestSetup(
builder_fn=build_box_on_plane,
max_world_contacts=4,
gravity=False,
perturb=False,
device=self.default_device,
)
# Creating a default solver metrics evaluator from the test model
metrics = SolutionMetrics(model=test.model)
# Define a trivial solution (all zeros)
with wp.ScopedDevice(test.model.device):
sigma = wp.zeros(test.model.size.num_worlds, dtype=wp.vec2f)
lambdas = wp.zeros(test.model.size.sum_of_max_total_cts, dtype=wp.float32)
v_plus = wp.zeros(test.model.size.sum_of_max_total_cts, dtype=wp.float32)
# Build the test problem and integrate the state over a single time-step
test.build()
integrate_euler_semi_implicit(model=test.model, data=test.data)
nl = test.limits.model_active_limits.numpy()[0] if test.limits.model_max_limits_host > 0 else 0
nc = test.contacts.model_active_contacts.numpy()[0] if test.contacts.model_max_contacts_host > 0 else 0
msg.info("num active limits: %s", nl)
msg.info("num active contacts: %s\n", nc)
self.assertEqual(nl, 0)
self.assertEqual(nc, 4)
# Compute the metrics on the trivial solution
metrics.reset()
metrics.evaluate(
sigma=sigma,
lambdas=lambdas,
v_plus=v_plus,
model=test.model,
data=test.data,
state_p=test.state_p,
problem=test.problem,
jacobians=test.jacobians,
limits=test.limits,
contacts=test.contacts,
)
# Optional verbose output
msg.info("metrics.r_eom: %s", metrics.data.r_eom)
msg.info("metrics.r_kinematics: %s", metrics.data.r_kinematics)
msg.info("metrics.r_cts_joints: %s", metrics.data.r_cts_joints)
msg.info("metrics.r_cts_limits: %s", metrics.data.r_cts_limits)
msg.info("metrics.r_cts_contacts: %s", metrics.data.r_cts_contacts)
msg.info("metrics.r_v_plus: %s", metrics.data.r_v_plus)
msg.info("metrics.r_ncp_primal: %s", metrics.data.r_ncp_primal)
msg.info("metrics.r_ncp_dual: %s", metrics.data.r_ncp_dual)
msg.info("metrics.r_ncp_compl: %s", metrics.data.r_ncp_compl)
msg.info("metrics.r_vi_natmap: %s\n", metrics.data.r_vi_natmap)
# Extract the maximum contact penetration to use for validation
nc = test.contacts.model_active_contacts.numpy()[0]
max_contact_penetration = 0.0
for cid in range(nc):
pen = test.contacts.gapfunc.numpy()[cid][3]
max_contact_penetration = max(max_contact_penetration, pen)
# Check that all metrics are zero
np.testing.assert_allclose(metrics.data.r_eom.numpy()[0], 0.0)
np.testing.assert_allclose(metrics.data.r_kinematics.numpy()[0], 0.0)
np.testing.assert_allclose(metrics.data.r_cts_joints.numpy()[0], 0.0)
np.testing.assert_allclose(metrics.data.r_cts_limits.numpy()[0], 0.0)
np.testing.assert_allclose(metrics.data.r_cts_contacts.numpy()[0], max_contact_penetration)
np.testing.assert_allclose(metrics.data.r_ncp_primal.numpy()[0], 0.0)
np.testing.assert_allclose(metrics.data.r_ncp_dual.numpy()[0], 0.0)
np.testing.assert_allclose(metrics.data.r_ncp_compl.numpy()[0], 0.0)
np.testing.assert_allclose(metrics.data.r_vi_natmap.numpy()[0], 0.0)
# Optional verbose output
msg.info("metrics.r_eom_argmax: %s", metrics.data.r_eom_argmax)
msg.info("metrics.r_kinematics_argmax: %s", metrics.data.r_kinematics_argmax)
msg.info("metrics.r_cts_joints_argmax: %s", metrics.data.r_cts_joints_argmax)
msg.info("metrics.r_cts_limits_argmax: %s", metrics.data.r_cts_limits_argmax)
msg.info("metrics.r_cts_contacts_argmax: %s", metrics.data.r_cts_contacts_argmax)
msg.info("metrics.r_v_plus_argmax: %s", metrics.data.r_v_plus_argmax)
msg.info("metrics.r_ncp_primal_argmax: %s", metrics.data.r_ncp_primal_argmax)
msg.info("metrics.r_ncp_dual_argmax: %s", metrics.data.r_ncp_dual_argmax)
msg.info("metrics.r_ncp_compl_argmax: %s", metrics.data.r_ncp_compl_argmax)
msg.info("metrics.r_vi_natmap_argmax: %s\n", metrics.data.r_vi_natmap_argmax)
# Check that all argmax indices are correct
np.testing.assert_allclose(metrics.data.r_eom_argmax.numpy()[0], 0) # only one body
np.testing.assert_allclose(metrics.data.r_kinematics_argmax.numpy()[0], -1) # no joints
np.testing.assert_allclose(metrics.data.r_cts_joints_argmax.numpy()[0], -1) # no joints
np.testing.assert_allclose(metrics.data.r_cts_limits_argmax.numpy()[0], -1) # no limits
# NOTE: all contacts will have the same residual,
# so the argmax will evaluate to the last constraint
np.testing.assert_allclose(metrics.data.r_v_plus_argmax.numpy()[0], 11)
# NOTE: all contacts will have the same penetration,
# so the argmax will evaluate to the last contact
np.testing.assert_allclose(metrics.data.r_cts_contacts_argmax.numpy()[0], 3)
np.testing.assert_allclose(metrics.data.r_ncp_primal_argmax.numpy()[0], 3)
np.testing.assert_allclose(metrics.data.r_ncp_dual_argmax.numpy()[0], 3)
np.testing.assert_allclose(metrics.data.r_ncp_compl_argmax.numpy()[0], 3)
np.testing.assert_allclose(metrics.data.r_vi_natmap_argmax.numpy()[0], 3)
def test_03_evaluate_padmm_solution_box_on_plane(self):
"""
Tests evaluating metrics on a solution computed with the Proximal-ADMM (PADMM) solver.
"""
# Create the test problem
test = TestSetup(
builder_fn=build_box_on_plane,
max_world_contacts=4,
gravity=True,
perturb=True,
device=self.default_device,
)
# Create the PADMM solver
solver = PADMMSolver(model=test.model, use_acceleration=False, collect_info=True)
# Creating a default solver metrics evaluator from the test model
metrics = SolutionMetrics(model=test.model)
# Solve the test problem
test.build()
solver.reset()
solver.coldstart()
solver.solve(problem=test.problem)
integrate_euler_semi_implicit(model=test.model, data=test.data)
# Compute the metrics on the trivial solution
metrics.reset()
metrics.evaluate(
sigma=solver.data.state.sigma,
lambdas=solver.data.solution.lambdas,
v_plus=solver.data.solution.v_plus,
model=test.model,
data=test.data,
state_p=test.state_p,
problem=test.problem,
jacobians=test.jacobians,
limits=test.limits,
contacts=test.contacts,
)
nl = test.limits.model_active_limits.numpy()[0] if test.limits.model_max_limits_host > 0 else 0
nc = test.contacts.model_active_contacts.numpy()[0] if test.contacts.model_max_contacts_host > 0 else 0
msg.info("num active limits: %s", nl)
msg.info("num active contacts: %s\n", nc)
# Optional verbose output
msg.info("metrics.r_eom: %s", metrics.data.r_eom)
msg.info("metrics.r_kinematics: %s", metrics.data.r_kinematics)
msg.info("metrics.r_cts_joints: %s", metrics.data.r_cts_joints)
msg.info("metrics.r_cts_limits: %s", metrics.data.r_cts_limits)
msg.info("metrics.r_cts_contacts: %s", metrics.data.r_cts_contacts)
msg.info("metrics.r_v_plus: %s", metrics.data.r_v_plus)
msg.info("metrics.r_ncp_primal: %s", metrics.data.r_ncp_primal)
msg.info("metrics.r_ncp_dual: %s", metrics.data.r_ncp_dual)
msg.info("metrics.r_ncp_compl: %s", metrics.data.r_ncp_compl)
msg.info("metrics.r_vi_natmap: %s\n", metrics.data.r_vi_natmap)
# Extract the maximum contact penetration to use for validation
nc = test.contacts.model_active_contacts.numpy()[0]
max_contact_penetration = 0.0
for cid in range(nc):
pen = test.contacts.gapfunc.numpy()[cid][3]
max_contact_penetration = max(max_contact_penetration, pen)
# Check that all metrics are zero
accuracy = 5 # number of decimal places for accuracy
self.assertAlmostEqual(metrics.data.r_eom.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_kinematics.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_cts_joints.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_cts_limits.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_cts_contacts.numpy()[0], max_contact_penetration, places=accuracy)
self.assertAlmostEqual(metrics.data.r_ncp_primal.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_ncp_dual.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_ncp_compl.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_vi_natmap.numpy()[0], 0.0, places=accuracy)
# Optional verbose output
msg.info("metrics.r_eom_argmax: %s", metrics.data.r_eom_argmax)
msg.info("metrics.r_kinematics_argmax: %s", metrics.data.r_kinematics_argmax)
msg.info("metrics.r_cts_joints_argmax: %s", metrics.data.r_cts_joints_argmax)
msg.info("metrics.r_cts_limits_argmax: %s", metrics.data.r_cts_limits_argmax)
msg.info("metrics.r_cts_contacts_argmax: %s", metrics.data.r_cts_contacts_argmax)
msg.info("metrics.r_v_plus_argmax: %s", metrics.data.r_v_plus_argmax)
msg.info("metrics.r_ncp_primal_argmax: %s", metrics.data.r_ncp_primal_argmax)
msg.info("metrics.r_ncp_dual_argmax: %s", metrics.data.r_ncp_dual_argmax)
msg.info("metrics.r_ncp_compl_argmax: %s", metrics.data.r_ncp_compl_argmax)
msg.info("metrics.r_vi_natmap_argmax: %s\n", metrics.data.r_vi_natmap_argmax)
def test_04_evaluate_padmm_solution_boxes_hinged(self):
"""
Tests evaluating metrics on a solution computed with the Proximal-ADMM (PADMM) solver.
"""
# Create the test problem
test = TestSetup(
builder_fn=build_boxes_hinged,
max_world_contacts=8,
gravity=True,
perturb=True,
device=self.default_device,
)
# Create the PADMM solver
solver = PADMMSolver(model=test.model, use_acceleration=False, collect_info=True)
# Creating a default solver metrics evaluator from the test model
metrics = SolutionMetrics(model=test.model)
# Solve the test problem
test.build()
solver.reset()
solver.coldstart()
solver.solve(problem=test.problem)
integrate_euler_semi_implicit(model=test.model, data=test.data)
# Compute the metrics on the trivial solution
metrics.evaluate(
sigma=solver.data.state.sigma,
lambdas=solver.data.solution.lambdas,
v_plus=solver.data.solution.v_plus,
model=test.model,
data=test.data,
state_p=test.state_p,
problem=test.problem,
jacobians=test.jacobians,
limits=test.limits,
contacts=test.contacts,
)
nl = test.limits.model_active_limits.numpy()[0] if test.limits.model_max_limits_host > 0 else 0
nc = test.contacts.model_active_contacts.numpy()[0] if test.contacts.model_max_contacts_host > 0 else 0
msg.info("num active limits: %s", nl)
msg.info("num active contacts: %s\n", nc)
# Optional verbose output
msg.info("metrics.r_eom: %s", metrics.data.r_eom)
msg.info("metrics.r_kinematics: %s", metrics.data.r_kinematics)
msg.info("metrics.r_cts_joints: %s", metrics.data.r_cts_joints)
msg.info("metrics.r_cts_limits: %s", metrics.data.r_cts_limits)
msg.info("metrics.r_cts_contacts: %s", metrics.data.r_cts_contacts)
msg.info("metrics.r_v_plus: %s", metrics.data.r_v_plus)
msg.info("metrics.r_ncp_primal: %s", metrics.data.r_ncp_primal)
msg.info("metrics.r_ncp_dual: %s", metrics.data.r_ncp_dual)
msg.info("metrics.r_ncp_compl: %s", metrics.data.r_ncp_compl)
msg.info("metrics.r_vi_natmap: %s\n", metrics.data.r_vi_natmap)
# Extract the maximum contact penetration to use for validation
max_contact_penetration = 0.0
for cid in range(nc):
pen = test.contacts.gapfunc.numpy()[cid][3]
max_contact_penetration = max(max_contact_penetration, pen)
# Check that all metrics are zero
accuracy = 5 # number of decimal places for accuracy
self.assertAlmostEqual(metrics.data.r_eom.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_kinematics.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_cts_joints.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_cts_limits.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_cts_contacts.numpy()[0], max_contact_penetration, places=accuracy)
self.assertAlmostEqual(metrics.data.r_ncp_primal.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_ncp_dual.numpy()[0], 0.0, places=4) # less accurate, but still correct
self.assertAlmostEqual(metrics.data.r_ncp_compl.numpy()[0], 0.0, places=accuracy)
self.assertAlmostEqual(metrics.data.r_vi_natmap.numpy()[0], 0.0, places=accuracy)
# Optional verbose output
msg.info("metrics.r_eom_argmax: %s", metrics.data.r_eom_argmax)
msg.info("metrics.r_kinematics_argmax: %s", metrics.data.r_kinematics_argmax)
msg.info("metrics.r_cts_joints_argmax: %s", metrics.data.r_cts_joints_argmax)
msg.info("metrics.r_cts_limits_argmax: %s", metrics.data.r_cts_limits_argmax)
msg.info("metrics.r_cts_contacts_argmax: %s", metrics.data.r_cts_contacts_argmax)
msg.info("metrics.r_v_plus_argmax: %s", metrics.data.r_v_plus_argmax)
msg.info("metrics.r_ncp_primal_argmax: %s", metrics.data.r_ncp_primal_argmax)
msg.info("metrics.r_ncp_dual_argmax: %s", metrics.data.r_ncp_dual_argmax)
msg.info("metrics.r_ncp_compl_argmax: %s", metrics.data.r_ncp_compl_argmax)
msg.info("metrics.r_vi_natmap_argmax: %s\n", metrics.data.r_vi_natmap_argmax)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_solvers_padmm.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the Proximal-ADMM Solver."""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.math import screw, vec3f
from newton._src.solvers.kamino._src.core.model import ModelKamino
from newton._src.solvers.kamino._src.dynamics.dual import DualProblem
from newton._src.solvers.kamino._src.kinematics.constraints import unpack_constraint_solutions
from newton._src.solvers.kamino._src.linalg import ConjugateResidualSolver, LLTBlockedSolver
from newton._src.solvers.kamino._src.linalg.utils.matrix import SquareSymmetricMatrixProperties
from newton._src.solvers.kamino._src.linalg.utils.range import in_range_via_gaussian_elimination
from newton._src.solvers.kamino._src.models.builders import basics
from newton._src.solvers.kamino._src.solvers.padmm import PADMMSolver, PADMMWarmStartMode
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.extract import (
extract_delassus,
extract_info_vectors,
extract_problem_vector,
)
from newton._src.solvers.kamino.tests.utils.make import make_containers, update_containers
###
# Helper functions
###
class TestSetup:
def __init__(
self,
builder_fn,
max_world_contacts: int = 32,
perturb: bool = True,
gravity: bool = True,
device: wp.DeviceLike = None,
sparse: bool = False,
**kwargs,
):
# Cache the max contacts allocated for the test problem
self.max_world_contacts = max_world_contacts
# Construct the model description using model builders for different systems
self.builder: ModelBuilderKamino = builder_fn(**kwargs)
# Set ad-hoc configurations
self.builder.gravity[0].enabled = gravity
if perturb:
u_0 = screw(vec3f(+10.0, 0.0, 0.0), vec3f(0.0, 0.0, 0.0))
for body in self.builder.bodies:
body.u_i_0 = u_0
# Create the model and containers from the builder
self.model, self.data, self.state, self.limits, self.detector, self.jacobians = make_containers(
builder=self.builder, max_world_contacts=max_world_contacts, device=device, sparse=sparse
)
self.contacts = self.detector.contacts
self.state_p = self.model.state()
# Create the DualProblem to be solved
self.problem = DualProblem(
model=self.model,
data=self.data,
limits=self.limits,
contacts=self.contacts,
solver=ConjugateResidualSolver if sparse else LLTBlockedSolver,
device=device,
sparse=sparse,
)
# Update the sim data containers
update_containers(
model=self.model,
data=self.data,
state=self.state,
limits=self.limits,
detector=self.detector,
jacobians=self.jacobians,
)
def build(self):
# Build the dual problem
self.problem.build(
model=self.model,
data=self.data,
limits=self.limits,
contacts=self.contacts,
jacobians=self.jacobians,
)
def cache(self, solver: PADMMSolver):
# Unpack the computed constraint multipliers to the respective joint-limit
# and contact data for post-processing and optional solver warm-starting
unpack_constraint_solutions(
lambdas=solver.data.solution.lambdas,
v_plus=solver.data.solution.v_plus,
model=self.model,
data=self.data,
limits=self.limits,
contacts=self.contacts,
)
def print_dual_problem_summary(D: np.ndarray, v_f: np.ndarray, notes: str = ""):
D_props = SquareSymmetricMatrixProperties(D)
v_f_is_in_range, *_ = in_range_via_gaussian_elimination(D, v_f)
msg.info("Delassus Properties %s:\n%s\n", notes, D_props)
msg.info("v_f is in range of D %s: %s\n", notes, v_f_is_in_range)
def check_padmm_solution(
test: unittest.TestCase, model: ModelKamino, problem: DualProblem, solver: PADMMSolver, verbose: bool = False
):
# Extract numpy arrays from the solver state and solution
only_active_dims = True
D_wp_np = extract_delassus(problem.delassus, only_active_dims=only_active_dims)
v_f_wp_np = extract_problem_vector(problem.delassus, problem.data.v_f.numpy(), only_active_dims=only_active_dims)
P_wp_np = extract_problem_vector(problem.delassus, problem.data.P.numpy(), only_active_dims=only_active_dims)
v_plus_wp_np = extract_problem_vector(
problem.delassus, solver.data.solution.v_plus.numpy(), only_active_dims=only_active_dims
)
lambdas_wp_np = extract_problem_vector(
problem.delassus, solver.data.solution.lambdas.numpy(), only_active_dims=only_active_dims
)
# Optional verbose output
status = solver.data.status.numpy()
for w in range(model.size.num_worlds):
# Recover the original (preconditioned) Delassua matrix from the in-place regularized storage
dtype = D_wp_np[w].dtype
ncts = D_wp_np[w].shape[0]
I_np = dtype.type(solver.config[0].eta + solver.config[0].rho_0) * np.eye(D_wp_np[w].shape[0], dtype=dtype)
D = D_wp_np[w] - I_np
# Recover original Delassus matrix and v_f from preconditioned versions
D_true = np.diag(np.reciprocal(P_wp_np[w])) @ D @ np.diag(np.reciprocal(P_wp_np[w]))
v_f_true = np.diag(np.reciprocal(P_wp_np[w])) @ v_f_wp_np[w]
# Compute the true dual solution and error
v_plus_true = np.matmul(D_true, lambdas_wp_np[w]) + v_f_true
error_dual_abs_l2 = np.linalg.norm(v_plus_true - v_plus_wp_np[w]) / float(ncts)
error_dual_abs_inf = np.linalg.norm(v_plus_true - v_plus_wp_np[w], ord=np.inf)
# Extract solver status
converged = True if status[w][0] == 1 else False
iterations = status[w][1]
r_p = status[w][2]
r_d = status[w][3]
r_c = status[w][4]
# Optionally print relevant solver data
if verbose:
print_dual_problem_summary(D, v_f_wp_np[w], "(preconditioned)")
print_dual_problem_summary(D_true, v_f_true)
msg.notif(
"\n---------"
f"\nconverged: {converged}"
f"\niterations: {iterations}"
"\n---------"
f"\nr_p: {r_p}"
f"\nr_d: {r_d}"
f"\nr_c: {r_c}"
"\n---------"
f"\nerror_dual_abs_l2: {error_dual_abs_l2}"
f"\nerror_dual_abs_inf: {error_dual_abs_inf}"
"\n---------"
f"\nsolution: lambda: {lambdas_wp_np[w]}"
f"\nsolution: v_plus: {v_plus_wp_np[w]}"
"\n---------\n"
)
# Check results
test.assertTrue(converged)
test.assertLessEqual(iterations, solver.config[w].max_iterations)
test.assertLessEqual(r_p, solver.config[w].primal_tolerance)
test.assertLessEqual(r_d, solver.config[w].dual_tolerance)
test.assertLessEqual(r_c, solver.config[w].compl_tolerance)
test.assertLessEqual(error_dual_abs_l2, solver.config[w].dual_tolerance)
test.assertLessEqual(error_dual_abs_inf, solver.config[w].dual_tolerance)
def save_solver_info(solver: PADMMSolver, path: str | None = None, verbose: bool = False):
# Attempt to import matplotlib for plotting
try:
import matplotlib.pyplot as plt
except ImportError:
return # matplotlib is not available so we skip plotting
solver_has_acceleration = solver._use_acceleration
nw = solver.size.num_worlds
status = solver.data.status.numpy()
iterations = [status[w][1] for w in range(nw)]
offsets_np = solver.data.info.offsets.numpy()
num_rho_updates_np = extract_info_vectors(offsets_np, solver.data.info.num_rho_updates.numpy(), iterations)
norm_s_np = extract_info_vectors(offsets_np, solver.data.info.norm_s.numpy(), iterations)
norm_x_np = extract_info_vectors(offsets_np, solver.data.info.norm_x.numpy(), iterations)
norm_y_np = extract_info_vectors(offsets_np, solver.data.info.norm_y.numpy(), iterations)
norm_z_np = extract_info_vectors(offsets_np, solver.data.info.norm_z.numpy(), iterations)
f_ccp_np = extract_info_vectors(offsets_np, solver.data.info.f_ccp.numpy(), iterations)
f_ncp_np = extract_info_vectors(offsets_np, solver.data.info.f_ncp.numpy(), iterations)
r_dx_np = extract_info_vectors(offsets_np, solver.data.info.r_dx.numpy(), iterations)
r_dy_np = extract_info_vectors(offsets_np, solver.data.info.r_dy.numpy(), iterations)
r_dz_np = extract_info_vectors(offsets_np, solver.data.info.r_dz.numpy(), iterations)
r_primal_np = extract_info_vectors(offsets_np, solver.data.info.r_primal.numpy(), iterations)
r_dual_np = extract_info_vectors(offsets_np, solver.data.info.r_dual.numpy(), iterations)
r_compl_np = extract_info_vectors(offsets_np, solver.data.info.r_compl.numpy(), iterations)
r_pd_np = extract_info_vectors(offsets_np, solver.data.info.r_pd.numpy(), iterations)
r_dp_np = extract_info_vectors(offsets_np, solver.data.info.r_dp.numpy(), iterations)
r_ncp_primal_np = extract_info_vectors(offsets_np, solver.data.info.r_ncp_primal.numpy(), iterations)
r_ncp_dual_np = extract_info_vectors(offsets_np, solver.data.info.r_ncp_dual.numpy(), iterations)
r_ncp_compl_np = extract_info_vectors(offsets_np, solver.data.info.r_ncp_compl.numpy(), iterations)
r_ncp_natmap_np = extract_info_vectors(offsets_np, solver.data.info.r_ncp_natmap.numpy(), iterations)
if solver_has_acceleration:
num_restarts_np = extract_info_vectors(offsets_np, solver.data.info.num_restarts.numpy(), iterations)
a_np = extract_info_vectors(offsets_np, solver.data.info.a.numpy(), iterations)
r_comb_np = extract_info_vectors(offsets_np, solver.data.info.r_comb.numpy(), iterations)
r_comb_ratio_np = extract_info_vectors(offsets_np, solver.data.info.r_comb_ratio.numpy(), iterations)
if verbose:
for w in range(nw):
print(f"[World {w}] =======================================================================")
print(f"solver.info.num_rho_updates: {num_rho_updates_np[w]}")
print(f"solver.info.norm_s: {norm_s_np[w]}")
print(f"solver.info.norm_x: {norm_x_np[w]}")
print(f"solver.info.norm_y: {norm_y_np[w]}")
print(f"solver.info.norm_z: {norm_z_np[w]}")
print(f"solver.info.f_ccp: {f_ccp_np[w]}")
print(f"solver.info.f_ncp: {f_ncp_np[w]}")
print(f"solver.info.r_dx: {r_dx_np[w]}")
print(f"solver.info.r_dy: {r_dy_np[w]}")
print(f"solver.info.r_dz: {r_dz_np[w]}")
print(f"solver.info.r_primal: {r_primal_np[w]}")
print(f"solver.info.r_dual: {r_dual_np[w]}")
print(f"solver.info.r_compl: {r_compl_np[w]}")
print(f"solver.info.r_pd: {r_pd_np[w]}")
print(f"solver.info.r_dp: {r_dp_np[w]}")
print(f"solver.info.r_ncp_primal: {r_ncp_primal_np[w]}")
print(f"solver.info.r_ncp_dual: {r_ncp_dual_np[w]}")
print(f"solver.info.r_ncp_compl: {r_ncp_compl_np[w]}")
print(f"solver.info.r_ncp_natmap: {r_ncp_natmap_np[w]}")
if solver_has_acceleration:
print(f"solver.info.num_restarts: {num_restarts_np[w]}")
print(f"solver.info.a: {a_np[w]}")
print(f"solver.info.r_comb: {r_comb_np[w]}")
print(f"solver.info.r_comb_ratio: {r_comb_ratio_np[w]}")
# List of (label, data) for plotting
info_list = [
("num_rho_updates", num_rho_updates_np),
("norm_s", norm_s_np),
("norm_x", norm_x_np),
("norm_y", norm_y_np),
("norm_z", norm_z_np),
("f_ccp", f_ccp_np),
("f_ncp", f_ncp_np),
("r_dx", r_dx_np),
("r_dy", r_dy_np),
("r_dz", r_dz_np),
("r_primal", r_primal_np),
("r_dual", r_dual_np),
("r_compl", r_compl_np),
("r_pd", r_pd_np),
("r_dp", r_dp_np),
("r_ncp_primal", r_ncp_primal_np),
("r_ncp_dual", r_ncp_dual_np),
("r_ncp_compl", r_ncp_compl_np),
("r_ncp_natmap", r_ncp_natmap_np),
]
if solver_has_acceleration:
info_list.extend(
[
("num_restarts", num_restarts_np),
("a", a_np),
("r_comb", r_comb_np),
("r_comb_ratio", r_comb_ratio_np),
]
)
# Plot all info as subplots: rows=info_list, cols=worlds
n_rows = len(info_list)
n_cols = nw
_fig, axes = plt.subplots(n_rows, n_cols, figsize=(4 * n_cols, 2.5 * n_rows), squeeze=False)
for row, (label, arr) in enumerate(info_list):
for col in range(nw):
ax = axes[row, col]
ax.plot(arr[col], label=f"{label}")
ax.set_xlabel("Iteration")
ax.set_ylabel(label)
if row == 0:
ax.set_title(f"World {col}")
if col == 0:
ax.set_ylabel(label)
else:
ax.set_ylabel("")
ax.grid(True)
plt.tight_layout()
if path is not None:
plt.savefig(path, format="pdf", dpi=300, bbox_inches="tight")
plt.close()
###
# Tests
###
class TestPADMMSolver(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for detailed output
self.savefig = test_context.verbose # Set to True to generate solver info plots
self.default_device = wp.get_device(test_context.device)
self.output_path = test_context.output_path / "test_solvers_padmm"
# Create output directory if saving figures
if self.savefig:
self.output_path.mkdir(parents=True, exist_ok=True)
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_padmm_default(self):
"""
Test creating a PADMMSolver with default initialization.
"""
# Creating a default PADMMSolver without any model or config
# should result in a solver without any memory allocation.
solver = PADMMSolver()
self.assertIsNone(solver._size)
self.assertIsNone(solver._data)
self.assertIsNone(solver._device)
self.assertEqual(solver.config, [])
# Requesting the solver data container when the
# solver has not been finalized should raise an
# error since no allocations have been made.
self.assertRaises(RuntimeError, lambda: solver.data)
def test_01_finalize_padmm_default(self):
"""
Test creating a PADMMSolver with default initialization and then finalizing all memory allocations.
"""
# Create a test setup
test = TestSetup(builder_fn=basics.build_box_on_plane, max_world_contacts=8, device=self.default_device)
# Creating a default PADMMSolver without any model or config
# should result in a solver without any memory allocation.
solver = PADMMSolver()
# Finalize the solver with a model
solver.finalize(test.model)
# Check that the solver has been properly allocated
self.assertIsNotNone(solver._size)
self.assertIsNotNone(solver._data)
self.assertIsNotNone(solver._device)
self.assertEqual(len(solver.config), test.model.size.num_worlds)
self.assertIs(solver._device, test.model.device)
self.assertIs(solver.size, test.model.size)
def test_02_padmm_solve(self):
"""
Tests the Proximal-ADMM (PADMM) solver with default config on the reference problem.
"""
# Create the test problem
test = TestSetup(builder_fn=basics.build_box_on_plane, max_world_contacts=8, device=self.default_device)
# Define solver config
# NOTE: These are all equal to their default values
# but are defined here explicitly for the purposes
# of experimentation and testing.
config = PADMMSolver.Config()
config.primal_tolerance = 1e-6
config.dual_tolerance = 1e-6
config.compl_tolerance = 1e-6
config.eta = 1e-5
config.rho_0 = 1.0
config.max_iterations = 200
# Create the PADMM solver
solver = PADMMSolver(
model=test.model,
config=config,
warmstart=PADMMWarmStartMode.NONE,
use_acceleration=False,
collect_info=True,
)
# Solve the test problem
test.build()
solver.reset()
solver.coldstart()
solver.solve(problem=test.problem)
# check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Extract solver info
if self.savefig:
msg.notif("Generating solver info plots...")
path = self.output_path / "test_02_padmm_solve.pdf"
save_solver_info(solver=solver, path=str(path))
# Check solution
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
def test_03_padmm_solve_with_acceleration(self):
"""
Tests the Accelerated Proximal-ADMM (APADMM) solver on the reference problem with Nesterov acceleration.
"""
# Create the test problem
test = TestSetup(builder_fn=basics.build_box_on_plane, max_world_contacts=8, device=self.default_device)
# Define solver config
# NOTE: These are all equal to their default values
# but are defined here explicitly for the purposes
# of experimentation and testing.
config = PADMMSolver.Config()
config.primal_tolerance = 1e-6
config.dual_tolerance = 1e-6
config.compl_tolerance = 1e-6
config.restart_tolerance = 0.999
config.eta = 1e-5
config.rho_0 = 1.0
config.max_iterations = 200
# Create the PADMM solver
solver = PADMMSolver(
model=test.model,
config=config,
warmstart=PADMMWarmStartMode.NONE,
use_acceleration=True,
collect_info=True,
)
# Solve the test problem
test.build()
solver.reset()
solver.coldstart()
solver.solve(problem=test.problem)
# check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Extract solver info
if self.savefig:
msg.notif("Generating solver info plots...")
path = self.output_path / "test_03_padmm_solve_with_acceleration.pdf"
save_solver_info(solver=solver, path=str(path))
# Check solution
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
def test_04_padmm_solve_with_internal_warmstart(self):
"""
Tests the Proximal-ADMM (PADMM) solver on the reference problem with internal warmstarting.
"""
# Create the test problem
test = TestSetup(builder_fn=basics.build_box_on_plane, max_world_contacts=8, device=self.default_device)
# Define solver config
# NOTE: These are all equal to their default values
# but are defined here explicitly for the purposes
# of experimentation and testing.
config = PADMMSolver.Config()
config.primal_tolerance = 1e-6
config.dual_tolerance = 1e-6
config.compl_tolerance = 1e-6
config.eta = 1e-5
config.rho_0 = 1.0
config.max_iterations = 200
# Create the ADMM solver
solver = PADMMSolver(
model=test.model,
config=config,
warmstart=PADMMWarmStartMode.INTERNAL,
use_acceleration=False,
collect_info=True,
)
# Initial cold-started solve
test.build()
solver.reset()
solver.coldstart()
solver.solve(problem=test.problem)
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Second solve with warm-starting from previous solution
test.build()
solver.warmstart(test.problem, test.model, test.data)
solver.solve(problem=test.problem)
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Extract solver info
if self.savefig:
msg.notif("Generating solver info plots...")
path = self.output_path / "test_04_padmm_solve_with_internal_warmstart.pdf"
save_solver_info(solver=solver, path=str(path))
def test_05_padmm_solve_with_container_warmstart(self):
"""
Tests the Proximal-ADMM (PADMM) solver on the reference problem with container-based warmstarting.
"""
# Create the test problem
test = TestSetup(builder_fn=basics.build_box_on_plane, max_world_contacts=8, device=self.default_device)
# Define solver config
# NOTE: These are all equal to their default values
# but are defined here explicitly for the purposes
# of experimentation and testing.
config = PADMMSolver.Config()
config.primal_tolerance = 1e-6
config.dual_tolerance = 1e-6
config.compl_tolerance = 1e-6
config.eta = 1e-5
config.rho_0 = 1.0
config.max_iterations = 200
# Create the ADMM solver
solver = PADMMSolver(
model=test.model,
config=config,
warmstart=PADMMWarmStartMode.CONTAINERS,
use_acceleration=False,
collect_info=True,
)
# Initial cold-started solve
test.build()
solver.reset()
solver.coldstart()
solver.solve(problem=test.problem)
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Second solve with warm-starting from previous solution
test.cache(solver=solver)
test.build()
solver.warmstart(test.problem, test.model, test.data, test.limits, test.contacts)
solver.solve(problem=test.problem)
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Extract solver info
if self.savefig:
msg.notif("Generating solver info plots...")
path = self.output_path / "test_05_padmm_solve_with_container_warmstart.pdf"
save_solver_info(solver=solver, path=str(path))
def test_06_padmm_solve_with_acceleration_and_internal_warmstart(self):
"""
Tests the Proximal-ADMM (PADMM) solver on the reference problem with container-based warmstarting.
"""
# Create the test problem
test = TestSetup(builder_fn=basics.build_box_on_plane, max_world_contacts=8, device=self.default_device)
# Define solver config
# NOTE: These are all equal to their default values
# but are defined here explicitly for the purposes
# of experimentation and testing.
config = PADMMSolver.Config()
config.primal_tolerance = 1e-6
config.dual_tolerance = 1e-6
config.compl_tolerance = 1e-6
config.restart_tolerance = 0.999
config.eta = 1e-5
config.rho_0 = 1.0
config.max_iterations = 200
# Create the ADMM solver
solver = PADMMSolver(
model=test.model,
config=config,
warmstart=PADMMWarmStartMode.INTERNAL,
use_acceleration=True,
collect_info=True,
)
# Initial cold-started solve
test.build()
solver.reset()
solver.coldstart()
solver.solve(problem=test.problem)
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Second solve with warm-starting from previous solution
test.build()
solver.warmstart(test.problem, test.model, test.data)
solver.solve(problem=test.problem)
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Extract solver info
if self.savefig:
msg.notif("Generating solver info plots...")
path = self.output_path / "test_06_padmm_solve_with_acceleration_and_internal_warmstart.pdf"
save_solver_info(solver=solver, path=str(path))
def test_07_padmm_solve_with_acceleration_and_container_warmstart(self):
"""
Tests the Proximal-ADMM (PADMM) solver on the reference problem with container-based warmstarting.
"""
# Create the test problem
test = TestSetup(builder_fn=basics.build_box_on_plane, max_world_contacts=8, device=self.default_device)
# Define solver config
# NOTE: These are all equal to their default values
# but are defined here explicitly for the purposes
# of experimentation and testing.
config = PADMMSolver.Config()
config.primal_tolerance = 1e-6
config.dual_tolerance = 1e-6
config.compl_tolerance = 1e-6
config.restart_tolerance = 0.999
config.eta = 1e-5
config.rho_0 = 1.0
config.max_iterations = 200
# Create the ADMM solver
solver = PADMMSolver(
model=test.model,
config=config,
warmstart=PADMMWarmStartMode.CONTAINERS,
use_acceleration=True,
collect_info=True,
)
# Initial cold-started solve
test.build()
solver.reset()
solver.coldstart()
solver.solve(problem=test.problem)
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Second solve with warm-starting from previous solution
test.cache(solver=solver)
test.build()
solver.warmstart(test.problem, test.model, test.data, test.limits, test.contacts)
solver.solve(problem=test.problem)
check_padmm_solution(self, test.model, test.problem, solver, verbose=self.verbose)
# Extract solver info
if self.savefig:
msg.notif("Generating solver info plots...")
path = self.output_path / "test_07_padmm_solve_with_acceleration_and_container_warmstart.pdf"
save_solver_info(solver=solver, path=str(path))
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_utils_control_animation.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the AnimationJointReference class."""
import unittest
import numpy as np
import warp as wp
import newton
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.control import AnimationJointReference
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestAnimationJointReference(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default(self):
animation = AnimationJointReference()
self.assertIsNotNone(animation)
self.assertEqual(animation.device, None)
self.assertEqual(animation._data, None)
def test_01_make_with_numpy_data(self):
# Load the DR Legs model and animation data from the
# `newton-assets` repository using the utility function
asset_path = newton.utils.download_asset("disneyresearch")
model_asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_boxes.usda")
animation_asset_file = str(asset_path / "dr_legs" / "animation" / "dr_legs_animation_100fps.npy")
# Import USD model of DR Legs
importer = USDImporter()
builder: ModelBuilderKamino = importer.import_from(source=model_asset_file)
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
# Retrieve the number of actuated coordinates and DoFs
njaq = model.size.sum_of_num_actuated_joint_coords
njad = model.size.sum_of_num_actuated_joint_dofs
msg.info(f"number of actuated joint coordinates: {njaq}")
msg.info(f"number of actuated joint DoFs: {njad}")
self.assertEqual(njaq, njad) # Ensure only 1-DoF joints
# Load numpy animation data
animation_np = np.load(animation_asset_file, allow_pickle=True)
msg.info(f"animation_np (shape={animation_np.shape}):\n{animation_np}\n")
self.assertEqual(animation_np.shape[1], njaq) # Ensure data matches number of joints
# Set animation parameters
animation_dt = 0.01 # 100 Hz/FPS
sim_dt = animation_dt # 100 Hz/FPS
decimation: int = 1 # No decimation, step through every frame
rate: int = 1 # No rate (i.e. skip), step through every frame
loop: bool = True
use_fd: bool = False
# Create a joint-space animation reference generator
animation = AnimationJointReference(
model=model,
data=animation_np,
data_dt=animation_dt,
target_dt=sim_dt,
decimation=decimation,
rate=rate,
loop=loop,
use_fd=use_fd,
device=self.default_device,
)
self.assertIsNotNone(animation)
self.assertIsNotNone(animation.data)
self.assertEqual(animation.device, self.default_device)
self.assertEqual(animation.sequence_length, animation_np.shape[0])
self.assertEqual(animation.data.q_j_ref.shape, animation_np.shape)
self.assertEqual(animation.data.dq_j_ref.shape, animation_np.shape)
self.assertEqual(animation.data.loop.shape, (1,))
self.assertEqual(animation.data.loop.numpy()[0], 1 if loop else 0)
self.assertEqual(animation.data.rate.shape, (1,))
self.assertEqual(animation.data.rate.numpy()[0], rate)
self.assertEqual(animation.data.frame.shape, (1,))
self.assertEqual(animation.data.frame.numpy()[0], 0)
# Check that the internal numpy arrays match the input data
np.testing.assert_array_almost_equal(animation.data.q_j_ref.numpy(), animation_np, decimal=6)
np.testing.assert_array_almost_equal(animation.data.dq_j_ref.numpy(), np.zeros_like(animation_np), decimal=6)
# Allocate output arrays for joint references
q_j_ref_out = wp.zeros(njad, dtype=float32, device=self.default_device)
dq_j_ref_out = wp.zeros(njad, dtype=float32, device=self.default_device)
# Retrieve the reference at the initial step (0)
animation.reset(q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([0], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[0, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Initialize simulation time steps
data.time.steps.fill_(0)
# Step through the animation and verify outputs
num_steps = 10
for step in range(1, num_steps + 1):
data.time.steps.fill_(step)
animation.step(time=data.time, q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
expected_step = (rate * step) % animation.sequence_length # Loop around if exceeding number of frames
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([expected_step], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[expected_step, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Reset the reference at the initial step (0)
animation.reset(q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([0], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[0, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Initialize simulation time steps
data.time.steps.fill_(0)
# Step through again but exceeding the number of frames to test looping
num_steps = animation.sequence_length + 5
for step in range(1, num_steps + 1):
data.time.steps.fill_(step)
animation.step(time=data.time, q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
expected_step = (rate * step) % animation.sequence_length # Loop around if exceeding number of frames
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([expected_step], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[expected_step, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
def test_02_make_with_numpy_data_and_decimation(self):
# Load the DR Legs model and animation data from the
# `newton-assets` repository using the utility function
asset_path = newton.utils.download_asset("disneyresearch")
model_asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_boxes.usda")
animation_asset_file = str(asset_path / "dr_legs" / "animation" / "dr_legs_animation_100fps.npy")
# Import USD model of DR Legs
importer = USDImporter()
builder: ModelBuilderKamino = importer.import_from(source=model_asset_file)
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
# Retrieve the number of actuated coordinates and DoFs
njaq = model.size.sum_of_num_actuated_joint_coords
njad = model.size.sum_of_num_actuated_joint_dofs
msg.info(f"number of actuated joint coordinates: {njaq}")
msg.info(f"number of actuated joint DoFs: {njad}")
self.assertEqual(njaq, njad) # Ensure only 1-DoF joints
# Load numpy animation data
animation_np = np.load(animation_asset_file, allow_pickle=True)
msg.info(f"animation_np (shape={animation_np.shape}):\n{animation_np}\n")
self.assertEqual(animation_np.shape[1], njaq) # Ensure data matches number of joints
# Set animation parameters
animation_dt = 0.01 # 100 Hz/FPS
sim_dt = animation_dt # 100 Hz/FPS
decimation: int = 15 # Advance frame index every 15th step
rate: int = 1 # No rate (i.e. skip), step through every frame
loop: bool = True
use_fd: bool = False
# Create a joint-space animation reference generator
animation = AnimationJointReference(
model=model,
data=animation_np,
data_dt=animation_dt,
target_dt=sim_dt,
decimation=decimation,
rate=rate,
loop=loop,
use_fd=use_fd,
device=self.default_device,
)
self.assertIsNotNone(animation)
self.assertIsNotNone(animation.data)
self.assertEqual(animation.device, self.default_device)
self.assertEqual(animation.sequence_length, animation_np.shape[0])
self.assertEqual(animation.data.q_j_ref.shape, animation_np.shape)
self.assertEqual(animation.data.dq_j_ref.shape, animation_np.shape)
self.assertEqual(animation.data.length.shape, (1,))
self.assertEqual(animation.data.length.numpy()[0], animation_np.shape[0])
self.assertEqual(animation.data.decimation.shape, (1,))
self.assertEqual(animation.data.decimation.numpy()[0], decimation)
self.assertEqual(animation.data.rate.shape, (1,))
self.assertEqual(animation.data.rate.numpy()[0], rate)
self.assertEqual(animation.data.loop.shape, (1,))
self.assertEqual(animation.data.loop.numpy()[0], 1 if loop else 0)
self.assertEqual(animation.data.frame.shape, (1,))
self.assertEqual(animation.data.frame.numpy()[0], 0)
# Check that the internal numpy arrays match the input data
np.testing.assert_array_almost_equal(animation.data.q_j_ref.numpy(), animation_np, decimal=6)
np.testing.assert_array_almost_equal(animation.data.dq_j_ref.numpy(), np.zeros_like(animation_np), decimal=6)
# Allocate output arrays for joint references
q_j_ref_out = wp.zeros(njad, dtype=float32, device=self.default_device)
dq_j_ref_out = wp.zeros(njad, dtype=float32, device=self.default_device)
# Retrieve the reference at the initial step (0)
animation.reset(q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([0], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[0, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Initialize simulation time steps
data.time.steps.fill_(0)
# Step through the animation and verify outputs
num_steps = 3 * decimation + 2 # Step through multiple decimation cycles
for s in range(1, num_steps + 1):
# Increment the global simulation step array
data.time.steps.fill_(s)
step = data.time.steps.numpy()[0]
msg.info(f"[s={s}]: step index: {step}")
self.assertEqual(step, s)
# Step the animation
# NOTE: In actual uses-cases this will be called on every sim step
# and we only want to update the frame index every `decimation` steps
animation.step(time=data.time, q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
# Retrieve the actual frame index
frame = animation.data.frame.numpy()[0]
msg.info(f"[s={s}]: frame index: {frame}")
# Compute the expected frame index based on decimation
expected = (step // decimation) % animation.sequence_length
msg.info(f"[s={s}]: expected index: {expected}")
# Check expected vs actual frame index and outputs
self.assertEqual(frame, expected)
# Check output references match expected frame
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[expected, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Reset the reference at the initial step (0)
animation.reset(q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([0], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[0, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Initialize simulation time steps
data.time.steps.fill_(0)
# Step through again but exceeding the number of frames to test looping
num_steps = animation.sequence_length + 5
for s in range(1, num_steps + 1):
# Increment the global simulation step array
data.time.steps.fill_(s)
step = data.time.steps.numpy()[0]
msg.info(f"[s={s}]: step index: {step}")
self.assertEqual(step, s)
# Step the animation
# NOTE: In actual uses-cases this will be called on every sim step
# and we only want to update the frame index every `decimation` steps
animation.step(time=data.time, q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
# Retrieve the actual frame index
frame = animation.data.frame.numpy()[0]
msg.info(f"[s={s}]: frame index: {frame}")
# Compute the expected frame index based on decimation
expected = (step // decimation) % animation.sequence_length
msg.info(f"[s={s}]: expected index: {expected}")
# Check expected vs actual frame index and outputs
self.assertEqual(frame, expected)
# Check output references match expected frame
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[expected, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
def test_03_make_with_numpy_data_and_decimation_plus_rate(self):
# Load the DR Legs model and animation data from the
# `newton-assets` repository using the utility function
asset_path = newton.utils.download_asset("disneyresearch")
model_asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_boxes.usda")
animation_asset_file = str(asset_path / "dr_legs" / "animation" / "dr_legs_animation_100fps.npy")
# Import USD model of DR Legs
importer = USDImporter()
builder: ModelBuilderKamino = importer.import_from(source=model_asset_file)
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
# Retrieve the number of actuated coordinates and DoFs
njaq = model.size.sum_of_num_actuated_joint_coords
njad = model.size.sum_of_num_actuated_joint_dofs
msg.info(f"number of actuated joint coordinates: {njaq}")
msg.info(f"number of actuated joint DoFs: {njad}")
self.assertEqual(njaq, njad) # Ensure only 1-DoF joints
# Load numpy animation data
animation_np = np.load(animation_asset_file, allow_pickle=True)
msg.info(f"animation_np (shape={animation_np.shape}):\n{animation_np}\n")
self.assertEqual(animation_np.shape[1], njaq) # Ensure data matches number of joints
# Set animation parameters
animation_dt = 0.01 # 100 Hz/FPS
sim_dt = animation_dt # 100 Hz/FPS
decimation: int = 15 # Advance frame index every 15th step
rate: int = 10 # No rate (i.e. skip), step through every frame
loop: bool = True
use_fd: bool = False
# Create a joint-space animation reference generator
animation = AnimationJointReference(
model=model,
data=animation_np,
data_dt=animation_dt,
target_dt=sim_dt,
decimation=decimation,
rate=rate,
loop=loop,
use_fd=use_fd,
device=self.default_device,
)
self.assertIsNotNone(animation)
self.assertIsNotNone(animation.data)
self.assertEqual(animation.device, self.default_device)
self.assertEqual(animation.sequence_length, animation_np.shape[0])
self.assertEqual(animation.data.q_j_ref.shape, animation_np.shape)
self.assertEqual(animation.data.dq_j_ref.shape, animation_np.shape)
self.assertEqual(animation.data.length.shape, (1,))
self.assertEqual(animation.data.length.numpy()[0], animation_np.shape[0])
self.assertEqual(animation.data.decimation.shape, (1,))
self.assertEqual(animation.data.decimation.numpy()[0], decimation)
self.assertEqual(animation.data.rate.shape, (1,))
self.assertEqual(animation.data.rate.numpy()[0], rate)
self.assertEqual(animation.data.loop.shape, (1,))
self.assertEqual(animation.data.loop.numpy()[0], 1 if loop else 0)
self.assertEqual(animation.data.frame.shape, (1,))
self.assertEqual(animation.data.frame.numpy()[0], 0)
# Check that the internal numpy arrays match the input data
np.testing.assert_array_almost_equal(animation.data.q_j_ref.numpy(), animation_np, decimal=6)
np.testing.assert_array_almost_equal(animation.data.dq_j_ref.numpy(), np.zeros_like(animation_np), decimal=6)
# Allocate output arrays for joint references
q_j_ref_out = wp.zeros(njad, dtype=float32, device=self.default_device)
dq_j_ref_out = wp.zeros(njad, dtype=float32, device=self.default_device)
# Retrieve the reference at the initial step (0)
animation.reset(q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([0], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[0, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Initialize simulation time steps
data.time.steps.fill_(0)
# Step through the animation and verify outputs
num_steps = 3 * decimation + 2 # Step through multiple decimation cycles
for s in range(1, num_steps + 1):
# Increment the global simulation step array
data.time.steps.fill_(s)
step = data.time.steps.numpy()[0]
msg.info(f"[s={s}]: step index: {step}")
self.assertEqual(step, s)
# Step the animation
# NOTE: In actual uses-cases this will be called on every sim step
# and we only want to update the frame index every `decimation` steps
animation.step(time=data.time, q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
# Retrieve the actual frame index
frame = animation.data.frame.numpy()[0]
msg.info(f"[s={s}]: frame index: {frame}")
# Compute the expected frame index based on decimation
expected = ((step // decimation) * rate) % animation.sequence_length
msg.info(f"[s={s}]: expected index: {expected}")
# Check expected vs actual frame index and outputs
self.assertEqual(frame, expected)
# Check output references match expected frame
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[expected, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Reset the reference at the initial step (0)
animation.reset(q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([0], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[0, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Initialize simulation time steps
data.time.steps.fill_(0)
# Step through again but exceeding the number of frames to test looping
num_steps = animation.sequence_length + 5
for s in range(1, num_steps + 1):
# Increment the global simulation step array
data.time.steps.fill_(s)
step = data.time.steps.numpy()[0]
msg.info(f"[s={s}]: step index: {step}")
self.assertEqual(step, s)
# Step the animation
# NOTE: In actual uses-cases this will be called on every sim step
# and we only want to update the frame index every `decimation` steps
animation.step(time=data.time, q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
# Retrieve the actual frame index
frame = animation.data.frame.numpy()[0]
msg.info(f"[s={s}]: frame index: {frame}")
# Compute the expected frame index based on decimation
expected = ((step // decimation) * rate) % animation.sequence_length
msg.info(f"[s={s}]: expected index: {expected}")
# Check expected vs actual frame index and outputs
self.assertEqual(frame, expected)
# Check output references match expected frame
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[expected, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
def test_04_make_with_numpy_data_and_decimation_plus_rate_no_looping(self):
# Load the DR Legs model and animation data from the
# `newton-assets` repository using the utility function
asset_path = newton.utils.download_asset("disneyresearch")
model_asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_boxes.usda")
animation_asset_file = str(asset_path / "dr_legs" / "animation" / "dr_legs_animation_100fps.npy")
# Import USD model of DR Legs
importer = USDImporter()
builder: ModelBuilderKamino = importer.import_from(source=model_asset_file)
model = builder.finalize(device=self.default_device)
data = model.data(device=self.default_device)
# Retrieve the number of actuated coordinates and DoFs
njaq = model.size.sum_of_num_actuated_joint_coords
njad = model.size.sum_of_num_actuated_joint_dofs
msg.info(f"number of actuated joint coordinates: {njaq}")
msg.info(f"number of actuated joint DoFs: {njad}")
self.assertEqual(njaq, njad) # Ensure only 1-DoF joints
# Load numpy animation data
animation_np = np.load(animation_asset_file, allow_pickle=True)
msg.info(f"animation_np (shape={animation_np.shape}):\n{animation_np}\n")
self.assertEqual(animation_np.shape[1], njaq) # Ensure data matches number of joints
# Set animation parameters
animation_dt = 0.01 # 100 Hz/FPS
sim_dt = animation_dt # 100 Hz/FPS
decimation: int = 15 # Advance frame index every 15th step
rate: int = 10 # No rate (i.e. skip), step through every frame
loop: bool = False
use_fd: bool = False
# Create a joint-space animation reference generator
animation = AnimationJointReference(
model=model,
data=animation_np,
data_dt=animation_dt,
target_dt=sim_dt,
decimation=decimation,
rate=rate,
loop=loop,
use_fd=use_fd,
device=self.default_device,
)
self.assertIsNotNone(animation)
self.assertIsNotNone(animation.data)
self.assertEqual(animation.device, self.default_device)
self.assertEqual(animation.sequence_length, animation_np.shape[0])
self.assertEqual(animation.data.q_j_ref.shape, animation_np.shape)
self.assertEqual(animation.data.dq_j_ref.shape, animation_np.shape)
self.assertEqual(animation.data.length.shape, (1,))
self.assertEqual(animation.data.length.numpy()[0], animation_np.shape[0])
self.assertEqual(animation.data.decimation.shape, (1,))
self.assertEqual(animation.data.decimation.numpy()[0], decimation)
self.assertEqual(animation.data.rate.shape, (1,))
self.assertEqual(animation.data.rate.numpy()[0], rate)
self.assertEqual(animation.data.loop.shape, (1,))
self.assertEqual(animation.data.loop.numpy()[0], 1 if loop else 0)
self.assertEqual(animation.data.frame.shape, (1,))
self.assertEqual(animation.data.frame.numpy()[0], 0)
# Check that the internal numpy arrays match the input data
np.testing.assert_array_almost_equal(animation.data.q_j_ref.numpy(), animation_np, decimal=6)
np.testing.assert_array_almost_equal(animation.data.dq_j_ref.numpy(), np.zeros_like(animation_np), decimal=6)
# Allocate output arrays for joint references
q_j_ref_out = wp.zeros(njad, dtype=float32, device=self.default_device)
dq_j_ref_out = wp.zeros(njad, dtype=float32, device=self.default_device)
# Reset the reference at the initial step (0)
animation.reset(q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
np.testing.assert_array_equal(animation.data.frame.numpy(), np.array([0], dtype=np.int32))
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[0, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
# Initialize simulation time steps
data.time.steps.fill_(0)
# Step through again but exceeding the number of frames to test looping
num_steps = animation.sequence_length + 5
for s in range(1, num_steps + 1):
# Increment the global simulation step array
data.time.steps.fill_(s)
step = data.time.steps.numpy()[0]
msg.info(f"[s={s}]: step index: {step}")
self.assertEqual(step, s)
# Step the animation
# NOTE: In actual uses-cases this will be called on every sim step
# and we only want to update the frame index every `decimation` steps
animation.step(time=data.time, q_j_ref_out=q_j_ref_out, dq_j_ref_out=dq_j_ref_out)
# Retrieve the actual frame index
frame = animation.data.frame.numpy()[0]
msg.info(f"[s={s}]: frame index: {frame}")
# Compute the expected frame index based on decimation
expected = (step // decimation) * rate
msg.info(f"[s={s}]: expected index: {expected}")
# Check expected vs actual frame index and outputs
if expected >= animation.sequence_length:
expected = animation.sequence_length - 1 # Clamp to last frame if exceeding length
self.assertEqual(frame, expected)
# Check output references match expected frame
np.testing.assert_array_almost_equal(q_j_ref_out.numpy(), animation_np[expected, :], decimal=6)
np.testing.assert_array_almost_equal(dq_j_ref_out.numpy(), np.zeros(njad, dtype=np.float32), decimal=6)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_utils_control_pid.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the JointSpacePIDController class."""
import unittest
import warp as wp
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.control.pid import JointSpacePIDController
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestJointSpacePIDController(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_make_default(self):
# Create a default PID controller
controller = JointSpacePIDController()
# Check default values
self.assertIsNotNone(controller)
self.assertEqual(controller.device, None)
self.assertEqual(controller._data, None)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_utils_control_rand.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the RandomController class."""
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.models.builders.basics import build_boxes_fourbar
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.control.rand import RandomJointController
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestRandomController(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.INFO)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_00_make_default(self):
# Create a default random controller
controller = RandomJointController()
# Check default values
self.assertIsNotNone(controller)
self.assertEqual(controller._model, None)
self.assertEqual(controller._data, None)
self.assertRaises(RuntimeError, lambda: controller.device)
self.assertRaises(RuntimeError, lambda: controller.seed)
self.assertRaises(RuntimeError, lambda: controller.model)
self.assertRaises(RuntimeError, lambda: controller.data)
def test_01_make_for_single_fourbar(self):
# Define a model builder for the boxes_fourbar problem with 1 world
builder = make_homogeneous_builder(num_worlds=1, build_fn=build_boxes_fourbar)
model = builder.finalize(device=self.default_device)
data = model.data()
control = model.control()
# Create a random controller with default arguments
controller = RandomJointController(model=model, seed=self.seed)
# Check contents
self.assertIsNotNone(controller)
self.assertIsNotNone(controller._model, None)
self.assertIsNotNone(controller._data, None)
self.assertIs(controller.device, model.device)
# Check dimensions of the decimation array
self.assertEqual(controller.data.decimation.shape, (model.size.num_worlds,))
self.assertTrue((controller.data.decimation.numpy() == 1).all())
# Check that the seed is set correctly
self.assertEqual(controller.seed, self.seed)
# Check that the generated control inputs are different than the default values
self.assertEqual(np.linalg.norm(control.tau_j.numpy()), 0.0)
controller.compute(time=data.time, control=control)
tau_j_np_0 = control.tau_j.numpy().copy()
msg.info("control.tau_j: %s", tau_j_np_0)
self.assertGreaterEqual(np.linalg.norm(control.tau_j.numpy()), 0.0)
def test_02_make_for_multiple_fourbar(self):
# Define a model builder for the boxes_fourbar problem with 4 worlds
builder = make_homogeneous_builder(num_worlds=4, build_fn=build_boxes_fourbar)
model = builder.finalize(device=self.default_device)
data = model.data()
control = model.control()
# Create a random controller with default arguments
controller = RandomJointController(model=model, seed=self.seed)
# Check contents
self.assertIsNotNone(controller)
self.assertIsNotNone(controller._model, None)
self.assertIsNotNone(controller._data, None)
self.assertIs(controller.device, model.device)
# Check dimensions of the decimation array
self.assertEqual(controller.data.decimation.shape, (model.size.num_worlds,))
self.assertTrue((controller.data.decimation.numpy() == 1).all())
# Check that the seed is set correctly
self.assertEqual(controller.seed, self.seed)
# Check that the generated control inputs are different than the default values
self.assertEqual(np.linalg.norm(control.tau_j.numpy()), 0.0)
controller.compute(time=data.time, control=control)
tau_j_np_0 = control.tau_j.numpy().copy()
msg.info("control.tau_j: %s", tau_j_np_0)
self.assertGreaterEqual(np.linalg.norm(control.tau_j.numpy()), 0.0)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_utils_io_usd.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the USD importer utility."""
import math
import os
import unittest
import numpy as np
import warp as wp
import newton
from newton import Model, ModelBuilder
from newton._src.geometry.types import GeoType
from newton._src.solvers.kamino import SolverKamino
from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
from newton._src.solvers.kamino._src.core.joints import JOINT_QMAX, JOINT_QMIN, JointActuationType, JointDoFType
from newton._src.solvers.kamino._src.models import get_basics_usd_assets_path, get_testing_usd_assets_path
from newton._src.solvers.kamino._src.models.builders import basics
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.io.usd import USDImporter
from newton._src.solvers.kamino.tests import setup_tests, test_context
from newton._src.solvers.kamino.tests.utils.checks import assert_builders_equal
from newton.tests.unittest_utils import USD_AVAILABLE
###
# Tests
###
class TestUSDImporter(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set the paths to the assets provided by the kamino package
self.TEST_USD_ASSETS_PATH = get_testing_usd_assets_path()
self.BASICS_USD_ASSETS_PATH = get_basics_usd_assets_path()
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
###
# Joints supported natively by USD
###
def test_import_joint_revolute_passive_unary(self):
"""Test importing a passive revolute joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_revolute_passive_unary.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.REVOLUTE)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, -1)
self.assertEqual(builder_usd.joints[0].bid_F, 0)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [0.5 * math.pi])
def test_import_joint_revolute_passive(self):
"""Test importing a passive revolute joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_revolute_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.REVOLUTE)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [0.5 * math.pi])
def test_import_joint_revolute_actuated(self):
"""Test importing a actuated revolute joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_revolute_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.REVOLUTE)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0])
def test_import_joint_prismatic_passive_unary(self):
"""Test importing a passive prismatic joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_prismatic_passive_unary.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.PRISMATIC)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, -1)
self.assertEqual(builder_usd.joints[0].bid_F, 0)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-1.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [1.0])
def test_import_joint_prismatic_passive(self):
"""Test importing a passive prismatic joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_prismatic_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.PRISMATIC)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-1.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [1.0])
def test_import_joint_prismatic_actuated(self):
"""Test importing a actuated prismatic joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_prismatic_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.PRISMATIC)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-1.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [1.0])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0])
def test_import_joint_spherical_unary(self):
"""Test importing a passive spherical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_spherical_unary.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.SPHERICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, -1)
self.assertEqual(builder_usd.joints[0].bid_F, 0)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [JOINT_QMIN, JOINT_QMIN, JOINT_QMIN])
self.assertEqual(builder_usd.joints[0].q_j_max, [JOINT_QMAX, JOINT_QMAX, JOINT_QMAX])
def test_import_joint_spherical(self):
"""Test importing a passive spherical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_spherical.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.SPHERICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [JOINT_QMIN, JOINT_QMIN, JOINT_QMIN])
self.assertEqual(builder_usd.joints[0].q_j_max, [JOINT_QMAX, JOINT_QMAX, JOINT_QMAX])
###
# Joints based on specializations of UsdPhysicsD6Joint
###
def test_import_joint_cylindrical_passive_unary(self):
"""Test importing a passive cylindrical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_cylindrical_passive_unary.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CYLINDRICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, -1)
self.assertEqual(builder_usd.joints[0].bid_F, 0)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-1, JOINT_QMIN])
self.assertEqual(builder_usd.joints[0].q_j_max, [1, JOINT_QMAX])
def test_import_joint_cylindrical_passive(self):
"""Test importing a passive cylindrical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_cylindrical_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CYLINDRICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-1, JOINT_QMIN])
self.assertEqual(builder_usd.joints[0].q_j_max, [1, JOINT_QMAX])
def test_import_joint_cylindrical_actuated(self):
"""Test importing a actuated cylindrical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_cylindrical_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CYLINDRICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-1, JOINT_QMIN])
self.assertEqual(builder_usd.joints[0].q_j_max, [1, JOINT_QMAX])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0, 200.0])
def test_import_joint_universal_passive_unary(self):
"""Test importing a passive universal joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_universal_passive_unary.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.UNIVERSAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, -1)
self.assertEqual(builder_usd.joints[0].bid_F, 0)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-0.5 * math.pi, -0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [0.5 * math.pi, 0.5 * math.pi])
def test_import_joint_universal_passive(self):
"""Test importing a passive universal joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_universal_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.UNIVERSAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-0.5 * math.pi, -0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [0.5 * math.pi, 0.5 * math.pi])
def test_import_joint_universal_actuated(self):
"""Test importing a actuated universal joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_universal_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.UNIVERSAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-0.5 * math.pi, -0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [0.5 * math.pi, 0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0, 200.0])
def test_import_joint_cartesian_passive_unary(self):
"""Test importing a passive cylindrical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_cartesian_passive_unary.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CARTESIAN)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, -1)
self.assertEqual(builder_usd.joints[0].bid_F, 0)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [-10.0, -20.0, -30.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [10.0, 20.0, 30.0])
def test_import_joint_cartesian_passive(self):
"""Test importing a passive cylindrical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_cartesian_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CARTESIAN)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [-10.0, -20.0, -30.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [10.0, 20.0, 30.0])
def test_import_joint_cartesian_actuated(self):
"""Test importing a actuated cylindrical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_cartesian_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CARTESIAN)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [-10.0, -20.0, -30.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [10.0, 20.0, 30.0])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0, 200.0, 300.0])
###
# Joints based on UsdPhysicsD6Joint
###
def test_import_joint_d6_revolute_passive(self):
"""Test importing a passive revolute joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_revolute_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.REVOLUTE)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [math.pi])
def test_import_joint_d6_revolute_actuated(self):
"""Test importing a actuated revolute joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_revolute_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.REVOLUTE)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [math.pi])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0])
def test_import_joint_d6_prismatic_passive(self):
"""Test importing a passive prismatic joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_prismatic_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.PRISMATIC)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-10.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [10.0])
def test_import_joint_d6_prismatic_actuated(self):
"""Test importing a actuated prismatic joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_prismatic_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.PRISMATIC)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 1)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 1)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 1)
self.assertEqual(builder_usd.joints[0].q_j_min, [-10.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [10.0])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0])
def test_import_joint_d6_cylindrical_passive(self):
"""Test importing a passive cylindrical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_cylindrical_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CYLINDRICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-1.0, JOINT_QMIN])
self.assertEqual(builder_usd.joints[0].q_j_max, [1.0, JOINT_QMAX])
def test_import_joint_d6_cylindrical_actuated(self):
"""Test importing a actuated cylindrical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_cylindrical_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CYLINDRICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-1.0, JOINT_QMIN])
self.assertEqual(builder_usd.joints[0].q_j_max, [1.0, JOINT_QMAX])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0, 200.0])
def test_import_joint_d6_universal_passive(self):
"""Test importing a passive universal joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_universal_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.UNIVERSAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-0.5 * math.pi, -0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [0.5 * math.pi, 0.5 * math.pi])
def test_import_joint_d6_universal_actuated(self):
"""Test importing a actuated universal joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_universal_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.UNIVERSAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 2)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 2)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 2)
self.assertEqual(builder_usd.joints[0].q_j_min, [-0.5 * math.pi, -0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [0.5 * math.pi, 0.5 * math.pi])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0, 200.0])
def test_import_joint_d6_cartesian_passive(self):
"""Test importing a passive cartesian joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_cartesian_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CARTESIAN)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [-10.0, -20.0, -30.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [10.0, 20.0, 30.0])
def test_importjoint__d6_cartesian_actuated(self):
"""Test importing a actuated cartesian joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_cartesian_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.CARTESIAN)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [-10.0, -20.0, -30.0])
self.assertEqual(builder_usd.joints[0].q_j_max, [10.0, 20.0, 30.0])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0, 200.0, 300.0])
def test_import_joint_d6_spherical_passive(self):
"""Test importing a passive spherical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_spherical_passive.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.SPHERICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [-math.pi, -math.pi, -math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [math.pi, math.pi, math.pi])
def test_import_joint_d6_spherical_actuated(self):
"""Test importing a actuated spherical joint with limits from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "joints/test_joint_d6_spherical_actuated.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 2)
self.assertEqual(builder_usd.num_joints, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.SPHERICAL)
self.assertEqual(builder_usd.joints[0].wid, 0)
self.assertEqual(builder_usd.joints[0].jid, 0)
self.assertEqual(builder_usd.joints[0].cts_offset, 0)
self.assertEqual(builder_usd.joints[0].dofs_offset, 0)
self.assertEqual(builder_usd.joints[0].bid_B, 0)
self.assertEqual(builder_usd.joints[0].bid_F, 1)
self.assertEqual(len(builder_usd.joints[0].q_j_min), 3)
self.assertEqual(len(builder_usd.joints[0].q_j_max), 3)
self.assertEqual(len(builder_usd.joints[0].tau_j_max), 3)
self.assertEqual(builder_usd.joints[0].q_j_min, [-math.pi, -math.pi, -math.pi])
self.assertEqual(builder_usd.joints[0].q_j_max, [math.pi, math.pi, math.pi])
self.assertEqual(builder_usd.joints[0].tau_j_max, [100.0, 200.0, 300.0])
###
# Primitive geometries/shapes
###
def test_import_geom_capsule(self):
"""Test importing a body with geometric primitive capsule shape from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "geoms/test_geom_capsule.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 0)
self.assertEqual(builder_usd.num_geoms, 2)
# Visual geoms are loaded first
self.assertEqual(builder_usd.geoms[0].wid, 0)
self.assertEqual(builder_usd.geoms[0].gid, 0)
self.assertEqual(builder_usd.geoms[0].body, 0)
self.assertEqual(builder_usd.geoms[0].shape.type, GeoType.CAPSULE)
self.assertAlmostEqual(builder_usd.geoms[0].shape.radius, 0.2)
self.assertAlmostEqual(builder_usd.geoms[0].shape.half_height, 1.65)
self.assertEqual(builder_usd.geoms[0].mid, -1)
self.assertEqual(builder_usd.geoms[0].group, 0)
self.assertEqual(builder_usd.geoms[0].collides, 0)
self.assertEqual(builder_usd.geoms[0].max_contacts, 0)
# Collidable geoms are loaded after visual geoms
self.assertEqual(builder_usd.geoms[1].wid, 0)
self.assertEqual(builder_usd.geoms[1].gid, 1)
self.assertEqual(builder_usd.geoms[1].body, 0)
self.assertEqual(builder_usd.geoms[1].shape.type, GeoType.CAPSULE)
self.assertAlmostEqual(builder_usd.geoms[1].shape.radius, 0.1)
self.assertAlmostEqual(builder_usd.geoms[1].shape.half_height, 1.1)
self.assertEqual(builder_usd.geoms[1].mid, 0)
self.assertEqual(builder_usd.geoms[1].group, 1)
self.assertEqual(builder_usd.geoms[1].collides, 1)
self.assertEqual(builder_usd.geoms[1].max_contacts, 10)
def test_import_geom_cone(self):
"""Test importing a body with geometric primitive cone shape from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "geoms/test_geom_cone.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 0)
self.assertEqual(builder_usd.num_geoms, 2)
# Visual geoms are loaded first
self.assertEqual(builder_usd.geoms[0].wid, 0)
self.assertEqual(builder_usd.geoms[0].gid, 0)
self.assertEqual(builder_usd.geoms[0].body, 0)
self.assertEqual(builder_usd.geoms[0].shape.type, GeoType.CONE)
self.assertAlmostEqual(builder_usd.geoms[0].shape.radius, 0.2)
self.assertAlmostEqual(builder_usd.geoms[0].shape.half_height, 1.65)
self.assertEqual(builder_usd.geoms[0].mid, -1)
self.assertEqual(builder_usd.geoms[0].group, 0)
self.assertEqual(builder_usd.geoms[0].collides, 0)
self.assertEqual(builder_usd.geoms[0].max_contacts, 0)
# Collidable geoms are loaded after visual geoms
self.assertEqual(builder_usd.geoms[1].wid, 0)
self.assertEqual(builder_usd.geoms[1].gid, 1)
self.assertEqual(builder_usd.geoms[1].body, 0)
self.assertEqual(builder_usd.geoms[1].shape.type, GeoType.CONE)
self.assertAlmostEqual(builder_usd.geoms[1].shape.radius, 0.1)
self.assertAlmostEqual(builder_usd.geoms[1].shape.half_height, 1.1)
self.assertEqual(builder_usd.geoms[1].mid, 0)
self.assertEqual(builder_usd.geoms[1].group, 1)
self.assertEqual(builder_usd.geoms[1].collides, 1)
self.assertEqual(builder_usd.geoms[1].max_contacts, 10)
def test_import_geom_cylinder(self):
"""Test importing a body with geometric primitive cylinder shape from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "geoms/test_geom_cylinder.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 0)
self.assertEqual(builder_usd.num_geoms, 2)
# Visual geoms are loaded first
self.assertEqual(builder_usd.geoms[0].wid, 0)
self.assertEqual(builder_usd.geoms[0].gid, 0)
self.assertEqual(builder_usd.geoms[0].body, 0)
self.assertEqual(builder_usd.geoms[0].shape.type, GeoType.CYLINDER)
self.assertAlmostEqual(builder_usd.geoms[0].shape.radius, 0.2)
self.assertAlmostEqual(builder_usd.geoms[0].shape.half_height, 1.65)
self.assertEqual(builder_usd.geoms[0].mid, -1)
self.assertEqual(builder_usd.geoms[0].group, 0)
self.assertEqual(builder_usd.geoms[0].collides, 0)
self.assertEqual(builder_usd.geoms[0].max_contacts, 0)
# Collidable geoms are loaded after visual geoms
self.assertEqual(builder_usd.geoms[1].wid, 0)
self.assertEqual(builder_usd.geoms[1].gid, 1)
self.assertEqual(builder_usd.geoms[1].body, 0)
self.assertEqual(builder_usd.geoms[1].shape.type, GeoType.CYLINDER)
self.assertAlmostEqual(builder_usd.geoms[1].shape.radius, 0.1)
self.assertAlmostEqual(builder_usd.geoms[1].shape.half_height, 1.1)
self.assertEqual(builder_usd.geoms[1].mid, 0)
self.assertEqual(builder_usd.geoms[1].group, 1)
self.assertEqual(builder_usd.geoms[1].collides, 1)
self.assertEqual(builder_usd.geoms[1].max_contacts, 10)
def test_import_geom_sphere(self):
"""Test importing a body with geometric primitive sphere shape from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "geoms/test_geom_sphere.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 0)
self.assertEqual(builder_usd.num_geoms, 2)
# Visual geoms are loaded first
self.assertEqual(builder_usd.geoms[0].wid, 0)
self.assertEqual(builder_usd.geoms[0].gid, 0)
self.assertEqual(builder_usd.geoms[0].body, 0)
self.assertEqual(builder_usd.geoms[0].shape.type, GeoType.SPHERE)
self.assertAlmostEqual(builder_usd.geoms[0].shape.radius, 0.22)
self.assertEqual(builder_usd.geoms[0].mid, -1)
self.assertEqual(builder_usd.geoms[0].group, 0)
self.assertEqual(builder_usd.geoms[0].collides, 0)
self.assertEqual(builder_usd.geoms[0].max_contacts, 0)
# Collidable geoms are loaded after visual geoms
self.assertEqual(builder_usd.geoms[1].wid, 0)
self.assertEqual(builder_usd.geoms[1].gid, 1)
self.assertEqual(builder_usd.geoms[1].body, 0)
self.assertEqual(builder_usd.geoms[1].shape.type, GeoType.SPHERE)
self.assertAlmostEqual(builder_usd.geoms[1].shape.radius, 0.11)
self.assertEqual(builder_usd.geoms[1].mid, 0)
self.assertEqual(builder_usd.geoms[1].group, 1)
self.assertEqual(builder_usd.geoms[1].collides, 1)
self.assertEqual(builder_usd.geoms[1].max_contacts, 10)
def test_import_geom_ellipsoid(self):
"""Test importing a body with geometric primitive ellipsoid shape from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "geoms/test_geom_ellipsoid.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 0)
self.assertEqual(builder_usd.num_geoms, 2)
# Visual geoms are loaded first
self.assertEqual(builder_usd.geoms[0].wid, 0)
self.assertEqual(builder_usd.geoms[0].gid, 0)
self.assertEqual(builder_usd.geoms[0].body, 0)
self.assertEqual(builder_usd.geoms[0].shape.type, GeoType.ELLIPSOID)
self.assertAlmostEqual(builder_usd.geoms[0].shape.a, 0.22)
self.assertAlmostEqual(builder_usd.geoms[0].shape.b, 0.33)
self.assertAlmostEqual(builder_usd.geoms[0].shape.c, 0.44)
self.assertEqual(builder_usd.geoms[0].mid, -1)
self.assertEqual(builder_usd.geoms[0].group, 0)
self.assertEqual(builder_usd.geoms[0].collides, 0)
self.assertEqual(builder_usd.geoms[0].max_contacts, 0)
# Collidable geoms are loaded after visual geoms
self.assertEqual(builder_usd.geoms[1].wid, 0)
self.assertEqual(builder_usd.geoms[1].gid, 1)
self.assertEqual(builder_usd.geoms[1].body, 0)
self.assertEqual(builder_usd.geoms[1].shape.type, GeoType.ELLIPSOID)
self.assertAlmostEqual(builder_usd.geoms[1].shape.a, 0.11)
self.assertAlmostEqual(builder_usd.geoms[1].shape.b, 0.22)
self.assertAlmostEqual(builder_usd.geoms[1].shape.c, 0.33)
self.assertEqual(builder_usd.geoms[1].mid, 0)
self.assertEqual(builder_usd.geoms[1].group, 1)
self.assertEqual(builder_usd.geoms[1].collides, 1)
self.assertEqual(builder_usd.geoms[1].max_contacts, 10)
def test_import_geom_box(self):
"""Test importing a body with geometric primitive box shape from a USD file"""
usd_asset_filename = os.path.join(self.TEST_USD_ASSETS_PATH, "geoms/test_geom_box.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=usd_asset_filename)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 1)
self.assertEqual(builder_usd.num_joints, 0)
self.assertEqual(builder_usd.num_geoms, 2)
# Visual geoms are loaded first
self.assertEqual(builder_usd.geoms[0].wid, 0)
self.assertEqual(builder_usd.geoms[0].gid, 0)
self.assertEqual(builder_usd.geoms[0].body, 0)
self.assertEqual(builder_usd.geoms[0].shape.type, GeoType.BOX)
self.assertAlmostEqual(builder_usd.geoms[0].shape.hx, 0.111)
self.assertAlmostEqual(builder_usd.geoms[0].shape.hy, 0.222)
self.assertAlmostEqual(builder_usd.geoms[0].shape.hz, 0.333)
self.assertEqual(builder_usd.geoms[0].mid, -1)
self.assertEqual(builder_usd.geoms[0].group, 0)
self.assertEqual(builder_usd.geoms[0].collides, 0)
self.assertEqual(builder_usd.geoms[0].max_contacts, 0)
# Collidable geoms are loaded after visual geoms
self.assertEqual(builder_usd.geoms[1].wid, 0)
self.assertEqual(builder_usd.geoms[1].gid, 1)
self.assertEqual(builder_usd.geoms[1].body, 0)
self.assertEqual(builder_usd.geoms[1].shape.type, GeoType.BOX)
self.assertAlmostEqual(builder_usd.geoms[1].shape.hx, 0.11)
self.assertAlmostEqual(builder_usd.geoms[1].shape.hy, 0.22)
self.assertAlmostEqual(builder_usd.geoms[1].shape.hz, 0.33)
self.assertEqual(builder_usd.geoms[1].mid, 0)
self.assertEqual(builder_usd.geoms[1].group, 1)
self.assertEqual(builder_usd.geoms[1].collides, 1)
self.assertEqual(builder_usd.geoms[1].max_contacts, 10)
###
# Basic models
###
def test_import_basic_box_on_plane(self):
"""Test importing the basic box_on_plane model from a USD file"""
# Construct a builder from imported USD asset
usd_asset_filename = os.path.join(self.BASICS_USD_ASSETS_PATH, "box_on_plane.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(
source=usd_asset_filename, load_static_geometry=False, load_materials=False
)
# Construct a reference builder using the basics generators
builder_ref = basics.build_box_on_plane(ground=False)
# Check the loaded contents against the reference builder
assert_builders_equal(self, builder_usd, builder_ref, skip_materials=True)
def test_import_basic_box_pendulum(self):
"""Test importing the basic box_pendulum model from a USD file"""
# Construct a builder from imported USD asset
usd_asset_filename = os.path.join(self.BASICS_USD_ASSETS_PATH, "box_pendulum.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(
source=usd_asset_filename, load_static_geometry=False, load_materials=False
)
# Construct a reference builder using the basics generators
builder_ref = basics.build_box_pendulum(ground=False)
# Check the loaded contents against the reference builder
assert_builders_equal(self, builder_usd, builder_ref, skip_materials=True)
def test_import_basic_boxes_hinged(self):
"""Test importing the basic boxes_hinged model from a USD file"""
# Construct a builder from imported USD asset
usd_asset_filename = os.path.join(self.BASICS_USD_ASSETS_PATH, "boxes_hinged.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(
source=usd_asset_filename, load_static_geometry=False, load_materials=False
)
# Construct a reference builder using the basics generators
builder_ref = basics.build_boxes_hinged(ground=False)
# Check the loaded contents against the reference builder
assert_builders_equal(self, builder_usd, builder_ref, skip_colliders=True, skip_materials=True)
def test_import_basic_boxes_nunchaku(self):
"""Test importing the basic boxes_nunchaku model from a USD file"""
# Construct a builder from imported USD asset
usd_asset_filename = os.path.join(self.BASICS_USD_ASSETS_PATH, "boxes_nunchaku.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(
source=usd_asset_filename, load_static_geometry=False, load_materials=False
)
# Construct a reference builder using the basics generators
builder_ref = basics.build_boxes_nunchaku(ground=False)
# Check the loaded contents against the reference builder
assert_builders_equal(self, builder_usd, builder_ref, skip_colliders=True, skip_materials=True)
def test_import_basic_boxes_fourbar(self):
"""Test importing the basic boxes_fourbar model from a USD file"""
# Construct a builder from imported USD asset
usd_asset_filename = os.path.join(self.BASICS_USD_ASSETS_PATH, "boxes_fourbar.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(
source=usd_asset_filename, load_static_geometry=False, load_materials=False
)
# Construct a reference builder using the basics generators
builder_ref = basics.build_boxes_fourbar(ground=False)
# Check the loaded contents against the reference builder
assert_builders_equal(self, builder_usd, builder_ref, skip_materials=True)
def test_import_basic_cartpole(self):
"""Test importing the basic cartpole model from a USD file"""
# Construct a builder from imported USD asset
usd_asset_filename = os.path.join(self.BASICS_USD_ASSETS_PATH, "cartpole.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(
source=usd_asset_filename, load_static_geometry=True, load_materials=False
)
# Construct a reference builder using the basics generators
builder_ref = basics.build_cartpole(z_offset=0.0, ground=False)
# Check the loaded contents against the reference builder
assert_builders_equal(self, builder_usd, builder_ref, skip_materials=True)
###
# Reference models
###
def test_import_model_dr_testmech(self):
"""Test importing the `DR Test Mechanism` example model with all joint types from a USD file"""
print("") # Add a newline for better readability
# Load the DR Test Mechanism model from the `newton-assets` repository
asset_path = newton.utils.download_asset("disneyresearch")
model_asset_file = str(asset_path / "dr_testmech" / "usd" / "dr_testmech.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=model_asset_file)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 10)
self.assertEqual(builder_usd.num_joints, 14)
self.assertEqual(builder_usd.num_geoms, 10)
self.assertEqual(builder_usd.num_materials, 1)
self.assertEqual(builder_usd.joints[0].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[0].dof_type, JointDoFType.FIXED)
self.assertEqual(builder_usd.joints[1].act_type, JointActuationType.FORCE)
self.assertEqual(builder_usd.joints[1].dof_type, JointDoFType.REVOLUTE)
self.assertEqual(builder_usd.joints[2].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[2].dof_type, JointDoFType.SPHERICAL)
self.assertEqual(builder_usd.joints[3].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[3].dof_type, JointDoFType.UNIVERSAL)
self.assertEqual(builder_usd.joints[4].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[4].dof_type, JointDoFType.SPHERICAL)
self.assertEqual(builder_usd.joints[5].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[5].dof_type, JointDoFType.REVOLUTE)
self.assertEqual(builder_usd.joints[6].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[6].dof_type, JointDoFType.UNIVERSAL)
self.assertEqual(builder_usd.joints[7].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[7].dof_type, JointDoFType.SPHERICAL)
self.assertEqual(builder_usd.joints[8].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[8].dof_type, JointDoFType.CYLINDRICAL)
self.assertEqual(builder_usd.joints[9].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[9].dof_type, JointDoFType.REVOLUTE)
self.assertEqual(builder_usd.joints[10].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[10].dof_type, JointDoFType.PRISMATIC)
self.assertEqual(builder_usd.joints[11].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[11].dof_type, JointDoFType.FIXED)
self.assertEqual(builder_usd.joints[12].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[12].dof_type, JointDoFType.SPHERICAL)
self.assertEqual(builder_usd.joints[13].act_type, JointActuationType.PASSIVE)
self.assertEqual(builder_usd.joints[13].dof_type, JointDoFType.CARTESIAN)
def test_import_model_dr_legs(self):
"""Test importing the `DR Legs` example model from a USD file"""
print("") # Add a newline for better readability
# Load the default DR Legs model from the `newton-assets` repository
asset_path = newton.utils.download_asset("disneyresearch")
model_asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=model_asset_file)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 31)
self.assertEqual(builder_usd.num_joints, 36)
self.assertEqual(builder_usd.num_geoms, 31)
def test_import_model_dr_legs_with_boxes(self):
"""Test importing the `DR Legs` example model from a USD file"""
print("") # Add a newline for better readability
# Load the primitives-only DR Legs model from the `newton-assets` repository
asset_path = newton.utils.download_asset("disneyresearch")
model_asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_boxes.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=model_asset_file)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 31)
self.assertEqual(builder_usd.num_joints, 36)
self.assertEqual(builder_usd.num_geoms, 3)
def test_import_model_dr_legs_with_meshes_and_boxes(self):
"""Test importing the `DR Legs` example model from a USD file"""
print("") # Add a newline for better readability
# Load the primitives-plus-meshes DR Legs model from the `newton-assets` repository
asset_path = newton.utils.download_asset("disneyresearch")
model_asset_file = str(asset_path / "dr_legs" / "usd" / "dr_legs_with_meshes_and_boxes.usda")
importer = USDImporter()
builder_usd: ModelBuilderKamino = importer.import_from(source=model_asset_file)
# Check the loaded contents
self.assertEqual(builder_usd.num_bodies, 31)
self.assertEqual(builder_usd.num_joints, 36)
self.assertEqual(builder_usd.num_geoms, 34)
class TestUSDKaminoSceneAPIImport(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
# Set the paths to the assets provided by the kamino package
self.TEST_USD_ASSETS_PATH = get_testing_usd_assets_path()
self.BASICS_USD_ASSETS_PATH = get_basics_usd_assets_path()
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def generate_single_body_usd_import(self, scene: str = "") -> Model:
from pxr import Usd
usd_text = (
"#usda 1.0\n\n"
+ scene
+ """def Xform "box"
{
def Scope "RigidBodies"
{
def Xform "box_body" (
prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
)
{
# Body Frame
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0.0, 0.0, 0.1)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
# Body Velocities
vector3f physics:linearVelocity = (0, 0, 0)
vector3f physics:angularVelocity = (0, 0, 0)
# Mass Properties
float physics:mass = 1.0
float3 physics:diagonalInertia = (0.01, 0.01, 0.01)
point3f physics:centerOfMass = (0, 0, 0)
quatf physics:principalAxes = (1, 0, 0, 0)
def Scope "Geometry"
{
def Cube "box_geom" (
prepend apiSchemas = ["PhysicsCollisionAPI"]
)
{
float3[] extent = [(-1, -1, -1), (1, 1, 1)]
float3 xformOp:scale = (0.1, 0.1, 0.1)
quatf xformOp:orient = (1, 0, 0, 0)
double3 xformOp:translate = (0, 0, 0)
uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
}
}
}
}
}
"""
)
stage = Usd.Stage.CreateInMemory()
stage.GetRootLayer().ImportFromString(usd_text)
builder = ModelBuilder()
SolverKamino.register_custom_attributes(builder)
builder.add_usd(stage)
model = builder.finalize()
return model
@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
def test_01_kamino_scene_api_import_no_scene(self):
"""Check that custom attributes are set with the right type."""
model = self.generate_single_body_usd_import()
self.assertTrue(hasattr(model, "kamino"))
kamino_attr = model.kamino
# Check existence and type of attributes
self.assertTrue(
hasattr(kamino_attr, "padmm_warmstarting") and isinstance(kamino_attr.padmm_warmstarting[0], str)
)
self.assertTrue(
hasattr(kamino_attr, "padmm_use_acceleration")
and isinstance(kamino_attr.padmm_use_acceleration.numpy()[0], np.bool)
)
self.assertTrue(hasattr(kamino_attr, "joint_correction") and isinstance(kamino_attr.joint_correction[0], str))
self.assertTrue(
hasattr(kamino_attr, "constraints_use_preconditioning")
and isinstance(kamino_attr.constraints_use_preconditioning.numpy()[0], np.bool)
)
self.assertTrue(
hasattr(kamino_attr, "constraints_alpha")
and isinstance(kamino_attr.constraints_alpha.numpy()[0], np.floating)
)
self.assertTrue(
hasattr(kamino_attr, "constraints_beta")
and isinstance(kamino_attr.constraints_beta.numpy()[0], np.floating)
)
self.assertTrue(
hasattr(kamino_attr, "constraints_gamma")
and isinstance(kamino_attr.constraints_gamma.numpy()[0], np.floating)
)
self.assertTrue(
hasattr(kamino_attr, "padmm_primal_tolerance")
and isinstance(kamino_attr.padmm_primal_tolerance.numpy()[0], np.floating)
)
self.assertTrue(
hasattr(kamino_attr, "padmm_dual_tolerance")
and isinstance(kamino_attr.padmm_dual_tolerance.numpy()[0], np.floating)
)
self.assertTrue(
hasattr(kamino_attr, "padmm_complementarity_tolerance")
and isinstance(kamino_attr.padmm_complementarity_tolerance.numpy()[0], np.floating)
)
self.assertTrue(
hasattr(kamino_attr, "max_solver_iterations")
and isinstance(kamino_attr.max_solver_iterations.numpy()[0], np.integer)
)
# Compare attribute values to KaminoSceneAPI defaults
self.assertEqual(kamino_attr.padmm_warmstarting[0], "containers")
self.assertEqual(bool(kamino_attr.padmm_use_acceleration.numpy()[0]), True)
self.assertEqual(kamino_attr.joint_correction[0], "twopi")
self.assertEqual(bool(kamino_attr.constraints_use_preconditioning.numpy()[0]), True)
self.assertAlmostEqual(kamino_attr.constraints_alpha.numpy()[0], 0.01)
self.assertAlmostEqual(kamino_attr.constraints_beta.numpy()[0], 0.01)
self.assertAlmostEqual(kamino_attr.constraints_gamma.numpy()[0], 0.01)
self.assertAlmostEqual(kamino_attr.padmm_primal_tolerance.numpy()[0], 1e-6)
self.assertAlmostEqual(kamino_attr.padmm_dual_tolerance.numpy()[0], 1e-6)
self.assertAlmostEqual(kamino_attr.padmm_complementarity_tolerance.numpy()[0], 1e-6)
self.assertEqual(kamino_attr.max_solver_iterations.numpy()[0], -1)
@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
def test_02_kamino_scene_api_import_full_scene(self):
"""Check that values defined in USD are properly imported."""
model = self.generate_single_body_usd_import("""
def PhysicsScene "PhysicsScene" (
prepend apiSchemas = ["NewtonKaminoSceneAPI"]
)
{
uniform int newton:maxSolverIterations = 10
uniform float newton:kamino:padmm:primalTolerance = 0.1
uniform float newton:kamino:padmm:dualTolerance = 0.2
uniform float newton:kamino:padmm:complementarityTolerance = 0.3
uniform token newton:kamino:padmm:warmstarting = "none"
uniform bool newton:kamino:padmm:useAcceleration = false
uniform bool newton:kamino:constraints:usePreconditioning = false
uniform float newton:kamino:constraints:alpha = 0.4
uniform float newton:kamino:constraints:beta = 0.5
uniform float newton:kamino:constraints:gamma = 0.6
uniform token newton:kamino:jointCorrection = "continuous"
}
""")
self.assertTrue(hasattr(model, "kamino"))
kamino_attr = model.kamino
self.assertEqual(kamino_attr.padmm_warmstarting[0], "none")
self.assertEqual(bool(kamino_attr.padmm_use_acceleration.numpy()[0]), False)
self.assertEqual(kamino_attr.joint_correction[0], "continuous")
self.assertEqual(bool(kamino_attr.constraints_use_preconditioning.numpy()[0]), False)
self.assertAlmostEqual(kamino_attr.constraints_alpha.numpy()[0], 0.4)
self.assertAlmostEqual(kamino_attr.constraints_beta.numpy()[0], 0.5)
self.assertAlmostEqual(kamino_attr.constraints_gamma.numpy()[0], 0.6)
self.assertAlmostEqual(kamino_attr.padmm_primal_tolerance.numpy()[0], 0.1)
self.assertAlmostEqual(kamino_attr.padmm_dual_tolerance.numpy()[0], 0.2)
self.assertAlmostEqual(kamino_attr.padmm_complementarity_tolerance.numpy()[0], 0.3)
self.assertEqual(kamino_attr.max_solver_iterations.numpy()[0], 10)
@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
def test_03_kamino_scene_api_import_faulty_scenes(self):
"""Check that faulty string attributes raise an error."""
with self.assertRaises(ValueError):
self.generate_single_body_usd_import("""
def PhysicsScene "PhysicsScene" (
prepend apiSchemas = ["NewtonKaminoSceneAPI"]
)
{
uniform token newton:kamino:padmm:warmstarting = "non"
}
""")
with self.assertRaises(ValueError):
self.generate_single_body_usd_import("""
def PhysicsScene "PhysicsScene" (
prepend apiSchemas = ["NewtonKaminoSceneAPI"]
)
{
uniform token newton:kamino:jointCorrection = "discrete"
}
""")
@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
def test_04_kamino_scene_api_import_full_scene_config(self):
"""Check that values defined in USD are properly imported."""
model = self.generate_single_body_usd_import("""
def PhysicsScene "PhysicsScene" (
prepend apiSchemas = ["NewtonKaminoSceneAPI"]
)
{
uniform int newton:maxSolverIterations = 10
uniform float newton:kamino:padmm:primalTolerance = 0.1
uniform float newton:kamino:padmm:dualTolerance = 0.2
uniform float newton:kamino:padmm:complementarityTolerance = 0.3
uniform token newton:kamino:padmm:warmstarting = "none"
uniform bool newton:kamino:padmm:useAcceleration = false
uniform bool newton:kamino:constraints:usePreconditioning = false
uniform float newton:kamino:constraints:alpha = 0.4
uniform float newton:kamino:constraints:beta = 0.5
uniform float newton:kamino:constraints:gamma = 0.6
uniform token newton:kamino:jointCorrection = "continuous"
}
""")
config = SolverKamino.Config.from_model(model)
self.assertEqual(config.rotation_correction, "continuous")
self.assertEqual(config.dynamics.preconditioning, False)
self.assertAlmostEqual(config.constraints.alpha, 0.4)
self.assertAlmostEqual(config.constraints.beta, 0.5)
self.assertAlmostEqual(config.constraints.gamma, 0.6)
self.assertEqual(config.padmm.max_iterations, 10)
self.assertAlmostEqual(config.padmm.primal_tolerance, 0.1)
self.assertAlmostEqual(config.padmm.dual_tolerance, 0.2)
self.assertAlmostEqual(config.padmm.compl_tolerance, 0.3)
self.assertEqual(config.padmm.use_acceleration, False)
self.assertEqual(config.padmm.warmstart_mode, "none")
@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
def test_05_kamino_scene_api_import_full_scene_solver(self):
"""Check that values defined in USD are properly imported."""
model = self.generate_single_body_usd_import("""
def PhysicsScene "PhysicsScene" (
prepend apiSchemas = ["NewtonKaminoSceneAPI"]
)
{
uniform int newton:maxSolverIterations = 10
uniform float newton:kamino:padmm:primalTolerance = 0.1
uniform float newton:kamino:padmm:dualTolerance = 0.2
uniform float newton:kamino:padmm:complementarityTolerance = 0.3
uniform token newton:kamino:padmm:warmstarting = "none"
uniform bool newton:kamino:padmm:useAcceleration = false
uniform bool newton:kamino:constraints:usePreconditioning = false
uniform float newton:kamino:constraints:alpha = 0.4
uniform float newton:kamino:constraints:beta = 0.5
uniform float newton:kamino:constraints:gamma = 0.6
uniform token newton:kamino:jointCorrection = "continuous"
}
""")
solver = SolverKamino(model)
config = solver._solver_kamino.config
self.assertEqual(config.rotation_correction, "continuous")
self.assertAlmostEqual(config.constraints.alpha, 0.4)
self.assertAlmostEqual(config.constraints.beta, 0.5)
self.assertAlmostEqual(config.constraints.gamma, 0.6)
self.assertEqual(config.dynamics.preconditioning, False)
self.assertEqual(config.padmm.max_iterations, 10)
self.assertAlmostEqual(config.padmm.primal_tolerance, 0.1)
self.assertAlmostEqual(config.padmm.dual_tolerance, 0.2)
self.assertAlmostEqual(config.padmm.compl_tolerance, 0.3)
self.assertEqual(config.padmm.use_acceleration, False)
self.assertEqual(config.padmm.warmstart_mode, "none")
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_utils_logger.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Kamino: Tests for logging utilities"""
import unittest
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.logger import Logger
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestUtilsLogger(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
def test_new_logger(self):
"""Test use of the custom logger."""
print("") # Print a newline for better readability in the output
msg.set_log_level(msg.LogLevel.DEBUG)
logger = Logger()
log = logger.get()
log.debug("This is a debug message.")
log.info("This is an info message.")
log.warning("This is a warning message.")
log.error("This is an error message.")
log.critical("This is a critical message.")
msg.reset_log_level()
def test_default_logger(self):
"""Test use of the custom logger."""
print("") # Print a newline for better readability in the output
msg.set_log_level(msg.LogLevel.DEBUG)
msg.debug("This is a debug message.")
msg.info("This is an info message.")
msg.notif("This is a notification message.")
msg.warning("This is a warning message.")
msg.error("This is an error message.")
msg.critical("This is a critical message.")
msg.reset_log_level()
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_utils_profiles.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Kamino: Tests for performance-profiles utilities"""
import unittest
import numpy as np
import newton._src.solvers.kamino._src.utils.profiles as profiles
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestUtilsPerformanceProfiles(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for verbose output
self.plots = test_context.verbose # Set to True to generate plots
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
if self.verbose:
msg.reset_log_level()
def test_01_perfprof_minimal_data(self):
# ns = 2 solvers, np = 1 problem
ns, np_ = 2, 1
data = np.zeros((ns, np_), dtype=float)
data[0, :] = [1.0] # Solver A
data[1, :] = [2.0] # Solver B
# Create a performance profile (taumax = 1.0)
pp = profiles.PerformanceProfile(data, taumax=1.0)
self.assertTrue(pp.is_valid)
# Optional plot
if self.plots:
pp.plot(["Solver A", "Solver B"]) # visual sanity check
def test_02_perfprof_tmigot_ex2(self):
# Example from https://tmigot.github.io/posts/2024/06/teaching/
ns, np_ = 2, 8
data = np.zeros((ns, np_), dtype=float)
data[0, :] = [1.0, 1.0, 1.0, 5.0, 7.0, 6.0, np.inf, np.inf] # Solver A
data[1, :] = [5.0, 10.0, 20.0, 10.0, 15.0, 5.0, 20.0, 20.0] # Solver B
pp = profiles.PerformanceProfile(data, taumax=np.inf)
self.assertTrue(pp.is_valid)
if self.plots:
pp.plot(["Solver A", "Solver B"]) # visual sanity check
def test_03_perfprof_tmigot_ex3(self):
# Example from https://tmigot.github.io/posts/2024/06/teaching/
ns, np_ = 2, 5
data = np.zeros((ns, np_), dtype=float)
data[0, :] = [1.0, 1.0, 1.0, 1.0, 1.0] # Solver A
data[1, :] = [1.0003, 1.0003, 1.0003, 1.0003, 1.0003] # Solver B
pp = profiles.PerformanceProfile(data, taumax=1.0005)
self.assertTrue(pp.is_valid)
if self.plots:
pp.plot(["Solver A", "Solver B"]) # visual sanity check
def test_04_perfprof_tmigot_ex4(self):
# Example from https://tmigot.github.io/posts/2024/06/teaching/
ns, np_ = 3, 5
data = np.zeros((ns, np_), dtype=float)
data[0, :] = [2.0, 1.0, 1.0, 1.0, 2.0] # Solver A
data[1, :] = [1.5, 1.2, 4.0, 5.0, 5.0] # Solver B
data[2, :] = [1.0, 2.0, 2.0, 20.0, 20.0] # Solver C
pp = profiles.PerformanceProfile(data, taumax=np.inf)
self.assertTrue(pp.is_valid)
if self.plots:
pp.plot(["Solver A", "Solver B", "Solver C"]) # visual sanity check
def test_05_perfprof_example_large(self):
# From perfprof.csv
data = [
[32.0, 15.0, 7.0, 9.0],
[547.0, 338.0, 196.0, 1082.0],
[11.0, 11.0, 18.0, 112.0],
[93.0, 102.0, 20.0, 3730.0],
[40.0, 38.0, 91.0, 74.0],
[1599.0, 1638.0, 3202.0, 2700.0],
[30.0, 56.0, 274.0, 75.0],
[30.0, 56.0, 274.0, 75.0],
[384.0, 361.0, 574.0, 843.0],
[19.0, 18.0, 18.0, 18.0],
[91.0, 87.0, 227.0, 374.0],
[65339.0, 49665.0, 58191.0, np.inf],
[np.inf, 68103.0, np.inf, np.inf],
[12.0, 12.0, 18.0, 13.0],
[12.0, 12.0, 15.0, 12.0],
[13.0, 13.0, 16.0, 14.0],
[15.0, 15.0, 19.0, 15.0],
[158.0, 167.0, 545.0, 448.0],
[133.0, 128.0, 280.0, 403.0],
[133.0, 127.0, 279.0, 356.0],
[130.0, 126.0, 250.0, 331.0],
[332.0, 286.0, 1185.0, 794.0],
[76.0, 64.0, 105.0, 130.0],
[67.0, 64.0, 125.0, 131.0],
[64.0, 57.0, 146.0, 151.0],
[313.0, 261.0, 616.0, 584.0],
[119.0, 101.0, 248.0, 388.0],
[103.0, 94.0, 250.0, 304.0],
[99.0, 88.0, 253.0, 264.0],
[1432.0, 2188.0, 1615.0, 1856.0],
[22.0, 21.0, 15.0, 205.0],
[51.0, 51.0, 76.0, 51.0],
[37.0, 40.0, 51.0, 63.0],
[5.0, 5.0, 5.0, 5.0],
[11552.0, 12992.0, 91294.0, 92516.0],
[2709.0, 3761.0, 3875.0, 5026.0],
[8639.0, 9820.0, 48442.0, 30701.0],
[19.0, 19.0, 31.0, 20.0],
[488.0, 489.0, 10311.0, 924.0],
[47.0, 50.0, 162.0, 135.0],
[5650.0, 5871.0, 15317.0, 9714.0],
[233.0, 225.0, 602.0, 828.0],
[322.0, 302.0, 1076.0, 1014.0],
[33.0, 31.0, 43.0, 194.0],
[7949.0, 10545.0, 9069.0, 7873.0],
[np.inf, np.inf, 374.0, np.inf],
[602.0, 617.0, 1835.0, 1865.0],
[26.0, 27.0, 42.0, 181.0],
[386.0, 398.0, 442.0, 938.0],
[12.0, 11.0, 13.0, 12.0],
[1438.0, 1368.0, 1462.0, 2218.0],
[1177.0, 1144.0, 1535.0, 2310.0],
[306.0, 257.0, 245.0, 915.0],
[1223.0, 1316.0, 23646.0, 1393.0],
[8093.0, 5603.0, 40011.0, 26782.0],
[89403.0, np.inf, np.inf, 62244.0],
[19.0, 22.0, 14.0, 87.0],
[404.0, 300.0, 308.0, 590.0],
[68.0, 68.0, 132.0, 96.0],
[48.0, 48.0, 48.0, 48.0],
[45.0, 38.0, 47.0, 65.0],
[357.0, 351.0, 1054.0, 840.0],
[51.0, 51.0, 76.0, 51.0],
[41.0, 40.0, 97.0, 57.0],
[40.0, 52.0, 29.0, 223.0],
[5112.0, 5119.0, np.inf, 15677.0],
[39.0, 29.0, 66.0, 60.0],
[154.0, 151.0, 351.0, 460.0],
[18.0, 17.0, 49.0, 37.0],
[8820.0, 6500.0, 53977.0, 74755.0],
[3791.0, 8193.0, 47028.0, 37111.0],
[20.0, 20.0, 28.0, 20.0],
[26.0, 24.0, 4820.0, 76.0],
[1543.0, 1254.0, 4309.0, 6017.0],
[135.0, 137.0, 171.0, 342.0],
[24.0, 23.0, 29.0, 41.0],
[30.0, 31.0, np.inf, 41.0],
]
data = np.array(data, dtype=float).T
pp = profiles.PerformanceProfile(data, taumax=np.inf)
self.assertTrue(pp.is_valid)
if self.plots:
pp.plot(["Alg1", "Alg2", "Alg3", "Alg4"]) # visual sanity check
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_utils_random.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Unit tests for random matrix and problem generation utilities in `linalg/utils/random.py`.
"""
import unittest
import numpy as np
import newton._src.solvers.kamino.tests.utils.rand as rand
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Tests
###
class TestRandomSymmetricMatrix(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for verbose output
def test_matrix_symmetry(self):
dim = 5
A = rand.random_symmetric_matrix(dim=dim)
# Verify symmetry: A should equal its transpose
np.testing.assert_array_equal(A, A.T, "Matrix is not symmetric.")
def test_matrix_rank(self):
dim = 5
rank = 3
A = rand.random_symmetric_matrix(dim=dim, rank=rank)
# Verify the rank: The rank should be equal to or less than the specified rank
actual_rank = np.linalg.matrix_rank(A)
self.assertEqual(actual_rank, rank, f"Matrix rank is {actual_rank}, expected {rank}.")
def test_matrix_eigenvalues(self):
dim = 5
eigenvalues = [1, 2, 3, 4, 5] # Expected eigenvalues
A = rand.random_symmetric_matrix(dim=dim, eigenvalues=eigenvalues)
# Compute eigenvalues of the generated matrix
actual_eigenvalues = np.linalg.eigvals(A)
# Check if the eigenvalues are close to the expected ones
np.testing.assert_allclose(
sorted(actual_eigenvalues), sorted(eigenvalues), rtol=1e-5, err_msg="Eigenvalues do not match."
)
def test_invalid_eigenvalues(self):
dim = 5
eigenvalues = [1, 2, 3] # Fewer eigenvalues than matrix dimension
with self.assertRaises(ValueError):
rand.random_symmetric_matrix(dim=dim, eigenvalues=eigenvalues)
def test_invalid_rank(self):
dim = 5
rank = 6 # Rank is greater than the dimension
with self.assertRaises(ValueError):
rand.random_symmetric_matrix(dim=dim, rank=rank)
class TestRandomProblemCholesky(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for verbose output
def test_generate_small_lower(self):
dim = 10
problem = rand.RandomProblemLLT(dims=[dim], seed=42, upper=False)
A, b = problem.A_np[0], problem.b_np[0]
# Verify the shapes of A and b
self.assertEqual(A.shape, (dim, dim), "Matrix A has incorrect shape.")
self.assertEqual(b.shape, (dim,), "Vector b has incorrect shape.")
def test_generate_small_upper(self):
dim = 10
problem = rand.RandomProblemLLT(dims=[dim], seed=42, upper=True)
A, b = problem.A_np[0], problem.b_np[0]
# Verify the shapes of A and b
self.assertEqual(A.shape, (dim, dim), "Matrix A has incorrect shape.")
self.assertEqual(b.shape, (dim,), "Vector b has incorrect shape.")
class TestRandomProblemLDLT(unittest.TestCase):
def setUp(self):
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.verbose = test_context.verbose # Set to True for verbose output
def test_generate_small_lower(self):
dim = 10
problem = rand.RandomProblemLDLT(dims=[dim], seed=42, lower=True)
A, b = problem.A_np[0], problem.b_np[0]
# Verify the shapes of A and b
self.assertEqual(A.shape, (dim, dim), "Matrix A has incorrect shape.")
self.assertEqual(b.shape, (dim,), "Vector b has incorrect shape.")
def test_generate_small_upper(self):
dim = 10
problem = rand.RandomProblemLDLT(dims=[dim], seed=42, lower=False)
A, b = problem.A_np[0], problem.b_np[0]
# Verify the shapes of A and b
self.assertEqual(A.shape, (dim, dim), "Matrix A has incorrect shape.")
self.assertEqual(b.shape, (dim,), "Vector b has incorrect shape.")
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/test_utils_sim_simulator.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit tests for the high-level Simulator class utility of Kamino"""
import time
import unittest
import numpy as np
import warp as wp
from newton._src.solvers.kamino._src.core.types import float32
from newton._src.solvers.kamino._src.models.builders.basics import build_cartpole
from newton._src.solvers.kamino._src.models.builders.utils import make_homogeneous_builder
from newton._src.solvers.kamino._src.utils import logger as msg
from newton._src.solvers.kamino._src.utils.sim.simulator import Simulator
from newton._src.solvers.kamino.examples import print_progress_bar
from newton._src.solvers.kamino.tests import setup_tests, test_context
###
# Kernels
###
@wp.kernel
def _test_control_callback(
model_dt: wp.array(dtype=float32),
data_time: wp.array(dtype=float32),
control_tau_j: wp.array(dtype=float32),
):
"""
An example control callback kernel.
"""
# Retrieve the world index from the thread ID
wid = wp.tid()
# Get the fixed time-step and current time
dt = model_dt[wid]
t = data_time[wid]
# Define the time window for the active external force profile
t_start = float32(0.0)
t_end = 10.0 * dt
# Compute the first actuated joint index for the current world
aid = wid * 2 + 0
# Apply a time-dependent external force
if t > t_start and t < t_end:
control_tau_j[aid] = 0.1
else:
control_tau_j[aid] = 0.0
###
# Launchers
###
def test_control_callback(sim: Simulator):
"""
A control callback function
"""
wp.launch(
_test_control_callback,
dim=sim.model.size.num_worlds,
inputs=[
sim.model.time.dt,
sim.solver.data.time.time,
sim.control.tau_j,
],
)
###
# Tests
###
class TestCartpoleSimulator(unittest.TestCase):
def setUp(self):
# Configs
if not test_context.setup_done:
setup_tests(clear_cache=False)
self.seed = 42
self.default_device = wp.get_device(test_context.device)
self.verbose = test_context.verbose # Set to True for verbose output
self.progress = test_context.verbose # Set to True for progress output
# Set debug-level logging to print verbose test output to console
if self.verbose:
print("\n") # Add newline before test output for better readability
msg.set_log_level(msg.LogLevel.DEBUG)
else:
msg.reset_log_level()
def tearDown(self):
self.default_device = None
if self.verbose:
msg.reset_log_level()
def test_01_step_multiple_cartpoles_all_from_initial_state(self):
"""
Test stepping multiple cartpole simulators initialized
uniformly from the default initial state multiple times.
"""
# Create a single-instance system
single_builder = build_cartpole(ground=False)
for i, body in enumerate(single_builder.bodies):
msg.info(f"[single]: [builder]: body {i}: q_i: {body.q_i_0}")
msg.info(f"[single]: [builder]: body {i}: u_i: {body.u_i_0}")
# Create simulator and check if the initial state is consistent with the contents of the builder
single_sim = Simulator(builder=single_builder, device=self.default_device)
single_sim.set_control_callback(test_control_callback)
self.assertEqual(single_sim.model.size.sum_of_num_bodies, 2)
self.assertEqual(single_sim.model.size.sum_of_num_joints, 2)
for i, body in enumerate(single_builder.bodies):
np.testing.assert_allclose(single_sim.model.bodies.q_i_0.numpy()[i], body.q_i_0)
np.testing.assert_allclose(single_sim.model.bodies.u_i_0.numpy()[i], body.u_i_0)
np.testing.assert_allclose(single_sim.state.q_i.numpy()[i], body.q_i_0)
np.testing.assert_allclose(single_sim.state.u_i.numpy()[i], body.u_i_0)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[single]: [init]: sim.model.size:\n{single_sim.model.size}\n\n")
msg.info(f"[single]: [init]: sim.model.state.q_i:\n{single_sim.state.q_i}\n\n")
msg.info(f"[single]: [init]: sim.model.state.u_i:\n{single_sim.state.u_i}\n\n")
msg.info(f"[single]: [init]: sim.model.state.q_j:\n{single_sim.state.q_j}\n\n")
msg.info(f"[single]: [init]: sim.model.state.dq_j:\n{single_sim.state.dq_j}\n\n")
# Define the total number of sample steps to collect, and the
# total number of execution steps from which to collect them
num_worlds = 42
num_steps = 1000
# Collect the initial states
initial_q_i = single_sim.state.q_i.numpy().copy()
initial_u_i = single_sim.state.u_i.numpy().copy()
initial_q_j = single_sim.state.q_j.numpy().copy()
initial_dq_j = single_sim.state.dq_j.numpy().copy()
msg.info(f"[samples]: [single]: [init]: q_i (shape={initial_q_i.shape}):\n{initial_q_i}\n")
msg.info(f"[samples]: [single]: [init]: u_i (shape={initial_u_i.shape}):\n{initial_u_i}\n")
msg.info(f"[samples]: [single]: [init]: q_j (shape={initial_q_j.shape}):\n{initial_q_j}\n")
msg.info(f"[samples]: [single]: [init]: dq_j (shape={initial_dq_j.shape}):\n{initial_dq_j}\n")
# Run the simulation for the specified number of steps
msg.info(f"[single]: Executing {num_steps} simulator steps")
start_time = time.time()
for step in range(num_steps):
# Execute a single simulation step
single_sim.step()
wp.synchronize()
if self.verbose or self.progress:
print_progress_bar(step + 1, num_steps, start_time, prefix="Progress", suffix="")
# Collect the initial and final states
final_q_i = single_sim.state.q_i.numpy().copy()
final_u_i = single_sim.state.u_i.numpy().copy()
final_q_j = single_sim.state.q_j.numpy().copy()
final_dq_j = single_sim.state.dq_j.numpy().copy()
msg.info(f"[samples]: [single]: [final]: q_i (shape={final_q_i.shape}):\n{final_q_i}\n")
msg.info(f"[samples]: [single]: [final]: u_i (shape={final_u_i.shape}):\n{final_u_i}\n")
msg.info(f"[samples]: [single]: [final]: q_j (shape={final_q_j.shape}):\n{final_q_j}\n")
msg.info(f"[samples]: [single]: [final]: dq_j (shape={final_dq_j.shape}):\n{final_dq_j}\n")
# Tile the collected states for comparison against the multi-instance simulator
multi_init_q_i = np.tile(initial_q_i, (num_worlds, 1))
multi_init_u_i = np.tile(initial_u_i, (num_worlds, 1))
multi_init_q_j = np.tile(initial_q_j, (num_worlds, 1)).reshape(-1)
multi_init_dq_j = np.tile(initial_dq_j, (num_worlds, 1)).reshape(-1)
multi_final_q_i = np.tile(final_q_i, (num_worlds, 1))
multi_final_u_i = np.tile(final_u_i, (num_worlds, 1))
multi_final_q_j = np.tile(final_q_j, (num_worlds, 1)).reshape(-1)
multi_final_dq_j = np.tile(final_dq_j, (num_worlds, 1)).reshape(-1)
msg.info(f"[samples]: [multi] [init]: q_i (shape={multi_init_q_i.shape}):\n{multi_init_q_i}\n")
msg.info(f"[samples]: [multi] [init]: u_i (shape={multi_init_u_i.shape}):\n{multi_init_u_i}\n")
msg.info(f"[samples]: [multi] [init]: q_j (shape={multi_init_q_j.shape}):\n{multi_init_q_j}\n")
msg.info(f"[samples]: [multi] [init]: dq_j (shape={multi_init_dq_j.shape}):\n{multi_init_dq_j}\n")
msg.info(f"[samples]: [multi] [final]: q_i (shape={multi_final_q_i.shape}):\n{multi_final_q_i}\n")
msg.info(f"[samples]: [multi] [final]: u_i (shape={multi_final_u_i.shape}):\n{multi_final_u_i}\n")
msg.info(f"[samples]: [multi] [final]: q_j (shape={multi_final_q_j.shape}):\n{multi_final_q_j}\n")
msg.info(f"[samples]: [multi] [final]: dq_j (shape={multi_final_dq_j.shape}):\n{multi_final_dq_j}\n")
# Create a multi-instance system by replicating the single-instance builder
multi_builder = make_homogeneous_builder(num_worlds=num_worlds, build_fn=build_cartpole, ground=False)
for i, body in enumerate(multi_builder.bodies):
msg.info(f"[multi]: [builder]: body {i}: bid: {body.bid}")
msg.info(f"[multi]: [builder]: body {i}: q_i: {body.q_i_0}")
msg.info(f"[multi]: [builder]: body {i}: u_i: {body.u_i_0}")
# Create simulator and check if the initial state is consistent with the contents of the builder
multi_sim = Simulator(builder=multi_builder, device=self.default_device)
multi_sim.set_control_callback(test_control_callback)
self.assertEqual(multi_sim.model.size.sum_of_num_bodies, single_sim.model.size.sum_of_num_bodies * num_worlds)
self.assertEqual(multi_sim.model.size.sum_of_num_joints, single_sim.model.size.sum_of_num_joints * num_worlds)
for i, body in enumerate(multi_builder.bodies):
np.testing.assert_allclose(multi_sim.model.bodies.q_i_0.numpy()[i], body.q_i_0)
np.testing.assert_allclose(multi_sim.model.bodies.u_i_0.numpy()[i], body.u_i_0)
np.testing.assert_allclose(multi_sim.state_previous.q_i.numpy()[i], body.q_i_0)
np.testing.assert_allclose(multi_sim.state_previous.u_i.numpy()[i], body.u_i_0)
np.testing.assert_allclose(multi_sim.state.q_i.numpy()[i], body.q_i_0)
np.testing.assert_allclose(multi_sim.state.u_i.numpy()[i], body.u_i_0)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [init]: sim.model.size:\n{multi_sim.model.size}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.q_i:\n{multi_sim.state_previous.q_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.u_i:\n{multi_sim.state_previous.u_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.q_j:\n{multi_sim.state_previous.q_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state_previous.dq_j:\n{multi_sim.state_previous.dq_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.q_i:\n{multi_sim.state.q_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.u_i:\n{multi_sim.state.u_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.q_j:\n{multi_sim.state.q_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.dq_j:\n{multi_sim.state.dq_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.control.tau_j:\n{multi_sim.control.tau_j}\n\n")
# Check if the multi-instance simulator has initial states matching the tiled samples
np.testing.assert_allclose(multi_sim.state_previous.q_i.numpy(), multi_init_q_i)
np.testing.assert_allclose(multi_sim.state_previous.u_i.numpy(), multi_init_u_i)
np.testing.assert_allclose(multi_sim.state.q_i.numpy(), multi_init_q_i)
np.testing.assert_allclose(multi_sim.state.u_i.numpy(), multi_init_u_i)
np.testing.assert_allclose(multi_sim.state_previous.q_j.numpy(), multi_init_q_j)
np.testing.assert_allclose(multi_sim.state_previous.dq_j.numpy(), multi_init_dq_j)
np.testing.assert_allclose(multi_sim.state.q_j.numpy(), multi_init_q_j)
np.testing.assert_allclose(multi_sim.state.dq_j.numpy(), multi_init_dq_j)
# Step the multi-instance simulator for the same number of steps
msg.info(f"[multi]: Executing {num_steps} simulator steps")
start_time = time.time()
for step in range(num_steps):
# Execute a single simulation step
multi_sim.step()
wp.synchronize()
if self.verbose or self.progress:
print_progress_bar(step + 1, num_steps, start_time, prefix="Progress", suffix="")
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [final]: sim.model.state.q_i:\n{multi_sim.state.q_i}\n\n")
msg.info(f"[multi]: [final]: sim.model.state.u_i:\n{multi_sim.state.u_i}\n\n")
msg.info(f"[multi]: [final]: sim.model.state.q_j:\n{multi_sim.state.q_j}\n\n")
msg.info(f"[multi]: [final]: sim.model.state.dq_j:\n{multi_sim.state.dq_j}\n\n")
# Check that the next states match the collected samples
np.testing.assert_allclose(multi_sim.state.q_i.numpy(), multi_final_q_i)
np.testing.assert_allclose(multi_sim.state.u_i.numpy(), multi_final_u_i)
np.testing.assert_allclose(multi_sim.state.q_j.numpy(), multi_final_q_j)
np.testing.assert_allclose(multi_sim.state.dq_j.numpy(), multi_final_dq_j)
def test_step_02_multiple_cartpoles_reset_all_from_sampled_states(self):
"""
Test stepping multiple cartpole simulators once but initialized from
states collected from a single-instance simulator over multiple steps.
"""
# Create a single-instance system
single_builder = build_cartpole(ground=False)
for i, body in enumerate(single_builder.bodies):
msg.info(f"[single]: [builder]: body {i}: q_i: {body.q_i_0}")
msg.info(f"[single]: [builder]: body {i}: u_i: {body.u_i_0}")
# Create simulator and check if the initial state is consistent with the contents of the builder
single_sim = Simulator(builder=single_builder, device=self.default_device)
single_sim.set_control_callback(test_control_callback)
self.assertEqual(single_sim.model.size.sum_of_num_bodies, 2)
self.assertEqual(single_sim.model.size.sum_of_num_joints, 2)
for i, b in enumerate(single_builder.bodies):
np.testing.assert_allclose(single_sim.model.bodies.q_i_0.numpy()[i], b.q_i_0)
np.testing.assert_allclose(single_sim.model.bodies.u_i_0.numpy()[i], b.u_i_0)
np.testing.assert_allclose(single_sim.state.q_i.numpy()[i], b.q_i_0)
np.testing.assert_allclose(single_sim.state.u_i.numpy()[i], b.u_i_0)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[single]: [init]: sim.model.size:\n{single_sim.model.size}\n\n")
msg.info(f"[single]: [init]: sim.model.state.q_i:\n{single_sim.state.q_i}\n\n")
msg.info(f"[single]: [init]: sim.model.state.u_i:\n{single_sim.state.u_i}\n\n")
msg.info(f"[single]: [init]: sim.model.state.q_j:\n{single_sim.state.q_j}\n\n")
msg.info(f"[single]: [init]: sim.model.state.dq_j:\n{single_sim.state.dq_j}\n\n")
# Define the total number of sample steps to collect, and the
# total number of execution steps from which to collect them
num_sample_steps = 37
num_skip_steps = 0
num_exec_steps = 1000
# Allocate arrays to hold the collected samples
num_bodies = single_sim.model.size.sum_of_num_bodies
num_joint_dofs = single_sim.model.size.sum_of_num_joint_dofs
num_joint_cts = single_sim.model.size.sum_of_num_joint_cts
sample_init_q_i = np.zeros((num_sample_steps, num_bodies, 7), dtype=np.float32)
sample_init_u_i = np.zeros((num_sample_steps, num_bodies, 6), dtype=np.float32)
sample_next_q_i = np.zeros((num_sample_steps, num_bodies, 7), dtype=np.float32)
sample_next_u_i = np.zeros((num_sample_steps, num_bodies, 6), dtype=np.float32)
sample_init_q_j = np.zeros((num_sample_steps, num_joint_dofs), dtype=np.float32)
sample_init_dq_j = np.zeros((num_sample_steps, num_joint_dofs), dtype=np.float32)
sample_init_lambda_j = np.zeros((num_sample_steps, num_joint_cts), dtype=np.float32)
sample_next_q_j = np.zeros((num_sample_steps, num_joint_dofs), dtype=np.float32)
sample_next_dq_j = np.zeros((num_sample_steps, num_joint_dofs), dtype=np.float32)
sample_ctrl_tau_j = np.zeros((num_sample_steps, num_joint_dofs), dtype=np.float32)
# Run the simulation for the specified number of steps
sample_freq = max(1, num_exec_steps // num_sample_steps)
sample = 0
msg.info(f"[sample]: sampling {num_sample_steps} transitions over {num_exec_steps} simulator steps")
total_steps = num_skip_steps + num_exec_steps
start_time = time.time()
for step in range(total_steps):
# Execute a single simulation step
single_sim.step()
wp.synchronize()
if self.verbose or self.progress:
print_progress_bar(step + 1, total_steps, start_time, prefix="Progress", suffix="")
# Collect the initial and next state samples at the specified frequency
if step >= num_skip_steps and step % sample_freq == 0 and sample < num_sample_steps:
sample_init_q_i[sample, :, :] = single_sim.state_previous.q_i.numpy().copy()
sample_init_u_i[sample, :, :] = single_sim.state_previous.u_i.numpy().copy()
sample_next_q_i[sample, :, :] = single_sim.state.q_i.numpy().copy()
sample_next_u_i[sample, :, :] = single_sim.state.u_i.numpy().copy()
sample_init_q_j[sample, :] = single_sim.state_previous.q_j.numpy().copy()
sample_init_dq_j[sample, :] = single_sim.state_previous.dq_j.numpy().copy()
sample_init_lambda_j[sample, :] = single_sim.state_previous.lambda_j.numpy().copy()
sample_next_q_j[sample, :] = single_sim.state.q_j.numpy().copy()
sample_next_dq_j[sample, :] = single_sim.state.dq_j.numpy().copy()
sample_ctrl_tau_j[sample, :] = single_sim.control.tau_j.numpy().copy()
sample += 1
# Reshape samples for easier comparison later
sample_init_q_i = sample_init_q_i.reshape(-1, 7)
sample_init_u_i = sample_init_u_i.reshape(-1, 6)
sample_next_q_i = sample_next_q_i.reshape(-1, 7)
sample_next_u_i = sample_next_u_i.reshape(-1, 6)
sample_init_q_j = sample_init_q_j.reshape(-1)
sample_init_dq_j = sample_init_dq_j.reshape(-1)
sample_init_lambda_j = sample_init_lambda_j.reshape(-1)
sample_next_q_j = sample_next_q_j.reshape(-1)
sample_next_dq_j = sample_next_dq_j.reshape(-1)
sample_ctrl_tau_j = sample_ctrl_tau_j.reshape(-1)
# Optional verbose output
msg.info(f"[samples]: init q_i (shape={sample_init_q_i.shape}):\n{sample_init_q_i}\n")
msg.info(f"[samples]: init u_i (shape={sample_init_u_i.shape}):\n{sample_init_u_i}\n")
msg.info(f"[samples]: init q_j (shape={sample_init_q_j.shape}):\n{sample_init_q_j}\n")
msg.info(f"[samples]: init dq_j (shape={sample_init_dq_j.shape}):\n{sample_init_dq_j}\n")
msg.info(f"[samples]: init lambda_j (shape={sample_init_lambda_j.shape}):\n{sample_init_lambda_j}\n")
msg.info(f"[samples]: next q_i (shape={sample_next_q_i.shape}):\n{sample_next_q_i}\n")
msg.info(f"[samples]: next u_i (shape={sample_next_u_i.shape}):\n{sample_next_u_i}\n")
msg.info(f"[samples]: next q_j (shape={sample_next_q_j.shape}):\n{sample_next_q_j}\n")
msg.info(f"[samples]: next dq_j (shape={sample_next_dq_j.shape}):\n{sample_next_dq_j}\n")
msg.info(f"[samples]: control tau_j (shape={sample_ctrl_tau_j.shape}):\n{sample_ctrl_tau_j}\n")
# Create a multi-instance system by replicating the single-instance builder
multi_builder = make_homogeneous_builder(num_worlds=num_sample_steps, build_fn=build_cartpole, ground=False)
for i, body in enumerate(multi_builder.bodies):
msg.info(f"[multi]: [builder]: body {i}: bid: {body.bid}")
msg.info(f"[multi]: [builder]: body {i}: q_i: {body.q_i_0}")
msg.info(f"[multi]: [builder]: body {i}: u_i: {body.u_i_0}")
# Create simulator and check if the initial state is consistent with the contents of the builder
multi_sim = Simulator(builder=multi_builder, device=self.default_device)
self.assertEqual(multi_sim.model.size.sum_of_num_bodies, 2 * num_sample_steps)
self.assertEqual(multi_sim.model.size.sum_of_num_joints, 2 * num_sample_steps)
for i, b in enumerate(multi_builder.bodies):
np.testing.assert_allclose(multi_sim.model.bodies.q_i_0.numpy()[i], b.q_i_0)
np.testing.assert_allclose(multi_sim.model.bodies.u_i_0.numpy()[i], b.u_i_0)
np.testing.assert_allclose(multi_sim.state_previous.q_i.numpy()[i], b.q_i_0)
np.testing.assert_allclose(multi_sim.state_previous.u_i.numpy()[i], b.u_i_0)
np.testing.assert_allclose(multi_sim.state.q_i.numpy()[i], b.q_i_0)
np.testing.assert_allclose(multi_sim.state.u_i.numpy()[i], b.u_i_0)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [start]: sim.model.size:\n{multi_sim.model.size}\n\n")
msg.info(f"[multi]: [start]: sim.model.state.q_i:\n{multi_sim.state.q_i}\n\n")
msg.info(f"[multi]: [start]: sim.model.state.u_i:\n{multi_sim.state.u_i}\n\n")
msg.info(f"[multi]: [start]: sim.model.state.q_j:\n{multi_sim.state.q_j}\n\n")
msg.info(f"[multi]: [start]: sim.model.state.dq_j:\n{multi_sim.state.dq_j}\n\n")
msg.info(f"[multi]: [start]: sim.model.control.tau_j:\n{multi_sim.control.tau_j}\n\n")
# Create a state & control containers to hold the sampled initial states
state_0 = multi_sim.model.state()
state_0.q_i.assign(sample_init_q_i)
state_0.u_i.assign(sample_init_u_i)
state_0.q_j.assign(sample_init_q_j)
state_0.q_j_p.assign(sample_init_q_j)
state_0.dq_j.assign(sample_init_dq_j)
state_0.lambda_j.assign(sample_init_lambda_j)
control_0 = multi_sim.model.control()
control_0.tau_j.assign(sample_ctrl_tau_j)
# Reset the multi-instance simulator to load the new initial states
multi_sim.data.state_n.copy_from(state_0)
multi_sim.data.state_p.copy_from(state_0)
multi_sim.data.control.copy_from(control_0)
msg.info(f"[multi]: [reset]: sim.model.state_previous.q_i:\n{multi_sim.state_previous.q_i}\n\n")
msg.info(f"[multi]: [reset]: sim.model.state_previous.u_i:\n{multi_sim.state_previous.u_i}\n\n")
msg.info(f"[multi]: [reset]: sim.model.state_previous.q_j:\n{multi_sim.state_previous.q_j}\n\n")
msg.info(f"[multi]: [reset]: sim.model.state_previous.dq_j:\n{multi_sim.state_previous.dq_j}\n\n")
msg.info(f"[multi]: [reset]: sim.model.state.q_i:\n{multi_sim.state.q_i}\n\n")
msg.info(f"[multi]: [reset]: sim.model.state.u_i:\n{multi_sim.state.u_i}\n\n")
msg.info(f"[multi]: [reset]: sim.model.state.q_j:\n{multi_sim.state.q_j}\n\n")
msg.info(f"[multi]: [reset]: sim.model.state.dq_j:\n{multi_sim.state.dq_j}\n\n")
np.testing.assert_allclose(multi_sim.state_previous.q_i.numpy(), sample_init_q_i)
np.testing.assert_allclose(multi_sim.state_previous.u_i.numpy(), sample_init_u_i)
np.testing.assert_allclose(multi_sim.state.q_i.numpy(), sample_init_q_i)
np.testing.assert_allclose(multi_sim.state.u_i.numpy(), sample_init_u_i)
np.testing.assert_allclose(multi_sim.state_previous.q_j.numpy(), sample_init_q_j)
np.testing.assert_allclose(multi_sim.state_previous.dq_j.numpy(), sample_init_dq_j)
np.testing.assert_allclose(multi_sim.state.q_j.numpy(), sample_init_q_j)
np.testing.assert_allclose(multi_sim.state.dq_j.numpy(), sample_init_dq_j)
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [init]: sim.model.state.q_i:\n{multi_sim.state.q_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.u_i:\n{multi_sim.state.u_i}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.q_j:\n{multi_sim.state.q_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.state.dq_j:\n{multi_sim.state.dq_j}\n\n")
msg.info(f"[multi]: [init]: sim.model.control.tau_j:\n{multi_sim.control.tau_j}\n\n")
# Step the multi-instance simulator once
multi_sim.step()
wp.synchronize()
# Optional verbose output - enabled globally via self.verbose
msg.info(f"[multi]: [next]: sim.model.state.q_i:\n{multi_sim.state.q_i}\n\n")
msg.info(f"[multi]: [next]: sim.model.state.u_i:\n{multi_sim.state.u_i}\n\n")
msg.info(f"[multi]: [next]: sim.model.state.q_j:\n{multi_sim.state.q_j}\n\n")
msg.info(f"[multi]: [next]: sim.model.state.dq_j:\n{multi_sim.state.dq_j}\n\n")
# Check that the next states match the collected samples
np.testing.assert_allclose(multi_sim.solver.data.joints.tau_j.numpy(), sample_ctrl_tau_j)
np.testing.assert_allclose(multi_sim.state_previous.q_i.numpy(), sample_init_q_i)
np.testing.assert_allclose(multi_sim.state_previous.u_i.numpy(), sample_init_u_i)
np.testing.assert_allclose(multi_sim.state.q_i.numpy(), sample_next_q_i)
np.testing.assert_allclose(multi_sim.state.u_i.numpy(), sample_next_u_i)
np.testing.assert_allclose(multi_sim.state_previous.q_j.numpy(), sample_init_q_j)
np.testing.assert_allclose(multi_sim.state_previous.dq_j.numpy(), sample_init_dq_j)
np.testing.assert_allclose(multi_sim.state.q_j.numpy(), sample_next_q_j)
np.testing.assert_allclose(multi_sim.state.dq_j.numpy(), sample_next_dq_j)
np.testing.assert_allclose(multi_sim.control.tau_j.numpy(), sample_ctrl_tau_j)
###
# Test execution
###
if __name__ == "__main__":
# Test setup
setup_tests()
# Run all tests
unittest.main(verbosity=2)
================================================
FILE: newton/_src/solvers/kamino/tests/utils/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
================================================
FILE: newton/_src/solvers/kamino/tests/utils/checks.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: UNIT TESTS: COMPARISON UTILITIES
"""
import unittest
from typing import Any
import numpy as np
from ..._src.core.bodies import RigidBodiesModel
from ..._src.core.builder import ModelBuilderKamino
from ..._src.core.control import ControlKamino
from ..._src.core.geometry import GeometriesModel
from ..._src.core.joints import JointsModel
from ..._src.core.materials import MaterialPairsModel, MaterialsModel
from ..._src.core.model import ModelKamino, ModelKaminoInfo
from ..._src.core.size import SizeKamino
from ..._src.core.state import StateKamino
from ..._src.utils import logger as msg
###
# Module interface
###
__all__ = [
"arrays_equal",
"assert_builders_equal",
"assert_control_equal",
"assert_model_bodies_equal",
"assert_model_equal",
"assert_model_geoms_equal",
"assert_model_info_equal",
"assert_model_joints_equal",
"assert_model_material_pairs_equal",
"assert_model_materials_equal",
"assert_model_size_equal",
"assert_state_equal",
"lists_equal",
"matrices_equal",
"vectors_equal",
]
###
# Array-like comparisons
###
def lists_equal(list1, list2) -> bool:
return np.array_equal(list1, list2)
def arrays_equal(arr1, arr2, tolerance=1e-6) -> bool:
return np.allclose(arr1, arr2, atol=tolerance)
def matrices_equal(m1, m2, tolerance=1e-6) -> bool:
return np.allclose(m1, m2, atol=tolerance)
def vectors_equal(v1, v2, tolerance=1e-6) -> bool:
return np.allclose(v1, v2, atol=tolerance)
###
# Utilities
###
def assert_scalar_attributes_equal(test: unittest.TestCase, obj0: Any, obj1: Any, attributes: list[str]) -> None:
for attr in attributes:
# Check if attribute exists in both objects
obj_name = obj0.__class__.__name__
has_attr0 = hasattr(obj0, attr)
has_attr1 = hasattr(obj1, attr)
if not has_attr0 and not has_attr1:
msg.debug(f"Skipping attribute '{attr}' comparison for {obj_name} because it is missing in both objects.")
continue
elif not has_attr0 or not has_attr1:
test.fail(
f"Attribute '{attr}' is missing in one of the objects: "
f" {obj_name} has_attr0={has_attr0}, has_attr1={has_attr1}"
)
# Retrieve attributes for logging
attr0 = getattr(obj0, attr)
attr1 = getattr(obj1, attr)
# Test scalar attribute values
msg.debug("Comparing %s.%s: actual=%s, desired=%s", obj_name, attr, attr0, attr1)
test.assertEqual(
first=attr0,
second=attr1,
msg=f"{obj0.__class__.__name__}.{attr} are not equal.",
)
def assert_array_attributes_equal(
test: unittest.TestCase,
obj0: Any,
obj1: Any,
attributes: list[str],
rtol: dict[str, float] | None = None,
atol: dict[str, float] | None = None,
) -> None:
for attr in attributes:
# Check if attribute exists in both objects
obj_name = obj0.__class__.__name__
has_attr0 = hasattr(obj0, attr)
has_attr1 = hasattr(obj1, attr)
if not has_attr0 and not has_attr1:
msg.debug(f"Skipping attribute '{attr}' comparison for {obj_name} because it is missing in both objects.")
continue
elif not has_attr0 or not has_attr1:
test.fail(
f"Attribute '{attr}' is missing in one of the objects: "
f" {obj_name} has_attr0={has_attr0}, has_attr1={has_attr1}"
)
# Retrieve attributes for logging
attr0 = getattr(obj0, attr)
attr1 = getattr(obj1, attr)
# Check if attributes are array-like
attr0_is_array = hasattr(attr0, "shape")
attr1_is_array = hasattr(attr1, "shape")
if not attr0_is_array and not attr1_is_array:
msg.debug(
f"\nSkipping attribute '{obj_name}.{attr}' comparison: both of the objects are not array-like: "
f"\n0: {obj_name}.{attr}: {type(attr0)}\n1: {obj_name}.{attr}: {type(attr1)}"
)
continue
elif not attr0_is_array or not attr1_is_array:
test.fail(
f"Attribute '{attr}' is not array-like in one of the objects: "
f" {obj_name}.{attr} has_attr0_shape={getattr(attr0, 'shape', None)}, "
f"has_attr1_shape={getattr(attr1, 'shape', None)}"
)
# Test array attribute shapes
shape0 = attr0.shape
shape1 = attr1.shape
test.assertEqual(shape0, shape1, f"{obj_name}.{attr} shapes are not equal.")
# Test array attribute values
diff = attr0 - attr1
msg.debug("Comparing %s:\nactual:\n%s\ndesired:\n%s\ndiff:\n%s", f"{obj_name}.{attr}", attr0, attr1, diff)
np.testing.assert_allclose(
actual=attr0.numpy(),
desired=attr1.numpy(),
err_msg=f"{obj_name}.{attr} are not equal.",
rtol=rtol.get(attr, 1e-6) if rtol else 1e-6,
atol=atol.get(attr, 1e-6) if atol else 1e-6,
)
###
# Container comparisons
###
def assert_builders_equal(
test: unittest.TestCase,
builder1: ModelBuilderKamino,
builder2: ModelBuilderKamino,
skip_colliders: bool = False,
skip_materials: bool = False,
):
"""
Compares two ModelBuilderKamino instances for equality.
"""
test.assertEqual(builder1.num_bodies, builder2.num_bodies)
test.assertEqual(builder1.num_joints, builder2.num_joints)
test.assertEqual(builder1.num_geoms, builder2.num_geoms)
test.assertEqual(builder1.num_materials, builder2.num_materials)
for i in range(builder1.num_bodies):
test.assertEqual(builder1.bodies[i].wid, builder2.bodies[i].wid)
test.assertEqual(builder1.bodies[i].bid, builder2.bodies[i].bid)
test.assertAlmostEqual(builder1.bodies[i].m_i, builder2.bodies[i].m_i)
test.assertTrue(matrices_equal(builder1.bodies[i].i_I_i, builder2.bodies[i].i_I_i))
test.assertTrue(vectors_equal(builder1.bodies[i].q_i_0, builder2.bodies[i].q_i_0))
test.assertTrue(vectors_equal(builder1.bodies[i].u_i_0, builder2.bodies[i].u_i_0))
for j in range(builder1.num_joints):
test.assertEqual(builder1.joints[j].wid, builder2.joints[j].wid)
test.assertEqual(builder1.joints[j].jid, builder2.joints[j].jid)
test.assertEqual(builder1.joints[j].act_type, builder2.joints[j].act_type)
test.assertEqual(builder1.joints[j].dof_type, builder2.joints[j].dof_type)
test.assertEqual(builder1.joints[j].bid_B, builder2.joints[j].bid_B)
test.assertEqual(builder1.joints[j].bid_F, builder2.joints[j].bid_F)
test.assertTrue(
vectors_equal(builder1.joints[j].B_r_Bj, builder2.joints[j].B_r_Bj),
f"Joint {j} B_r_Bj:\nleft:\n{builder1.joints[j].B_r_Bj}\nright:\n{builder2.joints[j].B_r_Bj}",
)
test.assertTrue(
vectors_equal(builder1.joints[j].F_r_Fj, builder2.joints[j].F_r_Fj),
f"Joint {j} F_r_Fj:\nleft:\n{builder1.joints[j].F_r_Fj}\nright:\n{builder2.joints[j].F_r_Fj}",
)
test.assertTrue(
matrices_equal(builder1.joints[j].X_j, builder2.joints[j].X_j),
f"Joint {j} X_j:\nleft:\n{builder1.joints[j].X_j}\nright:\n{builder2.joints[j].X_j}",
)
test.assertTrue(
arrays_equal(builder1.joints[j].q_j_min, builder2.joints[j].q_j_min),
f"Joint {j} q_j_min:\nleft:\n{builder1.joints[j].q_j_min}\nright:\n{builder2.joints[j].q_j_min}",
)
test.assertTrue(
arrays_equal(builder1.joints[j].q_j_max, builder2.joints[j].q_j_max),
f"Joint {j} q_j_max:\nleft:\n{builder1.joints[j].q_j_max}\nright:\n{builder2.joints[j].q_j_max}",
)
test.assertTrue(
arrays_equal(builder1.joints[j].dq_j_max, builder2.joints[j].dq_j_max),
f"Joint {j} dq_j_max:\nleft:\n{builder1.joints[j].dq_j_max}\nright:\n{builder2.joints[j].dq_j_max}",
)
test.assertTrue(
arrays_equal(builder1.joints[j].tau_j_max, builder2.joints[j].tau_j_max),
f"Joint {j} tau_j_max:\nleft:\n{builder1.joints[j].tau_j_max}\nright:\n{builder2.joints[j].tau_j_max}",
)
test.assertTrue(
arrays_equal(builder1.joints[j].a_j, builder2.joints[j].a_j),
f"Joint {j} a_j:\nleft:\n{builder1.joints[j].a_j}\nright:\n{builder2.joints[j].a_j}",
)
test.assertTrue(
arrays_equal(builder1.joints[j].b_j, builder2.joints[j].b_j),
f"Joint {j} b_j:\nleft:\n{builder1.joints[j].b_j}\nright:\n{builder2.joints[j].b_j}",
)
test.assertTrue(
arrays_equal(builder1.joints[j].k_p_j, builder2.joints[j].k_p_j),
f"Joint {j} k_p_j:\nleft:\n{builder1.joints[j].k_p_j}\nright:\n{builder2.joints[j].k_p_j}",
)
test.assertTrue(
arrays_equal(builder1.joints[j].k_d_j, builder2.joints[j].k_d_j),
f"Joint {j} k_d_j:\nleft:\n{builder1.joints[j].k_d_j}\nright:\n{builder2.joints[j].k_d_j}",
)
test.assertEqual(builder1.joints[j].num_coords, builder2.joints[j].num_coords)
test.assertEqual(builder1.joints[j].num_dofs, builder2.joints[j].num_dofs)
test.assertEqual(builder1.joints[j].num_passive_coords, builder2.joints[j].num_passive_coords)
test.assertEqual(builder1.joints[j].num_passive_dofs, builder2.joints[j].num_passive_dofs)
test.assertEqual(builder1.joints[j].num_actuated_coords, builder2.joints[j].num_actuated_coords)
test.assertEqual(builder1.joints[j].num_actuated_dofs, builder2.joints[j].num_actuated_dofs)
test.assertEqual(builder1.joints[j].num_actuated_dofs, builder2.joints[j].num_actuated_dofs)
test.assertEqual(builder1.joints[j].num_cts, builder2.joints[j].num_cts)
test.assertEqual(builder1.joints[j].num_dynamic_cts, builder2.joints[j].num_dynamic_cts)
test.assertEqual(builder1.joints[j].num_kinematic_cts, builder2.joints[j].num_kinematic_cts)
test.assertEqual(builder1.joints[j].coords_offset, builder2.joints[j].coords_offset)
test.assertEqual(builder1.joints[j].dofs_offset, builder2.joints[j].dofs_offset)
test.assertEqual(builder1.joints[j].passive_coords_offset, builder2.joints[j].passive_coords_offset)
test.assertEqual(builder1.joints[j].passive_dofs_offset, builder2.joints[j].passive_dofs_offset)
test.assertEqual(builder1.joints[j].actuated_coords_offset, builder2.joints[j].actuated_coords_offset)
test.assertEqual(builder1.joints[j].actuated_dofs_offset, builder2.joints[j].actuated_dofs_offset)
test.assertEqual(builder1.joints[j].cts_offset, builder2.joints[j].cts_offset)
test.assertEqual(builder1.joints[j].dynamic_cts_offset, builder2.joints[j].dynamic_cts_offset)
test.assertEqual(builder1.joints[j].kinematic_cts_offset, builder2.joints[j].kinematic_cts_offset)
test.assertEqual(builder1.joints[j].is_binary, builder2.joints[j].is_binary)
test.assertEqual(builder1.joints[j].is_passive, builder2.joints[j].is_passive)
test.assertEqual(builder1.joints[j].is_actuated, builder2.joints[j].is_actuated)
test.assertEqual(builder1.joints[j].is_dynamic, builder2.joints[j].is_dynamic)
test.assertEqual(builder1.joints[j].is_implicit_pd, builder2.joints[j].is_implicit_pd)
for k in range(builder1.num_geoms):
test.assertEqual(builder1.geoms[k].wid, builder2.geoms[k].wid)
test.assertEqual(builder1.geoms[k].gid, builder2.geoms[k].gid)
test.assertEqual(builder1.geoms[k].mid, builder2.geoms[k].mid)
test.assertEqual(builder1.geoms[k].body, builder2.geoms[k].body)
test.assertEqual(builder1.geoms[k].shape.type, builder2.geoms[k].shape.type)
test.assertTrue(lists_equal(builder1.geoms[k].shape.paramsvec, builder2.geoms[k].shape.paramsvec))
if not skip_materials:
test.assertEqual(builder1.geoms[k].material, builder2.geoms[k].material)
if not skip_colliders:
test.assertEqual(builder1.geoms[k].group, builder2.geoms[k].group)
test.assertEqual(builder1.geoms[k].collides, builder2.geoms[k].collides)
test.assertEqual(builder1.geoms[k].max_contacts, builder2.geoms[k].max_contacts)
test.assertEqual(builder1.geoms[k].gap, builder2.geoms[k].gap)
test.assertEqual(builder1.geoms[k].margin, builder2.geoms[k].margin)
if not skip_materials:
for m in range(builder1.num_materials):
test.assertEqual(builder1.materials[m].wid, builder2.materials[m].wid)
test.assertEqual(builder1.materials[m].mid, builder2.materials[m].mid)
test.assertEqual(builder1.materials[m].density, builder2.materials[m].density)
test.assertEqual(builder1.materials[m].restitution, builder2.materials[m].restitution)
test.assertEqual(builder1.materials[m].static_friction, builder2.materials[m].static_friction)
test.assertEqual(builder1.materials[m].dynamic_friction, builder2.materials[m].dynamic_friction)
###
# Container comparisons
###
def assert_state_equal(
test: unittest.TestCase, state0: StateKamino, state1: StateKamino, excluded: list[str] | None = None
) -> None:
attributes = ["q_i", "u_i", "w_i", "q_j", "q_j_p", "dq_j", "lambda_j"]
if excluded:
attributes = [attr for attr in attributes if attr not in excluded]
assert_array_attributes_equal(test, state0, state1, attributes)
def assert_control_equal(
test: unittest.TestCase, control0: ControlKamino, control1: ControlKamino, excluded: list[str] | None = None
) -> None:
attributes = ["tau_j", "q_j_ref", "dq_j_ref", "tau_j_ref"]
if excluded:
attributes = [attr for attr in attributes if attr not in excluded]
assert_array_attributes_equal(test, control0, control1, attributes)
def assert_model_size_equal(
test: unittest.TestCase, size0: SizeKamino, size1: SizeKamino, excluded: list[str] | None = None
) -> None:
attributes = [
"num_worlds",
"sum_of_num_bodies",
"max_of_num_bodies",
"sum_of_num_joints",
"max_of_num_joints",
"sum_of_num_passive_joints",
"max_of_num_passive_joints",
"sum_of_num_actuated_joints",
"max_of_num_actuated_joints",
"sum_of_num_dynamic_joints",
"max_of_num_dynamic_joints",
"sum_of_num_geoms",
"max_of_num_geoms",
"sum_of_num_material_pairs",
"max_of_num_material_pairs",
"sum_of_num_body_dofs",
"max_of_num_body_dofs",
"sum_of_num_joint_coords",
"max_of_num_joint_coords",
"sum_of_num_joint_dofs",
"max_of_num_joint_dofs",
"sum_of_num_passive_joint_coords",
"max_of_num_passive_joint_coords",
"sum_of_num_passive_joint_dofs",
"max_of_num_passive_joint_dofs",
"sum_of_num_actuated_joint_coords",
"max_of_num_actuated_joint_coords",
"sum_of_num_actuated_joint_dofs",
"max_of_num_actuated_joint_dofs",
"sum_of_num_joint_cts",
"max_of_num_joint_cts",
"sum_of_num_dynamic_joint_cts",
"max_of_num_dynamic_joint_cts",
"sum_of_num_kinematic_joint_cts",
"max_of_num_kinematic_joint_cts",
"sum_of_max_limits",
"max_of_max_limits",
"sum_of_max_contacts",
"max_of_max_contacts",
"sum_of_max_unilaterals",
"max_of_max_unilaterals",
"sum_of_max_total_cts",
"max_of_max_total_cts",
]
if excluded:
attributes = [attr for attr in attributes if attr not in excluded]
assert_scalar_attributes_equal(test, size0, size1, attributes)
def assert_model_info_equal(
test: unittest.TestCase, info0: ModelKaminoInfo, info1: ModelKaminoInfo, excluded: list[str] | None = None
) -> None:
assert_scalar_attributes_equal(test, info0, info1, ["num_worlds"])
array_attributes = [
"num_bodies",
"num_joints",
"num_passive_joints",
"num_actuated_joints",
"num_dynamic_joints",
"num_geoms",
"num_body_dofs",
"num_joint_coords",
"num_joint_dofs",
"num_passive_joint_coords",
"num_passive_joint_dofs",
"num_actuated_joint_coords",
"num_actuated_joint_dofs",
"num_joint_cts",
"num_joint_dynamic_cts",
"num_joint_kinematic_cts",
"max_limit_cts",
"max_contact_cts",
"max_total_cts",
"bodies_offset",
"joints_offset",
"geoms_offset",
"body_dofs_offset",
"joint_coords_offset",
"joint_dofs_offset",
"joint_passive_coords_offset",
"joint_passive_dofs_offset",
"joint_actuated_coords_offset",
"joint_actuated_dofs_offset",
"joint_cts_offset",
"joint_dynamic_cts_offset",
"joint_kinematic_cts_offset",
"total_cts_offset",
"joint_dynamic_cts_group_offset",
"joint_kinematic_cts_group_offset",
"base_body_index",
"base_joint_index",
"mass_min",
"mass_max",
"mass_total",
"inertia_total",
]
if excluded:
array_attributes = [attr for attr in array_attributes if attr not in excluded]
assert_array_attributes_equal(test, info0, info1, array_attributes)
def assert_model_bodies_equal(
test: unittest.TestCase,
bodies0: RigidBodiesModel,
bodies1: RigidBodiesModel,
excluded: list[str] | None = None,
rtol: dict[str, float] | None = None,
atol: dict[str, float] | None = None,
) -> None:
assert_scalar_attributes_equal(test, bodies0, bodies1, ["num_bodies", "label"])
array_attributes = [
"wid",
"bid",
"i_r_com_i",
"m_i",
"inv_m_i",
"i_I_i",
"inv_i_I_i",
"q_i_0",
"u_i_0",
]
if excluded:
array_attributes = [attr for attr in array_attributes if attr not in excluded]
assert_array_attributes_equal(test, bodies0, bodies1, array_attributes, rtol=rtol, atol=atol)
def assert_model_joints_equal(
test: unittest.TestCase, joints0: JointsModel, joints1: JointsModel, excluded: list[str] | None = None
) -> None:
assert_scalar_attributes_equal(test, joints0, joints1, ["num_joints", "label"])
array_attributes = [
"wid",
"jid",
"dof_type",
"act_type",
"bid_B",
"bid_F",
"B_r_Bj",
"F_r_Fj",
"X_j",
"q_j_min",
"q_j_max",
"dq_j_max",
"tau_j_max",
"a_j",
"b_j",
"k_p_j",
"k_d_j",
"q_j_0",
"dq_j_0",
"num_coords",
"num_dofs",
"num_cts",
"num_dynamic_cts",
"num_kinematic_cts",
"coords_offset",
"dofs_offset",
"passive_coords_offset",
"passive_dofs_offset",
"actuated_coords_offset",
"actuated_dofs_offset",
"cts_offset",
"dynamic_cts_offset",
"kinematic_cts_offset",
]
if excluded:
array_attributes = [attr for attr in array_attributes if attr not in excluded]
assert_array_attributes_equal(test, joints0, joints1, array_attributes)
def assert_model_geoms_equal(
test: unittest.TestCase,
geoms0: GeometriesModel,
geoms1: GeometriesModel,
excluded: list[str] | None = None,
) -> None:
scalar_attributes = [
"num_geoms",
"num_collidable",
"num_collidable_pairs",
"num_excluded_pairs",
"model_minimum_contacts",
"world_minimum_contacts",
"label",
]
array_attributes = [
"wid",
"gid",
"bid",
"type",
"flags",
"ptr",
"params",
"offset",
"material",
"group",
"gap",
"margin",
"collidable_pairs",
"excluded_pairs",
]
if excluded:
scalar_attributes = [attr for attr in scalar_attributes if attr not in excluded]
array_attributes = [attr for attr in array_attributes if attr not in excluded]
assert_scalar_attributes_equal(test, geoms0, geoms1, scalar_attributes)
assert_array_attributes_equal(test, geoms0, geoms1, array_attributes)
def assert_model_materials_equal(
test: unittest.TestCase, materials0: MaterialsModel, materials1: MaterialsModel, excluded: list[str] | None = None
) -> None:
assert_scalar_attributes_equal(test, materials0, materials1, ["num_materials"])
array_attributes = [
# "density",
"restitution",
"static_friction",
"dynamic_friction",
]
if excluded:
array_attributes = [attr for attr in array_attributes if attr not in excluded]
assert_array_attributes_equal(test, materials0, materials1, array_attributes)
def assert_model_material_pairs_equal(
test: unittest.TestCase,
matpairs0: MaterialPairsModel,
matpairs1: MaterialPairsModel,
excluded: list[str] | None = None,
) -> None:
assert_scalar_attributes_equal(test, matpairs0, matpairs1, ["num_material_pairs"])
array_attributes = [
"restitution",
"static_friction",
"dynamic_friction",
]
if excluded:
array_attributes = [attr for attr in array_attributes if attr not in excluded]
assert_array_attributes_equal(test, matpairs0, matpairs1, array_attributes)
def assert_model_equal(
test: unittest.TestCase,
model0: ModelKamino,
model1: ModelKamino,
skip_geom_source_ptr: bool = False,
skip_geom_group_and_collides: bool = False,
skip_geom_margin_and_gap: bool = False,
excluded: list[str] | None = None,
rtol: dict[str, float] | None = None,
atol: dict[str, float] | None = None,
) -> None:
assert_model_size_equal(test, model0.size, model1.size, excluded)
assert_model_info_equal(test, model0.info, model1.info, excluded)
assert_model_bodies_equal(test, model0.bodies, model1.bodies, excluded, rtol=rtol, atol=atol)
assert_model_joints_equal(test, model0.joints, model1.joints, excluded)
geom_excluded = excluded
if skip_geom_source_ptr or skip_geom_group_and_collides or skip_geom_margin_and_gap:
geom_excluded = [] if excluded is None else list(excluded)
if skip_geom_source_ptr:
geom_excluded.append("ptr")
if skip_geom_group_and_collides:
geom_excluded.extend(["group", "collides"])
if skip_geom_margin_and_gap:
geom_excluded.extend(["margin", "gap"])
assert_model_geoms_equal(
test,
model0.geoms,
model1.geoms,
excluded=geom_excluded,
)
assert_model_materials_equal(test, model0.materials, model1.materials, excluded)
assert_model_material_pairs_equal(test, model0.material_pairs, model1.material_pairs, excluded)
================================================
FILE: newton/_src/solvers/kamino/tests/utils/diff_check.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: UNIT TESTS: Utils for running derivative checks on single-joint examples
"""
from __future__ import annotations
import os
from collections.abc import Callable
import numpy as np
import warp as wp
from ..._src.models import get_testing_usd_assets_path
from ..._src.utils.io.usd import USDImporter
###
# Module interface
###
__all__ = ["central_finite_differences", "diff_check", "run_test_single_joint_examples"]
def run_test_single_joint_examples(
test_fun: Callable,
test_name: str = "test",
unary_joints: bool = True,
binary_joints: bool = True,
passive_joints: bool = True,
actuators: bool = True,
device: wp.DeviceLike = None,
):
"""
Runs a test function over all or a subset of the single-joint examples (e.g. to check some derivatives for all joint types)
Parameters
----------
test_fun: function
test function to run on each example, with signature kamino.core.ModelKamino -> bool, returning a success flag
test_name: str, optional
a name for the test to print as part of the error message upon failure (default: "test")
unary_joints: bool, optional
whether to include unary joint examples (NOTE: currently unsupported)
binary_joints: bool, optional
whether to include binary joint examples
passive_joints: bool, optional
whether to include passive joint examples
actuators: bool, optional
whether to include actuator examples
device: DeviceLike, optional
device on which to allocate the test models (default: None)
Returns
-------
success: bool
whether all tests succeeded
"""
# Resolve path of data folder
data_dir = os.path.join(get_testing_usd_assets_path(), "joints")
# List file paths of examples
file_paths = []
if unary_joints and passive_joints:
file_paths.extend(
[
os.path.join(data_dir, "test_joint_cartesian_passive_unary.usda"),
os.path.join(data_dir, "test_joint_cylindrical_passive_unary.usda"),
os.path.join(data_dir, "test_joint_fixed_unary.usda"),
os.path.join(data_dir, "test_joint_prismatic_passive_unary.usda"),
os.path.join(data_dir, "test_joint_revolute_passive_unary.usda"),
os.path.join(data_dir, "test_joint_spherical_unary.usda"),
os.path.join(data_dir, "test_joint_universal_passive_unary.usda"),
]
)
if binary_joints and passive_joints:
file_paths.extend(
[
os.path.join(data_dir, "test_joint_cartesian_passive.usda"),
os.path.join(data_dir, "test_joint_cylindrical_passive.usda"),
os.path.join(data_dir, "test_joint_fixed.usda"),
os.path.join(data_dir, "test_joint_prismatic_passive.usda"),
os.path.join(data_dir, "test_joint_revolute_passive.usda"),
os.path.join(data_dir, "test_joint_spherical.usda"),
os.path.join(data_dir, "test_joint_universal_passive.usda"),
]
)
if unary_joints and actuators:
file_paths.extend(
[
os.path.join(data_dir, "test_joint_cartesian_actuated_unary.usda"),
os.path.join(data_dir, "test_joint_cylindrical_actuated_unary.usda"),
os.path.join(data_dir, "test_joint_prismatic_actuated_unary.usda"),
os.path.join(data_dir, "test_joint_revolute_actuated_unary.usda"),
os.path.join(data_dir, "test_joint_universal_actuated_unary.usda"),
# Note: missing actuated spherical and free
]
)
if binary_joints and actuators:
file_paths.extend(
[
os.path.join(data_dir, "test_joint_cartesian_actuated.usda"),
os.path.join(data_dir, "test_joint_cylindrical_actuated.usda"),
os.path.join(data_dir, "test_joint_prismatic_actuated.usda"),
os.path.join(data_dir, "test_joint_revolute_actuated.usda"),
os.path.join(data_dir, "test_joint_universal_actuated.usda"),
# Note: missing actuated spherical and free
]
)
# Load and test all examples
success = True
for file_path in file_paths:
importer = USDImporter()
builder = importer.import_from(source=file_path)
file_stem_split = os.path.basename(file_path).split(".")[0].split("_")
unary_binary_str = "unary" if file_stem_split[-1] == "unary" else "binary"
passive_actuated_str = (
"actuated" if len(file_stem_split) > 3 and file_stem_split[3] == "actuated" else "passive"
)
joint_type_str = file_stem_split[2]
# Run test
model = builder.finalize(device=device, requires_grad=False, base_auto=False)
single_test_success = test_fun(model)
success &= single_test_success
if not single_test_success:
print(f"{test_name} failed for {unary_binary_str} {passive_actuated_str} {joint_type_str} joint")
return success
def central_finite_differences(fun: Callable, eval_point: float | np.ndarray(dtype=float), epsilon: float = 1e-5):
"""
Evaluates central finite differences of the given function at the given point
Supports scalar/vector-valued functions, of scalar/vector-valued variables
Parameters
----------
fun: function
function to take the derivative of with finite differences, accepting a scalar (float) or a vector (1D numpy array)
and returning a scalar or a vector
eval_point: float | np.ndarray
evaluation point, scalar or vector depending on the signature of function
epsilon: float, optional
step size for the central differences, i.e. we evaluate (f(x + epsilon) - f(x - epsilon)) / (2 * epsilon)
(default: 1e-5)
Returns
-------
derivative: float | np.ndarray
(approximate) derivative, a scalar for scalar functions of scalars, a 1D vector for vector functions of scalars or
scalar functions of vectors; a 2D Jacobian for vector functions of vectors
"""
dim_in = 0
if isinstance(eval_point, np.ndarray):
dim_in = len(eval_point)
if dim_in == 0:
return (fun(eval_point + epsilon) - fun(eval_point - epsilon)) / (2.0 * epsilon)
y = np.array(eval_point, copy=True)
for i in range(dim_in):
y[i] += epsilon
v_plus = fun(y)
y[i] -= 2 * epsilon
v_minus = fun(y)
y[i] += epsilon
fd_val = (v_plus - v_minus) / (2 * epsilon)
if i == 0:
dim_out = 0
if isinstance(fd_val, np.ndarray):
dim_out = len(fd_val)
res = np.zeros(dim_in) if dim_out == 0 else np.zeros((dim_out, dim_in))
if dim_out == 0:
res[i] = fd_val
else:
res[:, i] = fd_val
return res
def diff_check(
fun: Callable,
derivative: float | np.ndarray(dtype=float),
eval_point: float | np.ndarray(dtype=float),
epsilon: float = 1e-5,
tolerance_rel: float = 1e-6,
tolerance_abs: float = 1e-6,
):
"""
Checks the derivative of a function against central differences
Parameters:
-----------
fun: function
function to check the derivative of, accepting a scalar (float) or a vector (1D numpy array)
and returning a scalar or a vector
derivative: float | np.ndarray(dtype=float)
derivative to check against central differences. A scalar for scalar functions of scalars, a 1D vector for vector
functions of scalars or scalar functions of vectors; a 2D Jacobian for vector functions of vectors
eval_point: float | np.ndarray
evaluation point, scalar or vector depending on the signature of function
epsilon: float, optional
step size for the central differences (default: 1e-5)
tolerance_rel: float, optional
relative tolerance (default: 1e-6)
tolerance_abs: float, optional
absolute tolerance (default: 1e-6)
Returns:
--------
success: bool
whether the central differences derivative was close to the derivative to check. More specifically, whether
the absolute error or the relative error was below tolerance
"""
derivative_fd = central_finite_differences(fun, eval_point, epsilon)
error = derivative_fd - derivative
abs_test = np.max(np.abs(error)) <= tolerance_abs
rel_test = np.linalg.norm(error) <= tolerance_rel * np.linalg.norm(derivative_fd)
success = abs_test or rel_test
if not success:
print("DIFF CHECK FAILED")
print("DERIVATIVE: ")
print(derivative)
print("DERIVATIVE_FD")
print(derivative_fd)
if not abs_test:
print(f"Absolute test failed, error={np.max(np.abs(error))}, tolerance={tolerance_abs}")
if not rel_test:
print(
f"Relative test failed, error={np.linalg.norm(error)}, tolerance={tolerance_rel * np.linalg.norm(derivative_fd)}"
)
return success
================================================
FILE: newton/_src/solvers/kamino/tests/utils/extract.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Utilities for extracting data from Kamino data structures"""
import numpy as np
import warp as wp
from ..._src.core.data import DataKamino
from ..._src.core.model import ModelKamino
from ..._src.dynamics.delassus import BlockSparseMatrixFreeDelassusOperator, DelassusOperator
from ..._src.geometry.contacts import ContactsKamino
from ..._src.kinematics.jacobians import DenseSystemJacobians, SparseSystemJacobians
from ..._src.kinematics.limits import LimitsKamino
###
# Helper functions
###
def get_matrix_block(index: int, flatmat: np.ndarray, dims: list[int], maxdims: list[int] | None = None) -> np.ndarray:
"""Extract a specific matrix block from a flattened array of matrices."""
if maxdims is None:
maxdims = dims
mat_shape = (dims[index], dims[index])
mat_start = sum(n * n for n in maxdims[:index])
mat_end = mat_start + dims[index] ** 2
return flatmat[mat_start:mat_end].reshape(mat_shape)
def get_vector_block(index: int, flatvec: np.ndarray, dims: list[int], maxdims: list[int] | None = None) -> np.ndarray:
"""Extract a specific matrix block from a flattened array of matrices."""
if maxdims is None:
maxdims = dims
vec_start = sum(maxdims[:index])
vec_end = vec_start + dims[index]
return flatvec[vec_start:vec_end]
###
# Helper functions
###
def extract_active_constraint_dims(data: DataKamino) -> list[int]:
active_dim_np = data.info.num_total_cts.numpy()
return [int(active_dim_np[i]) for i in range(len(active_dim_np))]
def extract_active_constraint_vectors(model: ModelKamino, data: DataKamino, x: wp.array) -> list[np.ndarray]:
cts_start_np = model.info.total_cts_offset.numpy()
num_active_cts_np = extract_active_constraint_dims(data)
x_np = x.numpy()
return [x_np[cts_start_np[n] : cts_start_np[n] + num_active_cts_np[n]] for n in range(len(cts_start_np))]
def extract_actuation_forces(model: ModelKamino, data: DataKamino) -> list[np.ndarray]:
dofs_start_np = model.info.joint_dofs_offset.numpy()
num_dofs_np = model.info.num_joint_dofs.numpy()
tau_j_np = data.joints.tau_j.numpy()
return [tau_j_np[dofs_start_np[n] : dofs_start_np[n] + num_dofs_np[n]] for n in range(len(dofs_start_np))]
def extract_cts_jacobians(
model: ModelKamino,
limits: LimitsKamino | None,
contacts: ContactsKamino | None,
jacobians: DenseSystemJacobians | SparseSystemJacobians,
only_active_cts: bool = False,
verbose: bool = False,
) -> list[np.ndarray]:
if isinstance(jacobians, SparseSystemJacobians):
return jacobians._J_cts.bsm.numpy()
# Retrieve the number of worlds in the model
num_worlds = model.info.num_worlds
# Reshape the flat Jacobian as a set of matrices
J_cts_flat_offsets = jacobians.data.J_cts_offsets.numpy().astype(int).tolist()
J_cts_flat = jacobians.data.J_cts_data.numpy()
J_cts_flat_total_size = J_cts_flat.size
J_cts_flat_sizes = [0] * num_worlds
J_cts_flat_offsets_ext = [*J_cts_flat_offsets, J_cts_flat_total_size]
for w in range(num_worlds - 1, -1, -1):
J_cts_flat_sizes[w] = J_cts_flat_offsets_ext[w + 1] - J_cts_flat_offsets_ext[w]
# Retrieve the Jacobian dimensions in each world
has_limits = limits is not None and limits.model_max_limits_host > 0
has_contacts = contacts is not None and contacts.model_max_contacts_host > 0
num_bdofs = model.info.num_body_dofs.numpy().tolist()
num_jcts = model.info.num_joint_cts.numpy().tolist()
maxnl = limits.world_max_limits_host if has_limits else [0] * num_worlds
maxnc = contacts.world_max_contacts_host if has_contacts else [0] * num_worlds
nlact = limits.world_active_limits.numpy().tolist() if has_limits else [0] * num_worlds
ncact = contacts.world_active_contacts.numpy().tolist() if has_contacts else [0] * num_worlds
nl = nlact if only_active_cts else maxnl
nc = ncact if only_active_cts else maxnc
# Extract each Jacobian as a matrix
J_cts_mat: list[np.ndarray] = []
for w in range(num_worlds):
ncts = num_jcts[w] + nl[w] + 3 * nc[w]
J_cts_size = ncts * num_bdofs[w]
if J_cts_size > J_cts_flat_sizes[w]:
raise ValueError(f"Jacobian size {J_cts_size} exceeds flat size {J_cts_flat_sizes[w]} for world {w}")
start = J_cts_flat_offsets[w]
end = J_cts_flat_offsets[w] + J_cts_size
J_cts_mat.append(J_cts_flat[start:end].reshape((ncts, num_bdofs[w])))
# Optional verbose output
if verbose:
print(f"J_cts_flat_total_size: {J_cts_flat_total_size}")
print(f"sum(J_cts_flat_sizes): {sum(J_cts_flat_sizes)}")
print(f"J_cts_flat_sizes: {J_cts_flat_sizes}")
print(f"J_cts_flat_offsets: {J_cts_flat_offsets}")
print("") # Add a newline for better readability
for w in range(num_worlds):
print(f"{w}: start={J_cts_flat_offsets[w]}, end={J_cts_flat_offsets[w] + J_cts_flat_sizes[w]}")
print(f"J_cts_mat[{w}] ({J_cts_mat[w].shape}):\n{J_cts_mat[w]}\n")
# Return the extracted Jacobians
return J_cts_mat
def extract_dofs_jacobians(
model: ModelKamino,
jacobians: DenseSystemJacobians | SparseSystemJacobians,
verbose: bool = False,
) -> list[np.ndarray]:
if isinstance(jacobians, SparseSystemJacobians):
return jacobians._J_cts.bsm.numpy()
# Retrieve the number of worlds in the model
num_worlds = model.info.num_worlds
# Reshape the flat Jacobian as a set of matrices
ajmio = jacobians.data.J_dofs_offsets.numpy()
J_dofs_flat = jacobians.data.J_dofs_data.numpy()
J_dofs_flat_total_size = J_dofs_flat.size
J_dofs_flat_offsets = [int(ajmio[i]) for i in range(num_worlds)]
J_dofs_flat_sizes = [0] * num_worlds
J_dofs_flat_offsets_ext = [*J_dofs_flat_offsets, J_dofs_flat_total_size]
for i in range(num_worlds - 1, -1, -1):
J_dofs_flat_sizes[i] = J_dofs_flat_offsets_ext[i + 1] - J_dofs_flat_offsets_ext[i]
# Extract each Jacobian as a matrix
num_bdofs = model.info.num_body_dofs.numpy().tolist()
num_jdofs = model.info.num_joint_dofs.numpy().tolist()
J_dofs_mat: list[np.ndarray] = []
for i in range(num_worlds):
start = J_dofs_flat_offsets[i]
end = J_dofs_flat_offsets[i] + J_dofs_flat_sizes[i]
J_dofs_mat.append(J_dofs_flat[start:end].reshape((num_jdofs[i], num_bdofs[i])))
# Optional verbose output
if verbose:
print(f"J_dofs_flat_total_size: {J_dofs_flat_total_size}")
print(f"sum(J_dofs_flat_sizes): {sum(J_dofs_flat_sizes)}")
print(f"J_dofs_flat_sizes: {J_dofs_flat_sizes}")
print(f"J_dofs_flat_offsets: {J_dofs_flat_offsets}")
print("") # Add a newline for better readability
for i in range(num_worlds):
print(f"{i}: start={J_dofs_flat_offsets[i]}, end={J_dofs_flat_offsets[i] + J_dofs_flat_sizes[i]}")
print(f"J_dofs_mat[{i}] ({J_dofs_mat[i].shape}):\n{J_dofs_mat[i]}\n")
# Return the extracted Jacobians
return J_dofs_mat
def extract_delassus(
delassus: DelassusOperator | BlockSparseMatrixFreeDelassusOperator,
only_active_dims: bool = False,
) -> list[np.ndarray]:
if isinstance(delassus, BlockSparseMatrixFreeDelassusOperator):
return extract_delassus_sparse(delassus=delassus, only_active_dims=only_active_dims)
maxdim_wp_np = delassus.info.maxdim.numpy()
dim_wp_np = delassus.info.dim.numpy()
mio_wp_np = delassus.info.mio.numpy()
D_wp_np = delassus.D.numpy()
# Extract each Delassus matrix for each world
D_mat: list[np.ndarray] = []
for i in range(delassus.num_worlds):
D_maxdim = maxdim_wp_np[i]
D_start = mio_wp_np[i]
if only_active_dims:
D_dim = dim_wp_np[i]
else:
D_dim = D_maxdim
D_end = D_start + D_dim * D_dim
D_mat.append(D_wp_np[D_start:D_end].reshape((D_dim, D_dim)))
# Return the list of Delassus matrices
return D_mat
def extract_delassus_sparse(
delassus: BlockSparseMatrixFreeDelassusOperator, only_active_dims: bool = False
) -> list[np.ndarray]:
"""
Extracts the (dense) Delassus matrix from the sparse matrix-free Delassus operator by querying
individual matrix columns.
"""
num_worlds = delassus._model.size.num_worlds
sum_max_cts = delassus._model.size.sum_of_max_total_cts
max_cts_np = delassus._model.info.max_total_cts.numpy()
num_cts = delassus._data.info.num_total_cts
num_cts_np = num_cts.numpy()
max_dim = np.max(num_cts_np) if only_active_dims else np.max(max_cts_np)
D_mat: list[np.ndarray] = []
for world_id in range(num_worlds):
if only_active_dims:
D_mat.append(np.zeros((num_cts_np[world_id], num_cts_np[world_id]), dtype=np.float32))
else:
D_mat.append(np.zeros((max_cts_np[world_id], max_cts_np[world_id]), dtype=np.float32))
vec_query = wp.empty((sum_max_cts,), dtype=wp.float32, device=delassus._device)
vec_response = wp.empty((sum_max_cts,), dtype=wp.float32, device=delassus._device)
@wp.kernel
def _set_unit_entry(
# Inputs:
index: int,
world_dim: wp.array(dtype=wp.int32),
entry_start: wp.array(dtype=wp.int32),
# Output:
x: wp.array(dtype=wp.float32),
):
world_id = wp.tid()
if index >= world_dim[world_id]:
return
x[entry_start[world_id] + index] = 1.0
entry_start_np = delassus.bsm.row_start.numpy()
world_mask = wp.ones((num_worlds,), dtype=wp.int32, device=delassus._device)
for dim in range(max_dim):
# Query the operator by computing the product with a vector where only entry `dim` is set to 1.
vec_query.zero_()
wp.launch(
kernel=_set_unit_entry,
dim=num_worlds,
inputs=[
# Inputs:
dim,
num_cts,
delassus.bsm.row_start,
# Outputs:
vec_query,
],
)
delassus.matvec(vec_query, vec_response, world_mask)
vec_response_np = vec_response.numpy()
# Set the response as the corresponding column of each matrix
for world_id in range(num_worlds):
D_mat_dim = D_mat[world_id].shape[0]
if dim >= D_mat_dim:
continue
start_idx = entry_start_np[world_id]
D_mat[world_id][:, dim] = vec_response_np[start_idx : start_idx + D_mat_dim]
return D_mat
def extract_problem_vector(
delassus: DelassusOperator | BlockSparseMatrixFreeDelassusOperator,
vector: np.ndarray,
only_active_dims: bool = False,
) -> list[np.ndarray]:
maxdim_wp_np = delassus.info.maxdim.numpy()
dim_wp_np = delassus.info.dim.numpy()
vio_wp_np = delassus.info.vio.numpy()
num_worlds = delassus.num_worlds if isinstance(delassus, DelassusOperator) else delassus.num_matrices
# Extract each vector for each world
vectors_np: list[np.ndarray] = []
for i in range(num_worlds):
vec_maxdim = maxdim_wp_np[i]
vec_start = vio_wp_np[i]
vec_end = vec_start + vec_maxdim
if only_active_dims:
vec_end = vec_start + dim_wp_np[i]
else:
vec_end = vec_start + vec_maxdim
vectors_np.append(vector[vec_start:vec_end])
# Return the list of Delassus matrices
return vectors_np
def extract_info_vectors(offsets: np.ndarray, vectors: np.ndarray, dims: list[int] | None = None) -> list[np.ndarray]:
# Determine vector sizes
nv = offsets.size
maxn = vectors.size // nv
n = dims if dims is not None and len(dims) == nv else [maxn] * nv
# Extract each vector for each world
vectors_list: list[np.ndarray] = []
for i in range(nv):
vec_start = offsets[i]
vec_end = vec_start + n[i]
vectors_list.append(vectors[vec_start:vec_end])
# Return the list of Delassus matrices
return vectors_list
================================================
FILE: newton/_src/solvers/kamino/tests/utils/make.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Utilities for constructing test problem containers and data.
"""
import math
from collections.abc import Callable
import numpy as np
import warp as wp
from ..._src.core.bodies import update_body_inertias
from ..._src.core.builder import ModelBuilderKamino
from ..._src.core.data import DataKamino
from ..._src.core.math import quat_exp, screw, screw_angular, screw_linear
from ..._src.core.model import ModelKamino
from ..._src.core.state import StateKamino
from ..._src.core.types import float32, int32, mat33f, transformf, vec3f, vec6f
from ..._src.geometry.contacts import ContactsKamino
from ..._src.geometry.detector import CollisionDetector
from ..._src.kinematics.constraints import make_unilateral_constraints_info, update_constraints_info
from ..._src.kinematics.jacobians import DenseSystemJacobians, SparseSystemJacobians, SystemJacobiansType
from ..._src.kinematics.joints import compute_joints_data
from ..._src.kinematics.limits import LimitsKamino
from ..._src.models.builders import basics as _model_basics
from ..._src.models.builders import utils as _model_utils
from .print import (
print_data_info,
print_model_constraint_info,
)
###
# Module interface
###
__all__ = [
"make_containers",
"make_generalized_mass_matrices",
"make_inverse_generalized_mass_matrices",
"make_test_problem",
"make_test_problem_fourbar",
"update_containers",
]
###
# Module configs
###
wp.set_module_options({"enable_backward": False})
###
# NumPy Reference Data Generators
###
def make_generalized_mass_matrices(model: ModelKamino, data: DataKamino) -> list[np.ndarray]:
# Extract the masses and inertias as numpy arrays
m_i = model.bodies.m_i.numpy()
I_i = data.bodies.I_i.numpy()
# Initialize a list to hold the generalized mass matrices
M_np: list[np.ndarray] = []
# Iterate over each world in the model and construct the generalized mass matrix
num_worlds = model.info.num_worlds
num_bodies = model.info.num_bodies.numpy().tolist()
bodies_offset = model.info.bodies_offset.numpy().tolist()
for w in range(num_worlds):
nb = num_bodies[w]
bio = bodies_offset[w]
M = np.zeros((6 * nb, 6 * nb), dtype=np.float32)
for i in range(nb):
start = 6 * i
M[start : start + 3, start : start + 3] = m_i[bio + i] * np.eye(3) # Linear part
M[start + 3 : start + 6, start + 3 : start + 6] = I_i[bio + i] # Angular part
M_np.append(M)
# Return the list of generalized mass matrices
return M_np
def make_inverse_generalized_mass_matrices(model: ModelKamino, data: DataKamino) -> list[np.ndarray]:
# Extract the inverse masses and inertias as numpy arrays
inv_m_i = model.bodies.inv_m_i.numpy()
inv_I_i = data.bodies.inv_I_i.numpy()
# Initialize a list to hold the inverse generalized mass matrices
invM_np: list[np.ndarray] = []
# Iterate over each world in the model and construct the inverse generalized mass matrix
num_worlds = model.info.num_worlds
num_bodies = model.info.num_bodies.numpy().tolist()
bodies_offset = model.info.bodies_offset.numpy().tolist()
for w in range(num_worlds):
nb = num_bodies[w]
bio = bodies_offset[w]
invM = np.zeros((6 * nb, 6 * nb), dtype=np.float32)
for i in range(nb):
start = 6 * i
invM[start : start + 3, start : start + 3] = inv_m_i[bio + i] * np.eye(3) # Linear part
invM[start + 3 : start + 6, start + 3 : start + 6] = inv_I_i[bio + i] # Angular part
invM_np.append(invM)
# Return the list of inverse generalized mass matrices
return invM_np
###
# Test Problem Scaffolding
###
def make_containers(
builder: ModelBuilderKamino,
device: wp.DeviceLike = None,
max_world_contacts: int = 0,
sparse: bool = True,
dt: float = 0.001,
) -> tuple[ModelKamino, DataKamino, StateKamino, LimitsKamino, CollisionDetector, SystemJacobiansType]:
# Create the model from the builder
model = builder.finalize(device=device)
# Configure model time-steps
model.time.dt.fill_(wp.float32(dt))
model.time.inv_dt.fill_(wp.float32(1.0 / dt))
# Create a data and state container
data = model.data()
state = model.state()
# Create the limits container
limits = LimitsKamino(model=model)
# Create the collision detector
config = CollisionDetector.Config(max_contacts_per_world=max_world_contacts, pipeline="primitive")
detector = CollisionDetector(model=model, config=config)
# Construct the unilateral constraints members in the model info
make_unilateral_constraints_info(model, data, limits, detector.contacts)
# Create the Jacobians container
if sparse:
jacobians = SparseSystemJacobians(model=model, limits=limits, contacts=detector.contacts)
else:
jacobians = DenseSystemJacobians(model=model, limits=limits, contacts=detector.contacts)
# Return the model, data, detector, and jacobians
return model, data, state, limits, detector, jacobians
def update_containers(
model: ModelKamino,
data: DataKamino,
state: StateKamino,
limits: LimitsKamino | None = None,
detector: CollisionDetector | None = None,
jacobians: SystemJacobiansType | None = None,
):
# Update body inertias according to the current state of the bodies
update_body_inertias(model=model.bodies, data=data.bodies)
wp.synchronize()
# Update joint states according to the state of the bodies
compute_joints_data(model=model, data=data, q_j_p=wp.zeros_like(data.joints.q_j))
wp.synchronize()
# Run joint-limit detection to generate active limits
if limits is not None:
limits.detect(model=model, data=data)
wp.synchronize()
# Run collision detection to generate active contacts
if detector is not None:
detector.collide(data=data, state=state)
wp.synchronize()
# Update the constraint state info
update_constraints_info(model=model, data=data)
wp.synchronize()
# Build the dense system Jacobians
if jacobians is not None:
ldata = limits.data if limits is not None else None
cdata = detector.contacts.data if detector is not None else None
jacobians.build(model=model, data=data, limits=ldata, contacts=cdata)
wp.synchronize()
def make_test_problem(
builder: ModelBuilderKamino,
set_state_fn: Callable[[ModelKamino, DataKamino], None] | None = None,
device: wp.DeviceLike = None,
max_world_contacts: int = 12,
with_limits: bool = False,
with_contacts: bool = False,
dt: float = 0.001,
verbose: bool = False,
) -> tuple[ModelKamino, DataKamino, StateKamino, LimitsKamino | None, ContactsKamino | None]:
# Create the model from the builder
model = builder.finalize(device=device)
# Configure model time-steps
model.time.dt.fill_(wp.float32(dt))
model.time.inv_dt.fill_(wp.float32(1.0 / dt))
# Create a model state container
data = model.data(device=device)
state = model.state(device=device)
# Construct and allocate the limits container
limits = None
if with_limits:
limits = LimitsKamino(model=model, device=device)
# Create the collision detector
contacts = None
if with_contacts:
config = CollisionDetector.Config(max_contacts_per_world=max_world_contacts, pipeline="primitive")
detector = CollisionDetector(model=model, config=config, device=device)
contacts = detector.contacts
# Create the constraints info
make_unilateral_constraints_info(
model=model,
data=data,
limits=limits,
contacts=contacts,
device=device,
)
if verbose:
print("") # Add a newline for better readability
print_model_constraint_info(model)
print_data_info(data)
print("\n") # Add a newline for better readability
# If a set-state callback is provided, perturb the system
# NOTE: This is done to potentially trigger
# joint limits and contacts to become active
if set_state_fn is not None:
set_state_fn(model=model, data=data)
wp.synchronize()
if verbose:
print("data.bodies.q_i:\n", data.bodies.q_i)
print("data.bodies.u_i:\n\n", data.bodies.u_i)
# Compute the joints state
compute_joints_data(model=model, data=data, q_j_p=wp.zeros_like(data.joints.q_j))
wp.synchronize()
if verbose:
print("data.joints.p_j:\n", data.joints.p_j)
print("data.joints.r_j:\n", data.joints.r_j)
print("data.joints.dr_j:\n", data.joints.dr_j)
print("data.joints.q_j:\n", data.joints.q_j)
print("data.joints.dq_j:\n\n", data.joints.dq_j)
# Run limit detection to generate active limits
if with_limits:
limits.detect(model, data)
wp.synchronize()
if verbose:
print(f"limits.world_active_limits: {limits.world_active_limits}")
print(f"data.info.num_limits: {data.info.num_limits}\n\n")
# Run collision detection to generate active contacts
if with_contacts:
detector.collide(data, state)
wp.synchronize()
if verbose:
print(f"contacts.world_active_contacts: {detector.contacts.world_active_contacts}")
print(f"data.info.num_contacts: {data.info.num_contacts}\n\n")
# Update the constraints info
update_constraints_info(model=model, data=data)
if verbose:
print("") # Add a newline for better readability
print_data_info(data)
print("\n") # Add a newline for better readability
wp.synchronize()
# Return the problem data containers
return model, data, state, limits, contacts
def make_constraint_multiplier_arrays(model: ModelKamino) -> tuple[wp.array, wp.array]:
with wp.ScopedDevice(model.device):
lambdas = wp.zeros(model.size.sum_of_max_total_cts, dtype=float32)
return model.info.total_cts_offset, lambdas
###
# Fourbar
#
# Generates a problem using basics.boxes_fourbar model with a specific
# state configuration that induces active joint limits and contacts.
###
Q_X_J = 0.3 * math.pi
THETA_Y_J = 0.0
THETA_Z_J = 0.0
J_DR_J = vec3f(0.0)
J_DV_J = vec3f(0.0)
J_DOMEGA_J = vec3f(0.0)
@wp.kernel
def _set_fourbar_body_states(
model_joint_bid_B: wp.array(dtype=int32),
model_joint_bid_F: wp.array(dtype=int32),
model_joint_B_r_Bj: wp.array(dtype=vec3f),
model_joint_F_r_Fj: wp.array(dtype=vec3f),
model_joint_X_j: wp.array(dtype=mat33f),
state_body_q_i: wp.array(dtype=transformf),
state_body_u_i: wp.array(dtype=vec6f),
):
"""
Set the state of the bodies to a certain values in order to check computations of joint states.
"""
# Retrieve the thread index as the joint index
jid = wp.tid()
# Retrieve the joint parameters
bid_B = model_joint_bid_B[jid]
bid_F = model_joint_bid_F[jid]
B_r_Bj = model_joint_B_r_Bj[jid]
F_r_Fj = model_joint_F_r_Fj[jid]
X_j = model_joint_X_j[jid]
# Retrieve the current state of the Base body
p_B = state_body_q_i[bid_B]
u_B = state_body_u_i[bid_B]
# Extract the position and orientation of the Base body
r_B = wp.transform_get_translation(p_B)
q_B = wp.transform_get_rotation(p_B)
R_B = wp.quat_to_matrix(q_B)
# Extract the linear and angular velocity of the Base body
v_B = screw_linear(u_B)
omega_B = screw_angular(u_B)
# Define the joint rotation offset
# NOTE: X_j projects quantities into the joint frame
# NOTE: X_j^T projects quantities into the outer frame (world or body)
q_x_j = Q_X_J * wp.pow(-1.0, float(jid)) # Alternate sign for each joint
theta_y_j = THETA_Y_J
theta_z_j = THETA_Z_J
j_dR_j = vec3f(q_x_j, theta_y_j, theta_z_j) # Joint offset as rotation vector
q_jq = quat_exp(j_dR_j) # Joint offset as rotation quaternion
R_jq = wp.quat_to_matrix(q_jq) # Joint offset as rotation matrix
# Define the joint translation offset
j_dr_j = J_DR_J
# Define the joint twist offset
j_dv_j = J_DV_J
j_domega_j = J_DOMEGA_J
# Follower body rotation via the Base and joint frames
R_B_X_j = R_B @ X_j
R_F_new = R_B_X_j @ R_jq @ wp.transpose(X_j)
q_F_new = wp.quat_from_matrix(R_F_new)
# Follower body position via the Base and joint frames
r_Fj = R_F_new @ F_r_Fj
r_F_new = r_B + R_B @ B_r_Bj + R_B_X_j @ j_dr_j - r_Fj
# Follower body twist via the Base and joint frames
r_Bj = R_B @ B_r_Bj
r_Fj = R_F_new @ F_r_Fj
omega_F_new = R_B_X_j @ j_domega_j + omega_B
v_F_new = R_B_X_j @ j_dv_j + v_B + wp.cross(omega_B, r_Bj) - wp.cross(omega_F_new, r_Fj)
# Offset the pose of the body by a fixed amount
state_body_q_i[bid_F] = wp.transformation(r_F_new, q_F_new, dtype=float32)
state_body_u_i[bid_F] = screw(v_F_new, omega_F_new)
def set_fourbar_body_states(model: ModelKamino, data: DataKamino):
wp.launch(
_set_fourbar_body_states,
dim=3, # Set to three because we only need to set the first three joints
inputs=[
model.joints.bid_B,
model.joints.bid_F,
model.joints.B_r_Bj,
model.joints.F_r_Fj,
model.joints.X_j,
data.bodies.q_i,
data.bodies.u_i,
],
)
def make_test_problem_fourbar(
device: wp.DeviceLike = None,
max_world_contacts: int = 12,
num_worlds: int = 1,
with_limits: bool = False,
with_contacts: bool = False,
with_implicit_joints: bool = True,
verbose: bool = False,
) -> tuple[ModelKamino, DataKamino, StateKamino, LimitsKamino | None, ContactsKamino | None]:
# Define the problem using the ModelBuilderKamino
builder: ModelBuilderKamino = _model_utils.make_homogeneous_builder(
num_worlds=num_worlds,
build_fn=_model_basics.build_boxes_fourbar,
dynamic_joints=with_implicit_joints,
implicit_pd=with_implicit_joints,
)
# Generate the problem containers using the builder
return make_test_problem(
builder=builder,
set_state_fn=set_fourbar_body_states,
device=device,
max_world_contacts=max_world_contacts,
with_limits=with_limits,
with_contacts=with_contacts,
verbose=verbose,
)
def make_test_problem_heterogeneous(
device: wp.DeviceLike = None,
max_world_contacts: int = 12,
with_limits: bool = False,
with_contacts: bool = False,
with_implicit_joints: bool = True,
verbose: bool = False,
) -> tuple[ModelKamino, DataKamino, StateKamino, LimitsKamino | None, ContactsKamino | None]:
# Define the problem using the ModelBuilderKamino
builder: ModelBuilderKamino = _model_basics.make_basics_heterogeneous_builder(
dynamic_joints=with_implicit_joints,
implicit_pd=with_implicit_joints,
)
# Generate the problem containers using the builder
return make_test_problem(
builder=builder,
device=device,
max_world_contacts=max_world_contacts,
with_limits=with_limits,
with_contacts=with_contacts,
verbose=verbose,
)
================================================
FILE: newton/_src/solvers/kamino/tests/utils/print.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
KAMINO: UNIT TESTS: GENERAL UTILITIES
"""
import numpy as np
from ..._src.core.data import DataKamino
from ..._src.core.model import ModelKamino
###
# Model Functions
###
def print_model_size(model: ModelKamino):
print("Model Size:")
# Print the host-side model size meta-data
print(f"model.size.num_worlds: {model.size.num_worlds}")
# Print the device-side model size data
print(f"model.size.sum_of_num_bodies: {model.size.sum_of_num_bodies}")
print(f"model.size.max_of_num_bodies: {model.size.max_of_num_bodies}")
print(f"model.size.sum_of_num_joints: {model.size.sum_of_num_joints}")
print(f"model.size.max_of_num_joints: {model.size.max_of_num_joints}")
print(f"model.size.sum_of_num_material_pairs: {model.size.sum_of_num_material_pairs}")
print(f"model.size.max_of_num_material_pairs: {model.size.max_of_num_material_pairs}")
print(f"model.size.sum_of_num_body_dofs: {model.size.sum_of_num_body_dofs}")
print(f"model.size.max_of_num_body_dofs: {model.size.max_of_num_body_dofs}")
print(f"model.size.sum_of_num_joint_dofs: {model.size.sum_of_num_joint_dofs}")
print(f"model.size.max_of_num_joint_dofs: {model.size.max_of_num_joint_dofs}")
print(f"model.size.sum_of_max_unilaterals: {model.size.sum_of_max_unilaterals}")
print(f"model.size.max_of_max_unilaterals: {model.size.max_of_max_unilaterals}")
def print_model_info(model: ModelKamino):
print("===============================================================================")
print("Model Info:")
# Print the host-side model info meta-data
print("-------------------------------------------------------------------------------")
print(f"model.info.num_worlds: {model.info.num_worlds}")
# Print the device-side model info data
print("-------------------------------------------------------------------------------")
print(f"model.info.num_bodies: {model.info.num_bodies}")
print(f"model.info.num_joints: {model.info.num_joints}")
print(f"model.info.num_passive_joints: {model.info.num_passive_joints}")
print(f"model.info.num_actuated_joints: {model.info.num_actuated_joints}")
print(f"model.info.num_dynamic_joints: {model.info.num_dynamic_joints}")
print(f"model.info.num_geoms: {model.info.num_geoms}")
print(f"model.info.max_limits: {model.info.max_limits}")
print(f"model.info.max_contacts: {model.info.max_contacts}")
print("-------------------------------------------------------------------------------")
print(f"model.info.num_body_dofs: {model.info.num_body_dofs}")
print(f"model.info.num_joint_coords: {model.info.num_joint_coords}")
print(f"model.info.num_joint_dofs: {model.info.num_joint_dofs}")
print(f"model.info.num_passive_joint_coords: {model.info.num_passive_joint_coords}")
print(f"model.info.num_passive_joint_dofs: {model.info.num_passive_joint_dofs}")
print(f"model.info.num_actuated_joint_coords: {model.info.num_actuated_joint_coords}")
print(f"model.info.num_actuated_joint_dofs: {model.info.num_actuated_joint_dofs}")
print("-------------------------------------------------------------------------------")
print(f"model.info.num_joint_cts: {model.info.num_joint_cts}")
print(f"model.info.num_joint_dynamic_cts: {model.info.num_joint_dynamic_cts}")
print(f"model.info.num_joint_kinematic_cts: {model.info.num_joint_kinematic_cts}")
print(f"model.info.max_limit_cts: {model.info.max_limit_cts}")
print(f"model.info.max_contact_cts: {model.info.max_contact_cts}")
print(f"model.info.max_total_cts: {model.info.max_total_cts}")
# Print the element offsets
print("-------------------------------------------------------------------------------")
print(f"model.info.bodies_offset: {model.info.bodies_offset}")
print(f"model.info.joints_offset: {model.info.joints_offset}")
print(f"model.info.limits_offset: {model.info.limits_offset}")
print(f"model.info.contacts_offset: {model.info.contacts_offset}")
print(f"model.info.unilaterals_offset: {model.info.unilaterals_offset}")
# Print the coords, DoFs and constraint offsets
print("-------------------------------------------------------------------------------")
print(f"model.info.body_dofs_offset: {model.info.body_dofs_offset}")
print(f"model.info.joint_coords_offset: {model.info.joint_coords_offset}")
print(f"model.info.joint_dofs_offset: {model.info.joint_dofs_offset}")
print(f"model.info.joint_passive_coords_offset: {model.info.joint_passive_coords_offset}")
print(f"model.info.joint_passive_dofs_offset: {model.info.joint_passive_dofs_offset}")
print(f"model.info.joint_actuated_coords_offset: {model.info.joint_actuated_coords_offset}")
print(f"model.info.joint_actuated_dofs_offset: {model.info.joint_actuated_dofs_offset}")
print("-------------------------------------------------------------------------------")
print(f"model.info.joint_cts_offset: {model.info.joint_cts_offset}")
print(f"model.info.joint_dynamic_cts_offset: {model.info.joint_dynamic_cts_offset}")
print(f"model.info.joint_kinematic_cts_offset: {model.info.joint_kinematic_cts_offset}")
print("-------------------------------------------------------------------------------")
print(f"model.info.total_cts_offset: {model.info.total_cts_offset}")
print(f"model.info.joint_dynamic_cts_group_offset: {model.info.joint_dynamic_cts_group_offset}")
print(f"model.info.joint_kinematic_cts_group_offset: {model.info.joint_kinematic_cts_group_offset}")
# Print the inertial properties
print("-------------------------------------------------------------------------------")
print(f"model.info.mass_min: {model.info.mass_min}")
print(f"model.info.mass_max: {model.info.mass_max}")
print(f"model.info.mass_total: {model.info.mass_total}")
print(f"model.info.inertia_total: {model.info.inertia_total}")
def print_model_constraint_info(model: ModelKamino):
print("Model Constraint Info:")
print("-------------------------------------------------------------------------------")
print(f"model.info.max_limits: {model.info.max_limits}")
print(f"model.info.max_contacts: {model.info.max_contacts}")
print("-------------------------------------------------------------------------------")
print(f"model.info.num_joint_cts: {model.info.num_joint_cts}")
print(f"model.info.num_joint_dynamic_cts: {model.info.num_joint_dynamic_cts}")
print(f"model.info.num_joint_kinematic_cts: {model.info.num_joint_kinematic_cts}")
print(f"model.info.max_limit_cts: {model.info.max_limit_cts}")
print(f"model.info.max_contact_cts: {model.info.max_contact_cts}")
print(f"model.info.max_total_cts: {model.info.max_total_cts}")
print("-------------------------------------------------------------------------------")
print(f"model.info.limits_offset: {model.info.limits_offset}")
print(f"model.info.contacts_offset: {model.info.contacts_offset}")
print(f"model.info.unilaterals_offset: {model.info.unilaterals_offset}")
print("-------------------------------------------------------------------------------")
print(f"model.info.body_dofs_offset: {model.info.body_dofs_offset}")
print(f"model.info.joint_coords_offset: {model.info.joint_coords_offset}")
print(f"model.info.joint_dofs_offset: {model.info.joint_dofs_offset}")
print(f"model.info.joint_passive_coords_offset: {model.info.joint_passive_coords_offset}")
print(f"model.info.joint_passive_dofs_offset: {model.info.joint_passive_dofs_offset}")
print(f"model.info.joint_actuated_coords_offset: {model.info.joint_actuated_coords_offset}")
print(f"model.info.joint_actuated_dofs_offset: {model.info.joint_actuated_dofs_offset}")
print("-------------------------------------------------------------------------------")
print(f"model.info.joint_cts_offset: {model.info.joint_cts_offset}")
print(f"model.info.joint_dynamic_cts_offset: {model.info.joint_dynamic_cts_offset}")
print(f"model.info.joint_kinematic_cts_offset: {model.info.joint_kinematic_cts_offset}")
print("-------------------------------------------------------------------------------")
print(f"model.info.total_cts_offset: {model.info.total_cts_offset}")
print(f"model.info.joint_dynamic_cts_group_offset: {model.info.joint_dynamic_cts_group_offset}")
print(f"model.info.joint_kinematic_cts_group_offset: {model.info.joint_kinematic_cts_group_offset}")
def print_model_bodies(model: ModelKamino, inertias=True, initial_states=True):
print(f"model.bodies.num_bodies: {model.bodies.num_bodies}")
print(f"model.bodies.wid: {model.bodies.wid}")
print(f"model.bodies.bid: {model.bodies.bid}")
if inertias:
print(f"model.bodies.m_i: {model.bodies.m_i}")
print(f"model.bodies.inv_m_i:\n{model.bodies.inv_m_i}")
print(f"model.bodies.i_I_i:\n{model.bodies.i_I_i}")
print(f"model.bodies.inv_i_I_i:\n{model.bodies.inv_i_I_i}")
if initial_states:
print(f"model.bodies.q_i_0:\n{model.bodies.q_i_0}")
print(f"model.bodies.u_i_0:\n{model.bodies.u_i_0}")
def print_model_joints(
model: ModelKamino,
dimensions=True,
offsets=True,
parameters=True,
limits=True,
dynamics=True,
):
print(f"model.joints.num_joints: {model.joints.num_joints}")
print(f"model.joints.wid: {model.joints.wid}")
print(f"model.joints.jid: {model.joints.jid}")
print(f"model.joints.dof_type: {model.joints.dof_type}")
print(f"model.joints.act_type: {model.joints.act_type}")
print(f"model.joints.num_dynamic_cts: {model.joints.num_dynamic_cts}")
print(f"model.joints.num_kinematic_cts: {model.joints.num_kinematic_cts}")
print(f"model.joints.bid_B: {model.joints.bid_B}")
print(f"model.joints.bid_F: {model.joints.bid_F}")
print(f"model.joints.B_r_Bj:\n{model.joints.B_r_Bj}")
print(f"model.joints.F_r_Fj:\n{model.joints.F_r_Fj}")
print(f"model.joints.X_j:\n{model.joints.X_j}")
print(f"model.joints.q_j_0: {model.joints.q_j_0}")
print(f"model.joints.dq_j_0: {model.joints.dq_j_0}")
if dimensions:
print(f"model.joints.num_coords: {model.joints.num_coords}")
print(f"model.joints.num_dofs: {model.joints.num_dofs}")
# TODO: print(f"model.joints.num_cts: {model.joints.num_cts}")
print(f"model.joints.num_dynamic_cts: {model.joints.num_dynamic_cts}")
print(f"model.joints.num_kinematic_cts: {model.joints.num_kinematic_cts}")
if offsets:
print(f"model.joints.coords_offset: {model.joints.coords_offset}")
print(f"model.joints.dofs_offset: {model.joints.dofs_offset}")
print(f"model.joints.passive_coords_offset: {model.joints.passive_coords_offset}")
print(f"model.joints.passive_dofs_offset: {model.joints.passive_dofs_offset}")
print(f"model.joints.actuated_coords_offset: {model.joints.actuated_coords_offset}")
print(f"model.joints.actuated_dofs_offset: {model.joints.actuated_dofs_offset}")
# TODO: print(f"model.joints.cts_offset: {model.joints.cts_offset}")
print(f"model.joints.dynamic_cts_offset: {model.joints.dynamic_cts_offset}")
print(f"model.joints.kinematic_cts_offset: {model.joints.kinematic_cts_offset}")
if parameters:
print(f"model.joints.B_r_Bj: {model.joints.B_r_Bj}")
print(f"model.joints.F_r_Fj: {model.joints.F_r_Fj}")
print(f"model.joints.X_j: {model.joints.X_j}")
if limits:
print(f"model.joints.q_j_min: {model.joints.q_j_min}")
print(f"model.joints.q_j_max: {model.joints.q_j_max}")
print(f"model.joints.dq_j_max: {model.joints.dq_j_max}")
print(f"model.joints.tau_j_max: {model.joints.tau_j_max}")
if dynamics:
print(f"model.joints.a_j: {model.joints.a_j}")
print(f"model.joints.b_j: {model.joints.b_j}")
print(f"model.joints.k_p_j: {model.joints.k_p_j}")
print(f"model.joints.k_d_j: {model.joints.k_d_j}")
# TODO: RENAME print_data_info
def print_data_info(data: DataKamino):
print("===============================================================================")
print("data.info.num_limits: ", data.info.num_limits)
print("data.info.num_contacts: ", data.info.num_contacts)
print("-------------------------------------------------------------------------------")
print("data.info.num_total_cts: ", data.info.num_total_cts)
print("data.info.num_limit_cts: ", data.info.num_limit_cts)
print("data.info.num_contact_cts: ", data.info.num_contact_cts)
print("-------------------------------------------------------------------------------")
print("data.info.limit_cts_group_offset: ", data.info.limit_cts_group_offset)
print("data.info.contact_cts_group_offset: ", data.info.contact_cts_group_offset)
def print_data(data: DataKamino, info=True):
# Print the state info
if info:
print_data_info(data)
# Print body state data
print(f"data.bodies.I_i: {data.bodies.I_i}")
print(f"data.bodies.inv_I_i: {data.bodies.inv_I_i}")
print(f"data.bodies.q_i: {data.bodies.q_i}")
print(f"data.bodies.u_i: {data.bodies.u_i}")
print(f"data.bodies.w_i: {data.bodies.w_i}")
print(f"data.bodies.w_a_i: {data.bodies.w_a_i}")
print(f"data.bodies.w_j_i: {data.bodies.w_j_i}")
print(f"data.bodies.w_l_i: {data.bodies.w_l_i}")
print(f"data.bodies.w_c_i: {data.bodies.w_c_i}")
print(f"data.bodies.w_e_i: {data.bodies.w_e_i}")
# Print joint state data
print(f"data.joints.p_j: {data.joints.p_j}")
print(f"data.joints.q_j: {data.joints.q_j}")
print(f"data.joints.dq_j: {data.joints.dq_j}")
print(f"data.joints.tau_j: {data.joints.tau_j}")
print(f"data.joints.r_j: {data.joints.r_j}")
print(f"data.joints.dr_j: {data.joints.dr_j}")
print(f"data.joints.lambda_j: {data.joints.lambda_j}")
print(f"data.joints.m_j: {data.joints.m_j}")
print(f"data.joints.inv_m_j: {data.joints.inv_m_j}")
print(f"data.joints.q_j_ref: {data.joints.q_j_ref}")
print(f"data.joints.dq_j_ref: {data.joints.dq_j_ref}")
print(f"data.joints.j_w_j: {data.joints.j_w_j}")
print(f"data.joints.j_w_a_j: {data.joints.j_w_a_j}")
print(f"data.joints.j_w_c_j: {data.joints.j_w_c_j}")
print(f"data.joints.j_w_l_j: {data.joints.j_w_l_j}")
# Print the geometry state data
print(f"data.geoms.pose: {data.geoms.pose}")
###
# General-Purpose Functions
###
def print_error_stats(name, arr, ref, n, show_errors=False):
err = arr - ref
err_abs = np.abs(err)
err_l2 = np.linalg.norm(err)
err_mean = np.sum(err_abs) / n
err_max = np.max(err_abs)
if show_errors:
print(f"{name}_err ({err.shape}):\n{err}")
print(f"{name}_err_l2: {err_l2}")
print(f"{name}_err_mean: {err_mean}")
print(f"{name}_err_max: {err_max}\n\n")
================================================
FILE: newton/_src/solvers/kamino/tests/utils/rand.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Unit-test utilities for generating random linear system and factorization problems."""
import numpy as np
import warp as wp
from ..._src.core.types import FloatArrayLike, float32, int32
from ..._src.linalg.utils.rand import (
random_rhs_for_matrix,
random_spd_matrix,
random_symmetric_matrix,
)
###
# Classes
###
class RandomProblemLLT:
def __init__(
self,
seed: int = 42,
dims: list[int] | int | None = None,
maxdims: list[int] | int | None = None,
A: list[np.ndarray] | None = None,
b: list[np.ndarray] | None = None,
np_dtype=np.float32,
wp_dtype=float32,
device: wp.DeviceLike = None,
upper: bool = False,
):
# Check input data to ensure they are indeed lists of numpy arrays
if A is not None:
if not isinstance(A, list) or not all(isinstance(a, np.ndarray) for a in A):
raise TypeError("A must be a list of numpy arrays.")
dims = [a.shape[0] for a in A] # Update dims based on provided A
if b is not None:
if not isinstance(b, list) or not all(isinstance(b_i, np.ndarray) for b_i in b):
raise TypeError("b must be a list of numpy arrays.")
# Ensure the problem dimensions are valid
if isinstance(dims, int):
dims = [dims]
elif isinstance(dims, list):
if not all(isinstance(d, int) and d > 0 for d in dims):
raise ValueError("All dimensions must be positive integers.")
else:
raise TypeError("Dimensions must be an integer or a list of integers.")
# Ensure the max problem dimensions are valid if provided, otherwise set them to dims
if maxdims is not None:
if isinstance(maxdims, int):
maxdims = [maxdims] * len(dims)
elif isinstance(maxdims, list):
if not all(isinstance(md, int) and md > 0 for md in maxdims):
raise ValueError("All max dimensions must be positive integers.")
if len(maxdims) != len(dims):
raise ValueError("maxdims must have the same length as dims.")
if any(md < d for md, d in zip(maxdims, dims, strict=False)):
raise ValueError("All maxdims must be greater than or equal to dims.")
else:
raise TypeError("maxdims must be an integer or a list of integers.")
else:
maxdims = dims
# Cache the problem configurations
self.num_blocks: int = len(dims)
self.dims: list[int] = dims
self.maxdims: list[int] = maxdims
self.seed: int = seed
self.np_dtype = np_dtype
self.wp_dtype = wp_dtype
self.device: wp.DeviceLike = device
# Declare the lists of reference problem data
self.A_np: list[np.ndarray] = []
self.b_np: list[np.ndarray] = []
self.X_np: list[np.ndarray] = []
self.y_np: list[np.ndarray] = []
self.x_np: list[np.ndarray] = []
# Declare the warp arrays of concatenated problem data
self.maxdim_wp: wp.array | None = None
self.dim_wp: wp.array | None = None
self.mio_wp: wp.array | None = None
self.vio_wp: wp.array | None = None
self.A_wp: wp.array | None = None
self.b_wp: wp.array | None = None
# Initialize the flattened problem data
A_sizes = [n * n for n in self.maxdims]
A_offsets = [0] + [sum(A_sizes[:i]) for i in range(1, len(A_sizes) + 1)]
A_flat_size = sum(A_sizes)
A_flat = np.full(shape=(A_flat_size,), fill_value=np.inf, dtype=np_dtype)
b_sizes = list(self.maxdims)
b_offsets = [0] + [sum(b_sizes[:i]) for i in range(1, len(b_sizes) + 1)]
b_flat_size = sum(b_sizes)
b_flat = np.full(shape=(b_flat_size,), fill_value=np.inf, dtype=np_dtype)
# Generate randomized problem data
for i, (n, nmax) in enumerate(zip(self.dims, self.maxdims, strict=False)):
# Generate a random SPD matrix if not provided
if A is None:
A_mat = random_spd_matrix(dim=n, seed=self.seed, dtype=np_dtype)
else:
A_mat = A[i]
# Generate a random RHS vector if not provided
if b is None:
b_vec = random_rhs_for_matrix(A_mat)
else:
b_vec = b[i]
# Compute the Cholesky decomposition using numpy
X_mat = np.linalg.cholesky(A_mat, upper=upper)
# Compute final and intermediate reference solutions
if upper:
y_vec, x_vec = _solve_cholesky_upper_numpy(X_mat, b_vec)
else:
y_vec, x_vec = _solve_cholesky_lower_numpy(X_mat, b_vec)
# Store the reference data
self.A_np.append(A_mat)
self.b_np.append(b_vec)
self.X_np.append(X_mat)
self.y_np.append(y_vec)
self.x_np.append(x_vec)
# Flatten the matrix and store it in the A_flat array
A_start = A_offsets[len(self.A_np) - 1]
# Fill the flattened array row-wise to account for dim <= maxdim
if n == nmax:
A_end = A_offsets[len(self.A_np)]
A_flat[A_start:A_end] = A_mat.flat
else:
A_end = A_start + n * n
A_flat[A_start:A_end] = A_mat.flat
# Flatten the vector and store it in the b_flat array
b_start = b_offsets[len(self.b_np) - 1]
b_end = b_start + n
b_flat[b_start:b_end] = b_vec
# Construct the warp arrays
with wp.ScopedDevice(self.device):
self.maxdim_wp = wp.array(self.maxdims, dtype=int32)
self.dim_wp = wp.array(self.dims, dtype=int32)
self.mio_wp = wp.array(A_offsets[: self.num_blocks], dtype=int32)
self.vio_wp = wp.array(b_offsets[: self.num_blocks], dtype=int32)
self.A_wp = wp.array(A_flat, dtype=float32)
self.b_wp = wp.array(b_flat, dtype=float32)
def __str__(self) -> str:
return (
f"RandomProblemLLT("
f"\nseed: {self.seed}"
f"\nnum_blocks: {self.num_blocks}"
f"\nnp_dtype: {self.np_dtype}"
f"\nwp_dtype: {self.wp_dtype}"
f"\ndims: {self.dims}"
f"\nmaxdims: {self.maxdims}"
f"\ndevice: {self.device}"
f"\nA_np (shape): {[A.shape for A in self.A_np]}"
f"\nb_np (shape): {[b.shape for b in self.b_np]}"
f"\nX_np (shape): {[X.shape for X in self.X_np]}"
f"\ny_np (shape): {[y.shape for y in self.y_np]}"
f"\nx_np (shape): {[x.shape for x in self.x_np]}"
f"\nmaxdim_wp: {self.maxdim_wp.numpy()}"
f"\ndim_wp: {self.dim_wp.numpy()}"
f"\nmio_wp: {self.mio_wp.numpy()}"
f"\nvio_wp: {self.vio_wp.numpy()}"
f"\nA_wp (shape): {self.A_wp.shape}"
f"\nb_wp (shape): {self.b_wp.shape}"
f"\n)"
)
class RandomProblemLDLT:
def __init__(
self,
seed: int = 42,
dims: list[int] | int | None = None,
maxdims: list[int] | int | None = None,
ranks: list[int] | int | None = None,
eigenvalues: FloatArrayLike | None = None,
A: list[np.ndarray] | None = None,
b: list[np.ndarray] | None = None,
np_dtype=np.float32,
wp_dtype=float32,
device: wp.DeviceLike = None,
lower: bool = True,
):
# Attempt to import scipy.linalg, which is required
# for the LDLT decomposition and reference solutions
try:
import scipy.linalg
except ImportError as e:
raise ImportError(
"scipy is required for RandomProblemLDLT but is not installed. install with: pip install scipy"
) from e
# Check input data to ensure they are indeed lists of numpy arrays
if A is not None:
if not isinstance(A, list) or not all(isinstance(a, np.ndarray) for a in A):
raise TypeError("A must be a list of numpy arrays.")
dims = [a.shape[0] for a in A] # Update dims based on provided A
if b is not None:
if not isinstance(b, list) or not all(isinstance(b_i, np.ndarray) for b_i in b):
raise TypeError("b must be a list of numpy arrays.")
# Ensure the problem dimensions are valid
if isinstance(dims, int):
dims = [dims]
elif isinstance(dims, list):
if not all(isinstance(d, int) for d in dims):
raise ValueError("All dimensions must be integers.")
else:
raise TypeError("Dimensions must be an integer or a list of integers.")
# Ensure the max problem dimensions are valid if provided, otherwise set them to dims
if maxdims is not None:
if isinstance(maxdims, int):
maxdims = [maxdims] * len(dims)
elif isinstance(maxdims, list):
if not all(isinstance(md, int) for md in maxdims):
raise ValueError("All max dimensions must be integers.")
if len(maxdims) != len(dims):
raise ValueError("maxdims must have the same length as dims.")
else:
raise TypeError("maxdims must be an integer or a list of integers.")
else:
maxdims = dims
# Ensure the rank dimensions are valid
if ranks is not None:
if isinstance(ranks, int):
ranks = [ranks]
elif isinstance(ranks, list):
if not all(isinstance(r, int) for r in ranks):
raise ValueError("All ranks must be integers.")
else:
raise TypeError("Ranks must be an integer or a list of integers.")
else:
ranks = [None] * len(dims)
# Ensure the eigenvalues are valid
if eigenvalues is not None:
if not isinstance(eigenvalues, list) or not all(isinstance(ev, int | float) for ev in eigenvalues):
raise TypeError("Eigenvalues must be a list of numbers.")
else:
eigenvalues = [None] * len(dims)
# Cache the problem configurations
self.num_blocks: int = len(dims)
self.dims: list[int] = dims
self.maxdims: list[int] = maxdims
self.seed: int = seed
self.np_dtype = np_dtype
self.wp_dtype = wp_dtype
self.device: wp.DeviceLike = device
# Declare the lists of reference problem data
self.A_np: list[np.ndarray] = []
self.b_np: list[np.ndarray] = []
self.X_np: list[np.ndarray] = []
self.D_np: list[np.ndarray] = []
self.P_np: list[np.ndarray] = []
self.z_np: list[np.ndarray] = []
self.y_np: list[np.ndarray] = []
self.x_np: list[np.ndarray] = []
# Declare the warp arrays of concatenated problem data
self.maxdim_wp: wp.array | None = None
self.dim_wp: wp.array | None = None
self.mio_wp: wp.array | None = None
self.vio_wp: wp.array | None = None
self.A_wp: wp.array | None = None
self.b_wp: wp.array | None = None
# Initialize the flattened problem data
A_sizes = [n * n for n in self.maxdims]
A_offsets = [0] + [sum(A_sizes[:i]) for i in range(1, len(A_sizes) + 1)]
A_flat_size = sum(A_sizes)
A_flat = np.ndarray(shape=(A_flat_size,), dtype=np_dtype)
b_sizes = list(self.maxdims)
b_offsets = [0] + [sum(b_sizes[:i]) for i in range(1, len(b_sizes) + 1)]
b_flat_size = sum(b_sizes)
b_flat = np.ndarray(shape=(b_flat_size,), dtype=np_dtype)
# Generate randomized problem data
for i, (n, nmax) in enumerate(zip(self.dims, self.maxdims, strict=False)):
# Generate a random SPD matrix if not provided
if A is None:
A_mat = random_symmetric_matrix(
dim=n, seed=self.seed, rank=ranks[i], eigenvalues=eigenvalues[i], dtype=np_dtype
)
else:
A_mat = A[i]
# Generate a random RHS vector if not provided
if b is None:
b_vec = random_rhs_for_matrix(A_mat)
else:
b_vec = b[i]
# Compute the LDLT decomposition using numpy
X_mat, D_mat, P_mat = scipy.linalg.ldl(A_mat, lower=lower)
# Compute final and intermediate reference solutions
if lower:
z_vec, y_vec, x_vec = _solve_ldlt_lower_numpy(X_mat, D_mat, P_mat, b_vec)
else:
z_vec, y_vec, x_vec = _solve_ldlt_upper_numpy(X_mat, D_mat, P_mat, b_vec)
# Store the reference data
self.A_np.append(A_mat)
self.b_np.append(b_vec)
self.X_np.append(X_mat)
self.D_np.append(D_mat)
self.P_np.append(P_mat)
self.z_np.append(z_vec)
self.y_np.append(y_vec)
self.x_np.append(x_vec)
# Flatten the matrix and store it in the A_flat array
A_start = A_offsets[len(self.A_np) - 1]
# Fill the flattened array row-wise to account for dim <= maxdim
if n == nmax:
A_end = A_offsets[len(self.A_np)]
A_flat[A_start:A_end] = A_mat.flat
else:
A_end = A_start + n * n
A_flat[A_start:A_end] = A_mat.flat
# Flatten the vector and store it in the b_flat array
b_start = b_offsets[len(self.b_np) - 1]
b_end = b_offsets[len(self.b_np)]
b_flat[b_start:b_end] = b_vec
# Construct the warp arrays
with wp.ScopedDevice(self.device):
self.maxdim_wp = wp.array(self.maxdims, dtype=int32)
self.dim_wp = wp.array(self.dims, dtype=int32)
self.mio_wp = wp.array(A_offsets[: self.num_blocks], dtype=int32)
self.vio_wp = wp.array(b_offsets[: self.num_blocks], dtype=int32)
self.A_wp = wp.array(A_flat, dtype=float32)
self.b_wp = wp.array(b_flat, dtype=float32)
def __str__(self) -> str:
return (
f"RandomProblemLDLT("
f"\nseed: {self.seed}"
f"\nnum_blocks: {self.num_blocks}"
f"\nnp_dtype: {self.np_dtype}"
f"\nwp_dtype: {self.wp_dtype}"
f"\ndims: {self.dims}"
f"\nmaxdims: {self.maxdims}"
f"\ndevice: {self.device}"
f"\nA_np (shape): {[A.shape for A in self.A_np]}"
f"\nb_np (shape): {[b.shape for b in self.b_np]}"
f"\nX_np (shape): {[X.shape for X in self.X_np]}"
f"\nD_np (shape): {[D.shape for D in self.D_np]}"
f"\nP_np (shape): {[P.shape for P in self.P_np]}"
f"\nz_np (shape): {[z.shape for z in self.z_np]}"
f"\ny_np (shape): {[y.shape for y in self.y_np]}"
f"\nx_np (shape): {[x.shape for x in self.x_np]}"
f"\nmaxdim_wp: {self.maxdim_wp.numpy()}"
f"\ndim_wp: {self.dim_wp.numpy()}"
f"\nmio_wp: {self.mio_wp.numpy()}"
f"\nvio_wp: {self.vio_wp.numpy()}"
f"\nA_wp (shape): {self.A_wp.shape}"
f"\nb_wp (shape): {self.b_wp.shape}"
f"\n)"
)
###
# Utilities
###
def _solve_cholesky_lower_numpy(L: np.ndarray, b: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""
Solve the linear system Ax = b using Cholesky decomposition.
Args:
A (np.ndarray): The input matrix (must be symmetric positive definite).
b (np.ndarray): The RHS vector.
Returns:
np.ndarray: The solution vector x.
"""
# Attempt to import scipy.linalg, which is required
# for the LDLT decomposition and reference solutions
try:
import scipy.linalg
except ImportError as e:
raise ImportError(
"scipy is required for RandomProblemLDLT but is not installed. install with: pip install scipy"
) from e
y = scipy.linalg.solve_triangular(L, b, lower=True)
x = scipy.linalg.solve_triangular(L.T, y, lower=False)
return y, x
def _solve_cholesky_upper_numpy(U: np.ndarray, b: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""
Solve the linear system Ax = b using Cholesky decomposition.
Args:
A (np.ndarray): The input matrix (must be symmetric positive definite).
b (np.ndarray): The RHS vector.
Returns:
np.ndarray: The solution vector x.
"""
# Attempt to import scipy.linalg, which is required
# for the LDLT decomposition and reference solutions
try:
import scipy.linalg
except ImportError as e:
raise ImportError(
"scipy is required for RandomProblemLDLT but is not installed. install with: pip install scipy"
) from e
y = scipy.linalg.solve_triangular(U.T, b, lower=True)
x = scipy.linalg.solve_triangular(U, y, lower=False)
return y, x
def _solve_ldlt_lower_numpy(
L: np.ndarray, D: np.ndarray, P: np.ndarray, b: np.ndarray
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
"""
Solve the linear system Ax = b using LDL^T decomposition.
Args:
L (np.ndarray): The lower triangular matrix from the LDL^T decomposition.
D (np.ndarray): The diagonal matrix from the LDL^T decomposition.
P (np.ndarray): The permutation index array from the LDL^T decomposition.
b (np.ndarray): The RHS vector.
Returns:
np.ndarray: The solution vector x.
"""
# Attempt to import scipy.linalg, which is required
# for the LDLT decomposition and reference solutions
try:
import scipy.linalg
except ImportError as e:
raise ImportError(
"scipy is required for RandomProblemLDLT but is not installed. install with: pip install scipy"
) from e
PL = L[P, :]
z = scipy.linalg.solve_triangular(PL, b[P], lower=True)
y = z / np.diag(D)
x = scipy.linalg.solve_triangular(PL.T, y, lower=False)
x = x[np.argsort(P)]
return z, y, x
def _solve_ldlt_upper_numpy(
U: np.ndarray, D: np.ndarray, P: np.ndarray, b: np.ndarray
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
"""
Solve the linear system Ax = b using LDL^T decomposition.
Args:
U (np.ndarray): The upper triangular matrix from the LDL^T decomposition.
D (np.ndarray): The diagonal matrix from the LDL^T decomposition.
P (np.ndarray): The permutation index array from the LDL^T decomposition.
b (np.ndarray): The RHS vector.
Returns:
np.ndarray: The solution vector x.
"""
# Attempt to import scipy.linalg, which is required
# for the LDLT decomposition and reference solutions
try:
import scipy.linalg
except ImportError as e:
raise ImportError(
"scipy is required for RandomProblemLDLT but is not installed. install with: pip install scipy"
) from e
PU = U[P, :]
z = scipy.linalg.solve_triangular(PU.T, b[P], lower=True)
y = z / np.diag(D)
x = scipy.linalg.solve_triangular(PU, y, lower=False)
x = x[np.argsort(P)]
return z, y, x
================================================
FILE: newton/_src/solvers/mujoco/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .solver_mujoco import SolverMuJoCo
__all__ = [
"SolverMuJoCo",
]
================================================
FILE: newton/_src/solvers/mujoco/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Warp kernels for SolverMuJoCo."""
from __future__ import annotations
from typing import Any
import warp as wp
from ...core.types import vec5
from ...math import velocity_at_point
from ...sim import BodyFlags, EqType, JointTargetMode, JointType
def _import_contact_force_fn():
from mujoco_warp._src.support import contact_force_fn
return contact_force_fn
# Custom vector types
vec10 = wp.types.vector(length=10, dtype=wp.float32)
vec11 = wp.types.vector(length=11, dtype=wp.float32)
# Constants
MJ_MINVAL = 2.220446049250313e-16
# Utility functions
@wp.func
def orthogonals(a: wp.vec3):
y = wp.vec3(0.0, 1.0, 0.0)
z = wp.vec3(0.0, 0.0, 1.0)
b = wp.where((-0.5 < a[1]) and (a[1] < 0.5), y, z)
b = b - a * wp.dot(a, b)
b = wp.normalize(b)
if wp.length(a) == 0.0:
b = wp.vec3(0.0, 0.0, 0.0)
c = wp.cross(a, b)
return b, c
@wp.func
def make_frame(a: wp.vec3):
a = wp.normalize(a)
b, c = orthogonals(a)
# fmt: off
return wp.mat33(
a.x, a.y, a.z,
b.x, b.y, b.z,
c.x, c.y, c.z
)
# fmt: on
@wp.func
def write_contact(
# Data in:
# In:
dist_in: float,
pos_in: wp.vec3,
frame_in: wp.mat33,
margin_in: float,
gap_in: float,
condim_in: int,
friction_in: vec5,
solref_in: wp.vec2f,
solreffriction_in: wp.vec2f,
solimp_in: vec5,
geoms_in: wp.vec2i,
worldid_in: int,
contact_id_in: int,
# Data out:
contact_dist_out: wp.array[float],
contact_pos_out: wp.array[wp.vec3],
contact_frame_out: wp.array[wp.mat33],
contact_includemargin_out: wp.array[float],
contact_friction_out: wp.array[vec5],
contact_solref_out: wp.array[wp.vec2],
contact_solreffriction_out: wp.array[wp.vec2],
contact_solimp_out: wp.array[vec5],
contact_dim_out: wp.array[int],
contact_geom_out: wp.array[wp.vec2i],
contact_efc_address_out: wp.array2d[int],
contact_worldid_out: wp.array[int],
):
# See function write_contact in mujoco_warp, file collision_primitive.py
cid = contact_id_in
contact_dist_out[cid] = dist_in
contact_pos_out[cid] = pos_in
contact_frame_out[cid] = frame_in
contact_geom_out[cid] = geoms_in
contact_worldid_out[cid] = worldid_in
contact_includemargin_out[cid] = margin_in - gap_in
contact_dim_out[cid] = condim_in
contact_friction_out[cid] = friction_in
contact_solref_out[cid] = solref_in
contact_solreffriction_out[cid] = solreffriction_in
contact_solimp_out[cid] = solimp_in
# initialize constraint address to -1 (max 10 elements; populated during constraint generation)
for i in range(contact_efc_address_out.shape[1]):
contact_efc_address_out[cid, i] = -1
@wp.func
def contact_params(
geom_condim: wp.array[int],
geom_priority: wp.array[int],
geom_solmix: wp.array2d[float],
geom_solref: wp.array2d[wp.vec2],
geom_solimp: wp.array2d[vec5],
geom_friction: wp.array2d[wp.vec3],
geom_margin: wp.array2d[float],
geom_gap: wp.array2d[float],
geoms: wp.vec2i,
worldid: int,
):
# See function contact_params in mujoco_warp, file collision_primitive.py
g1 = geoms[0]
g2 = geoms[1]
p1 = geom_priority[g1]
p2 = geom_priority[g2]
solmix1 = geom_solmix[worldid, g1]
solmix2 = geom_solmix[worldid, g2]
mix = solmix1 / (solmix1 + solmix2)
mix = wp.where((solmix1 < MJ_MINVAL) and (solmix2 < MJ_MINVAL), 0.5, mix)
mix = wp.where((solmix1 < MJ_MINVAL) and (solmix2 >= MJ_MINVAL), 0.0, mix)
mix = wp.where((solmix1 >= MJ_MINVAL) and (solmix2 < MJ_MINVAL), 1.0, mix)
mix = wp.where(p1 == p2, mix, wp.where(p1 > p2, 1.0, 0.0))
# Sum margins for consistency with thickness summing
margin = geom_margin[worldid, g1] + geom_margin[worldid, g2]
gap = geom_gap[worldid, g1] + geom_gap[worldid, g2]
condim1 = geom_condim[g1]
condim2 = geom_condim[g2]
condim = wp.where(p1 == p2, wp.max(condim1, condim2), wp.where(p1 > p2, condim1, condim2))
max_geom_friction = wp.max(geom_friction[worldid, g1], geom_friction[worldid, g2])
friction = vec5(
max_geom_friction[0],
max_geom_friction[0],
max_geom_friction[1],
max_geom_friction[2],
max_geom_friction[2],
)
if geom_solref[worldid, g1].x > 0.0 and geom_solref[worldid, g2].x > 0.0:
solref = mix * geom_solref[worldid, g1] + (1.0 - mix) * geom_solref[worldid, g2]
else:
solref = wp.min(geom_solref[worldid, g1], geom_solref[worldid, g2])
solreffriction = wp.vec2(0.0, 0.0)
solimp = mix * geom_solimp[worldid, g1] + (1.0 - mix) * geom_solimp[worldid, g2]
return margin, gap, condim, friction, solref, solreffriction, solimp
@wp.func
def convert_solref(ke: float, kd: float, d_width: float, d_r: float) -> wp.vec2:
"""Convert from stiffness and damping to time constant and damp ratio
based on d(r) and d(width)."""
if ke > 0.0 and kd > 0.0:
# ke = d(r) / (d_width^2 * timeconst^2 * dampratio^2)
# kd = 2 / (d_width * timeconst)
timeconst = 2.0 / (kd * d_width)
dampratio = kd / 2.0 * wp.sqrt(d_r / ke)
else:
timeconst = 0.02
dampratio = 1.0
# see https://mujoco.readthedocs.io/en/latest/modeling.html#solver-parameters
return wp.vec2(timeconst, dampratio)
@wp.func
def quat_wxyz_to_xyzw(q: wp.quat) -> wp.quat:
"""Convert a quaternion from MuJoCo wxyz storage to Warp xyzw format."""
return wp.quat(q[1], q[2], q[3], q[0])
@wp.func
def quat_xyzw_to_wxyz(q: wp.quat) -> wp.quat:
"""Convert a Warp xyzw quaternion to MuJoCo wxyz storage order.
The returned wp.quat is NOT valid for Warp math — it is a container
for writing components to MuJoCo arrays.
"""
return wp.quat(q[3], q[0], q[1], q[2])
# Kernel functions
@wp.kernel
def convert_newton_contacts_to_mjwarp_kernel(
body_q: wp.array[wp.transform],
shape_body: wp.array[int],
body_flags: wp.array[int],
# Model:
geom_bodyid: wp.array[int],
body_weldid: wp.array[int],
geom_condim: wp.array[int],
geom_priority: wp.array[int],
geom_solmix: wp.array2d[float],
geom_solref: wp.array2d[wp.vec2],
geom_solimp: wp.array2d[vec5],
geom_friction: wp.array2d[wp.vec3],
geom_margin: wp.array2d[float],
geom_gap: wp.array2d[float],
# Newton contacts
rigid_contact_count: wp.array[wp.int32],
rigid_contact_shape0: wp.array[wp.int32],
rigid_contact_shape1: wp.array[wp.int32],
rigid_contact_point0: wp.array[wp.vec3],
rigid_contact_point1: wp.array[wp.vec3],
rigid_contact_normal: wp.array[wp.vec3],
rigid_contact_margin0: wp.array[wp.float32],
rigid_contact_margin1: wp.array[wp.float32],
rigid_contact_stiffness: wp.array[wp.float32],
rigid_contact_damping: wp.array[wp.float32],
rigid_contact_friction: wp.array[wp.float32],
shape_margin: wp.array[float],
bodies_per_world: int,
newton_shape_to_mjc_geom: wp.array[wp.int32],
# Mujoco warp contacts
naconmax: int,
nacon_out: wp.array[int],
contact_dist_out: wp.array[float],
contact_pos_out: wp.array[wp.vec3],
contact_frame_out: wp.array[wp.mat33],
contact_includemargin_out: wp.array[float],
contact_friction_out: wp.array[vec5],
contact_solref_out: wp.array[wp.vec2],
contact_solreffriction_out: wp.array[wp.vec2],
contact_solimp_out: wp.array[vec5],
contact_dim_out: wp.array[int],
contact_geom_out: wp.array[wp.vec2i],
contact_efc_address_out: wp.array2d[int],
contact_worldid_out: wp.array[int],
# Values to clear - see _zero_collision_arrays kernel from mujoco_warp
nworld_in: int,
ncollision_out: wp.array[int],
):
# See kernel solve_body_contact_positions for reference
# nacon_out must be zeroed before this kernel is launched so that
# wp.atomic_add below produces the correct compacted count.
tid = wp.tid()
count = rigid_contact_count[0]
if tid == 0:
if count > naconmax:
wp.printf(
"Number of Newton contacts (%d) exceeded MJWarp limit (%d). Increase nconmax.\n",
count,
naconmax,
)
ncollision_out[0] = 0
if count > naconmax:
count = naconmax
if tid >= count:
return
shape_a = rigid_contact_shape0[tid]
shape_b = rigid_contact_shape1[tid]
# Skip invalid contacts - both shapes must be specified
if shape_a < 0 or shape_b < 0:
return
# --- Filter contacts that would produce degenerate efc_D values ----------
# A body is "immovable" from the MuJoCo solver's perspective when it
# contributes zero (or near-zero) invweight. Three cases:
#
# 1. Static shapes (body < 0) — no MuJoCo body at all.
# 2. Kinematic bodies (BodyFlags.KINEMATIC) — Newton sets armature=1e10
# on their DOFs, giving near-zero invweight even though MuJoCo still
# sees DOFs (body_weldid != 0).
# 3. Fixed-root bodies welded to the world body (body_weldid == 0) —
# MuJoCo merges them into weld group 0, giving zero invweight.
#
# Each body is classified independently; a contact is skipped when both
# sides are immovable.
geom_a = newton_shape_to_mjc_geom[shape_a]
geom_b = newton_shape_to_mjc_geom[shape_b]
body_a = shape_body[shape_a]
body_b = shape_body[shape_b]
mj_body_a = geom_bodyid[geom_a]
mj_body_b = geom_bodyid[geom_b]
a_immovable = body_a < 0 or (body_flags[body_a] & BodyFlags.KINEMATIC) != 0 or body_weldid[mj_body_a] == 0
b_immovable = body_b < 0 or (body_flags[body_b] & BodyFlags.KINEMATIC) != 0 or body_weldid[mj_body_b] == 0
if a_immovable and b_immovable:
return
X_wb_a = wp.transform_identity()
X_wb_b = wp.transform_identity()
if body_a >= 0:
X_wb_a = body_q[body_a]
if body_b >= 0:
X_wb_b = body_q[body_b]
bx_a = wp.transform_point(X_wb_a, rigid_contact_point0[tid])
bx_b = wp.transform_point(X_wb_b, rigid_contact_point1[tid])
# rigid_contact_margin0/1 = radius_eff + shape_margin per shape.
# Subtract only radius_eff so dist is the surface-to-surface distance.
# shape_margin is handled by geom_margin (MuJoCo's includemargin threshold).
radius_eff = (rigid_contact_margin0[tid] - shape_margin[shape_a]) + (
rigid_contact_margin1[tid] - shape_margin[shape_b]
)
n = rigid_contact_normal[tid]
dist = wp.dot(n, bx_b - bx_a) - radius_eff
# Contact position: use midpoint between contact points (as in XPBD kernel)
pos = 0.5 * (bx_a + bx_b)
# Build contact frame
frame = make_frame(n)
geoms = wp.vec2i(geom_a, geom_b)
# Compute world ID from body indices (more reliable than shape mapping for static shapes)
# Static shapes like ground planes share the same Newton shape index across all worlds,
# so the inverse shape mapping may have the wrong world ID for them.
# Using body indices: body_index = world * bodies_per_world + body_in_world
worldid = body_a // bodies_per_world
if body_a < 0:
worldid = body_b // bodies_per_world
margin, gap, condim, friction, solref, solreffriction, solimp = contact_params(
geom_condim,
geom_priority,
geom_solmix,
geom_solref,
geom_solimp,
geom_friction,
geom_margin,
geom_gap,
geoms,
worldid,
)
if rigid_contact_stiffness:
# Use per-contact stiffness/damping parameters
contact_ke = rigid_contact_stiffness[tid]
if contact_ke > 0.0:
# set solimp to approximate linear force-to-displacement relationship at rest
# see https://mujoco.readthedocs.io/en/latest/modeling.html#solver-parameters
imp = solimp[1]
solimp = vec5(imp, imp, 0.001, 1.0, 0.5)
contact_ke = contact_ke * (1.0 - imp) # compensate for impedance scaling
kd = rigid_contact_damping[tid]
# convert from stiffness/damping to MuJoCo's solref timeconst and dampratio
if kd > 0.0:
timeconst = 2.0 / kd
dampratio = wp.sqrt(1.0 / (timeconst * timeconst * contact_ke))
else:
# if no damping was set, use default damping ratio
timeconst = wp.sqrt(1.0 / contact_ke)
dampratio = 1.0
solref = wp.vec2(timeconst, dampratio)
friction_scale = rigid_contact_friction[tid]
if friction_scale > 0.0:
friction = vec5(
friction[0] * friction_scale,
friction[1] * friction_scale,
friction[2],
friction[3],
friction[4],
)
# Atomically claim a compacted output slot (contacts may be filtered above)
cid = wp.atomic_add(nacon_out, 0, 1)
if cid >= naconmax:
return
write_contact(
dist_in=dist,
pos_in=pos,
frame_in=frame,
margin_in=margin,
gap_in=gap,
condim_in=condim,
friction_in=friction,
solref_in=solref,
solreffriction_in=solreffriction,
solimp_in=solimp,
geoms_in=geoms,
worldid_in=worldid,
contact_id_in=cid,
contact_dist_out=contact_dist_out,
contact_pos_out=contact_pos_out,
contact_frame_out=contact_frame_out,
contact_includemargin_out=contact_includemargin_out,
contact_friction_out=contact_friction_out,
contact_solref_out=contact_solref_out,
contact_solreffriction_out=contact_solreffriction_out,
contact_solimp_out=contact_solimp_out,
contact_dim_out=contact_dim_out,
contact_geom_out=contact_geom_out,
contact_efc_address_out=contact_efc_address_out,
contact_worldid_out=contact_worldid_out,
)
@wp.kernel
def convert_mj_coords_to_warp_kernel(
qpos: wp.array2d[wp.float32],
qvel: wp.array2d[wp.float32],
joints_per_world: int,
joint_type: wp.array[wp.int32],
joint_q_start: wp.array[wp.int32],
joint_qd_start: wp.array[wp.int32],
joint_dof_dim: wp.array2d[wp.int32],
joint_child: wp.array[wp.int32],
body_com: wp.array[wp.vec3],
dof_ref: wp.array[wp.float32],
body_flags: wp.array[wp.int32],
joint_q_in: wp.array[wp.float32],
joint_qd_in: wp.array[wp.float32],
mj_q_start: wp.array[wp.int32],
mj_qd_start: wp.array[wp.int32],
# outputs
joint_q: wp.array[wp.float32],
joint_qd: wp.array[wp.float32],
):
worldid, jntid = wp.tid()
joint_id = joints_per_world * worldid + jntid
# Skip loop joints — they have no MuJoCo qpos/qvel entries
q_i = mj_q_start[jntid]
if q_i < 0:
return
qd_i = mj_qd_start[jntid]
type = joint_type[joint_id]
wq_i = joint_q_start[joint_id]
wqd_i = joint_qd_start[joint_id]
child = joint_child[joint_id]
if (body_flags[child] & BodyFlags.KINEMATIC) != 0:
# Previous joint states pass through for kinematic bodies
wq_end = joint_q_start[joint_id + 1]
for i in range(wq_i, wq_end):
joint_q[i] = joint_q_in[i]
wqd_end = joint_qd_start[joint_id + 1]
for i in range(wqd_i, wqd_end):
joint_qd[i] = joint_qd_in[i]
return
if type == JointType.FREE:
# convert position components
for i in range(3):
joint_q[wq_i + i] = qpos[worldid, q_i + i]
# change quaternion order from wxyz to xyzw
rot = quat_wxyz_to_xyzw(
wp.quat(
qpos[worldid, q_i + 3],
qpos[worldid, q_i + 4],
qpos[worldid, q_i + 5],
qpos[worldid, q_i + 6],
)
)
joint_q[wq_i + 3] = rot[0]
joint_q[wq_i + 4] = rot[1]
joint_q[wq_i + 5] = rot[2]
joint_q[wq_i + 6] = rot[3]
# MuJoCo qvel: linear velocity of body ORIGIN (world frame), angular velocity (body frame)
# Newton joint_qd: linear velocity of CoM (world frame), angular velocity (world frame)
#
# Relationship: v_com = v_origin + ω x com_offset_world
# where com_offset_world = quat_rotate(body_rotation, body_com)
# Get angular velocity in body frame from MuJoCo and convert to world frame
w_body = wp.vec3(qvel[worldid, qd_i + 3], qvel[worldid, qd_i + 4], qvel[worldid, qd_i + 5])
w_world = wp.quat_rotate(rot, w_body)
# Get CoM offset in world frame
com_local = body_com[child]
com_world = wp.quat_rotate(rot, com_local)
# Get body origin velocity from MuJoCo
v_origin = wp.vec3(qvel[worldid, qd_i + 0], qvel[worldid, qd_i + 1], qvel[worldid, qd_i + 2])
# Convert to CoM velocity for Newton: v_com = v_origin + ω x com_offset
v_com = v_origin + wp.cross(w_world, com_world)
joint_qd[wqd_i + 0] = v_com[0]
joint_qd[wqd_i + 1] = v_com[1]
joint_qd[wqd_i + 2] = v_com[2]
# Angular velocity: convert from body frame (MuJoCo) to world frame (Newton)
joint_qd[wqd_i + 3] = w_world[0]
joint_qd[wqd_i + 4] = w_world[1]
joint_qd[wqd_i + 5] = w_world[2]
elif type == JointType.BALL:
# change quaternion order from wxyz to xyzw
rot = quat_wxyz_to_xyzw(
wp.quat(
qpos[worldid, q_i],
qpos[worldid, q_i + 1],
qpos[worldid, q_i + 2],
qpos[worldid, q_i + 3],
)
)
joint_q[wq_i] = rot[0]
joint_q[wq_i + 1] = rot[1]
joint_q[wq_i + 2] = rot[2]
joint_q[wq_i + 3] = rot[3]
for i in range(3):
# convert velocity components
joint_qd[wqd_i + i] = qvel[worldid, qd_i + i]
else:
axis_count = joint_dof_dim[joint_id, 0] + joint_dof_dim[joint_id, 1]
for i in range(axis_count):
ref = float(0.0)
if dof_ref:
ref = dof_ref[wqd_i + i]
joint_q[wq_i + i] = qpos[worldid, q_i + i] - ref
for i in range(axis_count):
# convert velocity components
joint_qd[wqd_i + i] = qvel[worldid, qd_i + i]
@wp.kernel
def convert_warp_coords_to_mj_kernel(
joint_q: wp.array[wp.float32],
joint_qd: wp.array[wp.float32],
joints_per_world: int,
joint_type: wp.array[wp.int32],
joint_q_start: wp.array[wp.int32],
joint_qd_start: wp.array[wp.int32],
joint_dof_dim: wp.array2d[wp.int32],
joint_child: wp.array[wp.int32],
body_com: wp.array[wp.vec3],
dof_ref: wp.array[wp.float32],
mj_q_start: wp.array[wp.int32],
mj_qd_start: wp.array[wp.int32],
# outputs
qpos: wp.array2d[wp.float32],
qvel: wp.array2d[wp.float32],
):
worldid, jntid = wp.tid()
# Skip loop joints — they have no MuJoCo qpos/qvel entries
q_i = mj_q_start[jntid]
if q_i < 0:
return
qd_i = mj_qd_start[jntid]
type = joint_type[jntid]
wq_i = joint_q_start[joints_per_world * worldid + jntid]
wqd_i = joint_qd_start[joints_per_world * worldid + jntid]
if type == JointType.FREE:
# convert position components
for i in range(3):
qpos[worldid, q_i + i] = joint_q[wq_i + i]
rot = wp.quat(
joint_q[wq_i + 3],
joint_q[wq_i + 4],
joint_q[wq_i + 5],
joint_q[wq_i + 6],
)
# change quaternion order from xyzw to wxyz
rot_wxyz = quat_xyzw_to_wxyz(rot)
qpos[worldid, q_i + 3] = rot_wxyz[0]
qpos[worldid, q_i + 4] = rot_wxyz[1]
qpos[worldid, q_i + 5] = rot_wxyz[2]
qpos[worldid, q_i + 6] = rot_wxyz[3]
# Newton joint_qd: linear velocity of CoM (world frame), angular velocity (world frame)
# MuJoCo qvel: linear velocity of body ORIGIN (world frame), angular velocity (body frame)
#
# Relationship: v_origin = v_com - ω x com_offset_world
# where com_offset_world = quat_rotate(body_rotation, body_com)
# Get angular velocity in world frame
w_world = wp.vec3(joint_qd[wqd_i + 3], joint_qd[wqd_i + 4], joint_qd[wqd_i + 5])
# Get CoM offset in world frame
child = joint_child[jntid]
com_local = body_com[child]
com_world = wp.quat_rotate(rot, com_local)
# Get CoM velocity from Newton
v_com = wp.vec3(joint_qd[wqd_i + 0], joint_qd[wqd_i + 1], joint_qd[wqd_i + 2])
# Convert to body origin velocity for MuJoCo: v_origin = v_com - ω x com_offset
v_origin = v_com - wp.cross(w_world, com_world)
qvel[worldid, qd_i + 0] = v_origin[0]
qvel[worldid, qd_i + 1] = v_origin[1]
qvel[worldid, qd_i + 2] = v_origin[2]
# Angular velocity: convert from world frame (Newton) to body frame (MuJoCo)
w_body = wp.quat_rotate_inv(rot, w_world)
qvel[worldid, qd_i + 3] = w_body[0]
qvel[worldid, qd_i + 4] = w_body[1]
qvel[worldid, qd_i + 5] = w_body[2]
elif type == JointType.BALL:
# change quaternion order from xyzw to wxyz
ball_q = wp.quat(joint_q[wq_i], joint_q[wq_i + 1], joint_q[wq_i + 2], joint_q[wq_i + 3])
ball_q_wxyz = quat_xyzw_to_wxyz(ball_q)
qpos[worldid, q_i + 0] = ball_q_wxyz[0]
qpos[worldid, q_i + 1] = ball_q_wxyz[1]
qpos[worldid, q_i + 2] = ball_q_wxyz[2]
qpos[worldid, q_i + 3] = ball_q_wxyz[3]
for i in range(3):
# convert velocity components
qvel[worldid, qd_i + i] = joint_qd[wqd_i + i]
else:
axis_count = joint_dof_dim[jntid, 0] + joint_dof_dim[jntid, 1]
for i in range(axis_count):
ref = float(0.0)
if dof_ref:
ref = dof_ref[wqd_i + i]
qpos[worldid, q_i + i] = joint_q[wq_i + i] + ref
for i in range(axis_count):
# convert velocity components
qvel[worldid, qd_i + i] = joint_qd[wqd_i + i]
@wp.kernel
def sync_qpos0_kernel(
joints_per_world: int,
bodies_per_world: int,
joint_type: wp.array[wp.int32],
joint_q_start: wp.array[wp.int32],
joint_qd_start: wp.array[wp.int32],
joint_dof_dim: wp.array2d[wp.int32],
joint_child: wp.array[wp.int32],
body_q: wp.array[wp.transform],
dof_ref: wp.array[wp.float32],
dof_springref: wp.array[wp.float32],
mj_q_start: wp.array[wp.int32],
# outputs
qpos0: wp.array2d[wp.float32],
qpos_spring: wp.array2d[wp.float32],
):
"""Sync MuJoCo qpos0 and qpos_spring from Newton model data.
For hinge/slide: qpos0 = ref, qpos_spring = springref.
For free: qpos0 from body_q (pos + quat in wxyz order).
For ball: qpos0 = [1, 0, 0, 0] (identity quaternion in wxyz).
"""
worldid, jntid = wp.tid()
# Skip loop joints — they have no MuJoCo qpos entries
q_i = mj_q_start[jntid]
if q_i < 0:
return
type = joint_type[jntid]
wqd_i = joint_qd_start[joints_per_world * worldid + jntid]
if type == JointType.FREE:
child = joint_child[jntid]
world_body = worldid * bodies_per_world + child
bq = body_q[world_body]
pos = wp.transform_get_translation(bq)
rot = wp.transform_get_rotation(bq)
# Position
for i in range(3):
qpos0[worldid, q_i + i] = pos[i]
qpos_spring[worldid, q_i + i] = pos[i]
# Quaternion: Newton stores xyzw, MuJoCo uses wxyz
rot_wxyz = quat_xyzw_to_wxyz(rot)
for i in range(4):
qpos0[worldid, q_i + 3 + i] = rot_wxyz[i]
qpos_spring[worldid, q_i + 3 + i] = rot_wxyz[i]
elif type == JointType.BALL:
# Identity quaternion in wxyz order
qpos0[worldid, q_i + 0] = 1.0
qpos0[worldid, q_i + 1] = 0.0
qpos0[worldid, q_i + 2] = 0.0
qpos0[worldid, q_i + 3] = 0.0
qpos_spring[worldid, q_i + 0] = 1.0
qpos_spring[worldid, q_i + 1] = 0.0
qpos_spring[worldid, q_i + 2] = 0.0
qpos_spring[worldid, q_i + 3] = 0.0
else:
axis_count = joint_dof_dim[jntid, 0] + joint_dof_dim[jntid, 1]
for i in range(axis_count):
ref = float(0.0)
springref = float(0.0)
if dof_ref:
ref = dof_ref[wqd_i + i]
if dof_springref:
springref = dof_springref[wqd_i + i]
qpos0[worldid, q_i + i] = ref
qpos_spring[worldid, q_i + i] = springref
def create_convert_mjw_contacts_to_newton_kernel():
"""Create contact conversion kernel; deferred so ``wp.static`` doesn't import mujoco_warp at module load."""
@wp.kernel
def convert_mjw_contacts_to_newton_kernel(
# inputs
mjc_geom_to_newton_shape: wp.array2d[wp.int32],
mj_opt_cone: int,
mj_nacon: wp.array[wp.int32],
mj_contact_pos: wp.array[wp.vec3],
mj_contact_frame: wp.array[wp.mat33f],
mj_contact_friction: wp.array[vec5],
mj_contact_dist: wp.array[float],
mj_contact_dim: wp.array[int],
mj_contact_geom: wp.array[wp.vec2i],
mj_contact_efc_address: wp.array2d[int],
mj_contact_worldid: wp.array[wp.int32],
mj_efc_force: wp.array2d[float],
mj_geom_bodyid: wp.array[int],
mj_xpos: wp.array2d[wp.vec3],
mj_xquat: wp.array2d[wp.quatf],
njmax: int,
# outputs
rigid_contact_count: wp.array[wp.int32],
rigid_contact_shape0: wp.array[wp.int32],
rigid_contact_shape1: wp.array[wp.int32],
rigid_contact_point0: wp.array[wp.vec3],
rigid_contact_point1: wp.array[wp.vec3],
rigid_contact_normal: wp.array[wp.vec3],
contact_force: wp.array[wp.spatial_vector],
):
"""Convert MuJoCo contacts to Newton contact format.
Uses mjc_geom_to_newton_shape to convert MuJoCo geom indices to Newton shape indices.
Contact positions are converted from MuJoCo world frame to Newton body-local frame.
Contact forces are computed via ``mujoco_warp`` ``contact_force_fn``.
"""
contact_idx = wp.tid()
n_contacts = mj_nacon[0]
if contact_idx == 0:
rigid_contact_count[0] = n_contacts
if contact_idx >= n_contacts:
return
world = mj_contact_worldid[contact_idx]
geoms_mjw = mj_contact_geom[contact_idx]
normal = mj_contact_frame[contact_idx][0]
pos_world = mj_contact_pos[contact_idx]
rigid_contact_shape0[contact_idx] = mjc_geom_to_newton_shape[world, geoms_mjw[0]]
rigid_contact_shape1[contact_idx] = mjc_geom_to_newton_shape[world, geoms_mjw[1]]
rigid_contact_normal[contact_idx] = normal
# Convert contact position from world frame to body-local frame for each shape.
# MuJoCo contact.pos is the midpoint in world frame; we transform it into each
# body's local frame to match Newton's convention (see collide.py write_contact).
body_a = mj_geom_bodyid[geoms_mjw[0]]
body_b = mj_geom_bodyid[geoms_mjw[1]]
X_wb_a = wp.transform_identity()
X_wb_b = wp.transform_identity()
if body_a > 0:
X_wb_a = wp.transform(mj_xpos[world, body_a], quat_wxyz_to_xyzw(mj_xquat[world, body_a]))
if body_b > 0:
X_wb_b = wp.transform(mj_xpos[world, body_b], quat_wxyz_to_xyzw(mj_xquat[world, body_b]))
dist = mj_contact_dist[contact_idx]
point0_world = pos_world - 0.5 * dist * normal
point1_world = pos_world + 0.5 * dist * normal
rigid_contact_point0[contact_idx] = wp.transform_point(wp.transform_inverse(X_wb_a), point0_world)
rigid_contact_point1[contact_idx] = wp.transform_point(wp.transform_inverse(X_wb_b), point1_world)
if contact_force:
# Negate: contact_force_fn returns force on geom2; Newton stores force on shape0 (geom1).
contact_force[contact_idx] = -wp.static(_import_contact_force_fn())(
mj_opt_cone,
mj_contact_frame,
mj_contact_friction,
mj_contact_dim,
mj_contact_efc_address,
mj_efc_force,
njmax,
mj_nacon,
world,
contact_idx,
True,
)
return convert_mjw_contacts_to_newton_kernel
# Import control source/type enums and create warp constants
CTRL_SOURCE_JOINT_TARGET = wp.constant(0)
CTRL_SOURCE_CTRL_DIRECT = wp.constant(1)
@wp.kernel
def apply_mjc_control_kernel(
mjc_actuator_ctrl_source: wp.array[wp.int32],
mjc_actuator_to_newton_idx: wp.array[wp.int32],
joint_target_pos: wp.array[wp.float32],
joint_target_vel: wp.array[wp.float32],
mujoco_ctrl: wp.array[wp.float32],
dofs_per_world: wp.int32,
ctrls_per_world: wp.int32,
# outputs
mj_ctrl: wp.array2d[wp.float32],
):
"""Apply Newton control inputs to MuJoCo control array.
For JOINT_TARGET (source=0), uses sign encoding in mjc_actuator_to_newton_idx:
- Positive value (>=0): position actuator, newton_axis = value
- Value of -1: unmapped/skip
- Negative value (<=-2): velocity actuator, newton_axis = -(value + 2)
For CTRL_DIRECT (source=1), mjc_actuator_to_newton_idx is the ctrl index.
Args:
mjc_actuator_ctrl_source: 0=JOINT_TARGET, 1=CTRL_DIRECT
mjc_actuator_to_newton_idx: Index into Newton array (sign-encoded for JOINT_TARGET)
joint_target_pos: Per-DOF position targets
joint_target_vel: Per-DOF velocity targets
mujoco_ctrl: Direct control inputs (from control.mujoco.ctrl)
dofs_per_world: Number of DOFs per world
ctrls_per_world: Number of ctrl inputs per world
mj_ctrl: Output MuJoCo control array
"""
world, actuator = wp.tid()
source = mjc_actuator_ctrl_source[actuator]
idx = mjc_actuator_to_newton_idx[actuator]
if source == CTRL_SOURCE_JOINT_TARGET:
if idx >= 0:
# Position actuator
world_dof = world * dofs_per_world + idx
mj_ctrl[world, actuator] = joint_target_pos[world_dof]
elif idx == -1:
# Unmapped/skip
return
else:
# Velocity actuator: newton_axis = -(idx + 2)
newton_axis = -(idx + 2)
world_dof = world * dofs_per_world + newton_axis
mj_ctrl[world, actuator] = joint_target_vel[world_dof]
else: # CTRL_SOURCE_CTRL_DIRECT
world_ctrl_idx = world * ctrls_per_world + idx
if world_ctrl_idx < mujoco_ctrl.shape[0]:
mj_ctrl[world, actuator] = mujoco_ctrl[world_ctrl_idx]
@wp.kernel
def apply_mjc_body_f_kernel(
mjc_body_to_newton: wp.array2d[wp.int32],
body_flags: wp.array[wp.int32],
body_f: wp.array[wp.spatial_vector],
# outputs
xfrc_applied: wp.array2d[wp.spatial_vector],
):
"""Apply Newton body forces to MuJoCo xfrc_applied array.
Iterates over MuJoCo bodies [world, mjc_body], looks up Newton body index,
and copies the force.
"""
world, mjc_body = wp.tid()
newton_body = mjc_body_to_newton[world, mjc_body]
if newton_body < 0 or (body_flags[newton_body] & BodyFlags.KINEMATIC) != 0:
xfrc_applied[world, mjc_body] = wp.spatial_vector(wp.vec3(0.0, 0.0, 0.0), wp.vec3(0.0, 0.0, 0.0))
return
f = body_f[newton_body]
v = wp.vec3(f[0], f[1], f[2])
w = wp.vec3(f[3], f[4], f[5])
xfrc_applied[world, mjc_body] = wp.spatial_vector(v, w)
@wp.kernel
def apply_mjc_qfrc_kernel(
joint_f: wp.array[wp.float32],
joint_type: wp.array[wp.int32],
joint_child: wp.array[wp.int32],
body_flags: wp.array[wp.int32],
joint_qd_start: wp.array[wp.int32],
joint_dof_dim: wp.array2d[wp.int32],
joints_per_world: int,
mj_qd_start: wp.array[wp.int32],
# outputs
qfrc_applied: wp.array2d[wp.float32],
):
worldid, jntid = wp.tid()
# Skip loop joints — they have no MuJoCo DOF entries
qd_i = mj_qd_start[jntid]
if qd_i < 0:
return
wqd_i = joint_qd_start[joints_per_world * worldid + jntid]
joint_id = joints_per_world * worldid + jntid
jtype = joint_type[jntid]
dof_count = joint_dof_dim[jntid, 0] + joint_dof_dim[jntid, 1]
for i in range(dof_count):
qfrc_applied[worldid, qd_i + i] = 0.0
if (body_flags[joint_child[joint_id]] & BodyFlags.KINEMATIC) != 0:
return
# Free/DISTANCE joint forces are routed via xfrc_applied in a separate kernel
# to preserve COM-wrench semantics; skip them here.
if jtype == JointType.FREE or jtype == JointType.DISTANCE:
return
elif jtype == JointType.BALL:
qfrc_applied[worldid, qd_i + 0] = joint_f[wqd_i + 0]
qfrc_applied[worldid, qd_i + 1] = joint_f[wqd_i + 1]
qfrc_applied[worldid, qd_i + 2] = joint_f[wqd_i + 2]
else:
for i in range(dof_count):
qfrc_applied[worldid, qd_i + i] = joint_f[wqd_i + i]
@wp.kernel
def apply_mjc_free_joint_f_to_body_f_kernel(
mjc_body_to_newton: wp.array2d[wp.int32],
body_flags: wp.array[wp.int32],
body_free_qd_start: wp.array[wp.int32],
joint_f: wp.array[wp.float32],
# outputs
xfrc_applied: wp.array2d[wp.spatial_vector],
):
worldid, mjc_body = wp.tid()
newton_body = mjc_body_to_newton[worldid, mjc_body]
if newton_body < 0 or (body_flags[newton_body] & BodyFlags.KINEMATIC) != 0:
return
qd_start = body_free_qd_start[newton_body]
if qd_start < 0:
return
v = wp.vec3(joint_f[qd_start + 0], joint_f[qd_start + 1], joint_f[qd_start + 2])
w = wp.vec3(joint_f[qd_start + 3], joint_f[qd_start + 4], joint_f[qd_start + 5])
xfrc = xfrc_applied[worldid, mjc_body]
xfrc_applied[worldid, mjc_body] = wp.spatial_vector(
wp.spatial_top(xfrc) + v,
wp.spatial_bottom(xfrc) + w,
)
@wp.func
def eval_single_articulation_fk(
joint_start: int,
joint_end: int,
joint_articulation: wp.array[int],
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_com: wp.array[wp.vec3],
# outputs
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
):
for i in range(joint_start, joint_end):
articulation = joint_articulation[i]
if articulation == -1:
continue
parent = joint_parent[i]
child = joint_child[i]
# compute transform across the joint
type = joint_type[i]
X_pj = joint_X_p[i]
X_cj = joint_X_c[i]
q_start = joint_q_start[i]
qd_start = joint_qd_start[i]
lin_axis_count = joint_dof_dim[i, 0]
ang_axis_count = joint_dof_dim[i, 1]
X_j = wp.transform_identity()
v_j = wp.spatial_vector(wp.vec3(), wp.vec3())
if type == JointType.PRISMATIC:
axis = joint_axis[qd_start]
q = joint_q[q_start]
qd = joint_qd[qd_start]
X_j = wp.transform(axis * q, wp.quat_identity())
v_j = wp.spatial_vector(axis * qd, wp.vec3())
if type == JointType.REVOLUTE:
axis = joint_axis[qd_start]
q = joint_q[q_start]
qd = joint_qd[qd_start]
X_j = wp.transform(wp.vec3(), wp.quat_from_axis_angle(axis, q))
v_j = wp.spatial_vector(wp.vec3(), axis * qd)
if type == JointType.BALL:
r = wp.quat(joint_q[q_start + 0], joint_q[q_start + 1], joint_q[q_start + 2], joint_q[q_start + 3])
w = wp.vec3(joint_qd[qd_start + 0], joint_qd[qd_start + 1], joint_qd[qd_start + 2])
X_j = wp.transform(wp.vec3(), r)
v_j = wp.spatial_vector(wp.vec3(), w)
if type == JointType.FREE or type == JointType.DISTANCE:
t = wp.transform(
wp.vec3(joint_q[q_start + 0], joint_q[q_start + 1], joint_q[q_start + 2]),
wp.quat(joint_q[q_start + 3], joint_q[q_start + 4], joint_q[q_start + 5], joint_q[q_start + 6]),
)
v = wp.spatial_vector(
wp.vec3(joint_qd[qd_start + 0], joint_qd[qd_start + 1], joint_qd[qd_start + 2]),
wp.vec3(joint_qd[qd_start + 3], joint_qd[qd_start + 4], joint_qd[qd_start + 5]),
)
X_j = t
v_j = v
if type == JointType.D6:
pos = wp.vec3(0.0)
rot = wp.quat_identity()
vel_v = wp.vec3(0.0)
vel_w = wp.vec3(0.0)
for j in range(lin_axis_count):
axis = joint_axis[qd_start + j]
pos += axis * joint_q[q_start + j]
vel_v += axis * joint_qd[qd_start + j]
iq = q_start + lin_axis_count
iqd = qd_start + lin_axis_count
for j in range(ang_axis_count):
axis = joint_axis[iqd + j]
rot = rot * wp.quat_from_axis_angle(axis, joint_q[iq + j])
vel_w += joint_qd[iqd + j] * axis
X_j = wp.transform(pos, rot)
v_j = wp.spatial_vector(vel_v, vel_w) # vel_v=linear, vel_w=angular
# transform from world to parent joint anchor frame
X_wpj = X_pj
if parent >= 0:
X_wp = body_q[parent]
X_wpj = X_wp * X_wpj
# transform from world to joint anchor frame at child body
X_wcj = X_wpj * X_j
# transform from world to child body frame
X_wc = X_wcj * wp.transform_inverse(X_cj)
v_parent_origin = wp.vec3()
w_parent = wp.vec3()
if parent >= 0:
v_wp = body_qd[parent]
w_parent = wp.spatial_bottom(v_wp)
v_parent_origin = velocity_at_point(
v_wp, wp.transform_get_translation(X_wc) - wp.transform_get_translation(X_wp)
)
linear_joint_anchor = wp.transform_vector(X_wpj, wp.spatial_top(v_j))
angular_joint_world = wp.transform_vector(X_wpj, wp.spatial_bottom(v_j))
child_origin_offset_world = wp.transform_get_translation(X_wc) - wp.transform_get_translation(X_wcj)
linear_joint_origin = linear_joint_anchor + wp.cross(angular_joint_world, child_origin_offset_world)
v_wc = wp.spatial_vector(
v_parent_origin + linear_joint_origin,
w_parent + angular_joint_world,
) # spatial vector with (linear, angular) ordering
body_q[child] = X_wc
body_qd[child] = v_wc
@wp.kernel
def eval_articulation_fk(
articulation_start: wp.array[int],
joint_articulation: wp.array[int],
joint_q: wp.array[float],
joint_qd: wp.array[float],
joint_q_start: wp.array[int],
joint_qd_start: wp.array[int],
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
body_com: wp.array[wp.vec3],
# outputs
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
):
tid = wp.tid()
joint_start = articulation_start[tid]
joint_end = articulation_start[tid + 1]
eval_single_articulation_fk(
joint_start,
joint_end,
joint_articulation,
joint_q,
joint_qd,
joint_q_start,
joint_qd_start,
joint_type,
joint_parent,
joint_child,
joint_X_p,
joint_X_c,
joint_axis,
joint_dof_dim,
body_com,
# outputs
body_q,
body_qd,
)
@wp.kernel
def convert_body_xforms_to_warp_kernel(
mjc_body_to_newton: wp.array2d[wp.int32],
xpos: wp.array2d[wp.vec3],
xquat: wp.array2d[wp.quat],
# outputs
body_q: wp.array[wp.transform],
):
"""Convert MuJoCo body transforms to Newton body_q array.
Iterates over MuJoCo bodies [world, mjc_body], looks up Newton body index,
reads MuJoCo position/quaternion, and writes to Newton body_q.
"""
world, mjc_body = wp.tid()
newton_body = mjc_body_to_newton[world, mjc_body]
if newton_body >= 0:
pos = xpos[world, mjc_body]
quat = xquat[world, mjc_body]
# convert from wxyz to xyzw
quat = quat_wxyz_to_xyzw(quat)
body_q[newton_body] = wp.transform(pos, quat)
@wp.kernel
def update_body_mass_ipos_kernel(
mjc_body_to_newton: wp.array2d[wp.int32],
body_com: wp.array[wp.vec3f],
body_mass: wp.array[float],
body_gravcomp: wp.array[float],
# outputs
body_ipos: wp.array2d[wp.vec3f],
body_mass_out: wp.array2d[float],
body_gravcomp_out: wp.array2d[float],
):
"""Update MuJoCo body mass and inertial position from Newton body properties.
Iterates over MuJoCo bodies [world, mjc_body], looks up Newton body index,
and copies mass, COM, and gravcomp.
"""
world, mjc_body = wp.tid()
newton_body = mjc_body_to_newton[world, mjc_body]
if newton_body < 0:
return
# update COM position
body_ipos[world, mjc_body] = body_com[newton_body]
# update mass
body_mass_out[world, mjc_body] = body_mass[newton_body]
# update gravcomp
if body_gravcomp:
body_gravcomp_out[world, mjc_body] = body_gravcomp[newton_body]
@wp.func
def _sort_eigenpairs_descending(eigenvalues: wp.vec3f, eigenvectors: wp.mat33f) -> tuple[wp.vec3f, wp.mat33f]:
"""Sort eigenvalues descending and reorder eigenvector columns to match."""
# Transpose to work with rows (easier swapping)
vecs_t = wp.transpose(eigenvectors)
vals = eigenvalues
# Bubble sort for 3 elements
for i in range(2):
for j in range(2 - i):
if vals[j] < vals[j + 1]:
# Swap eigenvalues
tmp_val = vals[j]
vals[j] = vals[j + 1]
vals[j + 1] = tmp_val
# Swap eigenvector rows
tmp_vec = vecs_t[j]
vecs_t[j] = vecs_t[j + 1]
vecs_t[j + 1] = tmp_vec
return vals, wp.transpose(vecs_t)
@wp.func
def _ensure_proper_rotation(V: wp.mat33f) -> wp.mat33f:
"""Ensure matrix is a proper rotation (det=+1) by negating a column if needed.
wp.eig3 can return eigenvector matrices with det=-1 (reflections), which
cannot be converted to valid quaternions. This fixes it by negating the
third column when det < 0.
"""
if wp.determinant(V) < 0.0:
# Negate third column to flip determinant sign
return wp.mat33(
V[0, 0],
V[0, 1],
-V[0, 2],
V[1, 0],
V[1, 1],
-V[1, 2],
V[2, 0],
V[2, 1],
-V[2, 2],
)
return V
@wp.kernel
def update_body_inertia_kernel(
mjc_body_to_newton: wp.array2d[wp.int32],
body_inertia: wp.array[wp.mat33f],
# outputs
body_inertia_out: wp.array2d[wp.vec3f],
body_iquat_out: wp.array2d[wp.quatf],
):
"""Update MuJoCo body inertia from Newton body inertia tensor.
Iterates over MuJoCo bodies [world, mjc_body], looks up Newton body index,
computes eigendecomposition, and writes to MuJoCo arrays.
"""
world, mjc_body = wp.tid()
newton_body = mjc_body_to_newton[world, mjc_body]
if newton_body < 0:
return
# Eigendecomposition of inertia tensor
eigenvectors, eigenvalues = wp.eig3(body_inertia[newton_body])
# Sort descending (MuJoCo convention)
eigenvalues, V = _sort_eigenpairs_descending(eigenvalues, eigenvectors)
# Ensure proper rotation matrix (det=+1) for valid quaternion conversion
V = _ensure_proper_rotation(V)
# Convert to quaternion (Warp uses xyzw, mujoco_warp stores wxyz)
q = wp.normalize(wp.quat_from_matrix(V))
# Convert from xyzw to wxyz
q = quat_xyzw_to_wxyz(q)
# Store results
body_inertia_out[world, mjc_body] = eigenvalues
body_iquat_out[world, mjc_body] = q
@wp.kernel(module="unique", enable_backward=False)
def repeat_array_kernel(
src: wp.array[Any],
nelems_per_world: int,
dst: wp.array[Any],
):
tid = wp.tid()
src_idx = tid % nelems_per_world
dst[tid] = src[src_idx]
@wp.kernel
def update_solver_options_kernel(
# WORLD frequency inputs (None if overridden/unavailable)
newton_impratio: wp.array[float],
newton_tolerance: wp.array[float],
newton_ls_tolerance: wp.array[float],
newton_ccd_tolerance: wp.array[float],
newton_density: wp.array[float],
newton_viscosity: wp.array[float],
newton_wind: wp.array[wp.vec3],
newton_magnetic: wp.array[wp.vec3],
# outputs - MuJoCo per-world arrays
opt_impratio_invsqrt: wp.array[float],
opt_tolerance: wp.array[float],
opt_ls_tolerance: wp.array[float],
opt_ccd_tolerance: wp.array[float],
opt_density: wp.array[float],
opt_viscosity: wp.array[float],
opt_wind: wp.array[wp.vec3],
opt_magnetic: wp.array[wp.vec3],
):
"""Update per-world solver options from Newton model.
Args:
newton_impratio: Per-world impratio values from Newton model (None if overridden)
newton_tolerance: Per-world tolerance values (None if overridden)
newton_ls_tolerance: Per-world line search tolerance values (None if overridden)
newton_ccd_tolerance: Per-world CCD tolerance values (None if overridden)
newton_density: Per-world medium density values (None if overridden)
newton_viscosity: Per-world medium viscosity values (None if overridden)
newton_wind: Per-world wind velocity vectors (None if overridden)
newton_magnetic: Per-world magnetic flux vectors (None if overridden)
opt_impratio_invsqrt: MuJoCo Warp opt.impratio_invsqrt array (shape: nworld)
opt_tolerance: MuJoCo Warp opt.tolerance array (shape: nworld)
opt_ls_tolerance: MuJoCo Warp opt.ls_tolerance array (shape: nworld)
opt_ccd_tolerance: MuJoCo Warp opt.ccd_tolerance array (shape: nworld)
opt_density: MuJoCo Warp opt.density array (shape: nworld)
opt_viscosity: MuJoCo Warp opt.viscosity array (shape: nworld)
opt_wind: MuJoCo Warp opt.wind array (shape: nworld)
opt_magnetic: MuJoCo Warp opt.magnetic array (shape: nworld)
"""
worldid = wp.tid()
# Only update if Newton array exists (None means overridden or not available)
if newton_impratio:
# MuJoCo stores impratio as inverse square root
# Guard against zero/negative values to avoid NaN/Inf
impratio_val = newton_impratio[worldid]
if impratio_val > 0.0:
opt_impratio_invsqrt[worldid] = 1.0 / wp.sqrt(impratio_val)
# else: skip update, keep existing MuJoCo default value
if newton_tolerance:
# MuJoCo Warp clamps tolerance to 1e-6 for float32 precision
# See mujoco_warp/_src/io.py: opt.tolerance = max(opt.tolerance, 1e-6)
opt_tolerance[worldid] = wp.max(newton_tolerance[worldid], 1.0e-6)
if newton_ls_tolerance:
opt_ls_tolerance[worldid] = newton_ls_tolerance[worldid]
if newton_ccd_tolerance:
opt_ccd_tolerance[worldid] = newton_ccd_tolerance[worldid]
if newton_density:
opt_density[worldid] = newton_density[worldid]
if newton_viscosity:
opt_viscosity[worldid] = newton_viscosity[worldid]
if newton_wind:
opt_wind[worldid] = newton_wind[worldid]
if newton_magnetic:
opt_magnetic[worldid] = newton_magnetic[worldid]
@wp.kernel
def update_axis_properties_kernel(
mjc_actuator_ctrl_source: wp.array[wp.int32],
mjc_actuator_to_newton_idx: wp.array[wp.int32],
joint_target_ke: wp.array[float],
joint_target_kd: wp.array[float],
joint_target_mode: wp.array[wp.int32],
dofs_per_world: wp.int32,
# outputs
actuator_bias: wp.array2d[vec10],
actuator_gain: wp.array2d[vec10],
):
"""Update MuJoCo actuator gains from Newton per-DOF arrays.
Only updates JOINT_TARGET actuators. CTRL_DIRECT actuators keep their gains
from custom attributes.
For JOINT_TARGET, uses sign encoding in mjc_actuator_to_newton_idx:
- Positive value (>=0): position actuator, newton_axis = value
- Value of -1: unmapped/skip
- Negative value (<=-2): velocity actuator, newton_axis = -(value + 2)
For POSITION-only actuators (joint_target_mode == JointTargetMode.POSITION), both
kp and kd are synced since the position actuator includes damping. For
POSITION_VELOCITY mode, only kp is synced to the position actuator (kd goes
to the separate velocity actuator).
Args:
mjc_actuator_ctrl_source: 0=JOINT_TARGET, 1=CTRL_DIRECT
mjc_actuator_to_newton_idx: Index into Newton array (sign-encoded for JOINT_TARGET)
joint_target_ke: Per-DOF position gains (kp)
joint_target_kd: Per-DOF velocity/damping gains (kd)
joint_target_mode: Per-DOF target mode from Model.joint_target_mode
dofs_per_world: Number of DOFs per world
"""
world, actuator = wp.tid()
source = mjc_actuator_ctrl_source[actuator]
if source != CTRL_SOURCE_JOINT_TARGET:
# CTRL_DIRECT: gains unchanged (set from custom attributes)
return
idx = mjc_actuator_to_newton_idx[actuator]
if idx >= 0:
# Position actuator - get kp from per-DOF array
world_dof = world * dofs_per_world + idx
kp = joint_target_ke[world_dof]
actuator_bias[world, actuator][1] = -kp
actuator_gain[world, actuator][0] = kp
# For POSITION-only mode, also sync kd (damping) to the position actuator
# For POSITION_VELOCITY mode, kd is handled by the separate velocity actuator
mode = joint_target_mode[idx] # Use template DOF index (idx) not world_dof
if mode == JointTargetMode.POSITION:
kd = joint_target_kd[world_dof]
actuator_bias[world, actuator][2] = -kd
elif idx == -1:
# Unmapped/skip
return
else:
# Velocity actuator - get kd from per-DOF array
newton_axis = -(idx + 2)
world_dof = world * dofs_per_world + newton_axis
kd = joint_target_kd[world_dof]
actuator_bias[world, actuator][2] = -kd
actuator_gain[world, actuator][0] = kd
@wp.kernel
def update_ctrl_direct_actuator_properties_kernel(
mjc_actuator_ctrl_source: wp.array[wp.int32],
mjc_actuator_to_newton_idx: wp.array[wp.int32],
newton_actuator_gainprm: wp.array[vec10],
newton_actuator_biasprm: wp.array[vec10],
newton_actuator_dynprm: wp.array[vec10],
newton_actuator_ctrlrange: wp.array[wp.vec2],
newton_actuator_forcerange: wp.array[wp.vec2],
newton_actuator_actrange: wp.array[wp.vec2],
newton_actuator_gear: wp.array[wp.spatial_vector],
newton_actuator_cranklength: wp.array[float],
actuators_per_world: wp.int32,
# outputs
actuator_gain: wp.array2d[vec10],
actuator_bias: wp.array2d[vec10],
actuator_dynprm: wp.array2d[vec10],
actuator_ctrlrange: wp.array2d[wp.vec2],
actuator_forcerange: wp.array2d[wp.vec2],
actuator_actrange: wp.array2d[wp.vec2],
actuator_gear: wp.array2d[wp.spatial_vector],
actuator_cranklength: wp.array2d[float],
):
"""Update MuJoCo actuator properties for CTRL_DIRECT actuators from Newton custom attributes.
Only updates actuators where mjc_actuator_ctrl_source == CTRL_DIRECT.
Uses mjc_actuator_to_newton_idx to map from MuJoCo actuator index to Newton's
mujoco:actuator frequency index.
Args:
mjc_actuator_ctrl_source: 0=JOINT_TARGET, 1=CTRL_DIRECT
mjc_actuator_to_newton_idx: Index into Newton's mujoco:actuator arrays
newton_actuator_gainprm: Newton's model.mujoco.actuator_gainprm
newton_actuator_biasprm: Newton's model.mujoco.actuator_biasprm
newton_actuator_dynprm: Newton's model.mujoco.actuator_dynprm
newton_actuator_ctrlrange: Newton's model.mujoco.actuator_ctrlrange
newton_actuator_forcerange: Newton's model.mujoco.actuator_forcerange
newton_actuator_actrange: Newton's model.mujoco.actuator_actrange
newton_actuator_gear: Newton's model.mujoco.actuator_gear
newton_actuator_cranklength: Newton's model.mujoco.actuator_cranklength
actuators_per_world: Number of actuators per world in Newton model
"""
world, actuator = wp.tid()
source = mjc_actuator_ctrl_source[actuator]
if source != CTRL_SOURCE_CTRL_DIRECT:
return
newton_idx = mjc_actuator_to_newton_idx[actuator]
if newton_idx < 0:
return
world_newton_idx = world * actuators_per_world + newton_idx
actuator_gain[world, actuator] = newton_actuator_gainprm[world_newton_idx]
actuator_bias[world, actuator] = newton_actuator_biasprm[world_newton_idx]
actuator_dynprm[world, actuator] = newton_actuator_dynprm[world_newton_idx]
actuator_ctrlrange[world, actuator] = newton_actuator_ctrlrange[world_newton_idx]
actuator_forcerange[world, actuator] = newton_actuator_forcerange[world_newton_idx]
actuator_actrange[world, actuator] = newton_actuator_actrange[world_newton_idx]
actuator_gear[world, actuator] = newton_actuator_gear[world_newton_idx]
actuator_cranklength[world, actuator] = newton_actuator_cranklength[world_newton_idx]
@wp.kernel
def update_dof_properties_kernel(
mjc_dof_to_newton_dof: wp.array2d[wp.int32],
newton_dof_to_body: wp.array[wp.int32],
body_flags: wp.array[wp.int32],
joint_armature: wp.array[float],
joint_friction: wp.array[float],
joint_damping: wp.array[float],
dof_solimp: wp.array[vec5],
dof_solref: wp.array[wp.vec2],
# outputs
dof_armature: wp.array2d[float],
dof_frictionloss: wp.array2d[float],
dof_damping: wp.array2d[float],
dof_solimp_out: wp.array2d[vec5],
dof_solref_out: wp.array2d[wp.vec2],
):
"""Update MuJoCo DOF properties from Newton DOF properties.
Iterates over MuJoCo DOFs [world, dof], looks up Newton DOF,
and copies armature, friction, damping, solimp, solref.
Armature updates are skipped for DOFs whose child body is marked kinematic.
"""
world, mjc_dof = wp.tid()
newton_dof = mjc_dof_to_newton_dof[world, mjc_dof]
if newton_dof < 0:
return
newton_body = newton_dof_to_body[newton_dof]
if newton_body < 0 or (body_flags[newton_body] & BodyFlags.KINEMATIC) == 0:
dof_armature[world, mjc_dof] = joint_armature[newton_dof]
dof_frictionloss[world, mjc_dof] = joint_friction[newton_dof]
if joint_damping:
dof_damping[world, mjc_dof] = joint_damping[newton_dof]
if dof_solimp:
dof_solimp_out[world, mjc_dof] = dof_solimp[newton_dof]
if dof_solref:
dof_solref_out[world, mjc_dof] = dof_solref[newton_dof]
@wp.kernel
def update_body_properties_kernel(
mjc_dof_to_newton_dof: wp.array2d[wp.int32],
newton_dof_to_body: wp.array[wp.int32],
body_flags: wp.array[wp.int32],
joint_armature: wp.array[float],
kinematic_armature: float,
# outputs
dof_armature: wp.array2d[float],
):
"""Update MuJoCo dof_armature from Newton body flags.
For each MuJoCo DOF, the mapped Newton child body controls armature source:
- kinematic body -> ``kinematic_armature``
- dynamic body -> Newton ``joint_armature``
"""
world, mjc_dof = wp.tid()
newton_dof = mjc_dof_to_newton_dof[world, mjc_dof]
if newton_dof < 0:
return
newton_body = newton_dof_to_body[newton_dof]
if newton_body >= 0 and (body_flags[newton_body] & BodyFlags.KINEMATIC) != 0:
dof_armature[world, mjc_dof] = kinematic_armature
else:
dof_armature[world, mjc_dof] = joint_armature[newton_dof]
@wp.kernel
def update_jnt_properties_kernel(
mjc_jnt_to_newton_dof: wp.array2d[wp.int32],
joint_limit_ke: wp.array[float],
joint_limit_kd: wp.array[float],
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
joint_effort_limit: wp.array[float],
solimplimit: wp.array[vec5],
joint_stiffness: wp.array[float],
limit_margin: wp.array[float],
# outputs
jnt_solimp: wp.array2d[vec5],
jnt_solref: wp.array2d[wp.vec2],
jnt_stiffness: wp.array2d[float],
jnt_margin: wp.array2d[float],
jnt_range: wp.array2d[wp.vec2],
jnt_actfrcrange: wp.array2d[wp.vec2],
):
"""Update MuJoCo joint properties from Newton DOF properties.
Iterates over MuJoCo joints [world, jnt], looks up Newton DOF,
and copies joint-level properties (limits, stiffness, solref, solimp).
"""
world, mjc_jnt = wp.tid()
newton_dof = mjc_jnt_to_newton_dof[world, mjc_jnt]
if newton_dof < 0:
return
# Update joint limit solref using negative convention
if joint_limit_ke[newton_dof] > 0.0:
jnt_solref[world, mjc_jnt] = wp.vec2(-joint_limit_ke[newton_dof], -joint_limit_kd[newton_dof])
# Update solimplimit
if solimplimit:
jnt_solimp[world, mjc_jnt] = solimplimit[newton_dof]
# Update passive stiffness
if joint_stiffness:
jnt_stiffness[world, mjc_jnt] = joint_stiffness[newton_dof]
# Update limit margin
if limit_margin:
jnt_margin[world, mjc_jnt] = limit_margin[newton_dof]
# Update joint range
jnt_range[world, mjc_jnt] = wp.vec2(joint_limit_lower[newton_dof], joint_limit_upper[newton_dof])
# update joint actuator force range (effort limit)
effort_limit = joint_effort_limit[newton_dof]
jnt_actfrcrange[world, mjc_jnt] = wp.vec2(-effort_limit, effort_limit)
@wp.kernel
def update_mocap_transforms_kernel(
mjc_mocap_to_newton_jnt: wp.array2d[wp.int32],
newton_joint_X_p: wp.array[wp.transform],
newton_joint_X_c: wp.array[wp.transform],
# outputs
mocap_pos: wp.array2d[wp.vec3],
mocap_quat: wp.array2d[wp.quat],
):
"""Update MuJoCo mocap body transforms from Newton joint data.
Iterates over MuJoCo mocap bodies [world, mocap_idx]. Each mocap body maps
to a fixed Newton joint and is updated from ``joint_X_p * inv(joint_X_c)``.
"""
world, mocap_idx = wp.tid()
newton_jnt = mjc_mocap_to_newton_jnt[world, mocap_idx]
if newton_jnt < 0:
return
parent_xform = newton_joint_X_p[newton_jnt]
child_xform = newton_joint_X_c[newton_jnt]
tf = parent_xform * wp.transform_inverse(child_xform)
mocap_pos[world, mocap_idx] = tf.p
mocap_quat[world, mocap_idx] = quat_xyzw_to_wxyz(tf.q)
@wp.kernel
def update_joint_transforms_kernel(
mjc_jnt_to_newton_jnt: wp.array2d[wp.int32],
mjc_jnt_to_newton_dof: wp.array2d[wp.int32],
mjc_jnt_bodyid: wp.array[wp.int32],
mjc_jnt_type: wp.array[wp.int32],
# Newton model data (joint-indexed)
newton_joint_X_p: wp.array[wp.transform],
newton_joint_X_c: wp.array[wp.transform],
# Newton model data (DOF-indexed)
newton_joint_axis: wp.array[wp.vec3],
# outputs
jnt_pos: wp.array2d[wp.vec3],
jnt_axis: wp.array2d[wp.vec3],
body_pos: wp.array2d[wp.vec3],
body_quat: wp.array2d[wp.quat],
):
"""Update MuJoCo joint transforms and body positions from Newton joint data.
Iterates over MuJoCo joints [world, jnt]. For each joint:
- Updates MuJoCo body_pos/body_quat from Newton joint transforms
- Updates MuJoCo jnt_pos and jnt_axis
Free joints are skipped because their motion is encoded directly in qpos/qvel.
"""
world, mjc_jnt = wp.tid()
# Get the Newton joint index for this MuJoCo joint (for joint-indexed arrays)
newton_jnt = mjc_jnt_to_newton_jnt[world, mjc_jnt]
if newton_jnt < 0:
return
# Get the Newton DOF for this MuJoCo joint (for DOF-indexed arrays like axis)
newton_dof = mjc_jnt_to_newton_dof[world, mjc_jnt]
# Skip free joints
jtype = mjc_jnt_type[mjc_jnt]
if jtype == 0: # mjJNT_FREE
return
# Get transforms from Newton (indexed by Newton joint)
child_xform = newton_joint_X_c[newton_jnt]
parent_xform = newton_joint_X_p[newton_jnt]
# Update body pos and quat from parent joint transform
tf = parent_xform * wp.transform_inverse(child_xform)
# Get the MuJoCo body for this joint and update its transform
mjc_body = mjc_jnt_bodyid[mjc_jnt]
body_pos[world, mjc_body] = tf.p
body_quat[world, mjc_body] = quat_xyzw_to_wxyz(tf.q)
# Update joint axis and position (DOF-indexed for axis)
if newton_dof >= 0:
axis = newton_joint_axis[newton_dof]
jnt_axis[world, mjc_jnt] = wp.quat_rotate(child_xform.q, axis)
jnt_pos[world, mjc_jnt] = child_xform.p
@wp.kernel(enable_backward=False)
def update_shape_mappings_kernel(
geom_to_shape_idx: wp.array[wp.int32],
geom_is_static: wp.array[bool],
shape_range_len: int,
first_env_shape_base: int,
# output - MuJoCo[world, geom] -> Newton shape
mjc_geom_to_newton_shape: wp.array2d[wp.int32],
):
"""
Build the mapping from MuJoCo [world, geom] to Newton shape index.
This is the primary mapping direction for the new unified design.
"""
world, geom_idx = wp.tid()
template_or_static_idx = geom_to_shape_idx[geom_idx]
if template_or_static_idx < 0:
return
# Check if this is a static shape using the precomputed mask
# For static shapes, template_or_static_idx is the absolute Newton shape index
# For non-static shapes, template_or_static_idx is 0-based offset from first env's first shape
is_static = geom_is_static[geom_idx]
if is_static:
# Static shape - use absolute index (same for all worlds)
newton_shape_idx = template_or_static_idx
else:
# Non-static shape - compute the absolute Newton shape index for this world
# template_or_static_idx is 0-based offset within first_group shapes
newton_shape_idx = first_env_shape_base + template_or_static_idx + world * shape_range_len
mjc_geom_to_newton_shape[world, geom_idx] = newton_shape_idx
@wp.kernel
def update_model_properties_kernel(
# Newton model properties
gravity_src: wp.array[wp.vec3],
# MuJoCo model properties
gravity_dst: wp.array[wp.vec3f],
):
world_idx = wp.tid()
gravity_dst[world_idx] = gravity_src[world_idx]
@wp.kernel
def update_geom_properties_kernel(
shape_mu: wp.array[float],
shape_ke: wp.array[float],
shape_kd: wp.array[float],
shape_size: wp.array[wp.vec3f],
shape_transform: wp.array[wp.transform],
mjc_geom_to_newton_shape: wp.array2d[wp.int32],
geom_type: wp.array[int],
GEOM_TYPE_MESH: int,
geom_dataid: wp.array[int],
mesh_pos: wp.array[wp.vec3],
mesh_quat: wp.array[wp.quat],
shape_mu_torsional: wp.array[float],
shape_mu_rolling: wp.array[float],
shape_geom_solimp: wp.array[vec5],
shape_geom_solmix: wp.array[float],
shape_margin: wp.array[float],
# outputs
geom_friction: wp.array2d[wp.vec3f],
geom_solref: wp.array2d[wp.vec2f],
geom_size: wp.array2d[wp.vec3f],
geom_pos: wp.array2d[wp.vec3f],
geom_quat: wp.array2d[wp.quatf],
geom_solimp: wp.array2d[vec5],
geom_solmix: wp.array2d[float],
geom_gap: wp.array2d[float],
geom_margin: wp.array2d[float],
):
"""Update MuJoCo geom properties from Newton shape properties.
Iterates over MuJoCo geoms [world, geom], looks up Newton shape index,
and copies shape properties to geom properties.
Note: geom_rbound (collision radius) is not updated here. MuJoCo computes
this internally based on the geometry, and Newton's shape_collision_radius
is not compatible with MuJoCo's bounding sphere calculation.
Note: geom_margin is always updated from shape_margin (unconditionally,
unlike the optional solimp/solmix fields). geom_gap is always set to 0
because Newton does not use MuJoCo's gap concept (inactive contacts have
no benefit when the collision pipeline runs every step).
"""
world, geom_idx = wp.tid()
shape_idx = mjc_geom_to_newton_shape[world, geom_idx]
if shape_idx < 0:
return
# update friction (slide, torsion, roll)
mu = shape_mu[shape_idx]
torsional = shape_mu_torsional[shape_idx]
rolling = shape_mu_rolling[shape_idx]
geom_friction[world, geom_idx] = wp.vec3f(mu, torsional, rolling)
# update geom_solref (timeconst, dampratio) using stiffness and damping
# we don't use the negative convention to support controlling the mixing of shapes' stiffnesses via solmix
# use approximation of d(0) = d(width) = 1
geom_solref[world, geom_idx] = convert_solref(shape_ke[shape_idx], shape_kd[shape_idx], 1.0, 1.0)
# update geom_solimp from custom attribute
if shape_geom_solimp:
geom_solimp[world, geom_idx] = shape_geom_solimp[shape_idx]
# update geom_solmix from custom attribute
if shape_geom_solmix:
geom_solmix[world, geom_idx] = shape_geom_solmix[shape_idx]
# update geom_margin from shape_margin, geom_gap always 0
geom_gap[world, geom_idx] = 0.0
geom_margin[world, geom_idx] = shape_margin[shape_idx]
# update size
geom_size[world, geom_idx] = shape_size[shape_idx]
# update position and orientation
# get shape transform
tf = shape_transform[shape_idx]
# check if this is a mesh geom and apply mesh transformation
if geom_type[geom_idx] == GEOM_TYPE_MESH:
mesh_id = geom_dataid[geom_idx]
mesh_p = mesh_pos[mesh_id]
mesh_q = mesh_quat[mesh_id]
mesh_tf = wp.transform(mesh_p, quat_wxyz_to_xyzw(mesh_q))
tf = tf * mesh_tf
# store position and orientation
geom_pos[world, geom_idx] = tf.p
geom_quat[world, geom_idx] = quat_xyzw_to_wxyz(tf.q)
@wp.kernel(enable_backward=False)
def create_inverse_shape_mapping_kernel(
mjc_geom_to_newton_shape: wp.array2d[wp.int32],
# output
newton_shape_to_mjc_geom: wp.array[wp.int32],
):
"""
Create partial inverse mapping from Newton shape index to MuJoCo geom index.
Note: The full inverse mapping (Newton [shape] -> MuJoCo [world, geom]) is not possible because
shape-to-geom is one-to-many: the same global Newton shape maps to one MuJoCo geom in every
world. This kernel only stores the geom index; world ID is computed from body indices
in the contact conversion kernel.
"""
world, geom_idx = wp.tid()
newton_shape_idx = mjc_geom_to_newton_shape[world, geom_idx]
if newton_shape_idx >= 0:
newton_shape_to_mjc_geom[newton_shape_idx] = geom_idx
@wp.kernel
def update_eq_properties_kernel(
mjc_eq_to_newton_eq: wp.array2d[wp.int32],
eq_solref: wp.array[wp.vec2],
eq_solimp: wp.array[vec5],
# outputs
eq_solref_out: wp.array2d[wp.vec2],
eq_solimp_out: wp.array2d[vec5],
):
"""Update MuJoCo equality constraint properties from Newton equality constraint properties.
Iterates over MuJoCo equality constraints [world, eq], looks up Newton eq constraint,
and copies solref and solimp.
"""
world, mjc_eq = wp.tid()
newton_eq = mjc_eq_to_newton_eq[world, mjc_eq]
if newton_eq < 0:
return
if eq_solref:
eq_solref_out[world, mjc_eq] = eq_solref[newton_eq]
if eq_solimp:
eq_solimp_out[world, mjc_eq] = eq_solimp[newton_eq]
@wp.kernel
def update_tendon_properties_kernel(
mjc_tendon_to_newton_tendon: wp.array2d[wp.int32],
# Newton tendon properties (inputs)
tendon_stiffness: wp.array[wp.float32],
tendon_damping: wp.array[wp.float32],
tendon_frictionloss: wp.array[wp.float32],
tendon_range: wp.array[wp.vec2],
tendon_margin: wp.array[wp.float32],
tendon_solref_limit: wp.array[wp.vec2],
tendon_solimp_limit: wp.array[vec5],
tendon_solref_friction: wp.array[wp.vec2],
tendon_solimp_friction: wp.array[vec5],
tendon_armature: wp.array[wp.float32],
tendon_actfrcrange: wp.array[wp.vec2],
# MuJoCo tendon properties (outputs)
tendon_stiffness_out: wp.array2d[wp.float32],
tendon_damping_out: wp.array2d[wp.float32],
tendon_frictionloss_out: wp.array2d[wp.float32],
tendon_range_out: wp.array2d[wp.vec2],
tendon_margin_out: wp.array2d[wp.float32],
tendon_solref_lim_out: wp.array2d[wp.vec2],
tendon_solimp_lim_out: wp.array2d[vec5],
tendon_solref_fri_out: wp.array2d[wp.vec2],
tendon_solimp_fri_out: wp.array2d[vec5],
tendon_armature_out: wp.array2d[wp.float32],
tendon_actfrcrange_out: wp.array2d[wp.vec2],
):
"""Update MuJoCo tendon properties from Newton tendon custom attributes.
Iterates over MuJoCo tendons [world, tendon], looks up Newton tendon,
and copies properties.
Note: tendon_lengthspring is NOT updated at runtime because it has special
initialization semantics in MuJoCo (value -1.0 means auto-compute from initial state).
"""
world, mjc_tendon = wp.tid()
newton_tendon = mjc_tendon_to_newton_tendon[world, mjc_tendon]
if newton_tendon < 0:
return
if tendon_stiffness:
tendon_stiffness_out[world, mjc_tendon] = tendon_stiffness[newton_tendon]
if tendon_damping:
tendon_damping_out[world, mjc_tendon] = tendon_damping[newton_tendon]
if tendon_frictionloss:
tendon_frictionloss_out[world, mjc_tendon] = tendon_frictionloss[newton_tendon]
if tendon_range:
tendon_range_out[world, mjc_tendon] = tendon_range[newton_tendon]
if tendon_margin:
tendon_margin_out[world, mjc_tendon] = tendon_margin[newton_tendon]
if tendon_solref_limit:
tendon_solref_lim_out[world, mjc_tendon] = tendon_solref_limit[newton_tendon]
if tendon_solimp_limit:
tendon_solimp_lim_out[world, mjc_tendon] = tendon_solimp_limit[newton_tendon]
if tendon_solref_friction:
tendon_solref_fri_out[world, mjc_tendon] = tendon_solref_friction[newton_tendon]
if tendon_solimp_friction:
tendon_solimp_fri_out[world, mjc_tendon] = tendon_solimp_friction[newton_tendon]
if tendon_armature:
tendon_armature_out[world, mjc_tendon] = tendon_armature[newton_tendon]
if tendon_actfrcrange:
tendon_actfrcrange_out[world, mjc_tendon] = tendon_actfrcrange[newton_tendon]
@wp.kernel
def update_eq_data_and_active_kernel(
mjc_eq_to_newton_eq: wp.array2d[wp.int32],
# Newton equality constraint data
eq_constraint_type: wp.array[wp.int32],
eq_constraint_anchor: wp.array[wp.vec3],
eq_constraint_relpose: wp.array[wp.transform],
eq_constraint_polycoef: wp.array2d[wp.float32],
eq_constraint_torquescale: wp.array[wp.float32],
eq_constraint_enabled: wp.array[wp.bool],
# outputs
eq_data_out: wp.array2d[vec11],
eq_active_out: wp.array2d[wp.bool],
):
"""Update MuJoCo equality constraint data and active status from Newton properties.
Iterates over MuJoCo equality constraints [world, eq], looks up Newton eq constraint,
and copies:
- eq_data based on constraint type:
- CONNECT: data[0:3] = anchor
- JOINT: data[0:5] = polycoef
- WELD: data[0:3] = anchor, data[3:6] = relpose translation, data[6:10] = relpose quaternion, data[10] = torquescale
- eq_active from equality_constraint_enabled
"""
world, mjc_eq = wp.tid()
newton_eq = mjc_eq_to_newton_eq[world, mjc_eq]
if newton_eq < 0:
return
constraint_type = eq_constraint_type[newton_eq]
# Read existing data to preserve fields we don't update
data = eq_data_out[world, mjc_eq]
if constraint_type == EqType.CONNECT:
# CONNECT: data[0:3] = anchor
anchor = eq_constraint_anchor[newton_eq]
data[0] = anchor[0]
data[1] = anchor[1]
data[2] = anchor[2]
elif constraint_type == EqType.JOINT:
# JOINT: data[0:5] = polycoef
for i in range(5):
data[i] = eq_constraint_polycoef[newton_eq, i]
elif constraint_type == EqType.WELD:
# WELD: data[0:3] = anchor
anchor = eq_constraint_anchor[newton_eq]
data[0] = anchor[0]
data[1] = anchor[1]
data[2] = anchor[2]
# data[3:6] = relpose translation
relpose = eq_constraint_relpose[newton_eq]
pos = wp.transform_get_translation(relpose)
data[3] = pos[0]
data[4] = pos[1]
data[5] = pos[2]
# data[6:10] = relpose quaternion in MuJoCo order (wxyz)
quat = quat_xyzw_to_wxyz(wp.transform_get_rotation(relpose))
for i in range(4):
data[6 + i] = quat[i]
# data[10] = torquescale
data[10] = eq_constraint_torquescale[newton_eq]
eq_data_out[world, mjc_eq] = data
eq_active_out[world, mjc_eq] = eq_constraint_enabled[newton_eq]
@wp.kernel
def update_mimic_eq_data_and_active_kernel(
mjc_eq_to_newton_mimic: wp.array2d[wp.int32],
# Newton mimic constraint data
constraint_mimic_coef0: wp.array[wp.float32],
constraint_mimic_coef1: wp.array[wp.float32],
constraint_mimic_enabled: wp.array[wp.bool],
# outputs
eq_data_out: wp.array2d[vec11],
eq_active_out: wp.array2d[wp.bool],
):
"""Update MuJoCo equality constraint data and active status from Newton mimic constraint properties.
Iterates over MuJoCo equality constraints [world, eq], looks up Newton mimic constraint,
and copies:
- eq_data: polycoef = [coef0, coef1, 0, 0, 0] (linear mimic relationship)
- eq_active from constraint_mimic_enabled
"""
world, mjc_eq = wp.tid()
newton_mimic = mjc_eq_to_newton_mimic[world, mjc_eq]
if newton_mimic < 0:
return
data = eq_data_out[world, mjc_eq]
# polycoef: data[0] + data[1]*q2 - q1 = 0 => q1 = coef0 + coef1*q2
data[0] = constraint_mimic_coef0[newton_mimic]
data[1] = constraint_mimic_coef1[newton_mimic]
data[2] = 0.0
data[3] = 0.0
data[4] = 0.0
eq_data_out[world, mjc_eq] = data
eq_active_out[world, mjc_eq] = constraint_mimic_enabled[newton_mimic]
@wp.func
def mj_body_acceleration(
body_rootid: wp.array[int],
xipos_in: wp.array2d[wp.vec3],
subtree_com_in: wp.array2d[wp.vec3],
cvel_in: wp.array2d[wp.spatial_vector],
cacc_in: wp.array2d[wp.spatial_vector],
worldid: int,
bodyid: int,
) -> wp.vec3:
"""Compute accelerations for bodies from mjwarp data."""
cacc = cacc_in[worldid, bodyid]
cvel = cvel_in[worldid, bodyid]
offset = xipos_in[worldid, bodyid] - subtree_com_in[worldid, body_rootid[bodyid]]
ang = wp.spatial_top(cvel)
lin = wp.spatial_bottom(cvel) - wp.cross(offset, ang)
acc = wp.spatial_bottom(cacc) - wp.cross(offset, wp.spatial_top(cacc))
correction = wp.cross(ang, lin)
return acc + correction
@wp.kernel
def convert_rigid_forces_from_mj_kernel(
mjc_body_to_newton: wp.array2d[wp.int32],
# mjw sources
mjw_body_rootid: wp.array[wp.int32],
mjw_gravity: wp.array[wp.vec3],
mjw_xipos: wp.array2d[wp.vec3],
mjw_subtree_com: wp.array2d[wp.vec3],
mjw_cacc: wp.array2d[wp.spatial_vector],
mjw_cvel: wp.array2d[wp.spatial_vector],
mjw_cint: wp.array2d[wp.spatial_vector],
# outputs
body_qdd: wp.array[wp.spatial_vector],
body_parent_f: wp.array[wp.spatial_vector],
):
"""Update RNE-computed rigid forces from mj_warp com-based forces."""
world, mjc_body = wp.tid()
newton_body = mjc_body_to_newton[world, mjc_body]
if newton_body < 0:
return
if body_qdd:
cacc = mjw_cacc[world, mjc_body]
qdd_lin = mj_body_acceleration(
mjw_body_rootid,
mjw_xipos,
mjw_subtree_com,
mjw_cvel,
mjw_cacc,
world,
mjc_body,
)
body_qdd[newton_body] = wp.spatial_vector(qdd_lin + mjw_gravity[world], wp.spatial_top(cacc))
if body_parent_f:
cint = mjw_cint[world, mjc_body]
parent_f_ang = wp.spatial_top(cint)
parent_f_lin = wp.spatial_bottom(cint)
offset = mjw_xipos[world, mjc_body] - mjw_subtree_com[world, mjw_body_rootid[mjc_body]]
body_parent_f[newton_body] = wp.spatial_vector(parent_f_lin, parent_f_ang - wp.cross(offset, parent_f_lin))
@wp.kernel
def convert_qfrc_actuator_from_mj_kernel(
mjw_qfrc_actuator: wp.array2d[wp.float32],
qpos: wp.array2d[wp.float32],
joints_per_world: int,
joint_type: wp.array[wp.int32],
joint_q_start: wp.array[wp.int32],
joint_qd_start: wp.array[wp.int32],
joint_dof_dim: wp.array2d[wp.int32],
joint_child: wp.array[wp.int32],
body_com: wp.array[wp.vec3],
mj_q_start: wp.array[wp.int32],
mj_qd_start: wp.array[wp.int32],
# output
qfrc_actuator: wp.array[wp.float32],
):
"""Convert MuJoCo qfrc_actuator [nworld, nv] into Newton flat DOF array.
Uses the same joint-based DOF mapping as the coordinate conversion
kernels. For free joints the wrench is transformed from MuJoCo's
(origin, body-frame) convention to Newton's (CoM, world-frame)
convention (dual of the velocity transform). Ball and other joints
are copied directly.
"""
worldid, jntid = wp.tid()
# Skip loop joints — they have no MuJoCo DOF entries
q_i = mj_q_start[jntid]
if q_i < 0:
return
qd_i = mj_qd_start[jntid]
wqd_i = joint_qd_start[joints_per_world * worldid + jntid]
type = joint_type[jntid]
if type == JointType.FREE:
# MuJoCo qfrc_actuator for free joint:
# [f_x, f_y, f_z] = linear force at body origin (world frame)
# [τ_x, τ_y, τ_z] = torque in body frame
# Newton convention (dual of velocity transform):
# f_lin_newton = f_lin_mujoco (unchanged)
# tau_newton = R * tau_body - r_com x f_lin
f_lin = wp.vec3(
mjw_qfrc_actuator[worldid, qd_i + 0],
mjw_qfrc_actuator[worldid, qd_i + 1],
mjw_qfrc_actuator[worldid, qd_i + 2],
)
tau_body = wp.vec3(
mjw_qfrc_actuator[worldid, qd_i + 3],
mjw_qfrc_actuator[worldid, qd_i + 4],
mjw_qfrc_actuator[worldid, qd_i + 5],
)
# Body rotation (MuJoCo quaternion wxyz)
rot = wp.quat(
qpos[worldid, q_i + 4],
qpos[worldid, q_i + 5],
qpos[worldid, q_i + 6],
qpos[worldid, q_i + 3],
)
# CoM offset in world frame
child = joint_child[jntid]
com_world = wp.quat_rotate(rot, body_com[child])
# Rotate torque body -> world and shift reference origin -> CoM
tau_world = wp.quat_rotate(rot, tau_body) - wp.cross(com_world, f_lin)
qfrc_actuator[wqd_i + 0] = f_lin[0]
qfrc_actuator[wqd_i + 1] = f_lin[1]
qfrc_actuator[wqd_i + 2] = f_lin[2]
qfrc_actuator[wqd_i + 3] = tau_world[0]
qfrc_actuator[wqd_i + 4] = tau_world[1]
qfrc_actuator[wqd_i + 5] = tau_world[2]
elif type == JointType.BALL:
for i in range(3):
qfrc_actuator[wqd_i + i] = mjw_qfrc_actuator[worldid, qd_i + i]
else:
axis_count = joint_dof_dim[jntid, 0] + joint_dof_dim[jntid, 1]
for i in range(axis_count):
qfrc_actuator[wqd_i + i] = mjw_qfrc_actuator[worldid, qd_i + i]
@wp.kernel
def update_pair_properties_kernel(
pairs_per_world: int,
pair_solref_in: wp.array[wp.vec2],
pair_solreffriction_in: wp.array[wp.vec2],
pair_solimp_in: wp.array[vec5],
pair_margin_in: wp.array[float],
pair_gap_in: wp.array[float],
pair_friction_in: wp.array[vec5],
# outputs
pair_solref_out: wp.array2d[wp.vec2],
pair_solreffriction_out: wp.array2d[wp.vec2],
pair_solimp_out: wp.array2d[vec5],
pair_margin_out: wp.array2d[float],
pair_gap_out: wp.array2d[float],
pair_friction_out: wp.array2d[vec5],
):
"""Update MuJoCo contact pair properties from Newton custom attributes.
Iterates over MuJoCo pairs [world, pair] and copies solver properties
(solref, solimp, margin, gap, friction) from Newton custom attributes.
"""
world, mjc_pair = wp.tid()
newton_pair = world * pairs_per_world + mjc_pair
if pair_solref_in:
pair_solref_out[world, mjc_pair] = pair_solref_in[newton_pair]
if pair_solreffriction_in:
pair_solreffriction_out[world, mjc_pair] = pair_solreffriction_in[newton_pair]
if pair_solimp_in:
pair_solimp_out[world, mjc_pair] = pair_solimp_in[newton_pair]
if pair_margin_in:
pair_margin_out[world, mjc_pair] = pair_margin_in[newton_pair]
if pair_gap_in:
pair_gap_out[world, mjc_pair] = pair_gap_in[newton_pair]
if pair_friction_in:
pair_friction_out[world, mjc_pair] = pair_friction_in[newton_pair]
================================================
FILE: newton/_src/solvers/mujoco/solver_mujoco.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import os
import warnings
from collections.abc import Iterable
from enum import IntEnum
from typing import TYPE_CHECKING, Any
import numpy as np
import warp as wp
from ...core.types import MAXVAL, override, vec5, vec10
from ...geometry import GeoType, ShapeFlags
from ...sim import (
BodyFlags,
Contacts,
Control,
EqType,
JointTargetMode,
JointType,
Model,
ModelBuilder,
State,
)
from ...sim.graph_coloring import color_graph, plot_graph
from ...utils import topological_sort
from ...utils.benchmark import event_scope
from ...utils.import_utils import string_to_warp
from ..flags import SolverNotifyFlags
from ..solver import SolverBase
from .kernels import (
apply_mjc_body_f_kernel,
apply_mjc_control_kernel,
apply_mjc_free_joint_f_to_body_f_kernel,
apply_mjc_qfrc_kernel,
convert_mj_coords_to_warp_kernel,
convert_newton_contacts_to_mjwarp_kernel,
convert_qfrc_actuator_from_mj_kernel,
convert_rigid_forces_from_mj_kernel,
convert_solref,
convert_warp_coords_to_mj_kernel,
create_convert_mjw_contacts_to_newton_kernel,
create_inverse_shape_mapping_kernel,
eval_articulation_fk,
repeat_array_kernel,
sync_qpos0_kernel,
update_axis_properties_kernel,
update_body_inertia_kernel,
update_body_mass_ipos_kernel,
update_body_properties_kernel,
update_ctrl_direct_actuator_properties_kernel,
update_dof_properties_kernel,
update_eq_data_and_active_kernel,
update_eq_properties_kernel,
update_geom_properties_kernel,
update_jnt_properties_kernel,
update_joint_transforms_kernel,
update_mimic_eq_data_and_active_kernel,
update_mocap_transforms_kernel,
update_model_properties_kernel,
update_pair_properties_kernel,
update_shape_mappings_kernel,
update_solver_options_kernel,
update_tendon_properties_kernel,
)
if TYPE_CHECKING:
from mujoco import MjData, MjModel
from mujoco_warp import Data as MjWarpData
from mujoco_warp import Model as MjWarpModel
else:
MjModel = object
MjData = object
MjWarpModel = object
MjWarpData = object
AttributeAssignment = Model.AttributeAssignment
AttributeFrequency = Model.AttributeFrequency
class SolverMuJoCo(SolverBase):
"""
This solver provides an interface to simulate physics using the `MuJoCo `_ physics engine,
optimized with GPU acceleration through `mujoco_warp `_. It supports both MuJoCo and
mujoco_warp backends, enabling efficient simulation of articulated systems with
contacts and constraints.
.. note::
- This solver requires `mujoco_warp`_ and its dependencies to be installed.
- For installation instructions, see the `mujoco_warp`_ repository.
- ``shape_collision_radius`` from Newton models is not used by MuJoCo. Instead, MuJoCo computes
bounding sphere radii (``geom_rbound``) internally based on the geometry definition.
Joint support:
- Supported joint types: PRISMATIC, REVOLUTE, BALL, FIXED, FREE, D6.
DISTANCE and CABLE joints are not supported.
- :attr:`~newton.Model.joint_armature`, :attr:`~newton.Model.joint_friction`,
:attr:`~newton.Model.joint_effort_limit`, :attr:`~newton.Model.joint_limit_ke`/:attr:`~newton.Model.joint_limit_kd`,
:attr:`~newton.Model.joint_target_ke`/:attr:`~newton.Model.joint_target_kd`,
:attr:`~newton.Model.joint_target_mode`, and :attr:`~newton.Control.joint_f` are supported.
- Equality constraints (CONNECT, WELD, JOINT) and mimic constraints (REVOLUTE and PRISMATIC only) are supported.
- :attr:`~newton.Model.joint_velocity_limit` and :attr:`~newton.Model.joint_enabled`
are not supported.
See :ref:`Joint feature support` for the full comparison across solvers.
Example
-------
.. code-block:: python
solver = newton.solvers.SolverMuJoCo(model)
# simulation loop
for i in range(100):
solver.step(state_in, state_out, control, contacts, dt)
state_in, state_out = state_out, state_in
Debugging
---------
To debug the SolverMuJoCo, you can save the MuJoCo model that is created from the :class:`newton.Model` in the constructor of the SolverMuJoCo:
.. code-block:: python
solver = newton.solvers.SolverMuJoCo(model, save_to_mjcf="model.xml")
This will save the MuJoCo model as an MJCF file, which can be opened in the MuJoCo simulator.
It is also possible to visualize the simulation running in the SolverMuJoCo through MuJoCo's own viewer.
This may help to debug the simulation and see how the MuJoCo model looks like when it is created from the Newton model.
.. code-block:: python
import newton
solver = newton.solvers.SolverMuJoCo(model)
for _ in range(num_frames):
# step the solver
solver.step(state_in, state_out, control, contacts, dt)
state_in, state_out = state_out, state_in
solver.render_mujoco_viewer()
"""
_KINEMATIC_ARMATURE = 1.0e10
class CtrlSource(IntEnum):
"""Control source for MuJoCo actuators.
Determines where an actuator gets its control input from:
- :attr:`JOINT_TARGET`: Maps from Newton's :attr:`~newton.Control.joint_target_pos`/:attr:`~newton.Control.joint_target_vel` arrays
- :attr:`CTRL_DIRECT`: Uses ``control.mujoco.ctrl`` directly (for MuJoCo-native control)
"""
JOINT_TARGET = 0
CTRL_DIRECT = 1
class CtrlType(IntEnum):
"""Control type for MuJoCo actuators.
For :attr:`~newton.solvers.SolverMuJoCo.CtrlSource.JOINT_TARGET` mode, determines which target array to read from:
- :attr:`POSITION`: Maps from :attr:`~newton.Control.joint_target_pos`, syncs gains from
:attr:`~newton.Model.joint_target_ke`. For :attr:`~newton.JointTargetMode.POSITION`-only actuators,
also syncs damping from :attr:`~newton.Model.joint_target_kd`. For
:attr:`~newton.JointTargetMode.POSITION_VELOCITY` mode, kd is handled by the separate velocity actuator.
- :attr:`VELOCITY`: Maps from :attr:`~newton.Control.joint_target_vel`, syncs gains from :attr:`~newton.Model.joint_target_kd`
- :attr:`GENERAL`: Used with :attr:`~newton.solvers.SolverMuJoCo.CtrlSource.CTRL_DIRECT` mode for motor/general actuators
"""
POSITION = 0
VELOCITY = 1
GENERAL = 2
class TrnType(IntEnum):
"""Transmission type values for MuJoCo actuators."""
UNDEFINED = -1
JOINT = 0
JOINT_IN_PARENT = 1
TENDON = 2
SITE = 3
BODY = 4
SLIDERCRANK = 5
# Class variables to cache the imported modules
_mujoco = None
_mujoco_warp = None
_convert_mjw_contacts_to_newton_kernel = None
@classmethod
def import_mujoco(cls):
"""Import the MuJoCo Warp dependencies and cache them as class variables."""
if cls._mujoco is None or cls._mujoco_warp is None:
try:
with warnings.catch_warnings():
# Set a filter to make all ImportWarnings "always" appear
# This is useful to debug import errors on Windows, for example
warnings.simplefilter("always", category=ImportWarning)
import mujoco
import mujoco_warp
cls._mujoco = mujoco
cls._mujoco_warp = mujoco_warp
except ImportError as e:
raise ImportError(
"MuJoCo backend not installed. Please refer to https://github.com/google-deepmind/mujoco_warp for installation instructions."
) from e
return cls._mujoco, cls._mujoco_warp
@staticmethod
def _parse_integrator(value: str | int, context: dict[str, Any] | None = None) -> int:
"""Parse integrator option: Euler=0, RK4=1, implicit=2, implicitfast=3."""
return SolverMuJoCo._parse_named_int(value, {"euler": 0, "rk4": 1, "implicit": 2, "implicitfast": 3})
@staticmethod
def _parse_solver(value: str | int, context: dict[str, Any] | None = None) -> int:
"""Parse solver option: CG=1, Newton=2. PGS (0) is not supported."""
return SolverMuJoCo._parse_named_int(value, {"cg": 1, "newton": 2})
@staticmethod
def _parse_cone(value: str | int, context: dict[str, Any] | None = None) -> int:
"""Parse cone option: pyramidal=0, elliptic=1."""
return SolverMuJoCo._parse_named_int(value, {"pyramidal": 0, "elliptic": 1})
@staticmethod
def _parse_jacobian(value: str | int, context: dict[str, Any] | None = None) -> int:
"""Parse jacobian option: dense=0, sparse=1, auto=2."""
return SolverMuJoCo._parse_named_int(value, {"dense": 0, "sparse": 1, "auto": 2})
@staticmethod
def _parse_named_int(value: str | int, mapping: dict[str, int], fallback_on_unknown: int | None = None) -> int:
"""Parse string-valued enums to int, otherwise return int(value)."""
if isinstance(value, (int, np.integer)):
return int(value)
lower_value = str(value).lower().strip()
if lower_value in mapping:
return mapping[lower_value]
# Support MuJoCo enum string reprs like "mjtCone.mjCONE_ELLIPTIC".
last_component = lower_value.rsplit(".", maxsplit=1)[-1]
if last_component in mapping:
return mapping[last_component]
enum_suffix = last_component.rsplit("_", maxsplit=1)[-1]
if enum_suffix in mapping:
return mapping[enum_suffix]
if fallback_on_unknown is not None:
return fallback_on_unknown
return int(lower_value)
@staticmethod
def _angle_value_transformer(value: str, context: dict[str, Any] | None) -> float:
"""Transform angle values from MJCF, converting deg to rad for angular joints.
For attributes like springref and ref that represent angles,
parses the string value and multiplies by pi/180 when use_degrees=True and joint is angular.
"""
parsed = string_to_warp(value, wp.float32, 0.0)
if context is not None:
joint_type = context.get("joint_type")
use_degrees = context.get("use_degrees", False)
is_angular = joint_type in ["hinge", "ball"]
if is_angular and use_degrees:
return parsed * (np.pi / 180)
return parsed
@staticmethod
def _per_angle_value_transformer(value: str, context: dict[str, Any] | None) -> float:
"""Transform per-angle values from MJCF, converting Nm/deg to Nm/rad for angular joints.
For attributes like stiffness (Nm/rad) and damping (Nm·s/rad) that have angle in the denominator,
parses the string value and multiplies by 180/pi when use_degrees=True and joint is angular.
"""
parsed = string_to_warp(value, wp.float32, 0.0)
if context is not None:
joint_type = context.get("joint_type")
use_degrees = context.get("use_degrees", False)
is_angular = joint_type in ["hinge", "ball"]
if is_angular and use_degrees:
return parsed * (180 / np.pi)
return parsed
@staticmethod
def _is_mjc_actuator_prim(prim: Any, _context: dict[str, Any]) -> bool:
"""Filter for prims of type ``MjcActuator`` for USD parsing.
This is used as the ``usd_prim_filter`` for the ``mujoco:actuator`` custom frequency.
Returns True for USD Prim objects whose type name is ``MjcActuator``.
Args:
prim: The USD prim to check.
_context: Context dictionary with parsing results (path maps, units, etc.).
This matches the return value of :meth:`newton.ModelBuilder.add_usd`.
Returns:
True if the prim is an MjcActuator, False otherwise.
"""
return prim.GetTypeName() == "MjcActuator"
@staticmethod
def _is_mjc_tendon_prim(prim: Any, _context: dict[str, Any]) -> bool:
"""Filter for prims of type ``MjcTendon`` for USD parsing.
This is used as the ``usd_prim_filter`` for the ``mujoco:tendon`` custom frequency.
Returns True for USD Prim objects whose type name is ``MjcTendon``.
Args:
prim: The USD prim to check.
_context: Context dictionary with parsing results (path maps, units, etc.).
This matches the return value of :meth:`newton.ModelBuilder.add_usd`.
Returns:
True if the prim is an MjcTendon, False otherwise.
"""
return prim.GetTypeName() == "MjcTendon"
@staticmethod
def _parse_mjc_fixed_tendon_joint_entries(prim, builder: ModelBuilder) -> list[tuple[int, float]]:
"""Parse fixed tendon joint/coefficient entries from an MjcTendon prim.
Returns:
List of ``(joint_index, coef)`` entries in authored tendon path order.
"""
tendon_type_attr = prim.GetAttribute("mjc:type")
tendon_type = tendon_type_attr.Get() if tendon_type_attr else None
if tendon_type is None or str(tendon_type).lower() != "fixed":
return []
path_rel = prim.GetRelationship("mjc:path")
path_targets = list(path_rel.GetTargets()) if path_rel else []
if len(path_targets) == 0:
return []
indices_attr = prim.GetAttribute("mjc:path:indices")
authored_indices = indices_attr.Get() if indices_attr else None
indices = list(authored_indices) if authored_indices is not None and len(authored_indices) > 0 else None
if indices is None:
# If indices are omitted, keep authored relationship order.
indices = list(range(len(path_targets)))
coef_attr = prim.GetAttribute("mjc:path:coef")
authored_coef = coef_attr.Get() if coef_attr else None
coefs = list(authored_coef) if authored_coef is not None else []
joint_entries: list[tuple[int, float]] = []
for i, path_idx in enumerate(indices):
path_idx_int = int(path_idx)
if path_idx_int < 0 or path_idx_int >= len(path_targets):
warnings.warn(
f"MjcTendon {prim.GetPath()} has out-of-range mjc:path:indices entry {path_idx_int}. Skipping.",
stacklevel=2,
)
continue
joint_path = str(path_targets[path_idx_int])
try:
joint_idx = builder.joint_label.index(joint_path)
except ValueError:
warnings.warn(
f"MjcTendon {prim.GetPath()} references unknown joint path {joint_path}. Skipping.",
stacklevel=2,
)
continue
coef = float(coefs[i]) if i < len(coefs) else 1.0
joint_entries.append((joint_idx, coef))
return joint_entries
@staticmethod
def _expand_mjc_tendon_joint_rows(prim, context: dict[str, Any]) -> Iterable[dict[str, Any]]:
"""Expand one MjcTendon prim into 0..N mujoco:tendon_joint rows."""
builder = context.get("builder")
if not isinstance(builder, ModelBuilder):
return []
joint_entries = SolverMuJoCo._parse_mjc_fixed_tendon_joint_entries(prim, builder)
return [
{
"mujoco:tendon_joint": joint_idx,
"mujoco:tendon_coef": coef,
}
for joint_idx, coef in joint_entries
]
@override
@classmethod
def register_custom_attributes(cls, builder: ModelBuilder) -> None:
"""
Declare custom attributes to be allocated on the Model object within the ``mujoco`` namespace.
Custom attributes use ``CustomAttribute.usd_attribute_name`` with the ``mjc:`` prefix (e.g. ``"mjc:condim"``)
to leverage the MuJoCo USD schema where attributes are named ``"mjc:attr"`` rather than ``"newton:mujoco:attr"``.
"""
# Register custom frequencies before adding attributes that use them
# This is required as custom frequencies must be registered before use
# Note: only attributes with usd_attribute_name defined are parsed from USD at the moment.
# region custom frequencies
builder.add_custom_frequency(ModelBuilder.CustomFrequency(name="pair", namespace="mujoco"))
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="actuator",
namespace="mujoco",
usd_prim_filter=cls._is_mjc_actuator_prim,
)
)
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="tendon",
namespace="mujoco",
usd_prim_filter=cls._is_mjc_tendon_prim,
)
)
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="tendon_joint",
namespace="mujoco",
usd_prim_filter=cls._is_mjc_tendon_prim,
usd_entry_expander=cls._expand_mjc_tendon_joint_rows,
)
)
builder.add_custom_frequency(
ModelBuilder.CustomFrequency(
name="tendon_wrap",
namespace="mujoco",
)
)
# endregion custom frequencies
# region geom attributes
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="condim",
frequency=AttributeFrequency.SHAPE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=3,
namespace="mujoco",
usd_attribute_name="mjc:condim",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="geom_priority",
frequency=AttributeFrequency.SHAPE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0,
namespace="mujoco",
usd_attribute_name="mjc:priority",
mjcf_attribute_name="priority",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="geom_solimp",
frequency=AttributeFrequency.SHAPE,
assignment=AttributeAssignment.MODEL,
dtype=vec5,
default=vec5(0.9, 0.95, 0.001, 0.5, 2.0),
namespace="mujoco",
usd_attribute_name="mjc:solimp",
mjcf_attribute_name="solimp",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="geom_solmix",
frequency=AttributeFrequency.SHAPE,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=1.0,
namespace="mujoco",
usd_attribute_name="mjc:solmix",
mjcf_attribute_name="solmix",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="limit_margin",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
usd_attribute_name="mjc:margin",
mjcf_attribute_name="margin",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="solimplimit",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=vec5,
default=vec5(0.9, 0.95, 0.001, 0.5, 2.0),
namespace="mujoco",
usd_attribute_name="mjc:solimplimit",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="solreffriction",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=wp.vec2,
default=wp.vec2(0.02, 1.0),
namespace="mujoco",
usd_attribute_name="mjc:solreffriction",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="solimpfriction",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=vec5,
default=vec5(0.9, 0.95, 0.001, 0.5, 2.0),
namespace="mujoco",
usd_attribute_name="mjc:solimpfriction",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="gravcomp",
frequency=AttributeFrequency.BODY,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
usd_attribute_name="mjc:gravcomp",
mjcf_attribute_name="gravcomp",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="dof_passive_stiffness",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
usd_attribute_name="mjc:stiffness",
mjcf_attribute_name="stiffness",
mjcf_value_transformer=cls._per_angle_value_transformer,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="dof_passive_damping",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
usd_attribute_name="mjc:damping",
mjcf_attribute_name="damping",
mjcf_value_transformer=cls._per_angle_value_transformer,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="dof_springref",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
usd_attribute_name="mjc:springref",
mjcf_attribute_name="springref",
mjcf_value_transformer=cls._angle_value_transformer,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="dof_ref",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
usd_attribute_name="mjc:ref",
mjcf_attribute_name="ref",
mjcf_value_transformer=cls._angle_value_transformer,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="jnt_actgravcomp",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=wp.bool,
default=False,
namespace="mujoco",
usd_attribute_name="mjc:actuatorgravcomp",
mjcf_attribute_name="actuatorgravcomp",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="eq_solref",
frequency=AttributeFrequency.EQUALITY_CONSTRAINT,
assignment=AttributeAssignment.MODEL,
dtype=wp.vec2,
default=wp.vec2(0.02, 1.0),
namespace="mujoco",
usd_attribute_name="mjc:solref",
mjcf_attribute_name="solref",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="eq_solimp",
frequency=AttributeFrequency.EQUALITY_CONSTRAINT,
assignment=AttributeAssignment.MODEL,
dtype=vec5,
default=vec5(0.9, 0.95, 0.001, 0.5, 2.0),
namespace="mujoco",
usd_attribute_name="mjc:solimp",
mjcf_attribute_name="solimp",
)
)
# endregion geom attributes
# region solver options
# Solver options (frequency WORLD for per-world values)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="impratio",
frequency=AttributeFrequency.WORLD,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=1.0,
namespace="mujoco",
usd_attribute_name="mjc:option:impratio",
mjcf_attribute_name="impratio",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tolerance",
frequency=AttributeFrequency.WORLD,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=1e-8,
namespace="mujoco",
usd_attribute_name="mjc:option:tolerance",
mjcf_attribute_name="tolerance",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="ls_tolerance",
frequency=AttributeFrequency.WORLD,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.01,
namespace="mujoco",
usd_attribute_name="mjc:option:ls_tolerance",
mjcf_attribute_name="ls_tolerance",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="ccd_tolerance",
frequency=AttributeFrequency.WORLD,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=1e-6,
namespace="mujoco",
usd_attribute_name="mjc:option:ccd_tolerance",
mjcf_attribute_name="ccd_tolerance",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="density",
frequency=AttributeFrequency.WORLD,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
usd_attribute_name="mjc:option:density",
mjcf_attribute_name="density",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="viscosity",
frequency=AttributeFrequency.WORLD,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
usd_attribute_name="mjc:option:viscosity",
mjcf_attribute_name="viscosity",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="wind",
frequency=AttributeFrequency.WORLD,
assignment=AttributeAssignment.MODEL,
dtype=wp.vec3,
default=wp.vec3(0.0, 0.0, 0.0),
namespace="mujoco",
usd_attribute_name="mjc:option:wind",
mjcf_attribute_name="wind",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="magnetic",
frequency=AttributeFrequency.WORLD,
assignment=AttributeAssignment.MODEL,
dtype=wp.vec3,
default=wp.vec3(0.0, -0.5, 0.0),
namespace="mujoco",
usd_attribute_name="mjc:option:magnetic",
mjcf_attribute_name="magnetic",
)
)
# Solver options (frequency ONCE for single value shared across all worlds)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="iterations",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=100,
namespace="mujoco",
usd_attribute_name="mjc:option:iterations",
mjcf_attribute_name="iterations",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="ls_iterations",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=50,
namespace="mujoco",
usd_attribute_name="mjc:option:ls_iterations",
mjcf_attribute_name="ls_iterations",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="ccd_iterations",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=35, # MuJoCo default
namespace="mujoco",
usd_attribute_name="mjc:option:ccd_iterations",
mjcf_attribute_name="ccd_iterations",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="sdf_iterations",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=10,
namespace="mujoco",
usd_attribute_name="mjc:option:sdf_iterations",
mjcf_attribute_name="sdf_iterations",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="sdf_initpoints",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=40,
namespace="mujoco",
usd_attribute_name="mjc:option:sdf_initpoints",
mjcf_attribute_name="sdf_initpoints",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="integrator",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=3, # Newton default: implicitfast (not MuJoCo's 0/Euler)
namespace="mujoco",
usd_attribute_name="mjc:option:integrator",
mjcf_attribute_name="integrator",
mjcf_value_transformer=cls._parse_integrator,
usd_value_transformer=cls._parse_integrator,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="solver",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=2, # Newton
namespace="mujoco",
usd_attribute_name="mjc:option:solver",
mjcf_attribute_name="solver",
mjcf_value_transformer=cls._parse_solver,
usd_value_transformer=cls._parse_solver,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="cone",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0, # pyramidal
namespace="mujoco",
usd_attribute_name="mjc:option:cone",
mjcf_attribute_name="cone",
mjcf_value_transformer=cls._parse_cone,
usd_value_transformer=cls._parse_cone,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="jacobian",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=2, # auto
namespace="mujoco",
usd_attribute_name="mjc:option:jacobian",
mjcf_attribute_name="jacobian",
mjcf_value_transformer=cls._parse_jacobian,
usd_value_transformer=cls._parse_jacobian,
)
)
# endregion solver options
# region pair attributes
# --- Pair attributes (from MJCF tag) ---
# Explicit contact pairs with custom properties. Pairs from the template world and
# global pairs (world < 0) are used.
# These are parsed automatically from MJCF elements.
# All pair attributes share the "mujoco:pair" custom frequency.
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_world",
frequency="mujoco:pair",
dtype=wp.int32,
default=0,
namespace="mujoco",
references="world",
# No mjcf_attribute_name - this is set automatically during parsing
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_geom1",
frequency="mujoco:pair",
dtype=wp.int32,
default=-1,
namespace="mujoco",
references="shape",
mjcf_attribute_name="geom1", # Maps to shape index via geom name lookup
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_geom2",
frequency="mujoco:pair",
dtype=wp.int32,
default=-1,
namespace="mujoco",
references="shape",
mjcf_attribute_name="geom2", # Maps to shape index via geom name lookup
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_condim",
frequency="mujoco:pair",
dtype=wp.int32,
default=3,
namespace="mujoco",
mjcf_attribute_name="condim",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_solref",
frequency="mujoco:pair",
dtype=wp.vec2,
default=wp.vec2(0.02, 1.0),
namespace="mujoco",
mjcf_attribute_name="solref",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_solreffriction",
frequency="mujoco:pair",
dtype=wp.vec2,
default=wp.vec2(0.02, 1.0),
namespace="mujoco",
mjcf_attribute_name="solreffriction",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_solimp",
frequency="mujoco:pair",
dtype=vec5,
default=vec5(0.9, 0.95, 0.001, 0.5, 2.0),
namespace="mujoco",
mjcf_attribute_name="solimp",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_margin",
frequency="mujoco:pair",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="margin",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_gap",
frequency="mujoco:pair",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="gap",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="pair_friction",
frequency="mujoco:pair",
dtype=vec5,
default=vec5(1.0, 1.0, 0.005, 0.0001, 0.0001),
namespace="mujoco",
mjcf_attribute_name="friction",
)
)
# endregion pair attributes
# region actuator attributes
# --- MuJoCo General Actuator attributes (mujoco:actuator frequency) ---
# These are used for general/motor actuators parsed from MJCF
# All actuator attributes share the "mujoco:actuator" custom frequency.
# Note: actuator_trnid[0] stores the target index, actuator_trntype determines its meaning (joint/tendon/site)
def parse_actuator_enum(value: Any, mapping: dict[str, int]) -> int:
"""Parse actuator enum values, defaulting to 0 for unknown strings."""
return SolverMuJoCo._parse_named_int(value, mapping, fallback_on_unknown=0)
def parse_trntype(s: str, _context: dict[str, Any] | None = None) -> int:
return parse_actuator_enum(
s,
{"joint": 0, "jointinparent": 1, "tendon": 2, "site": 3, "body": 4, "slidercrank": 5},
)
def parse_dyntype(s: str, _context: dict[str, Any] | None = None) -> int:
return parse_actuator_enum(
s, {"none": 0, "integrator": 1, "filter": 2, "filterexact": 3, "muscle": 4, "user": 5}
)
def parse_gaintype(s: str, _context: dict[str, Any] | None = None) -> int:
return parse_actuator_enum(s, {"fixed": 0, "affine": 1, "muscle": 2, "user": 3})
def parse_biastype(s: str, _context: dict[str, Any] | None = None) -> int:
return parse_actuator_enum(s, {"none": 0, "affine": 1, "muscle": 2, "user": 3})
def parse_bool(value: Any, context: dict[str, Any] | None = None) -> bool:
"""Parse MJCF/USD boolean values to bool."""
if isinstance(value, bool):
return value
if isinstance(value, (int, np.integer)):
return bool(value)
s = str(value).strip().lower()
if s == "auto":
if context is not None:
prim = context.get("prim")
attr = context.get("attr")
if prim is not None and attr is not None:
raise NotImplementedError(
f"Error while parsing value '{attr.usd_attribute_name}' at prim '{prim.GetPath()}'. Auto boolean values are not supported at the moment."
)
raise NotImplementedError("Auto boolean values are not supported at the moment.")
return s in ("true", "1")
def get_usd_range_if_authored(prim, range_attr_name: str) -> tuple[float, float] | None:
"""Return (min, max) for an authored USD range or None if no bounds are authored."""
min_attr = prim.GetAttribute(f"{range_attr_name}:min")
max_attr = prim.GetAttribute(f"{range_attr_name}:max")
min_authored = bool(min_attr and min_attr.HasAuthoredValue())
max_authored = bool(max_attr and max_attr.HasAuthoredValue())
if not min_authored and not max_authored:
return None
rmin = min_attr.Get() if min_attr else None
rmax = max_attr.Get() if max_attr else None
# Some USD assets omit one bound and rely on schema defaults (often 0).
# Mirror that behavior to avoid falling back to unrelated parser defaults.
if rmin is None:
rmin = 0.0
if rmax is None:
rmax = 0.0
return float(rmin), float(rmax)
def make_usd_range_transformer(range_attr_name: str):
"""Create a transformer that parses a USD min/max range pair."""
def transform(_: Any, context: dict[str, Any]) -> wp.vec2 | None:
range_vals = get_usd_range_if_authored(context["prim"], range_attr_name)
if range_vals is None:
return None
return wp.vec2(range_vals[0], range_vals[1])
return transform
def make_usd_has_range_transformer(range_attr_name: str):
"""Create a transformer that returns 1 when a USD range is authored."""
def transform(_: Any, context: dict[str, Any]) -> int:
range_vals = get_usd_range_if_authored(context["prim"], range_attr_name)
return int(range_vals is not None)
return transform
def make_usd_limited_transformer(limited_attr_name: str, range_attr_name: str):
"""Create a transformer for MuJoCo tri-state limited tokens.
The corresponding USD attributes are token-valued with allowed values
``"false"``, ``"true"``, and ``"auto"``. We preserve this tristate
representation as integers ``0/1/2`` and defer any autolimits-based
resolution to MuJoCo compilation.
"""
def transform(_: Any, context: dict[str, Any]) -> int:
prim = context["prim"]
limited_attr = prim.GetAttribute(limited_attr_name)
if limited_attr and limited_attr.HasAuthoredValue():
return parse_tristate(limited_attr.Get())
# Keep MuJoCo's default tri-state semantics: omitted means "auto" (2).
return 2
return transform
def _resolve_inheritrange_as_ctrlrange(prim, context: dict[str, Any]) -> tuple[float, float] | None:
"""Resolve mjc:inheritRange to a concrete (lower, upper) control range.
Reads the target joint's limits from the builder and computes the
control range Returns None if inheritRange is not authored, zero, or the target joint cannot be found.
"""
inherit_attr = prim.GetAttribute("mjc:inheritRange")
if not inherit_attr or not inherit_attr.HasAuthoredValue():
return None
inheritrange = float(inherit_attr.Get())
if inheritrange <= 0:
return None
result = context.get("result")
b = context.get("builder")
if not result or not b:
return None
try:
target_path = resolve_actuator_target_path(prim)
except ValueError:
return None
path_joint_map = result.get("path_joint_map", {})
joint_idx = path_joint_map.get(target_path, -1)
if joint_idx < 0 or joint_idx >= len(b.joint_qd_start):
return None
dof_idx = b.joint_qd_start[joint_idx]
if dof_idx < 0 or dof_idx >= len(b.joint_limit_lower):
return None
lower = b.joint_limit_lower[dof_idx]
upper = b.joint_limit_upper[dof_idx]
if lower >= upper:
return None
mean = (upper + lower) / 2.0
radius = (upper - lower) / 2.0 * inheritrange
return (mean - radius, mean + radius)
def transform_ctrlrange(_: Any, context: dict[str, Any]) -> wp.vec2 | None:
"""Parse mjc:ctrlRange, falling back to inheritrange-derived range."""
prim = context["prim"]
range_vals = get_usd_range_if_authored(prim, "mjc:ctrlRange")
if range_vals is not None:
return wp.vec2(range_vals[0], range_vals[1])
resolved = _resolve_inheritrange_as_ctrlrange(prim, context)
if resolved is not None:
return wp.vec2(float(resolved[0]), float(resolved[1]))
return None
def transform_has_ctrlrange(_: Any, context: dict[str, Any]) -> int:
"""Return 1 when ctrlRange is authored or inheritrange resolves a range."""
prim = context["prim"]
if get_usd_range_if_authored(prim, "mjc:ctrlRange") is not None:
return 1
if _resolve_inheritrange_as_ctrlrange(prim, context) is not None:
return 1
return 0
def transform_ctrllimited(_: Any, context: dict[str, Any]) -> int:
"""Parse mjc:ctrlLimited, defaulting to true when inheritrange resolves."""
prim = context["prim"]
limited_attr = prim.GetAttribute("mjc:ctrlLimited")
if limited_attr and limited_attr.HasAuthoredValue():
return parse_tristate(limited_attr.Get())
if _resolve_inheritrange_as_ctrlrange(prim, context) is not None:
return 1
return 2
def resolve_prim_name(_: str, context: dict[str, Any]) -> str:
"""Return the USD prim path as the attribute value.
Used as a ``usd_value_transformer`` for custom attributes whose value
should simply be the scene path of the prim they are defined on (e.g.
tendon labels).
Args:
_: The attribute name (unused).
context: A dictionary containing at least a ``"prim"`` key with the
USD prim being processed.
Returns:
The ``Sdf.Path`` of the prim.
"""
return str(context["prim"].GetPath())
def resolve_actuator_target_path(prim) -> str:
"""Resolve the single target path referenced by an ``MjcActuator`` prim."""
rel = prim.GetRelationship("mjc:target")
target_paths = rel.GetTargets() if rel else []
if len(target_paths) == 0:
raise ValueError(f"MjcActuator {prim.GetPath()} is missing a 'mjc:target' relationship")
if len(target_paths) != 1:
raise ValueError(f"MjcActuator {prim.GetPath()} has unsupported number of targets: {len(target_paths)}")
return str(target_paths[0])
def get_registered_string_values(attribute_name: str) -> list[str]:
"""Return registered string values for a custom attribute."""
attr = builder.custom_attributes.get(attribute_name)
if attr is None or attr.values is None:
return []
if isinstance(attr.values, dict):
return [str(attr.values[idx]) for idx in sorted(attr.values.keys())]
if isinstance(attr.values, list):
return [str(value) for value in attr.values]
return []
def resolve_actuator_target(
prim,
) -> tuple[int, int, str]:
"""Resolve actuator target to (trntype, target_index, target_path).
Returns (-1, -1, target_path) when the target path cannot be mapped yet.
This can happen during USD parsing when tendon rows are authored later in
prim traversal order.
"""
target_path = resolve_actuator_target_path(prim)
joint_dof_names = get_registered_string_values("mujoco:joint_dof_label")
try:
return int(SolverMuJoCo.TrnType.JOINT), joint_dof_names.index(target_path), target_path
except ValueError:
pass
tendon_names = get_registered_string_values("mujoco:tendon_label")
try:
return int(SolverMuJoCo.TrnType.TENDON), tendon_names.index(target_path), target_path
except ValueError:
pass
return -1, -1, target_path
def resolve_joint_dof_label(_: str, context: dict[str, Any]):
"""For each DOF, return the prim path(s) of the DOF(s).
The returned list length must match the joint's DOF count:
- PhysicsRevoluteJoint / PhysicsPrismaticJoint: 1 DOF → [prim_path]
- PhysicsFixedJoint: 0 DOFs → [] (empty list, no DOFs to name)
- PhysicsSphericalJoint: 3 DOFs → [prim_path:rotX, prim_path:rotY, prim_path:rotZ]
- PhysicsJoint (D6): N DOFs → one entry per free axis, determined from limit attributes
Args:
_: The attribute name (unused).
context: A dictionary containing at least a ``"prim"`` key with the USD prim
for the joint being processed.
Returns:
A list of DOF name strings whose length matches the joint's DOF count.
"""
prim = context["prim"]
prim_type = prim.GetTypeName()
prim_path = str(prim.GetPath())
if prim_type in ["PhysicsRevoluteJoint", "PhysicsPrismaticJoint"]:
return [prim_path]
if prim_type == "PhysicsFixedJoint":
return []
if prim_type == "PhysicsSphericalJoint":
# Spherical (ball) joints always have 3 rotational DOFs
return [f"{prim_path}:rotX", f"{prim_path}:rotY", f"{prim_path}:rotZ"]
if prim_type == "PhysicsJoint":
# Determine free axes from limit attributes on the prim.
# An axis is a DOF when its limit low < high.
# Linear axes are enumerated first, then angular, to match the DOF
# ordering used by add_joint_d6 (linear_axes before angular_axes).
dof_names = []
for axis_name in ["transX", "transY", "transZ", "rotX", "rotY", "rotZ"]:
low_attr = prim.GetAttribute(f"limit:{axis_name}:physics:low")
high_attr = prim.GetAttribute(f"limit:{axis_name}:physics:high")
if low_attr and high_attr:
low = low_attr.Get()
high = high_attr.Get()
if low is not None and high is not None and low < high:
dof_names.append(f"{prim_path}:{axis_name}")
return dof_names
warnings.warn(f"Unsupported joint type for DOF name resolution: {prim_type}", stacklevel=2)
return []
# First we get a list of all joint DOF names from USD
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="joint_dof_label",
frequency=AttributeFrequency.JOINT_DOF,
assignment=AttributeAssignment.MODEL,
dtype=str,
default="",
namespace="mujoco",
usd_attribute_name="*",
usd_value_transformer=resolve_joint_dof_label,
)
)
# Then we resolve each USD actuator transmission target from its mjc:target path.
# If target resolution is not possible yet (for example tendon target parsed later),
# we preserve sentinel values and resolve deterministically in _init_actuators
# using actuator_target_label.
def resolve_actuator_transmission_type(_: str, context: dict[str, Any]) -> int:
"""Resolve transmission type for a USD actuator prim from its target path."""
prim = context["prim"]
trntype, _target_idx, _target_path = resolve_actuator_target(prim)
if trntype < 0:
return int(SolverMuJoCo.TrnType.JOINT)
return trntype
def resolve_actuator_target_label(_: str, context: dict[str, Any]) -> str:
"""Resolve target path label for a USD actuator prim."""
return resolve_actuator_target_path(context["prim"])
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_trnid",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.vec2i,
default=wp.vec2i(-1, -1),
namespace="mujoco",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_target_label",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=str,
default="",
namespace="mujoco",
usd_attribute_name="*",
usd_value_transformer=resolve_actuator_target_label,
)
)
def parse_tristate(value: Any, _context: dict[str, Any] | None = None) -> int:
"""Parse MuJoCo tri-state values to int.
Accepts ``"false"``, ``"true"``, and ``"auto"`` (or their numeric
equivalents ``0``, ``1``, and ``2``) and returns the corresponding
integer code expected by MuJoCo custom attributes.
"""
return SolverMuJoCo._parse_named_int(value, {"false": 0, "true": 1, "auto": 2})
def parse_presence(_value: str, _context: dict[str, Any] | None = None) -> int:
"""Return 1 to indicate the attribute was explicitly present in the MJCF."""
return 1
# Compiler option (frequency ONCE for single value shared across all worlds)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="autolimits",
frequency=AttributeFrequency.ONCE,
assignment=AttributeAssignment.MODEL,
dtype=wp.bool,
default=True, # MuJoCo default: true
namespace="mujoco",
mjcf_value_transformer=parse_bool,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_trntype",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0, # TrnType.JOINT
namespace="mujoco",
mjcf_attribute_name="trntype",
mjcf_value_transformer=parse_trntype,
usd_attribute_name="*",
usd_value_transformer=resolve_actuator_transmission_type,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_dyntype",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0, # DynType.NONE
namespace="mujoco",
mjcf_attribute_name="dyntype",
mjcf_value_transformer=parse_dyntype,
usd_attribute_name="mjc:dynType",
usd_value_transformer=parse_dyntype,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_gaintype",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0, # GainType.FIXED
namespace="mujoco",
mjcf_attribute_name="gaintype",
mjcf_value_transformer=parse_gaintype,
usd_attribute_name="mjc:gainType",
usd_value_transformer=parse_gaintype,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_biastype",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0, # BiasType.NONE
namespace="mujoco",
mjcf_attribute_name="biastype",
mjcf_value_transformer=parse_biastype,
usd_attribute_name="mjc:biasType",
usd_value_transformer=parse_biastype,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_world",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=-1,
namespace="mujoco",
references="world",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_ctrllimited",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=2,
namespace="mujoco",
mjcf_attribute_name="ctrllimited",
mjcf_value_transformer=parse_tristate,
usd_attribute_name="*",
usd_value_transformer=transform_ctrllimited,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_forcelimited",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=2,
namespace="mujoco",
mjcf_attribute_name="forcelimited",
mjcf_value_transformer=parse_tristate,
usd_attribute_name="*",
usd_value_transformer=make_usd_limited_transformer("mjc:forceLimited", "mjc:forceRange"),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_ctrlrange",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.vec2,
default=wp.vec2(0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="ctrlrange",
usd_attribute_name="*",
usd_value_transformer=transform_ctrlrange,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_has_ctrlrange",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0,
namespace="mujoco",
mjcf_attribute_name="ctrlrange",
mjcf_value_transformer=parse_presence,
usd_attribute_name="*",
usd_value_transformer=transform_has_ctrlrange,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_forcerange",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.vec2,
default=wp.vec2(0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="forcerange",
usd_attribute_name="*",
usd_value_transformer=make_usd_range_transformer("mjc:forceRange"),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_has_forcerange",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0,
namespace="mujoco",
mjcf_attribute_name="forcerange",
mjcf_value_transformer=parse_presence,
usd_attribute_name="*",
usd_value_transformer=make_usd_has_range_transformer("mjc:forceRange"),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_gear",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.types.vector(length=6, dtype=wp.float32),
default=wp.types.vector(length=6, dtype=wp.float32)(1.0, 0.0, 0.0, 0.0, 0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="gear",
usd_attribute_name="mjc:gear",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_cranklength",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="cranklength",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_dynprm",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=vec10,
default=vec10(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="dynprm",
usd_attribute_name="mjc:dynPrm",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_gainprm",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=vec10,
default=vec10(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="gainprm",
usd_attribute_name="mjc:gainPrm",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_biasprm",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=vec10,
default=vec10(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="biasprm",
usd_attribute_name="mjc:biasPrm",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_actlimited",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=2,
namespace="mujoco",
mjcf_attribute_name="actlimited",
mjcf_value_transformer=parse_tristate,
usd_attribute_name="*",
usd_value_transformer=make_usd_limited_transformer("mjc:actLimited", "mjc:actRange"),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_actrange",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.vec2,
default=wp.vec2(0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="actrange",
usd_attribute_name="*",
usd_value_transformer=make_usd_range_transformer("mjc:actRange"),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_has_actrange",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=0,
namespace="mujoco",
mjcf_attribute_name="actrange",
mjcf_value_transformer=parse_presence,
usd_attribute_name="*",
usd_value_transformer=make_usd_has_range_transformer("mjc:actRange"),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_actdim",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=-1,
namespace="mujoco",
mjcf_attribute_name="actdim",
usd_attribute_name="mjc:actDim",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="actuator_actearly",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.bool,
default=False,
namespace="mujoco",
mjcf_attribute_name="actearly",
mjcf_value_transformer=parse_bool,
usd_attribute_name="mjc:actEarly",
usd_value_transformer=parse_bool,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="ctrl",
frequency="mujoco:actuator",
assignment=AttributeAssignment.CONTROL,
dtype=wp.float32,
default=0.0,
namespace="mujoco",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="ctrl_source",
frequency="mujoco:actuator",
assignment=AttributeAssignment.MODEL,
dtype=wp.int32,
default=int(SolverMuJoCo.CtrlSource.CTRL_DIRECT),
namespace="mujoco",
)
)
# endregion actuator attributes
# region tendon attributes
# --- Fixed Tendon attributes (variable-length, from MJCF tag) ---
# Fixed tendons compute length as a linear combination of joint positions.
# Only tendons from the template world are used; MuJoCo replicates them across worlds.
# Tendon-level attributes (one per tendon)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_world",
frequency="mujoco:tendon",
dtype=wp.int32,
default=0,
namespace="mujoco",
references="world",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_stiffness",
frequency="mujoco:tendon",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="stiffness",
usd_attribute_name="mjc:stiffness",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_damping",
frequency="mujoco:tendon",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="damping",
usd_attribute_name="mjc:damping",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_frictionloss",
frequency="mujoco:tendon",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="frictionloss",
usd_attribute_name="mjc:frictionloss",
)
)
def resolve_context_builder(context: dict[str, Any]) -> ModelBuilder:
"""Resolve builder from transformer context, falling back to current builder."""
context_builder = context.get("builder")
if isinstance(context_builder, ModelBuilder):
return context_builder
return builder
def resolve_tendon_joint_adr(_: Any, context: dict[str, Any]) -> int:
context_builder = resolve_context_builder(context)
tendon_joint_attr = context_builder.custom_attributes.get("mujoco:tendon_joint")
if tendon_joint_attr is None or not isinstance(tendon_joint_attr.values, list):
return 0
return len(tendon_joint_attr.values)
def resolve_tendon_joint_num(_: Any, context: dict[str, Any]) -> int:
context_builder = resolve_context_builder(context)
joint_entries = cls._parse_mjc_fixed_tendon_joint_entries(context["prim"], context_builder)
return len(joint_entries)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_limited",
frequency="mujoco:tendon",
dtype=wp.int32,
default=2, # 0=false, 1=true, 2=auto
namespace="mujoco",
mjcf_attribute_name="limited",
mjcf_value_transformer=parse_tristate,
usd_attribute_name="mjc:limited",
usd_value_transformer=parse_tristate,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_range",
frequency="mujoco:tendon",
dtype=wp.vec2,
default=wp.vec2(0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="range",
usd_attribute_name="mjc:range:min",
usd_value_transformer=make_usd_range_transformer("mjc:range"),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_margin",
frequency="mujoco:tendon",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="margin",
usd_attribute_name="mjc:margin",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_solref_limit",
frequency="mujoco:tendon",
dtype=wp.vec2,
default=wp.vec2(0.02, 1.0),
namespace="mujoco",
mjcf_attribute_name="solreflimit",
usd_attribute_name="mjc:solreflimit",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_solimp_limit",
frequency="mujoco:tendon",
dtype=vec5,
default=vec5(0.9, 0.95, 0.001, 0.5, 2.0),
namespace="mujoco",
mjcf_attribute_name="solimplimit",
usd_attribute_name="mjc:solimplimit",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_solref_friction",
frequency="mujoco:tendon",
dtype=wp.vec2,
default=wp.vec2(0.02, 1.0),
namespace="mujoco",
mjcf_attribute_name="solreffriction",
usd_attribute_name="mjc:solreffriction",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_solimp_friction",
frequency="mujoco:tendon",
dtype=vec5,
default=vec5(0.9, 0.95, 0.001, 0.5, 2.0),
namespace="mujoco",
mjcf_attribute_name="solimpfriction",
usd_attribute_name="mjc:solimpfriction",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_armature",
frequency="mujoco:tendon",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="armature",
usd_attribute_name="mjc:armature",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_springlength",
frequency="mujoco:tendon",
dtype=wp.vec2,
default=wp.vec2(-1.0, -1.0), # -1 means use default (model length)
namespace="mujoco",
mjcf_attribute_name="springlength",
usd_attribute_name="mjc:springlength",
)
)
# Addressing into joint arrays (one per tendon)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_joint_adr",
frequency="mujoco:tendon",
dtype=wp.int32,
default=0,
namespace="mujoco",
references="mujoco:tendon_joint", # Offset by joint entry count during merge
usd_attribute_name="*",
usd_value_transformer=resolve_tendon_joint_adr,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_joint_num",
frequency="mujoco:tendon",
dtype=wp.int32,
default=0,
namespace="mujoco",
usd_attribute_name="*",
usd_value_transformer=resolve_tendon_joint_num,
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_actuator_force_range",
frequency="mujoco:tendon",
dtype=wp.vec2,
default=wp.vec2(0.0, 0.0),
namespace="mujoco",
mjcf_attribute_name="actuatorfrcrange",
usd_attribute_name="mjc:actuatorfrcrange:min",
usd_value_transformer=make_usd_range_transformer("mjc:actuatorfrcrange"),
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_actuator_force_limited",
frequency="mujoco:tendon",
dtype=wp.int32,
default=2, # 0=false, 1=true, 2=auto
namespace="mujoco",
mjcf_attribute_name="actuatorfrclimited",
mjcf_value_transformer=parse_tristate,
usd_attribute_name="mjc:actuatorfrclimited",
usd_value_transformer=parse_tristate,
)
)
# Tendon names (string attribute - stored as list[str], not warp array)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_label",
frequency="mujoco:tendon",
dtype=str,
default="",
namespace="mujoco",
mjcf_attribute_name="name",
usd_attribute_name="*",
usd_value_transformer=resolve_prim_name,
)
)
# Joint arrays (one entry per joint in a fixed tendon's linear combination)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_joint",
frequency="mujoco:tendon_joint",
dtype=wp.int32,
default=-1,
namespace="mujoco",
references="joint", # Offset by joint count during merge
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_coef",
frequency="mujoco:tendon_joint",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
mjcf_attribute_name="coef",
)
)
# endregion tendon attributes
# --- Spatial tendon attributes ---
# Tendon type distinguishes fixed (0) from spatial (1) tendons.
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_type",
frequency="mujoco:tendon",
dtype=wp.int32,
default=0,
namespace="mujoco",
)
)
# Addressing into wrap path arrays (one per tendon, used by spatial tendons)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_wrap_adr",
frequency="mujoco:tendon",
dtype=wp.int32,
default=0,
namespace="mujoco",
references="mujoco:tendon_wrap",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_wrap_num",
frequency="mujoco:tendon",
dtype=wp.int32,
default=0,
namespace="mujoco",
)
)
# Wrap path arrays (one entry per wrap element in a spatial tendon's path)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_wrap_type",
frequency="mujoco:tendon_wrap",
dtype=wp.int32,
default=0, # 0=site, 1=geom, 2=pulley
namespace="mujoco",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_wrap_shape",
frequency="mujoco:tendon_wrap",
dtype=wp.int32,
default=-1,
namespace="mujoco",
references="shape", # Offset by shape count during merge
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_wrap_sidesite",
frequency="mujoco:tendon_wrap",
dtype=wp.int32,
default=-1,
namespace="mujoco",
references="shape", # Offset by shape count during merge
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tendon_wrap_prm",
frequency="mujoco:tendon_wrap",
dtype=wp.float32,
default=0.0,
namespace="mujoco",
)
)
def _init_pairs(self, model: Model, spec: Any, shape_mapping: dict[int, str], template_world: int) -> None:
"""
Initialize MuJoCo contact pairs from custom attributes.
Pairs belonging to the template world and global pairs (world < 0) are
added to the MuJoCo spec. MuJoCo will replicate these pairs across all
worlds automatically.
Args:
model: The Newton model.
spec: The MuJoCo spec to add pairs to.
shape_mapping: Mapping from Newton shape index to MuJoCo geom name.
template_world: The world index to use as the template (typically first_group).
"""
pair_count = model.custom_frequency_counts.get("mujoco:pair", 0)
if pair_count == 0:
return
mujoco_attrs = model.mujoco
def get_numpy(name):
attr = getattr(mujoco_attrs, name, None)
return attr.numpy() if attr is not None else None
pair_world = get_numpy("pair_world")
pair_geom1 = get_numpy("pair_geom1")
pair_geom2 = get_numpy("pair_geom2")
if pair_world is None or pair_geom1 is None or pair_geom2 is None:
return
pair_condim = get_numpy("pair_condim")
pair_solref = get_numpy("pair_solref")
pair_solreffriction = get_numpy("pair_solreffriction")
pair_solimp = get_numpy("pair_solimp")
pair_margin = get_numpy("pair_margin")
pair_gap = get_numpy("pair_gap")
pair_friction = get_numpy("pair_friction")
for i in range(pair_count):
# Only include pairs from the template world or global pairs (world < 0)
pw = int(pair_world[i])
if pw != template_world and pw >= 0:
continue
# Map Newton shape indices to MuJoCo geom names
newton_shape1 = int(pair_geom1[i])
newton_shape2 = int(pair_geom2[i])
# Skip invalid pairs
if newton_shape1 < 0 or newton_shape2 < 0:
continue
geom_name1 = shape_mapping.get(newton_shape1)
geom_name2 = shape_mapping.get(newton_shape2)
if geom_name1 is None or geom_name2 is None:
warnings.warn(
f"Skipping pair {i}: Newton shapes ({newton_shape1}, {newton_shape2}) "
f"not found in MuJoCo shape mapping.",
stacklevel=2,
)
continue
# Build pair kwargs
pair_kwargs: dict[str, Any] = {
"geomname1": geom_name1,
"geomname2": geom_name2,
}
if pair_condim is not None:
pair_kwargs["condim"] = int(pair_condim[i])
if pair_solref is not None:
pair_kwargs["solref"] = pair_solref[i].tolist()
if pair_solreffriction is not None:
pair_kwargs["solreffriction"] = pair_solreffriction[i].tolist()
if pair_solimp is not None:
pair_kwargs["solimp"] = pair_solimp[i].tolist()
if pair_margin is not None:
pair_kwargs["margin"] = float(pair_margin[i])
if pair_gap is not None:
pair_kwargs["gap"] = float(pair_gap[i])
if pair_friction is not None:
pair_kwargs["friction"] = pair_friction[i].tolist()
spec.add_pair(**pair_kwargs)
@staticmethod
def _validate_tendon_attributes(model: Model) -> tuple[int, int, int]:
"""
Validate that all tendon attributes have consistent lengths.
Args:
model: The Newton model to validate.
Returns:
tuple[int, int, int]: (tendon_count, joint_entry_count, wrap_entry_count).
Raises:
ValueError: If tendon attributes have inconsistent lengths.
"""
mujoco_attrs = getattr(model, "mujoco", None)
if mujoco_attrs is None:
return 0, 0, 0
# Tendon-level attributes
tendon_attr_names = [
"tendon_world",
"tendon_type",
"tendon_stiffness",
"tendon_damping",
"tendon_frictionloss",
"tendon_limited",
"tendon_range",
"tendon_margin",
"tendon_actuator_force_limited",
"tendon_actuator_force_range",
"tendon_solref_limit",
"tendon_solimp_limit",
"tendon_solref_friction",
"tendon_solimp_friction",
"tendon_springlength",
"tendon_armature",
"tendon_joint_adr",
"tendon_joint_num",
"tendon_wrap_adr",
"tendon_wrap_num",
]
# If the list above has N parameters then each tendon should have exactly N parameters.
# Count the number of parameters that we have for each tendon.
# Each entry in the array of counts should be N.
# We can then extract the number of unique entries in our array of counts.
# The number of unique entries should be 1 because every entry should be N.
# If the number of unique entries is not 1 then we are missing an attribute on at least one tendon.
tendon_lengths: dict[str, int] = {}
for name in tendon_attr_names:
attr = getattr(mujoco_attrs, name, None)
if attr is not None:
tendon_lengths[name] = len(attr)
if not tendon_lengths:
return 0, 0, 0
# Check all tendon-level lengths are the same
unique_tendon_lengths = set(tendon_lengths.values())
if len(unique_tendon_lengths) > 1:
raise ValueError(
f"MuJoCo tendon attributes have inconsistent lengths: {tendon_lengths}. "
"All tendon-level attributes must have the same number of elements."
)
# Compute the number of tendons.
tendon_count = next(iter(unique_tendon_lengths))
# Attributes per joint in the tendon that allow the tendon length to
# be calculated as a linear sum of coefficient and joint position.
# For each joint in a tendon (specified by joint index) there must be a corresponding coefficient.
joint_attr_names = ["tendon_joint", "tendon_coef"]
joint_lengths: dict[str, int] = {}
for name in joint_attr_names:
attr = getattr(mujoco_attrs, name, None)
if attr is not None:
joint_lengths[name] = len(attr)
if not joint_lengths:
joint_entry_count = 0
else:
unique_joint_lengths = set(joint_lengths.values())
if len(unique_joint_lengths) > 1:
raise ValueError(
f"MuJoCo tendon joint attributes have inconsistent lengths: {joint_lengths}. "
"All joint-level attributes must have the same number of elements."
)
joint_entry_count = next(iter(unique_joint_lengths))
# Wrap path attributes for spatial tendons
wrap_attr_names = ["tendon_wrap_type", "tendon_wrap_shape", "tendon_wrap_sidesite", "tendon_wrap_prm"]
wrap_lengths: dict[str, int] = {}
for name in wrap_attr_names:
attr = getattr(mujoco_attrs, name, None)
if attr is not None:
wrap_lengths[name] = len(attr)
if not wrap_lengths:
wrap_entry_count = 0
else:
unique_wrap_lengths = set(wrap_lengths.values())
if len(unique_wrap_lengths) > 1:
raise ValueError(
f"MuJoCo tendon wrap attributes have inconsistent lengths: {wrap_lengths}. "
"All wrap-level attributes must have the same number of elements."
)
wrap_entry_count = next(iter(unique_wrap_lengths))
return tendon_count, joint_entry_count, wrap_entry_count
def _init_tendons(
self,
model: Model,
spec: Any,
joint_mapping: dict[int, str],
shape_mapping: dict[int, str],
site_mapping: dict[int, str],
template_world: int,
) -> tuple[list[int], list[str]]:
"""
Initialize MuJoCo fixed and spatial tendons from custom attributes.
Only tendons belonging to the template world are added to the MuJoCo spec.
MuJoCo will replicate these tendons across all worlds automatically.
Args:
model: The Newton model.
spec: The MuJoCo spec to add tendons to.
joint_mapping: Mapping from Newton joint index to MuJoCo joint name.
shape_mapping: Mapping from Newton shape index to MuJoCo geom name.
site_mapping: Mapping from Newton shape index (sites) to MuJoCo site name.
template_world: The world index to use as the template (typically first_group).
Returns:
tuple[list[int], list[str]]: Tuple of (Newton tendon indices, MuJoCo tendon names).
"""
tendon_count, joint_entry_count, wrap_entry_count = self._validate_tendon_attributes(model)
if tendon_count == 0:
return [], []
mujoco_attrs = model.mujoco
# Get tendon-level arrays
tendon_world = mujoco_attrs.tendon_world.numpy()
tendon_type_attr = getattr(mujoco_attrs, "tendon_type", None)
tendon_type_np = tendon_type_attr.numpy() if tendon_type_attr is not None else None
tendon_stiffness = getattr(mujoco_attrs, "tendon_stiffness", None)
tendon_stiffness_np = tendon_stiffness.numpy() if tendon_stiffness is not None else None
tendon_damping = getattr(mujoco_attrs, "tendon_damping", None)
tendon_damping_np = tendon_damping.numpy() if tendon_damping is not None else None
tendon_frictionloss = getattr(mujoco_attrs, "tendon_frictionloss", None)
tendon_frictionloss_np = tendon_frictionloss.numpy() if tendon_frictionloss is not None else None
tendon_limited = getattr(mujoco_attrs, "tendon_limited", None)
tendon_limited_np = tendon_limited.numpy() if tendon_limited is not None else None
tendon_range = getattr(mujoco_attrs, "tendon_range", None)
tendon_range_np = tendon_range.numpy() if tendon_range is not None else None
tendon_actuator_force_limited = getattr(mujoco_attrs, "tendon_actuator_force_limited", None)
tendon_actuator_force_limited_np = (
tendon_actuator_force_limited.numpy() if tendon_actuator_force_limited is not None else None
)
tendon_actuator_force_range = getattr(mujoco_attrs, "tendon_actuator_force_range", None)
tendon_actuator_force_range_np = (
tendon_actuator_force_range.numpy() if tendon_actuator_force_range is not None else None
)
tendon_margin = getattr(mujoco_attrs, "tendon_margin", None)
tendon_margin_np = tendon_margin.numpy() if tendon_margin is not None else None
tendon_solref_limit = getattr(mujoco_attrs, "tendon_solref_limit", None)
tendon_solref_limit_np = tendon_solref_limit.numpy() if tendon_solref_limit is not None else None
tendon_solimp_limit = getattr(mujoco_attrs, "tendon_solimp_limit", None)
tendon_solimp_limit_np = tendon_solimp_limit.numpy() if tendon_solimp_limit is not None else None
tendon_solref_friction = getattr(mujoco_attrs, "tendon_solref_friction", None)
tendon_solref_friction_np = tendon_solref_friction.numpy() if tendon_solref_friction is not None else None
tendon_solimp_friction = getattr(mujoco_attrs, "tendon_solimp_friction", None)
tendon_solimp_friction_np = tendon_solimp_friction.numpy() if tendon_solimp_friction is not None else None
tendon_armature = getattr(mujoco_attrs, "tendon_armature", None)
tendon_armature_np = tendon_armature.numpy() if tendon_armature is not None else None
tendon_springlength = getattr(mujoco_attrs, "tendon_springlength", None)
tendon_springlength_np = tendon_springlength.numpy() if tendon_springlength is not None else None
tendon_label_arr = getattr(mujoco_attrs, "tendon_label", None)
# Fixed tendon arrays
tendon_joint_adr = mujoco_attrs.tendon_joint_adr.numpy()
tendon_joint_num = mujoco_attrs.tendon_joint_num.numpy()
tendon_joint = mujoco_attrs.tendon_joint.numpy() if joint_entry_count > 0 else None
tendon_coef = mujoco_attrs.tendon_coef.numpy() if joint_entry_count > 0 else None
# Spatial tendon wrap path arrays
tendon_wrap_adr_np = mujoco_attrs.tendon_wrap_adr.numpy() if wrap_entry_count > 0 else None
tendon_wrap_num_np = mujoco_attrs.tendon_wrap_num.numpy() if wrap_entry_count > 0 else None
tendon_wrap_type_np = mujoco_attrs.tendon_wrap_type.numpy() if wrap_entry_count > 0 else None
tendon_wrap_shape_np = mujoco_attrs.tendon_wrap_shape.numpy() if wrap_entry_count > 0 else None
tendon_wrap_sidesite_np = mujoco_attrs.tendon_wrap_sidesite.numpy() if wrap_entry_count > 0 else None
tendon_wrap_prm_np = mujoco_attrs.tendon_wrap_prm.numpy() if wrap_entry_count > 0 else None
model_joint_type_np = model.joint_type.numpy()
# Track which Newton tendon indices are added to MuJoCo and their names
selected_tendons: list[int] = []
tendon_names: list[str] = []
used_tendon_names: set[str] = set()
for i in range(tendon_count):
# Only include tendons from the template world or global tendons (world < 0)
tw = int(tendon_world[i])
if tw != template_world and tw >= 0:
continue
# Resolve tendon label early so it can be included in warnings.
tendon_label = ""
if isinstance(tendon_label_arr, list) and i < len(tendon_label_arr):
tendon_label = str(tendon_label_arr[i]).strip()
if tendon_label == "":
tendon_label = f"tendon_{i}"
ttype = int(tendon_type_np[i]) if tendon_type_np is not None else 0
# Pre-validate wrapping path before creating the tendon in the spec.
if ttype == 0:
# Fixed tendon: build joint wraps list
joint_start = int(tendon_joint_adr[i])
joint_num = int(tendon_joint_num[i])
if joint_num <= 0:
if wp.config.verbose:
print(f"Warning: Skipping tendon {i} during MuJoCo export because it has no joint wraps.")
continue
if joint_start < 0 or joint_start + joint_num > joint_entry_count:
warnings.warn(
f"Skipping fixed tendon '{tendon_label}': joint range "
f"[{joint_start}, {joint_start + joint_num}) "
f"out of bounds for joint entries ({joint_entry_count}).",
stacklevel=2,
)
continue
fixed_wraps: list[tuple[str, float]] = []
for j in range(joint_start, joint_start + joint_num):
if tendon_joint is None or tendon_coef is None:
break
newton_joint = int(tendon_joint[j])
coef = float(tendon_coef[j])
if newton_joint < 0:
warnings.warn(
f"Skipping joint entry {j} for tendon {i}: invalid joint index {newton_joint}.",
stacklevel=2,
)
continue
if model_joint_type_np[newton_joint] == JointType.D6:
warnings.warn(
f"Skipping joint entry {j} for tendon {i}: invalid D6 joint type {newton_joint}.",
stacklevel=2,
)
continue
joint_name = joint_mapping.get(newton_joint)
if joint_name is None:
warnings.warn(
f"Skipping joint entry {j} for tendon {i}: Newton joint {newton_joint} "
f"not found in MuJoCo joint mapping.",
stacklevel=2,
)
continue
fixed_wraps.append((joint_name, coef))
if len(fixed_wraps) == 0:
if wp.config.verbose:
print(
f"Warning: Skipping tendon {i} during MuJoCo export "
"because no valid joint wraps were resolved."
)
continue
elif ttype == 1:
# Spatial tendon: validate wrap path arrays and bounds
if tendon_wrap_adr_np is None or tendon_wrap_num_np is None:
warnings.warn(
f"Spatial tendon '{tendon_label}' has no wrap path arrays, skipping.",
stacklevel=2,
)
continue
wrap_start = int(tendon_wrap_adr_np[i])
wrap_num = int(tendon_wrap_num_np[i])
if wrap_start < 0 or wrap_num <= 0 or wrap_start + wrap_num > wrap_entry_count:
warnings.warn(
f"Skipping spatial tendon '{tendon_label}': wrap range "
f"[{wrap_start}, {wrap_start + wrap_num}) "
f"out of bounds for wrap entries ({wrap_entry_count}).",
stacklevel=2,
)
continue
# Pre-resolve all wrap elements; skip entire tendon if any element fails
spatial_wraps_valid = True
for w in range(wrap_start, wrap_start + wrap_num):
if tendon_wrap_type_np is None or tendon_wrap_shape_np is None:
spatial_wraps_valid = False
break
wtype = int(tendon_wrap_type_np[w])
if wtype == 0: # site
if site_mapping.get(int(tendon_wrap_shape_np[w])) is None:
warnings.warn(
f"Skipping spatial tendon '{tendon_label}': wrap site at index {w} "
f"(shape {int(tendon_wrap_shape_np[w])}) not in site mapping.",
stacklevel=2,
)
spatial_wraps_valid = False
break
elif wtype == 1: # geom
if shape_mapping.get(int(tendon_wrap_shape_np[w])) is None:
warnings.warn(
f"Skipping spatial tendon '{tendon_label}': wrap geom at index {w} "
f"(shape {int(tendon_wrap_shape_np[w])}) not in shape mapping.",
stacklevel=2,
)
spatial_wraps_valid = False
break
elif wtype == 2: # pulley
divisor = float(tendon_wrap_prm_np[w]) if tendon_wrap_prm_np is not None else 0.0
if divisor <= 0.0:
warnings.warn(
f"Skipping spatial tendon '{tendon_label}': pulley at index {w} "
f"has non-positive divisor {divisor}.",
stacklevel=2,
)
spatial_wraps_valid = False
break
else:
warnings.warn(
f"Skipping spatial tendon '{tendon_label}': unknown wrap type {wtype} at index {w}.",
stacklevel=2,
)
spatial_wraps_valid = False
break
if not spatial_wraps_valid:
continue
else:
warnings.warn(f"Skipping tendon '{tendon_label}': unknown tendon type {ttype}.", stacklevel=2)
continue
# Track this tendon only after confirming it can be exported.
selected_tendons.append(i)
# Use the label resolved earlier; ensure unique names for the spec.
tendon_name = tendon_label
suffix = 1
while tendon_name in used_tendon_names:
tendon_name = f"{tendon_label}_{suffix}"
suffix += 1
used_tendon_names.add(tendon_name)
tendon_names.append(tendon_name)
t = spec.add_tendon()
t.name = tendon_name
# Set tendon properties (shared between fixed and spatial)
if tendon_stiffness_np is not None:
t.stiffness = float(tendon_stiffness_np[i])
if tendon_damping_np is not None:
t.damping = float(tendon_damping_np[i])
if tendon_frictionloss_np is not None:
t.frictionloss = float(tendon_frictionloss_np[i])
if tendon_limited_np is not None:
t.limited = int(tendon_limited_np[i])
if tendon_range_np is not None:
t.range = tendon_range_np[i].tolist()
if tendon_actuator_force_limited_np is not None:
t.actfrclimited = int(tendon_actuator_force_limited_np[i])
if tendon_actuator_force_range_np is not None:
t.actfrcrange = tendon_actuator_force_range_np[i].tolist()
if tendon_margin_np is not None:
t.margin = float(tendon_margin_np[i])
if tendon_armature_np is not None:
t.armature = float(tendon_armature_np[i])
if tendon_solref_limit_np is not None:
t.solref_limit = tendon_solref_limit_np[i].tolist()
if tendon_solimp_limit_np is not None:
t.solimp_limit = tendon_solimp_limit_np[i].tolist()
if tendon_solref_friction_np is not None:
t.solref_friction = tendon_solref_friction_np[i].tolist()
if tendon_solimp_friction_np is not None:
t.solimp_friction = tendon_solimp_friction_np[i].tolist()
if tendon_springlength_np is not None:
val = tendon_springlength_np[i]
has_automatic_length_computation = val[0] == -1.0
has_dead_zone = val[1] >= val[0]
if has_automatic_length_computation:
t.springlength[0] = -1.0
t.springlength[1] = -1.0
elif has_dead_zone:
t.springlength[0] = val[0]
t.springlength[1] = val[1]
else:
t.springlength[0] = val[0]
t.springlength[1] = val[0]
# Add wrapping path (all elements pre-validated above)
if ttype == 0:
for joint_name, coef in fixed_wraps:
t.wrap_joint(joint_name, coef)
elif ttype == 1:
for w in range(wrap_start, wrap_start + wrap_num):
wtype = int(tendon_wrap_type_np[w])
if wtype == 0:
t.wrap_site(site_mapping[int(tendon_wrap_shape_np[w])])
elif wtype == 1:
geom_name = shape_mapping[int(tendon_wrap_shape_np[w])]
sidesite_name = ""
if tendon_wrap_sidesite_np is not None:
sidesite_idx = int(tendon_wrap_sidesite_np[w])
if sidesite_idx >= 0:
sidesite_name = site_mapping.get(sidesite_idx)
if sidesite_name is None:
warnings.warn(
f"Wrap geom {w} for tendon {i} references sidesite "
f"{sidesite_idx} not in site mapping; ignoring sidesite.",
stacklevel=2,
)
sidesite_name = ""
t.wrap_geom(geom_name, sidesite_name)
elif wtype == 2:
t.wrap_pulley(float(tendon_wrap_prm_np[w]))
# else: unknown wtype — already rejected during pre-validation
return selected_tendons, tendon_names
def _init_actuators(
self,
model: Model,
spec: Any,
template_world: int,
actuator_args: dict[str, Any],
mjc_actuator_ctrl_source_list: list[int],
mjc_actuator_to_newton_idx_list: list[int],
dof_to_mjc_joint: np.ndarray,
mjc_joint_names: list[str],
selected_tendons: list[int],
mjc_tendon_names: list[str],
body_name_mapping: dict[int, str],
) -> int:
"""Initialize MuJoCo general actuators from custom attributes.
Only processes CTRL_DIRECT actuators (motor, general, etc.) from the
mujoco:actuator custom attributes. JOINT_TARGET actuators (position/velocity)
are handled separately in the joint iteration loop.
For CTRL_DIRECT actuators targeting joints, this method uses the DOF index
stored in actuator_trnid (see import_mjcf.py) to look up the correct MuJoCo
joint name. This is necessary because Newton may combine multiple MJCF joints
into one, but MuJoCo needs the specific joint name (e.g., "joint_ang1" not "joint").
Args:
model: The Newton model.
spec: The MuJoCo spec to add actuators to.
template_world: The world index to use as the template.
actuator_args: Default actuator arguments.
mjc_actuator_ctrl_source_list: List to append control sources to.
mjc_actuator_to_newton_idx_list: List to append Newton indices to.
dof_to_mjc_joint: Mapping from Newton DOF index to MuJoCo joint index.
Used to resolve CTRL_DIRECT joint actuators to their MuJoCo targets.
mjc_joint_names: List of MuJoCo joint names indexed by MuJoCo joint index.
Used together with dof_to_mjc_joint to get the correct joint name.
body_name_mapping: Mapping from Newton body index to de-duplicated MuJoCo body name
Returns:
int: Number of actuators added.
"""
mujoco = self._mujoco
mujoco_attrs = getattr(model, "mujoco", None)
mujoco_actuator_count = model.custom_frequency_counts.get("mujoco:actuator", 0)
if mujoco_actuator_count == 0 or mujoco_attrs is None or not hasattr(mujoco_attrs, "actuator_trnid"):
return 0
actuator_count = 0
# actuator_trnid[:,0] is the target index, actuator_trntype determines its meaning
actuator_trnid = mujoco_attrs.actuator_trnid.numpy()
trntype_arr = mujoco_attrs.actuator_trntype.numpy() if hasattr(mujoco_attrs, "actuator_trntype") else None
ctrl_source_arr = mujoco_attrs.ctrl_source.numpy() if hasattr(mujoco_attrs, "ctrl_source") else None
actuator_world_arr = mujoco_attrs.actuator_world.numpy() if hasattr(mujoco_attrs, "actuator_world") else None
actuator_target_label_arr = getattr(mujoco_attrs, "actuator_target_label", None)
joint_dof_label_arr = getattr(mujoco_attrs, "joint_dof_label", None)
tendon_label_arr = getattr(mujoco_attrs, "tendon_label", None)
def resolve_target_from_label(target_label: str) -> tuple[int, int]:
if isinstance(joint_dof_label_arr, list):
try:
return int(SolverMuJoCo.TrnType.JOINT), joint_dof_label_arr.index(target_label)
except ValueError:
pass
if isinstance(tendon_label_arr, list):
try:
return int(SolverMuJoCo.TrnType.TENDON), tendon_label_arr.index(target_label)
except ValueError:
pass
return -1, -1
# Pre-fetch range/limited arrays to avoid per-element .numpy() calls
has_ctrlrange_arr = (
mujoco_attrs.actuator_has_ctrlrange.numpy() if hasattr(mujoco_attrs, "actuator_has_ctrlrange") else None
)
ctrlrange_arr = mujoco_attrs.actuator_ctrlrange.numpy() if hasattr(mujoco_attrs, "actuator_ctrlrange") else None
ctrllimited_arr = (
mujoco_attrs.actuator_ctrllimited.numpy() if hasattr(mujoco_attrs, "actuator_ctrllimited") else None
)
has_forcerange_arr = (
mujoco_attrs.actuator_has_forcerange.numpy() if hasattr(mujoco_attrs, "actuator_has_forcerange") else None
)
forcerange_arr = (
mujoco_attrs.actuator_forcerange.numpy() if hasattr(mujoco_attrs, "actuator_forcerange") else None
)
forcelimited_arr = (
mujoco_attrs.actuator_forcelimited.numpy() if hasattr(mujoco_attrs, "actuator_forcelimited") else None
)
has_actrange_arr = (
mujoco_attrs.actuator_has_actrange.numpy() if hasattr(mujoco_attrs, "actuator_has_actrange") else None
)
actrange_arr = mujoco_attrs.actuator_actrange.numpy() if hasattr(mujoco_attrs, "actuator_actrange") else None
actlimited_arr = (
mujoco_attrs.actuator_actlimited.numpy() if hasattr(mujoco_attrs, "actuator_actlimited") else None
)
for mujoco_act_idx in range(mujoco_actuator_count):
# Skip JOINT_TARGET actuators - they're already added via joint_target_mode path
if ctrl_source_arr is not None:
ctrl_source = int(ctrl_source_arr[mujoco_act_idx])
if ctrl_source == SolverMuJoCo.CtrlSource.JOINT_TARGET:
continue # Already handled in joint iteration
# Only include actuators from the first world (template) or global actuators
if actuator_world_arr is not None:
actuator_world = int(actuator_world_arr[mujoco_act_idx])
if actuator_world != template_world and actuator_world != -1:
continue # Skip actuators from other worlds
target_idx = int(actuator_trnid[mujoco_act_idx, 0])
target_idx_alt = int(actuator_trnid[mujoco_act_idx, 1])
# Determine target type from trntype enum (JOINT, TENDON, SITE, BODY, ...).
trntype = int(trntype_arr[mujoco_act_idx]) if trntype_arr is not None else 0
target_label = ""
if isinstance(actuator_target_label_arr, list) and mujoco_act_idx < len(actuator_target_label_arr):
target_label = actuator_target_label_arr[mujoco_act_idx]
# Backward compatibility for older USD parsing that wrote tendon index to trnid[1].
if trntype == int(SolverMuJoCo.TrnType.TENDON):
if target_idx < 0 and target_idx_alt >= 0:
target_idx = target_idx_alt
elif target_idx == 0 and target_idx_alt > 0:
target_idx = target_idx_alt
# Deferred target resolution: when USD parsing ran before tendon rows were available,
# keep actuator_target_label and resolve the final (type, index) here.
if target_idx < 0 and target_label:
resolved_type, resolved_idx = resolve_target_from_label(target_label)
if resolved_idx >= 0:
trntype = resolved_type
target_idx = resolved_idx
if target_idx < 0:
warnings.warn(
f"MuJoCo actuator {mujoco_act_idx} has unresolved target '{target_label}'. Skipping actuator.",
stacklevel=2,
)
continue
if trntype == int(SolverMuJoCo.TrnType.JOINT):
# For CTRL_DIRECT joint actuators, actuator_trnid stores a DOF index
# (not a Newton joint index). This allows us to find the specific MuJoCo
# joint when Newton has combined multiple MJCF joints into one.
dof_idx = target_idx
dofs_per_world = len(dof_to_mjc_joint)
if dof_idx < 0 or dof_idx >= dofs_per_world:
if wp.config.verbose:
print(f"Warning: MuJoCo actuator {mujoco_act_idx} has invalid DOF target {dof_idx}")
continue
mjc_joint_idx = dof_to_mjc_joint[dof_idx]
if mjc_joint_idx < 0 or mjc_joint_idx >= len(mjc_joint_names):
if wp.config.verbose:
print(f"Warning: MuJoCo actuator {mujoco_act_idx} DOF {dof_idx} not mapped to MuJoCo joint")
continue
target_name = mjc_joint_names[mjc_joint_idx]
elif trntype == int(SolverMuJoCo.TrnType.TENDON):
try:
mjc_tendon_idx = selected_tendons.index(target_idx)
target_name = mjc_tendon_names[mjc_tendon_idx]
except (ValueError, IndexError):
if wp.config.verbose:
print(f"Warning: MuJoCo actuator {mujoco_act_idx} references tendon {target_idx} not in MuJoCo")
continue
elif trntype == int(SolverMuJoCo.TrnType.BODY):
if target_idx < 0 or target_idx >= len(model.body_label):
if wp.config.verbose:
print(f"Warning: MuJoCo actuator {mujoco_act_idx} has invalid body target {target_idx}")
continue
target_name = body_name_mapping.get(target_idx)
if target_name is None:
if wp.config.verbose:
print(
f"Warning: MuJoCo actuator {mujoco_act_idx} references body {target_idx} "
"not present in the MuJoCo export."
)
continue
else:
# TODO: Support site, slidercrank, and jointinparent transmission types
if wp.config.verbose:
print(f"Warning: MuJoCo actuator {mujoco_act_idx} has unsupported trntype {trntype}")
continue
general_args = dict(actuator_args)
# Get custom attributes for this MuJoCo actuator
if hasattr(mujoco_attrs, "actuator_gainprm"):
gainprm = mujoco_attrs.actuator_gainprm.numpy()[mujoco_act_idx]
general_args["gainprm"] = list(gainprm) # All 10 elements
if hasattr(mujoco_attrs, "actuator_biasprm"):
biasprm = mujoco_attrs.actuator_biasprm.numpy()[mujoco_act_idx]
general_args["biasprm"] = list(biasprm) # All 10 elements
if hasattr(mujoco_attrs, "actuator_dynprm"):
dynprm = mujoco_attrs.actuator_dynprm.numpy()[mujoco_act_idx]
general_args["dynprm"] = list(dynprm) # All 10 elements
if hasattr(mujoco_attrs, "actuator_gear"):
gear_arr = mujoco_attrs.actuator_gear.numpy()[mujoco_act_idx]
general_args["gear"] = list(gear_arr)
if hasattr(mujoco_attrs, "actuator_cranklength"):
cranklength = float(mujoco_attrs.actuator_cranklength.numpy()[mujoco_act_idx])
general_args["cranklength"] = cranklength
# Only pass range to MuJoCo when explicitly set in MJCF (has_*range flags),
# so MuJoCo can correctly resolve auto-limited flags via spec.compiler.autolimits.
if has_ctrlrange_arr is not None and has_ctrlrange_arr[mujoco_act_idx]:
general_args["ctrlrange"] = tuple(ctrlrange_arr[mujoco_act_idx])
if ctrllimited_arr is not None:
general_args["ctrllimited"] = int(ctrllimited_arr[mujoco_act_idx])
if has_forcerange_arr is not None and has_forcerange_arr[mujoco_act_idx]:
general_args["forcerange"] = tuple(forcerange_arr[mujoco_act_idx])
if forcelimited_arr is not None:
general_args["forcelimited"] = int(forcelimited_arr[mujoco_act_idx])
if has_actrange_arr is not None and has_actrange_arr[mujoco_act_idx]:
general_args["actrange"] = tuple(actrange_arr[mujoco_act_idx])
if actlimited_arr is not None:
general_args["actlimited"] = int(actlimited_arr[mujoco_act_idx])
if hasattr(mujoco_attrs, "actuator_actearly"):
actearly = mujoco_attrs.actuator_actearly.numpy()[mujoco_act_idx]
general_args["actearly"] = bool(actearly)
if hasattr(mujoco_attrs, "actuator_actdim"):
actdim = mujoco_attrs.actuator_actdim.numpy()[mujoco_act_idx]
if actdim >= 0: # -1 means auto
general_args["actdim"] = int(actdim)
if hasattr(mujoco_attrs, "actuator_dyntype"):
dyntype = int(mujoco_attrs.actuator_dyntype.numpy()[mujoco_act_idx])
general_args["dyntype"] = dyntype
if hasattr(mujoco_attrs, "actuator_gaintype"):
gaintype = int(mujoco_attrs.actuator_gaintype.numpy()[mujoco_act_idx])
general_args["gaintype"] = gaintype
if hasattr(mujoco_attrs, "actuator_biastype"):
biastype = int(mujoco_attrs.actuator_biastype.numpy()[mujoco_act_idx])
general_args["biastype"] = biastype
# Detect position/velocity actuator shortcuts. Use set_to_position/
# set_to_velocity after add_actuator so MuJoCo's compiler computes kd
# from dampratio via mj_setConst (kd = dampratio * 2 * sqrt(kp * acc0)).
shortcut = None # "position" or "velocity" if detected
shortcut_args: dict[str, float] = {}
if general_args.get("biastype") == mujoco.mjtBias.mjBIAS_AFFINE and general_args.get("gainprm", [0])[0] > 0:
kp = general_args["gainprm"][0]
bp = general_args.get("biasprm", [0, 0, 0])
# Position shortcut: biasprm = [0, -kp, -kv]
# A positive biasprm[2] indicates a dampratio placeholder
if bp[0] == 0 and abs(bp[1] + kp) < 1e-8:
shortcut = "position"
shortcut_args["kp"] = kp
if bp[2] < 0.0:
shortcut_args["kv"] = -bp[2]
elif bp[2] > 0.0:
shortcut_args["dampratio"] = bp[2]
for key in ("biasprm", "biastype", "gainprm", "gaintype"):
general_args.pop(key, None)
# Velocity shortcut: biasprm = [0, 0, -kv] where kv = gainprm[0]
elif bp[0] == 0 and bp[1] == 0 and bp[2] != 0:
kv = general_args["gainprm"][0]
if abs(bp[2] + kv) < 1e-8:
shortcut = "velocity"
shortcut_args["kv"] = kv
for key in ("biasprm", "biastype", "gainprm", "gaintype"):
general_args.pop(key, None)
# Map trntype integer to MuJoCo enum and override default in general_args
trntype_enum = {
0: mujoco.mjtTrn.mjTRN_JOINT,
1: mujoco.mjtTrn.mjTRN_JOINTINPARENT,
2: mujoco.mjtTrn.mjTRN_TENDON,
3: mujoco.mjtTrn.mjTRN_SITE,
4: mujoco.mjtTrn.mjTRN_BODY,
5: mujoco.mjtTrn.mjTRN_SLIDERCRANK,
}.get(trntype, mujoco.mjtTrn.mjTRN_JOINT)
general_args["trntype"] = trntype_enum
act = spec.add_actuator(target=target_name, **general_args)
if shortcut == "position":
act.set_to_position(**shortcut_args)
elif shortcut == "velocity":
act.set_to_velocity(**shortcut_args)
# CTRL_DIRECT actuators - store MJCF-order index into control.mujoco.ctrl
# mujoco_act_idx is the index in Newton's mujoco:actuator frequency (MJCF order)
mjc_actuator_ctrl_source_list.append(1) # CTRL_DIRECT
mjc_actuator_to_newton_idx_list.append(mujoco_act_idx)
actuator_count += 1
return actuator_count
def __init__(
self,
model: Model,
*,
separate_worlds: bool | None = None,
njmax: int | None = None,
nconmax: int | None = None,
iterations: int | None = None,
ls_iterations: int | None = None,
ccd_iterations: int | None = None,
sdf_iterations: int | None = None,
sdf_initpoints: int | None = None,
solver: int | str | None = None,
integrator: int | str | None = None,
cone: int | str | None = None,
jacobian: int | str | None = None,
impratio: float | None = None,
tolerance: float | None = None,
ls_tolerance: float | None = None,
ccd_tolerance: float | None = None,
density: float | None = None,
viscosity: float | None = None,
wind: tuple | None = None,
magnetic: tuple | None = None,
use_mujoco_cpu: bool = False,
disable_contacts: bool = False,
update_data_interval: int = 1,
save_to_mjcf: str | None = None,
ls_parallel: bool = False,
use_mujoco_contacts: bool = True,
include_sites: bool = True,
skip_visual_only_geoms: bool = True,
):
"""
Solver options (e.g., ``impratio``) follow this resolution priority:
1. **Constructor argument** - If provided, same value is used for all worlds.
2. **Newton model custom attribute** (``model.mujoco.``) - Supports per-world values.
3. **MuJoCo default** - Used if neither of the above is set.
Args:
model: The model to be simulated.
separate_worlds: If True, each Newton world is mapped to a separate MuJoCo world. Defaults to `not use_mujoco_cpu`.
njmax: Maximum number of constraints per world. If None, a default value is estimated from the initial state. Note that the larger of the user-provided value or the default value is used.
nconmax: Number of contact points per world. If None, a default value is estimated from the initial state. Note that the larger of the user-provided value or the default value is used.
iterations: Number of solver iterations. If None, uses model custom attribute or MuJoCo's default (100).
ls_iterations: Number of line search iterations for the solver. If None, uses model custom attribute or MuJoCo's default (50).
ccd_iterations: Maximum CCD iterations. If None, uses model custom attribute or MuJoCo's default (35).
sdf_iterations: Maximum SDF iterations. If None, uses model custom attribute or MuJoCo's default (10).
sdf_initpoints: Number of SDF initialization points. If None, uses model custom attribute or MuJoCo's default (40).
solver: Solver type. Can be "cg" or "newton", or their corresponding MuJoCo integer constants. If None, uses model custom attribute or Newton's default ("newton").
integrator: Integrator type. Can be "euler", "rk4", or "implicitfast", or their corresponding MuJoCo integer constants. If None, uses model custom attribute or Newton's default ("implicitfast").
cone: The type of contact friction cone. Can be "pyramidal", "elliptic", or their corresponding MuJoCo integer constants. If None, uses model custom attribute or Newton's default ("pyramidal").
jacobian: Jacobian computation method. Can be "dense", "sparse", or "auto", or their corresponding MuJoCo integer constants. If None, uses model custom attribute or MuJoCo's default ("auto").
impratio: Frictional-to-normal constraint impedance ratio. If None, uses model custom attribute or MuJoCo's default (1.0).
tolerance: Solver tolerance for early termination. If None, uses model custom attribute or MuJoCo's default (1e-8).
ls_tolerance: Line search tolerance for early termination. If None, uses model custom attribute or MuJoCo's default (0.01).
ccd_tolerance: Continuous collision detection tolerance. If None, uses model custom attribute or MuJoCo's default (1e-6).
density: Medium density for lift and drag forces. If None, uses model custom attribute or MuJoCo's default (0.0).
viscosity: Medium viscosity for lift and drag forces. If None, uses model custom attribute or MuJoCo's default (0.0).
wind: Wind velocity vector (x, y, z) for lift and drag forces. If None, uses model custom attribute or MuJoCo's default (0, 0, 0).
magnetic: Global magnetic flux vector (x, y, z). If None, uses model custom attribute or MuJoCo's default (0, -0.5, 0).
use_mujoco_cpu: If True, use the MuJoCo-C CPU backend instead of `mujoco_warp`.
disable_contacts: If True, disable contact computation in MuJoCo.
update_data_interval: Frequency (in simulation steps) at which to update the MuJoCo Data object from the Newton state. If 0, Data is never updated after initialization.
save_to_mjcf: Optional path to save the generated MJCF model file.
ls_parallel: If True, enable parallel line search in MuJoCo. Defaults to False.
use_mujoco_contacts: If True, use the MuJoCo contact solver. If False, use the Newton contact solver (newton contacts must be passed in through the step function in that case).
include_sites: If ``True`` (default), Newton shapes marked with ``ShapeFlags.SITE`` are exported as MuJoCo sites. Sites are non-colliding reference points used for sensor attachment, debugging, or as frames of reference. If ``False``, sites are skipped during export. Defaults to ``True``.
skip_visual_only_geoms: If ``True`` (default), geometries used only for visualization (i.e. not involved in collision) are excluded from the exported MuJoCo spec. This avoids mismatches with models that use explicit ```` definitions for collision geometry.
"""
super().__init__(model)
# Import and cache MuJoCo modules (only happens once per class)
mujoco, _ = self.import_mujoco()
# Deferred from module scope: wp.static() in this kernel imports mujoco_warp.
if SolverMuJoCo._convert_mjw_contacts_to_newton_kernel is None:
SolverMuJoCo._convert_mjw_contacts_to_newton_kernel = create_convert_mjw_contacts_to_newton_kernel()
# --- New unified mappings: MuJoCo[world, entity] -> Newton[entity] ---
self.mjc_body_to_newton: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, body] to Newton body index. Shape [nworld, nbody], dtype int32."""
self.mjc_geom_to_newton_shape: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, geom] to Newton shape index. Shape [nworld, ngeom], dtype int32."""
self.mjc_jnt_to_newton_jnt: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, joint] to Newton joint index. Shape [nworld, njnt], dtype int32."""
self.mjc_jnt_to_newton_dof: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, joint] to Newton DOF index. Shape [nworld, njnt], dtype int32."""
self.mjc_dof_to_newton_dof: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, dof] to Newton DOF index. Shape [nworld, nv], dtype int32."""
self.newton_dof_to_body: wp.array[wp.int32] | None = None
"""Mapping from Newton DOF index to child body index. Shape [joint_dof_count], dtype int32."""
self.mjc_mocap_to_newton_jnt: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, mocap] to Newton joint index. Shape [nworld, nmocap], dtype int32."""
self.mjc_actuator_ctrl_source: wp.array[wp.int32] | None = None
"""Control source for each MuJoCo actuator.
Values: 0=JOINT_TARGET (uses joint_target_pos/vel), 1=CTRL_DIRECT (uses mujoco.ctrl)
Shape [nu], dtype int32."""
self.mjc_actuator_to_newton_idx: wp.array[wp.int32] | None = None
"""Mapping from MuJoCo actuator to Newton index.
For JOINT_TARGET: sign-encoded DOF index (>=0: position, -1: unmapped, <=-2: velocity with -(idx+2))
For CTRL_DIRECT: MJCF-order index into control.mujoco.ctrl array
Shape [nu], dtype int32."""
self.mjc_eq_to_newton_eq: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, eq] to Newton equality constraint index.
Corresponds to the equality constraints that are created in MuJoCo from Newton's equality constraints.
A value of -1 indicates that the MuJoCo equality constraint has been created from a Newton joint, see :attr:`mjc_eq_to_newton_jnt`
for the corresponding joint index.
Shape [nworld, neq], dtype int32."""
self.mjc_eq_to_newton_jnt: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, eq] to Newton joint index.
Corresponds to the equality constraints that are created in MuJoCo from Newton joints that have no associated articulation,
i.e. where :attr:`newton.Model.joint_articulation` is -1 for the joint which results in 2 equality constraints being created in MuJoCo.
A value of -1 indicates that the MuJoCo equality constraint is not associated with a Newton joint but an explicitly created Newton equality constraint,
see :attr:`mjc_eq_to_newton_eq` for the corresponding equality constraint index.
Shape [nworld, neq], dtype int32."""
self.mjc_eq_to_newton_mimic: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, eq] to Newton mimic constraint index.
Corresponds to the equality constraints that are created in MuJoCo from Newton's mimic constraints.
A value of -1 indicates that the MuJoCo equality constraint is not associated with a Newton mimic constraint.
Shape [nworld, neq], dtype int32."""
self.mjc_tendon_to_newton_tendon: wp.array2d[wp.int32] | None = None
"""Mapping from MuJoCo [world, tendon] to Newton tendon index.
Shape [nworld, ntendon], dtype int32."""
self.body_free_qd_start: wp.array[wp.int32] | None = None
"""Per-body mapping to the free-joint qd_start index (or -1 if not free)."""
# --- Conditional/lazy mappings ---
self.newton_shape_to_mjc_geom: wp.array[wp.int32] | None = None
"""Inverse mapping from Newton shape index to MuJoCo geom index. Only created when use_mujoco_contacts=False. Shape [nshape], dtype int32."""
# --- Helper arrays for actuator types ---
# --- Internal state for mapping creation ---
self._shapes_per_world: int = 0
"""Number of shapes per world (for computing Newton shape indices from template)."""
self._first_env_shape_base: int = 0
"""Base shape index for the first environment."""
self._viewer = None
"""Instance of the MuJoCo viewer for debugging."""
disableflags = 0
if disable_contacts:
disableflags |= mujoco.mjtDisableBit.mjDSBL_CONTACT
self.use_mujoco_cpu = use_mujoco_cpu
if separate_worlds is None:
separate_worlds = not use_mujoco_cpu and model.world_count > 1
with wp.ScopedTimer("convert_model_to_mujoco", active=False):
self._convert_to_mjc(
model,
disableflags=disableflags,
disable_contacts=disable_contacts,
separate_worlds=separate_worlds,
njmax=njmax,
nconmax=nconmax,
iterations=iterations,
ls_iterations=ls_iterations,
ccd_iterations=ccd_iterations,
sdf_iterations=sdf_iterations,
sdf_initpoints=sdf_initpoints,
cone=cone,
jacobian=jacobian,
impratio=impratio,
tolerance=tolerance,
ls_tolerance=ls_tolerance,
ccd_tolerance=ccd_tolerance,
density=density,
viscosity=viscosity,
wind=wind,
magnetic=magnetic,
solver=solver,
integrator=integrator,
target_filename=save_to_mjcf,
ls_parallel=ls_parallel,
include_sites=include_sites,
skip_visual_only_geoms=skip_visual_only_geoms,
)
self.update_data_interval = update_data_interval
self._step = 0
if self.mjw_model is not None:
self.mjw_model.opt.run_collision_detection = use_mujoco_contacts
@event_scope
def _mujoco_warp_step(self):
self._mujoco_warp.step(self.mjw_model, self.mjw_data)
@event_scope
@override
def step(self, state_in: State, state_out: State, control: Control, contacts: Contacts, dt: float) -> None:
if self.use_mujoco_cpu:
self._apply_mjc_control(self.model, state_in, control, self.mj_data)
if self.update_data_interval > 0 and self._step % self.update_data_interval == 0:
# XXX updating the mujoco state at every step may introduce numerical instability
self._update_mjc_data(self.mj_data, self.model, state_in)
self.mj_model.opt.timestep = dt
self._mujoco.mj_step(self.mj_model, self.mj_data)
self._update_newton_state(self.model, state_out, self.mj_data, state_prev=state_in)
else:
self._enable_rne_postconstraint(state_out)
self._apply_mjc_control(self.model, state_in, control, self.mjw_data)
if self.update_data_interval > 0 and self._step % self.update_data_interval == 0:
self._update_mjc_data(self.mjw_data, self.model, state_in)
self.mjw_model.opt.timestep.fill_(dt)
with wp.ScopedDevice(self.model.device):
if self.mjw_model.opt.run_collision_detection:
self._mujoco_warp_step()
else:
self._convert_contacts_to_mjwarp(self.model, state_in, contacts)
self._mujoco_warp_step()
self._update_newton_state(self.model, state_out, self.mjw_data, state_prev=state_in)
self._step += 1
def _enable_rne_postconstraint(self, state_out: State):
"""Request computation of RNE forces if required for state fields."""
rne_postconstraint_fields = {"body_qdd", "body_parent_f"}
# TODO: handle use_mujoco_cpu
m = self.mjw_model
if m.sensor_rne_postconstraint:
return
if any(getattr(state_out, field) is not None for field in rne_postconstraint_fields):
# required for cfrc_ext, cfrc_int, cacc
if wp.config.verbose:
print("Setting model.sensor_rne_postconstraint True")
m.sensor_rne_postconstraint = True
def _convert_contacts_to_mjwarp(self, model: Model, state_in: State, contacts: Contacts):
# Ensure the inverse shape mapping exists (lazy creation)
if self.newton_shape_to_mjc_geom is None:
self._create_inverse_shape_mapping()
# Zero nacon before the kernel — the kernel uses atomic_add to count
# only the contacts that survive immovable-pair filtering.
self.mjw_data.nacon.zero_()
bodies_per_world = self.model.body_count // self.model.world_count
wp.launch(
convert_newton_contacts_to_mjwarp_kernel,
dim=(contacts.rigid_contact_max,),
inputs=[
state_in.body_q,
model.shape_body,
model.body_flags,
self.mjw_model.geom_bodyid,
self.mjw_model.body_weldid,
self.mjw_model.geom_condim,
self.mjw_model.geom_priority,
self.mjw_model.geom_solmix,
self.mjw_model.geom_solref,
self.mjw_model.geom_solimp,
self.mjw_model.geom_friction,
self.mjw_model.geom_margin,
self.mjw_model.geom_gap,
# Newton contacts
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_normal,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
contacts.rigid_contact_stiffness,
contacts.rigid_contact_damping,
contacts.rigid_contact_friction,
model.shape_margin,
bodies_per_world,
self.newton_shape_to_mjc_geom,
# Mujoco warp contacts
self.mjw_data.naconmax,
self.mjw_data.nacon,
self.mjw_data.contact.dist,
self.mjw_data.contact.pos,
self.mjw_data.contact.frame,
self.mjw_data.contact.includemargin,
self.mjw_data.contact.friction,
self.mjw_data.contact.solref,
self.mjw_data.contact.solreffriction,
self.mjw_data.contact.solimp,
self.mjw_data.contact.dim,
self.mjw_data.contact.geom,
self.mjw_data.contact.efc_address,
self.mjw_data.contact.worldid,
# Data to clear
self.mjw_data.nworld,
self.mjw_data.ncollision,
],
device=model.device,
)
@override
def notify_model_changed(self, flags: int) -> None:
need_const_fixed = False
need_const_0 = False
need_length_range = False
if flags & SolverNotifyFlags.BODY_INERTIAL_PROPERTIES:
self._update_model_inertial_properties()
need_const_fixed = True
need_const_0 = True
if flags & SolverNotifyFlags.JOINT_PROPERTIES:
self._update_joint_properties()
if flags & SolverNotifyFlags.BODY_PROPERTIES:
self._update_body_properties()
need_const_0 = True
if flags & SolverNotifyFlags.JOINT_DOF_PROPERTIES:
self._update_joint_dof_properties()
need_const_0 = True
need_length_range = True
if flags & SolverNotifyFlags.SHAPE_PROPERTIES:
self._update_geom_properties()
self._update_pair_properties()
if flags & SolverNotifyFlags.MODEL_PROPERTIES:
self._update_model_properties()
if flags & SolverNotifyFlags.CONSTRAINT_PROPERTIES:
self._update_eq_properties()
self._update_mimic_eq_properties()
if flags & SolverNotifyFlags.TENDON_PROPERTIES:
self._update_tendon_properties()
need_const_0 = True
need_length_range = True
if flags & SolverNotifyFlags.ACTUATOR_PROPERTIES:
self._update_actuator_properties()
need_const_0 = True
need_length_range = True
if self.use_mujoco_cpu:
if flags & (SolverNotifyFlags.BODY_PROPERTIES | SolverNotifyFlags.JOINT_DOF_PROPERTIES):
self.mj_model.dof_armature[:] = self.mjw_model.dof_armature.numpy()[0]
self.mj_model.dof_frictionloss[:] = self.mjw_model.dof_frictionloss.numpy()[0]
self.mj_model.dof_damping[:] = self.mjw_model.dof_damping.numpy()[0]
self.mj_model.dof_solimp[:] = self.mjw_model.dof_solimp.numpy()[0]
self.mj_model.dof_solref[:] = self.mjw_model.dof_solref.numpy()[0]
self.mj_model.qpos0[:] = self.mjw_model.qpos0.numpy()[0]
self.mj_model.qpos_spring[:] = self.mjw_model.qpos_spring.numpy()[0]
if need_length_range or need_const_fixed or need_const_0:
self._mujoco.mj_setConst(self.mj_model, self.mj_data)
else:
if need_length_range or need_const_fixed or need_const_0:
with wp.ScopedDevice(self.model.device):
if need_length_range:
self._mujoco_warp.set_length_range(self.mjw_model, self.mjw_data)
if need_const_fixed:
self._mujoco_warp.set_const_fixed(self.mjw_model, self.mjw_data)
if need_const_0:
self._mujoco_warp.set_const_0(self.mjw_model, self.mjw_data)
def _create_inverse_shape_mapping(self):
"""
Create the inverse shape mapping (Newton shape -> MuJoCo [world, geom]).
This is lazily created only when use_mujoco_contacts=False.
"""
nworld = self.mjc_geom_to_newton_shape.shape[0]
ngeom = self.mjc_geom_to_newton_shape.shape[1]
# Create the inverse mapping array
self.newton_shape_to_mjc_geom = wp.full(self.model.shape_count, -1, dtype=wp.int32, device=self.model.device)
# Launch kernel to populate the inverse mapping
wp.launch(
create_inverse_shape_mapping_kernel,
dim=(nworld, ngeom),
inputs=[
self.mjc_geom_to_newton_shape,
],
outputs=[
self.newton_shape_to_mjc_geom,
],
device=self.model.device,
)
@staticmethod
def _data_is_mjwarp(data):
# Check if the data is a mujoco_warp Data object
return hasattr(data, "nworld")
def _apply_mjc_control(self, model: Model, state: State, control: Control | None, mj_data: MjWarpData | MjData):
if control is None or control.joint_f is None:
if state.body_f is None:
return
is_mjwarp = SolverMuJoCo._data_is_mjwarp(mj_data)
single_world_template = False
if is_mjwarp:
ctrl = mj_data.ctrl
qfrc = mj_data.qfrc_applied
xfrc = mj_data.xfrc_applied
nworld = mj_data.nworld
else:
effective_dof_count = model.joint_dof_count - self._total_loop_joint_dofs
single_world_template = len(mj_data.qfrc_applied) < effective_dof_count
ctrl = wp.zeros((1, len(mj_data.ctrl)), dtype=wp.float32, device=model.device)
qfrc = wp.zeros((1, len(mj_data.qfrc_applied)), dtype=wp.float32, device=model.device)
xfrc = wp.zeros((1, len(mj_data.xfrc_applied)), dtype=wp.spatial_vector, device=model.device)
nworld = 1
joints_per_world = (
model.joint_count // model.world_count if single_world_template else model.joint_count // nworld
)
if control is not None:
# Use instance arrays (built during MuJoCo model construction)
if self.mjc_actuator_ctrl_source is not None and self.mjc_actuator_to_newton_idx is not None:
nu = self.mjc_actuator_ctrl_source.shape[0]
dofs_per_world = model.joint_dof_count // nworld if nworld > 0 else model.joint_dof_count
# Get mujoco.ctrl (None if not available - won't be accessed if no CTRL_DIRECT actuators)
mujoco_ctrl_ns = getattr(control, "mujoco", None)
mujoco_ctrl = getattr(mujoco_ctrl_ns, "ctrl", None) if mujoco_ctrl_ns is not None else None
ctrls_per_world = mujoco_ctrl.shape[0] // nworld if mujoco_ctrl is not None and nworld > 0 else 0
wp.launch(
apply_mjc_control_kernel,
dim=(nworld, nu),
inputs=[
self.mjc_actuator_ctrl_source,
self.mjc_actuator_to_newton_idx,
control.joint_target_pos,
control.joint_target_vel,
mujoco_ctrl,
dofs_per_world,
ctrls_per_world,
],
outputs=[
ctrl,
],
device=model.device,
)
wp.launch(
apply_mjc_qfrc_kernel,
dim=(nworld, joints_per_world),
inputs=[
control.joint_f,
model.joint_type,
model.joint_child,
model.body_flags,
model.joint_qd_start,
model.joint_dof_dim,
joints_per_world,
self.mj_qd_start,
],
outputs=[
qfrc,
],
device=model.device,
)
if state.body_f is not None:
# Launch over MuJoCo bodies
nbody = self.mjc_body_to_newton.shape[1]
wp.launch(
apply_mjc_body_f_kernel,
dim=(nworld, nbody),
inputs=[
self.mjc_body_to_newton,
model.body_flags,
state.body_f,
],
outputs=[
xfrc,
],
device=model.device,
)
if control is not None and control.joint_f is not None:
# Free/DISTANCE joint forces are applied via xfrc_applied to preserve COM-wrench semantics.
nbody = self.mjc_body_to_newton.shape[1]
wp.launch(
apply_mjc_free_joint_f_to_body_f_kernel,
dim=(nworld, nbody),
inputs=[
self.mjc_body_to_newton,
model.body_flags,
self.body_free_qd_start,
control.joint_f,
],
outputs=[
xfrc,
],
device=model.device,
)
if not is_mjwarp:
mj_data.xfrc_applied = xfrc.numpy()
mj_data.ctrl[:] = ctrl.numpy().flatten()
mj_data.qfrc_applied[:] = qfrc.numpy()
def _update_mjc_data(self, mj_data: MjWarpData | MjData, model: Model, state: State | None = None):
is_mjwarp = SolverMuJoCo._data_is_mjwarp(mj_data)
single_world_template = False
if is_mjwarp:
# we have an MjWarp Data object
qpos = mj_data.qpos
qvel = mj_data.qvel
nworld = mj_data.nworld
else:
# we have an MjData object from Mujoco
effective_coord_count = model.joint_coord_count - self._total_loop_joint_coords
single_world_template = len(mj_data.qpos) < effective_coord_count
expected_qpos = (
effective_coord_count // model.world_count if single_world_template else effective_coord_count
)
assert len(mj_data.qpos) >= expected_qpos, (
f"MuJoCo qpos size ({len(mj_data.qpos)}) < expected joint coords ({expected_qpos})"
)
qpos = wp.empty((1, len(mj_data.qpos)), dtype=wp.float32, device=model.device)
qvel = wp.empty((1, len(mj_data.qvel)), dtype=wp.float32, device=model.device)
nworld = 1
if state is None:
joint_q = model.joint_q
joint_qd = model.joint_qd
else:
joint_q = state.joint_q
joint_qd = state.joint_qd
joints_per_world = (
model.joint_count // model.world_count if single_world_template else model.joint_count // nworld
)
mujoco_attrs = getattr(model, "mujoco", None)
dof_ref = getattr(mujoco_attrs, "dof_ref", None) if mujoco_attrs is not None else None
wp.launch(
convert_warp_coords_to_mj_kernel,
dim=(nworld, joints_per_world),
inputs=[
joint_q,
joint_qd,
joints_per_world,
model.joint_type,
model.joint_q_start,
model.joint_qd_start,
model.joint_dof_dim,
model.joint_child,
model.body_com,
dof_ref,
self.mj_q_start,
self.mj_qd_start,
],
outputs=[qpos, qvel],
device=model.device,
)
if not is_mjwarp:
mj_data.qpos[:] = qpos.numpy().flatten()[: len(mj_data.qpos)]
mj_data.qvel[:] = qvel.numpy().flatten()[: len(mj_data.qvel)]
def _update_newton_state(
self,
model: Model,
state: State,
mj_data: MjWarpData | MjData,
state_prev: State,
):
"""Update a Newton state from MuJoCo coordinates and kinematics.
Args:
model: Newton model that defines the joint and body layout.
state: Output Newton state to populate from MuJoCo data.
mj_data: MuJoCo runtime data source, either CPU `MjData` or Warp data.
state_prev: Previous Newton state. Kinematic joint coordinates and
velocities are copied from this state because MuJoCo does not
independently integrate those DOFs.
"""
is_mjwarp = SolverMuJoCo._data_is_mjwarp(mj_data)
single_world_template = False
if is_mjwarp:
# we have an MjWarp Data object
qpos = mj_data.qpos
qvel = mj_data.qvel
nworld = mj_data.nworld
else:
# we have an MjData object from Mujoco
effective_coord_count = model.joint_coord_count - self._total_loop_joint_coords
single_world_template = len(mj_data.qpos) < effective_coord_count
qpos = wp.array([mj_data.qpos], dtype=wp.float32, device=model.device)
qvel = wp.array([mj_data.qvel], dtype=wp.float32, device=model.device)
nworld = 1
joints_per_world = (
model.joint_count // model.world_count if single_world_template else model.joint_count // nworld
)
mujoco_attrs = getattr(model, "mujoco", None)
dof_ref = getattr(mujoco_attrs, "dof_ref", None) if mujoco_attrs is not None else None
wp.launch(
convert_mj_coords_to_warp_kernel,
dim=(nworld, joints_per_world),
inputs=[
qpos,
qvel,
joints_per_world,
model.joint_type,
model.joint_q_start,
model.joint_qd_start,
model.joint_dof_dim,
model.joint_child,
model.body_com,
dof_ref,
model.body_flags,
state_prev.joint_q,
state_prev.joint_qd,
self.mj_q_start,
self.mj_qd_start,
],
outputs=[state.joint_q, state.joint_qd],
device=model.device,
)
# custom forward kinematics for handling multi-dof joints
wp.launch(
kernel=eval_articulation_fk,
dim=model.articulation_count,
inputs=[
model.articulation_start,
model.joint_articulation,
state.joint_q,
state.joint_qd,
model.joint_q_start,
model.joint_qd_start,
model.joint_type,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_axis,
model.joint_dof_dim,
model.body_com,
],
outputs=[
state.body_q,
state.body_qd,
],
device=model.device,
)
# Update rigid force fields on state.
if state.body_qdd is not None or state.body_parent_f is not None:
# Launch over MuJoCo bodies
nbody = self.mjc_body_to_newton.shape[1]
wp.launch(
convert_rigid_forces_from_mj_kernel,
(nworld, nbody),
inputs=[
self.mjc_body_to_newton,
self.mjw_model.body_rootid,
self.mjw_model.opt.gravity,
self.mjw_data.xipos,
self.mjw_data.subtree_com,
self.mjw_data.cacc,
self.mjw_data.cvel,
self.mjw_data.cfrc_int,
],
outputs=[state.body_qdd, state.body_parent_f],
device=model.device,
)
# Update actuator forces in joint DOF space.
qfrc_actuator = getattr(getattr(state, "mujoco", None), "qfrc_actuator", None)
if qfrc_actuator is not None:
if is_mjwarp:
mjw_qfrc = mj_data.qfrc_actuator
mjw_qpos = mj_data.qpos
else:
mjw_qfrc = wp.array([mj_data.qfrc_actuator], dtype=wp.float32, device=model.device)
mjw_qpos = wp.array([mj_data.qpos], dtype=wp.float32, device=model.device)
wp.launch(
convert_qfrc_actuator_from_mj_kernel,
dim=(nworld, joints_per_world),
inputs=[
mjw_qfrc,
mjw_qpos,
joints_per_world,
model.joint_type,
model.joint_q_start,
model.joint_qd_start,
model.joint_dof_dim,
model.joint_child,
model.body_com,
self.mj_q_start,
self.mj_qd_start,
],
outputs=[qfrc_actuator],
device=model.device,
)
@staticmethod
def _find_body_collision_filter_pairs(
model: Model,
selected_bodies: np.ndarray,
colliding_shapes: np.ndarray,
):
"""For shape collision filter pairs, find body collision filter pairs that are contained within."""
body_exclude_pairs = []
shape_set = set(colliding_shapes)
body_shapes = {}
for body in selected_bodies:
shapes = model.body_shapes[body]
shapes = [s for s in shapes if s in shape_set]
body_shapes[body] = shapes
bodies_a, bodies_b = np.triu_indices(len(selected_bodies), k=1)
for body_a, body_b in zip(bodies_a, bodies_b, strict=True):
b1, b2 = selected_bodies[body_a], selected_bodies[body_b]
shapes_1 = body_shapes[b1]
shapes_2 = body_shapes[b2]
excluded = True
for shape_1 in shapes_1:
for shape_2 in shapes_2:
if shape_1 > shape_2:
s1, s2 = shape_2, shape_1
else:
s1, s2 = shape_1, shape_2
if (s1, s2) not in model.shape_collision_filter_pairs:
excluded = False
break
if excluded:
body_exclude_pairs.append((b1, b2))
return body_exclude_pairs
@staticmethod
def _color_collision_shapes(
model: Model, selected_shapes: np.ndarray, visualize_graph: bool = False, shape_labels: list[str] | None = None
) -> np.ndarray:
"""
Find a graph coloring of the collision filter pairs in the model.
Shapes within the same color cannot collide with each other.
Shapes can only collide with shapes of different colors.
Args:
model: The model to color the collision shapes of.
selected_shapes: The indices of the collision shapes to color.
visualize_graph: Whether to visualize the graph coloring.
shape_labels: The labels of the shapes, only used for visualization.
Returns:
np.ndarray: An integer array of shape (num_shapes,), where each element is the color of the corresponding shape.
"""
# we first create a mapping from selected shape to local color shape index
# to reduce the number of nodes in the graph to only the number of selected shapes
# without any gaps between the indices (otherwise we have to allocate max(selected_shapes) + 1 nodes)
to_color_shape_index = {}
for i, shape in enumerate(selected_shapes):
to_color_shape_index[shape] = i
# find graph coloring of collision filter pairs
num_shapes = len(selected_shapes)
shape_a, shape_b = np.triu_indices(num_shapes, k=1)
shape_collision_group_np = model.shape_collision_group.numpy()
cgroup = [shape_collision_group_np[i] for i in selected_shapes]
# edges representing colliding shape pairs
graph_edges = [
(i, j)
for i, j in zip(shape_a, shape_b, strict=True)
if (
(min(selected_shapes[i], selected_shapes[j]), max(selected_shapes[i], selected_shapes[j]))
not in model.shape_collision_filter_pairs
and (cgroup[i] == cgroup[j] or cgroup[i] == -1 or cgroup[j] == -1)
)
]
shape_color = np.zeros(model.shape_count, dtype=np.int32)
if len(graph_edges) > 0:
color_groups = color_graph(
num_nodes=num_shapes,
graph_edge_indices=wp.array(graph_edges, dtype=wp.int32),
balance_colors=False,
)
num_colors = 0
for group in color_groups:
num_colors += 1
shape_color[selected_shapes[group]] = num_colors
if visualize_graph:
plot_graph(
vertices=np.arange(num_shapes),
edges=graph_edges,
node_labels=[shape_labels[i] for i in selected_shapes] if shape_labels is not None else None,
node_colors=[shape_color[i] for i in selected_shapes],
)
return shape_color
def get_max_contact_count(self) -> int:
"""Return the maximum number of rigid contacts that can be generated by MuJoCo."""
if self.use_mujoco_cpu:
raise NotImplementedError()
return self.mjw_data.naconmax
@override
def update_contacts(self, contacts: Contacts, state: State | None = None) -> None:
"""Update `contacts` from MuJoCo contacts when running with ``use_mujoco_contacts``."""
if self.use_mujoco_cpu:
raise NotImplementedError()
# TODO: ensure that class invariants are preserved
# TODO: fill actual contact arrays instead of creating new ones
mj_data = self.mjw_data
mj_contact = mj_data.contact
if mj_data.naconmax > contacts.rigid_contact_max:
raise ValueError(
f"MuJoCo naconmax ({mj_data.naconmax}) exceeds contacts.rigid_contact_max "
f"({contacts.rigid_contact_max}). Create Contacts with at least "
f"rigid_contact_max={mj_data.naconmax}."
)
wp.launch(
self._convert_mjw_contacts_to_newton_kernel,
dim=mj_data.naconmax,
inputs=[
self.mjc_geom_to_newton_shape,
self.mjw_model.opt.cone,
mj_data.nacon,
mj_contact.pos,
mj_contact.frame,
mj_contact.friction,
mj_contact.dist,
mj_contact.dim,
mj_contact.geom,
mj_contact.efc_address,
mj_contact.worldid,
mj_data.efc.force,
self.mjw_model.geom_bodyid,
mj_data.xpos,
mj_data.xquat,
mj_data.njmax,
],
outputs=[
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_normal,
contacts.force,
],
device=self.model.device,
)
contacts.n_contacts = mj_data.nacon
def _convert_to_mjc(
self,
model: Model,
state: State | None = None,
*,
separate_worlds: bool | None = None,
iterations: int | None = None,
ls_iterations: int | None = None,
ccd_iterations: int | None = None,
sdf_iterations: int | None = None,
sdf_initpoints: int | None = None,
njmax: int | None = None, # number of constraints per world
nconmax: int | None = None,
solver: int | str | None = None,
integrator: int | str | None = None,
disableflags: int = 0,
disable_contacts: bool = False,
impratio: float | None = None,
tolerance: float | None = None,
ls_tolerance: float | None = None,
ccd_tolerance: float | None = None,
density: float | None = None,
viscosity: float | None = None,
wind: tuple | None = None,
magnetic: tuple | None = None,
cone: int | str | None = None,
jacobian: int | str | None = None,
target_filename: str | None = None,
skip_visual_only_geoms: bool = True,
include_sites: bool = True,
ls_parallel: bool = False,
) -> tuple[MjWarpModel, MjWarpData, MjModel, MjData]:
"""
Convert a Newton model and state to MuJoCo (Warp) model and data.
See ``docs/integrations/mujoco.rst`` for user-facing documentation of
all conversions performed here. Keep that file in sync when changing
this method.
Solver options (e.g., ``impratio``) follow this resolution priority:
1. **Constructor argument** - If provided, same value is used for all worlds.
2. **Newton model custom attribute** (``model.mujoco.``) - Supports per-world values.
3. **MuJoCo default** - Used if neither of the above is set.
Args:
model: The Newton model to convert.
state: The Newton state to convert (optional).
separate_worlds: If True, each world is a separate MuJoCo simulation. If None, defaults to True for GPU mode (not use_mujoco_cpu).
iterations: Maximum solver iterations. If None, uses model custom attribute or MuJoCo's default (100).
ls_iterations: Maximum line search iterations. If None, uses model custom attribute or MuJoCo's default (50).
njmax: Maximum number of constraints per world.
nconmax: Maximum number of contacts.
solver: Constraint solver type ("cg" or "newton"). If None, uses model custom attribute or Newton's default ("newton").
integrator: Integration method ("euler", "rk4", "implicit", "implicitfast"). If None, uses model custom attribute or Newton's default ("implicitfast").
disableflags: MuJoCo disable flags bitmask.
disable_contacts: If True, disable contact computation.
impratio: Impedance ratio for contacts. If None, uses model custom attribute or MuJoCo default (1.0).
tolerance: Solver tolerance. If None, uses model custom attribute or MuJoCo default (1e-8).
ls_tolerance: Line search tolerance. If None, uses model custom attribute or MuJoCo default (0.01).
ccd_tolerance: CCD tolerance. If None, uses model custom attribute or MuJoCo default (1e-6).
density: Medium density. If None, uses model custom attribute or MuJoCo default (0.0).
viscosity: Medium viscosity. If None, uses model custom attribute or MuJoCo default (0.0).
wind: Wind velocity vector (x, y, z). If None, uses model custom attribute or MuJoCo default (0, 0, 0).
magnetic: Magnetic flux vector (x, y, z). If None, uses model custom attribute or MuJoCo default (0, -0.5, 0).
cone: Friction cone type ("pyramidal" or "elliptic"). If None, uses model custom attribute or Newton's default ("pyramidal").
jacobian: Jacobian computation method ("dense", "sparse", or "auto"). If None, uses model custom attribute or MuJoCo default ("auto").
target_filename: Optional path to save generated MJCF file.
skip_visual_only_geoms: If True, skip geoms that are visual-only.
include_sites: If True, include sites in the model.
ls_parallel: If True, enable parallel line search.
Returns:
tuple[MjWarpModel, MjWarpData, MjModel, MjData]: Model and data objects for
``mujoco_warp`` and MuJoCo.
"""
if not model.joint_count:
raise ValueError("The model must have at least one joint to be able to convert it to MuJoCo.")
# Set default for separate_worlds if None
if separate_worlds is None:
separate_worlds = True
# Validate that separate_worlds=False is only used with single world
if not separate_worlds and model.world_count > 1:
raise ValueError(
f"separate_worlds=False is only supported for single-world models. "
f"Got world_count={model.world_count}. Use separate_worlds=True for multi-world models."
)
# Validate model compatibility with separate_worlds mode
if separate_worlds:
self._validate_model_for_separate_worlds(model)
mujoco, mujoco_warp = self.import_mujoco()
actuator_args = {
# "ctrllimited": True,
# "ctrlrange": (-1.0, 1.0),
"gear": [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
"trntype": mujoco.mjtTrn.mjTRN_JOINT,
# motor actuation properties (already the default settings in Mujoco)
"gainprm": [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
"biasprm": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
"dyntype": mujoco.mjtDyn.mjDYN_NONE,
"gaintype": mujoco.mjtGain.mjGAIN_FIXED,
"biastype": mujoco.mjtBias.mjBIAS_AFFINE,
}
# Convert string enum values to integers using the static parser methods
# (these methods handle both string and int inputs)
# Only convert if not None - will check custom attributes later if None
if solver is not None:
solver = self._parse_solver(solver)
if integrator is not None:
integrator = self._parse_integrator(integrator)
if cone is not None:
cone = self._parse_cone(cone)
if jacobian is not None:
jacobian = self._parse_jacobian(jacobian)
def quat_to_mjc(q):
# convert from xyzw to wxyz
# For Warp kernel equivalent, see quat_xyzw_to_wxyz() in kernels.py
return [q[3], q[0], q[1], q[2]]
def quat_from_mjc(q):
# convert from wxyz to xyzw
# For Warp kernel equivalent, see quat_wxyz_to_xyzw() in kernels.py
return [q[1], q[2], q[3], q[0]]
def fill_arr_from_dict(arr: np.ndarray, d: dict[int, Any]):
# fast way to fill an array from a dictionary
# keys and values can also be tuples of integers
keys = np.array(list(d.keys()), dtype=int)
vals = np.array(list(d.values()), dtype=int)
if keys.ndim == 1:
arr[keys] = vals
else:
arr[tuple(keys.T)] = vals
# Solver option resolution priority (highest to lowest):
# 1. Constructor argument (e.g., impratio=5.0) - same value for all worlds
# 2. Newton model custom attribute (model.mujoco. ) - supports per-world values
# 3. MuJoCo default
# Track which WORLD frequency options were overridden by constructor
overridden_options = set()
# Get mujoco custom attributes once
mujoco_attrs = getattr(model, "mujoco", None)
# Helper to resolve scalar option value
def resolve_option(name: str, constructor_value):
"""Resolve scalar option from constructor > model attribute > None (use MuJoCo default)."""
if constructor_value is not None:
overridden_options.add(name)
return constructor_value
if mujoco_attrs and hasattr(mujoco_attrs, name):
# Read from index 0 (template world) for initialization
return float(getattr(mujoco_attrs, name).numpy()[0])
return None
# Helper to resolve vector option value
def resolve_vector_option(name: str, constructor_value):
"""Resolve vector option from constructor > model attribute > None (use MuJoCo default)."""
if constructor_value is not None:
overridden_options.add(name)
return constructor_value
if mujoco_attrs and hasattr(mujoco_attrs, name):
# Read from index 0 (template world) for initialization
vec = getattr(mujoco_attrs, name).numpy()[0]
return tuple(vec)
return None
# Resolve all WORLD frequency scalar options
impratio = resolve_option("impratio", impratio)
tolerance = resolve_option("tolerance", tolerance)
ls_tolerance = resolve_option("ls_tolerance", ls_tolerance)
ccd_tolerance = resolve_option("ccd_tolerance", ccd_tolerance)
density = resolve_option("density", density)
viscosity = resolve_option("viscosity", viscosity)
# Resolve WORLD frequency vector options
wind = resolve_vector_option("wind", wind)
magnetic = resolve_vector_option("magnetic", magnetic)
# Resolve ONCE frequency numeric options from custom attributes if not provided
if iterations is None and mujoco_attrs and hasattr(mujoco_attrs, "iterations"):
iterations = int(mujoco_attrs.iterations.numpy()[0])
if ls_iterations is None and mujoco_attrs and hasattr(mujoco_attrs, "ls_iterations"):
ls_iterations = int(mujoco_attrs.ls_iterations.numpy()[0])
if ccd_iterations is None and mujoco_attrs and hasattr(mujoco_attrs, "ccd_iterations"):
ccd_iterations = int(mujoco_attrs.ccd_iterations.numpy()[0])
if sdf_iterations is None and mujoco_attrs and hasattr(mujoco_attrs, "sdf_iterations"):
sdf_iterations = int(mujoco_attrs.sdf_iterations.numpy()[0])
if sdf_initpoints is None and mujoco_attrs and hasattr(mujoco_attrs, "sdf_initpoints"):
sdf_initpoints = int(mujoco_attrs.sdf_initpoints.numpy()[0])
# Set defaults for numeric options if still None (use MuJoCo defaults)
if iterations is None:
iterations = 100
if ls_iterations is None:
ls_iterations = 50
# Resolve ONCE frequency enum options from custom attributes if not provided
if solver is None and mujoco_attrs and hasattr(mujoco_attrs, "solver"):
solver = int(mujoco_attrs.solver.numpy()[0])
if integrator is None and mujoco_attrs and hasattr(mujoco_attrs, "integrator"):
integrator = int(mujoco_attrs.integrator.numpy()[0])
if cone is None and mujoco_attrs and hasattr(mujoco_attrs, "cone"):
cone = int(mujoco_attrs.cone.numpy()[0])
if jacobian is None and mujoco_attrs and hasattr(mujoco_attrs, "jacobian"):
jacobian = int(mujoco_attrs.jacobian.numpy()[0])
# Set defaults for enum options if still None (use Newton defaults, not MuJoCo defaults)
if solver is None:
solver = mujoco.mjtSolver.mjSOL_NEWTON # Newton default (not CG)
if integrator is None:
integrator = mujoco.mjtIntegrator.mjINT_IMPLICITFAST # Newton default (not Euler)
if cone is None:
cone = mujoco.mjtCone.mjCONE_PYRAMIDAL
if jacobian is None:
jacobian = mujoco.mjtJacobian.mjJAC_AUTO
spec = mujoco.MjSpec()
spec.option.disableflags = disableflags
spec.option.gravity = np.array([*model.gravity.numpy()[0]])
spec.option.solver = solver
spec.option.integrator = integrator
spec.option.iterations = iterations
spec.option.ls_iterations = ls_iterations
spec.option.cone = cone
spec.option.jacobian = jacobian
# Set ONCE frequency numeric options (use MuJoCo defaults if None)
if ccd_iterations is not None:
spec.option.ccd_iterations = ccd_iterations
if sdf_iterations is not None:
spec.option.sdf_iterations = sdf_iterations
if sdf_initpoints is not None:
spec.option.sdf_initpoints = sdf_initpoints
# Set WORLD frequency options (use MuJoCo defaults if None)
if impratio is not None:
spec.option.impratio = impratio
if tolerance is not None:
spec.option.tolerance = tolerance
if ls_tolerance is not None:
spec.option.ls_tolerance = ls_tolerance
if ccd_tolerance is not None:
spec.option.ccd_tolerance = ccd_tolerance
if density is not None:
spec.option.density = density
if viscosity is not None:
spec.option.viscosity = viscosity
if wind is not None:
spec.option.wind = np.array(wind)
if magnetic is not None:
spec.option.magnetic = np.array(magnetic)
spec.compiler.inertiafromgeom = mujoco.mjtInertiaFromGeom.mjINERTIAFROMGEOM_AUTO
if mujoco_attrs and hasattr(mujoco_attrs, "autolimits"):
spec.compiler.autolimits = bool(mujoco_attrs.autolimits.numpy()[0])
joint_parent = model.joint_parent.numpy()
joint_child = model.joint_child.numpy()
joint_articulation = model.joint_articulation.numpy()
joint_parent_xform = model.joint_X_p.numpy()
joint_child_xform = model.joint_X_c.numpy()
joint_limit_lower = model.joint_limit_lower.numpy()
joint_limit_upper = model.joint_limit_upper.numpy()
joint_limit_ke = model.joint_limit_ke.numpy()
joint_limit_kd = model.joint_limit_kd.numpy()
joint_type = model.joint_type.numpy()
joint_axis = model.joint_axis.numpy()
joint_dof_dim = model.joint_dof_dim.numpy()
joint_qd_start = model.joint_qd_start.numpy()
joint_armature = model.joint_armature.numpy()
joint_effort_limit = model.joint_effort_limit.numpy()
# Per-DOF actuator arrays
joint_target_mode = model.joint_target_mode.numpy()
joint_target_ke = model.joint_target_ke.numpy()
joint_target_kd = model.joint_target_kd.numpy()
# MoJoCo doesn't have velocity limit
# joint_velocity_limit = model.joint_velocity_limit.numpy()
joint_friction = model.joint_friction.numpy()
joint_world = model.joint_world.numpy()
body_flags = model.body_flags.numpy()
body_q = model.body_q.numpy()
body_mass = model.body_mass.numpy()
body_inertia = model.body_inertia.numpy()
body_com = model.body_com.numpy()
body_world = model.body_world.numpy()
shape_transform = model.shape_transform.numpy()
shape_type = model.shape_type.numpy()
shape_size = model.shape_scale.numpy()
shape_flags = model.shape_flags.numpy()
shape_collision_group = model.shape_collision_group.numpy()
shape_world = model.shape_world.numpy()
shape_mu = model.shape_material_mu.numpy()
shape_ke = model.shape_material_ke.numpy()
shape_kd = model.shape_material_kd.numpy()
shape_mu_torsional = model.shape_material_mu_torsional.numpy()
shape_mu_rolling = model.shape_material_mu_rolling.numpy()
shape_margin = model.shape_margin.numpy()
# retrieve MuJoCo-specific attributes
mujoco_attrs = getattr(model, "mujoco", None)
def get_custom_attribute(name: str) -> np.ndarray | None:
if mujoco_attrs is None:
return None
attr = getattr(mujoco_attrs, name, None)
if attr is None:
return None
return attr.numpy()
shape_condim = get_custom_attribute("condim")
shape_priority = get_custom_attribute("geom_priority")
shape_geom_solimp = get_custom_attribute("geom_solimp")
shape_geom_solmix = get_custom_attribute("geom_solmix")
joint_dof_limit_margin = get_custom_attribute("limit_margin")
joint_solimp_limit = get_custom_attribute("solimplimit")
joint_dof_solref = get_custom_attribute("solreffriction")
joint_dof_solimp = get_custom_attribute("solimpfriction")
joint_stiffness = get_custom_attribute("dof_passive_stiffness")
joint_damping = get_custom_attribute("dof_passive_damping")
joint_actgravcomp = get_custom_attribute("jnt_actgravcomp")
body_gravcomp = get_custom_attribute("gravcomp")
joint_springref = get_custom_attribute("dof_springref")
joint_ref = get_custom_attribute("dof_ref")
eq_constraint_type = model.equality_constraint_type.numpy()
eq_constraint_body1 = model.equality_constraint_body1.numpy()
eq_constraint_body2 = model.equality_constraint_body2.numpy()
eq_constraint_anchor = model.equality_constraint_anchor.numpy()
eq_constraint_torquescale = model.equality_constraint_torquescale.numpy()
eq_constraint_relpose = model.equality_constraint_relpose.numpy()
eq_constraint_joint1 = model.equality_constraint_joint1.numpy()
eq_constraint_joint2 = model.equality_constraint_joint2.numpy()
eq_constraint_polycoef = model.equality_constraint_polycoef.numpy()
eq_constraint_enabled = model.equality_constraint_enabled.numpy()
eq_constraint_world = model.equality_constraint_world.numpy()
eq_constraint_solref = get_custom_attribute("eq_solref")
eq_constraint_solimp = get_custom_attribute("eq_solimp")
# Read mimic constraint arrays
mimic_joint0 = model.constraint_mimic_joint0.numpy()
mimic_joint1 = model.constraint_mimic_joint1.numpy()
mimic_coef0 = model.constraint_mimic_coef0.numpy()
mimic_coef1 = model.constraint_mimic_coef1.numpy()
mimic_enabled = model.constraint_mimic_enabled.numpy()
mimic_world = model.constraint_mimic_world.numpy()
INT32_MAX = np.iinfo(np.int32).max
collision_mask_everything = INT32_MAX
# mapping from joint axis to actuator index
# axis_to_actuator[i, 0] = position actuator index
# axis_to_actuator[i, 1] = velocity actuator index
axis_to_actuator = np.zeros((model.joint_dof_count, 2), dtype=np.int32) - 1
actuator_count = 0
# Track actuator mapping as they're created (indexed by MuJoCo actuator order)
# ctrl_source: 0=JOINT_TARGET, 1=CTRL_DIRECT
# to_newton_idx: for JOINT_TARGET: >=0 position DOF, -1 unmapped, <=-2 velocity (DOF = -(val+2))
# for CTRL_DIRECT: MJCF-order index into control.mujoco.ctrl
mjc_actuator_ctrl_source_list: list[int] = []
mjc_actuator_to_newton_idx_list: list[int] = []
# supported non-fixed joint types in MuJoCo (fixed joints are handled by nesting bodies)
supported_joint_types = {
JointType.FREE,
JointType.BALL,
JointType.PRISMATIC,
JointType.REVOLUTE,
JointType.D6,
}
geom_type_mapping = {
GeoType.SPHERE: mujoco.mjtGeom.mjGEOM_SPHERE,
GeoType.PLANE: mujoco.mjtGeom.mjGEOM_PLANE,
GeoType.HFIELD: mujoco.mjtGeom.mjGEOM_HFIELD,
GeoType.CAPSULE: mujoco.mjtGeom.mjGEOM_CAPSULE,
GeoType.CYLINDER: mujoco.mjtGeom.mjGEOM_CYLINDER,
GeoType.BOX: mujoco.mjtGeom.mjGEOM_BOX,
GeoType.ELLIPSOID: mujoco.mjtGeom.mjGEOM_ELLIPSOID,
GeoType.MESH: mujoco.mjtGeom.mjGEOM_MESH,
GeoType.CONVEX_MESH: mujoco.mjtGeom.mjGEOM_MESH,
}
mj_bodies = [spec.worldbody]
# mapping from Newton body id to MuJoCo body id
body_mapping = {-1: 0}
# mapping from Newton shape id to MuJoCo geom name
shape_mapping = {}
# mapping from Newton shape id (sites) to MuJoCo site name
site_mapping = {}
# Store mapping from Newton joint index to MuJoCo joint name
joint_mapping = {}
# Store mapping from Newton body index to MuJoCo body name
body_name_mapping = {}
# ensure unique names
body_name_counts = {}
joint_names = {}
if separate_worlds:
# determine which shapes, bodies and joints belong to the first world
# based on the body world indices: we pick objects from the first world and global shapes
non_negatives = body_world[body_world >= 0]
if len(non_negatives) > 0:
first_world = np.min(non_negatives)
else:
first_world = -1
selected_shapes = np.where((shape_world == first_world) | (shape_world < 0))[0].astype(np.int32)
selected_bodies = np.where((body_world == first_world) | (body_world < 0))[0].astype(np.int32)
selected_joints = np.where((joint_world == first_world) | (joint_world < 0))[0].astype(np.int32)
selected_constraints = np.where((eq_constraint_world == first_world) | (eq_constraint_world < 0))[0].astype(
np.int32
)
selected_mimic_constraints = np.where((mimic_world == first_world) | (mimic_world < 0))[0].astype(np.int32)
else:
# if we are not separating environments to worlds, we use all shapes, bodies, joints
first_world = 0
# if we are not separating worlds, we use all shapes, bodies, joints, constraints
selected_shapes = np.arange(model.shape_count, dtype=np.int32)
selected_bodies = np.arange(model.body_count, dtype=np.int32)
selected_joints = np.arange(model.joint_count, dtype=np.int32)
selected_constraints = np.arange(model.equality_constraint_count, dtype=np.int32)
selected_mimic_constraints = np.arange(model.constraint_mimic_count, dtype=np.int32)
# get the shapes for the first environment
first_env_shapes = np.where(shape_world == first_world)[0]
# split joints into loop and non-loop joints (loop joints will be instantiated separately as equality constraints)
joints_loop = selected_joints[joint_articulation[selected_joints] == -1]
joints_non_loop = selected_joints[joint_articulation[selected_joints] >= 0]
# sort joints topologically depth-first since this is the order that will also be used
# for placing bodies in the MuJoCo model
joints_simple = [(joint_parent[i], joint_child[i]) for i in joints_non_loop]
joint_order = topological_sort(joints_simple, use_dfs=True, custom_indices=joints_non_loop)
if any(joint_order[i] != joints_non_loop[i] for i in range(len(joints_simple))):
warnings.warn(
"Joint order is not in depth-first topological order while converting Newton model to MuJoCo, this may lead to diverging kinematics between MuJoCo and Newton.",
stacklevel=2,
)
# Count the total joint coordinates and DOFs that belong to loop joints
# across all worlds (not added to MuJoCo as joints). When
# separate_worlds=True, joints_loop is per-template so we multiply by
# world_count; otherwise it already spans all worlds.
joint_q_start_np = model.joint_q_start.numpy()
joint_qd_start_np = model.joint_qd_start.numpy()
loop_coord_count = 0
loop_dof_count = 0
for j in joints_loop:
loop_coord_count += int(joint_q_start_np[j + 1]) - int(joint_q_start_np[j])
loop_dof_count += int(joint_qd_start_np[j + 1]) - int(joint_qd_start_np[j])
if separate_worlds:
self._total_loop_joint_coords = loop_coord_count * model.world_count
self._total_loop_joint_dofs = loop_dof_count * model.world_count
else:
self._total_loop_joint_coords = loop_coord_count
self._total_loop_joint_dofs = loop_dof_count
# find graph coloring of collision filter pairs
# filter out shapes that are not colliding with anything
colliding_shapes = selected_shapes[shape_flags[selected_shapes] & ShapeFlags.COLLIDE_SHAPES != 0]
# number of shapes we are instantiating in MuJoCo (which will be replicated for the number of envs)
colliding_shapes_per_world = len(colliding_shapes)
# filter out non-colliding bodies using excludes
body_filters = self._find_body_collision_filter_pairs(
model,
selected_bodies,
colliding_shapes,
)
shape_color = self._color_collision_shapes(
model, colliding_shapes, visualize_graph=False, shape_labels=model.shape_label
)
selected_shapes_set = set(selected_shapes)
# Compute shapes required by spatial tendons (sites, wrapping geoms, sidesites)
# so they are not skipped when skip_visual_only_geoms=True or include_sites=False.
# Only collect from template-world tendons to avoid inflating the count with
# shape indices from other worlds.
tendon_required_shapes: set[int] = set()
mujoco_attrs = getattr(model, "mujoco", None)
if mujoco_attrs is not None:
_wrap_shape = getattr(mujoco_attrs, "tendon_wrap_shape", None)
_wrap_sidesite = getattr(mujoco_attrs, "tendon_wrap_sidesite", None)
_wrap_adr = getattr(mujoco_attrs, "tendon_wrap_adr", None)
_wrap_num = getattr(mujoco_attrs, "tendon_wrap_num", None)
_tendon_world = getattr(mujoco_attrs, "tendon_world", None)
if _wrap_shape is not None and _wrap_adr is not None and _wrap_num is not None:
wrap_shape_np = _wrap_shape.numpy()
wrap_sidesite_np = _wrap_sidesite.numpy() if _wrap_sidesite is not None else None
wrap_adr_np = _wrap_adr.numpy()
wrap_num_np = _wrap_num.numpy()
tendon_world_np = _tendon_world.numpy() if _tendon_world is not None else None
for ti in range(len(wrap_adr_np)):
tw = int(tendon_world_np[ti]) if tendon_world_np is not None else 0
if tw != first_world and tw >= 0:
continue
start = int(wrap_adr_np[ti])
num = int(wrap_num_np[ti])
for w in range(start, start + num):
if w < len(wrap_shape_np):
idx = int(wrap_shape_np[w])
if idx >= 0:
tendon_required_shapes.add(idx)
if wrap_sidesite_np is not None and w < len(wrap_sidesite_np):
ss = int(wrap_sidesite_np[w])
if ss >= 0:
tendon_required_shapes.add(ss)
def add_geoms(newton_body_id: int):
body = mj_bodies[body_mapping[newton_body_id]]
shapes = model.body_shapes.get(newton_body_id)
if not shapes:
return
for shape in shapes:
if shape not in selected_shapes_set:
# skip shapes that are not selected for this world
continue
# Skip visual-only geoms, but don't skip sites or shapes needed by spatial tendons
is_site = shape_flags[shape] & ShapeFlags.SITE
if skip_visual_only_geoms and not is_site and not (shape_flags[shape] & ShapeFlags.COLLIDE_SHAPES):
if shape not in tendon_required_shapes:
continue
stype = shape_type[shape]
name = f"{model.shape_label[shape]}_{shape}"
if is_site:
if not include_sites and shape not in tendon_required_shapes:
continue
# Map unsupported site types to SPHERE
# MuJoCo sites only support: SPHERE, CAPSULE, CYLINDER, BOX
supported_site_types = {GeoType.SPHERE, GeoType.CAPSULE, GeoType.CYLINDER, GeoType.BOX}
site_geom_type = stype if stype in supported_site_types else GeoType.SPHERE
tf = wp.transform(*shape_transform[shape])
site_params = {
"type": geom_type_mapping[site_geom_type],
"name": name,
"pos": tf.p,
"quat": quat_to_mjc(tf.q),
}
size = shape_size[shape]
# Ensure size is valid for the site type
if np.any(size > 0.0):
nonzero = size[size > 0.0][0]
size[size == 0.0] = nonzero
site_params["size"] = size
else:
site_params["size"] = [0.01, 0.01, 0.01]
if shape_flags[shape] & ShapeFlags.VISIBLE:
site_params["rgba"] = [0.0, 1.0, 0.0, 0.5]
else:
site_params["rgba"] = [0.0, 1.0, 0.0, 0.0]
body.add_site(**site_params)
site_mapping[shape] = name
continue
if stype == GeoType.PLANE and newton_body_id != -1:
raise ValueError("Planes can only be attached to static bodies")
geom_params = {
"type": geom_type_mapping[stype],
"name": name,
}
tf = wp.transform(*shape_transform[shape])
if stype == GeoType.HFIELD:
# Retrieve heightfield source
hfield_src = model.shape_source[shape]
if hfield_src is None:
if wp.config.verbose:
print(f"Warning: Heightfield shape {shape} has no source data, skipping")
continue
# Convert Newton heightfield to MuJoCo format
# MuJoCo size: (size_x, size_y, size_z, size_base) — all must be positive
# Our data is normalized [0,1], height range = max_z - min_z
# We set size_base to eps (MuJoCo requires positive) and shift the
# geom origin by min_z so the lowest point is at the right world Z.
eps = 1e-4
mj_size_z = max(hfield_src.max_z - hfield_src.min_z, eps)
mj_size = (hfield_src.hx, hfield_src.hy, mj_size_z, eps)
elevation_data = hfield_src.data.flatten()
hfield_name = f"{model.shape_label[shape].replace('/', '_')}_{shape}"
spec.add_hfield(
name=hfield_name,
nrow=hfield_src.nrow,
ncol=hfield_src.ncol,
size=mj_size,
userdata=elevation_data,
)
geom_params["hfieldname"] = hfield_name
# Shift geom origin so data=0 maps to min_z in world space
tf = wp.transform(
wp.vec3(tf.p[0], tf.p[1], tf.p[2] + hfield_src.min_z),
tf.q,
)
elif stype == GeoType.MESH or stype == GeoType.CONVEX_MESH:
mesh_src = model.shape_source[shape]
maxhullvert = mesh_src.maxhullvert
# apply scaling
size = shape_size[shape]
vertices = mesh_src.vertices * size
spec.add_mesh(
name=name,
uservert=vertices.flatten(),
userface=mesh_src.indices.flatten(),
maxhullvert=maxhullvert,
)
geom_params["meshname"] = name
geom_params["pos"] = tf.p
geom_params["quat"] = quat_to_mjc(tf.q)
size = shape_size[shape]
if np.any(size > 0.0):
# duplicate nonzero entries at places where size is 0
nonzero = size[size > 0.0][0]
size[size == 0.0] = nonzero
geom_params["size"] = size
else:
assert stype == GeoType.PLANE, "Only plane shapes are allowed to have a size of zero"
# planes are always infinite for collision purposes in mujoco
geom_params["size"] = [5.0, 5.0, 5.0]
# make ground plane blue in the MuJoCo viewer (only used for debugging)
geom_params["rgba"] = [0.0, 0.3, 0.6, 1.0]
# encode collision filtering information
if not (shape_flags[shape] & ShapeFlags.COLLIDE_SHAPES) or shape_collision_group[shape] == 0:
# Non-colliding shape, or collision_group=0 (e.g. MJCF contype=conaffinity=0
# geoms that only participate in explicit contacts)
geom_params["contype"] = 0
geom_params["conaffinity"] = 0
else:
color = shape_color[shape]
if color < 32:
contype = 1 << color
geom_params["contype"] = contype
# collide with anything except shapes from the same color
geom_params["conaffinity"] = collision_mask_everything & ~contype
# set friction from Newton shape materials
mu = shape_mu[shape]
torsional = shape_mu_torsional[shape]
rolling = shape_mu_rolling[shape]
geom_params["friction"] = [
mu,
torsional,
rolling,
]
# set solref from shape stiffness and damping
geom_params["solref"] = convert_solref(float(shape_ke[shape]), float(shape_kd[shape]), 1.0, 1.0)
if shape_condim is not None:
geom_params["condim"] = shape_condim[shape]
if shape_priority is not None:
geom_params["priority"] = shape_priority[shape]
if shape_geom_solimp is not None:
geom_params["solimp"] = shape_geom_solimp[shape]
if shape_geom_solmix is not None:
geom_params["solmix"] = shape_geom_solmix[shape]
geom_params["gap"] = 0.0
geom_params["margin"] = float(shape_margin[shape])
body.add_geom(**geom_params)
# store the geom name instead of assuming index
shape_mapping[shape] = name
# add static geoms attached to the worldbody
add_geoms(-1)
# Maps from Newton joint index (per-world/template) to MuJoCo DOF start index (per-world/template)
# Only populated for template joints; in kernels, use joint_in_world to index
joint_mjc_dof_start = np.full(len(selected_joints), -1, dtype=np.int32)
joint_mjc_qpos_start = np.full(len(selected_joints), -1, dtype=np.int32)
# Maps from Newton DOF index to MuJoCo joint index (first world only)
# Needed because jnt_solimp/jnt_solref are per-joint (not per-DOF) in MuJoCo
dof_to_mjc_joint = np.full(model.joint_dof_count // model.world_count, -1, dtype=np.int32)
# This is needed for CTRL_DIRECT actuators targeting joints within combined Newton joints.
mjc_joint_names: list[str] = []
# need to keep track of current dof and joint counts to make the indexing above correct
num_dofs = 0
num_qpos = 0
num_mjc_joints = 0
# add joints, bodies and geoms
for j in joint_order:
parent, child = int(joint_parent[j]), int(joint_child[j])
child_is_kinematic = (int(body_flags[child]) & int(BodyFlags.KINEMATIC)) != 0
if child in body_mapping:
raise ValueError(f"Body {child} already exists in the mapping")
# add body
body_mapping[child] = len(mj_bodies)
j_type = joint_type[j]
# Export every world-fixed root as a MuJoCo mocap body: fixed
# roots have no MuJoCo joint DOFs, but Newton can still update
# their pose through joint_X_p/joint_X_c. Static world-attached
# shapes are exported separately rather than as articulated bodies.
is_fixed_root = parent == -1 and j_type == JointType.FIXED
# Compute body transform for the MjSpec body pos/quat.
# For free joints, the parent/child xforms are identity and the
# initial position lives in body_q (see add_joint_free docstring).
child_xform = wp.transform(*joint_child_xform[j])
if j_type == JointType.FREE:
bq = body_q[child]
tf = wp.transform(bq[:3], bq[3:])
else:
tf = wp.transform(*joint_parent_xform[j])
tf = tf * wp.transform_inverse(child_xform)
joint_pos = child_xform.p
joint_rot = child_xform.q
# ensure unique body name
name = model.body_label[child].replace("/", "_")
if name not in body_name_counts:
body_name_counts[name] = 1
else:
while name in body_name_counts:
body_name_counts[name] += 1
name = f"{name}_{body_name_counts[name]}"
body_name_mapping[child] = name # store the final de-duplicated name
inertia = body_inertia[child]
mass = body_mass[child]
# MuJoCo requires positive-definite inertia. For zero-mass bodies
# (sensor frames, reference links), omit mass and inertia entirely
# and let MuJoCo handle them natively.
body_kwargs = {"name": name, "pos": tf.p, "quat": quat_to_mjc(tf.q), "mocap": is_fixed_root}
if body_gravcomp is not None and body_gravcomp[child] != 0.0:
body_kwargs["gravcomp"] = float(body_gravcomp[child])
if mass > 0.0:
body_kwargs["mass"] = mass
body_kwargs["ipos"] = body_com[child, :]
# Use diaginertia when off-diagonals are exactly zero to preserve
# MuJoCo's sameframe optimization (body_simple=1). fullinertia
# triggers eigendecomposition that reorders eigenvalues and applies
# a permutation rotation, setting body_simple=0 even for diagonal
# matrices whose entries are not in descending order.
if inertia[0, 1] == 0.0 and inertia[0, 2] == 0.0 and inertia[1, 2] == 0.0:
body_kwargs["inertia"] = [inertia[0, 0], inertia[1, 1], inertia[2, 2]]
else:
body_kwargs["fullinertia"] = [
inertia[0, 0],
inertia[1, 1],
inertia[2, 2],
inertia[0, 1],
inertia[0, 2],
inertia[1, 2],
]
body_kwargs["explicitinertial"] = True
body = mj_bodies[body_mapping[parent]].add_body(**body_kwargs)
mj_bodies.append(body)
# add joint
qd_start = joint_qd_start[j]
name = model.joint_label[j].replace("/", "_")
if name not in joint_names:
joint_names[name] = 1
else:
while name in joint_names:
joint_names[name] += 1
name = f"{name}_{joint_names[name]}"
# Store mapping from Newton joint index to MuJoCo joint name
joint_mapping[j] = name
joint_mjc_dof_start[j] = num_dofs
joint_mjc_qpos_start[j] = num_qpos
if j_type == JointType.FREE:
body.add_joint(
name=name,
type=mujoco.mjtJoint.mjJNT_FREE,
damping=0.0,
limited=False,
)
mjc_joint_names.append(name)
for i in range(6):
dof_to_mjc_joint[qd_start + i] = num_mjc_joints
num_dofs += 6
num_qpos += 7
num_mjc_joints += 1
elif j_type == JointType.BALL:
body.add_joint(
name=name,
type=mujoco.mjtJoint.mjJNT_BALL,
axis=wp.quat_rotate(joint_rot, wp.vec3(1.0, 0.0, 0.0)),
pos=joint_pos,
damping=0.0,
limited=False,
armature=self._KINEMATIC_ARMATURE if child_is_kinematic else joint_armature[qd_start],
frictionloss=joint_friction[qd_start],
)
mjc_joint_names.append(name)
# For ball joints, all 3 DOFs map to the same MuJoCo joint
for i in range(3):
dof_to_mjc_joint[qd_start + i] = num_mjc_joints
num_dofs += 3
num_qpos += 4
num_mjc_joints += 1
# Add actuators for the ball joint using per-DOF arrays
for i in range(3):
ai = qd_start + i
mode = joint_target_mode[ai]
if mode != int(JointTargetMode.NONE):
kp = joint_target_ke[ai]
kd = joint_target_kd[ai]
effort_limit = joint_effort_limit[ai]
args = {}
args.update(actuator_args)
args["gear"] = [0.0] * 6
args["gear"][i] = 1.0
args["forcerange"] = [-effort_limit, effort_limit]
template_dof = ai
# Add position actuator if mode includes position
if mode == JointTargetMode.POSITION:
args["gainprm"] = [kp, 0, 0, 0, 0, 0, 0, 0, 0, 0]
args["biasprm"] = [0, -kp, -kd, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=name, **args)
axis_to_actuator[ai, 0] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(template_dof) # positive = position
actuator_count += 1
elif mode == JointTargetMode.POSITION_VELOCITY:
args["gainprm"] = [kp, 0, 0, 0, 0, 0, 0, 0, 0, 0]
args["biasprm"] = [0, -kp, 0, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=name, **args)
axis_to_actuator[ai, 0] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(template_dof) # positive = position
actuator_count += 1
# Add velocity actuator if mode includes velocity
if mode in (JointTargetMode.VELOCITY, JointTargetMode.POSITION_VELOCITY):
args["gainprm"] = [kd, 0, 0, 0, 0, 0, 0, 0, 0, 0]
args["biasprm"] = [0, 0, -kd, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=name, **args)
axis_to_actuator[ai, 1] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(-(template_dof + 2)) # negative = velocity
actuator_count += 1
elif j_type in supported_joint_types:
lin_axis_count, ang_axis_count = joint_dof_dim[j]
num_dofs += lin_axis_count + ang_axis_count
num_qpos += lin_axis_count + ang_axis_count
# linear dofs
for i in range(lin_axis_count):
ai = qd_start + i
axis = wp.quat_rotate(joint_rot, wp.vec3(*joint_axis[ai]))
joint_params = {
"armature": self._KINEMATIC_ARMATURE if child_is_kinematic else joint_armature[qd_start + i],
"pos": joint_pos,
}
# Set friction
joint_params["frictionloss"] = joint_friction[ai]
# Set margin if available
if joint_dof_limit_margin is not None:
joint_params["margin"] = joint_dof_limit_margin[ai]
if joint_stiffness is not None:
joint_params["stiffness"] = joint_stiffness[ai]
if joint_damping is not None:
joint_params["damping"] = joint_damping[ai]
if joint_actgravcomp is not None:
joint_params["actgravcomp"] = joint_actgravcomp[ai]
lower, upper = joint_limit_lower[ai], joint_limit_upper[ai]
if lower <= -MAXVAL and upper >= MAXVAL:
joint_params["limited"] = False
else:
joint_params["limited"] = True
# we're piping these through unconditionally even though they are only active with limited joints
joint_params["range"] = (lower, upper)
# Use negative convention for solref_limit: (-stiffness, -damping)
if joint_limit_ke[ai] > 0:
joint_params["solref_limit"] = (-joint_limit_ke[ai], -joint_limit_kd[ai])
if joint_solimp_limit is not None:
joint_params["solimp_limit"] = joint_solimp_limit[ai]
if joint_dof_solref is not None:
joint_params["solref_friction"] = joint_dof_solref[ai]
if joint_dof_solimp is not None:
joint_params["solimp_friction"] = joint_dof_solimp[ai]
# Use actfrcrange to clamp total actuator force (P+D sum) on this joint
effort_limit = joint_effort_limit[ai]
joint_params["actfrclimited"] = True
joint_params["actfrcrange"] = (-effort_limit, effort_limit)
if joint_springref is not None:
joint_params["springref"] = joint_springref[ai]
if joint_ref is not None:
joint_params["ref"] = joint_ref[ai]
axname = name
if lin_axis_count > 1 or ang_axis_count > 1:
axname += "_lin"
if lin_axis_count > 1:
axname += str(i)
body.add_joint(
name=axname,
type=mujoco.mjtJoint.mjJNT_SLIDE,
axis=axis,
**joint_params,
)
mjc_joint_names.append(axname)
# Map this DOF to the current MuJoCo joint index
dof_to_mjc_joint[ai] = num_mjc_joints
num_mjc_joints += 1
mode = joint_target_mode[ai]
if mode != int(JointTargetMode.NONE):
kp = joint_target_ke[ai]
kd = joint_target_kd[ai]
template_dof = ai
# Add position actuator if mode includes position
if mode == JointTargetMode.POSITION:
actuator_args["gainprm"] = [kp, 0, 0, 0, 0, 0, 0, 0, 0, 0]
actuator_args["biasprm"] = [0, -kp, -kd, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=axname, **actuator_args)
axis_to_actuator[ai, 0] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(template_dof) # positive = position
actuator_count += 1
elif mode == JointTargetMode.POSITION_VELOCITY:
actuator_args["gainprm"] = [kp, 0, 0, 0, 0, 0, 0, 0, 0, 0]
actuator_args["biasprm"] = [0, -kp, 0, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=axname, **actuator_args)
axis_to_actuator[ai, 0] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(template_dof) # positive = position
actuator_count += 1
# Add velocity actuator if mode includes velocity
if mode in (JointTargetMode.VELOCITY, JointTargetMode.POSITION_VELOCITY):
actuator_args["gainprm"] = [kd, 0, 0, 0, 0, 0, 0, 0, 0, 0]
actuator_args["biasprm"] = [0, 0, -kd, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=axname, **actuator_args)
axis_to_actuator[ai, 1] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(-(template_dof + 2)) # negative = velocity
actuator_count += 1
# angular dofs
for i in range(lin_axis_count, lin_axis_count + ang_axis_count):
ai = qd_start + i
axis = wp.quat_rotate(joint_rot, wp.vec3(*joint_axis[ai]))
joint_params = {
"armature": self._KINEMATIC_ARMATURE if child_is_kinematic else joint_armature[qd_start + i],
"pos": joint_pos,
}
# Set friction
joint_params["frictionloss"] = joint_friction[ai]
# Set margin if available
if joint_dof_limit_margin is not None:
joint_params["margin"] = joint_dof_limit_margin[ai]
if joint_stiffness is not None:
joint_params["stiffness"] = joint_stiffness[ai] * (np.pi / 180)
if joint_damping is not None:
joint_params["damping"] = joint_damping[ai] * (np.pi / 180)
if joint_actgravcomp is not None:
joint_params["actgravcomp"] = joint_actgravcomp[ai]
lower, upper = joint_limit_lower[ai], joint_limit_upper[ai]
if lower <= -MAXVAL and upper >= MAXVAL:
joint_params["limited"] = False
else:
joint_params["limited"] = True
# we're piping these through unconditionally even though they are only active with limited joints
joint_params["range"] = (np.rad2deg(lower), np.rad2deg(upper))
# Use negative convention for solref_limit: (-stiffness, -damping)
if joint_limit_ke[ai] > 0:
joint_params["solref_limit"] = (-joint_limit_ke[ai], -joint_limit_kd[ai])
if joint_solimp_limit is not None:
joint_params["solimp_limit"] = joint_solimp_limit[ai]
if joint_dof_solref is not None:
joint_params["solref_friction"] = joint_dof_solref[ai]
if joint_dof_solimp is not None:
joint_params["solimp_friction"] = joint_dof_solimp[ai]
# Use actfrcrange to clamp total actuator force (P+D sum) on this joint
effort_limit = joint_effort_limit[ai]
joint_params["actfrclimited"] = True
joint_params["actfrcrange"] = (-effort_limit, effort_limit)
if joint_springref is not None:
joint_params["springref"] = np.rad2deg(joint_springref[ai])
if joint_ref is not None:
joint_params["ref"] = np.rad2deg(joint_ref[ai])
axname = name
if lin_axis_count > 1 or ang_axis_count > 1:
axname += "_ang"
if ang_axis_count > 1:
axname += str(i - lin_axis_count)
body.add_joint(
name=axname,
type=mujoco.mjtJoint.mjJNT_HINGE,
axis=axis,
**joint_params,
)
mjc_joint_names.append(axname)
# Map this DOF to the current MuJoCo joint index
dof_to_mjc_joint[ai] = num_mjc_joints
num_mjc_joints += 1
mode = joint_target_mode[ai]
if mode != int(JointTargetMode.NONE):
kp = joint_target_ke[ai]
kd = joint_target_kd[ai]
template_dof = ai
# Add position actuator if mode includes position
if mode == JointTargetMode.POSITION:
actuator_args["gainprm"] = [kp, 0, 0, 0, 0, 0, 0, 0, 0, 0]
actuator_args["biasprm"] = [0, -kp, -kd, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=axname, **actuator_args)
axis_to_actuator[ai, 0] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(template_dof) # positive = position
actuator_count += 1
elif mode == JointTargetMode.POSITION_VELOCITY:
actuator_args["gainprm"] = [kp, 0, 0, 0, 0, 0, 0, 0, 0, 0]
actuator_args["biasprm"] = [0, -kp, 0, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=axname, **actuator_args)
axis_to_actuator[ai, 0] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(template_dof) # positive = position
actuator_count += 1
# Add velocity actuator if mode includes velocity
if mode in (JointTargetMode.VELOCITY, JointTargetMode.POSITION_VELOCITY):
actuator_args["gainprm"] = [kd, 0, 0, 0, 0, 0, 0, 0, 0, 0]
actuator_args["biasprm"] = [0, 0, -kd, 0, 0, 0, 0, 0, 0, 0]
spec.add_actuator(target=axname, **actuator_args)
axis_to_actuator[ai, 1] = actuator_count
mjc_actuator_ctrl_source_list.append(0) # JOINT_TARGET
mjc_actuator_to_newton_idx_list.append(-(template_dof + 2)) # negative = velocity
actuator_count += 1
# Note: MuJoCo general actuators are handled separately via custom attributes
elif j_type != JointType.FIXED:
raise NotImplementedError(f"Joint type {j_type} is not supported yet")
add_geoms(child)
def get_body_name(body_idx: int) -> str:
"""Get body name, handling world body (-1) correctly."""
if body_idx == -1:
return "world"
target_name = body_name_mapping.get(body_idx)
if target_name is None:
target_name = model.body_label[body_idx].replace("/", "_")
if wp.config.verbose:
print(
f"Warning: MuJoCo equality constraint references body {body_idx} "
"not present in the MuJoCo export."
)
return target_name
for i in selected_constraints:
constraint_type = eq_constraint_type[i]
if constraint_type == EqType.CONNECT:
eq = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_BODY)
eq.type = mujoco.mjtEq.mjEQ_CONNECT
eq.active = eq_constraint_enabled[i]
eq.name1 = get_body_name(eq_constraint_body1[i])
eq.name2 = get_body_name(eq_constraint_body2[i])
eq.data[0:3] = eq_constraint_anchor[i]
if eq_constraint_solref is not None:
eq.solref = eq_constraint_solref[i]
if eq_constraint_solimp is not None:
eq.solimp = eq_constraint_solimp[i]
elif constraint_type == EqType.JOINT:
eq = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_JOINT)
eq.type = mujoco.mjtEq.mjEQ_JOINT
eq.active = eq_constraint_enabled[i]
j1_idx = int(eq_constraint_joint1[i])
j2_idx = int(eq_constraint_joint2[i])
eq.name1 = joint_mapping.get(j1_idx, model.joint_label[j1_idx].replace("/", "_"))
eq.name2 = joint_mapping.get(j2_idx, model.joint_label[j2_idx].replace("/", "_"))
eq.data[0:5] = eq_constraint_polycoef[i]
if eq_constraint_solref is not None:
eq.solref = eq_constraint_solref[i]
if eq_constraint_solimp is not None:
eq.solimp = eq_constraint_solimp[i]
elif constraint_type == EqType.WELD:
eq = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_BODY)
eq.type = mujoco.mjtEq.mjEQ_WELD
eq.active = eq_constraint_enabled[i]
eq.name1 = get_body_name(eq_constraint_body1[i])
eq.name2 = get_body_name(eq_constraint_body2[i])
cns_relpose = wp.transform(*eq_constraint_relpose[i])
eq.data[0:3] = eq_constraint_anchor[i]
eq.data[3:6] = wp.transform_get_translation(cns_relpose)
eq.data[6:10] = quat_to_mjc(wp.transform_get_rotation(cns_relpose))
eq.data[10] = eq_constraint_torquescale[i]
if eq_constraint_solref is not None:
eq.solref = eq_constraint_solref[i]
if eq_constraint_solimp is not None:
eq.solimp = eq_constraint_solimp[i]
# add equality constraints for joints that are excluded from the articulation
# (the UsdPhysics way of defining loop closures)
mjc_eq_to_newton_jnt = {}
for j in joints_loop:
j_type = joint_type[j]
parent_name = get_body_name(joint_parent[j])
child_name = get_body_name(joint_child[j])
lin_count, ang_count = joint_dof_dim[j]
if j_type == JointType.FIXED:
# Fixed loop joint → weld constraint (constrains all 6 DOFs).
# Set the anchor on body1; leave data[3:10] (relpose) at zero
# so that spec.compile() auto-computes it from the body positions
# at compile time. Manual relpose computation is fragile because
# the joint xforms define anchor offsets in body-local frames
# while the WELD relpose is measured in body2's local frame —
# these differ whenever the anchor is not at the body origin.
eq = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_BODY)
eq.type = mujoco.mjtEq.mjEQ_WELD
eq.active = True
eq.name1 = parent_name
eq.name2 = child_name
eq.data[0:3] = joint_parent_xform[j][:3]
mjc_eq_to_newton_jnt[eq.id] = j
elif lin_count == 0 and ang_count == 1:
# Single-hinge loop joint (revolute): 2x CONNECT at non-coincident
# points along the hinge axis constrains 5 DOFs (3 trans + 2 rot),
# leaving exactly 1 rotational DOF around the axis.
parent_anchor = joint_parent_xform[j][:3]
parent_xform_tf = wp.transform(*joint_parent_xform[j])
qd_start = joint_qd_start[j]
hinge_axis_local = wp.vec3(*joint_axis[qd_start])
# Rotate axis into the parent body frame (anchor data[0:3] is
# in the parent body frame, so the offset must be too).
hinge_axis = wp.quat_rotate(parent_xform_tf.q, hinge_axis_local)
offset = 0.1 # offset along axis for second constraint point
# First CONNECT at the joint anchor
eq1 = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_BODY)
eq1.type = mujoco.mjtEq.mjEQ_CONNECT
eq1.active = True
eq1.name1 = parent_name
eq1.name2 = child_name
eq1.data[0:3] = parent_anchor
mjc_eq_to_newton_jnt[eq1.id] = j
# Second CONNECT offset along the hinge axis
parent_anchor_offset = np.array(parent_anchor) + offset * np.array(hinge_axis)
eq2 = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_BODY)
eq2.type = mujoco.mjtEq.mjEQ_CONNECT
eq2.active = True
eq2.name1 = parent_name
eq2.name2 = child_name
eq2.data[0:3] = parent_anchor_offset
mjc_eq_to_newton_jnt[eq2.id] = j
elif lin_count == 0 and ang_count == 3:
# Ball loop joint: 1x CONNECT constrains 3 translational
# DOFs, leaving all 3 rotational DOFs free.
eq = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_BODY)
eq.type = mujoco.mjtEq.mjEQ_CONNECT
eq.active = True
eq.name1 = parent_name
eq.name2 = child_name
eq.data[0:3] = joint_parent_xform[j][:3]
mjc_eq_to_newton_jnt[eq.id] = j
else:
warnings.warn(
f"Loop joint {j} (type {JointType(j_type).name}, "
f"{lin_count} linear + {ang_count} angular DOFs) "
f"has no supported MuJoCo equality constraint mapping. "
f"Skipping loop closure for this joint.",
stacklevel=2,
)
continue
# add mimic constraints as mjEQ_JOINT equality constraints
mjc_eq_to_newton_mimic_dict = {}
for i in selected_mimic_constraints:
j0 = mimic_joint0[i] # follower
j1 = mimic_joint1[i] # leader
# check that both joints exist in the MuJoCo joint mapping
j0_name = joint_mapping.get(j0)
j1_name = joint_mapping.get(j1)
if j0_name is None or j1_name is None:
warnings.warn(
f"Skipping mimic constraint {i}: follower joint {j0} or leader joint {j1} "
f"not found in MuJoCo joint mapping.",
stacklevel=2,
)
continue
# mjEQ_JOINT only works with scalar joints (hinge/slide)
j0_type = joint_type[j0]
j1_type = joint_type[j1]
if j0_type not in (JointType.REVOLUTE, JointType.PRISMATIC):
warnings.warn(
f"Skipping mimic constraint {i}: follower joint {j0} has unsupported type "
f"{JointType(j0_type).name} for mjEQ_JOINT (only REVOLUTE and PRISMATIC supported).",
stacklevel=2,
)
continue
if j1_type not in (JointType.REVOLUTE, JointType.PRISMATIC):
warnings.warn(
f"Skipping mimic constraint {i}: leader joint {j1} has unsupported type "
f"{JointType(j1_type).name} for mjEQ_JOINT (only REVOLUTE and PRISMATIC supported).",
stacklevel=2,
)
continue
eq = spec.add_equality(objtype=mujoco.mjtObj.mjOBJ_JOINT)
eq.type = mujoco.mjtEq.mjEQ_JOINT
eq.active = bool(mimic_enabled[i])
eq.name1 = j0_name # follower (constrained joint)
eq.name2 = j1_name # leader (driving joint)
# polycoef: data[0] + data[1]*q2 + data[2]*q2^2 + ... - q1 = 0
# mimic: q1 = coef0 + coef1*q2
eq.data[0] = float(mimic_coef0[i])
eq.data[1] = float(mimic_coef1[i])
eq.data[2] = 0.0
eq.data[3] = 0.0
eq.data[4] = 0.0
mjc_eq_to_newton_mimic_dict[eq.id] = i
# Count non-colliding geoms that were kept because they are required by spatial tendons
tendon_extra_geoms = sum(
1
for s in tendon_required_shapes
if s in selected_shapes_set
and not (shape_flags[s] & ShapeFlags.SITE)
and not (shape_flags[s] & ShapeFlags.COLLIDE_SHAPES)
)
if skip_visual_only_geoms and len(spec.geoms) != colliding_shapes_per_world + tendon_extra_geoms:
raise ValueError(
"The number of geoms in the MuJoCo model does not match the number of colliding shapes in the Newton model."
)
if len(spec.bodies) != len(selected_bodies) + 1: # +1 for the world body
raise ValueError(
"The number of bodies in the MuJoCo model does not match the number of selected bodies in the Newton model. "
"Make sure that each body has an incoming joint and that the joints are part of an articulation."
)
# add contact exclusions between bodies to ensure parent <> child collisions are ignored
# even when one of the bodies is static
for b1, b2 in body_filters:
mb1, mb2 = body_mapping[b1], body_mapping[b2]
spec.add_exclude(bodyname1=spec.bodies[mb1].name, bodyname2=spec.bodies[mb2].name)
# add explicit contact pairs from custom attributes
self._init_pairs(model, spec, shape_mapping, first_world)
selected_tendons, mjc_tendon_names = self._init_tendons(
model, spec, joint_mapping, shape_mapping, site_mapping, first_world
)
# Process MuJoCo general actuators (motor, general, etc.) from custom attributes
actuator_count += self._init_actuators(
model,
spec,
first_world,
actuator_args,
mjc_actuator_ctrl_source_list,
mjc_actuator_to_newton_idx_list,
dof_to_mjc_joint,
mjc_joint_names,
selected_tendons,
mjc_tendon_names,
body_name_mapping,
)
# Convert actuator mapping lists to warp arrays
if mjc_actuator_ctrl_source_list:
self.mjc_actuator_ctrl_source = wp.array(
np.array(mjc_actuator_ctrl_source_list, dtype=np.int32),
dtype=wp.int32,
device=model.device,
)
self.mjc_actuator_to_newton_idx = wp.array(
np.array(mjc_actuator_to_newton_idx_list, dtype=np.int32),
dtype=wp.int32,
device=model.device,
)
else:
self.mjc_actuator_ctrl_source = None
self.mjc_actuator_to_newton_idx = None
self.mj_model = spec.compile()
self.mj_data = mujoco.MjData(self.mj_model)
# Build MuJoCo qpos/qvel start index arrays for coordinate conversion kernels.
# These map Newton template joint index → MuJoCo qpos/qvel start.
# Loop joints get -1 (they have no MuJoCo qpos/qvel slots).
# Must be created before _update_mjc_data which uses them.
n_template_joints = len(selected_joints)
mj_q_start_np = np.full(n_template_joints, -1, dtype=np.int32)
mj_qd_start_np = np.full(n_template_joints, -1, dtype=np.int32)
for j_template in range(n_template_joints):
j_idx = selected_joints[j_template]
mj_q_start_np[j_template] = joint_mjc_qpos_start[j_idx]
mj_qd_start_np[j_template] = joint_mjc_dof_start[j_idx]
# Validate that all non-loop joints got valid MuJoCo start indices
for j_template in range(n_template_joints):
j_idx = selected_joints[j_template]
if joint_articulation[j_idx] >= 0:
assert mj_q_start_np[j_template] >= 0, (
f"Non-loop joint {j_idx} (template {j_template}) has no MuJoCo qpos mapping"
)
assert mj_qd_start_np[j_template] >= 0, (
f"Non-loop joint {j_idx} (template {j_template}) has no MuJoCo DOF mapping"
)
self.mj_q_start = wp.array(mj_q_start_np, dtype=wp.int32, device=model.device)
self.mj_qd_start = wp.array(mj_qd_start_np, dtype=wp.int32, device=model.device)
self._update_mjc_data(self.mj_data, model, state)
# fill some MjWarp model fields that are outdated after _update_mjc_data.
# just setting qpos0 to d.qpos leads to weird behavior here, needs
# to be investigated.
mujoco.mj_forward(self.mj_model, self.mj_data)
if target_filename:
with open(target_filename, "w") as f:
f.write(spec.to_xml())
print(f"Saved mujoco model to {os.path.abspath(target_filename)}")
# now that the model is compiled, get the actual geom indices and compute
# shape transform corrections
shape_to_geom_idx = {}
geom_to_shape_idx = {}
for shape, geom_name in shape_mapping.items():
geom_idx = mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
if geom_idx >= 0:
shape_to_geom_idx[shape] = geom_idx
geom_to_shape_idx[geom_idx] = shape
with wp.ScopedDevice(model.device):
# create the MuJoCo Warp model
self.mjw_model = mujoco_warp.put_model(self.mj_model)
# patch mjw_model with mesh_pos if it doesn't have it
if not hasattr(self.mjw_model, "mesh_pos"):
self.mjw_model.mesh_pos = wp.array(self.mj_model.mesh_pos, dtype=wp.vec3)
# Determine nworld for mapping dimensions
nworld = model.world_count if separate_worlds else 1
# --- Create unified mappings: MuJoCo[world, entity] -> Newton[entity] ---
# Build geom -> shape mapping
# geom_to_shape_idx maps from MuJoCo geom index to absolute Newton shape index.
# Convert non-static shapes to template-relative indices for the kernel.
geom_to_shape_idx_np = np.full((self.mj_model.ngeom,), -1, dtype=np.int32)
# Find the minimum shape index for the first non-static group to use as the base
first_env_shape_base = int(np.min(first_env_shapes)) if len(first_env_shapes) > 0 else 0
# Store for lazy inverse creation
self._shapes_per_world = len(first_env_shapes)
self._first_env_shape_base = first_env_shape_base
# Per-geom static mask (True if static, False otherwise)
geom_is_static_np = np.zeros((self.mj_model.ngeom,), dtype=bool)
for geom_idx, abs_shape_idx in geom_to_shape_idx.items():
if shape_world[abs_shape_idx] < 0:
# Static shape - use absolute index and mark mask
geom_to_shape_idx_np[geom_idx] = abs_shape_idx
geom_is_static_np[geom_idx] = True
else:
# Non-static shape - convert to template-relative offset from first env base
geom_to_shape_idx_np[geom_idx] = abs_shape_idx - first_env_shape_base
geom_to_shape_idx_wp = wp.array(geom_to_shape_idx_np, dtype=wp.int32)
geom_is_static_wp = wp.array(geom_is_static_np, dtype=bool)
# Create mjc_geom_to_newton_shape: MuJoCo[world, geom] -> Newton shape
self.mjc_geom_to_newton_shape = wp.full((nworld, self.mj_model.ngeom), -1, dtype=wp.int32)
if self.mjw_model.geom_pos.size:
wp.launch(
update_shape_mappings_kernel,
dim=(nworld, self.mj_model.ngeom),
inputs=[
geom_to_shape_idx_wp,
geom_is_static_wp,
self._shapes_per_world,
first_env_shape_base,
],
outputs=[
self.mjc_geom_to_newton_shape,
],
device=model.device,
)
# Create mjc_body_to_newton: MuJoCo[world, body] -> Newton body
# body_mapping is {newton_body_id: mjc_body_id}, we need to invert it
# and expand to 2D for all worlds
nbody = self.mj_model.nbody
bodies_per_world = model.body_count // model.world_count
mjc_body_to_newton_np = np.full((nworld, nbody), -1, dtype=np.int32)
for newton_body, mjc_body in body_mapping.items():
if newton_body >= 0: # Skip world body (-1 -> 0)
newton_body_in_world = newton_body % bodies_per_world
for w in range(nworld):
mjc_body_to_newton_np[w, mjc_body] = w * bodies_per_world + newton_body_in_world
self.mjc_body_to_newton = wp.array(mjc_body_to_newton_np, dtype=wp.int32)
# Common variables for mapping creation
njnt = self.mj_model.njnt
joints_per_world = model.joint_count // model.world_count
dofs_per_world = model.joint_dof_count // model.world_count
# Map each Newton body to the qd_start of its free/DISTANCE joint (or -1).
# Use selected_joints as the template and tile offsets across worlds.
joint_type_np = model.joint_type.numpy()
joint_child_np = model.joint_child.numpy()
joint_qd_start_np = model.joint_qd_start.numpy()
joint_dof_dim_np = model.joint_dof_dim.numpy()
# Map each Newton DOF to the child body of its parent joint.
# This is used to apply kinematic body flags to MuJoCo dof_armature.
newton_dof_to_body_np = np.full(model.joint_dof_count, -1, dtype=np.int32)
for joint_idx in range(model.joint_count):
dof_start = int(joint_qd_start_np[joint_idx])
dof_count = int(joint_dof_dim_np[joint_idx, 0] + joint_dof_dim_np[joint_idx, 1])
if dof_count > 0:
newton_dof_to_body_np[dof_start : dof_start + dof_count] = int(joint_child_np[joint_idx])
self.newton_dof_to_body = wp.array(newton_dof_to_body_np, dtype=wp.int32)
template_joint_types = joint_type_np[selected_joints]
free_mask = np.isin(template_joint_types, (JointType.FREE, JointType.DISTANCE))
body_free_qd_start_np = np.full(model.body_count, -1, dtype=np.int32)
if np.any(free_mask):
template_children = joint_child_np[selected_joints] % bodies_per_world
template_qd_start = joint_qd_start_np[selected_joints] % dofs_per_world
child_free = template_children[free_mask]
qd_start_free = template_qd_start[free_mask]
world_body_offsets = (np.arange(model.world_count, dtype=np.int32) * bodies_per_world)[:, None]
world_qd_offsets = (np.arange(model.world_count, dtype=np.int32) * dofs_per_world)[:, None]
body_indices = (child_free[None, :] + world_body_offsets).ravel()
qd_starts = (qd_start_free[None, :] + world_qd_offsets).ravel()
body_free_qd_start_np[body_indices] = qd_starts
self.body_free_qd_start = wp.array(body_free_qd_start_np, dtype=wp.int32)
# Create mjc_mocap_to_newton_jnt: MuJoCo[world, mocap] -> Newton joint index.
# These mocap bodies are Newton roots attached to world by a
# FIXED joint. Static world shapes are not represented here.
nmocap = self.mj_model.nmocap
if nmocap > 0:
mjc_mocap_to_newton_jnt_np = np.full((nworld, nmocap), -1, dtype=np.int32)
body_mocapid = self.mj_model.body_mocapid
for mjc_body in range(nbody):
mocap_idx = body_mocapid[mjc_body]
if mocap_idx < 0:
continue
newton_body = mjc_body_to_newton_np[0, mjc_body]
if newton_body < 0:
continue
newton_body_template = newton_body % bodies_per_world
for j in range(joints_per_world):
if joint_child_np[j] == newton_body_template:
for w in range(nworld):
mjc_mocap_to_newton_jnt_np[w, mocap_idx] = w * joints_per_world + j
break
self.mjc_mocap_to_newton_jnt = wp.array(mjc_mocap_to_newton_jnt_np, dtype=wp.int32)
else:
self.mjc_mocap_to_newton_jnt = None
# Create mjc_jnt_to_newton_jnt: MuJoCo[world, joint] -> Newton joint index
# selected_joints[idx] is the Newton template joint index
mjc_jnt_to_newton_jnt_np = np.full((nworld, njnt), -1, dtype=np.int32)
# Invert dof_to_mjc_joint to get mjc_jnt -> template_dof, then find the joint
for template_dof, mjc_jnt in enumerate(dof_to_mjc_joint):
if mjc_jnt >= 0:
# Find which Newton template joint contains this DOF
# This is the first DOF of the joint, so we can search for it
for _ji, j in enumerate(selected_joints):
j_dof_start = joint_qd_start[j] % dofs_per_world
j_lin_count, j_ang_count = joint_dof_dim[j]
j_dof_end = j_dof_start + j_lin_count + j_ang_count
if j_dof_start <= template_dof < j_dof_end:
for w in range(nworld):
mjc_jnt_to_newton_jnt_np[w, mjc_jnt] = w * joints_per_world + j
break
self.mjc_jnt_to_newton_jnt = wp.array(mjc_jnt_to_newton_jnt_np, dtype=wp.int32)
# Create mjc_jnt_to_newton_dof: MuJoCo[world, joint] -> Newton DOF start
# joint_mjc_dof_start[template_joint] -> mjc_dof_start
# dof_to_mjc_joint[template_dof] -> mjc_joint
mjc_jnt_to_newton_dof_np = np.full((nworld, njnt), -1, dtype=np.int32)
for template_dof, mjc_jnt in enumerate(dof_to_mjc_joint):
if mjc_jnt >= 0:
for w in range(nworld):
mjc_jnt_to_newton_dof_np[w, mjc_jnt] = w * dofs_per_world + template_dof
self.mjc_jnt_to_newton_dof = wp.array(mjc_jnt_to_newton_dof_np, dtype=wp.int32)
# Create mjc_dof_to_newton_dof: MuJoCo[world, dof] -> Newton DOF
nv = self.mj_model.nv # Number of DOFs in MuJoCo
mjc_dof_to_newton_dof_np = np.full((nworld, nv), -1, dtype=np.int32)
# joint_mjc_dof_start tells us where each Newton template joint's DOFs start in MuJoCo
for j, mjc_dof_start in enumerate(joint_mjc_dof_start):
if mjc_dof_start >= 0:
newton_dof_start = joint_qd_start[j]
lin_count, ang_count = joint_dof_dim[j]
total_dofs = lin_count + ang_count
for d in range(total_dofs):
mjc_dof = mjc_dof_start + d
template_newton_dof = (newton_dof_start % dofs_per_world) + d
for w in range(nworld):
mjc_dof_to_newton_dof_np[w, mjc_dof] = w * dofs_per_world + template_newton_dof
self.mjc_dof_to_newton_dof = wp.array(mjc_dof_to_newton_dof_np, dtype=wp.int32)
# Create mjc_eq_to_newton_eq: MuJoCo[world, eq] -> Newton equality constraint
# selected_constraints[idx] is the Newton template constraint index
neq = self.mj_model.neq
eq_constraints_per_world = model.equality_constraint_count // model.world_count
mjc_eq_to_newton_eq_np = np.full((nworld, neq), -1, dtype=np.int32)
mjc_eq_to_newton_jnt_np = np.full((nworld, neq), -1, dtype=np.int32)
for mjc_eq, newton_eq in enumerate(selected_constraints):
template_eq = newton_eq % eq_constraints_per_world if eq_constraints_per_world > 0 else newton_eq
for w in range(nworld):
mjc_eq_to_newton_eq_np[w, mjc_eq] = w * eq_constraints_per_world + template_eq
for mjc_eq, newton_jnt in mjc_eq_to_newton_jnt.items():
for w in range(nworld):
mjc_eq_to_newton_jnt_np[w, mjc_eq] = w * joints_per_world + newton_jnt
self.mjc_eq_to_newton_eq = wp.array(mjc_eq_to_newton_eq_np, dtype=wp.int32)
self.mjc_eq_to_newton_jnt = wp.array(mjc_eq_to_newton_jnt_np, dtype=wp.int32)
# Create mjc_eq_to_newton_mimic: MuJoCo[world, eq] -> Newton mimic constraint
mimic_per_world = (
model.constraint_mimic_count // model.world_count
if model.world_count > 0
else model.constraint_mimic_count
)
mjc_eq_to_newton_mimic_np = np.full((nworld, neq), -1, dtype=np.int32)
for mjc_eq, newton_mimic in mjc_eq_to_newton_mimic_dict.items():
template_mimic = newton_mimic % mimic_per_world if mimic_per_world > 0 else newton_mimic
for w in range(nworld):
mjc_eq_to_newton_mimic_np[w, mjc_eq] = w * mimic_per_world + template_mimic
self.mjc_eq_to_newton_mimic = wp.array(mjc_eq_to_newton_mimic_np, dtype=wp.int32)
# Create mjc_tendon_to_newton_tendon: MuJoCo[world, tendon] -> Newton tendon
# selected_tendons[idx] is the Newton template tendon index
ntendon = self.mj_model.ntendon
if ntendon > 0:
# Get tendon count per world from custom attributes
mujoco_attrs = getattr(model, "mujoco", None)
tendon_world = getattr(mujoco_attrs, "tendon_world", None) if mujoco_attrs else None
if tendon_world is not None:
total_tendons = len(tendon_world)
tendons_per_world = total_tendons // model.world_count if model.world_count > 0 else total_tendons
else:
tendons_per_world = ntendon
mjc_tendon_to_newton_tendon_np = np.full((nworld, ntendon), -1, dtype=np.int32)
for mjc_tendon, newton_tendon in enumerate(selected_tendons):
template_tendon = newton_tendon % tendons_per_world if tendons_per_world > 0 else newton_tendon
for w in range(nworld):
mjc_tendon_to_newton_tendon_np[w, mjc_tendon] = w * tendons_per_world + template_tendon
self.mjc_tendon_to_newton_tendon = wp.array(mjc_tendon_to_newton_tendon_np, dtype=wp.int32)
# set mjwarp-only settings
self.mjw_model.opt.ls_parallel = ls_parallel
if separate_worlds:
nworld = model.world_count
else:
nworld = 1
# TODO find better heuristics to determine nconmax and njmax
if disable_contacts:
nconmax = 0
elif nconmax is not None and nconmax < self.mj_data.ncon:
warnings.warn(
f"[WARNING] Value for nconmax is changed from {nconmax} to {self.mj_data.ncon} following an MjWarp requirement.",
stacklevel=2,
)
nconmax = self.mj_data.ncon
if njmax is not None and njmax < self.mj_data.nefc:
warnings.warn(
f"[WARNING] Value for njmax is changed from {njmax} to {self.mj_data.nefc} following an MjWarp requirement.",
stacklevel=2,
)
njmax = self.mj_data.nefc
self.mjw_data = mujoco_warp.put_data(
self.mj_model,
self.mj_data,
nworld=nworld,
nconmax=nconmax,
njmax=njmax,
)
# expand model fields that can be expanded:
self._expand_model_fields(self.mjw_model, nworld)
# update solver options from Newton model (only if not overridden by constructor)
self._update_solver_options(overridden_options=overridden_options)
# so far we have only defined the first world,
# now complete the data from the Newton model
self.notify_model_changed(SolverNotifyFlags.ALL)
def _expand_model_fields(self, mj_model: MjWarpModel, nworld: int):
if nworld == 1:
return
model_fields_to_expand = {
"qpos0",
"qpos_spring",
"body_pos",
"body_quat",
"body_ipos",
"body_iquat",
"body_mass",
"body_subtreemass", # Derived from body_mass, computed by set_const_fixed
"body_inertia",
"body_invweight0", # Derived from inertia, computed by set_const_0
"body_gravcomp",
"jnt_solref",
"jnt_solimp",
"jnt_pos",
"jnt_axis",
"jnt_stiffness",
"jnt_range",
"jnt_actfrcrange", # joint-level actuator force range (effort limit)
"jnt_margin", # corresponds to newton custom attribute "limit_margin"
"dof_armature",
"dof_damping",
"dof_invweight0", # Derived from inertia, computed by set_const_0
"dof_frictionloss",
"dof_solimp",
"dof_solref",
# "geom_matid",
"geom_solmix",
"geom_solref",
"geom_solimp",
"geom_size",
"geom_rbound",
"geom_pos",
"geom_quat",
"geom_friction",
"geom_margin",
"geom_gap",
# "geom_rgba",
# "site_pos",
# "site_quat",
# "cam_pos",
# "cam_quat",
# "cam_poscom0",
# "cam_pos0",
# "cam_mat0",
# "light_pos",
# "light_dir",
# "light_poscom0",
# "light_pos0",
"eq_solref",
"eq_solimp",
"eq_data",
# "actuator_dynprm",
"actuator_gainprm",
"actuator_biasprm",
"actuator_dynprm",
"actuator_ctrlrange",
"actuator_forcerange",
"actuator_actrange",
"actuator_gear",
"actuator_cranklength",
"actuator_acc0",
"pair_solref",
"pair_solreffriction",
"pair_solimp",
"pair_margin",
"pair_gap",
"pair_friction",
"tendon_world",
"tendon_solref_lim",
"tendon_solimp_lim",
"tendon_solref_fri",
"tendon_solimp_fri",
"tendon_range",
"tendon_actfrcrange",
"tendon_margin",
"tendon_stiffness",
"tendon_damping",
"tendon_armature",
"tendon_frictionloss",
"tendon_lengthspring",
"tendon_length0", # Derived from tendon config, computed by set_const_0
"tendon_invweight0", # Derived from inertia, computed by set_const_0
# "mat_rgba",
}
# Solver option fields to expand (nested in mj_model.opt)
opt_fields_to_expand = {
# "timestep", # Excluded: conflicts with step() function parameter
"impratio_invsqrt",
"tolerance",
"ls_tolerance",
"ccd_tolerance",
"density",
"viscosity",
"gravity",
"wind",
"magnetic",
}
def tile(x: wp.array):
# Create new array with same shape but first dim multiplied by nworld
new_shape = list(x.shape)
new_shape[0] = nworld
wp_array = {1: wp.array, 2: wp.array2d, 3: wp.array3d, 4: wp.array4d}[len(new_shape)]
dst = wp_array(shape=new_shape, dtype=x.dtype, device=x.device)
# Flatten arrays for kernel
src_flat = x.flatten()
dst_flat = dst.flatten()
# Launch kernel to repeat data - one thread per destination element
n_elems_per_world = dst_flat.shape[0] // nworld
wp.launch(
repeat_array_kernel,
dim=dst_flat.shape[0],
inputs=[src_flat, n_elems_per_world],
outputs=[dst_flat],
device=x.device,
)
return dst
for field in mj_model.__dataclass_fields__:
if field in model_fields_to_expand:
array = getattr(mj_model, field)
setattr(mj_model, field, tile(array))
for field in mj_model.opt.__dataclass_fields__:
if field in opt_fields_to_expand:
array = getattr(mj_model.opt, field)
setattr(mj_model.opt, field, tile(array))
def _update_solver_options(self, overridden_options: set[str] | None = None):
"""Update WORLD frequency solver options from Newton model to MuJoCo Warp.
Called after tile() to update per-world option arrays in mjw_model.opt.
Only updates WORLD frequency options; ONCE frequency options are already
set on MjSpec before put_model() and shared across all worlds.
Args:
overridden_options: Set of option names that were overridden by constructor.
These options should not be updated from model custom attributes.
"""
if overridden_options is None:
overridden_options = set()
mujoco_attrs = getattr(self.model, "mujoco", None)
nworld = self.model.world_count
# Helper to get WORLD frequency option array or None
def get_option(name: str):
if name in overridden_options or not mujoco_attrs or not hasattr(mujoco_attrs, name):
return None
return getattr(mujoco_attrs, name)
# Get all WORLD frequency scalar arrays
newton_impratio = get_option("impratio")
newton_tolerance = get_option("tolerance")
newton_ls_tolerance = get_option("ls_tolerance")
newton_ccd_tolerance = get_option("ccd_tolerance")
newton_density = get_option("density")
newton_viscosity = get_option("viscosity")
# Get WORLD frequency vector arrays
newton_wind = get_option("wind")
newton_magnetic = get_option("magnetic")
# Skip kernel if all options are None
if all(
x is None
for x in [
newton_impratio,
newton_tolerance,
newton_ls_tolerance,
newton_ccd_tolerance,
newton_density,
newton_viscosity,
newton_wind,
newton_magnetic,
]
):
return
wp.launch(
update_solver_options_kernel,
dim=nworld,
inputs=[
newton_impratio,
newton_tolerance,
newton_ls_tolerance,
newton_ccd_tolerance,
newton_density,
newton_viscosity,
newton_wind,
newton_magnetic,
],
outputs=[
self.mjw_model.opt.impratio_invsqrt,
self.mjw_model.opt.tolerance,
self.mjw_model.opt.ls_tolerance,
self.mjw_model.opt.ccd_tolerance,
self.mjw_model.opt.density,
self.mjw_model.opt.viscosity,
self.mjw_model.opt.wind,
self.mjw_model.opt.magnetic,
],
device=self.model.device,
)
def _update_model_inertial_properties(self):
if self.model.body_count == 0:
return
# Get gravcomp if available
mujoco_attrs = getattr(self.model, "mujoco", None)
gravcomp = getattr(mujoco_attrs, "gravcomp", None) if mujoco_attrs is not None else None
# Launch over MuJoCo bodies [nworld, nbody]
nworld = self.mjc_body_to_newton.shape[0]
nbody = self.mjc_body_to_newton.shape[1]
wp.launch(
update_body_mass_ipos_kernel,
dim=(nworld, nbody),
inputs=[
self.mjc_body_to_newton,
self.model.body_com,
self.model.body_mass,
gravcomp,
],
outputs=[
self.mjw_model.body_ipos,
self.mjw_model.body_mass,
self.mjw_model.body_gravcomp,
],
device=self.model.device,
)
wp.launch(
update_body_inertia_kernel,
dim=(nworld, nbody),
inputs=[
self.mjc_body_to_newton,
self.model.body_inertia,
],
outputs=[self.mjw_model.body_inertia, self.mjw_model.body_iquat],
device=self.model.device,
)
def _update_body_properties(self):
"""Update body-property dependent MuJoCo DOF parameters.
This currently applies kinematic body flags by rewriting MuJoCo
``dof_armature`` from Newton ``body_flags`` and ``joint_armature``.
"""
if self.model.joint_dof_count == 0:
return
if self.mjc_dof_to_newton_dof is None or self.newton_dof_to_body is None:
return
nworld = self.mjc_dof_to_newton_dof.shape[0]
nv = self.mjc_dof_to_newton_dof.shape[1]
wp.launch(
update_body_properties_kernel,
dim=(nworld, nv),
inputs=[
self.mjc_dof_to_newton_dof,
self.newton_dof_to_body,
self.model.body_flags,
self.model.joint_armature,
self._KINEMATIC_ARMATURE,
],
outputs=[self.mjw_model.dof_armature],
device=self.model.device,
)
def _update_joint_dof_properties(self):
"""Update joint DOF properties in the MuJoCo model.
Updates effort limits, friction, damping, solimp/solref, passive
stiffness, and limit ranges. Armature is updated for dynamic DOFs only;
DOFs attached to kinematic bodies are preserved.
"""
if self.model.joint_dof_count == 0:
return
if self.newton_dof_to_body is None:
return
# Update actuator gains for JOINT_TARGET mode actuators
if self.mjc_actuator_ctrl_source is not None and self.mjc_actuator_to_newton_idx is not None:
nu = self.mjc_actuator_ctrl_source.shape[0]
nworld = self.mjw_model.actuator_biasprm.shape[0]
dofs_per_world = self.model.joint_dof_count // nworld if nworld > 0 else self.model.joint_dof_count
wp.launch(
update_axis_properties_kernel,
dim=(nworld, nu),
inputs=[
self.mjc_actuator_ctrl_source,
self.mjc_actuator_to_newton_idx,
self.model.joint_target_ke,
self.model.joint_target_kd,
self.model.joint_target_mode,
dofs_per_world,
],
outputs=[
self.mjw_model.actuator_biasprm,
self.mjw_model.actuator_gainprm,
],
device=self.model.device,
)
# Update DOF properties (armature, friction, damping, solimp, solref) - iterate over MuJoCo DOFs
mujoco_attrs = getattr(self.model, "mujoco", None)
joint_damping = getattr(mujoco_attrs, "dof_passive_damping", None) if mujoco_attrs is not None else None
dof_solimp = getattr(mujoco_attrs, "solimpfriction", None) if mujoco_attrs is not None else None
dof_solref = getattr(mujoco_attrs, "solreffriction", None) if mujoco_attrs is not None else None
nworld = self.mjc_dof_to_newton_dof.shape[0]
nv = self.mjc_dof_to_newton_dof.shape[1]
wp.launch(
update_dof_properties_kernel,
dim=(nworld, nv),
inputs=[
self.mjc_dof_to_newton_dof,
self.newton_dof_to_body,
self.model.body_flags,
self.model.joint_armature,
self.model.joint_friction,
joint_damping,
dof_solimp,
dof_solref,
],
outputs=[
self.mjw_model.dof_armature,
self.mjw_model.dof_frictionloss,
self.mjw_model.dof_damping,
self.mjw_model.dof_solimp,
self.mjw_model.dof_solref,
],
device=self.model.device,
)
# Update joint properties (limits, stiffness, solref, solimp) - iterate over MuJoCo joints
solimplimit = getattr(mujoco_attrs, "solimplimit", None) if mujoco_attrs is not None else None
joint_dof_limit_margin = getattr(mujoco_attrs, "limit_margin", None) if mujoco_attrs is not None else None
joint_stiffness = getattr(mujoco_attrs, "dof_passive_stiffness", None) if mujoco_attrs is not None else None
njnt = self.mjc_jnt_to_newton_dof.shape[1]
wp.launch(
update_jnt_properties_kernel,
dim=(nworld, njnt),
inputs=[
self.mjc_jnt_to_newton_dof,
self.model.joint_limit_ke,
self.model.joint_limit_kd,
self.model.joint_limit_lower,
self.model.joint_limit_upper,
self.model.joint_effort_limit,
solimplimit,
joint_stiffness,
joint_dof_limit_margin,
],
outputs=[
self.mjw_model.jnt_solimp,
self.mjw_model.jnt_solref,
self.mjw_model.jnt_stiffness,
self.mjw_model.jnt_margin,
self.mjw_model.jnt_range,
self.mjw_model.jnt_actfrcrange,
],
device=self.model.device,
)
# Sync qpos0 and qpos_spring from Newton model data before set_const.
# set_const copies qpos0 → d.qpos and runs FK to compute derived fields,
# so qpos0 must be correct before calling it.
dof_ref = getattr(mujoco_attrs, "dof_ref", None) if mujoco_attrs is not None else None
dof_springref = getattr(mujoco_attrs, "dof_springref", None) if mujoco_attrs is not None else None
joints_per_world = self.model.joint_count // nworld
bodies_per_world = self.model.body_count // nworld
wp.launch(
sync_qpos0_kernel,
dim=(nworld, joints_per_world),
inputs=[
joints_per_world,
bodies_per_world,
self.model.joint_type,
self.model.joint_q_start,
self.model.joint_qd_start,
self.model.joint_dof_dim,
self.model.joint_child,
self.model.body_q,
dof_ref,
dof_springref,
self.mj_q_start,
],
outputs=[
self.mjw_model.qpos0,
self.mjw_model.qpos_spring,
],
device=self.model.device,
)
def _update_joint_properties(self):
"""Update joint properties including joint positions, joint axes, and relative body transforms in the MuJoCo model."""
if self.model.joint_count == 0:
return
# Update mocap body transforms first (fixed-root bodies have no MuJoCo joints).
if self.mjc_mocap_to_newton_jnt is not None and self.mjc_mocap_to_newton_jnt.shape[1] > 0:
nworld = self.mjc_mocap_to_newton_jnt.shape[0]
nmocap = self.mjc_mocap_to_newton_jnt.shape[1]
wp.launch(
update_mocap_transforms_kernel,
dim=(nworld, nmocap),
inputs=[
self.mjc_mocap_to_newton_jnt,
self.model.joint_X_p,
self.model.joint_X_c,
],
outputs=[
self.mjw_data.mocap_pos,
self.mjw_data.mocap_quat,
],
device=self.model.device,
)
# Update joint positions, joint axes, and relative body transforms
# Iterates over MuJoCo joints [world, jnt]
if self.mjc_jnt_to_newton_jnt is not None and self.mjc_jnt_to_newton_jnt.shape[1] > 0:
nworld = self.mjc_jnt_to_newton_jnt.shape[0]
njnt = self.mjc_jnt_to_newton_jnt.shape[1]
wp.launch(
update_joint_transforms_kernel,
dim=(nworld, njnt),
inputs=[
self.mjc_jnt_to_newton_jnt,
self.mjc_jnt_to_newton_dof,
self.mjw_model.jnt_bodyid,
self.mjw_model.jnt_type,
# Newton model data (joint-indexed)
self.model.joint_X_p,
self.model.joint_X_c,
# Newton model data (DOF-indexed)
self.model.joint_axis,
],
outputs=[
self.mjw_model.jnt_pos,
self.mjw_model.jnt_axis,
self.mjw_model.body_pos,
self.mjw_model.body_quat,
],
device=self.model.device,
)
def _update_geom_properties(self):
"""Update geom properties including collision radius, friction, and contact parameters in the MuJoCo model."""
# Get number of geoms and worlds from MuJoCo model
num_geoms = self.mj_model.ngeom
if num_geoms == 0:
return
world_count = self.mjc_geom_to_newton_shape.shape[0]
# Get custom attribute for geom_solimp and geom_solmix
mujoco_attrs = getattr(self.model, "mujoco", None)
shape_geom_solimp = getattr(mujoco_attrs, "geom_solimp", None) if mujoco_attrs is not None else None
shape_geom_solmix = getattr(mujoco_attrs, "geom_solmix", None) if mujoco_attrs is not None else None
wp.launch(
update_geom_properties_kernel,
dim=(world_count, num_geoms),
inputs=[
self.model.shape_material_mu,
self.model.shape_material_ke,
self.model.shape_material_kd,
self.model.shape_scale,
self.model.shape_transform,
self.mjc_geom_to_newton_shape,
self.mjw_model.geom_type,
self._mujoco.mjtGeom.mjGEOM_MESH,
self.mjw_model.geom_dataid,
self.mjw_model.mesh_pos,
self.mjw_model.mesh_quat,
self.model.shape_material_mu_torsional,
self.model.shape_material_mu_rolling,
shape_geom_solimp,
shape_geom_solmix,
self.model.shape_margin,
],
outputs=[
self.mjw_model.geom_friction,
self.mjw_model.geom_solref,
self.mjw_model.geom_size,
self.mjw_model.geom_pos,
self.mjw_model.geom_quat,
self.mjw_model.geom_solimp,
self.mjw_model.geom_solmix,
self.mjw_model.geom_gap,
self.mjw_model.geom_margin,
],
device=self.model.device,
)
def _update_pair_properties(self):
"""Update MuJoCo contact pair properties from Newton custom attributes.
Updates the randomizable pair properties (solref, solreffriction, solimp,
margin, gap, friction) for explicit contact pairs defined in the model.
"""
if self.use_mujoco_cpu:
return # CPU mode not supported for pair runtime updates
npair = self.mj_model.npair
if npair == 0:
return
# Get custom attributes for pair properties
mujoco_attrs = getattr(self.model, "mujoco", None)
if mujoco_attrs is None:
return
pair_solref = getattr(mujoco_attrs, "pair_solref", None)
pair_solreffriction = getattr(mujoco_attrs, "pair_solreffriction", None)
pair_solimp = getattr(mujoco_attrs, "pair_solimp", None)
pair_margin = getattr(mujoco_attrs, "pair_margin", None)
pair_gap = getattr(mujoco_attrs, "pair_gap", None)
pair_friction = getattr(mujoco_attrs, "pair_friction", None)
# Only launch kernel if at least one attribute is defined
if any(
attr is not None
for attr in [pair_solref, pair_solreffriction, pair_solimp, pair_margin, pair_gap, pair_friction]
):
# Compute pairs_per_world from Newton custom attributes
pair_world_attr = getattr(mujoco_attrs, "pair_world", None)
if pair_world_attr is not None:
total_pairs = len(pair_world_attr)
pairs_per_world = total_pairs // self.model.world_count
else:
pairs_per_world = npair
world_count = self.mjw_data.nworld
wp.launch(
update_pair_properties_kernel,
dim=(world_count, npair),
inputs=[
pairs_per_world,
pair_solref,
pair_solreffriction,
pair_solimp,
pair_margin,
pair_gap,
pair_friction,
],
outputs=[
self.mjw_model.pair_solref,
self.mjw_model.pair_solreffriction,
self.mjw_model.pair_solimp,
self.mjw_model.pair_margin,
self.mjw_model.pair_gap,
self.mjw_model.pair_friction,
],
device=self.model.device,
)
def _update_model_properties(self):
"""Update model properties including gravity in the MuJoCo model."""
if self.use_mujoco_cpu:
self.mj_model.opt.gravity[:] = np.array([*self.model.gravity.numpy()[0]])
else:
if hasattr(self, "mjw_data"):
wp.launch(
kernel=update_model_properties_kernel,
dim=self.mjw_data.nworld,
inputs=[
self.model.gravity,
],
outputs=[
self.mjw_model.opt.gravity,
],
device=self.model.device,
)
def _update_eq_properties(self):
"""Update equality constraint properties in the MuJoCo model.
Updates:
- eq_solref/eq_solimp from MuJoCo custom attributes (if set)
- eq_data from Newton's equality_constraint_anchor, equality_constraint_relpose,
equality_constraint_polycoef, equality_constraint_torquescale
- eq_active from Newton's equality_constraint_enabled
.. note::
Note this update only affects the equality constraints explicitly defined in Newton,
not the equality constraints defined for joints that are excluded from articulations
(i.e. joints that have joint_articulation == -1, for example loop-closing joints).
Equality constraints for these joints are defined after the regular equality constraints
in the MuJoCo model."""
if self.model.equality_constraint_count == 0:
return
neq = self.mj_model.neq
if neq == 0:
return
world_count = self.mjc_eq_to_newton_eq.shape[0]
# Get custom attributes for eq_solref and eq_solimp
mujoco_attrs = getattr(self.model, "mujoco", None)
eq_solref = getattr(mujoco_attrs, "eq_solref", None) if mujoco_attrs is not None else None
eq_solimp = getattr(mujoco_attrs, "eq_solimp", None) if mujoco_attrs is not None else None
if eq_solref is not None or eq_solimp is not None:
wp.launch(
update_eq_properties_kernel,
dim=(world_count, neq),
inputs=[
self.mjc_eq_to_newton_eq,
eq_solref,
eq_solimp,
],
outputs=[
self.mjw_model.eq_solref,
self.mjw_model.eq_solimp,
],
device=self.model.device,
)
# Update eq_data and eq_active from Newton equality constraint properties
wp.launch(
update_eq_data_and_active_kernel,
dim=(world_count, neq),
inputs=[
self.mjc_eq_to_newton_eq,
self.model.equality_constraint_type,
self.model.equality_constraint_anchor,
self.model.equality_constraint_relpose,
self.model.equality_constraint_polycoef,
self.model.equality_constraint_torquescale,
self.model.equality_constraint_enabled,
],
outputs=[
self.mjw_model.eq_data,
self.mjw_data.eq_active,
],
device=self.model.device,
)
def _update_mimic_eq_properties(self):
"""Update mimic constraint properties in the MuJoCo model.
Updates:
- eq_data from Newton's constraint_mimic_coef0, constraint_mimic_coef1
- eq_active from Newton's constraint_mimic_enabled
Maps mimic constraints to MuJoCo mjEQ_JOINT equality constraints
using the polycoef representation: q1 = coef0 + coef1 * q2.
"""
if self.model.constraint_mimic_count == 0 or self.mjc_eq_to_newton_mimic is None:
return
neq = self.mj_model.neq
if neq == 0:
return
world_count = self.mjc_eq_to_newton_mimic.shape[0]
wp.launch(
update_mimic_eq_data_and_active_kernel,
dim=(world_count, neq),
inputs=[
self.mjc_eq_to_newton_mimic,
self.model.constraint_mimic_coef0,
self.model.constraint_mimic_coef1,
self.model.constraint_mimic_enabled,
],
outputs=[
self.mjw_model.eq_data,
self.mjw_data.eq_active,
],
device=self.model.device,
)
def _update_tendon_properties(self):
"""Update fixed tendon properties in the MuJoCo model.
Updates tendon stiffness, damping, frictionloss, range, margin, solref, solimp,
armature, and actfrcrange from Newton custom attributes.
"""
if self.mjc_tendon_to_newton_tendon is None:
return
ntendon = self.mj_model.ntendon
if ntendon == 0:
return
world_count = self.mjc_tendon_to_newton_tendon.shape[0]
# Get custom attributes for tendons
mujoco_attrs = getattr(self.model, "mujoco", None)
if mujoco_attrs is None:
return
# Get tendon custom attributes (may be None if not defined)
# Note: tendon_springlength is NOT updated at runtime because it has special
# initialization semantics in MuJoCo (value -1.0 means auto-compute from initial state).
tendon_stiffness = getattr(mujoco_attrs, "tendon_stiffness", None)
tendon_damping = getattr(mujoco_attrs, "tendon_damping", None)
tendon_frictionloss = getattr(mujoco_attrs, "tendon_frictionloss", None)
tendon_range = getattr(mujoco_attrs, "tendon_range", None)
tendon_margin = getattr(mujoco_attrs, "tendon_margin", None)
tendon_solref_limit = getattr(mujoco_attrs, "tendon_solref_limit", None)
tendon_solimp_limit = getattr(mujoco_attrs, "tendon_solimp_limit", None)
tendon_solref_friction = getattr(mujoco_attrs, "tendon_solref_friction", None)
tendon_solimp_friction = getattr(mujoco_attrs, "tendon_solimp_friction", None)
tendon_armature = getattr(mujoco_attrs, "tendon_armature", None)
tendon_actfrcrange = getattr(mujoco_attrs, "tendon_actuator_force_range", None)
wp.launch(
update_tendon_properties_kernel,
dim=(world_count, ntendon),
inputs=[
self.mjc_tendon_to_newton_tendon,
tendon_stiffness,
tendon_damping,
tendon_frictionloss,
tendon_range,
tendon_margin,
tendon_solref_limit,
tendon_solimp_limit,
tendon_solref_friction,
tendon_solimp_friction,
tendon_armature,
tendon_actfrcrange,
],
outputs=[
self.mjw_model.tendon_stiffness,
self.mjw_model.tendon_damping,
self.mjw_model.tendon_frictionloss,
self.mjw_model.tendon_range,
self.mjw_model.tendon_margin,
self.mjw_model.tendon_solref_lim,
self.mjw_model.tendon_solimp_lim,
self.mjw_model.tendon_solref_fri,
self.mjw_model.tendon_solimp_fri,
self.mjw_model.tendon_armature,
self.mjw_model.tendon_actfrcrange,
],
device=self.model.device,
)
def _update_actuator_properties(self):
"""Update CTRL_DIRECT actuator properties in the MuJoCo model.
Only updates actuators that use CTRL_DIRECT mode. JOINT_TARGET actuators are
updated via _update_joint_dof_properties() using joint_target_ke/kd.
"""
if self.mjc_actuator_ctrl_source is None or self.mjc_actuator_to_newton_idx is None:
return
nu = self.mjc_actuator_ctrl_source.shape[0]
if nu == 0:
return
mujoco_attrs = getattr(self.model, "mujoco", None)
if mujoco_attrs is None:
return
actuator_gainprm = getattr(mujoco_attrs, "actuator_gainprm", None)
actuator_biasprm = getattr(mujoco_attrs, "actuator_biasprm", None)
actuator_dynprm = getattr(mujoco_attrs, "actuator_dynprm", None)
actuator_ctrlrange = getattr(mujoco_attrs, "actuator_ctrlrange", None)
actuator_forcerange = getattr(mujoco_attrs, "actuator_forcerange", None)
actuator_actrange = getattr(mujoco_attrs, "actuator_actrange", None)
actuator_gear = getattr(mujoco_attrs, "actuator_gear", None)
actuator_cranklength = getattr(mujoco_attrs, "actuator_cranklength", None)
if (
actuator_gainprm is None
or actuator_biasprm is None
or actuator_dynprm is None
or actuator_ctrlrange is None
or actuator_forcerange is None
or actuator_actrange is None
or actuator_gear is None
or actuator_cranklength is None
):
return
nworld = self.mjw_model.actuator_biasprm.shape[0]
actuators_per_world = actuator_gainprm.shape[0] // nworld if nworld > 0 else actuator_gainprm.shape[0]
wp.launch(
update_ctrl_direct_actuator_properties_kernel,
dim=(nworld, nu),
inputs=[
self.mjc_actuator_ctrl_source,
self.mjc_actuator_to_newton_idx,
actuator_gainprm,
actuator_biasprm,
actuator_dynprm,
actuator_ctrlrange,
actuator_forcerange,
actuator_actrange,
actuator_gear,
actuator_cranklength,
actuators_per_world,
],
outputs=[
self.mjw_model.actuator_gainprm,
self.mjw_model.actuator_biasprm,
self.mjw_model.actuator_dynprm,
self.mjw_model.actuator_ctrlrange,
self.mjw_model.actuator_forcerange,
self.mjw_model.actuator_actrange,
self.mjw_model.actuator_gear,
self.mjw_model.actuator_cranklength,
],
device=self.model.device,
)
def _validate_model_for_separate_worlds(self, model: Model) -> None:
"""Validate that the Newton model is compatible with MuJoCo's separate_worlds mode.
MuJoCo's separate_worlds mode creates identical copies of a single MuJoCo model
for each Newton world. This requires:
1. All worlds have the same number of bodies, joints, shapes, and equality constraints
2. Entity types match across corresponding entities in each world
3. Global world (-1) only contains static shapes (no bodies, joints, or constraints)
Args:
model: The Newton model to validate.
Raises:
ValueError: If the model is not compatible with separate_worlds mode.
"""
world_count = model.world_count
# Check that we have at least one world
if world_count == 0:
raise ValueError(
"SolverMuJoCo with separate_worlds=True requires at least one world (world_count >= 1). "
"Found world_count=0 (all entities in global world -1)."
)
body_world = model.body_world.numpy()
joint_world = model.joint_world.numpy()
shape_world = model.shape_world.numpy()
eq_constraint_world = model.equality_constraint_world.numpy()
# --- Check global world restrictions (always, regardless of world_count) ---
# No bodies in global world
global_bodies = np.where(body_world == -1)[0]
if len(global_bodies) > 0:
body_names = [model.body_label[i] for i in global_bodies[:3]]
msg = f"Global world (-1) cannot contain bodies. Found {len(global_bodies)} body(ies) with world == -1"
if body_names:
msg += f": {body_names}"
raise ValueError(msg)
# No joints in global world
global_joints = np.where(joint_world == -1)[0]
if len(global_joints) > 0:
joint_names = [model.joint_label[i] for i in global_joints[:3]]
msg = f"Global world (-1) cannot contain joints. Found {len(global_joints)} joint(s) with world == -1"
if joint_names:
msg += f": {joint_names}"
raise ValueError(msg)
# No equality constraints in global world
global_constraints = np.where(eq_constraint_world == -1)[0]
if len(global_constraints) > 0:
msg = f"Global world (-1) cannot contain equality constraints. Found {len(global_constraints)} constraint(s) with world == -1"
raise ValueError(msg)
# No mimic constraints in global world
mimic_world = model.constraint_mimic_world.numpy()
global_mimic = np.where(mimic_world == -1)[0]
if len(global_mimic) > 0:
msg = f"Global world (-1) cannot contain mimic constraints. Found {len(global_mimic)} constraint(s) with world == -1"
raise ValueError(msg)
# Skip homogeneity checks for single-world models
if world_count <= 1:
return
# --- Check entity count homogeneity ---
# Count entities per world (excluding global shapes)
non_global_shapes = shape_world[shape_world >= 0]
for entity_name, world_arr in [
("bodies", body_world),
("joints", joint_world),
("shapes", non_global_shapes),
("equality constraints", eq_constraint_world),
("mimic constraints", mimic_world),
]:
# Use bincount for O(n) counting instead of O(n * world_count) loop
if len(world_arr) == 0:
continue
counts = np.bincount(world_arr.astype(np.int64), minlength=world_count)
# Vectorized check: all counts must equal the first
if not np.all(counts == counts[0]):
# Find first mismatch for error message (only on failure path)
expected = counts[0]
mismatched = np.where(counts != expected)[0]
w = mismatched[0]
raise ValueError(
f"SolverMuJoCo requires homogeneous worlds. "
f"World 0 has {expected} {entity_name}, but world {w} has {counts[w]}."
)
# --- Check type matching across worlds (vectorized) ---
# Load type arrays lazily - only when needed for validation
joints_per_world = model.joint_count // world_count
if joints_per_world > 0:
joint_type = model.joint_type.numpy()
joint_types_2d = joint_type.reshape(world_count, joints_per_world)
# Vectorized mismatch check: compare all rows to first row
mismatches = joint_types_2d != joint_types_2d[0]
if np.any(mismatches):
# Find first mismatch position using vectorized operations
j = np.argmax(np.any(mismatches, axis=0))
types = joint_types_2d[:, j]
raise ValueError(
f"SolverMuJoCo requires homogeneous worlds. "
f"Joint types mismatch at position {j}: world 0 has type {types[0]}, "
f"but other worlds have types {types[1:].tolist()}."
)
# Only check non-global shapes
shapes_per_world = len(non_global_shapes) // world_count if world_count > 0 else 0
if shapes_per_world > 0:
shape_type = model.shape_type.numpy()
# Get shape types for non-global shapes only
non_global_shape_types = shape_type[shape_world >= 0]
shape_types_2d = non_global_shape_types.reshape(world_count, shapes_per_world)
# Vectorized mismatch check
mismatches = shape_types_2d != shape_types_2d[0]
if np.any(mismatches):
s = np.argmax(np.any(mismatches, axis=0))
types = shape_types_2d[:, s]
raise ValueError(
f"SolverMuJoCo requires homogeneous worlds. "
f"Shape types mismatch at position {s}: world 0 has type {types[0]}, "
f"but other worlds have types {types[1:].tolist()}."
)
constraints_per_world = model.equality_constraint_count // world_count if world_count > 0 else 0
if constraints_per_world > 0:
eq_constraint_type = model.equality_constraint_type.numpy()
constraint_types_2d = eq_constraint_type.reshape(world_count, constraints_per_world)
# Vectorized mismatch check
mismatches = constraint_types_2d != constraint_types_2d[0]
if np.any(mismatches):
c = np.argmax(np.any(mismatches, axis=0))
types = constraint_types_2d[:, c]
raise ValueError(
f"SolverMuJoCo requires homogeneous worlds. "
f"Equality constraint types mismatch at position {c}: world 0 has type {types[0]}, "
f"but other worlds have types {types[1:].tolist()}."
)
def render_mujoco_viewer(
self,
show_left_ui: bool = True,
show_right_ui: bool = True,
show_contact_points: bool = True,
show_contact_forces: bool = False,
show_transparent_geoms: bool = True,
):
"""Create and synchronize the MuJoCo viewer.
The viewer will be created if it is not already open.
.. note::
The MuJoCo viewer only supports rendering Newton models with a single world,
unless ``use_mujoco_cpu`` is ``True`` or the solver was initialized with
``separate_worlds`` set to ``False``.
The MuJoCo viewer is only meant as a debugging tool.
Args:
show_left_ui: Whether to show the left UI.
show_right_ui: Whether to show the right UI.
show_contact_points: Whether to show contact points.
show_contact_forces: Whether to show contact forces.
show_transparent_geoms: Whether to show transparent geoms.
"""
if self._viewer is None:
import mujoco.viewer
mujoco = self._mujoco
# make the headlights brighter to improve visibility
# in the MuJoCo viewer
self.mj_model.vis.headlight.ambient[:] = [0.3, 0.3, 0.3]
self.mj_model.vis.headlight.diffuse[:] = [0.7, 0.7, 0.7]
self.mj_model.vis.headlight.specular[:] = [0.9, 0.9, 0.9]
self._viewer = mujoco.viewer.launch_passive(
self.mj_model, self.mj_data, show_left_ui=show_left_ui, show_right_ui=show_right_ui
)
# Enter the context manager to keep the viewer alive
self._viewer.__enter__()
self._viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = show_contact_points
self._viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = show_contact_forces
self._viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_TRANSPARENT] = show_transparent_geoms
if self._viewer.is_running():
if not self.use_mujoco_cpu:
with wp.ScopedDevice(self.model.device):
self._mujoco_warp.get_data_into(self.mj_data, self.mj_model, self.mjw_data)
self._viewer.sync()
def close_mujoco_viewer(self):
"""Close the MuJoCo viewer if it exists."""
if hasattr(self, "_viewer") and self._viewer is not None:
try:
self._viewer.__exit__(None, None, None)
except Exception:
pass # Ignore errors during cleanup
finally:
self._viewer = None
def __del__(self):
"""Cleanup method to close the viewer when the solver is destroyed."""
self.close_mujoco_viewer()
================================================
FILE: newton/_src/solvers/semi_implicit/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .solver_semi_implicit import SolverSemiImplicit
__all__ = [
"SolverSemiImplicit",
]
================================================
FILE: newton/_src/solvers/semi_implicit/kernels_body.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warp as wp
from ...math import quat_decompose
from ...sim import (
Control,
JointType,
Model,
State,
)
@wp.func
def joint_force(
q: float,
qd: float,
joint_target_pos: float,
joint_target_vel: float,
target_ke: float,
target_kd: float,
limit_lower: float,
limit_upper: float,
limit_ke: float,
limit_kd: float,
) -> float:
"""Joint force evaluation for a single degree of freedom."""
limit_f = 0.0
damping_f = 0.0
target_f = 0.0
target_f = target_ke * (joint_target_pos - q) + target_kd * (joint_target_vel - qd)
# When limit violated: apply limit restoration forces and disable target control
if q < limit_lower:
limit_f = limit_ke * (limit_lower - q)
damping_f = -limit_kd * qd
target_f = 0.0
elif q > limit_upper:
limit_f = limit_ke * (limit_upper - q)
damping_f = -limit_kd * qd
target_f = 0.0
return limit_f + damping_f + target_f
@wp.kernel
def eval_body_joints(
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
joint_qd_start: wp.array[int],
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_child: wp.array[int],
joint_parent: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_dof_dim: wp.array2d[int],
joint_f: wp.array[float],
joint_target_pos: wp.array[float],
joint_target_vel: wp.array[float],
joint_target_ke: wp.array[float],
joint_target_kd: wp.array[float],
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
joint_limit_ke: wp.array[float],
joint_limit_kd: wp.array[float],
joint_attach_ke: float,
joint_attach_kd: float,
body_f: wp.array[wp.spatial_vector],
):
tid = wp.tid()
type = joint_type[tid]
c_child = joint_child[tid]
c_parent = joint_parent[tid]
if not joint_enabled[tid]:
return
qd_start = joint_qd_start[tid]
if type == JointType.FREE or type == JointType.DISTANCE:
wrench = wp.spatial_vector(
joint_f[qd_start + 0],
joint_f[qd_start + 1],
joint_f[qd_start + 2],
joint_f[qd_start + 3],
joint_f[qd_start + 4],
joint_f[qd_start + 5],
)
wp.atomic_add(body_f, c_child, wrench)
return
X_pj = joint_X_p[tid]
X_cj = joint_X_c[tid]
X_wp = X_pj
r_p = wp.vec3()
w_p = wp.vec3()
v_p = wp.vec3()
# parent transform and moment arm
if c_parent >= 0:
X_wp = body_q[c_parent] * X_wp
r_p = wp.transform_get_translation(X_wp) - wp.transform_point(body_q[c_parent], body_com[c_parent])
twist_p = body_qd[c_parent]
w_p = wp.spatial_bottom(twist_p)
v_p = wp.spatial_top(twist_p) + wp.cross(w_p, r_p)
# child transform and moment arm
X_wc = body_q[c_child] * X_cj
r_c = wp.transform_get_translation(X_wc) - wp.transform_point(body_q[c_child], body_com[c_child])
twist_c = body_qd[c_child]
w_c = wp.spatial_bottom(twist_c)
v_c = wp.spatial_top(twist_c) + wp.cross(w_c, r_c)
lin_axis_count = joint_dof_dim[tid, 0]
ang_axis_count = joint_dof_dim[tid, 1]
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
q_p = wp.transform_get_rotation(X_wp)
q_c = wp.transform_get_rotation(X_wc)
# translational error
x_err = x_c - x_p
r_err = wp.quat_inverse(q_p) * q_c
v_err = v_c - v_p
w_err = w_c - w_p
# total force/torque on the parent
t_total = wp.vec3()
f_total = wp.vec3()
# reduce angular damping stiffness for stability
angular_damping_scale = 0.01
if type == JointType.FIXED:
ang_err = wp.normalize(wp.vec3(r_err[0], r_err[1], r_err[2])) * wp.acos(r_err[3]) * 2.0
f_total += x_err * joint_attach_ke + v_err * joint_attach_kd
t_total += (
wp.transform_vector(X_wp, ang_err) * joint_attach_ke + w_err * joint_attach_kd * angular_damping_scale
)
if type == JointType.PRISMATIC:
axis = joint_axis[qd_start]
# world space joint axis
axis_p = wp.transform_vector(X_wp, axis)
# evaluate joint coordinates
q = wp.dot(x_err, axis_p)
qd = wp.dot(v_err, axis_p)
f_total = axis_p * (
-joint_f[qd_start]
- joint_force(
q,
qd,
joint_target_pos[qd_start],
joint_target_vel[qd_start],
joint_target_ke[qd_start],
joint_target_kd[qd_start],
joint_limit_lower[qd_start],
joint_limit_upper[qd_start],
joint_limit_ke[qd_start],
joint_limit_kd[qd_start],
)
)
# attachment dynamics
ang_err = wp.normalize(wp.vec3(r_err[0], r_err[1], r_err[2])) * wp.acos(r_err[3]) * 2.0
# project off any displacement along the joint axis
f_total += (x_err - q * axis_p) * joint_attach_ke + (v_err - qd * axis_p) * joint_attach_kd
t_total += (
wp.transform_vector(X_wp, ang_err) * joint_attach_ke + w_err * joint_attach_kd * angular_damping_scale
)
if type == JointType.REVOLUTE:
axis = joint_axis[qd_start]
axis_p = wp.transform_vector(X_wp, axis)
axis_c = wp.transform_vector(X_wc, axis)
# swing twist decomposition
twist = wp.quat_twist(axis, r_err)
q = wp.acos(twist[3]) * 2.0 * wp.sign(wp.dot(axis, wp.vec3(twist[0], twist[1], twist[2])))
qd = wp.dot(w_err, axis_p)
t_total = axis_p * (
-joint_f[qd_start]
- joint_force(
q,
qd,
joint_target_pos[qd_start],
joint_target_vel[qd_start],
joint_target_ke[qd_start],
joint_target_kd[qd_start],
joint_limit_lower[qd_start],
joint_limit_upper[qd_start],
joint_limit_ke[qd_start],
joint_limit_kd[qd_start],
)
)
# attachment dynamics
swing_err = wp.cross(axis_p, axis_c)
f_total += x_err * joint_attach_ke + v_err * joint_attach_kd
t_total += swing_err * joint_attach_ke + (w_err - qd * axis_p) * joint_attach_kd * angular_damping_scale
if type == JointType.BALL:
ang_err = wp.normalize(wp.vec3(r_err[0], r_err[1], r_err[2])) * wp.acos(r_err[3]) * 2.0
# TODO joint limits
# TODO expose target_kd or target_ke for ball joints
# t_total += target_kd * w_err + target_ke * wp.transform_vector(X_wp, ang_err)
f_total += x_err * joint_attach_ke + v_err * joint_attach_kd
t_total += wp.vec3(-joint_f[qd_start], -joint_f[qd_start + 1], -joint_f[qd_start + 2])
if type == JointType.D6:
pos = wp.vec3(0.0)
vel = wp.vec3(0.0)
if lin_axis_count >= 1:
axis_0 = wp.transform_vector(X_wp, joint_axis[qd_start + 0])
q0 = wp.dot(x_err, axis_0)
qd0 = wp.dot(v_err, axis_0)
f_total += axis_0 * (
-joint_f[qd_start]
- joint_force(
q0,
qd0,
joint_target_pos[qd_start + 0],
joint_target_vel[qd_start + 0],
joint_target_ke[qd_start + 0],
joint_target_kd[qd_start + 0],
joint_limit_lower[qd_start + 0],
joint_limit_upper[qd_start + 0],
joint_limit_ke[qd_start + 0],
joint_limit_kd[qd_start + 0],
)
)
pos += q0 * axis_0
vel += qd0 * axis_0
if lin_axis_count >= 2:
axis_1 = wp.transform_vector(X_wp, joint_axis[qd_start + 1])
q1 = wp.dot(x_err, axis_1)
qd1 = wp.dot(v_err, axis_1)
f_total += axis_1 * (
-joint_f[qd_start + 1]
- joint_force(
q1,
qd1,
joint_target_pos[qd_start + 1],
joint_target_vel[qd_start + 1],
joint_target_ke[qd_start + 1],
joint_target_kd[qd_start + 1],
joint_limit_lower[qd_start + 1],
joint_limit_upper[qd_start + 1],
joint_limit_ke[qd_start + 1],
joint_limit_kd[qd_start + 1],
)
)
pos += q1 * axis_1
vel += qd1 * axis_1
if lin_axis_count == 3:
axis_2 = wp.transform_vector(X_wp, joint_axis[qd_start + 2])
q2 = wp.dot(x_err, axis_2)
qd2 = wp.dot(v_err, axis_2)
f_total += axis_2 * (
-joint_f[qd_start + 2]
- joint_force(
q2,
qd2,
joint_target_pos[qd_start + 2],
joint_target_vel[qd_start + 2],
joint_target_ke[qd_start + 2],
joint_target_kd[qd_start + 2],
joint_limit_lower[qd_start + 2],
joint_limit_upper[qd_start + 2],
joint_limit_ke[qd_start + 2],
joint_limit_kd[qd_start + 2],
)
)
pos += q2 * axis_2
vel += qd2 * axis_2
f_total += (x_err - pos) * joint_attach_ke + (v_err - vel) * joint_attach_kd
if ang_axis_count == 0:
ang_err = wp.normalize(wp.vec3(r_err[0], r_err[1], r_err[2])) * wp.acos(r_err[3]) * 2.0
t_total += (
wp.transform_vector(X_wp, ang_err) * joint_attach_ke + w_err * joint_attach_kd * angular_damping_scale
)
i_0 = lin_axis_count + qd_start + 0
i_1 = lin_axis_count + qd_start + 1
i_2 = lin_axis_count + qd_start + 2
qdi_start = qd_start + lin_axis_count
if ang_axis_count == 1:
axis = joint_axis[i_0]
axis_p = wp.transform_vector(X_wp, axis)
axis_c = wp.transform_vector(X_wc, axis)
# swing twist decomposition
twist = wp.quat_twist(axis, r_err)
q = wp.acos(twist[3]) * 2.0 * wp.sign(wp.dot(axis, wp.vec3(twist[0], twist[1], twist[2])))
qd = wp.dot(w_err, axis_p)
t_total = axis_p * (
-joint_f[qdi_start]
- joint_force(
q,
qd,
joint_target_pos[i_0],
joint_target_vel[i_0],
joint_target_ke[i_0],
joint_target_kd[i_0],
joint_limit_lower[i_0],
joint_limit_upper[i_0],
joint_limit_ke[i_0],
joint_limit_kd[i_0],
)
)
# attachment dynamics
swing_err = wp.cross(axis_p, axis_c)
t_total += swing_err * joint_attach_ke + (w_err - qd * axis_p) * joint_attach_kd * angular_damping_scale
if ang_axis_count == 2:
q_pc = wp.quat_inverse(q_p) * q_c
# decompose to a compound rotation each axis
angles = quat_decompose(q_pc)
orig_axis_0 = joint_axis[i_0]
orig_axis_1 = joint_axis[i_1]
orig_axis_2 = wp.cross(orig_axis_0, orig_axis_1)
# reconstruct rotation axes
axis_0 = orig_axis_0
q_0 = wp.quat_from_axis_angle(axis_0, angles[0])
axis_1 = wp.quat_rotate(q_0, orig_axis_1)
q_1 = wp.quat_from_axis_angle(axis_1, angles[1])
axis_2 = wp.quat_rotate(q_1 * q_0, orig_axis_2)
axis_0 = wp.transform_vector(X_wp, axis_0)
axis_1 = wp.transform_vector(X_wp, axis_1)
axis_2 = wp.transform_vector(X_wp, axis_2)
# joint dynamics
t_total += axis_0 * (
-joint_f[qdi_start]
- joint_force(
angles[0],
wp.dot(axis_0, w_err),
joint_target_pos[i_0],
joint_target_vel[i_0],
joint_target_ke[i_0],
joint_target_kd[i_0],
joint_limit_lower[i_0],
joint_limit_upper[i_0],
joint_limit_ke[i_0],
joint_limit_kd[i_0],
)
)
t_total += axis_1 * (
-joint_f[qdi_start + 1]
- joint_force(
angles[1],
wp.dot(axis_1, w_err),
joint_target_pos[i_1],
joint_target_vel[i_1],
joint_target_ke[i_1],
joint_target_kd[i_1],
joint_limit_lower[i_1],
joint_limit_upper[i_1],
joint_limit_ke[i_1],
joint_limit_kd[i_1],
)
)
# last axis (fixed)
t_total += axis_2 * -joint_force(
angles[2],
wp.dot(axis_2, w_err),
0.0,
0.0,
joint_attach_ke,
joint_attach_kd * angular_damping_scale,
0.0,
0.0,
0.0,
0.0,
)
if ang_axis_count == 3:
q_pc = wp.quat_inverse(q_p) * q_c
# decompose to a compound rotation each axis
angles = quat_decompose(q_pc)
orig_axis_0 = joint_axis[i_0]
orig_axis_1 = joint_axis[i_1]
orig_axis_2 = joint_axis[i_2]
# reconstruct rotation axes
axis_0 = orig_axis_0
q_0 = wp.quat_from_axis_angle(axis_0, angles[0])
axis_1 = wp.quat_rotate(q_0, orig_axis_1)
q_1 = wp.quat_from_axis_angle(axis_1, angles[1])
axis_2 = wp.quat_rotate(q_1 * q_0, orig_axis_2)
axis_0 = wp.transform_vector(X_wp, axis_0)
axis_1 = wp.transform_vector(X_wp, axis_1)
axis_2 = wp.transform_vector(X_wp, axis_2)
t_total += axis_0 * (
-joint_f[qdi_start]
- joint_force(
angles[0],
wp.dot(axis_0, w_err),
joint_target_pos[i_0],
joint_target_vel[i_0],
joint_target_ke[i_0],
joint_target_kd[i_0],
joint_limit_lower[i_0],
joint_limit_upper[i_0],
joint_limit_ke[i_0],
joint_limit_kd[i_0],
)
)
t_total += axis_1 * (
-joint_f[qdi_start + 1]
- joint_force(
angles[1],
wp.dot(axis_1, w_err),
joint_target_pos[i_1],
joint_target_vel[i_1],
joint_target_ke[i_1],
joint_target_kd[i_1],
joint_limit_lower[i_1],
joint_limit_upper[i_1],
joint_limit_ke[i_1],
joint_limit_kd[i_1],
)
)
t_total += axis_2 * (
-joint_f[qdi_start + 2]
- joint_force(
angles[2],
wp.dot(axis_2, w_err),
joint_target_pos[i_2],
joint_target_vel[i_2],
joint_target_ke[i_2],
joint_target_kd[i_2],
joint_limit_lower[i_2],
joint_limit_upper[i_2],
joint_limit_ke[i_2],
joint_limit_kd[i_2],
)
)
# write forces
if c_parent >= 0:
wp.atomic_add(body_f, c_parent, wp.spatial_vector(f_total, t_total + wp.cross(r_p, f_total)))
wp.atomic_sub(body_f, c_child, wp.spatial_vector(f_total, t_total + wp.cross(r_c, f_total)))
def eval_body_joint_forces(
model: Model, state: State, control: Control, body_f: wp.array, joint_attach_ke: float, joint_attach_kd: float
):
if model.joint_count:
wp.launch(
kernel=eval_body_joints,
dim=model.joint_count,
inputs=[
state.body_q,
state.body_qd,
model.body_com,
model.joint_qd_start,
model.joint_type,
model.joint_enabled,
model.joint_child,
model.joint_parent,
model.joint_X_p,
model.joint_X_c,
model.joint_axis,
model.joint_dof_dim,
control.joint_f,
control.joint_target_pos,
control.joint_target_vel,
model.joint_target_ke,
model.joint_target_kd,
model.joint_limit_lower,
model.joint_limit_upper,
model.joint_limit_ke,
model.joint_limit_kd,
joint_attach_ke,
joint_attach_kd,
],
outputs=[body_f],
device=model.device,
)
================================================
FILE: newton/_src/solvers/semi_implicit/kernels_contact.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...geometry import ParticleFlags
from ...geometry.kernels import triangle_closest_point_barycentric
from ...sim import Contacts, Model, State
@wp.func
def particle_force(n: wp.vec3, v: wp.vec3, c: float, k_n: float, k_d: float, k_f: float, k_mu: float):
# compute normal and tangential friction force for a single contact
vn = wp.dot(n, v)
jn = c * k_n
jd = min(vn, 0.0) * k_d
# contact force
fn = jn + jd
# friction force
vt = v - n * vn
vs = wp.length(vt)
if vs > 0.0:
vt = vt / vs
# Coulomb condition
ft = wp.min(vs * k_f, k_mu * wp.abs(fn))
# total force
return -n * fn - vt * ft
@wp.kernel
def eval_particle_contact(
grid: wp.uint64,
particle_x: wp.array[wp.vec3],
particle_v: wp.array[wp.vec3],
particle_radius: wp.array[float],
particle_flags: wp.array[wp.int32],
k_contact: float,
k_damp: float,
k_friction: float,
k_mu: float,
k_cohesion: float,
max_radius: float,
# outputs
particle_f: wp.array[wp.vec3],
):
tid = wp.tid()
# order threads by cell
i = wp.hash_grid_point_id(grid, tid)
if i == -1:
# hash grid has not been built yet
return
if (particle_flags[i] & ParticleFlags.ACTIVE) == 0:
return
x = particle_x[i]
v = particle_v[i]
radius = particle_radius[i]
f = wp.vec3(0.0)
# particle contact
query = wp.hash_grid_query(grid, x, radius + max_radius + k_cohesion)
index = int(0)
while wp.hash_grid_query_next(query, index):
if (particle_flags[index] & ParticleFlags.ACTIVE) != 0 and index != i:
# compute distance to point
n = x - particle_x[index]
d = wp.length(n)
err = d - radius - particle_radius[index]
if err <= k_cohesion:
n = n / d
vrel = v - particle_v[index]
f += particle_force(n, vrel, err, k_contact, k_damp, k_friction, k_mu)
particle_f[i] = f
@wp.kernel
def eval_triangle_contact(
# idx : wp.array[int], # list of indices for colliding particles
num_particles: int, # size of particles
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
indices: wp.array2d[int],
materials: wp.array2d[float],
particle_radius: wp.array[float],
contact_stiffness: float,
f: wp.array[wp.vec3],
):
tid = wp.tid()
face_no = tid // num_particles # which face
particle_no = tid % num_particles # which particle
# at the moment, just one particle
pos = x[particle_no]
i = indices[face_no, 0]
j = indices[face_no, 1]
k = indices[face_no, 2]
if i == particle_no or j == particle_no or k == particle_no:
return
p = x[i] # point zero
q = x[j] # point one
r = x[k] # point two
bary = triangle_closest_point_barycentric(p, q, r, pos)
closest = p * bary[0] + q * bary[1] + r * bary[2]
diff = pos - closest
dist = wp.length(diff)
# early exit if no contact or degenerate case
collision_radius = particle_radius[particle_no]
if dist >= collision_radius or dist < 1e-6:
return
# contact normal (points from triangle to particle)
n = diff / dist
# penetration depth
penetration_depth = collision_radius - dist
# contact force
fn = contact_stiffness * penetration_depth * n
wp.atomic_add(f, particle_no, fn)
wp.atomic_add(f, i, -fn * bary[0])
wp.atomic_add(f, j, -fn * bary[1])
wp.atomic_add(f, k, -fn * bary[2])
@wp.kernel
def eval_particle_body_contact(
particle_x: wp.array[wp.vec3],
particle_v: wp.array[wp.vec3],
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
particle_radius: wp.array[float],
particle_flags: wp.array[wp.int32],
body_com: wp.array[wp.vec3],
shape_body: wp.array[int],
shape_material_ke: wp.array[float],
shape_material_kd: wp.array[float],
shape_material_kf: wp.array[float],
shape_material_mu: wp.array[float],
shape_material_ka: wp.array[float],
particle_ke: float,
particle_kd: float,
particle_kf: float,
particle_mu: float,
particle_ka: float,
contact_count: wp.array[int],
contact_particle: wp.array[int],
contact_shape: wp.array[int],
contact_body_pos: wp.array[wp.vec3],
contact_body_vel: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
contact_max: int,
body_f_in_world_frame: bool,
# outputs
particle_f: wp.array[wp.vec3],
body_f: wp.array[wp.spatial_vector],
):
tid = wp.tid()
count = min(contact_max, contact_count[0])
if tid >= count:
return
shape_index = contact_shape[tid]
body_index = shape_body[shape_index]
particle_index = contact_particle[tid]
if (particle_flags[particle_index] & ParticleFlags.ACTIVE) == 0:
return
px = particle_x[particle_index]
pv = particle_v[particle_index]
X_wb = wp.transform_identity()
X_com = wp.vec3()
body_v_s = wp.spatial_vector()
if body_index >= 0:
X_wb = body_q[body_index]
X_com = body_com[body_index]
body_v_s = body_qd[body_index]
# body position in world space
bx = wp.transform_point(X_wb, contact_body_pos[tid])
r = bx - wp.transform_point(X_wb, X_com)
n = contact_normal[tid]
c = wp.dot(n, px - bx) - particle_radius[particle_index]
if c > particle_ka:
return
# take average material properties of shape and particle parameters
ke = 0.5 * (particle_ke + shape_material_ke[shape_index])
kd = 0.5 * (particle_kd + shape_material_kd[shape_index])
kf = 0.5 * (particle_kf + shape_material_kf[shape_index])
mu = 0.5 * (particle_mu + shape_material_mu[shape_index])
body_w = wp.spatial_bottom(body_v_s)
body_v = wp.spatial_top(body_v_s)
# compute the body velocity at the particle position
bv = body_v + wp.transform_vector(X_wb, contact_body_vel[tid])
if body_f_in_world_frame:
bv += wp.cross(body_w, bx)
else:
bv += wp.cross(body_w, r)
# relative velocity
v = pv - bv
# decompose relative velocity
vn = wp.dot(n, v)
vt = v - n * vn
# contact elastic
fn = n * c * ke
# contact damping
fd = n * wp.min(vn, 0.0) * kd
# viscous friction
# ft = vt*kf
# Coulomb friction (box)
# lower = mu * c * ke
# upper = -lower
# vx = wp.clamp(wp.dot(wp.vec3(kf, 0.0, 0.0), vt), lower, upper)
# vz = wp.clamp(wp.dot(wp.vec3(0.0, 0.0, kf), vt), lower, upper)
# ft = wp.vec3(vx, 0.0, vz)
# Coulomb friction (smooth, but gradients are numerically unstable around |vt| = 0)
ft = wp.normalize(vt) * wp.min(kf * wp.length(vt), abs(mu * c * ke))
f_total = fn + (fd + ft)
wp.atomic_sub(particle_f, particle_index, f_total)
if body_index >= 0:
if body_f_in_world_frame:
wp.atomic_sub(body_f, body_index, wp.spatial_vector(f_total, wp.cross(bx, f_total)))
else:
wp.atomic_add(body_f, body_index, wp.spatial_vector(f_total, wp.cross(r, f_total)))
@wp.kernel
def eval_triangles_body_contact(
num_particles: int, # number of particles (size of contact_point)
x: wp.array[wp.vec3], # position of particles
v: wp.array[wp.vec3],
indices: wp.array[int], # triangle indices
body_x: wp.array[wp.vec3], # body body positions
body_r: wp.array[wp.quat],
body_v: wp.array[wp.vec3],
body_w: wp.array[wp.vec3],
contact_body: wp.array[int],
contact_point: wp.array[wp.vec3], # position of contact points relative to body
contact_dist: wp.array[float],
contact_mat: wp.array[int],
materials: wp.array[float],
# body_f : wp.array[wp.vec3],
# body_t : wp.array[wp.vec3],
tri_f: wp.array[wp.vec3],
):
tid = wp.tid()
face_no = tid // num_particles # which face
particle_no = tid % num_particles # which particle
# -----------------------
# load body body point
c_body = contact_body[particle_no]
c_point = contact_point[particle_no]
c_dist = contact_dist[particle_no]
c_mat = contact_mat[particle_no]
# hard coded surface parameter tensor layout (ke, kd, kf, mu)
ke = materials[c_mat * 4 + 0] # restitution coefficient
kd = materials[c_mat * 4 + 1] # damping coefficient
kf = materials[c_mat * 4 + 2] # friction coefficient
mu = materials[c_mat * 4 + 3] # coulomb friction
x0 = body_x[c_body] # position of colliding body
r0 = body_r[c_body] # orientation of colliding body
v0 = body_v[c_body]
w0 = body_w[c_body]
# transform point to world space
pos = x0 + wp.quat_rotate(r0, c_point)
# use x0 as center, everything is offset from center of mass
# moment arm
r = pos - x0 # basically just c_point in the new coordinates
rhat = wp.normalize(r)
pos = pos + rhat * c_dist # add on 'thickness' of shape, e.g.: radius of sphere/capsule
# contact point velocity
dpdt = v0 + wp.cross(w0, r) # this is body velocity cross offset, so it's the velocity of the contact point.
# -----------------------
# load triangle
i = indices[face_no * 3 + 0]
j = indices[face_no * 3 + 1]
k = indices[face_no * 3 + 2]
p = x[i] # point zero
q = x[j] # point one
r = x[k] # point two
vp = v[i] # vel zero
vq = v[j] # vel one
vr = v[k] # vel two
bary = triangle_closest_point_barycentric(p, q, r, pos)
closest = p * bary[0] + q * bary[1] + r * bary[2]
diff = pos - closest # vector from tri to point
dist = wp.dot(diff, diff) # squared distance
n = wp.normalize(diff) # points into the object
c = wp.min(dist - 0.05, 0.0) # 0 unless within 0.05 of surface
# c = wp.leaky_min(wp.dot(n, x0)-0.01, 0.0, 0.0)
# fn = n * c * 1e6 # points towards cloth (both n and c are negative)
# wp.atomic_sub(tri_f, particle_no, fn)
fn = c * ke # normal force (restitution coefficient * how far inside for ground) (negative)
vtri = vp * bary[0] + vq * bary[1] + vr * bary[2] # bad approximation for centroid velocity
vrel = vtri - dpdt
vn = wp.dot(n, vrel) # velocity component of body in negative normal direction
vt = vrel - n * vn # velocity component not in normal direction
# contact damping
fd = -wp.max(vn, 0.0) * kd * wp.step(c) # again, negative, into the ground
# # viscous friction
# ft = vt*kf
# Coulomb friction (box)
lower = mu * (fn + fd)
upper = -lower
nx = wp.cross(n, wp.vec3(0.0, 0.0, 1.0)) # basis vectors for tangent
nz = wp.cross(n, wp.vec3(1.0, 0.0, 0.0))
vx = wp.clamp(wp.dot(nx * kf, vt), lower, upper)
vz = wp.clamp(wp.dot(nz * kf, vt), lower, upper)
ft = (nx * vx + nz * vz) * (-wp.step(c)) # wp.vec3(vx, 0.0, vz)*wp.step(c)
# # Coulomb friction (smooth, but gradients are numerically unstable around |vt| = 0)
# #ft = wp.normalize(vt)*wp.min(kf*wp.length(vt), -mu*c*ke)
f_total = n * (fn + fd) + ft
wp.atomic_add(tri_f, i, f_total * bary[0])
wp.atomic_add(tri_f, j, f_total * bary[1])
wp.atomic_add(tri_f, k, f_total * bary[2])
@wp.kernel
def eval_body_contact(
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
shape_material_ke: wp.array[float],
shape_material_kd: wp.array[float],
shape_material_kf: wp.array[float],
shape_material_ka: wp.array[float],
shape_material_mu: wp.array[float],
shape_body: wp.array[int],
contact_count: wp.array[int],
contact_point0: wp.array[wp.vec3],
contact_point1: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
contact_shape0: wp.array[int],
contact_shape1: wp.array[int],
contact_margin0: wp.array[float],
contact_margin1: wp.array[float],
rigid_contact_stiffness: wp.array[float],
rigid_contact_damping: wp.array[float],
rigid_contact_friction_scale: wp.array[float],
force_in_world_frame: bool,
friction_smoothing: float,
# outputs
body_f: wp.array[wp.spatial_vector],
):
tid = wp.tid()
count = contact_count[0]
if tid >= count:
return
# retrieve contact margins, compute average contact material properties
ke = 0.0 # contact normal force stiffness
kd = 0.0 # damping coefficient
kf = 0.0 # friction force stiffness
ka = 0.0 # adhesion distance
mu = 0.0 # friction coefficient
mat_nonzero = 0
margin_a = contact_margin0[tid]
margin_b = contact_margin1[tid]
shape_a = contact_shape0[tid]
shape_b = contact_shape1[tid]
if shape_a == shape_b:
return
body_a = -1
body_b = -1
if shape_a >= 0:
mat_nonzero += 1
ke += shape_material_ke[shape_a]
kd += shape_material_kd[shape_a]
kf += shape_material_kf[shape_a]
ka += shape_material_ka[shape_a]
mu += shape_material_mu[shape_a]
body_a = shape_body[shape_a]
if shape_b >= 0:
mat_nonzero += 1
ke += shape_material_ke[shape_b]
kd += shape_material_kd[shape_b]
kf += shape_material_kf[shape_b]
ka += shape_material_ka[shape_b]
mu += shape_material_mu[shape_b]
body_b = shape_body[shape_b]
if mat_nonzero > 0:
ke /= float(mat_nonzero)
kd /= float(mat_nonzero)
kf /= float(mat_nonzero)
ka /= float(mat_nonzero)
mu /= float(mat_nonzero)
# per-contact stiffness/damping/friction
if rigid_contact_stiffness:
contact_ke = rigid_contact_stiffness[tid]
ke = contact_ke if contact_ke > 0.0 else ke
contact_kd = rigid_contact_damping[tid]
kd = contact_kd if contact_kd > 0.0 else kd
contact_mu = rigid_contact_friction_scale[tid]
mu = mu * contact_mu if contact_mu > 0.0 else mu
# contact normal stored as A-to-B; this spring-damper kernel uses B-to-A
# internally so that the existing force-application signs are preserved.
n = -contact_normal[tid]
bx_a = contact_point0[tid]
bx_b = contact_point1[tid]
r_a = wp.vec3(0.0)
r_b = wp.vec3(0.0)
if body_a >= 0:
X_wb_a = body_q[body_a]
X_com_a = body_com[body_a]
bx_a = wp.transform_point(X_wb_a, bx_a) - margin_a * n
r_a = bx_a - wp.transform_point(X_wb_a, X_com_a)
if body_b >= 0:
X_wb_b = body_q[body_b]
X_com_b = body_com[body_b]
bx_b = wp.transform_point(X_wb_b, bx_b) + margin_b * n
r_b = bx_b - wp.transform_point(X_wb_b, X_com_b)
d = wp.dot(n, bx_a - bx_b)
if d >= ka:
return
# compute contact point velocity
bv_a = wp.vec3(0.0)
bv_b = wp.vec3(0.0)
if body_a >= 0:
body_v_s_a = body_qd[body_a]
body_w_a = wp.spatial_bottom(body_v_s_a)
body_v_a = wp.spatial_top(body_v_s_a)
if force_in_world_frame:
bv_a = body_v_a + wp.cross(body_w_a, bx_a)
else:
bv_a = body_v_a + wp.cross(body_w_a, r_a)
if body_b >= 0:
body_v_s_b = body_qd[body_b]
body_w_b = wp.spatial_bottom(body_v_s_b)
body_v_b = wp.spatial_top(body_v_s_b)
if force_in_world_frame:
bv_b = body_v_b + wp.cross(body_w_b, bx_b)
else:
bv_b = body_v_b + wp.cross(body_w_b, r_b)
# relative velocity
v = bv_a - bv_b
# print(v)
# decompose relative velocity
vn = wp.dot(n, v)
vt = v - n * vn
# contact elastic
fn = d * ke
# contact damping
fd = wp.min(vn, 0.0) * kd * wp.step(d)
# viscous friction
# ft = vt*kf
# Coulomb friction (box)
# lower = mu * d * ke
# upper = -lower
# vx = wp.clamp(wp.dot(wp.vec3(kf, 0.0, 0.0), vt), lower, upper)
# vz = wp.clamp(wp.dot(wp.vec3(0.0, 0.0, kf), vt), lower, upper)
# ft = wp.vec3(vx, 0.0, vz)
# Coulomb friction (smooth, but gradients are numerically unstable around |vt| = 0)
ft = wp.vec3(0.0)
if d < 0.0:
# use a smooth vector norm to avoid gradient instability at/around zero velocity
vs = wp.norm_huber(vt, delta=friction_smoothing)
if vs > 0.0:
fr = vt / vs
ft = fr * wp.min(kf * vs, -mu * (fn + fd))
f_total = n * (fn + fd) + ft
# f_total = n * (fn + fd)
# f_total = n * fn
if body_a >= 0:
if force_in_world_frame:
wp.atomic_add(body_f, body_a, wp.spatial_vector(f_total, wp.cross(bx_a, f_total)))
else:
wp.atomic_sub(body_f, body_a, wp.spatial_vector(f_total, wp.cross(r_a, f_total)))
if body_b >= 0:
if force_in_world_frame:
wp.atomic_sub(body_f, body_b, wp.spatial_vector(f_total, wp.cross(bx_b, f_total)))
else:
wp.atomic_add(body_f, body_b, wp.spatial_vector(f_total, wp.cross(r_b, f_total)))
def eval_particle_contact_forces(model: Model, state: State, particle_f: wp.array):
if model.particle_count > 1 and model.particle_grid is not None:
wp.launch(
kernel=eval_particle_contact,
dim=model.particle_count,
inputs=[
model.particle_grid.id,
state.particle_q,
state.particle_qd,
model.particle_radius,
model.particle_flags,
model.particle_ke,
model.particle_kd,
model.particle_kf,
model.particle_mu,
model.particle_cohesion,
model.particle_max_radius,
],
outputs=[particle_f],
device=model.device,
)
def eval_triangle_contact_forces(model: Model, state: State, particle_f: wp.array):
if model.tri_count and model.particle_count:
wp.launch(
kernel=eval_triangle_contact,
dim=model.tri_count * model.particle_count,
inputs=[
model.particle_count,
state.particle_q,
state.particle_qd,
model.tri_indices,
model.tri_materials,
model.particle_radius,
model.soft_contact_ke,
],
outputs=[particle_f],
device=model.device,
)
def eval_body_contact_forces(
model: Model,
state: State,
contacts: Contacts | None,
friction_smoothing: float = 1.0,
force_in_world_frame: bool = False,
body_f_out: wp.array | None = None,
):
if contacts is not None and contacts.rigid_contact_max:
if body_f_out is None:
body_f_out = state.body_f
wp.launch(
kernel=eval_body_contact,
dim=contacts.rigid_contact_max,
inputs=[
state.body_q,
state.body_qd,
model.body_com,
model.shape_material_ke,
model.shape_material_kd,
model.shape_material_kf,
model.shape_material_ka,
model.shape_material_mu,
model.shape_body,
contacts.rigid_contact_count,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_normal,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
contacts.rigid_contact_stiffness,
contacts.rigid_contact_damping,
contacts.rigid_contact_friction,
force_in_world_frame,
friction_smoothing,
],
outputs=[body_f_out],
device=model.device,
)
def eval_particle_body_contact_forces(
model: Model,
state: State,
contacts: Contacts | None,
particle_f: wp.array,
body_f: wp.array,
body_f_in_world_frame: bool = False,
):
if contacts is not None and contacts.soft_contact_max:
wp.launch(
kernel=eval_particle_body_contact,
dim=contacts.soft_contact_max,
inputs=[
state.particle_q,
state.particle_qd,
state.body_q,
state.body_qd,
model.particle_radius,
model.particle_flags,
model.body_com,
model.shape_body,
model.shape_material_ke,
model.shape_material_kd,
model.shape_material_kf,
model.shape_material_mu,
model.shape_material_ka,
model.soft_contact_ke,
model.soft_contact_kd,
model.soft_contact_kf,
model.soft_contact_mu,
model.particle_adhesion,
contacts.soft_contact_count,
contacts.soft_contact_particle,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_body_vel,
contacts.soft_contact_normal,
contacts.soft_contact_max,
body_f_in_world_frame,
],
# outputs
outputs=[particle_f, body_f],
device=model.device,
)
================================================
FILE: newton/_src/solvers/semi_implicit/kernels_muscle.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...sim import Control, Model, State
@wp.func
def muscle_force(
i: int,
body_X_s: wp.array[wp.transform],
body_v_s: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
muscle_links: wp.array[int],
muscle_points: wp.array[wp.vec3],
muscle_activation: float,
body_f_s: wp.array[wp.spatial_vector],
):
link_0 = muscle_links[i]
link_1 = muscle_links[i + 1]
if link_0 == link_1:
return 0
r_0 = muscle_points[i]
r_1 = muscle_points[i + 1]
xform_0 = body_X_s[link_0]
xform_1 = body_X_s[link_1]
pos_0 = wp.transform_point(xform_0, r_0 - body_com[link_0])
pos_1 = wp.transform_point(xform_1, r_1 - body_com[link_1])
n = wp.normalize(pos_1 - pos_0)
# todo: add passive elastic and viscosity terms
f = n * muscle_activation
wp.atomic_sub(body_f_s, link_0, wp.spatial_vector(f, wp.cross(pos_0, f)))
wp.atomic_add(body_f_s, link_1, wp.spatial_vector(f, wp.cross(pos_1, f)))
@wp.kernel
def eval_muscle(
body_X_s: wp.array[wp.transform],
body_v_s: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
muscle_start: wp.array[int],
muscle_params: wp.array[float],
muscle_links: wp.array[int],
muscle_points: wp.array[wp.vec3],
muscle_activation: wp.array[float],
# output
body_f_s: wp.array[wp.spatial_vector],
):
tid = wp.tid()
m_start = muscle_start[tid]
m_end = muscle_start[tid + 1] - 1
activation = muscle_activation[tid]
for i in range(m_start, m_end):
muscle_force(i, body_X_s, body_v_s, body_com, muscle_links, muscle_points, activation, body_f_s)
def eval_muscle_forces(model: Model, state: State, control: Control, body_f: wp.array):
if model.muscle_count:
wp.launch(
kernel=eval_muscle,
dim=model.muscle_count,
inputs=[
state.body_q,
state.body_qd,
model.body_com,
model.muscle_start,
model.muscle_params,
model.muscle_bodies,
model.muscle_points,
control.muscle_activations,
],
outputs=[body_f],
device=model.device,
)
================================================
FILE: newton/_src/solvers/semi_implicit/kernels_particle.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...sim import Control, Model, State
@wp.kernel
def eval_spring(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
spring_indices: wp.array[int],
spring_rest_lengths: wp.array[float],
spring_stiffness: wp.array[float],
spring_damping: wp.array[float],
f: wp.array[wp.vec3],
):
tid = wp.tid()
i = spring_indices[tid * 2 + 0]
j = spring_indices[tid * 2 + 1]
if i == -1 or j == -1:
return
ke = spring_stiffness[tid]
kd = spring_damping[tid]
rest = spring_rest_lengths[tid]
xi = x[i]
xj = x[j]
vi = v[i]
vj = v[j]
xij = xi - xj
vij = vi - vj
l = wp.length(xij)
l_inv = 1.0 / l
# normalized spring direction
dir = xij * l_inv
c = l - rest
dcdt = wp.dot(dir, vij)
# damping based on relative velocity
fs = dir * (ke * c + kd * dcdt)
wp.atomic_sub(f, i, fs)
wp.atomic_add(f, j, fs)
@wp.kernel
def eval_triangle(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
indices: wp.array2d[int],
pose: wp.array[wp.mat22],
activation: wp.array[float],
materials: wp.array2d[float],
f: wp.array[wp.vec3],
):
tid = wp.tid()
k_mu = materials[tid, 0]
k_lambda = materials[tid, 1]
k_damp = materials[tid, 2]
k_drag = materials[tid, 3]
k_lift = materials[tid, 4]
i = indices[tid, 0]
j = indices[tid, 1]
k = indices[tid, 2]
x0 = x[i] # point zero
x1 = x[j] # point one
x2 = x[k] # point two
v0 = v[i] # vel zero
v1 = v[j] # vel one
v2 = v[k] # vel two
x10 = x1 - x0 # barycentric coordinates (centered at p)
x20 = x2 - x0
v10 = v1 - v0
v20 = v2 - v0
Dm = pose[tid]
inv_rest_area = wp.determinant(Dm) * 2.0 # 1 / det(A) = det(A^-1)
rest_area = 1.0 / inv_rest_area
# scale stiffness coefficients to account for area
k_mu = k_mu * rest_area
k_lambda = k_lambda * rest_area
k_damp = k_damp * rest_area
# F = Xs*Xm^-1
F1 = x10 * Dm[0, 0] + x20 * Dm[1, 0]
F2 = x10 * Dm[0, 1] + x20 * Dm[1, 1]
# dFdt = Vs*Xm^-1
dFdt1 = v10 * Dm[0, 0] + v20 * Dm[1, 0]
dFdt2 = v10 * Dm[0, 1] + v20 * Dm[1, 1]
# deviatoric PK1 + damping term
P1 = F1 * k_mu + dFdt1 * k_damp
P2 = F2 * k_mu + dFdt2 * k_damp
# -----------------------------
# Neo-Hookean (with rest stability)
# force = P*Dm'
f1 = P1 * Dm[0, 0] + P2 * Dm[0, 1]
f2 = P1 * Dm[1, 0] + P2 * Dm[1, 1]
# -----------------------------
# Area Preservation
n = wp.cross(x10, x20)
area = wp.length(n) * 0.5
n = wp.normalize(n)
# actuation
act = activation[tid]
# Apply area preservation only when k_lambda > 0
if k_lambda > 0.0:
alpha = 1.0 + k_mu / k_lambda
# J-alpha
c = area * inv_rest_area - alpha + act
# dJdx
dcdq = wp.cross(x20, n) * inv_rest_area * 0.5
dcdr = wp.cross(n, x10) * inv_rest_area * 0.5
f_area = k_lambda * c
# -----------------------------
# Area Damping
dcdt = wp.dot(dcdq, v1) + wp.dot(dcdr, v2) - wp.dot(dcdq + dcdr, v0)
f_damp = k_damp * dcdt
f1 = f1 + dcdq * (f_area + f_damp)
f2 = f2 + dcdr * (f_area + f_damp)
f0 = f1 + f2
# -----------------------------
# Lift + Drag
vmid = (v0 + v1 + v2) * 0.3333
vdir = wp.normalize(vmid)
f_drag = vmid * (k_drag * area * wp.abs(wp.dot(n, vmid)))
f_lift = n * (k_lift * area * (wp.HALF_PI - wp.acos(wp.dot(n, vdir)))) * wp.dot(vmid, vmid)
f0 = f0 - f_drag - f_lift
f1 = f1 + f_drag + f_lift
f2 = f2 + f_drag + f_lift
# apply forces
wp.atomic_add(f, i, f0)
wp.atomic_sub(f, j, f1)
wp.atomic_sub(f, k, f2)
@wp.kernel
def eval_bending(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
indices: wp.array2d[int],
rest: wp.array[float],
bending_properties: wp.array2d[float],
f: wp.array[wp.vec3],
):
tid = wp.tid()
eps = 1.0e-6
ke = bending_properties[tid, 0]
kd = bending_properties[tid, 1]
i = indices[tid, 0]
j = indices[tid, 1]
k = indices[tid, 2]
l = indices[tid, 3]
if i == -1 or j == -1 or k == -1 or l == -1:
return
rest_angle = rest[tid]
x1 = x[i]
x2 = x[j]
x3 = x[k]
x4 = x[l]
v1 = v[i]
v2 = v[j]
v3 = v[k]
v4 = v[l]
n1 = wp.cross(x3 - x1, x4 - x1) # normal to face 1
n2 = wp.cross(x4 - x2, x3 - x2) # normal to face 2
e = x4 - x3
n1_length = wp.length(n1)
n2_length = wp.length(n2)
e_length = wp.length(e)
# Check for degenerate cases
if n1_length < eps or n2_length < eps or e_length < eps:
return
n1_hat = n1 / n1_length
n2_hat = n2 / n2_length
e_hat = e / e_length
cos_theta = wp.dot(n1_hat, n2_hat)
sin_theta = wp.dot(wp.cross(n1_hat, n2_hat), e_hat)
theta = wp.atan2(sin_theta, cos_theta)
d1 = -n1_hat * e_length
d2 = -n2_hat * e_length
d3 = -n1_hat * wp.dot(x1 - x4, e_hat) - n2_hat * wp.dot(x2 - x4, e_hat)
d4 = -n1_hat * wp.dot(x3 - x1, e_hat) - n2_hat * wp.dot(x3 - x2, e_hat)
# elastic
f_elastic = ke * (theta - rest_angle)
# damping
f_damp = kd * (wp.dot(d1, v1) + wp.dot(d2, v2) + wp.dot(d3, v3) + wp.dot(d4, v4))
# total force, proportional to edge length
f_total = -e_length * (f_elastic + f_damp)
wp.atomic_add(f, i, d1 * f_total)
wp.atomic_add(f, j, d2 * f_total)
wp.atomic_add(f, k, d3 * f_total)
wp.atomic_add(f, l, d4 * f_total)
@wp.kernel
def eval_tetrahedra(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
indices: wp.array2d[int],
pose: wp.array[wp.mat33],
activation: wp.array[float],
materials: wp.array2d[float],
f: wp.array[wp.vec3],
):
tid = wp.tid()
i = indices[tid, 0]
j = indices[tid, 1]
k = indices[tid, 2]
l = indices[tid, 3]
act = activation[tid]
k_mu = materials[tid, 0]
k_lambda = materials[tid, 1]
k_damp = materials[tid, 2]
x0 = x[i]
x1 = x[j]
x2 = x[k]
x3 = x[l]
v0 = v[i]
v1 = v[j]
v2 = v[k]
v3 = v[l]
x10 = x1 - x0
x20 = x2 - x0
x30 = x3 - x0
v10 = v1 - v0
v20 = v2 - v0
v30 = v3 - v0
Ds = wp.matrix_from_cols(x10, x20, x30)
Dm = pose[tid]
inv_rest_volume = wp.determinant(Dm) * 6.0
rest_volume = 1.0 / inv_rest_volume
alpha = 1.0 + k_mu / k_lambda - k_mu / (4.0 * k_lambda)
# scale stiffness coefficients to account for area
k_mu = k_mu * rest_volume
k_lambda = k_lambda * rest_volume
k_damp = k_damp * rest_volume
# F = Xs*Xm^-1
F = Ds * Dm
dFdt = wp.matrix_from_cols(v10, v20, v30) * Dm
col1 = wp.vec3(F[0, 0], F[1, 0], F[2, 0])
col2 = wp.vec3(F[0, 1], F[1, 1], F[2, 1])
col3 = wp.vec3(F[0, 2], F[1, 2], F[2, 2])
# -----------------------------
# Neo-Hookean (with rest stability [Smith et al 2018])
Ic = wp.dot(col1, col1) + wp.dot(col2, col2) + wp.dot(col3, col3)
# deviatoric part
P = F * k_mu * (1.0 - 1.0 / (Ic + 1.0)) + dFdt * k_damp
H = P * wp.transpose(Dm)
f1 = wp.vec3(H[0, 0], H[1, 0], H[2, 0])
f2 = wp.vec3(H[0, 1], H[1, 1], H[2, 1])
f3 = wp.vec3(H[0, 2], H[1, 2], H[2, 2])
# -----------------------------
# C_sqrt
# alpha = 1.0
# r_s = wp.sqrt(wp.abs(dot(col1, col1) + dot(col2, col2) + dot(col3, col3) - 3.0))
# f1 = wp.vec3()
# f2 = wp.vec3()
# f3 = wp.vec3()
# if (r_s > 0.0):
# r_s_inv = 1.0/r_s
# C = r_s
# dCdx = F*wp.transpose(Dm)*r_s_inv*wp.sign(r_s)
# grad1 = vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
# grad2 = vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
# grad3 = vec3(dCdx[0,2], dCdx[1,2], dCdx[2,2])
# f1 = grad1*C*k_mu
# f2 = grad2*C*k_mu
# f3 = grad3*C*k_mu
# -----------------------------
# C_spherical
# alpha = 1.0
# r_s = wp.sqrt(dot(col1, col1) + dot(col2, col2) + dot(col3, col3))
# r_s_inv = 1.0/r_s
# C = r_s - wp.sqrt(3.0)
# dCdx = F*wp.transpose(Dm)*r_s_inv
# grad1 = vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
# grad2 = vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
# grad3 = vec3(dCdx[0,2], dCdx[1,2], dCdx[2,2])
# f1 = grad1*C*k_mu
# f2 = grad2*C*k_mu
# f3 = grad3*C*k_mu
# ----------------------------
# C_D
# alpha = 1.0
# r_s = wp.sqrt(dot(col1, col1) + dot(col2, col2) + dot(col3, col3))
# C = r_s*r_s - 3.0
# dCdx = F*wp.transpose(Dm)*2.0
# grad1 = vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
# grad2 = vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
# grad3 = vec3(dCdx[0,2], dCdx[1,2], dCdx[2,2])
# f1 = grad1*C*k_mu
# f2 = grad2*C*k_mu
# f3 = grad3*C*k_mu
# ----------------------------
# Hookean
# alpha = 1.0
# I = wp.matrix_from_cols(wp.vec3(1.0, 0.0, 0.0),
# wp.vec3(0.0, 1.0, 0.0),
# wp.vec3(0.0, 0.0, 1.0))
# P = (F + wp.transpose(F) + I*(0.0-2.0))*k_mu
# H = P * wp.transpose(Dm)
# f1 = wp.vec3(H[0, 0], H[1, 0], H[2, 0])
# f2 = wp.vec3(H[0, 1], H[1, 1], H[2, 1])
# f3 = wp.vec3(H[0, 2], H[1, 2], H[2, 2])
# hydrostatic part
J = wp.determinant(F)
# print(J)
s = inv_rest_volume / 6.0
dJdx1 = wp.cross(x20, x30) * s
dJdx2 = wp.cross(x30, x10) * s
dJdx3 = wp.cross(x10, x20) * s
f_volume = (J - alpha + act) * k_lambda
f_damp = (wp.dot(dJdx1, v1) + wp.dot(dJdx2, v2) + wp.dot(dJdx3, v3)) * k_damp
f_total = f_volume + f_damp
f1 = f1 + dJdx1 * f_total
f2 = f2 + dJdx2 * f_total
f3 = f3 + dJdx3 * f_total
f0 = -(f1 + f2 + f3)
# apply forces
wp.atomic_sub(f, i, f0)
wp.atomic_sub(f, j, f1)
wp.atomic_sub(f, k, f2)
wp.atomic_sub(f, l, f3)
def eval_spring_forces(model: Model, state: State, particle_f: wp.array):
if model.spring_count:
wp.launch(
kernel=eval_spring,
dim=model.spring_count,
inputs=[
state.particle_q,
state.particle_qd,
model.spring_indices,
model.spring_rest_length,
model.spring_stiffness,
model.spring_damping,
],
outputs=[particle_f],
device=model.device,
)
def eval_triangle_forces(model: Model, state: State, control: Control, particle_f: wp.array):
if model.tri_count:
wp.launch(
kernel=eval_triangle,
dim=model.tri_count,
inputs=[
state.particle_q,
state.particle_qd,
model.tri_indices,
model.tri_poses,
control.tri_activations,
model.tri_materials,
],
outputs=[particle_f],
device=model.device,
)
def eval_bending_forces(model: Model, state: State, particle_f: wp.array):
if model.edge_count:
wp.launch(
kernel=eval_bending,
dim=model.edge_count,
inputs=[
state.particle_q,
state.particle_qd,
model.edge_indices,
model.edge_rest_angle,
model.edge_bending_properties,
],
outputs=[particle_f],
device=model.device,
)
def eval_tetrahedra_forces(model: Model, state: State, control: Control, particle_f: wp.array):
if model.tet_count:
wp.launch(
kernel=eval_tetrahedra,
dim=model.tet_count,
inputs=[
state.particle_q,
state.particle_qd,
model.tet_indices,
model.tet_poses,
control.tet_activations,
model.tet_materials,
],
outputs=[particle_f],
device=model.device,
)
================================================
FILE: newton/_src/solvers/semi_implicit/solver_semi_implicit.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...core.types import override
from ...sim import Contacts, Control, Model, State
from ..solver import SolverBase
from .kernels_body import (
eval_body_joint_forces,
)
from .kernels_contact import (
eval_body_contact_forces,
eval_particle_body_contact_forces,
eval_particle_contact_forces,
eval_triangle_contact_forces,
)
from .kernels_muscle import (
eval_muscle_forces,
)
from .kernels_particle import (
eval_bending_forces,
eval_spring_forces,
eval_tetrahedra_forces,
eval_triangle_forces,
)
class SolverSemiImplicit(SolverBase):
"""A semi-implicit integrator using symplectic Euler.
After constructing `Model` and `State` objects this time-integrator
may be used to advance the simulation state forward in time.
Semi-implicit time integration is a variational integrator that
preserves energy, however it not unconditionally stable, and requires a time-step
small enough to support the required stiffness and damping forces.
See: https://en.wikipedia.org/wiki/Semi-implicit_Euler_method
Joint limitations:
- Supported joint types: PRISMATIC, REVOLUTE, BALL, FIXED, FREE, DISTANCE (treated as FREE), D6.
CABLE joints are not supported.
- :attr:`~newton.Model.joint_enabled`, :attr:`~newton.Model.joint_limit_ke`/:attr:`~newton.Model.joint_limit_kd`,
:attr:`~newton.Model.joint_target_ke`/:attr:`~newton.Model.joint_target_kd`, and :attr:`~newton.Control.joint_f`
are supported.
- Joint limits and targets are not enforced for BALL joints.
- :attr:`~newton.Model.joint_armature`, :attr:`~newton.Model.joint_friction`,
:attr:`~newton.Model.joint_effort_limit`, :attr:`~newton.Model.joint_velocity_limit`,
and :attr:`~newton.Model.joint_target_mode` are not supported.
- Equality and mimic constraints are not supported.
See :ref:`Joint feature support` for the full comparison across solvers.
Example
-------
.. code-block:: python
solver = newton.solvers.SolverSemiImplicit(model)
# simulation loop
for i in range(100):
solver.step(state_in, state_out, control, contacts, dt)
state_in, state_out = state_out, state_in
"""
def __init__(
self,
model: Model,
angular_damping: float = 0.05,
friction_smoothing: float = 1.0,
joint_attach_ke: float = 1.0e4,
joint_attach_kd: float = 1.0e2,
enable_tri_contact: bool = True,
):
"""
Args:
model: The model to be simulated.
angular_damping: Angular damping factor to be used in rigid body integration. Defaults to 0.05.
friction_smoothing: Huber norm delta used for friction velocity normalization (see :func:`warp.norm_huber() `). Defaults to 1.0.
joint_attach_ke: Joint attachment spring stiffness. Defaults to 1.0e4.
joint_attach_kd: Joint attachment spring damping. Defaults to 1.0e2.
enable_tri_contact: Enable triangle contact. Defaults to True.
"""
super().__init__(model=model)
self.angular_damping = angular_damping
self.friction_smoothing = friction_smoothing
self.joint_attach_ke = joint_attach_ke
self.joint_attach_kd = joint_attach_kd
self.enable_tri_contact = enable_tri_contact
@override
def step(
self,
state_in: State,
state_out: State,
control: Control | None,
contacts: Contacts | None,
dt: float,
) -> None:
"""
Simulate the model for a given time step using the given control input.
Args:
state_in: The input state.
state_out: The output state.
control: The control input.
Defaults to `None` which means the control values from the
:class:`Model` are used.
contacts: The contact information.
Defaults to `None` which means no contacts are used.
dt: The time step (typically in seconds).
.. warning::
The ``eval_particle_contact`` kernel for particle-particle contact handling may corrupt the gradient computation
for simulations involving particle collisions.
To disable it, set :attr:`newton.Model.particle_grid` to `None` prior to calling :meth:`step`.
"""
with wp.ScopedTimer("simulate", False):
particle_f = None
body_f = None
if state_in.particle_count:
particle_f = state_in.particle_f
if state_in.body_count:
body_f = state_in.body_f
model = self.model
if control is None:
control = model.control(clone_variables=False)
body_f_work = body_f
if body_f is not None and model.joint_count and control.joint_f is not None:
# Avoid accumulating joint_f into the persistent state body_f buffer.
body_f_work = wp.clone(body_f)
# damped springs
eval_spring_forces(model, state_in, particle_f)
# triangle elastic and lift/drag forces
eval_triangle_forces(model, state_in, control, particle_f)
# triangle bending
eval_bending_forces(model, state_in, particle_f)
# tetrahedral FEM
eval_tetrahedra_forces(model, state_in, control, particle_f)
# body joints
eval_body_joint_forces(model, state_in, control, body_f_work, self.joint_attach_ke, self.joint_attach_kd)
# muscles
if False:
eval_muscle_forces(model, state_in, control, body_f)
# particle-particle interactions
eval_particle_contact_forces(model, state_in, particle_f)
# triangle/triangle contacts
if self.enable_tri_contact:
eval_triangle_contact_forces(model, state_in, particle_f)
# body contacts
eval_body_contact_forces(
model, state_in, contacts, friction_smoothing=self.friction_smoothing, body_f_out=body_f_work
)
# particle shape contact
eval_particle_body_contact_forces(
model, state_in, contacts, particle_f, body_f_work, body_f_in_world_frame=False
)
self.integrate_particles(model, state_in, state_out, dt)
if body_f_work is body_f:
self.integrate_bodies(model, state_in, state_out, dt, self.angular_damping)
else:
body_f_prev = state_in.body_f
state_in.body_f = body_f_work
self.integrate_bodies(model, state_in, state_out, dt, self.angular_damping)
state_in.body_f = body_f_prev
================================================
FILE: newton/_src/solvers/solver.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ..geometry import ParticleFlags
from ..sim import BodyFlags, Contacts, Control, Model, ModelBuilder, State
@wp.kernel
def integrate_particles(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
f: wp.array[wp.vec3],
w: wp.array[float],
particle_flags: wp.array[wp.int32],
particle_world: wp.array[wp.int32],
gravity: wp.array[wp.vec3],
dt: float,
v_max: float,
x_new: wp.array[wp.vec3],
v_new: wp.array[wp.vec3],
):
tid = wp.tid()
x0 = x[tid]
if (particle_flags[tid] & ParticleFlags.ACTIVE) == 0:
x_new[tid] = x0
return
v0 = v[tid]
f0 = f[tid]
inv_mass = w[tid]
world_idx = particle_world[tid]
world_g = gravity[wp.max(world_idx, 0)]
# simple semi-implicit Euler. v1 = v0 + a dt, x1 = x0 + v1 dt
v1 = v0 + (f0 * inv_mass + world_g * wp.step(-inv_mass)) * dt
# enforce velocity limit to prevent instability
v1_mag = wp.length(v1)
if v1_mag > v_max:
v1 *= v_max / v1_mag
x1 = x0 + v1 * dt
x_new[tid] = x1
v_new[tid] = v1
@wp.func
def integrate_rigid_body(
q: wp.transform,
qd: wp.spatial_vector,
f: wp.spatial_vector,
com: wp.vec3,
inertia: wp.mat33,
inv_mass: float,
inv_inertia: wp.mat33,
gravity: wp.vec3,
angular_damping: float,
dt: float,
):
# unpack transform
x0 = wp.transform_get_translation(q)
r0 = wp.transform_get_rotation(q)
# unpack spatial twist
w0 = wp.spatial_bottom(qd)
v0 = wp.spatial_top(qd)
# unpack spatial wrench
t0 = wp.spatial_bottom(f)
f0 = wp.spatial_top(f)
x_com = x0 + wp.quat_rotate(r0, com)
# linear part
v1 = v0 + (f0 * inv_mass + gravity * wp.nonzero(inv_mass)) * dt
x1 = x_com + v1 * dt
# angular part (compute in body frame)
wb = wp.quat_rotate_inv(r0, w0)
tb = wp.quat_rotate_inv(r0, t0) - wp.cross(wb, inertia * wb) # coriolis forces
w1 = wp.quat_rotate(r0, wb + inv_inertia * tb * dt)
r1 = wp.normalize(r0 + wp.quat(w1, 0.0) * r0 * 0.5 * dt)
# angular damping
w1 *= 1.0 - angular_damping * dt
q_new = wp.transform(x1 - wp.quat_rotate(r1, com), r1)
qd_new = wp.spatial_vector(v1, w1)
return q_new, qd_new
# semi-implicit Euler integration
@wp.kernel
def integrate_bodies(
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_f: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
m: wp.array[float],
I: wp.array[wp.mat33],
inv_m: wp.array[float],
inv_I: wp.array[wp.mat33],
body_flags: wp.array[wp.int32],
body_world: wp.array[wp.int32],
gravity: wp.array[wp.vec3],
angular_damping: float,
dt: float,
# outputs
body_q_new: wp.array[wp.transform],
body_qd_new: wp.array[wp.spatial_vector],
):
tid = wp.tid()
if (body_flags[tid] & BodyFlags.KINEMATIC) != 0:
# Kinematic bodies are user-prescribed and pass through unchanged.
# NOTE: SemiImplicit does not zero inv_mass/inv_inertia for kinematic
# bodies in the contact solver, so contact responses may be weaker
# than XPBD or MuJoCo/Featherstone which treat them as infinite-mass.
body_q_new[tid] = body_q[tid]
body_qd_new[tid] = body_qd[tid]
return
# positions
q = body_q[tid]
qd = body_qd[tid]
f = body_f[tid]
# masses
inv_mass = inv_m[tid] # 1 / mass
inertia = I[tid]
inv_inertia = inv_I[tid] # inverse of 3x3 inertia matrix
com = body_com[tid]
world_idx = body_world[tid]
world_g = gravity[wp.max(world_idx, 0)]
q_new, qd_new = integrate_rigid_body(
q,
qd,
f,
com,
inertia,
inv_mass,
inv_inertia,
world_g,
angular_damping,
dt,
)
body_q_new[tid] = q_new
body_qd_new[tid] = qd_new
@wp.kernel
def _update_effective_inv_mass_inertia(
body_flags: wp.array[wp.int32],
model_inv_mass: wp.array[float],
model_inv_inertia: wp.array[wp.mat33],
eff_inv_mass: wp.array[float],
eff_inv_inertia: wp.array[wp.mat33],
):
tid = wp.tid()
if (body_flags[tid] & BodyFlags.KINEMATIC) != 0:
eff_inv_mass[tid] = 0.0
eff_inv_inertia[tid] = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
else:
eff_inv_mass[tid] = model_inv_mass[tid]
eff_inv_inertia[tid] = model_inv_inertia[tid]
class SolverBase:
"""Generic base class for solvers.
The implementation provides helper kernels to integrate rigid bodies and
particles. Concrete solver back-ends should derive from this class and
override :py:meth:`step` as well as :py:meth:`notify_model_changed` where
necessary.
"""
def __init__(self, model: Model):
self.model = model
@property
def device(self) -> wp.Device:
"""
Get the device used by the solver.
Returns:
wp.Device: The device used by the solver.
"""
return self.model.device
def _init_kinematic_state(self):
"""Allocate and populate effective inverse mass/inertia arrays."""
model = self.model
self.body_inv_mass_effective = wp.empty_like(model.body_inv_mass)
self.body_inv_inertia_effective = wp.empty_like(model.body_inv_inertia)
if model.body_count:
self._refresh_kinematic_state()
def _refresh_kinematic_state(self):
"""Update effective arrays from model, zeroing kinematic bodies."""
model = self.model
if model.body_count:
wp.launch(
kernel=_update_effective_inv_mass_inertia,
dim=model.body_count,
inputs=[
model.body_flags,
model.body_inv_mass,
model.body_inv_inertia,
self.body_inv_mass_effective,
self.body_inv_inertia_effective,
],
device=model.device,
)
def integrate_bodies(
self,
model: Model,
state_in: State,
state_out: State,
dt: float,
angular_damping: float = 0.0,
) -> None:
"""
Integrate the rigid bodies of the model.
Args:
model (Model): The model to integrate.
state_in (State): The input state.
state_out (State): The output state.
dt (float): The time step (typically in seconds).
angular_damping (float, optional): The angular damping factor.
Defaults to 0.0.
"""
if model.body_count:
wp.launch(
kernel=integrate_bodies,
dim=model.body_count,
inputs=[
state_in.body_q,
state_in.body_qd,
state_in.body_f,
model.body_com,
model.body_mass,
model.body_inertia,
model.body_inv_mass,
model.body_inv_inertia,
model.body_flags,
model.body_world,
model.gravity,
angular_damping,
dt,
],
outputs=[state_out.body_q, state_out.body_qd],
device=model.device,
)
def integrate_particles(
self,
model: Model,
state_in: State,
state_out: State,
dt: float,
) -> None:
"""
Integrate the particles of the model.
Args:
model (Model): The model to integrate.
state_in (State): The input state.
state_out (State): The output state.
dt (float): The time step (typically in seconds).
"""
if model.particle_count:
wp.launch(
kernel=integrate_particles,
dim=model.particle_count,
inputs=[
state_in.particle_q,
state_in.particle_qd,
state_in.particle_f,
model.particle_inv_mass,
model.particle_flags,
model.particle_world,
model.gravity,
dt,
model.particle_max_velocity,
],
outputs=[state_out.particle_q, state_out.particle_qd],
device=model.device,
)
def step(
self, state_in: State, state_out: State, control: Control | None, contacts: Contacts | None, dt: float
) -> None:
"""
Simulate the model for a given time step using the given control input.
Args:
state_in: The input state.
state_out: The output state.
control: The control input.
Defaults to `None` which means the control values from the
:class:`Model` are used.
contacts: The contact information.
dt: The time step (typically in seconds).
"""
raise NotImplementedError()
def notify_model_changed(self, flags: int) -> None:
"""Notify the solver that parts of the :class:`~newton.Model` were modified.
The *flags* argument is a bit-mask composed of the
:class:`~newton.solvers.SolverNotifyFlags` enums defined in :mod:`newton.solvers`.
Each flag represents a category of model data that may have been
updated after the solver was created. Passing the appropriate
combination of flags enables a solver implementation to refresh its
internal buffers without having to recreate the whole solver object.
Valid flags are:
============================================== =============================================================
Constant Description
============================================== =============================================================
``SolverNotifyFlags.JOINT_PROPERTIES`` Joint transforms or coordinates have changed.
``SolverNotifyFlags.JOINT_DOF_PROPERTIES`` Joint axis limits, targets, modes, DOF state, or force buffers have changed.
``SolverNotifyFlags.BODY_PROPERTIES`` Rigid-body pose or velocity buffers have changed.
``SolverNotifyFlags.BODY_INERTIAL_PROPERTIES`` Rigid-body mass or inertia tensors have changed.
``SolverNotifyFlags.SHAPE_PROPERTIES`` Shape transforms or geometry have changed.
``SolverNotifyFlags.MODEL_PROPERTIES`` Model global properties (e.g., gravity) have changed.
============================================== =============================================================
Args:
flags (int): Bit-mask of model-update flags indicating which model
properties changed.
"""
pass
def update_contacts(self, contacts: Contacts, state: State | None = None) -> None:
"""
Update a Contacts object with forces from the solver state. Where the solver state contains
other contact data, convert that data to the Contacts format.
Args:
contacts: The object to update from the solver state.
state: Optional simulation state, used by some solvers.
"""
raise NotImplementedError()
@classmethod
def register_custom_attributes(cls, builder: ModelBuilder) -> None:
"""
Register custom attributes for the solver.
Args:
builder (ModelBuilder): The model builder to register the custom attributes to.
"""
pass
================================================
FILE: newton/_src/solvers/style3d/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Style3D solver module.
This module provides helper functions for setting up Style3D cloth assets.
Use :class:`~newton.solvers.SolverStyle3D` as the canonical public solver
import path.
"""
from .cloth import (
add_cloth_grid,
add_cloth_mesh,
)
__all__ = [
"add_cloth_grid",
"add_cloth_mesh",
]
================================================
FILE: newton/_src/solvers/style3d/builder.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import numpy as np
import warp as wp
from .linear_solver import NonZeroEntry
########################################################################################################################
########################################### PD Matrix Builder Kernels ############################################
########################################################################################################################
@wp.func
def add_connection(v0: int, v1: int, counts: wp.array[int], neighbors: wp.array2d[int]):
"""
Adds a connection from vertex v0 to vertex v1 in a sparse matrix format.
If the connection already exists, returns its slot index.
Otherwise, adds the connection in the next available slot and returns the new slot index.
Args:
v0 (int): Index of the source vertex.
v1 (int): Index of the target vertex (to be added as neighbor of v0).
counts (wp.array): 1D array storing how many neighbors each vertex currently has.
neighbors (wp.array2d): 2D array storing the neighbor list for each vertex.
Returns:
int: The slot index in `neighbors[v0]` where `v1` is stored,
or -1 if there is no more space to store the new neighbor.
"""
for slot in range(counts[v0]):
if neighbors[v0, slot] == v1:
return slot # Connection already exists
slot = counts[v0]
if slot < neighbors.shape[1]:
neighbors[v0, slot] = v1 # Add new neighbor
counts[v0] += 1
return slot
else:
wp.printf("Error: Too many neighbors for vertex %d (max %d)\n", v0, neighbors.shape[1])
return -1
@wp.kernel
def add_bend_constraints_kernel(
num_edge: int,
edge_inds: wp.array2d[int],
bend_hess: wp.array3d[float],
# outputs
neighbors: wp.array2d[int],
neighbor_counts: wp.array[int],
nz_values: wp.array2d[float],
diags: wp.array[float],
):
"""Accumulate contributions from bending constraints into a sparse matrix structure."""
for eid in range(num_edge):
edge = edge_inds[eid]
if edge[0] < 0 or edge[1] < 0:
continue # Skip invalid edge
tmp_bend_hess = bend_hess[eid]
for i in range(4):
for j in range(i, 4):
weight = tmp_bend_hess[i][j]
if i != j:
# Add off-diagonal symmetric entries
slot_ij = add_connection(edge[i], edge[j], neighbor_counts, neighbors)
slot_ji = add_connection(edge[j], edge[i], neighbor_counts, neighbors)
if slot_ij >= 0:
nz_values[edge[i], slot_ij] += weight
if slot_ji >= 0:
nz_values[edge[j], slot_ji] += weight
else:
# Diagonal contribution
diags[edge[i]] += weight
@wp.kernel
def add_stretch_constraints_kernel(
num_tri: int,
tri_indices: wp.array2d[int],
tri_areas: wp.array[float],
tri_poses: wp.array3d[float],
tri_aniso_ke: wp.array2d[float],
# outputs
neighbors: wp.array2d[int],
neighbor_counts: wp.array[int],
nz_values: wp.array2d[float],
diags: wp.array[float],
):
"""Accumulate contributions from stretch constraints into the sparse matrix."""
for fid in range(num_tri):
area = tri_areas[fid]
inv_dm = tri_poses[fid]
ku = tri_aniso_ke[fid][0]
kv = tri_aniso_ke[fid][1]
ks = tri_aniso_ke[fid][2]
face = wp.vec3i(tri_indices[fid][0], tri_indices[fid][1], tri_indices[fid][2])
# Derivatives of deformation gradient components
dFu_dx = wp.vec3(-inv_dm[0][0] - inv_dm[1][0], inv_dm[0][0], inv_dm[1][0])
dFv_dx = wp.vec3(-inv_dm[0][1] - inv_dm[1][1], inv_dm[0][1], inv_dm[1][1])
for i in range(3):
for j in range(i, 3):
# Weight is a combination of anisotropic stiffness components
weight = area * ((ku + ks) * dFu_dx[i] * dFu_dx[j] + (kv + ks) * dFv_dx[i] * dFv_dx[j])
if i != j:
# Off-diagonal symmetric terms
slot_ij = add_connection(face[i], face[j], neighbor_counts, neighbors)
slot_ji = add_connection(face[j], face[i], neighbor_counts, neighbors)
if slot_ij >= 0:
nz_values[face[i], slot_ij] += weight
if slot_ji >= 0:
nz_values[face[j], slot_ji] += weight
else:
# Diagonal contribution
diags[face[i]] += weight
@wp.kernel
def assemble_nz_ell_kernel(
neighbors: wp.array2d[int],
nz_values: wp.array2d[float],
neighbor_counts: wp.array[int],
# outputs
nz_ell: wp.array2d[NonZeroEntry],
):
tid = wp.tid()
for k in range(neighbor_counts[tid]):
nz_entry = NonZeroEntry()
nz_entry.value = nz_values[tid, k]
nz_entry.column_index = neighbors[tid, k]
nz_ell[k, tid] = nz_entry
########################################################################################################################
############################################### PD Matrix Builder ################################################
########################################################################################################################
class PDMatrixBuilder:
"""Helper class for building Projective Dynamics (PD) matrix in sparse ELL format."""
def __init__(self, num_verts: int, max_neighbor: int = 32):
self.num_verts = num_verts
self.max_neighbors = max_neighbor
self.counts = wp.zeros(num_verts, dtype=wp.int32, device="cpu")
self.diags = wp.zeros(num_verts, dtype=wp.float32, device="cpu")
self.values = wp.zeros(shape=(num_verts, max_neighbor), dtype=wp.float32, device="cpu")
self.neighbors = wp.zeros(shape=(num_verts, max_neighbor), dtype=wp.int32, device="cpu")
def add_stretch_constraints(
self,
tri_indices: list[list[int]],
tri_poses: list[list[list[float]]],
tri_aniso_ke: list[list[float]],
tri_areas: list[float],
):
if len(tri_indices) == 0:
return
# Convert inputs to Warp arrays
tri_inds_wp = wp.array2d(tri_indices, dtype=int, device="cpu").reshape((-1, 3))
tri_poses_wp = wp.array3d(tri_poses, dtype=float, device="cpu").reshape((-1, 2, 2))
tri_aniso_ke_wp = wp.array2d(tri_aniso_ke, dtype=float, device="cpu").reshape((-1, 3))
tri_areas_wp = wp.array(tri_areas, dtype=float, device="cpu")
# Launch kernel to compute stretch contributions
wp.launch(
add_stretch_constraints_kernel,
dim=1,
inputs=[
len(tri_indices),
tri_inds_wp,
tri_areas_wp,
tri_poses_wp,
tri_aniso_ke_wp,
],
outputs=[self.neighbors, self.counts, self.values, self.diags],
device="cpu",
)
def add_bend_constraints(
self,
edge_indices: list[list[int]],
edge_bending_properties: list[list[float]],
edge_rest_area: list[float],
edge_bending_cot: list[list[float]],
):
if len(edge_indices) == 0:
return
num_edge = len(edge_indices)
edge_inds = np.array(edge_indices).reshape(-1, 4)
edge_area = np.array(edge_rest_area)
edge_prop = np.array(edge_bending_properties).reshape(-1, 2)
edge_stiff = edge_prop[:, 0] / edge_area
bend_cot = np.array(edge_bending_cot).reshape(-1, 4)
bend_weight = np.zeros(shape=(num_edge, 4), dtype=np.float32)
# Compute per-vertex weights using cotangent terms
bend_weight[:, 2] = bend_cot[:, 2] + bend_cot[:, 3]
bend_weight[:, 3] = bend_cot[:, 0] + bend_cot[:, 1]
bend_weight[:, 0] = -bend_cot[:, 0] - bend_cot[:, 2]
bend_weight[:, 1] = -bend_cot[:, 1] - bend_cot[:, 3]
# Construct Hessian matrix per edge (outer product)
bend_hess = (
bend_weight[:, :, np.newaxis] * bend_weight[:, np.newaxis, :] * edge_stiff[:, np.newaxis, np.newaxis]
) # shape is num_edge,4,4
# Convert to Warp arrays
edge_inds_wp = wp.array2d(edge_inds, dtype=int, device="cpu")
bend_hess_wp = wp.array3d(bend_hess, dtype=float, device="cpu")
# Launch kernel to accumulate bend constraints
wp.launch(
add_bend_constraints_kernel,
dim=1,
inputs=[num_edge, edge_inds_wp, bend_hess_wp],
outputs=[self.neighbors, self.counts, self.values, self.diags],
device="cpu",
)
def finalize(self, device):
"""Assembles final sparse matrix in ELL format.
Returns:
diag: wp.array of diagonal entries
num_nz: wp.array of non-zero count per row
nz_ell: wp.array2d of NonZeroEntry (value + column)
"""
diag = wp.array(self.diags, dtype=float, device=device)
num_nz = wp.array(self.counts, dtype=int, device=device)
nz_ell = wp.array2d(shape=(self.max_neighbors, self.num_verts), dtype=NonZeroEntry, device=device)
nz_values = wp.array2d(self.values, dtype=float, device=device)
neighbors = wp.array2d(self.neighbors, dtype=int, device=device)
wp.launch(
assemble_nz_ell_kernel,
dim=self.num_verts,
inputs=[neighbors, nz_values, num_nz],
outputs=[nz_ell],
device=device,
)
return diag, num_nz, nz_ell
================================================
FILE: newton/_src/solvers/style3d/cloth.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Style3D cloth helpers built on :class:`newton.ModelBuilder` custom attributes."""
from __future__ import annotations
from collections.abc import Sequence
from typing import TYPE_CHECKING, Any
import numpy as np
import warp as wp
from ...core.types import Vec2, Vec3
from ...geometry.flags import ParticleFlags
from ...geometry.kernels import compute_edge_aabbs
from ...utils.mesh import MeshAdjacency
if TYPE_CHECKING:
from ...sim.builder import ModelBuilder
def _normalize_edge_aniso_values(edge_aniso_ke: Sequence[Vec3] | Vec3 | None, edge_count: int) -> list[Vec3]:
"""Expand anisotropic bending values to a per-edge list.
Args:
edge_aniso_ke: Optional anisotropic stiffness. A single value is
broadcast, a sequence must match ``edge_count``, and ``None`` yields
zeros.
edge_count: Number of edges to generate values for.
Returns:
Per-edge anisotropic stiffness values.
Raises:
ValueError: If a sequence length does not match ``edge_count``.
"""
if edge_aniso_ke is None:
return [wp.vec3(0.0, 0.0, 0.0)] * edge_count
if isinstance(edge_aniso_ke, (list, np.ndarray)):
values = list(edge_aniso_ke)
if len(values) == 1 and edge_count != 1:
return [values[0]] * edge_count
if len(values) != edge_count:
raise ValueError(f"Expected {edge_count} edge_aniso_ke values, got {len(values)}")
return values
return [edge_aniso_ke] * edge_count
def _compute_panel_triangles(panel_verts: np.ndarray, panel_indices: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""Compute panel-space rest data for triangles.
Args:
panel_verts: 2D panel-space coordinates per vertex.
panel_indices: Triangle indices into ``panel_verts``.
Returns:
Tuple of ``(inv_D, areas)`` where ``inv_D`` is the per-triangle inverse
rest matrix and ``areas`` is the signed panel area (degenerate triangles
are clamped to zero).
"""
p = panel_verts[panel_indices[:, 0]]
q = panel_verts[panel_indices[:, 1]]
r = panel_verts[panel_indices[:, 2]]
qp = q - p
rp = r - p
D = np.concatenate((qp[..., None], rp[..., None]), axis=-1)
areas = np.linalg.det(D) / 2.0
areas[areas < 0.0] = 0.0
D[areas == 0.0] = np.eye(2)[None, ...]
inv_D = np.linalg.inv(D)
return inv_D, areas
def _compute_edge_bending_data(
panel_verts: np.ndarray,
panel_indices: np.ndarray,
tri_indices: np.ndarray,
edge_aniso_ke: Sequence[Vec3] | Vec3 | None,
) -> tuple[
np.ndarray,
list[float] | None,
list[float],
list[tuple[float, float, float, float]],
list[Vec3] | None,
]:
"""Compute edge indices and bending data from panel-space geometry.
Args:
panel_verts: 2D panel-space coordinates per vertex.
panel_indices: Triangle indices into ``panel_verts``.
tri_indices: Triangle indices into the 3D vertex list.
edge_aniso_ke: Optional anisotropic bending stiffness values.
Returns:
Tuple of ``(edge_indices, edge_ke_values, edge_rest_area,
edge_bending_cot, edge_aniso_values)`` suitable for Style3D edge
attributes.
"""
adjacency = MeshAdjacency(tri_indices.tolist())
edge_indices = np.fromiter(
(x for e in adjacency.edges.values() for x in (e.o0, e.o1, e.v0, e.v1, e.f0, e.f1)),
int,
).reshape(-1, 6)
edge_count = edge_indices.shape[0]
edge_aniso_values = None
edge_ke = None
if edge_aniso_ke is not None:
edge_aniso_values = _normalize_edge_aniso_values(edge_aniso_ke, edge_count)
panel_tris = panel_indices
panel_pos2d = panel_verts
f0 = edge_indices[:, 4]
f1 = edge_indices[:, 5]
v0 = edge_indices[:, 0]
v1 = edge_indices[:, 1]
edge_v0_order = np.argmax(tri_indices[f0] == v0[:, None], axis=1)
edge_v1_order = np.argmax(tri_indices[f1] == v1[:, None], axis=1)
panel_tris_f0 = panel_tris[f0]
panel_tris_f1 = panel_tris[f1]
panel_x1_f0 = panel_pos2d[panel_tris_f0[np.arange(panel_tris_f0.shape[0]), edge_v0_order]]
panel_x3_f0 = panel_pos2d[panel_tris_f0[np.arange(panel_tris_f0.shape[0]), (edge_v0_order + 1) % 3]]
panel_x4_f0 = panel_pos2d[panel_tris_f0[np.arange(panel_tris_f0.shape[0]), (edge_v0_order + 2) % 3]]
panel_x2_f1 = panel_pos2d[panel_tris_f1[np.arange(panel_tris_f1.shape[0]), edge_v1_order]]
panel_x4_f1 = panel_pos2d[panel_tris_f1[np.arange(panel_tris_f1.shape[0]), (edge_v1_order + 1) % 3]]
panel_x3_f1 = panel_pos2d[panel_tris_f1[np.arange(panel_tris_f1.shape[0]), (edge_v1_order + 2) % 3]]
panel_x43_f0 = panel_x4_f0 - panel_x3_f0
panel_x43_f1 = panel_x4_f1 - panel_x3_f1
def dot(a, b):
return (a * b).sum(axis=-1)
def cross2d(a, b):
return a[:, 0] * b[:, 1] - a[:, 1] * b[:, 0]
if edge_aniso_values is not None:
angle_f0 = np.atan2(panel_x43_f0[:, 1], panel_x43_f0[:, 0])
angle_f1 = np.atan2(panel_x43_f1[:, 1], panel_x43_f1[:, 0])
angle = (angle_f0 + angle_f1) * 0.5
sin = np.sin(angle)
cos = np.cos(angle)
sin2 = np.pow(sin, 2)
cos2 = np.pow(cos, 2)
sin12 = np.pow(sin, 12)
cos12 = np.pow(cos, 12)
aniso_ke = np.array(edge_aniso_values, dtype=float).reshape(-1, 3)
edge_ke = aniso_ke[:, 0] * sin12 + aniso_ke[:, 1] * cos12 + aniso_ke[:, 2] * 4.0 * sin2 * cos2
edge_area = (
np.abs(cross2d(panel_x43_f0, panel_x1_f0 - panel_x3_f0))
+ np.abs(cross2d(panel_x43_f1, panel_x2_f1 - panel_x3_f1))
+ 1.0e-8
) / 3.0
def cot2d(a, b, c):
ba = b - a
ca = c - a
dot_a = dot(ba, ca)
cross_a = np.abs(cross2d(ba, ca)) + 1.0e-8
return dot_a / cross_a
cot1 = cot2d(panel_x3_f0, panel_x4_f0, panel_x1_f0)
cot2 = cot2d(panel_x3_f1, panel_x4_f1, panel_x2_f1)
cot3 = cot2d(panel_x4_f0, panel_x3_f0, panel_x1_f0)
cot4 = cot2d(panel_x4_f1, panel_x3_f1, panel_x2_f1)
edge_bending_cot = list(zip(cot1, cot2, cot3, cot4, strict=False))
edge_ke_values = edge_ke.tolist() if edge_ke is not None else None
return edge_indices[:, :4], edge_ke_values, edge_area.tolist(), edge_bending_cot, edge_aniso_values
def add_cloth_mesh(
builder: ModelBuilder,
*,
pos: Vec3,
rot: Any,
vel: Vec3,
vertices: Sequence[Vec3],
indices: Sequence[int],
density: float,
scale: float = 1.0,
panel_verts: Sequence[Vec2] | None = None,
panel_indices: Sequence[int] | None = None,
tri_aniso_ke: Sequence[Vec3] | Vec3 | None = None,
edge_aniso_ke: Sequence[Vec3] | Vec3 | None = None,
tri_ka: float | None = None,
tri_kd: float | None = None,
tri_drag: float | None = None,
tri_lift: float | None = None,
edge_kd: float | None = None,
add_springs: bool = False,
spring_ke: float | None = None,
spring_kd: float | None = None,
particle_radius: float | None = None,
custom_attributes_particles: dict[str, Any] | None = None,
custom_attributes_springs: dict[str, Any] | None = None,
) -> None:
"""Add a Style3D cloth mesh using :class:`newton.ModelBuilder` custom attributes.
This helper uses :meth:`newton.ModelBuilder.add_particles`,
:meth:`newton.ModelBuilder.add_triangles`, and
:meth:`newton.ModelBuilder.add_edges`. It computes panel-space rest data for
anisotropic stretch and
bending, then injects the Style3D custom attributes:
- ``style3d:tri_aniso_ke``
- ``style3d:edge_rest_area``
- ``style3d:edge_bending_cot``
- ``style3d:aniso_ke``
It overwrites :attr:`newton.ModelBuilder.tri_poses` and
:attr:`newton.ModelBuilder.tri_areas` with panel rest data. Call
:meth:`newton.solvers.SolverStyle3D.register_custom_attributes` before
invoking this helper.
Args:
builder: :class:`newton.ModelBuilder` to populate.
pos: World-space translation for the mesh.
rot: World-space rotation for the mesh.
vel: Initial velocity for all particles.
vertices: 3D mesh vertices (list of positions).
indices: Triangle indices (3 entries per face).
density: Mass per unit area in panel space.
scale: Uniform scale applied to ``vertices`` and ``panel_verts``.
panel_verts: 2D panel coordinates (UVs). Defaults to ``vertices`` XY.
panel_indices: Triangle indices in panel space. Defaults to ``indices``.
tri_aniso_ke: Anisotropic stretch stiffness (weft, warp, shear). Can be
a single value or per-triangle list. Full lists are filtered to
valid triangles after degenerates are removed.
edge_aniso_ke: Anisotropic bending stiffness values. Can be a single
value or per-edge list (computed from mesh adjacency).
tri_ka: Triangle area stiffness (defaults to
:attr:`newton.ModelBuilder.default_tri_ka`).
tri_kd: Triangle damping (defaults to
:attr:`newton.ModelBuilder.default_tri_kd`).
tri_drag: Triangle drag coefficient (defaults to
:attr:`newton.ModelBuilder.default_tri_drag`).
tri_lift: Triangle lift coefficient (defaults to
:attr:`newton.ModelBuilder.default_tri_lift`).
edge_kd: Edge damping (defaults to
:attr:`newton.ModelBuilder.default_edge_kd`).
add_springs: If True, add structural springs across mesh edges.
spring_ke: Spring stiffness (defaults to
:attr:`newton.ModelBuilder.default_spring_ke`).
spring_kd: Spring damping (defaults to
:attr:`newton.ModelBuilder.default_spring_kd`).
particle_radius: Per-particle radius (defaults to
:attr:`newton.ModelBuilder.default_particle_radius`).
custom_attributes_particles: Extra custom attributes for particles.
custom_attributes_springs: Extra custom attributes for springs.
"""
vertices_np = np.array(vertices, dtype=float) * scale
rot_mat = np.array(wp.quat_to_matrix(rot), dtype=np.float32).reshape(3, 3)
verts_3d = np.dot(vertices_np, rot_mat.T) + np.array(pos, dtype=float)
panel_verts_np = np.array(panel_verts, dtype=float) * scale if panel_verts is not None else vertices_np[:, :2]
panel_indices_np = np.array(panel_indices if panel_indices is not None else indices, dtype=int).reshape(-1, 3)
tri_indices_np = np.array(indices, dtype=int).reshape(-1, 3)
panel_inv_D_all, panel_areas_all = _compute_panel_triangles(panel_verts_np, panel_indices_np)
valid_inds = (panel_areas_all > 0.0).nonzero()[0]
if len(valid_inds) < len(panel_areas_all):
print("inverted or degenerate triangle elements")
tri_indices_valid = tri_indices_np[valid_inds]
panel_indices_valid = panel_indices_np[valid_inds]
start_vertex = len(builder.particle_q)
radius_value = particle_radius if particle_radius is not None else builder.default_particle_radius
builder.add_particles(
verts_3d.tolist(),
[vel] * len(verts_3d),
mass=[0.0] * len(verts_3d),
radius=[radius_value] * len(verts_3d),
custom_attributes=custom_attributes_particles,
)
tri_aniso_values = tri_aniso_ke or wp.vec3(builder.default_tri_ke)
if isinstance(tri_aniso_values, (list, tuple, np.ndarray)):
tri_aniso_values = list(tri_aniso_values)
if len(tri_aniso_values) == len(tri_indices_np):
tri_aniso_values = [tri_aniso_values[idx] for idx in valid_inds]
elif len(tri_aniso_values) == 1 and len(tri_indices_valid) != 1:
tri_aniso_values = [tri_aniso_values[0]] * len(tri_indices_valid)
elif len(tri_aniso_values) != len(tri_indices_valid):
raise ValueError(f"Expected {len(tri_indices_valid)} tri_aniso_ke values, got {len(tri_aniso_values)}")
tri_start = len(builder.tri_indices)
tri_areas = builder.add_triangles(
(tri_indices_valid[:, 0] + start_vertex).tolist(),
(tri_indices_valid[:, 1] + start_vertex).tolist(),
(tri_indices_valid[:, 2] + start_vertex).tolist(),
tri_ke=[builder.default_tri_ke] * len(tri_indices_valid),
tri_ka=[tri_ka if tri_ka is not None else builder.default_tri_ka] * len(tri_indices_valid),
tri_kd=[tri_kd if tri_kd is not None else builder.default_tri_kd] * len(tri_indices_valid),
tri_drag=[tri_drag if tri_drag is not None else builder.default_tri_drag] * len(tri_indices_valid),
tri_lift=[tri_lift if tri_lift is not None else builder.default_tri_lift] * len(tri_indices_valid),
custom_attributes={"style3d:tri_aniso_ke": tri_aniso_values},
)
panel_inv_D = panel_inv_D_all[valid_inds]
panel_areas = panel_areas_all[valid_inds]
tri_end = tri_start + len(tri_areas)
builder.tri_poses[tri_start:tri_end] = panel_inv_D.tolist()
builder.tri_areas[tri_start:tri_end] = panel_areas.tolist()
for t, area in enumerate(panel_areas.tolist()):
i, j, k = tri_indices_valid[t]
builder.particle_mass[start_vertex + i] += density * area / 3.0
builder.particle_mass[start_vertex + j] += density * area / 3.0
builder.particle_mass[start_vertex + k] += density * area / 3.0
edge_indices_local, edge_ke, edge_rest_area, edge_bending_cot, edge_aniso_values = _compute_edge_bending_data(
panel_verts_np,
panel_indices_valid,
tri_indices_valid,
edge_aniso_ke,
)
edge_indices_global = edge_indices_local.copy()
edge_indices_global[edge_indices_global >= 0] += start_vertex
if edge_ke is None:
edge_ke = [builder.default_edge_ke] * len(edge_indices_global)
edge_kd_value = edge_kd if edge_kd is not None else builder.default_edge_kd
edge_kd_list = [edge_kd_value] * len(edge_ke)
edge_custom_attrs = {
"style3d:edge_rest_area": edge_rest_area,
"style3d:edge_bending_cot": edge_bending_cot,
}
if edge_aniso_values is not None:
edge_custom_attrs["style3d:aniso_ke"] = edge_aniso_values
builder.add_edges(
edge_indices_global[:, 0].tolist(),
edge_indices_global[:, 1].tolist(),
edge_indices_global[:, 2].tolist(),
edge_indices_global[:, 3].tolist(),
edge_ke=edge_ke,
edge_kd=edge_kd_list,
custom_attributes=edge_custom_attrs,
)
if add_springs:
spring_indices = set()
for i, j, k, l in edge_indices_global:
spring_indices.add((min(k, l), max(k, l)))
if i != -1:
spring_indices.add((min(i, k), max(i, k)))
spring_indices.add((min(i, l), max(i, l)))
if j != -1:
spring_indices.add((min(j, k), max(j, k)))
spring_indices.add((min(j, l), max(j, l)))
if i != -1 and j != -1:
spring_indices.add((min(i, j), max(i, j)))
spring_ke_value = spring_ke if spring_ke is not None else builder.default_spring_ke
spring_kd_value = spring_kd if spring_kd is not None else builder.default_spring_kd
for i, j in spring_indices:
builder.add_spring(
i,
j,
spring_ke_value,
spring_kd_value,
control=0.0,
custom_attributes=custom_attributes_springs,
)
def add_cloth_grid(
builder: ModelBuilder,
*,
pos: Vec3,
rot: Any,
vel: Vec3,
dim_x: int,
dim_y: int,
cell_x: float,
cell_y: float,
mass: float,
reverse_winding: bool = False,
fix_left: bool = False,
fix_right: bool = False,
fix_top: bool = False,
fix_bottom: bool = False,
tri_aniso_ke: Sequence[Vec3] | Vec3 | None = None,
tri_ka: float | None = None,
tri_kd: float | None = None,
tri_drag: float | None = None,
tri_lift: float | None = None,
edge_aniso_ke: Sequence[Vec3] | Vec3 | None = None,
edge_kd: float | None = None,
add_springs: bool = False,
spring_ke: float | None = None,
spring_kd: float | None = None,
particle_radius: float | None = None,
custom_attributes_particles: dict[str, Any] | None = None,
custom_attributes_springs: dict[str, Any] | None = None,
) -> None:
"""Create a planar Style3D cloth grid.
Call :meth:`newton.solvers.SolverStyle3D.register_custom_attributes` before
invoking this helper so the Style3D custom attributes are available on the
builder. The grid uses ``mass`` per particle to compute panel density and
then delegates to :func:`newton.solvers.style3d.add_cloth_mesh`.
Args:
builder: :class:`newton.ModelBuilder` to populate.
pos: World-space translation for the grid.
rot: World-space rotation for the grid.
vel: Initial velocity for all particles.
dim_x: Number of grid cells along X (creates dim_x + 1 vertices).
dim_y: Number of grid cells along Y (creates dim_y + 1 vertices).
cell_x: Cell size along X in panel space.
cell_y: Cell size along Y in panel space.
mass: Mass per particle (used to compute density).
reverse_winding: If True, flip triangle winding.
fix_left: Fix particles on the left edge.
fix_right: Fix particles on the right edge.
fix_top: Fix particles on the top edge.
fix_bottom: Fix particles on the bottom edge.
tri_aniso_ke: Anisotropic stretch stiffness (weft, warp, shear).
tri_ka: Triangle area stiffness.
tri_kd: Triangle damping.
tri_drag: Triangle drag coefficient.
tri_lift: Triangle lift coefficient.
edge_aniso_ke: Anisotropic bending stiffness values.
edge_kd: Edge damping.
add_springs: If True, add structural springs across mesh edges.
spring_ke: Spring stiffness.
spring_kd: Spring damping.
particle_radius: Per-particle radius.
custom_attributes_particles: Extra custom attributes for particles.
custom_attributes_springs: Extra custom attributes for springs.
"""
def grid_index(x: int, y: int, dim_x: int) -> int:
return y * dim_x + x
indices: list[int] = []
vertices: list[Vec3] = []
panel_verts: list[Vec2] = []
for y in range(0, dim_y + 1):
for x in range(0, dim_x + 1):
local_pos = wp.vec3(x * cell_x, y * cell_y, 0.0)
vertices.append(local_pos)
panel_verts.append(wp.vec2(local_pos[0], local_pos[1]))
if x > 0 and y > 0:
v0 = grid_index(x - 1, y - 1, dim_x + 1)
v1 = grid_index(x, y - 1, dim_x + 1)
v2 = grid_index(x, y, dim_x + 1)
v3 = grid_index(x - 1, y, dim_x + 1)
if reverse_winding:
indices.extend([v0, v1, v2])
indices.extend([v0, v2, v3])
else:
indices.extend([v0, v1, v3])
indices.extend([v1, v2, v3])
total_mass = mass * (dim_x + 1) * (dim_y + 1)
total_area = cell_x * cell_y * dim_x * dim_y
density = total_mass / total_area
start_vertex = len(builder.particle_q)
add_cloth_mesh(
builder,
pos=pos,
rot=rot,
vel=vel,
vertices=vertices,
indices=indices,
density=density,
scale=1.0,
panel_verts=panel_verts,
panel_indices=indices,
tri_aniso_ke=tri_aniso_ke,
edge_aniso_ke=edge_aniso_ke,
tri_ka=tri_ka,
tri_kd=tri_kd,
tri_drag=tri_drag,
tri_lift=tri_lift,
edge_kd=edge_kd,
add_springs=add_springs,
spring_ke=spring_ke,
spring_kd=spring_kd,
particle_radius=particle_radius,
custom_attributes_particles=custom_attributes_particles,
custom_attributes_springs=custom_attributes_springs,
)
if fix_left or fix_right or fix_top or fix_bottom:
vertex_id = 0
for y in range(dim_y + 1):
for x in range(dim_x + 1):
if (
(x == 0 and fix_left)
or (x == dim_x and fix_right)
or (y == 0 and fix_bottom)
or (y == dim_y and fix_top)
):
builder.particle_flags[start_vertex + vertex_id] = (
builder.particle_flags[start_vertex + vertex_id] & ~ParticleFlags.ACTIVE
)
builder.particle_mass[start_vertex + vertex_id] = 0.0
vertex_id += 1
@wp.kernel
def compute_sew_v(
sew_dist: float,
bvh_id: wp.uint64,
pos: wp.array[wp.vec3],
edge_indices: wp.array2d[wp.int32],
vert_indices: wp.array[wp.int32],
# outputs
sew_vinds: wp.array2d[wp.vec2i],
sew_vdists: wp.array2d[wp.float32],
):
v_index = vert_indices[wp.tid()]
v = pos[v_index]
lower = wp.vec3(v[0] - sew_dist, v[1] - sew_dist, v[2] - sew_dist)
upper = wp.vec3(v[0] + sew_dist, v[1] + sew_dist, v[2] + sew_dist)
query = wp.bvh_query_aabb(bvh_id, lower, upper)
edge_index = wp.int32(-1)
vertex_num_sew = wp.int32(0)
max_num_sew = sew_vinds.shape[1]
while wp.bvh_query_next(query, edge_index):
va_ind = edge_indices[edge_index, 2]
vb_ind = edge_indices[edge_index, 3]
if v_index == va_ind or v_index == vb_ind:
continue
va = pos[va_ind]
vb = pos[vb_ind]
check_va = bool(True)
check_vb = bool(True)
for i in range(vertex_num_sew):
if sew_vinds[wp.tid()][i][1] == va_ind:
check_va = False
break
for i in range(vertex_num_sew):
if sew_vinds[wp.tid()][i][1] == vb_ind:
check_vb = False
break
if v_index < va_ind and check_va:
da = wp.length(va - v)
if da <= sew_dist:
if vertex_num_sew < max_num_sew:
sew_vinds[wp.tid()][vertex_num_sew][0] = v_index
sew_vinds[wp.tid()][vertex_num_sew][1] = va_ind
sew_vdists[wp.tid()][vertex_num_sew] = da
vertex_num_sew = vertex_num_sew + 1
else:
for i in range(max_num_sew):
if da < sew_vdists[wp.tid()][i]:
sew_vinds[wp.tid()][i][0] = v_index
sew_vinds[wp.tid()][i][1] = va_ind
sew_vdists[wp.tid()][i] = da
break
if v_index < vb_ind and check_vb:
db = wp.length(vb - v)
if db <= sew_dist:
if vertex_num_sew < max_num_sew:
sew_vinds[wp.tid()][vertex_num_sew][0] = v_index
sew_vinds[wp.tid()][vertex_num_sew][1] = vb_ind
sew_vdists[wp.tid()][vertex_num_sew] = db
vertex_num_sew = vertex_num_sew + 1
else:
for i in range(max_num_sew):
if db < sew_vdists[wp.tid()][i]:
sew_vinds[wp.tid()][i][0] = v_index
sew_vinds[wp.tid()][i][1] = vb_ind
sew_vdists[wp.tid()][i] = db
break
def create_mesh_sew_springs(
particle_q: np.ndarray,
edge_indices: np.ndarray,
sew_distance: float = 1.0e-3,
sew_interior: bool = False,
):
"""Compute sewing spring pairs for a mesh.
Vertices within ``sew_distance`` are connected by springs. When
``sew_interior`` is False, only boundary vertices are considered as
candidates for sewing.
Args:
particle_q: Particle positions.
edge_indices: Edge indices in :attr:`newton.Model.edge_indices` layout.
sew_distance: Maximum distance between vertices to sew.
sew_interior: If True, allow interior-interior sewing; otherwise only
boundary-interior or boundary-boundary vertices are sewn.
Returns:
Array of vertex index pairs to connect with springs.
"""
mesh_edge_indices = np.array(edge_indices)
# compute unique vert indices
flat_inds = mesh_edge_indices.flatten()
flat_inds = flat_inds[flat_inds >= 0]
vert_inds = np.unique(flat_inds)
# compute unique boundary vert indices
bound_condition = mesh_edge_indices[:, 1] < 0
bound_edge_inds = mesh_edge_indices[bound_condition]
bound_edge_inds = bound_edge_inds[:, 2:4]
bound_vert_inds = np.unique(bound_edge_inds.flatten())
# compute edge bvh
num_edge = mesh_edge_indices.shape[0]
lower_bounds_edges = wp.array(shape=(num_edge,), dtype=wp.vec3, device="cpu")
upper_bounds_edges = wp.array(shape=(num_edge,), dtype=wp.vec3, device="cpu")
wp_edge_indices = wp.array(edge_indices, dtype=wp.int32, device="cpu")
wp_vert_pos = wp.array(particle_q, dtype=wp.vec3, device="cpu")
wp.launch(
kernel=compute_edge_aabbs,
inputs=[wp_vert_pos, wp_edge_indices],
outputs=[lower_bounds_edges, upper_bounds_edges],
dim=num_edge,
device="cpu",
)
bvh_edges = wp.Bvh(lower_bounds_edges, upper_bounds_edges)
# compute sew springs
max_num_sew = 5
if sew_interior:
num_vert = vert_inds.shape[0]
wp_vert_inds = wp.array(vert_inds, dtype=wp.int32, device="cpu")
else:
num_vert = bound_vert_inds.shape[0]
wp_vert_inds = wp.array(bound_vert_inds, dtype=wp.int32, device="cpu")
wp_sew_vinds = wp.full(
shape=(num_vert, max_num_sew), value=wp.vec2i(-1, -1), dtype=wp.vec2i, device="cpu"
) # each vert sew max 5 other verts
wp_sew_vdists = wp.full(shape=(num_vert, max_num_sew), value=sew_distance, dtype=wp.float32, device="cpu")
wp.launch(
kernel=compute_sew_v,
inputs=[sew_distance, bvh_edges.id, wp_vert_pos, wp_edge_indices, wp_vert_inds],
outputs=[wp_sew_vinds, wp_sew_vdists],
dim=num_vert,
device="cpu",
)
np_sew_vinds = wp_sew_vinds.numpy().reshape(num_vert * max_num_sew, 2)
np_sew_vinds = np_sew_vinds[np_sew_vinds[:, 0] >= 0]
return np_sew_vinds
def sew_close_vertices(builder: ModelBuilder, sew_distance: float = 1.0e-3, sew_interior: bool = False) -> None:
"""Sew close vertices by creating springs between nearby mesh vertices.
Springs use :attr:`newton.ModelBuilder.default_spring_ke` and
:attr:`newton.ModelBuilder.default_spring_kd`.
Args:
builder: :class:`newton.ModelBuilder` with triangle/edge topology.
sew_distance: Vertices within this distance are connected by springs.
sew_interior: If True, allow interior-interior sewing; otherwise only
boundary-interior or boundary-boundary vertices are sewn.
"""
sew_springs = create_mesh_sew_springs(
builder.particle_q,
builder.edge_indices,
sew_distance,
sew_interior,
)
for spring in sew_springs:
builder.add_spring(
spring[0],
spring[1],
builder.default_spring_ke,
builder.default_spring_kd,
control=0.0,
)
__all__ = [
"add_cloth_grid",
"add_cloth_mesh",
"create_mesh_sew_springs",
"sew_close_vertices",
]
================================================
FILE: newton/_src/solvers/style3d/collision/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .collision import Collision
from .collision_legacy import CollisionHandler
__all__ = [
"Collision",
"CollisionHandler",
]
================================================
FILE: newton/_src/solvers/style3d/collision/bvh/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .bvh import BvhAabb, BvhEdge, BvhTri
__all__ = [
"BvhAabb",
"BvhEdge",
"BvhTri",
]
================================================
FILE: newton/_src/solvers/style3d/collision/bvh/bvh.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from newton._src.solvers.style3d.collision.bvh.kernels import (
aabb_vs_aabb_kernel,
aabb_vs_line_kernel,
compute_edge_aabbs_kernel,
compute_tri_aabbs_kernel,
edge_vs_edge_kernel,
triangle_vs_point_kernel,
)
########################################################################################################################
###################################################### Bvh #######################################################
########################################################################################################################
class BvhAabb:
"""A wrapper class for Warp's BVH (Bounding Volume Hierarchy) structure.
This class manages a BVH for efficient spatial queries such as AABB vs AABB
or AABB vs line segment intersections. It provides methods for building,
rebuilding, and refitting the hierarchy, as well as performing queries.
Query results are stored in a 2d-array for efficient read/write operations, where each
column corresponds to a query thread and each row represents a result slot (see below):
---------------------------------------------------------------------------------
| | thread_0 | thread_1 | thread_2 | thread_3 | ... | thread_m |
---------------------------------------------------------------------------------
| slot_0 | 2 | 0 | 1 | n - 1 | ... | 4 |
---------------------------------------------------------------------------------
| slot_1 | 522 | - | 333 | 10 | ... | 0 |
---------------------------------------------------------------------------------
| slot_2 | 1000 | - | - | 13 | ... | 1 |
---------------------------------------------------------------------------------
| ... | - | - | - | ... | ... | ... |
---------------------------------------------------------------------------------
| slot_n | - | - | - | 555 | ... | - |
---------------------------------------------------------------------------------
Notes:
- Row 0 stores the count of valid indices for each thread.
- Columns should be at least the number of query objects, ideally aligned to 32 for performance.
- Rows equal the maximum query count plus 1 (for the count row).
- Use the following pattern to iterate over results:
.. code-block:: python
for i in range(query_results[0, tid])
idx = query_results[i + 1, tid]
...
"""
def __init__(self, num_leaves: int, device: wp.Device):
self.bvh = None
self.device = device
self.lower_bounds = wp.zeros(num_leaves, dtype=wp.vec3, device=self.device)
self.upper_bounds = wp.zeros(num_leaves, dtype=wp.vec3, device=self.device)
def is_built(self) -> bool:
"""Returns True if the BVH has been built, otherwise False."""
return self.bvh is not None
def build(self):
"""Builds the BVH from the current lower and upper bounds.
This method allocates and constructs the BVH hierarchy from scratch
based on the provided leaf bounds. It must be called at least once
before using the BVH for queries.
Use this when:
- Initializing the BVH for the first time.
- The number of leaves has changed.
- The hierarchy structure must be recomputed.
Warning:
This function **must not** be called inside a `wp.ScopedCapture()` context,
since it performs out-of-place allocations.
"""
self.bvh = wp.Bvh(self.lower_bounds, self.upper_bounds)
def rebuild(self):
"""Rebuilds the BVH using the current lower and upper bounds.
Unlike `build()`, this does not reallocate or create a new BVH, but
instead updates the hierarchy using the existing BVH object. This is
more efficient than a full `build()`, but still recomputes the tree
topology.
Use this when:
- Leaf bounds have changed significantly.
- The BVH structure needs to be updated without a full rebuild.
Notes:
- Can be safely called inside a `wp.ScopedCapture()` context,
since all operations are performed in-place.
- Raises:
RuntimeError: If the BVH has not been built yet.
"""
if self.bvh is None:
raise RuntimeError("BVH hasn't been built yet!")
else:
self.bvh.rebuild()
def refit(self):
"""Refits the existing BVH to updated leaf bounds.
This is the most efficient update operation. It preserves the existing
BVH structure and only updates bounding volumes to fit the new leaf
positions.
Use this when:
- The number of leaves is unchanged.
- Only the positions of primitives have moved (e.g., rigid or deforming objects).
- You want the cheapest possible update.
Raises:
RuntimeError: If the BVH has not been built yet.
"""
if self.bvh is None:
raise RuntimeError("BVH hasn't been built yet!")
else:
self.bvh.refit()
def aabb_vs_aabb(
self,
lower_bounds: wp.array[wp.vec3],
upper_bounds: wp.array[wp.vec3],
query_results: wp.array2d[int],
query_radius: float = 0.0,
ignore_self_hits: bool = False,
):
"""Queries the BVH for overlapping AABBs.
For each query AABB defined by `lower_bounds[i]` and `upper_bounds[i]`, this method finds
all leaf nodes in the BVH that overlap with the query box (optionally expanded by `query_radius`).
Results are written to `query_results` in a row-major layout:
- Each column corresponds to a query thread.
- Row 0 stores the number of hits for that thread.
- Rows 1..N store the indices of the intersecting leaf nodes.
Args:
lower_bounds (wp.array): Array of lower corners of query AABBs.
upper_bounds (wp.array): Array of upper corners of query AABBs.
query_results (wp.array): 2D integer array for storing results [max_results + 1, num_queries].
query_radius (float, optional): Additional padding radius to apply to each query AABB.
ignore_self_hits (bool, optional): If True, suppresses self-intersections (e.g., for symmetric queries).
Note:
- query_results.shape[1] must be ≥ number of aabbs (i.e., lower_bounds.shape[0]).
- query_results.shape[0] must be ≥ max_results + 1 (for count row).
"""
# ================================ Runtime checks ================================
assert query_results.ndim == 2, "query_results must be a 2D array."
assert query_results.shape[0] >= 1, f"query_results must have at least 1 rows, got {query_results.shape[0]}."
assert lower_bounds.shape == upper_bounds.shape, "lower_bounds and upper_bounds must have the same shape."
assert self.bvh is not None, "BVH has not been built. Call build() or refit() first."
# ================================ Runtime checks ================================
wp.launch(
aabb_vs_aabb_kernel,
dim=lower_bounds.shape[0],
inputs=[self.bvh.id, query_results.shape[0], query_radius, ignore_self_hits, lower_bounds, upper_bounds],
outputs=[query_results],
device=self.device,
block_dim=64,
)
def aabb_vs_line(
self,
vertices: wp.array[wp.vec3],
edge_indices: wp.array2d[int],
query_results: wp.array2d[int],
ignore_self_hits: bool = False,
):
"""Queries the BVH for intersections between line segments and AABBs.
This function casts each line segment defined by `vertices[edge_indices[i, 2]]` to
`vertices[edge_indices[i, 3]]` against the BVH. For each segment, it collects all AABBs
(from the BVH leaves) that intersect with the segment.
Results are written to `query_results` in a row-major layout:
- Each column corresponds to a query thread.
- Row 0 stores the number of hits for that thread.
- Rows 1..N store the indices of the intersecting leaf nodes.
Args:
vertices (wp.array): Array of 3D points representing geometry vertices.
edge_indices (wp.array): (N, 2) array of vertex indices forming line segments.
query_results (wp.array): 2D int array of shape (max_results + 1, num_segments) for output.
ignore_self_hits (bool): Whether to ignore self-intersections (e.g., for symmetric geometry).
Note:
- query_results.shape[1] must be ≥ number of segments (i.e., edge_indices.shape[0]).
- query_results.shape[0] must be ≥ max_results + 1 (for count row).
"""
# ================================ Runtime checks ================================
assert query_results.ndim == 2, "query_results must be a 2D array."
assert query_results.shape[0] >= 1, f"query_results must have at least 1 rows, got {query_results.shape[0]}."
assert self.bvh is not None, "BVH has not been built. Call build() or refit() first."
# ================================ Runtime checks ================================
wp.launch(
aabb_vs_line_kernel,
dim=edge_indices.shape[0],
inputs=[
self.bvh.id,
query_results.shape[0],
ignore_self_hits,
vertices,
edge_indices,
self.lower_bounds,
self.upper_bounds,
],
outputs=[query_results],
device=self.device,
block_dim=64,
)
########################################################################################################################
#################################################### Edge Bvh ####################################################
########################################################################################################################
class BvhEdge(BvhAabb):
"""BVH structure specialized for edge primitives (line segments).
This class extends :class:`BvhAabb` with functionality to compute AABBs
from edges (pairs of vertices) and to perform edge-specific queries.
It supports building, refitting, and querying against edge-based BVHs.
"""
def __init__(self, edge_count: int, device: wp.Device):
super().__init__(edge_count, device)
def update_aabbs(self, pos: wp.array[wp.vec3], edge_indices: wp.array2d[int], enlarge: float):
"""Computes AABBs for all edges based on current vertex positions and edge indices.
Args:
pos (wp.array): Vertex position array (wp.vec3).
edge_indices (wp.array): Integer array of shape (M, 4). Columns 2 and 3
of each row contain indices into `pos` defining an edge.
enlarge (float): Optional margin to expand each bounding box
(useful for padding or motion blur).
"""
# ================================ Runtime checks ================================
assert edge_indices.shape[1] == 4, f"edge_indices must be of shape (M, 4), got {edge_indices.shape}"
assert edge_indices.shape[0] == self.lower_bounds.shape[0], "Mismatch between edge count and BVH leaf count."
# ================================ Runtime checks ================================
wp.launch(
compute_edge_aabbs_kernel,
dim=self.lower_bounds.shape[0],
inputs=[enlarge, pos, edge_indices],
outputs=[self.lower_bounds, self.upper_bounds],
device=self.device,
)
def build(self, pos: wp.array[wp.vec3], edge_indices: wp.array2d[int], enlarge: float = 0.0):
"""Builds the edge BVH from scratch using the given vertex positions and edge indices.
This computes the AABBs for all edges and then constructs a new BVH hierarchy.
Args:
pos (wp.array): Vertex positions (wp.vec3).
edge_indices (wp.array): Integer array of shape (M, 4). Columns 2 and 3
of each row contain the vertex indices defining an edge.
enlarge (float): Optional padding value to expand each edge's bounding box (default 0.0).
Warning:
This function **must not** be called inside a `wp.ScopedCapture()` context,
since it triggers allocations and memory movement.
"""
self.update_aabbs(pos, edge_indices, enlarge)
super().build()
def rebuild(self, pos: wp.array[wp.vec3], edge_indices: wp.array2d[int], enlarge: float = 0.0):
"""Rebuilds the edge BVH using the current vertex positions and edge indices.
This recomputes the edge AABBs and reconstructs the BVH hierarchy
from scratch (i.e., equivalent to `build()` but reuses the existing object).
Args:
pos (wp.array): Updated vertex positions (wp.vec3).
edge_indices (wp.array): Integer array of shape (M, 4). Columns 2 and 3
of each row contain the vertex indices defining an edge.
enlarge (float): Optional padding value to expand each edge's bounding box (default 0.0).
Notes:
- Unlike :func:`refit`, this recomputes the BVH topology, not just the bounds.
- May be significantly more expensive than `refit()` but more robust
when edge connectivity has changed or large movements occurred.
"""
self.update_aabbs(pos, edge_indices, enlarge)
super().rebuild()
def refit(self, pos: wp.array[wp.vec3], edge_indices: wp.array2d[int], enlarge: float = 0.0):
"""Refits the edge BVH after vertex positions have changed, without rebuilding the hierarchy.
This updates the leaf AABBs for all edges and adjusts the internal BVH bounds,
while preserving the existing hierarchy structure.
Args:
pos (wp.array): Updated vertex positions (wp.vec3).
edge_indices (wp.array): Integer array of shape (M, 4). Columns 2 and 3
of each row contain the vertex indices defining an edge.
enlarge (float): Optional padding value to expand each edge's bounding box (default 0.0).
Use this for dynamic geometry where connectivity stays the same but positions change.
"""
self.update_aabbs(pos, edge_indices, enlarge)
super().refit()
def edge_vs_edge(
self,
test_pos: wp.array[wp.vec3],
test_edge_indices: wp.array2d[int],
edge_pos: wp.array[wp.vec3],
edge_indices: wp.array2d[int],
query_results: wp.array2d[int],
ignore_self_hits: bool,
max_dist: float,
query_radius: float = 0.0,
):
"""Queries the BVH to find edges that are within a maximum distance from a set of points.
For each input edge (defined by `test_pos` and `test_edge_indices`), this function identifies edge
(defined by `edge_indices` and `edge_pos`) that fall within `max_dist` of the edge. It supports
optional self-hit suppression and radius-based padding for edge bounds.
Results are written to `query_results` in a row-major layout:
- Each column corresponds to a query thread.
- Row 0 stores the number of hits for that thread.
- Rows 1..N store the indices of the intersecting leaf nodes.
Args:
test_pos (wp.array): Query edge vertex positions (wp.vec3).
test_edge_indices (wp.array): Query edge indices (M x 4 int array).
edge_pos (wp.array): Edge vertex positions (same as used when building BVH).
edge_indices (wp.array): Edge indices (M x 4 int array).
query_results (wp.array): 2D int array to store the result layout (max_results + 1, P).
ignore_self_hits (bool): If True, skips hits between a point and its associated triangle (e.g. for self-collision).
max_dist (float): Maximum allowed distance between point and triangle for a match to be considered.
query_radius (float): Optional padding to enlarge triangle AABBs during the query (default: 0.0).
"""
wp.launch(
edge_vs_edge_kernel,
dim=test_edge_indices.shape[0],
inputs=[
self.bvh.id,
query_results.shape[0],
query_radius,
max_dist,
ignore_self_hits,
test_pos,
test_edge_indices,
edge_pos,
edge_indices,
],
outputs=[query_results],
device=self.device,
block_dim=64,
)
########################################################################################################################
################################################## Triangle Bvh ##################################################
########################################################################################################################
class BvhTri(BvhAabb):
"""BVH structure specialized for triangular face primitives.
This class extends :class:`BvhAabb` with functionality to compute AABBs
from triangle primitives and perform triangle-specific queries.
It supports building, rebuilding, refitting, and spatial queries
involving triangles.
"""
def __init__(self, tri_count: int, device: wp.Device):
super().__init__(tri_count, device)
def update_aabbs(self, pos: wp.array[wp.vec3], tri_indices: wp.array2d[int], enlarge: float):
"""Computes AABBs for all triangles based on current vertex positions and indices.
Args:
pos (wp.array): Vertex position array (wp.vec3).
tri_indices (wp.array): Integer array of shape (M, 3),
where each row contains vertex indices defining a triangle.
enlarge (float): Optional margin to expand each bounding box
(useful for padding or motion blur).
"""
# ================================ Runtime checks ================================
assert tri_indices.shape[1] == 3, f"tri_indices must be of shape (M, 3), got {tri_indices.shape}"
assert tri_indices.shape[0] == self.lower_bounds.shape[0], "Mismatch between triangle count and BVH leaf count."
# ================================ Runtime checks ================================
wp.launch(
compute_tri_aabbs_kernel,
dim=self.lower_bounds.shape[0],
inputs=[enlarge, pos, tri_indices],
outputs=[self.lower_bounds, self.upper_bounds],
device=self.device,
)
def build(self, pos: wp.array[wp.vec3], tri_indices: wp.array2d[int], enlarge: float = 0.0):
"""Builds the triangle BVH from scratch.
This computes AABBs for all triangles and constructs a new BVH hierarchy.
Args:
pos (wp.array): Vertex positions (wp.vec3).
tri_indices (wp.array): Integer array of shape (M, 3),
where each row defines a triangle.
enlarge (float): Optional padding value to expand each triangle's bounding box (default 0.0).
Warning:
This function **must not** be called inside a `wp.ScopedCapture()` context,
since it triggers allocations and memory movement.
"""
self.update_aabbs(pos, tri_indices, enlarge)
super().build()
def rebuild(self, pos: wp.array[wp.vec3], tri_indices: wp.array2d[int], enlarge: float = 0.0):
"""Rebuilds the triangle BVH using the current vertex positions and indices.
This recomputes the triangle AABBs and rebuilds the BVH hierarchy
in place (more expensive than `refit`, but more robust when triangles
move significantly or topology has changed).
Args:
pos (wp.array): Updated vertex positions (wp.vec3).
tri_indices (wp.array): Integer array of shape (M, 3),
where each row defines a triangle.
enlarge (float): Optional padding value to expand each triangle's bounding box (default 0.0).
Notes:
- Unlike :func:`refit`, this recomputes the BVH topology,
not just the bounding volumes.
- More efficient than a full :func:`build()` if the BVH already exists.
"""
self.update_aabbs(pos, tri_indices, enlarge)
super().rebuild()
def refit(self, pos: wp.array[wp.vec3], tri_indices: wp.array2d[int], enlarge: float = 0.0):
"""Refits the triangle BVH after vertex positions have changed, without rebuilding the hierarchy.
This updates AABBs for all triangles and propagates the changes up the hierarchy,
while preserving the existing BVH structure.
Args:
pos (wp.array): Updated vertex positions (wp.vec3).
tri_indices (wp.array): Integer array of shape (M, 3),
where each row defines a triangle.
enlarge (float): Optional bounding box padding for each triangle (default 0.0).
Use this for dynamic geometry where connectivity stays the same but positions change.
"""
self.update_aabbs(pos, tri_indices, enlarge)
super().refit()
def triangle_vs_point(
self,
pos: wp.array[wp.vec3],
tri_pos: wp.array[wp.vec3],
tri_indices: wp.array2d[int],
query_results: wp.array2d[int],
ignore_self_hits: bool,
max_dist: float,
query_radius: float = 0.0,
):
"""Queries the BVH to find triangles that are within a maximum distance from a set of points.
For each input point in `pos`, this function identifies triangles (defined by `tri_indices` and `tri_pos`)
that fall within `max_dist` of the point. It supports optional self-hit suppression and radius-based
padding for triangle bounds.
Results are written to `query_results` in a row-major layout:
- Each column corresponds to a query thread.
- Row 0 stores the number of hits for that thread.
- Rows 1..N store the indices of the intersecting leaf nodes.
Args:
pos (wp.array): Query point positions (wp.vec3).
tri_pos (wp.array): Triangle vertex positions (same as used when building BVH).
tri_indices (wp.array): Triangle indices (M x 3 int array).
query_results (wp.array): 2D int array to store the result layout (max_results + 1, P).
ignore_self_hits (bool): If True, skips hits between a point and its associated triangle (e.g. for self-collision).
max_dist (float): Maximum allowed distance between point and triangle for a match to be considered.
query_radius (float): Optional padding to enlarge triangle AABBs during the query (default: 0.0).
"""
# ================================ Runtime checks ================================
assert tri_indices.shape[1] == 3, f"tri_indices must be of shape (M, 3), got {tri_indices.shape}"
assert tri_indices.shape[0] == self.lower_bounds.shape[0], "Mismatch between triangle count and BVH leaf count."
# ================================ Runtime checks ================================
wp.launch(
triangle_vs_point_kernel,
dim=pos.shape[0],
inputs=[
self.bvh.id,
query_results.shape[0],
query_radius,
max_dist,
ignore_self_hits,
pos,
tri_pos,
tri_indices,
],
outputs=[query_results],
device=self.device,
block_dim=64,
)
########################################################################################################################
##################################################### Tests ######################################################
########################################################################################################################
if __name__ == "__main__":
wp.init()
# unit cube
verts = [
wp.vec3(0, 0, 0),
wp.vec3(1, 0, 0),
wp.vec3(1, 1, 0),
wp.vec3(0, 1, 0),
wp.vec3(0, 0, 1),
wp.vec3(1, 0, 1),
wp.vec3(1, 1, 1),
wp.vec3(0, 1, 1),
]
pos = wp.array(verts, dtype=wp.vec3)
edge_indices = wp.array([[0, 1, 2, 3]], dtype=int)
tri_indices = wp.array([[0, 1, 6], [3, 4, 5], [6, 7, 0]], dtype=int)
tri_bvh = BvhTri(3, wp.get_device())
edge_bvh = BvhEdge(1, wp.get_device())
tri_bvh.build(pos, tri_indices)
edge_bvh.build(pos, edge_indices)
tri_bvh.rebuild(pos, tri_indices)
edge_bvh.rebuild(pos, edge_indices)
print(f"tri_bvh.lower_bounds[0] = {tri_bvh.lower_bounds.numpy()[0]}")
print(f"tri_bvh.upper_bounds[0] = {tri_bvh.upper_bounds.numpy()[0]}")
print(f"edge_bvh.lower_bounds[0] = {edge_bvh.lower_bounds.numpy()[0]}")
print(f"edge_bvh.upper_bounds[0] = {edge_bvh.upper_bounds.numpy()[0]}")
test_vert = wp.array([wp.vec3(2, 0, 0.5), wp.vec3(0, 2, 0.5)], dtype=wp.vec3)
test_edge = wp.array([[0, 1, 0, 1]], dtype=int)
test_result = wp.array(shape=(2, 1), dtype=int)
tri_bvh.aabb_vs_line(test_vert, test_edge, test_result)
print(test_result)
test_vert = wp.array([wp.vec3(0.5, 0.5, 1.5)], dtype=wp.vec3)
tri_bvh.triangle_vs_point(test_vert, pos, tri_indices, test_result, False, 1, 1.0)
print(test_result)
================================================
FILE: newton/_src/solvers/style3d/collision/bvh/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from newton._src.geometry.kernels import (
triangle_closest_point,
vertex_adjacent_to_triangle,
)
@wp.func
def line_intersects_aabb(v0: wp.vec3, v1: wp.vec3, lower: wp.vec3, upper: wp.vec3):
# Slab method
dir = v1 - v0
tmin = 0.0
tmax = 1.0
for i in range(3):
if wp.abs(dir[i]) < 1.0e-8:
# Segment is parallel to slab. Reject if origin not within slab
if v0[i] < lower[i] or v0[i] > upper[i]:
return False
else:
invD = 1.0 / dir[i]
t1 = (lower[i] - v0[i]) * invD
t2 = (upper[i] - v0[i]) * invD
tmin = wp.max(tmin, wp.min(t1, t2))
tmax = wp.min(tmax, wp.max(t1, t2))
if tmax < tmin:
return False
return True
@wp.kernel
def compute_tri_aabbs_kernel(
enlarge: float,
pos: wp.array[wp.vec3],
tri_indices: wp.array2d[wp.int32],
# outputs
lower_bounds: wp.array[wp.vec3],
upper_bounds: wp.array[wp.vec3],
):
t_id = wp.tid()
v1 = pos[tri_indices[t_id, 0]]
v2 = pos[tri_indices[t_id, 1]]
v3 = pos[tri_indices[t_id, 2]]
lower = wp.min(wp.min(v1, v2), v3)
upper = wp.max(wp.max(v1, v2), v3)
lower_bounds[t_id] = lower - wp.vec3(enlarge)
upper_bounds[t_id] = upper + wp.vec3(enlarge)
@wp.kernel
def compute_edge_aabbs_kernel(
enlarge: float,
pos: wp.array[wp.vec3],
edge_indices: wp.array2d[wp.int32],
# outputs
lower_bounds: wp.array[wp.vec3],
upper_bounds: wp.array[wp.vec3],
):
e_id = wp.tid()
v1 = pos[edge_indices[e_id, 2]]
v2 = pos[edge_indices[e_id, 3]]
lower_bounds[e_id] = wp.min(v1, v2) - wp.vec3(enlarge)
upper_bounds[e_id] = wp.max(v1, v2) + wp.vec3(enlarge)
@wp.kernel
def aabb_vs_aabb_kernel(
bvh_id: wp.uint64,
query_list_rows: int,
query_radius: float,
ignore_self_hits: bool,
lower_bounds: wp.array[wp.vec3],
upper_bounds: wp.array[wp.vec3],
# outputs
query_results: wp.array2d[int],
):
tid = wp.int32(wp.tid())
lower = lower_bounds[tid] - wp.vec3(query_radius)
upper = upper_bounds[tid] + wp.vec3(query_radius)
query_count = wp.int32(0)
query_index = wp.int32(-1)
query = wp.bvh_query_aabb(bvh_id, lower, upper)
while (query_count < query_list_rows - 1) and wp.bvh_query_next(query, query_index):
if not (ignore_self_hits and query_index <= tid):
query_results[query_count + 1, tid] = query_index
query_count += 1
query_results[0, tid] = query_count
@wp.kernel
def aabb_vs_line_kernel(
bvh_id: wp.uint64,
query_list_rows: int,
ignore_self_hits: bool,
vertices: wp.array[wp.vec3],
edge_indices: wp.array2d[wp.int32],
lower_bounds: wp.array[wp.vec3],
upper_bounds: wp.array[wp.vec3],
# outputs
query_results: wp.array2d[int],
):
eid = wp.int32(wp.tid())
v1 = vertices[edge_indices[eid, 2]]
v2 = vertices[edge_indices[eid, 3]]
query_count = wp.int32(0)
query_index = wp.int32(-1)
query = wp.bvh_query_ray(bvh_id, v1, v2 - v1)
while (query_count < query_list_rows - 1) and wp.bvh_query_next(query, query_index):
if not (ignore_self_hits and query_index <= eid):
if line_intersects_aabb(v1, v2, lower_bounds[query_index], upper_bounds[query_index]):
query_results[query_count + 1, eid] = query_index
query_count += 1
query_results[0, eid] = query_count
@wp.kernel
def triangle_vs_point_kernel(
bvh_id: wp.uint64,
query_list_rows: int,
query_radius: float,
max_dist: float,
ignore_self_hits: bool,
pos: wp.array[wp.vec3],
tri_pos: wp.array[wp.vec3],
tri_indices: wp.array2d[int],
# outputs
query_results: wp.array2d[int],
):
vid = wp.tid()
x0 = pos[vid]
lower = x0 - wp.vec3(query_radius)
upper = x0 + wp.vec3(query_radius)
tri_index = wp.int32(-1)
query_count = wp.int32(0)
query = wp.bvh_query_aabb(bvh_id, lower, upper)
while (query_count < query_list_rows - 1) and wp.bvh_query_next(query, tri_index):
t1 = tri_indices[tri_index, 0]
t2 = tri_indices[tri_index, 1]
t3 = tri_indices[tri_index, 2]
if ignore_self_hits and vertex_adjacent_to_triangle(vid, t1, t2, t3):
continue
closest_p, _bary, _feature_type = triangle_closest_point(tri_pos[t1], tri_pos[t2], tri_pos[t3], x0)
dist = wp.length(closest_p - x0)
if dist < max_dist:
query_results[query_count + 1, vid] = tri_index
query_count += 1
query_results[0, vid] = query_count
@wp.kernel
def edge_vs_edge_kernel(
bvh_id: wp.uint64,
query_list_rows: int,
query_radius: float,
max_dist: float,
ignore_self_hits: bool,
test_pos: wp.array[wp.vec3],
test_edge_indices: wp.array2d[int],
edge_pos: wp.array[wp.vec3],
edge_indices: wp.array2d[int],
# outputs
query_results: wp.array2d[int],
):
eid = wp.int32(wp.tid())
v0 = test_edge_indices[eid, 2]
v1 = test_edge_indices[eid, 3]
x0 = test_pos[v0]
x1 = test_pos[v1]
lower = wp.min(x0, x1) - wp.vec3(query_radius)
upper = wp.max(x0, x1) + wp.vec3(query_radius)
edge_index = wp.int32(-1)
query_count = wp.int32(0)
query = wp.bvh_query_aabb(bvh_id, lower, upper)
while (query_count < query_list_rows - 1) and wp.bvh_query_next(query, edge_index):
if ignore_self_hits and edge_index <= eid:
continue
v2 = edge_indices[edge_index, 2]
v3 = edge_indices[edge_index, 3]
if ignore_self_hits and (v0 == v2 or v0 == v3 or v1 == v2 or v1 == v3):
continue
x2, x3 = edge_pos[v2], edge_pos[v3]
edge_edge_parallel_epsilon = wp.float32(1e-5)
st = wp.closest_point_edge_edge(x0, x1, x2, x3, edge_edge_parallel_epsilon)
s = st[0]
t = st[1]
c1 = wp.lerp(x0, x1, s)
c2 = wp.lerp(x2, x3, t)
dist = wp.length(c1 - c2)
if dist < max_dist:
query_results[query_count + 1, eid] = edge_index
query_count += 1
query_results[0, eid] = query_count
================================================
FILE: newton/_src/solvers/style3d/collision/collision.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from newton import Contacts, Model, State
from newton._src.solvers.style3d.collision.bvh import BvhEdge, BvhTri
from newton._src.solvers.style3d.collision.kernels import (
eval_body_contact_kernel,
handle_edge_edge_contacts_kernel,
handle_vertex_triangle_contacts_kernel,
solve_untangling_kernel,
)
########################################################################################################################
################################################### Collision ####################################################
########################################################################################################################
class Collision:
"""
Collision handler for cloth simulation.
"""
def __init__(self, model: Model):
"""
Initialize the collision handler, including BVHs and buffers.
Args:
model: The simulation model containing particle and geometry data.
"""
self.model = model
self.radius = 3e-3 # Contact radius
self.stiff_vf = 0.5 # Stiffness coefficient for vertex-face (VF) collision constraints
self.stiff_ee = 0.1 # Stiffness coefficient for edge-edge (EE) collision constraints
self.stiff_ef = 1.0 # Stiffness coefficient for edge-face (EF) collision constraints
self.friction_epsilon = 1e-2
self.integrate_with_external_rigid_solver = True
self.tri_bvh = BvhTri(model.tri_count, self.model.device)
self.edge_bvh = BvhEdge(model.edge_count, self.model.device)
self.body_contact_max = model.shape_count * model.particle_count
self.broad_phase_ee = wp.array(shape=(32, model.edge_count), dtype=int, device=self.model.device)
self.broad_phase_ef = wp.array(shape=(32, model.edge_count), dtype=int, device=self.model.device)
self.broad_phase_vf = wp.array(shape=(32, model.particle_count), dtype=int, device=self.model.device)
self.Hx = wp.zeros(model.particle_count, dtype=wp.vec3, device=self.model.device)
self.contact_hessian_diags = wp.zeros(model.particle_count, dtype=wp.mat33, device=self.model.device)
self.edge_bvh.build(model.particle_q, self.model.edge_indices, self.radius)
self.tri_bvh.build(model.particle_q, self.model.tri_indices, self.radius)
def rebuild_bvh(self, pos: wp.array[wp.vec3]):
"""
Rebuild triangle and edge BVHs.
Args:
pos: Array of vertex positions.
"""
self.tri_bvh.rebuild(pos, self.model.tri_indices, self.radius)
self.edge_bvh.rebuild(pos, self.model.edge_indices, self.radius)
def refit_bvh(self, pos: wp.array[wp.vec3]):
"""
Refit (update) triangle and edge BVHs based on new positions without changing topology.
Args:
pos: Array of vertex positions.
"""
self.tri_bvh.refit(pos, self.model.tri_indices, self.radius)
self.edge_bvh.refit(pos, self.model.edge_indices, self.radius)
def frame_begin(self, particle_q: wp.array[wp.vec3], particle_qd: wp.array[wp.vec3], dt: float):
"""
Perform broad-phase collision detection using BVHs.
Args:
particle_q: Array of vertex positions.
particle_qd: Array of vertex velocities.
dt: simulation time step.
"""
max_dist = self.radius * 3.0
query_radius = self.radius
self.refit_bvh(particle_q)
# Vertex-face collision candidates
if self.stiff_vf > 0.0:
self.tri_bvh.triangle_vs_point(
particle_q,
particle_q,
self.model.tri_indices,
self.broad_phase_vf,
True,
max_dist,
query_radius,
)
# Edge-edge collision candidates
if self.stiff_ee > 0.0:
self.edge_bvh.edge_vs_edge(
particle_q,
self.model.edge_indices,
particle_q,
self.model.edge_indices,
self.broad_phase_ee,
True,
max_dist,
query_radius,
)
# Face-edge collision candidates
if self.stiff_ef > 0.0:
self.tri_bvh.aabb_vs_aabb(
self.edge_bvh.lower_bounds,
self.edge_bvh.upper_bounds,
self.broad_phase_ef,
query_radius,
False,
)
def accumulate_contact_force(
self,
dt: float,
_iter: int,
state_in: State,
state_out: State,
contacts: Contacts,
particle_forces: wp.array[wp.vec3],
particle_q_prev: wp.array[wp.vec3],
particle_stiff: wp.array[wp.vec3] = None,
):
"""
Evaluates contact forces and the diagonal of the Hessian for implicit time integration.
This method launches kernels to compute contact forces and Hessian contributions
based on broad-phase collision candidates computed in frame_begin().
Args:
dt (float): Time step.
state_in (State): Current simulation state (input).
state_out (State): Next simulation state (output).
contacts (Contacts): Contact data structure containing contact information.
particle_forces (wp.array): Output array for computed contact forces.
particle_q_prev (wp.array): Previous positions (optional, for velocity-based damping).
particle_stiff (wp.array): Optional stiffness array for particles.
"""
thickness = 2.0 * self.radius
self.contact_hessian_diags.zero_()
if self.stiff_vf > 0:
wp.launch(
handle_vertex_triangle_contacts_kernel,
dim=len(state_in.particle_q),
inputs=[
thickness,
self.stiff_vf,
state_in.particle_q,
self.model.tri_indices,
self.broad_phase_vf,
particle_stiff,
],
outputs=[particle_forces, self.contact_hessian_diags],
device=self.model.device,
)
if self.stiff_ee > 0:
wp.launch(
handle_edge_edge_contacts_kernel,
dim=self.model.edge_indices.shape[0],
inputs=[
thickness,
self.stiff_ee,
state_in.particle_q,
self.model.edge_indices,
self.broad_phase_ee,
particle_stiff,
],
outputs=[particle_forces, self.contact_hessian_diags],
device=self.model.device,
)
if self.stiff_ef > 0:
wp.launch(
solve_untangling_kernel,
dim=self.model.edge_indices.shape[0],
inputs=[
thickness,
self.stiff_ef,
state_in.particle_q,
self.model.tri_indices,
self.model.edge_indices,
self.broad_phase_ef,
particle_stiff,
],
outputs=[particle_forces, self.contact_hessian_diags],
device=self.model.device,
)
wp.launch(
kernel=eval_body_contact_kernel,
dim=self.body_contact_max,
inputs=[
dt,
particle_q_prev,
state_in.particle_q,
# body-particle contact
self.model.soft_contact_ke,
self.model.soft_contact_kd,
self.model.soft_contact_mu,
self.friction_epsilon,
self.model.particle_radius,
contacts.soft_contact_particle,
contacts.soft_contact_count,
contacts.soft_contact_max,
self.model.shape_material_mu,
self.model.shape_body,
state_out.body_q if self.integrate_with_external_rigid_solver else state_in.body_q,
state_in.body_q if self.integrate_with_external_rigid_solver else None,
self.model.body_qd,
self.model.body_com,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_body_vel,
contacts.soft_contact_normal,
],
outputs=[particle_forces, self.contact_hessian_diags],
device=self.model.device,
)
def contact_hessian_diagonal(self):
"""Return diagonal of contact Hessian for preconditioning.
Note:
Should be called after `accumulate_contact_force()`.
"""
return self.contact_hessian_diags
def hessian_multiply(self, x: wp.array[wp.vec3]):
"""Computes the Hessian-vector product for implicit integration."""
@wp.kernel
def hessian_multiply_kernel(
hessian_diags: wp.array[wp.mat33],
x: wp.array[wp.vec3],
# outputs
Hx: wp.array[wp.vec3],
):
tid = wp.tid()
Hx[tid] = hessian_diags[tid] * x[tid]
wp.launch(
hessian_multiply_kernel,
dim=self.model.particle_count,
inputs=[self.contact_hessian_diags, x],
outputs=[self.Hx],
device=self.model.device,
)
return self.Hx
def linear_iteration_end(self, dx: wp.array[wp.vec3]):
"""Displacement constraints"""
pass
def frame_end(self, pos: wp.array[wp.vec3], vel: wp.array[wp.vec3], dt: float):
"""Apply post-processing"""
pass
================================================
FILE: newton/_src/solvers/style3d/collision/collision_legacy.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warnings
import warp as wp
from newton import Contacts, Model, State
from newton._src.solvers.vbd.particle_vbd_kernels import accumulate_contact_force_and_hessian
from newton._src.solvers.vbd.solver_vbd import NUM_THREADS_PER_COLLISION_PRIMITIVE
from newton._src.solvers.vbd.tri_mesh_collision import TriMeshCollisionDetector, TriMeshCollisionInfo
########################################################################################################################
################################################ Collision Handler ################################################
########################################################################################################################
@wp.kernel
def particle_conservative_bounds_kernel(
collision_query_radius: float,
conservative_bound_relaxation: float,
collision_info: TriMeshCollisionInfo,
# outputs
particle_dx: wp.array[wp.vec3],
):
"""
Ensures particle displacements remain within a conservative bound to prevent penetration.
Args:
collision_query_radius (float): Maximum allowed distance to check for collisions.
conservative_bound_relaxation (float): Relaxation factor for conservative displacement bound.
collision_info (TriMeshCollisionInfo): Collision information for all particles.
particle_dx (wp.array): Particle displacement vectors to be adjusted if necessary.
"""
particle_index = wp.tid()
# Compute the minimum distance between the query radius and the nearest collision triangle distance
min_dist = wp.min(collision_query_radius, collision_info.vertex_colliding_triangles_min_dist[particle_index])
# Calculate the conservative bound based on relaxation factor and minimum distance
conservative_bound = conservative_bound_relaxation * min_dist
# Current displacement of the particle
dx = particle_dx[particle_index]
# If displacement exceeds conservative bound, clamp it to avoid excessive movement
if wp.length(dx) > conservative_bound:
particle_dx[particle_index] = wp.normalize(dx) * conservative_bound
# wp.printf("conservative_bound = %f\n", conservative_bound)
@wp.kernel
def hessian_multiply_kernel(
hessian_diags: wp.array[wp.mat33],
x: wp.array[wp.vec3],
# outputs
Hx: wp.array[wp.vec3],
):
tid = wp.tid()
Hx[tid] = hessian_diags[tid] * x[tid]
########################################################################################################################
################################################ Collision Handler ################################################
########################################################################################################################
class CollisionHandler:
"""
Legacy collision handler for cloth simulation.
.. note::
This class is currently deprecated. Its functionality has been migrated
to the :class:`newton.solvers.style3d.Collision` class. The code is kept
temporarily for comparison and experimentation with new approaches.
"""
def __init__(
self,
model: Model,
friction_epsilon: float = 1e-2,
self_contact_radius: float = 2e-3,
self_contact_margin: float = 2e-3,
collision_detection_interval: int = 0,
edge_edge_parallel_epsilon: float = 1e-5,
edge_collision_buffer_pre_alloc: int = 64,
vertex_collision_buffer_pre_alloc: int = 32,
integrate_with_external_rigid_solver: bool = True,
penetration_free_conservative_bound_relaxation: float = 0.42,
):
"""
Initializes the collision handler.
Args:
model (Model): The simulation model containing particle and geometry data.
self_contact_radius: The radius used for self-contact detection. This is the distance at which vertex-triangle
pairs and edge-edge pairs will start to interact with each other.
self_contact_margin: The margin used for self-contact detection. This is the distance at which vertex-triangle
pairs and edge-edge will be considered in contact generation. It should be larger than `self_contact_radius`
to avoid missing contacts.
integrate_with_external_rigid_solver: an indicator of coupled rigid body - cloth simulation. When set to
`True`, the solver assumes the rigid body solve is handled externally.
penetration_free_conservative_bound_relaxation: Relaxation factor for conservative penetration-free projection.
friction_epsilon: Threshold to smooth small relative velocities in friction computation.
vertex_collision_buffer_pre_alloc: Preallocation size for each vertex's vertex-triangle collision buffer.
edge_collision_buffer_pre_alloc: Preallocation size for edge's edge-edge collision buffer.
edge_edge_parallel_epsilon: Threshold to detect near-parallel edges in edge-edge collision handling.
collision_detection_interval: Controls how frequently collision detection is applied during the simulation.
If set to a value < 0, collision detection is only performed once before the initialization step.
If set to 0, collision detection is applied twice: once before and once immediately after initialization.
If set to a value `k` >= 1, collision detection is applied before every `k` VBD iterations.
"""
warnings.warn(
"CollisionHandler is deprecated. Use `Collision` instead.", category=DeprecationWarning, stacklevel=2
)
if self_contact_margin < self_contact_radius:
raise ValueError(
"self_contact_margin is smaller than self_contact_radius, this will result in missing contacts and cause instability.\n"
"It is advisable to make self_contact_margin 1.5-2 times larger than self_contact_radius."
)
self.model = model
self.friction_epsilon = friction_epsilon
self.self_contact_margin = self_contact_margin
self.self_contact_radius = self_contact_radius
self.collision_detection_interval = collision_detection_interval
self.integrate_with_external_rigid_solver = integrate_with_external_rigid_solver
self.penetration_free_conservative_bound_relaxation = penetration_free_conservative_bound_relaxation
self.Hx = wp.zeros(model.particle_count, dtype=wp.vec3, device=self.model.device)
self.particle_colors = wp.zeros(model.particle_count, dtype=int, device=self.model.device)
self.contact_hessian_diags = wp.zeros(model.particle_count, dtype=wp.mat33, device=self.model.device)
self.trimesh_collision_detector = TriMeshCollisionDetector(
model,
vertex_collision_buffer_pre_alloc=vertex_collision_buffer_pre_alloc,
edge_collision_buffer_pre_alloc=edge_collision_buffer_pre_alloc,
edge_edge_parallel_epsilon=edge_edge_parallel_epsilon,
)
self.trimesh_collision_info = wp.array(
[self.trimesh_collision_detector.collision_info], dtype=TriMeshCollisionInfo, device=self.model.device
)
self.collision_evaluation_kernel_launch_size = max(
self.model.particle_count * NUM_THREADS_PER_COLLISION_PRIMITIVE,
self.model.edge_count * NUM_THREADS_PER_COLLISION_PRIMITIVE,
model.shape_count * model.particle_count,
)
def rebuild_bvh(self, pos: wp.array[wp.vec3]):
"""Rebuilds the BVH for collision detection using updated particle positions."""
self.trimesh_collision_detector.rebuild(pos)
def frame_begin(self, particle_q: wp.array[wp.vec3], vel: wp.array[wp.vec3], dt: float):
"""Initializes collision detection for a new simulation frame."""
pass
def accumulate_contact_force(
self,
dt: float,
_iter: int,
state_in: State,
state_out: State,
contacts: Contacts,
particle_forces: wp.array[wp.vec3],
particle_q_prev: wp.array[wp.vec3] = None,
particle_stiff: wp.array[wp.vec3] = None,
):
"""
Evaluates contact forces and the diagonal of the Hessian for implicit time integration.
Steps:
1. Refits BVH based on current positions.
2. Detects edge-edge and vertex-triangle collisions.
3. Launches kernel to accumulate forces and Hessians for all particles.
Args:
dt (float): Time step.
state_in (State): Current simulation state (input).
state_out (State): Next simulation state (output).
contacts (Contacts): Contact data structure containing contact information.
particle_forces (wp.array): Output array for computed contact forces.
particle_q_prev (wp.array): Previous positions (optional, for velocity-based damping).
particle_stiff (wp.array): Optional stiffness array for particles.
"""
if (self.collision_detection_interval == 0 and _iter == 0) or (
self.collision_detection_interval >= 1 and _iter % self.collision_detection_interval == 0
):
self.trimesh_collision_detector.refit(state_in.particle_q)
self.trimesh_collision_detector.edge_edge_collision_detection(self.self_contact_margin)
self.trimesh_collision_detector.vertex_triangle_collision_detection(self.self_contact_margin)
curr_color = 0
self.contact_hessian_diags.zero_()
wp.launch(
kernel=accumulate_contact_force_and_hessian,
dim=self.collision_evaluation_kernel_launch_size,
inputs=[
dt,
curr_color,
particle_q_prev,
state_in.particle_q,
self.particle_colors,
self.model.tri_indices,
self.model.edge_indices,
# self-contact
self.trimesh_collision_info,
self.self_contact_radius,
self.model.soft_contact_ke,
self.model.soft_contact_kd,
self.model.soft_contact_mu,
self.friction_epsilon,
self.trimesh_collision_detector.edge_edge_parallel_epsilon,
# body-particle contact
self.model.particle_radius,
contacts.soft_contact_particle,
contacts.soft_contact_count,
contacts.soft_contact_max,
self.model.shape_material_mu,
self.model.shape_body,
state_out.body_q if self.integrate_with_external_rigid_solver else state_in.body_q,
state_in.body_q if self.integrate_with_external_rigid_solver else None,
self.model.body_qd,
self.model.body_com,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_body_vel,
contacts.soft_contact_normal,
],
outputs=[particle_forces, self.contact_hessian_diags],
device=self.model.device,
max_blocks=self.model.device.sm_count,
)
def contact_hessian_diagonal(self):
"""Return diagonal of contact Hessian for preconditioning.
Note:
Should be called after `eval_contact_force_hessian()`.
"""
return self.contact_hessian_diags
def hessian_multiply(self, x: wp.array[wp.vec3]):
"""Computes the Hessian-vector product for implicit integration."""
wp.launch(
hessian_multiply_kernel,
dim=self.model.particle_count,
inputs=[self.contact_hessian_diags, x],
outputs=[self.Hx],
device=self.model.device,
)
return self.Hx
def linear_iteration_end(self, dx: wp.array[wp.vec3]):
"""
Enforces conservative displacement bounds after each solver iteration to maintain stability
and prevent excessive motion leading to penetration.
Args:
dx (wp.array): Current displacement for each particle, which may be modified.
"""
wp.launch(
particle_conservative_bounds_kernel,
dim=self.model.particle_count,
inputs=[
self.self_contact_margin,
self.penetration_free_conservative_bound_relaxation,
self.trimesh_collision_detector.collision_info,
],
outputs=[dx],
device=self.model.device,
)
def frame_end(self, pos: wp.array[wp.vec3], vel: wp.array[wp.vec3], dt: float):
"""Applies final collision response and velocity damping."""
pass
================================================
FILE: newton/_src/solvers/style3d/collision/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from newton._src.solvers.vbd.rigid_vbd_kernels import evaluate_body_particle_contact
@wp.func
def triangle_normal(A: wp.vec3, B: wp.vec3, C: wp.vec3):
n = wp.cross(B - A, C - A)
ln = wp.length(n)
return wp.vec3(0.0) if ln < 1.0e-12 else (n / ln)
@wp.func
def triangle_barycentric(A: wp.vec3, B: wp.vec3, C: wp.vec3, P: wp.vec3):
v0 = A - C
v1 = B - C
v2 = P - C
dot00 = wp.dot(v0, v0)
dot01 = wp.dot(v0, v1)
dot02 = wp.dot(v0, v2)
dot11 = wp.dot(v1, v1)
dot12 = wp.dot(v1, v2)
denom = dot00 * dot11 - dot01 * dot01
invDenom = 0.0 if wp.abs(denom) < 1.0e-12 else 1.0 / denom
u = (dot11 * dot02 - dot01 * dot12) * invDenom
v = (dot00 * dot12 - dot01 * dot02) * invDenom
return wp.vec3(u, v, 1.0 - u - v)
@wp.kernel
def eval_body_contact_kernel(
# inputs
dt: float,
pos_prev: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
# body-particle contact
soft_contact_ke: float,
soft_contact_kd: float,
friction_mu: float,
friction_epsilon: float,
particle_radius: wp.array[float],
soft_contact_particle: wp.array[int],
contact_count: wp.array[int],
contact_max: int,
shape_material_mu: wp.array[float],
shape_body: wp.array[int],
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
contact_shape: wp.array[int],
contact_body_pos: wp.array[wp.vec3],
contact_body_vel: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
# outputs: particle force and hessian
forces: wp.array[wp.vec3],
hessians: wp.array[wp.mat33],
):
t_id = wp.tid()
particle_body_contact_count = wp.min(contact_max, contact_count[0])
if t_id < particle_body_contact_count:
particle_idx = soft_contact_particle[t_id]
body_contact_force, body_contact_hessian = evaluate_body_particle_contact(
particle_idx,
pos[particle_idx],
pos_prev[particle_idx],
t_id,
soft_contact_ke,
soft_contact_kd,
friction_mu,
friction_epsilon,
particle_radius,
shape_material_mu,
shape_body,
body_q,
body_q_prev,
body_qd,
body_com,
contact_shape,
contact_body_pos,
contact_body_vel,
contact_normal,
dt,
)
wp.atomic_add(forces, particle_idx, body_contact_force)
wp.atomic_add(hessians, particle_idx, body_contact_hessian)
@wp.kernel
def handle_vertex_triangle_contacts_kernel(
thickness: float,
stiff_factor: float,
pos: wp.array[wp.vec3],
tri_indices: wp.array2d[int],
broad_phase_vf: wp.array2d[int],
static_diags: wp.array[float],
# outputs
forces: wp.array[wp.vec3],
hessian_diags: wp.array[wp.mat33],
):
vid = wp.tid()
x0 = pos[vid]
force0 = wp.vec3(0.0)
hess0 = wp.identity(n=3, dtype=float) * 0.0
vert_stiff = static_diags[vid]
is_collided = wp.int32(0)
count = broad_phase_vf[0, vid]
for i in range(count):
fid = broad_phase_vf[i + 1, vid]
face = wp.vec3i(tri_indices[fid, 0], tri_indices[fid, 1], tri_indices[fid, 2])
x1 = pos[face[0]]
x2 = pos[face[1]]
x3 = pos[face[2]]
tri_normal = triangle_normal(x1, x2, x3)
dist = wp.dot(x0 - x1, tri_normal)
p = x0 - tri_normal * dist
bary_coord = triangle_barycentric(x1, x2, x3, p)
if wp.abs(dist) > thickness:
continue
if bary_coord[0] < 0.0 or bary_coord[1] < 0.0 or bary_coord[2] < 0.0:
continue # is outside triangle
face_stiff = (static_diags[face[0]] + static_diags[face[1]] + static_diags[face[2]]) / 3.0
stiff = stiff_factor * (vert_stiff * face_stiff) / (vert_stiff + face_stiff)
force = stiff * tri_normal * (thickness - wp.abs(dist)) * wp.sign(dist)
hess = stiff * wp.outer(tri_normal, tri_normal)
force0 += force
wp.atomic_add(forces, face[0], -force * bary_coord[0])
wp.atomic_add(forces, face[1], -force * bary_coord[1])
wp.atomic_add(forces, face[2], -force * bary_coord[2])
hess0 += hess
wp.atomic_add(hessian_diags, face[0], hess * bary_coord[0] * bary_coord[0])
wp.atomic_add(hessian_diags, face[1], hess * bary_coord[1] * bary_coord[1])
wp.atomic_add(hessian_diags, face[2], hess * bary_coord[2] * bary_coord[2])
is_collided = 1
if is_collided != 0:
wp.atomic_add(forces, vid, force0)
wp.atomic_add(hessian_diags, vid, hess0)
@wp.kernel
def handle_edge_edge_contacts_kernel(
thickness: float,
stiff_factor: float,
pos: wp.array[wp.vec3],
edge_indices: wp.array2d[int],
broad_phase_ee: wp.array2d[int],
static_diags: wp.array[float],
# outputs
forces: wp.array[wp.vec3],
hessian_diags: wp.array[wp.mat33],
):
eid = wp.tid()
edge0 = wp.vec4i(edge_indices[eid, 2], edge_indices[eid, 3], edge_indices[eid, 0], edge_indices[eid, 1])
x0 = pos[edge0[0]]
x1 = pos[edge0[1]]
len0 = wp.length(x0 - x1)
force0 = wp.vec3(0.0)
force1 = wp.vec3(0.0)
hess0 = wp.identity(n=3, dtype=float) * 0.0
hess1 = wp.identity(n=3, dtype=float) * 0.0
stiff_0 = (static_diags[edge0[0]] + static_diags[edge0[1]]) / 2.0
is_collided = wp.int32(0)
count = broad_phase_ee[0, eid]
for i in range(count):
idx = broad_phase_ee[i + 1, eid]
edge1 = wp.vec4i(edge_indices[idx, 2], edge_indices[idx, 3], edge_indices[idx, 0], edge_indices[idx, 1])
x2, x3 = pos[edge1[0]], pos[edge1[1]]
edge_edge_parallel_epsilon = wp.float32(1e-5)
st = wp.closest_point_edge_edge(x0, x1, x2, x3, edge_edge_parallel_epsilon)
s, t = st[0], st[1]
if (s <= 0) or (s >= 1) or (t <= 0) or (t >= 1):
continue
c1 = wp.lerp(x0, x1, s)
c2 = wp.lerp(x2, x3, t)
dir = c1 - c2
dist = wp.length(dir)
limited_thickness = thickness
len1 = wp.length(x2 - x3)
avg_len = (len0 + len1) * 0.5
if edge0[2] == edge1[0] or edge0[3] == edge1[0]:
limited_thickness = wp.min(limited_thickness, avg_len * 0.5)
elif edge0[2] == edge1[1] or edge0[3] == edge1[1]:
limited_thickness = wp.min(limited_thickness, avg_len * 0.5)
if edge1[2] == edge0[0] or edge1[3] == edge0[0]:
limited_thickness = wp.min(limited_thickness, avg_len * 0.5)
elif edge1[2] == edge0[1] or edge1[3] == edge0[1]:
limited_thickness = wp.min(limited_thickness, avg_len * 0.5)
if 1e-6 < dist < limited_thickness:
stiff_1 = (static_diags[edge1[0]] + static_diags[edge1[1]]) / 2.0
stiff = stiff_factor * (stiff_0 * stiff_1) / (stiff_0 + stiff_1)
dir = wp.normalize(dir)
force = stiff * dir * (limited_thickness - dist)
hess = stiff * wp.outer(dir, dir)
force0 += force * (1.0 - s)
force1 += force * s
wp.atomic_add(forces, edge1[0], -force * (1.0 - t))
wp.atomic_add(forces, edge1[1], -force * t)
hess0 += hess * (1.0 - s) * (1.0 - s)
hess1 += hess * s * s
wp.atomic_add(hessian_diags, edge1[0], hess * (1.0 - t) * (1.0 - t))
wp.atomic_add(hessian_diags, edge1[1], hess * t * t)
is_collided = 1
if is_collided != 0:
wp.atomic_add(forces, edge0[0], force0)
wp.atomic_add(forces, edge0[1], force1)
wp.atomic_add(hessian_diags, edge0[0], hess0)
wp.atomic_add(hessian_diags, edge0[1], hess1)
@wp.func
def intersection_gradient_vector(R: wp.vec3, E: wp.vec3, N: wp.vec3):
"""
Reference: Resolving Surface Collisions through Intersection Contour Minimization, Pascal Volino & Magnenat-Thalmann, 2006.
Args:
R: The direction of the intersection segment
E: Direction vector of the edge
N: The normals of the polygons
"""
dot_EN = wp.dot(E, N)
if wp.abs(dot_EN) > 1e-6:
return R - 2.0 * N * wp.dot(E, R) / dot_EN
else:
return R
@wp.kernel
def solve_untangling_kernel(
thickness: float,
stiff_factor: float,
pos: wp.array[wp.vec3],
tri_indices: wp.array2d[int],
edge_indices: wp.array2d[int],
broad_phase_ef: wp.array2d[int],
static_diags: wp.array[float],
# outputs
forces: wp.array[wp.vec3],
hessian_diags: wp.array[wp.mat33],
):
eid = wp.tid()
edge = wp.vec4i(edge_indices[eid, 2], edge_indices[eid, 3], edge_indices[eid, 0], edge_indices[eid, 1])
v0 = pos[edge[0]]
v1 = pos[edge[1]]
# Skip invalid edge
len0 = wp.length(v0 - v1)
if len0 < 5e-4:
return
force0 = wp.vec3(0.0)
force1 = wp.vec3(0.0)
hess0 = wp.identity(n=3, dtype=float) * 0.0
hess1 = wp.identity(n=3, dtype=float) * 0.0
stiff_0 = (static_diags[edge[0]] + static_diags[edge[1]]) / 2.0
is_collided = wp.int32(0)
# Edge direction
E = wp.normalize(v0 - v1)
N2 = wp.vec3(0.0) if edge[2] < 0 else triangle_normal(v0, v1, pos[edge[2]])
N3 = wp.vec3(0.0) if edge[3] < 0 else triangle_normal(v0, v1, pos[edge[3]])
count = broad_phase_ef[0, eid]
for i in range(count):
fid = broad_phase_ef[i + 1, eid]
face = wp.vec3i(tri_indices[fid, 0], tri_indices[fid, 1], tri_indices[fid, 2])
if face[0] == edge[0] or face[0] == edge[1]:
continue
if face[1] == edge[0] or face[1] == edge[1]:
continue
if face[2] == edge[0] or face[2] == edge[1]:
continue
x0 = pos[face[0]]
x1 = pos[face[1]]
x2 = pos[face[2]]
face_normal = wp.cross(x1 - x0, x2 - x1)
normal_len = wp.length(face_normal)
if normal_len < 1e-8:
continue # invalid triangle
face_normal = wp.normalize(face_normal)
d1 = wp.dot(face_normal, v0 - x0)
d2 = wp.dot(face_normal, v1 - x0)
if d1 * d2 >= 0.0:
continue # on same side
d1, d2 = wp.abs(d1), wp.abs(d2)
hit_point = (v0 * d2 + v1 * d1) / (d2 + d1)
bary_coord = triangle_barycentric(x0, x1, x2, hit_point)
if (bary_coord[0] < 1e-2) or (bary_coord[1] < 1e-2) or (bary_coord[2] < 1e-2):
continue # hit outside
G = wp.vec3(0.0)
if edge[2] >= 0:
R = wp.cross(face_normal, N2)
R = wp.vec3(0.0) if wp.length(R) < 1e-6 else wp.normalize(R)
if wp.dot(wp.cross(E, R), wp.cross(E, pos[edge[2]] - hit_point)) < 0.0:
R *= -1.0
G += intersection_gradient_vector(R, E, face_normal)
if edge[3] >= 0:
R = wp.cross(face_normal, N3)
R = wp.vec3(0.0) if wp.length(R) < 1e-6 else wp.normalize(R)
if wp.dot(wp.cross(E, R), wp.cross(E, pos[edge[3]] - hit_point)) < 0.0:
R *= -1.0
G += intersection_gradient_vector(R, E, face_normal)
if wp.length(G) < 1.0e-12:
continue
G = wp.normalize(G)
# Can be precomputed
stiff_1 = (static_diags[face[0]] + static_diags[face[1]] + static_diags[face[2]]) / 3.0
stiff = stiff_factor * (stiff_0 * stiff_1) / (stiff_0 + stiff_1)
disp = 2.0 * thickness
force = stiff * G * disp
hess = stiff * wp.outer(G, G)
edge_bary = wp.vec2(d2, d1) / (d1 + d2)
force0 += force * edge_bary[0]
force1 += force * edge_bary[1]
hess0 += hess * edge_bary[0] * edge_bary[0]
hess1 += hess * edge_bary[1] * edge_bary[1]
wp.atomic_add(forces, face[0], -force * bary_coord[0])
wp.atomic_add(forces, face[1], -force * bary_coord[1])
wp.atomic_add(forces, face[2], -force * bary_coord[2])
wp.atomic_add(hessian_diags, face[0], hess * bary_coord[0] * bary_coord[0])
wp.atomic_add(hessian_diags, face[1], hess * bary_coord[1] * bary_coord[1])
wp.atomic_add(hessian_diags, face[2], hess * bary_coord[2] * bary_coord[2])
is_collided = 1
if is_collided != 0:
wp.atomic_add(forces, edge[0], force0)
wp.atomic_add(forces, edge[1], force1)
wp.atomic_add(hessian_diags, edge[0], hess0)
wp.atomic_add(hessian_diags, edge[1], hess1)
================================================
FILE: newton/_src/solvers/style3d/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...geometry import ParticleFlags
@wp.func
def triangle_deformation_gradient(x0: wp.vec3, x1: wp.vec3, x2: wp.vec3, inv_dm: wp.mat22):
x01, x02 = x1 - x0, x2 - x0
Fu = x01 * inv_dm[0, 0] + x02 * inv_dm[1, 0]
Fv = x01 * inv_dm[0, 1] + x02 * inv_dm[1, 1]
return Fu, Fv
@wp.kernel
def eval_stretch_kernel(
pos: wp.array[wp.vec3],
face_areas: wp.array[float],
inv_dms: wp.array[wp.mat22],
faces: wp.array2d[wp.int32],
aniso_ke: wp.array[wp.vec3],
# outputs
forces: wp.array[wp.vec3],
):
"""
Ref. Large Steps in Cloth Simulation, Baraff & Witkin in 1998.
"""
fid = wp.tid()
inv_dm = inv_dms[fid]
face_area = face_areas[fid]
face = wp.vec3i(faces[fid, 0], faces[fid, 1], faces[fid, 2])
Fu, Fv = triangle_deformation_gradient(pos[face[0]], pos[face[1]], pos[face[2]], inv_dm)
len_Fu = wp.length(Fu)
len_Fv = wp.length(Fv)
Fu = wp.normalize(Fu) if (len_Fu > 1e-6) else wp.vec3(0.0)
Fv = wp.normalize(Fv) if (len_Fv > 1e-6) else wp.vec3(0.0)
dFu_dx = wp.vec3(-inv_dm[0, 0] - inv_dm[1, 0], inv_dm[0, 0], inv_dm[1, 0])
dFv_dx = wp.vec3(-inv_dm[0, 1] - inv_dm[1, 1], inv_dm[0, 1], inv_dm[1, 1])
ku = aniso_ke[fid][0]
kv = aniso_ke[fid][1]
ks = aniso_ke[fid][2]
for i in range(3):
force = -face_area * (
ku * (len_Fu - 1.0) * dFu_dx[i] * Fu
+ kv * (len_Fv - 1.0) * dFv_dx[i] * Fv
+ ks * wp.dot(Fu, Fv) * (Fu * dFv_dx[i] + Fv * dFu_dx[i])
)
wp.atomic_add(forces, face[i], force)
@wp.kernel
def eval_bend_kernel(
pos: wp.array[wp.vec3],
edge_rest_area: wp.array[float],
edge_bending_cot: wp.array[wp.vec4],
edges: wp.array2d[wp.int32],
edge_bending_properties: wp.array2d[float],
# outputs
forces: wp.array[wp.vec3],
):
eid = wp.tid()
if edges[eid][0] < 0 or edges[eid][1] < 0:
return
edge = edges[eid]
edge_stiff = edge_bending_properties[eid][0] / edge_rest_area[eid]
bend_weight = wp.vec4(0.0)
bend_weight[2] = edge_bending_cot[eid][2] + edge_bending_cot[eid][3]
bend_weight[3] = edge_bending_cot[eid][0] + edge_bending_cot[eid][1]
bend_weight[0] = -edge_bending_cot[eid][0] - edge_bending_cot[eid][2]
bend_weight[1] = -edge_bending_cot[eid][1] - edge_bending_cot[eid][3]
bend_weight = bend_weight * edge_stiff
for i in range(4):
force = wp.vec3(0.0)
for j in range(4):
force = force - bend_weight[i] * bend_weight[j] * pos[edge[j]]
wp.atomic_add(forces, edge[i], force)
@wp.kernel
def eval_drag_force_kernel(
spring_stiff: float,
face_index: wp.array[int],
drag_pos: wp.array[wp.vec3],
drag_bary_coord: wp.array[wp.vec3],
faces: wp.array2d[wp.int32],
vert_pos: wp.array[wp.vec3],
# outputs
forces: wp.array[wp.vec3],
):
fid = face_index[0]
if fid != -1:
coord = drag_bary_coord[0]
face = wp.vec3i(faces[fid, 0], faces[fid, 1], faces[fid, 2])
x0 = vert_pos[face[0]]
x1 = vert_pos[face[1]]
x2 = vert_pos[face[2]]
p = x0 * coord[0] + x1 * coord[1] + x2 * coord[2]
dir = drag_pos[0] - p
# add force
force = spring_stiff * dir
wp.atomic_add(forces, face[0], force * coord[0])
wp.atomic_add(forces, face[1], force * coord[1])
wp.atomic_add(forces, face[2], force * coord[2])
# add hessian
# dir = wp.normalize(dir)
# hessian = wp.outer(dir, dir) * spring_stiff
# hessian_diags[face[0]] += hessian * coord[0]
# hessian_diags[face[1]] += hessian * coord[1]
# hessian_diags[face[2]] += hessian * coord[2]
@wp.kernel
def accumulate_dragging_pd_diag_kernel(
spring_stiff: float,
face_index: wp.array[int],
drag_bary_coord: wp.array[wp.vec3],
faces: wp.array2d[wp.int32],
particle_flags: wp.array[wp.int32],
# outputs
pd_diags: wp.array[float],
):
fid = face_index[0]
if fid != -1:
coord = drag_bary_coord[0]
face = wp.vec3i(faces[fid, 0], faces[fid, 1], faces[fid, 2])
if particle_flags[face[0]] & ParticleFlags.ACTIVE:
pd_diags[face[0]] += spring_stiff * coord[0]
if particle_flags[face[1]] & ParticleFlags.ACTIVE:
pd_diags[face[1]] += spring_stiff * coord[1]
if particle_flags[face[2]] & ParticleFlags.ACTIVE:
pd_diags[face[2]] += spring_stiff * coord[2]
@wp.kernel
def init_step_kernel(
dt: float,
gravity: wp.array[wp.vec3],
particle_world: wp.array[wp.int32],
f_ext: wp.array[wp.vec3],
v_curr: wp.array[wp.vec3],
x_curr: wp.array[wp.vec3],
x_prev: wp.array[wp.vec3],
pd_diags: wp.array[float],
particle_masses: wp.array[float],
particle_flags: wp.array[wp.int32],
# outputs
x_inertia: wp.array[wp.vec3],
static_A_diags: wp.array[float],
dx: wp.array[wp.vec3],
):
tid = wp.tid()
x_last = x_curr[tid]
x_prev[tid] = x_last
if not particle_flags[tid] & ParticleFlags.ACTIVE:
x_inertia[tid] = x_prev[tid]
static_A_diags[tid] = 0.0
dx[tid] = wp.vec3(0.0)
else:
v_prev = v_curr[tid]
mass = particle_masses[tid]
static_A_diags[tid] = pd_diags[tid] + mass / (dt * dt)
world_idx = particle_world[tid]
world_g = gravity[wp.max(world_idx, 0)]
x_inertia[tid] = x_last + v_prev * dt + (world_g + f_ext[tid] / mass) * (dt * dt)
dx[tid] = v_prev * dt
# temp
# x_curr[tid] = x_last + v_prev * dt
@wp.kernel
def init_rhs_kernel(
dt: float,
x_curr: wp.array[wp.vec3],
x_inertia: wp.array[wp.vec3],
particle_masses: wp.array[float],
# outputs
rhs: wp.array[wp.vec3],
):
tid = wp.tid()
rhs[tid] = (x_inertia[tid] - x_curr[tid]) * particle_masses[tid] / (dt * dt)
@wp.kernel
def prepare_jacobi_preconditioner_kernel(
static_A_diags: wp.array[float],
contact_hessian_diags: wp.array[wp.mat33],
particle_flags: wp.array[wp.int32],
# outputs
inv_A_diags: wp.array[wp.mat33],
):
tid = wp.tid()
diag = wp.identity(3, float) * static_A_diags[tid]
if particle_flags[tid] & ParticleFlags.ACTIVE:
diag += contact_hessian_diags[tid]
inv_A_diags[tid] = wp.inverse(diag) if static_A_diags[tid] > 0.0 else wp.identity(3, float) * 0.0
@wp.kernel
def prepare_jacobi_preconditioner_no_contact_hessian_kernel(
static_A_diags: wp.array[float],
# outputs
inv_A_diags: wp.array[wp.mat33],
):
tid = wp.tid()
diag = wp.identity(3, float) * static_A_diags[tid]
inv_A_diags[tid] = wp.inverse(diag) if static_A_diags[tid] > 0.0 else wp.identity(3, float) * 0.0
@wp.kernel
def PD_jacobi_step_kernel(
rhs: wp.array[wp.vec3],
x_in: wp.array[wp.vec3],
inv_diags: wp.array[wp.mat33],
# outputs
x_out: wp.array[wp.vec3],
):
tid = wp.tid()
x_out[tid] = x_in[tid] + inv_diags[tid] * rhs[tid]
@wp.kernel
def nonlinear_step_kernel(
x_in: wp.array[wp.vec3],
# outputs
x_out: wp.array[wp.vec3],
dx: wp.array[wp.vec3],
):
tid = wp.tid()
x_out[tid] = x_in[tid] + dx[tid]
dx[tid] = wp.vec3(0.0)
@wp.kernel
def update_velocity(
dt: float,
prev_pos: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
vel: wp.array[wp.vec3],
):
particle = wp.tid()
vel[particle] = 0.998 * (pos[particle] - prev_pos[particle]) / dt
================================================
FILE: newton/_src/solvers/style3d/linear_solver.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from collections.abc import Callable
from typing import Any
import warp as wp
@wp.struct
class NonZeroEntry:
"""Represents a non-zero entry in a sparse matrix.
This structure stores the column index and corresponding value in a packed format, which provides
better cache locality for sequential access patterns.
"""
column_index: int
value: float
@wp.struct
class SparseMatrixELL:
"""Represents a sparse matrix in ELLPACK (ELL) format."""
num_nz: wp.array[int] # Non-zeros count per column
nz_ell: wp.array2d[NonZeroEntry] # Padded ELL storage [row-major, fixed-height]
@wp.func
def ell_mat_vec_mul(
num_nz: wp.array[int],
nz_ell: wp.array2d[NonZeroEntry],
x: wp.array[wp.vec3],
tid: int,
):
Mx = wp.vec3(0.0)
for k in range(num_nz[tid]):
nz_entry = nz_ell[k, tid]
Mx += x[nz_entry.column_index] * nz_entry.value
return Mx
@wp.kernel
def eval_residual_kernel(
A_non_diag: SparseMatrixELL,
A_diag: wp.array[Any],
x: wp.array[wp.vec3],
b: wp.array[wp.vec3],
# outputs
r: wp.array[wp.vec3],
):
tid = wp.tid()
Ax = A_diag[tid] * x[tid]
Ax += ell_mat_vec_mul(A_non_diag.num_nz, A_non_diag.nz_ell, x, tid)
r[tid] = b[tid] - Ax
# Forward-declare instances of the generic kernel to support graph capture on CUDA <12.3 drivers
wp.overload(eval_residual_kernel, {"A_diag": wp.array[wp.float32]})
wp.overload(eval_residual_kernel, {"A_diag": wp.array[wp.mat33]})
@wp.kernel
def eval_residual_kernel_with_additional_Ax(
A_non_diag: SparseMatrixELL,
A_diag: wp.array[Any],
x: wp.array[wp.vec3],
b: wp.array[wp.vec3],
additional_Ax: wp.array[wp.vec3],
# outputs
r: wp.array[wp.vec3],
):
tid = wp.tid()
Ax = A_diag[tid] * x[tid] + additional_Ax[tid]
Ax += ell_mat_vec_mul(A_non_diag.num_nz, A_non_diag.nz_ell, x, tid)
r[tid] = b[tid] - Ax
# Forward-declare instances of the generic kernel to support graph capture on CUDA <12.3 drivers
wp.overload(eval_residual_kernel_with_additional_Ax, {"A_diag": wp.array[wp.float32]})
wp.overload(eval_residual_kernel_with_additional_Ax, {"A_diag": wp.array[wp.mat33]})
@wp.kernel
def array_mul_kernel(
a: wp.array[Any],
b: wp.array[wp.vec3],
# outputs
out: wp.array[wp.vec3],
):
tid = wp.tid()
out[tid] = a[tid] * b[tid]
# Forward-declare instances of the generic kernel to support graph capture on CUDA <12.3 drivers
wp.overload(array_mul_kernel, {"a": wp.array[wp.float32]})
wp.overload(array_mul_kernel, {"a": wp.array[wp.mat33]})
@wp.kernel
def ell_mat_vec_mul_kernel(
M_non_diag: SparseMatrixELL,
M_diag: wp.array[Any],
x: wp.array[wp.vec3],
# outputs
Mx: wp.array[wp.vec3],
):
tid = wp.tid()
Mx[tid] = (M_diag[tid] * x[tid]) + ell_mat_vec_mul(M_non_diag.num_nz, M_non_diag.nz_ell, x, tid)
# Forward-declare instances of the generic kernel to support graph capture on CUDA <12.3 drivers
wp.overload(ell_mat_vec_mul_kernel, {"M_diag": wp.array[wp.float32]})
wp.overload(ell_mat_vec_mul_kernel, {"M_diag": wp.array[wp.mat33]})
@wp.kernel
def ell_mat_vec_mul_add_kernel(
M_non_diag: SparseMatrixELL,
M_diag: wp.array[Any],
x: wp.array[wp.vec3],
additional_Mx: wp.array[wp.vec3],
# outputs
Mx: wp.array[wp.vec3],
):
tid = wp.tid()
result = (M_diag[tid] * x[tid]) + additional_Mx[tid]
result += ell_mat_vec_mul(M_non_diag.num_nz, M_non_diag.nz_ell, x, tid)
Mx[tid] = result
# Forward-declare instances of the generic kernel to support graph capture on CUDA <12.3 drivers
wp.overload(ell_mat_vec_mul_add_kernel, {"M_diag": wp.array[wp.float32]})
wp.overload(ell_mat_vec_mul_add_kernel, {"M_diag": wp.array[wp.mat33]})
@wp.kernel
def update_cg_direction_kernel(
iter: int,
z: wp.array[wp.vec3],
rTz: wp.array[float],
p_prev: wp.array[wp.vec3],
# outputs
p: wp.array[wp.vec3],
):
# p = r + (rz_new / rz_old) * p;
i = wp.tid()
new_p = z[i]
if iter > 0:
num = rTz[iter]
denom = rTz[iter - 1]
beta = wp.float32(0.0)
if (wp.abs(denom) > 1.0e-30) and (not wp.isnan(denom)) and (not wp.isnan(num)):
beta = num / denom
new_p += beta * p_prev[i]
p[i] = new_p
@wp.kernel
def step_cg_kernel(
iter: int,
rTz: wp.array[float],
pTAp: wp.array[float],
p: wp.array[wp.vec3],
Ap: wp.array[wp.vec3],
# outputs
x: wp.array[wp.vec3],
r: wp.array[wp.vec3],
):
i = wp.tid()
num = rTz[iter]
denom = pTAp[iter]
alpha = wp.float32(0.0)
if (wp.abs(denom) > 1.0e-30) and (not wp.isnan(denom)) and (not wp.isnan(num)):
alpha = num / denom
r[i] = r[i] - alpha * Ap[i]
x[i] = x[i] + alpha * p[i]
@wp.kernel
def generate_test_data_kernel(
dim: int,
diag_term: float,
A_non_diag: SparseMatrixELL,
A_diag: wp.array[Any],
b: wp.array[wp.vec3],
x0: wp.array[wp.vec3],
):
tid = wp.tid()
t = wp.float32(tid)
b[tid] = wp.vec3(wp.sin(t * 0.123), wp.cos(t * 0.456), wp.sin(t * 0.789))
x0[tid] = wp.vec3(wp.cos(t * 0.123), wp.tan(t * 0.456), wp.cos(t * 0.789))
A_diag[tid] = diag_term
if tid == 0:
A_non_diag.num_nz[tid] = 1
A_non_diag.nz_ell[0, tid].value = -1.0
A_non_diag.nz_ell[0, tid].column_index = 1
elif tid == dim - 1:
A_non_diag.num_nz[tid] = 1
A_non_diag.nz_ell[0, tid].value = -1.0
A_non_diag.nz_ell[0, tid].column_index = dim - 2
else:
A_non_diag.num_nz[tid] = 2
A_non_diag.nz_ell[0, tid].value = -1.0
A_non_diag.nz_ell[0, tid].column_index = tid + 1
A_non_diag.nz_ell[1, tid].value = -1.0
A_non_diag.nz_ell[1, tid].column_index = tid - 1
def array_inner(
a: wp.array[wp.vec3],
b: wp.array[wp.vec3],
out_ptr: wp.uint64,
):
from warp._src.context import runtime # noqa: PLC0415
if a.device.is_cpu:
func = runtime.core.wp_array_inner_float_host
else:
func = runtime.core.wp_array_inner_float_device
func(
a.ptr,
b.ptr,
out_ptr,
len(a),
wp.types.type_size_in_bytes(a.dtype),
wp.types.type_size_in_bytes(b.dtype),
wp.types.type_size(a.dtype),
)
class PcgSolver:
"""A Customized PCG implementation for efficient cloth simulation
Ref: https://en.wikipedia.org/wiki/Conjugate_gradient_method
Sparse Matrix Storages:
Part-1: (static)
1. Non-diagonals: SparseMatrixELL
2. Diagonals: wp.array(dtype = mat3x3)
Part-2: (dynamic)
1. Preconditioner: wp.array(wp.mat3x3)
2. Matrix-free Ax: wp.array(dtype = wp.vec3)
3. Matrix-free diagonals: wp.array(wp.mat3x3)
"""
def __init__(self, dim: int, device, maxIter: int = 999):
self.dim = dim # pre-allocation
self.device = device
self.maxIter = maxIter
self.r = wp.array(shape=dim, dtype=wp.vec3, device=device)
self.z = wp.array(shape=dim, dtype=wp.vec3, device=device)
self.p = wp.array(shape=dim, dtype=wp.vec3, device=device)
self.Ap = wp.array(shape=dim, dtype=wp.vec3, device=device)
self.pTAp = wp.array(shape=maxIter, dtype=float, device=device)
self.rTz = wp.array(shape=maxIter, dtype=float, device=device)
def step1_update_r(
self,
A_non_diag: SparseMatrixELL,
A_diag: wp.array[Any],
b: wp.array[wp.vec3],
x: wp.array[wp.vec3] = None, # Pass `None` if x[:] == 0.0
additional_Ax: wp.array[wp.vec3] = None, # Pass `None` if additional_Ax[:] == 0.0
):
"""Update residual: r = b - A * x"""
if x is None:
self.r.assign(b)
elif additional_Ax is None:
wp.launch(
eval_residual_kernel,
dim=self.dim,
inputs=[A_non_diag, A_diag, x, b],
outputs=[self.r],
device=self.device,
)
else:
wp.launch(
eval_residual_kernel_with_additional_Ax,
dim=self.dim,
inputs=[A_non_diag, A_diag, x, b, additional_Ax],
outputs=[self.r],
device=self.device,
)
def step2_update_z(self, inv_M: wp.array[Any]):
wp.launch(array_mul_kernel, dim=self.dim, inputs=[inv_M, self.r], outputs=[self.z], device=self.device)
def step3_update_rTz(self, iter: int):
array_inner(self.r, self.z, self.rTz.ptr + iter * self.rTz.strides[0])
def step4_update_p(self, iter: int):
wp.launch(
update_cg_direction_kernel,
dim=self.dim,
inputs=[iter, self.z, self.rTz, self.p],
outputs=[self.p],
device=self.device,
)
def step5_update_Ap(
self,
A_non_diag: SparseMatrixELL,
A_diag: wp.array[Any],
additional_Ap: wp.array[wp.vec3] = None,
):
if additional_Ap is None:
wp.launch(
ell_mat_vec_mul_kernel,
dim=self.dim,
inputs=[A_non_diag, A_diag, self.p],
outputs=[self.Ap],
device=self.device,
)
else:
wp.launch(
ell_mat_vec_mul_add_kernel,
dim=self.dim,
inputs=[A_non_diag, A_diag, self.p, additional_Ap],
outputs=[self.Ap],
device=self.device,
)
def step6_update_pTAp(self, iter: int):
array_inner(self.p, self.Ap, self.pTAp.ptr + iter * self.pTAp.strides[0])
def step7_update_x_r(self, x: wp.array[wp.vec3], iter: int):
wp.launch(
step_cg_kernel,
dim=self.dim,
inputs=[iter, self.rTz, self.pTAp, self.p, self.Ap],
outputs=[x, self.r],
device=self.device,
)
def solve(
self,
A_non_diag: SparseMatrixELL,
A_diag: wp.array[Any],
x0: wp.array[wp.vec3], # Pass `None` means x0[:] == 0.0
b: wp.array[wp.vec3],
inv_M: wp.array[Any],
x1: wp.array[wp.vec3],
iterations: int,
additional_multiplier: Callable | None = None,
):
# Prevent out-of-bounds in rTz/pTAp when iterations > maxIter.
iterations = wp.min(iterations, self.maxIter)
if x0 is None:
x1.zero_()
else:
x1.assign(x0)
if additional_multiplier is None:
self.step1_update_r(A_non_diag, A_diag, b, x0)
else:
additional_Ax = additional_multiplier(x0) if x0 is not None else None
self.step1_update_r(A_non_diag, A_diag, b, x0, additional_Ax)
for iter in range(iterations):
self.step2_update_z(inv_M)
self.step3_update_rTz(iter)
self.step4_update_p(iter)
if additional_multiplier is None:
self.step5_update_Ap(A_non_diag, A_diag)
else:
additional_Ap = additional_multiplier(self.p)
self.step5_update_Ap(A_non_diag, A_diag, additional_Ap)
self.step6_update_pTAp(iter)
self.step7_update_x_r(x1, iter)
if __name__ == "__main__":
wp.init()
dim = 100000
diag_term = 5.0
A_non_diag = SparseMatrixELL()
A_diag = wp.zeros(dim, dtype=wp.float32)
A_non_diag.num_nz = wp.zeros(dim, dtype=wp.int32)
A_non_diag.nz_ell = wp.zeros(shape=(2, dim), dtype=NonZeroEntry)
b = wp.zeros(dim, dtype=wp.vec3)
x0 = wp.zeros(dim, dtype=wp.vec3)
x1 = wp.zeros(dim, dtype=wp.vec3)
wp.launch(generate_test_data_kernel, dim=dim, inputs=[dim, diag_term], outputs=[A_non_diag, A_diag, b, x0])
inv_M = wp.array([1.0 / diag_term] * dim, dtype=float)
solver = PcgSolver(dim, device="cuda:0")
solver.solve(A_non_diag, A_diag, x0, b, inv_M, x1, iterations=30)
rTr = wp.zeros(1, dtype=float)
array_inner(solver.r, solver.r, rTr.ptr)
print(rTr.numpy()[0])
================================================
FILE: newton/_src/solvers/style3d/solver_style3d.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warp as wp
from ...core.types import override
from ...sim import Contacts, Control, Model, ModelBuilder, State
from ..solver import SolverBase
from .builder import PDMatrixBuilder
from .collision import Collision
from .kernels import (
accumulate_dragging_pd_diag_kernel,
eval_bend_kernel,
eval_drag_force_kernel,
eval_stretch_kernel,
init_rhs_kernel,
init_step_kernel,
nonlinear_step_kernel,
prepare_jacobi_preconditioner_kernel,
prepare_jacobi_preconditioner_no_contact_hessian_kernel,
update_velocity,
)
from .linear_solver import PcgSolver, SparseMatrixELL
AttributeAssignment = Model.AttributeAssignment
AttributeFrequency = Model.AttributeFrequency
########################################################################################################################
################################################# Style3D Solver #################################################
########################################################################################################################
class SolverStyle3D(SolverBase):
r"""Projective dynamics based cloth solver.
References:
1. Baraff, D. & Witkin, A. "Large Steps in Cloth Simulation."
2. Liu, T. et al. "Fast Simulation of Mass-Spring Systems."
Implicit-Euler method solves the following non-linear equation:
.. math::
(M / dt^2 + H(x)) \cdot dx &= (M / dt^2) \cdot (x_{prev} + v_{prev} \cdot dt - x) + f_{ext}(x) + f_{int}(x) \\
&= (M / dt^2) \cdot (x_{prev} + v_{prev} \cdot dt + (dt^2 / M) \cdot f_{ext}(x) - x) + f_{int}(x) \\
&= (M / dt^2) \cdot (x_{inertia} - x) + f_{int}(x)
Notations:
- :math:`M`: mass matrix
- :math:`x`: unsolved particle position
- :math:`H`: Hessian matrix (function of x)
- :math:`P`: PD-approximated Hessian matrix (constant)
- :math:`A`: :math:`M / dt^2 + H(x)` or :math:`M / dt^2 + P`
- :math:`rhs`: Right hand side of the equation: :math:`(M / dt^2) \cdot (x_{inertia} - x) + f_{int}(x)`
- :math:`res`: Residual: :math:`rhs - A \cdot dx_{init}`, or rhs if :math:`dx_{init} = 0`
See Also:
:doc:`newton.solvers.style3d ` exposes
helper functions that populate Style3D cloth data on a
:class:`~newton.ModelBuilder`.
Example:
Build a mesh-based cloth with
:func:`newton.solvers.style3d.add_cloth_mesh`::
from newton.solvers import style3d
builder = newton.ModelBuilder()
SolverStyle3D.register_custom_attributes(builder)
style3d.add_cloth_mesh(
builder,
pos=wp.vec3(0.0, 0.0, 0.0),
rot=wp.quat_identity(),
vel=wp.vec3(0.0, 0.0, 0.0),
vertices=mesh.vertices.tolist(),
indices=mesh.indices.tolist(),
density=0.3,
tri_aniso_ke=wp.vec3(1.0e2, 1.0e2, 1.0e1),
edge_aniso_ke=wp.vec3(2.0e-5, 1.0e-5, 5.0e-6),
)
Or build a grid with :func:`newton.solvers.style3d.add_cloth_grid`::
style3d.add_cloth_grid(
builder,
pos=wp.vec3(-0.5, 0.0, 2.0),
rot=wp.quat_identity(),
dim_x=64,
dim_y=32,
cell_x=0.1,
cell_y=0.1,
vel=wp.vec3(0.0, 0.0, 0.0),
mass=0.1,
tri_aniso_ke=wp.vec3(1.0e2, 1.0e2, 1.0e1),
edge_aniso_ke=wp.vec3(2.0e-4, 1.0e-4, 5.0e-5),
)
"""
def __init__(
self,
model: Model,
iterations: int = 10,
linear_iterations: int = 10,
drag_spring_stiff: float = 1e2,
enable_mouse_dragging: bool = False,
):
"""
Args:
model: The :class:`~newton.Model` containing Style3D attributes to integrate.
iterations: Number of non-linear iterations per step.
linear_iterations: Number of linear iterations (currently PCG iter) per non-linear iteration.
drag_spring_stiff: The stiffness of spring connecting barycentric-weighted drag-point and target-point.
enable_mouse_dragging: Enable/disable dragging kernel.
"""
super().__init__(model)
if not hasattr(model, "style3d"):
raise AttributeError(
"Style3D custom attributes are missing from the model. "
"Call SolverStyle3D.register_custom_attributes() before building the model."
)
self.style3d = model.style3d
self.collision: Collision | None = Collision(model) # set None to disable
self.linear_iterations = linear_iterations
self.nonlinear_iterations = iterations
self.drag_spring_stiff = drag_spring_stiff
self.enable_mouse_dragging = enable_mouse_dragging
self.pd_matrix_builder = PDMatrixBuilder(model.particle_count)
self.linear_solver = PcgSolver(model.particle_count, self.device)
# Fixed PD matrix
self.pd_non_diags = SparseMatrixELL()
self.pd_diags = wp.zeros(model.particle_count, dtype=float, device=self.device)
# Non-linear equation variables
self.dx = wp.zeros(model.particle_count, dtype=wp.vec3, device=self.device)
self.rhs = wp.zeros(model.particle_count, dtype=wp.vec3, device=self.device)
self.x_prev = wp.zeros(model.particle_count, dtype=wp.vec3, device=self.device)
self.x_inertia = wp.zeros(model.particle_count, dtype=wp.vec3, device=self.device)
# Static part of A_diag, full A_diag, and inverse of A_diag
self.static_A_diags = wp.zeros(model.particle_count, dtype=float, device=self.device)
self.inv_A_diags = wp.zeros(model.particle_count, dtype=wp.mat33, device=self.device)
self.A_diags = wp.zeros(model.particle_count, dtype=wp.mat33, device=self.device)
# Drag info
self.drag_pos = wp.zeros(1, dtype=wp.vec3, device=self.device)
self.drag_index = wp.array([-1], dtype=int, device=self.device)
self.drag_bary_coord = wp.zeros(1, dtype=wp.vec3, device=self.device)
@override
def step(self, state_in: State, state_out: State, control: Control, contacts: Contacts, dt: float) -> None:
"""Advance the Style3D solver by one time step.
The solver performs non-linear projective dynamics iterations with
optional collision handling. During the solve, positions in
``state_in`` are updated in-place to the current iterate; the final
positions and velocities are written to ``state_out``.
Args:
state_in: Input :class:`newton.State` (positions updated in-place).
state_out: Output :class:`newton.State` with the final state.
control: :class:`newton.Control` input (currently unused).
contacts: :class:`newton.Contacts` used for collision response.
dt: Time step in seconds.
"""
if self.collision is not None:
self.collision.frame_begin(state_in.particle_q, state_in.particle_qd, dt)
wp.launch(
kernel=init_step_kernel,
dim=self.model.particle_count,
inputs=[
dt,
self.model.gravity,
self.model.particle_world,
state_in.particle_f,
state_in.particle_qd,
state_in.particle_q,
self.x_prev,
self.pd_diags,
self.model.particle_mass,
self.model.particle_flags,
],
outputs=[
self.x_inertia,
self.static_A_diags,
self.dx,
],
device=self.device,
)
if self.enable_mouse_dragging:
wp.launch(
accumulate_dragging_pd_diag_kernel,
dim=1,
inputs=[
self.drag_spring_stiff,
self.drag_index,
self.drag_bary_coord,
self.model.tri_indices,
self.model.particle_flags,
],
outputs=[self.static_A_diags],
device=self.device,
)
for _iter in range(self.nonlinear_iterations):
wp.launch(
init_rhs_kernel,
dim=self.model.particle_count,
inputs=[
dt,
state_in.particle_q,
self.x_inertia,
self.model.particle_mass,
],
outputs=[self.rhs],
device=self.device,
)
wp.launch(
eval_stretch_kernel,
dim=len(self.model.tri_areas),
inputs=[
state_in.particle_q,
self.model.tri_areas,
self.model.tri_poses,
self.model.tri_indices,
self.style3d.tri_aniso_ke,
],
outputs=[self.rhs],
device=self.device,
)
wp.launch(
eval_bend_kernel,
dim=len(self.style3d.edge_rest_area),
inputs=[
state_in.particle_q,
self.style3d.edge_rest_area,
self.style3d.edge_bending_cot,
self.model.edge_indices,
self.model.edge_bending_properties,
],
outputs=[self.rhs],
device=self.device,
)
if self.enable_mouse_dragging:
wp.launch(
eval_drag_force_kernel,
dim=1,
inputs=[
self.drag_spring_stiff,
self.drag_index,
self.drag_pos,
self.drag_bary_coord,
self.model.tri_indices,
state_in.particle_q,
],
outputs=[self.rhs],
device=self.device,
)
if self.collision is not None:
self.collision.accumulate_contact_force(
dt,
_iter,
state_in,
state_out,
contacts,
self.rhs,
self.x_prev,
self.static_A_diags,
)
wp.launch(
prepare_jacobi_preconditioner_kernel,
dim=self.model.particle_count,
inputs=[
self.static_A_diags,
self.collision.contact_hessian_diagonal(),
self.model.particle_flags,
],
outputs=[self.inv_A_diags],
device=self.device,
)
else:
wp.launch(
prepare_jacobi_preconditioner_no_contact_hessian_kernel,
dim=self.model.particle_count,
inputs=[self.static_A_diags],
outputs=[self.inv_A_diags],
device=self.device,
)
self.linear_solver.solve(
self.pd_non_diags,
self.static_A_diags,
self.dx if _iter == 0 else None,
self.rhs,
self.inv_A_diags,
self.dx,
self.linear_iterations,
None if self.collision is None else self.collision.hessian_multiply,
)
if self.collision is not None:
self.collision.linear_iteration_end(self.dx)
wp.launch(
nonlinear_step_kernel,
dim=self.model.particle_count,
inputs=[state_in.particle_q],
outputs=[state_out.particle_q, self.dx],
device=self.device,
)
state_in.particle_q.assign(state_out.particle_q)
wp.launch(
kernel=update_velocity,
dim=self.model.particle_count,
inputs=[dt, self.x_prev, state_out.particle_q],
outputs=[state_out.particle_qd],
device=self.device,
)
if self.collision is not None:
self.collision.frame_end(state_out.particle_q, state_out.particle_qd, dt)
def rebuild_bvh(self, state: State):
if self.collision is not None:
self.collision.rebuild_bvh(state.particle_q)
@override
@classmethod
def register_custom_attributes(cls, builder: ModelBuilder) -> None:
"""Declare Style3D custom attributes under the ``style3d`` namespace.
See Also:
:ref:`custom_attributes` for the custom attribute system overview.
"""
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="tri_aniso_ke",
frequency=AttributeFrequency.TRIANGLE,
assignment=AttributeAssignment.MODEL,
dtype=wp.vec3,
default=wp.vec3(0.0),
namespace="style3d",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="edge_rest_area",
frequency=AttributeFrequency.EDGE,
assignment=AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.0,
namespace="style3d",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="edge_bending_cot",
frequency=AttributeFrequency.EDGE,
assignment=AttributeAssignment.MODEL,
dtype=wp.vec4,
default=wp.vec4(0.0, 0.0, 0.0, 0.0),
namespace="style3d",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="aniso_ke",
frequency=AttributeFrequency.EDGE,
assignment=AttributeAssignment.MODEL,
dtype=wp.vec3,
default=wp.vec3(0.0),
namespace="style3d",
)
)
def _precompute(self, builder: ModelBuilder):
with wp.ScopedTimer("SolverStyle3D::precompute()"):
tri_aniso_attr = builder.custom_attributes.get("style3d:tri_aniso_ke")
edge_rest_area_attr = builder.custom_attributes.get("style3d:edge_rest_area")
edge_bending_cot_attr = builder.custom_attributes.get("style3d:edge_bending_cot")
if tri_aniso_attr is None or edge_rest_area_attr is None or edge_bending_cot_attr is None:
raise AttributeError(
"Style3D custom attributes are missing from the builder. "
"Call SolverStyle3D.register_custom_attributes() before building the model."
)
tri_aniso_ke = tri_aniso_attr.build_array(len(builder.tri_indices), device="cpu").numpy().tolist()
edge_rest_area = edge_rest_area_attr.build_array(len(builder.edge_indices), device="cpu").numpy().tolist()
edge_bending_cot = (
edge_bending_cot_attr.build_array(len(builder.edge_indices), device="cpu").numpy().tolist()
)
self.pd_matrix_builder.add_stretch_constraints(
builder.tri_indices, builder.tri_poses, tri_aniso_ke, builder.tri_areas
)
self.pd_matrix_builder.add_bend_constraints(
builder.edge_indices,
builder.edge_bending_properties,
edge_rest_area,
edge_bending_cot,
)
self.pd_diags, self.pd_non_diags.num_nz, self.pd_non_diags.nz_ell = self.pd_matrix_builder.finalize(
self.device
)
def _update_drag_info(self, index: int, pos: wp.vec3, bary_coord: wp.vec3):
"""Should be invoked when state changed."""
# print([index, pos, bary_coord])
self.drag_bary_coord.fill_(bary_coord)
self.drag_index.fill_(index)
self.drag_pos.fill_(pos)
================================================
FILE: newton/_src/solvers/vbd/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .solver_vbd import SolverVBD
__all__ = [
"SolverVBD",
]
================================================
FILE: newton/_src/solvers/vbd/particle_vbd_kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Particle/soft-body VBD helper routines.
This module is intended to host the particle/soft-body specific parts of the
VBD solver (cloth, springs, triangles, tets, particle contacts, etc.).
The high-level :class:`SolverVBD` interface should remain in
``solver_vbd.py`` and call into functions defined here.
"""
from __future__ import annotations
import numpy as np
import warp as wp
from newton._src.math import orthonormal_basis
from newton._src.solvers.vbd.rigid_vbd_kernels import evaluate_body_particle_contact
from ...geometry import ParticleFlags
from ...geometry.kernels import triangle_closest_point
from .tri_mesh_collision import (
TriMeshCollisionInfo,
)
# TODO: Grab changes from Warp that has fixed the backward pass
wp.set_module_options({"enable_backward": False})
VBD_DEBUG_PRINTING_OPTIONS = {
# "elasticity_force_hessian",
# "contact_force_hessian",
# "contact_force_hessian_vt",
# "contact_force_hessian_ee",
# "overall_force_hessian",
# "inertia_force_hessian",
# "connectivity",
# "contact_info",
}
NUM_THREADS_PER_COLLISION_PRIMITIVE = 4
TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE = 16
TILE_SIZE_SELF_CONTACT_SOLVE = 8
class mat32(wp.types.matrix(shape=(3, 2), dtype=wp.float32)):
pass
class mat99(wp.types.matrix(shape=(9, 9), dtype=wp.float32)):
pass
class mat93(wp.types.matrix(shape=(9, 3), dtype=wp.float32)):
pass
class mat43(wp.types.matrix(shape=(4, 3), dtype=wp.float32)):
pass
class vec9(wp.types.vector(length=9, dtype=wp.float32)):
pass
@wp.struct
class ParticleForceElementAdjacencyInfo:
r"""
- vertex_adjacent_[element]: the flatten adjacency information. Its size is \sum_{i\inV} 2*N_i, where N_i is the
number of vertex i's adjacent [element]. For each adjacent element it stores 2 information:
- the id of the adjacent element
- the order of the vertex in the element, which is essential to compute the force and hessian for the vertex
- vertex_adjacent_[element]_offsets: stores where each vertex information starts in the flatten adjacency array.
Its size is |V|+1 such that the number of vertex i's adjacent [element] can be computed as
vertex_adjacent_[element]_offsets[i+1]-vertex_adjacent_[element]_offsets[i].
"""
v_adj_faces: wp.array[int]
v_adj_faces_offsets: wp.array[int]
v_adj_edges: wp.array[int]
v_adj_edges_offsets: wp.array[int]
v_adj_springs: wp.array[int]
v_adj_springs_offsets: wp.array[int]
v_adj_tets: wp.array[int]
v_adj_tets_offsets: wp.array[int]
def to(self, device):
if device == self.v_adj_faces.device:
return self
else:
adjacency_gpu = ParticleForceElementAdjacencyInfo()
adjacency_gpu.v_adj_faces = self.v_adj_faces.to(device)
adjacency_gpu.v_adj_faces_offsets = self.v_adj_faces_offsets.to(device)
adjacency_gpu.v_adj_edges = self.v_adj_edges.to(device)
adjacency_gpu.v_adj_edges_offsets = self.v_adj_edges_offsets.to(device)
adjacency_gpu.v_adj_springs = self.v_adj_springs.to(device)
adjacency_gpu.v_adj_springs_offsets = self.v_adj_springs_offsets.to(device)
adjacency_gpu.v_adj_tets = self.v_adj_tets.to(device)
adjacency_gpu.v_adj_tets_offsets = self.v_adj_tets_offsets.to(device)
return adjacency_gpu
@wp.func
def get_vertex_num_adjacent_edges(adjacency: ParticleForceElementAdjacencyInfo, vertex: wp.int32):
return (adjacency.v_adj_edges_offsets[vertex + 1] - adjacency.v_adj_edges_offsets[vertex]) >> 1
@wp.func
def get_vertex_adjacent_edge_id_order(adjacency: ParticleForceElementAdjacencyInfo, vertex: wp.int32, edge: wp.int32):
offset = adjacency.v_adj_edges_offsets[vertex]
return adjacency.v_adj_edges[offset + edge * 2], adjacency.v_adj_edges[offset + edge * 2 + 1]
@wp.func
def get_vertex_num_adjacent_faces(adjacency: ParticleForceElementAdjacencyInfo, vertex: wp.int32):
return (adjacency.v_adj_faces_offsets[vertex + 1] - adjacency.v_adj_faces_offsets[vertex]) >> 1
@wp.func
def get_vertex_adjacent_face_id_order(adjacency: ParticleForceElementAdjacencyInfo, vertex: wp.int32, face: wp.int32):
offset = adjacency.v_adj_faces_offsets[vertex]
return adjacency.v_adj_faces[offset + face * 2], adjacency.v_adj_faces[offset + face * 2 + 1]
@wp.func
def get_vertex_num_adjacent_springs(adjacency: ParticleForceElementAdjacencyInfo, vertex: wp.int32):
return adjacency.v_adj_springs_offsets[vertex + 1] - adjacency.v_adj_springs_offsets[vertex]
@wp.func
def get_vertex_adjacent_spring_id(adjacency: ParticleForceElementAdjacencyInfo, vertex: wp.int32, spring: wp.int32):
offset = adjacency.v_adj_springs_offsets[vertex]
return adjacency.v_adj_springs[offset + spring]
@wp.func
def get_vertex_num_adjacent_tets(adjacency: ParticleForceElementAdjacencyInfo, vertex: wp.int32):
return (adjacency.v_adj_tets_offsets[vertex + 1] - adjacency.v_adj_tets_offsets[vertex]) >> 1
@wp.func
def get_vertex_adjacent_tet_id_order(adjacency: ParticleForceElementAdjacencyInfo, vertex: wp.int32, tet: wp.int32):
offset = adjacency.v_adj_tets_offsets[vertex]
return adjacency.v_adj_tets[offset + tet * 2], adjacency.v_adj_tets[offset + tet * 2 + 1]
@wp.func
def assemble_tet_vertex_force_and_hessian(
dE_dF: vec9,
H: mat99,
m1: float,
m2: float,
m3: float,
):
f = wp.vec3(
-(dE_dF[0] * m1 + dE_dF[3] * m2 + dE_dF[6] * m3),
-(dE_dF[1] * m1 + dE_dF[4] * m2 + dE_dF[7] * m3),
-(dE_dF[2] * m1 + dE_dF[5] * m2 + dE_dF[8] * m3),
)
h = wp.mat33()
h[0, 0] += (
m1 * (H[0, 0] * m1 + H[3, 0] * m2 + H[6, 0] * m3)
+ m2 * (H[0, 3] * m1 + H[3, 3] * m2 + H[6, 3] * m3)
+ m3 * (H[0, 6] * m1 + H[3, 6] * m2 + H[6, 6] * m3)
)
h[1, 0] += (
m1 * (H[1, 0] * m1 + H[4, 0] * m2 + H[7, 0] * m3)
+ m2 * (H[1, 3] * m1 + H[4, 3] * m2 + H[7, 3] * m3)
+ m3 * (H[1, 6] * m1 + H[4, 6] * m2 + H[7, 6] * m3)
)
h[2, 0] += (
m1 * (H[2, 0] * m1 + H[5, 0] * m2 + H[8, 0] * m3)
+ m2 * (H[2, 3] * m1 + H[5, 3] * m2 + H[8, 3] * m3)
+ m3 * (H[2, 6] * m1 + H[5, 6] * m2 + H[8, 6] * m3)
)
h[0, 1] += (
m1 * (H[0, 1] * m1 + H[3, 1] * m2 + H[6, 1] * m3)
+ m2 * (H[0, 4] * m1 + H[3, 4] * m2 + H[6, 4] * m3)
+ m3 * (H[0, 7] * m1 + H[3, 7] * m2 + H[6, 7] * m3)
)
h[1, 1] += (
m1 * (H[1, 1] * m1 + H[4, 1] * m2 + H[7, 1] * m3)
+ m2 * (H[1, 4] * m1 + H[4, 4] * m2 + H[7, 4] * m3)
+ m3 * (H[1, 7] * m1 + H[4, 7] * m2 + H[7, 7] * m3)
)
h[2, 1] += (
m1 * (H[2, 1] * m1 + H[5, 1] * m2 + H[8, 1] * m3)
+ m2 * (H[2, 4] * m1 + H[5, 4] * m2 + H[8, 4] * m3)
+ m3 * (H[2, 7] * m1 + H[5, 7] * m2 + H[8, 7] * m3)
)
h[0, 2] += (
m1 * (H[0, 2] * m1 + H[3, 2] * m2 + H[6, 2] * m3)
+ m2 * (H[0, 5] * m1 + H[3, 5] * m2 + H[6, 5] * m3)
+ m3 * (H[0, 8] * m1 + H[3, 8] * m2 + H[6, 8] * m3)
)
h[1, 2] += (
m1 * (H[1, 2] * m1 + H[4, 2] * m2 + H[7, 2] * m3)
+ m2 * (H[1, 5] * m1 + H[4, 5] * m2 + H[7, 5] * m3)
+ m3 * (H[1, 8] * m1 + H[4, 8] * m2 + H[7, 8] * m3)
)
h[2, 2] += (
m1 * (H[2, 2] * m1 + H[5, 2] * m2 + H[8, 2] * m3)
+ m2 * (H[2, 5] * m1 + H[5, 5] * m2 + H[8, 5] * m3)
+ m3 * (H[2, 8] * m1 + H[5, 8] * m2 + H[8, 8] * m3)
)
return f, h
@wp.func
def damp_force_and_hessian(
particle_pos_prev: wp.vec3,
particle_pos: wp.vec3,
force: wp.vec3,
hessian: wp.mat33,
damping: float,
dt: float,
):
displacement = particle_pos_prev - particle_pos
h_d = hessian * (damping / dt)
f_d = h_d * displacement
return force + f_d, hessian + h_d
# @wp.func
# def evaluate_volumetric_neo_hookean_force_and_hessian(
# tet_id: int,
# v_order: int,
# pos_prev: wp.array[wp.vec3],
# pos: wp.array[wp.vec3],
# tet_indices: wp.array2d[wp.int32],
# Dm_inv: wp.mat33,
# mu: float,
# lmbd: float,
# damping: float,
# dt: float,
# ) -> tuple[wp.vec3, wp.mat33]:
# # ============ Get Vertices ============
# v0 = pos[tet_indices[tet_id, 0]]
# v1 = pos[tet_indices[tet_id, 1]]
# v2 = pos[tet_indices[tet_id, 2]]
# v3 = pos[tet_indices[tet_id, 3]]
# # ============ Compute rest volume from Dm_inv ============
# rest_volume = 1.0 / (wp.determinant(Dm_inv) * 6.0)
# # ============ Deformation Gradient ============
# Ds = wp.mat33(v1 - v0, v2 - v0, v3 - v0)
# F = Ds * Dm_inv
# # ============ Flatten F to vec9 ============
# f = vec9(
# F[0,0], F[1,0], F[2,0],
# F[0,1], F[1,1], F[2,1],
# F[0,2], F[1,2], F[2,2],
# )
# # ============ Useful Quantities ============
# J = wp.determinant(F)
# alpha = 1.0 + mu / lmbd
# F_inv = wp.inverse(F)
# cof = J * wp.transpose(F_inv)
# cof_vec = vec9(
# cof[0,0], cof[1,0], cof[2,0],
# cof[0,1], cof[1,1], cof[2,1],
# cof[0,2], cof[1,2], cof[2,2],
# )
# # ============ Stress ============
# P_vec = rest_volume * (mu * f + lmbd * (J - alpha) * cof_vec)
# # ============ Hessian ============
# H = (mu * wp.identity(n=9, dtype=float)
# + lmbd * wp.outer(cof_vec, cof_vec)
# + compute_cofactor_derivative(F, lmbd * (J - alpha)))
# H = rest_volume * H
# # ============ G_i ============
# G_i = compute_G_matrix(Dm_inv, v_order)
# # ============ Force & Hessian ============
# force = -wp.transpose(G_i) * P_vec
# hessian = wp.transpose(G_i) * H * G_i
# # ============ Damping ============
# if damping > 0.0:
# inv_dt = 1.0 / dt
# v0_prev = pos_prev[tet_indices[tet_id, 0]]
# v1_prev = pos_prev[tet_indices[tet_id, 1]]
# v2_prev = pos_prev[tet_indices[tet_id, 2]]
# v3_prev = pos_prev[tet_indices[tet_id, 3]]
# Ds_dot = wp.mat33(
# (v1 - v1_prev) - (v0 - v0_prev),
# (v2 - v2_prev) - (v0 - v0_prev),
# (v3 - v3_prev) - (v0 - v0_prev),
# ) * inv_dt
# F_dot = Ds_dot * Dm_inv
# f_dot = vec9(
# F_dot[0,0], F_dot[1,0], F_dot[2,0],
# F_dot[0,1], F_dot[1,1], F_dot[2,1],
# F_dot[0,2], F_dot[1,2], F_dot[2,2],
# )
# P_damp = damping * (H * f_dot)
# force = force - wp.transpose(G_i) * P_damp
# hessian = hessian + (damping * inv_dt) * wp.transpose(G_i) * H * G_i
# return force, hessian
@wp.func
def evaluate_volumetric_neo_hookean_force_and_hessian(
tet_id: int,
v_order: int,
pos_prev: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
tet_indices: wp.array2d[wp.int32],
Dm_inv: wp.mat33,
mu: float,
lmbd: float,
damping: float,
dt: float,
) -> tuple[wp.vec3, wp.mat33]:
# ============ Get Vertices ============
v0 = pos[tet_indices[tet_id, 0]]
v1 = pos[tet_indices[tet_id, 1]]
v2 = pos[tet_indices[tet_id, 2]]
v3 = pos[tet_indices[tet_id, 3]]
# ============ Compute rest volume from Dm_inv ============
rest_volume = 1.0 / (wp.determinant(Dm_inv) * 6.0)
# ============ Deformation Gradient ============
Ds = wp.matrix_from_cols(v1 - v0, v2 - v0, v3 - v0)
F = Ds * Dm_inv
# ============ Flatten F to vec9 ============
f = vec9(
F[0, 0],
F[1, 0],
F[2, 0],
F[0, 1],
F[1, 1],
F[2, 1],
F[0, 2],
F[1, 2],
F[2, 2],
)
# ============ Useful Quantities ============
J = wp.determinant(F)
# Guard against division by zero in lambda (Lamé's first parameter)
# For numerical stability, ensure lmbd has a reasonable minimum magnitude
lmbd_safe = wp.sign(lmbd) * wp.max(wp.abs(lmbd), 1e-6)
alpha = 1.0 + mu / lmbd_safe
# Compute cofactor (adjugate) matrix directly for numerical stability when J ≈ 0
cof = compute_cofactor(F)
cof_vec = vec9(
cof[0, 0],
cof[1, 0],
cof[2, 0],
cof[0, 1],
cof[1, 1],
cof[2, 1],
cof[0, 2],
cof[1, 2],
cof[2, 2],
)
# ============ Stress ============
P_vec = rest_volume * (mu * f + lmbd * (J - alpha) * cof_vec)
# ============ Hessian ============
H = (
mu * wp.identity(n=9, dtype=float)
+ lmbd * wp.outer(cof_vec, cof_vec)
+ compute_cofactor_derivative(F, lmbd * (J - alpha))
)
H = rest_volume * H
# ============ Assemble Pointwise Force ============
if v_order == 0:
m = wp.vec3(
-(Dm_inv[0, 0] + Dm_inv[1, 0] + Dm_inv[2, 0]),
-(Dm_inv[0, 1] + Dm_inv[1, 1] + Dm_inv[2, 1]),
-(Dm_inv[0, 2] + Dm_inv[1, 2] + Dm_inv[2, 2]),
)
elif v_order == 1:
m = wp.vec3(Dm_inv[0, 0], Dm_inv[0, 1], Dm_inv[0, 2])
elif v_order == 2:
m = wp.vec3(Dm_inv[1, 0], Dm_inv[1, 1], Dm_inv[1, 2])
else:
m = wp.vec3(Dm_inv[2, 0], Dm_inv[2, 1], Dm_inv[2, 2])
force, hessian = assemble_tet_vertex_force_and_hessian(P_vec, H, m[0], m[1], m[2])
# ============ Damping ============
if damping > 0.0:
inv_dt = 1.0 / dt
v0_prev = pos_prev[tet_indices[tet_id, 0]]
v1_prev = pos_prev[tet_indices[tet_id, 1]]
v2_prev = pos_prev[tet_indices[tet_id, 2]]
v3_prev = pos_prev[tet_indices[tet_id, 3]]
Ds_dot = (
wp.matrix_from_cols(
(v1 - v1_prev) - (v0 - v0_prev),
(v2 - v2_prev) - (v0 - v0_prev),
(v3 - v3_prev) - (v0 - v0_prev),
)
* inv_dt
)
F_dot = Ds_dot * Dm_inv
f_dot = vec9(
F_dot[0, 0],
F_dot[1, 0],
F_dot[2, 0],
F_dot[0, 1],
F_dot[1, 1],
F_dot[2, 1],
F_dot[0, 2],
F_dot[1, 2],
F_dot[2, 2],
)
P_damp = damping * (H * f_dot)
f_damp = wp.vec3(
-(P_damp[0] * m[0] + P_damp[3] * m[1] + P_damp[6] * m[2]),
-(P_damp[1] * m[0] + P_damp[4] * m[1] + P_damp[7] * m[2]),
-(P_damp[2] * m[0] + P_damp[5] * m[1] + P_damp[8] * m[2]),
)
force = force + f_damp
hessian = hessian * (1.0 + damping * inv_dt)
return force, hessian
# ============ Helper Functions ============
@wp.func
def compute_G_matrix(Dm_inv: wp.mat33, v_order: int) -> mat93:
"""G_i = ∂vec(F)/∂x_i"""
if v_order == 0:
m = wp.vec3(
-(Dm_inv[0, 0] + Dm_inv[1, 0] + Dm_inv[2, 0]),
-(Dm_inv[0, 1] + Dm_inv[1, 1] + Dm_inv[2, 1]),
-(Dm_inv[0, 2] + Dm_inv[1, 2] + Dm_inv[2, 2]),
)
elif v_order == 1:
m = wp.vec3(Dm_inv[0, 0], Dm_inv[0, 1], Dm_inv[0, 2])
elif v_order == 2:
m = wp.vec3(Dm_inv[1, 0], Dm_inv[1, 1], Dm_inv[1, 2])
else:
m = wp.vec3(Dm_inv[2, 0], Dm_inv[2, 1], Dm_inv[2, 2])
# G = [m[0]*I₃, m[1]*I₃, m[2]*I₃]ᵀ (stacked vertically)
return mat93(
m[0],
0.0,
0.0,
0.0,
m[0],
0.0,
0.0,
0.0,
m[0],
m[1],
0.0,
0.0,
0.0,
m[1],
0.0,
0.0,
0.0,
m[1],
m[2],
0.0,
0.0,
0.0,
m[2],
0.0,
0.0,
0.0,
m[2],
)
@wp.func
def compute_cofactor(F: wp.mat33) -> wp.mat33:
"""Compute the cofactor (adjugate) matrix directly without using inverse.
This is numerically stable even when det(F) ≈ 0, unlike J * transpose(inverse(F)).
"""
F11, F21, F31 = F[0, 0], F[1, 0], F[2, 0]
F12, F22, F32 = F[0, 1], F[1, 1], F[2, 1]
F13, F23, F33 = F[0, 2], F[1, 2], F[2, 2]
return wp.mat33(
F22 * F33 - F23 * F32,
F23 * F31 - F21 * F33,
F21 * F32 - F22 * F31,
F13 * F32 - F12 * F33,
F11 * F33 - F13 * F31,
F12 * F31 - F11 * F32,
F12 * F23 - F13 * F22,
F13 * F21 - F11 * F23,
F11 * F22 - F12 * F21,
)
@wp.func
def compute_cofactor_derivative(F: wp.mat33, scale: float) -> mat99:
"""scale * ∂cof(F)/∂F"""
F11, F21, F31 = F[0, 0], F[1, 0], F[2, 0]
F12, F22, F32 = F[0, 1], F[1, 1], F[2, 1]
F13, F23, F33 = F[0, 2], F[1, 2], F[2, 2]
return mat99(
0.0,
0.0,
0.0,
0.0,
scale * F33,
-scale * F23,
0.0,
-scale * F32,
scale * F22,
0.0,
0.0,
0.0,
-scale * F33,
0.0,
scale * F13,
scale * F32,
0.0,
-scale * F12,
0.0,
0.0,
0.0,
scale * F23,
-scale * F13,
0.0,
-scale * F22,
scale * F12,
0.0,
0.0,
-scale * F33,
scale * F23,
0.0,
0.0,
0.0,
0.0,
scale * F31,
-scale * F21,
scale * F33,
0.0,
-scale * F13,
0.0,
0.0,
0.0,
-scale * F31,
0.0,
scale * F11,
-scale * F23,
scale * F13,
0.0,
0.0,
0.0,
0.0,
scale * F21,
-scale * F11,
0.0,
0.0,
scale * F32,
-scale * F22,
0.0,
-scale * F31,
scale * F21,
0.0,
0.0,
0.0,
-scale * F32,
0.0,
scale * F12,
scale * F31,
0.0,
-scale * F11,
0.0,
0.0,
0.0,
scale * F22,
-scale * F12,
0.0,
-scale * F21,
scale * F11,
0.0,
0.0,
0.0,
0.0,
)
@wp.kernel
def _count_num_adjacent_edges(edges_array: wp.array2d[wp.int32], num_vertex_adjacent_edges: wp.array[wp.int32]):
for edge_id in range(edges_array.shape[0]):
o0 = edges_array[edge_id, 0]
o1 = edges_array[edge_id, 1]
v0 = edges_array[edge_id, 2]
v1 = edges_array[edge_id, 3]
num_vertex_adjacent_edges[v0] = num_vertex_adjacent_edges[v0] + 1
num_vertex_adjacent_edges[v1] = num_vertex_adjacent_edges[v1] + 1
if o0 != -1:
num_vertex_adjacent_edges[o0] = num_vertex_adjacent_edges[o0] + 1
if o1 != -1:
num_vertex_adjacent_edges[o1] = num_vertex_adjacent_edges[o1] + 1
@wp.kernel
def _fill_adjacent_edges(
edges_array: wp.array2d[wp.int32],
vertex_adjacent_edges_offsets: wp.array[wp.int32],
vertex_adjacent_edges_fill_count: wp.array[wp.int32],
vertex_adjacent_edges: wp.array[wp.int32],
):
for edge_id in range(edges_array.shape[0]):
v0 = edges_array[edge_id, 2]
v1 = edges_array[edge_id, 3]
fill_count_v0 = vertex_adjacent_edges_fill_count[v0]
buffer_offset_v0 = vertex_adjacent_edges_offsets[v0]
vertex_adjacent_edges[buffer_offset_v0 + fill_count_v0 * 2] = edge_id
vertex_adjacent_edges[buffer_offset_v0 + fill_count_v0 * 2 + 1] = 2
vertex_adjacent_edges_fill_count[v0] = fill_count_v0 + 1
fill_count_v1 = vertex_adjacent_edges_fill_count[v1]
buffer_offset_v1 = vertex_adjacent_edges_offsets[v1]
vertex_adjacent_edges[buffer_offset_v1 + fill_count_v1 * 2] = edge_id
vertex_adjacent_edges[buffer_offset_v1 + fill_count_v1 * 2 + 1] = 3
vertex_adjacent_edges_fill_count[v1] = fill_count_v1 + 1
o0 = edges_array[edge_id, 0]
if o0 != -1:
fill_count_o0 = vertex_adjacent_edges_fill_count[o0]
buffer_offset_o0 = vertex_adjacent_edges_offsets[o0]
vertex_adjacent_edges[buffer_offset_o0 + fill_count_o0 * 2] = edge_id
vertex_adjacent_edges[buffer_offset_o0 + fill_count_o0 * 2 + 1] = 0
vertex_adjacent_edges_fill_count[o0] = fill_count_o0 + 1
o1 = edges_array[edge_id, 1]
if o1 != -1:
fill_count_o1 = vertex_adjacent_edges_fill_count[o1]
buffer_offset_o1 = vertex_adjacent_edges_offsets[o1]
vertex_adjacent_edges[buffer_offset_o1 + fill_count_o1 * 2] = edge_id
vertex_adjacent_edges[buffer_offset_o1 + fill_count_o1 * 2 + 1] = 1
vertex_adjacent_edges_fill_count[o1] = fill_count_o1 + 1
@wp.kernel
def _count_num_adjacent_faces(face_indices: wp.array2d[wp.int32], num_vertex_adjacent_faces: wp.array[wp.int32]):
for face in range(face_indices.shape[0]):
v0 = face_indices[face, 0]
v1 = face_indices[face, 1]
v2 = face_indices[face, 2]
num_vertex_adjacent_faces[v0] = num_vertex_adjacent_faces[v0] + 1
num_vertex_adjacent_faces[v1] = num_vertex_adjacent_faces[v1] + 1
num_vertex_adjacent_faces[v2] = num_vertex_adjacent_faces[v2] + 1
@wp.kernel
def _fill_adjacent_faces(
face_indices: wp.array2d[wp.int32],
vertex_adjacent_faces_offsets: wp.array[wp.int32],
vertex_adjacent_faces_fill_count: wp.array[wp.int32],
vertex_adjacent_faces: wp.array[wp.int32],
):
for face in range(face_indices.shape[0]):
v0 = face_indices[face, 0]
v1 = face_indices[face, 1]
v2 = face_indices[face, 2]
fill_count_v0 = vertex_adjacent_faces_fill_count[v0]
buffer_offset_v0 = vertex_adjacent_faces_offsets[v0]
vertex_adjacent_faces[buffer_offset_v0 + fill_count_v0 * 2] = face
vertex_adjacent_faces[buffer_offset_v0 + fill_count_v0 * 2 + 1] = 0
vertex_adjacent_faces_fill_count[v0] = fill_count_v0 + 1
fill_count_v1 = vertex_adjacent_faces_fill_count[v1]
buffer_offset_v1 = vertex_adjacent_faces_offsets[v1]
vertex_adjacent_faces[buffer_offset_v1 + fill_count_v1 * 2] = face
vertex_adjacent_faces[buffer_offset_v1 + fill_count_v1 * 2 + 1] = 1
vertex_adjacent_faces_fill_count[v1] = fill_count_v1 + 1
fill_count_v2 = vertex_adjacent_faces_fill_count[v2]
buffer_offset_v2 = vertex_adjacent_faces_offsets[v2]
vertex_adjacent_faces[buffer_offset_v2 + fill_count_v2 * 2] = face
vertex_adjacent_faces[buffer_offset_v2 + fill_count_v2 * 2 + 1] = 2
vertex_adjacent_faces_fill_count[v2] = fill_count_v2 + 1
@wp.kernel
def _count_num_adjacent_springs(springs_array: wp.array[wp.int32], num_vertex_adjacent_springs: wp.array[wp.int32]):
num_springs = springs_array.shape[0] / 2
for spring_id in range(num_springs):
v0 = springs_array[spring_id * 2]
v1 = springs_array[spring_id * 2 + 1]
num_vertex_adjacent_springs[v0] = num_vertex_adjacent_springs[v0] + 1
num_vertex_adjacent_springs[v1] = num_vertex_adjacent_springs[v1] + 1
@wp.kernel
def _fill_adjacent_springs(
springs_array: wp.array[wp.int32],
vertex_adjacent_springs_offsets: wp.array[wp.int32],
vertex_adjacent_springs_fill_count: wp.array[wp.int32],
vertex_adjacent_springs: wp.array[wp.int32],
):
num_springs = springs_array.shape[0] / 2
for spring_id in range(num_springs):
v0 = springs_array[spring_id * 2]
v1 = springs_array[spring_id * 2 + 1]
fill_count_v0 = vertex_adjacent_springs_fill_count[v0]
buffer_offset_v0 = vertex_adjacent_springs_offsets[v0]
vertex_adjacent_springs[buffer_offset_v0 + fill_count_v0] = spring_id
vertex_adjacent_springs_fill_count[v0] = fill_count_v0 + 1
fill_count_v1 = vertex_adjacent_springs_fill_count[v1]
buffer_offset_v1 = vertex_adjacent_springs_offsets[v1]
vertex_adjacent_springs[buffer_offset_v1 + fill_count_v1] = spring_id
vertex_adjacent_springs_fill_count[v1] = fill_count_v1 + 1
@wp.kernel
def _count_num_adjacent_tets(tet_indices: wp.array2d[wp.int32], num_vertex_adjacent_tets: wp.array[wp.int32]):
for tet in range(tet_indices.shape[0]):
v0 = tet_indices[tet, 0]
v1 = tet_indices[tet, 1]
v2 = tet_indices[tet, 2]
v3 = tet_indices[tet, 3]
num_vertex_adjacent_tets[v0] = num_vertex_adjacent_tets[v0] + 1
num_vertex_adjacent_tets[v1] = num_vertex_adjacent_tets[v1] + 1
num_vertex_adjacent_tets[v2] = num_vertex_adjacent_tets[v2] + 1
num_vertex_adjacent_tets[v3] = num_vertex_adjacent_tets[v3] + 1
@wp.kernel
def _fill_adjacent_tets(
tet_indices: wp.array2d[wp.int32],
vertex_adjacent_tets_offsets: wp.array[wp.int32],
vertex_adjacent_tets_fill_count: wp.array[wp.int32],
vertex_adjacent_tets: wp.array[wp.int32],
):
for tet in range(tet_indices.shape[0]):
v0 = tet_indices[tet, 0]
v1 = tet_indices[tet, 1]
v2 = tet_indices[tet, 2]
v3 = tet_indices[tet, 3]
fill_count_v0 = vertex_adjacent_tets_fill_count[v0]
buffer_offset_v0 = vertex_adjacent_tets_offsets[v0]
vertex_adjacent_tets[buffer_offset_v0 + fill_count_v0 * 2] = tet
vertex_adjacent_tets[buffer_offset_v0 + fill_count_v0 * 2 + 1] = 0
vertex_adjacent_tets_fill_count[v0] = fill_count_v0 + 1
fill_count_v1 = vertex_adjacent_tets_fill_count[v1]
buffer_offset_v1 = vertex_adjacent_tets_offsets[v1]
vertex_adjacent_tets[buffer_offset_v1 + fill_count_v1 * 2] = tet
vertex_adjacent_tets[buffer_offset_v1 + fill_count_v1 * 2 + 1] = 1
vertex_adjacent_tets_fill_count[v1] = fill_count_v1 + 1
fill_count_v2 = vertex_adjacent_tets_fill_count[v2]
buffer_offset_v2 = vertex_adjacent_tets_offsets[v2]
vertex_adjacent_tets[buffer_offset_v2 + fill_count_v2 * 2] = tet
vertex_adjacent_tets[buffer_offset_v2 + fill_count_v2 * 2 + 1] = 2
vertex_adjacent_tets_fill_count[v2] = fill_count_v2 + 1
fill_count_v3 = vertex_adjacent_tets_fill_count[v3]
buffer_offset_v3 = vertex_adjacent_tets_offsets[v3]
vertex_adjacent_tets[buffer_offset_v3 + fill_count_v3 * 2] = tet
vertex_adjacent_tets[buffer_offset_v3 + fill_count_v3 * 2 + 1] = 3
vertex_adjacent_tets_fill_count[v3] = fill_count_v3 + 1
@wp.kernel
def _test_compute_force_element_adjacency(
adjacency: ParticleForceElementAdjacencyInfo,
edge_indices: wp.array2d[wp.int32],
face_indices: wp.array2d[wp.int32],
):
wp.printf("num vertices: %d\n", adjacency.v_adj_edges_offsets.shape[0] - 1)
for vertex in range(adjacency.v_adj_edges_offsets.shape[0] - 1):
num_adj_edges = get_vertex_num_adjacent_edges(adjacency, vertex)
for i_bd in range(num_adj_edges):
bd_id, v_order = get_vertex_adjacent_edge_id_order(adjacency, vertex, i_bd)
if edge_indices[bd_id, v_order] != vertex:
print("Error!!!")
wp.printf("vertex: %d | num_adj_edges: %d\n", vertex, num_adj_edges)
wp.printf("--iBd: %d | ", i_bd)
wp.printf("edge id: %d | v_order: %d\n", bd_id, v_order)
num_adj_faces = get_vertex_num_adjacent_faces(adjacency, vertex)
for i_face in range(num_adj_faces):
face, v_order = get_vertex_adjacent_face_id_order(
adjacency,
vertex,
i_face,
)
if face_indices[face, v_order] != vertex:
print("Error!!!")
wp.printf("vertex: %d | num_adj_faces: %d\n", vertex, num_adj_faces)
wp.printf("--i_face: %d | face id: %d | v_order: %d\n", i_face, face, v_order)
wp.printf(
"--face: %d %d %d\n",
face_indices[face, 0],
face_indices[face, 1],
face_indices[face, 2],
)
@wp.func
def evaluate_stvk_force_hessian(
face: int,
v_order: int,
pos: wp.array[wp.vec3],
pos_anchor: wp.array[wp.vec3],
tri_indices: wp.array2d[wp.int32],
tri_pose: wp.mat22,
area: float,
mu: float,
lmbd: float,
damping: float,
dt: float,
):
# StVK energy density: psi = mu * ||G||_F^2 + 0.5 * lambda * (trace(G))^2
# Deformation gradient F = [f0, f1] (3x2 matrix as two 3D column vectors)
v0 = tri_indices[face, 0]
v1 = tri_indices[face, 1]
v2 = tri_indices[face, 2]
x0 = pos[v0]
x01 = pos[v1] - x0
x02 = pos[v2] - x0
# Cache tri_pose elements
DmInv00 = tri_pose[0, 0]
DmInv01 = tri_pose[0, 1]
DmInv10 = tri_pose[1, 0]
DmInv11 = tri_pose[1, 1]
# Compute F columns directly: F = [x01, x02] * tri_pose = [f0, f1]
f0 = x01 * DmInv00 + x02 * DmInv10
f1 = x01 * DmInv01 + x02 * DmInv11
# Green strain tensor: G = 0.5(F^T F - I) = [[G00, G01], [G01, G11]] (symmetric 2x2)
f0_dot_f0 = wp.dot(f0, f0)
f1_dot_f1 = wp.dot(f1, f1)
f0_dot_f1 = wp.dot(f0, f1)
G00 = 0.5 * (f0_dot_f0 - 1.0)
G11 = 0.5 * (f1_dot_f1 - 1.0)
G01 = 0.5 * f0_dot_f1
# Frobenius norm squared of Green strain: ||G||_F^2 = G00^2 + G11^2 + 2 * G01^2
G_frobenius_sq = G00 * G00 + G11 * G11 + 2.0 * G01 * G01
if G_frobenius_sq < 1.0e-20:
return wp.vec3(0.0), wp.mat33(0.0)
trace_G = G00 + G11
# First Piola-Kirchhoff stress tensor (StVK model)
# PK1 = 2*mu*F*G + lambda*trace(G)*F = [PK1_col0, PK1_col1] (3x2)
lambda_trace_G = lmbd * trace_G
two_mu = 2.0 * mu
PK1_col0 = f0 * (two_mu * G00 + lambda_trace_G) + f1 * (two_mu * G01)
PK1_col1 = f0 * (two_mu * G01) + f1 * (two_mu * G11 + lambda_trace_G)
# Vertex selection using masks to avoid branching
mask0 = float(v_order == 0)
mask1 = float(v_order == 1)
mask2 = float(v_order == 2)
# Deformation gradient derivatives w.r.t. current vertex position
df0_dx = DmInv00 * (mask1 - mask0) + DmInv10 * (mask2 - mask0)
df1_dx = DmInv01 * (mask1 - mask0) + DmInv11 * (mask2 - mask0)
# Force via chain rule: force = -(dpsi/dF) : (dF/dx)
dpsi_dx = PK1_col0 * df0_dx + PK1_col1 * df1_dx
force = -dpsi_dx
# Hessian computation using Cauchy-Green invariants
df0_dx_sq = df0_dx * df0_dx
df1_dx_sq = df1_dx * df1_dx
df0_df1_cross = df0_dx * df1_dx
Ic = f0_dot_f0 + f1_dot_f1
two_dpsi_dIc = -mu + (0.5 * Ic - 1.0) * lmbd
I33 = wp.identity(n=3, dtype=float)
f0_outer_f0 = wp.outer(f0, f0)
f1_outer_f1 = wp.outer(f1, f1)
f0_outer_f1 = wp.outer(f0, f1)
f1_outer_f0 = wp.outer(f1, f0)
H_IIc00_scaled = mu * (f0_dot_f0 * I33 + 2.0 * f0_outer_f0 + f1_outer_f1)
H_IIc11_scaled = mu * (f1_dot_f1 * I33 + 2.0 * f1_outer_f1 + f0_outer_f0)
H_IIc01_scaled = mu * (f0_dot_f1 * I33 + f1_outer_f0)
# d2(psi)/dF^2 components
d2E_dF2_00 = lmbd * f0_outer_f0 + two_dpsi_dIc * I33 + H_IIc00_scaled
d2E_dF2_01 = lmbd * f0_outer_f1 + H_IIc01_scaled
d2E_dF2_11 = lmbd * f1_outer_f1 + two_dpsi_dIc * I33 + H_IIc11_scaled
# Chain rule: H = (dF/dx)^T * (d2(psi)/dF^2) * (dF/dx)
hessian = df0_dx_sq * d2E_dF2_00 + df1_dx_sq * d2E_dF2_11 + df0_df1_cross * (d2E_dF2_01 + wp.transpose(d2E_dF2_01))
if damping > 0.0:
inv_dt = 1.0 / dt
# Previous deformation gradient for velocity
x0_prev = pos_anchor[v0]
x01_prev = pos_anchor[v1] - x0_prev
x02_prev = pos_anchor[v2] - x0_prev
vel_x01 = (x01 - x01_prev) * inv_dt
vel_x02 = (x02 - x02_prev) * inv_dt
df0_dt = vel_x01 * DmInv00 + vel_x02 * DmInv10
df1_dt = vel_x01 * DmInv01 + vel_x02 * DmInv11
# First constraint: Cmu = ||G||_F (Frobenius norm of Green strain)
Cmu = wp.sqrt(G_frobenius_sq)
G00_normalized = G00 / Cmu
G01_normalized = G01 / Cmu
G11_normalized = G11 / Cmu
# Time derivative of Green strain: dG/dt = 0.5 * (F^T * dF/dt + (dF/dt)^T * F)
dG_dt_00 = wp.dot(f0, df0_dt) # dG00/dt
dG_dt_11 = wp.dot(f1, df1_dt) # dG11/dt
dG_dt_01 = 0.5 * (wp.dot(f0, df1_dt) + wp.dot(f1, df0_dt)) # dG01/dt
# Time derivative of first constraint: dCmu/dt = (1/||G||_F) * (G : dG/dt)
dCmu_dt = G00_normalized * dG_dt_00 + G11_normalized * dG_dt_11 + 2.0 * G01_normalized * dG_dt_01
# Gradient of first constraint w.r.t. deformation gradient: dCmu/dF = (G/||G||_F) * F
dCmu_dF_col0 = G00_normalized * f0 + G01_normalized * f1 # dCmu/df0
dCmu_dF_col1 = G01_normalized * f0 + G11_normalized * f1 # dCmu/df1
# Gradient of constraint w.r.t. vertex position: dCmu/dx = (dCmu/dF) : (dF/dx)
dCmu_dx = df0_dx * dCmu_dF_col0 + df1_dx * dCmu_dF_col1
# Damping force from first constraint: -mu * damping * (dCmu/dt) * (dCmu/dx)
kd_mu = mu * damping
force += -kd_mu * dCmu_dt * dCmu_dx
# Damping Hessian: mu * damping * (1/dt) * (dCmu/dx) x (dCmu/dx)
hessian += kd_mu * inv_dt * wp.outer(dCmu_dx, dCmu_dx)
# Second constraint: Clmbd = trace(G) = G00 + G11 (trace of Green strain)
# Time derivative of second constraint: dClmbd/dt = trace(dG/dt)
dClmbd_dt = dG_dt_00 + dG_dt_11
# Gradient of second constraint w.r.t. deformation gradient: dClmbd/dF = F
dClmbd_dF_col0 = f0 # dClmbd/df0
dClmbd_dF_col1 = f1 # dClmbd/df1
# Gradient of Clmbd w.r.t. vertex position: dClmbd/dx = (dClmbd/dF) : (dF/dx)
dClmbd_dx = df0_dx * dClmbd_dF_col0 + df1_dx * dClmbd_dF_col1
# Damping force from second constraint: -lambda * damping * (dClmbd/dt) * (dClmbd/dx)
kd_lmbd = lmbd * damping
force += -kd_lmbd * dClmbd_dt * dClmbd_dx
# Damping Hessian from second constraint: lambda * damping * (1/dt) * (dClmbd/dx) x (dClmbd/dx)
hessian += kd_lmbd * inv_dt * wp.outer(dClmbd_dx, dClmbd_dx)
# Apply area scaling
force *= area
hessian *= area
return force, hessian
@wp.func
def compute_normalized_vector_derivative(
unnormalized_vec_length: float, normalized_vec: wp.vec3, unnormalized_vec_derivative: wp.mat33
) -> wp.mat33:
projection_matrix = wp.identity(n=3, dtype=float) - wp.outer(normalized_vec, normalized_vec)
# d(normalized_vec)/dx = (1/|unnormalized_vec|) * (I - normalized_vec * normalized_vec^T) * d(unnormalized_vec)/dx
return (1.0 / unnormalized_vec_length) * projection_matrix * unnormalized_vec_derivative
@wp.func
def compute_angle_derivative(
n1_hat: wp.vec3,
n2_hat: wp.vec3,
e_hat: wp.vec3,
dn1hat_dx: wp.mat33,
dn2hat_dx: wp.mat33,
sin_theta: float,
cos_theta: float,
skew_n1: wp.mat33,
skew_n2: wp.mat33,
) -> wp.vec3:
dsin_dx = wp.transpose(skew_n1 * dn2hat_dx - skew_n2 * dn1hat_dx) * e_hat
dcos_dx = wp.transpose(dn1hat_dx) * n2_hat + wp.transpose(dn2hat_dx) * n1_hat
# dtheta/dx = dsin/dx * cos - dcos/dx * sin
return dsin_dx * cos_theta - dcos_dx * sin_theta
@wp.func
def evaluate_dihedral_angle_based_bending_force_hessian(
bending_index: int,
v_order: int,
pos: wp.array[wp.vec3],
pos_anchor: wp.array[wp.vec3],
edge_indices: wp.array2d[wp.int32],
edge_rest_angle: wp.array[float],
edge_rest_length: wp.array[float],
stiffness: float,
damping: float,
dt: float,
):
# Skip invalid edges (boundary edges with missing opposite vertices)
if edge_indices[bending_index, 0] == -1 or edge_indices[bending_index, 1] == -1:
return wp.vec3(0.0), wp.mat33(0.0)
eps = 1.0e-6
vi0 = edge_indices[bending_index, 0]
vi1 = edge_indices[bending_index, 1]
vi2 = edge_indices[bending_index, 2]
vi3 = edge_indices[bending_index, 3]
x0 = pos[vi0] # opposite 0
x1 = pos[vi1] # opposite 1
x2 = pos[vi2] # edge start
x3 = pos[vi3] # edge end
# Compute edge vectors
x02 = x2 - x0
x03 = x3 - x0
x13 = x3 - x1
x12 = x2 - x1
e = x3 - x2
# Compute normals
n1 = wp.cross(x02, x03)
n2 = wp.cross(x13, x12)
n1_norm = wp.length(n1)
n2_norm = wp.length(n2)
e_norm = wp.length(e)
# Early exit for degenerate cases
if n1_norm < eps or n2_norm < eps or e_norm < eps:
return wp.vec3(0.0), wp.mat33(0.0)
n1_hat = n1 / n1_norm
n2_hat = n2 / n2_norm
e_hat = e / e_norm
sin_theta = wp.dot(wp.cross(n1_hat, n2_hat), e_hat)
cos_theta = wp.dot(n1_hat, n2_hat)
theta = wp.atan2(sin_theta, cos_theta)
k = stiffness * edge_rest_length[bending_index]
dE_dtheta = k * (theta - edge_rest_angle[bending_index])
# Pre-compute skew matrices (shared across all angle derivative computations)
skew_e = wp.skew(e)
skew_x03 = wp.skew(x03)
skew_x02 = wp.skew(x02)
skew_x13 = wp.skew(x13)
skew_x12 = wp.skew(x12)
skew_n1 = wp.skew(n1_hat)
skew_n2 = wp.skew(n2_hat)
# Compute the derivatives of unit normals with respect to each vertex; required for computing angle derivatives
dn1hat_dx0 = compute_normalized_vector_derivative(n1_norm, n1_hat, skew_e)
dn2hat_dx0 = wp.mat33(0.0)
dn1hat_dx1 = wp.mat33(0.0)
dn2hat_dx1 = compute_normalized_vector_derivative(n2_norm, n2_hat, -skew_e)
dn1hat_dx2 = compute_normalized_vector_derivative(n1_norm, n1_hat, -skew_x03)
dn2hat_dx2 = compute_normalized_vector_derivative(n2_norm, n2_hat, skew_x13)
dn1hat_dx3 = compute_normalized_vector_derivative(n1_norm, n1_hat, skew_x02)
dn2hat_dx3 = compute_normalized_vector_derivative(n2_norm, n2_hat, -skew_x12)
# Compute all angle derivatives (required for damping)
dtheta_dx0 = compute_angle_derivative(
n1_hat, n2_hat, e_hat, dn1hat_dx0, dn2hat_dx0, sin_theta, cos_theta, skew_n1, skew_n2
)
dtheta_dx1 = compute_angle_derivative(
n1_hat, n2_hat, e_hat, dn1hat_dx1, dn2hat_dx1, sin_theta, cos_theta, skew_n1, skew_n2
)
dtheta_dx2 = compute_angle_derivative(
n1_hat, n2_hat, e_hat, dn1hat_dx2, dn2hat_dx2, sin_theta, cos_theta, skew_n1, skew_n2
)
dtheta_dx3 = compute_angle_derivative(
n1_hat, n2_hat, e_hat, dn1hat_dx3, dn2hat_dx3, sin_theta, cos_theta, skew_n1, skew_n2
)
# Use float masks for branch-free selection
mask0 = float(v_order == 0)
mask1 = float(v_order == 1)
mask2 = float(v_order == 2)
mask3 = float(v_order == 3)
# Select the derivative for the current vertex without branching
dtheta_dx = dtheta_dx0 * mask0 + dtheta_dx1 * mask1 + dtheta_dx2 * mask2 + dtheta_dx3 * mask3
# Compute elastic force and hessian
bending_force = -dE_dtheta * dtheta_dx
bending_hessian = k * wp.outer(dtheta_dx, dtheta_dx)
if damping > 0.0:
inv_dt = 1.0 / dt
x_prev0 = pos_anchor[vi0]
x_prev1 = pos_anchor[vi1]
x_prev2 = pos_anchor[vi2]
x_prev3 = pos_anchor[vi3]
# Compute displacement vectors
dx0 = x0 - x_prev0
dx1 = x1 - x_prev1
dx2 = x2 - x_prev2
dx3 = x3 - x_prev3
# Compute angular velocity using all derivatives
dtheta_dt = (
wp.dot(dtheta_dx0, dx0) + wp.dot(dtheta_dx1, dx1) + wp.dot(dtheta_dx2, dx2) + wp.dot(dtheta_dx3, dx3)
) * inv_dt
damping_coeff = damping * k # damping coefficients following the VBD convention
damping_force = -damping_coeff * dtheta_dt * dtheta_dx
damping_hessian = damping_coeff * inv_dt * wp.outer(dtheta_dx, dtheta_dx)
bending_force = bending_force + damping_force
bending_hessian = bending_hessian + damping_hessian
return bending_force, bending_hessian
@wp.func
def evaluate_self_contact_force_norm(dis: float, collision_radius: float, k: float):
# Adjust distance and calculate penetration depth
penetration_depth = collision_radius - dis
# Initialize outputs
dEdD = wp.float32(0.0)
d2E_dDdD = wp.float32(0.0)
# C2 continuity calculation
tau = collision_radius * 0.5
d_min = 1.0e-5
if tau > dis > d_min:
# Log-barrier region: E ∝ -ln(dis)
k2 = tau * tau * k
dEdD = -k2 / dis
d2E_dDdD = k2 / (dis * dis)
elif dis <= d_min:
# Quadratic extension below d_min (Taylor of the log-barrier at d_min)
# preserving C2 continuity: constant Hessian, linear gradient
k2 = tau * tau * k
d_min_sq = d_min * d_min
dEdD = k2 * (dis - 2.0 * d_min) / d_min_sq
d2E_dDdD = k2 / d_min_sq
else:
dEdD = -k * penetration_depth
d2E_dDdD = k
return dEdD, d2E_dDdD
@wp.func
def damp_collision(
displacement: wp.vec3,
collision_normal: wp.vec3,
collision_hessian: wp.mat33,
collision_damping: float,
dt: float,
):
if wp.dot(displacement, collision_normal) > 0:
damping_hessian = (collision_damping / dt) * collision_hessian
damping_force = damping_hessian * displacement
return damping_force, damping_hessian
else:
return wp.vec3(0.0), wp.mat33(0.0)
@wp.func
def evaluate_edge_edge_contact(
v: int,
v_order: int,
e1: int,
e2: int,
pos: wp.array[wp.vec3],
pos_anchor: wp.array[wp.vec3],
edge_indices: wp.array2d[wp.int32],
collision_radius: float,
collision_stiffness: float,
collision_damping: float,
friction_coefficient: float,
friction_epsilon: float,
dt: float,
edge_edge_parallel_epsilon: float,
):
r"""
Returns the edge-edge contact force and hessian, including the friction force.
Args:
v:
v_order: \in {0, 1, 2, 3}, 0, 1 is vertex 0, 1 of e1, 2,3 is vertex 0, 1 of e2
e0
e1
pos
pos_anchor,
edge_indices
collision_radius
collision_stiffness
dt
edge_edge_parallel_epsilon: threshold to determine whether 2 edges are parallel
"""
e1_v1 = edge_indices[e1, 2]
e1_v2 = edge_indices[e1, 3]
e1_v1_pos = pos[e1_v1]
e1_v2_pos = pos[e1_v2]
e2_v1 = edge_indices[e2, 2]
e2_v2 = edge_indices[e2, 3]
e2_v1_pos = pos[e2_v1]
e2_v2_pos = pos[e2_v2]
st = wp.closest_point_edge_edge(e1_v1_pos, e1_v2_pos, e2_v1_pos, e2_v2_pos, edge_edge_parallel_epsilon)
s = st[0]
t = st[1]
e1_vec = e1_v2_pos - e1_v1_pos
e2_vec = e2_v2_pos - e2_v1_pos
c1 = e1_v1_pos + e1_vec * s
c2 = e2_v1_pos + e2_vec * t
# c1, c2, s, t = closest_point_edge_edge_2(e1_v1_pos, e1_v2_pos, e2_v1_pos, e2_v2_pos)
diff = c1 - c2
dis = st[2]
collision_normal = diff / dis
if dis < collision_radius:
bs = wp.vec4(1.0 - s, s, -1.0 + t, -t)
v_bary = bs[v_order]
dEdD, d2E_dDdD = evaluate_self_contact_force_norm(dis, collision_radius, collision_stiffness)
collision_force = -dEdD * v_bary * collision_normal
collision_hessian = d2E_dDdD * v_bary * v_bary * wp.outer(collision_normal, collision_normal)
# friction
c1_prev = pos_anchor[e1_v1] + (pos_anchor[e1_v2] - pos_anchor[e1_v1]) * s
c2_prev = pos_anchor[e2_v1] + (pos_anchor[e2_v2] - pos_anchor[e2_v1]) * t
dx = (c1 - c1_prev) - (c2 - c2_prev)
axis_1, axis_2 = orthonormal_basis(collision_normal)
T = mat32(
axis_1[0],
axis_2[0],
axis_1[1],
axis_2[1],
axis_1[2],
axis_2[2],
)
u = wp.transpose(T) * dx
eps_U = friction_epsilon * dt
# fmt: off
if wp.static("contact_force_hessian_ee" in VBD_DEBUG_PRINTING_OPTIONS):
wp.printf(
" collision force:\n %f %f %f,\n collision hessian:\n %f %f %f,\n %f %f %f,\n %f %f %f\n",
collision_force[0], collision_force[1], collision_force[2], collision_hessian[0, 0], collision_hessian[0, 1], collision_hessian[0, 2], collision_hessian[1, 0], collision_hessian[1, 1], collision_hessian[1, 2], collision_hessian[2, 0], collision_hessian[2, 1], collision_hessian[2, 2],
)
# fmt: on
friction_force, friction_hessian = compute_friction(friction_coefficient, -dEdD, T, u, eps_U)
friction_force = friction_force * v_bary
friction_hessian = friction_hessian * v_bary * v_bary
# # fmt: off
# if wp.static("contact_force_hessian_ee" in VBD_DEBUG_PRINTING_OPTIONS):
# wp.printf(
# " friction force:\n %f %f %f,\n friction hessian:\n %f %f %f,\n %f %f %f,\n %f %f %f\n",
# friction_force[0], friction_force[1], friction_force[2], friction_hessian[0, 0], friction_hessian[0, 1], friction_hessian[0, 2], friction_hessian[1, 0], friction_hessian[1, 1], friction_hessian[1, 2], friction_hessian[2, 0], friction_hessian[2, 1], friction_hessian[2, 2],
# )
# # fmt: on
if v_order == 0:
displacement = pos_anchor[e1_v1] - e1_v1_pos
elif v_order == 1:
displacement = pos_anchor[e1_v2] - e1_v2_pos
elif v_order == 2:
displacement = pos_anchor[e2_v1] - e2_v1_pos
else:
displacement = pos_anchor[e2_v2] - e2_v2_pos
collision_normal_sign = wp.vec4(1.0, 1.0, -1.0, -1.0)
if wp.dot(displacement, collision_normal * collision_normal_sign[v_order]) > 0:
damping_hessian = (collision_damping / dt) * collision_hessian
collision_hessian = collision_hessian + damping_hessian
collision_force = collision_force + damping_hessian * displacement
collision_force = collision_force + friction_force
collision_hessian = collision_hessian + friction_hessian
else:
collision_force = wp.vec3(0.0, 0.0, 0.0)
collision_hessian = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
return collision_force, collision_hessian
@wp.func
def evaluate_edge_edge_contact_2_vertices(
e1: int,
e2: int,
pos: wp.array[wp.vec3],
pos_anchor: wp.array[wp.vec3],
edge_indices: wp.array2d[wp.int32],
collision_radius: float,
collision_stiffness: float,
collision_damping: float,
friction_coefficient: float,
friction_epsilon: float,
dt: float,
edge_edge_parallel_epsilon: float,
):
r"""
Returns the edge-edge contact force and hessian, including the friction force.
Args:
v:
v_order: \in {0, 1, 2, 3}, 0, 1 is vertex 0, 1 of e1, 2,3 is vertex 0, 1 of e2
e0
e1
pos
edge_indices
collision_radius
collision_stiffness
dt
"""
e1_v1 = edge_indices[e1, 2]
e1_v2 = edge_indices[e1, 3]
e1_v1_pos = pos[e1_v1]
e1_v2_pos = pos[e1_v2]
e2_v1 = edge_indices[e2, 2]
e2_v2 = edge_indices[e2, 3]
e2_v1_pos = pos[e2_v1]
e2_v2_pos = pos[e2_v2]
st = wp.closest_point_edge_edge(e1_v1_pos, e1_v2_pos, e2_v1_pos, e2_v2_pos, edge_edge_parallel_epsilon)
s = st[0]
t = st[1]
e1_vec = e1_v2_pos - e1_v1_pos
e2_vec = e2_v2_pos - e2_v1_pos
c1 = e1_v1_pos + e1_vec * s
c2 = e2_v1_pos + e2_vec * t
# c1, c2, s, t = closest_point_edge_edge_2(e1_v1_pos, e1_v2_pos, e2_v1_pos, e2_v2_pos)
diff = c1 - c2
dis = st[2]
collision_normal = diff / dis
if 0.0 < dis < collision_radius:
bs = wp.vec4(1.0 - s, s, -1.0 + t, -t)
dEdD, d2E_dDdD = evaluate_self_contact_force_norm(dis, collision_radius, collision_stiffness)
collision_force = -dEdD * collision_normal
collision_hessian = d2E_dDdD * wp.outer(collision_normal, collision_normal)
# friction
c1_prev = pos_anchor[e1_v1] + (pos_anchor[e1_v2] - pos_anchor[e1_v1]) * s
c2_prev = pos_anchor[e2_v1] + (pos_anchor[e2_v2] - pos_anchor[e2_v1]) * t
dx = (c1 - c1_prev) - (c2 - c2_prev)
axis_1, axis_2 = orthonormal_basis(collision_normal)
T = mat32(
axis_1[0],
axis_2[0],
axis_1[1],
axis_2[1],
axis_1[2],
axis_2[2],
)
u = wp.transpose(T) * dx
eps_U = friction_epsilon * dt
# fmt: off
if wp.static("contact_force_hessian_ee" in VBD_DEBUG_PRINTING_OPTIONS):
wp.printf(
" collision force:\n %f %f %f,\n collision hessian:\n %f %f %f,\n %f %f %f,\n %f %f %f\n",
collision_force[0], collision_force[1], collision_force[2], collision_hessian[0, 0], collision_hessian[0, 1], collision_hessian[0, 2], collision_hessian[1, 0], collision_hessian[1, 1], collision_hessian[1, 2], collision_hessian[2, 0], collision_hessian[2, 1], collision_hessian[2, 2],
)
# fmt: on
friction_force, friction_hessian = compute_friction(friction_coefficient, -dEdD, T, u, eps_U)
# # fmt: off
# if wp.static("contact_force_hessian_ee" in VBD_DEBUG_PRINTING_OPTIONS):
# wp.printf(
# " friction force:\n %f %f %f,\n friction hessian:\n %f %f %f,\n %f %f %f,\n %f %f %f\n",
# friction_force[0], friction_force[1], friction_force[2], friction_hessian[0, 0], friction_hessian[0, 1], friction_hessian[0, 2], friction_hessian[1, 0], friction_hessian[1, 1], friction_hessian[1, 2], friction_hessian[2, 0], friction_hessian[2, 1], friction_hessian[2, 2],
# )
# # fmt: on
displacement_0 = pos_anchor[e1_v1] - e1_v1_pos
displacement_1 = pos_anchor[e1_v2] - e1_v2_pos
collision_force_0 = collision_force * bs[0]
collision_force_1 = collision_force * bs[1]
collision_hessian_0 = collision_hessian * bs[0] * bs[0]
collision_hessian_1 = collision_hessian * bs[1] * bs[1]
collision_normal_sign = wp.vec4(1.0, 1.0, -1.0, -1.0)
damping_force, damping_hessian = damp_collision(
displacement_0,
collision_normal * collision_normal_sign[0],
collision_hessian_0,
collision_damping,
dt,
)
collision_force_0 += damping_force + bs[0] * friction_force
collision_hessian_0 += damping_hessian + bs[0] * bs[0] * friction_hessian
damping_force, damping_hessian = damp_collision(
displacement_1,
collision_normal * collision_normal_sign[1],
collision_hessian_1,
collision_damping,
dt,
)
collision_force_1 += damping_force + bs[1] * friction_force
collision_hessian_1 += damping_hessian + bs[1] * bs[1] * friction_hessian
return True, collision_force_0, collision_force_1, collision_hessian_0, collision_hessian_1
else:
collision_force = wp.vec3(0.0, 0.0, 0.0)
collision_hessian = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
return False, collision_force, collision_force, collision_hessian, collision_hessian
@wp.func
def evaluate_vertex_triangle_collision_force_hessian(
v: int,
v_order: int,
tri: int,
pos: wp.array[wp.vec3],
pos_anchor: wp.array[wp.vec3],
tri_indices: wp.array2d[wp.int32],
collision_radius: float,
collision_stiffness: float,
collision_damping: float,
friction_coefficient: float,
friction_epsilon: float,
dt: float,
):
a = pos[tri_indices[tri, 0]]
b = pos[tri_indices[tri, 1]]
c = pos[tri_indices[tri, 2]]
p = pos[v]
closest_p, bary, _feature_type = triangle_closest_point(a, b, c, p)
diff = p - closest_p
dis = wp.length(diff)
collision_normal = diff / dis
if dis < collision_radius:
bs = wp.vec4(-bary[0], -bary[1], -bary[2], 1.0)
v_bary = bs[v_order]
dEdD, d2E_dDdD = evaluate_self_contact_force_norm(dis, collision_radius, collision_stiffness)
collision_force = -dEdD * v_bary * collision_normal
collision_hessian = d2E_dDdD * v_bary * v_bary * wp.outer(collision_normal, collision_normal)
# friction force
dx_v = p - pos_anchor[v]
closest_p_prev = (
bary[0] * pos_anchor[tri_indices[tri, 0]]
+ bary[1] * pos_anchor[tri_indices[tri, 1]]
+ bary[2] * pos_anchor[tri_indices[tri, 2]]
)
dx = dx_v - (closest_p - closest_p_prev)
e0, e1 = orthonormal_basis(collision_normal)
T = mat32(e0[0], e1[0], e0[1], e1[1], e0[2], e1[2])
u = wp.transpose(T) * dx
eps_U = friction_epsilon * dt
friction_force, friction_hessian = compute_friction(friction_coefficient, -dEdD, T, u, eps_U)
# fmt: off
if wp.static("contact_force_hessian_vt" in VBD_DEBUG_PRINTING_OPTIONS):
wp.printf(
"v: %d dEdD: %f\nnormal force: %f %f %f\nfriction force: %f %f %f\n",
v,
dEdD,
collision_force[0], collision_force[1], collision_force[2], friction_force[0], friction_force[1], friction_force[2],
)
# fmt: on
if v_order == 0:
displacement = pos_anchor[tri_indices[tri, 0]] - a
elif v_order == 1:
displacement = pos_anchor[tri_indices[tri, 1]] - b
elif v_order == 2:
displacement = pos_anchor[tri_indices[tri, 2]] - c
else:
displacement = pos_anchor[v] - p
collision_normal_sign = wp.vec4(-1.0, -1.0, -1.0, 1.0)
if wp.dot(displacement, collision_normal * collision_normal_sign[v_order]) > 0:
damping_hessian = (collision_damping / dt) * collision_hessian
collision_hessian = collision_hessian + damping_hessian
collision_force = collision_force + damping_hessian * displacement
collision_force = collision_force + v_bary * friction_force
collision_hessian = collision_hessian + v_bary * v_bary * friction_hessian
else:
collision_force = wp.vec3(0.0, 0.0, 0.0)
collision_hessian = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
return collision_force, collision_hessian
@wp.func
def evaluate_vertex_triangle_collision_force_hessian_4_vertices(
v: int,
tri: int,
pos: wp.array[wp.vec3],
pos_anchor: wp.array[wp.vec3],
tri_indices: wp.array2d[wp.int32],
collision_radius: float,
collision_stiffness: float,
collision_damping: float,
friction_coefficient: float,
friction_epsilon: float,
dt: float,
):
a = pos[tri_indices[tri, 0]]
b = pos[tri_indices[tri, 1]]
c = pos[tri_indices[tri, 2]]
p = pos[v]
closest_p, bary, _feature_type = triangle_closest_point(a, b, c, p)
diff = p - closest_p
dis = wp.length(diff)
collision_normal = diff / dis
if 0.0 < dis < collision_radius:
bs = wp.vec4(-bary[0], -bary[1], -bary[2], 1.0)
dEdD, d2E_dDdD = evaluate_self_contact_force_norm(dis, collision_radius, collision_stiffness)
collision_force = -dEdD * collision_normal
collision_hessian = d2E_dDdD * wp.outer(collision_normal, collision_normal)
# friction force
dx_v = p - pos_anchor[v]
closest_p_prev = (
bary[0] * pos_anchor[tri_indices[tri, 0]]
+ bary[1] * pos_anchor[tri_indices[tri, 1]]
+ bary[2] * pos_anchor[tri_indices[tri, 2]]
)
dx = dx_v - (closest_p - closest_p_prev)
e0, e1 = orthonormal_basis(collision_normal)
T = mat32(e0[0], e1[0], e0[1], e1[1], e0[2], e1[2])
u = wp.transpose(T) * dx
eps_U = friction_epsilon * dt
friction_force, friction_hessian = compute_friction(friction_coefficient, -dEdD, T, u, eps_U)
# fmt: off
if wp.static("contact_force_hessian_vt" in VBD_DEBUG_PRINTING_OPTIONS):
wp.printf(
"v: %d dEdD: %f\nnormal force: %f %f %f\nfriction force: %f %f %f\n",
v,
dEdD,
collision_force[0], collision_force[1], collision_force[2], friction_force[0], friction_force[1],
friction_force[2],
)
# fmt: on
displacement_0 = pos_anchor[tri_indices[tri, 0]] - a
displacement_1 = pos_anchor[tri_indices[tri, 1]] - b
displacement_2 = pos_anchor[tri_indices[tri, 2]] - c
displacement_3 = pos_anchor[v] - p
collision_force_0 = collision_force * bs[0]
collision_force_1 = collision_force * bs[1]
collision_force_2 = collision_force * bs[2]
collision_force_3 = collision_force * bs[3]
collision_hessian_0 = collision_hessian * bs[0] * bs[0]
collision_hessian_1 = collision_hessian * bs[1] * bs[1]
collision_hessian_2 = collision_hessian * bs[2] * bs[2]
collision_hessian_3 = collision_hessian * bs[3] * bs[3]
collision_normal_sign = wp.vec4(-1.0, -1.0, -1.0, 1.0)
damping_force, damping_hessian = damp_collision(
displacement_0,
collision_normal * collision_normal_sign[0],
collision_hessian_0,
collision_damping,
dt,
)
collision_force_0 += damping_force + bs[0] * friction_force
collision_hessian_0 += damping_hessian + bs[0] * bs[0] * friction_hessian
damping_force, damping_hessian = damp_collision(
displacement_1,
collision_normal * collision_normal_sign[1],
collision_hessian_1,
collision_damping,
dt,
)
collision_force_1 += damping_force + bs[1] * friction_force
collision_hessian_1 += damping_hessian + bs[1] * bs[1] * friction_hessian
damping_force, damping_hessian = damp_collision(
displacement_2,
collision_normal * collision_normal_sign[2],
collision_hessian_2,
collision_damping,
dt,
)
collision_force_2 += damping_force + bs[2] * friction_force
collision_hessian_2 += damping_hessian + bs[2] * bs[2] * friction_hessian
damping_force, damping_hessian = damp_collision(
displacement_3,
collision_normal * collision_normal_sign[3],
collision_hessian_3,
collision_damping,
dt,
)
collision_force_3 += damping_force + bs[3] * friction_force
collision_hessian_3 += damping_hessian + bs[3] * bs[3] * friction_hessian
return (
True,
collision_force_0,
collision_force_1,
collision_force_2,
collision_force_3,
collision_hessian_0,
collision_hessian_1,
collision_hessian_2,
collision_hessian_3,
)
else:
collision_force = wp.vec3(0.0, 0.0, 0.0)
collision_hessian = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
return (
False,
collision_force,
collision_force,
collision_force,
collision_force,
collision_hessian,
collision_hessian,
collision_hessian,
collision_hessian,
)
@wp.func
def compute_friction(mu: float, normal_contact_force: float, T: mat32, u: wp.vec2, eps_u: float):
"""
Returns the 1D friction force and hessian.
Args:
mu: Friction coefficient.
normal_contact_force: normal contact force.
T: Transformation matrix (3x2 matrix).
u: 2D displacement vector.
"""
# Friction
u_norm = wp.length(u)
if u_norm > 0.0:
# IPC friction
if u_norm > eps_u:
# constant stage
f1_SF_over_x = 1.0 / u_norm
else:
# smooth transition
f1_SF_over_x = (-u_norm / eps_u + 2.0) / eps_u
force = -mu * normal_contact_force * T * (f1_SF_over_x * u)
# Different from IPC, we treat the contact normal as constant
# this significantly improves the stability
hessian = mu * normal_contact_force * T * (f1_SF_over_x * wp.identity(2, float)) * wp.transpose(T)
else:
force = wp.vec3(0.0, 0.0, 0.0)
hessian = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
return force, hessian
@wp.kernel
def forward_step(
dt: float,
gravity: wp.array[wp.vec3],
pos_prev: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
vel: wp.array[wp.vec3],
inv_mass: wp.array[float],
external_force: wp.array[wp.vec3],
particle_flags: wp.array[wp.int32],
inertia_out: wp.array[wp.vec3],
displacements_out: wp.array[wp.vec3],
):
particle = wp.tid()
pos_prev[particle] = pos[particle]
if not particle_flags[particle] & ParticleFlags.ACTIVE or inv_mass[particle] == 0:
inertia_out[particle] = pos_prev[particle]
if displacements_out:
displacements_out[particle] = wp.vec3(0.0, 0.0, 0.0)
return
vel_new = vel[particle] + (gravity[0] + external_force[particle] * inv_mass[particle]) * dt
inertia = pos[particle] + vel_new * dt
inertia_out[particle] = inertia
if displacements_out:
displacements_out[particle] = vel_new * dt
@wp.kernel
def compute_particle_conservative_bound(
# inputs
conservative_bound_relaxation: float,
collision_query_radius: float,
adjacency: ParticleForceElementAdjacencyInfo,
collision_info: TriMeshCollisionInfo,
# outputs
particle_conservative_bounds: wp.array[float],
):
particle_index = wp.tid()
min_dist = wp.min(collision_query_radius, collision_info.vertex_colliding_triangles_min_dist[particle_index])
# bound from neighbor triangles
for i_adj_tri in range(
get_vertex_num_adjacent_faces(
adjacency,
particle_index,
)
):
tri_index, _vertex_order = get_vertex_adjacent_face_id_order(
adjacency,
particle_index,
i_adj_tri,
)
min_dist = wp.min(min_dist, collision_info.triangle_colliding_vertices_min_dist[tri_index])
# bound from neighbor edges
for i_adj_edge in range(
get_vertex_num_adjacent_edges(
adjacency,
particle_index,
)
):
nei_edge_index, vertex_order_on_edge = get_vertex_adjacent_edge_id_order(
adjacency,
particle_index,
i_adj_edge,
)
# vertex is on the edge; otherwise it only effects the bending energy
if vertex_order_on_edge == 2 or vertex_order_on_edge == 3:
# collisions of neighbor edges
min_dist = wp.min(min_dist, collision_info.edge_colliding_edges_min_dist[nei_edge_index])
particle_conservative_bounds[particle_index] = conservative_bound_relaxation * min_dist
@wp.kernel
def validate_conservative_bound(
pos: wp.array[wp.vec3],
pos_prev_collision_detection: wp.array[wp.vec3],
particle_conservative_bounds: wp.array[float],
):
v_index = wp.tid()
displacement = wp.length(pos[v_index] - pos_prev_collision_detection[v_index])
if displacement > particle_conservative_bounds[v_index] * 1.01 and displacement > 1e-5:
# wp.expect_eq(displacement <= particle_conservative_bounds[v_index] * 1.01, True)
wp.printf(
"Vertex %d has moved by %f exceeded the limit of %f\n",
v_index,
displacement,
particle_conservative_bounds[v_index],
)
@wp.func
def apply_conservative_bound_truncation(
v_index: wp.int32,
pos_new: wp.vec3,
pos_prev_collision_detection: wp.array[wp.vec3],
particle_conservative_bounds: wp.array[float],
):
particle_pos_prev_collision_detection = pos_prev_collision_detection[v_index]
accumulated_displacement = pos_new - particle_pos_prev_collision_detection
conservative_bound = particle_conservative_bounds[v_index]
accumulated_displacement_norm = wp.length(accumulated_displacement)
if accumulated_displacement_norm > conservative_bound and conservative_bound > 1e-5:
accumulated_displacement_norm_truncated = conservative_bound
accumulated_displacement = accumulated_displacement * (
accumulated_displacement_norm_truncated / accumulated_displacement_norm
)
return particle_pos_prev_collision_detection + accumulated_displacement
else:
return pos_new
@wp.kernel
def update_velocity(dt: float, pos_prev: wp.array[wp.vec3], pos: wp.array[wp.vec3], vel: wp.array[wp.vec3]):
particle = wp.tid()
vel[particle] = (pos[particle] - pos_prev[particle]) / dt
@wp.kernel
def convert_body_particle_contact_data_kernel(
# inputs
body_particle_contact_buffer_pre_alloc: int,
soft_contact_particle: wp.array[int],
contact_count: wp.array[int],
contact_max: int,
# outputs
body_particle_contact_buffer: wp.array[int],
body_particle_contact_count: wp.array[int],
):
contact_index = wp.tid()
count = min(contact_max, contact_count[0])
if contact_index >= count:
return
particle_index = soft_contact_particle[contact_index]
offset = particle_index * body_particle_contact_buffer_pre_alloc
contact_counter = wp.atomic_add(body_particle_contact_count, particle_index, 1)
if contact_counter < body_particle_contact_buffer_pre_alloc:
body_particle_contact_buffer[offset + contact_counter] = contact_index
@wp.kernel
def accumulate_self_contact_force_and_hessian(
# inputs
dt: float,
current_color: int,
pos_prev: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
particle_colors: wp.array[int],
tri_indices: wp.array2d[wp.int32],
edge_indices: wp.array2d[wp.int32],
# self contact
collision_info_array: wp.array[TriMeshCollisionInfo],
collision_radius: float,
soft_contact_ke: float,
soft_contact_kd: float,
friction_mu: float,
friction_epsilon: float,
edge_edge_parallel_epsilon: float,
# outputs: particle force and hessian
particle_forces: wp.array[wp.vec3],
particle_hessians: wp.array[wp.mat33],
):
t_id = wp.tid()
collision_info = collision_info_array[0]
primitive_id = t_id // NUM_THREADS_PER_COLLISION_PRIMITIVE
t_id_current_primitive = t_id % NUM_THREADS_PER_COLLISION_PRIMITIVE
# process edge-edge collisions
if primitive_id < collision_info.edge_colliding_edges_buffer_sizes.shape[0]:
e1_idx = primitive_id
collision_buffer_counter = t_id_current_primitive
collision_buffer_offset = collision_info.edge_colliding_edges_offsets[primitive_id]
while collision_buffer_counter < collision_info.edge_colliding_edges_buffer_sizes[primitive_id]:
e2_idx = collision_info.edge_colliding_edges[2 * (collision_buffer_offset + collision_buffer_counter) + 1]
if e1_idx != -1 and e2_idx != -1:
e1_v1 = edge_indices[e1_idx, 2]
e1_v2 = edge_indices[e1_idx, 3]
c_e1_v1 = particle_colors[e1_v1]
c_e1_v2 = particle_colors[e1_v2]
if c_e1_v1 == current_color or c_e1_v2 == current_color:
has_contact, collision_force_0, collision_force_1, collision_hessian_0, collision_hessian_1 = (
evaluate_edge_edge_contact_2_vertices(
e1_idx,
e2_idx,
pos,
pos_prev,
edge_indices,
collision_radius,
soft_contact_ke,
soft_contact_kd,
friction_mu,
friction_epsilon,
dt,
edge_edge_parallel_epsilon,
)
)
if has_contact:
# here we only handle the e1 side, because e2 will also detection this contact and add force and hessian on its own
if c_e1_v1 == current_color:
wp.atomic_add(particle_forces, e1_v1, collision_force_0)
wp.atomic_add(particle_hessians, e1_v1, collision_hessian_0)
if c_e1_v2 == current_color:
wp.atomic_add(particle_forces, e1_v2, collision_force_1)
wp.atomic_add(particle_hessians, e1_v2, collision_hessian_1)
collision_buffer_counter += NUM_THREADS_PER_COLLISION_PRIMITIVE
# process vertex-triangle collisions
if primitive_id < collision_info.vertex_colliding_triangles_buffer_sizes.shape[0]:
particle_idx = primitive_id
collision_buffer_counter = t_id_current_primitive
collision_buffer_offset = collision_info.vertex_colliding_triangles_offsets[primitive_id]
while collision_buffer_counter < collision_info.vertex_colliding_triangles_buffer_sizes[primitive_id]:
tri_idx = collision_info.vertex_colliding_triangles[
(collision_buffer_offset + collision_buffer_counter) * 2 + 1
]
if particle_idx != -1 and tri_idx != -1:
tri_a = tri_indices[tri_idx, 0]
tri_b = tri_indices[tri_idx, 1]
tri_c = tri_indices[tri_idx, 2]
c_v = particle_colors[particle_idx]
c_tri_a = particle_colors[tri_a]
c_tri_b = particle_colors[tri_b]
c_tri_c = particle_colors[tri_c]
if (
c_v == current_color
or c_tri_a == current_color
or c_tri_b == current_color
or c_tri_c == current_color
):
(
has_contact,
collision_force_0,
collision_force_1,
collision_force_2,
collision_force_3,
collision_hessian_0,
collision_hessian_1,
collision_hessian_2,
collision_hessian_3,
) = evaluate_vertex_triangle_collision_force_hessian_4_vertices(
particle_idx,
tri_idx,
pos,
pos_prev,
tri_indices,
collision_radius,
soft_contact_ke,
soft_contact_kd,
friction_mu,
friction_epsilon,
dt,
)
if has_contact:
# particle
if c_v == current_color:
wp.atomic_add(particle_forces, particle_idx, collision_force_3)
wp.atomic_add(particle_hessians, particle_idx, collision_hessian_3)
# tri_a
if c_tri_a == current_color:
wp.atomic_add(particle_forces, tri_a, collision_force_0)
wp.atomic_add(particle_hessians, tri_a, collision_hessian_0)
# tri_b
if c_tri_b == current_color:
wp.atomic_add(particle_forces, tri_b, collision_force_1)
wp.atomic_add(particle_hessians, tri_b, collision_hessian_1)
# tri_c
if c_tri_c == current_color:
wp.atomic_add(particle_forces, tri_c, collision_force_2)
wp.atomic_add(particle_hessians, tri_c, collision_hessian_2)
collision_buffer_counter += NUM_THREADS_PER_COLLISION_PRIMITIVE
def _csr_row(vals: np.ndarray, offs: np.ndarray, i: int) -> np.ndarray:
"""Extract CSR row `i` from the flattened adjacency arrays."""
return vals[offs[i] : offs[i + 1]]
def set_to_csr(
list_of_sets: list[set[int]], dtype: np.dtype = np.int32, sort: bool = True
) -> tuple[np.ndarray, np.ndarray]:
"""
Convert a list of integer sets into CSR (Compressed Sparse Row) structure.
Args:
list_of_sets: Iterable where each entry is a set of ints.
dtype: Output dtype for the flattened arrays.
sort: Whether to sort each row when writing into ``flat``.
Returns:
A tuple ``(flat, offsets)`` representing the CSR values and offsets.
"""
offsets = np.zeros(len(list_of_sets) + 1, dtype=dtype)
sizes = np.fromiter((len(s) for s in list_of_sets), count=len(list_of_sets), dtype=dtype)
np.cumsum(sizes, out=offsets[1:])
flat = np.empty(offsets[-1], dtype=dtype)
idx = 0
for s in list_of_sets:
if sort:
arr = np.fromiter(sorted(s), count=len(s), dtype=dtype)
else:
arr = np.fromiter(s, count=len(s), dtype=dtype)
flat[idx : idx + len(arr)] = arr
idx += len(arr)
return flat, offsets
def one_ring_vertices(
v: int, edge_indices: np.ndarray, v_adj_edges: np.ndarray, v_adj_edges_offsets: np.ndarray
) -> np.ndarray:
"""
Find immediate neighboring vertices that share an edge with vertex `v`.
Args:
v: Vertex index whose neighborhood is queried.
edge_indices: Array of shape [num_edges, 4] storing edge endpoint indices.
v_adj_edges: Flattened CSR adjacency array listing edge ids and local order.
v_adj_edges_offsets: CSR offsets indexing into `v_adj_edges`.
Returns:
Sorted array of neighboring vertex indices, excluding `v`.
"""
e_u = edge_indices[:, 2]
e_v = edge_indices[:, 3]
# preserve only the adjacent edge information, remove the order information
inc_edges = _csr_row(v_adj_edges, v_adj_edges_offsets, v)[::2]
inc_edges_order = _csr_row(v_adj_edges, v_adj_edges_offsets, v)[1::2]
if inc_edges.size == 0:
return np.empty(0)
us = e_u[inc_edges[np.where(inc_edges_order >= 2)]]
vs = e_v[inc_edges[np.where(inc_edges_order >= 2)]]
assert (np.logical_or(us == v, vs == v)).all()
nbrs = np.unique(np.concatenate([us, vs]))
return nbrs[nbrs != v]
def leq_n_ring_vertices(
v: int, edge_indices: np.ndarray, n: int, v_adj_edges: np.ndarray, v_adj_edges_offsets: np.ndarray
) -> np.ndarray:
"""
Find all vertices within n-ring distance of vertex v using BFS.
Args:
v: Starting vertex index
edge_indices: Edge connectivity array
n: Maximum ring distance
v_adj_edges: CSR values for vertex-edge adjacency
v_adj_edges_offsets: CSR offsets for vertex-edge adjacency
Returns:
Array of all vertices within n-ring distance, including v itself
"""
visited = {v}
frontier = {v}
for _ in range(n):
next_frontier = set()
for u in frontier:
for w in one_ring_vertices(u, edge_indices, v_adj_edges, v_adj_edges_offsets): # iterable of neighbors of u
if w not in visited:
visited.add(w)
next_frontier.add(w)
if not next_frontier:
break
frontier = next_frontier
return np.fromiter(visited, dtype=int)
def build_vertex_n_ring_tris_collision_filter(
n: int,
num_vertices: int,
edge_indices: np.ndarray,
v_adj_edges: np.ndarray,
v_adj_edges_offsets: np.ndarray,
v_adj_faces: np.ndarray,
v_adj_faces_offsets: np.ndarray,
):
"""
For each vertex v, return ONLY triangles adjacent to v's one ring neighbor vertices.
Excludes triangles incident to v itself (dist 0).
Returns:
v_two_flat, v_two_offs: CSR of strict-2-ring triangle ids per vertex
"""
if n <= 1:
return None, None
v_nei_tri_sets = [set() for _ in range(num_vertices)]
for v in range(num_vertices):
# distance-1 vertices
if n == 2:
ring_n_minus_1 = one_ring_vertices(v, edge_indices, v_adj_edges, v_adj_edges_offsets)
else:
ring_n_minus_1 = leq_n_ring_vertices(v, edge_indices, n - 1, v_adj_edges, v_adj_edges_offsets)
ring_1_tri_set = set(_csr_row(v_adj_faces, v_adj_faces_offsets, v)[::2])
nei_tri_set = v_nei_tri_sets[v]
for w in ring_n_minus_1:
if w != v:
# preserve only the adjacent edge information, remove the order information
nei_tri_set.update(_csr_row(v_adj_faces, v_adj_faces_offsets, w)[::2])
nei_tri_set.difference_update(ring_1_tri_set)
return v_nei_tri_sets
def build_edge_n_ring_edge_collision_filter(
n: int,
edge_indices: np.ndarray,
v_adj_edges: np.ndarray,
v_adj_edges_offsets: np.ndarray,
):
"""
For each vertex v, return ONLY triangles adjacent to v's one ring neighbor vertices.
Excludes triangles incident to v itself (dist 0).
Returns:
v_two_flat, v_two_offs: CSR of strict-2-ring triangle ids per vertex
"""
if n <= 1:
return None, None
edge_nei_edge_sets = [set() for _ in range(edge_indices.shape[0])]
for e_idx in range(edge_indices.shape[0]):
# distance-1 vertices
v1 = edge_indices[e_idx, 2]
v2 = edge_indices[e_idx, 3]
if n == 2:
ring_n_minus_1_v1 = one_ring_vertices(v1, edge_indices, v_adj_edges, v_adj_edges_offsets)
ring_n_minus_1_v2 = one_ring_vertices(v2, edge_indices, v_adj_edges, v_adj_edges_offsets)
else:
ring_n_minus_1_v1 = leq_n_ring_vertices(v1, edge_indices, n - 1, v_adj_edges, v_adj_edges_offsets)
ring_n_minus_1_v2 = leq_n_ring_vertices(v2, edge_indices, n - 1, v_adj_edges, v_adj_edges_offsets)
all_neighbors = set(ring_n_minus_1_v1)
all_neighbors.update(ring_n_minus_1_v2)
ring_1_edge_set = set(_csr_row(v_adj_edges, v_adj_edges_offsets, v1)[::2])
ring_2_edge_set = set(_csr_row(v_adj_edges, v_adj_edges_offsets, v2)[::2])
nei_edge_set = edge_nei_edge_sets[e_idx]
for w in all_neighbors:
if w != v1 and w != v2:
# preserve only the adjacent edge information, remove the order information
# nei_tri_set.update(_csr_row(v_adj_faces, v_adj_faces_offsets, w)[::2])
adj_edges = _csr_row(v_adj_edges, v_adj_edges_offsets, w)[::2]
adj_edges_order = _csr_row(v_adj_edges, v_adj_edges_offsets, w)[1::2]
adj_collision_edges = adj_edges[np.where(adj_edges_order >= 2)]
nei_edge_set.update(adj_collision_edges)
nei_edge_set.difference_update(ring_1_edge_set)
nei_edge_set.difference_update(ring_2_edge_set)
return edge_nei_edge_sets
@wp.func
def evaluate_spring_force_and_hessian(
particle_idx: int,
spring_idx: int,
dt: float,
pos: wp.array[wp.vec3],
pos_anchor: wp.array[wp.vec3],
spring_indices: wp.array[int],
spring_rest_length: wp.array[float],
spring_stiffness: wp.array[float],
spring_damping: wp.array[float],
):
v0 = spring_indices[spring_idx * 2]
v1 = spring_indices[spring_idx * 2 + 1]
diff = pos[v0] - pos[v1]
spring_length = wp.length(diff)
# Clamp to epsilon to avoid division by zero for coincident vertices
spring_length = wp.max(spring_length, 1e-8)
l0 = spring_rest_length[spring_idx]
force_sign = 1.0 if particle_idx == v0 else -1.0
spring_force = force_sign * spring_stiffness[spring_idx] * (l0 - spring_length) / spring_length * diff
spring_hessian = spring_stiffness[spring_idx] * (
wp.identity(3, float)
- (l0 / spring_length) * (wp.identity(3, float) - wp.outer(diff, diff) / (spring_length * spring_length))
)
# compute damping
h_d = spring_hessian * (spring_damping[spring_idx] / dt)
f_d = h_d * (pos_anchor[particle_idx] - pos[particle_idx])
spring_force = spring_force + f_d
spring_hessian = spring_hessian + h_d
return spring_force, spring_hessian
@wp.func
def evaluate_spring_force_and_hessian_both_vertices(
spring_idx: int,
dt: float,
pos: wp.array[wp.vec3],
pos_anchor: wp.array[wp.vec3],
spring_indices: wp.array[int],
spring_rest_length: wp.array[float],
spring_stiffness: wp.array[float],
spring_damping: wp.array[float],
):
"""Evaluate spring force and hessian for both vertices of a spring.
Returns forces and hessians for v0 and v1 respectively.
"""
v0 = spring_indices[spring_idx * 2]
v1 = spring_indices[spring_idx * 2 + 1]
diff = pos[v0] - pos[v1]
spring_length = wp.length(diff)
# Clamp to epsilon to avoid division by zero for coincident vertices
spring_length = wp.max(spring_length, 1e-8)
l0 = spring_rest_length[spring_idx]
# Base spring force for v0 (v1 gets the opposite)
base_force = spring_stiffness[spring_idx] * (l0 - spring_length) / spring_length * diff
# Hessian is the same for both vertices (symmetric)
spring_hessian = spring_stiffness[spring_idx] * (
wp.identity(3, float)
- (l0 / spring_length) * (wp.identity(3, float) - wp.outer(diff, diff) / (spring_length * spring_length))
)
# Compute damping hessian contribution
h_d = spring_hessian * (spring_damping[spring_idx] / dt)
# Damping force for each vertex
f_d_v0 = h_d * (pos_anchor[v0] - pos[v0])
f_d_v1 = h_d * (pos_anchor[v1] - pos[v1])
# Total force and hessian for each vertex
force_v0 = base_force + f_d_v0
force_v1 = -base_force + f_d_v1 # Opposite direction for v1
hessian_total = spring_hessian + h_d
return v0, v1, force_v0, force_v1, hessian_total
@wp.kernel
def accumulate_spring_force_and_hessian(
# inputs
dt: float,
current_color: int,
pos_anchor: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
particle_colors: wp.array[int],
num_springs: int,
# spring constraints
spring_indices: wp.array[int],
spring_rest_length: wp.array[float],
spring_stiffness: wp.array[float],
spring_damping: wp.array[float],
# outputs: particle force and hessian
particle_forces: wp.array[wp.vec3],
particle_hessians: wp.array[wp.mat33],
):
"""Accumulate spring forces and hessians, parallelized by springs.
Each thread handles one spring and uses atomic operations to add
forces and hessians to vertices with the current color.
"""
spring_idx = wp.tid()
if spring_idx < num_springs:
v0 = spring_indices[spring_idx * 2]
v1 = spring_indices[spring_idx * 2 + 1]
c_v0 = particle_colors[v0]
c_v1 = particle_colors[v1]
# Only evaluate if at least one vertex has the current color
if c_v0 == current_color or c_v1 == current_color:
_, _, force_v0, force_v1, hessian = evaluate_spring_force_and_hessian_both_vertices(
spring_idx,
dt,
pos,
pos_anchor,
spring_indices,
spring_rest_length,
spring_stiffness,
spring_damping,
)
# Only add to vertices with the current color
if c_v0 == current_color:
wp.atomic_add(particle_forces, v0, force_v0)
wp.atomic_add(particle_hessians, v0, hessian)
if c_v1 == current_color:
wp.atomic_add(particle_forces, v1, force_v1)
wp.atomic_add(particle_hessians, v1, hessian)
@wp.kernel
def accumulate_contact_force_and_hessian_no_self_contact(
# inputs
dt: float,
current_color: int,
pos_anchor: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
particle_colors: wp.array[int],
# body-particle contact
friction_epsilon: float,
particle_radius: wp.array[float],
body_particle_contact_particle: wp.array[int],
body_particle_contact_count: wp.array[int],
body_particle_contact_max: int,
# per-contact soft AVBD parameters for body-particle contacts (shared with rigid side)
body_particle_contact_penalty_k: wp.array[float],
body_particle_contact_material_kd: wp.array[float],
body_particle_contact_material_mu: wp.array[float],
shape_material_mu: wp.array[float],
shape_body: wp.array[int],
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
contact_shape: wp.array[int],
contact_body_pos: wp.array[wp.vec3],
contact_body_vel: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
# outputs: particle force and hessian
particle_forces: wp.array[wp.vec3],
particle_hessians: wp.array[wp.mat33],
):
t_id = wp.tid()
particle_body_contact_count = min(body_particle_contact_max, body_particle_contact_count[0])
if t_id < particle_body_contact_count:
particle_idx = body_particle_contact_particle[t_id]
if particle_colors[particle_idx] == current_color:
# Read per-contact AVBD penalty and material properties shared with the rigid side
contact_ke = body_particle_contact_penalty_k[t_id]
contact_kd = body_particle_contact_material_kd[t_id]
contact_mu = body_particle_contact_material_mu[t_id]
body_contact_force, body_contact_hessian = evaluate_body_particle_contact(
particle_idx,
pos[particle_idx],
pos_anchor[particle_idx],
t_id,
contact_ke,
contact_kd,
contact_mu,
friction_epsilon,
particle_radius,
shape_material_mu,
shape_body,
body_q,
body_q_prev,
body_qd,
body_com,
contact_shape,
contact_body_pos,
contact_body_vel,
contact_normal,
dt,
)
wp.atomic_add(particle_forces, particle_idx, body_contact_force)
wp.atomic_add(particle_hessians, particle_idx, body_contact_hessian)
# =============================================================================
# Planar DAT (Divide and Truncate) kernels
# =============================================================================
@wp.func
def segment_plane_intersects(
v: wp.vec3,
delta_v: wp.vec3,
n: wp.vec3,
d: wp.vec3,
eps_parallel: float, # e.g., 1e-8
eps_intersect_near: float, # e.g., 1e-8
eps_intersect_far: float, # e.g., 1e-8
coplanar_counts: bool, # True if you want a coplanar segment to count as "hit"
) -> bool:
# Plane eq: n·(p - d) = 0
# Segment: p(t) = v + t * delta_v, t in [0, 1]
nv = wp.dot(n, delta_v)
num = -wp.dot(n, v - d)
# Parallel (or nearly): either coplanar or no hit
if wp.abs(nv) < eps_parallel:
return coplanar_counts and (wp.abs(num) < eps_parallel)
t = num / nv
# consider tiny tolerance at ends
return (t >= eps_intersect_near) and (t <= 1.0 + eps_intersect_far)
@wp.func
def create_vertex_triangle_division_plane_closest_pt(
v: wp.vec3,
delta_v: wp.vec3,
t1: wp.vec3,
delta_t1: wp.vec3,
t2: wp.vec3,
delta_t2: wp.vec3,
t3: wp.vec3,
delta_t3: wp.vec3,
):
"""
n points to the vertex side
"""
closest_p, _bary, _feature_type = triangle_closest_point(t1, t2, t3, v)
n_hat = v - closest_p
if wp.length(n_hat) < 1e-12:
return wp.vector(False, False, False, False, length=4, dtype=wp.bool), wp.vec3(0.0), v
n = wp.normalize(n_hat)
delta_v_n = wp.max(-wp.dot(n, delta_v), 0.0)
delta_t_n = wp.max(
wp.vec4(
wp.dot(n, delta_t1),
wp.dot(n, delta_t2),
wp.dot(n, delta_t3),
0.0,
)
)
if delta_t_n + delta_v_n == 0.0:
d = closest_p + 0.5 * n_hat
else:
lmbd = delta_t_n / (delta_t_n + delta_v_n)
lmbd = wp.clamp(lmbd, 0.05, 0.95)
d = closest_p + lmbd * n_hat
if delta_v_n == 0.0:
is_dummy_for_v = True
else:
is_dummy_for_v = not segment_plane_intersects(v, delta_v, n, d, 1e-6, -1e-8, 1e-8, False)
if delta_t_n == 0.0:
is_dummy_for_t_1 = True
is_dummy_for_t_2 = True
is_dummy_for_t_3 = True
else:
is_dummy_for_t_1 = not segment_plane_intersects(t1, delta_t1, n, d, 1e-6, -1e-8, 1e-8, False)
is_dummy_for_t_2 = not segment_plane_intersects(t2, delta_t2, n, d, 1e-6, -1e-8, 1e-8, False)
is_dummy_for_t_3 = not segment_plane_intersects(t3, delta_t3, n, d, 1e-6, -1e-8, 1e-8, False)
return (
wp.vector(is_dummy_for_v, is_dummy_for_t_1, is_dummy_for_t_2, is_dummy_for_t_3, length=4, dtype=wp.bool),
n,
d,
)
@wp.func
def robust_edge_pair_normal(
e0_v0_pos: wp.vec3,
e0_v1_pos: wp.vec3,
e1_v0_pos: wp.vec3,
e1_v1_pos: wp.vec3,
eps: float = 1.0e-6,
) -> wp.vec3:
# Edge directions
dir0 = e0_v1_pos - e0_v0_pos
dir1 = e1_v1_pos - e1_v0_pos
len0 = wp.length(dir0)
len1 = wp.length(dir1)
if len0 > eps:
dir0 = dir0 / len0
else:
dir0 = wp.vec3(0.0, 0.0, 0.0)
if len1 > eps:
dir1 = dir1 / len1
else:
dir1 = wp.vec3(0.0, 0.0, 0.0)
# Primary: cross of two valid directions
n = wp.cross(dir0, dir1)
len_n = wp.length(n)
if len_n > eps:
return n / len_n
# Parallel or degenerate: pick best non-zero direction
reference = dir0
if wp.length(reference) <= eps:
reference = dir1
if wp.length(reference) <= eps:
# Both edges collapsed: fall back to canonical axis
return wp.vec3(1.0, 0.0, 0.0)
# Try bridge vector between midpoints
bridge = 0.5 * ((e1_v0_pos + e1_v1_pos) - (e0_v0_pos + e0_v1_pos))
bridge_len = wp.length(bridge)
if bridge_len > eps:
n = wp.cross(reference, bridge / bridge_len)
len_n = wp.length(n)
if len_n > eps:
return n / len_n
# Use an axis guaranteed (numerically) to be non-parallel
fallback_axis = wp.vec3(1.0, 0.0, 0.0)
if wp.abs(wp.dot(reference, fallback_axis)) > 0.9:
fallback_axis = wp.vec3(0.0, 1.0, 0.0)
n = wp.cross(reference, fallback_axis)
len_n = wp.length(n)
if len_n > eps:
return n / len_n
# Final guard: use the remaining canonical axis
fallback_axis = wp.vec3(0.0, 0.0, 1.0)
n = wp.cross(reference, fallback_axis)
len_n = wp.length(n)
if len_n > eps:
return n / len_n
return wp.vec3(1.0, 0.0, 0.0)
@wp.func
def create_edge_edge_division_plane_closest_pt(
e0_v0_pos: wp.vec3,
delta_e0_v0: wp.vec3,
e0_v1_pos: wp.vec3,
delta_e0_v1: wp.vec3,
e1_v0_pos: wp.vec3,
delta_e1_v0: wp.vec3,
e1_v1_pos: wp.vec3,
delta_e1_v1: wp.vec3,
):
st = wp.closest_point_edge_edge(e0_v0_pos, e0_v1_pos, e1_v0_pos, e1_v1_pos, 1e-6)
s = st[0]
t = st[1]
c1 = e0_v0_pos + (e0_v1_pos - e0_v0_pos) * s
c2 = e1_v0_pos + (e1_v1_pos - e1_v0_pos) * t
n_hat = c1 - c2
if wp.length(n_hat) < 1e-12:
return (
wp.vector(False, False, False, False, length=4, dtype=wp.bool),
robust_edge_pair_normal(e0_v0_pos, e0_v1_pos, e1_v0_pos, e1_v1_pos),
c1 * 0.5 + c2 * 0.5,
)
n = wp.normalize(n_hat)
delta_e0 = wp.max(
wp.vec3(
-wp.dot(n, delta_e0_v0),
-wp.dot(n, delta_e0_v1),
0.0,
)
)
delta_e1 = wp.max(
wp.vec3(
wp.dot(n, delta_e1_v0),
wp.dot(n, delta_e1_v1),
0.0,
)
)
if delta_e0 + delta_e1 == 0.0:
d = c2 + 0.5 * n_hat
else:
lmbd = delta_e1 / (delta_e1 + delta_e0)
lmbd = wp.clamp(lmbd, 0.05, 0.95)
d = c2 + lmbd * n_hat
if delta_e0 == 0.0:
is_dummy_for_e0_v0 = True
is_dummy_for_e0_v1 = True
else:
is_dummy_for_e0_v0 = not segment_plane_intersects(e0_v0_pos, delta_e0_v0, n, d, 1e-6, -1e-8, 1e-6, False)
is_dummy_for_e0_v1 = not segment_plane_intersects(e0_v1_pos, delta_e0_v1, n, d, 1e-6, -1e-8, 1e-6, False)
if delta_e1 == 0.0:
is_dummy_for_e1_v0 = True
is_dummy_for_e1_v1 = True
else:
is_dummy_for_e1_v0 = not segment_plane_intersects(e1_v0_pos, delta_e1_v0, n, d, 1e-6, -1e-8, 1e-6, False)
is_dummy_for_e1_v1 = not segment_plane_intersects(e1_v1_pos, delta_e1_v1, n, d, 1e-6, -1e-8, 1e-6, False)
return (
wp.vector(
is_dummy_for_e0_v0, is_dummy_for_e0_v1, is_dummy_for_e1_v0, is_dummy_for_e1_v1, length=4, dtype=wp.bool
),
n,
d,
)
@wp.func
def planar_truncation(
v: wp.vec3, delta_v: wp.vec3, n: wp.vec3, d: wp.vec3, eps: float, gamma_r: float, gamma_min: float = 1e-3
):
nv = wp.dot(n, delta_v)
num = wp.dot(n, d - v)
# Parallel (or nearly): do not truncate
if wp.abs(nv) < eps:
return delta_v
t = num / nv
t = wp.max(wp.min(t * gamma_r, t - gamma_min), 0.0)
if t >= 1:
return delta_v
else:
return t * delta_v
@wp.func
def planar_truncation_t(
v: wp.vec3, delta_v: wp.vec3, n: wp.vec3, d: wp.vec3, eps: float, gamma_r: float, gamma_min: float = 1e-3
):
denom = wp.dot(n, delta_v)
# Parallel (or nearly parallel) → no intersection
if wp.abs(denom) < eps:
return 1.0
# Solve: dot(n, v + t*delta_v - d) = 0
t = wp.dot(n, d - v) / denom
if t < 0:
return 1.0
t = wp.clamp(wp.min(t * gamma_r, t - gamma_min), 0.0, 1.0)
return t
@wp.kernel
def apply_planar_truncation_parallel_by_collision(
# inputs
pos: wp.array[wp.vec3],
displacement_in: wp.array[wp.vec3],
tri_indices: wp.array2d[wp.int32],
edge_indices: wp.array2d[wp.int32],
collision_info_array: wp.array[TriMeshCollisionInfo],
parallel_eps: float,
gamma: float,
truncation_t_out: wp.array[float],
):
t_id = wp.tid()
collision_info = collision_info_array[0]
primitive_id = t_id // NUM_THREADS_PER_COLLISION_PRIMITIVE
t_id_current_primitive = t_id % NUM_THREADS_PER_COLLISION_PRIMITIVE
# process edge-edge collisions
if primitive_id < collision_info.edge_colliding_edges_buffer_sizes.shape[0]:
e1_idx = primitive_id
collision_buffer_counter = t_id_current_primitive
collision_buffer_offset = collision_info.edge_colliding_edges_offsets[primitive_id]
while collision_buffer_counter < collision_info.edge_colliding_edges_buffer_sizes[primitive_id]:
e2_idx = collision_info.edge_colliding_edges[2 * (collision_buffer_offset + collision_buffer_counter) + 1]
if e1_idx != -1 and e2_idx != -1:
e1_v1 = edge_indices[e1_idx, 2]
e1_v2 = edge_indices[e1_idx, 3]
e1_v1_pos = pos[e1_v1]
e1_v2_pos = pos[e1_v2]
delta_e1_v1 = displacement_in[e1_v1]
delta_e1_v2 = displacement_in[e1_v2]
e2_v1 = edge_indices[e2_idx, 2]
e2_v2 = edge_indices[e2_idx, 3]
e2_v1_pos = pos[e2_v1]
e2_v2_pos = pos[e2_v2]
delta_e2_v1 = displacement_in[e2_v1]
delta_e2_v2 = displacement_in[e2_v2]
# n points to the edge 1 side
is_dummy, n, d = create_edge_edge_division_plane_closest_pt(
e1_v1_pos,
delta_e1_v1,
e1_v2_pos,
delta_e1_v2,
e2_v1_pos,
delta_e2_v1,
e2_v2_pos,
delta_e2_v2,
)
# For each, check the corresponding is_dummy entry in the vec4 is_dummy
if not is_dummy[0]:
t = planar_truncation_t(e1_v1_pos, delta_e1_v1, n, d, parallel_eps, gamma)
wp.atomic_min(truncation_t_out, e1_v1, t)
if not is_dummy[1]:
t = planar_truncation_t(e1_v2_pos, delta_e1_v2, n, d, parallel_eps, gamma)
wp.atomic_min(truncation_t_out, e1_v2, t)
if not is_dummy[2]:
t = planar_truncation_t(e2_v1_pos, delta_e2_v1, n, d, parallel_eps, gamma)
wp.atomic_min(truncation_t_out, e2_v1, t)
if not is_dummy[3]:
t = planar_truncation_t(e2_v2_pos, delta_e2_v2, n, d, parallel_eps, gamma)
wp.atomic_min(truncation_t_out, e2_v2, t)
# planar truncation for 2 sides
collision_buffer_counter += NUM_THREADS_PER_COLLISION_PRIMITIVE
# process vertex-triangle collisions
if primitive_id < collision_info.vertex_colliding_triangles_buffer_sizes.shape[0]:
particle_idx = primitive_id
colliding_particle_pos = pos[particle_idx]
colliding_particle_displacement = displacement_in[particle_idx]
collision_buffer_counter = t_id_current_primitive
collision_buffer_offset = collision_info.vertex_colliding_triangles_offsets[primitive_id]
while collision_buffer_counter < collision_info.vertex_colliding_triangles_buffer_sizes[primitive_id]:
tri_idx = collision_info.vertex_colliding_triangles[
(collision_buffer_offset + collision_buffer_counter) * 2 + 1
]
if particle_idx != -1 and tri_idx != -1:
tri_a = tri_indices[tri_idx, 0]
tri_b = tri_indices[tri_idx, 1]
tri_c = tri_indices[tri_idx, 2]
t1 = pos[tri_a]
t2 = pos[tri_b]
t3 = pos[tri_c]
delta_t1 = displacement_in[tri_a]
delta_t2 = displacement_in[tri_b]
delta_t3 = displacement_in[tri_c]
is_dummy, n, d = create_vertex_triangle_division_plane_closest_pt(
colliding_particle_pos,
colliding_particle_displacement,
t1,
delta_t1,
t2,
delta_t2,
t3,
delta_t3,
)
# planar truncation for 2 sides
if not is_dummy[0]:
t = planar_truncation_t(
colliding_particle_pos, colliding_particle_displacement, n, d, parallel_eps, gamma
)
wp.atomic_min(truncation_t_out, particle_idx, t)
if not is_dummy[1]:
t = planar_truncation_t(t1, delta_t1, n, d, parallel_eps, gamma)
wp.atomic_min(truncation_t_out, tri_a, t)
if not is_dummy[2]:
t = planar_truncation_t(t2, delta_t2, n, d, parallel_eps, gamma)
wp.atomic_min(truncation_t_out, tri_b, t)
if not is_dummy[3]:
t = planar_truncation_t(t3, delta_t3, n, d, parallel_eps, gamma)
wp.atomic_min(truncation_t_out, tri_c, t)
collision_buffer_counter += NUM_THREADS_PER_COLLISION_PRIMITIVE
# Don't forget to do the final truncation based on the maximum displacement allowance!
@wp.kernel
def apply_truncation_ts(
pos: wp.array[wp.vec3],
displacement_in: wp.array[wp.vec3],
truncation_ts: wp.array[float],
max_displacement: float,
displacement_out: wp.array[wp.vec3],
pos_out: wp.array[wp.vec3],
):
i = wp.tid()
t = truncation_ts[i]
particle_displacement = displacement_in[i] * t
# Nuts-saving truncation: clamp displacement magnitude to max_displacement
len_displacement = wp.length(particle_displacement)
if len_displacement > max_displacement:
particle_displacement = particle_displacement * max_displacement / len_displacement
displacement_out[i] = particle_displacement
if pos_out:
pos_out[i] = pos[i] + particle_displacement
@wp.kernel
def accumulate_particle_body_contact_force_and_hessian(
# inputs
dt: float,
current_color: int,
pos_anchor: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
particle_colors: wp.array[int],
# body-particle contact
friction_epsilon: float,
particle_radius: wp.array[float],
body_particle_contact_particle: wp.array[int],
body_particle_contact_count: wp.array[int],
body_particle_contact_max: int,
# per-contact soft AVBD parameters for body-particle contacts (shared with rigid side)
body_particle_contact_penalty_k: wp.array[float],
body_particle_contact_material_kd: wp.array[float],
body_particle_contact_material_mu: wp.array[float],
shape_material_mu: wp.array[float],
shape_body: wp.array[int],
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
contact_shape: wp.array[int],
contact_body_pos: wp.array[wp.vec3],
contact_body_vel: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
# outputs: particle force and hessian
particle_forces: wp.array[wp.vec3],
particle_hessians: wp.array[wp.mat33],
):
t_id = wp.tid()
particle_body_contact_count = min(body_particle_contact_max, body_particle_contact_count[0])
if t_id < particle_body_contact_count:
particle_idx = body_particle_contact_particle[t_id]
if particle_colors[particle_idx] == current_color:
# Read per-contact AVBD penalty and material properties shared with the rigid side
contact_ke = body_particle_contact_penalty_k[t_id]
contact_kd = body_particle_contact_material_kd[t_id]
contact_mu = body_particle_contact_material_mu[t_id]
body_contact_force, body_contact_hessian = evaluate_body_particle_contact(
particle_idx,
pos[particle_idx],
pos_anchor[particle_idx],
t_id,
contact_ke,
contact_kd,
contact_mu,
friction_epsilon,
particle_radius,
shape_material_mu,
shape_body,
body_q,
body_q_prev,
body_qd,
body_com,
contact_shape,
contact_body_pos,
contact_body_vel,
contact_normal,
dt,
)
wp.atomic_add(particle_forces, particle_idx, body_contact_force)
wp.atomic_add(particle_hessians, particle_idx, body_contact_hessian)
@wp.kernel
def solve_elasticity_tile(
dt: float,
particle_ids_in_color: wp.array[wp.int32],
pos_prev: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
mass: wp.array[float],
inertia: wp.array[wp.vec3],
particle_flags: wp.array[wp.int32],
tri_indices: wp.array2d[wp.int32],
tri_poses: wp.array[wp.mat22],
tri_materials: wp.array2d[float],
tri_areas: wp.array[float],
edge_indices: wp.array2d[wp.int32],
edge_rest_angles: wp.array[float],
edge_rest_length: wp.array[float],
edge_bending_properties: wp.array2d[float],
tet_indices: wp.array2d[wp.int32],
tet_poses: wp.array[wp.mat33],
tet_materials: wp.array2d[float],
particle_adjacency: ParticleForceElementAdjacencyInfo,
particle_forces: wp.array[wp.vec3],
particle_hessians: wp.array[wp.mat33],
# output
particle_displacements: wp.array[wp.vec3],
):
tid = wp.tid()
block_idx = tid // TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE
thread_idx = tid % TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE
particle_index = particle_ids_in_color[block_idx]
if not particle_flags[particle_index] & ParticleFlags.ACTIVE or mass[particle_index] == 0:
if thread_idx == 0:
particle_displacements[particle_index] = wp.vec3(0.0)
return
dt_sqr_reciprocal = 1.0 / (dt * dt)
# elastic force and hessian
num_adj_faces = get_vertex_num_adjacent_faces(particle_adjacency, particle_index)
f = wp.vec3(0.0)
h = wp.mat33(0.0)
batch_counter = wp.int32(0)
if tri_indices:
# loop through all the adjacent triangles using whole block
while batch_counter + thread_idx < num_adj_faces:
adj_tri_counter = thread_idx + batch_counter
batch_counter += TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE
# elastic force and hessian
tri_index, vertex_order = get_vertex_adjacent_face_id_order(
particle_adjacency, particle_index, adj_tri_counter
)
# fmt: off
if wp.static("connectivity" in VBD_DEBUG_PRINTING_OPTIONS):
wp.printf(
"particle: %d | num_adj_faces: %d | ",
particle_index,
get_vertex_num_adjacent_faces(particle_adjacency, particle_index),
)
wp.printf("i_face: %d | face id: %d | v_order: %d | ", adj_tri_counter, tri_index, vertex_order)
wp.printf(
"face: %d %d %d\n",
tri_indices[tri_index, 0],
tri_indices[tri_index, 1],
tri_indices[tri_index, 2],
)
# fmt: on
if tri_materials[tri_index, 0] > 0.0 or tri_materials[tri_index, 1] > 0.0:
f_tri, h_tri = evaluate_stvk_force_hessian(
tri_index,
vertex_order,
pos,
pos_prev,
tri_indices,
tri_poses[tri_index],
tri_areas[tri_index],
tri_materials[tri_index, 0],
tri_materials[tri_index, 1],
tri_materials[tri_index, 2],
dt,
)
f += f_tri
h += h_tri
if edge_indices:
batch_counter = wp.int32(0)
num_adj_edges = get_vertex_num_adjacent_edges(particle_adjacency, particle_index)
while batch_counter + thread_idx < num_adj_edges:
adj_edge_counter = batch_counter + thread_idx
batch_counter += TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE
nei_edge_index, vertex_order_on_edge = get_vertex_adjacent_edge_id_order(
particle_adjacency, particle_index, adj_edge_counter
)
if edge_bending_properties[nei_edge_index, 0] > 0.0:
f_edge, h_edge = evaluate_dihedral_angle_based_bending_force_hessian(
nei_edge_index,
vertex_order_on_edge,
pos,
pos_prev,
edge_indices,
edge_rest_angles,
edge_rest_length,
edge_bending_properties[nei_edge_index, 0],
edge_bending_properties[nei_edge_index, 1],
dt,
)
f += f_edge
h += h_edge
if tet_indices:
# solve tet elasticity
batch_counter = wp.int32(0)
num_adj_tets = get_vertex_num_adjacent_tets(particle_adjacency, particle_index)
while batch_counter + thread_idx < num_adj_tets:
adj_tet_counter = batch_counter + thread_idx
batch_counter += TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE
nei_tet_index, vertex_order_on_tet = get_vertex_adjacent_tet_id_order(
particle_adjacency, particle_index, adj_tet_counter
)
if tet_materials[nei_tet_index, 0] > 0.0 or tet_materials[nei_tet_index, 1] > 0.0:
f_tet, h_tet = evaluate_volumetric_neo_hookean_force_and_hessian(
nei_tet_index,
vertex_order_on_tet,
pos_prev,
pos,
tet_indices,
tet_poses[nei_tet_index],
tet_materials[nei_tet_index, 0],
tet_materials[nei_tet_index, 1],
tet_materials[nei_tet_index, 2],
dt,
)
f += f_tet
h += h_tet
f_tile = wp.tile(f, preserve_type=True)
h_tile = wp.tile(h, preserve_type=True)
f_total = wp.tile_reduce(wp.add, f_tile)[0]
h_total = wp.tile_reduce(wp.add, h_tile)[0]
if thread_idx == 0:
h_total = (
h_total
+ mass[particle_index] * dt_sqr_reciprocal * wp.identity(n=3, dtype=float)
+ particle_hessians[particle_index]
)
if abs(wp.determinant(h_total)) > 1e-8:
h_inv = wp.inverse(h_total)
f_total = (
f_total
+ mass[particle_index] * (inertia[particle_index] - pos[particle_index]) * (dt_sqr_reciprocal)
+ particle_forces[particle_index]
)
particle_displacements[particle_index] = particle_displacements[particle_index] + h_inv * f_total
@wp.kernel
def solve_elasticity(
dt: float,
particle_ids_in_color: wp.array[wp.int32],
pos_prev: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
mass: wp.array[float],
inertia: wp.array[wp.vec3],
particle_flags: wp.array[wp.int32],
tri_indices: wp.array2d[wp.int32],
tri_poses: wp.array[wp.mat22],
tri_materials: wp.array2d[float],
tri_areas: wp.array[float],
edge_indices: wp.array2d[wp.int32],
edge_rest_angles: wp.array[float],
edge_rest_length: wp.array[float],
edge_bending_properties: wp.array2d[float],
tet_indices: wp.array2d[wp.int32],
tet_poses: wp.array[wp.mat33],
tet_materials: wp.array2d[float],
particle_adjacency: ParticleForceElementAdjacencyInfo,
particle_forces: wp.array[wp.vec3],
particle_hessians: wp.array[wp.mat33],
# output
particle_displacements: wp.array[wp.vec3],
):
t_id = wp.tid()
particle_index = particle_ids_in_color[t_id]
if not particle_flags[particle_index] & ParticleFlags.ACTIVE or mass[particle_index] == 0:
particle_displacements[particle_index] = wp.vec3(0.0)
return
dt_sqr_reciprocal = 1.0 / (dt * dt)
# inertia force and hessian
f = mass[particle_index] * (inertia[particle_index] - pos[particle_index]) * (dt_sqr_reciprocal)
h = mass[particle_index] * dt_sqr_reciprocal * wp.identity(n=3, dtype=float)
# fmt: off
if wp.static("inertia_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS):
wp.printf(
"particle: %d after accumulate inertia\nforce:\n %f %f %f, \nhessian:, \n%f %f %f, \n%f %f %f, \n%f %f %f\n",
particle_index,
f[0], f[1], f[2], h[0, 0], h[0, 1], h[0, 2], h[1, 0], h[1, 1], h[1, 2], h[2, 0], h[2, 1], h[2, 2],
)
if tri_indices:
# elastic force and hessian
for i_adj_tri in range(get_vertex_num_adjacent_faces(particle_adjacency, particle_index)):
tri_index, vertex_order = get_vertex_adjacent_face_id_order(particle_adjacency, particle_index, i_adj_tri)
# fmt: off
if wp.static("connectivity" in VBD_DEBUG_PRINTING_OPTIONS):
wp.printf(
"particle: %d | num_adj_faces: %d | ",
particle_index,
get_vertex_num_adjacent_faces(particle_adjacency, particle_index),
)
wp.printf("i_face: %d | face id: %d | v_order: %d | ", i_adj_tri, tri_index, vertex_order)
wp.printf(
"face: %d %d %d\n",
tri_indices[tri_index, 0],
tri_indices[tri_index, 1],
tri_indices[tri_index, 2],
)
# fmt: on
if tri_materials[tri_index, 0] > 0.0 or tri_materials[tri_index, 1] > 0.0:
f_tri, h_tri = evaluate_stvk_force_hessian(
tri_index,
vertex_order,
pos,
pos_prev,
tri_indices,
tri_poses[tri_index],
tri_areas[tri_index],
tri_materials[tri_index, 0],
tri_materials[tri_index, 1],
tri_materials[tri_index, 2],
dt,
)
f = f + f_tri
h = h + h_tri
if edge_indices:
for i_adj_edge in range(get_vertex_num_adjacent_edges(particle_adjacency, particle_index)):
nei_edge_index, vertex_order_on_edge = get_vertex_adjacent_edge_id_order(particle_adjacency, particle_index, i_adj_edge)
# vertex is on the edge; otherwise it only effects the bending energy n
if edge_bending_properties[nei_edge_index, 0] > 0.0:
f_edge, h_edge = evaluate_dihedral_angle_based_bending_force_hessian(
nei_edge_index, vertex_order_on_edge, pos, pos_prev, edge_indices, edge_rest_angles, edge_rest_length,
edge_bending_properties[nei_edge_index, 0], edge_bending_properties[nei_edge_index, 1], dt
)
f = f + f_edge
h = h + h_edge
if tet_indices:
# solve tet elasticity
num_adj_tets = get_vertex_num_adjacent_tets(particle_adjacency, particle_index)
for adj_tet_counter in range(num_adj_tets):
nei_tet_index, vertex_order_on_tet = get_vertex_adjacent_tet_id_order(
particle_adjacency, particle_index, adj_tet_counter
)
if tet_materials[nei_tet_index, 0] > 0.0 or tet_materials[nei_tet_index, 1] > 0.0:
f_tet, h_tet = evaluate_volumetric_neo_hookean_force_and_hessian(
nei_tet_index,
vertex_order_on_tet,
pos_prev,
pos,
tet_indices,
tet_poses[nei_tet_index],
tet_materials[nei_tet_index, 0],
tet_materials[nei_tet_index, 1],
tet_materials[nei_tet_index, 2],
dt,
)
f += f_tet
h += h_tet
# fmt: off
if wp.static("overall_force_hessian" in VBD_DEBUG_PRINTING_OPTIONS):
wp.printf(
"vertex: %d final\noverall force:\n %f %f %f, \noverall hessian:, \n%f %f %f, \n%f %f %f, \n%f %f %f\n",
particle_index,
f[0], f[1], f[2], h[0, 0], h[0, 1], h[0, 2], h[1, 0], h[1, 1], h[1, 2], h[2, 0], h[2, 1], h[2, 2],
)
# fmt: on
h = h + particle_hessians[particle_index]
f = f + particle_forces[particle_index]
if abs(wp.determinant(h)) > 1e-8:
h_inv = wp.inverse(h)
particle_displacements[particle_index] = particle_displacements[particle_index] + h_inv * f
@wp.kernel
def accumulate_contact_force_and_hessian(
# inputs
dt: float,
current_color: int,
pos_prev: wp.array[wp.vec3],
pos: wp.array[wp.vec3],
particle_colors: wp.array[int],
tri_indices: wp.array2d[wp.int32],
edge_indices: wp.array2d[wp.int32],
# self contact
collision_info_array: wp.array[TriMeshCollisionInfo],
collision_radius: float,
soft_contact_ke: float,
soft_contact_kd: float,
friction_mu: float,
friction_epsilon: float,
edge_edge_parallel_epsilon: float,
# body-particle contact
particle_radius: wp.array[float],
soft_contact_particle: wp.array[int],
contact_count: wp.array[int],
contact_max: int,
shape_material_mu: wp.array[float],
shape_body: wp.array[int],
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
contact_shape: wp.array[int],
contact_body_pos: wp.array[wp.vec3],
contact_body_vel: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
# outputs: particle force and hessian
particle_forces: wp.array[wp.vec3],
particle_hessians: wp.array[wp.mat33],
):
t_id = wp.tid()
collision_info = collision_info_array[0]
primitive_id = t_id // NUM_THREADS_PER_COLLISION_PRIMITIVE
t_id_current_primitive = t_id % NUM_THREADS_PER_COLLISION_PRIMITIVE
# process edge-edge collisions
if primitive_id < collision_info.edge_colliding_edges_buffer_sizes.shape[0]:
e1_idx = primitive_id
collision_buffer_counter = t_id_current_primitive
collision_buffer_offset = collision_info.edge_colliding_edges_offsets[primitive_id]
while collision_buffer_counter < collision_info.edge_colliding_edges_buffer_sizes[primitive_id]:
e2_idx = collision_info.edge_colliding_edges[2 * (collision_buffer_offset + collision_buffer_counter) + 1]
if e1_idx != -1 and e2_idx != -1:
e1_v1 = edge_indices[e1_idx, 2]
e1_v2 = edge_indices[e1_idx, 3]
c_e1_v1 = particle_colors[e1_v1]
c_e1_v2 = particle_colors[e1_v2]
if c_e1_v1 == current_color or c_e1_v2 == current_color:
has_contact, collision_force_0, collision_force_1, collision_hessian_0, collision_hessian_1 = (
evaluate_edge_edge_contact_2_vertices(
e1_idx,
e2_idx,
pos,
pos_prev,
edge_indices,
collision_radius,
soft_contact_ke,
soft_contact_kd,
friction_mu,
friction_epsilon,
dt,
edge_edge_parallel_epsilon,
)
)
if has_contact:
# here we only handle the e1 side, because e2 will also detection this contact and add force and hessian on its own
if c_e1_v1 == current_color:
wp.atomic_add(particle_forces, e1_v1, collision_force_0)
wp.atomic_add(particle_hessians, e1_v1, collision_hessian_0)
if c_e1_v2 == current_color:
wp.atomic_add(particle_forces, e1_v2, collision_force_1)
wp.atomic_add(particle_hessians, e1_v2, collision_hessian_1)
collision_buffer_counter += NUM_THREADS_PER_COLLISION_PRIMITIVE
# process vertex-triangle collisions
if primitive_id < collision_info.vertex_colliding_triangles_buffer_sizes.shape[0]:
particle_idx = primitive_id
collision_buffer_counter = t_id_current_primitive
collision_buffer_offset = collision_info.vertex_colliding_triangles_offsets[primitive_id]
while collision_buffer_counter < collision_info.vertex_colliding_triangles_buffer_sizes[primitive_id]:
tri_idx = collision_info.vertex_colliding_triangles[
(collision_buffer_offset + collision_buffer_counter) * 2 + 1
]
if particle_idx != -1 and tri_idx != -1:
tri_a = tri_indices[tri_idx, 0]
tri_b = tri_indices[tri_idx, 1]
tri_c = tri_indices[tri_idx, 2]
c_v = particle_colors[particle_idx]
c_tri_a = particle_colors[tri_a]
c_tri_b = particle_colors[tri_b]
c_tri_c = particle_colors[tri_c]
if (
c_v == current_color
or c_tri_a == current_color
or c_tri_b == current_color
or c_tri_c == current_color
):
(
has_contact,
collision_force_0,
collision_force_1,
collision_force_2,
collision_force_3,
collision_hessian_0,
collision_hessian_1,
collision_hessian_2,
collision_hessian_3,
) = evaluate_vertex_triangle_collision_force_hessian_4_vertices(
particle_idx,
tri_idx,
pos,
pos_prev,
tri_indices,
collision_radius,
soft_contact_ke,
soft_contact_kd,
friction_mu,
friction_epsilon,
dt,
)
if has_contact:
# particle
if c_v == current_color:
wp.atomic_add(particle_forces, particle_idx, collision_force_3)
wp.atomic_add(particle_hessians, particle_idx, collision_hessian_3)
# tri_a
if c_tri_a == current_color:
wp.atomic_add(particle_forces, tri_a, collision_force_0)
wp.atomic_add(particle_hessians, tri_a, collision_hessian_0)
# tri_b
if c_tri_b == current_color:
wp.atomic_add(particle_forces, tri_b, collision_force_1)
wp.atomic_add(particle_hessians, tri_b, collision_hessian_1)
# tri_c
if c_tri_c == current_color:
wp.atomic_add(particle_forces, tri_c, collision_force_2)
wp.atomic_add(particle_hessians, tri_c, collision_hessian_2)
collision_buffer_counter += NUM_THREADS_PER_COLLISION_PRIMITIVE
particle_body_contact_count = min(contact_max, contact_count[0])
if t_id < particle_body_contact_count:
particle_idx = soft_contact_particle[t_id]
if particle_colors[particle_idx] == current_color:
body_contact_force, body_contact_hessian = evaluate_body_particle_contact(
particle_idx,
pos[particle_idx],
pos_prev[particle_idx],
t_id,
soft_contact_ke,
soft_contact_kd,
friction_mu,
friction_epsilon,
particle_radius,
shape_material_mu,
shape_body,
body_q,
body_q_prev,
body_qd,
body_com,
contact_shape,
contact_body_pos,
contact_body_vel,
contact_normal,
dt,
)
wp.atomic_add(particle_forces, particle_idx, body_contact_force)
wp.atomic_add(particle_hessians, particle_idx, body_contact_hessian)
================================================
FILE: newton/_src/solvers/vbd/rigid_vbd_kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Rigid body VBD solver kernels and utilities.
This module contains all rigid body-specific kernels, device functions, data structures,
and constants for the VBD solver's rigid body domain (AVBD algorithm).
Organization:
- Constants: Solver parameters and thresholds
- Data structures: RigidForceElementAdjacencyInfo and related structs
- Device functions: Helper functions for rigid body dynamics
- Utility kernels: Adjacency building
- Pre-iteration kernels: Forward integration, warmstarting, Dahl parameter computation
- Iteration kernels: Contact accumulation, rigid body solve, dual updates
- Post-iteration kernels: Velocity updates, Dahl state updates
"""
import warp as wp
from newton._src.core.types import MAXVAL
from newton._src.math import quat_velocity
from newton._src.sim import JointType
from newton._src.solvers.solver import integrate_rigid_body
wp.set_module_options({"enable_backward": False})
# ---------------------------------
# Constants
# ---------------------------------
_SMALL_ANGLE_EPS = wp.constant(1.0e-7)
"""Small-angle threshold [rad] for guards and series expansions"""
_SMALL_ANGLE_EPS2 = _SMALL_ANGLE_EPS * _SMALL_ANGLE_EPS
"""Square of _SMALL_ANGLE_EPS"""
_DRIVE_LIMIT_MODE_NONE = wp.constant(0)
_DRIVE_LIMIT_MODE_LIMIT_LOWER = wp.constant(1)
_DRIVE_LIMIT_MODE_LIMIT_UPPER = wp.constant(2)
_DRIVE_LIMIT_MODE_DRIVE = wp.constant(3)
_SMALL_LENGTH_EPS = wp.constant(1.0e-9)
"""Small length tolerance (e.g., segment length checks)"""
_USE_SMALL_ANGLE_APPROX = wp.constant(True)
"""If True use first-order small-angle rotation approximation; if False use closed-form rotation update"""
_DAHL_KAPPADOT_DEADBAND = wp.constant(1.0e-6)
"""Deadband threshold for hysteresis direction selection"""
_NUM_CONTACT_THREADS_PER_BODY = 16
"""Threads per body for contact accumulation using strided iteration"""
# ---------------------------------
# Helper classes and device functions
# ---------------------------------
class vec6(wp.types.vector(length=6, dtype=wp.float32)):
"""Packed lower-triangular 3x3 matrix storage: [L00, L10, L11, L20, L21, L22]."""
pass
@wp.func
def chol33(A: wp.mat33) -> vec6:
"""
Compute Cholesky factorization A = L*L^T for 3x3 SPD matrix.
Uses packed storage for lower-triangular L to save memory and improve cache efficiency.
Packed format: [L00, L10, L11, L20, L21, L22] stores only the 6 non-zero elements.
Algorithm: Standard column-by-column Cholesky decomposition
Column 0: L00 = sqrt(a00), L10 = a10/L00, L20 = a20/L00
Column 1: L11 = sqrt(a11 - L10^2), L21 = (a21 - L20*L10)/L11
Column 2: L22 = sqrt(a22 - L20^2 - L21^2)
Args:
A: Symmetric positive-definite 3x3 matrix (only lower triangle is accessed)
Returns:
vec6: Packed lower-triangular Cholesky factor L
Layout: [L00, L10, L11, L20, L21, L22]
Represents: L = [[L00, 0, 0],
[L10, L11, 0],
[L20, L21, L22]]
Note: Assumes A is SPD. No checking for negative square roots.
"""
# Extract lower triangle (A is symmetric, only lower half needed)
a00 = A[0, 0]
a10 = A[1, 0]
a11 = A[1, 1]
a20 = A[2, 0]
a21 = A[2, 1]
a22 = A[2, 2]
# Column 0: Compute first column of L
L00 = wp.sqrt(a00)
L10 = a10 / L00
L20 = a20 / L00
# Column 1: Compute second column of L
L11 = wp.sqrt(a11 - L10 * L10)
L21 = (a21 - L20 * L10) / L11
# Column 2: Compute third column of L
L22 = wp.sqrt(a22 - L20 * L20 - L21 * L21)
# Pack into vec6: [L00, L10, L11, L20, L21, L22]
return vec6(L00, L10, L11, L20, L21, L22)
@wp.func
def chol33_solve(Lp: vec6, b: wp.vec3) -> wp.vec3:
"""
Solve A*x = b given packed Cholesky factorization A = L*L^T.
Uses two-stage triangular solve:
1. Forward substitution: L*y = b (solve for y)
2. Backward substitution: L^T*x = y (solve for x)
This is more efficient than computing A^-1 explicitly and avoids
numerical issues from matrix inversion.
Args:
Lp: Packed lower-triangular Cholesky factor from chol33()
Layout: [L00, L10, L11, L20, L21, L22]
b: Right-hand side vector
Returns:
vec3: Solution x to A*x = b
Complexity: 6 multiplies, 6 divides (optimal for 3x3)
"""
# Unpack Cholesky factor for readability
L00 = Lp[0]
L10 = Lp[1]
L11 = Lp[2]
L20 = Lp[3]
L21 = Lp[4]
L22 = Lp[5]
# Forward substitution: L*y = b
y0 = b[0] / L00
y1 = (b[1] - L10 * y0) / L11
y2 = (b[2] - L20 * y0 - L21 * y1) / L22
# Backward substitution: L^T*x = y
x2 = y2 / L22
x1 = (y1 - L21 * x2) / L11
x0 = (y0 - L10 * x1 - L20 * x2) / L00
return wp.vec3(x0, x1, x2)
@wp.func
def cable_get_kappa(q_wp: wp.quat, q_wc: wp.quat, q_wp_rest: wp.quat, q_wc_rest: wp.quat) -> wp.vec3:
"""Compute cable bending curvature vector kappa in the parent frame.
Kappa is the rotation vector (theta*axis) from the rest-aligned relative rotation.
Args:
q_wp: Parent orientation (world).
q_wc: Child orientation (world).
q_wp_rest: Parent rest orientation (world).
q_wc_rest: Child rest orientation (world).
Returns:
wp.vec3: Curvature vector kappa in parent frame (rotation vector form).
"""
# Build R_align = R_rel * R_rel_rest^T using quaternions
q_rel = wp.mul(wp.quat_inverse(q_wp), q_wc)
q_rel_rest = wp.mul(wp.quat_inverse(q_wp_rest), q_wc_rest)
q_align = wp.mul(q_rel, wp.quat_inverse(q_rel_rest))
# Enforce shortest path (w > 0) to avoid double-cover ambiguity
if q_align[3] < 0.0:
q_align = wp.quat(-q_align[0], -q_align[1], -q_align[2], -q_align[3])
# Log map to rotation vector
axis, angle = wp.quat_to_axis_angle(q_align)
return axis * angle
@wp.func
def compute_right_jacobian_inverse(kappa: wp.vec3) -> wp.mat33:
"""Inverse right Jacobian Jr^{-1}(kappa) for SO(3) rotation vectors.
Args:
kappa: Rotation vector theta*axis (any frame).
Returns:
wp.mat33: Jr^{-1}(kappa) in the same frame as kappa.
"""
theta = wp.length(kappa)
kappa_skew = wp.skew(kappa)
if (theta < _SMALL_ANGLE_EPS) or (_USE_SMALL_ANGLE_APPROX):
return wp.identity(3, float) + 0.5 * kappa_skew + (1.0 / 12.0) * (kappa_skew * kappa_skew)
sin_theta = wp.sin(theta)
cos_theta = wp.cos(theta)
b = (1.0 / (theta * theta)) - (1.0 + cos_theta) / (2.0 * theta * sin_theta)
return wp.identity(3, float) + 0.5 * kappa_skew + b * (kappa_skew * kappa_skew)
@wp.func
def compute_kappa_dot_analytic(
q_wp: wp.quat,
q_wc: wp.quat,
q_wp_rest: wp.quat,
q_wc_rest: wp.quat,
omega_p_world: wp.vec3,
omega_c_world: wp.vec3,
kappa_now: wp.vec3,
) -> wp.vec3:
"""Analytical time derivative of curvature vector d(kappa)/dt in parent frame.
R_align = R_rel * R_rel_rest^T represents the rotation from rest to current configuration,
which is the same deformation measure used in cable_get_kappa. This removes the rest offset
so bending is measured relative to the undeformed state.
Args:
q_wp: Parent orientation (world).
q_wc: Child orientation (world).
q_wp_rest: Parent rest orientation (world).
q_wc_rest: Child rest orientation (world).
omega_p_world: Parent angular velocity (world) [rad/s].
omega_c_world: Child angular velocity (world) [rad/s].
kappa_now: Current curvature vector in parent frame.
Returns:
wp.vec3: Curvature rate kappa_dot in parent frame [rad/s].
"""
R_wp = wp.quat_to_matrix(q_wp)
omega_rel_parent = wp.transpose(R_wp) * (omega_c_world - omega_p_world)
q_rel = wp.quat_inverse(q_wp) * q_wc
q_rel_rest = wp.quat_inverse(q_wp_rest) * q_wc_rest
R_align = wp.quat_to_matrix(q_rel * wp.quat_inverse(q_rel_rest))
Jr_inv = compute_right_jacobian_inverse(kappa_now)
omega_right = wp.transpose(R_align) * omega_rel_parent
return Jr_inv * omega_right
@wp.func
def build_joint_projectors(
jt: int,
joint_axis: wp.array[wp.vec3],
qd_start: int,
lin_count: int,
ang_count: int,
q_wp_rot: wp.quat,
):
"""Build orthogonal-complement projectors P_lin and P_ang for a joint.
P = I - sum(ai * ai^T) removes free DOF directions.
Invariant: free axes must be orthonormal for P to be a valid projector (P^2 = P).
Args:
jt: Joint type (JointType enum).
joint_axis: Per-DOF axis directions.
qd_start: Start index into joint_axis for this joint's DOFs.
lin_count: Number of free linear DOFs (0 for most joints; 1 for PRISMATIC; 0-3 for D6).
ang_count: Number of free angular DOFs (0 for most joints; 1 for REVOLUTE; 0-3 for D6).
q_wp_rot: Parent joint frame rotation (world). Used to rotate linear axes to world space.
Returns:
(P_lin, P_ang): Orthogonal-complement projectors for linear and angular constraints.
"""
P_lin = wp.identity(3, float)
P_ang = wp.identity(3, float)
if jt == JointType.PRISMATIC:
a_w = wp.normalize(wp.quat_rotate(q_wp_rot, joint_axis[qd_start]))
P_lin = P_lin - wp.outer(a_w, a_w)
elif jt == JointType.D6:
if lin_count > 0:
a0_w = wp.normalize(wp.quat_rotate(q_wp_rot, joint_axis[qd_start]))
P_lin = P_lin - wp.outer(a0_w, a0_w)
if lin_count > 1:
a1_w = wp.normalize(wp.quat_rotate(q_wp_rot, joint_axis[qd_start + 1]))
P_lin = P_lin - wp.outer(a1_w, a1_w)
if lin_count > 2:
a2_w = wp.normalize(wp.quat_rotate(q_wp_rot, joint_axis[qd_start + 2]))
P_lin = P_lin - wp.outer(a2_w, a2_w)
if jt == JointType.REVOLUTE:
a = wp.normalize(joint_axis[qd_start])
P_ang = P_ang - wp.outer(a, a)
elif jt == JointType.D6:
if ang_count > 0:
a0 = wp.normalize(joint_axis[qd_start + lin_count])
P_ang = P_ang - wp.outer(a0, a0)
if ang_count > 1:
a1 = wp.normalize(joint_axis[qd_start + lin_count + 1])
P_ang = P_ang - wp.outer(a1, a1)
if ang_count > 2:
a2 = wp.normalize(joint_axis[qd_start + lin_count + 2])
P_ang = P_ang - wp.outer(a2, a2)
return P_lin, P_ang
@wp.func
def evaluate_angular_constraint_force_hessian(
q_wp: wp.quat,
q_wc: wp.quat,
q_wp_rest: wp.quat,
q_wc_rest: wp.quat,
q_wp_prev: wp.quat,
q_wc_prev: wp.quat,
is_parent: bool,
k_eff: float,
P: wp.mat33,
sigma0: wp.vec3,
C_fric: wp.vec3,
damping: float,
dt: float,
):
"""Projected angular constraint force/Hessian using rotation-vector error (kappa).
Unified evaluator for all joint types. Computes constraint force and Hessian
in the constrained subspace defined by the orthogonal-complement projector P.
Special cases by projector:
- P = I: isotropic (CABLE bend, FIXED angular)
- P = I - a*a^T: revolute (1 free angular axis)
- arbitrary P: D6 (0-3 free angular axes)
Dahl friction (sigma0, C_fric) is only valid when P = I (isotropic).
Pass vec3(0) for both when P != I.
Returns:
(tau_world, H_aa, kappa, J_world) -- constraint torque and Hessian in world
frame, plus the curvature vector and world-frame Jacobian for reuse by the
drive/limit block.
"""
inv_dt = 1.0 / dt
kappa_now_vec = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest)
kappa_perp = P * kappa_now_vec
Jr_inv = compute_right_jacobian_inverse(kappa_now_vec)
R_wp = wp.quat_to_matrix(q_wp)
q_rel = wp.quat_inverse(q_wp) * q_wc
q_rel_rest = wp.quat_inverse(q_wp_rest) * q_wc_rest
R_align = wp.quat_to_matrix(q_rel * wp.quat_inverse(q_rel_rest))
J_world = R_wp * (R_align * wp.transpose(Jr_inv))
f_local = k_eff * kappa_perp + sigma0
H_local = k_eff * P + wp.mat33(
C_fric[0],
0.0,
0.0,
0.0,
C_fric[1],
0.0,
0.0,
0.0,
C_fric[2],
)
if damping > 0.0:
omega_p_world = quat_velocity(q_wp, q_wp_prev, dt)
omega_c_world = quat_velocity(q_wc, q_wc_prev, dt)
dkappa_dt_vec = compute_kappa_dot_analytic(
q_wp, q_wc, q_wp_rest, q_wc_rest, omega_p_world, omega_c_world, kappa_now_vec
)
dkappa_perp = P * dkappa_dt_vec
f_damp_local = (damping * k_eff) * dkappa_perp
f_local = f_local + f_damp_local
k_damp = (damping * inv_dt) * k_eff
H_local = H_local + k_damp * P
H_aa = J_world * (H_local * wp.transpose(J_world))
tau_world = J_world * f_local
if not is_parent:
tau_world = -tau_world
return tau_world, H_aa, kappa_now_vec, J_world
@wp.func
def evaluate_linear_constraint_force_hessian(
X_wp: wp.transform,
X_wc: wp.transform,
X_wp_prev: wp.transform,
X_wc_prev: wp.transform,
parent_pose: wp.transform,
child_pose: wp.transform,
parent_com: wp.vec3,
child_com: wp.vec3,
is_parent: bool,
penalty_k: float,
P: wp.mat33,
damping: float,
dt: float,
):
"""Projected linear constraint force/Hessian for anchor coincidence.
Unified evaluator for all joint types. Computes C = x_c - x_p, projects
with P, and returns force/Hessian in world frame.
Special cases by projector:
- P = I: isotropic (BALL, CABLE stretch, FIXED linear, REVOLUTE linear)
- P = I - a*a^T: prismatic (1 free linear axis)
- arbitrary P: D6 (0-3 free linear axes)
Returns:
- force (wp.vec3): Linear force (world)
- torque (wp.vec3): Angular torque (world)
- H_ll (wp.mat33): Linear-linear block
- H_al (wp.mat33): Angular-linear block
- H_aa (wp.mat33): Angular-angular block
"""
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
if is_parent:
com_w = wp.transform_point(parent_pose, parent_com)
r = x_p - com_w
else:
com_w = wp.transform_point(child_pose, child_com)
r = x_c - com_w
C_vec = x_c - x_p
C_perp = P * C_vec
f_attachment = penalty_k * C_perp
rx = wp.skew(r)
K_point = penalty_k * P
H_ll = K_point
H_al = rx * K_point
H_aa = wp.transpose(rx) * K_point * rx
if damping > 0.0:
x_p_prev = wp.transform_get_translation(X_wp_prev)
x_c_prev = wp.transform_get_translation(X_wc_prev)
C_vec_prev = x_c_prev - x_p_prev
inv_dt = 1.0 / dt
dC_dt = (C_vec - C_vec_prev) * inv_dt
dC_dt_perp = P * dC_dt
damping_coeff = damping * penalty_k
f_damping = damping_coeff * dC_dt_perp
f_attachment = f_attachment + f_damping
damp_scale = damping * inv_dt
H_ll_damp = damp_scale * H_ll
H_al_damp = damp_scale * H_al
H_aa_damp = damp_scale * H_aa
H_ll = H_ll + H_ll_damp
H_al = H_al + H_al_damp
H_aa = H_aa + H_aa_damp
force = f_attachment if is_parent else -f_attachment
torque = wp.cross(r, force)
return force, torque, H_ll, H_al, H_aa
# ---------------------------------
# Data structures
# ---------------------------------
@wp.struct
class RigidForceElementAdjacencyInfo:
r"""
Stores adjacency information for rigid bodies and their connected joints using CSR (Compressed Sparse Row) format.
- body_adj_joints: Flattened array of joint IDs. Size is sum over all bodies of N_i, where N_i is the
number of joints connected to body i.
- body_adj_joints_offsets: Offset array indicating where each body's joint list starts.
Size is |B|+1 (number of bodies + 1).
The number of joints adjacent to body i is: body_adj_joints_offsets[i+1] - body_adj_joints_offsets[i]
"""
# Rigid body joint adjacency
body_adj_joints: wp.array[wp.int32]
body_adj_joints_offsets: wp.array[wp.int32]
def to(self, device):
if device == self.body_adj_joints.device:
return self
else:
adjacency_gpu = RigidForceElementAdjacencyInfo()
adjacency_gpu.body_adj_joints = self.body_adj_joints.to(device)
adjacency_gpu.body_adj_joints_offsets = self.body_adj_joints_offsets.to(device)
return adjacency_gpu
@wp.func
def get_body_num_adjacent_joints(adjacency: RigidForceElementAdjacencyInfo, body: wp.int32):
"""Number of joints adjacent to the given body from CSR offsets."""
return adjacency.body_adj_joints_offsets[body + 1] - adjacency.body_adj_joints_offsets[body]
@wp.func
def get_body_adjacent_joint_id(adjacency: RigidForceElementAdjacencyInfo, body: wp.int32, joint: wp.int32):
"""Joint id at local index `joint` within the body's CSR-adjacent joint list."""
offset = adjacency.body_adj_joints_offsets[body]
return adjacency.body_adj_joints[offset + joint]
@wp.func
def evaluate_rigid_contact_from_collision(
body_a_index: int,
body_b_index: int,
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
contact_point_a_local: wp.vec3, # Local contact point on body A
contact_point_b_local: wp.vec3, # Local contact point on body B
contact_normal: wp.vec3, # Contact normal (A to B)
penetration_depth: float, # Penetration depth (> 0 when penetrating)
contact_ke: float, # Contact normal stiffness
contact_kd: float, # Contact damping coefficient
friction_mu: float, # Coulomb friction coefficient
friction_epsilon: float, # Friction regularization parameter
dt: float,
):
"""Compute contact forces and 3x3 Hessian blocks for a rigid contact pair.
All returned forces, torques, and Hessians are in world frame.
Args:
body_a_index: Body A index (-1 for static/kinematic).
body_b_index: Body B index (-1 for static/kinematic).
body_q: Current body transforms (world frame).
body_q_prev: Previous body transforms (world frame) used as the
"previous" pose for finite-difference contact-relative velocity.
body_com: Body COM offsets (local body frame).
contact_point_a_local: Contact point on A (local body frame).
contact_point_b_local: Contact point on B (local body frame).
contact_normal: Unit normal from A to B (world frame).
penetration_depth: Penetration depth (> 0 when penetrating).
contact_ke: Normal stiffness.
contact_kd: Damping coefficient.
friction_mu: Coulomb friction coefficient.
friction_epsilon: Friction regularization length.
dt: Time window [s] for finite-difference contact damping/friction.
Returns:
tuple: (force_a, torque_a, h_ll_a, h_al_a, h_aa_a,
force_b, torque_b, h_ll_b, h_al_b, h_aa_b)
- h_ll: Linear-linear block
- h_al: Angular-linear block
- h_aa: Angular-angular block
"""
# Early exit: no penetration or zero stiffness
if penetration_depth <= 0.0 or contact_ke <= 0.0:
zero_vec = wp.vec3(0.0)
zero_mat = wp.mat33(0.0)
return (zero_vec, zero_vec, zero_mat, zero_mat, zero_mat, zero_vec, zero_vec, zero_mat, zero_mat, zero_mat)
if body_a_index < 0:
X_wa = wp.transform_identity()
X_wa_prev = wp.transform_identity()
body_a_com_local = wp.vec3(0.0)
else:
X_wa = body_q[body_a_index]
X_wa_prev = body_q_prev[body_a_index]
body_a_com_local = body_com[body_a_index]
if body_b_index < 0:
X_wb = wp.transform_identity()
X_wb_prev = wp.transform_identity()
body_b_com_local = wp.vec3(0.0)
else:
X_wb = body_q[body_b_index]
X_wb_prev = body_q_prev[body_b_index]
body_b_com_local = body_com[body_b_index]
# Centers of mass in world coordinates
x_com_a_now = wp.transform_point(X_wa, body_a_com_local)
x_com_b_now = wp.transform_point(X_wb, body_b_com_local)
# Contact points in world coordinates (current and previous)
x_c_a_now = wp.transform_point(X_wa, contact_point_a_local)
x_c_b_now = wp.transform_point(X_wb, contact_point_b_local)
x_c_a_prev = wp.transform_point(X_wa_prev, contact_point_a_local)
x_c_b_prev = wp.transform_point(X_wb_prev, contact_point_b_local)
# Contact motion for damping and friction (finite difference velocity estimation)
dx_a = x_c_a_now - x_c_a_prev # Motion of contact point on A over timestep dt
dx_b = x_c_b_now - x_c_b_prev # Motion of contact point on B over timestep dt
dx_rel = dx_b - dx_a # Relative contact motion (B relative to A)
# Contact geometry - assume contact_normal is already unit length from collision detection
# Normal force: f = contact_ke * penetration
n_outer = wp.outer(contact_normal, contact_normal)
f_total = contact_normal * (contact_ke * penetration_depth)
K_total = contact_ke * n_outer
# Compute relative velocity for damping and friction
v_rel = dx_rel / dt
v_dot_n = wp.dot(contact_normal, v_rel)
# Damping only when compressing (v_n < 0, bodies approaching)
if contact_kd > 0.0 and v_dot_n < 0.0:
damping_coeff = contact_kd * contact_ke
damping_force = -damping_coeff * v_dot_n * contact_normal
damping_hessian = (damping_coeff / dt) * n_outer
f_total = f_total + damping_force
K_total = K_total + damping_hessian
# Normal load for friction (elastic normal only)
normal_load = contact_ke * penetration_depth
# Friction forces (isotropic, no explicit tangent basis)
if friction_mu > 0.0 and normal_load > 0.0:
# Tangential slip (world space)
v_n = contact_normal * v_dot_n
v_t = v_rel - v_n
u = v_t * dt
eps_u = friction_epsilon * dt
# Projected isotropic friction (no explicit tangent basis)
f_friction, K_friction = compute_projected_isotropic_friction(
friction_mu, normal_load, contact_normal, u, eps_u
)
f_total = f_total + f_friction
K_total = K_total + K_friction
# Split total contact force to both bodies (Newton's 3rd law)
force_a = -f_total # Force on A (opposite to normal, pushes A away from B)
force_b = f_total # Force on B (along normal, pushes B away from A)
# Torque arms and resulting torques
r_a = x_c_a_now - x_com_a_now # Moment arm from A's COM to contact point
r_b = x_c_b_now - x_com_b_now # Moment arm from B's COM to contact point
# Angular/linear coupling using contact-point Jacobian J = [-[r]x, I]
r_a_skew = wp.skew(r_a)
r_a_skew_T_K = wp.transpose(r_a_skew) * K_total
torque_a = wp.cross(r_a, force_a)
h_aa_a = r_a_skew_T_K * r_a_skew
h_al_a = -r_a_skew_T_K
h_ll_a = K_total # Linear-linear
r_b_skew = wp.skew(r_b)
r_b_skew_T_K = wp.transpose(r_b_skew) * K_total
torque_b = wp.cross(r_b, force_b)
h_aa_b = r_b_skew_T_K * r_b_skew
h_al_b = -r_b_skew_T_K
h_ll_b = K_total # Linear-linear
return (force_a, torque_a, h_ll_a, h_al_a, h_aa_a, force_b, torque_b, h_ll_b, h_al_b, h_aa_b)
@wp.func
def evaluate_body_particle_contact(
particle_index: int,
particle_pos: wp.vec3,
particle_prev_pos: wp.vec3,
contact_index: int,
body_particle_contact_ke: float,
body_particle_contact_kd: float,
friction_mu: float,
friction_epsilon: float,
particle_radius: wp.array[float],
shape_material_mu: wp.array[float],
shape_body: wp.array[int],
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
contact_shape: wp.array[int],
contact_body_pos: wp.array[wp.vec3],
contact_body_vel: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
dt: float,
):
"""
Evaluate particle-rigid body contact force and Hessian (on particle side).
Computes contact forces and Hessians for a particle interacting with a rigid body shape.
The function is agnostic to whether the rigid body is static, kinematic, or dynamic.
Contact model:
- Normal: Linear spring-damper (stiffness: body_particle_contact_ke, damping: body_particle_contact_kd)
- Friction: 3D projector-based Coulomb friction with IPC regularization
- Normal direction: Points from rigid surface towards particle (into particle)
Args:
particle_index: Index of the particle
particle_pos: Current particle position (world frame)
particle_prev_pos: Previous particle position (world frame) used as the
"previous" position for finite-difference contact-relative velocity.
contact_index: Index in the body-particle contact arrays
body_particle_contact_ke: Contact stiffness (model-level or AVBD adaptive)
body_particle_contact_kd: Contact damping (model-level or AVBD averaged)
friction_mu: Friction coefficient (model-level or AVBD averaged)
friction_epsilon: Friction regularization distance
particle_radius: Array of particle radii
shape_material_mu: Array of shape friction coefficients
shape_body: Array mapping shape index to body index
body_q: Current body transforms
body_q_prev: Previous body transforms (for finite-difference body
velocity when available)
body_qd: Body spatial velocities (fallback when no previous pose is provided)
body_com: Body centers of mass (local frame)
contact_shape: Array of shape indices for each soft contact
contact_body_pos: Array of contact points (local to shape)
contact_body_vel: Array of contact velocities (local frame)
contact_normal: Array of contact normals (world frame, from rigid to particle)
dt: Time window [s] used for finite-difference damping/friction.
Returns:
tuple[wp.vec3, wp.mat33]: (force, Hessian) on the particle (world frame)
"""
shape_index = contact_shape[contact_index]
body_index = shape_body[shape_index]
X_wb = wp.transform_identity()
X_com = wp.vec3()
if body_index >= 0:
X_wb = body_q[body_index]
X_com = body_com[body_index]
# body position in world space
bx = wp.transform_point(X_wb, contact_body_pos[contact_index])
n = contact_normal[contact_index]
penetration_depth = -(wp.dot(n, particle_pos - bx) - particle_radius[particle_index])
if penetration_depth > 0.0:
body_contact_force_norm = penetration_depth * body_particle_contact_ke
body_contact_force = n * body_contact_force_norm
body_contact_hessian = body_particle_contact_ke * wp.outer(n, n)
# Combine body-particle friction and shape material friction using geometric mean.
mu = wp.sqrt(friction_mu * shape_material_mu[shape_index])
dx = particle_pos - particle_prev_pos
if wp.dot(n, dx) < 0.0:
# Damping coefficient is scaled by contact stiffness (consistent with rigid-rigid)
damping_coeff = body_particle_contact_kd * body_particle_contact_ke
damping_hessian = (damping_coeff / dt) * wp.outer(n, n)
body_contact_hessian = body_contact_hessian + damping_hessian
body_contact_force = body_contact_force - damping_hessian * dx
# body velocity
if body_q_prev:
# if body_q_prev is available, compute velocity using finite difference method
# this is more accurate for simulating static friction
X_wb_prev = wp.transform_identity()
if body_index >= 0:
X_wb_prev = body_q_prev[body_index]
bx_prev = wp.transform_point(X_wb_prev, contact_body_pos[contact_index])
bv = (bx - bx_prev) / dt + wp.transform_vector(X_wb, contact_body_vel[contact_index])
else:
# otherwise use the instantaneous velocity
r = bx - wp.transform_point(X_wb, X_com)
body_v_s = wp.spatial_vector()
if body_index >= 0:
body_v_s = body_qd[body_index]
body_w = wp.spatial_bottom(body_v_s)
body_v = wp.spatial_top(body_v_s)
# compute the body velocity at the particle position
bv = body_v + wp.cross(body_w, r) + wp.transform_vector(X_wb, contact_body_vel[contact_index])
relative_translation = dx - bv * dt
# Friction using 3D projector approach (consistent with rigid-rigid contacts)
eps_u = friction_epsilon * dt
friction_force, friction_hessian = compute_projected_isotropic_friction(
mu, body_contact_force_norm, n, relative_translation, eps_u
)
body_contact_force = body_contact_force + friction_force
body_contact_hessian = body_contact_hessian + friction_hessian
else:
body_contact_force = wp.vec3(0.0, 0.0, 0.0)
body_contact_hessian = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
return body_contact_force, body_contact_hessian
@wp.func
def compute_projected_isotropic_friction(
friction_mu: float,
normal_load: float,
n_hat: wp.vec3,
slip_u: wp.vec3,
eps_u: float,
) -> tuple[wp.vec3, wp.mat33]:
"""Isotropic Coulomb friction in world frame using projector P = I - n n^T.
Regularization: if ||u_t|| <= eps_u, uses a linear ramp; otherwise 1/||u_t||.
Args:
friction_mu: Coulomb friction coefficient (>= 0).
normal_load: Normal load magnitude (>= 0).
n_hat: Unit contact normal (world frame).
slip_u: Tangential slip displacement over dt (world frame).
eps_u: Smoothing distance (same units as slip_u, > 0).
Returns:
tuple[wp.vec3, wp.mat33]: (force, Hessian) in world frame.
"""
# Tangential slip in the contact tangent plane without forming P: u_t = u - n * (n dot u)
dot_nu = wp.dot(n_hat, slip_u)
u_t = slip_u - n_hat * dot_nu
u_norm = wp.length(u_t)
if u_norm > 0.0:
# IPC-style regularization
if u_norm > eps_u:
f1_SF_over_x = 1.0 / u_norm
else:
f1_SF_over_x = (-u_norm / eps_u + 2.0) / eps_u
# Factor common scalar; force aligned with u_t, Hessian proportional to projector
scale = friction_mu * normal_load * f1_SF_over_x
f = -(scale * u_t)
K = scale * (wp.identity(3, float) - wp.outer(n_hat, n_hat))
else:
f = wp.vec3(0.0)
K = wp.mat33(0.0)
return f, K
@wp.func
def resolve_drive_limit_mode(
q: float,
target_pos: float,
lim_lower: float,
lim_upper: float,
has_drive: bool,
has_limits: bool,
):
mode = _DRIVE_LIMIT_MODE_NONE
err_pos = float(0.0)
drive_target = target_pos
if has_limits:
drive_target = wp.clamp(target_pos, lim_lower, lim_upper)
if q < lim_lower:
mode = _DRIVE_LIMIT_MODE_LIMIT_LOWER
err_pos = q - lim_lower
elif q > lim_upper:
mode = _DRIVE_LIMIT_MODE_LIMIT_UPPER
err_pos = q - lim_upper
if mode == _DRIVE_LIMIT_MODE_NONE and has_drive:
mode = _DRIVE_LIMIT_MODE_DRIVE
err_pos = q - drive_target
return mode, err_pos
@wp.func
def compute_kappa_and_jacobian(
q_wp: wp.quat,
q_wc: wp.quat,
q_wp_rest: wp.quat,
q_wc_rest: wp.quat,
):
kappa = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest)
Jr_inv = compute_right_jacobian_inverse(kappa)
R_wp = wp.quat_to_matrix(q_wp)
q_rel = wp.quat_inverse(q_wp) * q_wc
q_rel_rest = wp.quat_inverse(q_wp_rest) * q_wc_rest
R_align = wp.quat_to_matrix(q_rel * wp.quat_inverse(q_rel_rest))
J_world = R_wp * (R_align * wp.transpose(Jr_inv))
return kappa, J_world
@wp.func
def apply_angular_drive_limit_torque(
a: wp.vec3,
J_world: wp.mat33,
is_parent: bool,
f_scalar: float,
H_scalar: float,
):
f_local = f_scalar * a
H_local = H_scalar * wp.outer(a, a)
tau = J_world * f_local
Haa = J_world * (H_local * wp.transpose(J_world))
if not is_parent:
tau = -tau
return tau, Haa
@wp.func
def apply_linear_drive_limit_force(
axis_w: wp.vec3,
r: wp.vec3,
is_parent: bool,
f_scalar: float,
H_scalar: float,
):
f_attachment = f_scalar * axis_w
aa = wp.outer(axis_w, axis_w)
K_point = H_scalar * aa
rx = wp.skew(r)
Hll = K_point
Hal = rx * K_point
Haa = wp.transpose(rx) * K_point * rx
force = f_attachment if is_parent else -f_attachment
torque = wp.cross(r, force)
return force, torque, Hll, Hal, Haa
@wp.func
def evaluate_joint_force_hessian(
body_index: int,
joint_index: int,
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_q_rest: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_qd_start: wp.array[int],
joint_constraint_start: wp.array[int],
joint_penalty_k: wp.array[float],
joint_penalty_kd: wp.array[float],
joint_sigma_start: wp.array[wp.vec3],
joint_C_fric: wp.array[wp.vec3],
# Drive parameters (DOF-indexed via joint_qd_start)
joint_target_ke: wp.array[float],
joint_target_kd: wp.array[float],
joint_target_pos: wp.array[float],
joint_target_vel: wp.array[float],
# Limit parameters (DOF-indexed via joint_qd_start)
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
joint_limit_ke: wp.array[float],
joint_limit_kd: wp.array[float],
joint_dof_dim: wp.array2d[int],
joint_rest_angle: wp.array[float],
dt: float,
):
"""Compute AVBD joint force and Hessian contributions for one body.
Supported joint types: CABLE, BALL, FIXED, REVOLUTE, PRISMATIC, D6.
Uses unified projector-based constraint evaluators for all joint types.
Indexing:
``joint_constraint_start[j]`` is a solver-owned start offset into the per-constraint arrays
(``joint_penalty_k``, ``joint_penalty_kd``). The layout is:
- ``JointType.CABLE``: 2 scalars -> [stretch (linear), bend (angular)]
- ``JointType.BALL``: 1 scalar -> [linear]
- ``JointType.FIXED``: 2 scalars -> [linear, angular]
- ``JointType.REVOLUTE``: 3 scalars -> [linear, angular, ang_drive_limit]
- ``JointType.PRISMATIC``: 3 scalars -> [linear, angular, lin_drive_limit]
- ``JointType.D6``: 2 + lin_count + ang_count scalars
-> [linear, angular, lin_dl_0, ..., ang_dl_0, ...]
"""
jt = joint_type[joint_index]
if (
jt != JointType.CABLE
and jt != JointType.BALL
and jt != JointType.FIXED
and jt != JointType.REVOLUTE
and jt != JointType.PRISMATIC
and jt != JointType.D6
):
return wp.vec3(0.0), wp.vec3(0.0), wp.mat33(0.0), wp.mat33(0.0), wp.mat33(0.0)
if not joint_enabled[joint_index]:
return wp.vec3(0.0), wp.vec3(0.0), wp.mat33(0.0), wp.mat33(0.0), wp.mat33(0.0)
parent_index = joint_parent[joint_index]
child_index = joint_child[joint_index]
if body_index != child_index and (parent_index < 0 or body_index != parent_index):
return wp.vec3(0.0), wp.vec3(0.0), wp.mat33(0.0), wp.mat33(0.0), wp.mat33(0.0)
is_parent_body = parent_index >= 0 and body_index == parent_index
X_pj = joint_X_p[joint_index]
X_cj = joint_X_c[joint_index]
if parent_index >= 0:
parent_pose = body_q[parent_index]
parent_pose_prev = body_q_prev[parent_index]
parent_pose_rest = body_q_rest[parent_index]
parent_com = body_com[parent_index]
else:
parent_pose = wp.transform(wp.vec3(0.0), wp.quat_identity())
parent_pose_prev = parent_pose
parent_pose_rest = parent_pose
parent_com = wp.vec3(0.0)
child_pose = body_q[child_index]
child_pose_prev = body_q_prev[child_index]
child_pose_rest = body_q_rest[child_index]
child_com = body_com[child_index]
X_wp = parent_pose * X_pj
X_wc = child_pose * X_cj
X_wp_prev = parent_pose_prev * X_pj
X_wc_prev = child_pose_prev * X_cj
X_wp_rest = parent_pose_rest * X_pj
X_wc_rest = child_pose_rest * X_cj
c_start = joint_constraint_start[joint_index]
# Hoist quaternion extraction (shared by all angular constraints and drive/limits)
q_wp = wp.transform_get_rotation(X_wp)
q_wc = wp.transform_get_rotation(X_wc)
q_wp_rest = wp.transform_get_rotation(X_wp_rest)
q_wc_rest = wp.transform_get_rotation(X_wc_rest)
q_wp_prev = wp.transform_get_rotation(X_wp_prev)
q_wc_prev = wp.transform_get_rotation(X_wc_prev)
P_I = wp.identity(3, float)
no_dahl = wp.vec3(0.0)
if jt == JointType.CABLE:
k_stretch = joint_penalty_k[c_start]
k_bend = joint_penalty_k[c_start + 1]
kd_stretch = joint_penalty_kd[c_start]
kd_bend = joint_penalty_kd[c_start + 1]
total_force = wp.vec3(0.0)
total_torque = wp.vec3(0.0)
total_H_ll = wp.mat33(0.0)
total_H_al = wp.mat33(0.0)
total_H_aa = wp.mat33(0.0)
if k_bend > 0.0:
sigma0 = joint_sigma_start[joint_index]
C_fric = joint_C_fric[joint_index]
bend_torque, bend_H_aa, _bend_kappa, _bend_J = evaluate_angular_constraint_force_hessian(
q_wp,
q_wc,
q_wp_rest,
q_wc_rest,
q_wp_prev,
q_wc_prev,
is_parent_body,
k_bend,
P_I,
sigma0,
C_fric,
kd_bend,
dt,
)
total_torque = total_torque + bend_torque
total_H_aa = total_H_aa + bend_H_aa
if k_stretch > 0.0:
f_s, t_s, Hll_s, Hal_s, Haa_s = evaluate_linear_constraint_force_hessian(
X_wp,
X_wc,
X_wp_prev,
X_wc_prev,
parent_pose,
child_pose,
parent_com,
child_com,
is_parent_body,
k_stretch,
P_I,
kd_stretch,
dt,
)
total_force = total_force + f_s
total_torque = total_torque + t_s
total_H_ll = total_H_ll + Hll_s
total_H_al = total_H_al + Hal_s
total_H_aa = total_H_aa + Haa_s
return total_force, total_torque, total_H_ll, total_H_al, total_H_aa
elif jt == JointType.BALL:
k = joint_penalty_k[c_start]
damping = joint_penalty_kd[c_start]
if k > 0.0:
return evaluate_linear_constraint_force_hessian(
X_wp,
X_wc,
X_wp_prev,
X_wc_prev,
parent_pose,
child_pose,
parent_com,
child_com,
is_parent_body,
k,
P_I,
damping,
dt,
)
return wp.vec3(0.0), wp.vec3(0.0), wp.mat33(0.0), wp.mat33(0.0), wp.mat33(0.0)
elif jt == JointType.FIXED:
k_lin = joint_penalty_k[c_start + 0]
kd_lin = joint_penalty_kd[c_start + 0]
if k_lin > 0.0:
f_lin, t_lin, Hll_lin, Hal_lin, Haa_lin = evaluate_linear_constraint_force_hessian(
X_wp,
X_wc,
X_wp_prev,
X_wc_prev,
parent_pose,
child_pose,
parent_com,
child_com,
is_parent_body,
k_lin,
P_I,
kd_lin,
dt,
)
else:
f_lin = wp.vec3(0.0)
t_lin = wp.vec3(0.0)
Hll_lin = wp.mat33(0.0)
Hal_lin = wp.mat33(0.0)
Haa_lin = wp.mat33(0.0)
k_ang = joint_penalty_k[c_start + 1]
kd_ang = joint_penalty_kd[c_start + 1]
if k_ang > 0.0:
t_ang, Haa_ang, _ang_kappa, _ang_J = evaluate_angular_constraint_force_hessian(
q_wp,
q_wc,
q_wp_rest,
q_wc_rest,
q_wp_prev,
q_wc_prev,
is_parent_body,
k_ang,
P_I,
no_dahl,
no_dahl,
kd_ang,
dt,
)
else:
t_ang = wp.vec3(0.0)
Haa_ang = wp.mat33(0.0)
return f_lin, t_lin + t_ang, Hll_lin, Hal_lin, Haa_lin + Haa_ang
elif jt == JointType.REVOLUTE:
qd_start = joint_qd_start[joint_index]
P_lin, P_ang = build_joint_projectors(jt, joint_axis, qd_start, 0, 1, q_wp)
a = wp.normalize(joint_axis[qd_start])
k_lin = joint_penalty_k[c_start + 0]
kd_lin = joint_penalty_kd[c_start + 0]
if k_lin > 0.0:
f_lin, t_lin, Hll_lin, Hal_lin, Haa_lin = evaluate_linear_constraint_force_hessian(
X_wp,
X_wc,
X_wp_prev,
X_wc_prev,
parent_pose,
child_pose,
parent_com,
child_com,
is_parent_body,
k_lin,
P_lin,
kd_lin,
dt,
)
else:
f_lin = wp.vec3(0.0)
t_lin = wp.vec3(0.0)
Hll_lin = wp.mat33(0.0)
Hal_lin = wp.mat33(0.0)
Haa_lin = wp.mat33(0.0)
k_ang = joint_penalty_k[c_start + 1]
kd_ang = joint_penalty_kd[c_start + 1]
kappa_cached = wp.vec3(0.0)
J_world_cached = wp.mat33(0.0)
has_cached = False
if k_ang > 0.0:
t_ang, Haa_ang, kappa_cached, J_world_cached = evaluate_angular_constraint_force_hessian(
q_wp,
q_wc,
q_wp_rest,
q_wc_rest,
q_wp_prev,
q_wc_prev,
is_parent_body,
k_ang,
P_ang,
no_dahl,
no_dahl,
kd_ang,
dt,
)
has_cached = True
else:
t_ang = wp.vec3(0.0)
Haa_ang = wp.mat33(0.0)
# Drive + limits on free angular DOF
dof_idx = qd_start
model_drive_ke = joint_target_ke[dof_idx]
drive_kd = joint_target_kd[dof_idx]
target_pos = joint_target_pos[dof_idx]
target_vel = joint_target_vel[dof_idx]
lim_lower = joint_limit_lower[dof_idx]
lim_upper = joint_limit_upper[dof_idx]
model_limit_ke = joint_limit_ke[dof_idx]
lim_kd = joint_limit_kd[dof_idx]
has_drive = model_drive_ke > 0.0 or drive_kd > 0.0
has_limits = model_limit_ke > 0.0 and (lim_lower > -MAXVAL or lim_upper < MAXVAL)
avbd_ke = joint_penalty_k[c_start + 2]
drive_ke = wp.min(avbd_ke, model_drive_ke)
lim_ke = wp.min(avbd_ke, model_limit_ke)
if has_drive or has_limits:
inv_dt = 1.0 / dt
if has_cached:
kappa = kappa_cached
J_world = J_world_cached
else:
kappa, J_world = compute_kappa_and_jacobian(q_wp, q_wc, q_wp_rest, q_wc_rest)
theta = wp.dot(kappa, a)
theta_abs = theta + joint_rest_angle[dof_idx]
omega_p = quat_velocity(q_wp, q_wp_prev, dt)
omega_c = quat_velocity(q_wc, q_wc_prev, dt)
dkappa_dt = compute_kappa_dot_analytic(q_wp, q_wc, q_wp_rest, q_wc_rest, omega_p, omega_c, kappa)
dtheta_dt = wp.dot(dkappa_dt, a)
mode, err_pos = resolve_drive_limit_mode(theta_abs, target_pos, lim_lower, lim_upper, has_drive, has_limits)
f_scalar = float(0.0)
H_scalar = float(0.0)
if mode == _DRIVE_LIMIT_MODE_LIMIT_LOWER or mode == _DRIVE_LIMIT_MODE_LIMIT_UPPER:
lim_d = lim_kd * lim_ke
f_scalar = lim_ke * err_pos + lim_d * dtheta_dt
H_scalar = lim_ke + lim_d * inv_dt
elif mode == _DRIVE_LIMIT_MODE_DRIVE:
drive_d = drive_kd * drive_ke
vel_err = dtheta_dt - target_vel
f_scalar = drive_ke * err_pos + drive_d * vel_err
H_scalar = drive_ke + drive_d * inv_dt
if H_scalar > 0.0:
tau_drive, Haa_drive = apply_angular_drive_limit_torque(a, J_world, is_parent_body, f_scalar, H_scalar)
t_ang = t_ang + tau_drive
Haa_ang = Haa_ang + Haa_drive
return f_lin, t_lin + t_ang, Hll_lin, Hal_lin, Haa_lin + Haa_ang
elif jt == JointType.PRISMATIC:
qd_start = joint_qd_start[joint_index]
axis_local = joint_axis[qd_start]
P_lin, P_ang = build_joint_projectors(jt, joint_axis, qd_start, 1, 0, q_wp)
k_lin = joint_penalty_k[c_start + 0]
kd_lin = joint_penalty_kd[c_start + 0]
if k_lin > 0.0:
f_lin, t_lin, Hll_lin, Hal_lin, Haa_lin = evaluate_linear_constraint_force_hessian(
X_wp,
X_wc,
X_wp_prev,
X_wc_prev,
parent_pose,
child_pose,
parent_com,
child_com,
is_parent_body,
k_lin,
P_lin,
kd_lin,
dt,
)
else:
f_lin = wp.vec3(0.0)
t_lin = wp.vec3(0.0)
Hll_lin = wp.mat33(0.0)
Hal_lin = wp.mat33(0.0)
Haa_lin = wp.mat33(0.0)
k_ang = joint_penalty_k[c_start + 1]
kd_ang = joint_penalty_kd[c_start + 1]
if k_ang > 0.0:
t_ang, Haa_ang, _ang_kappa, _ang_J = evaluate_angular_constraint_force_hessian(
q_wp,
q_wc,
q_wp_rest,
q_wc_rest,
q_wp_prev,
q_wc_prev,
is_parent_body,
k_ang,
P_ang,
no_dahl,
no_dahl,
kd_ang,
dt,
)
else:
t_ang = wp.vec3(0.0)
Haa_ang = wp.mat33(0.0)
# Drive + limits on free linear DOF
dof_idx = qd_start
model_drive_ke = joint_target_ke[dof_idx]
drive_kd = joint_target_kd[dof_idx]
target_pos = joint_target_pos[dof_idx]
target_vel = joint_target_vel[dof_idx]
lim_lower = joint_limit_lower[dof_idx]
lim_upper = joint_limit_upper[dof_idx]
model_limit_ke = joint_limit_ke[dof_idx]
lim_kd = joint_limit_kd[dof_idx]
has_drive = model_drive_ke > 0.0 or drive_kd > 0.0
has_limits = model_limit_ke > 0.0 and (lim_lower > -MAXVAL or lim_upper < MAXVAL)
avbd_ke = joint_penalty_k[c_start + 2]
drive_ke = wp.min(avbd_ke, model_drive_ke)
lim_ke = wp.min(avbd_ke, model_limit_ke)
if has_drive or has_limits:
inv_dt = 1.0 / dt
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
C_vec = x_c - x_p
axis_w = wp.normalize(wp.quat_rotate(q_wp, axis_local))
d_along = wp.dot(C_vec, axis_w)
x_p_prev = wp.transform_get_translation(X_wp_prev)
x_c_prev = wp.transform_get_translation(X_wc_prev)
C_vec_prev = x_c_prev - x_p_prev
dC_dt = (C_vec - C_vec_prev) * inv_dt
dd_dt = wp.dot(dC_dt, axis_w)
mode, err_pos = resolve_drive_limit_mode(d_along, target_pos, lim_lower, lim_upper, has_drive, has_limits)
f_scalar = float(0.0)
H_scalar = float(0.0)
if mode == _DRIVE_LIMIT_MODE_LIMIT_LOWER or mode == _DRIVE_LIMIT_MODE_LIMIT_UPPER:
lim_d = lim_kd * lim_ke
f_scalar = lim_ke * err_pos + lim_d * dd_dt
H_scalar = lim_ke + lim_d * inv_dt
elif mode == _DRIVE_LIMIT_MODE_DRIVE:
drive_d = drive_kd * drive_ke
vel_err = dd_dt - target_vel
f_scalar = drive_ke * err_pos + drive_d * vel_err
H_scalar = drive_ke + drive_d * inv_dt
if H_scalar > 0.0:
if is_parent_body:
com_w = wp.transform_point(parent_pose, parent_com)
r = x_p - com_w
else:
com_w = wp.transform_point(child_pose, child_com)
r = x_c - com_w
force_drive, torque_drive, Hll_drive, Hal_drive, Haa_drive = apply_linear_drive_limit_force(
axis_w, r, is_parent_body, f_scalar, H_scalar
)
f_lin = f_lin + force_drive
t_lin = t_lin + torque_drive
Hll_lin = Hll_lin + Hll_drive
Hal_lin = Hal_lin + Hal_drive
Haa_lin = Haa_lin + Haa_drive
return f_lin, t_lin + t_ang, Hll_lin, Hal_lin, Haa_lin + Haa_ang
elif jt == JointType.D6:
lin_count = joint_dof_dim[joint_index, 0]
ang_count = joint_dof_dim[joint_index, 1]
qd_start = joint_qd_start[joint_index]
P_lin, P_ang = build_joint_projectors(
jt,
joint_axis,
qd_start,
lin_count,
ang_count,
q_wp,
)
total_force = wp.vec3(0.0)
total_torque = wp.vec3(0.0)
total_H_ll = wp.mat33(0.0)
total_H_al = wp.mat33(0.0)
total_H_aa = wp.mat33(0.0)
# Linear constraint (constrained when lin_count < 3)
k_lin = joint_penalty_k[c_start + 0]
kd_lin = joint_penalty_kd[c_start + 0]
if lin_count < 3 and k_lin > 0.0:
f_l, t_l, Hll_l, Hal_l, Haa_l = evaluate_linear_constraint_force_hessian(
X_wp,
X_wc,
X_wp_prev,
X_wc_prev,
parent_pose,
child_pose,
parent_com,
child_com,
is_parent_body,
k_lin,
P_lin,
kd_lin,
dt,
)
total_force = total_force + f_l
total_torque = total_torque + t_l
total_H_ll = total_H_ll + Hll_l
total_H_al = total_H_al + Hal_l
total_H_aa = total_H_aa + Haa_l
# Angular constraint (constrained when ang_count < 3)
k_ang = joint_penalty_k[c_start + 1]
kd_ang = joint_penalty_kd[c_start + 1]
kappa_cached = wp.vec3(0.0)
J_world_cached = wp.mat33(0.0)
has_cached = False
if ang_count < 3 and k_ang > 0.0:
t_ang, Haa_ang, kappa_cached, J_world_cached = evaluate_angular_constraint_force_hessian(
q_wp,
q_wc,
q_wp_rest,
q_wc_rest,
q_wp_prev,
q_wc_prev,
is_parent_body,
k_ang,
P_ang,
no_dahl,
no_dahl,
kd_ang,
dt,
)
has_cached = True
total_torque = total_torque + t_ang
total_H_aa = total_H_aa + Haa_ang
# Linear drives/limits (per free linear DOF)
if lin_count > 0:
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
C_vec = x_c - x_p
q_wp_rot = q_wp
x_p_prev = wp.transform_get_translation(X_wp_prev)
x_c_prev = wp.transform_get_translation(X_wc_prev)
C_vec_prev = x_c_prev - x_p_prev
inv_dt = 1.0 / dt
dC_dt = (C_vec - C_vec_prev) * inv_dt
if is_parent_body:
com_w = wp.transform_point(parent_pose, parent_com)
r_drive = x_p - com_w
else:
com_w = wp.transform_point(child_pose, child_com)
r_drive = x_c - com_w
for li in range(3):
if li < lin_count:
dof_idx = qd_start + li
model_drive_ke = joint_target_ke[dof_idx]
drive_kd = joint_target_kd[dof_idx]
target_pos = joint_target_pos[dof_idx]
target_vel = joint_target_vel[dof_idx]
lim_lower = joint_limit_lower[dof_idx]
lim_upper = joint_limit_upper[dof_idx]
model_limit_ke = joint_limit_ke[dof_idx]
lim_kd = joint_limit_kd[dof_idx]
has_drive = model_drive_ke > 0.0 or drive_kd > 0.0
has_limits = model_limit_ke > 0.0 and (lim_lower > -MAXVAL or lim_upper < MAXVAL)
avbd_ke = joint_penalty_k[c_start + 2 + li]
drive_ke = wp.min(avbd_ke, model_drive_ke)
lim_ke = wp.min(avbd_ke, model_limit_ke)
if has_drive or has_limits:
axis_w = wp.normalize(wp.quat_rotate(q_wp_rot, joint_axis[dof_idx]))
d_along = wp.dot(C_vec, axis_w)
dd_dt = wp.dot(dC_dt, axis_w)
mode, err_pos = resolve_drive_limit_mode(
d_along, target_pos, lim_lower, lim_upper, has_drive, has_limits
)
f_scalar = float(0.0)
H_scalar = float(0.0)
if mode == _DRIVE_LIMIT_MODE_LIMIT_LOWER or mode == _DRIVE_LIMIT_MODE_LIMIT_UPPER:
lim_d = lim_kd * lim_ke
f_scalar = lim_ke * err_pos + lim_d * dd_dt
H_scalar = lim_ke + lim_d * inv_dt
elif mode == _DRIVE_LIMIT_MODE_DRIVE:
drive_d = drive_kd * drive_ke
vel_err = dd_dt - target_vel
f_scalar = drive_ke * err_pos + drive_d * vel_err
H_scalar = drive_ke + drive_d * inv_dt
if H_scalar > 0.0:
force_drive, torque_drive, Hll_drive, Hal_drive, Haa_drive = apply_linear_drive_limit_force(
axis_w, r_drive, is_parent_body, f_scalar, H_scalar
)
total_force = total_force + force_drive
total_torque = total_torque + torque_drive
total_H_ll = total_H_ll + Hll_drive
total_H_al = total_H_al + Hal_drive
total_H_aa = total_H_aa + Haa_drive
# Angular drives/limits (per free angular DOF)
if ang_count > 0:
inv_dt = 1.0 / dt
if has_cached:
kappa = kappa_cached
J_world = J_world_cached
else:
kappa, J_world = compute_kappa_and_jacobian(q_wp, q_wc, q_wp_rest, q_wc_rest)
omega_p = quat_velocity(q_wp, q_wp_prev, dt)
omega_c = quat_velocity(q_wc, q_wc_prev, dt)
dkappa_dt = compute_kappa_dot_analytic(q_wp, q_wc, q_wp_rest, q_wc_rest, omega_p, omega_c, kappa)
for ai in range(3):
if ai < ang_count:
dof_idx = qd_start + lin_count + ai
model_drive_ke = joint_target_ke[dof_idx]
drive_kd = joint_target_kd[dof_idx]
target_pos = joint_target_pos[dof_idx]
target_vel = joint_target_vel[dof_idx]
lim_lower = joint_limit_lower[dof_idx]
lim_upper = joint_limit_upper[dof_idx]
model_limit_ke = joint_limit_ke[dof_idx]
lim_kd = joint_limit_kd[dof_idx]
has_drive = model_drive_ke > 0.0 or drive_kd > 0.0
has_limits = model_limit_ke > 0.0 and (lim_lower > -MAXVAL or lim_upper < MAXVAL)
avbd_ke = joint_penalty_k[c_start + 2 + lin_count + ai]
drive_ke = wp.min(avbd_ke, model_drive_ke)
lim_ke = wp.min(avbd_ke, model_limit_ke)
if has_drive or has_limits:
a = wp.normalize(joint_axis[dof_idx])
theta = wp.dot(kappa, a)
theta_abs = theta + joint_rest_angle[dof_idx]
dtheta_dt = wp.dot(dkappa_dt, a)
mode, err_pos = resolve_drive_limit_mode(
theta_abs, target_pos, lim_lower, lim_upper, has_drive, has_limits
)
f_scalar = float(0.0)
H_scalar = float(0.0)
if mode == _DRIVE_LIMIT_MODE_LIMIT_LOWER or mode == _DRIVE_LIMIT_MODE_LIMIT_UPPER:
lim_d = lim_kd * lim_ke
f_scalar = lim_ke * err_pos + lim_d * dtheta_dt
H_scalar = lim_ke + lim_d * inv_dt
elif mode == _DRIVE_LIMIT_MODE_DRIVE:
drive_d = drive_kd * drive_ke
vel_err = dtheta_dt - target_vel
f_scalar = drive_ke * err_pos + drive_d * vel_err
H_scalar = drive_ke + drive_d * inv_dt
if H_scalar > 0.0:
tau_drive, Haa_drive = apply_angular_drive_limit_torque(
a, J_world, is_parent_body, f_scalar, H_scalar
)
total_torque = total_torque + tau_drive
total_H_aa = total_H_aa + Haa_drive
return total_force, total_torque, total_H_ll, total_H_al, total_H_aa
return wp.vec3(0.0), wp.vec3(0.0), wp.mat33(0.0), wp.mat33(0.0), wp.mat33(0.0)
# -----------------------------
# Utility kernels
# -----------------------------
@wp.kernel
def _count_num_adjacent_joints(
joint_parent: wp.array[wp.int32],
joint_child: wp.array[wp.int32],
num_body_adjacent_joints: wp.array[wp.int32],
):
joint_count = joint_parent.shape[0]
for joint_id in range(joint_count):
parent_id = joint_parent[joint_id]
child_id = joint_child[joint_id]
# Skip world joints (parent/child == -1)
if parent_id >= 0:
num_body_adjacent_joints[parent_id] = num_body_adjacent_joints[parent_id] + 1
if child_id >= 0:
num_body_adjacent_joints[child_id] = num_body_adjacent_joints[child_id] + 1
@wp.kernel
def _fill_adjacent_joints(
joint_parent: wp.array[wp.int32],
joint_child: wp.array[wp.int32],
body_adjacent_joints_offsets: wp.array[wp.int32],
body_adjacent_joints_fill_count: wp.array[wp.int32],
body_adjacent_joints: wp.array[wp.int32],
):
joint_count = joint_parent.shape[0]
for joint_id in range(joint_count):
parent_id = joint_parent[joint_id]
child_id = joint_child[joint_id]
# Add joint to parent body's adjacency list
if parent_id >= 0:
fill_count_parent = body_adjacent_joints_fill_count[parent_id]
buffer_offset_parent = body_adjacent_joints_offsets[parent_id]
body_adjacent_joints[buffer_offset_parent + fill_count_parent] = joint_id
body_adjacent_joints_fill_count[parent_id] = fill_count_parent + 1
# Add joint to child body's adjacency list
if child_id >= 0:
fill_count_child = body_adjacent_joints_fill_count[child_id]
buffer_offset_child = body_adjacent_joints_offsets[child_id]
body_adjacent_joints[buffer_offset_child + fill_count_child] = joint_id
body_adjacent_joints_fill_count[child_id] = fill_count_child + 1
# -----------------------------
# Pre-iteration kernels (once per step)
# -----------------------------
@wp.kernel
def forward_step_rigid_bodies(
# Inputs
dt: float,
gravity: wp.array[wp.vec3],
body_world: wp.array[wp.int32],
body_f: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
body_inertia: wp.array[wp.mat33],
body_inv_mass: wp.array[float],
body_inv_inertia: wp.array[wp.mat33],
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_inertia_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
):
"""
Forward integration step for rigid bodies in the AVBD/VBD solver.
Snapshots ``body_q_prev`` for dynamic bodies only. Kinematic bodies keep
the previous step's pose so contact friction sees correct velocity.
Args:
dt: Time step [s].
gravity: Gravity vector array (world frame).
body_world: World index for each body.
body_f: External forces on bodies (spatial wrenches, world frame).
body_com: Centers of mass (local body frame).
body_inertia: Inertia tensors (local body frame).
body_inv_mass: Inverse masses (0 for kinematic bodies).
body_inv_inertia: Inverse inertia tensors (local body frame).
body_q: Body transforms (input: start-of-step pose, output: integrated pose).
body_qd: Body velocities (input: start-of-step velocity, output: integrated velocity).
body_inertia_q: Inertial target body transforms for the AVBD solve (output).
body_q_prev: Previous body transforms (output, dynamic bodies only).
"""
tid = wp.tid()
q_current = body_q[tid]
# Early exit for kinematic bodies (inv_mass == 0).
# Do not snapshot body_q_prev here: kinematic bodies need body_q_prev from previous step.
inv_m = body_inv_mass[tid]
if inv_m == 0.0:
body_inertia_q[tid] = q_current
return
# Snapshot current pose as previous before integration (dynamic bodies only).
body_q_prev[tid] = q_current
# Read body state (only for dynamic bodies)
qd_current = body_qd[tid]
f_current = body_f[tid]
com_local = body_com[tid]
I_local = body_inertia[tid]
inv_I = body_inv_inertia[tid]
world_idx = body_world[tid]
world_g = gravity[wp.max(world_idx, 0)]
# Integrate rigid body motion (semi-implicit Euler, no angular damping)
q_new, qd_new = integrate_rigid_body(
q_current,
qd_current,
f_current,
com_local,
I_local,
inv_m,
inv_I,
world_g,
0.0, # angular_damping = 0 (consistent with particle VBD)
dt,
)
# Update current transform, velocity, and set inertial target
body_q[tid] = q_new
body_qd[tid] = qd_new
body_inertia_q[tid] = q_new
@wp.kernel
def build_body_body_contact_lists(
rigid_contact_count: wp.array[int],
rigid_contact_shape0: wp.array[int],
rigid_contact_shape1: wp.array[int],
shape_body: wp.array[wp.int32],
body_contact_buffer_pre_alloc: int,
body_contact_counts: wp.array[wp.int32],
body_contact_indices: wp.array[wp.int32],
):
"""
Build per-body contact lists for body-centric per-color contact evaluation.
Each contact index t_id is appended to both bodies' lists (shape0's body and
shape1's body), enabling efficient lookup of all contacts involving a given body
during per-body solve iterations.
Notes:
- body_contact_counts[b] is reset to 0 on the host before launch and
atomically incremented here; consumers must only read the first
body_contact_counts[b] indices for each body.
- If a body has more than body_contact_buffer_pre_alloc contacts, extra indices
are dropped (overflow is safely ignored here).
- body_contact_indices is not cleared each step; only the prefix defined
by body_contact_counts is considered valid.
"""
t_id = wp.tid()
if t_id >= rigid_contact_count[0]:
return
s0 = rigid_contact_shape0[t_id]
s1 = rigid_contact_shape1[t_id]
b0 = shape_body[s0] if s0 >= 0 else -1
b1 = shape_body[s1] if s1 >= 0 else -1
# Add contact to body0's list
if b0 >= 0:
idx = wp.atomic_add(body_contact_counts, b0, 1)
if idx < body_contact_buffer_pre_alloc:
body_contact_indices[b0 * body_contact_buffer_pre_alloc + idx] = t_id
# Add contact to body1's list
if b1 >= 0:
idx = wp.atomic_add(body_contact_counts, b1, 1)
if idx < body_contact_buffer_pre_alloc:
body_contact_indices[b1 * body_contact_buffer_pre_alloc + idx] = t_id
@wp.kernel
def build_body_particle_contact_lists(
body_particle_contact_count: wp.array[int],
body_particle_contact_shape: wp.array[int],
shape_body: wp.array[wp.int32],
body_particle_contact_buffer_pre_alloc: int,
body_particle_contact_counts: wp.array[wp.int32],
body_particle_contact_indices: wp.array[wp.int32],
):
"""
Build per-body contact lists for body-particle (particle-rigid) contacts.
Each body-particle contact index tid (from the external contacts.soft_contact_*
arrays) is appended to the list of the rigid body associated with
body_particle_contact_shape[tid] via shape_body. This enables efficient
per-body processing of particle-rigid contacts during the rigid-body solve
(e.g., Gauss-Seidel color sweeps).
Notes:
- body_particle_contact_counts[b] must be zeroed on the host before launch.
- Only the first body_particle_contact_counts[b] entries in the flat
body_particle_contact_indices array are considered valid for body b.
- If a body has more than body_particle_contact_buffer_pre_alloc body-particle
contacts, extra indices are dropped (overflow is ignored safely).
"""
tid = wp.tid()
if tid >= body_particle_contact_count[0]:
return
shape = body_particle_contact_shape[tid]
body = shape_body[shape] if shape >= 0 else -1
if body < 0:
return
idx = wp.atomic_add(body_particle_contact_counts, body, 1)
if idx < body_particle_contact_buffer_pre_alloc:
body_particle_contact_indices[body * body_particle_contact_buffer_pre_alloc + idx] = tid
@wp.kernel
def warmstart_joints(
# Inputs
joint_penalty_k_max: wp.array[float],
joint_penalty_k_min: wp.array[float],
gamma: float,
# Input/output
joint_penalty_k: wp.array[float],
):
"""
Warm-start per-scalar-constraint penalty stiffness for rigid joint constraints (runs once per step).
Algorithm (per constraint scalar i):
- Decay previous penalty: k <- gamma * k (typically gamma in [0, 1])
- Clamp to [joint_penalty_k_min[i], joint_penalty_k_max[i]] so k_min <= k <= k_max always.
"""
i = wp.tid()
k_prev = joint_penalty_k[i]
k_new = wp.clamp(gamma * k_prev, joint_penalty_k_min[i], joint_penalty_k_max[i])
joint_penalty_k[i] = k_new
@wp.kernel
def warmstart_body_body_contacts(
rigid_contact_count: wp.array[int],
rigid_contact_shape0: wp.array[int],
rigid_contact_shape1: wp.array[int],
shape_material_ke: wp.array[float],
shape_material_kd: wp.array[float],
shape_material_mu: wp.array[float],
k_start_body_contact: float,
# Outputs
contact_penalty_k: wp.array[float],
contact_material_ke: wp.array[float],
contact_material_kd: wp.array[float],
contact_material_mu: wp.array[float],
):
"""
Warm-start contact penalties and cache material properties.
Computes averaged material properties for each rigid contact and resets the
AVBD penalty to a bounded initial value based on `k_start_body_contact` and the
material stiffness.
"""
i = wp.tid()
if i >= rigid_contact_count[0]:
return
# Read shape indices
shape_id_0 = rigid_contact_shape0[i]
shape_id_1 = rigid_contact_shape1[i]
# Cache averaged material properties (arithmetic mean for stiffness/damping, geometric for friction)
avg_ke = 0.5 * (shape_material_ke[shape_id_0] + shape_material_ke[shape_id_1])
avg_kd = 0.5 * (shape_material_kd[shape_id_0] + shape_material_kd[shape_id_1])
avg_mu = wp.sqrt(shape_material_mu[shape_id_0] * shape_material_mu[shape_id_1])
contact_material_ke[i] = avg_ke
contact_material_kd[i] = avg_kd
contact_material_mu[i] = avg_mu
# Reset contact penalty to k_start every frame because contact indices are not persistent across frames.
k_new = wp.min(k_start_body_contact, avg_ke)
contact_penalty_k[i] = k_new
@wp.kernel
def warmstart_body_particle_contacts(
body_particle_contact_count: wp.array[int],
body_particle_contact_shape: wp.array[int],
soft_contact_ke: float,
soft_contact_kd: float,
soft_contact_mu: float,
shape_material_ke: wp.array[float],
shape_material_kd: wp.array[float],
shape_material_mu: wp.array[float],
k_start_body_contact: float,
# Outputs
body_particle_contact_penalty_k: wp.array[float],
body_particle_contact_material_ke: wp.array[float],
body_particle_contact_material_kd: wp.array[float],
body_particle_contact_material_mu: wp.array[float],
):
"""
Warm-start body-particle (particle-rigid) contact penalties and cache material
properties.
The scalar inputs `soft_contact_ke/kd/mu` are the particle-side soft-contact
material parameters (from `model.soft_contact_*`). For each body-particle
contact, this kernel averages those particle-side values with the rigid
shape's material parameters and resets the AVBD penalty to a bounded
initial value based on `k_start_body_contact` and the averaged stiffness.
"""
i = wp.tid()
if i >= body_particle_contact_count[0]:
return
# Read shape index for the rigid body side
shape_idx = body_particle_contact_shape[i]
# Cache averaged material properties (arithmetic mean for stiffness/damping, geometric for friction)
avg_ke = 0.5 * (soft_contact_ke + shape_material_ke[shape_idx])
avg_kd = 0.5 * (soft_contact_kd + shape_material_kd[shape_idx])
avg_mu = wp.sqrt(soft_contact_mu * shape_material_mu[shape_idx])
body_particle_contact_material_ke[i] = avg_ke
body_particle_contact_material_kd[i] = avg_kd
body_particle_contact_material_mu[i] = avg_mu
# Reset contact penalty to k_start every frame because contact indices are not persistent across frames.
k_new = wp.min(k_start_body_contact, avg_ke)
body_particle_contact_penalty_k[i] = k_new
@wp.kernel
def compute_cable_dahl_parameters(
# Inputs
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_constraint_start: wp.array[int],
joint_penalty_k_max: wp.array[float],
body_q: wp.array[wp.transform],
body_q_rest: wp.array[wp.transform],
joint_sigma_prev: wp.array[wp.vec3],
joint_kappa_prev: wp.array[wp.vec3],
joint_dkappa_prev: wp.array[wp.vec3],
joint_eps_max: wp.array[float],
joint_tau: wp.array[float],
# Outputs
joint_sigma_start: wp.array[wp.vec3],
joint_C_fric: wp.array[wp.vec3],
):
"""
Compute Dahl hysteresis parameters (sigma0, C_fric) for cable bending,
given the current curvature state and the stored previous Dahl state.
The outputs are:
- sigma0: linearized friction stress at the start of the step (per component)
- C_fric: tangent stiffness d(sigma)/d(kappa) (per component)
"""
j = wp.tid()
if not joint_enabled[j]:
joint_sigma_start[j] = wp.vec3(0.0)
joint_C_fric[j] = wp.vec3(0.0)
return
# Only process cable joints
if joint_type[j] != JointType.CABLE:
joint_sigma_start[j] = wp.vec3(0.0)
joint_C_fric[j] = wp.vec3(0.0)
return
parent = joint_parent[j]
child = joint_child[j]
# World-parent joints are valid; child body must exist.
if child < 0:
joint_sigma_start[j] = wp.vec3(0.0)
joint_C_fric[j] = wp.vec3(0.0)
return
# Compute joint frames in world space (current and rest only)
if parent >= 0:
X_wp = body_q[parent] * joint_X_p[j]
X_wp_rest = body_q_rest[parent] * joint_X_p[j]
else:
X_wp = joint_X_p[j]
X_wp_rest = joint_X_p[j]
X_wc = body_q[child] * joint_X_c[j]
X_wc_rest = body_q_rest[child] * joint_X_c[j]
# Extract quaternions (current and rest configurations)
q_wp = wp.transform_get_rotation(X_wp)
q_wc = wp.transform_get_rotation(X_wc)
q_wp_rest = wp.transform_get_rotation(X_wp_rest)
q_wc_rest = wp.transform_get_rotation(X_wc_rest)
# Compute current curvature vector at beginning-of-step (predicted state)
kappa_now = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest)
# Read previous state (from last converged timestep)
kappa_prev = joint_kappa_prev[j]
d_kappa_prev = joint_dkappa_prev[j]
sigma_prev = joint_sigma_prev[j]
# Read per-joint Dahl parameters (isotropic)
eps_max = joint_eps_max[j]
tau = joint_tau[j]
# Use the per-joint bend stiffness target (cap) from the solver constraint caps (constraint slot 1 for cables).
c_start = joint_constraint_start[j]
k_bend_target = joint_penalty_k_max[c_start + 1]
# Friction envelope: sigma_max = k_bend_target * eps_max.
sigma_max = k_bend_target * eps_max
if sigma_max <= 0.0 or tau <= 0.0:
joint_sigma_start[j] = wp.vec3(0.0)
joint_C_fric[j] = wp.vec3(0.0)
return
sigma_out = wp.vec3(0.0)
C_fric_out = wp.vec3(0.0)
for axis in range(3):
kappa_i = kappa_now[axis]
kappa_i_prev = kappa_prev[axis]
sigma_i_prev = sigma_prev[axis]
# Geometric curvature change
d_kappa_i = kappa_i - kappa_i_prev
# Direction flag based primarily on geometric change, with stored Delta-kappa fallback
s_i = 1.0
if d_kappa_i > _DAHL_KAPPADOT_DEADBAND:
s_i = 1.0
elif d_kappa_i < -_DAHL_KAPPADOT_DEADBAND:
s_i = -1.0
else:
# Within deadband: maintain previous direction from stored Delta kappa
s_i = 1.0 if d_kappa_prev[axis] >= 0.0 else -1.0
exp_term = wp.exp(-s_i * d_kappa_i / tau)
sigma0_i = s_i * sigma_max * (1.0 - exp_term) + sigma_i_prev * exp_term
sigma0_i = wp.clamp(sigma0_i, -sigma_max, sigma_max)
numerator = sigma_max - s_i * sigma0_i
# Use geometric curvature change for the length scale
denominator = tau + wp.abs(d_kappa_i)
# Store pure stiffness K = numerator / (tau + |d_kappa|)
C_fric_i = wp.max(numerator / denominator, 0.0)
sigma_out[axis] = sigma0_i
C_fric_out[axis] = C_fric_i
joint_sigma_start[j] = sigma_out
joint_C_fric[j] = C_fric_out
# -----------------------------
# Iteration kernels (per color per iteration)
# -----------------------------
@wp.kernel
def accumulate_body_body_contacts_per_body(
dt: float,
color_group: wp.array[wp.int32],
body_q_prev: wp.array[wp.transform],
body_q: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
body_inv_mass: wp.array[float],
friction_epsilon: float,
contact_penalty_k: wp.array[float],
contact_material_kd: wp.array[float],
contact_material_mu: wp.array[float],
rigid_contact_count: wp.array[int],
rigid_contact_shape0: wp.array[int],
rigid_contact_shape1: wp.array[int],
rigid_contact_point0: wp.array[wp.vec3],
rigid_contact_point1: wp.array[wp.vec3],
rigid_contact_normal: wp.array[wp.vec3],
rigid_contact_margin0: wp.array[float],
rigid_contact_margin1: wp.array[float],
shape_body: wp.array[wp.int32],
body_contact_buffer_pre_alloc: int,
body_contact_counts: wp.array[wp.int32],
body_contact_indices: wp.array[wp.int32],
body_forces: wp.array[wp.vec3],
body_torques: wp.array[wp.vec3],
body_hessian_ll: wp.array[wp.mat33],
body_hessian_al: wp.array[wp.mat33],
body_hessian_aa: wp.array[wp.mat33],
):
"""
Per-body contact accumulation with _NUM_CONTACT_THREADS_PER_BODY strided threads.
"""
tid = wp.tid()
body_idx_in_group = tid // _NUM_CONTACT_THREADS_PER_BODY
thread_id_within_body = tid % _NUM_CONTACT_THREADS_PER_BODY
if body_idx_in_group >= color_group.shape[0]:
return
body_id = color_group[body_idx_in_group]
if body_inv_mass[body_id] <= 0.0:
return
num_contacts = body_contact_counts[body_id]
if num_contacts > body_contact_buffer_pre_alloc:
num_contacts = body_contact_buffer_pre_alloc
force_acc = wp.vec3(0.0)
torque_acc = wp.vec3(0.0)
h_ll_acc = wp.mat33(0.0)
h_al_acc = wp.mat33(0.0)
h_aa_acc = wp.mat33(0.0)
i = thread_id_within_body
while i < num_contacts:
contact_idx = body_contact_indices[body_id * body_contact_buffer_pre_alloc + i]
if contact_idx >= rigid_contact_count[0]:
i += _NUM_CONTACT_THREADS_PER_BODY
continue
s0 = rigid_contact_shape0[contact_idx]
s1 = rigid_contact_shape1[contact_idx]
b0 = shape_body[s0] if s0 >= 0 else -1
b1 = shape_body[s1] if s1 >= 0 else -1
if b0 != body_id and b1 != body_id:
i += _NUM_CONTACT_THREADS_PER_BODY
continue
cp0_local = rigid_contact_point0[contact_idx]
cp1_local = rigid_contact_point1[contact_idx]
contact_normal = rigid_contact_normal[contact_idx]
cp0_world = wp.transform_point(body_q[b0], cp0_local) if b0 >= 0 else cp0_local
cp1_world = wp.transform_point(body_q[b1], cp1_local) if b1 >= 0 else cp1_local
thickness = rigid_contact_margin0[contact_idx] + rigid_contact_margin1[contact_idx]
dist = wp.dot(contact_normal, cp1_world - cp0_world)
penetration = thickness - dist
if penetration <= _SMALL_LENGTH_EPS:
i += _NUM_CONTACT_THREADS_PER_BODY
continue
contact_ke = contact_penalty_k[contact_idx]
contact_kd = contact_material_kd[contact_idx]
contact_mu = contact_material_mu[contact_idx]
(
force_0,
torque_0,
h_ll_0,
h_al_0,
h_aa_0,
force_1,
torque_1,
h_ll_1,
h_al_1,
h_aa_1,
) = evaluate_rigid_contact_from_collision(
b0,
b1,
body_q,
body_q_prev,
body_com,
cp0_local,
cp1_local,
contact_normal,
penetration,
contact_ke,
contact_kd,
contact_mu,
friction_epsilon,
dt,
)
if body_id == b0:
force_acc += force_0
torque_acc += torque_0
h_ll_acc += h_ll_0
h_al_acc += h_al_0
h_aa_acc += h_aa_0
else:
force_acc += force_1
torque_acc += torque_1
h_ll_acc += h_ll_1
h_al_acc += h_al_1
h_aa_acc += h_aa_1
i += _NUM_CONTACT_THREADS_PER_BODY
wp.atomic_add(body_forces, body_id, force_acc)
wp.atomic_add(body_torques, body_id, torque_acc)
wp.atomic_add(body_hessian_ll, body_id, h_ll_acc)
wp.atomic_add(body_hessian_al, body_id, h_al_acc)
wp.atomic_add(body_hessian_aa, body_id, h_aa_acc)
@wp.kernel
def compute_rigid_contact_forces(
dt: float,
# Contact data
rigid_contact_count: wp.array[int],
rigid_contact_shape0: wp.array[int],
rigid_contact_shape1: wp.array[int],
rigid_contact_point0: wp.array[wp.vec3],
rigid_contact_point1: wp.array[wp.vec3],
rigid_contact_normal: wp.array[wp.vec3],
rigid_contact_thickness0: wp.array[float],
rigid_contact_thickness1: wp.array[float],
# Model/state
shape_body: wp.array[wp.int32],
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
# Contact material properties (per-contact)
contact_penalty_k: wp.array[float],
contact_material_kd: wp.array[float],
contact_material_mu: wp.array[float],
friction_epsilon: float,
# Outputs (length = rigid_contact_max)
out_body0: wp.array[wp.int32],
out_body1: wp.array[wp.int32],
out_point0_world: wp.array[wp.vec3],
out_point1_world: wp.array[wp.vec3],
out_force_on_body1: wp.array[wp.vec3],
):
"""Compute per-contact forces in world space, matching the AVBD rigid contact model.
Produces a **contact-specific** force vector (world frame) for each rigid contact.
Conventions:
- The computed force is the force **on body1** (shape1/body1) in world frame.
- The force on body0 is `-out_force_on_body1`.
- World-space contact points for both shapes are provided for wrench construction.
"""
contact_idx = wp.tid()
rc = rigid_contact_count[0]
if contact_idx >= rc:
# Fill sentinel values for inactive entries (useful when launching with rigid_contact_max)
out_body0[contact_idx] = wp.int32(-1)
out_body1[contact_idx] = wp.int32(-1)
out_point0_world[contact_idx] = wp.vec3(0.0)
out_point1_world[contact_idx] = wp.vec3(0.0)
out_force_on_body1[contact_idx] = wp.vec3(0.0)
return
s0 = rigid_contact_shape0[contact_idx]
s1 = rigid_contact_shape1[contact_idx]
if s0 < 0 or s1 < 0:
out_body0[contact_idx] = wp.int32(-1)
out_body1[contact_idx] = wp.int32(-1)
out_point0_world[contact_idx] = wp.vec3(0.0)
out_point1_world[contact_idx] = wp.vec3(0.0)
out_force_on_body1[contact_idx] = wp.vec3(0.0)
return
b0 = shape_body[s0]
b1 = shape_body[s1]
out_body0[contact_idx] = b0
out_body1[contact_idx] = b1
cp0_local = rigid_contact_point0[contact_idx]
cp1_local = rigid_contact_point1[contact_idx]
contact_normal = rigid_contact_normal[contact_idx]
# Static/kinematic shapes use shape_body == -1. In that case, contact points are already in world
# frame for that side and must not index into body_q[-1].
cp0_world = wp.transform_point(body_q[b0], cp0_local) if b0 >= 0 else cp0_local
cp1_world = wp.transform_point(body_q[b1], cp1_local) if b1 >= 0 else cp1_local
out_point0_world[contact_idx] = cp0_world
out_point1_world[contact_idx] = cp1_world
thickness = rigid_contact_thickness0[contact_idx] + rigid_contact_thickness1[contact_idx]
dist = wp.dot(contact_normal, cp1_world - cp0_world)
penetration = thickness - dist
if penetration <= _SMALL_LENGTH_EPS:
out_force_on_body1[contact_idx] = wp.vec3(0.0)
return
contact_ke = contact_penalty_k[contact_idx]
contact_kd = contact_material_kd[contact_idx]
contact_mu = contact_material_mu[contact_idx]
# Reuse the exact same contact model used by AVBD accumulation
(
_force_0,
_torque_0,
_h_ll_0,
_h_al_0,
_h_aa_0,
force_1,
_torque_1,
_h_ll_1,
_h_al_1,
_h_aa_1,
) = evaluate_rigid_contact_from_collision(
int(b0),
int(b1),
body_q,
body_q_prev,
body_com,
cp0_local,
cp1_local,
contact_normal,
penetration,
contact_ke,
contact_kd,
contact_mu,
friction_epsilon,
dt,
)
out_force_on_body1[contact_idx] = force_1
@wp.kernel
def accumulate_body_particle_contacts_per_body(
dt: float,
color_group: wp.array[wp.int32],
# Particle state
particle_q: wp.array[wp.vec3],
particle_q_prev: wp.array[wp.vec3],
particle_radius: wp.array[float],
# Rigid body state
body_q_prev: wp.array[wp.transform],
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
body_inv_mass: wp.array[float],
# AVBD body-particle soft contact penalties and material properties
friction_epsilon: float,
body_particle_contact_penalty_k: wp.array[float],
body_particle_contact_material_kd: wp.array[float],
body_particle_contact_material_mu: wp.array[float],
# Soft contact data (body-particle)
body_particle_contact_count: wp.array[int],
body_particle_contact_particle: wp.array[int],
body_particle_contact_shape: wp.array[int],
body_particle_contact_body_pos: wp.array[wp.vec3],
body_particle_contact_body_vel: wp.array[wp.vec3],
body_particle_contact_normal: wp.array[wp.vec3],
# Shape/material data
shape_material_mu: wp.array[float],
shape_body: wp.array[wp.int32],
# Per-body soft-contact adjacency (body-particle)
body_particle_contact_buffer_pre_alloc: int,
body_particle_contact_counts: wp.array[wp.int32],
body_particle_contact_indices: wp.array[wp.int32],
# Outputs
body_forces: wp.array[wp.vec3],
body_torques: wp.array[wp.vec3],
body_hessian_ll: wp.array[wp.mat33],
body_hessian_al: wp.array[wp.mat33],
body_hessian_aa: wp.array[wp.mat33],
):
"""
Per-body accumulation of body-particle (particle-rigid) soft contact forces and
Hessians on rigid bodies.
This kernel mirrors the Gauss-Seidel per-body pattern used for body-body contacts,
but iterates over body-particle contacts associated with each body via the
precomputed body_particle_contact_* adjacency.
For each body-particle contact, we:
1. Reuse the particle-side contact model via evaluate_body_particle_contact to
compute the force and Hessian on the particle using adaptive AVBD penalties.
2. Apply the equal-and-opposite reaction force, torque, and Hessian contributions
to the rigid body.
Notes:
- Only dynamic bodies (inv_mass > 0) are updated.
- Hessian contributions are accumulated into body_hessian_ll/al/aa.
- Uses AVBD adaptive penalty parameters (warmstarted and updated per iteration).
"""
tid = wp.tid()
body_idx_in_group = tid // _NUM_CONTACT_THREADS_PER_BODY
thread_id_within_body = tid % _NUM_CONTACT_THREADS_PER_BODY
if body_idx_in_group >= color_group.shape[0]:
return
body_id = color_group[body_idx_in_group]
if body_inv_mass[body_id] <= 0.0:
return
num_contacts = body_particle_contact_counts[body_id]
if num_contacts > body_particle_contact_buffer_pre_alloc:
num_contacts = body_particle_contact_buffer_pre_alloc
i = thread_id_within_body
max_contacts = body_particle_contact_count[0]
while i < num_contacts:
contact_idx = body_particle_contact_indices[body_id * body_particle_contact_buffer_pre_alloc + i]
if contact_idx >= max_contacts:
i += _NUM_CONTACT_THREADS_PER_BODY
continue
particle_idx = body_particle_contact_particle[contact_idx]
if particle_idx < 0:
i += _NUM_CONTACT_THREADS_PER_BODY
continue
# Early penetration check to avoid unnecessary function call
particle_pos = particle_q[particle_idx]
X_wb = body_q[body_id]
cp_local = body_particle_contact_body_pos[contact_idx]
cp_world = wp.transform_point(X_wb, cp_local)
n = body_particle_contact_normal[contact_idx]
radius = particle_radius[particle_idx]
penetration_depth = -(wp.dot(n, particle_pos - cp_world) - radius)
if penetration_depth <= 0.0:
i += _NUM_CONTACT_THREADS_PER_BODY
continue
# Compute contact force and Hessian on particle using AVBD adaptive penalty
particle_prev_pos = particle_q_prev[particle_idx]
# Read per-contact AVBD penalty and material properties
contact_ke = body_particle_contact_penalty_k[contact_idx]
contact_kd = body_particle_contact_material_kd[contact_idx]
contact_mu = body_particle_contact_material_mu[contact_idx]
force_on_particle, hessian_particle = evaluate_body_particle_contact(
particle_idx,
particle_pos,
particle_prev_pos,
contact_idx,
contact_ke,
contact_kd,
contact_mu,
friction_epsilon,
particle_radius,
shape_material_mu,
shape_body,
body_q,
body_q_prev,
body_qd,
body_com,
body_particle_contact_shape,
body_particle_contact_body_pos,
body_particle_contact_body_vel,
body_particle_contact_normal,
dt,
)
# Reaction on the body (Newton's 3rd law)
f_body = -force_on_particle
# Compute torque and Hessian contributions: tau = r x f, where r is from COM to contact point
com_world = wp.transform_point(X_wb, body_com[body_id])
r = cp_world - com_world
tau_body = wp.cross(r, f_body)
# Hessian contributions for the rigid body
# The particle Hessian K_particle contributes to the body's linear-linear block
# and we need to add angular-linear and angular-angular blocks from the moment arm
K_total = hessian_particle
r_skew = wp.skew(r)
r_skew_T_K = wp.transpose(r_skew) * K_total
h_ll_body = K_total # Linear-linear block
h_al_body = -r_skew_T_K # Angular-linear block
h_aa_body = r_skew_T_K * r_skew # Angular-angular block
wp.atomic_add(body_forces, body_id, f_body)
wp.atomic_add(body_torques, body_id, tau_body)
wp.atomic_add(body_hessian_ll, body_id, h_ll_body)
wp.atomic_add(body_hessian_al, body_id, h_al_body)
wp.atomic_add(body_hessian_aa, body_id, h_aa_body)
i += _NUM_CONTACT_THREADS_PER_BODY
@wp.kernel
def solve_rigid_body(
dt: float,
body_ids_in_color: wp.array[wp.int32],
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_q_rest: wp.array[wp.transform],
body_mass: wp.array[float],
body_inv_mass: wp.array[float],
body_inertia: wp.array[wp.mat33],
body_inertia_q: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
adjacency: RigidForceElementAdjacencyInfo,
# Joint data
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_qd_start: wp.array[int],
joint_constraint_start: wp.array[int],
# AVBD per-constraint penalty state (scalar constraints indexed via joint_constraint_start)
joint_penalty_k: wp.array[float],
joint_penalty_kd: wp.array[float],
# Dahl hysteresis parameters (frozen for this timestep, component-wise vec3 per joint)
joint_sigma_start: wp.array[wp.vec3],
joint_C_fric: wp.array[wp.vec3],
# Drive parameters (DOF-indexed via joint_qd_start)
joint_target_ke: wp.array[float],
joint_target_kd: wp.array[float],
joint_target_pos: wp.array[float],
joint_target_vel: wp.array[float],
# Limit parameters (DOF-indexed via joint_qd_start)
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
joint_limit_ke: wp.array[float],
joint_limit_kd: wp.array[float],
joint_dof_dim: wp.array2d[int],
joint_rest_angle: wp.array[float],
external_forces: wp.array[wp.vec3],
external_torques: wp.array[wp.vec3],
external_hessian_ll: wp.array[wp.mat33], # Linear-linear block from rigid contacts
external_hessian_al: wp.array[wp.mat33], # Angular-linear coupling block from rigid contacts
external_hessian_aa: wp.array[wp.mat33], # Angular-angular block from rigid contacts
# Output
body_q_new: wp.array[wp.transform],
):
"""
AVBD solve step for rigid bodies using block Cholesky decomposition.
Solves the 6-DOF rigid body system by assembling inertial, joint, and collision
contributions into a 6x6 block system:
[ H_ll H_al^T ]
[ H_al H_aa ]
and solving via Schur complement.
Consistent with VBD particle solve pattern: inertia + external + constraint forces.
Algorithm:
1. Compute inertial forces/Hessians
2. Accumulate external forces/Hessians from rigid contacts
3. Accumulate joint forces/Hessians from adjacent joints
4. Solve 6x6 block system via Schur complement: S = A - C*M^-1*C^T
5. Update pose: rotation from angular increment, position from linear increment
Args:
dt: Time step.
body_ids_in_color: Body indices in current color group (for parallel coloring).
body_q_prev: Previous body transforms (for damping and friction).
body_q_rest: Rest transforms (for joint targets).
body_mass: Body masses.
body_inv_mass: Inverse masses (0 for kinematic bodies).
body_inertia: Inertia tensors (local body frame).
body_inertia_q: Inertial target transforms (from forward integration).
body_com: Center of mass offsets (local body frame).
adjacency: Body-joint adjacency (CSR format).
joint_*: Joint configuration arrays.
joint_penalty_k: AVBD per-constraint penalty stiffness (one scalar per solver constraint component).
joint_sigma_start: Dahl hysteresis state at start of step.
joint_C_fric: Dahl friction configuration per joint.
external_forces: External linear forces from rigid contacts.
external_torques: External angular torques from rigid contacts.
external_hessian_ll: Linear-linear Hessian block (3x3) from rigid contacts.
external_hessian_al: Angular-linear coupling Hessian block (3x3) from rigid contacts.
external_hessian_aa: Angular-angular Hessian block (3x3) from rigid contacts.
body_q: Current body transforms (input).
body_q_new: Updated body transforms (output) for the current solve sweep.
Note:
- All forces, torques, and Hessian blocks are expressed in the world frame.
"""
tid = wp.tid()
body_index = body_ids_in_color[tid]
q_current = body_q[body_index]
# Early exit for kinematic bodies
if body_inv_mass[body_index] == 0.0:
body_q_new[body_index] = q_current
return
# Inertial force and Hessian
dt_sqr_reciprocal = 1.0 / (dt * dt)
# Read body properties
q_inertial = body_inertia_q[body_index]
body_com_local = body_com[body_index]
m = body_mass[body_index]
I_body = body_inertia[body_index]
# Extract poses
pos_current = wp.transform_get_translation(q_current)
rot_current = wp.transform_get_rotation(q_current)
pos_star = wp.transform_get_translation(q_inertial)
rot_star = wp.transform_get_rotation(q_inertial)
# Compute COM positions
com_current = pos_current + wp.quat_rotate(rot_current, body_com_local)
com_star = pos_star + wp.quat_rotate(rot_star, body_com_local)
# Linear inertial force and Hessian
inertial_coeff = m * dt_sqr_reciprocal
f_lin = (com_star - com_current) * inertial_coeff
# Compute relative rotation via quaternion difference
# dq = q_current^-1 * q_star
q_delta = wp.mul(wp.quat_inverse(rot_current), rot_star)
# Enforce shortest path (w > 0) to avoid double-cover ambiguity
if q_delta[3] < 0.0:
q_delta = wp.quat(-q_delta[0], -q_delta[1], -q_delta[2], -q_delta[3])
# Rotation vector
axis_body, angle_body = wp.quat_to_axis_angle(q_delta)
theta_body = axis_body * angle_body
# Angular inertial torque
tau_body = I_body * (theta_body * dt_sqr_reciprocal)
tau_world = wp.quat_rotate(rot_current, tau_body)
# Angular Hessian in world frame: use full inertia (supports off-diagonal products of inertia)
R_cur = wp.quat_to_matrix(rot_current)
I_world = R_cur * I_body * wp.transpose(R_cur)
angular_hessian = dt_sqr_reciprocal * I_world
# Accumulate external forces (rigid contacts)
# Read external contributions
ext_torque = external_torques[body_index]
ext_force = external_forces[body_index]
ext_h_aa = external_hessian_aa[body_index]
ext_h_al = external_hessian_al[body_index]
ext_h_ll = external_hessian_ll[body_index]
f_torque = tau_world + ext_torque
f_force = f_lin + ext_force
h_aa = angular_hessian + ext_h_aa
h_al = ext_h_al
h_ll = wp.mat33(
ext_h_ll[0, 0] + inertial_coeff,
ext_h_ll[0, 1],
ext_h_ll[0, 2],
ext_h_ll[1, 0],
ext_h_ll[1, 1] + inertial_coeff,
ext_h_ll[1, 2],
ext_h_ll[2, 0],
ext_h_ll[2, 1],
ext_h_ll[2, 2] + inertial_coeff,
)
# Accumulate joint forces (constraints)
num_adj_joints = get_body_num_adjacent_joints(adjacency, body_index)
for joint_counter in range(num_adj_joints):
joint_idx = get_body_adjacent_joint_id(adjacency, body_index, joint_counter)
joint_force, joint_torque, joint_H_ll, joint_H_al, joint_H_aa = evaluate_joint_force_hessian(
body_index,
joint_idx,
body_q,
body_q_prev,
body_q_rest,
body_com,
joint_type,
joint_enabled,
joint_parent,
joint_child,
joint_X_p,
joint_X_c,
joint_axis,
joint_qd_start,
joint_constraint_start,
joint_penalty_k,
joint_penalty_kd,
joint_sigma_start,
joint_C_fric,
joint_target_ke,
joint_target_kd,
joint_target_pos,
joint_target_vel,
joint_limit_lower,
joint_limit_upper,
joint_limit_ke,
joint_limit_kd,
joint_dof_dim,
joint_rest_angle,
dt,
)
f_force = f_force + joint_force
f_torque = f_torque + joint_torque
h_ll = h_ll + joint_H_ll
h_al = h_al + joint_H_al
h_aa = h_aa + joint_H_aa
# Solve 6x6 block system via Schur complement
# Regularize angular Hessian (in-place)
trA = wp.trace(h_aa) / 3.0
epsA = 1.0e-9 * (trA + 1.0)
h_aa[0, 0] = h_aa[0, 0] + epsA
h_aa[1, 1] = h_aa[1, 1] + epsA
h_aa[2, 2] = h_aa[2, 2] + epsA
# Factorize linear Hessian
Lm_p = chol33(h_ll)
# Compute M^-1 * f_force
MinvF = chol33_solve(Lm_p, f_force)
# Compute H_ll^{-1} * (H_al^T)
C_r0 = wp.vec3(h_al[0, 0], h_al[0, 1], h_al[0, 2])
C_r1 = wp.vec3(h_al[1, 0], h_al[1, 1], h_al[1, 2])
C_r2 = wp.vec3(h_al[2, 0], h_al[2, 1], h_al[2, 2])
X0 = chol33_solve(Lm_p, C_r0)
X1 = chol33_solve(Lm_p, C_r1)
X2 = chol33_solve(Lm_p, C_r2)
# Columns are the solved vectors X0, X1, X2
MinvCt = wp.mat33(X0[0], X1[0], X2[0], X0[1], X1[1], X2[1], X0[2], X1[2], X2[2])
# Compute Schur complement
S = h_aa - (h_al * MinvCt)
# Factorize Schur complement
Ls_p = chol33(S)
# Solve for angular increment
rhs_w = f_torque - (h_al * MinvF)
w_world = chol33_solve(Ls_p, rhs_w)
# Solve for linear increment
Ct_w = wp.transpose(h_al) * w_world
x_inc = chol33_solve(Lm_p, f_force - Ct_w)
# Update pose from increments
# Convert angular increment to quaternion
if _USE_SMALL_ANGLE_APPROX:
half_w = w_world * 0.5
dq_world = wp.quat(half_w[0], half_w[1], half_w[2], 1.0)
dq_world = wp.normalize(dq_world)
else:
ang_mag = wp.length(w_world)
if ang_mag > _SMALL_ANGLE_EPS:
dq_world = wp.quat_from_axis_angle(w_world / ang_mag, ang_mag)
else:
half_w = w_world * 0.5
dq_world = wp.quat(half_w[0], half_w[1], half_w[2], 1.0)
dq_world = wp.normalize(dq_world)
# Apply rotation
rot_new = wp.mul(dq_world, rot_current)
rot_new = wp.normalize(rot_new)
# Update position
com_new = com_current + x_inc
pos_new = com_new - wp.quat_rotate(rot_new, body_com_local)
body_q_new[body_index] = wp.transform(pos_new, rot_new)
@wp.kernel
def copy_rigid_body_transforms_back(
body_ids_in_color: wp.array[wp.int32],
body_q_in: wp.array[wp.transform],
body_q_out: wp.array[wp.transform],
):
"""
Copy body transforms for the current color group from source to destination.
Used after the per-color solve to write updated world transforms from a temporary
buffer back to the main array.
Args:
body_ids_in_color: Body indices in the current color group
body_q_in: Source body transforms (input)
body_q_out: Destination body transforms (output)
"""
tid = wp.tid()
body_index = body_ids_in_color[tid]
body_q_out[body_index] = body_q_in[body_index]
@wp.kernel
def update_duals_joint(
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_axis: wp.array[wp.vec3],
joint_qd_start: wp.array[int],
joint_constraint_start: wp.array[int],
joint_penalty_k_max: wp.array[float],
body_q: wp.array[wp.transform],
body_q_rest: wp.array[wp.transform],
beta: float,
joint_penalty_k: wp.array[float], # input/output
joint_dof_dim: wp.array2d[int],
joint_rest_angle: wp.array[float],
# Drive/limit parameters for adaptive drive/limit penalty growth
joint_target_ke: wp.array[float],
joint_target_pos: wp.array[float],
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
joint_limit_ke: wp.array[float],
):
"""
Update AVBD penalty parameters for joint constraints and drive/limits (per-iteration).
Increases per-constraint penalties based on current constraint violation magnitudes,
clamped by the per-constraint stiffness cap ``joint_penalty_k_max``.
For drive/limit slots, the cap is conditional on the active mode:
limit violated -> cap = ``model.joint_limit_ke``, drive active -> cap = ``model.joint_target_ke``.
Solver invariant: For each free DOF, drive and limit are mutually exclusive at runtime.
Limits have priority. The shared AVBD slot relies on this ordering.
Notes:
- For ``JointType.CABLE``, ``joint_penalty_k_max`` is populated from ``model.joint_target_ke`` (material/constraint tuning).
- For rigid constraint slots like ``JointType.BALL``, ``JointType.FIXED``, ``JointType.REVOLUTE``,
``JointType.PRISMATIC``, and ``JointType.D6``, ``joint_penalty_k_max`` is populated from solver-level caps
(e.g. ``rigid_joint_linear_ke`` / ``rigid_joint_angular_ke``).
- For drive/limit slots, ``joint_penalty_k_max`` stores the conservative warmstart ceiling
``max(model.joint_target_ke, model.joint_limit_ke)``; the dual still clamps growth to the
active branch's cap (drive or limit) on each iteration.
Args:
joint_type: Joint types
joint_parent: Parent body indices
joint_child: Child body indices
joint_X_p: Parent joint frames (local)
joint_X_c: Child joint frames (local)
joint_axis: Joint axis directions (per-DOF, used by REVOLUTE, PRISMATIC, and D6)
joint_qd_start: Joint DOF start indices (used to index into joint_axis)
joint_constraint_start: Start index per joint in the solver constraint layout
joint_penalty_k_max: Per-constraint stiffness cap (used for penalty clamping)
body_q: Current body transforms (world)
body_q_rest: Rest body transforms (world)
beta: Adaptation rate
joint_penalty_k: In/out per-constraint adaptive penalties
joint_dof_dim: Per-joint [lin_count, ang_count] for D6 joints
joint_target_ke: Model drive stiffness (for conditional cap in drive/limit dual update)
joint_target_pos: Control drive target position/angle (for drive violation computation)
joint_limit_lower: Model limit lower bound
joint_limit_upper: Model limit upper bound
joint_limit_ke: Model limit stiffness (for conditional cap in drive/limit dual update)
"""
j = wp.tid()
if not joint_enabled[j]:
return
parent = joint_parent[j]
child = joint_child[j]
# Early exit for invalid joints
if child < 0:
return
jt = joint_type[j]
if (
jt != JointType.CABLE
and jt != JointType.BALL
and jt != JointType.FIXED
and jt != JointType.REVOLUTE
and jt != JointType.PRISMATIC
and jt != JointType.D6
):
return
# Read solver constraint start index
c_start = joint_constraint_start[j]
# Compute joint frames in world space
if parent >= 0:
X_wp = body_q[parent] * joint_X_p[j]
X_wp_rest = body_q_rest[parent] * joint_X_p[j]
else:
X_wp = joint_X_p[j]
X_wp_rest = joint_X_p[j]
X_wc = body_q[child] * joint_X_c[j]
X_wc_rest = body_q_rest[child] * joint_X_c[j]
# Cable joint: adaptive penalty for stretch and bend constraints
if jt == JointType.CABLE:
# Read joint frame rotations
q_wp = wp.transform_get_rotation(X_wp)
q_wc = wp.transform_get_rotation(X_wc)
q_wp_rest = wp.transform_get_rotation(X_wp_rest)
q_wc_rest = wp.transform_get_rotation(X_wc_rest)
# Compute scalar violation magnitudes used for penalty growth.
# Stretch uses meters residual magnitude ||x_c - x_p|| to update an effective [N/m] stiffness.
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
C_stretch = wp.length(x_c - x_p)
# Compute bend constraint violation (rotation vector magnitude)
kappa = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest)
C_bend = wp.length(kappa)
# Update stretch penalty (constraint slot 0)
stretch_idx = c_start
stiffness_stretch = joint_penalty_k_max[stretch_idx]
k_stretch = joint_penalty_k[stretch_idx]
k_stretch_new = wp.min(k_stretch + beta * C_stretch, stiffness_stretch)
joint_penalty_k[stretch_idx] = k_stretch_new
# Update bend penalty (constraint slot 1)
bend_idx = c_start + 1
stiffness_bend = joint_penalty_k_max[bend_idx]
k_bend = joint_penalty_k[bend_idx]
k_bend_new = wp.min(k_bend + beta * C_bend, stiffness_bend)
joint_penalty_k[bend_idx] = k_bend_new
return
# BALL joint: update isotropic linear anchor-coincidence penalty (single scalar).
if jt == JointType.BALL:
# Scalar violation magnitude used for penalty growth; force/Hessian uses C_vec directly.
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
C_vec = x_c - x_p
C_lin = wp.length(C_vec)
i0 = c_start
k0 = joint_penalty_k[i0]
joint_penalty_k[i0] = wp.min(k0 + beta * C_lin, joint_penalty_k_max[i0])
return
# FIXED joint: update isotropic linear + isotropic angular penalties (2 scalars).
if jt == JointType.FIXED:
i_lin = c_start + 0
i_ang = c_start + 1
# Linear violation magnitude
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
C_lin = wp.length(x_c - x_p)
k_lin = joint_penalty_k[i_lin]
joint_penalty_k[i_lin] = wp.min(k_lin + beta * C_lin, joint_penalty_k_max[i_lin])
# Angular violation magnitude from kappa
q_wp = wp.transform_get_rotation(X_wp)
q_wc = wp.transform_get_rotation(X_wc)
q_wp_rest = wp.transform_get_rotation(X_wp_rest)
q_wc_rest = wp.transform_get_rotation(X_wc_rest)
kappa = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest)
C_ang = wp.length(kappa)
k_ang = joint_penalty_k[i_ang]
joint_penalty_k[i_ang] = wp.min(k_ang + beta * C_ang, joint_penalty_k_max[i_ang])
return
# REVOLUTE joint: isotropic linear + perpendicular angular penalties (2 scalars).
if jt == JointType.REVOLUTE:
i_lin = c_start + 0
i_ang = c_start + 1
qd_start = joint_qd_start[j]
q_wp = wp.transform_get_rotation(X_wp)
P_lin, P_ang = build_joint_projectors(jt, joint_axis, qd_start, 0, 1, q_wp)
a = wp.normalize(joint_axis[qd_start])
# Linear violation magnitude (full, P_lin = I for REVOLUTE)
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
C_lin = wp.length(P_lin * (x_c - x_p))
k_lin = joint_penalty_k[i_lin]
joint_penalty_k[i_lin] = wp.min(k_lin + beta * C_lin, joint_penalty_k_max[i_lin])
# Angular violation: projected perpendicular components of kappa
q_wc = wp.transform_get_rotation(X_wc)
q_wp_rest = wp.transform_get_rotation(X_wp_rest)
q_wc_rest = wp.transform_get_rotation(X_wc_rest)
kappa = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest)
C_ang = wp.length(P_ang * kappa)
k_ang = joint_penalty_k[i_ang]
joint_penalty_k[i_ang] = wp.min(k_ang + beta * C_ang, joint_penalty_k_max[i_ang])
# Drive/limit dual update for free angular DOF (slot c_start + 2)
dof_idx = qd_start
model_drive_ke = joint_target_ke[dof_idx]
model_limit_ke = joint_limit_ke[dof_idx]
lim_lower = joint_limit_lower[dof_idx]
lim_upper = joint_limit_upper[dof_idx]
has_drive = model_drive_ke > 0.0
has_limits = model_limit_ke > 0.0 and (lim_lower > -MAXVAL or lim_upper < MAXVAL)
if has_drive or has_limits:
theta = wp.dot(kappa, a)
theta_abs = theta + joint_rest_angle[dof_idx]
target_pos = joint_target_pos[dof_idx]
mode, err_pos = resolve_drive_limit_mode(theta_abs, target_pos, lim_lower, lim_upper, has_drive, has_limits)
i_dl = c_start + 2
k_dl = joint_penalty_k[i_dl]
C_dl = wp.abs(err_pos)
cap = joint_penalty_k_max[i_dl]
if mode == _DRIVE_LIMIT_MODE_LIMIT_LOWER or mode == _DRIVE_LIMIT_MODE_LIMIT_UPPER:
cap = model_limit_ke
elif mode == _DRIVE_LIMIT_MODE_DRIVE:
cap = model_drive_ke
joint_penalty_k[i_dl] = wp.min(k_dl + beta * C_dl, cap)
return
# PRISMATIC joint: perpendicular linear + isotropic angular penalties (2 scalars).
if jt == JointType.PRISMATIC:
i_lin = c_start + 0
i_ang = c_start + 1
qd_start = joint_qd_start[j]
axis_local = joint_axis[qd_start]
q_wp = wp.transform_get_rotation(X_wp)
P_lin, P_ang = build_joint_projectors(jt, joint_axis, qd_start, 1, 0, q_wp)
# Linear violation: projected perpendicular components
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
C_vec = x_c - x_p
C_lin = wp.length(P_lin * C_vec)
k_lin = joint_penalty_k[i_lin]
joint_penalty_k[i_lin] = wp.min(k_lin + beta * C_lin, joint_penalty_k_max[i_lin])
# Angular violation: full kappa (P_ang = I for PRISMATIC)
q_wc = wp.transform_get_rotation(X_wc)
q_wp_rest = wp.transform_get_rotation(X_wp_rest)
q_wc_rest = wp.transform_get_rotation(X_wc_rest)
kappa = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest)
C_ang = wp.length(P_ang * kappa)
k_ang = joint_penalty_k[i_ang]
joint_penalty_k[i_ang] = wp.min(k_ang + beta * C_ang, joint_penalty_k_max[i_ang])
# Drive/limit dual update for free linear DOF (slot c_start + 2)
dof_idx = qd_start
model_drive_ke = joint_target_ke[dof_idx]
model_limit_ke = joint_limit_ke[dof_idx]
lim_lower = joint_limit_lower[dof_idx]
lim_upper = joint_limit_upper[dof_idx]
has_drive = model_drive_ke > 0.0
has_limits = model_limit_ke > 0.0 and (lim_lower > -MAXVAL or lim_upper < MAXVAL)
if has_drive or has_limits:
axis_w_dl = wp.normalize(wp.quat_rotate(q_wp, axis_local))
d_along = wp.dot(C_vec, axis_w_dl)
target_pos = joint_target_pos[dof_idx]
mode, err_pos = resolve_drive_limit_mode(d_along, target_pos, lim_lower, lim_upper, has_drive, has_limits)
i_dl = c_start + 2
k_dl = joint_penalty_k[i_dl]
C_dl = wp.abs(err_pos)
cap = joint_penalty_k_max[i_dl]
if mode == _DRIVE_LIMIT_MODE_LIMIT_LOWER or mode == _DRIVE_LIMIT_MODE_LIMIT_UPPER:
cap = model_limit_ke
elif mode == _DRIVE_LIMIT_MODE_DRIVE:
cap = model_drive_ke
joint_penalty_k[i_dl] = wp.min(k_dl + beta * C_dl, cap)
return
# D6 joint: projected linear + projected angular penalties (2 scalars).
if jt == JointType.D6:
i_lin = c_start + 0
i_ang = c_start + 1
lin_count = joint_dof_dim[j, 0]
ang_count = joint_dof_dim[j, 1]
qd_start = joint_qd_start[j]
q_wp_rot = wp.transform_get_rotation(X_wp)
P_lin, P_ang = build_joint_projectors(jt, joint_axis, qd_start, lin_count, ang_count, q_wp_rot)
# Linear violation: measure projected error
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
C_vec = x_c - x_p
if lin_count < 3:
C_lin = wp.length(P_lin * C_vec)
else:
C_lin = 0.0
k_lin = joint_penalty_k[i_lin]
joint_penalty_k[i_lin] = wp.min(k_lin + beta * C_lin, joint_penalty_k_max[i_lin])
# Angular violation: measure projected kappa
q_wc = wp.transform_get_rotation(X_wc)
q_wp_rest = wp.transform_get_rotation(X_wp_rest)
q_wc_rest = wp.transform_get_rotation(X_wc_rest)
kappa = cable_get_kappa(q_wp_rot, q_wc, q_wp_rest, q_wc_rest)
if ang_count < 3:
C_ang = wp.length(P_ang * kappa)
else:
C_ang = 0.0
k_ang = joint_penalty_k[i_ang]
joint_penalty_k[i_ang] = wp.min(k_ang + beta * C_ang, joint_penalty_k_max[i_ang])
# Drive/limit dual update for D6 free DOFs
# Per free linear DOF
for li in range(3):
if li < lin_count:
dof_idx = qd_start + li
model_drive_ke = joint_target_ke[dof_idx]
model_limit_ke = joint_limit_ke[dof_idx]
lim_lower = joint_limit_lower[dof_idx]
lim_upper = joint_limit_upper[dof_idx]
has_drive = model_drive_ke > 0.0
has_limits = model_limit_ke > 0.0 and (lim_lower > -MAXVAL or lim_upper < MAXVAL)
if has_drive or has_limits:
axis_w_dl = wp.normalize(wp.quat_rotate(q_wp_rot, joint_axis[dof_idx]))
d_along = wp.dot(C_vec, axis_w_dl)
target_pos_dl = joint_target_pos[dof_idx]
mode, err_pos = resolve_drive_limit_mode(
d_along, target_pos_dl, lim_lower, lim_upper, has_drive, has_limits
)
i_dl = c_start + 2 + li
k_dl = joint_penalty_k[i_dl]
C_dl = wp.abs(err_pos)
cap = joint_penalty_k_max[i_dl]
if mode == _DRIVE_LIMIT_MODE_LIMIT_LOWER or mode == _DRIVE_LIMIT_MODE_LIMIT_UPPER:
cap = model_limit_ke
elif mode == _DRIVE_LIMIT_MODE_DRIVE:
cap = model_drive_ke
joint_penalty_k[i_dl] = wp.min(k_dl + beta * C_dl, cap)
# Per free angular DOF
for ai in range(3):
if ai < ang_count:
dof_idx = qd_start + lin_count + ai
model_drive_ke = joint_target_ke[dof_idx]
model_limit_ke = joint_limit_ke[dof_idx]
lim_lower = joint_limit_lower[dof_idx]
lim_upper = joint_limit_upper[dof_idx]
has_drive = model_drive_ke > 0.0
has_limits = model_limit_ke > 0.0 and (lim_lower > -MAXVAL or lim_upper < MAXVAL)
if has_drive or has_limits:
a_dl = wp.normalize(joint_axis[dof_idx])
theta = wp.dot(kappa, a_dl)
theta_abs = theta + joint_rest_angle[dof_idx]
target_pos_dl = joint_target_pos[dof_idx]
mode, err_pos = resolve_drive_limit_mode(
theta_abs, target_pos_dl, lim_lower, lim_upper, has_drive, has_limits
)
i_dl = c_start + 2 + lin_count + ai
k_dl = joint_penalty_k[i_dl]
C_dl = wp.abs(err_pos)
cap = joint_penalty_k_max[i_dl]
if mode == _DRIVE_LIMIT_MODE_LIMIT_LOWER or mode == _DRIVE_LIMIT_MODE_LIMIT_UPPER:
cap = model_limit_ke
elif mode == _DRIVE_LIMIT_MODE_DRIVE:
cap = model_drive_ke
joint_penalty_k[i_dl] = wp.min(k_dl + beta * C_dl, cap)
return
@wp.kernel
def update_duals_body_body_contacts(
rigid_contact_count: wp.array[int],
rigid_contact_shape0: wp.array[int],
rigid_contact_shape1: wp.array[int],
rigid_contact_point0: wp.array[wp.vec3],
rigid_contact_point1: wp.array[wp.vec3],
rigid_contact_normal: wp.array[wp.vec3],
rigid_contact_margin0: wp.array[float],
rigid_contact_margin1: wp.array[float],
shape_body: wp.array[int],
body_q: wp.array[wp.transform],
contact_material_ke: wp.array[float],
beta: float,
contact_penalty_k: wp.array[float], # input/output
):
"""
Update AVBD penalty parameters for contact constraints (per-iteration).
Increases each contact's penalty by beta * penetration and clamps to the
per-contact material stiffness.
Args:
rigid_contact_count: Number of active contacts
rigid_contact_shape0/1: Shape ids for each contact pair
rigid_contact_point0/1: Contact points in local shape frames
rigid_contact_normal: Contact normals (pointing from shape0 to shape1)
rigid_contact_margin0/1: Per-shape margin (for SDF/capsule padding)
shape_body: Map from shape id to body id (-1 if kinematic/ground)
body_q: Current body transforms
contact_material_ke: Per-contact target stiffness
beta: Adaptation rate
contact_penalty_k: In/out per-contact adaptive penalties
"""
idx = wp.tid()
if idx >= rigid_contact_count[0]:
return
# Read contact geometry
shape_id_0 = rigid_contact_shape0[idx]
shape_id_1 = rigid_contact_shape1[idx]
body_id_0 = shape_body[shape_id_0]
body_id_1 = shape_body[shape_id_1]
# Skip invalid contacts (both bodies kinematic/ground)
if body_id_0 < 0 and body_id_1 < 0:
return
# Read cached material stiffness
stiffness = contact_material_ke[idx]
# Transform contact points to world space
if body_id_0 >= 0:
p0_world = wp.transform_point(body_q[body_id_0], rigid_contact_point0[idx])
else:
p0_world = rigid_contact_point0[idx]
if body_id_1 >= 0:
p1_world = wp.transform_point(body_q[body_id_1], rigid_contact_point1[idx])
else:
p1_world = rigid_contact_point1[idx]
# Compute penetration depth (constraint violation)
# Distance along the A-to-B normal; positive implies separation
d = p1_world - p0_world
dist = wp.dot(rigid_contact_normal[idx], d)
thickness_total = rigid_contact_margin0[idx] + rigid_contact_margin1[idx]
penetration = wp.max(0.0, thickness_total - dist)
# Update penalty: k_new = min(k + beta * |C|, stiffness)
k = contact_penalty_k[idx]
k_new = wp.min(k + beta * penetration, stiffness)
contact_penalty_k[idx] = k_new
@wp.kernel
def update_duals_body_particle_contacts(
body_particle_contact_count: wp.array[int],
body_particle_contact_particle: wp.array[int],
body_particle_contact_shape: wp.array[int],
body_particle_contact_body_pos: wp.array[wp.vec3],
body_particle_contact_normal: wp.array[wp.vec3],
particle_q: wp.array[wp.vec3],
particle_radius: wp.array[float],
shape_body: wp.array[int],
body_q: wp.array[wp.transform],
body_particle_contact_material_ke: wp.array[float],
beta: float,
body_particle_contact_penalty_k: wp.array[float], # input/output
):
"""
Update AVBD penalty parameters for soft contact constraints (per-iteration).
Increases each soft contact's penalty by beta * penetration and clamps to the
per-contact material stiffness.
Args:
body_particle_contact_count: Number of active body-particle soft contacts
body_particle_contact_particle: Particle index for each body-particle soft contact
body_particle_contact_shape: Shape index for each body-particle soft contact
body_particle_contact_body_pos: Contact points in local shape frames
body_particle_contact_normal: Contact normals (pointing from rigid to particle)
particle_q: Current particle positions
particle_radius: Particle radii
shape_body: Map from shape id to body id (-1 if kinematic/ground)
body_q: Current body transforms
body_particle_contact_material_ke: Per-contact target stiffness (averaged)
beta: Adaptation rate
body_particle_contact_penalty_k: In/out per-contact adaptive penalties
"""
idx = wp.tid()
if idx >= body_particle_contact_count[0]:
return
# Read contact data
particle_idx = body_particle_contact_particle[idx]
shape_idx = body_particle_contact_shape[idx]
body_idx = shape_body[shape_idx] if shape_idx >= 0 else -1
# Read cached material stiffness
stiffness = body_particle_contact_material_ke[idx]
# Transform contact point to world space
# For rigid bodies (body_idx >= 0), transform from body-local to world space
X_wb = wp.transform_identity()
if body_idx >= 0:
X_wb = body_q[body_idx]
cp_local = body_particle_contact_body_pos[idx]
cp_world = wp.transform_point(X_wb, cp_local)
# Get particle data
particle_pos = particle_q[particle_idx]
radius = particle_radius[particle_idx]
n = body_particle_contact_normal[idx]
# Compute penetration depth (constraint violation)
penetration = -(wp.dot(n, particle_pos - cp_world) - radius)
penetration = wp.max(0.0, penetration)
# Update penalty: k_new = min(k + beta * |C|, stiffness)
k = body_particle_contact_penalty_k[idx]
k_new = wp.min(k + beta * penetration, stiffness)
body_particle_contact_penalty_k[idx] = k_new
# -----------------------------
# Post-iteration kernels (after all iterations)
# -----------------------------
@wp.kernel
def update_body_velocity(
dt: float,
body_q: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
body_q_prev: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
):
"""
Update body velocities from position changes (world frame).
Computes linear and angular velocities using finite differences.
Linear: v = (com_current - com_prev) / dt
Angular: omega from quaternion difference dq = q * q_prev^-1
Args:
dt: Time step.
body_q: Current body transforms (world).
body_com: Center of mass offsets (local frame).
body_q_prev: Previous body transforms (input/output, advanced to current pose for next step).
body_qd: Output body velocities (spatial vectors, world frame).
"""
tid = wp.tid()
# Read transforms
pose = body_q[tid]
pose_prev = body_q_prev[tid]
x = wp.transform_get_translation(pose)
x_prev = wp.transform_get_translation(pose_prev)
q = wp.transform_get_rotation(pose)
q_prev = wp.transform_get_rotation(pose_prev)
# Compute COM positions
com_local = body_com[tid]
x_com = x + wp.quat_rotate(q, com_local)
x_com_prev = x_prev + wp.quat_rotate(q_prev, com_local)
# Linear velocity
v = (x_com - x_com_prev) / dt
# Angular velocity
omega = quat_velocity(q, q_prev, dt)
body_qd[tid] = wp.spatial_vector(v, omega)
# Advance body_q_prev for next step (for kinematic bodies this is the only write).
body_q_prev[tid] = pose
@wp.kernel
def update_cable_dahl_state(
# Joint geometry
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_constraint_start: wp.array[int],
joint_penalty_k_max: wp.array[float],
# Body states (final, after solver convergence)
body_q: wp.array[wp.transform],
body_q_rest: wp.array[wp.transform],
# Dahl model parameters (PER-JOINT arrays, isotropic)
joint_eps_max: wp.array[float],
joint_tau: wp.array[float],
# Dahl state (inputs - from previous timestep, outputs - to next timestep) - component-wise (vec3)
joint_sigma_prev: wp.array[wp.vec3], # input/output
joint_kappa_prev: wp.array[wp.vec3], # input/output
joint_dkappa_prev: wp.array[wp.vec3], # input/output (stores Delta kappa)
):
"""
Post-iteration kernel: update Dahl hysteresis state after solver convergence (component-wise).
Stores final curvature, friction stress, and curvature Delta kappa for the next step. Each
curvature component (x, y, z) is updated independently to preserve path-dependent memory.
Args:
joint_type: Joint type (only updates for cable joints)
joint_parent, joint_child: Parent/child body indices
joint_X_p, joint_X_c: Joint frames in parent/child
joint_constraint_start: Start index per joint in the solver constraint layout
joint_penalty_k_max: Per-constraint stiffness cap; for cables, bend cap stores effective per-joint bend stiffness [N*m]
body_q: Final body transforms (after convergence)
body_q_rest: Rest body transforms
joint_sigma_prev: Friction stress state (read old, write new), wp.vec3 per joint
joint_kappa_prev: Curvature state (read old, write new), wp.vec3 per joint
joint_dkappa_prev: Delta-kappa state (write new), wp.vec3 per joint
joint_eps_max: Maximum persistent strain [rad] (scalar per joint)
joint_tau: Memory decay length [rad] (scalar per joint)
"""
j = wp.tid()
# Only update cable joints
if joint_type[j] != JointType.CABLE:
return
# Get parent and child body indices
parent = joint_parent[j]
child = joint_child[j]
# World-parent joints are valid; child body must exist.
if child < 0:
return
# Compute joint frames in world space (final state)
if parent >= 0:
X_wp = body_q[parent] * joint_X_p[j]
X_wp_rest = body_q_rest[parent] * joint_X_p[j]
else:
X_wp = joint_X_p[j]
X_wp_rest = joint_X_p[j]
X_wc = body_q[child] * joint_X_c[j]
X_wc_rest = body_q_rest[child] * joint_X_c[j]
q_wp = wp.transform_get_rotation(X_wp)
q_wc = wp.transform_get_rotation(X_wc)
q_wp_rest = wp.transform_get_rotation(X_wp_rest)
q_wc_rest = wp.transform_get_rotation(X_wc_rest)
# Compute final curvature vector at end of timestep
kappa_final = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest)
if not joint_enabled[j]:
# Refresh Dahl state to current configuration with zero preload so that
# re-enabling the joint does not see a stale kappa delta.
joint_kappa_prev[j] = kappa_final
joint_sigma_prev[j] = wp.vec3(0.0)
joint_dkappa_prev[j] = wp.vec3(0.0)
return
# Read stored Dahl state (component-wise vectors)
kappa_old = joint_kappa_prev[j] # stored curvature
d_kappa_old = joint_dkappa_prev[j] # stored Delta kappa
sigma_old = joint_sigma_prev[j] # stored friction stress
# Read per-joint Dahl parameters (isotropic)
eps_max = joint_eps_max[j] # Maximum persistent strain [rad]
tau = joint_tau[j] # Memory decay length [rad]
# Bend stiffness target (cap) is stored in constraint slot 1 for cable joints.
c_start = joint_constraint_start[j]
k_bend_target = joint_penalty_k_max[c_start + 1] # [N*m]
# Friction envelope: sigma_max = k_bend_target * eps_max.
sigma_max = k_bend_target * eps_max # [N*m]
# Early-out: disable friction if envelope is zero/invalid
if sigma_max <= 0.0 or tau <= 0.0:
joint_sigma_prev[j] = wp.vec3(0.0)
joint_kappa_prev[j] = kappa_final
joint_dkappa_prev[j] = kappa_final - kappa_old # store Delta kappa
return
# Update each component independently (3 separate hysteresis loops)
sigma_final_out = wp.vec3(0.0)
d_kappa_out = wp.vec3(0.0)
for axis in range(3):
# Get component values
kappa_i_final = kappa_final[axis]
kappa_i_prev = kappa_old[axis]
d_kappa_i_prev = d_kappa_old[axis]
sigma_i_prev = sigma_old[axis]
# Curvature change for this component
d_kappa_i = kappa_i_final - kappa_i_prev
# Direction flag (same logic as pre-iteration kernel), in kappa-space
s_i = 1.0
if d_kappa_i > _DAHL_KAPPADOT_DEADBAND:
s_i = 1.0
elif d_kappa_i < -_DAHL_KAPPADOT_DEADBAND:
s_i = -1.0
else:
# Within deadband: maintain previous direction
s_i = 1.0 if d_kappa_i_prev >= 0.0 else -1.0
# sigma_i_next = s_i*sigma_max * [1 - exp(-s_i*d_kappa_i/tau)] + sigma_i_prev * exp(-s_i*d_kappa_i/tau)
exp_term = wp.exp(-s_i * d_kappa_i / tau)
sigma_i_next = s_i * sigma_max * (1.0 - exp_term) + sigma_i_prev * exp_term
# Store component results
sigma_final_out[axis] = sigma_i_next
d_kappa_out[axis] = d_kappa_i
# Store final vector state for next timestep
joint_sigma_prev[j] = sigma_final_out
joint_kappa_prev[j] = kappa_final
joint_dkappa_prev[j] = d_kappa_out
================================================
FILE: newton/_src/solvers/vbd/solver_vbd.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from typing import Any
import numpy as np
import warp as wp
from ...core.types import override
from ...sim import (
Contacts,
Control,
JointType,
Model,
ModelBuilder,
State,
)
from ..flags import SolverNotifyFlags
from ..solver import SolverBase
from ..xpbd.kernels import apply_joint_forces
from .particle_vbd_kernels import (
NUM_THREADS_PER_COLLISION_PRIMITIVE,
TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE,
ParticleForceElementAdjacencyInfo,
# Adjacency building kernels
_count_num_adjacent_edges,
_count_num_adjacent_faces,
_count_num_adjacent_springs,
_count_num_adjacent_tets,
_fill_adjacent_edges,
_fill_adjacent_faces,
_fill_adjacent_springs,
_fill_adjacent_tets,
# Topological filtering helper functions
accumulate_particle_body_contact_force_and_hessian,
accumulate_self_contact_force_and_hessian,
accumulate_spring_force_and_hessian,
# Planar DAT (Divide and Truncate) kernels
apply_planar_truncation_parallel_by_collision,
apply_truncation_ts,
build_edge_n_ring_edge_collision_filter,
build_vertex_n_ring_tris_collision_filter,
# Solver kernels (particle VBD)
forward_step,
set_to_csr,
solve_elasticity,
solve_elasticity_tile,
update_velocity,
)
from .rigid_vbd_kernels import (
_NUM_CONTACT_THREADS_PER_BODY,
RigidForceElementAdjacencyInfo,
# Adjacency building kernels
_count_num_adjacent_joints,
_fill_adjacent_joints,
# Iteration kernels
accumulate_body_body_contacts_per_body, # Body-body (rigid-rigid) contacts (Gauss-Seidel mode)
accumulate_body_particle_contacts_per_body, # Body-particle soft contacts (two-way coupling)
build_body_body_contact_lists, # Body-body (rigid-rigid) contact adjacency
build_body_particle_contact_lists, # Body-particle (rigid-particle) soft-contact adjacency
compute_cable_dahl_parameters, # Cable bending plasticity
compute_rigid_contact_forces,
copy_rigid_body_transforms_back,
# Pre-iteration kernels (rigid AVBD)
forward_step_rigid_bodies,
solve_rigid_body,
# Post-iteration kernels
update_body_velocity,
update_cable_dahl_state,
update_duals_body_body_contacts, # Body-body (rigid-rigid) contacts (AVBD penalty update)
update_duals_body_particle_contacts, # Body-particle soft contacts (AVBD penalty update)
update_duals_joint, # Cable joints (AVBD penalty update)
warmstart_body_body_contacts, # Body-body (rigid-rigid) contacts (penalty warmstart)
warmstart_body_particle_contacts, # Body-particle soft contacts (penalty warmstart)
warmstart_joints, # Cable joints (stretch & bend)
)
from .tri_mesh_collision import (
TriMeshCollisionDetector,
TriMeshCollisionInfo,
)
# Export accumulate_contact_force_and_hessian for legacy collision_legacy.py compatibility
__all__ = ["SolverVBD"]
class SolverVBD(SolverBase):
"""An implicit solver using Vertex Block Descent (VBD) for particles and Augmented VBD (AVBD) for rigid bodies.
This unified solver supports:
- Particle simulation (cloth, soft bodies) using the VBD algorithm
- Rigid body simulation (joints, contacts) using the AVBD algorithm
- Coupled particle-rigid body systems
For rigid bodies, the AVBD algorithm uses **soft constraints** with adaptive penalty parameters
for joints and contacts. Hard constraints are not currently enforced.
Joint limitations:
- Supported joint types: BALL, FIXED, FREE, REVOLUTE, PRISMATIC, D6, CABLE.
DISTANCE joints are not supported.
- :attr:`~newton.Model.joint_enabled` is supported for all joint types.
- :attr:`~newton.Model.joint_target_ke`/:attr:`~newton.Model.joint_target_kd` are supported
for REVOLUTE, PRISMATIC, D6 (as drives), and CABLE (as stretch/bend stiffness and damping).
VBD interprets ``kd`` as a dimensionless Rayleigh coefficient (``D = kd * ke``).
- :attr:`~newton.Model.joint_limit_lower`/:attr:`~newton.Model.joint_limit_upper` and
:attr:`~newton.Model.joint_limit_ke`/:attr:`~newton.Model.joint_limit_kd` are supported
for REVOLUTE, PRISMATIC, and D6 joints. The default ``limit_kd`` in
:class:`~newton.ModelBuilder.JointDofConfig` is ``1e1``, which under VBD's Rayleigh
convention (``D = kd * ke``) can produce excessive damping. When using joint limits
with VBD, explicitly set ``limit_kd`` to a small value.
- :attr:`~newton.Control.joint_f` (feedforward forces) is supported.
- Not supported: :attr:`~newton.Model.joint_armature`, :attr:`~newton.Model.joint_friction`,
:attr:`~newton.Model.joint_target_mode`, equality constraints, mimic constraints.
See :ref:`Joint feature support` for the full comparison across solvers.
References:
- Anka He Chen, Ziheng Liu, Yin Yang, and Cem Yuksel. 2024. Vertex Block Descent. ACM Trans. Graph. 43, 4, Article 116 (July 2024), 16 pages.
https://doi.org/10.1145/3658179
Note:
`SolverVBD` requires coloring information for both particles and rigid bodies:
- Particle coloring: :attr:`newton.Model.particle_color_groups` (required if particles are present)
- Rigid body coloring: :attr:`newton.Model.body_color_groups` (required if rigid bodies are present)
Call :meth:`newton.ModelBuilder.color` to automatically color both particles and rigid bodies.
VBD uses ``model.body_q`` as the structural rest pose and reads
``model.joint_q`` for drive/limit rest-angle offsets. The body
transforms must match the joint angles at solver creation time
(see example below).
Example
-------
.. code-block:: python
# Automatically color both particles and rigid bodies
builder.color()
model = builder.finalize()
solver = newton.solvers.SolverVBD(model)
# Initialize states and contacts
state_in = model.state()
state_out = model.state()
control = model.control()
contacts = model.contacts()
# Simulation loop
for i in range(100):
model.collide(state_in, contacts) # Update contacts
solver.step(state_in, state_out, control, contacts, dt)
state_in, state_out = state_out, state_in
"""
def __init__(
self,
model: Model,
# Common parameters
iterations: int = 10,
friction_epsilon: float = 1e-2,
integrate_with_external_rigid_solver: bool = False,
# Particle parameters
particle_enable_self_contact: bool = False,
particle_self_contact_radius: float = 0.2,
particle_self_contact_margin: float = 0.2,
particle_conservative_bound_relaxation: float = 0.85,
particle_vertex_contact_buffer_size: int = 32,
particle_edge_contact_buffer_size: int = 64,
particle_collision_detection_interval: int = 0,
particle_edge_parallel_epsilon: float = 1e-5,
particle_enable_tile_solve: bool = True,
particle_topological_contact_filter_threshold: int = 2,
particle_rest_shape_contact_exclusion_radius: float = 0.0,
particle_external_vertex_contact_filtering_map: dict | None = None,
particle_external_edge_contact_filtering_map: dict | None = None,
# Rigid body parameters
rigid_avbd_beta: float = 1.0e5,
rigid_avbd_gamma: float = 0.99,
rigid_contact_k_start: float = 1.0e2, # AVBD: initial stiffness for all body contacts (body-body + body-particle)
rigid_joint_linear_k_start: float = 1.0e4, # AVBD: initial stiffness seed for linear joint constraints
rigid_joint_angular_k_start: float = 1.0e1, # AVBD: initial stiffness seed for angular joint constraints
rigid_joint_linear_ke: float = 1.0e9, # AVBD: stiffness cap for non-cable linear joint constraints (BALL/FIXED/REVOLUTE/PRISMATIC/D6)
rigid_joint_angular_ke: float = 1.0e9, # AVBD: stiffness cap for non-cable angular joint constraints (FIXED/REVOLUTE/PRISMATIC/D6)
rigid_joint_linear_kd: float = 1.0e-2, # AVBD: Rayleigh damping coefficient for non-cable linear joint constraints
rigid_joint_angular_kd: float = 0.0, # AVBD: Rayleigh damping coefficient for non-cable angular joint constraints
rigid_body_contact_buffer_size: int = 64,
rigid_body_particle_contact_buffer_size: int = 256,
rigid_enable_dahl_friction: bool = False, # Cable bending plasticity/hysteresis
):
"""
Args:
model: The `Model` object used to initialize the integrator. Must be identical to the `Model` object passed
to the `step` function.
Common parameters:
iterations: Number of VBD iterations per step.
friction_epsilon: Threshold to smooth small relative velocities in friction computation (used for both particle
and rigid body contacts).
Particle parameters:
particle_enable_self_contact: Whether to enable self-contact detection for particles.
particle_self_contact_radius: The radius used for self-contact detection. This is the distance at which
vertex-triangle pairs and edge-edge pairs will start to interact with each other.
particle_self_contact_margin: The margin used for self-contact detection. This is the distance at which
vertex-triangle pairs and edge-edge will be considered in contact generation. It should be larger than
`particle_self_contact_radius` to avoid missing contacts.
integrate_with_external_rigid_solver: Indicator for coupled rigid body-cloth simulation. When set to `True`,
the solver assumes rigid bodies are integrated by an external solver (one-way coupling).
particle_conservative_bound_relaxation: Relaxation factor for conservative penetration-free projection.
particle_vertex_contact_buffer_size: Preallocation size for each vertex's vertex-triangle collision buffer.
particle_edge_contact_buffer_size: Preallocation size for edge's edge-edge collision buffer.
particle_collision_detection_interval: Controls how frequently particle self-contact detection is applied
during the simulation. If set to a value < 0, collision detection is only performed once before the
initialization step. If set to 0, collision detection is applied twice: once before and once immediately
after initialization. If set to a value `n` >= 1, collision detection is applied before every `n` VBD
iterations.
particle_edge_parallel_epsilon: Threshold to detect near-parallel edges in edge-edge collision handling.
particle_enable_tile_solve: Whether to accelerate the particle solver using tile API.
particle_topological_contact_filter_threshold: Maximum topological distance (measured in rings) under which candidate
self-contacts are discarded. Set to a higher value to tolerate contacts between more closely connected mesh
elements. Only used when `particle_enable_self_contact` is `True`. Note that setting this to a value larger than 3 will
result in a significant increase in computation time.
particle_rest_shape_contact_exclusion_radius: Additional world-space distance threshold for filtering topologically close
primitives. Candidate contacts with a rest separation shorter than this value are ignored. The distance is
evaluated in the rest configuration conveyed by `model.particle_q`. Only used when `particle_enable_self_contact` is `True`.
particle_external_vertex_contact_filtering_map: Optional dictionary used to exclude additional vertex-triangle pairs during
contact generation. Keys must be vertex primitive ids (integers), and each value must be a `list` or
`set` containing the triangle primitives to be filtered out. Only used when `particle_enable_self_contact` is `True`.
particle_external_edge_contact_filtering_map: Optional dictionary used to exclude additional edge-edge pairs during contact
generation. Keys must be edge primitive ids (integers), and each value must be a `list` or `set`
containing the edges to be filtered out. Only used when `particle_enable_self_contact` is `True`.
Rigid body parameters:
rigid_avbd_beta: Penalty ramp rate for rigid body constraints (how fast k grows with constraint violation).
rigid_avbd_gamma: Warmstart decay for penalty k (cross-step decay factor for rigid body constraints).
rigid_contact_k_start: Initial penalty stiffness for all body contact constraints, including both body-body (rigid-rigid)
and body-particle (rigid-particle) contacts (AVBD).
rigid_joint_linear_k_start: Initial penalty seed for linear joint constraints (e.g., cable stretch, BALL linear).
Used to seed the per-constraint adaptive penalties for all linear joint constraints.
rigid_joint_angular_k_start: Initial penalty seed for angular joint constraints (e.g., cable bend, FIXED angular).
Used to seed the per-constraint adaptive penalties for all angular joint constraints.
rigid_joint_linear_ke: Stiffness cap used by AVBD for **non-cable** linear joint constraint scalars
(BALL, FIXED, REVOLUTE, PRISMATIC, and D6 projected linear slots). Cable joints use the
per-joint caps in ``model.joint_target_ke`` instead (cable interprets ``joint_target_ke/kd`` as
constraint tuning).
rigid_joint_angular_ke: Stiffness cap used by AVBD for **non-cable** angular joint constraint scalars
(FIXED, REVOLUTE, PRISMATIC, and D6 projected angular slots).
rigid_joint_linear_kd: Rayleigh damping coefficient for non-cable linear joint constraints (paired with
``rigid_joint_linear_ke``).
rigid_joint_angular_kd: Rayleigh damping coefficient for non-cable angular joint constraints (paired with
``rigid_joint_angular_ke``).
rigid_body_contact_buffer_size: Max body-body (rigid-rigid) contacts per rigid body for per-body contact lists (tune based on expected body-body contact density).
rigid_body_particle_contact_buffer_size: Max body-particle (rigid-particle) contacts per rigid body for per-body soft-contact lists (tune based on expected body-particle contact density).
rigid_enable_dahl_friction: Enable Dahl hysteresis friction model for cable bending (default: False).
Configure per-joint Dahl parameters via the solver-registered custom model attributes
``model.vbd.dahl_eps_max`` and ``model.vbd.dahl_tau``.
Note:
- The `integrate_with_external_rigid_solver` argument enables one-way coupling between rigid body and soft body
solvers. If set to True, the rigid states should be integrated externally, with `state_in` passed to `step`
representing the previous rigid state and `state_out` representing the current one. Frictional forces are
computed accordingly.
- `particle_vertex_contact_buffer_size`, `particle_edge_contact_buffer_size`, `rigid_body_contact_buffer_size`,
and `rigid_body_particle_contact_buffer_size` are fixed and will not be dynamically resized during runtime.
Setting them too small may result in undetected collisions (particles) or contact overflow (rigid body
contacts).
Setting them excessively large may increase memory usage and degrade performance.
"""
super().__init__(model)
# Common parameters
self.iterations = iterations
self.friction_epsilon = friction_epsilon
# Rigid integration mode: when True, rigid bodies are integrated by an external
# solver (one-way coupling). SolverVBD will not move rigid bodies, but can still
# participate in particle-rigid interaction on the particle side.
self.integrate_with_external_rigid_solver = integrate_with_external_rigid_solver
# Initialize particle system
self._init_particle_system(
model,
particle_enable_self_contact,
particle_self_contact_radius,
particle_self_contact_margin,
particle_conservative_bound_relaxation,
particle_vertex_contact_buffer_size,
particle_edge_contact_buffer_size,
particle_collision_detection_interval,
particle_edge_parallel_epsilon,
particle_enable_tile_solve,
particle_topological_contact_filter_threshold,
particle_rest_shape_contact_exclusion_radius,
particle_external_vertex_contact_filtering_map,
particle_external_edge_contact_filtering_map,
)
# Initialize rigid body system and rigid-particle (body-particle) interaction state
self._init_rigid_system(
model,
rigid_avbd_beta,
rigid_avbd_gamma,
rigid_contact_k_start,
rigid_joint_linear_k_start,
rigid_joint_angular_k_start,
rigid_joint_linear_ke,
rigid_joint_angular_ke,
rigid_joint_linear_kd,
rigid_joint_angular_kd,
rigid_body_contact_buffer_size,
rigid_body_particle_contact_buffer_size,
rigid_enable_dahl_friction,
)
# Rigid-only flag to control whether to update cross-step history
# (rigid warmstart state such as contact/joint history).
# Defaults to True. This setting applies only to the next call to :meth:`step` and is then
# reset to ``True``. This is useful for substepping, where history update frequency might
# differ from the simulation step frequency (e.g. updating only on the first substep).
# This flag is automatically reset to True after each step().
# Rigid warmstart update flag (contacts/joints).
self.update_rigid_history = True
# Cached empty arrays for kernels that require wp.array arguments even when counts are zero.
self._empty_body_q = wp.empty(0, dtype=wp.transform, device=self.device)
def _init_particle_system(
self,
model: Model,
particle_enable_self_contact: bool,
particle_self_contact_radius: float,
particle_self_contact_margin: float,
particle_conservative_bound_relaxation: float,
particle_vertex_contact_buffer_size: int,
particle_edge_contact_buffer_size: int,
particle_collision_detection_interval: int,
particle_edge_parallel_epsilon: float,
particle_enable_tile_solve: bool,
particle_topological_contact_filter_threshold: int,
particle_rest_shape_contact_exclusion_radius: float,
particle_external_vertex_contact_filtering_map: dict | None,
particle_external_edge_contact_filtering_map: dict | None,
):
"""Initialize particle-specific data structures and settings."""
# Early exit if no particles
if model.particle_count == 0:
return
self.particle_collision_detection_interval = particle_collision_detection_interval
self.particle_topological_contact_filter_threshold = particle_topological_contact_filter_threshold
self.particle_rest_shape_contact_exclusion_radius = particle_rest_shape_contact_exclusion_radius
# Particle state storage
self.particle_q_prev = wp.zeros_like(
model.particle_q, device=self.device
) # per-substep previous q (for velocity)
self.inertia = wp.zeros_like(model.particle_q, device=self.device) # inertial target positions
# Particle adjacency info
self.particle_adjacency = self._compute_particle_force_element_adjacency().to(self.device)
# Self-contact settings
self.particle_enable_self_contact = particle_enable_self_contact
self.particle_self_contact_radius = particle_self_contact_radius
self.particle_self_contact_margin = particle_self_contact_margin
self.particle_q_rest = model.particle_q
# Tile solve settings
if model.device.is_cpu and particle_enable_tile_solve and wp.config.verbose:
print("Info: Tiled solve requires model.device='cuda'. Tiled solve is disabled.")
self.use_particle_tile_solve = particle_enable_tile_solve and model.device.is_cuda
if particle_enable_self_contact:
if particle_self_contact_margin < particle_self_contact_radius:
raise ValueError(
"particle_self_contact_margin is smaller than particle_self_contact_radius, this will result in missing contacts and cause instability.\n"
"It is advisable to make particle_self_contact_margin 1.5-2 times larger than particle_self_contact_radius."
)
self.particle_conservative_bound_relaxation = particle_conservative_bound_relaxation
self.particle_conservative_bounds = wp.zeros((model.particle_count,), dtype=float, device=self.device)
self.trimesh_collision_detector = TriMeshCollisionDetector(
self.model,
vertex_collision_buffer_pre_alloc=particle_vertex_contact_buffer_size,
edge_collision_buffer_pre_alloc=particle_edge_contact_buffer_size,
edge_edge_parallel_epsilon=particle_edge_parallel_epsilon,
)
self._compute_particle_contact_filtering_list(
particle_external_vertex_contact_filtering_map, particle_external_edge_contact_filtering_map
)
self.trimesh_collision_detector.set_collision_filter_list(
self.particle_vertex_triangle_contact_filtering_list,
self.particle_vertex_triangle_contact_filtering_list_offsets,
self.particle_edge_edge_contact_filtering_list,
self.particle_edge_edge_contact_filtering_list_offsets,
)
self.trimesh_collision_info = wp.array(
[self.trimesh_collision_detector.collision_info], dtype=TriMeshCollisionInfo, device=self.device
)
self.particle_self_contact_evaluation_kernel_launch_size = max(
self.model.particle_count * NUM_THREADS_PER_COLLISION_PRIMITIVE,
self.model.edge_count * NUM_THREADS_PER_COLLISION_PRIMITIVE,
)
else:
self.particle_self_contact_evaluation_kernel_launch_size = None
# Particle force and hessian storage
self.particle_forces = wp.zeros(self.model.particle_count, dtype=wp.vec3, device=self.device)
self.particle_hessians = wp.zeros(self.model.particle_count, dtype=wp.mat33, device=self.device)
# Validation
if len(self.model.particle_color_groups) == 0:
raise ValueError(
"model.particle_color_groups is empty! When using the SolverVBD you must call ModelBuilder.color() "
"or ModelBuilder.set_coloring() before calling ModelBuilder.finalize()."
)
self.pos_prev_collision_detection = wp.zeros_like(model.particle_q, device=self.device)
self.particle_displacements = wp.zeros(self.model.particle_count, dtype=wp.vec3, device=self.device)
self.truncation_ts = wp.zeros(self.model.particle_count, dtype=float, device=self.device)
def _init_rigid_system(
self,
model: Model,
rigid_avbd_beta: float,
rigid_avbd_gamma: float,
rigid_contact_k_start: float,
rigid_joint_linear_k_start: float,
rigid_joint_angular_k_start: float,
rigid_joint_linear_ke: float,
rigid_joint_angular_ke: float,
rigid_joint_linear_kd: float,
rigid_joint_angular_kd: float,
rigid_body_contact_buffer_size: int,
rigid_body_particle_contact_buffer_size: int,
rigid_enable_dahl_friction: bool,
):
"""Initialize rigid body-specific AVBD data structures and settings.
This includes:
- Rigid-only AVBD state (joints, body-body contacts, Dahl friction)
- Shared interaction state for body-particle (rigid-particle) soft contacts
"""
# AVBD penalty parameters
self.avbd_beta = rigid_avbd_beta
self.avbd_gamma = rigid_avbd_gamma
# Common initial penalty seed / lower bound for body contacts (clamped to non-negative)
self.k_start_body_contact = max(0.0, rigid_contact_k_start)
# Joint constraint caps and damping for non-cable joints (constraint enforcement, not drives)
self.rigid_joint_linear_ke = max(0.0, rigid_joint_linear_ke)
self.rigid_joint_angular_ke = max(0.0, rigid_joint_angular_ke)
self.rigid_joint_linear_kd = max(0.0, rigid_joint_linear_kd)
self.rigid_joint_angular_kd = max(0.0, rigid_joint_angular_kd)
# -------------------------------------------------------------
# Rigid-only AVBD state (used when SolverVBD integrates bodies)
# -------------------------------------------------------------
if not self.integrate_with_external_rigid_solver and model.body_count > 0:
# State storage
# Initialize to the current poses for the first step to avoid spurious finite-difference
# velocities/friction impulses.
self.body_q_prev = wp.clone(model.body_q).to(self.device)
self.body_inertia_q = wp.zeros_like(model.body_q, device=self.device) # inertial target poses for AVBD
# Adjacency and dimensions
self.rigid_adjacency = self._compute_rigid_force_element_adjacency(model).to(self.device)
# Force accumulation arrays
self.body_torques = wp.zeros(self.model.body_count, dtype=wp.vec3, device=self.device)
self.body_forces = wp.zeros(self.model.body_count, dtype=wp.vec3, device=self.device)
# Hessian blocks (6x6 block structure: angular-angular, angular-linear, linear-linear)
self.body_hessian_aa = wp.zeros(self.model.body_count, dtype=wp.mat33, device=self.device)
self.body_hessian_al = wp.zeros(self.model.body_count, dtype=wp.mat33, device=self.device)
self.body_hessian_ll = wp.zeros(self.model.body_count, dtype=wp.mat33, device=self.device)
# Per-body contact lists
# Body-body (rigid-rigid) contact adjacency (CSR-like: per-body counts and flat index array)
self.body_body_contact_buffer_pre_alloc = rigid_body_contact_buffer_size
self.body_body_contact_counts = wp.zeros(self.model.body_count, dtype=wp.int32, device=self.device)
self.body_body_contact_indices = wp.zeros(
self.model.body_count * self.body_body_contact_buffer_pre_alloc, dtype=wp.int32, device=self.device
)
# Body-particle (rigid-particle) contact adjacency (CSR-like: per-body counts and flat index array)
self.body_particle_contact_buffer_pre_alloc = rigid_body_particle_contact_buffer_size
self.body_particle_contact_counts = wp.zeros(self.model.body_count, dtype=wp.int32, device=self.device)
self.body_particle_contact_indices = wp.zeros(
self.model.body_count * self.body_particle_contact_buffer_pre_alloc,
dtype=wp.int32,
device=self.device,
)
# AVBD constraint penalties
# Joint constraint layout + penalties (solver constraint scalars)
self._init_joint_constraint_layout()
self.joint_penalty_k = self._init_joint_penalty_k(rigid_joint_linear_k_start, rigid_joint_angular_k_start)
self.joint_rest_angle = self._init_joint_rest_angle()
# Contact penalties (adaptive penalties for body-body contacts)
if model.shape_count > 0:
max_contacts = getattr(model, "rigid_contact_max", 0) or 0
if max_contacts <= 0:
# Estimate from shape contact pairs (same heuristic previously in finalize())
pair_count = model.shape_contact_pair_count if hasattr(model, "shape_contact_pair_count") else 0
max_contacts = max(10000, pair_count * 20)
# Per-contact AVBD penalty for body-body contacts
self.body_body_contact_penalty_k = wp.full(
(max_contacts,), self.k_start_body_contact, dtype=float, device=self.device
)
# Pre-computed averaged body-body contact material properties (computed once per step in warmstart)
self.body_body_contact_material_ke = wp.zeros(max_contacts, dtype=float, device=self.device)
self.body_body_contact_material_kd = wp.zeros(max_contacts, dtype=float, device=self.device)
self.body_body_contact_material_mu = wp.zeros(max_contacts, dtype=float, device=self.device)
# Dahl friction model (cable bending plasticity)
# State variables for Dahl hysteresis (persistent across timesteps)
self.joint_sigma_prev = wp.zeros(model.joint_count, dtype=wp.vec3, device=self.device)
self.joint_kappa_prev = wp.zeros(model.joint_count, dtype=wp.vec3, device=self.device)
self.joint_dkappa_prev = wp.zeros(model.joint_count, dtype=wp.vec3, device=self.device)
# Pre-computed Dahl parameters (frozen during iterations, updated per timestep)
self.joint_sigma_start = wp.zeros(model.joint_count, dtype=wp.vec3, device=self.device)
self.joint_C_fric = wp.zeros(model.joint_count, dtype=wp.vec3, device=self.device)
# Dahl model configuration
self.enable_dahl_friction = rigid_enable_dahl_friction
self.joint_dahl_eps_max = wp.zeros(model.joint_count, dtype=float, device=self.device)
self.joint_dahl_tau = wp.zeros(model.joint_count, dtype=float, device=self.device)
if rigid_enable_dahl_friction:
if model.joint_count == 0:
self.enable_dahl_friction = False
else:
# Read per-joint Dahl parameters from model.vbd if present; otherwise use defaults (eps_max=0.5, tau=1.0).
# Recommended: call SolverVBD.register_custom_attributes(builder) before finalize() to allocate these arrays.
vbd_attrs: Any = getattr(model, "vbd", None)
if vbd_attrs is not None and hasattr(vbd_attrs, "dahl_eps_max") and hasattr(vbd_attrs, "dahl_tau"):
self.joint_dahl_eps_max = vbd_attrs.dahl_eps_max
self.joint_dahl_tau = vbd_attrs.dahl_tau
else:
self._init_dahl_params(0.5, 1.0, model)
# -------------------------------------------------------------
# Body-particle interaction - shared state
# -------------------------------------------------------------
# Soft contact penalties (adaptive penalties for body-particle contacts)
# Use same initial penalty as body-body contacts
max_soft_contacts = model.shape_count * model.particle_count
# Per-contact AVBD penalty for body-particle soft contacts (same initial seed as body-body)
self.body_particle_contact_penalty_k = wp.full(
(max_soft_contacts,), self.k_start_body_contact, dtype=float, device=self.device
)
# Pre-computed averaged body-particle soft contact material properties (computed once per step in warmstart)
# These correspond to body-particle soft contacts and are averaged between model.soft_contact_*
# and shape material properties.
self.body_particle_contact_material_ke = wp.zeros(max_soft_contacts, dtype=float, device=self.device)
self.body_particle_contact_material_kd = wp.zeros(max_soft_contacts, dtype=float, device=self.device)
self.body_particle_contact_material_mu = wp.zeros(max_soft_contacts, dtype=float, device=self.device)
# Kinematic body support: create effective inv_mass / inv_inertia arrays
# with kinematic bodies zeroed out.
self._init_kinematic_state()
# Validation
has_bodies = self.model.body_count > 0
has_body_coloring = len(self.model.body_color_groups) > 0
if has_bodies and not has_body_coloring:
raise ValueError(
"model.body_color_groups is empty but rigid bodies are present! When using the SolverVBD you must call ModelBuilder.color() "
"or ModelBuilder.set_coloring() before calling ModelBuilder.finalize()."
)
@override
def notify_model_changed(self, flags: int) -> None:
if flags & (SolverNotifyFlags.BODY_PROPERTIES | SolverNotifyFlags.BODY_INERTIAL_PROPERTIES):
self._refresh_kinematic_state()
# =====================================================
# Initialization Helper Methods
# =====================================================
def _init_joint_constraint_layout(self) -> None:
"""Initialize VBD-owned joint constraint indexing.
VBD stores and adapts penalty stiffness values for *scalar constraint components*:
- ``JointType.CABLE``: 2 scalars (stretch/linear, bend/angular)
- ``JointType.BALL``: 1 scalar (isotropic linear anchor-coincidence)
- ``JointType.FIXED``: 2 scalars (isotropic linear anchor-coincidence + isotropic angular)
- ``JointType.REVOLUTE``: 3 scalars (isotropic linear + 2-DOF perpendicular angular + angular drive/limit)
- ``JointType.PRISMATIC``: 3 scalars (2-DOF perpendicular linear + isotropic angular + linear drive/limit)
- ``JointType.D6``: 2 + lin_count + ang_count scalars (projected linear + projected angular + per-DOF drive/limit)
- ``JointType.FREE``: 0 scalars (not a constraint)
Ordering (must match kernel indexing via ``joint_constraint_start``):
- ``JointType.CABLE``: [stretch (linear), bend (angular)]
- ``JointType.BALL``: [linear]
- ``JointType.FIXED``: [linear, angular]
- ``JointType.REVOLUTE``: [linear, angular, ang_drive_limit]
- ``JointType.PRISMATIC``: [linear, angular, lin_drive_limit]
- ``JointType.D6``: [linear, angular, lin_dl_0, ..., ang_dl_0, ...]
Drive and limit for each free DOF share one AVBD slot (mutually exclusive at runtime).
Any other joint type will raise ``NotImplementedError``.
"""
n_j = self.model.joint_count
with wp.ScopedDevice("cpu"):
jt_cpu = self.model.joint_type.to("cpu")
jt = jt_cpu.numpy() if hasattr(jt_cpu, "numpy") else np.asarray(jt_cpu, dtype=int)
jdof_dim_cpu = self.model.joint_dof_dim.to("cpu")
jdof_dim = jdof_dim_cpu.numpy() if hasattr(jdof_dim_cpu, "numpy") else np.asarray(jdof_dim_cpu, dtype=int)
dim_np = np.zeros((n_j,), dtype=np.int32)
for j in range(n_j):
if jt[j] == JointType.CABLE:
dim_np[j] = 2
elif jt[j] == JointType.BALL:
dim_np[j] = 1
elif jt[j] == JointType.FIXED:
dim_np[j] = 2
elif jt[j] == JointType.REVOLUTE:
dim_np[j] = 3 # [linear, angular, ang_drive_limit]
elif jt[j] == JointType.PRISMATIC:
dim_np[j] = 3 # [linear, angular, lin_drive_limit]
elif jt[j] == JointType.D6:
dim_np[j] = 2 + int(jdof_dim[j, 0]) + int(jdof_dim[j, 1]) # [linear, angular, per-DOF drive/limit]
else:
if jt[j] != JointType.FREE:
raise NotImplementedError(
f"SolverVBD rigid joints: JointType.{JointType(jt[j]).name} is not implemented yet "
"(only CABLE, BALL, FIXED, REVOLUTE, PRISMATIC, and D6 are supported)."
)
dim_np[j] = 0
start_np = np.zeros((n_j,), dtype=np.int32)
c = 0
for j in range(n_j):
start_np[j] = np.int32(c)
c += int(dim_np[j])
self.joint_constraint_count = int(c)
self.joint_constraint_dim = wp.array(dim_np, dtype=wp.int32, device=self.device)
self.joint_constraint_start = wp.array(start_np, dtype=wp.int32, device=self.device)
def _init_joint_penalty_k(self, k_start_joint_linear: float, k_start_joint_angular: float):
"""
Build initial joint penalty state on CPU and upload to solver device.
This initializes the solver-owned joint constraint parameter arrays used by VBD.
The arrays are sized by ``self.joint_constraint_count`` and indexed using
``self.joint_constraint_start`` (solver constraint indexing), not by model DOF indexing.
Arrays:
- ``k0``: initial penalty stiffness for each solver constraint scalar (stored as ``self.joint_penalty_k``)
- ``k_min``: warmstart floor for each solver constraint scalar (stored as ``self.joint_penalty_k_min``)
- ``k_max``: stiffness cap for each solver constraint scalar (stored as ``self.joint_penalty_k_max``)
- ``kd``: damping coefficient for each solver constraint scalar (stored as ``self.joint_penalty_kd``)
Supported rigid joint constraint types in SolverVBD:
- ``JointType.CABLE`` (2 scalars: stretch + bend)
- ``JointType.BALL`` (1 scalar: isotropic linear anchor-coincidence)
- ``JointType.FIXED`` (2 scalars: isotropic linear + isotropic angular)
- ``JointType.REVOLUTE`` (3 scalars: isotropic linear + 2-DOF perpendicular angular + angular drive/limit)
- ``JointType.PRISMATIC`` (3 scalars: 2-DOF perpendicular linear + isotropic angular + linear drive/limit)
- ``JointType.D6`` (2 + lin_count + ang_count scalars: projected linear + projected angular + per-DOF drive/limit)
Drive/limit slots use AVBD with per-mode clamping in the primal (``wp.min(avbd_ke, model_ke)``).
Drive and limit share one slot per free DOF (mutually exclusive at runtime).
``JointType.FREE`` joints (created by :meth:`ModelBuilder.add_body`) are not constraints and are ignored.
"""
if (
not hasattr(self, "joint_constraint_start")
or not hasattr(self, "joint_constraint_dim")
or not hasattr(self, "joint_constraint_count")
):
raise RuntimeError(
"SolverVBD joint constraint layout is not initialized. "
"Call SolverVBD._init_joint_constraint_layout() before _init_joint_penalty_k()."
)
if self.joint_constraint_count < 0:
raise RuntimeError(
f"SolverVBD joint constraint layout is invalid: joint_constraint_count={self.joint_constraint_count!r}"
)
constraint_count = self.joint_constraint_count
with wp.ScopedDevice("cpu"):
# Per-constraint AVBD penalty state:
# - k0: initial penalty stiffness for this scalar constraint
# - k_min: warmstart floor (so k doesn't decay below this across steps)
# - k_max: stiffness cap (so k never exceeds the chosen target for this constraint)
#
# We start from solver-level seeds (k_start_*), but clamp to the per-constraint cap (k_max) so we always
# satisfy k_min <= k0 <= k_max.
stretch_k = max(0.0, k_start_joint_linear)
bend_k = max(0.0, k_start_joint_angular)
joint_k_min_np = np.zeros((constraint_count,), dtype=float)
joint_k0_np = np.zeros((constraint_count,), dtype=float)
# Per-constraint stiffness caps used for AVBD warmstart clamping and penalty growth limiting.
# - Cable constraints: use model.joint_target_ke (cable material/constraint tuning; still model-DOF indexed)
# - Rigid constraints (BALL/FIXED/REVOLUTE/PRISMATIC/D6): use solver-level caps (rigid_joint_linear_ke/angular_ke)
# Start from zeros and explicitly fill per joint/constraint-slot below for clarity.
joint_k_max_np = np.zeros((constraint_count,), dtype=float)
joint_kd_np = np.zeros((constraint_count,), dtype=float)
jt_cpu = self.model.joint_type.to("cpu")
jdofs_cpu = self.model.joint_qd_start.to("cpu")
jtarget_ke_cpu = self.model.joint_target_ke.to("cpu")
jtarget_kd_cpu = self.model.joint_target_kd.to("cpu")
jlimit_ke_cpu = self.model.joint_limit_ke.to("cpu")
jdof_dim_cpu = self.model.joint_dof_dim.to("cpu")
jc_start_cpu = self.joint_constraint_start.to("cpu")
jt = jt_cpu.numpy() if hasattr(jt_cpu, "numpy") else np.asarray(jt_cpu, dtype=int)
jdofs = jdofs_cpu.numpy() if hasattr(jdofs_cpu, "numpy") else np.asarray(jdofs_cpu, dtype=int)
jc_start = (
jc_start_cpu.numpy() if hasattr(jc_start_cpu, "numpy") else np.asarray(jc_start_cpu, dtype=np.int32)
)
jtarget_ke = (
jtarget_ke_cpu.numpy() if hasattr(jtarget_ke_cpu, "numpy") else np.asarray(jtarget_ke_cpu, dtype=float)
)
jtarget_kd = (
jtarget_kd_cpu.numpy() if hasattr(jtarget_kd_cpu, "numpy") else np.asarray(jtarget_kd_cpu, dtype=float)
)
jlimit_ke = (
jlimit_ke_cpu.numpy() if hasattr(jlimit_ke_cpu, "numpy") else np.asarray(jlimit_ke_cpu, dtype=float)
)
jdof_dim = jdof_dim_cpu.numpy() if hasattr(jdof_dim_cpu, "numpy") else np.asarray(jdof_dim_cpu, dtype=int)
n_j = self.model.joint_count
for j in range(n_j):
if jt[j] == JointType.CABLE:
c0 = int(jc_start[j])
dof0 = int(jdofs[j])
# CABLE requires 2 DOF entries in model.joint_target_ke/kd starting at joint_qd_start[j].
if dof0 < 0 or (dof0 + 1) >= len(jtarget_ke) or (dof0 + 1) >= len(jtarget_kd):
raise RuntimeError(
"SolverVBD _init_joint_penalty_k: JointType.CABLE requires 2 DOF entries in "
"model.joint_target_ke/kd starting at joint_qd_start[j]. "
f"Got joint_index={j}, joint_qd_start={dof0}, "
f"len(joint_target_ke)={len(jtarget_ke)}, len(joint_target_kd)={len(jtarget_kd)}."
)
# Constraint 0: cable stretch; constraint 1: cable bend
# Caps come from model.joint_target_ke (still model DOF indexed for cable material tuning).
joint_k_max_np[c0] = jtarget_ke[dof0]
joint_k_max_np[c0 + 1] = jtarget_ke[dof0 + 1]
# Per-slot warmstart lower bounds:
# - Use k_start_* as the floor, but clamp to the cap so k_min <= k_max always.
joint_k_min_np[c0] = min(stretch_k, joint_k_max_np[c0])
joint_k_min_np[c0 + 1] = min(bend_k, joint_k_max_np[c0 + 1])
# Initial seed: clamp to cap so k0 <= k_max
joint_k0_np[c0] = min(stretch_k, joint_k_max_np[c0])
joint_k0_np[c0 + 1] = min(bend_k, joint_k_max_np[c0 + 1])
# Damping comes from model.joint_target_kd (still model DOF indexed for cable tuning).
joint_kd_np[c0] = jtarget_kd[dof0]
joint_kd_np[c0 + 1] = jtarget_kd[dof0 + 1]
elif jt[j] == JointType.BALL:
# BALL joints: isotropic linear anchor-coincidence constraint stored as a single scalar.
c0 = int(jc_start[j])
joint_k_max_np[c0] = self.rigid_joint_linear_ke
k_floor = min(stretch_k, self.rigid_joint_linear_ke)
joint_k_min_np[c0] = k_floor
joint_k0_np[c0] = k_floor
joint_kd_np[c0] = self.rigid_joint_linear_kd
elif jt[j] == JointType.FIXED:
# FIXED joints are enforced as:
# - 1 isotropic linear anchor-coincidence constraint (vector error, scalar penalty)
# - 1 isotropic angular constraint (rotation-vector error, scalar penalty)
c0 = int(jc_start[j])
# Linear cap + floor (isotropic)
joint_k_max_np[c0 + 0] = self.rigid_joint_linear_ke
k_lin_floor = min(stretch_k, self.rigid_joint_linear_ke)
joint_k_min_np[c0 + 0] = k_lin_floor
joint_k0_np[c0 + 0] = k_lin_floor
joint_kd_np[c0 + 0] = self.rigid_joint_linear_kd
# Angular cap + floor (isotropic)
joint_k_max_np[c0 + 1] = self.rigid_joint_angular_ke
k_ang_floor = min(bend_k, self.rigid_joint_angular_ke)
joint_k_min_np[c0 + 1] = k_ang_floor
joint_k0_np[c0 + 1] = k_ang_floor
joint_kd_np[c0 + 1] = self.rigid_joint_angular_kd
elif jt[j] == JointType.REVOLUTE:
# REVOLUTE joints: isotropic linear + 2-DOF perpendicular angular + angular drive/limit
c0 = int(jc_start[j])
joint_k_max_np[c0 + 0] = self.rigid_joint_linear_ke
k_lin_floor = min(stretch_k, self.rigid_joint_linear_ke)
joint_k_min_np[c0 + 0] = k_lin_floor
joint_k0_np[c0 + 0] = k_lin_floor
joint_kd_np[c0 + 0] = self.rigid_joint_linear_kd
joint_k_max_np[c0 + 1] = self.rigid_joint_angular_ke
k_ang_floor = min(bend_k, self.rigid_joint_angular_ke)
joint_k_min_np[c0 + 1] = k_ang_floor
joint_k0_np[c0 + 1] = k_ang_floor
joint_kd_np[c0 + 1] = self.rigid_joint_angular_kd
# Drive/limit slot for free angular DOF (slot c0 + 2).
# Drive and limit share one AVBD slot (mutually exclusive at runtime).
# Per-mode clamping in the primal prevents branch-switch overshoot.
dof0 = int(jdofs[j])
dl_k_max = max(float(jtarget_ke[dof0]), float(jlimit_ke[dof0]))
dl_seed = min(bend_k, dl_k_max) # angular DOF -> bend_k seed
joint_k_max_np[c0 + 2] = dl_k_max
joint_k_min_np[c0 + 2] = dl_seed
joint_k0_np[c0 + 2] = dl_seed
joint_kd_np[c0 + 2] = 0.0 # damping is non-adaptive, read from model in primal
elif jt[j] == JointType.PRISMATIC:
# PRISMATIC joints: 2-DOF perpendicular linear + isotropic angular + linear drive/limit
c0 = int(jc_start[j])
joint_k_max_np[c0 + 0] = self.rigid_joint_linear_ke
k_lin_floor = min(stretch_k, self.rigid_joint_linear_ke)
joint_k_min_np[c0 + 0] = k_lin_floor
joint_k0_np[c0 + 0] = k_lin_floor
joint_kd_np[c0 + 0] = self.rigid_joint_linear_kd
joint_k_max_np[c0 + 1] = self.rigid_joint_angular_ke
k_ang_floor = min(bend_k, self.rigid_joint_angular_ke)
joint_k_min_np[c0 + 1] = k_ang_floor
joint_k0_np[c0 + 1] = k_ang_floor
joint_kd_np[c0 + 1] = self.rigid_joint_angular_kd
# Drive/limit slot for free linear DOF (slot c0 + 2).
dof0 = int(jdofs[j])
dl_k_max = max(float(jtarget_ke[dof0]), float(jlimit_ke[dof0]))
dl_seed = min(stretch_k, dl_k_max) # linear DOF -> stretch_k seed
joint_k_max_np[c0 + 2] = dl_k_max
joint_k_min_np[c0 + 2] = dl_seed
joint_k0_np[c0 + 2] = dl_seed
joint_kd_np[c0 + 2] = 0.0
elif jt[j] == JointType.D6:
# D6 joints: projected linear + projected angular + per-DOF drive/limit
c0 = int(jc_start[j])
dof0 = int(jdofs[j])
lc = int(jdof_dim[j, 0]) # free linear DOF count
ac = int(jdof_dim[j, 1]) # free angular DOF count
joint_k_max_np[c0 + 0] = self.rigid_joint_linear_ke
k_lin_floor = min(stretch_k, self.rigid_joint_linear_ke)
joint_k_min_np[c0 + 0] = k_lin_floor
joint_k0_np[c0 + 0] = k_lin_floor
joint_kd_np[c0 + 0] = self.rigid_joint_linear_kd
joint_k_max_np[c0 + 1] = self.rigid_joint_angular_ke
k_ang_floor = min(bend_k, self.rigid_joint_angular_ke)
joint_k_min_np[c0 + 1] = k_ang_floor
joint_k0_np[c0 + 1] = k_ang_floor
joint_kd_np[c0 + 1] = self.rigid_joint_angular_kd
# Per free linear DOF drive/limit slots
for li in range(lc):
dof_idx = dof0 + li
slot = c0 + 2 + li
dl_k_max = max(float(jtarget_ke[dof_idx]), float(jlimit_ke[dof_idx]))
dl_seed = min(stretch_k, dl_k_max)
joint_k_max_np[slot] = dl_k_max
joint_k_min_np[slot] = dl_seed
joint_k0_np[slot] = dl_seed
joint_kd_np[slot] = 0.0
# Per free angular DOF drive/limit slots
for ai in range(ac):
dof_idx = dof0 + lc + ai
slot = c0 + 2 + lc + ai
dl_k_max = max(float(jtarget_ke[dof_idx]), float(jlimit_ke[dof_idx]))
dl_seed = min(bend_k, dl_k_max)
joint_k_max_np[slot] = dl_k_max
joint_k_min_np[slot] = dl_seed
joint_k0_np[slot] = dl_seed
joint_kd_np[slot] = 0.0
else:
# Layout builder already validated supported types; nothing to do for FREE.
pass
# Upload to device: initial penalties, per-constraint caps, damping, and warmstart floors.
self.joint_penalty_k_min = wp.array(joint_k_min_np, dtype=float, device=self.device)
self.joint_penalty_k_max = wp.array(joint_k_max_np, dtype=float, device=self.device)
self.joint_penalty_kd = wp.array(joint_kd_np, dtype=float, device=self.device)
return wp.array(joint_k0_np, dtype=float, device=self.device)
def _init_joint_rest_angle(self):
"""Compute per-DOF rest-pose joint angles from ``model.joint_q``.
VBD computes angular joint angles via ``kappa`` (rotation vector relative to
the rest pose stored in ``model.body_q``). After ``eval_fk(model, ..., model)``,
the rest pose encodes the initial joint configuration, so ``kappa = 0`` at the
initial angles. Drive targets and limits, however, are specified in absolute
joint coordinates. This array stores the rest-pose angle offset per DOF so that
``theta_abs = theta + joint_rest_angle[dof_idx]`` converts rest-relative
``theta`` back to absolute coordinates for drive/limit comparison.
Only angular DOFs of REVOLUTE and D6 joints need nonzero entries. Linear DOFs
(PRISMATIC, D6 linear) use absolute geometric measurements (``d_along``) and
are unaffected — their entries are left at 0.
"""
dof_count = self.model.joint_dof_count
rest_angle_np = np.zeros(dof_count, dtype=float)
with wp.ScopedDevice("cpu"):
jt_cpu = self.model.joint_type.to("cpu")
jq_cpu = self.model.joint_q.to("cpu")
jq_start_cpu = self.model.joint_q_start.to("cpu")
jqd_start_cpu = self.model.joint_qd_start.to("cpu")
jdof_dim_cpu = self.model.joint_dof_dim.to("cpu")
jt = jt_cpu.numpy() if hasattr(jt_cpu, "numpy") else np.asarray(jt_cpu, dtype=int)
jq = jq_cpu.numpy() if hasattr(jq_cpu, "numpy") else np.asarray(jq_cpu, dtype=float)
jq_start = jq_start_cpu.numpy() if hasattr(jq_start_cpu, "numpy") else np.asarray(jq_start_cpu, dtype=int)
jqd_start = (
jqd_start_cpu.numpy() if hasattr(jqd_start_cpu, "numpy") else np.asarray(jqd_start_cpu, dtype=int)
)
jdof_dim = jdof_dim_cpu.numpy() if hasattr(jdof_dim_cpu, "numpy") else np.asarray(jdof_dim_cpu, dtype=int)
for j in range(self.model.joint_count):
if jt[j] == JointType.REVOLUTE:
q_start = int(jq_start[j])
qd_start = int(jqd_start[j])
rest_angle_np[qd_start] = float(jq[q_start])
elif jt[j] == JointType.D6:
q_start = int(jq_start[j])
qd_start = int(jqd_start[j])
lin_count = int(jdof_dim[j, 0])
ang_count = int(jdof_dim[j, 1])
for ai in range(ang_count):
rest_angle_np[qd_start + lin_count + ai] = float(jq[q_start + lin_count + ai])
return wp.array(rest_angle_np, dtype=float, device=self.device)
def _init_dahl_params(self, eps_max_input, tau_input, model):
"""
Initialize per-joint Dahl friction parameters.
Args:
eps_max_input: float or array-like. Maximum strain (curvature) [rad].
- Scalar: broadcast to all joints
- Array-like (length = model.joint_count): per-joint values
- Per-joint disable: set value to 0 for that joint
tau_input: float or array-like. Memory decay length [rad].
- Scalar: broadcast to all joints
- Array-like (length = model.joint_count): per-joint values
- Per-joint disable: set value to 0 for that joint
model: Model object
Notes:
- This function validates shapes and converts to device arrays; it does not clamp or validate ranges.
Kernels perform any necessary early-outs based on zero values.
- To disable Dahl friction:
- Globally: pass enable_dahl_friction=False to the constructor
- Per-joint: set dahl_eps_max=0 or dahl_tau=0 for those joints
"""
n = model.joint_count
# eps_max
if isinstance(eps_max_input, (int, float)):
self.joint_dahl_eps_max = wp.full(n, eps_max_input, dtype=float, device=self.device)
else:
# Convert to numpy first
x = eps_max_input.to("cpu") if hasattr(eps_max_input, "to") else eps_max_input
eps_np = x.numpy() if hasattr(x, "numpy") else np.asarray(x, dtype=float)
if eps_np.shape[0] != n:
raise ValueError(f"dahl_eps_max length {eps_np.shape[0]} != joint_count {n}")
# Direct host-to-device copy
self.joint_dahl_eps_max = wp.array(eps_np, dtype=float, device=self.device)
# tau
if isinstance(tau_input, (int, float)):
self.joint_dahl_tau = wp.full(n, tau_input, dtype=float, device=self.device)
else:
# Convert to numpy first
x = tau_input.to("cpu") if hasattr(tau_input, "to") else tau_input
tau_np = x.numpy() if hasattr(x, "numpy") else np.asarray(x, dtype=float)
if tau_np.shape[0] != n:
raise ValueError(f"dahl_tau length {tau_np.shape[0]} != joint_count {n}")
# Direct host-to-device copy
self.joint_dahl_tau = wp.array(tau_np, dtype=float, device=self.device)
@override
@classmethod
def register_custom_attributes(cls, builder: ModelBuilder) -> None:
"""Register solver-specific custom Model attributes for SolverVBD.
Currently used for cable bending plasticity/hysteresis (Dahl friction model).
Attributes are declared in the ``vbd`` namespace so they can be authored in scenes
and in USD as ``newton:vbd:``.
"""
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="dahl_eps_max",
frequency=Model.AttributeFrequency.JOINT,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=0.5,
namespace="vbd",
)
)
builder.add_custom_attribute(
ModelBuilder.CustomAttribute(
name="dahl_tau",
frequency=Model.AttributeFrequency.JOINT,
assignment=Model.AttributeAssignment.MODEL,
dtype=wp.float32,
default=1.0,
namespace="vbd",
)
)
# =====================================================
# Adjacency Building Methods
# =====================================================
def _compute_particle_force_element_adjacency(self):
particle_adjacency = ParticleForceElementAdjacencyInfo()
with wp.ScopedDevice("cpu"):
if self.model.edge_indices:
edges_array = self.model.edge_indices.to("cpu")
# build vertex-edge particle_adjacency data
num_vertex_adjacent_edges = wp.zeros(shape=(self.model.particle_count,), dtype=wp.int32)
wp.launch(
kernel=_count_num_adjacent_edges,
inputs=[edges_array, num_vertex_adjacent_edges],
dim=1,
device="cpu",
)
num_vertex_adjacent_edges = num_vertex_adjacent_edges.numpy()
vertex_adjacent_edges_offsets = np.empty(shape=(self.model.particle_count + 1,), dtype=wp.int32)
vertex_adjacent_edges_offsets[1:] = np.cumsum(2 * num_vertex_adjacent_edges)[:]
vertex_adjacent_edges_offsets[0] = 0
particle_adjacency.v_adj_edges_offsets = wp.array(vertex_adjacent_edges_offsets, dtype=wp.int32)
# temporal variables to record how much adjacent edges has been filled to each vertex
vertex_adjacent_edges_fill_count = wp.zeros(shape=(self.model.particle_count,), dtype=wp.int32)
edge_particle_adjacency_array_size = 2 * num_vertex_adjacent_edges.sum()
# vertex order: o0: 0, o1: 1, v0: 2, v1: 3,
particle_adjacency.v_adj_edges = wp.empty(shape=(edge_particle_adjacency_array_size,), dtype=wp.int32)
wp.launch(
kernel=_fill_adjacent_edges,
inputs=[
edges_array,
particle_adjacency.v_adj_edges_offsets,
vertex_adjacent_edges_fill_count,
particle_adjacency.v_adj_edges,
],
dim=1,
device="cpu",
)
else:
particle_adjacency.v_adj_edges_offsets = wp.empty(shape=(0,), dtype=wp.int32)
particle_adjacency.v_adj_edges = wp.empty(shape=(0,), dtype=wp.int32)
if self.model.tri_indices:
face_indices = self.model.tri_indices.to("cpu")
# compute adjacent triangles
# count number of adjacent faces for each vertex
num_vertex_adjacent_faces = wp.zeros(shape=(self.model.particle_count,), dtype=wp.int32, device="cpu")
wp.launch(
kernel=_count_num_adjacent_faces,
inputs=[face_indices, num_vertex_adjacent_faces],
dim=1,
device="cpu",
)
# preallocate memory based on counting results
num_vertex_adjacent_faces = num_vertex_adjacent_faces.numpy()
vertex_adjacent_faces_offsets = np.empty(shape=(self.model.particle_count + 1,), dtype=wp.int32)
vertex_adjacent_faces_offsets[1:] = np.cumsum(2 * num_vertex_adjacent_faces)[:]
vertex_adjacent_faces_offsets[0] = 0
particle_adjacency.v_adj_faces_offsets = wp.array(vertex_adjacent_faces_offsets, dtype=wp.int32)
vertex_adjacent_faces_fill_count = wp.zeros(shape=(self.model.particle_count,), dtype=wp.int32)
face_particle_adjacency_array_size = 2 * num_vertex_adjacent_faces.sum()
# (face, vertex_order) * num_adj_faces * num_particles
# vertex order: v0: 0, v1: 1, o0: 2, v2: 3
particle_adjacency.v_adj_faces = wp.empty(shape=(face_particle_adjacency_array_size,), dtype=wp.int32)
wp.launch(
kernel=_fill_adjacent_faces,
inputs=[
face_indices,
particle_adjacency.v_adj_faces_offsets,
vertex_adjacent_faces_fill_count,
particle_adjacency.v_adj_faces,
],
dim=1,
device="cpu",
)
else:
particle_adjacency.v_adj_faces_offsets = wp.empty(shape=(0,), dtype=wp.int32)
particle_adjacency.v_adj_faces = wp.empty(shape=(0,), dtype=wp.int32)
if self.model.tet_indices:
tet_indices = self.model.tet_indices.to("cpu")
num_vertex_adjacent_tets = wp.zeros(shape=(self.model.particle_count,), dtype=wp.int32)
wp.launch(
kernel=_count_num_adjacent_tets,
inputs=[tet_indices, num_vertex_adjacent_tets],
dim=1,
device="cpu",
)
num_vertex_adjacent_tets = num_vertex_adjacent_tets.numpy()
vertex_adjacent_tets_offsets = np.empty(shape=(self.model.particle_count + 1,), dtype=wp.int32)
vertex_adjacent_tets_offsets[1:] = np.cumsum(2 * num_vertex_adjacent_tets)[:]
vertex_adjacent_tets_offsets[0] = 0
particle_adjacency.v_adj_tets_offsets = wp.array(vertex_adjacent_tets_offsets, dtype=wp.int32)
vertex_adjacent_tets_fill_count = wp.zeros(shape=(self.model.particle_count,), dtype=wp.int32)
tet_particle_adjacency_array_size = 2 * num_vertex_adjacent_tets.sum()
particle_adjacency.v_adj_tets = wp.empty(shape=(tet_particle_adjacency_array_size,), dtype=wp.int32)
wp.launch(
kernel=_fill_adjacent_tets,
inputs=[
tet_indices,
particle_adjacency.v_adj_tets_offsets,
vertex_adjacent_tets_fill_count,
particle_adjacency.v_adj_tets,
],
dim=1,
device="cpu",
)
else:
particle_adjacency.v_adj_tets_offsets = wp.empty(shape=(0,), dtype=wp.int32)
particle_adjacency.v_adj_tets = wp.empty(shape=(0,), dtype=wp.int32)
if self.model.spring_indices:
spring_array = self.model.spring_indices.to("cpu")
# build vertex-springs particle_adjacency data
num_vertex_adjacent_spring = wp.zeros(shape=(self.model.particle_count,), dtype=wp.int32)
wp.launch(
kernel=_count_num_adjacent_springs,
inputs=[spring_array, num_vertex_adjacent_spring],
dim=1,
device="cpu",
)
num_vertex_adjacent_spring = num_vertex_adjacent_spring.numpy()
vertex_adjacent_springs_offsets = np.empty(shape=(self.model.particle_count + 1,), dtype=wp.int32)
vertex_adjacent_springs_offsets[1:] = np.cumsum(num_vertex_adjacent_spring)[:]
vertex_adjacent_springs_offsets[0] = 0
particle_adjacency.v_adj_springs_offsets = wp.array(vertex_adjacent_springs_offsets, dtype=wp.int32)
# temporal variables to record how much adjacent springs has been filled to each vertex
vertex_adjacent_springs_fill_count = wp.zeros(shape=(self.model.particle_count,), dtype=wp.int32)
particle_adjacency.v_adj_springs = wp.empty(shape=(num_vertex_adjacent_spring.sum(),), dtype=wp.int32)
wp.launch(
kernel=_fill_adjacent_springs,
inputs=[
spring_array,
particle_adjacency.v_adj_springs_offsets,
vertex_adjacent_springs_fill_count,
particle_adjacency.v_adj_springs,
],
dim=1,
device="cpu",
)
else:
particle_adjacency.v_adj_springs_offsets = wp.empty(shape=(0,), dtype=wp.int32)
particle_adjacency.v_adj_springs = wp.empty(shape=(0,), dtype=wp.int32)
return particle_adjacency
def _compute_particle_contact_filtering_list(
self, external_vertex_contact_filtering_map, external_edge_contact_filtering_map
):
if self.model.tri_count:
v_tri_filter_sets = None
edge_edge_filter_sets = None
if self.particle_topological_contact_filter_threshold >= 2:
if self.particle_adjacency.v_adj_faces_offsets.size > 0:
v_tri_filter_sets = build_vertex_n_ring_tris_collision_filter(
self.particle_topological_contact_filter_threshold,
self.model.particle_count,
self.model.edge_indices.numpy(),
self.particle_adjacency.v_adj_edges.numpy(),
self.particle_adjacency.v_adj_edges_offsets.numpy(),
self.particle_adjacency.v_adj_faces.numpy(),
self.particle_adjacency.v_adj_faces_offsets.numpy(),
)
if self.particle_adjacency.v_adj_edges_offsets.size > 0:
edge_edge_filter_sets = build_edge_n_ring_edge_collision_filter(
self.particle_topological_contact_filter_threshold,
self.model.edge_indices.numpy(),
self.particle_adjacency.v_adj_edges.numpy(),
self.particle_adjacency.v_adj_edges_offsets.numpy(),
)
if external_vertex_contact_filtering_map is not None:
if v_tri_filter_sets is None:
v_tri_filter_sets = [set() for _ in range(self.model.particle_count)]
for vertex_id, filter_set in external_vertex_contact_filtering_map.items():
v_tri_filter_sets[vertex_id].update(filter_set)
if external_edge_contact_filtering_map is not None:
if edge_edge_filter_sets is None:
edge_edge_filter_sets = [set() for _ in range(self.model.edge_indices.shape[0])]
for edge_id, filter_set in external_edge_contact_filtering_map.items():
edge_edge_filter_sets[edge_id].update(filter_set)
if v_tri_filter_sets is None:
self.particle_vertex_triangle_contact_filtering_list = None
self.particle_vertex_triangle_contact_filtering_list_offsets = None
else:
(
self.particle_vertex_triangle_contact_filtering_list,
self.particle_vertex_triangle_contact_filtering_list_offsets,
) = set_to_csr(v_tri_filter_sets)
self.particle_vertex_triangle_contact_filtering_list = wp.array(
self.particle_vertex_triangle_contact_filtering_list, dtype=int, device=self.device
)
self.particle_vertex_triangle_contact_filtering_list_offsets = wp.array(
self.particle_vertex_triangle_contact_filtering_list_offsets, dtype=int, device=self.device
)
if edge_edge_filter_sets is None:
self.particle_edge_edge_contact_filtering_list = None
self.particle_edge_edge_contact_filtering_list_offsets = None
else:
(
self.particle_edge_edge_contact_filtering_list,
self.particle_edge_edge_contact_filtering_list_offsets,
) = set_to_csr(edge_edge_filter_sets)
self.particle_edge_edge_contact_filtering_list = wp.array(
self.particle_edge_edge_contact_filtering_list, dtype=int, device=self.device
)
self.particle_edge_edge_contact_filtering_list_offsets = wp.array(
self.particle_edge_edge_contact_filtering_list_offsets, dtype=int, device=self.device
)
def _compute_rigid_force_element_adjacency(self, model):
"""
Build CSR adjacency between rigid bodies and joints.
Returns an instance of RigidForceElementAdjacencyInfo with:
- body_adj_joints: flattened joint ids
- body_adj_joints_offsets: CSR offsets of size body_count + 1
Notes:
- Runs on CPU to avoid GPU atomics; kernels iterate serially over joints (dim=1).
- When there are no joints, offsets are an all-zero array of length body_count + 1.
"""
adjacency = RigidForceElementAdjacencyInfo()
with wp.ScopedDevice("cpu"):
# Build body-joint adjacency data (rigid-only)
if model.joint_count > 0:
joint_parent_cpu = model.joint_parent.to("cpu")
joint_child_cpu = model.joint_child.to("cpu")
num_body_adjacent_joints = wp.zeros(shape=(model.body_count,), dtype=wp.int32)
wp.launch(
kernel=_count_num_adjacent_joints,
inputs=[joint_parent_cpu, joint_child_cpu, num_body_adjacent_joints],
dim=1,
device="cpu",
)
num_body_adjacent_joints = num_body_adjacent_joints.numpy()
body_adjacent_joints_offsets = np.empty(shape=(model.body_count + 1,), dtype=wp.int32)
body_adjacent_joints_offsets[1:] = np.cumsum(num_body_adjacent_joints)[:]
body_adjacent_joints_offsets[0] = 0
adjacency.body_adj_joints_offsets = wp.array(body_adjacent_joints_offsets, dtype=wp.int32)
body_adjacent_joints_fill_count = wp.zeros(shape=(model.body_count,), dtype=wp.int32)
adjacency.body_adj_joints = wp.empty(shape=(num_body_adjacent_joints.sum(),), dtype=wp.int32)
wp.launch(
kernel=_fill_adjacent_joints,
inputs=[
joint_parent_cpu,
joint_child_cpu,
adjacency.body_adj_joints_offsets,
body_adjacent_joints_fill_count,
adjacency.body_adj_joints,
],
dim=1,
device="cpu",
)
else:
# No joints: create offset array of zeros (size body_count + 1) so indexing works
adjacency.body_adj_joints_offsets = wp.zeros(shape=(model.body_count + 1,), dtype=wp.int32)
adjacency.body_adj_joints = wp.empty(shape=(0,), dtype=wp.int32)
return adjacency
# =====================================================
# Main Solver Methods
# =====================================================
def set_rigid_history_update(self, update: bool):
"""Set whether the next step() should update rigid solver history (warmstarts).
This setting applies only to the next call to :meth:`step` and is then reset to ``True``.
This is useful for substepping, where history update frequency might differ from the
simulation step frequency (e.g. updating only on the first substep).
Args:
update: If True, update rigid warmstart state. If False, reuse previous.
"""
self.update_rigid_history = update
@override
def step(
self,
state_in: State,
state_out: State,
control: Control,
contacts: Contacts | None,
dt: float,
) -> None:
"""Execute one simulation timestep using VBD (particles) and AVBD (rigid bodies).
The solver follows a 3-phase structure:
1. Initialize: Forward integrate particles and rigid bodies, detect collisions, warmstart penalties
2. Iterate: Interleave particle VBD iterations and rigid body AVBD iterations
3. Finalize: Update velocities and persistent state (Dahl friction)
To control rigid substepping behavior (warmstart history), call
:meth:`set_rigid_history_update`
before calling this method. It defaults to ``True`` and is reset to ``True`` after each call.
Args:
state_in: Input state.
state_out: Output state.
control: Control inputs.
contacts: Contact data produced by :meth:`~newton.Model.collide` (rigid-rigid and rigid-particle contacts).
If None, rigid contact handling is skipped. Note that particle self-contact (if enabled) does not
depend on this argument.
dt: Time step size.
"""
# Use and reset the rigid history update flag (warmstarts).
update_rigid_history = self.update_rigid_history
self.update_rigid_history = True
if control is None:
control = self.model.control(clone_variables=False)
self._initialize_rigid_bodies(state_in, control, contacts, dt, update_rigid_history)
self._initialize_particles(state_in, state_out, dt)
for iter_num in range(self.iterations):
self._solve_rigid_body_iteration(state_in, state_out, control, contacts, dt)
self._solve_particle_iteration(state_in, state_out, contacts, dt, iter_num)
self._finalize_rigid_bodies(state_out, dt)
self._finalize_particles(state_out, dt)
def _penetration_free_truncation(self, particle_q_out=None):
"""
Modify displacements_in in-place, also modify particle_q if its not None
"""
if not self.particle_enable_self_contact:
self.truncation_ts.fill_(1.0)
wp.launch(
kernel=apply_truncation_ts,
dim=self.model.particle_count,
inputs=[
self.pos_prev_collision_detection, # pos: wp.array[wp.vec3],
self.particle_displacements, # displacement_in: wp.array[wp.vec3],
self.truncation_ts, # truncation_ts: wp.array[float],
wp.inf, # max_displacement: float (input threshold)
],
outputs=[
self.particle_displacements, # displacement_out: wp.array[wp.vec3],
particle_q_out, # pos_out: wp.array[wp.vec3],
],
device=self.device,
)
else:
## parallel by collision and atomic operation
self.truncation_ts.fill_(1.0)
wp.launch(
kernel=apply_planar_truncation_parallel_by_collision,
inputs=[
self.pos_prev_collision_detection, # pos_prev_collision_detection: wp.array[wp.vec3],
self.particle_displacements, # particle_displacements: wp.array[wp.vec3],
self.model.tri_indices,
self.model.edge_indices,
self.trimesh_collision_info,
self.trimesh_collision_detector.edge_edge_parallel_epsilon,
self.particle_conservative_bound_relaxation,
],
outputs=[
self.truncation_ts,
],
dim=self.particle_self_contact_evaluation_kernel_launch_size,
device=self.device,
)
wp.launch(
kernel=apply_truncation_ts,
dim=self.model.particle_count,
inputs=[
self.pos_prev_collision_detection,
self.particle_displacements,
self.truncation_ts,
self.particle_self_contact_margin
* self.particle_conservative_bound_relaxation
* 0.5, # max_displacement: degenerate to isotropic truncation
],
outputs=[
self.particle_displacements,
particle_q_out,
],
device=self.device,
)
def _initialize_particles(self, state_in: State, state_out: State, dt: float):
"""Initialize particle positions for the VBD iteration."""
model = self.model
# Early exit if no particles
if model.particle_count == 0:
return
# Collision detection before initialization to compute conservative bounds
if self.particle_enable_self_contact:
self._collision_detection_penetration_free(state_in)
else:
self.pos_prev_collision_detection.assign(state_in.particle_q)
self.particle_displacements.zero_()
model = self.model
wp.launch(
kernel=forward_step,
inputs=[
dt,
model.gravity,
self.particle_q_prev,
state_in.particle_q,
state_in.particle_qd,
self.model.particle_inv_mass,
state_in.particle_f,
self.model.particle_flags,
],
outputs=[
self.inertia,
self.particle_displacements,
],
dim=self.model.particle_count,
device=self.device,
)
self._penetration_free_truncation(state_in.particle_q)
def _initialize_rigid_bodies(
self,
state_in: State,
control: Control,
contacts: Contacts | None,
dt: float,
update_rigid_history: bool,
):
"""Initialize rigid body states for AVBD solver (pre-iteration phase).
Performs forward integration and initializes contact-related AVBD state when contacts are provided.
If ``contacts`` is None, rigid contact-related work is skipped:
no per-body contact adjacency is built, and no contact penalties are warmstarted.
If ``control`` provides ``joint_f``, per-DOF joint forces are mapped to body spatial
wrenches and included in the forward integration (shifting the inertial target).
"""
model = self.model
# ---------------------------
# Rigid-only initialization
# ---------------------------
if model.body_count > 0 and not self.integrate_with_external_rigid_solver:
# Accumulate per-DOF joint forces (joint_f) into body spatial wrenches.
# Clone body_f to avoid mutating user state; the clone is used only for integration.
body_f_for_integration = state_in.body_f
if model.joint_count > 0 and control is not None and control.joint_f is not None:
body_f_for_integration = wp.clone(state_in.body_f)
wp.launch(
kernel=apply_joint_forces,
dim=model.joint_count,
inputs=[
state_in.body_q,
model.body_com,
model.joint_type,
model.joint_enabled,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_qd_start,
model.joint_dof_dim,
model.joint_axis,
control.joint_f,
],
outputs=[
body_f_for_integration,
],
device=self.device,
)
# Forward integrate rigid bodies (snapshots body_q_prev for dynamic bodies only)
wp.launch(
kernel=forward_step_rigid_bodies,
inputs=[
dt,
model.gravity,
model.body_world,
body_f_for_integration,
model.body_com,
model.body_inertia,
self.body_inv_mass_effective,
self.body_inv_inertia_effective,
state_in.body_q, # input/output
state_in.body_qd, # input/output
],
outputs=[
self.body_inertia_q,
self.body_q_prev,
],
dim=model.body_count,
device=self.device,
)
if update_rigid_history:
# Contact warmstarts / adjacency are optional: skip completely if contacts=None.
if contacts is not None:
# Use the Contacts buffer capacity as launch dimension
contact_launch_dim = contacts.rigid_contact_max
# Build per-body contact lists once per step
# Build body-body (rigid-rigid) contact lists
self.body_body_contact_counts.zero_()
wp.launch(
kernel=build_body_body_contact_lists,
dim=contact_launch_dim,
inputs=[
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
model.shape_body,
self.body_body_contact_buffer_pre_alloc,
],
outputs=[
self.body_body_contact_counts,
self.body_body_contact_indices,
],
device=self.device,
)
# Warmstart AVBD body-body contact penalties and pre-compute material properties
wp.launch(
kernel=warmstart_body_body_contacts,
inputs=[
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
model.shape_material_ke,
model.shape_material_kd,
model.shape_material_mu,
self.k_start_body_contact,
],
outputs=[
self.body_body_contact_penalty_k,
self.body_body_contact_material_ke,
self.body_body_contact_material_kd,
self.body_body_contact_material_mu,
],
dim=contact_launch_dim,
device=self.device,
)
# Warmstart AVBD penalty parameters for joints using the same cadence
# as rigid history updates.
if model.joint_count > 0:
wp.launch(
kernel=warmstart_joints,
inputs=[
self.joint_penalty_k_max,
self.joint_penalty_k_min,
self.avbd_gamma,
self.joint_penalty_k, # input/output
],
dim=self.joint_constraint_count,
device=self.device,
)
# Compute Dahl hysteresis parameters for cable bending (once per timestep, frozen during iterations)
if self.enable_dahl_friction and model.joint_count > 0:
wp.launch(
kernel=compute_cable_dahl_parameters,
inputs=[
model.joint_type,
model.joint_enabled,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
self.joint_constraint_start,
self.joint_penalty_k_max,
self.body_q_prev, # Use previous body transforms (start of step) for linearization
model.body_q, # rest body transforms
self.joint_sigma_prev,
self.joint_kappa_prev,
self.joint_dkappa_prev,
self.joint_dahl_eps_max,
self.joint_dahl_tau,
],
outputs=[
self.joint_sigma_start,
self.joint_C_fric,
],
dim=model.joint_count,
device=self.device,
)
# ---------------------------
# Body-particle interaction
# ---------------------------
if model.particle_count > 0 and update_rigid_history and contacts is not None:
# Build body-particle (rigid-particle) contact lists only when SolverVBD
# is integrating rigid bodies itself; the external rigid solver path
# does not use these per-body adjacency structures. Also skip if there
# are no rigid bodies in the model.
if not self.integrate_with_external_rigid_solver and model.body_count > 0:
self.body_particle_contact_counts.zero_()
wp.launch(
kernel=build_body_particle_contact_lists,
dim=contacts.soft_contact_max,
inputs=[
contacts.soft_contact_count,
contacts.soft_contact_shape,
model.shape_body,
self.body_particle_contact_buffer_pre_alloc,
],
outputs=[
self.body_particle_contact_counts,
self.body_particle_contact_indices,
],
device=self.device,
)
# Warmstart AVBD body-particle contact penalties and pre-compute material properties.
# This is useful both when SolverVBD integrates rigid bodies and when an external
# rigid solver is used, since cloth-rigid soft contacts still rely on these penalties.
soft_contact_launch_dim = contacts.soft_contact_max
wp.launch(
kernel=warmstart_body_particle_contacts,
inputs=[
contacts.soft_contact_count,
contacts.soft_contact_shape,
model.soft_contact_ke,
model.soft_contact_kd,
model.soft_contact_mu,
model.shape_material_ke,
model.shape_material_kd,
model.shape_material_mu,
self.k_start_body_contact,
],
outputs=[
self.body_particle_contact_penalty_k,
self.body_particle_contact_material_ke,
self.body_particle_contact_material_kd,
self.body_particle_contact_material_mu,
],
dim=soft_contact_launch_dim,
device=self.device,
)
def _solve_particle_iteration(
self, state_in: State, state_out: State, contacts: Contacts | None, dt: float, iter_num: int
):
"""Solve one VBD iteration for particles."""
model = self.model
# Select rigid-body poses for particle-rigid contact evaluation
if self.integrate_with_external_rigid_solver:
body_q_for_particles = state_out.body_q
body_q_prev_for_particles = state_in.body_q
body_qd_for_particles = state_out.body_qd
else:
body_q_for_particles = state_in.body_q
if model.body_count > 0:
body_q_prev_for_particles = self.body_q_prev
else:
body_q_prev_for_particles = None
body_qd_for_particles = state_in.body_qd
# Early exit if no particles
if model.particle_count == 0:
return
# Update collision detection if needed (penetration-free mode only)
if self.particle_enable_self_contact:
if (self.particle_collision_detection_interval == 0 and iter_num == 0) or (
self.particle_collision_detection_interval >= 1
and iter_num % self.particle_collision_detection_interval == 0
):
self._collision_detection_penetration_free(state_in)
# Zero out forces and hessians
self.particle_forces.zero_()
self.particle_hessians.zero_()
# Iterate over color groups
for color in range(len(self.model.particle_color_groups)):
if contacts is not None:
wp.launch(
kernel=accumulate_particle_body_contact_force_and_hessian,
dim=contacts.soft_contact_max,
inputs=[
dt,
color,
self.particle_q_prev,
state_in.particle_q,
model.particle_colors,
# body-particle contact
self.friction_epsilon,
model.particle_radius,
contacts.soft_contact_particle,
contacts.soft_contact_count,
contacts.soft_contact_max,
self.body_particle_contact_penalty_k,
self.body_particle_contact_material_kd,
self.body_particle_contact_material_mu,
model.shape_material_mu,
model.shape_body,
body_q_for_particles,
body_q_prev_for_particles,
body_qd_for_particles,
model.body_com,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_body_vel,
contacts.soft_contact_normal,
],
outputs=[
self.particle_forces,
self.particle_hessians,
],
device=self.device,
)
if model.spring_count:
wp.launch(
kernel=accumulate_spring_force_and_hessian,
inputs=[
dt,
color,
self.particle_q_prev,
state_in.particle_q,
self.model.particle_colors,
model.spring_count,
self.model.spring_indices,
self.model.spring_rest_length,
self.model.spring_stiffness,
self.model.spring_damping,
],
outputs=[self.particle_forces, self.particle_hessians],
dim=model.spring_count,
device=self.device,
)
if self.particle_enable_self_contact:
wp.launch(
kernel=accumulate_self_contact_force_and_hessian,
dim=self.particle_self_contact_evaluation_kernel_launch_size,
inputs=[
dt,
color,
self.particle_q_prev,
state_in.particle_q,
self.model.particle_colors,
self.model.tri_indices,
self.model.edge_indices,
# self-contact
self.trimesh_collision_info,
self.particle_self_contact_radius,
self.model.soft_contact_ke,
self.model.soft_contact_kd,
self.model.soft_contact_mu,
self.friction_epsilon,
self.trimesh_collision_detector.edge_edge_parallel_epsilon,
],
outputs=[self.particle_forces, self.particle_hessians],
device=self.device,
max_blocks=self.model.device.sm_count,
)
if self.use_particle_tile_solve:
wp.launch(
kernel=solve_elasticity_tile,
dim=self.model.particle_color_groups[color].size * TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE,
block_dim=TILE_SIZE_TRI_MESH_ELASTICITY_SOLVE,
inputs=[
dt,
self.model.particle_color_groups[color],
self.particle_q_prev,
state_in.particle_q,
self.model.particle_mass,
self.inertia,
self.model.particle_flags,
self.model.tri_indices,
self.model.tri_poses,
self.model.tri_materials,
self.model.tri_areas,
self.model.edge_indices,
self.model.edge_rest_angle,
self.model.edge_rest_length,
self.model.edge_bending_properties,
self.model.tet_indices,
self.model.tet_poses,
self.model.tet_materials,
self.particle_adjacency,
self.particle_forces,
self.particle_hessians,
],
outputs=[
self.particle_displacements,
],
device=self.device,
)
else:
wp.launch(
kernel=solve_elasticity,
dim=self.model.particle_color_groups[color].size,
inputs=[
dt,
self.model.particle_color_groups[color],
self.particle_q_prev,
state_in.particle_q,
self.model.particle_mass,
self.inertia,
self.model.particle_flags,
self.model.tri_indices,
self.model.tri_poses,
self.model.tri_materials,
self.model.tri_areas,
self.model.edge_indices,
self.model.edge_rest_angle,
self.model.edge_rest_length,
self.model.edge_bending_properties,
self.model.tet_indices,
self.model.tet_poses,
self.model.tet_materials,
self.particle_adjacency,
self.particle_forces,
self.particle_hessians,
],
outputs=[
self.particle_displacements,
],
device=self.device,
)
self._penetration_free_truncation(state_in.particle_q)
wp.copy(state_out.particle_q, state_in.particle_q)
def _solve_rigid_body_iteration(
self, state_in: State, state_out: State, control: Control, contacts: Contacts | None, dt: float
):
"""Solve one AVBD iteration for rigid bodies (per-iteration phase).
Accumulates contact and joint forces/hessians, solves 6x6 rigid body systems per color,
and updates AVBD penalty parameters (dual update).
"""
model = self.model
# Early-return path:
# - If rigid bodies are integrated by an external solver, skip the AVBD rigid-body solve but still
# update body-particle soft-contact penalties so adaptive stiffness is used for particle-shape
# interaction.
# - If there are no rigid bodies at all (body_count == 0), we likewise skip the rigid-body solve,
# but must still update particle-shape soft-contact penalties (e.g., particles colliding with the
# ground plane where shape_body == -1).
skip_rigid_solve = self.integrate_with_external_rigid_solver or model.body_count == 0
if skip_rigid_solve:
if model.particle_count > 0 and contacts is not None:
# Use external rigid poses when enabled; otherwise use the current VBD poses.
body_q = state_out.body_q if self.integrate_with_external_rigid_solver else state_in.body_q
# Model.state() leaves State.body_q as None when body_count == 0. Warp kernels still
# require a wp.array argument; for static shapes (shape_body == -1) the kernel never
# indexes this array, so an empty placeholder is sufficient.
if body_q is None:
body_q = self._empty_body_q
wp.launch(
kernel=update_duals_body_particle_contacts,
dim=contacts.soft_contact_max,
inputs=[
contacts.soft_contact_count,
contacts.soft_contact_particle,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_normal,
state_in.particle_q,
model.particle_radius,
model.shape_body,
body_q,
self.body_particle_contact_material_ke,
self.avbd_beta,
self.body_particle_contact_penalty_k, # input/output
],
device=self.device,
)
return
# Zero out forces and hessians
self.body_torques.zero_()
self.body_forces.zero_()
self.body_hessian_aa.zero_()
self.body_hessian_al.zero_()
self.body_hessian_ll.zero_()
body_color_groups = model.body_color_groups
# Gauss-Seidel-style per-color updates
for color in range(len(body_color_groups)):
color_group = body_color_groups[color]
# Gauss-Seidel contact accumulation: evaluate contacts for bodies in this color
# Accumulate body-particle forces and Hessians on bodies (per-body, per-color)
if model.particle_count > 0 and contacts is not None:
wp.launch(
kernel=accumulate_body_particle_contacts_per_body,
dim=color_group.size * _NUM_CONTACT_THREADS_PER_BODY,
inputs=[
dt,
color_group,
# particle state
state_in.particle_q,
self.particle_q_prev,
model.particle_radius,
# rigid body state
self.body_q_prev,
state_in.body_q,
state_in.body_qd,
model.body_com,
self.body_inv_mass_effective,
# AVBD body-particle soft contact penalties and material properties
self.friction_epsilon,
self.body_particle_contact_penalty_k,
self.body_particle_contact_material_kd,
self.body_particle_contact_material_mu,
# soft contact data (body-particle contacts)
contacts.soft_contact_count,
contacts.soft_contact_particle,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_body_vel,
contacts.soft_contact_normal,
# shape/material data
model.shape_material_mu,
model.shape_body,
# per-body adjacency (body-particle contacts)
self.body_particle_contact_buffer_pre_alloc,
self.body_particle_contact_counts,
self.body_particle_contact_indices,
],
outputs=[
self.body_forces,
self.body_torques,
self.body_hessian_ll,
self.body_hessian_al,
self.body_hessian_aa,
],
device=self.device,
)
# Accumulate body-body (rigid-rigid) contact forces and Hessians on bodies (per-body, per-color)
if contacts is not None:
wp.launch(
kernel=accumulate_body_body_contacts_per_body,
dim=color_group.size * _NUM_CONTACT_THREADS_PER_BODY,
inputs=[
dt,
color_group,
self.body_q_prev,
state_in.body_q,
model.body_com,
self.body_inv_mass_effective,
self.friction_epsilon,
self.body_body_contact_penalty_k,
self.body_body_contact_material_kd,
self.body_body_contact_material_mu,
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_normal,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
model.shape_body,
self.body_body_contact_buffer_pre_alloc,
self.body_body_contact_counts,
self.body_body_contact_indices,
],
outputs=[
self.body_forces,
self.body_torques,
self.body_hessian_ll,
self.body_hessian_al,
self.body_hessian_aa,
],
device=self.device,
)
wp.launch(
kernel=solve_rigid_body,
inputs=[
dt,
color_group,
state_in.body_q,
self.body_q_prev,
model.body_q,
model.body_mass,
self.body_inv_mass_effective,
model.body_inertia,
self.body_inertia_q,
model.body_com,
self.rigid_adjacency,
model.joint_type,
model.joint_enabled,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_axis,
model.joint_qd_start,
self.joint_constraint_start,
self.joint_penalty_k,
self.joint_penalty_kd,
self.joint_sigma_start,
self.joint_C_fric,
# Drive parameters (DOF-indexed)
model.joint_target_ke,
model.joint_target_kd,
control.joint_target_pos,
control.joint_target_vel,
# Limit parameters (DOF-indexed)
model.joint_limit_lower,
model.joint_limit_upper,
model.joint_limit_ke,
model.joint_limit_kd,
model.joint_dof_dim,
self.joint_rest_angle,
self.body_forces,
self.body_torques,
self.body_hessian_ll,
self.body_hessian_al,
self.body_hessian_aa,
],
outputs=[
state_out.body_q,
],
dim=color_group.size,
device=self.device,
)
wp.launch(
kernel=copy_rigid_body_transforms_back,
inputs=[color_group, state_out.body_q],
outputs=[state_in.body_q],
dim=color_group.size,
device=self.device,
)
if contacts is not None:
# AVBD dual update: update adaptive penalties based on constraint violation
# Update body-body (rigid-rigid) contact penalties
contact_launch_dim = contacts.rigid_contact_max
wp.launch(
kernel=update_duals_body_body_contacts,
dim=contact_launch_dim,
inputs=[
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_normal,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
model.shape_body,
state_out.body_q,
self.body_body_contact_material_ke,
self.avbd_beta,
self.body_body_contact_penalty_k, # input/output
],
device=self.device,
)
# Update body-particle contact penalties
if model.particle_count > 0:
soft_contact_launch_dim = contacts.soft_contact_max
wp.launch(
kernel=update_duals_body_particle_contacts,
dim=soft_contact_launch_dim,
inputs=[
contacts.soft_contact_count,
contacts.soft_contact_particle,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_normal,
state_in.particle_q,
model.particle_radius,
model.shape_body,
# Rigid poses come from SolverVBD itself when
# integrate_with_external_rigid_solver=False
state_in.body_q,
self.body_particle_contact_material_ke,
self.avbd_beta,
self.body_particle_contact_penalty_k, # input/output
],
device=self.device,
)
# Update joint penalties at new positions
if model.joint_count > 0:
wp.launch(
kernel=update_duals_joint,
dim=model.joint_count,
inputs=[
model.joint_type,
model.joint_enabled,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_axis,
model.joint_qd_start,
self.joint_constraint_start,
self.joint_penalty_k_max,
state_out.body_q,
model.body_q,
self.avbd_beta,
self.joint_penalty_k, # input/output
model.joint_dof_dim,
self.joint_rest_angle,
# Drive/limit parameters for adaptive drive/limit penalty growth
model.joint_target_ke,
control.joint_target_pos,
model.joint_limit_lower,
model.joint_limit_upper,
model.joint_limit_ke,
],
device=self.device,
)
def collect_rigid_contact_forces(
self, state: State, contacts: Contacts | None, dt: float
) -> tuple[wp.array, wp.array, wp.array, wp.array, wp.array, wp.array]:
"""Collect per-contact rigid contact forces and world-space application points.
This produces a **contact-specific** buffer that coupling code can filter (e.g., proxy contacts only).
Args:
state (State): Simulation state containing rigid body transforms/velocities
used for contact-force evaluation.
contacts (Optional[Contacts]): Contact data buffers containing rigid
contact geometry/material references. If None, the function
returns default zero/sentinel outputs.
dt (float): Time step size [s].
Returns:
tuple[
wp.array[wp.int32],
wp.array[wp.int32],
wp.array[wp.vec3],
wp.array[wp.vec3],
wp.array[wp.vec3],
wp.array[wp.int32],
]: Tuple of per-contact outputs:
- body0: Body index for shape0, int32.
- body1: Body index for shape1, int32.
- point0_world: World-space contact point on body0, wp.vec3 [m].
- point1_world: World-space contact point on body1, wp.vec3 [m].
- force_on_body1: Contact force applied to body1 in world frame, wp.vec3 [N].
- rigid_contact_count: Length-1 active rigid-contact count, int32.
"""
# Allocate/resize persistent buffers to match contact capacity.
max_contacts = int(contacts.rigid_contact_shape0.shape[0]) if contacts is not None else 0
if not hasattr(self, "_rigid_contact_body0") or self._rigid_contact_body0 is None:
self._rigid_contact_body0 = None
if self._rigid_contact_body0 is None or int(self._rigid_contact_body0.shape[0]) != max_contacts:
self._rigid_contact_body0 = wp.full(max_contacts, -1, dtype=wp.int32, device=self.device)
self._rigid_contact_body1 = wp.full(max_contacts, -1, dtype=wp.int32, device=self.device)
self._rigid_contact_point0_world = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
self._rigid_contact_point1_world = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
missing_rigid_state = any(
arr is None
for arr in (
getattr(self, "body_q_prev", None),
getattr(self, "body_body_contact_penalty_k", None),
getattr(self, "body_body_contact_material_kd", None),
getattr(self, "body_body_contact_material_mu", None),
)
)
no_active_contacts = contacts is None or max_contacts == 0
if contacts is not None and contacts.rigid_contact_force is not None:
contacts.rigid_contact_force.zero_()
if no_active_contacts or missing_rigid_state:
# Keep outputs in a known default state for coupling paths where rigid AVBD
# internal buffers are not initialized (e.g., external rigid solver mode).
self._rigid_contact_body0 = wp.full(max_contacts, -1, dtype=wp.int32, device=self.device)
self._rigid_contact_body1 = wp.full(max_contacts, -1, dtype=wp.int32, device=self.device)
self._rigid_contact_point0_world = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
self._rigid_contact_point1_world = wp.zeros(max_contacts, dtype=wp.vec3, device=self.device)
rigid_contact_count = (
contacts.rigid_contact_count
if contacts is not None and contacts.rigid_contact_count is not None
else wp.zeros(1, dtype=wp.int32, device=self.device)
)
return (
self._rigid_contact_body0,
self._rigid_contact_body1,
self._rigid_contact_point0_world,
self._rigid_contact_point1_world,
contacts.rigid_contact_force
if contacts is not None
else wp.zeros(0, dtype=wp.vec3, device=self.device),
rigid_contact_count,
)
# Type narrowing: remaining path requires a valid Contacts instance.
assert contacts is not None
# Reuse the existing per-contact force buffer in Contacts (allocated by default).
# Force convention: force is applied to body1, and -force is applied to body0.
wp.launch(
kernel=compute_rigid_contact_forces,
dim=max_contacts,
inputs=[
float(dt),
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_normal,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
self.model.shape_body,
state.body_q,
self.body_q_prev,
self.model.body_com,
self.body_body_contact_penalty_k,
self.body_body_contact_material_kd,
self.body_body_contact_material_mu,
float(self.friction_epsilon),
],
outputs=[
self._rigid_contact_body0,
self._rigid_contact_body1,
self._rigid_contact_point0_world,
self._rigid_contact_point1_world,
contacts.rigid_contact_force,
],
device=self.device,
)
return (
self._rigid_contact_body0,
self._rigid_contact_body1,
self._rigid_contact_point0_world,
self._rigid_contact_point1_world,
contacts.rigid_contact_force,
contacts.rigid_contact_count,
)
def _finalize_particles(self, state_out: State, dt: float):
"""Finalize particle velocities after VBD iterations."""
# Early exit if no particles
if self.model.particle_count == 0:
return
wp.launch(
kernel=update_velocity,
inputs=[dt, self.particle_q_prev, state_out.particle_q, state_out.particle_qd],
dim=self.model.particle_count,
device=self.device,
)
def _finalize_rigid_bodies(self, state_out: State, dt: float):
"""Finalize rigid body velocities and Dahl friction state after AVBD iterations (post-iteration phase).
Updates rigid body velocities using BDF1 and updates Dahl hysteresis state for cable bending.
"""
model = self.model
# Early exit if no rigid bodies or rigid bodies are driven by an external solver
if model.body_count == 0 or self.integrate_with_external_rigid_solver:
return
# Velocity update (BDF1) after all iterations
wp.launch(
kernel=update_body_velocity,
inputs=[
dt,
state_out.body_q,
model.body_com,
],
outputs=[self.body_q_prev, state_out.body_qd],
dim=model.body_count,
device=self.device,
)
# Update Dahl hysteresis state after solver convergence (for next timestep's memory)
if self.enable_dahl_friction and model.joint_count > 0:
wp.launch(
kernel=update_cable_dahl_state,
inputs=[
model.joint_type,
model.joint_enabled,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
self.joint_constraint_start,
self.joint_penalty_k_max,
state_out.body_q,
model.body_q,
self.joint_dahl_eps_max,
self.joint_dahl_tau,
self.joint_sigma_prev, # input/output
self.joint_kappa_prev, # input/output
self.joint_dkappa_prev, # input/output
],
dim=model.joint_count,
device=self.device,
)
def _collision_detection_penetration_free(self, current_state: State):
# particle_displacements is based on pos_prev_collision_detection
# so reset them every time we do collision detection
self.pos_prev_collision_detection.assign(current_state.particle_q)
self.particle_displacements.zero_()
self.trimesh_collision_detector.refit(current_state.particle_q)
self.trimesh_collision_detector.vertex_triangle_collision_detection(
self.particle_self_contact_margin,
min_query_radius=self.particle_rest_shape_contact_exclusion_radius,
min_distance_filtering_ref_pos=self.particle_q_rest,
)
self.trimesh_collision_detector.edge_edge_collision_detection(
self.particle_self_contact_margin,
min_query_radius=self.particle_rest_shape_contact_exclusion_radius,
min_distance_filtering_ref_pos=self.particle_q_rest,
)
def rebuild_bvh(self, state: State):
"""This function will rebuild the BVHs used for detecting self-contacts using the input `state`.
When the simulated object deforms significantly, simply refitting the BVH can lead to deterioration of the BVH's
quality. In these cases, rebuilding the entire tree is necessary to achieve better querying efficiency.
Args:
state (newton.State): The state whose particle positions (:attr:`~newton.State.particle_q`) will be used for rebuilding the BVHs.
"""
if self.particle_enable_self_contact:
self.trimesh_collision_detector.rebuild(state.particle_q)
================================================
FILE: newton/_src/solvers/vbd/tri_mesh_collision.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import numpy as np
import warp as wp
from ...geometry.kernels import (
compute_edge_aabbs,
compute_tri_aabbs,
edge_colliding_edges_detection_kernel,
init_triangle_collision_data_kernel,
triangle_triangle_collision_detection_kernel,
vertex_triangle_collision_detection_kernel,
)
from ...sim import Model
@wp.struct
class TriMeshCollisionInfo:
# size: 2 x sum(vertex_colliding_triangles_buffer_sizes)
# every two elements records the vertex index and a triangle index it collides to
vertex_colliding_triangles: wp.array[wp.int32]
vertex_colliding_triangles_offsets: wp.array[wp.int32]
vertex_colliding_triangles_buffer_sizes: wp.array[wp.int32]
vertex_colliding_triangles_count: wp.array[wp.int32]
vertex_colliding_triangles_min_dist: wp.array[float]
triangle_colliding_vertices: wp.array[wp.int32]
triangle_colliding_vertices_offsets: wp.array[wp.int32]
triangle_colliding_vertices_buffer_sizes: wp.array[wp.int32]
triangle_colliding_vertices_count: wp.array[wp.int32]
triangle_colliding_vertices_min_dist: wp.array[float]
# size: 2 x sum(edge_colliding_edges_buffer_sizes)
# every two elements records the edge index and an edge index it collides to
edge_colliding_edges: wp.array[wp.int32]
edge_colliding_edges_offsets: wp.array[wp.int32]
edge_colliding_edges_buffer_sizes: wp.array[wp.int32]
edge_colliding_edges_count: wp.array[wp.int32]
edge_colliding_edges_min_dist: wp.array[float]
@wp.func
def get_vertex_colliding_triangles_count(col_info: TriMeshCollisionInfo, v: int):
return wp.min(col_info.vertex_colliding_triangles_count[v], col_info.vertex_colliding_triangles_buffer_sizes[v])
@wp.func
def get_vertex_colliding_triangles(col_info: TriMeshCollisionInfo, v: int, i_collision: int):
offset = col_info.vertex_colliding_triangles_offsets[v]
return col_info.vertex_colliding_triangles[2 * (offset + i_collision) + 1]
@wp.func
def get_vertex_collision_buffer_vertex_index(col_info: TriMeshCollisionInfo, v: int, i_collision: int):
offset = col_info.vertex_colliding_triangles_offsets[v]
return col_info.vertex_colliding_triangles[2 * (offset + i_collision)]
@wp.func
def get_triangle_colliding_vertices_count(col_info: TriMeshCollisionInfo, tri: int):
return wp.min(
col_info.triangle_colliding_vertices_count[tri], col_info.triangle_colliding_vertices_buffer_sizes[tri]
)
@wp.func
def get_triangle_colliding_vertices(col_info: TriMeshCollisionInfo, tri: int, i_collision: int):
offset = col_info.triangle_colliding_vertices_offsets[tri]
return col_info.triangle_colliding_vertices[offset + i_collision]
@wp.func
def get_edge_colliding_edges_count(col_info: TriMeshCollisionInfo, e: int):
return wp.min(col_info.edge_colliding_edges_count[e], col_info.edge_colliding_edges_buffer_sizes[e])
@wp.func
def get_edge_colliding_edges(col_info: TriMeshCollisionInfo, e: int, i_collision: int):
offset = col_info.edge_colliding_edges_offsets[e]
return col_info.edge_colliding_edges[2 * (offset + i_collision) + 1]
@wp.func
def get_edge_collision_buffer_edge_index(col_info: TriMeshCollisionInfo, e: int, i_collision: int):
offset = col_info.edge_colliding_edges_offsets[e]
return col_info.edge_colliding_edges[2 * (offset + i_collision)]
class TriMeshCollisionDetector:
def __init__(
self,
model: Model,
record_triangle_contacting_vertices=False,
vertex_positions=None,
vertex_collision_buffer_pre_alloc=8,
vertex_collision_buffer_max_alloc=256,
vertex_triangle_filtering_list=None,
vertex_triangle_filtering_list_offsets=None,
triangle_collision_buffer_pre_alloc=16,
triangle_collision_buffer_max_alloc=256,
edge_collision_buffer_pre_alloc=8,
edge_collision_buffer_max_alloc=256,
edge_filtering_list=None,
edge_filtering_list_offsets=None,
triangle_triangle_collision_buffer_pre_alloc=8,
triangle_triangle_collision_buffer_max_alloc=256,
edge_edge_parallel_epsilon=1e-5,
collision_detection_block_size=16,
):
self.model = model
self.record_triangle_contacting_vertices = record_triangle_contacting_vertices
self.vertex_positions = model.particle_q if vertex_positions is None else vertex_positions
self.device = model.device
self.vertex_collision_buffer_pre_alloc = vertex_collision_buffer_pre_alloc
self.vertex_collision_buffer_max_alloc = vertex_collision_buffer_max_alloc
self.triangle_collision_buffer_pre_alloc = triangle_collision_buffer_pre_alloc
self.triangle_collision_buffer_max_alloc = triangle_collision_buffer_max_alloc
self.edge_collision_buffer_pre_alloc = edge_collision_buffer_pre_alloc
self.edge_collision_buffer_max_alloc = edge_collision_buffer_max_alloc
self.triangle_triangle_collision_buffer_pre_alloc = triangle_triangle_collision_buffer_pre_alloc
self.triangle_triangle_collision_buffer_max_alloc = triangle_triangle_collision_buffer_max_alloc
self.vertex_triangle_filtering_list = vertex_triangle_filtering_list
self.vertex_triangle_filtering_list_offsets = vertex_triangle_filtering_list_offsets
self.edge_filtering_list = edge_filtering_list
self.edge_filtering_list_offsets = edge_filtering_list_offsets
self.edge_edge_parallel_epsilon = edge_edge_parallel_epsilon
self.collision_detection_block_size = collision_detection_block_size
self.lower_bounds_tris = wp.array(shape=(model.tri_count,), dtype=wp.vec3, device=model.device)
self.upper_bounds_tris = wp.array(shape=(model.tri_count,), dtype=wp.vec3, device=model.device)
wp.launch(
kernel=compute_tri_aabbs,
inputs=[self.vertex_positions, model.tri_indices, self.lower_bounds_tris, self.upper_bounds_tris],
dim=model.tri_count,
device=model.device,
)
self.bvh_tris = wp.Bvh(self.lower_bounds_tris, self.upper_bounds_tris)
# collision detections results
# vertex collision buffers
self.vertex_colliding_triangles = wp.zeros(
shape=(2 * model.particle_count * self.vertex_collision_buffer_pre_alloc,),
dtype=wp.int32,
device=self.device,
)
self.vertex_colliding_triangles_count = wp.array(
shape=(model.particle_count,), dtype=wp.int32, device=self.device
)
self.vertex_colliding_triangles_min_dist = wp.array(
shape=(model.particle_count,), dtype=float, device=self.device
)
self.vertex_colliding_triangles_buffer_sizes = wp.full(
shape=(model.particle_count,),
value=self.vertex_collision_buffer_pre_alloc,
dtype=wp.int32,
device=self.device,
)
self.vertex_colliding_triangles_offsets = wp.array(
shape=(model.particle_count + 1,), dtype=wp.int32, device=self.device
)
self.compute_collision_buffer_offsets(
self.vertex_colliding_triangles_buffer_sizes, self.vertex_colliding_triangles_offsets
)
if record_triangle_contacting_vertices:
# triangle collision buffers
self.triangle_colliding_vertices = wp.zeros(
shape=(model.tri_count * self.triangle_collision_buffer_pre_alloc,), dtype=wp.int32, device=self.device
)
self.triangle_colliding_vertices_count = wp.zeros(
shape=(model.tri_count,), dtype=wp.int32, device=self.device
)
self.triangle_colliding_vertices_buffer_sizes = wp.full(
shape=(model.tri_count,),
value=self.triangle_collision_buffer_pre_alloc,
dtype=wp.int32,
device=self.device,
)
self.triangle_colliding_vertices_offsets = wp.array(
shape=(model.tri_count + 1,), dtype=wp.int32, device=self.device
)
self.compute_collision_buffer_offsets(
self.triangle_colliding_vertices_buffer_sizes, self.triangle_colliding_vertices_offsets
)
else:
self.triangle_colliding_vertices = None
self.triangle_colliding_vertices_count = None
self.triangle_colliding_vertices_buffer_sizes = None
self.triangle_colliding_vertices_offsets = None
# this is need regardless of whether we record triangle contacting vertices
self.triangle_colliding_vertices_min_dist = wp.array(shape=(model.tri_count,), dtype=float, device=self.device)
# edge collision buffers
self.edge_colliding_edges = wp.zeros(
shape=(2 * model.edge_count * self.edge_collision_buffer_pre_alloc,), dtype=wp.int32, device=self.device
)
self.edge_colliding_edges_count = wp.zeros(shape=(model.edge_count,), dtype=wp.int32, device=self.device)
self.edge_colliding_edges_buffer_sizes = wp.full(
shape=(model.edge_count,),
value=self.edge_collision_buffer_pre_alloc,
dtype=wp.int32,
device=self.device,
)
self.edge_colliding_edges_offsets = wp.array(shape=(model.edge_count + 1,), dtype=wp.int32, device=self.device)
self.compute_collision_buffer_offsets(self.edge_colliding_edges_buffer_sizes, self.edge_colliding_edges_offsets)
self.edge_colliding_edges_min_dist = wp.array(shape=(model.edge_count,), dtype=float, device=self.device)
self.lower_bounds_edges = wp.array(shape=(model.edge_count,), dtype=wp.vec3, device=model.device)
self.upper_bounds_edges = wp.array(shape=(model.edge_count,), dtype=wp.vec3, device=model.device)
wp.launch(
kernel=compute_edge_aabbs,
inputs=[self.vertex_positions, model.edge_indices, self.lower_bounds_edges, self.upper_bounds_edges],
dim=model.edge_count,
device=model.device,
)
self.bvh_edges = wp.Bvh(self.lower_bounds_edges, self.upper_bounds_edges)
self.resize_flags = wp.zeros(shape=(4,), dtype=wp.int32, device=self.device)
self.collision_info = self.get_collision_data()
# data for triangle-triangle intersection; they will only be initialized on demand, as triangle-triangle intersection is not needed for simulation
self.triangle_intersecting_triangles = None
self.triangle_intersecting_triangles_count = None
self.triangle_intersecting_triangles_offsets = None
def set_collision_filter_list(
self,
vertex_triangle_filtering_list,
vertex_triangle_filtering_list_offsets,
edge_filtering_list,
edge_filtering_list_offsets,
):
self.vertex_triangle_filtering_list = vertex_triangle_filtering_list
self.vertex_triangle_filtering_list_offsets = vertex_triangle_filtering_list_offsets
self.edge_filtering_list = edge_filtering_list
self.edge_filtering_list_offsets = edge_filtering_list_offsets
def get_collision_data(self):
collision_info = TriMeshCollisionInfo()
collision_info.vertex_colliding_triangles = self.vertex_colliding_triangles
collision_info.vertex_colliding_triangles_offsets = self.vertex_colliding_triangles_offsets
collision_info.vertex_colliding_triangles_buffer_sizes = self.vertex_colliding_triangles_buffer_sizes
collision_info.vertex_colliding_triangles_count = self.vertex_colliding_triangles_count
collision_info.vertex_colliding_triangles_min_dist = self.vertex_colliding_triangles_min_dist
if self.record_triangle_contacting_vertices:
collision_info.triangle_colliding_vertices = self.triangle_colliding_vertices
collision_info.triangle_colliding_vertices_offsets = self.triangle_colliding_vertices_offsets
collision_info.triangle_colliding_vertices_buffer_sizes = self.triangle_colliding_vertices_buffer_sizes
collision_info.triangle_colliding_vertices_count = self.triangle_colliding_vertices_count
collision_info.triangle_colliding_vertices_min_dist = self.triangle_colliding_vertices_min_dist
collision_info.edge_colliding_edges = self.edge_colliding_edges
collision_info.edge_colliding_edges_offsets = self.edge_colliding_edges_offsets
collision_info.edge_colliding_edges_buffer_sizes = self.edge_colliding_edges_buffer_sizes
collision_info.edge_colliding_edges_count = self.edge_colliding_edges_count
collision_info.edge_colliding_edges_min_dist = self.edge_colliding_edges_min_dist
return collision_info
def compute_collision_buffer_offsets(self, buffer_sizes: wp.array[wp.int32], offsets: wp.array[wp.int32]):
assert offsets.size == buffer_sizes.size + 1
offsets_np = np.empty(shape=(offsets.size,), dtype=np.int32)
offsets_np[1:] = np.cumsum(buffer_sizes.numpy())[:]
offsets_np[0] = 0
offsets.assign(offsets_np)
def rebuild(self, new_pos=None):
if new_pos is not None:
self.vertex_positions = new_pos
wp.launch(
kernel=compute_tri_aabbs,
inputs=[
self.vertex_positions,
self.model.tri_indices,
],
outputs=[self.lower_bounds_tris, self.upper_bounds_tris],
dim=self.model.tri_count,
device=self.model.device,
)
self.bvh_tris.rebuild()
wp.launch(
kernel=compute_edge_aabbs,
inputs=[self.vertex_positions, self.model.edge_indices],
outputs=[self.lower_bounds_edges, self.upper_bounds_edges],
dim=self.model.edge_count,
device=self.model.device,
)
self.bvh_edges.rebuild()
def refit(self, new_pos=None):
if new_pos is not None:
self.vertex_positions = new_pos
self.refit_triangles()
self.refit_edges()
def refit_triangles(self):
wp.launch(
kernel=compute_tri_aabbs,
inputs=[self.vertex_positions, self.model.tri_indices, self.lower_bounds_tris, self.upper_bounds_tris],
dim=self.model.tri_count,
device=self.model.device,
)
self.bvh_tris.refit()
def refit_edges(self):
wp.launch(
kernel=compute_edge_aabbs,
inputs=[self.vertex_positions, self.model.edge_indices, self.lower_bounds_edges, self.upper_bounds_edges],
dim=self.model.edge_count,
device=self.model.device,
)
self.bvh_edges.refit()
def vertex_triangle_collision_detection(
self, max_query_radius, min_query_radius=0.0, min_distance_filtering_ref_pos=None
):
self.vertex_colliding_triangles.fill_(-1)
if self.record_triangle_contacting_vertices:
wp.launch(
kernel=init_triangle_collision_data_kernel,
inputs=[
max_query_radius,
],
outputs=[
self.triangle_colliding_vertices_count,
self.triangle_colliding_vertices_min_dist,
self.resize_flags,
],
dim=self.model.tri_count,
device=self.model.device,
)
else:
self.triangle_colliding_vertices_min_dist.fill_(max_query_radius)
wp.launch(
kernel=vertex_triangle_collision_detection_kernel,
inputs=[
max_query_radius,
min_query_radius,
self.bvh_tris.id,
self.vertex_positions,
self.model.tri_indices,
self.vertex_colliding_triangles_offsets,
self.vertex_colliding_triangles_buffer_sizes,
self.triangle_colliding_vertices_offsets,
self.triangle_colliding_vertices_buffer_sizes,
self.vertex_triangle_filtering_list,
self.vertex_triangle_filtering_list_offsets,
min_distance_filtering_ref_pos if min_distance_filtering_ref_pos is not None else self.vertex_positions,
],
outputs=[
self.vertex_colliding_triangles,
self.vertex_colliding_triangles_count,
self.vertex_colliding_triangles_min_dist,
self.triangle_colliding_vertices,
self.triangle_colliding_vertices_count,
self.triangle_colliding_vertices_min_dist,
self.resize_flags,
],
dim=self.model.particle_count,
device=self.model.device,
block_dim=self.collision_detection_block_size,
)
def edge_edge_collision_detection(
self, max_query_radius, min_query_radius=0.0, min_distance_filtering_ref_pos=None
):
self.edge_colliding_edges.fill_(-1)
wp.launch(
kernel=edge_colliding_edges_detection_kernel,
inputs=[
max_query_radius,
min_query_radius,
self.bvh_edges.id,
self.vertex_positions,
self.model.edge_indices,
self.edge_colliding_edges_offsets,
self.edge_colliding_edges_buffer_sizes,
self.edge_edge_parallel_epsilon,
self.edge_filtering_list,
self.edge_filtering_list_offsets,
min_distance_filtering_ref_pos if min_distance_filtering_ref_pos is not None else self.vertex_positions,
],
outputs=[
self.edge_colliding_edges,
self.edge_colliding_edges_count,
self.edge_colliding_edges_min_dist,
self.resize_flags,
],
dim=self.model.edge_count,
device=self.model.device,
block_dim=self.collision_detection_block_size,
)
def triangle_triangle_intersection_detection(self):
if self.triangle_intersecting_triangles is None:
self.triangle_intersecting_triangles = wp.zeros(
shape=(self.model.tri_count * self.triangle_triangle_collision_buffer_pre_alloc,),
dtype=wp.int32,
device=self.device,
)
if self.triangle_intersecting_triangles_count is None:
self.triangle_intersecting_triangles_count = wp.array(
shape=(self.model.tri_count,), dtype=wp.int32, device=self.device
)
if self.triangle_intersecting_triangles_offsets is None:
buffer_sizes = np.full((self.model.tri_count,), self.triangle_triangle_collision_buffer_pre_alloc)
offsets = np.zeros((self.model.tri_count + 1,), dtype=np.int32)
offsets[1:] = np.cumsum(buffer_sizes)
self.triangle_intersecting_triangles_offsets = wp.array(offsets, dtype=wp.int32, device=self.device)
wp.launch(
kernel=triangle_triangle_collision_detection_kernel,
inputs=[
self.bvh_tris.id,
self.vertex_positions,
self.model.tri_indices,
self.triangle_intersecting_triangles_offsets,
],
outputs=[
self.triangle_intersecting_triangles,
self.triangle_intersecting_triangles_count,
self.resize_flags,
],
dim=self.model.tri_count,
device=self.model.device,
)
================================================
FILE: newton/_src/solvers/xpbd/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from .solver_xpbd import SolverXPBD
__all__ = [
"SolverXPBD",
]
================================================
FILE: newton/_src/solvers/xpbd/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...geometry import ParticleFlags
from ...math import (
vec_abs,
vec_leaky_max,
vec_leaky_min,
vec_max,
vec_min,
velocity_at_point,
)
from ...sim import BodyFlags, JointType
@wp.kernel
def copy_kinematic_body_state_kernel(
body_flags: wp.array[wp.int32],
body_q_in: wp.array[wp.transform],
body_qd_in: wp.array[wp.spatial_vector],
body_q_out: wp.array[wp.transform],
body_qd_out: wp.array[wp.spatial_vector],
):
"""Copy prescribed maximal state through the solve for kinematic bodies."""
tid = wp.tid()
if (body_flags[tid] & int(BodyFlags.KINEMATIC)) == 0:
return
body_q_out[tid] = body_q_in[tid]
body_qd_out[tid] = body_qd_in[tid]
@wp.kernel
def apply_particle_shape_restitution(
particle_v_new: wp.array[wp.vec3],
particle_x_old: wp.array[wp.vec3],
particle_v_old: wp.array[wp.vec3],
particle_radius: wp.array[float],
particle_flags: wp.array[wp.int32],
body_q: wp.array[wp.transform],
body_q_prev: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_qd_prev: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
shape_body: wp.array[int],
particle_ka: float,
restitution: float,
contact_count: wp.array[int],
contact_particle: wp.array[int],
contact_shape: wp.array[int],
contact_body_pos: wp.array[wp.vec3],
contact_body_vel: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
contact_max: int,
particle_v_out: wp.array[wp.vec3],
):
tid = wp.tid()
count = min(contact_max, contact_count[0])
if tid >= count:
return
shape_index = contact_shape[tid]
body_index = shape_body[shape_index]
particle_index = contact_particle[tid]
if (particle_flags[particle_index] & ParticleFlags.ACTIVE) == 0:
return
v_new = particle_v_new[particle_index]
px = particle_x_old[particle_index]
v_old = particle_v_old[particle_index]
X_wb = wp.transform_identity()
X_wb_prev = wp.transform_identity()
X_com = wp.vec3()
if body_index >= 0:
X_wb = body_q[body_index]
X_wb_prev = body_q_prev[body_index]
X_com = body_com[body_index]
# body position in world space
bx = wp.transform_point(X_wb, contact_body_pos[tid])
n = contact_normal[tid]
c = wp.dot(n, px - bx) - particle_radius[particle_index]
if c > particle_ka:
return
# lever arm from previous pose (consistent with apply_rigid_restitution)
bx_prev = wp.transform_point(X_wb_prev, contact_body_pos[tid])
r = bx_prev - wp.transform_point(X_wb_prev, X_com)
# compute body velocity at the contact point
bv_contact = wp.transform_vector(X_wb_prev, contact_body_vel[tid])
bv_old = bv_contact
bv_new = bv_contact
if body_index >= 0:
bv_old = velocity_at_point(body_qd_prev[body_index], r) + bv_contact
bv_new = velocity_at_point(body_qd[body_index], r) + bv_contact
rel_vel_old = wp.dot(n, v_old - bv_old)
rel_vel_new = wp.dot(n, v_new - bv_new)
if rel_vel_old < 0.0:
dv = n * (-rel_vel_new + wp.max(-restitution * rel_vel_old, 0.0))
wp.atomic_add(particle_v_out, particle_index, dv)
@wp.kernel
def solve_particle_shape_contacts(
particle_x: wp.array[wp.vec3],
particle_v: wp.array[wp.vec3],
particle_invmass: wp.array[float],
particle_radius: wp.array[float],
particle_flags: wp.array[wp.int32],
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
body_m_inv: wp.array[float],
body_I_inv: wp.array[wp.mat33],
shape_body: wp.array[int],
shape_material_mu: wp.array[float],
particle_mu: float,
particle_ka: float,
contact_count: wp.array[int],
contact_particle: wp.array[int],
contact_shape: wp.array[int],
contact_body_pos: wp.array[wp.vec3],
contact_body_vel: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
contact_max: int,
dt: float,
relaxation: float,
# outputs
delta: wp.array[wp.vec3],
body_delta: wp.array[wp.spatial_vector],
):
tid = wp.tid()
count = min(contact_max, contact_count[0])
if tid >= count:
return
shape_index = contact_shape[tid]
body_index = shape_body[shape_index]
particle_index = contact_particle[tid]
if (particle_flags[particle_index] & ParticleFlags.ACTIVE) == 0:
return
px = particle_x[particle_index]
pv = particle_v[particle_index]
X_wb = wp.transform_identity()
X_com = wp.vec3()
if body_index >= 0:
X_wb = body_q[body_index]
X_com = body_com[body_index]
# body position in world space
bx = wp.transform_point(X_wb, contact_body_pos[tid])
r = bx - wp.transform_point(X_wb, X_com)
n = contact_normal[tid]
c = wp.dot(n, px - bx) - particle_radius[particle_index]
if c > particle_ka:
return
# take average material properties of shape and particle parameters
mu = 0.5 * (particle_mu + shape_material_mu[shape_index])
# body velocity
body_v_s = wp.spatial_vector()
if body_index >= 0:
body_v_s = body_qd[body_index]
body_w = wp.spatial_bottom(body_v_s)
body_v = wp.spatial_top(body_v_s)
# compute the body velocity at the particle position
bv = body_v + wp.cross(body_w, r) + wp.transform_vector(X_wb, contact_body_vel[tid])
# relative velocity
v = pv - bv
# normal
lambda_n = c
delta_n = n * lambda_n
# friction
vn = wp.dot(n, v)
vt = v - n * vn
# compute inverse masses
w1 = particle_invmass[particle_index]
w2 = 0.0
if body_index >= 0:
angular = wp.cross(r, n)
q = wp.transform_get_rotation(X_wb)
rot_angular = wp.quat_rotate_inv(q, angular)
I_inv = body_I_inv[body_index]
w2 = body_m_inv[body_index] + wp.dot(rot_angular, I_inv * rot_angular)
denom = w1 + w2
if denom == 0.0:
return
lambda_f = wp.max(mu * lambda_n, -wp.length(vt) * dt)
delta_f = wp.normalize(vt) * lambda_f
delta_total = (delta_f - delta_n) / denom * relaxation
wp.atomic_add(delta, particle_index, w1 * delta_total)
if body_index >= 0:
delta_t = wp.cross(r, delta_total)
wp.atomic_sub(body_delta, body_index, wp.spatial_vector(delta_total, delta_t))
@wp.kernel
def solve_particle_particle_contacts(
grid: wp.uint64,
particle_x: wp.array[wp.vec3],
particle_v: wp.array[wp.vec3],
particle_invmass: wp.array[float],
particle_radius: wp.array[float],
particle_flags: wp.array[wp.int32],
k_mu: float,
k_cohesion: float,
max_radius: float,
dt: float,
relaxation: float,
# outputs
deltas: wp.array[wp.vec3],
):
tid = wp.tid()
# order threads by cell
i = wp.hash_grid_point_id(grid, tid)
if i == -1:
# hash grid has not been built yet
return
if (particle_flags[i] & ParticleFlags.ACTIVE) == 0:
return
x = particle_x[i]
v = particle_v[i]
radius = particle_radius[i]
w1 = particle_invmass[i]
# particle contact
query = wp.hash_grid_query(grid, x, radius + max_radius + k_cohesion)
index = int(0)
delta = wp.vec3(0.0)
while wp.hash_grid_query_next(query, index):
if (particle_flags[index] & ParticleFlags.ACTIVE) != 0 and index != i:
# compute distance to point
n = x - particle_x[index]
d = wp.length(n)
err = d - radius - particle_radius[index]
# compute inverse masses
w2 = particle_invmass[index]
denom = w1 + w2
if err <= k_cohesion and denom > 0.0:
n = n / d
vrel = v - particle_v[index]
# normal
lambda_n = err
delta_n = n * lambda_n
# friction
vn = wp.dot(n, vrel)
vt = vrel - n * vn
lambda_f = wp.max(k_mu * lambda_n, -wp.length(vt) * dt)
delta_f = wp.normalize(vt) * lambda_f
delta += (delta_f - delta_n) / denom
wp.atomic_add(deltas, i, delta * w1 * relaxation)
@wp.kernel
def solve_springs(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
invmass: wp.array[float],
spring_indices: wp.array[int],
spring_rest_lengths: wp.array[float],
spring_stiffness: wp.array[float],
spring_damping: wp.array[float],
dt: float,
lambdas: wp.array[float],
delta: wp.array[wp.vec3],
):
tid = wp.tid()
i = spring_indices[tid * 2 + 0]
j = spring_indices[tid * 2 + 1]
ke = spring_stiffness[tid]
kd = spring_damping[tid]
rest = spring_rest_lengths[tid]
xi = x[i]
xj = x[j]
vi = v[i]
vj = v[j]
xij = xi - xj
vij = vi - vj
l = wp.length(xij)
if l == 0.0:
return
n = xij / l
c = l - rest
grad_c_xi = n
grad_c_xj = -1.0 * n
wi = invmass[i]
wj = invmass[j]
denom = wi + wj
# Note strict inequality for damping -- 0 damping is ok
if denom <= 0.0 or ke <= 0.0 or kd < 0.0:
return
alpha = 1.0 / (ke * dt * dt)
gamma = kd / (ke * dt)
grad_c_dot_v = dt * wp.dot(grad_c_xi, vij) # Note: dt because from the paper we want x_i - x^n, not v...
dlambda = -1.0 * (c + alpha * lambdas[tid] + gamma * grad_c_dot_v) / ((1.0 + gamma) * denom + alpha)
dxi = wi * dlambda * grad_c_xi
dxj = wj * dlambda * grad_c_xj
lambdas[tid] = lambdas[tid] + dlambda
wp.atomic_add(delta, i, dxi)
wp.atomic_add(delta, j, dxj)
@wp.kernel
def bending_constraint(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
invmass: wp.array[float],
indices: wp.array2d[int],
rest: wp.array[float],
bending_properties: wp.array2d[float],
dt: float,
lambdas: wp.array[float],
delta: wp.array[wp.vec3],
):
tid = wp.tid()
eps = 1.0e-6
ke = bending_properties[tid, 0]
kd = bending_properties[tid, 1]
i = indices[tid, 0]
j = indices[tid, 1]
k = indices[tid, 2]
l = indices[tid, 3]
if i == -1 or j == -1 or k == -1 or l == -1:
return
rest_angle = rest[tid]
x1 = x[i]
x2 = x[j]
x3 = x[k]
x4 = x[l]
v1 = v[i]
v2 = v[j]
v3 = v[k]
v4 = v[l]
w1 = invmass[i]
w2 = invmass[j]
w3 = invmass[k]
w4 = invmass[l]
n1 = wp.cross(x3 - x1, x4 - x1) # normal to face 1
n2 = wp.cross(x4 - x2, x3 - x2) # normal to face 2
e = x4 - x3
n1_length = wp.length(n1)
n2_length = wp.length(n2)
e_length = wp.length(e)
# Check for degenerate cases
if n1_length < eps or n2_length < eps or e_length < eps:
return
n1_hat = n1 / n1_length
n2_hat = n2 / n2_length
e_hat = e / e_length
cos_theta = wp.dot(n1_hat, n2_hat)
sin_theta = wp.dot(wp.cross(n1_hat, n2_hat), e_hat)
theta = wp.atan2(sin_theta, cos_theta)
c = theta - rest_angle
grad_x1 = -n1_hat * e_length
grad_x2 = -n2_hat * e_length
grad_x3 = -n1_hat * wp.dot(x1 - x4, e_hat) - n2_hat * wp.dot(x2 - x4, e_hat)
grad_x4 = -n1_hat * wp.dot(x3 - x1, e_hat) - n2_hat * wp.dot(x3 - x2, e_hat)
denominator = (
w1 * wp.length_sq(grad_x1)
+ w2 * wp.length_sq(grad_x2)
+ w3 * wp.length_sq(grad_x3)
+ w4 * wp.length_sq(grad_x4)
)
# Note strict inequality for damping -- 0 damping is ok
if denominator <= 0.0 or ke <= 0.0 or kd < 0.0:
return
alpha = 1.0 / (ke * dt * dt)
gamma = kd / (ke * dt)
grad_dot_v = dt * (wp.dot(grad_x1, v1) + wp.dot(grad_x2, v2) + wp.dot(grad_x3, v3) + wp.dot(grad_x4, v4))
dlambda = -1.0 * (c + alpha * lambdas[tid] + gamma * grad_dot_v) / ((1.0 + gamma) * denominator + alpha)
delta0 = w1 * dlambda * grad_x1
delta1 = w2 * dlambda * grad_x2
delta2 = w3 * dlambda * grad_x3
delta3 = w4 * dlambda * grad_x4
lambdas[tid] = lambdas[tid] + dlambda
wp.atomic_add(delta, i, delta0)
wp.atomic_add(delta, j, delta1)
wp.atomic_add(delta, k, delta2)
wp.atomic_add(delta, l, delta3)
@wp.kernel
def solve_tetrahedra(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
inv_mass: wp.array[float],
indices: wp.array2d[int],
rest_matrix: wp.array[wp.mat33],
activation: wp.array[float],
materials: wp.array2d[float],
dt: float,
relaxation: float,
delta: wp.array[wp.vec3],
):
tid = wp.tid()
i = indices[tid, 0]
j = indices[tid, 1]
k = indices[tid, 2]
l = indices[tid, 3]
# act = activation[tid]
# k_mu = materials[tid, 0]
# k_lambda = materials[tid, 1]
# k_damp = materials[tid, 2]
x0 = x[i]
x1 = x[j]
x2 = x[k]
x3 = x[l]
# v0 = v[i]
# v1 = v[j]
# v2 = v[k]
# v3 = v[l]
w0 = inv_mass[i]
w1 = inv_mass[j]
w2 = inv_mass[k]
w3 = inv_mass[l]
x10 = x1 - x0
x20 = x2 - x0
x30 = x3 - x0
Ds = wp.matrix_from_cols(x10, x20, x30)
Dm = rest_matrix[tid]
inv_QT = wp.transpose(Dm)
inv_rest_volume = wp.determinant(Dm) * 6.0
# F = Xs*Xm^-1
F = Ds * Dm
f1 = wp.vec3(F[0, 0], F[1, 0], F[2, 0])
f2 = wp.vec3(F[0, 1], F[1, 1], F[2, 1])
f3 = wp.vec3(F[0, 2], F[1, 2], F[2, 2])
tr = wp.dot(f1, f1) + wp.dot(f2, f2) + wp.dot(f3, f3)
C = float(0.0)
dC = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
compliance = float(0.0)
stretching_compliance = relaxation
volume_compliance = relaxation
num_terms = 2
for term in range(0, num_terms):
if term == 0:
# deviatoric, stable
C = tr - 3.0
dC = F * 2.0
compliance = stretching_compliance
elif term == 1:
# volume conservation
C = wp.determinant(F) - 1.0
dC = wp.matrix_from_cols(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))
compliance = volume_compliance
if C != 0.0:
dP = dC * inv_QT
grad1 = wp.vec3(dP[0][0], dP[1][0], dP[2][0])
grad2 = wp.vec3(dP[0][1], dP[1][1], dP[2][1])
grad3 = wp.vec3(dP[0][2], dP[1][2], dP[2][2])
grad0 = -grad1 - grad2 - grad3
w = (
wp.dot(grad0, grad0) * w0
+ wp.dot(grad1, grad1) * w1
+ wp.dot(grad2, grad2) * w2
+ wp.dot(grad3, grad3) * w3
)
if w > 0.0:
alpha = compliance / dt / dt
if inv_rest_volume > 0.0:
alpha *= inv_rest_volume
dlambda = -C / (w + alpha)
wp.atomic_add(delta, i, w0 * dlambda * grad0)
wp.atomic_add(delta, j, w1 * dlambda * grad1)
wp.atomic_add(delta, k, w2 * dlambda * grad2)
wp.atomic_add(delta, l, w3 * dlambda * grad3)
# wp.atomic_add(particle.num_corr, id0, 1)
# wp.atomic_add(particle.num_corr, id1, 1)
# wp.atomic_add(particle.num_corr, id2, 1)
# wp.atomic_add(particle.num_corr, id3, 1)
# C_Spherical
# r_s = wp.sqrt(wp.dot(f1, f1) + wp.dot(f2, f2) + wp.dot(f3, f3))
# r_s_inv = 1.0/r_s
# C = r_s - wp.sqrt(3.0)
# dCdx = F*wp.transpose(Dm)*r_s_inv
# alpha = 1.0
# C_D
# r_s = wp.sqrt(wp.dot(f1, f1) + wp.dot(f2, f2) + wp.dot(f3, f3))
# C = r_s*r_s - 3.0
# dCdx = F*wp.transpose(Dm)*2.0
# alpha = 1.0
# grad1 = wp.vec3(dCdx[0, 0], dCdx[1, 0], dCdx[2, 0])
# grad2 = wp.vec3(dCdx[0, 1], dCdx[1, 1], dCdx[2, 1])
# grad3 = wp.vec3(dCdx[0, 2], dCdx[1, 2], dCdx[2, 2])
# grad0 = (grad1 + grad2 + grad3) * (0.0 - 1.0)
# denom = (
# wp.dot(grad0, grad0) * w0 + wp.dot(grad1, grad1) * w1 + wp.dot(grad2, grad2) * w2 + wp.dot(grad3, grad3) * w3
# )
# multiplier = C / (denom + 1.0 / (k_mu * dt * dt * rest_volume))
# delta0 = grad0 * multiplier
# delta1 = grad1 * multiplier
# delta2 = grad2 * multiplier
# delta3 = grad3 * multiplier
# # hydrostatic part
# J = wp.determinant(F)
# C_vol = J - alpha
# # dCdx = wp.matrix_from_cols(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))*wp.transpose(Dm)
# # grad1 = wp.vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
# # grad2 = wp.vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
# # grad3 = wp.vec3(dCdx[0,2], dCdx[1,2], dCdx[2,2])
# # grad0 = (grad1 + grad2 + grad3)*(0.0 - 1.0)
# s = inv_rest_volume / 6.0
# grad1 = wp.cross(x20, x30) * s
# grad2 = wp.cross(x30, x10) * s
# grad3 = wp.cross(x10, x20) * s
# grad0 = -(grad1 + grad2 + grad3)
# denom = (
# wp.dot(grad0, grad0) * w0 + wp.dot(grad1, grad1) * w1 + wp.dot(grad2, grad2) * w2 + wp.dot(grad3, grad3) * w3
# )
# multiplier = C_vol / (denom + 1.0 / (k_lambda * dt * dt * rest_volume))
# delta0 += grad0 * multiplier
# delta1 += grad1 * multiplier
# delta2 += grad2 * multiplier
# delta3 += grad3 * multiplier
# # # apply forces
# # wp.atomic_sub(delta, i, delta0 * w0 * relaxation)
# # wp.atomic_sub(delta, j, delta1 * w1 * relaxation)
# # wp.atomic_sub(delta, k, delta2 * w2 * relaxation)
# # wp.atomic_sub(delta, l, delta3 * w3 * relaxation)
@wp.kernel
def solve_tetrahedra2(
x: wp.array[wp.vec3],
v: wp.array[wp.vec3],
inv_mass: wp.array[float],
indices: wp.array2d[int],
pose: wp.array[wp.mat33],
activation: wp.array[float],
materials: wp.array2d[float],
dt: float,
relaxation: float,
delta: wp.array[wp.vec3],
):
tid = wp.tid()
i = indices[tid, 0]
j = indices[tid, 1]
k = indices[tid, 2]
l = indices[tid, 3]
# act = activation[tid]
k_mu = materials[tid, 0]
k_lambda = materials[tid, 1]
# k_damp = materials[tid, 2]
x0 = x[i]
x1 = x[j]
x2 = x[k]
x3 = x[l]
# v0 = v[i]
# v1 = v[j]
# v2 = v[k]
# v3 = v[l]
w0 = inv_mass[i]
w1 = inv_mass[j]
w2 = inv_mass[k]
w3 = inv_mass[l]
x10 = x1 - x0
x20 = x2 - x0
x30 = x3 - x0
Ds = wp.matrix_from_cols(x10, x20, x30)
Dm = pose[tid]
inv_rest_volume = wp.determinant(Dm) * 6.0
rest_volume = 1.0 / inv_rest_volume
# F = Xs*Xm^-1
F = Ds * Dm
f1 = wp.vec3(F[0, 0], F[1, 0], F[2, 0])
f2 = wp.vec3(F[0, 1], F[1, 1], F[2, 1])
f3 = wp.vec3(F[0, 2], F[1, 2], F[2, 2])
# C_sqrt
# tr = wp.dot(f1, f1) + wp.dot(f2, f2) + wp.dot(f3, f3)
# r_s = wp.sqrt(abs(tr - 3.0))
# C = r_s
# if (r_s == 0.0):
# return
# if (tr < 3.0):
# r_s = 0.0 - r_s
# dCdx = F*wp.transpose(Dm)*(1.0/r_s)
# alpha = 1.0 + k_mu / k_lambda
# C_Neo
r_s = wp.sqrt(wp.dot(f1, f1) + wp.dot(f2, f2) + wp.dot(f3, f3))
if r_s == 0.0:
return
# tr = wp.dot(f1, f1) + wp.dot(f2, f2) + wp.dot(f3, f3)
# if (tr < 3.0):
# r_s = -r_s
r_s_inv = 1.0 / r_s
C = r_s
dCdx = F * wp.transpose(Dm) * r_s_inv
alpha = 1.0 + k_mu / k_lambda
# C_Spherical
# r_s = wp.sqrt(wp.dot(f1, f1) + wp.dot(f2, f2) + wp.dot(f3, f3))
# r_s_inv = 1.0/r_s
# C = r_s - wp.sqrt(3.0)
# dCdx = F*wp.transpose(Dm)*r_s_inv
# alpha = 1.0
# C_D
# r_s = wp.sqrt(wp.dot(f1, f1) + wp.dot(f2, f2) + wp.dot(f3, f3))
# C = r_s*r_s - 3.0
# dCdx = F*wp.transpose(Dm)*2.0
# alpha = 1.0
grad1 = wp.vec3(dCdx[0, 0], dCdx[1, 0], dCdx[2, 0])
grad2 = wp.vec3(dCdx[0, 1], dCdx[1, 1], dCdx[2, 1])
grad3 = wp.vec3(dCdx[0, 2], dCdx[1, 2], dCdx[2, 2])
grad0 = (grad1 + grad2 + grad3) * (0.0 - 1.0)
denom = (
wp.dot(grad0, grad0) * w0 + wp.dot(grad1, grad1) * w1 + wp.dot(grad2, grad2) * w2 + wp.dot(grad3, grad3) * w3
)
multiplier = C / (denom + 1.0 / (k_mu * dt * dt * rest_volume))
delta0 = grad0 * multiplier
delta1 = grad1 * multiplier
delta2 = grad2 * multiplier
delta3 = grad3 * multiplier
# hydrostatic part
J = wp.determinant(F)
C_vol = J - alpha
# dCdx = wp.matrix_from_cols(wp.cross(f2, f3), wp.cross(f3, f1), wp.cross(f1, f2))*wp.transpose(Dm)
# grad1 = wp.vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
# grad2 = wp.vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
# grad3 = wp.vec3(dCdx[0,2], dCdx[1,2], dCdx[2,2])
# grad0 = (grad1 + grad2 + grad3)*(0.0 - 1.0)
s = inv_rest_volume / 6.0
grad1 = wp.cross(x20, x30) * s
grad2 = wp.cross(x30, x10) * s
grad3 = wp.cross(x10, x20) * s
grad0 = -(grad1 + grad2 + grad3)
denom = (
wp.dot(grad0, grad0) * w0 + wp.dot(grad1, grad1) * w1 + wp.dot(grad2, grad2) * w2 + wp.dot(grad3, grad3) * w3
)
multiplier = C_vol / (denom + 1.0 / (k_lambda * dt * dt * rest_volume))
delta0 += grad0 * multiplier
delta1 += grad1 * multiplier
delta2 += grad2 * multiplier
delta3 += grad3 * multiplier
# apply forces
wp.atomic_sub(delta, i, delta0 * w0 * relaxation)
wp.atomic_sub(delta, j, delta1 * w1 * relaxation)
wp.atomic_sub(delta, k, delta2 * w2 * relaxation)
wp.atomic_sub(delta, l, delta3 * w3 * relaxation)
@wp.kernel
def apply_particle_deltas(
x_orig: wp.array[wp.vec3],
x_pred: wp.array[wp.vec3],
particle_flags: wp.array[wp.int32],
delta: wp.array[wp.vec3],
dt: float,
v_max: float,
x_out: wp.array[wp.vec3],
v_out: wp.array[wp.vec3],
):
tid = wp.tid()
if (particle_flags[tid] & ParticleFlags.ACTIVE) == 0:
return
x0 = x_orig[tid]
xp = x_pred[tid]
# constraint deltas
d = delta[tid]
x_new = xp + d
v_new = (x_new - x0) / dt
# enforce velocity limit to prevent instability
v_new_mag = wp.length(v_new)
if v_new_mag > v_max:
v_new *= v_max / v_new_mag
x_out[tid] = x_new
v_out[tid] = v_new
@wp.kernel
def apply_body_deltas(
q_in: wp.array[wp.transform],
qd_in: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
body_I: wp.array[wp.mat33],
body_inv_m: wp.array[float],
body_inv_I: wp.array[wp.mat33],
deltas: wp.array[wp.spatial_vector],
constraint_inv_weights: wp.array[float],
dt: float,
# outputs
q_out: wp.array[wp.transform],
qd_out: wp.array[wp.spatial_vector],
):
tid = wp.tid()
inv_m = body_inv_m[tid]
if inv_m == 0.0:
q_out[tid] = q_in[tid]
qd_out[tid] = qd_in[tid]
return
inv_I = body_inv_I[tid]
tf = q_in[tid]
delta = deltas[tid]
v0 = wp.spatial_top(qd_in[tid])
w0 = wp.spatial_bottom(qd_in[tid])
p0 = wp.transform_get_translation(tf)
q0 = wp.transform_get_rotation(tf)
weight = 1.0
if constraint_inv_weights:
inv_weight = constraint_inv_weights[tid]
if inv_weight > 0.0:
weight = 1.0 / inv_weight
dp = wp.spatial_top(delta) * (inv_m * weight)
dq = wp.spatial_bottom(delta) * weight
wb = wp.quat_rotate_inv(q0, w0)
dwb = inv_I * wp.quat_rotate_inv(q0, dq)
# coriolis forces delta from dwb = (wb + dwb) I (wb + dwb) - wb I wb
tb = wp.cross(dwb, body_I[tid] * (wb + dwb)) + wp.cross(wb, body_I[tid] * dwb)
dw1 = wp.quat_rotate(q0, dwb - dt * inv_I * tb)
# update orientation
q1 = q0 + 0.5 * wp.quat(dw1 * dt, 0.0) * q0
q1 = wp.normalize(q1)
# update position
com = body_com[tid]
x_com = p0 + wp.quat_rotate(q0, com)
p1 = x_com + dp * dt
p1 -= wp.quat_rotate(q1, com)
q_out[tid] = wp.transform(p1, q1)
# update linear and angular velocity
v1 = v0 + dp
w1 = w0 + dw1
# XXX this improves gradient stability
if wp.length(v1) < 1e-4:
v1 = wp.vec3(0.0)
if wp.length(w1) < 1e-4:
w1 = wp.vec3(0.0)
qd_out[tid] = wp.spatial_vector(v1, w1)
@wp.kernel
def apply_body_delta_velocities(
deltas: wp.array[wp.spatial_vector],
qd_out: wp.array[wp.spatial_vector],
):
tid = wp.tid()
wp.atomic_add(qd_out, tid, deltas[tid])
@wp.kernel
def apply_joint_forces(
body_q: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_qd_start: wp.array[int],
joint_dof_dim: wp.array2d[int],
joint_axis: wp.array[wp.vec3],
joint_f: wp.array[float],
body_f: wp.array[wp.spatial_vector],
):
tid = wp.tid()
type = joint_type[tid]
if not joint_enabled[tid]:
return
if type == JointType.FIXED or type == JointType.CABLE:
return
# rigid body indices of the child and parent
id_c = joint_child[tid]
id_p = joint_parent[tid]
X_pj = joint_X_p[tid]
X_cj = joint_X_c[tid]
X_wp = X_pj
pose_p = X_pj
com_p = wp.vec3(0.0)
# parent transform and moment arm
if id_p >= 0:
pose_p = body_q[id_p]
X_wp = pose_p * X_wp
com_p = body_com[id_p]
r_p = wp.transform_get_translation(X_wp) - wp.transform_point(pose_p, com_p)
# child transform and moment arm
pose_c = body_q[id_c]
X_wc = pose_c * X_cj
com_c = body_com[id_c]
r_c = wp.transform_get_translation(X_wc) - wp.transform_point(pose_c, com_c)
# # local joint rotations
# q_p = wp.transform_get_rotation(X_wp)
# q_c = wp.transform_get_rotation(X_wc)
# joint properties (for 1D joints)
qd_start = joint_qd_start[tid]
lin_axis_count = joint_dof_dim[tid, 0]
ang_axis_count = joint_dof_dim[tid, 1]
# total force/torque on the parent
t_total = wp.vec3()
f_total = wp.vec3()
if type == JointType.FREE or type == JointType.DISTANCE:
f_total = wp.vec3(joint_f[qd_start + 0], joint_f[qd_start + 1], joint_f[qd_start + 2])
t_total = wp.vec3(joint_f[qd_start + 3], joint_f[qd_start + 4], joint_f[qd_start + 5])
# Interpret free-joint forces as spatial wrench at the COM (same as body_f).
# Avoid adding a moment arm that would introduce torque for pure forces.
wp.atomic_add(body_f, id_c, wp.spatial_vector(f_total, t_total))
if id_p >= 0:
wp.atomic_sub(body_f, id_p, wp.spatial_vector(f_total, t_total))
return
elif type == JointType.BALL:
t_total = wp.vec3(joint_f[qd_start + 0], joint_f[qd_start + 1], joint_f[qd_start + 2])
elif type == JointType.REVOLUTE or type == JointType.PRISMATIC or type == JointType.D6:
# unroll for loop to ensure joint actions remain differentiable
# (since differentiating through a dynamic for loop that updates a local variable is not supported)
if lin_axis_count > 0:
axis = joint_axis[qd_start + 0]
f = joint_f[qd_start + 0]
a_p = wp.transform_vector(X_wp, axis)
f_total += f * a_p
if lin_axis_count > 1:
axis = joint_axis[qd_start + 1]
f = joint_f[qd_start + 1]
a_p = wp.transform_vector(X_wp, axis)
f_total += f * a_p
if lin_axis_count > 2:
axis = joint_axis[qd_start + 2]
f = joint_f[qd_start + 2]
a_p = wp.transform_vector(X_wp, axis)
f_total += f * a_p
if ang_axis_count > 0:
axis = joint_axis[qd_start + lin_axis_count + 0]
f = joint_f[qd_start + lin_axis_count + 0]
a_p = wp.transform_vector(X_wp, axis)
t_total += f * a_p
if ang_axis_count > 1:
axis = joint_axis[qd_start + lin_axis_count + 1]
f = joint_f[qd_start + lin_axis_count + 1]
a_p = wp.transform_vector(X_wp, axis)
t_total += f * a_p
if ang_axis_count > 2:
axis = joint_axis[qd_start + lin_axis_count + 2]
f = joint_f[qd_start + lin_axis_count + 2]
a_p = wp.transform_vector(X_wp, axis)
t_total += f * a_p
else:
print("joint type not handled in apply_joint_forces")
# write forces
if id_p >= 0:
wp.atomic_sub(body_f, id_p, wp.spatial_vector(f_total, t_total + wp.cross(r_p, f_total)))
wp.atomic_add(body_f, id_c, wp.spatial_vector(f_total, t_total + wp.cross(r_c, f_total)))
@wp.func
def update_joint_axis_limits(axis: wp.vec3, limit_lower: float, limit_upper: float, input_limits: wp.spatial_vector):
# update the 3D linear/angular limits (spatial_vector [lower, upper]) given the axis vector and limits
lo_temp = axis * limit_lower
up_temp = axis * limit_upper
lo = vec_min(lo_temp, up_temp)
up = vec_max(lo_temp, up_temp)
input_lower = wp.spatial_top(input_limits)
input_upper = wp.spatial_bottom(input_limits)
lower = vec_min(input_lower, lo)
upper = vec_max(input_upper, up)
return wp.spatial_vector(lower, upper)
@wp.func
def update_joint_axis_weighted_target(
axis: wp.vec3, target: float, weight: float, input_target_weight: wp.spatial_vector
):
axis_targets = wp.spatial_top(input_target_weight)
axis_weights = wp.spatial_bottom(input_target_weight)
weighted_axis = axis * weight
axis_targets += weighted_axis * target # weighted target (to be normalized later by sum of weights)
axis_weights += vec_abs(weighted_axis)
return wp.spatial_vector(axis_targets, axis_weights)
@wp.func
def compute_linear_correction_3d(
dx: wp.vec3,
r1: wp.vec3,
r2: wp.vec3,
tf1: wp.transform,
tf2: wp.transform,
m_inv1: float,
m_inv2: float,
I_inv1: wp.mat33,
I_inv2: wp.mat33,
lambda_in: float,
compliance: float,
damping: float,
dt: float,
) -> float:
c = wp.length(dx)
if c == 0.0:
# print("c == 0.0 in positional correction")
return 0.0
n = wp.normalize(dx)
q1 = wp.transform_get_rotation(tf1)
q2 = wp.transform_get_rotation(tf2)
# Eq. 2-3 (make sure to project into the frame of the body)
r1xn = wp.quat_rotate_inv(q1, wp.cross(r1, n))
r2xn = wp.quat_rotate_inv(q2, wp.cross(r2, n))
w1 = m_inv1 + wp.dot(r1xn, I_inv1 * r1xn)
w2 = m_inv2 + wp.dot(r2xn, I_inv2 * r2xn)
w = w1 + w2
if w == 0.0:
return 0.0
alpha = compliance
gamma = compliance * damping
# Eq. 4-5
d_lambda = -c - alpha * lambda_in
# TODO consider damping for velocity correction?
# delta_lambda = -(err + alpha * lambda_in + gamma * derr)
if w + alpha > 0.0:
d_lambda /= w * (dt + gamma) + alpha / dt
return d_lambda
@wp.func
def compute_angular_correction_3d(
corr: wp.vec3,
q1: wp.quat,
q2: wp.quat,
m_inv1: float,
m_inv2: float,
I_inv1: wp.mat33,
I_inv2: wp.mat33,
alpha_tilde: float,
# lambda_prev: float,
relaxation: float,
dt: float,
):
# compute and apply the correction impulse for an angular constraint
theta = wp.length(corr)
if theta == 0.0:
return 0.0
n = wp.normalize(corr)
# project variables to body rest frame as they are in local matrix
n1 = wp.quat_rotate_inv(q1, n)
n2 = wp.quat_rotate_inv(q2, n)
# Eq. 11-12
w1 = wp.dot(n1, I_inv1 * n1)
w2 = wp.dot(n2, I_inv2 * n2)
w = w1 + w2
if w == 0.0:
return 0.0
# Eq. 13-14
lambda_prev = 0.0
d_lambda = (-theta - alpha_tilde * lambda_prev) / (w * dt + alpha_tilde / dt)
# TODO consider lambda_prev?
# p = d_lambda * n * relaxation
# Eq. 15-16
return d_lambda
@wp.kernel
def solve_simple_body_joints(
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
body_inv_m: wp.array[float],
body_inv_I: wp.array[wp.mat33],
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
joint_qd_start: wp.array[int],
joint_dof_dim: wp.array2d[int],
joint_axis: wp.array[wp.vec3],
joint_target: wp.array[float],
joint_target_ke: wp.array[float],
joint_target_kd: wp.array[float],
joint_linear_compliance: float,
joint_angular_compliance: float,
angular_relaxation: float,
linear_relaxation: float,
dt: float,
deltas: wp.array[wp.spatial_vector],
):
tid = wp.tid()
type = joint_type[tid]
if not joint_enabled[tid]:
return
if type == JointType.FREE:
return
if type == JointType.DISTANCE:
return
if type == JointType.D6:
return
# rigid body indices of the child and parent
id_c = joint_child[tid]
id_p = joint_parent[tid]
X_pj = joint_X_p[tid]
X_cj = joint_X_c[tid]
X_wp = X_pj
m_inv_p = 0.0
I_inv_p = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
pose_p = X_pj
com_p = wp.vec3(0.0)
# parent transform and moment arm
if id_p >= 0:
pose_p = body_q[id_p]
X_wp = pose_p * X_wp
com_p = body_com[id_p]
m_inv_p = body_inv_m[id_p]
I_inv_p = body_inv_I[id_p]
r_p = wp.transform_get_translation(X_wp) - wp.transform_point(pose_p, com_p)
# child transform and moment arm
pose_c = body_q[id_c]
X_wc = pose_c * X_cj
com_c = body_com[id_c]
m_inv_c = body_inv_m[id_c]
I_inv_c = body_inv_I[id_c]
r_c = wp.transform_get_translation(X_wc) - wp.transform_point(pose_c, com_c)
if m_inv_p == 0.0 and m_inv_c == 0.0:
# connection between two immovable bodies
return
# accumulate constraint deltas
lin_delta_p = wp.vec3(0.0)
ang_delta_p = wp.vec3(0.0)
lin_delta_c = wp.vec3(0.0)
ang_delta_c = wp.vec3(0.0)
# rel_pose = wp.transform_inverse(X_wp) * X_wc
# rel_p = wp.transform_get_translation(rel_pose)
# joint connection points
# x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
# linear_compliance = joint_linear_compliance
angular_compliance = joint_angular_compliance
damping = 0.0
axis_start = joint_qd_start[tid]
# mode = joint_dof_mode[axis_start]
# local joint rotations
q_p = wp.transform_get_rotation(X_wp)
q_c = wp.transform_get_rotation(X_wc)
inertial_q_p = wp.transform_get_rotation(pose_p)
inertial_q_c = wp.transform_get_rotation(pose_c)
# joint properties (for 1D joints)
axis = joint_axis[axis_start]
if type == JointType.FIXED:
limit_lower = 0.0
limit_upper = 0.0
else:
limit_lower = joint_limit_lower[axis_start]
limit_upper = joint_limit_upper[axis_start]
# linear_alpha_tilde = linear_compliance / dt / dt
angular_alpha_tilde = angular_compliance / dt / dt
# prevent division by zero
# linear_alpha_tilde = wp.max(linear_alpha_tilde, 1e-6)
# angular_alpha_tilde = wp.max(angular_alpha_tilde, 1e-6)
# accumulate constraint deltas
lin_delta_p = wp.vec3(0.0)
ang_delta_p = wp.vec3(0.0)
lin_delta_c = wp.vec3(0.0)
ang_delta_c = wp.vec3(0.0)
# handle angular constraints
if type == JointType.REVOLUTE:
# align joint axes
a_p = wp.quat_rotate(q_p, axis)
a_c = wp.quat_rotate(q_c, axis)
# Eq. 20
corr = wp.cross(a_p, a_c)
ncorr = wp.normalize(corr)
angular_relaxation = 0.2
# angular_correction(
# corr, inertial_q_p, inertial_q_c, m_inv_p, m_inv_c, I_inv_p, I_inv_c,
# angular_alpha_tilde, angular_relaxation, deltas, id_p, id_c)
lambda_n = compute_angular_correction_3d(
corr, inertial_q_p, inertial_q_c, m_inv_p, m_inv_c, I_inv_p, I_inv_c, angular_alpha_tilde, damping, dt
)
lambda_n *= angular_relaxation
ang_delta_p -= lambda_n * ncorr
ang_delta_c += lambda_n * ncorr
# limit joint angles (Alg. 3)
pi = 3.14159265359
two_pi = 2.0 * pi
if limit_lower > -two_pi or limit_upper < two_pi:
# find a perpendicular vector to joint axis
a = axis
# https://math.stackexchange.com/a/3582461
g = wp.sign(a[2])
h = a[2] + g
b = wp.vec3(g - a[0] * a[0] / h, -a[0] * a[1] / h, -a[0])
c = wp.normalize(wp.cross(a, b))
# b = c # TODO verify
# joint axis
n = wp.quat_rotate(q_p, a)
# the axes n1 and n2 are aligned with the two bodies
n1 = wp.quat_rotate(q_p, b)
n2 = wp.quat_rotate(q_c, b)
phi = wp.asin(wp.dot(wp.cross(n1, n2), n))
# print("phi")
# print(phi)
if wp.dot(n1, n2) < 0.0:
phi = pi - phi
if phi > pi:
phi -= two_pi
if phi < -pi:
phi += two_pi
if phi < limit_lower or phi > limit_upper:
phi = wp.clamp(phi, limit_lower, limit_upper)
# print("clamped phi")
# print(phi)
# rot = wp.quat(phi, n[0], n[1], n[2])
# rot = wp.quat(n, phi)
rot = wp.quat_from_axis_angle(n, phi)
n1 = wp.quat_rotate(rot, n1)
corr = wp.cross(n1, n2)
# print("corr")
# print(corr)
# TODO expose
# angular_alpha_tilde = 0.0001 / dt / dt
# angular_relaxation = 0.5
# TODO fix this constraint
# angular_correction(
# corr, inertial_q_p, inertial_q_c, m_inv_p, m_inv_c, I_inv_p, I_inv_c,
# angular_alpha_tilde, angular_relaxation, deltas, id_p, id_c)
lambda_n = compute_angular_correction_3d(
corr,
inertial_q_p,
inertial_q_c,
m_inv_p,
m_inv_c,
I_inv_p,
I_inv_c,
angular_alpha_tilde,
damping,
dt,
)
lambda_n *= angular_relaxation
ncorr = wp.normalize(corr)
ang_delta_p -= lambda_n * ncorr
ang_delta_c += lambda_n * ncorr
# handle joint targets
target_ke = joint_target_ke[axis_start]
# target_kd = joint_target_kd[axis_start]
target = joint_target[axis_start]
if target_ke > 0.0:
# find a perpendicular vector to joint axis
a = axis
# https://math.stackexchange.com/a/3582461
g = wp.sign(a[2])
h = a[2] + g
b = wp.vec3(g - a[0] * a[0] / h, -a[0] * a[1] / h, -a[0])
c = wp.normalize(wp.cross(a, b))
b = c
q = wp.quat_from_axis_angle(a_p, target)
b_target = wp.quat_rotate(q, wp.quat_rotate(q_p, b))
b2 = wp.quat_rotate(q_c, b)
# Eq. 21
d_target = wp.cross(b_target, b2)
target_compliance = 1.0 / target_ke # / dt / dt
# angular_correction(
# d_target, inertial_q_p, inertial_q_c, m_inv_p, m_inv_c, I_inv_p, I_inv_c,
# target_compliance, angular_relaxation, deltas, id_p, id_c)
lambda_n = compute_angular_correction_3d(
d_target, inertial_q_p, inertial_q_c, m_inv_p, m_inv_c, I_inv_p, I_inv_c, target_compliance, damping, dt
)
lambda_n *= angular_relaxation
ncorr = wp.normalize(d_target)
# TODO fix
ang_delta_p -= lambda_n * ncorr
ang_delta_c += lambda_n * ncorr
if (type == JointType.FIXED) or (type == JointType.PRISMATIC):
# align the mutual orientations of the two bodies
# Eq. 18-19
q = q_p * wp.quat_inverse(q_c)
corr = -2.0 * wp.vec3(q[0], q[1], q[2])
# angular_correction(
# -corr, inertial_q_p, inertial_q_c, m_inv_p, m_inv_c, I_inv_p, I_inv_c,
# angular_alpha_tilde, angular_relaxation, deltas, id_p, id_c)
lambda_n = compute_angular_correction_3d(
corr, inertial_q_p, inertial_q_c, m_inv_p, m_inv_c, I_inv_p, I_inv_c, angular_alpha_tilde, damping, dt
)
lambda_n *= angular_relaxation
ncorr = wp.normalize(corr)
ang_delta_p -= lambda_n * ncorr
ang_delta_c += lambda_n * ncorr
# handle positional constraints
# joint connection points
x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
# compute error between the joint attachment points on both bodies
# delta x is the difference of point r_2 minus point r_1 (Fig. 3)
dx = x_c - x_p
# rotate the error vector into the joint frame
q_dx = q_p
# q_dx = q_c
# q_dx = wp.transform_get_rotation(pose_p)
dx = wp.quat_rotate_inv(q_dx, dx)
lower_pos_limits = wp.vec3(0.0)
upper_pos_limits = wp.vec3(0.0)
if type == JointType.PRISMATIC:
lower_pos_limits = axis * limit_lower
upper_pos_limits = axis * limit_upper
# compute linear constraint violations
corr = wp.vec3(0.0)
zero = wp.vec3(0.0)
corr -= vec_leaky_min(zero, upper_pos_limits - dx)
corr -= vec_leaky_max(zero, lower_pos_limits - dx)
# if (type == JointType.PRISMATIC):
# if mode == JointMode.TARGET_POSITION:
# target = wp.clamp(target, limit_lower, limit_upper)
# if target_ke > 0.0:
# err = dx - target * axis
# compliance = 1.0 / target_ke
# damping = axis_damping[dim]
# elif mode == JointMode.TARGET_VELOCITY:
# if target_ke > 0.0:
# err = (derr - target) * dt
# compliance = 1.0 / target_ke
# damping = axis_damping[dim]
# rotate correction vector into world frame
corr = wp.quat_rotate(q_dx, corr)
lambda_in = 0.0
linear_alpha = joint_linear_compliance
lambda_n = compute_linear_correction_3d(
corr, r_p, r_c, pose_p, pose_c, m_inv_p, m_inv_c, I_inv_p, I_inv_c, lambda_in, linear_alpha, damping, dt
)
lambda_n *= linear_relaxation
n = wp.normalize(corr)
lin_delta_p -= n * lambda_n
lin_delta_c += n * lambda_n
ang_delta_p -= wp.cross(r_p, n) * lambda_n
ang_delta_c += wp.cross(r_c, n) * lambda_n
if id_p >= 0:
wp.atomic_add(deltas, id_p, wp.spatial_vector(lin_delta_p, ang_delta_p))
if id_c >= 0:
wp.atomic_add(deltas, id_c, wp.spatial_vector(lin_delta_c, ang_delta_c))
@wp.kernel
def solve_body_joints(
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
body_inv_m: wp.array[float],
body_inv_I: wp.array[wp.mat33],
joint_type: wp.array[int],
joint_enabled: wp.array[bool],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_X_p: wp.array[wp.transform],
joint_X_c: wp.array[wp.transform],
joint_limit_lower: wp.array[float],
joint_limit_upper: wp.array[float],
joint_qd_start: wp.array[int],
joint_dof_dim: wp.array2d[int],
joint_axis: wp.array[wp.vec3],
joint_target_pos: wp.array[float],
joint_target_vel: wp.array[float],
joint_target_ke: wp.array[float],
joint_target_kd: wp.array[float],
joint_linear_compliance: float,
joint_angular_compliance: float,
angular_relaxation: float,
linear_relaxation: float,
dt: float,
deltas: wp.array[wp.spatial_vector],
):
tid = wp.tid()
type = joint_type[tid]
if not joint_enabled[tid]:
return
if type == JointType.FREE:
return
# if type == JointType.FIXED:
# return
# if type == JointType.REVOLUTE:
# return
# if type == JointType.PRISMATIC:
# return
# if type == JointType.BALL:
# return
# rigid body indices of the child and parent
id_c = joint_child[tid]
id_p = joint_parent[tid]
X_pj = joint_X_p[tid]
X_cj = joint_X_c[tid]
X_wp = X_pj
m_inv_p = 0.0
I_inv_p = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
pose_p = X_pj
com_p = wp.vec3(0.0)
vel_p = wp.vec3(0.0)
omega_p = wp.vec3(0.0)
# parent transform and moment arm
if id_p >= 0:
pose_p = body_q[id_p]
X_wp = pose_p * X_wp
com_p = body_com[id_p]
m_inv_p = body_inv_m[id_p]
I_inv_p = body_inv_I[id_p]
vel_p = wp.spatial_top(body_qd[id_p])
omega_p = wp.spatial_bottom(body_qd[id_p])
# child transform and moment arm
pose_c = body_q[id_c]
X_wc = pose_c * X_cj
com_c = body_com[id_c]
m_inv_c = body_inv_m[id_c]
I_inv_c = body_inv_I[id_c]
vel_c = wp.spatial_top(body_qd[id_c])
omega_c = wp.spatial_bottom(body_qd[id_c])
if m_inv_p == 0.0 and m_inv_c == 0.0:
# connection between two immovable bodies
return
# accumulate constraint deltas
lin_delta_p = wp.vec3(0.0)
ang_delta_p = wp.vec3(0.0)
lin_delta_c = wp.vec3(0.0)
ang_delta_c = wp.vec3(0.0)
rel_pose = wp.transform_inverse(X_wp) * X_wc
rel_p = wp.transform_get_translation(rel_pose)
# joint connection points
# x_p = wp.transform_get_translation(X_wp)
x_c = wp.transform_get_translation(X_wc)
linear_compliance = joint_linear_compliance
angular_compliance = joint_angular_compliance
axis_start = joint_qd_start[tid]
lin_axis_count = joint_dof_dim[tid, 0]
ang_axis_count = joint_dof_dim[tid, 1]
world_com_p = wp.transform_point(pose_p, com_p)
world_com_c = wp.transform_point(pose_c, com_c)
# handle positional constraints
if type == JointType.DISTANCE:
r_p = wp.transform_get_translation(X_wp) - world_com_p
r_c = wp.transform_get_translation(X_wc) - world_com_c
lower = joint_limit_lower[axis_start]
upper = joint_limit_upper[axis_start]
if lower < 0.0 and upper < 0.0:
# no limits
return
d = wp.length(rel_p)
err = 0.0
if lower >= 0.0 and d < lower:
err = d - lower
# use a more descriptive direction vector for the constraint
# in case the joint parent and child anchors are very close
rel_p = err * wp.normalize(world_com_c - world_com_p)
elif upper >= 0.0 and d > upper:
err = d - upper
if wp.abs(err) > 1e-9:
# compute gradients
linear_c = rel_p
linear_p = -linear_c
r_c = x_c - world_com_c
angular_p = -wp.cross(r_p, linear_c)
angular_c = wp.cross(r_c, linear_c)
# constraint time derivative
derr = (
wp.dot(linear_p, vel_p)
+ wp.dot(linear_c, vel_c)
+ wp.dot(angular_p, omega_p)
+ wp.dot(angular_c, omega_c)
)
lambda_in = 0.0
compliance = linear_compliance
ke = joint_target_ke[axis_start]
if ke > 0.0:
compliance = 1.0 / ke
damping = joint_target_kd[axis_start]
d_lambda = compute_positional_correction(
err,
derr,
pose_p,
pose_c,
m_inv_p,
m_inv_c,
I_inv_p,
I_inv_c,
linear_p,
linear_c,
angular_p,
angular_c,
lambda_in,
compliance,
damping,
dt,
)
lin_delta_p += linear_p * (d_lambda * linear_relaxation)
ang_delta_p += angular_p * (d_lambda * angular_relaxation)
lin_delta_c += linear_c * (d_lambda * linear_relaxation)
ang_delta_c += angular_c * (d_lambda * angular_relaxation)
else:
# compute joint target, stiffness, damping
axis_limits = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
axis_target_pos_ke = wp.spatial_vector()
axis_target_vel_kd = wp.spatial_vector()
# avoid a for loop here since local variables would need to be modified which is not yet differentiable
if lin_axis_count > 0:
axis = joint_axis[axis_start]
lo_temp = axis * joint_limit_lower[axis_start]
up_temp = axis * joint_limit_upper[axis_start]
axis_limits = wp.spatial_vector(vec_min(lo_temp, up_temp), vec_max(lo_temp, up_temp))
ke = joint_target_ke[axis_start]
kd = joint_target_kd[axis_start]
target_pos = joint_target_pos[axis_start]
target_vel = joint_target_vel[axis_start]
if ke > 0.0: # has position control
axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke)
if kd > 0.0: # has velocity control
axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd)
if lin_axis_count > 1:
axis_idx = axis_start + 1
axis = joint_axis[axis_idx]
lower = joint_limit_lower[axis_idx]
upper = joint_limit_upper[axis_idx]
axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits)
ke = joint_target_ke[axis_idx]
kd = joint_target_kd[axis_idx]
target_pos = joint_target_pos[axis_idx]
target_vel = joint_target_vel[axis_idx]
if ke > 0.0: # has position control
axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke)
if kd > 0.0: # has velocity control
axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd)
if lin_axis_count > 2:
axis_idx = axis_start + 2
axis = joint_axis[axis_idx]
lower = joint_limit_lower[axis_idx]
upper = joint_limit_upper[axis_idx]
axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits)
ke = joint_target_ke[axis_idx]
kd = joint_target_kd[axis_idx]
target_pos = joint_target_pos[axis_idx]
target_vel = joint_target_vel[axis_idx]
if ke > 0.0: # has position control
axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke)
if kd > 0.0: # has velocity control
axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd)
axis_target_pos = wp.spatial_top(axis_target_pos_ke)
axis_stiffness = wp.spatial_bottom(axis_target_pos_ke)
axis_target_vel = wp.spatial_top(axis_target_vel_kd)
axis_damping = wp.spatial_bottom(axis_target_vel_kd)
for i in range(3):
if axis_stiffness[i] > 0.0:
axis_target_pos[i] /= axis_stiffness[i]
for i in range(3):
if axis_damping[i] > 0.0:
axis_target_vel[i] /= axis_damping[i]
axis_limits_lower = wp.spatial_top(axis_limits)
axis_limits_upper = wp.spatial_bottom(axis_limits)
frame_p = wp.quat_to_matrix(wp.transform_get_rotation(X_wp))
# note that x_c appearing in both is correct
r_p = x_c - world_com_p
r_c = x_c - wp.transform_point(pose_c, com_c)
# for loop will be unrolled, so we can modify local variables
for dim in range(3):
e = rel_p[dim]
# compute gradients
linear_c = wp.vec3(frame_p[0, dim], frame_p[1, dim], frame_p[2, dim])
linear_p = -linear_c
angular_p = -wp.cross(r_p, linear_c)
angular_c = wp.cross(r_c, linear_c)
# constraint time derivative
derr = (
wp.dot(linear_p, vel_p)
+ wp.dot(linear_c, vel_c)
+ wp.dot(angular_p, omega_p)
+ wp.dot(angular_c, omega_c)
)
err = 0.0
compliance = linear_compliance
damping = 0.0
target_vel = axis_target_vel[dim]
derr_rel = derr - target_vel
# consider joint limits irrespective of axis mode
lower = axis_limits_lower[dim]
upper = axis_limits_upper[dim]
if e < lower:
err = e - lower
elif e > upper:
err = e - upper
else:
target_pos = axis_target_pos[dim]
target_pos = wp.clamp(target_pos, lower, upper)
if axis_stiffness[dim] > 0.0:
err = e - target_pos
compliance = 1.0 / axis_stiffness[dim]
damping = axis_damping[dim]
elif axis_damping[dim] > 0.0:
compliance = 1.0 / axis_damping[dim]
damping = axis_damping[dim]
if wp.abs(err) > 1e-9 or wp.abs(derr_rel) > 1e-9:
lambda_in = 0.0
d_lambda = compute_positional_correction(
err,
derr_rel,
pose_p,
pose_c,
m_inv_p,
m_inv_c,
I_inv_p,
I_inv_c,
linear_p,
linear_c,
angular_p,
angular_c,
lambda_in,
compliance,
damping,
dt,
)
lin_delta_p += linear_p * (d_lambda * linear_relaxation)
ang_delta_p += angular_p * (d_lambda * angular_relaxation)
lin_delta_c += linear_c * (d_lambda * linear_relaxation)
ang_delta_c += angular_c * (d_lambda * angular_relaxation)
if type == JointType.FIXED or type == JointType.PRISMATIC or type == JointType.REVOLUTE or type == JointType.D6:
# handle angular constraints
# local joint rotations
q_p = wp.transform_get_rotation(X_wp)
q_c = wp.transform_get_rotation(X_wc)
# make quats lie in same hemisphere
if wp.dot(q_p, q_c) < 0.0:
q_c *= -1.0
rel_q = wp.quat_inverse(q_p) * q_c
qtwist = wp.normalize(wp.quat(rel_q[0], 0.0, 0.0, rel_q[3]))
qswing = rel_q * wp.quat_inverse(qtwist)
# decompose to a compound rotation each axis
s = wp.sqrt(rel_q[0] * rel_q[0] + rel_q[3] * rel_q[3])
invs = 1.0 / s
invscube = invs * invs * invs
# handle axis-angle joints
# rescale twist from quaternion space to angular
err_0 = 2.0 * wp.asin(wp.clamp(qtwist[0], -1.0, 1.0))
err_1 = qswing[1]
err_2 = qswing[2]
# analytic gradients of swing-twist decomposition
grad_0 = wp.quat(invs - rel_q[0] * rel_q[0] * invscube, 0.0, 0.0, -(rel_q[3] * rel_q[0]) * invscube)
grad_1 = wp.quat(
-rel_q[3] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube,
rel_q[3] * invs,
-rel_q[0] * invs,
rel_q[0] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube,
)
grad_2 = wp.quat(
rel_q[3] * (rel_q[3] * rel_q[1] - rel_q[0] * rel_q[2]) * invscube,
rel_q[0] * invs,
rel_q[3] * invs,
rel_q[0] * (rel_q[2] * rel_q[0] - rel_q[3] * rel_q[1]) * invscube,
)
grad_0 *= 2.0 / wp.abs(qtwist[3])
# grad_0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2)
# rescale swing
swing_sq = qswing[3] * qswing[3]
# if swing axis magnitude close to zero vector, just treat in quaternion space
angularEps = 1.0e-4
if swing_sq + angularEps < 1.0:
d = wp.sqrt(1.0 - qswing[3] * qswing[3])
theta = 2.0 * wp.acos(wp.clamp(qswing[3], -1.0, 1.0))
scale = theta / d
err_1 *= scale
err_2 *= scale
grad_1 *= scale
grad_2 *= scale
errs = wp.vec3(err_0, err_1, err_2)
grad_x = wp.vec3(grad_0[0], grad_1[0], grad_2[0])
grad_y = wp.vec3(grad_0[1], grad_1[1], grad_2[1])
grad_z = wp.vec3(grad_0[2], grad_1[2], grad_2[2])
grad_w = wp.vec3(grad_0[3], grad_1[3], grad_2[3])
# compute joint target, stiffness, damping
axis_limits = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
axis_target_pos_ke = wp.spatial_vector() # [weighted_target_pos, ke_weights]
axis_target_vel_kd = wp.spatial_vector() # [weighted_target_vel, kd_weights]
# avoid a for loop here since local variables would need to be modified which is not yet differentiable
if ang_axis_count > 0:
axis_idx = axis_start + lin_axis_count
axis = joint_axis[axis_idx]
lo_temp = axis * joint_limit_lower[axis_idx]
up_temp = axis * joint_limit_upper[axis_idx]
axis_limits = wp.spatial_vector(vec_min(lo_temp, up_temp), vec_max(lo_temp, up_temp))
ke = joint_target_ke[axis_idx]
kd = joint_target_kd[axis_idx]
target_pos = joint_target_pos[axis_idx]
target_vel = joint_target_vel[axis_idx]
if ke > 0.0: # has position control
axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke)
if kd > 0.0: # has velocity control
axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd)
if ang_axis_count > 1:
axis_idx = axis_start + lin_axis_count + 1
axis = joint_axis[axis_idx]
lower = joint_limit_lower[axis_idx]
upper = joint_limit_upper[axis_idx]
axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits)
ke = joint_target_ke[axis_idx]
kd = joint_target_kd[axis_idx]
target_pos = joint_target_pos[axis_idx]
target_vel = joint_target_vel[axis_idx]
if ke > 0.0: # has position control
axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke)
if kd > 0.0: # has velocity control
axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd)
if ang_axis_count > 2:
axis_idx = axis_start + lin_axis_count + 2
axis = joint_axis[axis_idx]
lower = joint_limit_lower[axis_idx]
upper = joint_limit_upper[axis_idx]
axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits)
ke = joint_target_ke[axis_idx]
kd = joint_target_kd[axis_idx]
target_pos = joint_target_pos[axis_idx]
target_vel = joint_target_vel[axis_idx]
if ke > 0.0: # has position control
axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke)
if kd > 0.0: # has velocity control
axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd)
axis_target_pos = wp.spatial_top(axis_target_pos_ke)
axis_stiffness = wp.spatial_bottom(axis_target_pos_ke)
axis_target_vel = wp.spatial_top(axis_target_vel_kd)
axis_damping = wp.spatial_bottom(axis_target_vel_kd)
for i in range(3):
if axis_stiffness[i] > 0.0:
axis_target_pos[i] /= axis_stiffness[i]
for i in range(3):
if axis_damping[i] > 0.0:
axis_target_vel[i] /= axis_damping[i]
axis_limits_lower = wp.spatial_top(axis_limits)
axis_limits_upper = wp.spatial_bottom(axis_limits)
# if type == JointType.D6:
# wp.printf("axis_target: %f %f %f\t axis_stiffness: %f %f %f\t axis_damping: %f %f %f\t axis_limits_lower: %f %f %f \t axis_limits_upper: %f %f %f\n",
# axis_target[0], axis_target[1], axis_target[2],
# axis_stiffness[0], axis_stiffness[1], axis_stiffness[2],
# axis_damping[0], axis_damping[1], axis_damping[2],
# axis_limits_lower[0], axis_limits_lower[1], axis_limits_lower[2],
# axis_limits_upper[0], axis_limits_upper[1], axis_limits_upper[2])
# # wp.printf("wp.sqrt(1.0-qtwist[0]*qtwist[0]) = %f\n", wp.sqrt(1.0-qtwist[0]*qtwist[0]))
for dim in range(3):
e = errs[dim]
# analytic gradients of swing-twist decomposition
grad = wp.quat(grad_x[dim], grad_y[dim], grad_z[dim], grad_w[dim])
quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c)
angular_c = wp.vec3(quat_c[0], quat_c[1], quat_c[2])
angular_p = -angular_c
# time derivative of the constraint
derr = wp.dot(angular_p, omega_p) + wp.dot(angular_c, omega_c)
err = 0.0
compliance = angular_compliance
damping = 0.0
target_vel = axis_target_vel[dim]
angular_c_len = wp.length(angular_c)
derr_rel = derr - target_vel * angular_c_len
# consider joint limits irrespective of mode
lower = axis_limits_lower[dim]
upper = axis_limits_upper[dim]
if e < lower:
err = e - lower
elif e > upper:
err = e - upper
else:
target_pos = axis_target_pos[dim]
target_pos = wp.clamp(target_pos, lower, upper)
if axis_stiffness[dim] > 0.0:
err = e - target_pos
compliance = 1.0 / axis_stiffness[dim]
damping = axis_damping[dim]
elif axis_damping[dim] > 0.0:
damping = axis_damping[dim]
compliance = 1.0 / axis_damping[dim]
d_lambda = (
compute_angular_correction(
err, derr_rel, pose_p, pose_c, I_inv_p, I_inv_c, angular_p, angular_c, 0.0, compliance, damping, dt
)
* angular_relaxation
)
# update deltas
ang_delta_p += angular_p * d_lambda
ang_delta_c += angular_c * d_lambda
if id_p >= 0:
wp.atomic_add(deltas, id_p, wp.spatial_vector(lin_delta_p, ang_delta_p))
if id_c >= 0:
wp.atomic_add(deltas, id_c, wp.spatial_vector(lin_delta_c, ang_delta_c))
@wp.func
def compute_contact_constraint_delta(
err: float,
tf_a: wp.transform,
tf_b: wp.transform,
m_inv_a: float,
m_inv_b: float,
I_inv_a: wp.mat33,
I_inv_b: wp.mat33,
linear_a: wp.vec3,
linear_b: wp.vec3,
angular_a: wp.vec3,
angular_b: wp.vec3,
relaxation: float,
dt: float,
) -> float:
denom = 0.0
denom += wp.length_sq(linear_a) * m_inv_a
denom += wp.length_sq(linear_b) * m_inv_b
q1 = wp.transform_get_rotation(tf_a)
q2 = wp.transform_get_rotation(tf_b)
# Eq. 2-3 (make sure to project into the frame of the body)
rot_angular_a = wp.quat_rotate_inv(q1, angular_a)
rot_angular_b = wp.quat_rotate_inv(q2, angular_b)
denom += wp.dot(rot_angular_a, I_inv_a * rot_angular_a)
denom += wp.dot(rot_angular_b, I_inv_b * rot_angular_b)
delta_lambda = -err
if denom > 0.0:
delta_lambda /= dt * denom
return delta_lambda * relaxation
@wp.func
def compute_positional_correction(
err: float,
derr: float,
tf_a: wp.transform,
tf_b: wp.transform,
m_inv_a: float,
m_inv_b: float,
I_inv_a: wp.mat33,
I_inv_b: wp.mat33,
linear_a: wp.vec3,
linear_b: wp.vec3,
angular_a: wp.vec3,
angular_b: wp.vec3,
lambda_in: float,
compliance: float,
damping: float,
dt: float,
) -> float:
denom = 0.0
denom += wp.length_sq(linear_a) * m_inv_a
denom += wp.length_sq(linear_b) * m_inv_b
q1 = wp.transform_get_rotation(tf_a)
q2 = wp.transform_get_rotation(tf_b)
# Eq. 2-3 (make sure to project into the frame of the body)
rot_angular_a = wp.quat_rotate_inv(q1, angular_a)
rot_angular_b = wp.quat_rotate_inv(q2, angular_b)
denom += wp.dot(rot_angular_a, I_inv_a * rot_angular_a)
denom += wp.dot(rot_angular_b, I_inv_b * rot_angular_b)
alpha = compliance
gamma = compliance * damping
delta_lambda = -(err + alpha * lambda_in + gamma * derr)
if denom + alpha > 0.0:
delta_lambda /= (dt + gamma) * denom + alpha / dt
return delta_lambda
@wp.func
def compute_angular_correction(
err: float,
derr: float,
tf_a: wp.transform,
tf_b: wp.transform,
I_inv_a: wp.mat33,
I_inv_b: wp.mat33,
angular_a: wp.vec3,
angular_b: wp.vec3,
lambda_in: float,
compliance: float,
damping: float,
dt: float,
) -> float:
denom = 0.0
q1 = wp.transform_get_rotation(tf_a)
q2 = wp.transform_get_rotation(tf_b)
# Eq. 2-3 (make sure to project into the frame of the body)
rot_angular_a = wp.quat_rotate_inv(q1, angular_a)
rot_angular_b = wp.quat_rotate_inv(q2, angular_b)
denom += wp.dot(rot_angular_a, I_inv_a * rot_angular_a)
denom += wp.dot(rot_angular_b, I_inv_b * rot_angular_b)
alpha = compliance
gamma = compliance * damping
delta_lambda = -(err + alpha * lambda_in + gamma * derr)
if denom + alpha > 0.0:
delta_lambda /= (dt + gamma) * denom + alpha / dt
return delta_lambda
@wp.kernel
def solve_body_contact_positions(
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_flags: wp.array[wp.int32],
body_com: wp.array[wp.vec3],
body_m_inv: wp.array[float],
body_I_inv: wp.array[wp.mat33],
shape_body: wp.array[int],
contact_count: wp.array[int],
contact_point0: wp.array[wp.vec3],
contact_point1: wp.array[wp.vec3],
contact_offset0: wp.array[wp.vec3],
contact_offset1: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
contact_thickness0: wp.array[float],
contact_thickness1: wp.array[float],
contact_shape0: wp.array[int],
contact_shape1: wp.array[int],
shape_material_mu: wp.array[float],
shape_material_mu_torsional: wp.array[float],
shape_material_mu_rolling: wp.array[float],
relaxation: float,
dt: float,
# outputs
deltas: wp.array[wp.spatial_vector],
contact_inv_weight: wp.array[float],
):
tid = wp.tid()
count = contact_count[0]
if tid >= count:
return
shape_a = contact_shape0[tid]
shape_b = contact_shape1[tid]
if shape_a == shape_b:
return
body_a = -1
if shape_a >= 0:
body_a = shape_body[shape_a]
body_b = -1
if shape_b >= 0:
body_b = shape_body[shape_b]
if body_a == body_b:
return
# find body to world transform
X_wb_a = wp.transform_identity()
X_wb_b = wp.transform_identity()
if body_a >= 0:
X_wb_a = body_q[body_a]
if body_b >= 0:
X_wb_b = body_q[body_b]
# compute body position in world space
bx_a = wp.transform_point(X_wb_a, contact_point0[tid])
bx_b = wp.transform_point(X_wb_b, contact_point1[tid])
thickness = contact_thickness0[tid] + contact_thickness1[tid]
n = contact_normal[tid]
d = wp.dot(n, bx_b - bx_a) - thickness
if d >= 0.0:
return
m_inv_a = 0.0
m_inv_b = 0.0
I_inv_a = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
I_inv_b = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
# center of mass in body frame
com_a = wp.vec3(0.0)
com_b = wp.vec3(0.0)
# body to world transform
X_wb_a = wp.transform_identity()
X_wb_b = wp.transform_identity()
# angular velocities
omega_a = wp.vec3(0.0)
omega_b = wp.vec3(0.0)
# contact offset in body frame
offset_a = contact_offset0[tid]
offset_b = contact_offset1[tid]
if body_a >= 0:
X_wb_a = body_q[body_a]
com_a = body_com[body_a]
m_inv_a = body_m_inv[body_a]
I_inv_a = body_I_inv[body_a]
omega_a = wp.spatial_bottom(body_qd[body_a])
if body_b >= 0:
X_wb_b = body_q[body_b]
com_b = body_com[body_b]
m_inv_b = body_m_inv[body_b]
I_inv_b = body_I_inv[body_b]
omega_b = wp.spatial_bottom(body_qd[body_b])
# use average contact material properties
mat_nonzero = 0
mu = 0.0
mu_torsional = 0.0
mu_rolling = 0.0
if shape_a >= 0:
mat_nonzero += 1
mu += shape_material_mu[shape_a]
mu_torsional += shape_material_mu_torsional[shape_a]
mu_rolling += shape_material_mu_rolling[shape_a]
if shape_b >= 0:
mat_nonzero += 1
mu += shape_material_mu[shape_b]
mu_torsional += shape_material_mu_torsional[shape_b]
mu_rolling += shape_material_mu_rolling[shape_b]
if mat_nonzero > 0:
mu /= float(mat_nonzero)
mu_torsional /= float(mat_nonzero)
mu_rolling /= float(mat_nonzero)
r_a = bx_a - wp.transform_point(X_wb_a, com_a)
r_b = bx_b - wp.transform_point(X_wb_b, com_b)
angular_a = -wp.cross(r_a, n)
angular_b = wp.cross(r_b, n)
if contact_inv_weight:
if body_a >= 0:
wp.atomic_add(contact_inv_weight, body_a, 1.0)
if body_b >= 0:
wp.atomic_add(contact_inv_weight, body_b, 1.0)
lambda_n = compute_contact_constraint_delta(
d, X_wb_a, X_wb_b, m_inv_a, m_inv_b, I_inv_a, I_inv_b, -n, n, angular_a, angular_b, relaxation, dt
)
lin_delta_a = -n * lambda_n
lin_delta_b = n * lambda_n
ang_delta_a = angular_a * lambda_n
ang_delta_b = angular_b * lambda_n
# linear friction
if mu > 0.0:
# add on displacement from surface offsets, this ensures we include any rotational effects due to thickness from feature
# need to use the current rotation to account for friction due to angular effects (e.g.: slipping contact)
bx_a += wp.transform_vector(X_wb_a, offset_a)
bx_b += wp.transform_vector(X_wb_b, offset_b)
# update delta
delta = bx_b - bx_a
friction_delta = delta - wp.dot(n, delta) * n
r_a = bx_a - wp.transform_point(X_wb_a, com_a)
r_b = bx_b - wp.transform_point(X_wb_b, com_b)
# Add only prescribed kinematic surface motion here.
# Dynamic-body tangential motion is already reflected in the
# positional slip `delta`; adding full relative velocity would
# double-count ordinary ground friction and destabilize contacts.
rel_v_kin_t = wp.vec3(0.0)
if body_a >= 0 and (body_flags[body_a] & int(BodyFlags.KINEMATIC)) != 0:
v_a = velocity_at_point(body_qd[body_a], r_a)
rel_v_kin_t = rel_v_kin_t - (v_a - wp.dot(n, v_a) * n)
if body_b >= 0 and (body_flags[body_b] & int(BodyFlags.KINEMATIC)) != 0:
v_b = velocity_at_point(body_qd[body_b], r_b)
rel_v_kin_t = rel_v_kin_t + (v_b - wp.dot(n, v_b) * n)
friction_delta += rel_v_kin_t * dt
perp = wp.normalize(friction_delta)
angular_a = -wp.cross(r_a, perp)
angular_b = wp.cross(r_b, perp)
err = wp.length(friction_delta)
if err > 0.0:
lambda_fr = compute_contact_constraint_delta(
err,
X_wb_a,
X_wb_b,
m_inv_a,
m_inv_b,
I_inv_a,
I_inv_b,
-perp,
perp,
angular_a,
angular_b,
relaxation,
dt,
)
# limit friction based on incremental normal force, good approximation to limiting on total force
lambda_fr = wp.max(lambda_fr, -lambda_n * mu)
lin_delta_a -= perp * lambda_fr
lin_delta_b += perp * lambda_fr
ang_delta_a += angular_a * lambda_fr
ang_delta_b += angular_b * lambda_fr
delta_omega = omega_b - omega_a
if mu_torsional > 0.0:
err = wp.dot(delta_omega, n) * dt
if wp.abs(err) > 0.0:
lin = wp.vec3(0.0)
lambda_torsion = compute_contact_constraint_delta(
err, X_wb_a, X_wb_b, m_inv_a, m_inv_b, I_inv_a, I_inv_b, lin, lin, -n, n, relaxation, dt
)
lambda_torsion = wp.clamp(lambda_torsion, -lambda_n * mu_torsional, lambda_n * mu_torsional)
ang_delta_a -= n * lambda_torsion
ang_delta_b += n * lambda_torsion
if mu_rolling > 0.0:
delta_omega -= wp.dot(n, delta_omega) * n
err = wp.length(delta_omega) * dt
if err > 0.0:
lin = wp.vec3(0.0)
roll_n = wp.normalize(delta_omega)
lambda_roll = compute_contact_constraint_delta(
err, X_wb_a, X_wb_b, m_inv_a, m_inv_b, I_inv_a, I_inv_b, lin, lin, -roll_n, roll_n, relaxation, dt
)
lambda_roll = wp.max(lambda_roll, -lambda_n * mu_rolling)
ang_delta_a -= roll_n * lambda_roll
ang_delta_b += roll_n * lambda_roll
if body_a >= 0:
wp.atomic_add(deltas, body_a, wp.spatial_vector(lin_delta_a, ang_delta_a))
if body_b >= 0:
wp.atomic_add(deltas, body_b, wp.spatial_vector(lin_delta_b, ang_delta_b))
@wp.kernel
def update_body_velocities(
poses: wp.array[wp.transform],
poses_prev: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
dt: float,
qd_out: wp.array[wp.spatial_vector],
):
tid = wp.tid()
pose = poses[tid]
pose_prev = poses_prev[tid]
x = wp.transform_get_translation(pose)
x_prev = wp.transform_get_translation(pose_prev)
q = wp.transform_get_rotation(pose)
q_prev = wp.transform_get_rotation(pose_prev)
# Update body velocities according to Alg. 2
# XXX we consider the body COM as the origin of the body frame
x_com = x + wp.quat_rotate(q, body_com[tid])
x_com_prev = x_prev + wp.quat_rotate(q_prev, body_com[tid])
# XXX consider the velocity of the COM
v = (x_com - x_com_prev) / dt
dq = q * wp.quat_inverse(q_prev)
omega = 2.0 / dt * wp.vec3(dq[0], dq[1], dq[2])
if dq[3] < 0.0:
omega = -omega
qd_out[tid] = wp.spatial_vector(v, omega)
@wp.kernel
def apply_rigid_restitution(
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_q_prev: wp.array[wp.transform],
body_qd_prev: wp.array[wp.spatial_vector],
body_com: wp.array[wp.vec3],
body_m_inv: wp.array[float],
body_I_inv: wp.array[wp.mat33],
body_world: wp.array[wp.int32],
shape_body: wp.array[int],
contact_count: wp.array[int],
contact_normal: wp.array[wp.vec3],
contact_shape0: wp.array[int],
contact_shape1: wp.array[int],
shape_material_restitution: wp.array[float],
contact_point0: wp.array[wp.vec3],
contact_point1: wp.array[wp.vec3],
contact_offset0: wp.array[wp.vec3],
contact_offset1: wp.array[wp.vec3],
contact_thickness0: wp.array[float],
contact_thickness1: wp.array[float],
contact_inv_weight: wp.array[float],
gravity: wp.array[wp.vec3],
dt: float,
# outputs
deltas: wp.array[wp.spatial_vector],
):
tid = wp.tid()
count = contact_count[0]
if tid >= count:
return
shape_a = contact_shape0[tid]
shape_b = contact_shape1[tid]
if shape_a == shape_b:
return
body_a = -1
body_b = -1
# use average contact material properties
mat_nonzero = 0
restitution = 0.0
if shape_a >= 0:
mat_nonzero += 1
restitution += shape_material_restitution[shape_a]
body_a = shape_body[shape_a]
if shape_b >= 0:
mat_nonzero += 1
restitution += shape_material_restitution[shape_b]
body_b = shape_body[shape_b]
if mat_nonzero > 0:
restitution /= float(mat_nonzero)
if body_a == body_b:
return
m_inv_a = 0.0
m_inv_b = 0.0
I_inv_a = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
I_inv_b = wp.mat33(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
# body to world transform
X_wb_a_prev = wp.transform_identity()
X_wb_b_prev = wp.transform_identity()
# center of mass in body frame
com_a = wp.vec3(0.0)
com_b = wp.vec3(0.0)
# previous velocity at contact points
v_a = wp.vec3(0.0)
v_b = wp.vec3(0.0)
# new velocity at contact points
v_a_new = wp.vec3(0.0)
v_b_new = wp.vec3(0.0)
# inverse mass used to compute the impulse
inv_mass = 0.0
if body_a >= 0:
X_wb_a_prev = body_q_prev[body_a]
# X_wb_a = body_q[body_a]
m_inv_a = body_m_inv[body_a]
I_inv_a = body_I_inv[body_a]
com_a = body_com[body_a]
if body_b >= 0:
X_wb_b_prev = body_q_prev[body_b]
# X_wb_b = body_q[body_b]
m_inv_b = body_m_inv[body_b]
I_inv_b = body_I_inv[body_b]
com_b = body_com[body_b]
# compute body position in world space
bx_a = wp.transform_point(X_wb_a_prev, contact_point0[tid] + contact_offset0[tid])
bx_b = wp.transform_point(X_wb_b_prev, contact_point1[tid] + contact_offset1[tid])
thickness = contact_thickness0[tid] + contact_thickness1[tid]
n = contact_normal[tid]
d = wp.dot(n, bx_b - bx_a) - thickness
if d >= 0.0:
return
r_a = bx_a - wp.transform_point(X_wb_a_prev, com_a)
r_b = bx_b - wp.transform_point(X_wb_b_prev, com_b)
rxn_a = wp.vec3(0.0)
rxn_b = wp.vec3(0.0)
if body_a >= 0:
world_idx_a = body_world[body_a]
world_a_g = gravity[wp.max(world_idx_a, 0)]
v_a = velocity_at_point(body_qd_prev[body_a], r_a) + world_a_g * dt
v_a_new = velocity_at_point(body_qd[body_a], r_a)
q_a = wp.transform_get_rotation(X_wb_a_prev)
rxn_a = wp.quat_rotate_inv(q_a, wp.cross(r_a, n))
# Eq. 2
inv_mass_a = m_inv_a + wp.dot(rxn_a, I_inv_a * rxn_a)
inv_mass += inv_mass_a
if body_b >= 0:
world_idx_b = body_world[body_b]
world_b_g = gravity[wp.max(world_idx_b, 0)]
v_b = velocity_at_point(body_qd_prev[body_b], r_b) + world_b_g * dt
v_b_new = velocity_at_point(body_qd[body_b], r_b)
q_b = wp.transform_get_rotation(X_wb_b_prev)
rxn_b = wp.quat_rotate_inv(q_b, wp.cross(r_b, n))
# Eq. 3
inv_mass_b = m_inv_b + wp.dot(rxn_b, I_inv_b * rxn_b)
inv_mass += inv_mass_b
if inv_mass == 0.0:
return
# Eq. 29 — relative velocity of B w.r.t. A along the A-to-B normal
rel_vel_old = wp.dot(n, v_b - v_a)
rel_vel_new = wp.dot(n, v_b_new - v_a_new)
if rel_vel_old >= 0.0:
return
# Eq. 34
dv = (-rel_vel_new - restitution * rel_vel_old) / inv_mass
# Eq. 33 — push A in -n direction, B in +n direction
if body_a >= 0:
dv_a = -dv
q_a = wp.transform_get_rotation(X_wb_a_prev)
dq = wp.quat_rotate(q_a, I_inv_a * rxn_a * dv_a)
wp.atomic_add(deltas, body_a, wp.spatial_vector(n * m_inv_a * dv_a, dq))
if body_b >= 0:
dv_b = dv
q_b = wp.transform_get_rotation(X_wb_b_prev)
dq = wp.quat_rotate(q_b, I_inv_b * rxn_b * dv_b)
wp.atomic_add(deltas, body_b, wp.spatial_vector(n * m_inv_b * dv_b, dq))
================================================
FILE: newton/_src/solvers/xpbd/solver_xpbd.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import warp as wp
from ...core.types import override
from ...sim import Contacts, Control, Model, State
from ..flags import SolverNotifyFlags
from ..solver import SolverBase
from .kernels import (
apply_body_delta_velocities,
apply_body_deltas,
apply_joint_forces,
apply_particle_deltas,
apply_particle_shape_restitution,
apply_rigid_restitution,
bending_constraint,
copy_kinematic_body_state_kernel,
solve_body_contact_positions,
solve_body_joints,
solve_particle_particle_contacts,
solve_particle_shape_contacts,
# solve_simple_body_joints,
solve_springs,
solve_tetrahedra,
update_body_velocities,
)
class SolverXPBD(SolverBase):
"""An implicit integrator using eXtended Position-Based Dynamics (XPBD) for rigid and soft body simulation.
References:
- Miles Macklin, Matthias Müller, and Nuttapong Chentanez. 2016. XPBD: position-based simulation of compliant constrained dynamics. In Proceedings of the 9th International Conference on Motion in Games (MIG '16). Association for Computing Machinery, New York, NY, USA, 49-54. https://doi.org/10.1145/2994258.2994272
- Matthias Müller, Miles Macklin, Nuttapong Chentanez, Stefan Jeschke, and Tae-Yong Kim. 2020. Detailed rigid body simulation with extended position based dynamics. In Proceedings of the ACM SIGGRAPH/Eurographics Symposium on Computer Animation (SCA '20). Eurographics Association, Goslar, DEU, Article 10, 1-12. https://doi.org/10.1111/cgf.14105
After constructing :class:`Model`, :class:`State`, and :class:`Control` (optional) objects, this time-integrator
may be used to advance the simulation state forward in time.
Joint limitations:
- Supported joint types: PRISMATIC, REVOLUTE, BALL, FIXED, FREE, DISTANCE, D6.
CABLE joints are not supported.
- :attr:`~newton.Model.joint_enabled`,
:attr:`~newton.Model.joint_target_ke`/:attr:`~newton.Model.joint_target_kd`, and
:attr:`~newton.Control.joint_f` are supported.
Joint limits are enforced as hard positional constraints (``joint_limit_ke``/``joint_limit_kd`` are not used).
- :attr:`~newton.Model.joint_armature`, :attr:`~newton.Model.joint_friction`,
:attr:`~newton.Model.joint_effort_limit`, :attr:`~newton.Model.joint_velocity_limit`,
and :attr:`~newton.Model.joint_target_mode` are not supported.
- Equality and mimic constraints are not supported.
See :ref:`Joint feature support` for the full comparison across solvers.
Example
-------
.. code-block:: python
solver = newton.solvers.SolverXPBD(model)
# simulation loop
for i in range(100):
solver.step(state_in, state_out, control, contacts, dt)
state_in, state_out = state_out, state_in
"""
def __init__(
self,
model: Model,
iterations: int = 2,
soft_body_relaxation: float = 0.9,
soft_contact_relaxation: float = 0.9,
joint_linear_relaxation: float = 0.7,
joint_angular_relaxation: float = 0.4,
joint_linear_compliance: float = 0.0,
joint_angular_compliance: float = 0.0,
rigid_contact_relaxation: float = 0.8,
rigid_contact_con_weighting: bool = True,
angular_damping: float = 0.0,
enable_restitution: bool = False,
):
super().__init__(model=model)
self.iterations = iterations
self.soft_body_relaxation = soft_body_relaxation
self.soft_contact_relaxation = soft_contact_relaxation
self.joint_linear_relaxation = joint_linear_relaxation
self.joint_angular_relaxation = joint_angular_relaxation
self.joint_linear_compliance = joint_linear_compliance
self.joint_angular_compliance = joint_angular_compliance
self.rigid_contact_relaxation = rigid_contact_relaxation
self.rigid_contact_con_weighting = rigid_contact_con_weighting
self.angular_damping = angular_damping
self.enable_restitution = enable_restitution
self.compute_body_velocity_from_position_delta = False
self._init_kinematic_state()
# helper variables to track constraint resolution vars
self._particle_delta_counter = 0
self._body_delta_counter = 0
if model.particle_count > 1 and model.particle_grid is not None:
# reserve space for the particle hash grid
with wp.ScopedDevice(model.device):
model.particle_grid.reserve(model.particle_count)
@override
def notify_model_changed(self, flags: int) -> None:
if flags & (SolverNotifyFlags.BODY_PROPERTIES | SolverNotifyFlags.BODY_INERTIAL_PROPERTIES):
self._refresh_kinematic_state()
def copy_kinematic_body_state(self, model: Model, state_in: State, state_out: State):
if model.body_count == 0:
return
wp.launch(
kernel=copy_kinematic_body_state_kernel,
dim=model.body_count,
inputs=[model.body_flags, state_in.body_q, state_in.body_qd],
outputs=[state_out.body_q, state_out.body_qd],
device=model.device,
)
def _apply_particle_deltas(
self,
model: Model,
state_in: State,
state_out: State,
particle_deltas: wp.array,
dt: float,
):
if state_in.requires_grad:
particle_q = state_out.particle_q
# allocate new particle arrays so gradients can be tracked correctly without overwriting
new_particle_q = wp.empty_like(state_out.particle_q)
new_particle_qd = wp.empty_like(state_out.particle_qd)
self._particle_delta_counter += 1
else:
if self._particle_delta_counter == 0:
particle_q = state_out.particle_q
new_particle_q = state_in.particle_q
new_particle_qd = state_in.particle_qd
else:
particle_q = state_in.particle_q
new_particle_q = state_out.particle_q
new_particle_qd = state_out.particle_qd
self._particle_delta_counter = 1 - self._particle_delta_counter
wp.launch(
kernel=apply_particle_deltas,
dim=model.particle_count,
inputs=[
self.particle_q_init,
particle_q,
model.particle_flags,
particle_deltas,
dt,
model.particle_max_velocity,
],
outputs=[new_particle_q, new_particle_qd],
device=model.device,
)
if state_in.requires_grad:
state_out.particle_q = new_particle_q
state_out.particle_qd = new_particle_qd
return new_particle_q, new_particle_qd
def _apply_body_deltas(
self,
model: Model,
state_in: State,
state_out: State,
body_deltas: wp.array,
dt: float,
rigid_contact_inv_weight: wp.array = None,
):
with wp.ScopedTimer("apply_body_deltas", False):
if state_in.requires_grad:
body_q = state_out.body_q
body_qd = state_out.body_qd
new_body_q = wp.clone(body_q)
new_body_qd = wp.clone(body_qd)
self._body_delta_counter += 1
else:
if self._body_delta_counter == 0:
body_q = state_out.body_q
body_qd = state_out.body_qd
new_body_q = state_in.body_q
new_body_qd = state_in.body_qd
else:
body_q = state_in.body_q
body_qd = state_in.body_qd
new_body_q = state_out.body_q
new_body_qd = state_out.body_qd
self._body_delta_counter = 1 - self._body_delta_counter
wp.launch(
kernel=apply_body_deltas,
dim=model.body_count,
inputs=[
body_q,
body_qd,
model.body_com,
model.body_inertia,
self.body_inv_mass_effective,
self.body_inv_inertia_effective,
body_deltas,
rigid_contact_inv_weight,
dt,
],
outputs=[
new_body_q,
new_body_qd,
],
device=model.device,
)
if state_in.requires_grad:
state_out.body_q = new_body_q
state_out.body_qd = new_body_qd
return new_body_q, new_body_qd
@override
def step(self, state_in: State, state_out: State, control: Control, contacts: Contacts, dt: float) -> None:
requires_grad = state_in.requires_grad
self._particle_delta_counter = 0
self._body_delta_counter = 0
model = self.model
particle_q = None
particle_qd = None
particle_deltas = None
body_q = None
body_qd = None
body_q_init = None
body_qd_init = None
body_deltas = None
rigid_contact_inv_weight = None
if contacts:
if self.rigid_contact_con_weighting:
rigid_contact_inv_weight = wp.zeros(model.body_count, dtype=float, device=model.device)
rigid_contact_inv_weight_init = None
if control is None:
control = model.control(clone_variables=False)
with wp.ScopedTimer("simulate", False):
if model.particle_count:
particle_q = state_out.particle_q
particle_qd = state_out.particle_qd
self.particle_q_init = wp.clone(state_in.particle_q)
if self.enable_restitution:
self.particle_qd_init = wp.clone(state_in.particle_qd)
particle_deltas = wp.empty_like(state_out.particle_qd)
self.integrate_particles(model, state_in, state_out, dt)
# Build/update the particle hash grid for particle-particle contact queries
if model.particle_count > 1 and model.particle_grid is not None:
# Search radius must cover the maximum interaction distance used by the contact query
search_radius = model.particle_max_radius * 2.0 + model.particle_cohesion
with wp.ScopedDevice(model.device):
model.particle_grid.build(state_out.particle_q, radius=search_radius)
if model.body_count:
body_q = state_out.body_q
body_qd = state_out.body_qd
if self.compute_body_velocity_from_position_delta or self.enable_restitution:
body_q_init = wp.clone(state_in.body_q)
body_qd_init = wp.clone(state_in.body_qd)
body_deltas = wp.empty_like(state_out.body_qd)
body_f_tmp = state_in.body_f
if model.joint_count:
# Avoid accumulating joint_f into the persistent state body_f buffer.
body_f_tmp = wp.clone(state_in.body_f)
wp.launch(
kernel=apply_joint_forces,
dim=model.joint_count,
inputs=[
state_in.body_q,
model.body_com,
model.joint_type,
model.joint_enabled,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_qd_start,
model.joint_dof_dim,
model.joint_axis,
control.joint_f,
],
outputs=[body_f_tmp],
device=model.device,
)
if body_f_tmp is state_in.body_f:
self.integrate_bodies(model, state_in, state_out, dt, self.angular_damping)
else:
body_f_prev = state_in.body_f
state_in.body_f = body_f_tmp
self.integrate_bodies(model, state_in, state_out, dt, self.angular_damping)
state_in.body_f = body_f_prev
spring_constraint_lambdas = None
if model.spring_count:
spring_constraint_lambdas = wp.empty_like(model.spring_rest_length)
edge_constraint_lambdas = None
if model.edge_count:
edge_constraint_lambdas = wp.empty_like(model.edge_rest_angle)
for i in range(self.iterations):
with wp.ScopedTimer(f"iteration_{i}", False):
if model.body_count:
if requires_grad and i > 0:
body_deltas = wp.zeros_like(body_deltas)
else:
body_deltas.zero_()
if model.particle_count:
if requires_grad and i > 0:
particle_deltas = wp.zeros_like(particle_deltas)
else:
particle_deltas.zero_()
# particle-rigid body contacts (besides ground plane)
if model.shape_count:
wp.launch(
kernel=solve_particle_shape_contacts,
dim=contacts.soft_contact_max,
inputs=[
particle_q,
particle_qd,
model.particle_inv_mass,
model.particle_radius,
model.particle_flags,
body_q,
body_qd,
model.body_com,
self.body_inv_mass_effective,
self.body_inv_inertia_effective,
model.shape_body,
model.shape_material_mu,
model.soft_contact_mu,
model.particle_adhesion,
contacts.soft_contact_count,
contacts.soft_contact_particle,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_body_vel,
contacts.soft_contact_normal,
contacts.soft_contact_max,
dt,
self.soft_contact_relaxation,
],
# outputs
outputs=[particle_deltas, body_deltas],
device=model.device,
)
if model.particle_max_radius > 0.0 and model.particle_count > 1:
# assert model.particle_grid.reserved, "model.particle_grid must be built, see HashGrid.build()"
assert model.particle_grid is not None
wp.launch(
kernel=solve_particle_particle_contacts,
dim=model.particle_count,
inputs=[
model.particle_grid.id,
particle_q,
particle_qd,
model.particle_inv_mass,
model.particle_radius,
model.particle_flags,
model.particle_mu,
model.particle_cohesion,
model.particle_max_radius,
dt,
self.soft_contact_relaxation,
],
outputs=[particle_deltas],
device=model.device,
)
# distance constraints
if model.spring_count:
spring_constraint_lambdas.zero_()
wp.launch(
kernel=solve_springs,
dim=model.spring_count,
inputs=[
particle_q,
particle_qd,
model.particle_inv_mass,
model.spring_indices,
model.spring_rest_length,
model.spring_stiffness,
model.spring_damping,
dt,
spring_constraint_lambdas,
],
outputs=[particle_deltas],
device=model.device,
)
# bending constraints
if model.edge_count:
edge_constraint_lambdas.zero_()
wp.launch(
kernel=bending_constraint,
dim=model.edge_count,
inputs=[
particle_q,
particle_qd,
model.particle_inv_mass,
model.edge_indices,
model.edge_rest_angle,
model.edge_bending_properties,
dt,
edge_constraint_lambdas,
],
outputs=[particle_deltas],
device=model.device,
)
# tetrahedral FEM
if model.tet_count:
wp.launch(
kernel=solve_tetrahedra,
dim=model.tet_count,
inputs=[
particle_q,
particle_qd,
model.particle_inv_mass,
model.tet_indices,
model.tet_poses,
model.tet_activations,
model.tet_materials,
dt,
self.soft_body_relaxation,
],
outputs=[particle_deltas],
device=model.device,
)
particle_q, particle_qd = self._apply_particle_deltas(
model, state_in, state_out, particle_deltas, dt
)
# handle rigid bodies
# ----------------------------
# Solve rigid contact constraints
if model.body_count and contacts is not None:
if self.rigid_contact_con_weighting:
rigid_contact_inv_weight.zero_()
wp.launch(
kernel=solve_body_contact_positions,
dim=contacts.rigid_contact_max,
inputs=[
body_q,
body_qd,
model.body_flags,
model.body_com,
self.body_inv_mass_effective,
self.body_inv_inertia_effective,
model.shape_body,
contacts.rigid_contact_count,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_offset0,
contacts.rigid_contact_offset1,
contacts.rigid_contact_normal,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
model.shape_material_mu,
model.shape_material_mu_torsional,
model.shape_material_mu_rolling,
self.rigid_contact_relaxation,
dt,
],
outputs=[
body_deltas,
rigid_contact_inv_weight,
],
device=model.device,
)
# if model.rigid_contact_count.numpy()[0] > 0:
# print("rigid_contact_count:", model.rigid_contact_count.numpy().flatten())
# # print("rigid_active_contact_distance:", rigid_active_contact_distance.numpy().flatten())
# # print("rigid_active_contact_point0:", rigid_active_contact_point0.numpy().flatten())
# # print("rigid_active_contact_point1:", rigid_active_contact_point1.numpy().flatten())
# print("body_deltas:", body_deltas.numpy().flatten())
# print(rigid_active_contact_distance.numpy().flatten())
if self.enable_restitution and i == 0:
# remember contact constraint weighting from the first iteration
if self.rigid_contact_con_weighting:
rigid_contact_inv_weight_init = wp.clone(rigid_contact_inv_weight)
else:
rigid_contact_inv_weight_init = None
body_q, body_qd = self._apply_body_deltas(
model, state_in, state_out, body_deltas, dt, rigid_contact_inv_weight
)
if model.joint_count:
if requires_grad:
body_deltas = wp.zeros_like(body_deltas)
else:
body_deltas.zero_()
wp.launch(
kernel=solve_body_joints,
dim=model.joint_count,
inputs=[
body_q,
body_qd,
model.body_com,
self.body_inv_mass_effective,
self.body_inv_inertia_effective,
model.joint_type,
model.joint_enabled,
model.joint_parent,
model.joint_child,
model.joint_X_p,
model.joint_X_c,
model.joint_limit_lower,
model.joint_limit_upper,
model.joint_qd_start,
model.joint_dof_dim,
model.joint_axis,
control.joint_target_pos,
control.joint_target_vel,
model.joint_target_ke,
model.joint_target_kd,
self.joint_linear_compliance,
self.joint_angular_compliance,
self.joint_angular_relaxation,
self.joint_linear_relaxation,
dt,
],
outputs=[body_deltas],
device=model.device,
)
body_q, body_qd = self._apply_body_deltas(model, state_in, state_out, body_deltas, dt)
if model.particle_count:
if particle_q.ptr != state_out.particle_q.ptr:
state_out.particle_q.assign(particle_q)
state_out.particle_qd.assign(particle_qd)
if model.body_count:
if body_q.ptr != state_out.body_q.ptr:
state_out.body_q.assign(body_q)
state_out.body_qd.assign(body_qd)
# update body velocities from position changes
if self.compute_body_velocity_from_position_delta and model.body_count and not requires_grad:
# causes gradient issues (probably due to numerical problems
# when computing velocities from position changes)
if requires_grad:
out_body_qd = wp.clone(state_out.body_qd)
else:
out_body_qd = state_out.body_qd
# update body velocities
wp.launch(
kernel=update_body_velocities,
dim=model.body_count,
inputs=[state_out.body_q, body_q_init, model.body_com, dt],
outputs=[out_body_qd],
device=model.device,
)
if self.enable_restitution and contacts is not None:
if model.particle_count:
wp.launch(
kernel=apply_particle_shape_restitution,
dim=contacts.soft_contact_max,
inputs=[
particle_qd,
self.particle_q_init,
self.particle_qd_init,
model.particle_radius,
model.particle_flags,
body_q,
body_q_init,
body_qd,
body_qd_init,
model.body_com,
model.shape_body,
model.particle_adhesion,
model.soft_contact_restitution,
contacts.soft_contact_count,
contacts.soft_contact_particle,
contacts.soft_contact_shape,
contacts.soft_contact_body_pos,
contacts.soft_contact_body_vel,
contacts.soft_contact_normal,
contacts.soft_contact_max,
],
outputs=[state_out.particle_qd],
device=model.device,
)
if model.body_count:
body_deltas.zero_()
wp.launch(
kernel=apply_rigid_restitution,
dim=contacts.rigid_contact_max,
inputs=[
state_out.body_q,
state_out.body_qd,
body_q_init,
body_qd_init,
model.body_com,
self.body_inv_mass_effective,
self.body_inv_inertia_effective,
model.body_world,
model.shape_body,
contacts.rigid_contact_count,
contacts.rigid_contact_normal,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
model.shape_material_restitution,
contacts.rigid_contact_point0,
contacts.rigid_contact_point1,
contacts.rigid_contact_offset0,
contacts.rigid_contact_offset1,
contacts.rigid_contact_margin0,
contacts.rigid_contact_margin1,
rigid_contact_inv_weight_init,
model.gravity,
dt,
],
outputs=[
body_deltas,
],
device=model.device,
)
wp.launch(
kernel=apply_body_delta_velocities,
dim=model.body_count,
inputs=[
body_deltas,
],
outputs=[state_out.body_qd],
device=model.device,
)
if model.body_count:
self.copy_kinematic_body_state(model, state_in, state_out)
================================================
FILE: newton/_src/usd/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
try:
# register the newton schema plugin before any other USD code is executed
import newton_usd_schemas # noqa: F401
except ImportError:
pass
from .utils import (
get_attribute,
get_attributes_in_namespace,
get_custom_attribute_declarations,
get_custom_attribute_values,
get_float,
get_gaussian,
get_gprim_axis,
get_mesh,
get_quat,
get_scale,
get_transform,
has_attribute,
type_to_warp,
value_to_warp,
)
__all__ = [
"get_attribute",
"get_attributes_in_namespace",
"get_custom_attribute_declarations",
"get_custom_attribute_values",
"get_float",
"get_gaussian",
"get_gprim_axis",
"get_mesh",
"get_quat",
"get_scale",
"get_transform",
"has_attribute",
"type_to_warp",
"value_to_warp",
]
================================================
FILE: newton/_src/usd/schema_resolver.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
USD schema resolver infrastructure.
This module defines the base resolver types used to map authored USD schema
attributes onto Newton builder attributes. Public users should import resolver
types from :mod:`newton.usd`.
"""
from __future__ import annotations
from collections.abc import Callable, Sequence
from dataclasses import dataclass
from enum import IntEnum
from typing import TYPE_CHECKING, Any, ClassVar
from . import utils as usd
if TYPE_CHECKING:
from pxr import Usd
from ..sim.builder import ModelBuilder
class PrimType(IntEnum):
"""Enumeration of USD prim types that can be resolved by schema resolvers."""
SCENE = 0
"""PhysicsScene prim type."""
JOINT = 1
"""Joint prim type."""
SHAPE = 2
"""Shape prim type."""
BODY = 3
"""Body prim type."""
MATERIAL = 4
"""Material prim type."""
ACTUATOR = 5
"""Actuator prim type."""
ARTICULATION = 6
"""Articulation root prim type."""
class SchemaResolver:
"""Base class mapping USD schema attributes to Newton attributes."""
@dataclass
class SchemaAttribute:
"""
Specifies a USD attribute and its transformation function.
Args:
name: The name of the USD attribute (or primary attribute when using a getter).
default: Default USD-authored value from schema, if any.
usd_value_transformer: Optional function to transform the raw value into the format expected by Newton.
usd_value_getter: Optional function (prim) -> value used instead of reading a single attribute (e.g. to compute gap from contactOffset - restOffset).
attribute_names: When set, names used for collect_prim_attrs; otherwise [name] is used.
"""
name: str
default: Any | None = None
usd_value_transformer: Callable[[Any], Any] | None = None
usd_value_getter: Callable[[Usd.Prim], Any] | None = None
attribute_names: Sequence[str] = ()
# mapping is a dictionary for known variables in Newton. Its purpose is to map USD attributes to existing Newton data.
# PrimType -> Newton variable -> Attribute
mapping: ClassVar[dict[PrimType, dict[str, SchemaAttribute]]]
# Name of the schema resolver
name: ClassVar[str]
# extra_attr_namespaces is a list of additional USD attribute namespaces in which the schema attributes may be authored.
extra_attr_namespaces: ClassVar[list[str]] = []
def __init__(self) -> None:
# Precompute the full set of USD attribute names referenced by this resolver's mapping.
names: set[str] = set()
try:
mapping_items = self.mapping.items()
except AttributeError:
mapping_items = []
for _prim_type, var_map in mapping_items:
try:
var_items = var_map.items()
except AttributeError:
continue
for _var, spec in var_items:
if spec.attribute_names:
names.update(spec.attribute_names)
else:
names.add(spec.name)
self._solver_attributes: list[str] = list(names)
def get_value(self, prim: Usd.Prim, prim_type: PrimType, key: str) -> Any | None:
"""Get an authored value for a resolver key.
Args:
prim: USD prim to query.
prim_type: Prim type category.
key: Logical Newton attribute key within the prim category.
Returns:
Resolved authored value, or ``None`` when not found.
"""
if prim is None:
return None
spec = self.mapping.get(prim_type, {}).get(key)
if spec is not None:
if spec.usd_value_getter is not None:
v = spec.usd_value_getter(prim)
else:
v = usd.get_attribute(prim, spec.name)
if v is not None:
return spec.usd_value_transformer(v) if spec.usd_value_transformer is not None else v
return None
def collect_prim_attrs(self, prim: Usd.Prim) -> dict[str, Any]:
"""Collect all resolver-relevant attributes for a prim.
Args:
prim: USD prim to inspect.
Returns:
Dictionary mapping authored USD attribute names to values.
"""
if prim is None:
return {}
# Collect attributes by known prefixes
# USD expects namespace tokens without ':' (e.g., 'newton', 'mjc', 'physxArticulation')
main_prefix = self.name
all_prefixes = [main_prefix]
if self.extra_attr_namespaces:
all_prefixes.extend(self.extra_attr_namespaces)
prefixed_attrs: dict[str, Any] = _collect_attrs_by_namespace(prim, all_prefixes)
# Collect explicit attribute names defined in the resolver mapping (precomputed)
prim_solver_attrs = _collect_attrs_by_name(prim, self._solver_attributes) if self._solver_attributes else {}
# Merge and return (explicit names take precedence)
merged: dict[str, Any] = {}
merged.update(prefixed_attrs)
merged.update(prim_solver_attrs)
return merged
def validate_custom_attributes(self, builder: ModelBuilder) -> None:
"""
Validate that solver-specific custom attributes are registered on the builder.
Override in subclasses to check that required custom attributes have been
registered before parsing. Called by parse_usd() before processing entities.
Args:
builder: The ModelBuilder to validate custom attributes on.
"""
del builder
# Backward-compatible alias; prefer SchemaResolver.SchemaAttribute.
SchemaAttribute = SchemaResolver.SchemaAttribute
def _collect_attrs_by_name(prim: Usd.Prim, names: Sequence[str]) -> dict[str, Any]:
"""Collect attributes authored on the prim that have direct mappings in the resolver mapping"""
out: dict[str, Any] = {}
for n in names:
v = usd.get_attribute(prim, n)
if v is not None:
out[n] = v
return out
def _collect_attrs_by_namespace(prim: Usd.Prim, namespaces: Sequence[str]) -> dict[str, Any]:
"""Collect authored attributes using USD namespace queries."""
out: dict[str, Any] = {}
if prim is None:
return out
for ns in namespaces:
out.update(usd.get_attributes_in_namespace(prim, ns))
return out
class SchemaResolverManager:
"""
Manager for resolving multiple USD schemas in a priority order.
"""
def __init__(self, resolvers: Sequence[SchemaResolver]):
"""
Initialize resolver manager with resolver instances in priority order.
Args:
resolvers: List of instantiated resolvers in priority order.
"""
# Use provided resolver instances directly
self.resolvers = list(resolvers)
# Dictionary to accumulate schema attributes as prims are encountered
# Pre-initialize maps for each configured resolver
self._schema_attrs: dict[str, dict[str, dict[str, Any]]] = {r.name: {} for r in self.resolvers}
def _collect_on_first_use(self, resolver: SchemaResolver, prim: Usd.Prim) -> None:
"""Collect and store attributes for this resolver/prim on first use."""
if prim is None:
return
prim_path = str(prim.GetPath())
if prim_path in self._schema_attrs[resolver.name]:
return
self._schema_attrs[resolver.name][prim_path] = resolver.collect_prim_attrs(prim)
def get_value(
self, prim: Usd.Prim, prim_type: PrimType, key: str, default: Any = None, verbose: bool = False
) -> Any:
"""
Resolve value using schema priority, with layered fallbacks:
1) First authored value found in resolver order (highest priority first)
2) If none authored, use the provided 'default' argument if not None
3) If no default provided, use the first non-None mapping default from resolvers in priority order
4) If no mapping default found, return None
Args:
prim: USD prim to query (for scene prim_type, this should be scene_prim)
prim_type: Prim type (PrimType enum)
key: Attribute key within the prim type
default: Default value if not found
Returns:
Resolved value according to the precedence above.
"""
# 1) Authored value by schema priority
for r in self.resolvers:
val = r.get_value(prim, prim_type, key)
if val is None:
continue
self._collect_on_first_use(r, prim)
return val
# 2) Caller-provided default, if any
if default is not None:
return default
# 3) Resolver mapping defaults in priority order
for resolver in self.resolvers:
spec = resolver.mapping.get(prim_type, {}).get(key) if hasattr(resolver, "mapping") else None
if spec is not None:
d = getattr(spec, "default", None)
if d is not None:
transformer = getattr(spec, "usd_value_transformer", None)
return transformer(d) if transformer is not None else d
# Nothing found
try:
prim_path = str(prim.GetPath()) if prim is not None else ""
except (AttributeError, RuntimeError):
prim_path = ""
if verbose:
error_message = (
f"Error: Cannot resolve value for '{prim_type.name.lower()}:{key}' on prim '{prim_path}'; "
+ "no authored value, no explicit default, and no solver mapping default."
)
print(error_message)
return None
def collect_prim_attrs(self, prim: Usd.Prim) -> None:
"""
Collect and accumulate schema attributes for a single prim.
Args:
prim: USD prim to collect attributes from
"""
if prim is None:
return
prim_path = str(prim.GetPath())
for resolver in self.resolvers:
# only collect if we haven't seen this prim for this resolver
if prim_path not in self._schema_attrs[resolver.name]:
self._schema_attrs[resolver.name][prim_path] = resolver.collect_prim_attrs(prim)
@property
def schema_attrs(self) -> dict[str, dict[str, dict[str, Any]]]:
"""
Get the accumulated attributes.
Returns:
Dictionary with structure: schema_name -> prim_path -> {attr_name: attr_value}
e.g., {"mjc": {"/World/Cube": {"mjc:option:timestep": 0.01}}}
"""
return self._schema_attrs
================================================
FILE: newton/_src/usd/schemas.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""Concrete USD schema resolvers used by :mod:`newton.usd`."""
from __future__ import annotations
import warnings
from collections.abc import Sequence
from typing import TYPE_CHECKING, ClassVar
from ..core.types import override
from ..usd.schema_resolver import PrimType, SchemaResolver
from . import utils as usd
if TYPE_CHECKING:
from pxr import Usd
from ..sim.builder import ModelBuilder
SchemaAttribute = SchemaResolver.SchemaAttribute
def _physx_gap_from_prim(prim: Usd.Prim) -> float | None:
"""Compute Newton gap from PhysX: contactOffset - restOffset [m].
Returns None if either attribute is missing or -inf (PhysX uses -inf for "engine default").
Only when both are finite do we compute a concrete gap.
"""
contact_offset = usd.get_attribute(prim, "physxCollision:contactOffset")
rest_offset = usd.get_attribute(prim, "physxCollision:restOffset")
if contact_offset is None or rest_offset is None:
return None
inf = float("-inf")
if contact_offset == inf or rest_offset == inf:
return None
return float(contact_offset) - float(rest_offset)
class SchemaResolverNewton(SchemaResolver):
"""Schema resolver for Newton-authored USD attributes.
.. note::
The Newton USD schema is under development and may change in the future.
"""
name: ClassVar[str] = "newton"
mapping: ClassVar[dict[PrimType, dict[str, SchemaAttribute]]] = {
PrimType.SCENE: {
"max_solver_iterations": SchemaAttribute("newton:maxSolverIterations", -1),
"time_steps_per_second": SchemaAttribute("newton:timeStepsPerSecond", 1000),
"gravity_enabled": SchemaAttribute("newton:gravityEnabled", True),
},
PrimType.JOINT: {
# warning: there is no NewtonJointAPI, none of these are schema attributes
"armature": SchemaAttribute("newton:armature", 0.0),
"friction": SchemaAttribute("newton:friction", 0.0),
"limit_linear_ke": SchemaAttribute("newton:linear:limitStiffness", 1.0e4),
"limit_angular_ke": SchemaAttribute("newton:angular:limitStiffness", 1.0e4),
"limit_rotX_ke": SchemaAttribute("newton:rotX:limitStiffness", 1.0e4),
"limit_rotY_ke": SchemaAttribute("newton:rotY:limitStiffness", 1.0e4),
"limit_rotZ_ke": SchemaAttribute("newton:rotZ:limitStiffness", 1.0e4),
"limit_linear_kd": SchemaAttribute("newton:linear:limitDamping", 1.0e1),
"limit_angular_kd": SchemaAttribute("newton:angular:limitDamping", 1.0e1),
"limit_rotX_kd": SchemaAttribute("newton:rotX:limitDamping", 1.0e1),
"limit_rotY_kd": SchemaAttribute("newton:rotY:limitDamping", 1.0e1),
"limit_rotZ_kd": SchemaAttribute("newton:rotZ:limitDamping", 1.0e1),
"angular_position": SchemaAttribute("newton:angular:position", 0.0),
"linear_position": SchemaAttribute("newton:linear:position", 0.0),
"rotX_position": SchemaAttribute("newton:rotX:position", 0.0),
"rotY_position": SchemaAttribute("newton:rotY:position", 0.0),
"rotZ_position": SchemaAttribute("newton:rotZ:position", 0.0),
"angular_velocity": SchemaAttribute("newton:angular:velocity", 0.0),
"linear_velocity": SchemaAttribute("newton:linear:velocity", 0.0),
"rotX_velocity": SchemaAttribute("newton:rotX:velocity", 0.0),
"rotY_velocity": SchemaAttribute("newton:rotY:velocity", 0.0),
"rotZ_velocity": SchemaAttribute("newton:rotZ:velocity", 0.0),
},
PrimType.SHAPE: {
# Mesh
"max_hull_vertices": SchemaAttribute("newton:maxHullVertices", -1),
# Collisions: newton margin == newton:contactMargin, newton gap == newton:contactGap
"margin": SchemaAttribute("newton:contactMargin", 0.0),
"gap": SchemaAttribute("newton:contactGap", float("-inf")),
# Contact stiffness/damping
"ke": SchemaAttribute("newton:contact_ke", None),
"kd": SchemaAttribute("newton:contact_kd", None),
},
PrimType.BODY: {},
PrimType.ARTICULATION: {
"self_collision_enabled": SchemaAttribute("newton:selfCollisionEnabled", True),
},
PrimType.MATERIAL: {
"mu_torsional": SchemaAttribute("newton:torsionalFriction", 0.25),
"mu_rolling": SchemaAttribute("newton:rollingFriction", 0.0005),
},
PrimType.ACTUATOR: {},
}
class SchemaResolverPhysx(SchemaResolver):
"""Schema resolver for PhysX USD attributes."""
name: ClassVar[str] = "physx"
extra_attr_namespaces: ClassVar[list[str]] = [
# Scene and rigid body
"physxScene",
"physxRigidBody",
# Collisions and meshes
"physxCollision",
"physxConvexHullCollision",
"physxConvexDecompositionCollision",
"physxTriangleMeshCollision",
"physxTriangleMeshSimplificationCollision",
"physxSDFMeshCollision",
# Materials
"physxMaterial",
# Joints and limits
"physxJoint",
"physxLimit",
# Articulations
"physxArticulation",
# State attributes (for joint position/velocity initialization)
"state",
# Drive attributes
"drive",
]
mapping: ClassVar[dict[PrimType, dict[str, SchemaAttribute]]] = {
PrimType.SCENE: {
"max_solver_iterations": SchemaAttribute("physxScene:maxVelocityIterationCount", 255),
"time_steps_per_second": SchemaAttribute("physxScene:timeStepsPerSecond", 60),
"gravity_enabled": SchemaAttribute("physxRigidBody:disableGravity", False, lambda value: not value),
},
PrimType.JOINT: {
"armature": SchemaAttribute("physxJoint:armature", 0.0),
"velocity_limit": SchemaAttribute("physxJoint:maxJointVelocity", None),
# Per-axis linear limit aliases
"limit_transX_ke": SchemaAttribute("physxLimit:linear:stiffness", 0.0),
"limit_transY_ke": SchemaAttribute("physxLimit:linear:stiffness", 0.0),
"limit_transZ_ke": SchemaAttribute("physxLimit:linear:stiffness", 0.0),
"limit_transX_kd": SchemaAttribute("physxLimit:linear:damping", 0.0),
"limit_transY_kd": SchemaAttribute("physxLimit:linear:damping", 0.0),
"limit_transZ_kd": SchemaAttribute("physxLimit:linear:damping", 0.0),
"limit_linear_ke": SchemaAttribute("physxLimit:linear:stiffness", 0.0),
"limit_angular_ke": SchemaAttribute("physxLimit:angular:stiffness", 0.0),
"limit_rotX_ke": SchemaAttribute("physxLimit:rotX:stiffness", 0.0),
"limit_rotY_ke": SchemaAttribute("physxLimit:rotY:stiffness", 0.0),
"limit_rotZ_ke": SchemaAttribute("physxLimit:rotZ:stiffness", 0.0),
"limit_linear_kd": SchemaAttribute("physxLimit:linear:damping", 0.0),
"limit_angular_kd": SchemaAttribute("physxLimit:angular:damping", 0.0),
"limit_rotX_kd": SchemaAttribute("physxLimit:rotX:damping", 0.0),
"limit_rotY_kd": SchemaAttribute("physxLimit:rotY:damping", 0.0),
"limit_rotZ_kd": SchemaAttribute("physxLimit:rotZ:damping", 0.0),
"angular_position": SchemaAttribute("state:angular:physics:position", 0.0),
"linear_position": SchemaAttribute("state:linear:physics:position", 0.0),
"rotX_position": SchemaAttribute("state:rotX:physics:position", 0.0),
"rotY_position": SchemaAttribute("state:rotY:physics:position", 0.0),
"rotZ_position": SchemaAttribute("state:rotZ:physics:position", 0.0),
"angular_velocity": SchemaAttribute("state:angular:physics:velocity", 0.0),
"linear_velocity": SchemaAttribute("state:linear:physics:velocity", 0.0),
"rotX_velocity": SchemaAttribute("state:rotX:physics:velocity", 0.0),
"rotY_velocity": SchemaAttribute("state:rotY:physics:velocity", 0.0),
"rotZ_velocity": SchemaAttribute("state:rotZ:physics:velocity", 0.0),
},
PrimType.SHAPE: {
# Mesh
"max_hull_vertices": SchemaAttribute("physxConvexHullCollision:hullVertexLimit", 64),
# Collisions: newton margin == physx restOffset, newton gap == physx contactOffset - restOffset.
# PhysX uses -inf to mean "engine default"; treat as unset (None).
"margin": SchemaAttribute(
"physxCollision:restOffset", 0.0, lambda v: None if v == float("-inf") else float(v)
),
"gap": SchemaAttribute(
"physxCollision:contactOffset",
float("-inf"),
usd_value_getter=_physx_gap_from_prim,
attribute_names=("physxCollision:contactOffset", "physxCollision:restOffset"),
),
},
PrimType.MATERIAL: {
"stiffness": SchemaAttribute("physxMaterial:compliantContactStiffness", 0.0),
"damping": SchemaAttribute("physxMaterial:compliantContactDamping", 0.0),
},
PrimType.BODY: {
# Rigid body damping
"rigid_body_linear_damping": SchemaAttribute("physxRigidBody:linearDamping", 0.0),
"rigid_body_angular_damping": SchemaAttribute("physxRigidBody:angularDamping", 0.05),
},
PrimType.ARTICULATION: {
"self_collision_enabled": SchemaAttribute("physxArticulation:enabledSelfCollisions", True),
},
}
def solref_to_stiffness_damping(solref: Sequence[float] | None) -> tuple[float | None, float | None]:
"""Convert MuJoCo solref (timeconst, dampratio) to internal stiffness and damping.
Returns a tuple (stiffness, damping).
Standard mode (timeconst > 0):
k = 1 / (timeconst^2 * dampratio^2)
b = 2 / timeconst
Direct mode (both negative):
solref encodes (-stiffness, -damping) directly
k = -timeconst
b = -dampratio
"""
if solref is None:
return None, None
try:
timeconst = float(solref[0])
dampratio = float(solref[1])
except (TypeError, ValueError, IndexError):
return None, None
# Direct mode: both negative → solref encodes (-stiffness, -damping)
if timeconst < 0.0 and dampratio < 0.0:
return -timeconst, -dampratio
# Standard mode: compute stiffness and damping
if timeconst <= 0.0 or dampratio <= 0.0:
return None, None
stiffness = 1.0 / (timeconst * timeconst * dampratio * dampratio)
damping = 2.0 / timeconst
return stiffness, damping
def solref_to_stiffness(solref: Sequence[float] | None) -> float | None:
"""Convert MuJoCo solref (timeconst, dampratio) to internal stiffness.
Standard mode (timeconst > 0): k = 1 / (timeconst^2 * dampratio^2)
Direct mode (both negative): k = -timeconst (encodes -stiffness directly)
"""
stiffness, _ = solref_to_stiffness_damping(solref)
return stiffness
def solref_to_damping(solref: Sequence[float] | None) -> float | None:
"""Convert MuJoCo solref (timeconst, dampratio) to internal damping.
Standard mode (both positive): b = 2 / timeconst
Direct mode (both negative): b = -dampratio (encodes -damping directly)
"""
_, damping = solref_to_stiffness_damping(solref)
return damping
def _mjc_margin_from_prim(prim: Usd.Prim) -> float | None:
"""Compute Newton margin from MuJoCo: margin - gap [m].
MuJoCo uses ``margin`` as the full contact detection envelope and ``gap``
as a sub-threshold that suppresses constraint activation. Newton stores
them separately, so: ``newton_margin = mjc_margin - mjc_gap``.
Returns None if the MuJoCo margin attribute is not authored.
"""
mjc_margin = usd.get_attribute(prim, "mjc:margin")
if mjc_margin is None:
return None
mjc_gap = usd.get_attribute(prim, "mjc:gap")
if mjc_gap is None:
mjc_gap = 0.0
result = float(mjc_margin) - float(mjc_gap)
if result < 0.0:
warnings.warn(
f"Prim '{prim.GetPath()}': MuJoCo gap ({mjc_gap}) exceeds margin ({mjc_margin}), "
f"resulting Newton margin is negative ({result}). "
f"This may indicate an invalid MuJoCo model.",
stacklevel=4,
)
return result
class SchemaResolverMjc(SchemaResolver):
"""Schema resolver for MuJoCo USD attributes."""
name: ClassVar[str] = "mjc"
mapping: ClassVar[dict[PrimType, dict[str, SchemaAttribute]]] = {
PrimType.SCENE: {
"max_solver_iterations": SchemaAttribute("mjc:option:iterations", 100),
"time_steps_per_second": SchemaAttribute(
"mjc:option:timestep", 0.002, lambda s: int(1.0 / s) if (s and s > 0) else None
),
"gravity_enabled": SchemaAttribute("mjc:flag:gravity", True),
},
PrimType.JOINT: {
"armature": SchemaAttribute("mjc:armature", 0.0),
"friction": SchemaAttribute("mjc:frictionloss", 0.0),
# Per-axis linear aliases mapped to solref
"limit_transX_ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"limit_transY_ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"limit_transZ_ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"limit_transX_kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
"limit_transY_kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
"limit_transZ_kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
"limit_linear_ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"limit_angular_ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"limit_rotX_ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"limit_rotY_ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"limit_rotZ_ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"limit_linear_kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
"limit_angular_kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
"limit_rotX_kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
"limit_rotY_kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
"limit_rotZ_kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
},
PrimType.SHAPE: {
# Mesh
"max_hull_vertices": SchemaAttribute("mjc:maxhullvert", -1),
# Collisions: MuJoCo -> Newton conversion applied via getter.
# newton_margin = mjc_margin - mjc_gap (see _mjc_margin_from_prim).
"margin": SchemaAttribute(
"mjc:margin",
0.0,
usd_value_getter=_mjc_margin_from_prim,
attribute_names=("mjc:margin", "mjc:gap"),
),
"gap": SchemaAttribute("mjc:gap", 0.0),
# Contact stiffness/damping from per-geom solref
"ke": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"kd": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
},
PrimType.MATERIAL: {
# Materials
"mu_torsional": SchemaAttribute("mjc:torsionalfriction", 0.005),
"mu_rolling": SchemaAttribute("mjc:rollingfriction", 0.0001),
# Contact models
"priority": SchemaAttribute("mjc:priority", 0),
"weight": SchemaAttribute("mjc:solmix", 1.0),
"stiffness": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_stiffness),
"damping": SchemaAttribute("mjc:solref", [0.02, 1.0], solref_to_damping),
},
PrimType.BODY: {
# Rigid body / joint domain
"rigid_body_linear_damping": SchemaAttribute("mjc:damping", 0.0),
},
PrimType.ACTUATOR: {
# Actuators
"ctrl_low": SchemaAttribute("mjc:ctrlRange:min", 0.0),
"ctrl_high": SchemaAttribute("mjc:ctrlRange:max", 0.0),
"force_low": SchemaAttribute("mjc:forceRange:min", 0.0),
"force_high": SchemaAttribute("mjc:forceRange:max", 0.0),
"act_low": SchemaAttribute("mjc:actRange:min", 0.0),
"act_high": SchemaAttribute("mjc:actRange:max", 0.0),
"length_low": SchemaAttribute("mjc:lengthRange:min", 0.0),
"length_high": SchemaAttribute("mjc:lengthRange:max", 0.0),
"gainPrm": SchemaAttribute("mjc:gainPrm", [1, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
"gainType": SchemaAttribute("mjc:gainType", "fixed"),
"biasPrm": SchemaAttribute("mjc:biasPrm", [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
"biasType": SchemaAttribute("mjc:biasType", "none"),
"dynPrm": SchemaAttribute("mjc:dynPrm", [1, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
"dynType": SchemaAttribute("mjc:dynType", "none"),
"gear": SchemaAttribute("mjc:gear", [1, 0, 0, 0, 0, 0]),
},
}
@override
def validate_custom_attributes(self, builder: ModelBuilder) -> None:
"""
Validate that MuJoCo custom attributes have been registered on the builder.
Users must call :meth:`newton.solvers.SolverMuJoCo.register_custom_attributes` before parsing
USD files with this resolver.
Raises:
RuntimeError: If required MuJoCo custom attributes are not registered.
"""
has_mujoco_attrs = any(attr.namespace == "mujoco" for attr in builder.custom_attributes.values())
if not has_mujoco_attrs:
raise RuntimeError(
"MuJoCo custom attributes not registered. Call "
+ "SolverMuJoCo.register_custom_attributes(builder) before parsing "
+ "USD with SchemaResolverMjc."
)
================================================
FILE: newton/_src/usd/utils.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import os
import warnings
from collections.abc import Iterable, Sequence
from typing import TYPE_CHECKING, Any, Literal, overload
import numpy as np
import warp as wp
from ..core.types import Axis, AxisType
from ..geometry import Gaussian, Mesh
from ..sim.model import Model
AttributeAssignment = Model.AttributeAssignment
AttributeFrequency = Model.AttributeFrequency
if TYPE_CHECKING:
from ..geometry.types import TetMesh
from ..sim.builder import ModelBuilder
try:
from pxr import Gf, Sdf, Usd, UsdGeom, UsdShade
except ImportError:
Usd = None
Gf = None
UsdGeom = None
Sdf = None
UsdShade = None
@overload
def get_attribute(prim: Usd.Prim, name: str, default: None = None) -> Any | None: ...
@overload
def get_attribute(prim: Usd.Prim, name: str, default: Any) -> Any: ...
def get_attribute(prim: Usd.Prim, name: str, default: Any | None = None) -> Any | None:
"""
Get an attribute value from a USD prim, returning a default if not found.
Args:
prim: The USD prim to query.
name: The name of the attribute to retrieve.
default: The default value to return if the attribute is not found or invalid.
Returns:
The attribute value if it exists and is valid, otherwise the default value.
"""
attr = prim.GetAttribute(name)
if not attr or not attr.HasAuthoredValue():
return default
return attr.Get()
def get_attributes_in_namespace(prim: Usd.Prim, namespace: str) -> dict[str, Any]:
"""
Get all attributes in a namespace from a USD prim.
Args:
prim: The USD prim to query.
namespace: The namespace to query.
Returns:
A dictionary of attributes in the namespace mapping from attribute name to value.
"""
out: dict[str, Any] = {}
for prop in prim.GetAuthoredPropertiesInNamespace(namespace):
if not prop.IsValid():
continue
if hasattr(prop, "GetTargets"):
continue
if hasattr(prop, "HasAuthoredValue") and prop.HasAuthoredValue():
out[prop.GetName()] = prop.Get()
return out
def has_attribute(prim: Usd.Prim, name: str) -> bool:
"""
Check if a USD prim has a valid and authored attribute.
Args:
prim: The USD prim to query.
name: The name of the attribute to check.
Returns:
True if the attribute exists, is valid, and has an authored value, False otherwise.
"""
attr = prim.GetAttribute(name)
return attr and attr.HasAuthoredValue()
def has_applied_api_schema(prim: Usd.Prim, schema_name: str) -> bool:
"""
Check if a USD prim has an applied API schema, even if the schema is not
registered with USD's schema registry.
For registered schemas (e.g. ``UsdPhysics.RigidBodyAPI``), ``prim.HasAPI()``
is sufficient. However, non-core schemas that may be in draft state or not
yet registered (e.g. MuJoCo-specific schemas like ``MjcSiteAPI``) will not
be found by ``HasAPI()``. This helper falls back to inspecting the raw
``apiSchemas`` metadata on the prim.
Args:
prim: The USD prim to query.
schema_name: The API schema name to check for (e.g. ``"MjcSiteAPI"``).
Returns:
True if the schema is applied to the prim, False otherwise.
"""
if prim.HasAPI(schema_name):
return True
schemas_listop = prim.GetMetadata("apiSchemas")
if schemas_listop:
all_schemas = (
list(schemas_listop.prependedItems)
+ list(schemas_listop.appendedItems)
+ list(schemas_listop.explicitItems)
)
return schema_name in all_schemas
return False
@overload
def get_float(prim: Usd.Prim, name: str, default: float) -> float: ...
@overload
def get_float(prim: Usd.Prim, name: str, default: None = None) -> float | None: ...
def get_float(prim: Usd.Prim, name: str, default: float | None = None) -> float | None:
"""
Get a float attribute value from a USD prim, validating that it's finite.
Args:
prim: The USD prim to query.
name: The name of the float attribute to retrieve.
default: The default value to return if the attribute is not found or is not finite.
Returns:
The float attribute value if it exists and is finite, otherwise the default value.
"""
attr = prim.GetAttribute(name)
if not attr or not attr.HasAuthoredValue():
return default
val = attr.Get()
if np.isfinite(val):
return val
return default
def get_float_with_fallback(prims: Iterable[Usd.Prim], name: str, default: float = 0.0) -> float:
"""
Get a float attribute value from the first prim in a list that has it defined.
Args:
prims: An iterable of USD prims to query in order.
name: The name of the float attribute to retrieve.
default: The default value to return if no prim has the attribute.
Returns:
The float attribute value from the first prim that has a finite value,
otherwise the default value.
"""
ret = default
for prim in prims:
if not prim:
continue
attr = prim.GetAttribute(name)
if not attr or not attr.HasAuthoredValue():
continue
val = attr.Get()
if np.isfinite(val):
ret = val
break
return ret
@overload
def get_quat(prim: Usd.Prim, name: str, default: wp.quat) -> wp.quat: ...
@overload
def get_quat(prim: Usd.Prim, name: str, default: None = None) -> wp.quat | None: ...
def get_quat(prim: Usd.Prim, name: str, default: wp.quat | None = None) -> wp.quat | None:
"""
Get a quaternion attribute value from a USD prim, validating that it's finite and non-zero.
Args:
prim: The USD prim to query.
name: The name of the quaternion attribute to retrieve.
default: The default value to return if the attribute is not found or invalid.
Returns:
The quaternion attribute value as a Warp quaternion if it exists and is valid,
otherwise the default value.
"""
attr = prim.GetAttribute(name)
if not attr or not attr.HasAuthoredValue():
return default
val = attr.Get()
quat = value_to_warp(val)
l = wp.length(quat)
if np.isfinite(l) and l > 0.0:
return quat
return default
@overload
def get_vector(prim: Usd.Prim, name: str, default: np.ndarray) -> np.ndarray: ...
@overload
def get_vector(prim: Usd.Prim, name: str, default: None = None) -> np.ndarray | None: ...
def get_vector(prim: Usd.Prim, name: str, default: np.ndarray | None = None) -> np.ndarray | None:
"""
Get a vector attribute value from a USD prim, validating that all components are finite.
Args:
prim: The USD prim to query.
name: The name of the vector attribute to retrieve.
default: The default value to return if the attribute is not found or has non-finite values.
Returns:
The vector attribute value as a numpy array with dtype float32 if it exists and
all components are finite, otherwise the default value.
"""
attr = prim.GetAttribute(name)
if not attr or not attr.HasAuthoredValue():
return default
val = attr.Get()
if np.isfinite(val).all():
return np.array(val, dtype=np.float32)
return default
def _get_xform_matrix(
prim: Usd.Prim,
local: bool = True,
xform_cache: UsdGeom.XformCache | None = None,
) -> np.ndarray:
"""
Get the transformation matrix for a USD prim.
Args:
prim: The USD prim to query.
local: If True, get the local transformation; if False, get the world transformation.
xform_cache: Optional USD XformCache to reuse when computing world transforms (only used if ``local`` is False).
Returns:
The transformation matrix as a numpy array (float32).
"""
xform = UsdGeom.Xformable(prim)
if local:
mat = xform.GetLocalTransformation()
# USD may return (matrix, resetXformStack)
if isinstance(mat, tuple):
mat = mat[0]
else:
if xform_cache is None:
time = Usd.TimeCode.Default()
mat = xform.ComputeLocalToWorldTransform(time)
else:
mat = xform_cache.GetLocalToWorldTransform(prim)
return np.array(mat, dtype=np.float32)
def get_scale(prim: Usd.Prim, local: bool = True, xform_cache: UsdGeom.XformCache | None = None) -> wp.vec3:
"""
Extract the scale component from a USD prim's transformation.
Args:
prim: The USD prim to query for scale information.
local: If True, get the local scale; if False, get the world scale.
xform_cache: Optional USD XformCache to reuse when computing world transforms (only used if ``local`` is False).
Returns:
The scale as a Warp vec3.
"""
mat = get_transform_matrix(prim, local=local, xform_cache=xform_cache)
_pos, _rot, scale = wp.transform_decompose(mat)
return wp.vec3(*scale)
def get_gprim_axis(prim: Usd.Prim, name: str = "axis", default: AxisType = "Z") -> Axis:
"""
Get an axis attribute from a USD prim and convert it to an :class:`~newton.Axis` enum.
Args:
prim: The USD prim to query.
name: The name of the axis attribute to retrieve.
default: The default axis string to use if the attribute is not found.
Returns:
An :class:`~newton.Axis` enum value converted from the attribute string.
"""
axis_str = get_attribute(prim, name, default)
return Axis.from_string(axis_str)
def get_transform_matrix(prim: Usd.Prim, local: bool = True, xform_cache: UsdGeom.XformCache | None = None) -> wp.mat44:
"""
Extract the full transformation matrix from a USD Xform prim.
Args:
prim: The USD prim to query.
local: If True, get the local transformation; if False, get the world transformation.
xform_cache: Optional USD XformCache to reuse when computing world transforms (only used if ``local`` is False).
Returns:
A Warp 4x4 transform matrix. This representation composes left-to-right with `@`, matching
`wp.transform_decompose` expectations.
"""
mat = _get_xform_matrix(prim, local=local, xform_cache=xform_cache)
return wp.mat44(mat.T)
def get_transform(prim: Usd.Prim, local: bool = True, xform_cache: UsdGeom.XformCache | None = None) -> wp.transform:
"""
Extract the transform (position and rotation) from a USD Xform prim.
Args:
prim: The USD prim to query.
local: If True, get the local transformation; if False, get the world transformation.
xform_cache: Optional USD XformCache to reuse when computing world transforms (only used if ``local`` is False).
Returns:
A Warp transform containing the position and rotation extracted from the prim.
"""
mat = _get_xform_matrix(prim, local=local, xform_cache=xform_cache)
xform_pos, xform_rot, _scale = wp.transform_decompose(wp.mat44(mat.T))
return wp.transform(xform_pos, xform_rot)
def value_to_warp(v: Any, warp_dtype: Any | None = None) -> Any:
"""
Convert a USD value (such as Gf.Quat, Gf.Vec3, or float) to a Warp value.
If a dtype is given, the value will be converted to that dtype.
Otherwise, the value will be converted to the most appropriate Warp dtype.
Args:
v: The value to convert.
warp_dtype: The Warp dtype to convert to. If None, the value will be converted to the most appropriate Warp dtype.
Returns:
The converted value.
"""
if warp_dtype is wp.quat or (hasattr(v, "real") and hasattr(v, "imaginary")):
return wp.normalize(wp.quat(*v.imaginary, v.real))
if warp_dtype is not None:
# assume the type is a vector, matrix, or scalar
if hasattr(v, "__len__"):
return warp_dtype(*v)
else:
return warp_dtype(v)
# without a given Warp dtype, we attempt to infer the dtype from the value
if hasattr(v, "__len__"):
if len(v) == 2:
return wp.vec2(*v)
if len(v) == 3:
return wp.vec3(*v)
if len(v) == 4:
return wp.vec4(*v)
# the value is a scalar or we weren't able to resolve the dtype
return v
def type_to_warp(v: Any) -> Any:
"""
Determine the Warp type, e.g. wp.quat, wp.vec3, or wp.float32, from a USD value.
Args:
v: The USD value from which to infer the Warp type.
Returns:
The Warp type.
"""
try:
# Check for quat first (before generic length checks)
if hasattr(v, "real") and hasattr(v, "imaginary"):
return wp.quat
# Vector3-like
if hasattr(v, "__len__") and len(v) == 3:
return wp.vec3
# Vector2-like
if hasattr(v, "__len__") and len(v) == 2:
return wp.vec2
# Vector4-like (but not quat)
if hasattr(v, "__len__") and len(v) == 4:
return wp.vec4
except (TypeError, AttributeError):
# fallthrough to scalar checks
pass
if isinstance(v, bool):
return wp.bool
if isinstance(v, int):
return wp.int32
# default to float32 for scalars
return wp.float32
def get_custom_attribute_declarations(prim: Usd.Prim) -> dict[str, ModelBuilder.CustomAttribute]:
"""
Get custom attribute declarations from a USD prim, typically from a ``PhysicsScene`` prim.
Supports metadata format with assignment and frequency specified as ``customData``:
.. code-block:: usda
custom float newton:namespace:attr_name = 150.0 (
customData = {
string assignment = "control"
string frequency = "joint_dof"
}
)
Args:
prim: USD ``PhysicsScene`` prim to parse declarations from.
Returns:
A dictionary of custom attribute declarations mapping from attribute name to :class:`ModelBuilder.CustomAttribute` object.
"""
from ..sim.builder import ModelBuilder # noqa: PLC0415
def is_schema_attribute(prim, attr_name: str) -> bool:
"""Check if attribute is defined by a registered schema."""
# Check the prim's type schema
prim_def = Usd.SchemaRegistry().FindConcretePrimDefinition(prim.GetTypeName())
if prim_def and attr_name in prim_def.GetPropertyNames():
return True
# Check all applied API schemas
for schema_name in prim.GetAppliedSchemas():
api_def = Usd.SchemaRegistry().FindAppliedAPIPrimDefinition(schema_name)
if api_def and attr_name in api_def.GetPropertyNames():
return True
# TODO: handle multi-apply schemas once newton-usd-schemas has support for them
return False
def parse_custom_attr_name(name: str) -> tuple[str | None, str | None]:
"""
Parse custom attribute names in the format 'newton:namespace:attr_name' or 'newton:attr_name'.
Returns:
Tuple of (namespace, attr_name) where namespace can be None for default namespace,
and attr_name can be None if the name is invalid.
"""
parts = name.split(":")
if len(parts) == 2:
# newton:attr_name (default namespace)
return None, parts[1]
elif len(parts) == 3:
# newton:namespace:attr_name
return parts[1], parts[2]
else:
# Invalid format
return None, None
out: dict[str, ModelBuilder.CustomAttribute] = {}
for attr in prim.GetAuthoredPropertiesInNamespace("newton"):
if is_schema_attribute(prim, attr.GetName()):
continue
attr_name = attr.GetName()
namespace, local_name = parse_custom_attr_name(attr_name)
if not local_name:
continue
default_value = attr.Get()
# Try to read customData for assignment and frequency
assignment_meta = attr.GetCustomDataByKey("assignment")
frequency_meta = attr.GetCustomDataByKey("frequency")
if assignment_meta and frequency_meta:
# Metadata format
try:
assignment_val = AttributeAssignment[assignment_meta.upper()]
frequency_val = AttributeFrequency[frequency_meta.upper()]
except KeyError:
print(
f"Warning: Custom attribute '{attr_name}' has invalid assignment or frequency in customData. Skipping."
)
continue
else:
# No metadata found - skip with warning
print(
f"Warning: Custom attribute '{attr_name}' is missing required customData (assignment and frequency). Skipping."
)
continue
# Infer dtype from default value
converted_value = value_to_warp(default_value)
dtype = type_to_warp(default_value)
# Create custom attribute specification
# Note: name should be the local name, namespace is stored separately
custom_attr = ModelBuilder.CustomAttribute(
assignment=assignment_val,
frequency=frequency_val,
name=local_name,
dtype=dtype,
default=converted_value,
namespace=namespace,
)
out[custom_attr.key] = custom_attr
return out
def get_custom_attribute_values(
prim: Usd.Prim,
custom_attributes: Sequence[ModelBuilder.CustomAttribute],
context: dict[str, Any] | None = None,
) -> dict[str, Any]:
"""
Get custom attribute values from a USD prim and a set of known custom attributes.
Returns a dictionary mapping from :attr:`~newton.ModelBuilder.CustomAttribute.key` to the converted Warp value.
The conversion is performed by the ``CustomAttribute.usd_value_transformer`` callable.
The context dictionary passed to the transformer function always contains:
- ``"prim"``: The USD prim to query.
- ``"attr"``: The :class:`~newton.ModelBuilder.CustomAttribute` object to get the value for.
It may additionally include caller-provided keys from the ``context`` argument.
Args:
prim: The USD prim to query.
custom_attributes: The :class:`~newton.ModelBuilder.CustomAttribute` objects to get values for.
context: Optional extra context keys to forward to transformers.
Returns:
A dictionary of found custom attribute values mapping from attribute name to value.
"""
out: dict[str, Any] = {}
for attr in custom_attributes:
transformer_context: dict[str, Any] = {}
if context:
transformer_context.update(context)
# Keep builtin keys authoritative even if caller passes same names.
transformer_context["prim"] = prim
transformer_context["attr"] = attr
usd_attr_name = attr.usd_attribute_name
if usd_attr_name == "*":
# Just apply the transformer to all prims of this frequency
if attr.usd_value_transformer is not None:
value = attr.usd_value_transformer(None, transformer_context)
if value is None:
# Treat None as "undefined" to allow defaults to be applied later.
continue
out[attr.key] = value
continue
usd_attr = prim.GetAttribute(usd_attr_name)
if usd_attr is not None and usd_attr.HasAuthoredValue():
if attr.usd_value_transformer is not None:
value = attr.usd_value_transformer(usd_attr.Get(), transformer_context)
if value is None:
# Treat None as "undefined" to allow defaults to be applied later.
continue
out[attr.key] = value
else:
out[attr.key] = value_to_warp(usd_attr.Get(), attr.dtype)
return out
def _newell_normal(P: np.ndarray) -> np.ndarray:
"""Newell's method for polygon normal (not normalized)."""
x = y = z = 0.0
n = len(P)
for i in range(n):
p0 = P[i]
p1 = P[(i + 1) % n]
x += (p0[1] - p1[1]) * (p0[2] + p1[2])
y += (p0[2] - p1[2]) * (p0[0] + p1[0])
z += (p0[0] - p1[0]) * (p0[1] + p1[1])
return np.array([x, y, z], dtype=np.float64)
def _orthonormal_basis_from_normal(n: np.ndarray):
"""Given a unit normal n, return orthonormal (tangent u, bitangent v, normal n)."""
# Pick the largest non-collinear axis for stability
if abs(n[2]) < 0.9:
a = np.array([0.0, 0.0, 1.0])
else:
a = np.array([1.0, 0.0, 0.0])
u = np.cross(a, n)
nu = np.linalg.norm(u)
if nu < 1e-20:
# fallback (degenerate normal); pick arbitrary
u = np.array([1.0, 0.0, 0.0])
else:
u /= nu
v = np.cross(n, u)
return u, v, n
def corner_angles(face_pos: np.ndarray) -> np.ndarray:
"""
Compute interior corner angles (radians) for a single polygon face.
Args:
face_pos: (N, 3) float array
Vertex positions of the face in winding order (CW or CCW).
Returns:
angles: (N,) float array
Interior angle at each vertex in [0, pi] (radians). For degenerate
corners/edges, the angle is set to 0.
"""
P = np.asarray(face_pos, dtype=np.float64)
N = len(P)
if N < 3:
return np.zeros((N,), dtype=np.float64)
# Face plane via Newell
n = _newell_normal(P)
n_norm = np.linalg.norm(n)
if n_norm < 1e-20:
# Degenerate polygon (nearly collinear); fallback: use 3D formula via atan2 on cross/dot
# after constructing tangents from edges. But simplest is to return zeros.
return np.zeros((N,), dtype=np.float64)
n /= n_norm
# Local 2D frame on the plane
u, v, _ = _orthonormal_basis_from_normal(n)
# Project to 2D (u,v)
# (subtract centroid for numerical stability)
c = P.mean(axis=0)
Q = P - c
x = Q @ u # (N,)
y = Q @ v # (N,)
# Roll arrays to get prev/next for each vertex
x_prev = np.roll(x, 1)
y_prev = np.roll(y, 1)
x_next = np.roll(x, -1)
y_next = np.roll(y, -1)
# Edge vectors at each corner (pointing into the corner from prev/next to current)
# a: current->prev, b: current->next (sign doesn't matter for angle magnitude)
ax = x_prev - x
ay = y_prev - y
bx = x_next - x
by = y_next - y
# Normalize edge vectors to improve numerical stability on very different scales
a_len = np.hypot(ax, ay)
b_len = np.hypot(bx, by)
valid = (a_len > 1e-30) & (b_len > 1e-30)
ax[valid] /= a_len[valid]
ay[valid] /= a_len[valid]
bx[valid] /= b_len[valid]
by[valid] /= b_len[valid]
# Angle via atan2(||a x b||, a·b) in 2D; ||a x b|| = |ax*by - ay*bx|
cross = ax * by - ay * bx
dot = ax * bx + ay * by
# Clamp dot to [-1,1] only where needed; atan2 handles it well, but clamp helps with noise
dot = np.clip(dot, -1.0, 1.0)
angles = np.zeros((N,), dtype=np.float64)
angles[valid] = np.arctan2(np.abs(cross[valid]), dot[valid]) # [0, pi]
return angles
def fan_triangulate_faces(counts: np.ndarray, indices: np.ndarray) -> np.ndarray:
"""
Perform fan triangulation on polygonal faces.
Args:
counts: Array of vertex counts per face
indices: Flattened array of vertex indices
Returns:
Array of shape (num_triangles, 3) containing triangle indices (dtype=np.int32)
"""
counts = np.asarray(counts, dtype=np.int32)
indices = np.asarray(indices, dtype=np.int32)
num_tris = int(np.sum(counts - 2))
if num_tris == 0:
return np.zeros((0, 3), dtype=np.int32)
# Vectorized approach: build all triangle indices at once
# For each face with n vertices, we create (n-2) triangles
# Each triangle uses: [base, base+i+1, base+i+2] for i in range(n-2)
# Array to track which face each triangle belongs to
tri_face_ids = np.repeat(np.arange(len(counts), dtype=np.int32), counts - 2)
# Array for triangle index within each face (0 to n-3)
tri_local_ids = np.concatenate([np.arange(n - 2, dtype=np.int32) for n in counts])
# Base index for each face
face_bases = np.concatenate([[0], np.cumsum(counts[:-1], dtype=np.int32)])
out = np.empty((num_tris, 3), dtype=np.int32)
out[:, 0] = indices[face_bases[tri_face_ids]] # First vertex (anchor)
out[:, 1] = indices[face_bases[tri_face_ids] + tri_local_ids + 1] # Second vertex
out[:, 2] = indices[face_bases[tri_face_ids] + tri_local_ids + 2] # Third vertex
return out
def _expand_indexed_primvar(
values: np.ndarray,
indices: np.ndarray | None,
primvar_name: str,
prim_path: str,
) -> np.ndarray:
"""
Expand primvar values using indices if provided.
USD primvars can be stored in an indexed form where a compact set of unique
values is stored along with an index array that maps each face corner (or vertex)
to the appropriate value. This function expands such indexed primvars to their
full form.
Args:
values: The primvar values array.
indices: Optional index array for expansion.
primvar_name: Name of the primvar (for error messages).
prim_path: Path to the prim (for error messages).
Returns:
The expanded values array (same as input if no indices provided).
Raises:
ValueError: If indices are out of range.
"""
if indices is None or len(indices) == 0:
return values
indices = np.asarray(indices, dtype=np.int64)
# Validate indices are within range
if indices.max() >= len(values):
raise ValueError(
f"{primvar_name} primvar index out of range: max index {indices.max()} >= "
f"number of values {len(values)} for mesh {prim_path}"
)
if indices.min() < 0:
raise ValueError(f"Negative {primvar_name} primvar index found: {indices.min()} for mesh {prim_path}")
return values[indices]
def _triangulate_face_varying_indices(counts: Sequence[int], flip_winding: bool) -> np.ndarray:
"""Return flattened corner indices for fan-triangulated face-varying data."""
counts_i32 = np.asarray(counts, dtype=np.int32)
num_tris = int(np.sum(counts_i32 - 2))
if num_tris <= 0:
return np.zeros((0,), dtype=np.int32)
tri_face_ids = np.repeat(np.arange(len(counts_i32), dtype=np.int32), counts_i32 - 2)
tri_local_ids = np.concatenate([np.arange(n - 2, dtype=np.int32) for n in counts_i32])
face_bases = np.concatenate([[0], np.cumsum(counts_i32[:-1], dtype=np.int32)])
corner_faces = np.empty((num_tris, 3), dtype=np.int32)
corner_faces[:, 0] = face_bases[tri_face_ids]
corner_faces[:, 1] = face_bases[tri_face_ids] + tri_local_ids + 1
corner_faces[:, 2] = face_bases[tri_face_ids] + tri_local_ids + 2
if flip_winding:
corner_faces = corner_faces[:, ::-1]
return corner_faces.reshape(-1)
@overload
def get_mesh(
prim: Usd.Prim,
load_normals: bool = False,
load_uvs: bool = False,
maxhullvert: int | None = None,
face_varying_normal_conversion: Literal[
"vertex_averaging", "angle_weighted", "vertex_splitting"
] = "vertex_splitting",
vertex_splitting_angle_threshold_deg: float = 25.0,
preserve_facevarying_uvs: bool = False,
return_uv_indices: Literal[False] = False,
) -> Mesh: ...
@overload
def get_mesh(
prim: Usd.Prim,
load_normals: bool = False,
load_uvs: bool = False,
maxhullvert: int | None = None,
face_varying_normal_conversion: Literal[
"vertex_averaging", "angle_weighted", "vertex_splitting"
] = "vertex_splitting",
vertex_splitting_angle_threshold_deg: float = 25.0,
preserve_facevarying_uvs: bool = False,
return_uv_indices: Literal[True] = True,
) -> tuple[Mesh, np.ndarray | None]: ...
def get_mesh(
prim: Usd.Prim,
load_normals: bool = False,
load_uvs: bool = False,
maxhullvert: int | None = None,
face_varying_normal_conversion: Literal[
"vertex_averaging", "angle_weighted", "vertex_splitting"
] = "vertex_splitting",
vertex_splitting_angle_threshold_deg: float = 25.0,
preserve_facevarying_uvs: bool = False,
return_uv_indices: bool = False,
) -> Mesh | tuple[Mesh, np.ndarray | None]:
"""
Load a triangle mesh from a USD prim that has the ``UsdGeom.Mesh`` schema.
Example:
.. testcode::
from pxr import Usd
import newton.examples
import newton.usd
usd_stage = Usd.Stage.Open(newton.examples.get_asset("bunny.usd"))
demo_mesh = newton.usd.get_mesh(usd_stage.GetPrimAtPath("/root/bunny"), load_normals=True)
builder = newton.ModelBuilder()
body_mesh = builder.add_body()
builder.add_shape_mesh(body_mesh, mesh=demo_mesh)
assert len(demo_mesh.vertices) == 6102
assert len(demo_mesh.indices) == 36600
assert len(demo_mesh.normals) == 6102
Args:
prim (Usd.Prim): The USD prim to load the mesh from.
load_normals (bool): Whether to load the normals.
load_uvs (bool): Whether to load the UVs.
maxhullvert (int): The maximum number of vertices for the convex hull approximation.
face_varying_normal_conversion (Literal["vertex_averaging", "angle_weighted", "vertex_splitting"]):
This argument specifies how to convert "faceVarying" normals
(normals defined per-corner rather than per-vertex) into per-vertex normals for the mesh.
If ``load_normals`` is False, this argument is ignored.
The options are summarized below:
.. list-table::
:widths: 20 80
:header-rows: 1
* - Method
- Description
* - ``"vertex_averaging"``
- For each vertex, averages all the normals of the corners that share that vertex. This produces smooth shading except at explicit vertex splits. This method is the most efficient.
* - ``"angle_weighted"``
- For each vertex, computes a weighted average of the normals of the corners it belongs to, using the corner angle as a weight (i.e., larger face angles contribute more), for more visually-accurate smoothing at sharp edges.
* - ``"vertex_splitting"``
- Splits a vertex into multiple vertices if the difference between the corner normals exceeds a threshold angle (see ``vertex_splitting_angle_threshold_deg``). This preserves sharp features by assigning separate (duplicated) vertices to corners with widely different normals.
vertex_splitting_angle_threshold_deg (float): The threshold angle in degrees for splitting vertices based on the face normals in case of faceVarying normals and ``face_varying_normal_conversion`` is "vertex_splitting". Corners whose normals differ by more than angle_deg will be split
into different vertex clusters. Lower = more splits (sharper), higher = fewer splits (smoother).
preserve_facevarying_uvs (bool): If True, keep faceVarying UVs in their
original corner layout and avoid UV-driven vertex splitting. The
returned mesh keeps its original topology. This is useful when the
caller needs the original UV indexing (e.g., panel-space cloth).
return_uv_indices (bool): If True, return a tuple ``(mesh, uv_indices)``
where ``uv_indices`` is a flattened triangle index buffer for the
UVs when available. For faceVarying UVs and
``preserve_facevarying_uvs=True``, these indices reference the
face-varying UV array.
Returns:
newton.Mesh: The loaded mesh, or ``(mesh, uv_indices)`` if
``return_uv_indices`` is True.
"""
if maxhullvert is None:
maxhullvert = Mesh.MAX_HULL_VERTICES
mesh = UsdGeom.Mesh(prim)
points = np.array(mesh.GetPointsAttr().Get(), dtype=np.float64)
indices = np.array(mesh.GetFaceVertexIndicesAttr().Get(), dtype=np.int32)
counts = mesh.GetFaceVertexCountsAttr().Get()
uvs = None
uvs_interpolation = None
# Tracks whether we already duplicated vertices (and per-vertex UVs) during
# faceVarying normal conversion, so we don't split again in the UV pass.
did_split_vertices = False
if load_uvs:
uv_primvar = UsdGeom.PrimvarsAPI(prim).GetPrimvar("st")
if uv_primvar:
uvs = uv_primvar.Get()
if uvs is not None:
uvs = np.array(uvs)
# Get interpolation from primvar
uvs_interpolation = uv_primvar.GetInterpolation()
# Check if this primvar is indexed and expand if so
if uv_primvar.IsIndexed():
uv_indices = uv_primvar.GetIndices()
uvs = _expand_indexed_primvar(uvs, uv_indices, "UV", str(prim.GetPath()))
normals = None
normals_interpolation = None
normal_indices = None
if load_normals:
# First, try to load normals from primvars:normals (takes precedence)
normals_primvar = UsdGeom.PrimvarsAPI(prim).GetPrimvar("normals")
if normals_primvar:
normals = normals_primvar.Get()
if normals is not None:
# Use primvar interpolation
normals_interpolation = normals_primvar.GetInterpolation()
# Check for primvar indices
if normals_primvar.IsIndexed():
normal_indices = normals_primvar.GetIndices()
# Fall back to direct attribute access for backwards compatibility
if normal_indices is None:
normals_index_attr = prim.GetAttribute("primvars:normals:indices")
if normals_index_attr and normals_index_attr.HasAuthoredValue():
normal_indices = normals_index_attr.Get()
# Fall back to mesh.GetNormalsAttr() only if primvar is not present or has no data
if normals is None:
normals_attr = mesh.GetNormalsAttr()
if normals_attr:
normals = normals_attr.Get()
if normals is not None:
# Use mesh normals interpolation (only relevant for non-primvar normals)
normals_interpolation = mesh.GetNormalsInterpolation()
if normals is not None:
normals = np.array(normals, dtype=np.float64)
if normals_interpolation == UsdGeom.Tokens.faceVarying:
prim_path = str(prim.GetPath())
if normal_indices is not None and len(normal_indices) > 0:
normals_fv = _expand_indexed_primvar(normals, normal_indices, "Normal", prim_path)
else:
# If faceVarying, values length must match number of corners
if len(normals) != len(indices):
raise ValueError(
f"Length of normals ({len(normals)}) does not match length of indices ({len(indices)}) for mesh {prim_path}"
)
normals_fv = normals # (C,3)
V = len(points)
accum = np.zeros((V, 3), dtype=np.float64)
if face_varying_normal_conversion == "vertex_splitting":
C = len(indices)
Nfv = np.asarray(normals_fv, dtype=np.float64)
if indices.shape[0] != Nfv.shape[0]:
raise ValueError(
f"Length of indices ({indices.shape[0]}) does not match length of faceVarying normals ({Nfv.shape[0]}) for mesh {prim.GetPath()}"
)
# Normalize corner normals (direction only)
nlen = np.linalg.norm(Nfv, axis=1, keepdims=True)
nlen = np.clip(nlen, 1e-30, None)
Ndir = Nfv / nlen
cos_thresh = np.cos(np.deg2rad(vertex_splitting_angle_threshold_deg))
# For each original vertex v, we'll keep a list of clusters:
# each cluster stores (sum_dir, count, new_vid)
clusters_per_v = [[] for _ in range(V)]
new_points = []
new_norm_sums = [] # accumulate directions per new vertex id
new_indices = np.empty_like(indices)
new_uvs = [] if uvs is not None else None
# Helper to create a new vertex clone from original v
def _new_vertex_from(v, n_dir, corner_idx):
new_vid = len(new_points)
new_points.append(points[v])
new_norm_sums.append(n_dir.copy())
clusters_per_v[v].append([n_dir.copy(), 1, new_vid])
if new_uvs is not None:
# Use corner UV if faceVarying, otherwise use vertex UV
if uvs_interpolation == UsdGeom.Tokens.faceVarying:
new_uvs.append(uvs[corner_idx])
else:
new_uvs.append(uvs[v])
return new_vid
# Assign each corner to a cluster (new vertex) based on angular proximity
for c in range(C):
v = int(indices[c])
n_dir = Ndir[c]
clusters = clusters_per_v[v]
assigned = False
# try to match an existing cluster
for cl in clusters:
sum_dir, cnt, new_vid = cl
# compare with current mean direction (sum_dir normalized)
mean_dir = sum_dir / max(np.linalg.norm(sum_dir), 1e-30)
if float(np.dot(mean_dir, n_dir)) >= cos_thresh:
# assign to this cluster
cl[0] = sum_dir + n_dir
cl[1] = cnt + 1
new_norm_sums[new_vid] += n_dir
new_indices[c] = new_vid
assigned = True
break
if not assigned:
new_vid = _new_vertex_from(v, n_dir, c)
new_indices[c] = new_vid
new_points = np.asarray(new_points, dtype=np.float64)
# Produce per-vertex normalized normals for the new vertices
new_norm_sums = np.asarray(new_norm_sums, dtype=np.float64)
nn = np.linalg.norm(new_norm_sums, axis=1, keepdims=True)
nn = np.clip(nn, 1e-30, None)
new_vertex_normals = (new_norm_sums / nn).astype(np.float32)
points = new_points
indices = new_indices
normals = new_vertex_normals
uvs = new_uvs
# Vertex splitting creates a new per-vertex layout (and UVs
# if available). Skip the later faceVarying UV split to avoid
# dropping/duplicating UVs.
did_split_vertices = True
elif face_varying_normal_conversion == "vertex_averaging":
# basic averaging
for c, v in enumerate(indices):
accum[v] += normals_fv[c]
# normalize
lengths = np.linalg.norm(accum, axis=1, keepdims=True)
lengths[lengths < 1e-20] = 1.0
# vertex normals
normals = (accum / lengths).astype(np.float32)
elif face_varying_normal_conversion == "angle_weighted":
# area- or corner-angle weighting
offset = 0
for nverts in counts:
face_idx = indices[offset : offset + nverts]
face_pos = points[face_idx] # (n,3)
# compute per-corner angles at each vertex in the face (omitted here for brevity)
weights = corner_angles(face_pos) # (n,)
for i in range(nverts):
v = face_idx[i]
accum[v] += normals_fv[offset + i] * weights[i]
offset += nverts
vertex_normals = accum / np.clip(np.linalg.norm(accum, axis=1, keepdims=True), 1e-20, None)
normals = vertex_normals.astype(np.float32)
else:
raise ValueError(f"Invalid face_varying_normal_conversion: {face_varying_normal_conversion}")
faces = fan_triangulate_faces(counts, indices)
flip_winding = False
orientation_attr = mesh.GetOrientationAttr()
if orientation_attr:
handedness = orientation_attr.Get()
if handedness and handedness.lower() == "lefthanded":
flip_winding = True
if flip_winding:
faces = faces[:, ::-1]
uv_indices = None
if uvs is not None:
uvs = np.array(uvs, dtype=np.float32)
# If vertices were already split for faceVarying normals, UVs (if any)
# were converted to per-vertex. Avoid a second split here.
if uvs_interpolation == UsdGeom.Tokens.faceVarying and not did_split_vertices:
if len(uvs) != len(indices):
warnings.warn(
f"UV primvar length ({len(uvs)}) does not match indices length ({len(indices)}) for mesh {prim.GetPath()}; "
"dropping UVs.",
stacklevel=2,
)
uvs = None
else:
corner_flat = _triangulate_face_varying_indices(counts, flip_winding)
if not preserve_facevarying_uvs:
points_original = points
points = points_original[indices[corner_flat]]
if normals is not None:
if len(normals) == len(points_original):
normals = normals[indices[corner_flat]]
elif len(normals) == len(corner_flat):
normals = normals[corner_flat]
else:
warnings.warn(
f"Normals length ({len(normals)}) does not match vertices after UV splitting for mesh {prim.GetPath()}; "
"dropping normals.",
stacklevel=2,
)
normals = None
uvs = uvs[corner_flat]
faces = np.arange(len(corner_flat), dtype=np.int32).reshape(-1, 3)
elif return_uv_indices:
uv_indices = corner_flat
if return_uv_indices and uvs is not None and uv_indices is None:
uv_indices = faces.reshape(-1)
material_props = resolve_material_properties_for_prim(prim)
mesh_out = Mesh(
points,
faces.flatten(),
normals=normals,
uvs=uvs,
maxhullvert=maxhullvert,
color=material_props.get("color"),
texture=material_props.get("texture"),
metallic=material_props.get("metallic"),
roughness=material_props.get("roughness"),
)
if return_uv_indices:
return mesh_out, uv_indices
return mesh_out
# Schema-defined TetMesh attribute names excluded from custom attribute parsing.
_TETMESH_SCHEMA_ATTRS = frozenset(
{
"points",
"tetVertexIndices",
"surfaceFaceVertexIndices",
"extent",
"orientation",
"purpose",
"visibility",
"xformOpOrder",
"proxyPrim",
}
)
def get_tetmesh(prim: Usd.Prim) -> TetMesh:
"""Load a tetrahedral mesh from a USD prim with the ``UsdGeom.TetMesh`` schema.
Reads vertex positions from the ``points`` attribute and tetrahedral
connectivity from ``tetVertexIndices``. If a physics material is bound
to the prim (via ``material:binding:physics``) and contains
``youngsModulus``, ``poissonsRatio``, or ``density`` attributes
(under the ``omniphysics:`` or ``physxDeformableBody:`` namespaces),
those values are read and converted to Lame parameters (``k_mu``,
``k_lambda``) and density on the returned TetMesh. Material properties
are set to ``None`` if not present.
Example:
.. code-block:: python
from pxr import Usd
import newton
import newton.usd
usd_stage = Usd.Stage.Open("tetmesh.usda")
tetmesh = newton.usd.get_tetmesh(usd_stage.GetPrimAtPath("/MyTetMesh"))
# tetmesh.vertices -- np.ndarray, shape (N, 3)
# tetmesh.tet_indices -- np.ndarray, flattened (4 per tet)
Args:
prim: The USD prim to load the tetrahedral mesh from.
Returns:
TetMesh: A :class:`newton.TetMesh` with vertex positions and tet connectivity.
"""
from ..geometry.types import TetMesh # noqa: PLC0415
tet_mesh = UsdGeom.TetMesh(prim)
points_attr = tet_mesh.GetPointsAttr().Get()
if points_attr is None:
raise ValueError(f"TetMesh prim '{prim.GetPath()}' has no points attribute.")
tet_indices_attr = tet_mesh.GetTetVertexIndicesAttr().Get()
if tet_indices_attr is None:
raise ValueError(f"TetMesh prim '{prim.GetPath()}' has no tetVertexIndices attribute.")
vertices = np.array(points_attr, dtype=np.float32)
tet_indices = np.array(tet_indices_attr, dtype=np.int32).flatten()
# Flip winding order for left-handed meshes (e.g. Houdini exports)
handedness = tet_mesh.GetOrientationAttr().Get()
if handedness and handedness.lower() == "lefthanded" and tet_indices.size % 4 == 0:
tet_indices = tet_indices.reshape(-1, 4)
tet_indices[:, [1, 2]] = tet_indices[:, [2, 1]]
tet_indices = tet_indices.reshape(-1)
# Try to read physics material properties if bound
k_mu = None
k_lambda = None
density = None
material_prim = _find_physics_material_prim(prim)
if material_prim is not None:
youngs = _read_physics_attr(material_prim, "youngsModulus")
poissons = _read_physics_attr(material_prim, "poissonsRatio")
density_val = _read_physics_attr(material_prim, "density")
if youngs is not None and poissons is not None:
E = float(youngs)
nu = float(poissons)
# Clamp Poisson's ratio to the open interval (-1, 0.5) to avoid
# division by zero in the Lame parameter conversion.
nu = max(-0.999, min(nu, 0.499))
k_mu = E / (2.0 * (1.0 + nu))
k_lambda = E * nu / ((1.0 + nu) * (1.0 - 2.0 * nu))
if density_val is not None:
density = float(density_val)
# Read custom primvars and attributes (per-vertex, per-tet, etc.)
# Primvar interpolation is used to determine the attribute frequency
# when available, falling back to length-based inference in TetMesh.__init__.
from ..sim.model import Model as _Model # noqa: PLC0415
# USD interpolation → Newton frequency for TetMesh prims.
# "uniform" means one value per geometric element (cell); for a TetMesh
# the cells are tetrahedra, so it maps to TETRAHEDRON.
_INTERP_TO_FREQ = {
"vertex": _Model.AttributeFrequency.PARTICLE,
"varying": _Model.AttributeFrequency.PARTICLE,
"uniform": _Model.AttributeFrequency.TETRAHEDRON,
"constant": _Model.AttributeFrequency.ONCE,
}
custom_attributes: dict[str, np.ndarray | tuple[np.ndarray, _Model.AttributeFrequency]] = {}
primvars_api = UsdGeom.PrimvarsAPI(prim)
for primvar in primvars_api.GetPrimvarsWithValues():
name = primvar.GetPrimvarName()
if name in ("st", "normals"):
continue # skip well-known primvars handled elsewhere
val = primvar.Get()
if val is not None:
arr = np.array(val)
interp = primvar.GetInterpolation()
freq = _INTERP_TO_FREQ.get(interp)
if freq is not None:
custom_attributes[str(name)] = (arr, freq)
else:
# Unknown interpolation — let TetMesh infer from length
custom_attributes[str(name)] = arr
# Also read non-schema custom attributes (not primvars, not relationships)
for attr in prim.GetAttributes():
name = attr.GetName()
if name in _TETMESH_SCHEMA_ATTRS:
continue
if name.startswith("primvars:") or name.startswith("xformOp:"):
continue
if not attr.HasAuthoredValue():
continue
val = attr.Get()
if val is not None:
try:
arr = np.array(val)
if arr.ndim >= 1:
custom_attributes[name] = arr
except (TypeError, ValueError):
pass # skip non-array attributes
return TetMesh(
vertices=vertices,
tet_indices=tet_indices,
k_mu=k_mu,
k_lambda=k_lambda,
density=density,
custom_attributes=custom_attributes if custom_attributes else None,
)
def _find_physics_material_prim(prim: Usd.Prim):
"""Find the physics material prim bound to a prim or its ancestors."""
p = prim
while p and p.IsValid():
binding_api = UsdShade.MaterialBindingAPI(p)
rel = binding_api.GetDirectBindingRel("physics")
if rel and rel.GetTargets():
mat_path = rel.GetTargets()[0]
mat_prim = prim.GetStage().GetPrimAtPath(mat_path)
if mat_prim and mat_prim.IsValid():
return mat_prim
p = p.GetParent()
return None
def _read_physics_attr(prim: Usd.Prim, name: str):
"""Read a physics attribute from a prim, trying known namespaces."""
for prefix in ("omniphysics:", "physxDeformableBody:", "physics:"):
attr = prim.GetAttribute(f"{prefix}{name}")
if attr and attr.HasAuthoredValue():
return attr.Get()
return None
def find_tetmesh_prims(stage: Usd.Stage) -> list[Usd.Prim]:
"""Find all prims with the ``UsdGeom.TetMesh`` schema in a USD stage.
Example:
.. code-block:: python
from pxr import Usd
import newton.usd
stage = Usd.Stage.Open("scene.usda")
prims = newton.usd.find_tetmesh_prims(stage)
tetmeshes = [newton.usd.get_tetmesh(p) for p in prims]
Args:
stage: The USD stage to search.
Returns:
list[Usd.Prim]: All prims in the stage that have the TetMesh schema.
"""
return [prim for prim in stage.Traverse() if prim.IsA(UsdGeom.TetMesh)]
def _resolve_asset_path(
asset: Sdf.AssetPath | str | os.PathLike[str] | None,
prim: Usd.Prim,
asset_attr: Any | None = None,
) -> str | None:
"""Resolve a USD asset reference to a usable path or URL.
Args:
asset: The asset value or asset path authored on a shader input.
prim: The prim providing the stage context for relative paths.
asset_attr: Optional USD attribute providing authored layer resolution.
Returns:
Absolute path or URL to the asset, or ``None`` when missing.
"""
if asset is None:
return None
if asset_attr is not None:
try:
resolved_attr_path = asset_attr.GetResolvedPath()
except Exception:
resolved_attr_path = None
if resolved_attr_path:
return resolved_attr_path
if isinstance(asset, Sdf.AssetPath):
if asset.resolvedPath:
return asset.resolvedPath
asset_path = asset.path
elif isinstance(asset, os.PathLike):
asset_path = os.fspath(asset)
elif isinstance(asset, str):
asset_path = asset
else:
# Ignore non-path inputs (e.g. numeric shader parameters).
return None
if not asset_path:
return None
if asset_path.startswith(("http://", "https://", "file:")):
return asset_path
if os.path.isabs(asset_path):
return asset_path
source_layer = None
if asset_attr is not None:
try:
resolve_info = asset_attr.GetResolveInfo()
except Exception:
resolve_info = None
if resolve_info is not None:
for getter_name in ("GetSourceLayer", "GetLayer"):
getter = getattr(resolve_info, getter_name, None)
if getter is None:
continue
try:
source_layer = getter()
except Exception:
source_layer = None
if source_layer is not None:
break
if source_layer is None:
try:
spec = asset_attr.GetSpec()
except Exception:
spec = None
if spec is not None:
source_layer = getattr(spec, "layer", None)
root_layer = prim.GetStage().GetRootLayer()
base_layer = source_layer or root_layer
if base_layer is not None:
try:
resolved = Sdf.ComputeAssetPathRelativeToLayer(base_layer, asset_path)
except Exception:
resolved = None
if resolved:
return resolved
base_dir = os.path.dirname(base_layer.realPath or base_layer.identifier or "")
if base_dir:
return os.path.abspath(os.path.join(base_dir, asset_path))
return asset_path
def _find_texture_in_shader(shader: UsdShade.Shader | None, prim: Usd.Prim) -> str | None:
"""Search a shader network for a connected texture asset.
Args:
shader: The shader node to inspect.
prim: The prim providing stage context for asset resolution.
Returns:
Resolved texture asset path, or ``None`` if not found.
"""
if shader is None:
return None
shader_id = shader.GetIdAttr().Get()
if shader_id == "UsdUVTexture":
file_input = shader.GetInput("file")
if file_input:
attrs = UsdShade.Utils.GetValueProducingAttributes(file_input)
if attrs:
asset = attrs[0].Get()
return _resolve_asset_path(asset, prim, attrs[0])
return None
if shader_id == "UsdPreviewSurface":
for input_name in ("diffuseColor", "baseColor"):
shader_input = shader.GetInput(input_name)
if shader_input:
source = shader_input.GetConnectedSource()
if source:
source_shader = UsdShade.Shader(source[0].GetPrim())
texture = _find_texture_in_shader(source_shader, prim)
if texture:
return texture
return None
def _get_input_value(shader: UsdShade.Shader | None, names: tuple[str, ...]) -> Any | None:
"""Fetch the effective input value from a shader, following connections."""
if shader is None:
return None
try:
if not shader.GetPrim().IsValid():
return None
except Exception:
return None
for name in names:
inp = shader.GetInput(name)
if inp is None:
continue
try:
attrs = UsdShade.Utils.GetValueProducingAttributes(inp)
except Exception:
continue
if attrs:
value = attrs[0].Get()
if value is not None:
return value
return None
def _empty_material_properties() -> dict[str, Any]:
"""Return an empty material properties dictionary."""
return {"color": None, "metallic": None, "roughness": None, "texture": None}
def _coerce_color(value: Any) -> tuple[float, float, float] | None:
"""Coerce a value to an RGB color tuple, or None if not possible."""
if value is None:
return None
color_np = np.array(value, dtype=np.float32).reshape(-1)
if color_np.size >= 3:
return (float(color_np[0]), float(color_np[1]), float(color_np[2]))
return None
def _coerce_float(value: Any) -> float | None:
"""Coerce a value to a float, or None if not possible."""
if value is None:
return None
try:
return float(value)
except (TypeError, ValueError):
return None
def _extract_preview_surface_properties(shader: UsdShade.Shader | None, prim: Usd.Prim) -> dict[str, Any]:
"""Extract material properties from a UsdPreviewSurface shader.
Args:
shader: The UsdPreviewSurface shader node to inspect.
prim: The prim providing stage context for asset resolution.
Returns:
Dictionary with ``color``, ``metallic``, ``roughness``, and ``texture``.
"""
properties = _empty_material_properties()
if shader is None:
return properties
shader_id = shader.GetIdAttr().Get()
if shader_id != "UsdPreviewSurface":
return properties
color_input = shader.GetInput("baseColor") or shader.GetInput("diffuseColor")
if color_input:
source = color_input.GetConnectedSource()
if source:
source_shader = UsdShade.Shader(source[0].GetPrim())
properties["texture"] = _find_texture_in_shader(source_shader, prim)
if properties["texture"] is None:
color_value = _get_input_value(
source_shader,
(
"diffuseColor",
"baseColor",
"diffuse_color",
"base_color",
"diffuse_color_constant",
"displayColor",
),
)
properties["color"] = _coerce_color(color_value)
else:
properties["color"] = _coerce_color(color_input.Get())
metallic_input = shader.GetInput("metallic")
if metallic_input:
try:
has_metallic_source = metallic_input.HasConnectedSource()
except Exception:
has_metallic_source = False
if has_metallic_source:
source = metallic_input.GetConnectedSource()
source_shader = UsdShade.Shader(source[0].GetPrim()) if source else None
metallic_value = _get_input_value(source_shader, ("metallic", "metallic_constant"))
properties["metallic"] = _coerce_float(metallic_value)
if properties["metallic"] is None:
warnings.warn(
"Metallic texture inputs are not yet supported; using scalar fallback.",
stacklevel=2,
)
else:
properties["metallic"] = _coerce_float(metallic_input.Get())
roughness_input = shader.GetInput("roughness")
if roughness_input:
try:
has_roughness_source = roughness_input.HasConnectedSource()
except Exception:
has_roughness_source = False
if has_roughness_source:
source = roughness_input.GetConnectedSource()
source_shader = UsdShade.Shader(source[0].GetPrim()) if source else None
roughness_value = _get_input_value(
source_shader,
("roughness", "roughness_constant", "reflection_roughness_constant"),
)
properties["roughness"] = _coerce_float(roughness_value)
if properties["roughness"] is None:
warnings.warn(
"Roughness texture inputs are not yet supported; using scalar fallback.",
stacklevel=2,
)
else:
properties["roughness"] = _coerce_float(roughness_input.Get())
return properties
def _extract_shader_properties(shader: UsdShade.Shader | None, prim: Usd.Prim) -> dict[str, Any]:
"""Extract common material properties from a shader node.
This routine starts with UsdPreviewSurface parsing and then falls back to
common input names used by other shader implementations.
Args:
shader: The shader node to inspect.
prim: The prim providing stage context for asset resolution.
Returns:
Dictionary with ``color``, ``metallic``, ``roughness``, and ``texture``.
"""
properties = _extract_preview_surface_properties(shader, prim)
if shader is None:
return properties
try:
if not shader.GetPrim().IsValid():
return properties
except Exception:
return properties
if properties["color"] is None:
color_value = _get_input_value(
shader,
(
"diffuse_color_constant",
"diffuse_color",
"diffuseColor",
"base_color",
"baseColor",
"displayColor",
),
)
properties["color"] = _coerce_color(color_value)
if properties["metallic"] is None:
metallic_value = _get_input_value(shader, ("metallic_constant", "metallic"))
properties["metallic"] = _coerce_float(metallic_value)
if properties["roughness"] is None:
roughness_value = _get_input_value(shader, ("reflection_roughness_constant", "roughness_constant", "roughness"))
properties["roughness"] = _coerce_float(roughness_value)
if properties["texture"] is None:
for inp in shader.GetInputs():
name = inp.GetBaseName()
if inp.HasConnectedSource():
source = inp.GetConnectedSource()
source_shader = UsdShade.Shader(source[0].GetPrim())
texture = _find_texture_in_shader(source_shader, prim)
if texture:
properties["texture"] = texture
break
elif "file" in name or "texture" in name:
asset = inp.Get()
if asset:
properties["texture"] = _resolve_asset_path(asset, prim, inp.GetAttr())
break
return properties
def _extract_material_input_properties(material: UsdShade.Material | None, prim: Usd.Prim) -> dict[str, Any]:
"""Extract material properties from inputs on a UsdShade.Material prim.
This supports assets that author texture references directly on the Material,
without creating a shader network.
"""
properties = _empty_material_properties()
if material is None:
return properties
for inp in material.GetInputs():
name = inp.GetBaseName()
name_lower = name.lower()
try:
if inp.HasConnectedSource():
continue
except Exception:
continue
value = inp.Get()
if value is None:
continue
if properties["texture"] is None and ("texture" in name_lower or "file" in name_lower):
texture = _resolve_asset_path(value, prim, inp.GetAttr())
if texture:
properties["texture"] = texture
continue
if properties["color"] is None and name_lower in (
"diffusecolor",
"basecolor",
"diffuse_color",
"base_color",
"displaycolor",
):
color = _coerce_color(value)
if color is not None:
properties["color"] = color
continue
if properties["metallic"] is None and name_lower in ("metallic", "metallic_constant"):
metallic = _coerce_float(value)
if metallic is not None:
properties["metallic"] = metallic
continue
if properties["roughness"] is None and name_lower in (
"roughness",
"roughness_constant",
"reflection_roughness_constant",
):
roughness = _coerce_float(value)
if roughness is not None:
properties["roughness"] = roughness
return properties
def _get_bound_material(target_prim: Usd.Prim) -> UsdShade.Material | None:
"""Get the material bound to a prim."""
if not target_prim or not target_prim.IsValid():
return None
if target_prim.HasAPI(UsdShade.MaterialBindingAPI):
binding_api = UsdShade.MaterialBindingAPI(target_prim)
bound_material, _ = binding_api.ComputeBoundMaterial()
return bound_material
# Some assets author material:binding relationships without applying MaterialBindingAPI.
rels = [rel for rel in target_prim.GetRelationships() if rel.GetName().startswith("material:binding")]
if not rels:
return None
rels.sort(
key=lambda rel: 0
if rel.GetName() == "material:binding"
else 1
if rel.GetName() == "material:binding:preview"
else 2
)
for rel in rels:
targets = rel.GetTargets()
if targets:
mat_prim = target_prim.GetStage().GetPrimAtPath(targets[0])
if mat_prim and mat_prim.IsValid():
return UsdShade.Material(mat_prim)
return None
def _resolve_prim_material_properties(target_prim: Usd.Prim) -> dict[str, Any] | None:
"""Resolve material properties from a prim's bound material.
Returns None if no material is bound or no properties could be extracted.
"""
material = _get_bound_material(target_prim)
if not material:
return None
surface_output = material.GetSurfaceOutput()
if not surface_output:
surface_output = material.GetOutput("surface")
if not surface_output:
surface_output = material.GetOutput("mdl:surface")
source_shader = None
if surface_output:
source = surface_output.GetConnectedSource()
if source:
source_shader = UsdShade.Shader(source[0].GetPrim())
if source_shader is None:
# Fallback: scan material children for a shader node (MDL-style materials).
for child in material.GetPrim().GetChildren():
if child.IsA(UsdShade.Shader):
source_shader = UsdShade.Shader(child)
break
if source_shader is None:
material_props = _extract_material_input_properties(material, target_prim)
if any(value is not None for value in material_props.values()):
return material_props
return None
# Always call _extract_shader_properties even if shader_id is None (e.g., for MDL shaders like OmniPBR)
# because _extract_shader_properties has fallback logic for common input names
properties = _extract_shader_properties(source_shader, target_prim)
material_props = _extract_material_input_properties(material, target_prim)
for key in ("texture", "color", "metallic", "roughness"):
if properties.get(key) is None and material_props.get(key) is not None:
properties[key] = material_props[key]
if properties["color"] is None and properties["texture"] is None:
display_color = UsdGeom.PrimvarsAPI(target_prim).GetPrimvar("displayColor")
if display_color:
properties["color"] = _coerce_color(display_color.Get())
return properties
def resolve_material_properties_for_prim(prim: Usd.Prim) -> dict[str, Any]:
"""Resolve surface material properties bound to a prim.
Args:
prim: The prim whose bound material should be inspected.
Returns:
Dictionary with ``color``, ``metallic``, ``roughness``, and ``texture``.
"""
if not prim or not prim.IsValid():
return _empty_material_properties()
properties = _resolve_prim_material_properties(prim)
if properties is not None:
return properties
proto_prim = None
try:
if prim.IsInstanceProxy():
proto_prim = prim.GetPrimInPrototype()
elif prim.IsInstance():
proto_prim = prim.GetPrototype()
except Exception:
proto_prim = None
if proto_prim and proto_prim.IsValid():
properties = _resolve_prim_material_properties(proto_prim)
if properties is not None:
return properties
if UsdGeom is not None:
try:
is_mesh = prim.IsA(UsdGeom.Mesh)
except Exception:
is_mesh = False
if is_mesh:
fallback_props = None
for child in prim.GetChildren():
try:
is_subset = child.IsA(UsdGeom.Subset)
except Exception:
is_subset = False
if not is_subset:
continue
subset_props = _resolve_prim_material_properties(child)
if subset_props is None:
continue
if subset_props.get("texture") is not None or subset_props.get("color") is not None:
return subset_props
if fallback_props is None:
fallback_props = subset_props
if fallback_props is not None:
return fallback_props
return _empty_material_properties()
def get_gaussian(prim: Usd.Prim, min_response: float = 0.1) -> Gaussian:
"""Load Gaussian splat data from a USD prim.
Reads positions from attributes: `positions`, `orientations`, `scales`, `opacities` and `radiance:sphericalHarmonicsCoefficients`.
Args:
prim: A USD prim containing Gaussian splat data.
min_response: Min response (default = 0.1).
Returns:
A new :class:`Gaussian` instance.
"""
def _get_float_array_attr(name):
attr = prim.GetAttribute(name)
if attr and attr.HasValue():
return np.array(attr.Get(), dtype=np.float32)
attr = prim.GetAttribute(f"{name}h")
if attr and attr.HasValue():
return np.array(attr.Get(), dtype=np.float32)
return None
positions = _get_float_array_attr("positions")
if positions is None:
raise ValueError("USD Gaussian prim is missing required 'positions' attribute")
return Gaussian(
positions=positions,
rotations=_get_float_array_attr("orientations"),
scales=_get_float_array_attr("scales"),
opacities=_get_float_array_attr("opacities"),
sh_coeffs=_get_float_array_attr("radiance:sphericalHarmonicsCoefficients"),
sh_degree=get_attribute(prim, "radiance:sphericalHarmonicsDegree"),
min_response=min_response,
)
================================================
FILE: newton/_src/utils/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from typing import Any
import numpy as np
import warp as wp
from ..core.types import Axis
from .download_assets import clear_git_cache, download_asset
from .texture import load_texture, normalize_texture
from .topology import topological_sort, topological_sort_undirected
def check_conditional_graph_support():
"""
Check if conditional graph support is available in the current world.
Returns:
bool: True if conditional graph support is available, False otherwise.
"""
return wp.is_conditional_graph_supported()
def compute_world_offsets(world_count: int, spacing: tuple[float, float, float], up_axis: Any = None):
"""
Compute positional offsets for multiple worlds arranged in a grid.
This function computes 3D offsets for arranging multiple worlds based on the provided spacing.
The worlds are arranged in a regular grid pattern, with the layout automatically determined
based on the non-zero dimensions in the spacing tuple.
Args:
world_count: The number of worlds to arrange.
spacing: The spacing between worlds along each axis.
Non-zero values indicate active dimensions for the grid layout.
up_axis: The up axis to ensure worlds are not shifted below the ground plane.
If provided, the offset correction along this axis will be zero.
Returns:
np.ndarray: An array of shape (world_count, 3) containing the 3D offsets for each world.
"""
# Handle edge case
if world_count <= 0:
return np.zeros((0, 3), dtype=np.float32)
# Compute positional offsets per world
spacing = np.array(spacing, dtype=np.float32)
nonzeros = np.nonzero(spacing)[0]
num_dim = nonzeros.shape[0]
if num_dim > 0:
side_length = int(np.ceil(world_count ** (1.0 / num_dim)))
spacings = []
if num_dim == 1:
for i in range(world_count):
spacings.append(i * spacing)
elif num_dim == 2:
for i in range(world_count):
d0 = i // side_length
d1 = i % side_length
offset = np.zeros(3)
offset[nonzeros[0]] = d1 * spacing[nonzeros[0]]
offset[nonzeros[1]] = d0 * spacing[nonzeros[1]]
spacings.append(offset)
elif num_dim == 3:
for i in range(world_count):
d0 = i // (side_length * side_length)
d1 = (i // side_length) % side_length
d2 = i % side_length
offset = np.zeros(3)
offset[0] = d2 * spacing[0]
offset[1] = d1 * spacing[1]
offset[2] = d0 * spacing[2]
spacings.append(offset)
spacings = np.array(spacings, dtype=np.float32)
else:
spacings = np.zeros((world_count, 3), dtype=np.float32)
# Center the grid
min_offsets = np.min(spacings, axis=0)
correction = min_offsets + (np.max(spacings, axis=0) - min_offsets) / 2.0
# Ensure the worlds are not shifted below the ground plane
if up_axis is not None:
correction[Axis.from_any(up_axis)] = 0.0
spacings -= correction
return spacings
__all__ = [
"check_conditional_graph_support",
"clear_git_cache",
"compute_world_offsets",
"download_asset",
"load_texture",
"normalize_texture",
"topological_sort",
"topological_sort_undirected",
]
================================================
FILE: newton/_src/utils/benchmark.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import functools
import itertools
import time
import warp as wp
class EventTracer:
"""
Calculates elapsed times of functions annotated with `event_scope`.
.. note::
This class has been copied from:
https://github.com/google-deepmind/mujoco_warp/blob/660f8e2f0fb3ccde78c4e70cf24658a1a14ecf1b/mujoco_warp/_src/warp_util.py#L28
Then modified to change _STACK from being a global.
Example
-------
.. code-block:: python
@event_scope
def my_warp_function(...):
...
with EventTracer() as tracer:
my_warp_function(...)
print(tracer.trace())
"""
_STACK = None
_active_instance = None
def __new__(cls, enabled):
if EventTracer._active_instance is not None and enabled:
raise ValueError("only one EventTracer can run at a time")
return super().__new__(cls)
def __init__(self, enabled: bool = True):
"""
Args:
enabled (bool): If True, elapsed times of annotated functions are measured.
"""
if enabled:
self._STACK = {}
EventTracer._active_instance = self
def __enter__(self):
return self
def trace(self) -> dict:
"""Calculates elapsed times for every node of the trace."""
if self._STACK is None:
return {}
ret = {}
for k, v in self._STACK.items():
events, sub_stack = v
# push into next level of stack
saved_stack, self._STACK = self._STACK, sub_stack
sub_trace = self.trace()
# pop!
self._STACK = saved_stack
events = tuple(wp.get_event_elapsed_time(beg, end) for beg, end in events)
ret[k] = (events, sub_trace)
return ret
def add_trace(self, stack, new_stack):
"""Sums elapsed times from two difference traces."""
ret = {}
for k in new_stack:
times, sub_stack = stack[k] if k in stack.keys() else (0, {})
new_times, new_sub_stack = new_stack[k]
times = times + sum(new_times)
ret[k] = (times, self.add_trace(sub_stack, new_sub_stack))
return ret
def __exit__(self, type, value, traceback):
self._STACK = None
if EventTracer._active_instance is self:
EventTracer._active_instance = None
def _merge(a: dict, b: dict) -> dict:
"""
Merges two event trace stacks.
.. note::
This function has been copied from:
https://github.com/google-deepmind/mujoco_warp/blob/660f8e2f0fb3ccde78c4e70cf24658a1a14ecf1b/mujoco_warp/_src/warp_util.py#L78
Then modified to change how the dictionary items were accessed.
Parameters:
a : Base event trace stack.
b : Second event trace stack to add to the base event trace stack.
Returns:
A dictionary where the two event traces are merged.
"""
ret = {}
if not a or not b:
return dict(**a, **b)
if set(a) != set(b):
raise ValueError("incompatible stacks")
for key, (a1_events, a1_substack) in a.items():
a2_events, a2_substack = b[key]
ret[key] = (a1_events + a2_events, _merge(a1_substack, a2_substack))
return ret
def event_scope(fn, name: str = ""):
"""
Wraps a function and records an event before and after the function invocation.
.. note::
This function has been copied from:
https://github.com/google-deepmind/mujoco_warp/blob/660f8e2f0fb3ccde78c4e70cf24658a1a14ecf1b/mujoco_warp/_src/warp_util.py#L92
Then modified to change _STACK from being a global.
Parameters:
fn : Function to be wrapped.
name : Custom name associated with the function.
"""
name = name or fn.__name__
@functools.wraps(fn)
def wrapper(*args, **kwargs):
if EventTracer._active_instance is None:
return fn(*args, **kwargs)
# push into next level of stack
saved_stack, EventTracer._active_instance._STACK = EventTracer._active_instance._STACK, {}
beg = wp.Event(enable_timing=True)
end = wp.Event(enable_timing=True)
wp.record_event(beg)
res = fn(*args, **kwargs)
wp.record_event(end)
# pop back up to current level
sub_stack, EventTracer._active_instance._STACK = EventTracer._active_instance._STACK, saved_stack
# append events and substack
prev_events, prev_substack = EventTracer._active_instance._STACK.get(name, ((), {}))
events = (*prev_events, (beg, end))
sub_stack = _merge(prev_substack, sub_stack)
EventTracer._active_instance._STACK[name] = (events, sub_stack)
return res
return wrapper
def run_benchmark(benchmark_cls, number=1, print_results=True):
"""
Simple scaffold to run a benchmark class.
Parameters:
benchmark_cls : ASV-compatible benchmark class.
number : Number of iterations to run each benchmark method.
Returns:
A dictionary mapping (method name, parameter tuple) to the average result.
"""
# Determine all parameter combinations (if any).
if hasattr(benchmark_cls, "params"):
param_lists = benchmark_cls.params
combinations = list(itertools.product(*param_lists))
else:
combinations = [()]
results = {}
# For each parameter combination:
for params in combinations:
# Create a fresh benchmark instance.
instance = benchmark_cls()
if hasattr(instance, "setup"):
instance.setup(*params)
# Iterate over all attributes to find benchmark methods.
for attr in dir(instance):
if attr.startswith("time_") or attr.startswith("track_"):
method = getattr(instance, attr)
print(f"\n[Benchmark] Running {benchmark_cls.__name__}.{attr} with parameters {params}")
samples = []
if attr.startswith("time_"):
# Warmup run (not measured).
method(*params)
wp.synchronize()
# Run timing benchmarks multiple times and measure elapsed time.
for _ in range(number):
start = time.perf_counter()
method(*params)
t = time.perf_counter() - start
samples.append(t)
elif attr.startswith("track_"):
# Run tracking benchmarks multiple times and record returned values.
for _ in range(number):
val = method(*params)
samples.append(val)
# Compute the average result.
avg = sum(samples) / len(samples)
results[(attr, params)] = avg
if hasattr(instance, "teardown"):
instance.teardown(*params)
if print_results:
print("\n=== Benchmark Results ===")
for (method_name, params), avg in results.items():
print(f"{benchmark_cls.__name__}.{method_name} {params}: {avg:.6f}")
return results
================================================
FILE: newton/_src/utils/cable.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import math
from collections.abc import Sequence
import warp as wp
from ..math import quat_between_vectors_robust
def create_cable_stiffness_from_elastic_moduli(
youngs_modulus: float,
radius: float,
) -> tuple[float, float]:
"""Create rod/cable stiffness parameters from elastic moduli (circular cross-section).
This returns the *material-like* stiffness values expected by `ModelBuilder.add_rod()` /
`ModelBuilder.add_rod_graph()`:
- stretch_stiffness = E * A [N]
- bend_stiffness = E * I [N*m^2]
where:
- A = pi * r^2
- I = (pi * r^4) / 4 (area moment of inertia for a solid circular rod)
Note: Newton internally converts these into per-joint effective stiffnesses by dividing by
segment length. This helper intentionally does *not* perform any length normalization.
Args:
youngs_modulus: Young's modulus E in Pascals [N/m^2].
radius: Rod/cable radius r in meters.
Returns:
Tuple `(stretch_stiffness, bend_stiffness)` = `(E*A, E*I)`.
"""
# Accept ints / numpy scalars, but return plain Python floats.
E = float(youngs_modulus)
r = float(radius)
if not math.isfinite(E):
raise ValueError("youngs_modulus must be finite")
if not math.isfinite(r):
raise ValueError("radius must be finite")
if E < 0.0:
raise ValueError("youngs_modulus must be >= 0")
if r <= 0.0:
raise ValueError("radius must be > 0")
area = math.pi * r * r
inertia = 0.25 * math.pi * r**4
return E * area, E * inertia
def create_straight_cable_points(
start: wp.vec3,
direction: wp.vec3,
length: float,
num_segments: int,
) -> list[wp.vec3]:
"""Create straight cable polyline points.
This is a convenience helper for constructing ``positions`` inputs for ``ModelBuilder.add_rod``.
Args:
start: First point in world space.
direction: World-space direction of the cable (need not be normalized).
length: Total length of the cable (meters).
num_segments: Number of segments (edges). The number of points is ``num_segments + 1``.
Returns:
List of ``wp.vec3`` points of length ``num_segments + 1``.
"""
if num_segments < 1:
raise ValueError("num_segments must be >= 1")
length_m = float(length)
if not math.isfinite(length_m):
raise ValueError("length must be finite")
if length_m < 0.0:
raise ValueError("length must be >= 0")
dir_len = float(wp.length(direction))
if dir_len <= 0.0:
raise ValueError("direction must be non-zero")
d = direction / dir_len
ds = length_m / num_segments
return [start + d * (ds * i) for i in range(num_segments + 1)]
def create_parallel_transport_cable_quaternions(
points: Sequence[wp.vec3],
*,
twist_total: float = 0.0,
) -> list[wp.quat]:
"""Generate per-segment quaternions using a parallel-transport style construction.
The intended use is for rod/cable capsules whose internal axis is local +Z.
The returned quaternions rotate local +Z to each segment direction,
while minimizing twist between successive segments. Optionally, a total twist can be
distributed uniformly along the cable.
Args:
points: Polyline points of length >= 2.
twist_total: Total twist (radians) distributed along the cable (applied about the segment direction).
Returns:
List of ``wp.quat`` of length ``len(points) - 1``.
"""
if len(points) < 2:
raise ValueError("points must have length >= 2")
from_direction = wp.vec3(0.0, 0.0, 1.0)
num_segments = len(points) - 1
twist_total_rad = float(twist_total)
twist_step = (twist_total_rad / num_segments) if twist_total_rad != 0.0 else 0.0
eps = 1.0e-8
quats: list[wp.quat] = []
for i in range(num_segments):
p0 = points[i]
p1 = points[i + 1]
seg = p1 - p0
seg_len = float(wp.length(seg))
if seg_len <= 0.0:
raise ValueError("points must not contain duplicate consecutive points")
to_direction = seg / seg_len
# Robustly handle the anti-parallel (180-degree) case, e.g. +Z -> -Z.
dq_dir = quat_between_vectors_robust(from_direction, to_direction, eps)
q = dq_dir if i == 0 else wp.mul(dq_dir, quats[i - 1])
if twist_total_rad != 0.0:
twist_q = wp.quat_from_axis_angle(to_direction, twist_step)
q = wp.mul(twist_q, q)
quats.append(q)
from_direction = to_direction
return quats
def create_straight_cable_points_and_quaternions(
start: wp.vec3,
direction: wp.vec3,
length: float,
num_segments: int,
*,
twist_total: float = 0.0,
) -> tuple[list[wp.vec3], list[wp.quat]]:
"""Generate straight cable points and matching per-segment quaternions.
This is a convenience wrapper around:
- :func:`create_straight_cable_points`
- :func:`create_parallel_transport_cable_quaternions`
"""
points = create_straight_cable_points(
start=start,
direction=direction,
length=length,
num_segments=num_segments,
)
quats = create_parallel_transport_cable_quaternions(points, twist_total=twist_total)
return points, quats
================================================
FILE: newton/_src/utils/download_assets.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import errno
import hashlib
import os
import re
import shutil
import stat
import threading
import time
from pathlib import Path
from warp._src.thirdparty.appdirs import user_cache_dir
# External asset repositories and their pinned revisions.
# Pinning to commit SHAs ensures reproducible downloads for any given Newton
# commit. Update these SHAs when assets change upstream and the new versions
# have been validated against Newton's test suite.
NEWTON_ASSETS_URL = "https://github.com/newton-physics/newton-assets.git"
NEWTON_ASSETS_REF = "8e8df07d2e4829442d3d3d3aeecee1857f9951d7"
MENAGERIE_URL = "https://github.com/google-deepmind/mujoco_menagerie.git"
MENAGERIE_REF = "feadf76d42f8a2162426f7d226a3b539556b3bf5"
_SHA_RE = re.compile(r"[0-9a-f]{40}")
def _get_newton_cache_dir() -> str:
"""Gets the persistent Newton cache directory."""
if "NEWTON_CACHE_PATH" in os.environ:
return os.environ["NEWTON_CACHE_PATH"]
return user_cache_dir("newton", "newton-physics")
def _handle_remove_readonly(func, path, exc):
"""Error handler for Windows readonly files during shutil.rmtree()."""
if os.path.exists(path):
# Make the file writable and try again
os.chmod(path, stat.S_IWRITE)
func(path)
def _safe_rmtree(path):
"""Safely remove directory tree, handling Windows readonly files."""
if os.path.exists(path):
shutil.rmtree(path, onerror=_handle_remove_readonly)
def _safe_rename(src, dst, attempts=5, delay=0.1):
"""Rename src to dst, tolerating races where another process wins.
If *dst* already exists (``FileExistsError`` or ``ENOTEMPTY``), the call
returns silently — the caller should clean up *src*. Transient OS errors
(e.g. Windows file-lock contention) are retried up to *attempts* times.
"""
for i in range(attempts):
try:
os.rename(src, dst)
return
except FileExistsError:
return
except OSError as e:
if e.errno == errno.ENOTEMPTY:
return
if i < attempts - 1:
time.sleep(delay)
else:
raise
def _temp_cache_path(final_dir: Path) -> Path:
"""Return a per-process, per-thread temp path next to *final_dir*."""
return Path(f"{final_dir}_p{os.getpid()}_t{threading.get_ident()}")
_TEMP_DIR_RE = re.compile(r"_p\d+_t\d+$")
def _cleanup_stale_temp_dirs(cache_path: Path, base_prefix: str, max_age: float = 3600.0) -> None:
"""Remove orphaned temp directories left by crashed processes.
Scans *cache_path* for directories matching ``{base_prefix}_*`` whose names
contain a temp-dir suffix (``_p{pid}_t{tid}``) and whose mtime is older
than *max_age* seconds. Safe to call concurrently.
"""
now = time.time()
try:
for entry in cache_path.iterdir():
name = entry.name
if not name.startswith(f"{base_prefix}_") or not entry.is_dir():
continue
suffix = name[len(base_prefix) + 1 :]
if not _TEMP_DIR_RE.search(suffix):
continue
try:
age = now - entry.stat().st_mtime
except OSError:
continue
if age > max_age:
try:
_safe_rmtree(entry)
except OSError:
pass
except OSError:
pass
def _find_cached_version(cache_path: Path, base_prefix: str) -> Path | None:
"""Find an existing content-hash cache directory for the given prefix.
Scans ``{cache_path}`` for directories matching ``{base_prefix}_*/``,
filters out temp directories (matching ``_p\\d+_t\\d+`` suffix), and
returns the match with the newest mtime. Returns ``None`` if no match
is found.
"""
candidates = []
try:
for entry in cache_path.iterdir():
name = entry.name
if not name.startswith(f"{base_prefix}_") or not entry.is_dir():
continue
suffix = name[len(base_prefix) + 1 :]
if _TEMP_DIR_RE.search(suffix):
continue
try:
mtime = entry.stat().st_mtime
except OSError:
continue
candidates.append((mtime, entry))
except OSError:
return None
if not candidates:
return None
candidates.sort(key=lambda x: x[0], reverse=True)
return candidates[0][1]
def _get_latest_commit_via_git(git_url: str, ref: str) -> str | None:
"""Resolve latest commit SHA for a branch or tag via 'git ls-remote'.
If *ref* is already a 40-character commit SHA it is returned as-is.
For annotated tags the dereferenced commit SHA is preferred.
"""
if _SHA_RE.fullmatch(ref):
return ref
try:
import git
# Request the ref and its dereferenced form (for annotated tags).
out = git.cmd.Git().ls_remote(git_url, ref, f"{ref}^{{}}")
if not out:
return None
# Parse lines: "\t[\n"
# Prefer dereferenced tag (^{}) > branch > lightweight tag
best = None
for line in out.strip().splitlines():
sha, refname = line.split("\t", 1)
if refname == f"refs/tags/{ref}^{{}}":
return sha # annotated tag → underlying commit SHA
if refname in (f"refs/heads/{ref}", f"refs/tags/{ref}"):
best = sha
return best
except Exception:
# Fail silently on any error (offline, auth issue, etc.)
return None
def _find_parent_cache(
cache_path: Path,
repo_name: str,
folder_path: str,
ref: str,
git_url: str,
) -> tuple[Path, Path] | None:
"""Check if folder_path exists inside an already-cached parent folder.
For example, if folder_path is "unitree_g1/usd" and we have
"newton-assets_unitree_g1_]" cached, return the paths.
Args:
cache_path: The base cache directory
repo_name: Repository name (e.g., "newton-assets")
folder_path: The requested folder path (e.g., "unitree_g1/usd")
ref: Git branch, tag, or commit SHA.
git_url: Full git URL for hash computation
Returns:
Tuple of (parent_cache_folder, target_subfolder) if found, None otherwise.
"""
parts = folder_path.split("/")
if len(parts) <= 1:
return None # No parent to check
# Generate all potential parent paths: "a/b/c" -> ["a", "a/b"]
parent_paths = ["/".join(parts[:i]) for i in range(1, len(parts))]
for parent_path in parent_paths:
# Generate the cache folder name for this parent
parent_hash = hashlib.md5(f"{git_url}#{parent_path}#{ref}".encode()).hexdigest()[:8]
parent_folder_name = parent_path.replace("/", "_").replace("\\", "_")
base_prefix = f"{repo_name}_{parent_folder_name}_{parent_hash}"
cached = _find_cached_version(cache_path, base_prefix)
if cached is None:
continue
# Check if this parent cache contains our target
target_in_parent = cached / folder_path
if target_in_parent.exists() and (cached / ".git").exists():
return (cached, target_in_parent)
return None
def _cleanup_old_versions(cache_path: Path, base_prefix: str, current_dir: Path) -> None:
"""Best-effort removal of old content-hash directories after a new download.
Scans for directories matching *base_prefix* (excluding temp dirs and
*current_dir*) and removes them. Failures are silently ignored.
"""
try:
for entry in cache_path.iterdir():
if entry == current_dir or not entry.is_dir():
continue
name = entry.name
if not name.startswith(f"{base_prefix}_"):
continue
suffix = name[len(base_prefix) + 1 :]
if _TEMP_DIR_RE.search(suffix):
continue
try:
_safe_rmtree(entry)
except OSError:
pass
except OSError:
pass
def download_git_folder(
git_url: str, folder_path: str, cache_dir: str | None = None, ref: str = "main", force_refresh: bool = False
) -> Path:
"""Downloads a specific folder from a git repository into a local cache.
Uses content-addressed directories: each cached version includes the Git
commit SHA in its directory name. When upstream publishes new assets the
SHA changes, producing a new directory — no in-place eviction is needed.
Args:
git_url: The git repository URL (HTTPS or SSH).
folder_path: Path to the folder within the repository.
cache_dir: Directory to cache downloads. If ``None``, determined by
``NEWTON_CACHE_PATH`` env-var or the system user cache directory.
ref: Git branch, tag, or commit SHA to checkout (default: ``"main"``).
force_refresh: If ``True``, bypass TTL and verify the cached version
against the remote. Re-downloads only if the remote SHA differs.
Returns:
Path to the downloaded folder in the local cache.
"""
try:
import git as gitpython
from git.exc import GitCommandError
except ImportError as e:
raise ImportError(
"GitPython package is required for downloading git folders. Install it with: pip install GitPython"
) from e
# Set up cache directory
if cache_dir is None:
cache_dir = _get_newton_cache_dir()
cache_path = Path(cache_dir)
cache_path.mkdir(parents=True, exist_ok=True)
# Compute identity hash (stable across content changes)
identity_hash = hashlib.md5(f"{git_url}#{folder_path}#{ref}".encode()).hexdigest()[:8]
repo_name = Path(git_url.rstrip("/")).stem.replace(".git", "")
folder_name = folder_path.replace("/", "_").replace("\\", "_")
base_prefix = f"{repo_name}_{folder_name}_{identity_hash}"
ttl_seconds = 3600
latest_commit = None # reused across parent-cache check and primary resolution to avoid redundant ls-remote
# --- Parent folder optimization ---
if not force_refresh:
parent_result = _find_parent_cache(cache_path, repo_name, folder_path, ref, git_url)
if parent_result is not None:
parent_dir, target_in_parent = parent_result
try:
age = time.time() - parent_dir.stat().st_mtime
except OSError:
age = ttl_seconds + 1
if age < ttl_seconds:
return target_in_parent
# TTL expired — check remote
parent_sha_suffix = parent_dir.name.rsplit("_", 1)[-1]
latest_commit = _get_latest_commit_via_git(git_url, ref)
if latest_commit is None:
# Offline — touch mtime and return cached
try:
os.utime(parent_dir, None)
except OSError:
pass
return target_in_parent
if latest_commit[:8] == parent_sha_suffix:
try:
os.utime(parent_dir, None)
except OSError:
pass
return target_in_parent
# Parent is stale — fall through to download
# --- Resolution flow ---
cached = _find_cached_version(cache_path, base_prefix)
if cached is not None and not force_refresh:
try:
age = time.time() - cached.stat().st_mtime
except OSError:
age = ttl_seconds + 1
if age < ttl_seconds:
return cached / folder_path
# Check remote for current commit (reuse result from parent check if available)
if latest_commit is None:
latest_commit = _get_latest_commit_via_git(git_url, ref)
if latest_commit is None:
if cached is not None:
try:
os.utime(cached, None)
except OSError:
pass
return cached / folder_path
raise RuntimeError(
f"Cannot determine remote commit SHA for {git_url} (ref: {ref}) and no cached version exists."
)
# Check if we already have this exact version
if cached is not None and not force_refresh:
cached_sha_suffix = cached.name.rsplit("_", 1)[-1]
if latest_commit[:8] == cached_sha_suffix:
try:
os.utime(cached, None)
except OSError:
pass
return cached / folder_path
# --- Download into content-addressed directory ---
final_dir = cache_path / f"{base_prefix}_{latest_commit[:8]}"
temp_dir = _temp_cache_path(final_dir)
# Clean up orphaned temp directories
_cleanup_stale_temp_dirs(cache_path, base_prefix)
try:
if temp_dir.exists():
_safe_rmtree(temp_dir)
if cached is not None:
print(
f"New version of {folder_path} found "
f"(cached: {cached.name.rsplit('_', 1)[-1]}, "
f"latest: {latest_commit[:8]}). Refreshing..."
)
print(f"Cloning {git_url} (ref: {ref})...")
is_sha = bool(_SHA_RE.fullmatch(ref))
if is_sha:
# Single fetch — skip the clone, which would download the
# default-branch tip only to throw it away.
repo = gitpython.Repo.init(temp_dir)
try:
repo.create_remote("origin", git_url)
repo.git.sparse_checkout("init")
repo.git.sparse_checkout("set", folder_path)
repo.git.fetch("origin", ref, "--depth=1", "--filter=blob:none")
repo.git.checkout("FETCH_HEAD")
finally:
repo.close()
else:
repo = gitpython.Repo.clone_from(
git_url,
temp_dir,
branch=ref,
depth=1,
no_checkout=True,
multi_options=["--filter=blob:none", "--sparse"],
)
try:
repo.git.sparse_checkout("set", folder_path)
repo.git.checkout(ref)
finally:
repo.close()
temp_target = temp_dir / folder_path
if not temp_target.exists():
raise RuntimeError(f"Folder '{folder_path}' not found in repository {git_url}")
# Place the finished download into its final location
_safe_rename(temp_dir, final_dir)
if temp_dir.exists():
# Another process already placed this exact version — use theirs
_safe_rmtree(temp_dir)
# Set mtime to now for TTL tracking
os.utime(final_dir, None)
print(f"Successfully downloaded folder to: {final_dir / folder_path}")
# Best-effort cleanup of old versions
_cleanup_old_versions(cache_path, base_prefix, final_dir)
return final_dir / folder_path
except GitCommandError as e:
raise RuntimeError(f"Git operation failed: {e}") from e
except RuntimeError:
raise
except Exception as e:
raise RuntimeError(f"Failed to download git folder: {e}") from e
finally:
try:
if temp_dir.exists():
_safe_rmtree(temp_dir)
except OSError:
pass
def clear_git_cache(cache_dir: str | None = None) -> None:
"""
Clears the git download cache directory.
Args:
cache_dir: Cache directory to clear.
If ``None``, the path is determined in the following order:
1. ``NEWTON_CACHE_PATH`` environment variable.
2. System's user cache directory (via ``appdirs.user_cache_dir``).
"""
if cache_dir is None:
cache_dir = _get_newton_cache_dir()
cache_path = Path(cache_dir)
if cache_path.exists():
_safe_rmtree(cache_path)
print(f"Cleared git cache: {cache_path}")
else:
print("Git cache directory does not exist")
def download_asset(
asset_folder: str,
cache_dir: str | None = None,
force_refresh: bool = False,
ref: str | None = None,
) -> Path:
"""Download a specific folder from the newton-assets GitHub repository into a local cache.
Args:
asset_folder: The folder within the repository to download (e.g., "assets/models")
cache_dir: Directory to cache downloads.
If ``None``, the path is determined in the following order:
1. ``NEWTON_CACHE_PATH`` environment variable.
2. System's user cache directory (via ``appdirs.user_cache_dir``).
force_refresh: If ``True``, bypass TTL and verify the cached version
against the remote. Re-downloads only if the remote SHA differs.
ref: Git branch, tag, or commit SHA to checkout. Defaults to the
revision pinned in :data:`NEWTON_ASSETS_REF`.
Returns:
Path to the downloaded folder in the local cache.
"""
return download_git_folder(
NEWTON_ASSETS_URL,
asset_folder,
cache_dir=cache_dir,
ref=ref or NEWTON_ASSETS_REF,
force_refresh=force_refresh,
)
================================================
FILE: newton/_src/utils/heightfield.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import os
import numpy as np
import warp as wp
from ..geometry.support_function import GenericShapeData, GeoTypeEx
def load_heightfield_elevation(
filename: str,
nrow: int,
ncol: int,
) -> np.ndarray:
"""Load elevation data from a PNG or binary file.
Supports two formats following MuJoCo conventions:
- PNG: Grayscale image where white=high, black=low
(normalized to [0, 1])
- Binary: MuJoCo custom format with int32 header
(nrow, ncol) followed by float32 data
Args:
filename: Path to the heightfield file (PNG or binary).
nrow: Expected number of rows.
ncol: Expected number of columns.
Returns:
(nrow, ncol) float32 array of elevation values.
"""
ext = os.path.splitext(filename)[1].lower()
if ext == ".png":
from PIL import Image
img = Image.open(filename).convert("L")
data = np.array(img, dtype=np.float32) / 255.0
if data.shape != (nrow, ncol):
raise ValueError(f"PNG heightfield dimensions {data.shape} don't match expected ({nrow}, {ncol})")
return data
# Default: MuJoCo binary format
# Header: (int32) nrow, (int32) ncol; payload: float32[nrow*ncol]
with open(filename, "rb") as f:
header = np.fromfile(f, dtype=np.int32, count=2)
if header.size != 2 or header[0] <= 0 or header[1] <= 0:
raise ValueError(
f"Invalid binary heightfield header in '{filename}': expected 2 positive int32 values, got {header}"
)
expected_count = int(header[0]) * int(header[1])
data = np.fromfile(f, dtype=np.float32, count=expected_count)
if data.size != expected_count:
raise ValueError(
f"Binary heightfield '{filename}' payload size mismatch: "
f"expected {expected_count} float32 values for {header[0]}x{header[1]} grid, got {data.size}"
)
return data.reshape(header[0], header[1])
@wp.struct
class HeightfieldData:
"""Per-shape heightfield metadata for collision kernels.
The actual elevation data is stored in a separate concatenated array
passed to kernels. ``data_offset`` is the starting index into that array.
"""
data_offset: wp.int32 # Offset into the concatenated elevation array
nrow: wp.int32
ncol: wp.int32
hx: wp.float32 # Half-extent X
hy: wp.float32 # Half-extent Y
min_z: wp.float32
max_z: wp.float32
def create_empty_heightfield_data() -> HeightfieldData:
"""Create an empty HeightfieldData for non-heightfield shapes."""
hd = HeightfieldData()
hd.data_offset = 0
hd.nrow = 0
hd.ncol = 0
hd.hx = 0.0
hd.hy = 0.0
hd.min_z = 0.0
hd.max_z = 0.0
return hd
@wp.func
def _heightfield_surface_query(
hfd: HeightfieldData,
elevation_data: wp.array[wp.float32],
pos: wp.vec3,
) -> tuple[float, wp.vec3, float]:
"""Core heightfield surface query returning (plane_dist, normal, lateral_dist_sq).
Computes the signed distance to the nearest triangle plane at the closest
point within the heightfield XY extent, plus the squared lateral distance
from the query point to that extent boundary.
"""
if hfd.nrow <= 1 or hfd.ncol <= 1:
return 1.0e10, wp.vec3(0.0, 0.0, 1.0), 0.0
dx = 2.0 * hfd.hx / wp.float32(hfd.ncol - 1)
dy = 2.0 * hfd.hy / wp.float32(hfd.nrow - 1)
z_range = hfd.max_z - hfd.min_z
# Clamp to heightfield XY extent and track lateral overshoot
cx = wp.clamp(pos[0], -hfd.hx, hfd.hx)
cy = wp.clamp(pos[1], -hfd.hy, hfd.hy)
out_x = pos[0] - cx
out_y = pos[1] - cy
lateral_dist_sq = out_x * out_x + out_y * out_y
col_f = (cx + hfd.hx) / dx
row_f = (cy + hfd.hy) / dy
col_f = wp.clamp(col_f, 0.0, wp.float32(hfd.ncol - 1))
row_f = wp.clamp(row_f, 0.0, wp.float32(hfd.nrow - 1))
col = wp.min(wp.int32(col_f), hfd.ncol - 2)
row = wp.min(wp.int32(row_f), hfd.nrow - 2)
fx = col_f - wp.float32(col)
fy = row_f - wp.float32(row)
base = hfd.data_offset
h00 = hfd.min_z + elevation_data[base + row * hfd.ncol + col] * z_range
h10 = hfd.min_z + elevation_data[base + row * hfd.ncol + col + 1] * z_range
h01 = hfd.min_z + elevation_data[base + (row + 1) * hfd.ncol + col] * z_range
h11 = hfd.min_z + elevation_data[base + (row + 1) * hfd.ncol + col + 1] * z_range
x0 = -hfd.hx + wp.float32(col) * dx
y0 = -hfd.hy + wp.float32(row) * dy
if fx >= fy:
v0 = wp.vec3(x0, y0, h00)
e1 = wp.vec3(dx, 0.0, h10 - h00)
e2 = wp.vec3(dx, dy, h11 - h00)
else:
v0 = wp.vec3(x0, y0, h00)
e1 = wp.vec3(dx, dy, h11 - h00)
e2 = wp.vec3(0.0, dy, h01 - h00)
normal = wp.normalize(wp.cross(e1, e2))
d_plane = wp.dot(pos - v0, normal)
return d_plane, normal, lateral_dist_sq
@wp.func
def sample_sdf_heightfield(
hfd: HeightfieldData,
elevation_data: wp.array[wp.float32],
pos: wp.vec3,
) -> float:
"""On-the-fly signed distance to a piecewise-planar heightfield surface.
Positive above the surface, negative below. Exact for the piecewise-linear
triangulation when the query point projects inside the heightfield XY extent.
Outside the extent the lateral gap is folded in, yielding a positive distance
that prevents false contacts.
Note: This means objects penetrating near the boundary will experience a
discontinuous contact loss at the edge (the distance jumps from negative to
positive). This is an intentional tradeoff to avoid ghost contacts outside
the heightfield footprint.
"""
d_plane, _normal, lateral_dist_sq = _heightfield_surface_query(hfd, elevation_data, pos)
if lateral_dist_sq > 0.0:
return wp.sqrt(lateral_dist_sq + d_plane * d_plane)
return d_plane
@wp.func
def sample_sdf_grad_heightfield(
hfd: HeightfieldData,
elevation_data: wp.array[wp.float32],
pos: wp.vec3,
) -> tuple[float, wp.vec3]:
"""On-the-fly signed distance and gradient for a heightfield surface.
Inside the XY extent the gradient is the triangle face normal. Outside,
it blends the face normal with the lateral displacement direction.
"""
d_plane, normal, lateral_dist_sq = _heightfield_surface_query(hfd, elevation_data, pos)
if lateral_dist_sq > 0.0:
dist = wp.sqrt(lateral_dist_sq + d_plane * d_plane)
cx = wp.clamp(pos[0], -hfd.hx, hfd.hx)
cy = wp.clamp(pos[1], -hfd.hy, hfd.hy)
lateral = wp.vec3(pos[0] - cx, pos[1] - cy, 0.0)
raw_grad = lateral + d_plane * normal
if wp.length_sq(raw_grad) > 1.0e-20:
grad = wp.normalize(raw_grad)
else:
grad = wp.vec3(0.0, 0.0, 1.0)
return dist, grad
return d_plane, normal
@wp.func
def get_triangle_shape_from_heightfield(
hfd: HeightfieldData,
elevation_data: wp.array[wp.float32],
X_ws: wp.transform,
tri_idx: int,
) -> tuple[GenericShapeData, wp.vec3]:
"""Extract a triangle from a heightfield by packed triangle index.
``tri_idx`` encodes ``(row * (ncol - 1) + col) * 2 + tri_sub``.
Returns ``(GenericShapeData, v0_world)`` in the same format as
:func:`get_triangle_shape_from_mesh`, so GJK/MPR works unchanged.
Triangle layout for cell (row, col)::
p01 --- p11
| \\ 1 |
| 0 \\ |
p00 --- p10
tri_sub=0: (p00, p10, p11)
tri_sub=1: (p00, p11, p01)
"""
# Decode packed triangle index
cell_idx = tri_idx // 2
tri_sub = tri_idx - cell_idx * 2
cols = hfd.ncol - 1
row = cell_idx // cols
col = cell_idx - row * cols
# Grid spacing
dx = 2.0 * hfd.hx / wp.float32(hfd.ncol - 1)
dy = 2.0 * hfd.hy / wp.float32(hfd.nrow - 1)
z_range = hfd.max_z - hfd.min_z
# Corner positions in local space
x0 = -hfd.hx + wp.float32(col) * dx
x1 = x0 + dx
y0 = -hfd.hy + wp.float32(row) * dy
y1 = y0 + dy
# Read elevation values from concatenated array
base = hfd.data_offset
h00 = elevation_data[base + row * hfd.ncol + col]
h10 = elevation_data[base + row * hfd.ncol + (col + 1)]
h01 = elevation_data[base + (row + 1) * hfd.ncol + col]
h11 = elevation_data[base + (row + 1) * hfd.ncol + (col + 1)]
# Convert to world Z: min_z + h * (max_z - min_z)
z00 = hfd.min_z + h00 * z_range
z10 = hfd.min_z + h10 * z_range
z01 = hfd.min_z + h01 * z_range
z11 = hfd.min_z + h11 * z_range
# Local-space corner positions
p00 = wp.vec3(x0, y0, z00)
p10 = wp.vec3(x1, y0, z10)
p01 = wp.vec3(x0, y1, z01)
p11 = wp.vec3(x1, y1, z11)
# Select triangle vertices
if tri_sub == 0:
v0_local = p00
v1_local = p10
v2_local = p11
else:
v0_local = p00
v1_local = p11
v2_local = p01
# Transform to world space
v0_world = wp.transform_point(X_ws, v0_local)
v1_world = wp.transform_point(X_ws, v1_local)
v2_world = wp.transform_point(X_ws, v2_local)
# Create triangle shape data (same convention as get_triangle_shape_from_mesh)
shape_data = GenericShapeData()
shape_data.shape_type = int(GeoTypeEx.TRIANGLE)
shape_data.scale = v1_world - v0_world # B - A
shape_data.auxiliary = v2_world - v0_world # C - A
return shape_data, v0_world
@wp.func
def heightfield_vs_convex_midphase(
hfield_shape: int,
other_shape: int,
hfd: HeightfieldData,
shape_transform: wp.array[wp.transform],
shape_collision_radius: wp.array[float],
shape_gap: wp.array[float],
triangle_pairs: wp.array[wp.vec3i],
triangle_pairs_count: wp.array[int],
):
"""Find heightfield triangles that overlap with a convex shape's bounding sphere.
Projects the convex shape onto the heightfield grid and emits triangle pairs
for each overlapping cell (two triangles per cell).
Args:
hfield_shape: Index of the heightfield shape.
other_shape: Index of the convex shape.
hfd: Heightfield data struct.
shape_transform: World-space transforms for all shapes.
shape_collision_radius: Bounding-sphere radii for all shapes.
shape_gap: Per-shape contact gaps.
triangle_pairs: Output buffer for ``(hfield_shape, other_shape, tri_idx)`` triples.
triangle_pairs_count: Atomic counter for emitted triangle pairs.
"""
# Transform other shape's position to heightfield local space
X_hfield_ws = shape_transform[hfield_shape]
X_hfield_inv = wp.transform_inverse(X_hfield_ws)
X_other_ws = shape_transform[other_shape]
pos_in_hfield = wp.transform_point(X_hfield_inv, wp.transform_get_translation(X_other_ws))
# Conservative AABB using bounding sphere radius
radius = shape_collision_radius[other_shape]
gap_sum = shape_gap[hfield_shape] + shape_gap[other_shape]
extent = radius + gap_sum
aabb_lower = pos_in_hfield - wp.vec3(extent, extent, extent)
aabb_upper = pos_in_hfield + wp.vec3(extent, extent, extent)
# Map AABB to grid cell indices
dx = 2.0 * hfd.hx / wp.float32(hfd.ncol - 1)
dy = 2.0 * hfd.hy / wp.float32(hfd.nrow - 1)
col_min_f = (aabb_lower[0] + hfd.hx) / dx
col_max_f = (aabb_upper[0] + hfd.hx) / dx
row_min_f = (aabb_lower[1] + hfd.hy) / dy
row_max_f = (aabb_upper[1] + hfd.hy) / dy
col_min = wp.max(wp.int32(wp.floor(col_min_f)), 0)
col_max = wp.min(wp.int32(wp.floor(col_max_f)), hfd.ncol - 2)
row_min = wp.max(wp.int32(wp.floor(row_min_f)), 0)
row_max = wp.min(wp.int32(wp.floor(row_max_f)), hfd.nrow - 2)
cols = hfd.ncol - 1
for r in range(row_min, row_max + 1):
for c in range(col_min, col_max + 1):
for tri_sub in range(2):
tri_idx = (r * cols + c) * 2 + tri_sub
out_idx = wp.atomic_add(triangle_pairs_count, 0, 1)
if out_idx < triangle_pairs.shape[0]:
triangle_pairs[out_idx] = wp.vec3i(hfield_shape, other_shape, tri_idx)
================================================
FILE: newton/_src/utils/import_mjcf.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import os
import re
import warnings
import xml.etree.ElementTree as ET
from collections.abc import Callable
from typing import Any
import numpy as np
import warp as wp
from ..core import quat_between_axes
from ..core.types import Axis, AxisType, Sequence, Transform, vec10
from ..geometry import Mesh, ShapeFlags
from ..geometry.types import Heightfield
from ..geometry.utils import compute_aabb, compute_inertia_box_mesh
from ..sim import JointTargetMode, JointType, ModelBuilder
from ..sim.model import Model
from ..solvers.mujoco import SolverMuJoCo
from ..usd.schemas import solref_to_stiffness_damping
from .heightfield import load_heightfield_elevation
from .import_utils import (
is_xml_content,
parse_custom_attributes,
sanitize_name,
sanitize_xml_content,
should_show_collider,
)
from .mesh import load_meshes_from_file
def _default_path_resolver(base_dir: str | None, file_path: str) -> str:
"""Default path resolver - joins base_dir with file_path.
Args:
base_dir: Base directory for resolving relative paths (None for XML string input)
file_path: The 'file' attribute value to resolve
Returns:
Resolved absolute file path
Raises:
ValueError: If file_path is relative and base_dir is None
"""
if os.path.isabs(file_path):
return os.path.normpath(file_path)
elif base_dir:
return os.path.abspath(os.path.join(base_dir, file_path))
else:
raise ValueError(f"Cannot resolve relative path '{file_path}' without base directory")
def _load_and_expand_mjcf(
source: str,
path_resolver: Callable[[str | None, str], str] = _default_path_resolver,
included_files: set[str] | None = None,
) -> tuple[ET.Element, str | None]:
"""Load MJCF source and recursively expand elements.
Args:
source: File path or XML string
path_resolver: Callback to resolve file paths. Takes (base_dir, file_path) and returns:
- For elements: either an absolute file path or XML content directly
- For asset elements (mesh, texture, etc.): must return an absolute file path
Default resolver joins paths and returns absolute file paths.
included_files: Set of already-included file paths for cycle detection
Returns:
Tuple of (root element, base directory or None for XML string input)
Raises:
ValueError: If a circular include is detected
"""
if included_files is None:
included_files = set()
# Load source
if is_xml_content(source):
base_dir = None # No base directory for XML strings
root = ET.fromstring(sanitize_xml_content(source))
else:
# Treat as file path
base_dir = os.path.dirname(source) or "."
root = ET.parse(source).getroot()
# Extract this file's own meshdir/texturedir BEFORE expanding
# includes, so nested-include compilers cannot shadow it.
own_compiler = root.find("compiler")
own_meshdir = own_compiler.attrib.get("meshdir", ".") if own_compiler is not None else "."
own_texturedir = own_compiler.attrib.get("texturedir", own_meshdir) if own_compiler is not None else "."
# Strip consumed meshdir/texturedir so they don't leak into the parent tree
# and affect parent-file asset resolution. Other compiler attributes (angle, etc.)
# are left intact to match MuJoCo's include-as-paste semantics.
if own_compiler is not None:
own_compiler.attrib.pop("meshdir", None)
own_compiler.attrib.pop("texturedir", None)
# Find all (parent, include) pairs in a single pass
include_pairs = [(parent, child) for parent in root.iter() for child in parent if child.tag == "include"]
for parent, include in include_pairs:
file_attr = include.get("file")
if not file_attr:
continue
resolved = path_resolver(base_dir, file_attr)
if not is_xml_content(resolved):
# Cycle detection for file paths
if resolved in included_files:
raise ValueError(f"Circular include detected: {resolved}")
included_files.add(resolved)
# Recursive call - each included file extracts its own compiler and
# resolves its own asset paths before returning.
included_root, _ = _load_and_expand_mjcf(resolved, path_resolver, included_files)
# Replace include element with children of included root
idx = list(parent).index(include)
parent.remove(include)
for i, child in enumerate(included_root):
parent.insert(idx + i, child)
# Resolve this file's own relative asset paths using the pre-extracted
# meshdir/texturedir. Paths from nested includes are already absolute
# (resolved in their own recursive call), so the isabs check skips them.
_asset_dir_tags = {"mesh": own_meshdir, "hfield": own_meshdir, "texture": own_texturedir}
for elem in root.iter():
file_attr = elem.get("file")
if file_attr and not os.path.isabs(file_attr):
asset_dir = _asset_dir_tags.get(elem.tag, ".")
resolved_path = os.path.join(asset_dir, file_attr) if asset_dir != "." else file_attr
if base_dir is not None or os.path.isabs(resolved_path):
elem.set("file", path_resolver(base_dir, resolved_path))
return root, base_dir
AttributeFrequency = Model.AttributeFrequency
def parse_mjcf(
builder: ModelBuilder,
source: str,
*,
xform: Transform | None = None,
floating: bool | None = None,
base_joint: dict | None = None,
parent_body: int = -1,
armature_scale: float = 1.0,
scale: float = 1.0,
hide_visuals: bool = False,
parse_visuals_as_colliders: bool = False,
parse_meshes: bool = True,
parse_sites: bool = True,
parse_visuals: bool = True,
parse_mujoco_options: bool = True,
up_axis: AxisType = Axis.Z,
ignore_names: Sequence[str] = (),
ignore_classes: Sequence[str] = (),
visual_classes: Sequence[str] = ("visual",),
collider_classes: Sequence[str] = ("collision",),
no_class_as_colliders: bool = True,
force_show_colliders: bool = False,
enable_self_collisions: bool = True,
ignore_inertial_definitions: bool = False,
collapse_fixed_joints: bool = False,
verbose: bool = False,
skip_equality_constraints: bool = False,
convert_3d_hinge_to_ball_joints: bool = False,
mesh_maxhullvert: int | None = None,
ctrl_direct: bool = False,
path_resolver: Callable[[str | None, str], str] | None = None,
override_root_xform: bool = False,
):
"""
Parses MuJoCo XML (MJCF) file and adds the bodies and joints to the given ModelBuilder.
MuJoCo-specific custom attributes are registered on the builder automatically.
Args:
builder (ModelBuilder): The :class:`ModelBuilder` to add the bodies and joints to.
source: The filename of the MuJoCo file to parse, or the MJCF XML string content.
xform: The transform to apply to the imported mechanism.
override_root_xform: If ``True``, the articulation root's world-space
transform is replaced by ``xform`` instead of being composed with it,
preserving only the internal structure (relative body positions). Useful
for cloning articulations at explicit positions. Not intended for sources
containing multiple articulations, as all roots would be placed at the
same ``xform``. Defaults to ``False``.
floating: Controls the base joint type for the root body.
- ``None`` (default): Uses format-specific default (honors ```` tags in MJCF,
otherwise creates a FIXED joint).
- ``True``: Creates a FREE joint with 6 DOF (3 translation + 3 rotation). Only valid when
``parent_body == -1`` since FREE joints must connect to world frame.
- ``False``: Creates a FIXED joint (0 DOF).
Cannot be specified together with ``base_joint``.
base_joint: Custom joint specification for connecting the root body to the world
(or to ``parent_body`` if specified). This parameter enables hierarchical composition with
custom mobility. Dictionary with joint parameters as accepted by
:meth:`ModelBuilder.add_joint` (e.g., joint type, axes, limits, stiffness).
Cannot be specified together with ``floating``.
parent_body: Parent body index for hierarchical composition. If specified, attaches the
imported root body to this existing body, making them part of the same kinematic articulation.
The connection type is determined by ``floating`` or ``base_joint``. If ``-1`` (default),
the root connects to the world frame. **Restriction**: Only the most recently added
articulation can be used as parent; attempting to attach to an older articulation will raise
a ``ValueError``.
.. note::
Valid combinations of ``floating``, ``base_joint``, and ``parent_body``:
.. list-table::
:header-rows: 1
:widths: 15 15 15 55
* - floating
- base_joint
- parent_body
- Result
* - ``None``
- ``None``
- ``-1``
- Format default (MJCF: honors ````, else FIXED)
* - ``True``
- ``None``
- ``-1``
- FREE joint to world (6 DOF)
* - ``False``
- ``None``
- ``-1``
- FIXED joint to world (0 DOF)
* - ``None``
- ``{dict}``
- ``-1``
- Custom joint to world (e.g., D6)
* - ``False``
- ``None``
- ``body_idx``
- FIXED joint to parent body
* - ``None``
- ``{dict}``
- ``body_idx``
- Custom joint to parent body (e.g., D6)
* - *explicitly set*
- *explicitly set*
- *any*
- ❌ Error: mutually exclusive (cannot specify both)
* - ``True``
- ``None``
- ``body_idx``
- ❌ Error: FREE joints require world frame
armature_scale: Scaling factor to apply to the MJCF-defined joint armature values.
scale: The scaling factor to apply to the imported mechanism.
hide_visuals: If True, hide visual shapes after loading them (affects visibility, not loading).
parse_visuals_as_colliders: If True, the geometry defined under the `visual_classes` tags is used for collision handling instead of the `collider_classes` geometries.
parse_meshes: Whether geometries of type `"mesh"` should be parsed. If False, geometries of type `"mesh"` are ignored.
parse_sites: Whether sites (non-colliding reference points) should be parsed. If False, sites are ignored.
parse_visuals: Whether visual geometries (non-collision shapes) should be loaded. If False, visual shapes are not loaded (different from `hide_visuals` which loads but hides them). Default is True.
parse_mujoco_options: Whether solver options from the MJCF `` tag should be parsed. If False, solver options are not loaded and custom attributes retain their default values. Default is True.
up_axis: The up axis of the MuJoCo scene. The default is Z up.
ignore_names: A list of regular expressions. Bodies and joints with a name matching one of the regular expressions will be ignored.
ignore_classes: A list of regular expressions. Bodies and joints with a class matching one of the regular expressions will be ignored.
visual_classes: A list of regular expressions. Visual geometries with a class matching one of the regular expressions will be parsed.
collider_classes: A list of regular expressions. Collision geometries with a class matching one of the regular expressions will be parsed.
no_class_as_colliders: If True, geometries without a class are parsed as collision geometries. If False, geometries without a class are parsed as visual geometries.
force_show_colliders: If True, the collision shapes are always shown, even if there are visual shapes.
enable_self_collisions: If True, self-collisions are enabled.
ignore_inertial_definitions: If True, the inertial parameters defined in the MJCF are ignored and the inertia is calculated from the shape geometry.
collapse_fixed_joints: If True, fixed joints are removed and the respective bodies are merged.
verbose: If True, print additional information about parsing the MJCF.
skip_equality_constraints: Whether tags should be parsed. If True, equality constraints are ignored.
convert_3d_hinge_to_ball_joints: If True, series of three hinge joints are converted to a single ball joint. Default is False.
mesh_maxhullvert: Maximum vertices for convex hull approximation of meshes.
ctrl_direct: If True, all actuators use :attr:`~newton.solvers.SolverMuJoCo.CtrlSource.CTRL_DIRECT` mode
where control comes directly from ``control.mujoco.ctrl`` (MuJoCo-native behavior).
See :ref:`custom_attributes` for details on custom attributes. If False (default), position/velocity
actuators use :attr:`~newton.solvers.SolverMuJoCo.CtrlSource.JOINT_TARGET` mode where control comes
from :attr:`newton.Control.joint_target_pos` and :attr:`newton.Control.joint_target_vel`.
path_resolver: Callback to resolve file paths. Takes (base_dir, file_path) and returns a resolved path. For elements, can return either a file path or XML content directly. For asset elements (mesh, texture, etc.), must return an absolute file path. The default resolver joins paths and returns absolute file paths.
"""
# Early validation of base joint parameters
builder._validate_base_joint_params(floating, base_joint, parent_body)
if override_root_xform and xform is None:
raise ValueError("override_root_xform=True requires xform to be set")
if mesh_maxhullvert is None:
mesh_maxhullvert = Mesh.MAX_HULL_VERTICES
if xform is None:
xform = wp.transform_identity()
else:
xform = wp.transform(*xform)
if path_resolver is None:
path_resolver = _default_path_resolver
# Convert Path objects to string
source = os.fspath(source) if hasattr(source, "__fspath__") else source
root, base_dir = _load_and_expand_mjcf(source, path_resolver)
mjcf_dirname = base_dir or "." # Backward compatible fallback for mesh paths
use_degrees = True # angles are in degrees by default
euler_seq = [0, 1, 2] # XYZ by default
# load joint defaults
default_joint_limit_lower = builder.default_joint_cfg.limit_lower
default_joint_limit_upper = builder.default_joint_cfg.limit_upper
default_joint_target_ke = builder.default_joint_cfg.target_ke
default_joint_target_kd = builder.default_joint_cfg.target_kd
default_joint_armature = builder.default_joint_cfg.armature
default_joint_effort_limit = builder.default_joint_cfg.effort_limit
# load shape defaults
default_shape_density = builder.default_shape_cfg.density
# Process custom attributes defined for different kinds of shapes, bodies, joints, etc.
builder_custom_attr_shape: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.SHAPE]
)
builder_custom_attr_body: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.BODY]
)
builder_custom_attr_joint: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.JOINT]
)
builder_custom_attr_dof: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.JOINT_DOF]
)
builder_custom_attr_eq: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.EQUALITY_CONSTRAINT]
)
# MuJoCo actuator custom attributes (from "mujoco:actuator" frequency)
builder_custom_attr_actuator: list[ModelBuilder.CustomAttribute] = [
attr for attr in builder.custom_attributes.values() if attr.frequency == "mujoco:actuator"
]
compiler = root.find("compiler")
if compiler is not None:
use_degrees = compiler.attrib.get("angle", "degree").lower() == "degree"
euler_seq = ["xyz".index(c) for c in compiler.attrib.get("eulerseq", "xyz").lower()]
mesh_dir = compiler.attrib.get("meshdir", ".")
texture_dir = compiler.attrib.get("texturedir", mesh_dir)
fitaabb = compiler.attrib.get("fitaabb", "false").lower() == "true"
else:
mesh_dir = "."
texture_dir = "."
fitaabb = False
# Parse MJCF compiler and option tags for ONCE and WORLD frequency custom attributes
# WORLD frequency attributes use index 0 here; they get remapped during add_world()
# Use findall for to handle multiple elements after include expansion
# (later values override earlier ones, matching MuJoCo's merge behavior).
if parse_mujoco_options:
builder_custom_attr_option: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.ONCE, AttributeFrequency.WORLD]
)
if builder_custom_attr_option:
option_elems = [compiler, *root.findall("option")]
for elem in option_elems:
if elem is not None:
parsed = parse_custom_attributes(elem.attrib, builder_custom_attr_option, "mjcf")
for key, value in parsed.items():
if key in builder.custom_attributes:
builder.custom_attributes[key].values[0] = value
class_parent = {}
class_children = {}
class_defaults = {"__all__": {}}
def get_class(element) -> str:
return element.get("class", "__all__")
def parse_default(node, parent):
nonlocal class_parent
nonlocal class_children
nonlocal class_defaults
class_name = "__all__"
if "class" in node.attrib:
class_name = node.attrib["class"]
class_parent[class_name] = parent
parent = parent or "__all__"
if parent not in class_children:
class_children[parent] = []
class_children[parent].append(class_name)
if class_name not in class_defaults:
class_defaults[class_name] = {}
for child in node:
if child.tag == "default":
parse_default(child, node.get("class"))
else:
class_defaults[class_name][child.tag] = child.attrib
for default in root.findall("default"):
parse_default(default, None)
def merge_attrib(default_attrib: dict, incoming_attrib: dict) -> dict:
attrib = default_attrib.copy()
for key, value in incoming_attrib.items():
if key in attrib:
if isinstance(attrib[key], dict):
attrib[key] = merge_attrib(attrib[key], value)
else:
attrib[key] = value
else:
attrib[key] = value
return attrib
def resolve_defaults(class_name):
if class_name in class_children:
for child_name in class_children[class_name]:
if class_name in class_defaults and child_name in class_defaults:
class_defaults[child_name] = merge_attrib(class_defaults[class_name], class_defaults[child_name])
resolve_defaults(child_name)
resolve_defaults("__all__")
mesh_assets = {}
texture_assets = {}
material_assets = {}
hfield_assets = {}
for asset in root.findall("asset"):
for mesh in asset.findall("mesh"):
if "file" in mesh.attrib:
fname = os.path.join(mesh_dir, mesh.attrib["file"])
# handle stl relative paths
if not os.path.isabs(fname):
fname = os.path.abspath(os.path.join(mjcf_dirname, fname))
# resolve mesh element's class defaults
mesh_class = mesh.attrib.get("class", "__all__")
mesh_defaults = class_defaults.get(mesh_class, {}).get("mesh", {})
mesh_attrib = merge_attrib(mesh_defaults, mesh.attrib)
name = mesh.attrib.get("name", ".".join(os.path.basename(fname).split(".")[:-1]))
s = mesh_attrib.get("scale", "1.0 1.0 1.0")
s = np.fromstring(s, sep=" ", dtype=np.float32)
# parse maxhullvert attribute, default to mesh_maxhullvert if not specified
maxhullvert = int(mesh_attrib.get("maxhullvert", str(mesh_maxhullvert)))
mesh_assets[name] = {"file": fname, "scale": s, "maxhullvert": maxhullvert}
for texture in asset.findall("texture"):
tex_name = texture.attrib.get("name")
tex_file = texture.attrib.get("file")
if not tex_name or not tex_file:
continue
tex_path = os.path.join(texture_dir, tex_file)
if not os.path.isabs(tex_path):
tex_path = os.path.abspath(os.path.join(mjcf_dirname, tex_path))
texture_assets[tex_name] = {"file": tex_path}
for material in asset.findall("material"):
mat_name = material.attrib.get("name")
if not mat_name:
continue
material_assets[mat_name] = {
"rgba": material.attrib.get("rgba"),
"texture": material.attrib.get("texture"),
}
for hfield in asset.findall("hfield"):
hfield_name = hfield.attrib.get("name")
if not hfield_name:
continue
# Parse attributes
nrow = int(hfield.attrib.get("nrow", "100"))
ncol = int(hfield.attrib.get("ncol", "100"))
size_str = hfield.attrib.get("size", "1 1 1 0")
size_arr = np.fromstring(size_str, sep=" ", dtype=np.float32)
if size_arr.size < 4:
size_arr = np.pad(size_arr, (0, 4 - size_arr.size), constant_values=0.0)
size = tuple(size_arr[:4])
# Parse optional file path
file_attr = hfield.attrib.get("file")
file_path = None
if file_attr:
file_path = path_resolver(base_dir, file_attr)
# Parse optional inline elevation data
elevation_str = hfield.attrib.get("elevation")
elevation_data = None
if elevation_str and not file_attr:
elevation_arr = np.fromstring(elevation_str, sep=" ", dtype=np.float32)
if elevation_arr.size == nrow * ncol:
elevation_data = elevation_arr.reshape(nrow, ncol)
elif verbose:
print(
f"Warning: hfield '{hfield_name}' elevation has {elevation_arr.size} values, "
f"expected {nrow * ncol} ({nrow}x{ncol}), ignoring"
)
hfield_assets[hfield_name] = {
"nrow": nrow,
"ncol": ncol,
"size": size, # (size_x, size_y, size_z, size_base)
"file": file_path,
"elevation": elevation_data,
}
axis_xform = wp.transform(wp.vec3(0.0), quat_between_axes(up_axis, builder.up_axis))
xform = xform * axis_xform
def parse_float(attrib, key, default) -> float:
if key in attrib:
return float(attrib[key])
else:
return default
def parse_vec(attrib, key, default):
if key in attrib:
out = np.fromstring(attrib[key], sep=" ", dtype=np.float32)
else:
out = np.array(default, dtype=np.float32)
length = len(out)
if length == 1:
return wp.types.vector(len(default), wp.float32)(out[0], out[0], out[0])
return wp.types.vector(length, wp.float32)(out)
def quat_from_euler_mjcf(e: wp.vec3, i: int, j: int, k: int) -> wp.quat:
"""Convert Euler angles using MuJoCo's axis-sequence convention."""
half_e = e * 0.5
cr = wp.cos(half_e[i])
sr = wp.sin(half_e[i])
cp = wp.cos(half_e[j])
sp = wp.sin(half_e[j])
cy = wp.cos(half_e[k])
sy = wp.sin(half_e[k])
return wp.quat(
(cy * sr * cp - sy * cr * sp),
(cy * cr * sp + sy * sr * cp),
(sy * cr * cp - cy * sr * sp),
(cy * cr * cp + sy * sr * sp),
)
def parse_orientation(attrib) -> wp.quat:
if "quat" in attrib:
wxyz = np.fromstring(attrib["quat"], sep=" ")
return wp.normalize(wp.quat(*wxyz[1:], wxyz[0]))
if "euler" in attrib:
euler = np.fromstring(attrib["euler"], sep=" ")
if use_degrees:
euler *= np.pi / 180
# Keep MuJoCo-compatible semantics for non-XYZ sequences.
return quat_from_euler_mjcf(wp.vec3(euler), *euler_seq)
if "axisangle" in attrib:
axisangle = np.fromstring(attrib["axisangle"], sep=" ")
angle = axisangle[3]
if use_degrees:
angle *= np.pi / 180
axis = wp.normalize(wp.vec3(*axisangle[:3]))
return wp.quat_from_axis_angle(axis, float(angle))
if "xyaxes" in attrib:
xyaxes = np.fromstring(attrib["xyaxes"], sep=" ")
xaxis = wp.normalize(wp.vec3(*xyaxes[:3]))
zaxis = wp.normalize(wp.vec3(*xyaxes[3:]))
yaxis = wp.normalize(wp.cross(zaxis, xaxis))
rot_matrix = np.array([xaxis, yaxis, zaxis]).T
return wp.quat_from_matrix(wp.mat33(rot_matrix))
if "zaxis" in attrib:
zaxis = np.fromstring(attrib["zaxis"], sep=" ")
zaxis = wp.normalize(wp.vec3(*zaxis))
xaxis = wp.normalize(wp.cross(wp.vec3(0, 0, 1), zaxis))
yaxis = wp.normalize(wp.cross(zaxis, xaxis))
rot_matrix = np.array([xaxis, yaxis, zaxis]).T
return wp.quat_from_matrix(wp.mat33(rot_matrix))
return wp.quat_identity()
def parse_shapes(
defaults, body_name, link, geoms, density, visible=True, just_visual=False, incoming_xform=None, label_prefix=""
):
shapes = []
for geo_count, geom in enumerate(geoms):
geom_defaults = defaults
if "class" in geom.attrib:
geom_class = geom.attrib["class"]
ignore_geom = False
for pattern in ignore_classes:
if re.match(pattern, geom_class):
ignore_geom = True
break
if ignore_geom:
continue
if geom_class in class_defaults:
geom_defaults = merge_attrib(defaults, class_defaults[geom_class])
if "geom" in geom_defaults:
geom_attrib = merge_attrib(geom_defaults["geom"], geom.attrib)
else:
geom_attrib = geom.attrib
geom_name = geom_attrib.get("name", f"{body_name}_geom_{geo_count}{'_visual' if just_visual else ''}")
geom_type = geom_attrib.get("type", "sphere")
fit_to_mesh = False
if "mesh" in geom_attrib:
if "type" in geom_attrib and geom_type in {"sphere", "capsule", "cylinder", "ellipsoid", "box"}:
fit_to_mesh = True
else:
geom_type = "mesh"
if "hfield" in geom_attrib:
geom_type = "hfield"
ignore_geom = False
for pattern in ignore_names:
if re.match(pattern, geom_name):
ignore_geom = True
break
if ignore_geom:
continue
geom_size = parse_vec(geom_attrib, "size", [1.0, 1.0, 1.0]) * scale
geom_pos = parse_vec(geom_attrib, "pos", (0.0, 0.0, 0.0)) * scale
geom_rot = parse_orientation(geom_attrib)
tf = wp.transform(geom_pos, geom_rot)
if incoming_xform is not None:
tf = incoming_xform * tf
geom_density = parse_float(geom_attrib, "density", density)
geom_mass_explicit = None
# MuJoCo: explicit mass attribute (from or class defaults).
# Skip density-based mass contribution and compute inertia directly from mass.
if "mass" in geom_attrib:
geom_mass_explicit = parse_float(geom_attrib, "mass", 0.0)
# Set density to 0 to skip density-based mass contribution
# We'll add the explicit mass to the body separately
geom_density = 0.0
shape_cfg = builder.default_shape_cfg.copy()
shape_cfg.is_visible = visible
shape_cfg.has_shape_collision = not just_visual
shape_cfg.has_particle_collision = not just_visual
shape_cfg.density = geom_density
# Respect MJCF contype/conaffinity=0: disable automatic broadphase contacts
# while keeping the shape as a collider for explicit contacts.
contype = int(geom_attrib.get("contype", 1))
conaffinity = int(geom_attrib.get("conaffinity", 1))
if contype == 0 and conaffinity == 0 and not just_visual:
shape_cfg.collision_group = 0
# Parse MJCF friction: "slide [torsion [roll]]"
# Can't use parse_vec - it would replicate single values to all dimensions
if "friction" in geom_attrib:
friction_values = np.fromstring(geom_attrib["friction"], sep=" ", dtype=np.float32)
if len(friction_values) >= 1:
shape_cfg.mu = float(friction_values[0])
if len(friction_values) >= 2:
shape_cfg.mu_torsional = float(friction_values[1])
if len(friction_values) >= 3:
shape_cfg.mu_rolling = float(friction_values[2])
# Parse MJCF solref for contact stiffness/damping (only if explicitly specified)
# Like friction, only override Newton defaults if solref is authored in MJCF
if "solref" in geom_attrib:
solref = parse_vec(geom_attrib, "solref", (0.02, 1.0))
geom_ke, geom_kd = solref_to_stiffness_damping(solref)
if geom_ke is not None:
shape_cfg.ke = geom_ke
if geom_kd is not None:
shape_cfg.kd = geom_kd
# Parse MJCF margin and gap for collision.
# MuJoCo -> Newton conversion: newton_margin = mj_margin - mj_gap.
# When gap is absent, mj_gap defaults to 0 for the margin conversion.
# When margin is absent but gap is present, shape_cfg.margin keeps its
# default (matching MuJoCo's default margin=0 minus gap would produce a
# negative value, which is invalid).
mj_gap = float(geom_attrib.get("gap", "0")) * scale
if "margin" in geom_attrib:
mj_margin = float(geom_attrib["margin"]) * scale
newton_margin = mj_margin - mj_gap
if newton_margin < 0.0:
warnings.warn(
f"Geom '{geom_name}': MuJoCo gap ({mj_gap}) exceeds margin ({mj_margin}), "
f"resulting Newton margin is negative ({newton_margin}). "
f"This may indicate an invalid MuJoCo model.",
stacklevel=2,
)
shape_cfg.margin = newton_margin
if "gap" in geom_attrib:
shape_cfg.gap = mj_gap
custom_attributes = parse_custom_attributes(geom_attrib, builder_custom_attr_shape, parsing_mode="mjcf")
shape_label = f"{label_prefix}/{geom_name}" if label_prefix else geom_name
shape_kwargs = {
"label": shape_label,
"body": link,
"cfg": shape_cfg,
"custom_attributes": custom_attributes,
}
material_name = geom_attrib.get("material")
material_info = material_assets.get(material_name, {})
rgba = geom_attrib.get("rgba", material_info.get("rgba"))
material_color = None
if rgba is not None:
rgba_values = np.fromstring(rgba, sep=" ", dtype=np.float32)
if len(rgba_values) >= 3:
material_color = (
float(rgba_values[0]),
float(rgba_values[1]),
float(rgba_values[2]),
)
texture = None
texture_name = material_info.get("texture")
if texture_name:
texture_asset = texture_assets.get(texture_name)
if texture_asset and "file" in texture_asset:
# Pass texture path directly for lazy loading by the viewer
texture = texture_asset["file"]
# Fit primitive to mesh: load mesh vertices, compute fitted sizes,
# and override geom_size / tf so the primitive handlers below work
# transparently.
if fit_to_mesh:
mesh_name = geom_attrib.get("mesh")
if mesh_name is None or mesh_name not in mesh_assets:
if verbose:
print(f"Warning: mesh asset for fitting not found for {geom_name}, skipping geom")
continue
else:
stl_file = mesh_assets[mesh_name]["file"]
if "mesh" in geom_defaults:
mesh_scale = parse_vec(geom_defaults["mesh"], "scale", mesh_assets[mesh_name]["scale"])
else:
mesh_scale = mesh_assets[mesh_name]["scale"]
scaling = np.array(mesh_scale) * scale
maxhullvert = mesh_assets[mesh_name].get("maxhullvert", mesh_maxhullvert)
m_meshes = load_meshes_from_file(
stl_file,
scale=scaling,
maxhullvert=maxhullvert,
)
# Combine all sub-meshes into one vertex array for fitting.
all_vertices = np.concatenate([m.vertices for m in m_meshes], axis=0)
fitscale = parse_float(geom_attrib, "fitscale", 1.0)
if fitaabb:
# AABB mode: compute axis-aligned bounding box.
aabb_min, aabb_max = compute_aabb(all_vertices)
center = (aabb_min + aabb_max) / 2.0
half_sizes = (aabb_max - aabb_min) / 2.0
if geom_type == "sphere":
geom_size = np.array([np.max(half_sizes)]) * fitscale
elif geom_type in {"capsule", "cylinder"}:
r = max(half_sizes[0], half_sizes[1])
h = half_sizes[2]
if geom_type == "capsule":
h = max(h - r, 0.0)
geom_size = np.array([r, h]) * fitscale
elif geom_type in {"box", "ellipsoid"}:
geom_size = half_sizes * fitscale
else:
if verbose:
print(f"Warning: unsupported fit type {geom_type} for {geom_name}")
fit_to_mesh = False
if fit_to_mesh:
# Shift the geom origin to the AABB center.
center_offset = wp.vec3(*center)
tf = tf * wp.transform(center_offset, wp.quat_identity())
else:
# Equivalent inertia box mode (default): compute the box whose
# inertia tensor matches the mesh.
all_indices = np.concatenate(
[
m.indices.reshape(-1, 3) + offset
for m, offset in zip(
m_meshes,
np.cumsum([0] + [len(m.vertices) for m in m_meshes[:-1]]),
strict=True,
)
],
axis=0,
).flatten()
com, half_extents, principal_rot = compute_inertia_box_mesh(all_vertices, all_indices)
# Sort half-extents so the largest is last (Z), matching MuJoCo's
# convention where capsule/cylinder axis aligns with Z.
he_arr = np.array([*half_extents])
sort_order = np.argsort(he_arr)
he = he_arr[sort_order]
if geom_type == "sphere":
geom_size = np.array([np.mean(he)]) * fitscale
elif geom_type in {"capsule", "cylinder"}:
r = (he[0] + he[1]) / 2.0
h = he[2]
if geom_type == "capsule":
# Subtract r/2 (not full r) to match MuJoCo.
h = max(h - r / 2.0, 0.0)
geom_size = np.array([r, h]) * fitscale
elif geom_type in {"box", "ellipsoid"}:
geom_size = he * fitscale
else:
if verbose:
print(f"Warning: unsupported fit type {geom_type} for {geom_name}")
fit_to_mesh = False
if fit_to_mesh:
# Build a rotation that maps the sorted principal axes
# to the standard frame (X, Y, Z). The eigenvectors in
# principal_rot are in the original eigenvalue order; we
# need to reorder columns to match the sorted half-extents.
# Rows of warp mat33 = basis vectors of the rotated frame.
rot_mat = np.array(wp.quat_to_matrix(principal_rot)).reshape(3, 3)
# rot_mat rows are the principal axes; reorder them so
# the axis with the largest half-extent becomes row 2 (Z).
sorted_mat = rot_mat[sort_order, :]
if np.linalg.det(sorted_mat) < 0:
sorted_mat[0, :] = -sorted_mat[0, :]
fit_rot = wp.quat_from_matrix(wp.mat33(*sorted_mat.flatten().tolist()))
# Shift the geom origin to the mesh COM and rotate to
# the principal-axis frame.
center_offset = wp.vec3(*com)
tf = tf * wp.transform(center_offset, fit_rot)
if geom_type == "sphere":
s = builder.add_shape_sphere(
xform=tf,
radius=geom_size[0],
**shape_kwargs,
)
shapes.append(s)
elif geom_type == "box":
s = builder.add_shape_box(
xform=tf,
hx=geom_size[0],
hy=geom_size[1],
hz=geom_size[2],
**shape_kwargs,
)
shapes.append(s)
elif geom_type == "mesh" and parse_meshes:
mesh_attrib = geom_attrib.get("mesh")
if mesh_attrib is None:
if verbose:
print(f"Warning: mesh attribute not defined for {geom_name}, skipping")
continue
elif mesh_attrib not in mesh_assets:
if verbose:
print(f"Warning: mesh asset {geom_attrib['mesh']} not found, skipping")
continue
stl_file = mesh_assets[geom_attrib["mesh"]]["file"]
mesh_scale = mesh_assets[geom_attrib["mesh"]]["scale"]
scaling = np.array(mesh_scale) * scale
# as per the Mujoco XML reference, ignore geom size attribute
assert len(geom_size) == 3, "need to specify size for mesh geom"
# get maxhullvert value from mesh assets
maxhullvert = mesh_assets[geom_attrib["mesh"]].get("maxhullvert", mesh_maxhullvert)
m_meshes = load_meshes_from_file(
stl_file,
scale=scaling,
maxhullvert=maxhullvert,
override_color=material_color,
override_texture=texture,
)
for m_mesh in m_meshes:
if m_mesh.texture is not None and m_mesh.uvs is None:
if verbose:
print(f"Warning: mesh {stl_file} has a texture but no UVs; texture will be ignored.")
m_mesh.texture = None
# Mesh shapes must not use cfg.sdf_*; SDFs are built on the mesh itself.
mesh_shape_kwargs = dict(shape_kwargs)
mesh_cfg = shape_cfg.copy()
mesh_cfg.sdf_max_resolution = None
mesh_cfg.sdf_target_voxel_size = None
mesh_cfg.sdf_narrow_band_range = (-0.1, 0.1)
mesh_shape_kwargs["cfg"] = mesh_cfg
s = builder.add_shape_mesh(
xform=tf,
mesh=m_mesh,
**mesh_shape_kwargs,
)
shapes.append(s)
elif geom_type in {"capsule", "cylinder"}:
if "fromto" in geom_attrib:
geom_fromto = parse_vec(geom_attrib, "fromto", (0.0, 0.0, 0.0, 1.0, 0.0, 0.0))
start = wp.vec3(geom_fromto[0:3]) * scale
end = wp.vec3(geom_fromto[3:6]) * scale
# Apply incoming_xform to fromto coordinates
if incoming_xform is not None:
start = wp.transform_point(incoming_xform, start)
end = wp.transform_point(incoming_xform, end)
# Compute pos and quat matching MuJoCo's fromto convention:
# direction = start - end, align Z axis with it (mjuu_z2quat).
# quat_between_vectors degenerates for anti-parallel vectors,
# so handle that case with an explicit 180° rotation around X.
# Guard against zero-length fromto (start == end) which would
# produce NaN from wp.quat_between_vectors.
geom_pos = (start + end) * 0.5
dir_vec = start - end
dir_len = wp.length(dir_vec)
if dir_len < 1.0e-6:
geom_rot = wp.quat_identity()
else:
direction = dir_vec / dir_len
if float(direction[2]) < -0.999999:
geom_rot = wp.quat(1.0, 0.0, 0.0, 0.0) # 180° around X
else:
geom_rot = wp.quat_between_vectors(wp.vec3(0.0, 0.0, 1.0), direction)
tf = wp.transform(geom_pos, geom_rot)
geom_radius = geom_size[0]
geom_height = dir_len * 0.5
else:
geom_radius = geom_size[0]
geom_height = geom_size[1]
if geom_type == "cylinder":
s = builder.add_shape_cylinder(
xform=tf,
radius=geom_radius,
half_height=geom_height,
**shape_kwargs,
)
shapes.append(s)
else:
s = builder.add_shape_capsule(
xform=tf,
radius=geom_radius,
half_height=geom_height,
**shape_kwargs,
)
shapes.append(s)
elif geom_type == "hfield" and parse_meshes:
hfield_name = geom_attrib.get("hfield")
if hfield_name is None:
if verbose:
print(f"Warning: hfield attribute not defined for {geom_name}, skipping")
continue
elif hfield_name not in hfield_assets:
if verbose:
print(f"Warning: hfield asset '{hfield_name}' not found, skipping")
continue
hfield_asset = hfield_assets[hfield_name]
nrow, ncol = hfield_asset["nrow"], hfield_asset["ncol"]
if hfield_asset["elevation"] is not None:
elevation = hfield_asset["elevation"]
elif hfield_asset["file"] is not None:
elevation = load_heightfield_elevation(hfield_asset["file"], nrow, ncol)
else:
elevation = np.zeros((nrow, ncol), dtype=np.float32)
# Convert MuJoCo size (size_x, size_y, size_z, size_base) to Newton format.
# In MuJoCo, the heightfield's lowest point (data=0) is at the geom origin,
# so min_z=0 and max_z=size_z. size_base (depth below origin) is ignored.
mj_size_x, mj_size_y, mj_size_z, _mj_size_base = hfield_asset["size"]
heightfield = Heightfield(
data=elevation,
nrow=nrow,
ncol=ncol,
hx=mj_size_x * scale,
hy=mj_size_y * scale,
min_z=0.0,
max_z=mj_size_z * scale,
)
# Heightfields are always static — don't pass body from shape_kwargs
hfield_kwargs = {k: v for k, v in shape_kwargs.items() if k != "body"}
s = builder.add_shape_heightfield(
xform=tf,
heightfield=heightfield,
**hfield_kwargs,
)
shapes.append(s)
elif geom_type == "plane":
# Use xform directly - plane has local normal (0,0,1) and passes through origin
# The transform tf positions and orients the plane in world space
s = builder.add_shape_plane(
xform=tf,
width=geom_size[0],
length=geom_size[1],
**shape_kwargs,
)
shapes.append(s)
elif geom_type == "ellipsoid":
s = builder.add_shape_ellipsoid(
xform=tf,
rx=geom_size[0],
ry=geom_size[1],
rz=geom_size[2],
**shape_kwargs,
)
shapes.append(s)
else:
if verbose:
print(f"MJCF parsing shape {geom_name} issue: geom type {geom_type} is unsupported")
# Handle explicit mass: compute inertia using existing functions, add to body
if geom_mass_explicit is not None and geom_mass_explicit > 0.0 and link >= 0 and not just_visual:
from ..geometry.inertia import ( # noqa: PLC0415
compute_inertia_box_from_mass,
compute_inertia_capsule,
compute_inertia_cylinder,
compute_inertia_ellipsoid,
compute_inertia_sphere,
)
# Compute inertia by calling functions with density=1.0, then scale by mass ratio
# This avoids manual volume computation - functions handle it internally
com = wp.vec3() # center of mass (at origin for primitives)
inertia_tensor = wp.mat33()
inertia_computed = False
if geom_type == "sphere":
r = geom_size[0]
m_computed, com, inertia_tensor = compute_inertia_sphere(1.0, r)
if m_computed > 1e-6:
inertia_tensor = inertia_tensor * (geom_mass_explicit / m_computed)
inertia_computed = True
elif geom_type == "box":
# Box has a direct mass-based function - no scaling needed
# geom_size is already half-extents, so use directly
hx, hy, hz = geom_size[0], geom_size[1], geom_size[2]
inertia_tensor = compute_inertia_box_from_mass(geom_mass_explicit, hx, hy, hz)
inertia_computed = True
elif geom_type == "cylinder":
m_computed, com, inertia_tensor = compute_inertia_cylinder(1.0, geom_radius, geom_height)
if m_computed > 1e-6:
inertia_tensor = inertia_tensor * (geom_mass_explicit / m_computed)
inertia_computed = True
elif geom_type == "capsule":
m_computed, com, inertia_tensor = compute_inertia_capsule(1.0, geom_radius, geom_height)
if m_computed > 1e-6:
inertia_tensor = inertia_tensor * (geom_mass_explicit / m_computed)
inertia_computed = True
elif geom_type == "ellipsoid":
rx, ry, rz = geom_size[0], geom_size[1], geom_size[2]
m_computed, com, inertia_tensor = compute_inertia_ellipsoid(1.0, rx, ry, rz)
if m_computed > 1e-6:
inertia_tensor = inertia_tensor * (geom_mass_explicit / m_computed)
inertia_computed = True
else:
warnings.warn(
f"explicit mass ({geom_mass_explicit}) on geom '{geom_name}' "
f"with type '{geom_type}' is not supported — mass will be ignored",
stacklevel=2,
)
# Add explicit mass and computed inertia to body (skip if inertia is locked by )
if inertia_computed and not builder.body_lock_inertia[link]:
com_body = wp.transform_point(tf, com)
builder._update_body_mass(link, geom_mass_explicit, inertia_tensor, com_body, tf.q)
return shapes
def _parse_sites_impl(defaults, body_name, link, sites, incoming_xform=None, label_prefix=""):
"""Parse site elements from MJCF."""
from ..geometry import GeoType # noqa: PLC0415
site_shapes = []
for site_count, site in enumerate(sites):
site_defaults = defaults
if "class" in site.attrib:
site_class = site.attrib["class"]
ignore_site = False
for pattern in ignore_classes:
if re.match(pattern, site_class):
ignore_site = True
break
if ignore_site:
continue
if site_class in class_defaults:
site_defaults = merge_attrib(defaults, class_defaults[site_class])
if "site" in site_defaults:
site_attrib = merge_attrib(site_defaults["site"], site.attrib)
else:
site_attrib = site.attrib
site_name = site_attrib.get("name", f"{body_name}_site_{site_count}")
# Check if site should be ignored by name
ignore_site = False
for pattern in ignore_names:
if re.match(pattern, site_name):
ignore_site = True
break
if ignore_site:
continue
# Parse site transform
site_pos = parse_vec(site_attrib, "pos", (0.0, 0.0, 0.0)) * scale
site_rot = parse_orientation(site_attrib)
site_xform = wp.transform(site_pos, site_rot)
if incoming_xform is not None:
site_xform = incoming_xform * site_xform
# Parse site type (defaults to sphere if not specified)
site_type = site_attrib.get("type", "sphere")
# Parse site size matching MuJoCo behavior:
# - Default is [0.005, 0.005, 0.005]
# - Partial values fill remaining with defaults (NOT replicating first value)
# - size="0.001" → [0.001, 0.005, 0.005] (matches MuJoCo)
# Note: This differs from parse_vec which would replicate single values
site_size = np.array([0.005, 0.005, 0.005], dtype=np.float32)
if "size" in site_attrib:
size_values = np.fromstring(site_attrib["size"], sep=" ", dtype=np.float32)
for i, val in enumerate(size_values):
if i < 3:
site_size[i] = val
site_size = wp.vec3(site_size * scale)
# Map MuJoCo site types to Newton GeoType
type_map = {
"sphere": GeoType.SPHERE,
"box": GeoType.BOX,
"capsule": GeoType.CAPSULE,
"cylinder": GeoType.CYLINDER,
"ellipsoid": GeoType.ELLIPSOID,
}
geo_type = type_map.get(site_type, GeoType.SPHERE)
# Sites are typically hidden by default
visible = False
# Expand to 3-element vector if needed
if len(site_size) == 2:
# Two values (e.g., capsule/cylinder: radius, half-height)
radius = site_size[0]
half_height = site_size[1]
site_size = wp.vec3(radius, half_height, 0.0)
# Add site using builder.add_site()
site_label = f"{label_prefix}/{site_name}" if label_prefix else site_name
s = builder.add_site(
body=link,
xform=site_xform,
type=geo_type,
scale=site_size,
label=site_label,
visible=visible,
)
site_shapes.append(s)
site_name_to_idx[site_name] = s
return site_shapes
def get_frame_xform(frame_element, incoming_xform: wp.transform) -> wp.transform:
"""Compute composed transform for a frame element."""
frame_pos = parse_vec(frame_element.attrib, "pos", (0.0, 0.0, 0.0)) * scale
frame_rot = parse_orientation(frame_element.attrib)
return incoming_xform * wp.transform(frame_pos, frame_rot)
def _process_body_geoms(
geoms,
defaults: dict,
body_name: str,
link: int,
incoming_xform: wp.transform | None = None,
label_prefix: str = "",
) -> list:
"""Process geoms for a body, partitioning into visuals and colliders.
This helper applies the same filtering/partitioning logic for geoms whether
they appear directly in a or inside a within a body.
Args:
geoms: Iterable of geom XML elements to process.
defaults: The current defaults dictionary.
body_name: Name of the parent body (for naming).
link: The body index.
incoming_xform: Optional transform to apply to geoms.
label_prefix: Hierarchical label prefix for shape labels.
Returns:
List of visual shape indices (if parse_visuals is True).
"""
visuals = []
colliders = []
for geo_count, geom in enumerate(geoms):
geom_defaults = defaults
geom_class = None
if "class" in geom.attrib:
geom_class = geom.attrib["class"]
ignore_geom = False
for pattern in ignore_classes:
if re.match(pattern, geom_class):
ignore_geom = True
break
if ignore_geom:
continue
if geom_class in class_defaults:
geom_defaults = merge_attrib(defaults, class_defaults[geom_class])
if "geom" in geom_defaults:
geom_attrib = merge_attrib(geom_defaults["geom"], geom.attrib)
else:
geom_attrib = geom.attrib
geom_name = geom_attrib.get("name", f"{body_name}_geom_{geo_count}")
contype = geom_attrib.get("contype", 1)
conaffinity = geom_attrib.get("conaffinity", 1)
collides_with_anything = not (int(contype) == 0 and int(conaffinity) == 0)
if geom_class is not None:
neither_visual_nor_collider = True
for pattern in visual_classes:
if re.match(pattern, geom_class):
visuals.append(geom)
neither_visual_nor_collider = False
break
for pattern in collider_classes:
if re.match(pattern, geom_class):
colliders.append(geom)
neither_visual_nor_collider = False
break
if neither_visual_nor_collider:
if no_class_as_colliders and collides_with_anything:
colliders.append(geom)
else:
visuals.append(geom)
else:
no_class_class = "collision" if no_class_as_colliders else "visual"
if verbose:
print(f"MJCF parsing shape {geom_name} issue: no class defined for geom, assuming {no_class_class}")
if no_class_as_colliders and collides_with_anything:
colliders.append(geom)
else:
visuals.append(geom)
visual_shape_indices = []
if parse_visuals_as_colliders:
colliders = visuals
elif parse_visuals:
s = parse_shapes(
defaults,
body_name,
link,
geoms=visuals,
density=default_shape_density,
just_visual=True,
visible=not hide_visuals,
incoming_xform=incoming_xform,
label_prefix=label_prefix,
)
visual_shape_indices.extend(s)
show_colliders = should_show_collider(
force_show_colliders,
has_visual_shapes=len(visuals) > 0 and parse_visuals,
parse_visuals_as_colliders=parse_visuals_as_colliders,
)
parse_shapes(
defaults,
body_name,
link,
geoms=colliders,
density=default_shape_density,
visible=show_colliders,
incoming_xform=incoming_xform,
label_prefix=label_prefix,
)
return visual_shape_indices
def process_frames(
frames,
parent_body: int,
defaults: dict,
childclass: str | None,
world_xform: wp.transform,
body_relative_xform: wp.transform | None = None,
label_prefix: str = "",
track_root_boundaries: bool = False,
):
"""Process frame elements, composing transforms with children.
Frames are pure coordinate transformations that can wrap bodies, geoms, sites, and nested frames.
Args:
frames: Iterable of frame XML elements to process.
parent_body: The parent body index (-1 for world).
defaults: The current defaults dictionary.
childclass: The current childclass for body inheritance.
world_xform: World transform for positioning child bodies.
body_relative_xform: Body-relative transform for geoms/sites. If None, uses world_xform
(appropriate for static geoms at worldbody level).
label_prefix: Hierarchical label prefix for child entity labels.
track_root_boundaries: If True, record root body boundaries for articulation splitting.
"""
# Stack entries: (frame, world_xform, body_relative_xform, frame_defaults, frame_childclass)
# For worldbody frames, body_relative equals world (static geoms use world coords)
if body_relative_xform is None:
frame_stack = [(f, world_xform, world_xform, defaults, childclass) for f in frames]
else:
frame_stack = [(f, world_xform, body_relative_xform, defaults, childclass) for f in frames]
while frame_stack:
frame, frame_world, frame_body_rel, frame_defaults, frame_childclass = frame_stack.pop()
frame_local = get_frame_xform(frame, wp.transform_identity())
composed_world = frame_world * frame_local
composed_body_rel = frame_body_rel * frame_local
# Resolve childclass for this frame's children
_childclass = frame.get("childclass") or frame_childclass
# Compute merged defaults for this frame's children
if _childclass is None:
_defaults = frame_defaults
else:
_defaults = merge_attrib(frame_defaults, class_defaults.get(_childclass, {}))
# Process child bodies (need world transform)
for child_body in frame.findall("body"):
if track_root_boundaries:
cb_name = sanitize_name(child_body.attrib.get("name", f"body_{builder.body_count}"))
root_body_boundaries.append((len(joint_indices), cb_name))
parse_body(
child_body,
parent_body,
_defaults,
childclass=_childclass,
incoming_xform=composed_world,
parent_label_path=label_prefix,
)
# Process child geoms (need body-relative transform)
# Use the same visual/collider partitioning logic as parse_body
child_geoms = frame.findall("geom")
if child_geoms:
body_name = "world" if parent_body == -1 else builder.body_label[parent_body]
frame_visual_shapes = _process_body_geoms(
child_geoms,
_defaults,
body_name,
parent_body,
incoming_xform=composed_body_rel,
label_prefix=label_prefix,
)
visual_shapes.extend(frame_visual_shapes)
# Process child sites (need body-relative transform)
if parse_sites:
child_sites = frame.findall("site")
if child_sites:
body_name = "world" if parent_body == -1 else builder.body_label[parent_body]
_parse_sites_impl(
_defaults,
body_name,
parent_body,
child_sites,
incoming_xform=composed_body_rel,
label_prefix=label_prefix,
)
# Add nested frames to stack with current defaults and childclass (in reverse to maintain order)
frame_stack.extend(
(f, composed_world, composed_body_rel, _defaults, _childclass) for f in reversed(frame.findall("frame"))
)
def parse_body(
body,
parent,
incoming_defaults: dict,
childclass: str | None = None,
incoming_xform: Transform | None = None,
parent_label_path: str = "",
):
"""Parse a body element from MJCF.
Args:
body: The XML body element.
parent: Parent body index. For root bodies in the MJCF, this will be the parent_body
parameter from parse_mjcf (-1 for world, or a body index for hierarchical composition).
For nested bodies within the MJCF tree, this is the parent body index in the tree.
incoming_defaults: Default attributes dictionary.
childclass: Child class name for inheritance.
incoming_xform: Accumulated transform from parent (may include frame offsets).
parent_label_path: The hierarchical label path of the parent body (XPath-style).
Note:
Root bodies (direct children of ) are automatically detected by checking if
parent matches the parent_body parameter from parse_mjcf. Only root bodies respect the
floating/base_joint parameters; nested bodies use their defined joints from the MJCF.
"""
# Infer if this is a root body by checking if parent matches the outer parent_body parameter
# Root bodies are direct children of , where parent == parent_body (closure variable)
is_mjcf_root = parent == parent_body
body_class = body.get("class") or body.get("childclass")
if body_class is None:
body_class = childclass
defaults = incoming_defaults
else:
for pattern in ignore_classes:
if re.match(pattern, body_class):
return
defaults = merge_attrib(incoming_defaults, class_defaults[body_class])
if "body" in defaults:
body_attrib = merge_attrib(defaults["body"], body.attrib)
else:
body_attrib = body.attrib
body_name = body_attrib.get("name", f"body_{builder.body_count}")
body_name = sanitize_name(body_name)
# Build XPath-style hierarchical label path for this body
body_label_path = f"{parent_label_path}/{body_name}" if parent_label_path else body_name
body_pos = parse_vec(body_attrib, "pos", (0.0, 0.0, 0.0))
body_ori = parse_orientation(body_attrib)
# Create local transform from parsed position and orientation
local_xform = wp.transform(body_pos * scale, body_ori)
parent_xform = incoming_xform if incoming_xform is not None else xform
if override_root_xform and is_mjcf_root:
world_xform = parent_xform
else:
world_xform = parent_xform * local_xform
# For joint positioning, compute body position relative to the actual parent body
if parent >= 0:
# Look up parent body's world transform and compute relative position
parent_body_xform = builder.body_q[parent]
relative_xform = wp.transform_inverse(parent_body_xform) * world_xform
body_pos_for_joints = relative_xform.p
body_ori_for_joints = relative_xform.q
else:
# World parent: use the composed world_xform (includes frame/import root transforms)
body_pos_for_joints = world_xform.p
body_ori_for_joints = world_xform.q
joint_armature = []
joint_name = []
joint_pos = []
joint_custom_attributes: dict[str, Any] = {}
dof_custom_attributes: dict[str, dict[int, Any]] = {}
linear_axes = []
angular_axes = []
joint_type = None
freejoint_tags = body.findall("freejoint")
if len(freejoint_tags) > 0:
joint_type = JointType.FREE
freejoint_name = sanitize_name(freejoint_tags[0].attrib.get("name", f"{body_name}_freejoint"))
joint_name.append(freejoint_name)
joint_armature.append(0.0)
joint_custom_attributes = parse_custom_attributes(
freejoint_tags[0].attrib, builder_custom_attr_joint, parsing_mode="mjcf"
)
else:
# DOF index relative to the joint being created (multiple MJCF joints in a body are combined into one Newton joint)
current_dof_index = 0
# Track MJCF joint names and their DOF offsets within the combined Newton joint
mjcf_joint_dof_offsets: list[tuple[str, int]] = []
joints = body.findall("joint")
for i, joint in enumerate(joints):
joint_defaults = defaults
if "class" in joint.attrib:
joint_class = joint.attrib["class"]
if joint_class in class_defaults:
joint_defaults = merge_attrib(joint_defaults, class_defaults[joint_class])
if "joint" in joint_defaults:
joint_attrib = merge_attrib(joint_defaults["joint"], joint.attrib)
else:
joint_attrib = joint.attrib
# default to hinge if not specified
joint_type_str = joint_attrib.get("type", "hinge")
joint_name.append(sanitize_name(joint_attrib.get("name") or f"{body_name}_joint_{i}"))
joint_pos.append(parse_vec(joint_attrib, "pos", (0.0, 0.0, 0.0)) * scale)
joint_range = parse_vec(joint_attrib, "range", (default_joint_limit_lower, default_joint_limit_upper))
joint_armature.append(parse_float(joint_attrib, "armature", default_joint_armature) * armature_scale)
if joint_type_str == "free":
joint_type = JointType.FREE
break
if joint_type_str == "fixed":
joint_type = JointType.FIXED
break
is_angular = joint_type_str == "hinge"
axis_vec = parse_vec(joint_attrib, "axis", (0.0, 0.0, 1.0))
limit_lower = np.deg2rad(joint_range[0]) if is_angular and use_degrees else joint_range[0]
limit_upper = np.deg2rad(joint_range[1]) if is_angular and use_degrees else joint_range[1]
# Parse solreflimit for joint limit stiffness and damping
solreflimit = parse_vec(joint_attrib, "solreflimit", (0.02, 1.0))
limit_ke, limit_kd = solref_to_stiffness_damping(solreflimit)
# Handle None return values (invalid solref)
if limit_ke is None:
limit_ke = 2500.0 # From MuJoCo's default solref (0.02, 1.0)
if limit_kd is None:
limit_kd = 100.0 # From MuJoCo's default solref (0.02, 1.0)
effort_limit = default_joint_effort_limit
if "actuatorfrcrange" in joint_attrib:
actuatorfrcrange = parse_vec(joint_attrib, "actuatorfrcrange", None)
if actuatorfrcrange is not None and len(actuatorfrcrange) == 2:
actuatorfrclimited = joint_attrib.get("actuatorfrclimited", "auto").lower()
autolimits_attr = builder.custom_attributes.get("mujoco:autolimits")
autolimits_val = True
if autolimits_attr is not None:
autolimits_values = autolimits_attr.values
autolimits_raw = (
autolimits_values.get(0, autolimits_attr.default)
if isinstance(autolimits_values, dict)
else autolimits_attr.default
)
autolimits_val = bool(autolimits_raw)
if actuatorfrclimited == "true" or (actuatorfrclimited == "auto" and autolimits_val):
effort_limit = max(abs(actuatorfrcrange[0]), abs(actuatorfrcrange[1]))
elif verbose:
print(
f"Warning: Joint '{joint_attrib.get('name', 'unnamed')}' has actuatorfrcrange "
f"but actuatorfrclimited='{actuatorfrclimited}'. Force clamping will be disabled."
)
ax = ModelBuilder.JointDofConfig(
axis=axis_vec,
limit_lower=limit_lower,
limit_upper=limit_upper,
limit_ke=limit_ke,
limit_kd=limit_kd,
target_ke=default_joint_target_ke,
target_kd=default_joint_target_kd,
armature=joint_armature[-1],
friction=parse_float(joint_attrib, "frictionloss", 0.0),
effort_limit=effort_limit,
actuator_mode=JointTargetMode.NONE, # Will be set by parse_actuators
)
if is_angular:
angular_axes.append(ax)
else:
linear_axes.append(ax)
dof_attr = parse_custom_attributes(
joint_attrib,
builder_custom_attr_dof,
parsing_mode="mjcf",
context={"use_degrees": use_degrees, "joint_type": joint_type_str},
)
# assemble custom attributes for each DOF (dict mapping DOF index to value)
# Only store values that were explicitly specified in the source
for key, value in dof_attr.items():
if key not in dof_custom_attributes:
dof_custom_attributes[key] = {}
dof_custom_attributes[key][current_dof_index] = value
# Track this MJCF joint's name and DOF offset within the combined Newton joint
mjcf_joint_dof_offsets.append((joint_name[-1], current_dof_index))
current_dof_index += 1
body_custom_attributes = parse_custom_attributes(body_attrib, builder_custom_attr_body, parsing_mode="mjcf")
link = builder.add_link(
xform=world_xform, # Use the composed world transform
label=body_label_path,
custom_attributes=body_custom_attributes,
)
body_name_to_idx[body_name] = link
if joint_type is None:
joint_type = JointType.D6
if len(linear_axes) == 0:
if len(angular_axes) == 0:
joint_type = JointType.FIXED
elif len(angular_axes) == 1:
joint_type = JointType.REVOLUTE
elif convert_3d_hinge_to_ball_joints and len(angular_axes) == 3:
joint_type = JointType.BALL
elif len(linear_axes) == 1 and len(angular_axes) == 0:
joint_type = JointType.PRISMATIC
# Handle base joint overrides for root bodies or FREE joints with explicit parameters
# Only apply base_joint logic when:
# (1) This is an MJCF root body AND (base_joint or floating are explicitly set OR parent_body is set)
# (2) This is a FREE joint AND it's an MJCF root being attached with parent_body
#
# NOTE: For root bodies in the MJCF, parent will equal the parent_body parameter from parse_mjcf.
# For nested bodies in the MJCF tree, parent will be a different body index within the tree.
# We check is_mjcf_root (parent == parent_body) to distinguish these cases.
# has_override_params: True if user explicitly provided base_joint or floating parameters
has_override_params = base_joint is not None or floating is not None
# has_hierarchical_composition: True if we're doing hierarchical composition (parent_body != -1)
has_hierarchical_composition = parent_body != -1
# is_free_joint_with_override: True if this is a FREE joint at MJCF root with hierarchical composition
# This handles the case where a MJCF with a is being attached to an existing body
is_free_joint_with_override = joint_type == JointType.FREE and is_mjcf_root and has_hierarchical_composition
if (is_mjcf_root and (has_override_params or has_hierarchical_composition)) or is_free_joint_with_override:
# Extract joint position (used for transform calculation)
joint_pos = joint_pos[0] if len(joint_pos) > 0 else wp.vec3(0.0, 0.0, 0.0)
# Rotate joint_pos by body orientation before adding to body position
rotated_joint_pos = wp.quat_rotate(body_ori_for_joints, joint_pos)
_xform = wp.transform(body_pos_for_joints + rotated_joint_pos, body_ori_for_joints)
# Add base joint based on parameters
if base_joint is not None:
if override_root_xform:
base_parent_xform = _xform
base_child_xform = wp.transform_identity()
else:
# Split xform: position goes to parent, rotation to child (inverted)
# so the custom base joint's axis isn't rotated by xform.
base_parent_xform = wp.transform(_xform.p, wp.quat_identity())
base_child_xform = wp.transform((0.0, 0.0, 0.0), wp.quat_inverse(_xform.q))
joint_indices.append(
builder._add_base_joint(
child=link,
base_joint=base_joint,
label=f"{body_label_path}/base_joint",
parent_xform=base_parent_xform,
child_xform=base_child_xform,
parent=parent,
)
)
elif floating is not None and floating and parent == -1:
# floating=True only makes sense when connecting to world
joint_indices.append(
builder._add_base_joint(
child=link, floating=True, label=f"{body_label_path}/floating_base", parent=parent
)
)
else:
# Fixed joint to world or to parent_body
# When parent_body is set, _xform is already relative to parent body (computed via effective_xform)
joint_indices.append(
builder._add_base_joint(
child=link,
floating=False,
label=f"{body_label_path}/fixed_base",
parent_xform=_xform,
parent=parent,
)
)
else:
# Extract joint position for non-root bodies
joint_pos = joint_pos[0] if len(joint_pos) > 0 else wp.vec3(0.0, 0.0, 0.0)
if len(joint_name) == 0:
joint_name = [f"{body_name}_joint"]
joint_label_name = "_".join(joint_name)
joint_label = f"{body_label_path}/{joint_label_name}"
if joint_type == JointType.FREE:
assert parent == -1, "Free joints must have the world body as parent"
joint_idx = builder.add_joint_free(
link,
label=joint_label,
custom_attributes=joint_custom_attributes,
)
joint_indices.append(joint_idx)
# Map free joint names so actuators can target them
for jn in joint_name:
joint_name_to_idx[jn] = joint_idx
else:
# When parent is world (-1), use world_xform to respect the xform argument
if parent == -1:
parent_xform_for_joint = world_xform * wp.transform(joint_pos, wp.quat_identity())
else:
# Rotate joint_pos by body orientation before adding to body position
rotated_joint_pos = wp.quat_rotate(body_ori_for_joints, joint_pos)
parent_xform_for_joint = wp.transform(body_pos_for_joints + rotated_joint_pos, body_ori_for_joints)
joint_idx = builder.add_joint(
joint_type,
parent=parent,
child=link,
linear_axes=linear_axes,
angular_axes=angular_axes,
label=joint_label,
parent_xform=parent_xform_for_joint,
child_xform=wp.transform(joint_pos, wp.quat_identity()),
custom_attributes=joint_custom_attributes | dof_custom_attributes,
)
joint_indices.append(joint_idx)
# Populate per-MJCF-joint DOF mapping for actuator resolution
# This allows actuators to target specific DOFs when multiple MJCF joints are combined
if mjcf_joint_dof_offsets:
qd_start = builder.joint_qd_start[joint_idx]
for mjcf_name, dof_offset in mjcf_joint_dof_offsets:
mjcf_joint_name_to_dof[mjcf_name] = qd_start + dof_offset
# Map raw MJCF joint names to Newton joint index for tendon/actuator resolution
for jn in joint_name:
joint_name_to_idx[jn] = joint_idx
# -----------------
# add shapes (using shared helper for visual/collider partitioning)
geoms = body.findall("geom")
body_visual_shapes = _process_body_geoms(geoms, defaults, body_name, link, label_prefix=body_label_path)
visual_shapes.extend(body_visual_shapes)
# Parse sites (non-colliding reference points)
if parse_sites:
sites = body.findall("site")
if sites:
_parse_sites_impl(
defaults,
body_name,
link,
sites=sites,
label_prefix=body_label_path,
)
m = builder.body_mass[link]
if not ignore_inertial_definitions and body.find("inertial") is not None:
inertial = body.find("inertial")
if "inertial" in defaults:
inertial_attrib = merge_attrib(defaults["inertial"], inertial.attrib)
else:
inertial_attrib = inertial.attrib
# overwrite inertial parameters if defined
inertial_pos = parse_vec(inertial_attrib, "pos", (0.0, 0.0, 0.0)) * scale
inertial_rot = parse_orientation(inertial_attrib)
inertial_frame = wp.transform(inertial_pos, inertial_rot)
com = inertial_frame.p
if inertial_attrib.get("diaginertia") is not None:
diaginertia = parse_vec(inertial_attrib, "diaginertia", (0.0, 0.0, 0.0))
I_m = np.zeros((3, 3))
I_m[0, 0] = diaginertia[0] * scale**2
I_m[1, 1] = diaginertia[1] * scale**2
I_m[2, 2] = diaginertia[2] * scale**2
else:
fullinertia = inertial_attrib.get("fullinertia")
assert fullinertia is not None
fullinertia = np.fromstring(fullinertia, sep=" ", dtype=np.float32)
I_m = np.zeros((3, 3))
I_m[0, 0] = fullinertia[0] * scale**2
I_m[1, 1] = fullinertia[1] * scale**2
I_m[2, 2] = fullinertia[2] * scale**2
I_m[0, 1] = fullinertia[3] * scale**2
I_m[0, 2] = fullinertia[4] * scale**2
I_m[1, 2] = fullinertia[5] * scale**2
I_m[1, 0] = I_m[0, 1]
I_m[2, 0] = I_m[0, 2]
I_m[2, 1] = I_m[1, 2]
rot = wp.quat_to_matrix(inertial_frame.q)
rot_np = np.array(rot).reshape(3, 3)
I_m = rot_np @ I_m @ rot_np.T
I_m = wp.mat33(I_m)
m = float(inertial_attrib.get("mass", "0"))
builder.body_mass[link] = m
builder.body_inv_mass[link] = 1.0 / m if m > 0.0 else 0.0
builder.body_com[link] = com
builder.body_inertia[link] = I_m
if any(x for x in I_m):
builder.body_inv_inertia[link] = wp.inverse(I_m)
else:
builder.body_inv_inertia[link] = I_m
# Lock inertia so subsequent shapes (e.g. from child elements)
# don't modify the explicitly specified mass/com/inertia. This matches
# MuJoCo's behavior where completely overrides geom contributions.
builder.body_lock_inertia[link] = True
# -----------------
# recurse
for child in body.findall("body"):
_childclass = body.get("childclass")
if _childclass is None:
_childclass = childclass
_incoming_defaults = defaults
else:
_incoming_defaults = merge_attrib(defaults, class_defaults[_childclass])
parse_body(
child,
link,
_incoming_defaults,
childclass=_childclass,
incoming_xform=world_xform,
parent_label_path=body_label_path,
)
# Process frame elements within this body
# Use body's childclass if declared, otherwise inherit from parent
frame_childclass = body.get("childclass") or childclass
frame_defaults = (
merge_attrib(defaults, class_defaults.get(frame_childclass, {})) if frame_childclass else defaults
)
process_frames(
body.findall("frame"),
parent_body=link,
defaults=frame_defaults,
childclass=frame_childclass,
world_xform=world_xform,
body_relative_xform=wp.transform_identity(), # Geoms/sites need body-relative coords
label_prefix=body_label_path,
)
def parse_equality_constraints(equality):
def parse_common_attributes(element):
return {
"name": element.attrib.get("name"),
"active": element.attrib.get("active", "true").lower() == "true",
}
def get_site_body_and_anchor(site_name: str) -> tuple[int, wp.vec3] | None:
"""Look up a site by name and return its body index and position (anchor).
Returns:
Tuple of (body_idx, anchor_position) or None if site not found or not a site.
"""
if site_name not in site_name_to_idx:
if verbose:
print(f"Warning: Site '{site_name}' not found")
return None
site_idx = site_name_to_idx[site_name]
if not (builder.shape_flags[site_idx] & ShapeFlags.SITE):
if verbose:
print(f"Warning: Shape '{site_name}' is not a site")
return None
body_idx = builder.shape_body[site_idx]
site_xform = builder.shape_transform[site_idx]
anchor = wp.vec3(site_xform[0], site_xform[1], site_xform[2])
return (body_idx, anchor)
for connect in equality.findall("connect"):
common = parse_common_attributes(connect)
custom_attrs = parse_custom_attributes(connect.attrib, builder_custom_attr_eq, parsing_mode="mjcf")
body1_name = sanitize_name(connect.attrib.get("body1", "")) if connect.attrib.get("body1") else None
body2_name = (
sanitize_name(connect.attrib.get("body2", "worldbody")) if connect.attrib.get("body2") else None
)
anchor = connect.attrib.get("anchor")
site1 = connect.attrib.get("site1")
site2 = connect.attrib.get("site2")
if body1_name and anchor:
if verbose:
print(f"Connect constraint: {body1_name} to {body2_name} at anchor {anchor}")
anchor_vec = wp.vec3(*[float(x) * scale for x in anchor.split()]) if anchor else None
body1_idx = body_name_to_idx.get(body1_name, -1) if body1_name else -1
body2_idx = body_name_to_idx.get(body2_name, -1) if body2_name else -1
builder.add_equality_constraint_connect(
body1=body1_idx,
body2=body2_idx,
anchor=anchor_vec,
label=f"{articulation_label}/{common['name']}"
if articulation_label and common["name"]
else common["name"],
enabled=common["active"],
custom_attributes=custom_attrs,
)
elif site1:
if site2:
# Site-based connect: both site1 and site2 must be specified
site1_info = get_site_body_and_anchor(site1)
site2_info = get_site_body_and_anchor(site2)
if site1_info is None or site2_info is None:
if verbose:
print(f"Warning: Connect constraint '{common['name']}' failed.")
continue
body1_idx, anchor_vec = site1_info
body2_idx, _ = site2_info
if verbose:
print(
f"Connect constraint (site-based): site '{site1}' on body {body1_idx} to body {body2_idx}"
)
builder.add_equality_constraint_connect(
body1=body1_idx,
body2=body2_idx,
anchor=anchor_vec,
label=f"{articulation_label}/{common['name']}"
if articulation_label and common["name"]
else common["name"],
enabled=common["active"],
custom_attributes=custom_attrs,
)
else:
if verbose:
print(
f"Warning: Connect constraint '{common['name']}' has site1 but no site2. "
"When using sites, both site1 and site2 must be specified. Skipping."
)
for weld in equality.findall("weld"):
common = parse_common_attributes(weld)
custom_attrs = parse_custom_attributes(weld.attrib, builder_custom_attr_eq, parsing_mode="mjcf")
body1_name = sanitize_name(weld.attrib.get("body1", "")) if weld.attrib.get("body1") else None
body2_name = sanitize_name(weld.attrib.get("body2", "worldbody")) if weld.attrib.get("body2") else None
anchor = weld.attrib.get("anchor", "0 0 0")
relpose = weld.attrib.get("relpose", "0 1 0 0 0 0 0")
torquescale = parse_float(weld.attrib, "torquescale", 1.0)
site1 = weld.attrib.get("site1")
site2 = weld.attrib.get("site2")
if body1_name:
if verbose:
print(f"Weld constraint: {body1_name} to {body2_name}")
anchor_vec = wp.vec3(*[float(x) * scale for x in anchor.split()])
body1_idx = body_name_to_idx.get(body1_name, -1) if body1_name else -1
body2_idx = body_name_to_idx.get(body2_name, -1) if body2_name else -1
relpose_list = [float(x) for x in relpose.split()]
relpose_transform = wp.transform(
wp.vec3(relpose_list[0], relpose_list[1], relpose_list[2]),
wp.quat(relpose_list[4], relpose_list[5], relpose_list[6], relpose_list[3]),
)
builder.add_equality_constraint_weld(
body1=body1_idx,
body2=body2_idx,
anchor=anchor_vec,
relpose=relpose_transform,
torquescale=torquescale,
label=f"{articulation_label}/{common['name']}"
if articulation_label and common["name"]
else common["name"],
enabled=common["active"],
custom_attributes=custom_attrs,
)
elif site1:
if site2:
# Site-based weld: both site1 and site2 must be specified
site1_info = get_site_body_and_anchor(site1)
site2_info = get_site_body_and_anchor(site2)
if site1_info is None or site2_info is None:
if verbose:
print(f"Warning: Weld constraint '{common['name']}' failed.")
continue
body1_idx, _ = site1_info
body2_idx, anchor_vec = site2_info
relpose_list = [float(x) for x in relpose.split()]
relpose_transform = wp.transform(
wp.vec3(relpose_list[0], relpose_list[1], relpose_list[2]),
wp.quat(relpose_list[4], relpose_list[5], relpose_list[6], relpose_list[3]),
)
if verbose:
print(f"Weld constraint (site-based): body {body1_idx} to body {body2_idx}")
builder.add_equality_constraint_weld(
body1=body1_idx,
body2=body2_idx,
anchor=anchor_vec,
relpose=relpose_transform,
torquescale=torquescale,
label=f"{articulation_label}/{common['name']}"
if articulation_label and common["name"]
else common["name"],
enabled=common["active"],
custom_attributes=custom_attrs,
)
else:
if verbose:
print(
f"Warning: Weld constraint '{common['name']}' has site1 but no site2. "
"When using sites, both site1 and site2 must be specified. Skipping."
)
for joint in equality.findall("joint"):
common = parse_common_attributes(joint)
custom_attrs = parse_custom_attributes(joint.attrib, builder_custom_attr_eq, parsing_mode="mjcf")
joint1_name = joint.attrib.get("joint1")
joint2_name = joint.attrib.get("joint2")
polycoef = joint.attrib.get("polycoef", "0 1 0 0 0")
if joint1_name:
if verbose:
print(f"Joint constraint: {joint1_name} coupled to {joint2_name} with polycoef {polycoef}")
joint1_idx = joint_name_to_idx.get(joint1_name, -1) if joint1_name else -1
joint2_idx = joint_name_to_idx.get(joint2_name, -1) if joint2_name else -1
builder.add_equality_constraint_joint(
joint1=joint1_idx,
joint2=joint2_idx,
polycoef=[float(x) for x in polycoef.split()],
label=f"{articulation_label}/{common['name']}"
if articulation_label and common["name"]
else common["name"],
enabled=common["active"],
custom_attributes=custom_attrs,
)
# TODO: add support for equality constraint type "flex" once Newton supports it
# -----------------
# start articulation
visual_shapes = []
start_shape_count = len(builder.shape_type)
joint_indices = [] # Collect joint indices as we create them
root_body_boundaries = [] # (start_idx, body_name) for each root body under
# Mapping from individual MJCF joint name to (qd_start, dof_count) for actuator resolution
# This allows actuators to target specific DOFs when multiple MJCF joints are combined into one Newton joint
# Maps individual MJCF joint names to their specific DOF index.
# Used to resolve actuators targeting specific joints within combined Newton joints.
mjcf_joint_name_to_dof: dict[str, int] = {}
# Maps tendon names to their index in the tendon custom attributes.
# Used to resolve actuators targeting tendons.
tendon_name_to_idx: dict[str, int] = {}
# Maps raw MJCF body/site names to their builder indices.
# Used to resolve equality constraints and actuators that reference entities by their short name.
body_name_to_idx: dict[str, int] = {}
site_name_to_idx: dict[str, int] = {}
joint_name_to_idx: dict[str, int] = {}
# Extract articulation label early for hierarchical label construction
articulation_label = root.attrib.get("model")
# Build the root label path: "{model_name}/worldbody" or just "worldbody"
root_label_path = f"{articulation_label}/worldbody" if articulation_label else "worldbody"
# Process all worldbody elements (MuJoCo allows multiple, e.g. from includes)
for world in root.findall("worldbody"):
world_class = get_class(world)
world_defaults = merge_attrib(class_defaults["__all__"], class_defaults.get(world_class, {}))
# -----------------
# add bodies
# Use parent_body if specified for hierarchical composition, otherwise connect to world (-1)
root_parent = parent_body
# When parent_body is specified, xform is interpreted as relative to the parent body.
# Compose it with the parent body's world transform to get the actual world transform.
if parent_body != -1:
effective_xform = builder.body_q[parent_body] * xform
else:
effective_xform = xform
for body in world.findall("body"):
body_name = sanitize_name(body.attrib.get("name", f"body_{builder.body_count}"))
root_body_boundaries.append((len(joint_indices), body_name))
parse_body(
body,
root_parent,
world_defaults,
incoming_xform=effective_xform,
parent_label_path=root_label_path,
)
# -----------------
# add static geoms
parse_shapes(
defaults=world_defaults,
body_name="world",
link=-1,
geoms=world.findall("geom"),
density=default_shape_density,
incoming_xform=xform,
label_prefix=root_label_path,
)
if parse_sites:
_parse_sites_impl(
defaults=world_defaults,
body_name="world",
link=-1,
sites=world.findall("site"),
incoming_xform=xform,
label_prefix=root_label_path,
)
# -----------------
# process frame elements at worldbody level
process_frames(
world.findall("frame"),
parent_body=root_parent,
defaults=world_defaults,
childclass=None,
world_xform=effective_xform,
body_relative_xform=None, # Static geoms use world coords
label_prefix=root_label_path,
track_root_boundaries=True,
)
# -----------------
# add equality constraints
equality = root.find("equality")
if equality is not None and not skip_equality_constraints:
parse_equality_constraints(equality)
# -----------------
# parse contact pairs
# Get custom attributes with custom frequency for pair parsing
# Exclude pair_geom1/pair_geom2/pair_world as they're handled specially (geom name lookup, world assignment)
builder_custom_attr_pair: list[ModelBuilder.CustomAttribute] = [
attr
for attr in builder.custom_attributes.values()
if isinstance(attr.frequency, str)
and attr.name.startswith("pair_")
and attr.name not in ("pair_geom1", "pair_geom2", "pair_world")
]
# Only parse contact pairs if custom attributes are registered
has_pair_attrs = "mujoco:pair_geom1" in builder.custom_attributes
contact = root.find("contact")
def _find_shape_idx(name: str) -> int | None:
"""Look up shape index by name, supporting hierarchical labels (e.g. "prefix/geom_name")."""
for idx in range(start_shape_count, len(builder.shape_label)):
label = builder.shape_label[idx]
if label == name or label.endswith(f"/{name}"):
return idx
return None
if contact is not None and has_pair_attrs:
# Parse elements - explicit contact pairs with custom properties
for pair in contact.findall("pair"):
geom1_name = pair.attrib.get("geom1")
geom2_name = pair.attrib.get("geom2")
if not geom1_name or not geom2_name:
if verbose:
print("Warning: element missing geom1 or geom2 attribute, skipping")
continue
geom1_idx = _find_shape_idx(geom1_name)
if geom1_idx is None:
if verbose:
print(f"Warning: references unknown geom '{geom1_name}', skipping")
continue
geom2_idx = _find_shape_idx(geom2_name)
if geom2_idx is None:
if verbose:
print(f"Warning: references unknown geom '{geom2_name}', skipping")
continue
# Parse attributes using the standard custom attribute parsing
pair_attrs = parse_custom_attributes(pair.attrib, builder_custom_attr_pair, parsing_mode="mjcf")
# Build values dict for all pair attributes
pair_values: dict[str, Any] = {
"mujoco:pair_world": builder.current_world,
"mujoco:pair_geom1": geom1_idx,
"mujoco:pair_geom2": geom2_idx,
}
# Add remaining attributes with parsed values or defaults
for attr in builder_custom_attr_pair:
pair_values[attr.key] = pair_attrs.get(attr.key, attr.default)
builder.add_custom_values(**pair_values)
if verbose:
print(f"Parsed contact pair: {geom1_name} ({geom1_idx}) <-> {geom2_name} ({geom2_idx})")
# Parse elements - body pairs to exclude from collision detection
if contact is not None:
for exclude in contact.findall("exclude"):
body1_name = exclude.attrib.get("body1")
body2_name = exclude.attrib.get("body2")
if not body1_name or not body2_name:
if verbose:
print("Warning: element missing body1 or body2 attribute, skipping")
continue
# Normalize body names the same way parse_body() does (replace '-' with '_')
body1_name = body1_name.replace("-", "_")
body2_name = body2_name.replace("-", "_")
# Look up body indices by raw MJCF name
body1_idx = body_name_to_idx.get(body1_name)
if body1_idx is None:
if verbose:
print(f"Warning: references unknown body '{body1_name}', skipping")
continue
body2_idx = body_name_to_idx.get(body2_name)
if body2_idx is None:
if verbose:
print(f"Warning: references unknown body '{body2_name}', skipping")
continue
# Find all shapes belonging to body1 and body2
body1_shapes = [i for i, body in enumerate(builder.shape_body) if body == body1_idx]
body2_shapes = [i for i, body in enumerate(builder.shape_body) if body == body2_idx]
# Add all shape pairs from these bodies to collision filter
for shape1_idx in body1_shapes:
for shape2_idx in body2_shapes:
builder.add_shape_collision_filter_pair(shape1_idx, shape2_idx)
if verbose:
print(
f"Parsed collision exclude: {body1_name} ({len(body1_shapes)} shapes) <-> "
f"{body2_name} ({len(body2_shapes)} shapes), added {len(body1_shapes) * len(body2_shapes)} filter pairs"
)
# -----------------
# Parse fixed and spatial tendons.
# Get variable-length custom attributes for tendon parsing (frequency="mujoco:tendon")
# Exclude attributes that are handled specially during parsing
_tendon_special_attrs = {
"tendon_world",
"tendon_type",
"tendon_joint_adr",
"tendon_joint_num",
"tendon_joint",
"tendon_coef",
"tendon_wrap_adr",
"tendon_wrap_num",
"tendon_wrap_type",
"tendon_wrap_shape",
"tendon_wrap_sidesite",
"tendon_wrap_prm",
}
builder_custom_attr_tendon: list[ModelBuilder.CustomAttribute] = [
attr
for attr in builder.custom_attributes.values()
if isinstance(attr.frequency, str)
and attr.name.startswith("tendon_")
and attr.name not in _tendon_special_attrs
]
def parse_tendons(tendon_section):
"""Parse tendons from a tendon section.
Args:
tendon_section: XML element containing tendon definitions.
"""
for fixed in tendon_section.findall("fixed"):
tendon_name = fixed.attrib.get("name", "")
# Parse joint elements within this fixed tendon
joint_entries = []
for joint_elem in fixed.findall("joint"):
joint_name = joint_elem.attrib.get("joint")
coef_str = joint_elem.attrib.get("coef", "1.0")
if not joint_name:
if verbose:
print(f"Warning: in tendon '{tendon_name}' missing joint attribute, skipping")
continue
# Look up joint index by name
joint_idx = joint_name_to_idx.get(joint_name)
if joint_idx is None:
if verbose:
print(
f"Warning: Tendon '{tendon_name}' references unknown joint '{joint_name}', skipping joint"
)
continue
coef = float(coef_str)
joint_entries.append((joint_idx, coef))
if not joint_entries:
if verbose:
print(f"Warning: Fixed tendon '{tendon_name}' has no valid joint elements, skipping")
continue
# Parse tendon-level attributes using the standard custom attribute parsing
tendon_attrs = parse_custom_attributes(fixed.attrib, builder_custom_attr_tendon, parsing_mode="mjcf")
# Determine wrap array start index
tendon_joint_attr = builder.custom_attributes.get("mujoco:tendon_joint")
joint_start = len(tendon_joint_attr.values) if tendon_joint_attr and tendon_joint_attr.values else 0
# Add joints to the joint arrays
for joint_idx, coef in joint_entries:
builder.add_custom_values(
**{
"mujoco:tendon_joint": joint_idx,
"mujoco:tendon_coef": coef,
}
)
# Build values dict for tendon-level attributes
tendon_values: dict[str, Any] = {
"mujoco:tendon_world": builder.current_world,
"mujoco:tendon_type": 0, # fixed tendon
"mujoco:tendon_joint_adr": joint_start,
"mujoco:tendon_joint_num": len(joint_entries),
"mujoco:tendon_wrap_adr": 0,
"mujoco:tendon_wrap_num": 0,
}
# Add remaining attributes with parsed values or defaults
for attr in builder_custom_attr_tendon:
tendon_values[attr.key] = tendon_attrs.get(attr.key, attr.default)
indices = builder.add_custom_values(**tendon_values)
# Track tendon name for actuator resolution (get index from add_custom_values return)
if tendon_name:
tendon_idx = indices.get("mujoco:tendon_world", 0)
tendon_name_to_idx[sanitize_name(tendon_name)] = tendon_idx
if verbose:
joint_names_str = ", ".join(f"{builder.joint_label[j]}*{c}" for j, c in joint_entries)
print(f"Parsed fixed tendon: {tendon_name} ({joint_names_str})")
def find_shape_by_name(name: str, want_site: bool) -> int:
"""Find a shape index by name, disambiguating sites from geoms.
MuJoCo allows sites and geoms to share the same name (different namespaces).
Newton stores both as shapes in shape_label, so we need to pick the right one
based on whether we're looking for a site or a geom.
Returns -1 if no shape with the matching name and type is found.
"""
for i, label in enumerate(builder.shape_label):
if label == name or label.endswith(f"/{name}"):
is_site = bool(builder.shape_flags[i] & ShapeFlags.SITE)
if is_site == want_site:
return i
return -1
for spatial in tendon_section.findall("spatial"):
# Apply default class inheritance for spatial tendon attributes.
# MuJoCo defaults use tag for both and tendons.
elem_class = get_class(spatial)
elem_defaults = class_defaults.get(elem_class, {}).get("tendon", {})
all_defaults = class_defaults.get("__all__", {}).get("tendon", {})
merged_attrib = merge_attrib(merge_attrib(all_defaults, elem_defaults), dict(spatial.attrib))
tendon_name = merged_attrib.get("name", "")
# Parse wrap path elements in order
wrap_entries: list[tuple[int, int, int, float]] = [] # (type, shape_idx, sidesite_idx, prm)
for child in spatial:
if child.tag == "site":
site_name = child.attrib.get("site", "")
if site_name:
site_name = sanitize_name(site_name)
site_idx = find_shape_by_name(site_name, want_site=True) if site_name else -1
if site_idx < 0:
warnings.warn(
f"Spatial tendon '{tendon_name}' references unknown site '{site_name}', skipping element.",
stacklevel=2,
)
continue
wrap_entries.append((0, site_idx, -1, 0.0))
elif child.tag == "geom":
geom_name = child.attrib.get("geom", "")
if geom_name:
geom_name = sanitize_name(geom_name)
geom_idx = find_shape_by_name(geom_name, want_site=False) if geom_name else -1
if geom_idx < 0:
warnings.warn(
f"Spatial tendon '{tendon_name}' references unknown geom '{geom_name}', skipping element.",
stacklevel=2,
)
continue
sidesite_name = child.attrib.get("sidesite", "")
sidesite_idx = -1
if sidesite_name:
sidesite_name = sanitize_name(sidesite_name)
sidesite_idx = find_shape_by_name(sidesite_name, want_site=True)
if sidesite_idx < 0:
warnings.warn(
f"Spatial tendon '{tendon_name}' sidesite '{sidesite_name}' not found.",
stacklevel=2,
)
wrap_entries.append((1, geom_idx, sidesite_idx, 0.0))
elif child.tag == "pulley":
divisor = float(child.attrib.get("divisor", "0.0"))
wrap_entries.append((2, -1, -1, divisor))
if not wrap_entries:
warnings.warn(
f"Spatial tendon '{tendon_name}' has no valid wrap elements, skipping.",
stacklevel=2,
)
continue
# Parse tendon-level attributes using the standard custom attribute parsing
tendon_attrs = parse_custom_attributes(merged_attrib, builder_custom_attr_tendon, parsing_mode="mjcf")
# Determine wrap array start index
tendon_wrap_type_attr = builder.custom_attributes.get("mujoco:tendon_wrap_type")
wrap_start = (
len(tendon_wrap_type_attr.values) if tendon_wrap_type_attr and tendon_wrap_type_attr.values else 0
)
# Add wrap entries to the wrap path arrays
for wrap_type, shape_idx, sidesite_idx, prm in wrap_entries:
builder.add_custom_values(
**{
"mujoco:tendon_wrap_type": wrap_type,
"mujoco:tendon_wrap_shape": shape_idx,
"mujoco:tendon_wrap_sidesite": sidesite_idx,
"mujoco:tendon_wrap_prm": prm,
}
)
# Build values dict for tendon-level attributes
tendon_values: dict[str, Any] = {
"mujoco:tendon_world": builder.current_world,
"mujoco:tendon_type": 1, # spatial tendon
"mujoco:tendon_joint_adr": 0,
"mujoco:tendon_joint_num": 0,
"mujoco:tendon_wrap_adr": wrap_start,
"mujoco:tendon_wrap_num": len(wrap_entries),
}
# Add remaining attributes with parsed values or defaults
for attr in builder_custom_attr_tendon:
tendon_values[attr.key] = tendon_attrs.get(attr.key, attr.default)
indices = builder.add_custom_values(**tendon_values)
# Track tendon name for actuator resolution
if tendon_name:
tendon_idx = indices.get("mujoco:tendon_world", 0)
tendon_name_to_idx[sanitize_name(tendon_name)] = tendon_idx
if verbose:
print(f"Parsed spatial tendon: {tendon_name} ({len(wrap_entries)} wrap elements)")
# -----------------
# parse actuators
def parse_actuators(actuator_section):
"""Parse actuators from MJCF preserving order.
All actuators are added as custom attributes with mujoco:actuator frequency,
preserving their order from the MJCF file. This ensures control.mujoco.ctrl
has the same ordering as native MuJoCo.
For position/velocity actuators: also set mode/target_ke/target_kd on per-DOF arrays
for compatibility with Newton's joint target interface.
Args:
actuator_section: The XML element
"""
# Process ALL actuators in MJCF order
for actuator_elem in actuator_section:
actuator_type = actuator_elem.tag # position, velocity, motor, general
# Merge class defaults for this actuator element
# This handles MJCF class inheritance (e.g., )
elem_class = get_class(actuator_elem)
elem_defaults = class_defaults.get(elem_class, {}).get(actuator_type, {})
all_defaults = class_defaults.get("__all__", {}).get(actuator_type, {})
merged_attrib = merge_attrib(merge_attrib(all_defaults, elem_defaults), dict(actuator_elem.attrib))
joint_name = merged_attrib.get("joint")
body_name = merged_attrib.get("body")
tendon_name = merged_attrib.get("tendon")
# Sanitize names to match how they were stored in the builder
if joint_name:
joint_name = sanitize_name(joint_name)
if body_name:
body_name = sanitize_name(body_name)
if tendon_name:
tendon_name = sanitize_name(tendon_name)
# Determine transmission type and target
trntype = 0 # Default: joint
target_name_for_log = ""
qd_start = -1
total_dofs = 0
if joint_name:
# Joint transmission (trntype=0)
# First check per-MJCF-joint mapping (for targeting specific DOFs in combined joints)
if joint_name in mjcf_joint_name_to_dof:
qd_start = mjcf_joint_name_to_dof[joint_name]
total_dofs = 1 # Individual MJCF joints always map to exactly 1 DOF
target_idx = qd_start # DOF index for joint actuators
target_name_for_log = joint_name
trntype = 0 # TrnType.JOINT
elif joint_name in joint_name_to_idx:
# Fallback: combined Newton joint (applies to all DOFs)
joint_idx = joint_name_to_idx[joint_name]
qd_start = builder.joint_qd_start[joint_idx]
lin_dofs, ang_dofs = builder.joint_dof_dim[joint_idx]
total_dofs = lin_dofs + ang_dofs
target_idx = qd_start # DOF index for joint actuators
target_name_for_log = joint_name
trntype = 0 # TrnType.JOINT
else:
if verbose:
print(f"Warning: {actuator_type} actuator references unknown joint '{joint_name}'")
continue
elif body_name:
# Body transmission (trntype=4)
body_idx = body_name_to_idx.get(body_name)
if body_idx is None:
if verbose:
print(f"Warning: {actuator_type} actuator references unknown body '{body_name}'")
continue
target_idx = body_idx
target_name_for_log = body_name
trntype = 4 # TrnType.BODY
elif tendon_name:
# Tendon transmission (trntype=2 in MuJoCo)
if tendon_name not in tendon_name_to_idx:
if verbose:
print(f"Warning: {actuator_type} actuator references unknown tendon '{tendon_name}'")
continue
tendon_idx = tendon_name_to_idx[tendon_name]
target_idx = tendon_idx
target_name_for_log = tendon_name
trntype = 2 # TrnType.TENDON
else:
if verbose:
print(f"Warning: {actuator_type} actuator has no joint, body, or tendon target, skipping")
continue
act_name = merged_attrib.get("name", f"{actuator_type}_{target_name_for_log}")
# Extract gains based on actuator type
if actuator_type == "position":
kp = parse_float(merged_attrib, "kp", 1.0) # MuJoCo default kp=1
kv = parse_float(merged_attrib, "kv", 0.0) # Optional velocity damping
dampratio = parse_float(merged_attrib, "dampratio", 0.0)
gainprm = vec10(kp, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
biasprm = vec10(0.0, -kp, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
if kv > 0.0:
if dampratio > 0.0 and verbose:
print(
f"Warning: position actuator '{act_name}' sets both kv={kv} "
f"and dampratio={dampratio}; using kv and ignoring dampratio."
)
biasprm = vec10(0.0, -kp, -kv, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
elif dampratio > 0.0:
# Store unresolved dampratio in biasprm[2] (USD convention).
biasprm = vec10(0.0, -kp, dampratio, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
# Resolve inheritrange: copy target joint's range to ctrlrange.
# Uses only the first DOF (qd_start) since inheritrange is only
# meaningful for single-DOF joints (hinge, slide).
inheritrange = parse_float(merged_attrib, "inheritrange", 0.0)
if inheritrange > 0 and joint_name and qd_start >= 0:
lower = builder.joint_limit_lower[qd_start]
upper = builder.joint_limit_upper[qd_start]
if lower < upper:
mean = (upper + lower) / 2.0
radius = (upper - lower) / 2.0 * inheritrange
merged_attrib["ctrlrange"] = f"{mean - radius} {mean + radius}"
merged_attrib["ctrllimited"] = "true"
# Non-joint actuators (body, tendon, etc.) must use CTRL_DIRECT
if trntype != 0 or total_dofs == 0 or ctrl_direct:
ctrl_source_val = SolverMuJoCo.CtrlSource.CTRL_DIRECT
else:
ctrl_source_val = SolverMuJoCo.CtrlSource.JOINT_TARGET
if ctrl_source_val == SolverMuJoCo.CtrlSource.JOINT_TARGET:
for i in range(total_dofs):
dof_idx = qd_start + i
builder.joint_target_ke[dof_idx] = kp
current_mode = builder.joint_target_mode[dof_idx]
if current_mode == int(JointTargetMode.VELOCITY):
# A velocity actuator was already parsed for this DOF - upgrade to POSITION_VELOCITY.
# We intentionally preserve the existing kd from the velocity actuator rather than
# overwriting it with this position actuator's kv, since the velocity actuator's
# kv takes precedence for velocity control.
builder.joint_target_mode[dof_idx] = int(JointTargetMode.POSITION_VELOCITY)
elif current_mode == int(JointTargetMode.NONE):
builder.joint_target_mode[dof_idx] = int(JointTargetMode.POSITION)
builder.joint_target_kd[dof_idx] = kv
elif actuator_type == "velocity":
kv = parse_float(merged_attrib, "kv", 1.0) # MuJoCo default kv=1
gainprm = vec10(kv, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
biasprm = vec10(0.0, 0.0, -kv, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
# Non-joint actuators (body, tendon, etc.) must use CTRL_DIRECT
if trntype != 0 or total_dofs == 0 or ctrl_direct:
ctrl_source_val = SolverMuJoCo.CtrlSource.CTRL_DIRECT
else:
ctrl_source_val = SolverMuJoCo.CtrlSource.JOINT_TARGET
if ctrl_source_val == SolverMuJoCo.CtrlSource.JOINT_TARGET:
for i in range(total_dofs):
dof_idx = qd_start + i
current_mode = builder.joint_target_mode[dof_idx]
if current_mode == int(JointTargetMode.POSITION):
builder.joint_target_mode[dof_idx] = int(JointTargetMode.POSITION_VELOCITY)
elif current_mode == int(JointTargetMode.NONE):
builder.joint_target_mode[dof_idx] = int(JointTargetMode.VELOCITY)
builder.joint_target_kd[dof_idx] = kv
elif actuator_type == "motor":
gainprm = vec10(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
biasprm = vec10(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
ctrl_source_val = SolverMuJoCo.CtrlSource.CTRL_DIRECT
elif actuator_type == "general":
gainprm_str = merged_attrib.get("gainprm", "1 0 0 0 0 0 0 0 0 0")
biasprm_str = merged_attrib.get("biasprm", "0 0 0 0 0 0 0 0 0 0")
gainprm_vals = [float(x) for x in gainprm_str.split()[:10]]
biasprm_vals = [float(x) for x in biasprm_str.split()[:10]]
while len(gainprm_vals) < 10:
gainprm_vals.append(0.0)
while len(biasprm_vals) < 10:
biasprm_vals.append(0.0)
gainprm = vec10(*gainprm_vals)
biasprm = vec10(*biasprm_vals)
ctrl_source_val = SolverMuJoCo.CtrlSource.CTRL_DIRECT
else:
if verbose:
print(f"Warning: Unknown actuator type '{actuator_type}', skipping")
continue
# Add actuator via custom attributes
parsed_attrs = parse_custom_attributes(merged_attrib, builder_custom_attr_actuator, parsing_mode="mjcf")
# Set implicit type defaults per actuator shortcut type.
# MuJoCo shortcut elements (position, velocity, etc.) implicitly set
# biastype/gaintype/dyntype without writing them to the XML. We mirror
# these defaults here so the CTRL_DIRECT path recreates faithful actuators.
# Only override when the XML didn't explicitly specify the attribute.
shortcut_type_defaults = {
"position": {"mujoco:actuator_biastype": 1}, # affine
"velocity": {"mujoco:actuator_biastype": 1}, # affine
}
for key, value in shortcut_type_defaults.get(actuator_type, {}).items():
if key not in parsed_attrs:
parsed_attrs[key] = value
# Build full values dict
actuator_values: dict[str, Any] = {}
for attr in builder_custom_attr_actuator:
if attr.key in (
"mujoco:ctrl_source",
"mujoco:actuator_trntype",
"mujoco:actuator_gainprm",
"mujoco:actuator_biasprm",
"mujoco:ctrl",
):
continue
actuator_values[attr.key] = parsed_attrs.get(attr.key, attr.default)
actuator_values["mujoco:ctrl_source"] = ctrl_source_val
actuator_values["mujoco:actuator_gainprm"] = gainprm
actuator_values["mujoco:actuator_biasprm"] = biasprm
actuator_values["mujoco:actuator_trnid"] = wp.vec2i(target_idx, 0)
actuator_values["mujoco:actuator_trntype"] = trntype
actuator_values["mujoco:actuator_world"] = builder.current_world
builder.add_custom_values(**actuator_values)
if verbose:
source_name = (
"CTRL_DIRECT" if ctrl_source_val == SolverMuJoCo.CtrlSource.CTRL_DIRECT else "JOINT_TARGET"
)
trn_name = {0: "joint", 2: "tendon", 4: "body"}.get(trntype, "unknown")
print(
f"{actuator_type.capitalize()} actuator '{act_name}' on {trn_name} '{target_name_for_log}': "
f"trntype={trntype}, source={source_name}"
)
# Only parse tendons if custom tendon attributes are registered
has_tendon_attrs = "mujoco:tendon_world" in builder.custom_attributes
if has_tendon_attrs:
# Find all sections marked
tendon_sections = root.findall(".//tendon")
for tendon_section in tendon_sections:
parse_tendons(tendon_section)
actuator_section = root.find("actuator")
if actuator_section is not None:
parse_actuators(actuator_section)
# -----------------
end_shape_count = len(builder.shape_type)
for i in range(start_shape_count, end_shape_count):
for j in visual_shapes:
builder.add_shape_collision_filter_pair(i, j)
if not enable_self_collisions:
for i in range(start_shape_count, end_shape_count):
for j in range(i + 1, end_shape_count):
builder.add_shape_collision_filter_pair(i, j)
# Create articulations from collected joints
if parent_body != -1 or len(root_body_boundaries) <= 1:
# Hierarchical composition or single root body: one articulation
builder._finalize_imported_articulation(
joint_indices=joint_indices,
parent_body=parent_body,
articulation_label=articulation_label,
)
else:
# Multiple root bodies: create one articulation per root body
for i, (start_idx, body_name) in enumerate(root_body_boundaries):
end_idx = root_body_boundaries[i + 1][0] if i + 1 < len(root_body_boundaries) else len(joint_indices)
root_joints = joint_indices[start_idx:end_idx]
if not root_joints:
continue
label = f"{articulation_label}/{body_name}" if articulation_label else body_name
builder._finalize_imported_articulation(
joint_indices=root_joints,
parent_body=-1,
articulation_label=label,
)
if collapse_fixed_joints:
builder.collapse_fixed_joints()
================================================
FILE: newton/_src/utils/import_urdf.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import os
import tempfile
import warnings
import xml.etree.ElementTree as ET
from typing import Literal
from urllib.parse import unquote, urlsplit
import numpy as np
import warp as wp
from ..core import Axis, AxisType, quat_between_axes
from ..core.types import Transform
from ..geometry import Mesh
from ..sim import ModelBuilder
from ..sim.enums import JointTargetMode
from ..sim.model import Model
from .import_utils import parse_custom_attributes, sanitize_xml_content, should_show_collider
from .mesh import load_meshes_from_file
from .texture import load_texture
from .topology import topological_sort
AttributeFrequency = Model.AttributeFrequency
# Optional dependency for robust URI resolution
try:
from resolve_robotics_uri_py import resolve_robotics_uri
except ImportError:
resolve_robotics_uri = None
def _download_file(dst, url: str) -> None:
import requests
with requests.get(url, stream=True, timeout=10) as response:
response.raise_for_status()
for chunk in response.iter_content(chunk_size=8192):
dst.write(chunk)
def download_asset_tmpfile(url: str):
"""Download a file into a NamedTemporaryFile.
A closed NamedTemporaryFile is returned. It must be deleted by the caller."""
urlpath = unquote(urlsplit(url).path)
file_od = tempfile.NamedTemporaryFile("wb", suffix=os.path.splitext(urlpath)[1], delete=False)
_download_file(file_od, url)
file_od.close()
return file_od
def parse_urdf(
builder: ModelBuilder,
source: str,
*,
xform: Transform | None = None,
floating: bool | None = None,
base_joint: dict | None = None,
parent_body: int = -1,
scale: float = 1.0,
hide_visuals: bool = False,
parse_visuals_as_colliders: bool = False,
up_axis: AxisType = Axis.Z,
force_show_colliders: bool = False,
enable_self_collisions: bool = True,
ignore_inertial_definitions: bool = False,
joint_ordering: Literal["bfs", "dfs"] | None = "dfs",
bodies_follow_joint_ordering: bool = True,
collapse_fixed_joints: bool = False,
mesh_maxhullvert: int | None = None,
force_position_velocity_actuation: bool = False,
override_root_xform: bool = False,
):
"""
Parses a URDF file and adds the bodies and joints to the given ModelBuilder.
Args:
builder (ModelBuilder): The :class:`ModelBuilder` to add the bodies and joints to.
source: The filename of the URDF file to parse, or the URDF XML string content.
xform: The transform to apply to the root body. If None, the transform is set to identity.
override_root_xform: If ``True``, the articulation root's world-space
transform is replaced by ``xform`` instead of being composed with it,
preserving only the internal structure (relative body positions). Useful
for cloning articulations at explicit positions. When a ``base_joint`` is
specified, ``xform`` is applied as the full parent transform (including
rotation) rather than splitting position/rotation. Not intended for
sources containing multiple articulations, as all roots would be placed
at the same ``xform``. Defaults to ``False``.
floating: Controls the base joint type for the root body.
- ``None`` (default): Uses format-specific default (creates a FIXED joint for URDF).
- ``True``: Creates a FREE joint with 6 DOF (3 translation + 3 rotation). Only valid when
``parent_body == -1`` since FREE joints must connect to world frame.
- ``False``: Creates a FIXED joint (0 DOF).
Cannot be specified together with ``base_joint``.
base_joint: Custom joint specification for connecting the root body to the world
(or to ``parent_body`` if specified). This parameter enables hierarchical composition with
custom mobility. Dictionary with joint parameters as accepted by
:meth:`ModelBuilder.add_joint` (e.g., joint type, axes, limits, stiffness).
Cannot be specified together with ``floating``.
parent_body: Parent body index for hierarchical composition. If specified, attaches the
imported root body to this existing body, making them part of the same kinematic articulation.
The connection type is determined by ``floating`` or ``base_joint``. If ``-1`` (default),
the root connects to the world frame. **Restriction**: Only the most recently added
articulation can be used as parent; attempting to attach to an older articulation will raise
a ``ValueError``.
.. note::
Valid combinations of ``floating``, ``base_joint``, and ``parent_body``:
.. list-table::
:header-rows: 1
:widths: 15 15 15 55
* - floating
- base_joint
- parent_body
- Result
* - ``None``
- ``None``
- ``-1``
- Format default (URDF: FIXED joint)
* - ``True``
- ``None``
- ``-1``
- FREE joint to world (6 DOF)
* - ``False``
- ``None``
- ``-1``
- FIXED joint to world (0 DOF)
* - ``None``
- ``{dict}``
- ``-1``
- Custom joint to world (e.g., D6)
* - ``False``
- ``None``
- ``body_idx``
- FIXED joint to parent body
* - ``None``
- ``{dict}``
- ``body_idx``
- Custom joint to parent body (e.g., D6)
* - *explicitly set*
- *explicitly set*
- *any*
- ❌ Error: mutually exclusive (cannot specify both)
* - ``True``
- ``None``
- ``body_idx``
- ❌ Error: FREE joints require world frame
scale: The scaling factor to apply to the imported mechanism.
hide_visuals: If True, hide visual shapes.
parse_visuals_as_colliders: If True, the geometry defined under the `` tags is used for collision handling instead of the `` geometries.
up_axis: The up axis of the URDF. This is used to transform the URDF to the builder's up axis. It also determines the up axis of capsules and cylinders in the URDF. The default is Z.
force_show_colliders: If True, the collision shapes are always shown, even if there are visual shapes.
enable_self_collisions: If True, self-collisions are enabled.
ignore_inertial_definitions: If True, the inertial parameters defined in the URDF are ignored and the inertia is calculated from the shape geometry.
joint_ordering: The ordering of the joints in the simulation. Can be either "bfs" or "dfs" for breadth-first or depth-first search, or ``None`` to keep joints in the order in which they appear in the URDF. Default is "dfs".
bodies_follow_joint_ordering: If True, the bodies are added to the builder in the same order as the joints (parent then child body). Otherwise, bodies are added in the order they appear in the URDF. Default is True.
collapse_fixed_joints: If True, fixed joints are removed and the respective bodies are merged.
mesh_maxhullvert: Maximum vertices for convex hull approximation of meshes.
force_position_velocity_actuation: If True and both position (stiffness) and velocity
(damping) gains are non-zero, joints use :attr:`~newton.JointTargetMode.POSITION_VELOCITY` actuation mode.
If False (default), actuator modes are inferred per joint via :func:`newton.JointTargetMode.from_gains`:
:attr:`~newton.JointTargetMode.POSITION` if stiffness > 0, :attr:`~newton.JointTargetMode.VELOCITY` if only
damping > 0, :attr:`~newton.JointTargetMode.EFFORT` if a drive is present but both gains are zero
(direct torque control), or :attr:`~newton.JointTargetMode.NONE` if no drive/actuation is applied.
"""
# Early validation of base joint parameters
builder._validate_base_joint_params(floating, base_joint, parent_body)
if override_root_xform and xform is None:
raise ValueError("override_root_xform=True requires xform to be set")
if mesh_maxhullvert is None:
mesh_maxhullvert = Mesh.MAX_HULL_VERTICES
axis_xform = wp.transform(wp.vec3(0.0), quat_between_axes(up_axis, builder.up_axis))
if xform is None:
xform = axis_xform
else:
xform = wp.transform(*xform) * axis_xform
source = os.fspath(source) if hasattr(source, "__fspath__") else source
if source.startswith(("package://", "model://")):
if resolve_robotics_uri is not None:
try:
source = resolve_robotics_uri(source)
except FileNotFoundError:
raise FileNotFoundError(
f'Could not resolve URDF source URI "{source}". '
f"Check that the package is installed and that relevant environment variables "
f"(ROS_PACKAGE_PATH, AMENT_PREFIX_PATH, GZ_SIM_RESOURCE_PATH, etc.) are set correctly. "
f"See https://github.com/ami-iit/resolve-robotics-uri-py for details."
) from None
else:
raise ImportError(
f'Cannot resolve URDF source URI "{source}" without resolve-robotics-uri-py. '
f"Install it with: pip install resolve-robotics-uri-py"
)
if os.path.isfile(source):
file = ET.parse(source)
urdf_root = file.getroot()
else:
xml_content = sanitize_xml_content(source)
urdf_root = ET.fromstring(xml_content)
# load joint defaults
default_joint_limit_lower = builder.default_joint_cfg.limit_lower
default_joint_limit_upper = builder.default_joint_cfg.limit_upper
default_joint_damping = builder.default_joint_cfg.target_kd
# load shape defaults
default_shape_density = builder.default_shape_cfg.density
def resolve_urdf_asset(filename: str | None) -> tuple[str | None, tempfile.NamedTemporaryFile | None]:
"""Resolve a URDF asset URI/path to a local filename.
Args:
filename: Asset filename or URI from the URDF (may be None).
Returns:
A tuple of (resolved_filename, tmpfile). The tmpfile is only
populated for temporary downloads (e.g., http/https) and must be
cleaned up by the caller (e.g., remove tmpfile.name).
"""
if filename is None:
return None, None
file_tmp = None
if filename.startswith(("package://", "model://")):
if resolve_robotics_uri is not None:
try:
if filename.startswith("package://"):
fn = filename.replace("package://", "")
parent_urdf_folder = os.path.abspath(
os.path.join(os.path.abspath(source), os.pardir, os.pardir, os.pardir)
)
package_dirs = [parent_urdf_folder]
else:
package_dirs = []
filename = resolve_robotics_uri(filename, package_dirs=package_dirs)
except FileNotFoundError:
warnings.warn(
f'Warning: could not resolve URI "{filename}". '
f"Check that the package is installed and that relevant environment variables "
f"(ROS_PACKAGE_PATH, AMENT_PREFIX_PATH, GZ_SIM_RESOURCE_PATH, etc.) are set correctly. "
f"See https://github.com/ami-iit/resolve-robotics-uri-py for details.",
stacklevel=2,
)
return None, None
else:
if not os.path.isfile(source):
warnings.warn(
f'Warning: cannot resolve URI "{filename}" when URDF is loaded from XML string. '
f"Load URDF from a file path, or install resolve-robotics-uri-py: "
f"pip install resolve-robotics-uri-py",
stacklevel=2,
)
return None, None
if filename.startswith("package://"):
fn = filename.replace("package://", "")
package_name = fn.split("/")[0]
urdf_folder = os.path.dirname(source)
if package_name in urdf_folder:
filename = os.path.join(urdf_folder[: urdf_folder.rindex(package_name)], fn)
else:
warnings.warn(
f'Warning: could not resolve package "{package_name}" in URI "{filename}". '
f"For robust URI resolution, install resolve-robotics-uri-py: "
f"pip install resolve-robotics-uri-py",
stacklevel=2,
)
return None, None
else:
warnings.warn(
f'Warning: cannot resolve model:// URI "{filename}" without resolve-robotics-uri-py. '
f"Install it with: pip install resolve-robotics-uri-py",
stacklevel=2,
)
return None, None
elif filename.startswith(("http://", "https://")):
file_tmp = download_asset_tmpfile(filename)
filename = file_tmp.name
else:
if not os.path.isabs(filename):
if not os.path.isfile(source):
warnings.warn(
f'Warning: cannot resolve relative URI "{filename}" when URDF is loaded from XML string. '
f"Load URDF from a file path.",
stacklevel=2,
)
return None, None
filename = os.path.join(os.path.dirname(source), filename)
if not os.path.exists(filename):
warnings.warn(f"Warning: asset file {filename} does not exist", stacklevel=2)
return None, None
return filename, file_tmp
def _parse_material_properties(material_element):
if material_element is None:
return None, None
color = None
texture = None
color_el = material_element.find("color")
if color_el is not None:
rgba = color_el.get("rgba")
if rgba:
values = np.fromstring(rgba, sep=" ", dtype=np.float32)
if len(values) >= 3:
color = (float(values[0]), float(values[1]), float(values[2]))
texture_el = material_element.find("texture")
if texture_el is not None:
texture_name = texture_el.get("filename")
if texture_name:
resolved, tmpfile = resolve_urdf_asset(texture_name)
try:
if resolved is not None:
if tmpfile is not None:
# Temp file will be deleted, so load texture eagerly
texture = load_texture(resolved)
else:
# Local file, pass path for lazy loading by viewer
texture = resolved
finally:
if tmpfile is not None:
os.remove(tmpfile.name)
return color, texture
materials: dict[str, dict[str, np.ndarray | None]] = {}
for material in urdf_root.findall("material"):
mat_name = material.get("name")
if not mat_name:
continue
color, texture = _parse_material_properties(material)
materials[mat_name] = {
"color": color,
"texture": texture,
}
def resolve_material(material_element):
if material_element is None:
return {"color": None, "texture": None}
mat_name = material_element.get("name")
color, texture = _parse_material_properties(material_element)
if mat_name and mat_name in materials:
resolved = dict(materials[mat_name])
else:
resolved = {"color": None, "texture": None}
if color is not None:
resolved["color"] = color
if texture is not None:
resolved["texture"] = texture
if mat_name and mat_name not in materials and any(value is not None for value in (color, texture)):
materials[mat_name] = dict(resolved)
return resolved
# Process custom attributes defined for different kinds of shapes, bodies, joints, etc.
builder_custom_attr_shape: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.SHAPE]
)
builder_custom_attr_body: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.BODY]
)
builder_custom_attr_joint: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.JOINT]
)
builder_custom_attr_articulation: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.ARTICULATION]
)
def parse_transform(element):
if element is None or element.find("origin") is None:
return wp.transform()
origin = element.find("origin")
xyz = origin.get("xyz") or "0 0 0"
rpy = origin.get("rpy") or "0 0 0"
xyz = [float(x) * scale for x in xyz.split()]
rpy = [float(x) for x in rpy.split()]
return wp.transform(xyz, wp.quat_rpy(*rpy))
def parse_shapes(link: int, geoms, density, incoming_xform=None, visible=True, just_visual=False):
shape_cfg = builder.default_shape_cfg.copy()
shape_cfg.density = density
shape_cfg.is_visible = visible
shape_cfg.has_shape_collision = not just_visual
shape_cfg.has_particle_collision = not just_visual
shape_kwargs = {
"body": link,
"cfg": shape_cfg,
"custom_attributes": {},
}
shapes = []
# add geometry
for geom_group in geoms:
geo = geom_group.find("geometry")
if geo is None:
continue
custom_attributes = parse_custom_attributes(geo.attrib, builder_custom_attr_shape, parsing_mode="urdf")
shape_kwargs["custom_attributes"] = custom_attributes
tf = parse_transform(geom_group)
if incoming_xform is not None:
tf = incoming_xform * tf
material_info = {"color": None, "texture": None}
if just_visual:
material_info = resolve_material(geom_group.find("material"))
for box in geo.findall("box"):
size = box.get("size") or "1 1 1"
size = [float(x) for x in size.split()]
s = builder.add_shape_box(
xform=tf,
hx=size[0] * 0.5 * scale,
hy=size[1] * 0.5 * scale,
hz=size[2] * 0.5 * scale,
**shape_kwargs,
)
shapes.append(s)
for sphere in geo.findall("sphere"):
s = builder.add_shape_sphere(
xform=tf,
radius=float(sphere.get("radius") or "1") * scale,
**shape_kwargs,
)
shapes.append(s)
for cylinder in geo.findall("cylinder"):
# Apply axis rotation to transform
xform = wp.transform(tf.p, tf.q * quat_between_axes(Axis.Z, up_axis))
s = builder.add_shape_cylinder(
xform=xform,
radius=float(cylinder.get("radius") or "1") * scale,
half_height=float(cylinder.get("length") or "1") * 0.5 * scale,
**shape_kwargs,
)
shapes.append(s)
for capsule in geo.findall("capsule"):
# Apply axis rotation to transform
xform = wp.transform(tf.p, tf.q * quat_between_axes(Axis.Z, up_axis))
s = builder.add_shape_capsule(
xform=xform,
radius=float(capsule.get("radius") or "1") * scale,
half_height=float(capsule.get("height") or "1") * 0.5 * scale,
**shape_kwargs,
)
shapes.append(s)
for mesh in geo.findall("mesh"):
filename = mesh.get("filename")
if filename is None:
continue
scaling = mesh.get("scale") or "1 1 1"
scaling = np.array([float(x) * scale for x in scaling.split()])
resolved, file_tmp = resolve_urdf_asset(filename)
if resolved is None:
continue
m_meshes = load_meshes_from_file(
resolved,
scale=scaling,
maxhullvert=mesh_maxhullvert,
override_color=material_info["color"],
override_texture=material_info["texture"],
)
for m_mesh in m_meshes:
if m_mesh.texture is not None and m_mesh.uvs is None:
warnings.warn(
f"Warning: mesh {resolved} has a texture but no UVs; texture will be ignored.",
stacklevel=2,
)
m_mesh.texture = None
# Mesh shapes must not use cfg.sdf_*; SDFs are built on the mesh itself.
mesh_shape_kwargs = dict(shape_kwargs)
mesh_cfg = shape_cfg.copy()
mesh_cfg.sdf_max_resolution = None
mesh_cfg.sdf_target_voxel_size = None
mesh_cfg.sdf_narrow_band_range = (-0.1, 0.1)
mesh_shape_kwargs["cfg"] = mesh_cfg
s = builder.add_shape_mesh(
xform=tf,
mesh=m_mesh,
**mesh_shape_kwargs,
)
shapes.append(s)
if file_tmp is not None:
os.remove(file_tmp.name)
file_tmp = None
return shapes
joint_indices = [] # Collect joint indices as we create them
# add joints
# mapping from parent, child link names to joint
parent_child_joint = {}
joints = []
for joint in urdf_root.findall("joint"):
parent = joint.find("parent").get("link")
child = joint.find("child").get("link")
joint_custom_attributes = parse_custom_attributes(joint.attrib, builder_custom_attr_joint, parsing_mode="urdf")
joint_data = {
"name": joint.get("name"),
"parent": parent,
"child": child,
"type": joint.get("type"),
"origin": parse_transform(joint),
"damping": default_joint_damping,
"friction": 0.0,
"axis": wp.vec3(1.0, 0.0, 0.0),
"limit_lower": default_joint_limit_lower,
"limit_upper": default_joint_limit_upper,
"custom_attributes": joint_custom_attributes,
}
el_axis = joint.find("axis")
if el_axis is not None:
ax = el_axis.get("xyz", "1 0 0").strip().split()
joint_data["axis"] = wp.vec3(float(ax[0]), float(ax[1]), float(ax[2]))
el_dynamics = joint.find("dynamics")
if el_dynamics is not None:
joint_data["damping"] = float(el_dynamics.get("damping", default_joint_damping))
joint_data["friction"] = float(el_dynamics.get("friction", 0))
el_limit = joint.find("limit")
if el_limit is not None:
joint_data["limit_lower"] = float(el_limit.get("lower", default_joint_limit_lower))
joint_data["limit_upper"] = float(el_limit.get("upper", default_joint_limit_upper))
el_mimic = joint.find("mimic")
if el_mimic is not None:
joint_data["mimic_joint"] = el_mimic.get("joint")
joint_data["mimic_coef0"] = float(el_mimic.get("offset", 0))
joint_data["mimic_coef1"] = float(el_mimic.get("multiplier", 1))
parent_child_joint[(parent, child)] = joint_data
joints.append(joint_data)
# Extract the articulation label early so we can build hierarchical labels
articulation_label = urdf_root.attrib.get("name")
def make_label(name: str) -> str:
"""Build a hierarchical label for an entity name.
Args:
name: The entity name to label.
Returns:
Hierarchical label ``{articulation_label}/{name}`` when an
articulation label is present, otherwise ``name``.
"""
return f"{articulation_label}/{name}" if articulation_label else name
# topological sorting of joints because the FK function will resolve body transforms
# in joint order and needs the parent link transform to be resolved before the child
urdf_links = []
sorted_joints = []
if len(joints) > 0:
if joint_ordering is not None:
joint_edges = [(joint["parent"], joint["child"]) for joint in joints]
sorted_joint_ids = topological_sort(joint_edges, use_dfs=joint_ordering == "dfs")
sorted_joints = [joints[i] for i in sorted_joint_ids]
else:
sorted_joints = joints
if bodies_follow_joint_ordering:
body_order: list[str] = [sorted_joints[0]["parent"]] + [joint["child"] for joint in sorted_joints]
for body in body_order:
urdf_link = urdf_root.find(f"link[@name='{body}']")
if urdf_link is None:
raise ValueError(f"Link {body} not found in URDF")
urdf_links.append(urdf_link)
if len(urdf_links) == 0:
urdf_links = urdf_root.findall("link")
# add links and shapes
# maps from link name -> link index
link_index: dict[str, int] = {}
visual_shapes: list[int] = []
start_shape_count = len(builder.shape_type)
for urdf_link in urdf_links:
name = urdf_link.get("name")
if name is None:
raise ValueError("Link has no name")
link = builder.add_link(
label=make_label(name),
custom_attributes=parse_custom_attributes(urdf_link.attrib, builder_custom_attr_body, parsing_mode="urdf"),
)
# add ourselves to the index
link_index[name] = link
visuals = urdf_link.findall("visual")
colliders = urdf_link.findall("collision")
if parse_visuals_as_colliders:
colliders = visuals
else:
s = parse_shapes(link, visuals, density=0.0, just_visual=True, visible=not hide_visuals)
visual_shapes.extend(s)
show_colliders = should_show_collider(
force_show_colliders,
has_visual_shapes=len(visuals) > 0,
parse_visuals_as_colliders=parse_visuals_as_colliders,
)
parse_shapes(link, colliders, density=default_shape_density, visible=show_colliders)
m = builder.body_mass[link]
el_inertia = urdf_link.find("inertial")
if not ignore_inertial_definitions and el_inertia is not None:
# overwrite inertial parameters if defined
inertial_frame = parse_transform(el_inertia)
com = inertial_frame.p
builder.body_com[link] = com
I_m = np.zeros((3, 3))
el_i_m = el_inertia.find("inertia")
if el_i_m is not None:
I_m[0, 0] = float(el_i_m.get("ixx", 0)) * scale**2
I_m[1, 1] = float(el_i_m.get("iyy", 0)) * scale**2
I_m[2, 2] = float(el_i_m.get("izz", 0)) * scale**2
I_m[0, 1] = float(el_i_m.get("ixy", 0)) * scale**2
I_m[0, 2] = float(el_i_m.get("ixz", 0)) * scale**2
I_m[1, 2] = float(el_i_m.get("iyz", 0)) * scale**2
I_m[1, 0] = I_m[0, 1]
I_m[2, 0] = I_m[0, 2]
I_m[2, 1] = I_m[1, 2]
rot = wp.quat_to_matrix(inertial_frame.q)
I_m = rot @ wp.mat33(I_m) @ wp.transpose(rot)
builder.body_inertia[link] = I_m
if any(x for x in I_m):
builder.body_inv_inertia[link] = wp.inverse(I_m)
else:
builder.body_inv_inertia[link] = I_m
el_mass = el_inertia.find("mass")
if el_mass is not None:
m = float(el_mass.get("value", 0))
builder.body_mass[link] = m
builder.body_inv_mass[link] = 1.0 / m if m > 0.0 else 0.0
end_shape_count = len(builder.shape_type)
# add base joint
if len(sorted_joints) > 0:
base_link_name = sorted_joints[0]["parent"]
else:
base_link_name = next(iter(link_index.keys()))
root = link_index[base_link_name]
# Determine the parent for the base joint (-1 for world, or an existing body index)
base_parent = parent_body
if base_joint is not None:
if override_root_xform:
base_parent_xform = xform
base_child_xform = wp.transform_identity()
else:
# Split xform: position goes to parent, rotation to child (inverted)
# so the custom base joint's axis isn't rotated by xform.
base_parent_xform = wp.transform(xform.p, wp.quat_identity())
base_child_xform = wp.transform((0.0, 0.0, 0.0), wp.quat_inverse(xform.q))
base_joint_id = builder._add_base_joint(
child=root,
base_joint=base_joint,
label=make_label("base_joint"),
parent_xform=base_parent_xform,
child_xform=base_child_xform,
parent=base_parent,
)
joint_indices.append(base_joint_id)
elif floating and base_parent == -1:
# floating=True only makes sense when connecting to world
floating_joint_id = builder._add_base_joint(
child=root,
floating=True,
label=make_label("floating_base"),
parent_xform=xform,
parent=base_parent,
)
joint_indices.append(floating_joint_id)
# set dofs to transform for the floating base joint
start = builder.joint_q_start[floating_joint_id]
builder.joint_q[start + 0] = xform.p[0]
builder.joint_q[start + 1] = xform.p[1]
builder.joint_q[start + 2] = xform.p[2]
builder.joint_q[start + 3] = xform.q[0]
builder.joint_q[start + 4] = xform.q[1]
builder.joint_q[start + 5] = xform.q[2]
builder.joint_q[start + 6] = xform.q[3]
else:
# Fixed joint to world or to parent_body
# When parent_body is set, xform is interpreted as relative to the parent body
joint_indices.append(
builder._add_base_joint(
child=root,
floating=False,
label=make_label("fixed_base"),
parent_xform=xform,
parent=base_parent,
)
)
# add joints, in the desired order starting from root body
# Track only joints that are actually created (some may be skipped if their child body wasn't inserted).
joint_name_to_idx: dict[str, int] = {}
for joint in sorted_joints:
parent = link_index[joint["parent"]]
child = link_index[joint["child"]]
if child == -1:
# we skipped the insertion of the child body
continue
lower = joint.get("limit_lower", None)
upper = joint.get("limit_upper", None)
joint_damping = joint["damping"]
parent_xform = joint["origin"]
joint_params = {
"parent": parent,
"child": child,
"parent_xform": parent_xform,
"label": make_label(joint["name"]),
"custom_attributes": joint["custom_attributes"],
}
# URDF doesn't contain gain information (only damping, no stiffness), so we can't infer
# actuator mode. Default to POSITION.
actuator_mode = (
JointTargetMode.POSITION_VELOCITY if force_position_velocity_actuation else JointTargetMode.POSITION
)
created_joint_idx: int
if joint["type"] == "revolute" or joint["type"] == "continuous":
created_joint_idx = builder.add_joint_revolute(
axis=joint["axis"],
target_kd=joint_damping,
actuator_mode=actuator_mode,
limit_lower=lower,
limit_upper=upper,
**joint_params,
)
elif joint["type"] == "prismatic":
created_joint_idx = builder.add_joint_prismatic(
axis=joint["axis"],
target_kd=joint_damping,
actuator_mode=actuator_mode,
limit_lower=lower * scale,
limit_upper=upper * scale,
**joint_params,
)
elif joint["type"] == "fixed":
created_joint_idx = builder.add_joint_fixed(**joint_params)
elif joint["type"] == "floating":
created_joint_idx = builder.add_joint_free(**joint_params)
elif joint["type"] == "planar":
# find plane vectors perpendicular to axis
axis = np.array(joint["axis"])
axis /= np.linalg.norm(axis)
# create helper vector that is not parallel to the axis
helper = np.array([1, 0, 0]) if np.allclose(axis, [0, 1, 0]) else np.array([0, 1, 0])
u = np.cross(helper, axis)
u /= np.linalg.norm(u)
v = np.cross(axis, u)
v /= np.linalg.norm(v)
created_joint_idx = builder.add_joint_d6(
linear_axes=[
ModelBuilder.JointDofConfig(
u,
limit_lower=lower * scale,
limit_upper=upper * scale,
target_kd=joint_damping,
actuator_mode=actuator_mode,
),
ModelBuilder.JointDofConfig(
v,
limit_lower=lower * scale,
limit_upper=upper * scale,
target_kd=joint_damping,
actuator_mode=actuator_mode,
),
],
**joint_params,
)
else:
raise Exception("Unsupported joint type: " + joint["type"])
joint_indices.append(created_joint_idx)
joint_name_to_idx[joint["name"]] = created_joint_idx
# Create mimic constraints
for joint in sorted_joints:
if "mimic_joint" in joint:
mimic_target_name = joint["mimic_joint"]
if mimic_target_name not in joint_name_to_idx:
warnings.warn(
f"Mimic joint '{joint['name']}' references unknown joint '{mimic_target_name}', skipping mimic constraint",
stacklevel=2,
)
continue
follower_idx = joint_name_to_idx.get(joint["name"])
leader_idx = joint_name_to_idx.get(mimic_target_name)
if follower_idx is None:
warnings.warn(
f"Mimic joint '{joint['name']}' was not created, skipping mimic constraint",
stacklevel=2,
)
continue
builder.add_constraint_mimic(
joint0=follower_idx,
joint1=leader_idx,
coef0=joint.get("mimic_coef0", 0.0),
coef1=joint.get("mimic_coef1", 1.0),
label=make_label(f"mimic_{joint['name']}"),
)
# Create articulation from all collected joints
articulation_custom_attrs = parse_custom_attributes(
urdf_root.attrib, builder_custom_attr_articulation, parsing_mode="urdf"
)
builder._finalize_imported_articulation(
joint_indices=joint_indices,
parent_body=parent_body,
articulation_label=articulation_label,
custom_attributes=articulation_custom_attrs,
)
for i in range(start_shape_count, end_shape_count):
for j in visual_shapes:
builder.add_shape_collision_filter_pair(i, j)
if not enable_self_collisions:
for i in range(start_shape_count, end_shape_count):
for j in range(i + 1, end_shape_count):
builder.add_shape_collision_filter_pair(i, j)
if collapse_fixed_joints:
builder.collapse_fixed_joints()
================================================
FILE: newton/_src/utils/import_usd.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import collections
import datetime
import inspect
import itertools
import os
import posixpath
import re
import warnings
from dataclasses import dataclass
from typing import TYPE_CHECKING, Any, Literal
from urllib.parse import urljoin
if TYPE_CHECKING:
from pxr import Usd
UsdStage = Usd.Stage
else:
UsdStage = Any
import numpy as np
import warp as wp
from ..core import quat_between_axes
from ..core.types import Axis, Transform
from ..geometry import GeoType, Mesh, ShapeFlags, compute_inertia_shape, compute_inertia_sphere
from ..sim.builder import ModelBuilder
from ..sim.enums import JointTargetMode
from ..sim.model import Model
from ..usd import utils as usd
from ..usd.schema_resolver import PrimType, SchemaResolver, SchemaResolverManager
from ..usd.schemas import SchemaResolverNewton
from .import_utils import should_show_collider
AttributeFrequency = Model.AttributeFrequency
_NEWTON_SRC_DIR = os.path.normpath(os.path.join(os.path.dirname(__file__), os.pardir)) + os.sep
def _external_stacklevel() -> int:
"""Return a ``stacklevel`` that points past all ``newton._src`` frames."""
frame = inspect.currentframe()
if frame is None:
return 2
frame = frame.f_back
stacklevel = 1
try:
while frame is not None and os.path.normpath(frame.f_code.co_filename).startswith(_NEWTON_SRC_DIR):
frame = frame.f_back
stacklevel += 1
return stacklevel
finally:
del frame
def parse_usd(
builder: ModelBuilder,
source: str | UsdStage,
*,
xform: Transform | None = None,
floating: bool | None = None,
base_joint: dict | None = None,
parent_body: int = -1,
only_load_enabled_rigid_bodies: bool = False,
only_load_enabled_joints: bool = True,
joint_drive_gains_scaling: float = 1.0,
verbose: bool = False,
ignore_paths: list[str] | None = None,
collapse_fixed_joints: bool = False,
enable_self_collisions: bool = True,
apply_up_axis_from_stage: bool = False,
root_path: str = "/",
joint_ordering: Literal["bfs", "dfs"] | None = "dfs",
bodies_follow_joint_ordering: bool = True,
skip_mesh_approximation: bool = False,
load_sites: bool = True,
load_visual_shapes: bool = True,
hide_collision_shapes: bool = False,
force_show_colliders: bool = False,
parse_mujoco_options: bool = True,
mesh_maxhullvert: int | None = None,
schema_resolvers: list[SchemaResolver] | None = None,
force_position_velocity_actuation: bool = False,
override_root_xform: bool = False,
) -> dict[str, Any]:
"""Parses a Universal Scene Description (USD) stage containing UsdPhysics schema definitions for rigid-body articulations and adds the bodies, shapes and joints to the given ModelBuilder.
The USD description has to be either a path (file name or URL), or an existing USD stage instance that implements the `Stage `_ interface.
See :ref:`usd_parsing` for more information.
Args:
builder (ModelBuilder): The :class:`ModelBuilder` to add the bodies and joints to.
source: The file path to the USD file, or an existing USD stage instance.
xform: The transform to apply to the entire scene.
override_root_xform: If ``True``, the articulation root's world-space
transform is replaced by ``xform`` instead of being composed with it,
preserving only the internal structure (relative body positions). Useful
for cloning articulations at explicit positions. Not intended for sources
containing multiple articulations, as all roots would be placed at the
same ``xform``. Defaults to ``False``.
floating: Controls the base joint type for the root body (bodies not connected as
a child to any joint).
- ``None`` (default): Uses format-specific default (creates a FREE joint for USD bodies without joints).
- ``True``: Creates a FREE joint with 6 DOF (3 translation + 3 rotation). Only valid when
``parent_body == -1`` since FREE joints must connect to world frame.
- ``False``: Creates a FIXED joint (0 DOF).
Cannot be specified together with ``base_joint``.
base_joint: Custom joint specification for connecting the root body to the world
(or to ``parent_body`` if specified). This parameter enables hierarchical composition with
custom mobility. Dictionary with joint parameters as accepted by
:meth:`ModelBuilder.add_joint` (e.g., joint type, axes, limits, stiffness).
Cannot be specified together with ``floating``.
parent_body: Parent body index for hierarchical composition. If specified, attaches the
imported root body to this existing body, making them part of the same kinematic articulation.
The connection type is determined by ``floating`` or ``base_joint``. If ``-1`` (default),
the root connects to the world frame. **Restriction**: Only the most recently added
articulation can be used as parent; attempting to attach to an older articulation will raise
a ``ValueError``.
.. note::
Valid combinations of ``floating``, ``base_joint``, and ``parent_body``:
.. list-table::
:header-rows: 1
:widths: 15 15 15 55
* - floating
- base_joint
- parent_body
- Result
* - ``None``
- ``None``
- ``-1``
- Format default (USD: FREE joint for bodies without joints)
* - ``True``
- ``None``
- ``-1``
- FREE joint to world (6 DOF)
* - ``False``
- ``None``
- ``-1``
- FIXED joint to world (0 DOF)
* - ``None``
- ``{dict}``
- ``-1``
- Custom joint to world (e.g., D6)
* - ``False``
- ``None``
- ``body_idx``
- FIXED joint to parent body
* - ``None``
- ``{dict}``
- ``body_idx``
- Custom joint to parent body (e.g., D6)
* - *explicitly set*
- *explicitly set*
- *any*
- ❌ Error: mutually exclusive (cannot specify both)
* - ``True``
- ``None``
- ``body_idx``
- ❌ Error: FREE joints require world frame
only_load_enabled_rigid_bodies: If True, only rigid bodies which do not have `physics:rigidBodyEnabled` set to False are loaded.
only_load_enabled_joints: If True, only joints which do not have `physics:jointEnabled` set to False are loaded.
joint_drive_gains_scaling: The default scaling of the PD control gains (stiffness and damping), if not set in the PhysicsScene with as "newton:joint_drive_gains_scaling".
verbose: If True, print additional information about the parsed USD file. Default is False.
ignore_paths: A list of regular expressions matching prim paths to ignore.
collapse_fixed_joints: If True, fixed joints are removed and the respective bodies are merged. Only considered if not set on the PhysicsScene as "newton:collapse_fixed_joints".
enable_self_collisions: Default for whether self-collisions are enabled for all shapes within an articulation. Resolved via the schema resolver from ``newton:selfCollisionEnabled`` (NewtonArticulationRootAPI) or ``physxArticulation:enabledSelfCollisions``; if neither is authored, this value takes precedence.
apply_up_axis_from_stage: If True, the up axis of the stage will be used to set :attr:`newton.ModelBuilder.up_axis`. Otherwise, the stage will be rotated such that its up axis aligns with the builder's up axis. Default is False.
root_path: The USD path to import, defaults to "/".
joint_ordering: The ordering of the joints in the simulation. Can be either "bfs" or "dfs" for breadth-first or depth-first search, or ``None`` to keep joints in the order in which they appear in the USD. Default is "dfs".
bodies_follow_joint_ordering: If True, the bodies are added to the builder in the same order as the joints (parent then child body). Otherwise, bodies are added in the order they appear in the USD. Default is True.
skip_mesh_approximation: If True, mesh approximation is skipped. Otherwise, meshes are approximated according to the ``physics:approximation`` attribute defined on the UsdPhysicsMeshCollisionAPI (if it is defined). Default is False.
load_sites: If True, sites (prims with MjcSiteAPI) are loaded as non-colliding reference points. If False, sites are ignored. Default is True.
load_visual_shapes: If True, non-physics visual geometry is loaded. If False, visual-only shapes are ignored (sites are still controlled by ``load_sites``). Default is True.
hide_collision_shapes: If True, collision shapes on bodies that already
have visual-only geometry are hidden unconditionally, regardless of
whether the collider has authored PBR material data. Collision
shapes on bodies without visual-only geometry remain visible as a
rendering fallback. Default is False.
force_show_colliders: If True, collision shapes get the VISIBLE flag
regardless of whether visual shapes exist on the same body. Note that
``hide_collision_shapes=True`` still suppresses the VISIBLE flag for
colliders on bodies with visual-only geometry. Default is False.
parse_mujoco_options: Whether MuJoCo solver options from the PhysicsScene should be parsed. If False, solver options are not loaded and custom attributes retain their default values. Default is True.
mesh_maxhullvert: Maximum vertices for convex hull approximation of meshes. Note that an authored ``newton:maxHullVertices`` attribute on any shape with a ``NewtonMeshCollisionAPI`` will take priority over this value.
schema_resolvers: Resolver instances in priority order. Default is to only parse Newton-specific attributes.
Schema resolvers collect per-prim "solver-specific" attributes, see :ref:`schema_resolvers` for more information.
These include namespaced attributes such as ``newton:*``, ``physx*``
(e.g., ``physxScene:*``, ``physxRigidBody:*``, ``physxSDFMeshCollision:*``), and ``mjc:*`` that
are authored in the USD but not strictly required to build the simulation. This is useful for
inspection, experimentation, or custom pipelines that read these values via
``result["schema_attrs"]`` returned from ``parse_usd()``.
.. note::
Using the ``schema_resolvers`` argument is an experimental feature that may be removed or changed significantly in the future.
force_position_velocity_actuation: If True and both stiffness (kp) and damping (kd)
are non-zero, joints use :attr:`~newton.JointTargetMode.POSITION_VELOCITY` actuation mode.
If False (default), actuator modes are inferred per joint via :func:`newton.JointTargetMode.from_gains`:
:attr:`~newton.JointTargetMode.POSITION` if stiffness > 0, :attr:`~newton.JointTargetMode.VELOCITY` if only
damping > 0, :attr:`~newton.JointTargetMode.EFFORT` if a drive is present but both gains are zero
(direct torque control), or :attr:`~newton.JointTargetMode.NONE` if no drive/actuation is applied.
Returns:
The returned mapping has the following entries:
.. list-table::
:widths: 25 75
* - ``"fps"``
- USD stage frames per second
* - ``"duration"``
- Difference between end time code and start time code of the USD stage
* - ``"up_axis"``
- :class:`Axis` representing the stage's up axis ("X", "Y", or "Z")
* - ``"path_body_map"``
- Mapping from prim path (str) of a rigid body prim (e.g. that implements the PhysicsRigidBodyAPI) to the respective body index in :class:`~newton.ModelBuilder`
* - ``"path_joint_map"``
- Mapping from prim path (str) of a joint prim (e.g. that implements the PhysicsJointAPI) to the respective joint index in :class:`~newton.ModelBuilder`
* - ``"path_shape_map"``
- Mapping from prim path (str) of the UsdGeom to the respective shape index in :class:`~newton.ModelBuilder`
* - ``"path_shape_scale"``
- Mapping from prim path (str) of the UsdGeom to its respective 3D world scale
* - ``"mass_unit"``
- The stage's Kilograms Per Unit (KGPU) definition (1.0 by default)
* - ``"linear_unit"``
- The stage's Meters Per Unit (MPU) definition (1.0 by default)
* - ``"scene_attributes"``
- Dictionary of all attributes applied to the PhysicsScene prim
* - ``"collapse_results"``
- Dictionary returned by :meth:`newton.ModelBuilder.collapse_fixed_joints` if ``collapse_fixed_joints`` is True, otherwise None.
* - ``"physics_dt"``
- The resolved physics scene time step (float or None)
* - ``"schema_attrs"``
- Dictionary of collected per-prim schema attributes (dict)
* - ``"max_solver_iterations"``
- The resolved maximum solver iterations (int or None)
* - ``"path_body_relative_transform"``
- Mapping from prim path to relative transform for bodies merged via ``collapse_fixed_joints``
* - ``"path_original_body_map"``
- Mapping from prim path to original body index before ``collapse_fixed_joints``
* - ``"actuator_count"``
- Number of external actuators parsed from the USD stage
"""
# Early validation of base joint parameters
builder._validate_base_joint_params(floating, base_joint, parent_body)
if mesh_maxhullvert is None:
mesh_maxhullvert = Mesh.MAX_HULL_VERTICES
if schema_resolvers is None:
schema_resolvers = [SchemaResolverNewton()]
collect_schema_attrs = len(schema_resolvers) > 0
try:
from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics
except ImportError as e:
raise ImportError("Failed to import pxr. Please install USD (e.g. via `pip install usd-core`).") from e
from .topology import topological_sort_undirected # noqa: PLC0415
@dataclass
class PhysicsMaterial:
staticFriction: float = builder.default_shape_cfg.mu
dynamicFriction: float = builder.default_shape_cfg.mu
torsionalFriction: float = builder.default_shape_cfg.mu_torsional
rollingFriction: float = builder.default_shape_cfg.mu_rolling
restitution: float = builder.default_shape_cfg.restitution
density: float = builder.default_shape_cfg.density
# load joint defaults
default_joint_friction = builder.default_joint_cfg.friction
default_joint_limit_ke = builder.default_joint_cfg.limit_ke
default_joint_limit_kd = builder.default_joint_cfg.limit_kd
default_joint_armature = builder.default_joint_cfg.armature
default_joint_velocity_limit = builder.default_joint_cfg.velocity_limit
# load shape defaults
default_shape_density = builder.default_shape_cfg.density
# mapping from physics:approximation attribute (lower case) to remeshing method
approximation_to_remeshing_method = {
"convexdecomposition": "coacd",
"convexhull": "convex_hull",
"boundingsphere": "bounding_sphere",
"boundingcube": "bounding_box",
"meshsimplification": "quadratic",
}
# mapping from remeshing method to a list of shape indices
remeshing_queue = {}
if ignore_paths is None:
ignore_paths = []
usd_axis_to_axis = {
UsdPhysics.Axis.X: Axis.X,
UsdPhysics.Axis.Y: Axis.Y,
UsdPhysics.Axis.Z: Axis.Z,
}
if isinstance(source, str):
stage = Usd.Stage.Open(source, Usd.Stage.LoadAll)
_raise_on_stage_errors(stage, source)
else:
stage = source
_raise_on_stage_errors(stage, "provided stage")
DegreesToRadian = float(np.pi / 180)
mass_unit = 1.0
try:
if UsdPhysics.StageHasAuthoredKilogramsPerUnit(stage):
mass_unit = UsdPhysics.GetStageKilogramsPerUnit(stage)
except Exception as e:
if verbose:
print(f"Failed to get mass unit: {e}")
linear_unit = 1.0
try:
if UsdGeom.StageHasAuthoredMetersPerUnit(stage):
linear_unit = UsdGeom.GetStageMetersPerUnit(stage)
except Exception as e:
if verbose:
print(f"Failed to get linear unit: {e}")
non_regex_ignore_paths = [path for path in ignore_paths if ".*" not in path]
ret_dict = UsdPhysics.LoadUsdPhysicsFromRange(stage, [root_path], excludePaths=non_regex_ignore_paths)
# Initialize schema resolver according to precedence
R = SchemaResolverManager(schema_resolvers)
# Validate solver-specific custom attributes are registered
for resolver in schema_resolvers:
resolver.validate_custom_attributes(builder)
# mapping from prim path to body index in ModelBuilder
path_body_map: dict[str, int] = {}
# mapping from prim path to shape index in ModelBuilder
path_shape_map: dict[str, int] = {}
path_shape_scale: dict[str, wp.vec3] = {}
# mapping from prim path to joint index in ModelBuilder
path_joint_map: dict[str, int] = {}
# cache for resolved material properties (keyed by prim path)
material_props_cache: dict[str, dict[str, Any]] = {}
# cache for mesh data loaded from USD prims
mesh_cache: dict[tuple[str, bool, bool], Mesh] = {}
physics_scene_prim = None
physics_dt = None
max_solver_iters = None
visual_shape_cfg = ModelBuilder.ShapeConfig(
density=0.0,
has_shape_collision=False,
has_particle_collision=False,
)
# Create a cache for world transforms to avoid recomputing them for each prim.
xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default())
def _is_enabled_collider(prim: Usd.Prim) -> bool:
if collider := UsdPhysics.CollisionAPI(prim):
return collider.GetCollisionEnabledAttr().Get()
return False
def _xform_to_mat44(xform: wp.transform) -> wp.mat44:
return wp.transform_compose(xform.p, xform.q, wp.vec3(1.0))
def _get_material_props_cached(prim: Usd.Prim) -> dict[str, Any]:
"""Get material properties with caching to avoid repeated traversal."""
prim_path = str(prim.GetPath())
if prim_path not in material_props_cache:
material_props_cache[prim_path] = usd.resolve_material_properties_for_prim(prim)
return material_props_cache[prim_path]
def _get_mesh_cached(prim: Usd.Prim, *, load_uvs: bool = False, load_normals: bool = False) -> Mesh:
"""Load and cache mesh data to avoid repeated expensive USD mesh extraction."""
prim_path = str(prim.GetPath())
key = (prim_path, load_uvs, load_normals)
if key in mesh_cache:
return mesh_cache[key]
# A mesh loaded with more data is a superset of simpler representations.
for cached_key in [
(prim_path, True, True),
(prim_path, load_uvs, True),
(prim_path, True, load_normals),
]:
if cached_key != key and cached_key in mesh_cache:
return mesh_cache[cached_key]
mesh = usd.get_mesh(prim, load_uvs=load_uvs, load_normals=load_normals)
mesh_cache[key] = mesh
return mesh
def _get_mesh_with_visual_material(prim: Usd.Prim, *, path_name: str) -> Mesh:
"""Load a renderable mesh without changing physics mass properties."""
material_props = _get_material_props_cached(prim)
texture = material_props.get("texture")
physics_mesh = _get_mesh_cached(prim)
if texture is not None:
render_mesh = _get_mesh_cached(prim, load_uvs=True)
# Texture UV expansion is render-only. Preserve the collision mesh's
# mass/inertia so visibility changes do not perturb simulation.
mesh = Mesh(
render_mesh.vertices,
render_mesh.indices,
normals=render_mesh.normals,
uvs=render_mesh.uvs,
compute_inertia=False,
is_solid=physics_mesh.is_solid,
maxhullvert=physics_mesh.maxhullvert,
sdf=physics_mesh.sdf,
)
mesh.mass = physics_mesh.mass
mesh.com = physics_mesh.com
mesh.inertia = physics_mesh.inertia
mesh.has_inertia = physics_mesh.has_inertia
else:
mesh = physics_mesh.copy(recompute_inertia=False)
if texture:
mesh.texture = texture
if mesh.texture is not None and mesh.uvs is None:
warnings.warn(
f"Warning: mesh {path_name} has a texture but no UVs; texture will be ignored.",
stacklevel=2,
)
mesh.texture = None
if material_props.get("color") is not None and mesh.texture is None:
mesh.color = material_props["color"]
if material_props.get("roughness") is not None:
mesh.roughness = material_props["roughness"]
if material_props.get("metallic") is not None:
mesh.metallic = material_props["metallic"]
return mesh
def _has_visual_material_properties(material_props: dict[str, Any]) -> bool:
# Require PBR-like material cues to avoid promoting generic displayColor-only colliders.
return any(material_props.get(key) is not None for key in ("texture", "roughness", "metallic"))
bodies_with_visual_shapes: set[int] = set()
warned_deprecated_body_armature_paths: set[str] = set()
def _get_prim_world_mat(prim, articulation_root_xform, incoming_world_xform):
prim_world_mat = usd.get_transform_matrix(prim, local=False, xform_cache=xform_cache)
if articulation_root_xform is not None:
rebase_mat = _xform_to_mat44(wp.transform_inverse(articulation_root_xform))
prim_world_mat = rebase_mat @ prim_world_mat
if incoming_world_xform is not None:
# Apply the incoming world transform in model space (static shapes or when using body_xform).
incoming_mat = _xform_to_mat44(incoming_world_xform)
prim_world_mat = incoming_mat @ prim_world_mat
return prim_world_mat
def _load_visual_shapes_impl(
parent_body_id: int,
prim: Usd.Prim,
body_xform: wp.transform | None = None,
articulation_root_xform: wp.transform | None = None,
):
"""Load visual-only shapes (non-physics) for a prim subtree.
Args:
parent_body_id: ModelBuilder body id to attach shapes to. Use -1 for
static shapes that are not bound to any rigid body.
prim: USD prim to inspect for visual geometry and recurse into.
body_xform: Rigid body transform actually used by the builder.
This matches any physics-authored pose, scene-level transforms,
and incoming transforms that were applied when the body was created.
articulation_root_xform: The articulation root's world-space transform,
passed when override_root_xform=True. Strips the root's original
pose from visual prim transforms to match the rebased body transforms.
"""
if _is_enabled_collider(prim) or prim.HasAPI(UsdPhysics.RigidBodyAPI):
return
path_name = str(prim.GetPath())
if any(re.match(path, path_name) for path in ignore_paths):
return
prim_world_mat = _get_prim_world_mat(
prim,
articulation_root_xform,
incoming_world_xform if (parent_body_id == -1 or body_xform is not None) else None,
)
if body_xform is not None:
# Use the body transform used by the builder to avoid USD/physics pose mismatches.
body_world_mat = _xform_to_mat44(body_xform)
rel_mat = wp.inverse(body_world_mat) @ prim_world_mat
else:
rel_mat = prim_world_mat
xform_pos, xform_rot, scale = wp.transform_decompose(rel_mat)
xform = wp.transform(xform_pos, xform_rot)
if prim.IsInstance():
proto = prim.GetPrototype()
for child in proto.GetChildren():
# remap prototype child path to this instance's path (instance proxy)
inst_path = child.GetPath().ReplacePrefix(proto.GetPath(), prim.GetPath())
inst_child = stage.GetPrimAtPath(inst_path)
_load_visual_shapes_impl(parent_body_id, inst_child, body_xform, articulation_root_xform)
return
type_name = str(prim.GetTypeName()).lower()
if type_name.endswith("joint"):
return
shape_id = -1
is_site = usd.has_applied_api_schema(prim, "MjcSiteAPI")
# Skip based on granular loading flags
if is_site and not load_sites:
return
if not is_site and not load_visual_shapes:
return
if path_name not in path_shape_map:
if type_name == "cube":
size = usd.get_float(prim, "size", 2.0)
side_lengths = scale * size
shape_id = builder.add_shape_box(
parent_body_id,
xform,
hx=side_lengths[0] / 2,
hy=side_lengths[1] / 2,
hz=side_lengths[2] / 2,
cfg=visual_shape_cfg,
as_site=is_site,
label=path_name,
)
elif type_name == "sphere":
if not (scale[0] == scale[1] == scale[2]):
print("Warning: Non-uniform scaling of spheres is not supported.")
radius = usd.get_float(prim, "radius", 1.0) * max(scale)
shape_id = builder.add_shape_sphere(
parent_body_id,
xform,
radius,
cfg=visual_shape_cfg,
as_site=is_site,
label=path_name,
)
elif type_name == "plane":
axis = usd.get_gprim_axis(prim)
plane_xform = xform
# Apply axis rotation to transform
xform = wp.transform(xform.p, xform.q * quat_between_axes(Axis.Z, axis))
width = usd.get_float(prim, "width", 0.0) * scale[0]
length = usd.get_float(prim, "length", 0.0) * scale[1]
shape_id = builder.add_shape_plane(
body=parent_body_id,
xform=plane_xform,
width=width,
length=length,
cfg=visual_shape_cfg,
label=path_name,
)
elif type_name == "capsule":
axis = usd.get_gprim_axis(prim)
radius = usd.get_float(prim, "radius", 0.5) * scale[0]
half_height = usd.get_float(prim, "height", 2.0) / 2 * scale[1]
# Apply axis rotation to transform
xform = wp.transform(xform.p, xform.q * quat_between_axes(Axis.Z, axis))
shape_id = builder.add_shape_capsule(
parent_body_id,
xform,
radius,
half_height,
cfg=visual_shape_cfg,
as_site=is_site,
label=path_name,
)
elif type_name == "cylinder":
axis = usd.get_gprim_axis(prim)
radius = usd.get_float(prim, "radius", 0.5) * scale[0]
half_height = usd.get_float(prim, "height", 2.0) / 2 * scale[1]
# Apply axis rotation to transform
xform = wp.transform(xform.p, xform.q * quat_between_axes(Axis.Z, axis))
shape_id = builder.add_shape_cylinder(
parent_body_id,
xform,
radius,
half_height,
cfg=visual_shape_cfg,
as_site=is_site,
label=path_name,
)
elif type_name == "cone":
axis = usd.get_gprim_axis(prim)
radius = usd.get_float(prim, "radius", 0.5) * scale[0]
half_height = usd.get_float(prim, "height", 2.0) / 2 * scale[1]
# Apply axis rotation to transform
xform = wp.transform(xform.p, xform.q * quat_between_axes(Axis.Z, axis))
shape_id = builder.add_shape_cone(
parent_body_id,
xform,
radius,
half_height,
cfg=visual_shape_cfg,
as_site=is_site,
label=path_name,
)
elif type_name == "mesh":
mesh = _get_mesh_with_visual_material(prim, path_name=path_name)
shape_id = builder.add_shape_mesh(
parent_body_id,
xform,
scale=scale,
mesh=mesh,
cfg=visual_shape_cfg,
label=path_name,
)
elif type_name == "particlefield3dgaussiansplat":
gaussian = usd.get_gaussian(prim)
shape_id = builder.add_shape_gaussian(
parent_body_id,
gaussian=gaussian,
xform=xform,
scale=scale,
cfg=visual_shape_cfg,
label=path_name,
)
elif len(type_name) > 0 and type_name != "xform" and verbose:
print(f"Warning: Unsupported geometry type {type_name} at {path_name} while loading visual shapes.")
if shape_id >= 0:
path_shape_map[path_name] = shape_id
path_shape_scale[path_name] = scale
if not is_site:
bodies_with_visual_shapes.add(parent_body_id)
if verbose:
print(f"Added visual shape {path_name} ({type_name}) with id {shape_id}.")
for child in prim.GetChildren():
_load_visual_shapes_impl(parent_body_id, child, body_xform, articulation_root_xform)
def add_body(
prim: Usd.Prim,
xform: wp.transform,
label: str,
armature: float | None,
articulation_root_xform: wp.transform | None = None,
is_kinematic: bool = False,
) -> int:
"""Add a rigid body to the builder and optionally load its visual shapes and sites among the body prim's children. Returns the resulting body index."""
# Extract custom attributes for this body
body_custom_attrs = usd.get_custom_attribute_values(
prim, builder_custom_attr_body, context={"builder": builder}
)
body_inertia = wp.mat33(np.eye(3, dtype=np.float32)) * armature if armature is not None else None
b = builder.add_link(
xform=xform,
label=label,
inertia=body_inertia,
armature=0.0 if armature is not None else None,
is_kinematic=is_kinematic,
custom_attributes=body_custom_attrs,
)
path_body_map[label] = b
if load_sites or load_visual_shapes:
for child in prim.GetChildren():
_load_visual_shapes_impl(b, child, body_xform=xform, articulation_root_xform=articulation_root_xform)
return b
def parse_body(
rigid_body_desc: UsdPhysics.RigidBodyDesc,
prim: Usd.Prim,
incoming_xform: wp.transform | None = None,
add_body_to_builder: bool = True,
articulation_root_xform: wp.transform | None = None,
) -> int | dict[str, Any]:
"""Parses a rigid body description.
If `add_body_to_builder` is True, adds it to the builder and returns the resulting body index.
Otherwise returns a dictionary of body data that can be passed to ModelBuilder.add_body()."""
nonlocal path_body_map
nonlocal physics_scene_prim
if not rigid_body_desc.rigidBodyEnabled and only_load_enabled_rigid_bodies:
return -1
rot = rigid_body_desc.rotation
origin = wp.transform(rigid_body_desc.position, usd.value_to_warp(rot))
if incoming_xform is not None:
origin = wp.mul(incoming_xform, origin)
path = str(prim.GetPath())
body_armature_source_path: str | None = None
body_armature = usd.get_float(prim, "newton:armature", None)
if body_armature is not None:
body_armature_source_path = path
if body_armature is None and physics_scene_prim:
body_armature = usd.get_float(physics_scene_prim, "newton:armature", None)
if body_armature is not None:
body_armature_source_path = str(physics_scene_prim.GetPath())
if (
body_armature_source_path is not None
and body_armature_source_path not in warned_deprecated_body_armature_paths
):
warnings.warn(
f"USD-authored 'newton:armature' on {body_armature_source_path} is deprecated and will be "
"removed in a future release. Add any isotropic artificial inertia directly to the body's "
"inertia instead.",
DeprecationWarning,
stacklevel=_external_stacklevel(),
)
warned_deprecated_body_armature_paths.add(body_armature_source_path)
is_kinematic = rigid_body_desc.kinematicBody
if add_body_to_builder:
return add_body(
prim,
origin,
path,
body_armature,
articulation_root_xform=articulation_root_xform,
is_kinematic=is_kinematic,
)
else:
result = {
"prim": prim,
"xform": origin,
"label": path,
"armature": body_armature,
"is_kinematic": is_kinematic,
}
if articulation_root_xform is not None:
result["articulation_root_xform"] = articulation_root_xform
return result
def resolve_joint_parent_child(
joint_desc: UsdPhysics.JointDesc,
body_index_map: dict[str, int],
get_transforms: bool = True,
):
"""Resolve the parent and child of a joint and return their parent + child transforms if requested."""
if get_transforms:
parent_tf = wp.transform(joint_desc.localPose0Position, usd.value_to_warp(joint_desc.localPose0Orientation))
child_tf = wp.transform(joint_desc.localPose1Position, usd.value_to_warp(joint_desc.localPose1Orientation))
else:
parent_tf = None
child_tf = None
parent_path = str(joint_desc.body0)
child_path = str(joint_desc.body1)
parent_id = body_index_map.get(parent_path, -1)
child_id = body_index_map.get(child_path, -1)
# If child_id is -1, swap parent and child
if child_id == -1:
if parent_id == -1:
raise ValueError(f"Unable to parse joint {joint_desc.primPath}: both bodies unresolved")
parent_id, child_id = child_id, parent_id
if get_transforms:
parent_tf, child_tf = child_tf, parent_tf
if verbose:
print(f"Joint {joint_desc.primPath} connects {parent_path} to world")
if get_transforms:
return parent_id, child_id, parent_tf, child_tf
else:
return parent_id, child_id
def parse_joint(
joint_desc: UsdPhysics.JointDesc,
incoming_xform: wp.transform | None = None,
) -> int | None:
"""Parse a joint description and add it to the builder. Returns the resulting joint index if successful, None otherwise."""
if not joint_desc.jointEnabled and only_load_enabled_joints:
return None
key = joint_desc.type
joint_path = str(joint_desc.primPath)
joint_prim = stage.GetPrimAtPath(joint_desc.primPath)
# collect engine-specific attributes on the joint prim if requested
if collect_schema_attrs:
R.collect_prim_attrs(joint_prim)
parent_id, child_id, parent_tf, child_tf = resolve_joint_parent_child( # pyright: ignore[reportAssignmentType]
joint_desc, path_body_map, get_transforms=True
)
if incoming_xform is not None:
parent_tf = incoming_xform * parent_tf
joint_armature = R.get_value(
joint_prim, prim_type=PrimType.JOINT, key="armature", default=default_joint_armature, verbose=verbose
)
joint_friction = R.get_value(
joint_prim, prim_type=PrimType.JOINT, key="friction", default=default_joint_friction, verbose=verbose
)
joint_velocity_limit = R.get_value(
joint_prim,
prim_type=PrimType.JOINT,
key="velocity_limit",
default=None,
verbose=verbose,
)
# Extract custom attributes for this joint
joint_custom_attrs = usd.get_custom_attribute_values(
joint_prim, builder_custom_attr_joint, context={"builder": builder}
)
joint_params = {
"parent": parent_id,
"child": child_id,
"parent_xform": parent_tf,
"child_xform": child_tf,
"label": joint_path,
"enabled": joint_desc.jointEnabled,
"custom_attributes": joint_custom_attrs,
}
joint_index: int | None = None
if key == UsdPhysics.ObjectType.FixedJoint:
joint_index = builder.add_joint_fixed(**joint_params)
elif key == UsdPhysics.ObjectType.RevoluteJoint or key == UsdPhysics.ObjectType.PrismaticJoint:
# we need to scale the builder defaults for the joint limits to degrees for revolute joints
if key == UsdPhysics.ObjectType.RevoluteJoint:
limit_gains_scaling = DegreesToRadian
else:
limit_gains_scaling = 1.0
# Resolve limit gains with precedence, fallback to builder defaults when missing
current_joint_limit_ke = R.get_value(
joint_prim,
prim_type=PrimType.JOINT,
key="limit_angular_ke" if key == UsdPhysics.ObjectType.RevoluteJoint else "limit_linear_ke",
default=default_joint_limit_ke * limit_gains_scaling,
verbose=verbose,
)
current_joint_limit_kd = R.get_value(
joint_prim,
prim_type=PrimType.JOINT,
key="limit_angular_kd" if key == UsdPhysics.ObjectType.RevoluteJoint else "limit_linear_kd",
default=default_joint_limit_kd * limit_gains_scaling,
verbose=verbose,
)
joint_params["axis"] = usd_axis_to_axis[joint_desc.axis]
joint_params["limit_lower"] = joint_desc.limit.lower
joint_params["limit_upper"] = joint_desc.limit.upper
joint_params["limit_ke"] = current_joint_limit_ke
joint_params["limit_kd"] = current_joint_limit_kd
joint_params["armature"] = joint_armature
joint_params["friction"] = joint_friction
joint_params["velocity_limit"] = joint_velocity_limit
if joint_desc.drive.enabled:
target_vel = joint_desc.drive.targetVelocity
target_pos = joint_desc.drive.targetPosition
target_ke = joint_desc.drive.stiffness
target_kd = joint_desc.drive.damping
joint_params["target_vel"] = target_vel
joint_params["target_pos"] = target_pos
joint_params["target_ke"] = target_ke
joint_params["target_kd"] = target_kd
joint_params["effort_limit"] = joint_desc.drive.forceLimit
joint_params["actuator_mode"] = JointTargetMode.from_gains(
target_ke, target_kd, force_position_velocity_actuation, has_drive=True
)
else:
joint_params["actuator_mode"] = JointTargetMode.NONE
# Read initial joint state BEFORE creating/overwriting USD attributes
initial_position = None
initial_velocity = None
dof_type = "linear" if key == UsdPhysics.ObjectType.PrismaticJoint else "angular"
# Resolve initial joint state from schema resolver
if dof_type == "angular":
initial_position = R.get_value(
joint_prim, PrimType.JOINT, "angular_position", default=None, verbose=verbose
)
initial_velocity = R.get_value(
joint_prim, PrimType.JOINT, "angular_velocity", default=None, verbose=verbose
)
else: # linear
initial_position = R.get_value(
joint_prim, PrimType.JOINT, "linear_position", default=None, verbose=verbose
)
initial_velocity = R.get_value(
joint_prim, PrimType.JOINT, "linear_velocity", default=None, verbose=verbose
)
if key == UsdPhysics.ObjectType.PrismaticJoint:
joint_index = builder.add_joint_prismatic(**joint_params)
else:
if joint_desc.drive.enabled:
joint_params["target_pos"] *= DegreesToRadian
joint_params["target_vel"] *= DegreesToRadian
joint_params["target_kd"] /= DegreesToRadian / joint_drive_gains_scaling
joint_params["target_ke"] /= DegreesToRadian / joint_drive_gains_scaling
joint_params["limit_lower"] *= DegreesToRadian
joint_params["limit_upper"] *= DegreesToRadian
joint_params["limit_ke"] /= DegreesToRadian
joint_params["limit_kd"] /= DegreesToRadian
if joint_params["velocity_limit"] is not None:
joint_params["velocity_limit"] *= DegreesToRadian
joint_index = builder.add_joint_revolute(**joint_params)
elif key == UsdPhysics.ObjectType.SphericalJoint:
joint_index = builder.add_joint_ball(**joint_params)
elif key == UsdPhysics.ObjectType.D6Joint:
linear_axes = []
angular_axes = []
num_dofs = 0
# Store initial state for D6 joints
d6_initial_positions = {}
d6_initial_velocities = {}
# Track which axes were added as DOFs (in order)
d6_dof_axes = []
# print(joint_desc.jointLimits, joint_desc.jointDrives)
# print(joint_desc.body0)
# print(joint_desc.body1)
# print(joint_desc.jointLimits)
# print("Limits")
# for limit in joint_desc.jointLimits:
# print("joint_path :", joint_path, limit.first, limit.second.lower, limit.second.upper)
# print("Drives")
# for drive in joint_desc.jointDrives:
# print("joint_path :", joint_path, drive.first, drive.second.targetPosition, drive.second.targetVelocity)
for limit in joint_desc.jointLimits:
dof = limit.first
if limit.second.enabled:
limit_lower = limit.second.lower
limit_upper = limit.second.upper
else:
limit_lower = builder.default_joint_cfg.limit_lower
limit_upper = builder.default_joint_cfg.limit_upper
free_axis = limit_lower < limit_upper
def define_joint_targets(dof, joint_desc):
target_pos = 0.0 # TODO: parse target from state:*:physics:appliedForce usd attribute when no drive is present
target_vel = 0.0
target_ke = 0.0
target_kd = 0.0
effort_limit = np.inf
has_drive = False
for drive in joint_desc.jointDrives:
if drive.first != dof:
continue
if drive.second.enabled:
has_drive = True
target_vel = drive.second.targetVelocity
target_pos = drive.second.targetPosition
target_ke = drive.second.stiffness
target_kd = drive.second.damping
effort_limit = drive.second.forceLimit
actuator_mode = JointTargetMode.from_gains(
target_ke, target_kd, force_position_velocity_actuation, has_drive=has_drive
)
return target_pos, target_vel, target_ke, target_kd, effort_limit, actuator_mode
target_pos, target_vel, target_ke, target_kd, effort_limit, actuator_mode = define_joint_targets(
dof, joint_desc
)
_trans_axes = {
UsdPhysics.JointDOF.TransX: (1.0, 0.0, 0.0),
UsdPhysics.JointDOF.TransY: (0.0, 1.0, 0.0),
UsdPhysics.JointDOF.TransZ: (0.0, 0.0, 1.0),
}
_rot_axes = {
UsdPhysics.JointDOF.RotX: (1.0, 0.0, 0.0),
UsdPhysics.JointDOF.RotY: (0.0, 1.0, 0.0),
UsdPhysics.JointDOF.RotZ: (0.0, 0.0, 1.0),
}
_rot_names = {
UsdPhysics.JointDOF.RotX: "rotX",
UsdPhysics.JointDOF.RotY: "rotY",
UsdPhysics.JointDOF.RotZ: "rotZ",
}
if free_axis and dof in _trans_axes:
# Per-axis translation names: transX/transY/transZ
trans_name = {
UsdPhysics.JointDOF.TransX: "transX",
UsdPhysics.JointDOF.TransY: "transY",
UsdPhysics.JointDOF.TransZ: "transZ",
}[dof]
# Store initial state for this axis
d6_initial_positions[trans_name] = R.get_value(
joint_prim,
PrimType.JOINT,
f"{trans_name}_position",
default=None,
verbose=verbose,
)
d6_initial_velocities[trans_name] = R.get_value(
joint_prim,
PrimType.JOINT,
f"{trans_name}_velocity",
default=None,
verbose=verbose,
)
current_joint_limit_ke = R.get_value(
joint_prim,
prim_type=PrimType.JOINT,
key=f"limit_{trans_name}_ke",
default=default_joint_limit_ke,
verbose=verbose,
)
current_joint_limit_kd = R.get_value(
joint_prim,
prim_type=PrimType.JOINT,
key=f"limit_{trans_name}_kd",
default=default_joint_limit_kd,
verbose=verbose,
)
linear_axes.append(
ModelBuilder.JointDofConfig(
axis=_trans_axes[dof],
limit_lower=limit_lower,
limit_upper=limit_upper,
limit_ke=current_joint_limit_ke,
limit_kd=current_joint_limit_kd,
target_pos=target_pos,
target_vel=target_vel,
target_ke=target_ke,
target_kd=target_kd,
armature=joint_armature,
effort_limit=effort_limit,
velocity_limit=joint_velocity_limit
if joint_velocity_limit is not None
else default_joint_velocity_limit,
friction=joint_friction,
actuator_mode=actuator_mode,
)
)
# Track that this axis was added as a DOF
d6_dof_axes.append(trans_name)
elif free_axis and dof in _rot_axes:
# Resolve per-axis rotational gains
rot_name = _rot_names[dof]
# Store initial state for this axis
d6_initial_positions[rot_name] = R.get_value(
joint_prim,
PrimType.JOINT,
f"{rot_name}_position",
default=None,
verbose=verbose,
)
d6_initial_velocities[rot_name] = R.get_value(
joint_prim,
PrimType.JOINT,
f"{rot_name}_velocity",
default=None,
verbose=verbose,
)
current_joint_limit_ke = R.get_value(
joint_prim,
prim_type=PrimType.JOINT,
key=f"limit_{rot_name}_ke",
default=default_joint_limit_ke * DegreesToRadian,
verbose=verbose,
)
current_joint_limit_kd = R.get_value(
joint_prim,
prim_type=PrimType.JOINT,
key=f"limit_{rot_name}_kd",
default=default_joint_limit_kd * DegreesToRadian,
verbose=verbose,
)
angular_axes.append(
ModelBuilder.JointDofConfig(
axis=_rot_axes[dof],
limit_lower=limit_lower * DegreesToRadian,
limit_upper=limit_upper * DegreesToRadian,
limit_ke=current_joint_limit_ke / DegreesToRadian,
limit_kd=current_joint_limit_kd / DegreesToRadian,
target_pos=target_pos * DegreesToRadian,
target_vel=target_vel * DegreesToRadian,
target_ke=target_ke / DegreesToRadian / joint_drive_gains_scaling,
target_kd=target_kd / DegreesToRadian / joint_drive_gains_scaling,
armature=joint_armature,
effort_limit=effort_limit,
velocity_limit=joint_velocity_limit * DegreesToRadian
if joint_velocity_limit is not None
else default_joint_velocity_limit,
friction=joint_friction,
actuator_mode=actuator_mode,
)
)
# Track that this axis was added as a DOF
d6_dof_axes.append(rot_name)
num_dofs += 1
joint_index = builder.add_joint_d6(**joint_params, linear_axes=linear_axes, angular_axes=angular_axes)
elif key == UsdPhysics.ObjectType.DistanceJoint:
if joint_desc.limit.enabled and joint_desc.minEnabled:
min_dist = joint_desc.limit.lower
else:
min_dist = -1.0 # no limit
if joint_desc.limit.enabled and joint_desc.maxEnabled:
max_dist = joint_desc.limit.upper
else:
max_dist = -1.0
joint_index = builder.add_joint_distance(**joint_params, min_distance=min_dist, max_distance=max_dist)
else:
raise NotImplementedError(f"Unsupported joint type {key}")
if joint_index is None:
raise ValueError(f"Failed to add joint {joint_path}")
# map the joint path to the index at insertion time
path_joint_map[joint_path] = joint_index
# Apply saved initial joint state after joint creation
if key in (UsdPhysics.ObjectType.RevoluteJoint, UsdPhysics.ObjectType.PrismaticJoint):
if initial_position is not None:
q_start = builder.joint_q_start[joint_index]
if key == UsdPhysics.ObjectType.RevoluteJoint:
builder.joint_q[q_start] = initial_position * DegreesToRadian
else:
builder.joint_q[q_start] = initial_position
if verbose:
joint_type_str = "revolute" if key == UsdPhysics.ObjectType.RevoluteJoint else "prismatic"
print(
f"Set {joint_type_str} joint {joint_index} position to {initial_position} ({'rad' if key == UsdPhysics.ObjectType.RevoluteJoint else 'm'})"
)
if initial_velocity is not None:
qd_start = builder.joint_qd_start[joint_index]
if key == UsdPhysics.ObjectType.RevoluteJoint:
builder.joint_qd[qd_start] = initial_velocity # velocity is already in rad/s
else:
builder.joint_qd[qd_start] = initial_velocity
if verbose:
joint_type_str = "revolute" if key == UsdPhysics.ObjectType.RevoluteJoint else "prismatic"
print(f"Set {joint_type_str} joint {joint_index} velocity to {initial_velocity} rad/s")
elif key == UsdPhysics.ObjectType.D6Joint:
# Apply D6 joint initial state
q_start = builder.joint_q_start[joint_index]
qd_start = builder.joint_qd_start[joint_index]
# Get joint coordinate and DOF ranges
if joint_index + 1 < len(builder.joint_q_start):
q_end = builder.joint_q_start[joint_index + 1]
qd_end = builder.joint_qd_start[joint_index + 1]
else:
q_end = len(builder.joint_q)
qd_end = len(builder.joint_qd)
# Apply initial values for each axis that was actually added as a DOF
for dof_idx, axis_name in enumerate(d6_dof_axes):
if dof_idx >= (qd_end - qd_start):
break
is_rot = axis_name.startswith("rot")
pos = d6_initial_positions.get(axis_name)
vel = d6_initial_velocities.get(axis_name)
if pos is not None and q_start + dof_idx < q_end:
coord_val = pos * DegreesToRadian if is_rot else pos
builder.joint_q[q_start + dof_idx] = coord_val
if verbose:
print(f"Set D6 joint {joint_index} {axis_name} position to {pos} ({'deg' if is_rot else 'm'})")
if vel is not None and qd_start + dof_idx < qd_end:
vel_val = vel # D6 velocities are already in correct units
builder.joint_qd[qd_start + dof_idx] = vel_val
if verbose:
print(f"Set D6 joint {joint_index} {axis_name} velocity to {vel} rad/s")
return joint_index
# Looking for and parsing the attributes on PhysicsScene prims
scene_attributes = {}
physics_scene_prim = None
if UsdPhysics.ObjectType.Scene in ret_dict:
paths, scene_descs = ret_dict[UsdPhysics.ObjectType.Scene]
if len(paths) > 1 and verbose:
print("Only the first PhysicsScene is considered")
path, scene_desc = paths[0], scene_descs[0]
if verbose:
print("Found PhysicsScene:", path)
print("Gravity direction:", scene_desc.gravityDirection)
print("Gravity magnitude:", scene_desc.gravityMagnitude)
builder.gravity = -scene_desc.gravityMagnitude * linear_unit
# Storing Physics Scene attributes
physics_scene_prim = stage.GetPrimAtPath(path)
for a in physics_scene_prim.GetAttributes():
scene_attributes[a.GetName()] = a.Get()
# Parse custom attribute declarations from PhysicsScene prim
# This must happen before processing any other prims
declarations = usd.get_custom_attribute_declarations(physics_scene_prim)
for attr in declarations.values():
builder.add_custom_attribute(attr)
# Updating joint_drive_gains_scaling if set of the PhysicsScene
joint_drive_gains_scaling = usd.get_float(
physics_scene_prim, "newton:joint_drive_gains_scaling", joint_drive_gains_scaling
)
time_steps_per_second = R.get_value(
physics_scene_prim, prim_type=PrimType.SCENE, key="time_steps_per_second", default=1000, verbose=verbose
)
physics_dt = (1.0 / time_steps_per_second) if time_steps_per_second > 0 else 0.001
gravity_enabled = R.get_value(
physics_scene_prim, prim_type=PrimType.SCENE, key="gravity_enabled", default=True, verbose=verbose
)
if not gravity_enabled:
builder.gravity = 0.0
max_solver_iters = R.get_value(
physics_scene_prim, prim_type=PrimType.SCENE, key="max_solver_iterations", default=None, verbose=verbose
)
stage_up_axis = Axis.from_string(str(UsdGeom.GetStageUpAxis(stage)))
if apply_up_axis_from_stage:
builder.up_axis = stage_up_axis
axis_xform = wp.transform_identity()
if verbose:
print(f"Using stage up axis {stage_up_axis} as builder up axis")
else:
axis_xform = wp.transform(wp.vec3(0.0), quat_between_axes(stage_up_axis, builder.up_axis))
if verbose:
print(f"Rotating stage to align its up axis {stage_up_axis} with builder up axis {builder.up_axis}")
if override_root_xform and xform is None:
raise ValueError("override_root_xform=True requires xform to be set")
if xform is None:
incoming_world_xform = axis_xform
else:
incoming_world_xform = wp.transform(*xform) * axis_xform
if verbose:
print(
f"Scaling PD gains by (joint_drive_gains_scaling / DegreesToRadian) = {joint_drive_gains_scaling / DegreesToRadian}, default scale for joint_drive_gains_scaling=1 is 1.0/DegreesToRadian = {1.0 / DegreesToRadian}"
)
# Process custom attributes defined for different kinds of prim.
# Note that at this time we may have more custom attributes than before since they may have been
# declared on the PhysicsScene prim.
builder_custom_attr_shape: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.SHAPE]
)
builder_custom_attr_body: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.BODY]
)
builder_custom_attr_joint: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.JOINT, AttributeFrequency.JOINT_DOF, AttributeFrequency.JOINT_COORD]
)
builder_custom_attr_articulation: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency(
[AttributeFrequency.ARTICULATION]
)
if physics_scene_prim is not None:
# Collect schema-defined attributes from the scene prim for inspection (e.g., mjc:* attributes)
if collect_schema_attrs:
R.collect_prim_attrs(physics_scene_prim)
# Extract custom attributes for model (ONCE and WORLD frequency) from the PhysicsScene prim
# WORLD frequency attributes use index 0 here; they get remapped during add_world()
builder_custom_attr_model: list[ModelBuilder.CustomAttribute] = [
attr
for attr in builder.custom_attributes.values()
if attr.frequency in (AttributeFrequency.ONCE, AttributeFrequency.WORLD)
]
# Filter out MuJoCo attributes if parse_mujoco_options is False
if not parse_mujoco_options:
builder_custom_attr_model = [attr for attr in builder_custom_attr_model if attr.namespace != "mujoco"]
# Read custom attribute values from the PhysicsScene prim
scene_custom_attrs = usd.get_custom_attribute_values(
physics_scene_prim, builder_custom_attr_model, context={"builder": builder}
)
scene_attributes.update(scene_custom_attrs)
# Set values on builder's custom attributes
for key, value in scene_custom_attrs.items():
if key in builder.custom_attributes:
builder.custom_attributes[key].values[0] = value
joint_descriptions = {}
# stores physics spec for every RigidBody in the selected range
body_specs = {}
# set of prim paths of rigid bodies that are ignored
# (to avoid repeated regex evaluations)
ignored_body_paths = set()
material_specs = {}
# maps from articulation_id to list of body_ids
articulation_bodies = {}
# TODO: uniform interface for iterating
def data_for_key(physics_utils_results, key):
if key not in physics_utils_results:
return
if verbose:
print(physics_utils_results[key])
yield from zip(*physics_utils_results[key], strict=False)
# Setting up the default material
material_specs[""] = PhysicsMaterial()
def warn_invalid_desc(path, descriptor) -> bool:
if not descriptor.isValid:
warnings.warn(
f'Warning: Invalid {type(descriptor).__name__} descriptor for prim at path "{path}".',
stacklevel=2,
)
return True
return False
# Parsing physics materials from the stage
for sdf_path, desc in data_for_key(ret_dict, UsdPhysics.ObjectType.RigidBodyMaterial):
if warn_invalid_desc(sdf_path, desc):
continue
prim = stage.GetPrimAtPath(sdf_path)
material_specs[str(sdf_path)] = PhysicsMaterial(
staticFriction=desc.staticFriction,
dynamicFriction=desc.dynamicFriction,
restitution=desc.restitution,
torsionalFriction=R.get_value(
prim,
prim_type=PrimType.MATERIAL,
key="mu_torsional",
default=builder.default_shape_cfg.mu_torsional,
verbose=verbose,
),
rollingFriction=R.get_value(
prim,
prim_type=PrimType.MATERIAL,
key="mu_rolling",
default=builder.default_shape_cfg.mu_rolling,
verbose=verbose,
),
# Treat non-positive/unauthored material density as "use importer default".
# Authored collider/body MassAPI mass+inertia is handled later.
density=desc.density if desc.density > 0.0 else default_shape_density,
)
if UsdPhysics.ObjectType.RigidBody in ret_dict:
prim_paths, rigid_body_descs = ret_dict[UsdPhysics.ObjectType.RigidBody]
for prim_path, rigid_body_desc in zip(prim_paths, rigid_body_descs, strict=False):
if warn_invalid_desc(prim_path, rigid_body_desc):
continue
body_path = str(prim_path)
if any(re.match(p, body_path) for p in ignore_paths):
ignored_body_paths.add(body_path)
continue
body_specs[body_path] = rigid_body_desc
prim = stage.GetPrimAtPath(prim_path)
# Bodies with MassAPI that need ComputeMassProperties fallback (missing mass, inertia, or CoM).
bodies_requiring_mass_properties_fallback: set[str] = set()
if UsdPhysics.ObjectType.RigidBody in ret_dict:
prim_paths, rigid_body_descs = ret_dict[UsdPhysics.ObjectType.RigidBody]
for prim_path, rigid_body_desc in zip(prim_paths, rigid_body_descs, strict=False):
if warn_invalid_desc(prim_path, rigid_body_desc):
continue
body_path = str(prim_path)
if body_path in ignored_body_paths:
continue
prim = stage.GetPrimAtPath(prim_path)
if not prim.HasAPI(UsdPhysics.MassAPI):
continue
mass_api = UsdPhysics.MassAPI(prim)
has_authored_mass = mass_api.GetMassAttr().HasAuthoredValue()
has_authored_inertia = mass_api.GetDiagonalInertiaAttr().HasAuthoredValue()
has_authored_com = mass_api.GetCenterOfMassAttr().HasAuthoredValue()
if not (has_authored_mass and has_authored_inertia and has_authored_com):
bodies_requiring_mass_properties_fallback.add(body_path)
# Collect joint descriptions regardless of whether articulations are authored.
for key, value in ret_dict.items():
if key in {
UsdPhysics.ObjectType.FixedJoint,
UsdPhysics.ObjectType.RevoluteJoint,
UsdPhysics.ObjectType.PrismaticJoint,
UsdPhysics.ObjectType.SphericalJoint,
UsdPhysics.ObjectType.D6Joint,
UsdPhysics.ObjectType.DistanceJoint,
}:
paths, joint_specs = value
for path, joint_spec in zip(paths, joint_specs, strict=False):
joint_descriptions[str(path)] = joint_spec
# Track which joints have been processed during articulation parsing.
# This allows us to parse orphan joints (joints not included in any articulation)
# even when articulations are present in the USD.
processed_joints: set[str] = set()
# maps from articulation_id to bool indicating if self-collisions are enabled
articulation_has_self_collision = {}
if UsdPhysics.ObjectType.Articulation in ret_dict:
paths, articulation_descs = ret_dict[UsdPhysics.ObjectType.Articulation]
articulation_id = builder.articulation_count
parent_prim = None
body_data = {}
for path, desc in zip(paths, articulation_descs, strict=False):
if warn_invalid_desc(path, desc):
continue
articulation_path = str(path)
if any(re.match(p, articulation_path) for p in ignore_paths):
continue
articulation_prim = stage.GetPrimAtPath(path)
articulation_root_xform = usd.get_transform(articulation_prim, local=False, xform_cache=xform_cache)
root_joint_xform = (
incoming_world_xform if override_root_xform else incoming_world_xform * articulation_root_xform
)
# Collect engine-specific attributes for the articulation root on first encounter
if collect_schema_attrs:
R.collect_prim_attrs(articulation_prim)
# Also collect on the parent prim (e.g. Xform with PhysxArticulationAPI)
try:
parent_prim = articulation_prim.GetParent()
except Exception:
parent_prim = None
if parent_prim is not None and parent_prim.IsValid():
R.collect_prim_attrs(parent_prim)
# Extract custom attributes for articulation frequency from the articulation root prim
# (the one with PhysicsArticulationRootAPI, typically the articulation_prim itself or its parent)
articulation_custom_attrs = {}
# First check if articulation_prim itself has the PhysicsArticulationRootAPI
if articulation_prim.HasAPI(UsdPhysics.ArticulationRootAPI):
if verbose:
print(f"Extracting articulation custom attributes from {articulation_prim.GetPath()}")
articulation_custom_attrs = usd.get_custom_attribute_values(
articulation_prim, builder_custom_attr_articulation
)
# If not, check the parent prim
elif (
parent_prim is not None and parent_prim.IsValid() and parent_prim.HasAPI(UsdPhysics.ArticulationRootAPI)
):
if verbose:
print(f"Extracting articulation custom attributes from parent {parent_prim.GetPath()}")
articulation_custom_attrs = usd.get_custom_attribute_values(
parent_prim, builder_custom_attr_articulation
)
if verbose and articulation_custom_attrs:
print(f"Extracted articulation custom attributes: {articulation_custom_attrs}")
body_ids = {}
body_labels = []
current_body_id = 0
art_bodies = []
if verbose:
print(f"Bodies under articulation {path!s}:")
for p in desc.articulatedBodies:
if verbose:
print(f"\t{p!s}")
if p == Sdf.Path.emptyPath:
continue
key = str(p)
if key in ignored_body_paths:
continue
usd_prim = stage.GetPrimAtPath(p)
if collect_schema_attrs:
# Collect on each articulated body prim encountered
R.collect_prim_attrs(usd_prim)
if key in body_specs:
body_desc = body_specs[key]
desc_xform = wp.transform(body_desc.position, usd.value_to_warp(body_desc.rotation))
body_world = usd.get_transform(usd_prim, local=False, xform_cache=xform_cache)
if override_root_xform:
# Strip the articulation root's world-space pose and rebase at the user-specified xform.
body_in_root_frame = wp.transform_inverse(articulation_root_xform) * body_world
desired_world = incoming_world_xform * body_in_root_frame
else:
desired_world = incoming_world_xform * body_world
body_incoming_xform = desired_world * wp.transform_inverse(desc_xform)
art_root_for_visuals = articulation_root_xform if override_root_xform else None
if bodies_follow_joint_ordering:
# we just parse the body information without yet adding it to the builder
body_data[current_body_id] = parse_body(
body_desc,
stage.GetPrimAtPath(p),
incoming_xform=body_incoming_xform,
add_body_to_builder=False,
articulation_root_xform=art_root_for_visuals,
)
else:
# look up description and add body to builder
bid: int = parse_body( # pyright: ignore[reportAssignmentType]
body_desc,
stage.GetPrimAtPath(p),
incoming_xform=body_incoming_xform,
add_body_to_builder=True,
articulation_root_xform=art_root_for_visuals,
)
if bid >= 0:
art_bodies.append(bid)
# remove body spec once we inserted it
del body_specs[key]
body_ids[key] = current_body_id
body_labels.append(key)
current_body_id += 1
if len(body_ids) == 0:
# no bodies under the articulation or we ignored all of them
continue
# determine the joint graph for this articulation
joint_names: list[str] = []
joint_edges: list[tuple[int, int]] = []
# keys of joints that are excluded from the articulation (loop joints)
joint_excluded: set[str] = set()
for p in desc.articulatedJoints:
joint_path = str(p)
joint_desc = joint_descriptions[joint_path]
# it may be possible that a joint is filtered out in the middle of
# a chain of joints, which results in a disconnected graph
# we should raise an error in this case
if any(re.match(p, joint_path) for p in ignore_paths):
continue
if str(joint_desc.body0) in ignored_body_paths:
continue
if str(joint_desc.body1) in ignored_body_paths:
continue
parent_id, child_id = resolve_joint_parent_child(joint_desc, body_ids, get_transforms=False) # pyright: ignore[reportAssignmentType]
if joint_desc.excludeFromArticulation:
joint_excluded.add(joint_path)
else:
joint_edges.append((parent_id, child_id))
joint_names.append(joint_path)
articulation_joint_indices = []
if len(joint_edges) == 0:
# We have an articulation without joints, i.e. only free rigid bodies
# Use add_base_joint to honor floating, base_joint, and parent_body parameters
base_parent = parent_body
if bodies_follow_joint_ordering:
for i in body_ids.values():
child_body_id = add_body(**body_data[i])
# Compute parent_xform to preserve imported pose when attaching to parent_body
parent_xform = None
if base_parent != -1:
# When parent_body is specified, interpret xform parameter as parent-relative offset
# body_data[i]["xform"] = USD_local * incoming_world_xform
# We want parent_xform to position the child at this location relative to parent
# Use incoming_world_xform as the base parent-relative offset
parent_xform = incoming_world_xform
# If the USD body has a non-identity local transform, compose it with incoming_xform
# Note: incoming_world_xform already includes the child's USD local transform via body_incoming_xform
# So we can use body_data[i]["xform"] directly for the intended position
# But we need it relative to parent. Since parent's body_q may not reflect joint offsets,
# we interpret body_data[i]["xform"] as the intended parent-relative transform directly.
# For articulations without joints, incoming_world_xform IS the parent-relative offset.
parent_xform = incoming_world_xform
joint_id = builder._add_base_joint(
child_body_id,
floating=floating,
base_joint=base_joint,
parent=base_parent,
parent_xform=parent_xform,
)
# note the free joint's coordinates will be initialized by the body_q of the
# child body
builder._finalize_imported_articulation(
joint_indices=[joint_id],
parent_body=parent_body,
articulation_label=body_data[i]["label"],
custom_attributes=articulation_custom_attrs,
)
else:
for i, child_body_id in enumerate(art_bodies):
# Compute parent_xform to preserve imported pose when attaching to parent_body
parent_xform = None
if base_parent != -1:
# When parent_body is specified, interpret xform parameter as parent-relative offset
parent_xform = incoming_world_xform
joint_id = builder._add_base_joint(
child_body_id,
floating=floating,
base_joint=base_joint,
parent=base_parent,
parent_xform=parent_xform,
)
# note the free joint's coordinates will be initialized by the body_q of the
# child body
builder._finalize_imported_articulation(
joint_indices=[joint_id],
parent_body=parent_body,
articulation_label=body_labels[i],
custom_attributes=articulation_custom_attrs,
)
sorted_joints = []
else:
# we have an articulation with joints, we need to sort them topologically
if joint_ordering is not None:
if verbose:
print(f"Sorting joints using {joint_ordering} ordering...")
sorted_joints, reversed_joint_list = topological_sort_undirected(
joint_edges, use_dfs=joint_ordering == "dfs", ensure_single_root=True
)
if reversed_joint_list:
reversed_joint_paths = [joint_names[joint_id] for joint_id in reversed_joint_list]
reversed_joint_names = ", ".join(reversed_joint_paths)
raise ValueError(
f"Reversed joints are not supported: {reversed_joint_names}. Ensure that the joint parent body is defined as physics:body0 and the child is defined as physics:body1 in the joint prim."
)
if verbose:
print("Joint ordering:", sorted_joints)
else:
# we keep the original order of the joints
sorted_joints = np.arange(len(joint_names))
if len(sorted_joints) > 0:
# insert the bodies in the order of the joints
if bodies_follow_joint_ordering:
inserted_bodies = set()
for jid in sorted_joints:
parent, child = joint_edges[jid]
if parent >= 0 and parent not in inserted_bodies:
b = add_body(**body_data[parent])
inserted_bodies.add(parent)
art_bodies.append(b)
path_body_map[body_data[parent]["label"]] = b
if child >= 0 and child not in inserted_bodies:
b = add_body(**body_data[child])
inserted_bodies.add(child)
art_bodies.append(b)
path_body_map[body_data[child]["label"]] = b
first_joint_parent = joint_edges[sorted_joints[0]][0]
if first_joint_parent != -1:
# the mechanism is floating since there is no joint connecting it to the world
# we explicitly add a joint connecting the first body in the articulation to the world
# (or to parent_body if specified) to make sure generalized-coordinate solvers can simulate it
base_parent = parent_body
if bodies_follow_joint_ordering:
child_body = body_data[first_joint_parent]
child_body_id = path_body_map[child_body["label"]]
else:
child_body_id = art_bodies[first_joint_parent]
# Compute parent_xform to preserve imported pose when attaching to parent_body
parent_xform = None
if base_parent != -1:
# When parent_body is specified, use incoming_world_xform as parent-relative offset
parent_xform = incoming_world_xform
base_joint_id = builder._add_base_joint(
child_body_id,
floating=floating,
base_joint=base_joint,
parent=base_parent,
parent_xform=parent_xform,
)
articulation_joint_indices.append(base_joint_id)
# insert the remaining joints in topological order
for joint_id, i in enumerate(sorted_joints):
if joint_id == 0 and first_joint_parent == -1:
# The root joint connects to the world (parent_id=-1).
# If base_joint or floating is specified, override the USD's root joint.
if base_joint is not None or floating is not None:
# Get the child body of the root joint
root_joint_child = joint_edges[sorted_joints[0]][1]
if bodies_follow_joint_ordering:
child_body = body_data[root_joint_child]
child_body_id = path_body_map[child_body["label"]]
else:
child_body_id = art_bodies[root_joint_child]
base_parent = parent_body
# Compute parent_xform to preserve imported pose
parent_xform = None
if base_parent != -1:
# When parent_body is specified, use incoming_world_xform as parent-relative offset
parent_xform = incoming_world_xform
else:
# body_q is already in world space, use it directly
parent_xform = builder.body_q[child_body_id]
base_joint_id = builder._add_base_joint(
child_body_id,
floating=floating,
base_joint=base_joint,
parent=base_parent,
parent_xform=parent_xform,
)
articulation_joint_indices.append(base_joint_id)
continue # Skip parsing the USD's root joint
# When body0 maps to world the physics API may resolve
# localPose0 into world space (baking the non-body prim's
# transform). JointDesc.body0 returns "" for non-rigid
# targets, so we attempt to look up the prim directly.
root_joint_desc = joint_descriptions[joint_names[i]]
b0 = str(root_joint_desc.body0)
b1 = str(root_joint_desc.body1)
# Determine the world-facing side from this articulation's body set.
# path_body_map includes previously imported articulations, so using
# it here can misidentify the world-side path for the current root
# joint when b0 references an external rigid body.
if b0 not in body_ids:
world_body_path = b0
elif b1 not in body_ids:
world_body_path = b1
else:
# Defensive fallback; root joints should have exactly one side
# outside the articulation.
world_body_path = b0
world_body_prim = stage.GetPrimAtPath(world_body_path) if world_body_path else None
if world_body_prim is not None and world_body_prim.IsValid():
world_body_xform = usd.get_transform(world_body_prim, local=False, xform_cache=xform_cache)
else:
# body0/body1 can resolve to world with an empty path (""),
# leaving no world-side prim to query.
# If the authored world-side local pose is identity, recover
# the missing world-side frame from the resolved child body
# pose and local poses so root-joint FK stays consistent with
# imported body_q.
# If the world-side local pose is non-identity, keep the
# previous identity fallback: USD often bakes non-rigid world
# anchors directly into localPose0/localPose1 in this case.
_, child_local_id, parent_tf, child_tf = resolve_joint_parent_child( # pyright: ignore[reportAssignmentType]
root_joint_desc,
body_ids,
get_transforms=True,
)
assert parent_tf is not None and child_tf is not None
identity_tf = wp.transform_identity()
parent_pos = np.array(parent_tf.p, dtype=float)
parent_quat = np.array(parent_tf.q, dtype=float)
identity_pos = np.array(identity_tf.p, dtype=float)
identity_quat = np.array(identity_tf.q, dtype=float)
parent_pos_is_identity = np.allclose(parent_pos, identity_pos, atol=1e-6)
# q and -q represent the same rotation
parent_rot_is_identity = abs(np.dot(parent_quat, identity_quat)) > 1.0 - 1e-6
if (
parent_pos_is_identity
and parent_rot_is_identity
and 0 <= child_local_id < len(body_labels)
):
child_path = body_labels[child_local_id]
child_prim = stage.GetPrimAtPath(child_path)
else:
child_prim = None
if child_prim is not None and child_prim.IsValid():
child_world_xform = usd.get_transform(child_prim, local=False, xform_cache=xform_cache)
world_body_xform = child_world_xform * child_tf * wp.transform_inverse(parent_tf)
else:
world_body_xform = wp.transform_identity()
root_frame_xform = (
wp.transform_inverse(articulation_root_xform)
if override_root_xform
else wp.transform_identity()
)
root_incoming_xform = incoming_world_xform * root_frame_xform * world_body_xform
joint = parse_joint(
joint_descriptions[joint_names[i]],
incoming_xform=root_incoming_xform,
)
else:
joint = parse_joint(
joint_descriptions[joint_names[i]],
)
if joint is not None:
articulation_joint_indices.append(joint)
processed_joints.add(joint_names[i])
# insert loop joints
for joint_path in joint_excluded:
parent_id, _ = resolve_joint_parent_child(
joint_descriptions[joint_path], path_body_map, get_transforms=False
)
if parent_id == -1:
joint = parse_joint(
joint_descriptions[joint_path],
incoming_xform=root_joint_xform,
)
else:
# localPose0 is already in the parent body's local frame;
# body positions were correctly set during body parsing above.
joint = parse_joint(
joint_descriptions[joint_path],
)
if joint is not None:
processed_joints.add(joint_path)
# Create the articulation from all collected joints
if articulation_joint_indices:
builder._finalize_imported_articulation(
joint_indices=articulation_joint_indices,
parent_body=parent_body,
articulation_label=articulation_path,
custom_attributes=articulation_custom_attrs,
)
articulation_bodies[articulation_id] = art_bodies
articulation_has_self_collision[articulation_id] = bool(
R.get_value(
articulation_prim,
prim_type=PrimType.ARTICULATION,
key="self_collision_enabled",
default=enable_self_collisions,
verbose=verbose,
)
)
articulation_id += 1
no_articulations = UsdPhysics.ObjectType.Articulation not in ret_dict
has_joints = any(
(
not (only_load_enabled_joints and not joint_desc.jointEnabled)
and not any(re.match(p, joint_path) for p in ignore_paths)
and str(joint_desc.body0) not in ignored_body_paths
and str(joint_desc.body1) not in ignored_body_paths
)
for joint_path, joint_desc in joint_descriptions.items()
)
# insert remaining bodies that were not part of any articulation so far
# (joints for these bodies will be added later by _add_base_joints_to_floating_bodies)
for path, rigid_body_desc in body_specs.items():
key = str(path)
body_id: int = parse_body( # pyright: ignore[reportAssignmentType]
rigid_body_desc,
stage.GetPrimAtPath(path),
incoming_xform=incoming_world_xform,
add_body_to_builder=True,
)
# Parse orphan joints: joints that exist in the USD but were not included in any articulation.
# This can happen when:
# 1. No articulations are defined in the USD (no_articulations == True)
# 2. A joint connects bodies that are not under any PhysicsArticulationRootAPI
orphan_joints = []
for joint_path, joint_desc in joint_descriptions.items():
# Skip joints that were already processed as part of an articulation
if joint_path in processed_joints:
continue
# Skip disabled joints if only_load_enabled_joints is True
if only_load_enabled_joints and not joint_desc.jointEnabled:
continue
if any(re.match(p, joint_path) for p in ignore_paths):
continue
if str(joint_desc.body0) in ignored_body_paths or str(joint_desc.body1) in ignored_body_paths:
continue
# Skip body-to-world joints (where one body is empty/world) only when
# FREE joints will be auto-inserted for remaining bodies — but always
# keep body-to-world FIXED joints so the body is properly welded to
# world instead of receiving an incorrect FREE base joint.
body0_path = str(joint_desc.body0)
body1_path = str(joint_desc.body1)
is_body_to_world = body0_path in ("", "/") or body1_path in ("", "/")
is_fixed_joint = joint_desc.type == UsdPhysics.ObjectType.FixedJoint
free_joints_auto_inserted = not (no_articulations and has_joints)
if is_body_to_world and free_joints_auto_inserted and not is_fixed_joint:
continue
try:
joint_index = parse_joint(joint_desc, incoming_xform=incoming_world_xform)
# Handle body-to-world FIXED joints separately to ensure proper welding.
# Creates an articulation for the body-to-world FIXED joint (consistent with MuJoCo approach)
if joint_index is not None and is_body_to_world and is_fixed_joint:
child_body = builder.joint_child[joint_index]
builder.add_articulation([joint_index], label=builder.body_label[child_body])
else:
orphan_joints.append(joint_path)
except ValueError as exc:
if verbose:
print(f"Skipping joint {joint_path}: {exc}")
if len(orphan_joints) > 0:
if no_articulations:
warn_str = (
f"No articulation was found but {len(orphan_joints)} joints were parsed: [{', '.join(orphan_joints)}]. "
)
warn_str += (
"Make sure your USD asset includes an articulation root prim with the PhysicsArticulationRootAPI.\n"
)
else:
warn_str = (
f"{len(orphan_joints)} joints were not included in any articulation and were parsed as orphan joints: "
f"[{', '.join(orphan_joints)}]. "
)
warn_str += (
"This can happen when a joint connects bodies that are not under any PhysicsArticulationRootAPI.\n"
)
warn_str += "If you want to proceed with these orphan joints, make sure to call ModelBuilder.finalize(skip_validation_joints=True) "
warn_str += "to avoid raising a ValueError. Note that not all solvers will support such a configuration."
warnings.warn(warn_str, stacklevel=2)
def _build_mass_info_from_authored_properties(
prim: Usd.Prim,
local_pos,
local_rot,
shape_geo_type: int,
shape_scale: wp.vec3,
shape_src: Mesh | None,
shape_axis=None,
):
"""Build unit-density collider mass information from authored collider MassAPI properties.
This helper is used for rigid-body fallback mass aggregation via
``UsdPhysics.RigidBodyAPI.ComputeMassProperties``. When a collider prim has authored
``MassAPI`` mass and diagonal inertia, we convert those values into a
``RigidBodyAPI.MassInformation`` payload that represents unit-density collider properties.
"""
if not prim.HasAPI(UsdPhysics.MassAPI):
return None
mass_api = UsdPhysics.MassAPI(prim)
mass_attr = mass_api.GetMassAttr()
diag_attr = mass_api.GetDiagonalInertiaAttr()
if not (mass_attr.HasAuthoredValue() and diag_attr.HasAuthoredValue()):
return None
mass = float(mass_attr.Get())
if mass <= 0.0:
warnings.warn(
f"Skipping collider {prim.GetPath()}: authored MassAPI mass must be > 0 to derive volume and density.",
stacklevel=2,
)
return None
shape_volume, _, _ = compute_inertia_shape(shape_geo_type, shape_scale, shape_src, density=1.0)
if shape_volume <= 0.0:
warnings.warn(
f"Skipping collider {prim.GetPath()}: unable to derive positive collider volume from authored shape parameters.",
stacklevel=2,
)
return None
density = mass / shape_volume
if density <= 0.0:
warnings.warn(
f"Skipping collider {prim.GetPath()}: derived density from authored mass is non-positive.",
stacklevel=2,
)
return None
diag = np.array(diag_attr.Get(), dtype=np.float32)
if np.any(diag < 0.0):
warnings.warn(
f"Skipping collider {prim.GetPath()}: authored diagonal inertia contains negative values.",
stacklevel=2,
)
return None
inertia_diag_unit = diag / density
if mass_api.GetPrincipalAxesAttr().HasAuthoredValue():
principal_axes = mass_api.GetPrincipalAxesAttr().Get()
else:
principal_axes = Gf.Quatf(1.0, 0.0, 0.0, 0.0)
if mass_api.GetCenterOfMassAttr().HasAuthoredValue():
center_of_mass = mass_api.GetCenterOfMassAttr().Get()
else:
center_of_mass = Gf.Vec3f(0.0, 0.0, 0.0)
i_rot = usd.value_to_warp(principal_axes)
rot = np.array(wp.quat_to_matrix(i_rot), dtype=np.float32).reshape(3, 3)
inertia_full_unit = rot @ np.diag(inertia_diag_unit) @ rot.T
mass_info = UsdPhysics.RigidBodyAPI.MassInformation()
mass_info.volume = float(shape_volume)
mass_info.centerOfMass = center_of_mass
mass_info.localPos = Gf.Vec3f(*local_pos)
mass_info.localRot = _resolve_mass_info_local_rotation(local_rot, shape_geo_type, shape_axis)
mass_info.inertia = Gf.Matrix3f(*inertia_full_unit.flatten().tolist())
return mass_info
def _resolve_mass_info_local_rotation(local_rot, shape_geo_type: int, shape_axis):
"""Match collider mass frame rotation with shape axis correction used by shape insertion."""
if shape_geo_type not in {GeoType.CAPSULE, GeoType.CYLINDER, GeoType.CONE} or shape_axis is None:
return local_rot
axis = usd_axis_to_axis.get(shape_axis)
if axis is None:
axis_int_map = {
int(UsdPhysics.Axis.X): Axis.X,
int(UsdPhysics.Axis.Y): Axis.Y,
int(UsdPhysics.Axis.Z): Axis.Z,
}
axis = axis_int_map.get(int(shape_axis))
if axis is None or axis == Axis.Z:
return local_rot
local_rot_wp = usd.value_to_warp(local_rot)
corrected_rot = wp.mul(local_rot_wp, quat_between_axes(Axis.Z, axis))
return Gf.Quatf(
float(corrected_rot[3]),
float(corrected_rot[0]),
float(corrected_rot[1]),
float(corrected_rot[2]),
)
def _build_mass_info_from_shape_geometry(
prim: Usd.Prim,
local_pos,
local_rot,
shape_geo_type: int,
shape_scale: wp.vec3,
shape_src: Mesh | None,
shape_axis=None,
):
"""Build unit-density collider mass information from geometric shape parameters.
This fallback path derives collider volume, center of mass, and inertia from shape
geometry (box/sphere/capsule/cylinder/cone/mesh) when collider-authored MassAPI mass
properties are not available.
"""
shape_mass, shape_com, shape_inertia = compute_inertia_shape(
shape_geo_type, shape_scale, shape_src, density=1.0
)
if shape_mass <= 0.0:
warnings.warn(
f"Skipping collider {prim.GetPath()} in mass aggregation: unable to derive positive unit-density mass.",
stacklevel=2,
)
return None
shape_inertia_np = np.array(shape_inertia, dtype=np.float32).reshape(3, 3)
mass_info = UsdPhysics.RigidBodyAPI.MassInformation()
mass_info.volume = float(shape_mass)
mass_info.centerOfMass = Gf.Vec3f(*shape_com)
mass_info.localPos = Gf.Vec3f(*local_pos)
mass_info.localRot = _resolve_mass_info_local_rotation(local_rot, shape_geo_type, shape_axis)
mass_info.inertia = Gf.Matrix3f(*shape_inertia_np.flatten().tolist())
return mass_info
# parse shapes attached to the rigid bodies
path_collision_filters = set()
no_collision_shapes = set()
collision_group_ids = {}
rigid_body_mass_info_map = {}
expected_fallback_collider_paths: set[str] = set()
for key, value in ret_dict.items():
if key in {
UsdPhysics.ObjectType.CubeShape,
UsdPhysics.ObjectType.SphereShape,
UsdPhysics.ObjectType.CapsuleShape,
UsdPhysics.ObjectType.CylinderShape,
UsdPhysics.ObjectType.ConeShape,
UsdPhysics.ObjectType.MeshShape,
UsdPhysics.ObjectType.PlaneShape,
}:
paths, shape_specs = value
for xpath, shape_spec in zip(paths, shape_specs, strict=False):
if warn_invalid_desc(xpath, shape_spec):
continue
path = str(xpath)
if any(re.match(p, path) for p in ignore_paths):
continue
prim = stage.GetPrimAtPath(xpath)
if path in path_shape_map:
if verbose:
print(f"Shape at {path} already added, skipping.")
continue
body_path = str(shape_spec.rigidBody)
if verbose:
print(f"collision shape {prim.GetPath()} ({prim.GetTypeName()}), body = {body_path}")
body_id = path_body_map.get(body_path, -1)
scale = usd.get_scale(prim, local=False)
collision_group = builder.default_shape_cfg.collision_group
if len(shape_spec.collisionGroups) > 0:
cgroup_name = str(shape_spec.collisionGroups[0])
if cgroup_name not in collision_group_ids:
# Start from 1 to avoid collision_group = 0 (which means "no collisions")
collision_group_ids[cgroup_name] = len(collision_group_ids) + 1
collision_group = collision_group_ids[cgroup_name]
material = material_specs[""]
has_shape_material = len(shape_spec.materials) >= 1
if has_shape_material:
if len(shape_spec.materials) > 1 and verbose:
print(f"Warning: More than one material found on shape at '{path}'.\nUsing only the first one.")
material = material_specs[str(shape_spec.materials[0])]
if verbose:
print(
f"\tMaterial of '{path}':\tfriction: {material.dynamicFriction},\ttorsional friction: {material.torsionalFriction},\trolling friction: {material.rollingFriction},\trestitution: {material.restitution},\tdensity: {material.density}"
)
elif verbose:
print(f"No material found for shape at '{path}'.")
# Non-MassAPI body mass accumulation in ModelBuilder uses shape cfg density.
# Use per-shape physics material density when present; otherwise use default density.
if has_shape_material:
shape_density = material.density
else:
shape_density = default_shape_density
prim_and_scene = (prim, physics_scene_prim)
local_xform = wp.transform(shape_spec.localPos, usd.value_to_warp(shape_spec.localRot))
if body_id == -1:
shape_xform = incoming_world_xform * local_xform
else:
shape_xform = local_xform
# Extract custom attributes for this shape
shape_custom_attrs = usd.get_custom_attribute_values(
prim, builder_custom_attr_shape, context={"builder": builder}
)
if collect_schema_attrs:
R.collect_prim_attrs(prim)
margin_val = R.get_value(
prim,
prim_type=PrimType.SHAPE,
key="margin",
default=builder.default_shape_cfg.margin,
verbose=verbose,
)
gap_val = R.get_value(
prim,
prim_type=PrimType.SHAPE,
key="gap",
verbose=verbose,
)
if gap_val == float("-inf"):
gap_val = builder.default_shape_cfg.gap
has_body_visual_shapes = load_visual_shapes and body_id in bodies_with_visual_shapes
collider_has_visual_material = (
key == UsdPhysics.ObjectType.MeshShape
and _has_visual_material_properties(_get_material_props_cached(prim))
)
# Explicit hide_collision_shapes overrides material-based visibility:
# if the body already has visual shapes, hide its colliders unconditionally.
hide_collider_for_body = hide_collision_shapes and has_body_visual_shapes
show_collider_by_policy = should_show_collider(
force_show_colliders,
has_visual_shapes=has_body_visual_shapes,
)
collider_is_visible = (
show_collider_by_policy or collider_has_visual_material
) and not hide_collider_for_body
shape_ke = R.get_value(
prim,
prim_type=PrimType.SHAPE,
key="ke",
verbose=verbose,
)
if shape_ke is None:
shape_ke = builder.default_shape_cfg.ke
shape_kd = R.get_value(
prim,
prim_type=PrimType.SHAPE,
key="kd",
verbose=verbose,
)
if shape_kd is None:
shape_kd = builder.default_shape_cfg.kd
shape_params = {
"body": body_id,
"xform": shape_xform,
"cfg": ModelBuilder.ShapeConfig(
ke=shape_ke,
kd=shape_kd,
kf=usd.get_float_with_fallback(
prim_and_scene, "newton:contact_kf", builder.default_shape_cfg.kf
),
ka=usd.get_float_with_fallback(
prim_and_scene, "newton:contact_ka", builder.default_shape_cfg.ka
),
margin=margin_val,
gap=gap_val,
mu=material.dynamicFriction,
restitution=material.restitution,
mu_torsional=material.torsionalFriction,
mu_rolling=material.rollingFriction,
density=shape_density,
collision_group=collision_group,
is_visible=collider_is_visible,
),
"label": path,
"custom_attributes": shape_custom_attrs,
}
# print(path, shape_params)
if key == UsdPhysics.ObjectType.CubeShape:
hx, hy, hz = shape_spec.halfExtents
shape_id = builder.add_shape_box(
**shape_params,
hx=hx,
hy=hy,
hz=hz,
)
elif key == UsdPhysics.ObjectType.SphereShape:
if not (scale[0] == scale[1] == scale[2]):
print("Warning: Non-uniform scaling of spheres is not supported.")
radius = shape_spec.radius
shape_id = builder.add_shape_sphere(
**shape_params,
radius=radius,
)
elif key == UsdPhysics.ObjectType.CapsuleShape:
# Apply axis rotation to transform
axis = int(shape_spec.axis)
shape_params["xform"] = wp.transform(
shape_params["xform"].p, shape_params["xform"].q * quat_between_axes(Axis.Z, axis)
)
radius = shape_spec.radius
half_height = shape_spec.halfHeight
shape_id = builder.add_shape_capsule(
**shape_params,
radius=radius,
half_height=half_height,
)
elif key == UsdPhysics.ObjectType.CylinderShape:
# Apply axis rotation to transform
axis = int(shape_spec.axis)
shape_params["xform"] = wp.transform(
shape_params["xform"].p, shape_params["xform"].q * quat_between_axes(Axis.Z, axis)
)
radius = shape_spec.radius
half_height = shape_spec.halfHeight
shape_id = builder.add_shape_cylinder(
**shape_params,
radius=radius,
half_height=half_height,
)
elif key == UsdPhysics.ObjectType.ConeShape:
# Apply axis rotation to transform
axis = int(shape_spec.axis)
shape_params["xform"] = wp.transform(
shape_params["xform"].p, shape_params["xform"].q * quat_between_axes(Axis.Z, axis)
)
radius = shape_spec.radius
half_height = shape_spec.halfHeight
shape_id = builder.add_shape_cone(
**shape_params,
radius=radius,
half_height=half_height,
)
elif key == UsdPhysics.ObjectType.MeshShape:
# Resolve mesh hull vertex limit from schema with fallback to parameter
if collider_is_visible:
# Visible colliders should render with the same visual material metadata
# as visual-only mesh imports.
mesh = _get_mesh_with_visual_material(prim, path_name=path)
else:
mesh = _get_mesh_cached(prim)
mesh.maxhullvert = R.get_value(
prim,
prim_type=PrimType.SHAPE,
key="max_hull_vertices",
default=mesh_maxhullvert,
verbose=verbose,
)
shape_id = builder.add_shape_mesh(
scale=wp.vec3(*shape_spec.meshScale),
mesh=mesh,
**shape_params,
)
if not skip_mesh_approximation:
approximation = usd.get_attribute(prim, "physics:approximation", None)
if approximation is not None:
remeshing_method = approximation_to_remeshing_method.get(approximation.lower(), None)
if remeshing_method is None:
if verbose:
print(
f"Warning: Unknown physics:approximation attribute '{approximation}' on shape at '{path}'."
)
else:
if remeshing_method not in remeshing_queue:
remeshing_queue[remeshing_method] = []
remeshing_queue[remeshing_method].append(shape_id)
elif key == UsdPhysics.ObjectType.PlaneShape:
# Warp uses +Z convention for planes
if shape_spec.axis != UsdPhysics.Axis.Z:
xform = shape_params["xform"]
axis_q = quat_between_axes(Axis.Z, usd_axis_to_axis[shape_spec.axis])
shape_params["xform"] = wp.transform(xform.p, xform.q * axis_q)
shape_id = builder.add_shape_plane(
**shape_params,
width=0.0,
length=0.0,
)
else:
raise NotImplementedError(f"Shape type {key} not supported yet")
path_shape_map[path] = shape_id
path_shape_scale[path] = scale
if body_path in bodies_requiring_mass_properties_fallback:
# Prepare collider mass information for ComputeMassProperties fallback path.
# Prefer authored collider MassAPI mass+diagonalInertia; otherwise derive
# unit-density mass information from shape geometry.
shape_geo_type = None
shape_scale = wp.vec3(1.0, 1.0, 1.0)
shape_src = None
if key == UsdPhysics.ObjectType.CubeShape:
shape_geo_type = GeoType.BOX
hx, hy, hz = shape_spec.halfExtents
shape_scale = wp.vec3(hx, hy, hz)
elif key == UsdPhysics.ObjectType.SphereShape:
shape_geo_type = GeoType.SPHERE
shape_scale = wp.vec3(shape_spec.radius, 0.0, 0.0)
elif key == UsdPhysics.ObjectType.CapsuleShape:
shape_geo_type = GeoType.CAPSULE
shape_scale = wp.vec3(shape_spec.radius, shape_spec.halfHeight, 0.0)
elif key == UsdPhysics.ObjectType.CylinderShape:
shape_geo_type = GeoType.CYLINDER
shape_scale = wp.vec3(shape_spec.radius, shape_spec.halfHeight, 0.0)
elif key == UsdPhysics.ObjectType.ConeShape:
shape_geo_type = GeoType.CONE
shape_scale = wp.vec3(shape_spec.radius, shape_spec.halfHeight, 0.0)
elif key == UsdPhysics.ObjectType.MeshShape:
shape_geo_type = GeoType.MESH
shape_scale = wp.vec3(*shape_spec.meshScale)
shape_src = _get_mesh_cached(prim)
if shape_geo_type is not None:
expected_fallback_collider_paths.add(path)
shape_axis = getattr(shape_spec, "axis", None)
mass_info = _build_mass_info_from_authored_properties(
prim,
shape_spec.localPos,
shape_spec.localRot,
shape_geo_type,
shape_scale,
shape_src,
shape_axis,
)
if mass_info is None:
mass_info = _build_mass_info_from_shape_geometry(
prim,
shape_spec.localPos,
shape_spec.localRot,
shape_geo_type,
shape_scale,
shape_src,
shape_axis,
)
if mass_info is not None:
rigid_body_mass_info_map[path] = mass_info
if prim.HasRelationship("physics:filteredPairs"):
other_paths = prim.GetRelationship("physics:filteredPairs").GetTargets()
for other_path in other_paths:
path_collision_filters.add((path, str(other_path)))
if not _is_enabled_collider(prim):
no_collision_shapes.add(shape_id)
builder.shape_flags[shape_id] &= ~ShapeFlags.COLLIDE_SHAPES
# approximate meshes
for remeshing_method, shape_ids in remeshing_queue.items():
builder.approximate_meshes(method=remeshing_method, shape_indices=shape_ids)
# apply collision filters now that we have added all shapes
for path1, path2 in path_collision_filters:
shape1 = path_shape_map[path1]
shape2 = path_shape_map[path2]
builder.add_shape_collision_filter_pair(shape1, shape2)
# apply collision filters to all shapes that have no collision
for shape_id in no_collision_shapes:
for other_shape_id in range(builder.shape_count):
if other_shape_id != shape_id:
builder.add_shape_collision_filter_pair(shape_id, other_shape_id)
# apply collision filters from articulations that have self collisions disabled
for art_id, bodies in articulation_bodies.items():
if not articulation_has_self_collision[art_id]:
for body1, body2 in itertools.combinations(bodies, 2):
for shape1 in builder.body_shapes[body1]:
for shape2 in builder.body_shapes[body2]:
builder.add_shape_collision_filter_pair(shape1, shape2)
# Load Gaussian splat prims that weren't already captured as children of rigid bodies.
if load_visual_shapes:
prims = iter(Usd.PrimRange(stage.GetPrimAtPath(root_path), Usd.TraverseInstanceProxies()))
for gaussian_prim in prims:
if str(gaussian_prim.GetPath()).startswith("/Prototypes/"):
continue
if gaussian_prim.HasAPI(UsdPhysics.RigidBodyAPI):
prims.PruneChildren()
continue
if str(gaussian_prim.GetTypeName()) != "ParticleField3DGaussianSplat":
continue
gaussian_path = str(gaussian_prim.GetPath())
if gaussian_path in path_shape_map:
continue
if any(re.match(p, gaussian_path) for p in ignore_paths):
continue
body_id = -1
prim_world_mat = _get_prim_world_mat(prim, None, incoming_world_xform)
g_pos, g_rot, g_scale = wp.transform_decompose(prim_world_mat)
gaussian = usd.get_gaussian(gaussian_prim)
shape_id = builder.add_shape_gaussian(
body_id,
gaussian=gaussian,
xform=wp.transform(g_pos, g_rot),
scale=g_scale,
cfg=visual_shape_cfg,
label=gaussian_path,
)
path_shape_map[gaussian_path] = shape_id
path_shape_scale[gaussian_path] = g_scale
if verbose:
print(f"Added Gaussian splat shape {gaussian_path} with id {shape_id}.")
def _zero_mass_information():
"""Create a reusable zero-contribution collider mass payload for callback fallback."""
mass_info = UsdPhysics.RigidBodyAPI.MassInformation()
mass_info.volume = 0.0
mass_info.centerOfMass = Gf.Vec3f(0.0)
mass_info.localPos = Gf.Vec3f(0.0)
mass_info.localRot = Gf.Quatf(1.0, 0.0, 0.0, 0.0)
mass_info.inertia = Gf.Matrix3f(0.0)
return mass_info
zero_mass_information = _zero_mass_information()
warned_missing_collider_mass_info: set[str] = set()
def _get_collision_mass_information(collider_prim: Usd.Prim):
"""MassInformation callback for ``ComputeMassProperties`` with one-time warning on misses."""
collider_path = str(collider_prim.GetPath())
is_expected_missing = (
collider_path in expected_fallback_collider_paths and collider_path not in rigid_body_mass_info_map
)
if is_expected_missing and collider_path not in warned_missing_collider_mass_info:
warnings.warn(
f"Skipping collider {collider_path} in mass aggregation: missing usable collider mass information.",
stacklevel=2,
)
warned_missing_collider_mass_info.add(collider_path)
return rigid_body_mass_info_map.get(collider_path, zero_mass_information)
# overwrite inertial properties of bodies that have PhysicsMassAPI schema applied
if UsdPhysics.ObjectType.RigidBody in ret_dict:
paths, rigid_body_descs = ret_dict[UsdPhysics.ObjectType.RigidBody]
for path, _rigid_body_desc in zip(paths, rigid_body_descs, strict=False):
prim = stage.GetPrimAtPath(path)
if not prim.HasAPI(UsdPhysics.MassAPI):
continue
body_path = str(path)
body_id = path_body_map.get(body_path, -1)
if body_id == -1:
continue
mass_api = UsdPhysics.MassAPI(prim)
has_authored_mass = mass_api.GetMassAttr().HasAuthoredValue()
has_authored_inertia = mass_api.GetDiagonalInertiaAttr().HasAuthoredValue()
has_authored_com = mass_api.GetCenterOfMassAttr().HasAuthoredValue()
# Compute baseline mass properties via mass computer when at least one property needs resolving.
if not (has_authored_mass and has_authored_inertia and has_authored_com):
rigid_body_api = UsdPhysics.RigidBodyAPI(prim)
cmp_mass, cmp_i_diag, cmp_com, cmp_principal_axes = rigid_body_api.ComputeMassProperties(
_get_collision_mass_information
)
if cmp_mass < 0.0:
# ComputeMassProperties failed to discover colliders (e.g. shapes
# created by schema resolvers are not real USD prims). Fall back to
# builder-accumulated mass properties from add_shape_*() calls.
cmp_mass = builder.body_mass[body_id]
cmp_com = builder.body_com[body_id]
# When the body has an authored density, rescale accumulated mass
# and inertia from the builder's default shape density to the
# body-level density (USD body density overrides per-shape density).
body_density_attr = mass_api.GetDensityAttr()
if (
body_density_attr.HasAuthoredValue()
and float(body_density_attr.Get()) > 0.0
and default_shape_density > 0.0
):
density_scale = float(body_density_attr.Get()) / default_shape_density
cmp_mass *= density_scale
builder.body_inertia[body_id] = wp.mat33(
np.array(builder.body_inertia[body_id]) * density_scale
)
cmp_i_diag = Gf.Vec3f(0.0, 0.0, 0.0)
cmp_principal_axes = Gf.Quatf(1.0, 0.0, 0.0, 0.0)
# Inertia: authored diagonal + principal axes take precedence over mass computer.
# When mass is authored but inertia is not, keep accumulated inertia
# (scaled to match authored mass below) instead of using mass computer
# inertia, which may already reflect the authored mass.
if has_authored_inertia:
i_diag_np = np.array(mass_api.GetDiagonalInertiaAttr().Get(), dtype=np.float32)
if np.any(i_diag_np < 0.0):
warnings.warn(
f"Body {body_path}: authored diagonal inertia contains negative values. "
"Falling back to mass-computer result.",
stacklevel=2,
)
has_authored_inertia = False
i_diag_np = np.array(cmp_i_diag, dtype=np.float32)
principal_axes = cmp_principal_axes
elif mass_api.GetPrincipalAxesAttr().HasAuthoredValue():
principal_axes = mass_api.GetPrincipalAxesAttr().Get()
else:
principal_axes = Gf.Quatf(1.0, 0.0, 0.0, 0.0)
elif not has_authored_mass:
i_diag_np = np.array(cmp_i_diag, dtype=np.float32)
principal_axes = cmp_principal_axes
else:
# Mass authored, inertia not: keep accumulated inertia and scale
# to match authored mass in the mass block below.
i_diag_np = None
if i_diag_np is not None and np.linalg.norm(i_diag_np) > 0.0:
i_rot = usd.value_to_warp(principal_axes)
rot = np.array(wp.quat_to_matrix(i_rot), dtype=np.float32).reshape(3, 3)
inertia = rot @ np.diag(i_diag_np) @ rot.T
builder.body_inertia[body_id] = wp.mat33(inertia)
if inertia.any():
builder.body_inv_inertia[body_id] = wp.inverse(wp.mat33(*inertia))
else:
builder.body_inv_inertia[body_id] = wp.mat33(0.0)
# Mass: authored value takes precedence over mass computer.
if has_authored_mass:
mass = float(mass_api.GetMassAttr().Get())
shape_accumulated_mass = builder.body_mass[body_id]
if not has_authored_inertia and mass_api.GetDensityAttr().HasAuthoredValue():
warnings.warn(
f"Body {body_path}: authored mass and density without authored diagonalInertia. "
f"Ignoring body-level density.",
stacklevel=2,
)
# When mass is authored but inertia is not, scale the accumulated
# inertia to be consistent with the authored mass.
if not has_authored_inertia and shape_accumulated_mass > 0.0 and mass > 0.0:
scale = mass / shape_accumulated_mass
builder.body_inertia[body_id] = wp.mat33(np.array(builder.body_inertia[body_id]) * scale)
builder.body_inv_inertia[body_id] = wp.inverse(builder.body_inertia[body_id])
else:
mass = cmp_mass
builder.body_mass[body_id] = mass
builder.body_inv_mass[body_id] = 1.0 / mass if mass > 0.0 else 0.0
# Center of mass: authored value takes precedence over mass computer.
if has_authored_com:
builder.body_com[body_id] = wp.vec3(*mass_api.GetCenterOfMassAttr().Get())
else:
builder.body_com[body_id] = wp.vec3(*cmp_com)
# Assign nonzero inertia if mass is nonzero to make sure the body can be simulated.
I_m = np.array(builder.body_inertia[body_id])
mass = builder.body_mass[body_id]
if I_m.max() == 0.0:
if mass > 0.0:
# Heuristic: assume a uniform density sphere with the given mass
# For a sphere: I = (2/5) * m * r^2
# Estimate radius from mass assuming reasonable density (e.g., water density ~1000 kg/m³)
# This gives r = (3*m/(4*π*p))^(1/3)
density = default_shape_density # kg/m^3
volume = mass / density
radius = (3.0 * volume / (4.0 * np.pi)) ** (1.0 / 3.0)
_, _, I_default = compute_inertia_sphere(density, radius)
# Apply parallel axis theorem if center of mass is offset
com = builder.body_com[body_id]
if np.linalg.norm(com) > 1e-6:
# I = I_cm + m * d² where d is distance from COM to body origin
d_squared = np.sum(com**2)
I_default += wp.mat33(mass * d_squared * np.eye(3, dtype=np.float32))
builder.body_inertia[body_id] = I_default
builder.body_inv_inertia[body_id] = wp.inverse(I_default)
if verbose:
print(
f"Applied default inertia matrix for body {body_path}: diagonal elements = [{I_default[0, 0]}, {I_default[1, 1]}, {I_default[2, 2]}]"
)
else:
warnings.warn(
f"Body {body_path} has zero mass and zero inertia despite having the MassAPI USD schema applied.",
stacklevel=2,
)
# add joints to floating bodies (bodies not connected as children to any joint)
if not (no_articulations and has_joints):
new_bodies = list(path_body_map.values())
if parent_body != -1:
# When parent_body is specified, manually add joints to floating bodies with correct parent
joint_children = set(builder.joint_child)
for body_id in new_bodies:
if body_id in joint_children:
continue # Already has a joint
if builder.body_mass[body_id] <= 0:
continue # Skip static bodies
# Compute parent_xform to preserve imported pose when attaching to parent_body
# When parent_body is specified, use incoming_world_xform as parent-relative offset
parent_xform = incoming_world_xform
joint_id = builder._add_base_joint(
body_id,
floating=floating,
base_joint=base_joint,
parent=parent_body,
parent_xform=parent_xform,
)
# Attach to parent's articulation
builder._finalize_imported_articulation(
joint_indices=[joint_id],
parent_body=parent_body,
articulation_label=None,
)
else:
builder._add_base_joints_to_floating_bodies(new_bodies, floating=floating, base_joint=base_joint)
# collapsing fixed joints to reduce the number of simulated bodies connected by fixed joints.
collapse_results = None
path_body_relative_transform = {}
if scene_attributes.get("newton:collapse_fixed_joints", collapse_fixed_joints):
collapse_results = builder.collapse_fixed_joints()
body_merged_parent = collapse_results["body_merged_parent"]
body_merged_transform = collapse_results["body_merged_transform"]
body_remap = collapse_results["body_remap"]
# remap body ids in articulation bodies
for art_id, bodies in articulation_bodies.items():
articulation_bodies[art_id] = [body_remap[b] for b in bodies if b in body_remap]
for path, body_id in path_body_map.items():
if body_id in body_remap:
new_id = body_remap[body_id]
elif body_id in body_merged_parent:
# this body has been merged with another body
new_id = body_remap[body_merged_parent[body_id]]
path_body_relative_transform[path] = body_merged_transform[body_id]
else:
# this body has not been merged
new_id = body_id
path_body_map[path] = new_id
# Joint indices may have shifted after collapsing fixed joints; refresh the joint path map accordingly.
path_joint_map = {label: idx for idx, label in enumerate(builder.joint_label)}
# Mimic constraints from PhysxMimicJointAPI (run after collapse so joint indices are final).
# PhysxMimicJointAPI is an instance-applied schema (e.g. PhysxMimicJointAPI:rotZ)
# that couples a follower joint to a leader (reference) joint with a gearing ratio.
# PhysX convention: jointPos + gearing * refJointPos + offset = 0
# Newton/URDF convention: joint0 = coef0 + coef1 * joint1
# Therefore: coef1 = -gearing, coef0 = -offset
for joint_path, joint_idx in path_joint_map.items():
joint_prim = stage.GetPrimAtPath(joint_path)
if not joint_prim or not joint_prim.IsValid():
continue
# Skip if NewtonMimicAPI is present — it takes precedence over PhysxMimicJointAPI.
if usd.has_applied_api_schema(joint_prim, "NewtonMimicAPI"):
continue
schemas_listop = joint_prim.GetMetadata("apiSchemas")
if not schemas_listop:
continue
all_schemas = (
list(schemas_listop.prependedItems)
+ list(schemas_listop.appendedItems)
+ list(schemas_listop.explicitItems)
)
for schema in all_schemas:
schema_str = str(schema)
if not schema_str.startswith("PhysxMimicJointAPI"):
continue
# Extract the axis instance name (e.g. "rotZ" from "PhysxMimicJointAPI:rotZ")
parts = schema_str.split(":")
if len(parts) < 2:
continue
axis_instance = parts[1]
ref_joint_rel = joint_prim.GetRelationship(f"physxMimicJoint:{axis_instance}:referenceJoint")
if not ref_joint_rel:
continue
targets = ref_joint_rel.GetTargets()
if not targets:
continue
leader_path = targets[0]
if not leader_path.IsAbsolutePath():
leader_path = joint_prim.GetPath().GetParentPath().AppendPath(leader_path)
leader_path = str(leader_path)
leader_idx = path_joint_map.get(leader_path)
if leader_idx is None:
warnings.warn(
f"PhysxMimicJointAPI on '{joint_path}' references '{leader_path}' "
f"but leader joint was not found, skipping mimic constraint",
stacklevel=2,
)
continue
gearing_attr = joint_prim.GetAttribute(f"physxMimicJoint:{axis_instance}:gearing")
gearing = float(gearing_attr.Get()) if gearing_attr and gearing_attr.HasValue() else 1.0
offset_attr = joint_prim.GetAttribute(f"physxMimicJoint:{axis_instance}:offset")
offset = float(offset_attr.Get()) if offset_attr and offset_attr.HasValue() else 0.0
builder.add_constraint_mimic(
joint0=joint_idx,
joint1=leader_idx,
coef0=-offset,
coef1=-gearing,
enabled=True,
label=joint_path,
)
if verbose:
print(
f"Added PhysxMimicJointAPI constraint: '{joint_path}' follows '{leader_path}' "
f"(gearing={gearing}, offset={offset}, axis={axis_instance})"
)
# Mimic constraints from NewtonMimicAPI (run after collapse so joint indices are final).
for joint_path, joint_idx in path_joint_map.items():
joint_prim = stage.GetPrimAtPath(joint_path)
if not joint_prim.IsValid() or not joint_prim.HasAPI("NewtonMimicAPI"):
continue
mimic_enabled = usd.get_attribute(joint_prim, "newton:mimicEnabled", default=True)
if not mimic_enabled:
continue
mimic_rel = joint_prim.GetRelationship("newton:mimicJoint")
if not mimic_rel or not mimic_rel.HasAuthoredTargets():
if verbose:
print(f"NewtonMimicAPI on {joint_path} has no newton:mimicJoint target; skipping")
continue
targets = mimic_rel.GetTargets()
if not targets:
if verbose:
print(f"NewtonMimicAPI on {joint_path}: newton:mimicJoint has no targets; skipping")
continue
leader_path = targets[0]
if not leader_path.IsAbsolutePath():
leader_path = joint_prim.GetPath().GetParentPath().AppendPath(leader_path)
leader_path_str = str(leader_path)
if leader_path_str not in path_joint_map:
warnings.warn(
f"NewtonMimicAPI on {joint_path}: leader {leader_path_str} not in path_joint_map; skipping mimic constraint.",
stacklevel=2,
)
continue
coef0 = usd.get_attribute(joint_prim, "newton:mimicCoef0", default=0.0)
coef1 = usd.get_attribute(joint_prim, "newton:mimicCoef1", default=1.0)
leader_idx = path_joint_map[leader_path_str]
builder.add_constraint_mimic(
joint0=joint_idx,
joint1=leader_idx,
coef0=coef0,
coef1=coef1,
enabled=True,
label=joint_path,
)
# Parse Newton actuator prims from the USD stage.
try:
from newton_actuators import parse_actuator_prim # noqa: PLC0415
except ImportError:
parse_actuator_prim = None
actuator_count = 0
if parse_actuator_prim is not None:
path_to_dof = {
path: builder.joint_qd_start[idx]
for path, idx in path_joint_map.items()
if idx < len(builder.joint_qd_start)
}
for prim in Usd.PrimRange(stage.GetPrimAtPath(root_path)):
parsed = parse_actuator_prim(prim)
if parsed is None:
continue
dof_indices = [path_to_dof[p] for p in parsed.target_paths if p in path_to_dof]
if dof_indices:
builder.add_actuator(parsed.actuator_class, input_indices=dof_indices, **parsed.kwargs)
actuator_count += 1
else:
# TODO: Replace this string-based type name check with a proper schema query
# once the Newton actuator USD schema is merged
for prim in Usd.PrimRange(stage.GetPrimAtPath(root_path)):
if prim.GetTypeName() == "Actuator":
raise ImportError(
f"USD stage contains actuator prims (e.g. {prim.GetPath()}) but newton-actuators is not installed. "
"Install with: pip install newton[sim]"
)
if verbose and actuator_count > 0:
print(f"Added {actuator_count} actuator(s) from USD")
result = {
"fps": stage.GetFramesPerSecond(),
"duration": stage.GetEndTimeCode() - stage.GetStartTimeCode(),
"up_axis": stage_up_axis,
"path_body_map": path_body_map,
"path_joint_map": path_joint_map,
"path_shape_map": path_shape_map,
"path_shape_scale": path_shape_scale,
"mass_unit": mass_unit,
"linear_unit": linear_unit,
"scene_attributes": scene_attributes,
"physics_dt": physics_dt,
"collapse_results": collapse_results,
"schema_attrs": R.schema_attrs,
# "articulation_roots": articulation_roots,
# "articulation_bodies": articulation_bodies,
"path_body_relative_transform": path_body_relative_transform,
"max_solver_iterations": max_solver_iters,
"actuator_count": actuator_count,
}
# Process custom frequencies with USD prim filters
# Collect frequencies with filters and their attributes, then traverse stage once
frequencies_with_filters = []
for freq_key, freq_obj in builder.custom_frequencies.items():
if freq_obj.usd_prim_filter is None:
continue
freq_attrs = [attr for attr in builder.custom_attributes.values() if attr.frequency == freq_key]
if not freq_attrs:
continue
frequencies_with_filters.append((freq_key, freq_obj, freq_attrs))
# Traverse stage once and check all filters for each prim
# Use TraverseInstanceProxies to include prims under instanceable prims
if frequencies_with_filters:
for prim in stage.Traverse(Usd.TraverseInstanceProxies()):
for freq_key, freq_obj, freq_attrs in frequencies_with_filters:
# Build per-frequency callback context and pass the same object to
# usd_prim_filter and usd_entry_expander.
callback_context = {"prim": prim, "result": result, "builder": builder}
try:
matches_frequency = freq_obj.usd_prim_filter(prim, callback_context)
except Exception as e:
raise RuntimeError(
f"usd_prim_filter for frequency '{freq_key}' raised an error on prim '{prim.GetPath()}': {e}"
) from e
if not matches_frequency:
continue
if freq_obj.usd_entry_expander is not None:
try:
expanded_rows = list(freq_obj.usd_entry_expander(prim, callback_context))
except Exception as e:
raise RuntimeError(
f"usd_entry_expander for frequency '{freq_key}' raised an error on prim '{prim.GetPath()}': {e}"
) from e
values_rows = [{attr.key: row.get(attr.key, None) for attr in freq_attrs} for row in expanded_rows]
builder.add_custom_values_batch(values_rows)
if verbose and len(expanded_rows) > 0:
print(
f"Parsed custom frequency '{freq_key}' from prim {prim.GetPath()} with {len(expanded_rows)} rows"
)
continue
prim_custom_attrs = usd.get_custom_attribute_values(
prim,
freq_attrs,
context={"result": result, "builder": builder},
)
# Build a complete values dict for all attributes in this frequency
# Use None for missing values so add_custom_values can apply defaults
values_dict = {}
for attr in freq_attrs:
# Use authored value if present, otherwise None (defaults applied at finalize)
values_dict[attr.key] = prim_custom_attrs.get(attr.key, None)
# Always add values for this prim to increment the frequency count,
# even if all values are None (defaults will be applied during finalization)
builder.add_custom_values(**values_dict)
if verbose:
print(f"Parsed custom frequency '{freq_key}' from prim {prim.GetPath()}")
return result
def resolve_usd_from_url(url: str, target_folder_name: str | None = None, export_usda: bool = False) -> str:
"""Download a USD file from a URL and resolves all references to other USD files to be downloaded to the given target folder.
Args:
url: URL to the USD file.
target_folder_name: Target folder name. If ``None``, a time-stamped
folder will be created in the current directory.
export_usda: If ``True``, converts each downloaded USD file to USDA and
saves the additional USDA file in the target folder with the same
base name as the original USD file.
Returns:
File path to the downloaded USD file.
"""
import requests
try:
from pxr import Usd
except ImportError as e:
raise ImportError("Failed to import pxr. Please install USD (e.g. via `pip install usd-core`).") from e
request_timeout_s = 30
response = requests.get(url, allow_redirects=True, timeout=request_timeout_s)
if response.status_code != 200:
raise RuntimeError(f"Failed to download USD file. Status code: {response.status_code}")
file = response.content
dot = os.path.extsep
base = os.path.basename(url)
url_folder = os.path.dirname(url)
base_name = dot.join(base.split(dot)[:-1])
if target_folder_name is None:
timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M%S")
target_folder_name = os.path.join(".usd_cache", f"{base_name}_{timestamp}")
os.makedirs(target_folder_name, exist_ok=True)
target_filename = os.path.join(target_folder_name, base)
with open(target_filename, "wb") as f:
f.write(file)
stage = Usd.Stage.Open(target_filename, Usd.Stage.LoadNone)
stage_str = stage.GetRootLayer().ExportToString()
print(f"Downloaded USD file to {target_filename}.")
if export_usda:
usda_filename = os.path.join(target_folder_name, base_name + ".usda")
with open(usda_filename, "w") as f:
f.write(stage_str)
print(f"Exported USDA file to {usda_filename}.")
# Recursively resolve referenced USD files like `references = @./franka_collisions.usd@`
# Each entry in the queue is (resolved_url, cache_relative_path).
downloaded_urls: set[str] = {url}
pending: collections.deque[tuple[str, str]] = collections.deque()
def _extract_references(layer_str, parent_url_folder, parent_local_folder):
"""Extract reference paths from a USD layer string and queue them for download."""
for match in re.finditer(r"references.=.@(.*?)@", layer_str):
raw_ref = match.group(1)
ref_url = urljoin(parent_url_folder + "/", raw_ref)
local_path = os.path.normpath(os.path.join(parent_local_folder, raw_ref))
if os.path.isabs(local_path) or local_path.startswith(".."):
print(f"Skipping reference that escapes target folder: {raw_ref}")
continue
if ref_url not in downloaded_urls:
pending.append((ref_url, local_path))
_extract_references(stage_str, url_folder, "")
while pending:
ref_url, local_path = pending.popleft()
if ref_url in downloaded_urls:
continue
downloaded_urls.add(ref_url)
try:
response = requests.get(ref_url, allow_redirects=True, timeout=request_timeout_s)
if response.status_code != 200:
print(f"Failed to download reference {local_path}. Status code: {response.status_code}")
continue
file = response.content
local_dir = os.path.dirname(local_path)
if local_dir:
os.makedirs(os.path.join(target_folder_name, local_dir), exist_ok=True)
ref_filename = os.path.join(target_folder_name, local_path)
if not os.path.exists(ref_filename):
with open(ref_filename, "wb") as f:
f.write(file)
print(f"Downloaded USD reference {local_path} to {ref_filename}.")
ref_stage = Usd.Stage.Open(ref_filename, Usd.Stage.LoadNone)
ref_stage_str = ref_stage.GetRootLayer().ExportToString()
if export_usda:
ref_base = os.path.basename(ref_filename)
ref_base_name = dot.join(ref_base.split(dot)[:-1])
usda_filename = (
os.path.join(target_folder_name, local_dir, ref_base_name + ".usda")
if local_dir
else os.path.join(target_folder_name, ref_base_name + ".usda")
)
with open(usda_filename, "w") as f:
f.write(ref_stage_str)
print(f"Exported USDA file to {usda_filename}.")
# Recurse: resolve references relative to this file's location
_extract_references(ref_stage_str, posixpath.dirname(ref_url), local_dir)
except Exception:
print(f"Failed to download {local_path}.")
return target_filename
def _raise_on_stage_errors(usd_stage, stage_source: str):
get_errors = getattr(usd_stage, "GetCompositionErrors", None)
if get_errors is None:
return
errors = get_errors()
if not errors:
return
messages = []
for err in errors:
try:
messages.append(err.GetMessage())
except Exception:
messages.append(str(err))
formatted = "\n".join(f"- {message}" for message in messages)
raise RuntimeError(f"USD stage has composition errors while loading {stage_source}:\n{formatted}")
================================================
FILE: newton/_src/utils/import_utils.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from collections.abc import Sequence
from typing import Any, Literal
import warp as wp
from ..sim.builder import ModelBuilder
def string_to_warp(value: str, warp_dtype: Any, default: Any = None) -> Any:
"""
Parse a Warp value from a string. This is useful for parsing values from XML files.
For example, "1.0 2.0 3.0" will be parsed as wp.vec3(1.0, 2.0, 3.0).
If fewer values are provided than expected for vector/matrix types, the remaining
values will be filled from the default value if provided.
Raises:
ValueError: If the dtype is invalid.
Args:
value: The string value to parse.
warp_dtype: The Warp dtype to parse the value as.
default: Optional default value to use for padding incomplete vectors/matrices.
Returns:
The parsed Warp value.
"""
def get_vector(scalar_type: Any):
return [scalar_type(x) for x in value.split()]
def get_bool(tok: str) -> bool:
# just casting string to bool is not enough, we need to actually evaluate the
# falsey values
s = tok.strip().lower()
if s in {"1", "true", "t", "yes", "y"}:
return True
if s in {"0", "false", "f", "no", "n"}:
return False
# fall back to numeric interpretation if provided
try:
return bool(int(float(s)))
except Exception as e:
raise ValueError(f"Unable to parse boolean value: {tok}") from e
if wp.types.type_is_quaternion(warp_dtype):
parsed_values = get_vector(float)
# Pad with default values if necessary
expected_length = 4 # Quaternions always have 4 components
if len(parsed_values) < expected_length and default is not None:
if hasattr(default, "__len__"):
default_values = [default[i] for i in range(len(default))]
else:
default_values = [default] * expected_length
parsed_values.extend(default_values[len(parsed_values) : expected_length])
return warp_dtype(*parsed_values)
if wp.types.type_is_int(warp_dtype):
return warp_dtype(int(value))
if wp.types.type_is_float(warp_dtype):
return warp_dtype(float(value))
if warp_dtype is wp.bool or warp_dtype is bool:
return warp_dtype(get_bool(value))
if warp_dtype is str:
return value # String values are used as-is
if wp.types.type_is_vector(warp_dtype) or wp.types.type_is_matrix(warp_dtype):
scalar_type = warp_dtype._wp_scalar_type_
parsed_values = None
if wp.types.type_is_int(scalar_type):
parsed_values = get_vector(int)
elif wp.types.type_is_float(scalar_type):
parsed_values = get_vector(float)
elif scalar_type is wp.bool or scalar_type is bool:
parsed_values = get_vector(bool)
else:
raise ValueError(f"Unable to parse vector/matrix value: {value} as {warp_dtype}.")
# Pad with default values if necessary
expected_length = warp_dtype._length_
if len(parsed_values) < expected_length and default is not None:
# Extract default values and pad
if hasattr(default, "__len__"):
default_values = [default[i] for i in range(len(default))]
else:
default_values = [default] * expected_length
parsed_values.extend(default_values[len(parsed_values) : expected_length])
return warp_dtype(*parsed_values)
raise ValueError(f"Invalid dtype: {warp_dtype}. Must be a valid Warp dtype or str.")
def parse_custom_attributes(
dictlike: dict[str, str],
custom_attributes: Sequence[ModelBuilder.CustomAttribute],
parsing_mode: Literal["usd", "mjcf", "urdf"],
context: dict[str, Any] | None = None,
) -> dict[str, Any]:
"""
Parse custom attributes from a dictionary.
Args:
dictlike: The dictionary (or XML element) to parse the custom attributes from. This object behaves like a string-valued dictionary that implements the ``get`` method and returns the value for the given key.
custom_attributes: The custom attributes to parse. This is a sequence of :class:`ModelBuilder.CustomAttribute` objects.
parsing_mode: The parsing mode to use. This can be "usd", "mjcf", or "urdf". It determines which attribute name and value transformer to use.
context: Optional context dictionary passed to the value transformer. Can contain parsing-time information such as ``use_degrees`` or ``joint_type``.
Returns:
A dictionary of the parsed custom attributes. The keys are the custom attribute keys :attr:`ModelBuilder.CustomAttribute.key`
and the values are the parsed values. Only attributes that were explicitly specified in the source are included
in the output dict. Unspecified attributes are not included, allowing defaults to be filled in during model finalization.
"""
out = {}
for attr in custom_attributes:
transformer = None
name = None
if parsing_mode == "mjcf":
name = attr.mjcf_attribute_name
transformer = attr.mjcf_value_transformer
elif parsing_mode == "urdf":
name = attr.urdf_attribute_name
transformer = attr.urdf_value_transformer
elif parsing_mode == "usd":
name = attr.usd_attribute_name
transformer = attr.usd_value_transformer
if transformer is None:
def transform(
x: str, _context: dict[str, Any] | None, dtype: Any = attr.dtype, default: Any = attr.default
) -> Any:
return string_to_warp(x, dtype, default)
transformer = transform
if name is None:
name = attr.name
dict_value = dictlike.get(name)
if dict_value is not None:
value = transformer(dict_value, context)
if value is None:
# Treat None as "undefined" so defaults are applied later.
continue
out[attr.key] = value
return out
def sanitize_xml_content(source: str) -> str:
# Strip leading whitespace and byte-order marks
xml_content = source.strip()
# Remove BOM if present
if xml_content.startswith("\ufeff"):
xml_content = xml_content[1:]
# Remove leading XML comments
while xml_content.strip().startswith("")
if end_comment != -1:
xml_content = xml_content[end_comment + 3 :].strip()
else:
break
xml_content = xml_content.strip()
return xml_content
def sanitize_name(name: str) -> str:
"""Sanitize a name for use as a key in the ModelBuilder.
Replaces characters that are invalid in USD paths (e.g., "-") with underscores.
Args:
name: The name string to sanitize.
Returns:
The sanitized name with invalid characters replaced by underscores.
"""
return name.replace("-", "_")
def should_show_collider(
force_show_colliders: bool,
has_visual_shapes: bool,
parse_visuals_as_colliders: bool = False,
) -> bool:
"""Determine whether collision shapes should have the VISIBLE flag.
Collision shapes are shown (VISIBLE flag) when explicitly forced, when
visual shapes are used as colliders, or when no visual shapes exist for
the owning body (so there is something to render). Otherwise, collision
shapes get only COLLIDE_SHAPES and are controlled by the viewer's
"Show Collision" toggle.
Args:
force_show_colliders: User explicitly wants collision shapes visible.
has_visual_shapes: Whether the body/link has visual (non-collision) shapes.
parse_visuals_as_colliders: Whether visual geometry is repurposed as collision geometry.
Returns:
True if the collision shape should carry the VISIBLE flag; False if it should
be hidden by default and only revealed via the viewer's "Show Collision" toggle.
"""
if force_show_colliders or parse_visuals_as_colliders:
return True
return not has_visual_shapes
def is_xml_content(source: str) -> bool:
"""Check if a string appears to be XML content rather than a file path.
Uses the presence of XML angle brackets which are required for any XML
content and practically never appear in file paths.
Args:
source: String to check
Returns:
True if the string appears to be XML content, False if it looks like a file path
"""
return any(char in source for char in "<>")
================================================
FILE: newton/_src/utils/mesh.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import os
import warnings
import xml.etree.ElementTree as ET
from collections.abc import Sequence
from dataclasses import dataclass
from typing import cast, overload
import numpy as np
import warp as wp
from ..geometry.types import Mesh
@wp.kernel
def accumulate_vertex_normals(
points: wp.array[wp.vec3],
indices: wp.array[wp.int32],
# output
normals: wp.array[wp.vec3],
):
"""Accumulate per-face normals into per-vertex normals (not normalized)."""
face = wp.tid()
i0 = indices[face * 3]
i1 = indices[face * 3 + 1]
i2 = indices[face * 3 + 2]
v0 = points[i0]
v1 = points[i1]
v2 = points[i2]
normal = wp.cross(v1 - v0, v2 - v0)
wp.atomic_add(normals, i0, normal)
wp.atomic_add(normals, i1, normal)
wp.atomic_add(normals, i2, normal)
@wp.kernel
def normalize_vertex_normals(normals: wp.array[wp.vec3]):
"""Normalize per-vertex normals in-place."""
tid = wp.tid()
normals[tid] = wp.normalize(normals[tid])
@overload
def compute_vertex_normals(
points: wp.array,
indices: wp.array | np.ndarray,
normals: wp.array | None = None,
*,
device: wp.DeviceLike = None,
normalize: bool = True,
) -> wp.array: ...
@overload
def compute_vertex_normals(
points: np.ndarray,
indices: np.ndarray,
normals: np.ndarray | None = None,
*,
device: wp.DeviceLike = None,
normalize: bool = True,
) -> np.ndarray: ...
def compute_vertex_normals(
points: wp.array | np.ndarray,
indices: wp.array | np.ndarray,
normals: wp.array | np.ndarray | None = None,
*,
device: wp.DeviceLike = None,
normalize: bool = True,
) -> wp.array | np.ndarray:
"""Compute per-vertex normals from triangle indices.
Supports Warp and NumPy arrays. NumPy inputs run on the CPU via Warp and return
NumPy output.
Args:
points: Vertex positions (wp.vec3 array or Nx3 NumPy array).
indices: Triangle indices (flattened or Nx3). Warp arrays are expected to be flattened.
normals: Optional output array to reuse (Warp or NumPy to match ``points``).
device: Warp device to run on. NumPy inputs default to CPU.
normalize: Whether to normalize the accumulated normals.
Returns:
Per-vertex normals as a Warp array or NumPy array matching the input type.
"""
if isinstance(points, wp.array):
if normals is not None and not isinstance(normals, wp.array):
raise TypeError("normals must be a Warp array when points is a Warp array.")
device_obj = points.device if device is None else wp.get_device(device)
indices_wp = indices
if isinstance(indices, np.ndarray):
indices_np = np.asarray(indices, dtype=np.int32)
if indices_np.ndim == 2:
indices_np = indices_np.reshape(-1)
elif indices_np.ndim != 1:
raise ValueError("indices must be flat or (N, 3) for NumPy inputs.")
indices_wp = wp.array(indices_np, dtype=wp.int32, device=device_obj)
indices_wp = cast(wp.array, indices_wp)
if normals is None:
normals_wp = wp.zeros_like(points)
else:
normals_wp = cast(wp.array, normals)
normals_wp.zero_()
if len(indices_wp) == 0 or len(points) == 0:
return normals_wp
indices_i32 = indices_wp if indices_wp.dtype == wp.int32 else indices_wp.view(dtype=wp.int32)
wp.launch(
accumulate_vertex_normals,
dim=len(indices_i32) // 3,
inputs=[points, indices_i32],
outputs=[normals_wp],
device=device_obj,
)
if normalize:
wp.launch(normalize_vertex_normals, dim=len(normals_wp), inputs=[normals_wp], device=device_obj)
return normals_wp
points_np = np.asarray(points, dtype=np.float32).reshape(-1, 3)
indices_np = np.asarray(indices, dtype=np.int32)
if indices_np.ndim == 2:
indices_np = indices_np.reshape(-1)
elif indices_np.ndim != 1:
raise ValueError("indices must be flat or (N, 3) for NumPy inputs.")
normals_np = None
if normals is not None:
normals_np = np.asarray(normals, dtype=np.float32).reshape(points_np.shape)
device_obj = wp.get_device("cpu") if device is None else wp.get_device(device)
points_wp = wp.array(points_np, dtype=wp.vec3, device=device_obj)
indices_wp = wp.array(indices_np, dtype=wp.int32, device=device_obj)
if normals_np is None:
normals_wp = wp.zeros_like(points_wp)
else:
normals_wp = wp.array(normals_np, dtype=wp.vec3, device=device_obj)
normals_wp.zero_()
if len(points_wp) == 0 or len(indices_wp) == 0:
if normals_np is None:
return np.zeros_like(points_np, dtype=np.float32)
normals_np[...] = 0.0
return normals_np
wp.launch(
accumulate_vertex_normals,
dim=len(indices_wp) // 3,
inputs=[points_wp, indices_wp],
outputs=[normals_wp],
device=device_obj,
)
if normalize:
wp.launch(normalize_vertex_normals, dim=len(normals_wp), inputs=[normals_wp], device=device_obj)
normals_out = normals_wp.numpy()
if normals_np is not None:
normals_np[...] = normals_out
return normals_np
return normals_out
def smooth_vertex_normals_by_position(
mesh_vertices: np.ndarray, mesh_faces: np.ndarray, eps: float = 1.0e-6
) -> np.ndarray:
"""Smooth vertex normals by averaging normals of vertices with shared positions."""
normals = compute_vertex_normals(mesh_vertices, mesh_faces)
if len(mesh_vertices) == 0:
return normals
keys = np.round(mesh_vertices / eps).astype(np.int64)
unique_keys, inverse = np.unique(keys, axis=0, return_inverse=True)
accum = np.zeros((len(unique_keys), 3), dtype=np.float32)
np.add.at(accum, inverse, normals)
lengths = np.linalg.norm(accum, axis=1, keepdims=True)
lengths = np.maximum(lengths, 1.0e-8)
accum = accum / lengths
return accum[inverse]
# Default number of segments for mesh generation
default_num_segments = 32
class MeshAdjacency:
"""Builds and stores edge adjacency information for a triangle mesh.
This class processes triangle indices to create a mapping from edges to
their adjacent triangles. Each edge stores references to both adjacent
triangles (if they exist) along with the opposite vertices.
Attributes:
edges: Dictionary mapping edge keys (min_vertex, max_vertex) to MeshAdjacency.Edge objects.
indices: The original triangle indices used to build the adjacency.
"""
@dataclass
class Edge:
"""Represents an edge in a triangle mesh with adjacency information.
Stores the two vertices of the edge, the opposite vertices from each
adjacent triangle, and the indices of those triangles. The winding order
is consistent: the first triangle is reconstructed as {v0, v1, o0}, and
the second triangle as {v1, v0, o1}.
For boundary edges (edges with only one adjacent triangle), o1 and f1
are set to -1.
"""
v0: int
"""Index of the first vertex of the edge."""
v1: int
"""Index of the second vertex of the edge."""
o0: int
"""Index of the vertex opposite to the edge in the first adjacent triangle."""
o1: int
"""Index of the vertex opposite to the edge in the second adjacent triangle, or -1 if boundary."""
f0: int
"""Index of the first adjacent triangle."""
f1: int
"""Index of the second adjacent triangle, or -1 if boundary edge."""
def __init__(self, indices: Sequence[Sequence[int]] | np.ndarray):
"""Build edge adjacency from triangle indices.
Args:
indices: Array-like of triangle indices, where each element is a
sequence of 3 vertex indices defining a triangle.
"""
self.edges: dict[tuple[int, int], MeshAdjacency.Edge] = {}
self.indices = indices
for index, tri in enumerate(indices):
self.add_edge(tri[0], tri[1], tri[2], index)
self.add_edge(tri[1], tri[2], tri[0], index)
self.add_edge(tri[2], tri[0], tri[1], index)
def add_edge(self, i0: int, i1: int, o: int, f: int):
"""Add or update an edge in the adjacency structure.
If the edge already exists, updates it with the second adjacent triangle.
If the edge would have more than two adjacent triangles, prints a warning
(non-manifold edge).
Args:
i0: Index of the first vertex of the edge.
i1: Index of the second vertex of the edge.
o: Index of the opposite vertex in the triangle.
f: Index of the triangle containing this edge.
"""
key = (min(i0, i1), max(i0, i1))
edge = None
if key in self.edges:
edge = self.edges[key]
if edge.f1 != -1:
warnings.warn("Detected non-manifold edge", stacklevel=2)
return
else:
# update other side of the edge
edge.o1 = o
edge.f1 = f
else:
# create new edge with opposite yet to be filled
edge = MeshAdjacency.Edge(i0, i1, o, -1, f, -1)
self.edges[key] = edge
def create_mesh_sphere(
radius: float = 1.0,
*,
num_latitudes: int = default_num_segments,
num_longitudes: int = default_num_segments,
reverse_winding: bool = False,
compute_normals: bool = True,
compute_uvs: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None]:
"""Create sphere geometry data with optional normals and UVs."""
positions = []
normals = [] if compute_normals else None
uvs = [] if compute_uvs else None
indices = []
for i in range(num_latitudes + 1):
theta = i * np.pi / num_latitudes
sin_theta = np.sin(theta)
cos_theta = np.cos(theta)
for j in range(num_longitudes + 1):
phi = j * 2 * np.pi / num_longitudes
sin_phi = np.sin(phi)
cos_phi = np.cos(phi)
x = cos_phi * sin_theta
y = cos_theta
z = sin_phi * sin_theta
positions.append([x * radius, y * radius, z * radius])
if compute_normals:
normals.append([x, y, z])
if compute_uvs:
u = float(j) / num_longitudes
v = float(i) / num_latitudes
uvs.append([u, v])
for i in range(num_latitudes):
for j in range(num_longitudes):
first = i * (num_longitudes + 1) + j
second = first + num_longitudes + 1
if reverse_winding:
indices.extend([first, second, first + 1, second, second + 1, first + 1])
else:
indices.extend([first, first + 1, second, second, first + 1, second + 1])
return (
np.asarray(positions, dtype=np.float32),
np.asarray(indices, dtype=np.uint32),
None if normals is None else np.asarray(normals, dtype=np.float32),
None if uvs is None else np.asarray(uvs, dtype=np.float32),
)
def create_mesh_ellipsoid(
rx: float = 1.0,
ry: float = 1.0,
rz: float = 1.0,
*,
num_latitudes: int = default_num_segments,
num_longitudes: int = default_num_segments,
reverse_winding: bool = False,
compute_normals: bool = True,
compute_uvs: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None]:
"""Create ellipsoid geometry data with optional normals and UVs."""
positions = []
normals = [] if compute_normals else None
uvs = [] if compute_uvs else None
indices = []
for i in range(num_latitudes + 1):
theta = i * np.pi / num_latitudes
sin_theta = np.sin(theta)
cos_theta = np.cos(theta)
for j in range(num_longitudes + 1):
phi = j * 2 * np.pi / num_longitudes
sin_phi = np.sin(phi)
cos_phi = np.cos(phi)
ux = cos_phi * sin_theta
uy = cos_theta
uz = sin_phi * sin_theta
px = ux * rx
py = uy * ry
pz = uz * rz
positions.append([px, py, pz])
if compute_normals:
nx = ux / rx
ny = uy / ry
nz = uz / rz
n_len = np.sqrt(nx * nx + ny * ny + nz * nz)
if n_len > 1e-10:
nx /= n_len
ny /= n_len
nz /= n_len
normals.append([nx, ny, nz])
if compute_uvs:
u = float(j) / num_longitudes
v = float(i) / num_latitudes
uvs.append([u, v])
for i in range(num_latitudes):
for j in range(num_longitudes):
first = i * (num_longitudes + 1) + j
second = first + num_longitudes + 1
if reverse_winding:
indices.extend([first, second, first + 1, second, second + 1, first + 1])
else:
indices.extend([first, first + 1, second, second, first + 1, second + 1])
return (
np.asarray(positions, dtype=np.float32),
np.asarray(indices, dtype=np.uint32),
None if normals is None else np.asarray(normals, dtype=np.float32),
None if uvs is None else np.asarray(uvs, dtype=np.float32),
)
def _normalize_color(color) -> tuple[float, float, float] | None:
if color is None:
return None
color = np.asarray(color, dtype=np.float32).flatten()
if color.size >= 3:
if np.max(color) > 1.0:
color = color / 255.0
return (float(color[0]), float(color[1]), float(color[2]))
return None
def _extract_trimesh_texture(visual_or_material, base_dir: str) -> np.ndarray | str | None:
"""Extract texture from a trimesh visual or a single material object."""
material = getattr(visual_or_material, "material", visual_or_material)
if material is None:
return None
image = getattr(material, "image", None)
image_path = getattr(material, "image_path", None)
if image is None:
base_color_texture = getattr(material, "baseColorTexture", None)
if base_color_texture is not None:
image = getattr(base_color_texture, "image", None)
image_path = image_path or getattr(base_color_texture, "image_path", None)
if image is not None:
try:
return np.array(image)
except Exception:
pass
if image_path:
if not os.path.isabs(image_path):
image_path = os.path.abspath(os.path.join(base_dir, image_path))
return image_path
return None
def _extract_trimesh_material_params(
material,
) -> tuple[float | None, float | None, tuple[float, float, float] | None]:
if material is None:
return None, None, None
base_color = None
metallic = None
roughness = None
color_candidates = [
getattr(material, "baseColorFactor", None),
getattr(material, "diffuse", None),
getattr(material, "diffuseColor", None),
]
for candidate in color_candidates:
if candidate is not None:
base_color = _normalize_color(candidate)
break
for attr_name in ("metallicFactor", "metallic"):
value = getattr(material, attr_name, None)
if value is not None:
metallic = float(value)
break
for attr_name in ("roughnessFactor", "roughness"):
value = getattr(material, attr_name, None)
if value is not None:
roughness = float(value)
break
if roughness is None:
for attr_name in ("glossiness", "shininess"):
value = getattr(material, attr_name, None)
if value is not None:
gloss = float(value)
if attr_name == "shininess":
gloss = min(max(gloss / 1000.0, 0.0), 1.0)
roughness = 1.0 - min(max(gloss, 0.0), 1.0)
break
return roughness, metallic, base_color
def load_meshes_from_file(
filename: str,
*,
scale: np.ndarray | list[float] | tuple[float, ...] = (1.0, 1.0, 1.0),
maxhullvert: int,
override_color: np.ndarray | list[float] | tuple[float, float, float] | None = None,
override_texture: np.ndarray | str | None = None,
) -> list[Mesh]:
"""Load meshes from a file using trimesh and capture texture data if present.
Args:
filename: Path to the mesh file.
scale: Per-axis scale to apply to vertices.
maxhullvert: Maximum vertices for convex hull approximation.
override_color: Optional base color override (RGB).
override_texture: Optional texture path/URL or image override.
Returns:
List of Mesh objects.
"""
import trimesh
filename = os.fspath(filename)
scale = np.asarray(scale, dtype=np.float32)
base_dir = os.path.dirname(filename)
def _parse_dae_material_colors(
path: str,
) -> tuple[list[str], dict[str, dict[str, float | tuple[float, float, float] | None]]]:
try:
tree = ET.parse(path)
root = tree.getroot()
except Exception:
return [], {}
def strip(tag: str) -> str:
return tag.split("}", 1)[-1] if "}" in tag else tag
# Map effect id -> material properties
effect_props: dict[str, dict[str, float | tuple[float, float, float] | None]] = {}
for effect in root.iter():
if strip(effect.tag) != "effect":
continue
effect_id = effect.attrib.get("id")
if not effect_id:
continue
diffuse_color = None
specular_color = None
specular_intensity = None
shininess = None
for shader_tag in ("phong", "lambert", "blinn"):
shader = None
for elem in effect.iter():
if strip(elem.tag) == shader_tag:
shader = elem
break
if shader is None:
continue
for node in shader.iter():
tag = strip(node.tag)
if tag == "diffuse":
for col in node.iter():
if strip(col.tag) == "color" and col.text:
values = [float(x) for x in col.text.strip().split()]
if len(values) >= 3:
# DAE diffuse colors are commonly authored in linear space.
# Convert to sRGB for the viewer shader (which converts to linear).
diffuse = np.clip(values[:3], 0.0, 1.0)
srgb = np.power(diffuse, 1.0 / 2.2)
diffuse_color = (float(srgb[0]), float(srgb[1]), float(srgb[2]))
break
continue
if tag == "specular":
for col in node.iter():
if strip(col.tag) == "color" and col.text:
values = [float(x) for x in col.text.strip().split()]
if len(values) >= 3:
specular_color = (values[0], values[1], values[2])
break
continue
if tag == "reflectivity":
for val in node.iter():
if strip(val.tag) == "float" and val.text:
try:
specular_intensity = float(val.text.strip())
except ValueError:
specular_intensity = None
break
continue
if tag == "shininess":
for val in node.iter():
if strip(val.tag) == "float" and val.text:
try:
shininess = float(val.text.strip())
except ValueError:
shininess = None
break
continue
if diffuse_color is not None:
break
metallic = None
if specular_color is not None:
metallic = float(np.clip(np.max(specular_color), 0.0, 1.0))
elif specular_intensity is not None:
metallic = float(np.clip(specular_intensity, 0.0, 1.0))
roughness = None
if shininess is not None:
if shininess > 1.0:
shininess = min(shininess / 128.0, 1.0)
roughness = float(np.clip(1.0 - shininess, 0.0, 1.0))
if diffuse_color is not None:
effect_props[effect_id] = {
"color": diffuse_color,
"metallic": metallic,
"roughness": roughness,
}
# Map material id/name -> material properties
material_colors: dict[str, dict[str, float | tuple[float, float, float] | None]] = {}
for material in root.iter():
if strip(material.tag) != "material":
continue
mat_id = material.attrib.get("id") or material.attrib.get("name")
effect_url = None
for inst in material.iter():
if strip(inst.tag) == "instance_effect":
effect_url = inst.attrib.get("url")
break
if mat_id and effect_url and effect_url.startswith("#"):
effect_id = effect_url[1:]
if effect_id in effect_props:
material_colors[mat_id] = effect_props[effect_id]
# Collect triangle material assignments in order
face_materials: list[str] = []
for triangles in root.iter():
if strip(triangles.tag) != "triangles":
continue
mat = triangles.attrib.get("material")
count = triangles.attrib.get("count")
if not mat or count is None:
continue
try:
tri_count = int(count)
except ValueError:
continue
face_materials.extend([mat] * tri_count)
return face_materials, material_colors
dae_face_materials: list[str] = []
dae_material_colors: dict[str, dict[str, float | tuple[float, float, float] | None]] = {}
if filename.lower().endswith(".dae"):
dae_face_materials, dae_material_colors = _parse_dae_material_colors(filename)
tri = trimesh.load(filename, force="mesh")
tri_meshes = tri.geometry.values() if hasattr(tri, "geometry") else [tri]
meshes = []
for tri_mesh in tri_meshes:
vertices = np.array(tri_mesh.vertices, dtype=np.float32) * scale
faces = np.array(tri_mesh.faces, dtype=np.int32)
normals = np.array(tri_mesh.vertex_normals, dtype=np.float32) if tri_mesh.vertex_normals is not None else None
if normals is None or not np.isfinite(normals).all() or np.allclose(normals, 0.0):
normals = compute_vertex_normals(vertices, faces)
uvs = None
if hasattr(tri_mesh, "visual") and getattr(tri_mesh.visual, "uv", None) is not None:
uvs = np.array(tri_mesh.visual.uv, dtype=np.float32)
color = _normalize_color(override_color) if override_color is not None else None
texture = override_texture
def add_mesh_from_faces(
face_indices,
*,
mat_color=None,
mat_roughness=None,
mat_metallic=None,
mesh_vertices=None,
mesh_normals=None,
mesh_uvs=None,
mesh_texture=None,
):
used = np.unique(face_indices.flatten())
remap = {int(old): i for i, old in enumerate(used)}
remapped_faces = np.vectorize(remap.get)(face_indices).astype(np.int32)
sub_vertices = mesh_vertices[used]
sub_normals = mesh_normals[used] if mesh_normals is not None else None
force_smooth = False
if mat_metallic is not None and mat_metallic > 0.0:
force_smooth = True
if mat_roughness is not None and mat_roughness < 0.6:
force_smooth = True
if sub_normals is None or force_smooth:
sub_normals = smooth_vertex_normals_by_position(sub_vertices, remapped_faces)
sub_uvs = mesh_uvs[used] if mesh_uvs is not None else None
meshes.append(
Mesh(
sub_vertices,
remapped_faces.flatten(),
normals=sub_normals,
uvs=sub_uvs,
maxhullvert=maxhullvert,
color=mat_color,
texture=mesh_texture,
roughness=mat_roughness,
metallic=mat_metallic,
)
)
# If a uniform override is provided, skip per-material splitting.
if color is not None or texture is not None:
add_mesh_from_faces(
faces,
mat_color=color,
mesh_vertices=vertices,
mesh_normals=normals,
mesh_uvs=uvs,
mesh_texture=texture,
)
continue
# Handle per-face materials if available (e.g. DAE with multiple materials)
face_materials = getattr(tri_mesh.visual, "face_materials", None) if hasattr(tri_mesh, "visual") else None
materials = getattr(tri_mesh.visual, "materials", None) if hasattr(tri_mesh, "visual") else None
if face_materials is not None and materials is not None:
face_materials = np.array(face_materials, dtype=np.int32).flatten()
for mat_index in np.unique(face_materials):
mat_faces = faces[face_materials == mat_index]
material = materials[int(mat_index)] if int(mat_index) < len(materials) else None
roughness, metallic, base_color = _extract_trimesh_material_params(material)
mat_color = base_color
mat_texture = _extract_trimesh_texture(material, base_dir)
if mat_color is None and hasattr(tri_mesh.visual, "main_color"):
mat_color = _normalize_color(tri_mesh.visual.main_color)
add_mesh_from_faces(
mat_faces,
mat_color=mat_color,
mat_roughness=roughness,
mat_metallic=metallic,
mesh_vertices=vertices,
mesh_normals=normals,
mesh_uvs=uvs,
mesh_texture=mat_texture,
)
continue
# DAE fallback: use material groups from the source file if trimesh didn't expose them
if dae_face_materials and len(dae_face_materials) == len(faces):
face_materials = np.array(dae_face_materials, dtype=object)
for mat_name in np.unique(face_materials):
mat_faces = faces[face_materials == mat_name]
mat_props = dae_material_colors.get(str(mat_name), {})
mat_color = mat_props.get("color")
mat_roughness = mat_props.get("roughness")
mat_metallic = mat_props.get("metallic")
add_mesh_from_faces(
mat_faces,
mat_color=mat_color,
mat_roughness=mat_roughness,
mat_metallic=mat_metallic,
mesh_vertices=vertices,
mesh_normals=normals,
mesh_uvs=uvs,
mesh_texture=texture,
)
continue
# Handle per-face color visuals (common for DAE via ColorVisuals)
face_colors = getattr(tri_mesh.visual, "face_colors", None) if hasattr(tri_mesh, "visual") else None
if face_colors is not None:
face_colors = np.array(face_colors, dtype=np.float32)
if face_colors.shape[0] == faces.shape[0]:
# Normalize to 0..1 rgb
if np.max(face_colors) > 1.0:
face_colors = face_colors / 255.0
rgb = face_colors[:, :3]
# quantize to avoid tiny float differences
rgb = np.round(rgb, 4)
unique_colors, inverse = np.unique(rgb, axis=0, return_inverse=True)
for color_idx, mat_color in enumerate(unique_colors):
mat_faces = faces[inverse == color_idx]
add_mesh_from_faces(
mat_faces,
mat_color=(float(mat_color[0]), float(mat_color[1]), float(mat_color[2])),
mesh_vertices=vertices,
mesh_normals=normals,
mesh_uvs=uvs,
mesh_texture=texture,
)
continue
# Handle per-vertex colors by computing face colors
vertex_colors = getattr(tri_mesh.visual, "vertex_colors", None) if hasattr(tri_mesh, "visual") else None
if vertex_colors is not None:
vertex_colors = np.array(vertex_colors, dtype=np.float32)
if np.max(vertex_colors) > 1.0:
vertex_colors = vertex_colors / 255.0
rgb = vertex_colors[:, :3]
face_rgb = rgb[faces].mean(axis=1)
face_rgb = np.round(face_rgb, 4)
unique_colors, inverse = np.unique(face_rgb, axis=0, return_inverse=True)
for color_idx, mat_color in enumerate(unique_colors):
mat_faces = faces[inverse == color_idx]
add_mesh_from_faces(
mat_faces,
mat_color=(float(mat_color[0]), float(mat_color[1]), float(mat_color[2])),
mesh_vertices=vertices,
mesh_normals=normals,
mesh_uvs=uvs,
mesh_texture=texture,
)
continue
# Single-material mesh fallback
roughness = None
metallic = None
if color is None and hasattr(tri_mesh, "visual") and hasattr(tri_mesh.visual, "main_color"):
color = _normalize_color(tri_mesh.visual.main_color)
if hasattr(tri_mesh, "visual") and texture is None:
texture = _extract_trimesh_texture(tri_mesh.visual, base_dir)
material = getattr(tri_mesh.visual, "material", None)
roughness, metallic, base_color = _extract_trimesh_material_params(material)
if color is None and base_color is not None:
color = base_color
meshes.append(
Mesh(
vertices,
faces.flatten(),
normals=normals,
uvs=uvs,
maxhullvert=maxhullvert,
color=color,
texture=texture,
roughness=roughness,
metallic=metallic,
)
)
return meshes
def create_mesh_capsule(
radius: float,
half_height: float,
*,
up_axis: int = 1,
segments: int = default_num_segments,
compute_normals: bool = True,
compute_uvs: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None]:
"""Create capsule geometry data with optional normals and UVs."""
positions = []
normals = [] if compute_normals else None
uvs = [] if compute_uvs else None
indices = []
if up_axis not in (0, 1, 2):
raise ValueError("up_axis must be between 0 and 2")
x_dir, y_dir, z_dir = ((1, 2, 0), (0, 1, 2), (2, 0, 1))[up_axis]
up_vector = np.zeros(3, dtype=np.float32)
up_vector[up_axis] = half_height
for i in range(segments + 1):
theta = i * np.pi / segments
sin_theta = np.sin(theta)
cos_theta = np.cos(theta)
for j in range(segments + 1):
phi = j * 2 * np.pi / segments
sin_phi = np.sin(phi)
cos_phi = np.cos(phi)
z = cos_phi * sin_theta
y = cos_theta
x = sin_phi * sin_theta
xyz = np.array((x, y, z), dtype=np.float32)
normal = xyz[[x_dir, y_dir, z_dir]]
pos = normal * radius
if normal[up_axis] >= 0.0:
pos += up_vector
else:
pos -= up_vector
positions.append(pos.tolist())
if compute_normals:
normals.append(normal.tolist())
if compute_uvs:
u = cos_theta * 0.5 + 0.5
v = cos_phi * sin_theta * 0.5 + 0.5
uvs.append([u, v])
nv = len(positions)
for i in range(segments):
for j in range(segments):
first = (i * (segments + 1) + j) % nv
second = (first + segments + 1) % nv
indices.extend([first, second, (first + 1) % nv, second, (second + 1) % nv, (first + 1) % nv])
return (
np.asarray(positions, dtype=np.float32),
np.asarray(indices, dtype=np.uint32),
None if normals is None else np.asarray(normals, dtype=np.float32),
None if uvs is None else np.asarray(uvs, dtype=np.float32),
)
def create_mesh_cone(
radius: float,
half_height: float,
*,
up_axis: int = 1,
segments: int = default_num_segments,
compute_normals: bool = True,
compute_uvs: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None]:
"""Create cone geometry data with optional normals and UVs."""
return create_mesh_cylinder(
radius,
half_height,
up_axis=up_axis,
segments=segments,
top_radius=0.0,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
def create_mesh_cylinder(
radius: float,
half_height: float,
*,
up_axis: int = 1,
segments: int = default_num_segments,
top_radius: float | None = None,
compute_normals: bool = True,
compute_uvs: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None]:
"""Create cylinder/truncated cone geometry data with optional normals/UVs."""
if up_axis not in (0, 1, 2):
raise ValueError("up_axis must be between 0 and 2")
x_dir, y_dir, z_dir = ((1, 2, 0), (0, 1, 2), (2, 0, 1))[up_axis]
if top_radius is None:
top_radius = radius
indices = []
positions = []
normals = [] if compute_normals else None
uvs = [] if compute_uvs else None
def add_vertex(position: np.ndarray, normal: np.ndarray | None, uv: tuple[float, float] | None) -> int:
idx = len(positions)
positions.append(position.tolist())
if compute_normals:
assert normals is not None
normals.append([0.0, 0.0, 0.0] if normal is None else normal.tolist())
if compute_uvs:
assert uvs is not None
uvs.append([0.0, 0.0] if uv is None else [uv[0], uv[1]])
return idx
side_radial_component = 2.0 * half_height
side_axial_component = radius - top_radius
# Side vertices first (contiguous layout for robust indexing).
side_bottom_indices = []
for i in range(segments):
theta = 2 * np.pi * i / segments
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
position = np.array([radius * cos_theta, -half_height, radius * sin_theta], dtype=np.float32)
position = position[[x_dir, y_dir, z_dir]]
side_normal = None
if compute_normals:
side_normal = np.array(
[
side_radial_component * cos_theta,
side_axial_component,
side_radial_component * sin_theta,
],
dtype=np.float32,
)
normal_length = np.linalg.norm(side_normal)
if normal_length > 0.0:
side_normal = side_normal / normal_length
side_normal = side_normal[[x_dir, y_dir, z_dir]]
side_uv = (i / max(segments - 1, 1), 0.0) if compute_uvs else None
side_bottom_indices.append(add_vertex(position, side_normal, side_uv))
side_top_indices = []
side_apex_index: int | None = None
if top_radius > 0.0:
for i in range(segments):
theta = 2 * np.pi * i / segments
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
position = np.array([top_radius * cos_theta, half_height, top_radius * sin_theta], dtype=np.float32)
position = position[[x_dir, y_dir, z_dir]]
side_normal = None
if compute_normals:
side_normal = np.array(
[
side_radial_component * cos_theta,
side_axial_component,
side_radial_component * sin_theta,
],
dtype=np.float32,
)
normal_length = np.linalg.norm(side_normal)
if normal_length > 0.0:
side_normal = side_normal / normal_length
side_normal = side_normal[[x_dir, y_dir, z_dir]]
side_uv = (i / max(segments - 1, 1), 1.0) if compute_uvs else None
side_top_indices.append(add_vertex(position, side_normal, side_uv))
else:
apex_position = np.array([0.0, half_height, 0.0], dtype=np.float32)[[x_dir, y_dir, z_dir]]
apex_normal = None
if compute_normals:
apex_normal = np.array([0.0, 1.0, 0.0], dtype=np.float32)[[x_dir, y_dir, z_dir]]
side_apex_index = add_vertex(apex_position, apex_normal, (0.5, 1.0) if compute_uvs else None)
# Cap vertices after side vertices (also contiguous per cap).
cap_center_bottom_idx: int | None = None
cap_center_top_idx: int | None = None
if radius > 0.0:
cap_center_bottom_pos = np.array([0.0, -half_height, 0.0], dtype=np.float32)[[x_dir, y_dir, z_dir]]
cap_center_bottom_n = (
np.array([0.0, -1.0, 0.0], dtype=np.float32)[[x_dir, y_dir, z_dir]] if compute_normals else None
)
cap_center_bottom_idx = add_vertex(
cap_center_bottom_pos, cap_center_bottom_n, (0.5, 0.5) if compute_uvs else None
)
if top_radius > 0.0:
cap_center_top_pos = np.array([0.0, half_height, 0.0], dtype=np.float32)[[x_dir, y_dir, z_dir]]
cap_center_top_n = (
np.array([0.0, 1.0, 0.0], dtype=np.float32)[[x_dir, y_dir, z_dir]] if compute_normals else None
)
cap_center_top_idx = add_vertex(cap_center_top_pos, cap_center_top_n, (0.5, 0.5) if compute_uvs else None)
cap_ring_bottom_indices = []
if radius > 0.0:
for i in range(segments):
theta = 2 * np.pi * i / segments
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
position = np.array([radius * cos_theta, -half_height, radius * sin_theta], dtype=np.float32)
position = position[[x_dir, y_dir, z_dir]]
cap_normal = (
np.array([0.0, -1.0, 0.0], dtype=np.float32)[[x_dir, y_dir, z_dir]] if compute_normals else None
)
cap_uv = (cos_theta * 0.5 + 0.5, sin_theta * 0.5 + 0.5) if compute_uvs else None
cap_ring_bottom_indices.append(add_vertex(position, cap_normal, cap_uv))
cap_ring_top_indices = []
if top_radius > 0.0:
for i in range(segments):
theta = 2 * np.pi * i / segments
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
position = np.array([top_radius * cos_theta, half_height, top_radius * sin_theta], dtype=np.float32)
position = position[[x_dir, y_dir, z_dir]]
cap_normal = np.array([0.0, 1.0, 0.0], dtype=np.float32)[[x_dir, y_dir, z_dir]] if compute_normals else None
cap_uv = (cos_theta * 0.5 + 0.5, sin_theta * 0.5 + 0.5) if compute_uvs else None
cap_ring_top_indices.append(add_vertex(position, cap_normal, cap_uv))
# Bottom cap
if cap_center_bottom_idx is not None and cap_ring_bottom_indices:
for i in range(segments):
i0 = cap_ring_bottom_indices[i]
i1 = cap_ring_bottom_indices[(i + 1) % segments]
indices.extend([cap_center_bottom_idx, i0, i1])
# Top cap
if cap_center_top_idx is not None and cap_ring_top_indices:
for i in range(segments):
i0 = cap_ring_top_indices[i]
i1 = cap_ring_top_indices[(i + 1) % segments]
indices.extend([cap_center_top_idx, i1, i0])
# Side faces
for i in range(segments):
bottom_i = side_bottom_indices[i]
bottom_next = side_bottom_indices[(i + 1) % segments]
if top_radius > 0.0:
top_i = side_top_indices[i]
top_next = side_top_indices[(i + 1) % segments]
indices.extend([top_i, top_next, bottom_i, top_next, bottom_next, bottom_i])
else:
assert side_apex_index is not None
indices.extend([side_apex_index, bottom_next, bottom_i])
return (
np.asarray(positions, dtype=np.float32),
np.asarray(indices, dtype=np.uint32),
None if normals is None else np.asarray(normals, dtype=np.float32),
None if uvs is None else np.asarray(uvs, dtype=np.float32),
)
def create_mesh_arrow(
base_radius: float,
base_height: float,
*,
cap_radius: float | None = None,
cap_height: float | None = None,
up_axis: int = 1,
segments: int = default_num_segments,
compute_normals: bool = True,
compute_uvs: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None]:
"""Create arrow geometry data with optional normals and UVs."""
if up_axis not in (0, 1, 2):
raise ValueError("up_axis must be between 0 and 2")
if cap_radius is None:
cap_radius = base_radius * 1.8
if cap_height is None:
cap_height = base_height * 0.18
up_vector = np.array([0.0, 0.0, 0.0], dtype=np.float32)
up_vector[up_axis] = 1.0
base_positions, base_indices, base_normals, base_uvs = create_mesh_cylinder(
base_radius,
base_height / 2,
up_axis=up_axis,
segments=segments,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
cap_positions, cap_indices, cap_normals, cap_uvs = create_mesh_cone(
cap_radius,
cap_height / 2,
up_axis=up_axis,
segments=segments,
compute_normals=compute_normals,
compute_uvs=compute_uvs,
)
base_positions = base_positions.copy()
cap_positions = cap_positions.copy()
base_positions += base_height / 2 * up_vector
cap_positions += (base_height + cap_height / 2 - 1e-3 * base_height) * up_vector
positions = np.vstack((base_positions, cap_positions))
indices = np.hstack((base_indices, cap_indices + len(base_positions)))
normals = None
uvs = None
if compute_normals:
normals = np.vstack((base_normals, cap_normals))
if compute_uvs:
uvs = np.vstack((base_uvs, cap_uvs))
return positions.astype(np.float32), indices.astype(np.uint32), normals, uvs
def create_mesh_box(
hx: float,
hy: float,
hz: float,
*,
duplicate_vertices: bool = True,
compute_normals: bool = True,
compute_uvs: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None]:
"""Create box geometry data with optional duplicated vertices, normals, and UVs."""
if duplicate_vertices:
# fmt: off
positions = np.array(
[
[-hx, -hy, -hz], [-hx, -hy, hz], [-hx, hy, hz], [-hx, hy, -hz],
[ hx, -hy, -hz], [ hx, -hy, hz], [ hx, hy, hz], [ hx, hy, -hz],
[-hx, -hy, -hz], [-hx, -hy, hz], [ hx, -hy, hz], [ hx, -hy, -hz],
[-hx, hy, -hz], [-hx, hy, hz], [ hx, hy, hz], [ hx, hy, -hz],
[-hx, -hy, -hz], [-hx, hy, -hz], [ hx, hy, -hz], [ hx, -hy, -hz],
[-hx, -hy, hz], [-hx, hy, hz], [ hx, hy, hz], [ hx, -hy, hz],
],
dtype=np.float32,
)
indices = np.array(
[
0, 1, 2, 0, 2, 3, 4, 6, 5, 4, 7, 6,
8, 10, 9, 8, 11, 10, 12, 13, 14, 12, 14, 15,
16, 17, 18, 16, 18, 19, 20, 22, 21, 20, 23, 22,
],
dtype=np.uint32,
)
# fmt: on
normals = None
uvs = None
if compute_normals:
normals = np.array(
[
[-1, 0, 0],
[-1, 0, 0],
[-1, 0, 0],
[-1, 0, 0],
[1, 0, 0],
[1, 0, 0],
[1, 0, 0],
[1, 0, 0],
[0, -1, 0],
[0, -1, 0],
[0, -1, 0],
[0, -1, 0],
[0, 1, 0],
[0, 1, 0],
[0, 1, 0],
[0, 1, 0],
[0, 0, -1],
[0, 0, -1],
[0, 0, -1],
[0, 0, -1],
[0, 0, 1],
[0, 0, 1],
[0, 0, 1],
[0, 0, 1],
],
dtype=np.float32,
)
if compute_uvs:
face_uv = np.array([[0, 0], [1, 0], [1, 1], [0, 1]], dtype=np.float32)
uvs = np.vstack([face_uv] * 6).astype(np.float32)
return positions, indices, normals, uvs
positions = np.array(
[
[-hx, -hy, -hz],
[hx, -hy, -hz],
[hx, hy, -hz],
[-hx, hy, -hz],
[-hx, -hy, hz],
[hx, -hy, hz],
[hx, hy, hz],
[-hx, hy, hz],
],
dtype=np.float32,
)
# fmt: off
indices = np.array(
[
0, 2, 1, 0, 3, 2, 4, 5, 6, 4, 6, 7,
0, 1, 5, 0, 5, 4, 2, 3, 7, 2, 7, 6,
0, 4, 7, 0, 7, 3, 1, 2, 6, 1, 6, 5,
],
dtype=np.uint32,
)
# fmt: on
normals = None
uvs = None
if compute_normals:
normals = compute_vertex_normals(positions, indices).astype(np.float32)
if compute_uvs:
uvs = np.zeros((len(positions), 2), dtype=np.float32)
return positions, indices, normals, uvs
def create_mesh_plane(
width: float,
length: float,
*,
compute_normals: bool = True,
compute_uvs: bool = True,
) -> tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None]:
"""Create plane geometry data with optional normals and UVs."""
half_width = width / 2
half_length = length / 2
positions = np.array(
[
[-half_width, -half_length, 0.0],
[half_width, -half_length, 0.0],
[half_width, half_length, 0.0],
[-half_width, half_length, 0.0],
],
dtype=np.float32,
)
indices = np.array([0, 1, 2, 0, 2, 3], dtype=np.uint32)
normals = None
uvs = None
if compute_normals:
normals = np.array([[0.0, 0.0, 1.0]] * 4, dtype=np.float32)
if compute_uvs:
uvs = np.array([[0.0, 0.0], [1.0, 0.0], [1.0, 1.0], [0.0, 1.0]], dtype=np.float32)
return positions, indices, normals, uvs
@wp.kernel
def solidify_mesh_kernel(
indices: wp.array2d[int],
vertices: wp.array[wp.vec3],
thickness: wp.array[float],
# outputs
out_vertices: wp.array[wp.vec3],
out_indices: wp.array2d[int],
):
"""Extrude each triangle into a triangular prism (wedge) for solidification.
For each input triangle, creates 6 vertices (3 on each side of the surface)
and 8 output triangles forming a closed wedge. The extrusion is along the
face normal, with per-vertex thickness values.
Launch with dim=num_triangles.
Args:
indices: Triangle indices of shape (num_triangles, 3).
vertices: Vertex positions of shape (num_vertices,).
thickness: Per-vertex thickness values of shape (num_vertices,).
out_vertices: Output vertices of shape (num_vertices * 2,). Each input
vertex produces two output vertices (offset ± thickness along normal).
out_indices: Output triangle indices of shape (num_triangles * 8, 3).
"""
tid = wp.tid()
i = indices[tid, 0]
j = indices[tid, 1]
k = indices[tid, 2]
vi = vertices[i]
vj = vertices[j]
vk = vertices[k]
normal = wp.normalize(wp.cross(vj - vi, vk - vi))
ti = normal * thickness[i]
tj = normal * thickness[j]
tk = normal * thickness[k]
# wedge vertices
vi0 = vi + ti
vi1 = vi - ti
vj0 = vj + tj
vj1 = vj - tj
vk0 = vk + tk
vk1 = vk - tk
i0 = i * 2
i1 = i * 2 + 1
j0 = j * 2
j1 = j * 2 + 1
k0 = k * 2
k1 = k * 2 + 1
out_vertices[i0] = vi0
out_vertices[i1] = vi1
out_vertices[j0] = vj0
out_vertices[j1] = vj1
out_vertices[k0] = vk0
out_vertices[k1] = vk1
oid = tid * 8
out_indices[oid + 0, 0] = i0
out_indices[oid + 0, 1] = j0
out_indices[oid + 0, 2] = k0
out_indices[oid + 1, 0] = j0
out_indices[oid + 1, 1] = k1
out_indices[oid + 1, 2] = k0
out_indices[oid + 2, 0] = j0
out_indices[oid + 2, 1] = j1
out_indices[oid + 2, 2] = k1
out_indices[oid + 3, 0] = j0
out_indices[oid + 3, 1] = i1
out_indices[oid + 3, 2] = j1
out_indices[oid + 4, 0] = j0
out_indices[oid + 4, 1] = i0
out_indices[oid + 4, 2] = i1
out_indices[oid + 5, 0] = j1
out_indices[oid + 5, 1] = i1
out_indices[oid + 5, 2] = k1
out_indices[oid + 6, 0] = i1
out_indices[oid + 6, 1] = i0
out_indices[oid + 6, 2] = k0
out_indices[oid + 7, 0] = i1
out_indices[oid + 7, 1] = k0
out_indices[oid + 7, 2] = k1
def solidify_mesh(
faces: np.ndarray,
vertices: np.ndarray,
thickness: float | list | np.ndarray,
) -> tuple[np.ndarray, np.ndarray]:
"""Convert a surface mesh into a solid mesh by extruding along face normals.
Takes a triangle mesh representing a surface and creates a closed solid
mesh by extruding each triangle into a triangular prism (wedge). Each input
triangle produces 8 output triangles forming the top, bottom, and sides
of the prism.
Args:
faces: Triangle indices of shape (N, 3), where N is the number of
triangles.
vertices: Vertex positions of shape (M, 3), where M is the number of
vertices.
thickness: Extrusion distance from the surface. Can be a single float
(uniform thickness), a list, or an array of shape (M,) for
per-vertex thickness.
Returns:
A tuple containing:
- faces: Output triangle indices of shape (N * 8, 3).
- vertices: Output vertex positions of shape (M * 2, 3).
"""
faces = np.array(faces).reshape(-1, 3)
out_faces = wp.zeros((len(faces) * 8, 3), dtype=wp.int32)
out_vertices = wp.zeros(len(vertices) * 2, dtype=wp.vec3)
if not isinstance(thickness, np.ndarray) and not isinstance(thickness, list):
thickness = [thickness] * len(vertices)
wp.launch(
solidify_mesh_kernel,
dim=len(faces),
inputs=[wp.array(faces, dtype=int), wp.array(vertices, dtype=wp.vec3), wp.array(thickness, dtype=float)],
outputs=[out_vertices, out_faces],
)
faces = out_faces.numpy()
vertices = out_vertices.numpy()
return faces, vertices
================================================
FILE: newton/_src/utils/render.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import warp as wp
def bourke_color_map(low: float, high: float, v: float) -> list[float]:
"""Map a scalar value to an RGB color using Bourke's color ramp.
Apply smooth rainbow color mapping where the value is linearly
interpolated across five color bands: blue → cyan → green → yellow → red.
Values outside the [low, high] range are clamped.
Based on Paul Bourke's colour ramping method:
https://paulbourke.net/texture_colour/colourspace/
Args:
low: Minimum value of the input range.
high: Maximum value of the input range.
v: The scalar value to map to a color.
Returns:
RGB color as a list of three floats in the range [0.0, 1.0].
"""
c = [1.0, 1.0, 1.0]
if v < low:
v = low
if v > high:
v = high
dv = high - low
if v < (low + 0.25 * dv):
c[0] = 0.0
c[1] = 4.0 * (v - low) / dv
elif v < (low + 0.5 * dv):
c[0] = 0.0
c[2] = 1.0 + 4.0 * (low + 0.25 * dv - v) / dv
elif v < (low + 0.75 * dv):
c[0] = 4.0 * (v - low - 0.5 * dv) / dv
c[2] = 0.0
else:
c[1] = 1.0 + 4.0 * (low + 0.75 * dv - v) / dv
c[2] = 0.0
return c
@wp.kernel
def copy_rgb_frame_uint8(
input_img: wp.array[wp.uint8],
width: int,
height: int,
output_img: wp.array3d[wp.uint8],
):
"""Copy a flat RGB buffer to a 3D array with vertical flip.
Converts a flat RGB uint8 buffer (as produced by OpenGL readPixels) into
a 3D array of shape (height, width, 3) with the image flipped vertically
to convert from OpenGL's bottom-left origin to top-left origin.
Launch with dim=(width, height).
Args:
input_img: Flat uint8 array of size (width * height * 3) containing
packed RGB values.
width: Image width in pixels.
height: Image height in pixels.
output_img: Output array of shape (height, width, 3) to write the
flipped RGB image.
"""
w, v = wp.tid()
pixel = v * width + w
pixel *= 3
# flip vertically (OpenGL coordinates start at bottom)
v = height - v - 1
output_img[v, w, 0] = input_img[pixel + 0]
output_img[v, w, 1] = input_img[pixel + 1]
output_img[v, w, 2] = input_img[pixel + 2]
================================================
FILE: newton/_src/utils/selection.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import functools
from fnmatch import fnmatch
from types import NoneType
from typing import TYPE_CHECKING, Any
import warp as wp
from warp.types import is_array
from ..sim import Control, JointType, Model, State, eval_fk, eval_jacobian, eval_mass_matrix
if TYPE_CHECKING:
from newton_actuators import Actuator
AttributeFrequency = Model.AttributeFrequency
@wp.kernel
def set_model_articulation_mask_kernel(
world_arti_mask: wp.array2d[bool], # (world, arti) mask in ArticulationView
view_to_model_map: wp.array2d[int], # map (world, arti) indices to Model articulation id
model_articulation_mask: wp.array[bool], # output: mask of Model articulation indices
):
"""
Set Model articulation mask from a 2D (world, arti) mask in an ArticulationView.
"""
world, arti = wp.tid()
if world_arti_mask[world, arti]:
model_articulation_mask[view_to_model_map[world, arti]] = True
@wp.kernel
def set_model_articulation_mask_per_world_kernel(
world_mask: wp.array[bool], # world mask in ArticulationView
view_to_model_map: wp.array2d[int], # map (world, arti) indices to Model articulation id
model_articulation_mask: wp.array[bool], # output: mask of Model articulation indices
):
"""
Set Model articulation mask from a 1D world mask in an ArticulationView.
"""
world, arti = wp.tid()
if world_mask[world]:
model_articulation_mask[view_to_model_map[world, arti]] = True
# @wp.kernel
# def set_articulation_attribute_1d_kernel(
# view_mask: wp.array2d[bool], # (world, arti) mask in ArticulationView
# values: Any, # 1d array or indexedarray
# attrib: Any, # 1d array or indexedarray
# ):
# i = wp.tid()
# if view_mask[i]:
# attrib[i] = values[i]
# @wp.kernel
# def set_articulation_attribute_2d_kernel(
# view_mask: wp.array2d[bool], # (world, arti) mask in ArticulationView
# values: Any, # 2d array or indexedarray
# attrib: Any, # 2d array or indexedarray
# ):
# i, j = wp.tid()
# if view_mask[i, j]:
# attrib[i, j] = values[i, j]
@wp.kernel
def set_articulation_attribute_3d_kernel(
view_mask: wp.array2d[bool], # (world, arti) mask in ArticulationView
values: Any, # 3d array or indexedarray
attrib: Any, # 3d array or indexedarray
):
i, j, k = wp.tid()
if view_mask[i, j]:
attrib[i, j, k] = values[i, j, k]
@wp.kernel
def set_articulation_attribute_4d_kernel(
view_mask: wp.array2d[bool], # (world, arti) mask in ArticulationView
values: Any, # 4d array or indexedarray
attrib: Any, # 4d array or indexedarray
):
i, j, k, l = wp.tid()
if view_mask[i, j]:
attrib[i, j, k, l] = values[i, j, k, l]
# @wp.kernel
# def set_articulation_attribute_1d_per_world_kernel(
# view_mask: wp.array[bool], # world mask in ArticulationView
# values: Any, # 1d array or indexedarray
# attrib: Any, # 1d array or indexedarray
# ):
# i = wp.tid()
# if view_mask[i]:
# attrib[i] = values[i]
# @wp.kernel
# def set_articulation_attribute_2d_per_world_kernel(
# view_mask: wp.array[bool], # world mask in ArticulationView
# values: Any, # 2d array or indexedarray
# attrib: Any, # 2d array or indexedarray
# ):
# i, j = wp.tid()
# if view_mask[i]:
# attrib[i, j] = values[i, j]
@wp.kernel
def set_articulation_attribute_3d_per_world_kernel(
view_mask: wp.array[bool], # world mask in ArticulationView
values: Any, # 3d array or indexedarray
attrib: Any, # 3d array or indexedarray
):
i, j, k = wp.tid()
if view_mask[i]:
attrib[i, j, k] = values[i, j, k]
@wp.kernel
def set_articulation_attribute_4d_per_world_kernel(
view_mask: wp.array[bool], # world mask in ArticulationView
values: Any, # 4d array or indexedarray
attrib: Any, # 4d array or indexedarray
):
i, j, k, l = wp.tid()
if view_mask[i]:
attrib[i, j, k, l] = values[i, j, k, l]
# explicit overloads to avoid module reloading
for dtype in [float, int, wp.transform, wp.spatial_vector]:
for src_array_type in [wp.array, wp.indexedarray]:
for dst_array_type in [wp.array, wp.indexedarray]:
# wp.overload(
# set_articulation_attribute_1d_kernel,
# {"values": src_array_type(dtype=dtype, ndim=1), "attrib": dst_array_type(dtype=dtype, ndim=1)},
# )
# wp.overload(
# set_articulation_attribute_2d_kernel,
# {"values": src_array_type(dtype=dtype, ndim=2), "attrib": dst_array_type(dtype=dtype, ndim=2)},
# )
wp.overload(
set_articulation_attribute_3d_kernel,
{"values": src_array_type(dtype=dtype, ndim=3), "attrib": dst_array_type(dtype=dtype, ndim=3)},
)
wp.overload(
set_articulation_attribute_4d_kernel,
{"values": src_array_type(dtype=dtype, ndim=4), "attrib": dst_array_type(dtype=dtype, ndim=4)},
)
wp.overload(
set_articulation_attribute_3d_per_world_kernel,
{"values": src_array_type(dtype=dtype, ndim=3), "attrib": dst_array_type(dtype=dtype, ndim=3)},
)
wp.overload(
set_articulation_attribute_4d_per_world_kernel,
{"values": src_array_type(dtype=dtype, ndim=4), "attrib": dst_array_type(dtype=dtype, ndim=4)},
)
# ========================================================================================
# Differentiable gather kernels for indexed -> contiguous copy
@wp.kernel
def _gather_indexed_3d_kernel(
src: Any, # 3d wp.array (pre-indexed, has .grad)
indices: wp.array[int], # index mapping for dimension 2
dst: Any, # 3d wp.array (contiguous staging buffer, has .grad)
):
i, j, k = wp.tid()
dst[i, j, k] = src[i, j, indices[k]]
@wp.kernel
def _gather_indexed_4d_kernel(
src: Any, # 4d wp.array
indices: wp.array[int],
dst: Any, # 4d wp.array
):
i, j, k, l = wp.tid()
dst[i, j, k, l] = src[i, j, indices[k], l]
for _dtype in [float, wp.transform, wp.spatial_vector]:
wp.overload(
_gather_indexed_3d_kernel,
{"src": wp.array3d[_dtype], "dst": wp.array3d[_dtype]},
)
wp.overload(
_gather_indexed_4d_kernel,
{"src": wp.array4d[_dtype], "dst": wp.array4d[_dtype]},
)
# ========================================================================================
# Actuator scatter/gather kernels
@wp.kernel
def build_actuator_dof_mapping_slice_kernel(
actuator_input_indices: wp.array[wp.uint32],
actuators_per_world: int,
base_offset: int,
slice_start: int,
slice_stop: int,
stride_within_worlds: int,
count_per_world: int,
dofs_per_arti: int,
dofs_per_world: int,
num_worlds: int,
mapping: wp.array[int],
):
"""Build DOF-to-actuator mapping for slice-based view selection.
Iterates over first world's actuators only, replicates pattern to all worlds.
For each actuator, checks all articulations in the view to find matching DOF ranges.
"""
local_idx = wp.tid() # 0 to actuators_per_world-1
# Get global DOF from first world's actuator entry
global_dof = int(actuator_input_indices[local_idx])
for arti_idx in range(count_per_world):
arti_global_start = base_offset + arti_idx * stride_within_worlds + slice_start
arti_global_stop = base_offset + arti_idx * stride_within_worlds + slice_stop
if global_dof >= arti_global_start and global_dof < arti_global_stop:
view_local_pos = arti_idx * dofs_per_arti + (global_dof - arti_global_start)
# Replicate to all worlds
for world_idx in range(num_worlds):
view_pos = world_idx * dofs_per_world + view_local_pos
actuator_idx = world_idx * actuators_per_world + local_idx
mapping[view_pos] = actuator_idx
break
@wp.kernel
def build_actuator_dof_mapping_indices_kernel(
actuator_input_indices: wp.array[wp.uint32],
view_dof_indices: wp.array[int],
base_offset: int,
stride_within_worlds: int,
count_per_world: int,
actuators_per_world: int,
dofs_per_arti: int,
dofs_per_world: int,
num_worlds: int,
mapping: wp.array[int],
):
"""Build DOF-to-actuator mapping for index-array-based view selection.
Iterates over first world's actuators only, replicates pattern to all worlds.
For each actuator, checks all articulations in the view to find matching DOF indices.
"""
local_idx = wp.tid() # 0 to actuators_per_world-1
global_dof = int(actuator_input_indices[local_idx])
for arti_idx in range(count_per_world):
arti_base = base_offset + arti_idx * stride_within_worlds
for i in range(dofs_per_arti):
# view_dof_indices[i] is local within the articulation, add arti_base to get global
if arti_base + view_dof_indices[i] == global_dof:
view_local_pos = arti_idx * dofs_per_arti + i
# Replicate to all worlds
for world_idx in range(num_worlds):
view_pos = world_idx * dofs_per_world + view_local_pos
actuator_idx = world_idx * actuators_per_world + local_idx
mapping[view_pos] = actuator_idx
break
@wp.kernel
def gather_actuator_by_indices_kernel(
src: wp.array[float],
indices: wp.array[int],
dst: wp.array[float],
):
"""Gather values from src at specified indices into dst. Index -1 means skip (leave dst unchanged)."""
tid = wp.tid()
idx = indices[tid]
if idx >= 0:
dst[tid] = src[idx]
@wp.kernel
def scatter_actuator_with_mask_kernel(
values: wp.array2d[float],
mapping: wp.array[int],
mask: wp.array[bool],
dofs_per_world: int,
dst: wp.array[float],
):
"""Scatter actuator values with articulation mask support.
values: shape (world_count, dofs_per_world)
mapping: flat array mapping DOF positions to actuator indices (-1 = not actuated)
mask: per-world mask, shape (world_count,)
dst: flat actuator parameter array
"""
world_idx, local_idx = wp.tid()
if mask[world_idx]:
flat_idx = world_idx * dofs_per_world + local_idx
actuator_idx = mapping[flat_idx]
if actuator_idx >= 0:
dst[actuator_idx] = values[world_idx, local_idx]
# NOTE: Python slice objects are not hashable in Python < 3.12, so we use this instead.
class Slice:
def __init__(self, start=None, stop=None):
self.start = start
self.stop = stop
def __hash__(self):
return hash((self.start, self.stop))
def __eq__(self, other):
return isinstance(other, Slice) and self.start == other.start and self.stop == other.stop
def __str__(self):
return f"({self.start}, {self.stop})"
def get(self):
return slice(self.start, self.stop)
class FrequencyLayout:
def __init__(
self,
offset: int,
stride_between_worlds: int,
stride_within_worlds: int,
value_count: int,
indices: list[int],
device,
):
self.offset = offset # number of values to skip at the beginning of attribute array
self.stride_between_worlds = stride_between_worlds
self.stride_within_worlds = stride_within_worlds
self.value_count = value_count
self.slice = None
self.indices = None
if len(indices) == 0:
self.slice = slice(0, 0)
elif is_contiguous_slice(indices):
self.slice = slice(indices[0], indices[-1] + 1)
else:
self.indices = wp.array(indices, dtype=int, device=device)
@property
def is_contiguous(self):
return self.slice is not None
@property
def selected_value_count(self):
if self.slice is not None:
return self.slice.stop - self.slice.start
else:
return len(self.indices)
def __str__(self):
indices = self.indices if self.indices is not None else self.slice
return f"FrequencyLayout(\n offset: {self.offset}\n stride_between_worlds: {self.stride_between_worlds}\n stride_within_worlds: {self.stride_within_worlds}\n indices: {indices}\n)"
def get_name_from_label(label: str):
"""Return the leaf component of a hierarchical label.
Args:
label: Slash-delimited label string (e.g. ``"robot/link1"``).
Returns:
The final path component of the label.
"""
return label.split("/")[-1]
def find_matching_ids(pattern: str, labels: list[str], world_ids, world_count: int):
grouped_ids = [[] for _ in range(world_count)] # ids grouped by world (exclude world -1)
global_ids = [] # ids in world -1
for id, label in enumerate(labels):
if fnmatch(label, pattern):
world = world_ids[id]
if world == -1:
global_ids.append(id)
elif world >= 0 and world < world_count:
grouped_ids[world].append(id)
else:
raise ValueError(f"World index out of range: {world}")
return grouped_ids, global_ids
def match_labels(labels: list[str], pattern: str | list[str] | list[int]) -> list[int]:
"""Find indices of elements in ``labels`` that match ``pattern``.
See :ref:`label-matching` for the pattern syntax accepted across Newton APIs.
Args:
labels: List of label strings to match against.
pattern: A ``str`` is matched via :func:`fnmatch.fnmatch` against each label.
A ``list[str]`` matches any pattern.
A ``list[int]`` is returned as-is (indices used directly).
Mixing ``str`` and ``int`` in the same list is not allowed.
Returns:
Unique list of matching indices, or ``pattern`` itself for ``list[int]``.
Raises:
TypeError: If list elements are not all ``str`` or all ``int``.
"""
if isinstance(pattern, str):
return [idx for idx, label in enumerate(labels) if fnmatch(label, pattern)]
if not isinstance(pattern, list):
raise TypeError(f"Expected a list of str patterns or a list of int indices, got: {type(pattern)}")
if len(pattern) == 0:
return pattern
validation_failure = False
if isinstance(pattern[0], int):
# fast path for list[int]
for item in pattern:
if not isinstance(item, int):
validation_failure = True
break
if not validation_failure:
return pattern
elif all(isinstance(item, str) for item in pattern):
return [idx for idx, label in enumerate(labels) if any(fnmatch(label, p) for p in pattern)]
types = {type(item).__name__ for item in pattern}
raise TypeError(f"Expected a list of str patterns or a list of int indices, got: {', '.join(sorted(types))}")
def all_equal(values):
return all(x == values[0] for x in values)
def list_of_lists(n):
return [[] for _ in range(n)]
def get_world_offset(world_ids):
for i in range(len(world_ids)):
if world_ids[i] > -1:
return i
return None
def is_contiguous_slice(indices):
n = len(indices)
if n > 1:
for i in range(1, n):
if indices[i] != indices[i - 1] + 1:
return False
return True
class ArticulationView:
"""
ArticulationView provides a flexible interface for selecting and manipulating
subsets of articulations and their joints, links, and shapes within a Model.
It supports pattern-based selection, inclusion/exclusion filters, and convenient
attribute access and modification for simulation and control.
This is useful in RL and batched simulation workflows where a single policy or
control routine operates on many parallel environments with consistent tensor shapes.
Example:
.. code-block:: python
import newton
view = newton.selection.ArticulationView(model, pattern="robot*")
q = view.get_dof_positions(state)
q_np = q.numpy()
q_np[..., 0] = 0.0
view.set_dof_positions(state, q_np)
The ``pattern``, ``include_joints``, ``exclude_joints``, ``include_links``,
and ``exclude_links`` parameters accept label patterns — see :ref:`label-matching`.
Args:
model (Model): The model containing the articulations.
pattern (str): Pattern to match articulation labels.
include_joints (list[str] | list[int] | None): List of joint names, patterns, or indices to include.
exclude_joints (list[str] | list[int] | None): List of joint names, patterns, or indices to exclude.
include_links (list[str] | list[int] | None): List of link names, patterns, or indices to include.
exclude_links (list[str] | list[int] | None): List of link names, patterns, or indices to exclude.
include_joint_types (list[int] | None): List of joint types to include.
exclude_joint_types (list[int] | None): List of joint types to exclude.
verbose (bool | None): If True, prints selection summary.
"""
def __init__(
self,
model: Model,
pattern: str,
include_joints: list[str] | list[int] | None = None,
exclude_joints: list[str] | list[int] | None = None,
include_links: list[str] | list[int] | None = None,
exclude_links: list[str] | list[int] | None = None,
include_joint_types: list[int] | None = None,
exclude_joint_types: list[int] | None = None,
verbose: bool | None = None,
):
self.model = model
self.device = model.device
if verbose is None:
verbose = wp.config.verbose
# FIXME: avoid/reduce this readback?
model_articulation_start = model.articulation_start.numpy()
model_articulation_world = model.articulation_world.numpy()
model_joint_type = model.joint_type.numpy()
model_joint_child = model.joint_child.numpy()
model_joint_q_start = model.joint_q_start.numpy()
model_joint_qd_start = model.joint_qd_start.numpy()
# get articulation ids grouped by world
articulation_ids, global_articulation_ids = find_matching_ids(
pattern, model.articulation_label, model_articulation_world, model.world_count
)
# determine articulation counts per world
world_count = model.world_count
articulation_count = 0
counts_per_world = [0] * world_count
for world_id in range(world_count):
count = len(articulation_ids[world_id])
counts_per_world[world_id] += count
articulation_count += count
# can't mix global and per-world articulations in the same view
if articulation_count > 0 and global_articulation_ids:
raise ValueError(
f"Articulation pattern '{pattern}' matches global and per-world articulations, which is currently not supported"
)
# handle scenes with only global articulations
if articulation_count == 0 and global_articulation_ids:
world_count = 1
articulation_count = len(global_articulation_ids)
counts_per_world = [articulation_count]
articulation_ids = [global_articulation_ids]
if articulation_count == 0:
raise KeyError(f"No articulations matching pattern '{pattern}'")
if not all_equal(counts_per_world):
raise ValueError("Varying articulation counts per world are not supported")
count_per_world = counts_per_world[0]
# use the first articulation as a "template"
arti_0 = articulation_ids[0][0]
arti_joint_ids = []
arti_joint_names = []
arti_joint_types = []
arti_link_ids = []
arti_link_names = []
arti_shape_ids = []
arti_shape_names = []
# gather joint info
arti_joint_begin = int(model_articulation_start[arti_0])
arti_joint_end = int(model_articulation_start[arti_0 + 1])
arti_joint_count = arti_joint_end - arti_joint_begin
arti_joint_dof_begin = int(model_joint_qd_start[arti_joint_begin])
arti_joint_dof_end = int(model_joint_qd_start[arti_joint_end])
arti_joint_dof_count = arti_joint_dof_end - arti_joint_dof_begin
arti_joint_coord_begin = int(model_joint_q_start[arti_joint_begin])
arti_joint_coord_end = int(model_joint_q_start[arti_joint_end])
arti_joint_coord_count = arti_joint_coord_end - arti_joint_coord_begin
for joint_id in range(arti_joint_begin, arti_joint_end):
# joint_id = arti_joint_begin + idx
arti_joint_ids.append(joint_id)
arti_joint_names.append(get_name_from_label(model.joint_label[joint_id]))
arti_joint_types.append(model_joint_type[joint_id])
link_id = int(model_joint_child[joint_id])
arti_link_ids.append(link_id)
# use link order as they appear in the model
arti_link_ids = sorted(arti_link_ids)
arti_link_count = len(arti_link_ids)
for link_id in arti_link_ids:
arti_link_names.append(get_name_from_label(model.body_label[link_id]))
arti_shape_ids.extend(model.body_shapes[link_id])
# use shape order as they appear in the model
arti_shape_ids = sorted(arti_shape_ids)
arti_shape_count = len(arti_shape_ids)
for shape_id in arti_shape_ids:
arti_shape_names.append(get_name_from_label(model.shape_label[shape_id]))
# compute counts and offsets of joints, links, etc.
joint_starts = list_of_lists(world_count)
joint_counts = list_of_lists(world_count)
joint_dof_starts = list_of_lists(world_count)
joint_dof_counts = list_of_lists(world_count)
joint_coord_starts = list_of_lists(world_count)
joint_coord_counts = list_of_lists(world_count)
root_joint_types = list_of_lists(world_count)
link_starts = list_of_lists(world_count)
shape_starts = list_of_lists(world_count)
shape_counts = list_of_lists(world_count)
for world_id in range(world_count):
for arti_id in articulation_ids[world_id]:
# joints
joint_start = int(model_articulation_start[arti_id])
joint_end = int(model_articulation_start[arti_id + 1])
joint_starts[world_id].append(joint_start)
joint_counts[world_id].append(joint_end - joint_start)
# joint dofs
joint_dof_start = int(model_joint_qd_start[joint_start])
joint_dof_end = int(model_joint_qd_start[joint_end])
joint_dof_starts[world_id].append(joint_dof_start)
joint_dof_counts[world_id].append(joint_dof_end - joint_dof_start)
# joint coords
joint_coord_start = int(model_joint_q_start[joint_start])
joint_coord_end = int(model_joint_q_start[joint_end])
joint_coord_starts[world_id].append(joint_coord_start)
joint_coord_counts[world_id].append(joint_coord_end - joint_coord_start)
# root joint types
root_joint_types[world_id].append(int(model_joint_type[joint_start]))
# links and shapes
link_ids = []
shape_ids = []
for j in range(joint_start, joint_end):
link_id = int(model_joint_child[j])
link_ids.append(link_id)
link_shapes = model.body_shapes.get(link_id, [])
shape_ids.extend(link_shapes)
link_starts[world_id].append(min(link_ids))
num_shapes = len(shape_ids)
if num_shapes > 0:
shape_starts[world_id].append(min(shape_ids))
else:
shape_starts[world_id].append(-1)
shape_counts[world_id].append(num_shapes)
# make sure counts are the same for all articulations
# NOTE: we currently assume that link count is the same as joint count
if not (
all_equal(joint_counts)
and all_equal(joint_dof_counts)
and all_equal(joint_coord_counts)
and all_equal(root_joint_types)
and all_equal(shape_counts)
):
raise ValueError("Articulations are not identical")
self.root_joint_type = root_joint_types[0][0]
# fixed base means that all linear and angular degrees of freedom are locked at the root
self.is_fixed_base = self.root_joint_type == JointType.FIXED
# floating base means that all linear and angular degrees of freedom are unlocked at the root
# (though there might be constraints like distance)
self.is_floating_base = self.root_joint_type in (JointType.FREE, JointType.DISTANCE)
joint_offset = joint_starts[0][0]
joint_dof_offset = joint_dof_starts[0][0]
joint_coord_offset = joint_coord_starts[0][0]
link_offset = link_starts[0][0]
if arti_shape_count > 0:
shape_offset = shape_starts[0][0]
else:
shape_offset = 0
# compute "outer" strides (strides between worlds)
if world_count > 1:
outer_joint_strides = []
outer_joint_dof_strides = []
outer_joint_coord_strides = []
outer_link_strides = []
outer_shape_strides = []
for world_id in range(1, world_count):
outer_joint_strides.append(joint_starts[world_id][0] - joint_starts[world_id - 1][0])
outer_joint_dof_strides.append(joint_dof_starts[world_id][0] - joint_dof_starts[world_id - 1][0])
outer_joint_coord_strides.append(joint_coord_starts[world_id][0] - joint_coord_starts[world_id - 1][0])
outer_link_strides.append(link_starts[world_id][0] - link_starts[world_id - 1][0])
outer_shape_strides.append(shape_starts[world_id][0] - shape_starts[world_id - 1][0])
# make sure outer strides are uniform
if not (
all_equal(outer_joint_strides)
and all_equal(outer_joint_dof_strides)
and all_equal(outer_joint_coord_strides)
and all_equal(outer_link_strides)
and all_equal(outer_shape_strides)
):
raise ValueError("Non-uniform strides between worlds are not supported")
outer_joint_stride = outer_joint_strides[0]
outer_joint_dof_stride = outer_joint_dof_strides[0]
outer_joint_coord_stride = outer_joint_coord_strides[0]
outer_link_stride = outer_link_strides[0]
outer_shape_stride = outer_shape_strides[0]
else:
outer_joint_stride = arti_joint_count
outer_joint_dof_stride = arti_joint_dof_count
outer_joint_coord_stride = arti_joint_coord_count
outer_link_stride = arti_link_count
outer_shape_stride = arti_shape_count
# compute "inner" strides (strides within worlds)
if count_per_world > 1:
inner_joint_strides = list_of_lists(world_count)
inner_joint_dof_strides = list_of_lists(world_count)
inner_joint_coord_strides = list_of_lists(world_count)
inner_link_strides = list_of_lists(world_count)
inner_shape_strides = list_of_lists(world_count)
for world_id in range(world_count):
for i in range(1, count_per_world):
inner_joint_strides[world_id].append(joint_starts[world_id][i] - joint_starts[world_id][i - 1])
inner_joint_dof_strides[world_id].append(
joint_dof_starts[world_id][i] - joint_dof_starts[world_id][i - 1]
)
inner_joint_coord_strides[world_id].append(
joint_coord_starts[world_id][i] - joint_coord_starts[world_id][i - 1]
)
inner_link_strides[world_id].append(link_starts[world_id][i] - link_starts[world_id][i - 1])
inner_shape_strides[world_id].append(shape_starts[world_id][i] - shape_starts[world_id][i - 1])
# make sure inner strides are uniform
if not (
all_equal(inner_joint_strides)
and all_equal(inner_joint_dof_strides)
and all_equal(inner_joint_coord_strides)
and all_equal(inner_link_strides)
and all_equal(inner_shape_strides)
):
raise ValueError("Non-uniform strides within worlds are not supported")
inner_joint_stride = inner_joint_strides[0][0]
inner_joint_dof_stride = inner_joint_dof_strides[0][0]
inner_joint_coord_stride = inner_joint_coord_strides[0][0]
inner_link_stride = inner_link_strides[0][0]
inner_shape_stride = inner_shape_strides[0][0]
else:
inner_joint_stride = arti_joint_count
inner_joint_dof_stride = arti_joint_dof_count
inner_joint_coord_stride = arti_joint_coord_count
inner_link_stride = arti_link_count
inner_shape_stride = arti_shape_count
# create joint inclusion set
if include_joints is None and include_joint_types is None:
joint_include_indices = set(range(arti_joint_count))
else:
joint_include_indices = set()
if include_joints is not None:
joint_include_indices.update(
idx for idx in match_labels(arti_joint_names, include_joints) if 0 <= idx < arti_joint_count
)
if include_joint_types is not None:
for idx in range(arti_joint_count):
if arti_joint_types[idx] in include_joint_types:
joint_include_indices.add(idx)
# create joint exclusion set
joint_exclude_indices = set()
if exclude_joints is not None:
joint_exclude_indices.update(
idx for idx in match_labels(arti_joint_names, exclude_joints) if 0 <= idx < arti_joint_count
)
if exclude_joint_types is not None:
for idx in range(arti_joint_count):
if arti_joint_types[idx] in exclude_joint_types:
joint_exclude_indices.add(idx)
# create link inclusion set
if include_links is None:
link_include_indices = set(range(arti_link_count))
else:
link_include_indices = {
idx for idx in match_labels(arti_link_names, include_links) if 0 <= idx < arti_link_count
}
# create link exclusion set
link_exclude_indices = set()
if exclude_links is not None:
link_exclude_indices.update(
idx for idx in match_labels(arti_link_names, exclude_links) if 0 <= idx < arti_link_count
)
# compute selected indices
selected_joint_indices = sorted(joint_include_indices - joint_exclude_indices)
selected_link_indices = sorted(link_include_indices - link_exclude_indices)
self.joint_names = []
self.joint_dof_names = []
self.joint_dof_counts = []
self.joint_coord_names = []
self.joint_coord_counts = []
self.link_names = []
self.link_shapes = []
self.shape_names = []
# populate info for selected joints and dofs
selected_joint_dof_indices = []
selected_joint_coord_indices = []
for joint_idx in selected_joint_indices:
joint_id = arti_joint_ids[joint_idx]
joint_name = arti_joint_names[joint_idx]
self.joint_names.append(joint_name)
# joint dofs
dof_begin = int(model_joint_qd_start[joint_id])
dof_end = int(model_joint_qd_start[joint_id + 1])
dof_count = dof_end - dof_begin
self.joint_dof_counts.append(dof_count)
if dof_count == 1:
self.joint_dof_names.append(joint_name)
selected_joint_dof_indices.append(dof_begin - joint_dof_offset)
elif dof_count > 1:
for dof in range(dof_count):
self.joint_dof_names.append(f"{joint_name}:{dof}")
selected_joint_dof_indices.append(dof_begin + dof - joint_dof_offset)
# joint coords
coord_begin = int(model_joint_q_start[joint_id])
coord_end = int(model_joint_q_start[joint_id + 1])
coord_count = coord_end - coord_begin
self.joint_coord_counts.append(coord_count)
if coord_count == 1:
self.joint_coord_names.append(joint_name)
selected_joint_coord_indices.append(coord_begin - joint_coord_offset)
elif coord_count > 1:
for coord in range(coord_count):
self.joint_coord_names.append(f"{joint_name}:{coord}")
selected_joint_coord_indices.append(coord_begin + coord - joint_coord_offset)
# populate info for selected links and shapes
selected_shape_indices = []
shape_link_idx = {} # map arti_shape_idx to local link index in the view
for link_idx, arti_link_idx in enumerate(selected_link_indices):
body_id = arti_link_ids[arti_link_idx]
self.link_names.append(arti_link_names[arti_link_idx])
shape_ids = model.body_shapes[body_id]
for shape_id in shape_ids:
arti_shape_idx = arti_shape_ids.index(shape_id)
selected_shape_indices.append(arti_shape_idx)
shape_link_idx[arti_shape_idx] = link_idx
self.link_shapes.append([])
selected_shape_indices = sorted(selected_shape_indices)
for shape_idx, arti_shape_idx in enumerate(selected_shape_indices):
self.shape_names.append(arti_shape_names[arti_shape_idx])
link_idx = shape_link_idx[arti_shape_idx]
self.link_shapes[link_idx].append(shape_idx)
# selection counts
self.count = articulation_count
self.world_count = world_count
self.count_per_world = count_per_world
self.joint_count = len(selected_joint_indices)
self.joint_dof_count = len(selected_joint_dof_indices)
self.joint_coord_count = len(selected_joint_coord_indices)
self.link_count = len(selected_link_indices)
self.shape_count = len(selected_shape_indices)
# TODO: document the layout conventions and requirements
#
# |ooXXXoXXXoXXXooo|ooXXXoXXXoXXXooo|ooXXXoXXXoXXXooo|ooXXXoXXXoXXXooo|
# | ^ ^ ^ | ^ ^ ^ | ^ ^ ^ | ^ ^ ^ |
#
self.frequency_layouts = {
AttributeFrequency.JOINT: FrequencyLayout(
joint_offset,
outer_joint_stride,
inner_joint_stride,
arti_joint_count,
selected_joint_indices,
self.device,
),
AttributeFrequency.JOINT_DOF: FrequencyLayout(
joint_dof_offset,
outer_joint_dof_stride,
inner_joint_dof_stride,
arti_joint_dof_count,
selected_joint_dof_indices,
self.device,
),
AttributeFrequency.JOINT_COORD: FrequencyLayout(
joint_coord_offset,
outer_joint_coord_stride,
inner_joint_coord_stride,
arti_joint_coord_count,
selected_joint_coord_indices,
self.device,
),
AttributeFrequency.BODY: FrequencyLayout(
link_offset, outer_link_stride, inner_link_stride, arti_link_count, selected_link_indices, self.device
),
AttributeFrequency.SHAPE: FrequencyLayout(
shape_offset,
outer_shape_stride,
inner_shape_stride,
arti_shape_count,
selected_shape_indices,
self.device,
),
}
# ========================================================================================
# Tendon discovery (for MuJoCo fixed tendons)
# Tendons are associated with articulations by checking which articulation owns all their joints
self.tendon_count = 0
self.tendon_names = []
# Check if model has MuJoCo tendon attributes
if hasattr(model, "mujoco") and hasattr(model.mujoco, "tendon_joint"):
mujoco_attrs = model.mujoco
tendon_world_arr = mujoco_attrs.tendon_world.numpy()
tendon_joint_adr_arr = mujoco_attrs.tendon_joint_adr.numpy()
tendon_joint_num_arr = mujoco_attrs.tendon_joint_num.numpy()
tendon_joint_arr = mujoco_attrs.tendon_joint.numpy()
total_tendon_count = len(tendon_world_arr)
if total_tendon_count > 0:
# Build a mapping from joint index to articulation index
joint_to_articulation = {}
for arti_idx in range(len(model_articulation_start) - 1):
joint_begin = int(model_articulation_start[arti_idx])
joint_end = int(model_articulation_start[arti_idx + 1])
for j in range(joint_begin, joint_end):
joint_to_articulation[j] = arti_idx
# For each articulation, find its tendons
# A tendon belongs to an articulation if ALL its joints belong to that articulation
tendon_to_articulation = {}
for tendon_idx in range(total_tendon_count):
joint_adr = int(tendon_joint_adr_arr[tendon_idx])
joint_num = int(tendon_joint_num_arr[tendon_idx])
if joint_num == 0:
continue # Skip empty tendons
articulations_in_tendon = set()
for j in range(joint_adr, joint_adr + joint_num):
joint_id = int(tendon_joint_arr[j])
if joint_id in joint_to_articulation:
articulations_in_tendon.add(joint_to_articulation[joint_id])
if len(articulations_in_tendon) > 1:
raise ValueError(
f"Tendon {tendon_idx} spans multiple articulations {articulations_in_tendon}, "
f"which is not supported by ArticulationView"
)
if len(articulations_in_tendon) == 1:
tendon_to_articulation[tendon_idx] = articulations_in_tendon.pop()
# Group tendons by (world, articulation) and filter for selected articulations
# Build a set of selected articulation IDs for fast lookup
selected_arti_set = set()
for world_artis in articulation_ids:
for arti_id in world_artis:
selected_arti_set.add(arti_id)
# Find tendons belonging to the template articulation (first selected articulation)
template_arti_id = articulation_ids[0][0]
arti_tendon_ids = [] # Tendon indices belonging to the template articulation
for tendon_idx, arti_id in tendon_to_articulation.items():
if arti_id == template_arti_id:
arti_tendon_ids.append(tendon_idx)
arti_tendon_ids = sorted(arti_tendon_ids)
arti_tendon_count = len(arti_tendon_ids)
if arti_tendon_count > 0:
# Compute tendon layout similar to joints
# Group tendons by world and articulation to compute strides
tendon_starts = list_of_lists(world_count)
tendon_counts = list_of_lists(world_count)
for world_id in range(world_count):
for arti_id in articulation_ids[world_id]:
arti_tendons = [t for t, a in tendon_to_articulation.items() if a == arti_id]
arti_tendons = sorted(arti_tendons)
if len(arti_tendons) > 0:
tendon_starts[world_id].append(min(arti_tendons))
else:
tendon_starts[world_id].append(-1)
tendon_counts[world_id].append(len(arti_tendons))
# Validate uniform tendon counts
if not all_equal(tendon_counts):
raise ValueError("Articulations have different tendon counts, which is not supported")
tendon_offset = arti_tendon_ids[0] if arti_tendon_ids else 0
# Compute outer stride (between worlds)
if world_count > 1:
outer_tendon_strides = []
for world_id in range(1, world_count):
if tendon_starts[world_id][0] >= 0 and tendon_starts[world_id - 1][0] >= 0:
outer_tendon_strides.append(tendon_starts[world_id][0] - tendon_starts[world_id - 1][0])
if outer_tendon_strides and not all_equal(outer_tendon_strides):
raise ValueError("Non-uniform tendon strides between worlds are not supported")
outer_tendon_stride = outer_tendon_strides[0] if outer_tendon_strides else arti_tendon_count
else:
outer_tendon_stride = arti_tendon_count
# Compute inner stride (within worlds)
if count_per_world > 1:
inner_tendon_strides = list_of_lists(world_count)
for world_id in range(world_count):
for i in range(1, count_per_world):
if tendon_starts[world_id][i] >= 0 and tendon_starts[world_id][i - 1] >= 0:
inner_tendon_strides[world_id].append(
tendon_starts[world_id][i] - tendon_starts[world_id][i - 1]
)
# Flatten and check uniformity
flat_inner = [s for lst in inner_tendon_strides for s in lst]
if flat_inner and not all_equal(flat_inner):
raise ValueError("Non-uniform tendon strides within worlds are not supported")
inner_tendon_stride = flat_inner[0] if flat_inner else arti_tendon_count
else:
inner_tendon_stride = arti_tendon_count
# Validate that tendon indices are contiguous
# Non-contiguous tendons (e.g., interleaved with other articulations) are not supported
expected_contiguous = list(range(tendon_offset, tendon_offset + arti_tendon_count))
if arti_tendon_ids != expected_contiguous:
raise ValueError(
f"Tendons for articulation are not contiguous (indices {arti_tendon_ids}, "
f"expected {expected_contiguous}). Non-contiguous tendons are not supported "
f"by ArticulationView."
)
# Tendons are contiguous, use range-based indexing
selected_tendon_indices = list(range(arti_tendon_count))
# Store with the full namespaced frequency key (mujoco:tendon)
self.frequency_layouts["mujoco:tendon"] = FrequencyLayout(
tendon_offset,
outer_tendon_stride,
inner_tendon_stride,
arti_tendon_count,
selected_tendon_indices,
self.device,
)
self.tendon_count = arti_tendon_count
# Populate tendon_names from model.mujoco.tendon_label if available
if hasattr(mujoco_attrs, "tendon_label"):
for tendon_idx in arti_tendon_ids:
if tendon_idx < len(mujoco_attrs.tendon_label):
self.tendon_names.append(get_name_from_label(mujoco_attrs.tendon_label[tendon_idx]))
else:
self.tendon_names.append(f"tendon_{tendon_idx}")
self.joints_contiguous = self.frequency_layouts[AttributeFrequency.JOINT].is_contiguous
self.joint_dofs_contiguous = self.frequency_layouts[AttributeFrequency.JOINT_DOF].is_contiguous
self.joint_coords_contiguous = self.frequency_layouts[AttributeFrequency.JOINT_COORD].is_contiguous
self.links_contiguous = self.frequency_layouts[AttributeFrequency.BODY].is_contiguous
self.shapes_contiguous = self.frequency_layouts[AttributeFrequency.SHAPE].is_contiguous
# articulation ids grouped by world
self.articulation_ids = wp.array(articulation_ids, dtype=int, device=self.device)
# default mask includes all articulations in all worlds
self.full_mask = wp.full(world_count, True, dtype=bool, device=self.device)
# create articulation mask
self.articulation_mask = wp.zeros(model.articulation_count, dtype=bool, device=self.device)
wp.launch(
set_model_articulation_mask_per_world_kernel,
dim=self.articulation_ids.shape,
inputs=[self.full_mask, self.articulation_ids, self.articulation_mask],
device=self.device,
)
if verbose:
print(f"Articulation '{pattern}': {self.count}")
print(f" Link count: {self.link_count} ({'' if self.links_contiguous else 'non-'}contiguous)")
print(f" Shape count: {self.shape_count} ({'' if self.shapes_contiguous else 'non-'}contiguous)")
print(f" Joint count: {self.joint_count} ({'' if self.joints_contiguous else 'non-'}contiguous)")
print(
f" DOF count: {self.joint_dof_count} ({'' if self.joint_dofs_contiguous else 'non-'}contiguous)"
)
print(f" Fixed base? {self.is_fixed_base}")
print(f" Floating base? {self.is_floating_base}")
print("Link names:")
print(f" {self.link_names}")
print("Joint names:")
print(f" {self.joint_names}")
print("Joint DOF names:")
print(f" {self.joint_dof_names}")
print("Shapes:")
for link_idx in range(self.link_count):
shape_names = [self.shape_names[shape_idx] for shape_idx in self.link_shapes[link_idx]]
print(f" Link '{self.link_names[link_idx]}': {shape_names}")
@property
def body_names(self):
"""Alias for `link_names`."""
return self.link_names
@property
def body_shapes(self):
"""Alias for `link_shapes`."""
return self.link_shapes
# ========================================================================================
# Generic attribute API
@functools.lru_cache(maxsize=None) # noqa
def _get_attribute_array(self, name: str, source: Model | State | Control, _slice: Slice | int | None = None):
# get the attribute (handle namespaced attributes like "mujoco.tendon_stiffness")
# Note: the user-facing API uses dots (e.g., "mujoco.tendon_stiffness")
# but internally attributes are stored with colons (e.g., "mujoco:tendon_stiffness")
if "." in name:
parts = name.split(".")
attrib = source
for part in parts:
attrib = getattr(attrib, part)
# Convert dot notation to colon notation for frequency lookup
frequency_name = ":".join(parts)
else:
attrib = getattr(source, name)
frequency_name = name
assert isinstance(attrib, wp.array)
# get frequency info
frequency = self.model.get_attribute_frequency(frequency_name)
# Handle custom frequencies (string frequencies)
if isinstance(frequency, str):
# Check if this is a supported custom frequency
# Tendon frequency can be "tendon" or "mujoco:tendon" (with namespace prefix)
if frequency == "tendon" or frequency.endswith(":tendon"):
# Normalize to the stored key format "mujoco:tendon"
normalized_frequency = "mujoco:tendon"
layout = self.frequency_layouts.get(normalized_frequency)
if layout is None:
raise AttributeError(
f"Attribute '{name}' has frequency '{frequency}' but no tendons were found "
f"in the selected articulations"
)
else:
raise AttributeError(
f"Attribute '{name}' has custom frequency '{frequency}' which is not "
f"supported by ArticulationView. Custom frequencies are for custom entity types "
f"that are not part of articulations."
)
else:
layout = self.frequency_layouts.get(frequency)
if layout is None:
raise AttributeError(
f"Unable to determine the layout of frequency '{frequency.name}' for attribute '{name}'"
)
value_stride = attrib.strides[0]
is_indexed = layout.indices is not None
# handle custom slice
if isinstance(_slice, Slice):
_slice = _slice.get()
elif not isinstance(_slice, (NoneType, int, slice)):
raise ValueError(f"Invalid slice type: expected slice or int, got {type(_slice)}")
if _slice is None:
value_slice = layout.indices if is_indexed else layout.slice
value_count = layout.value_count
else:
value_slice = _slice
value_count = 1 if isinstance(_slice, int) else _slice.stop - _slice.start
# trailing dimensions for multidimensional attributes
trailing_shape = attrib.shape[1:]
trailing_strides = attrib.strides[1:]
trailing_slices = [slice(s) for s in trailing_shape]
shape = (self.world_count, self.count_per_world, value_count, *trailing_shape)
strides = (
layout.stride_between_worlds * value_stride,
layout.stride_within_worlds * value_stride,
value_stride,
*trailing_strides,
)
slices = (slice(self.world_count), slice(self.count_per_world), value_slice, *trailing_slices)
# early out for empty source arrays (e.g. articulations with only fixed joints)
if attrib.ptr is None:
result = wp.empty(shape, dtype=attrib.dtype, device=attrib.device)
result.ptr = None
return result
# construct reshaped attribute array, preserving grad connectivity
source_grad = attrib.grad if attrib.requires_grad else None
grad_view = None
if source_grad is not None:
grad_stride = source_grad.strides[0]
grad_view = wp.array(
ptr=int(source_grad.ptr) + layout.offset * grad_stride,
dtype=source_grad.dtype,
shape=shape,
strides=(
layout.stride_between_worlds * grad_stride,
layout.stride_within_worlds * grad_stride,
grad_stride,
*source_grad.strides[1:],
),
device=source_grad.device,
copy=False,
)
attrib = wp.array(
ptr=int(attrib.ptr) + layout.offset * value_stride,
dtype=attrib.dtype,
shape=shape,
strides=strides,
device=attrib.device,
copy=False,
grad=grad_view,
)
# apply selection (slices or indices)
pre_indexed = attrib
attrib = attrib[slices]
if is_indexed:
attrib._staging_array = wp.empty_like(attrib)
if grad_view is not None:
attrib._staging_array.requires_grad = True
attrib._gather_src = pre_indexed
attrib._gather_indices = layout.indices
else:
# fixup for empty slices - FIXME: this should be handled by Warp, above
if attrib.size == 0:
attrib.ptr = None
return attrib
def _get_attribute_values(self, name: str, source: Model | State | Control, _slice: slice | None = None):
attrib = self._get_attribute_array(name, source, _slice=_slice)
if hasattr(attrib, "_staging_array"):
if hasattr(attrib, "_gather_src"):
kernel = _gather_indexed_4d_kernel if attrib.ndim == 4 else _gather_indexed_3d_kernel
wp.launch(
kernel,
dim=attrib._staging_array.shape,
inputs=[attrib._gather_src, attrib._gather_indices],
outputs=[attrib._staging_array],
)
src_grad = attrib._gather_src.grad
dst_grad = attrib._staging_array.grad
if src_grad is not None and dst_grad is not None:
grad_slices = tuple(attrib._gather_indices if d == 2 else slice(None) for d in range(src_grad.ndim))
wp.copy(dst_grad, src_grad[grad_slices])
else:
wp.copy(attrib._staging_array, attrib)
return attrib._staging_array
return attrib
def _set_attribute_values(
self, name: str, target: Model | State | Control, values, mask=None, _slice: slice | None = None
):
attrib = self._get_attribute_array(name, target, _slice=_slice)
if not is_array(values) or values.dtype != attrib.dtype:
values = wp.array(values, dtype=attrib.dtype, shape=attrib.shape, device=self.device, copy=False)
assert values.shape == attrib.shape
assert values.dtype == attrib.dtype
# early out for in-place modifications
if isinstance(attrib, wp.array) and isinstance(values, wp.array):
if values.ptr == attrib.ptr:
return
if isinstance(attrib, wp.indexedarray) and isinstance(values, wp.indexedarray):
if values.data.ptr == attrib.data.ptr:
return
# get mask
if mask is None:
mask = self.full_mask
else:
mask = self._resolve_mask(mask)
# launch appropriate kernel based on attribute dimensionality
# TODO: cache concrete overload per attribute?
if mask.ndim == 1:
if attrib.ndim == 3:
wp.launch(
set_articulation_attribute_3d_per_world_kernel,
dim=attrib.shape,
inputs=[mask, values, attrib],
device=self.device,
)
elif attrib.ndim == 4:
wp.launch(
set_articulation_attribute_4d_per_world_kernel,
dim=attrib.shape,
inputs=[mask, values, attrib],
device=self.device,
)
else:
raise NotImplementedError(f"Unsupported attribute with ndim={attrib.ndim}")
else: # mask.ndim == 2
if attrib.ndim == 3:
wp.launch(
set_articulation_attribute_3d_kernel,
dim=attrib.shape,
inputs=[mask, values, attrib],
device=self.device,
)
elif attrib.ndim == 4:
wp.launch(
set_articulation_attribute_4d_kernel,
dim=attrib.shape,
inputs=[mask, values, attrib],
device=self.device,
)
else:
raise NotImplementedError(f"Unsupported attribute with ndim={attrib.ndim}")
def get_attribute(self, name: str, source: Model | State | Control):
"""
Get an attribute from the source (Model, State, or Control).
Args:
name (str): The name of the attribute to get.
source (Model | State | Control): The source from which to get the attribute.
Returns:
array: The attribute values (dtype matches the attribute).
"""
return self._get_attribute_values(name, source)
def set_attribute(self, name: str, target: Model | State | Control, values, mask=None):
"""
Set an attribute in the target (Model, State, or Control).
Args:
name (str): The name of the attribute to set.
target (Model | State | Control): The target where to set the attribute.
values (array): The values to set for the attribute.
mask (array): Mask of articulations in this ArticulationView (all by default).
.. note::
When setting attributes on the Model, it may be necessary to inform the solver about
such changes by calling :meth:`newton.solvers.SolverBase.notify_model_changed` after finished
setting Model attributes.
"""
self._set_attribute_values(name, target, values, mask=mask)
# ========================================================================================
# Convenience wrappers to align with legacy tensor API
def get_root_transforms(self, source: Model | State):
"""
Get the root transforms of the articulations.
Args:
source (Model | State): Where to get the root transforms (Model or State).
Returns:
array: The root transforms (dtype=wp.transform).
"""
if self.is_floating_base:
attrib = self._get_attribute_values("joint_q", source, _slice=Slice(0, 7))
else:
attrib = self._get_attribute_values("joint_X_p", self.model, _slice=0)
if attrib.dtype is wp.transform:
return attrib
else:
return wp.array(attrib, dtype=wp.transform, device=self.device, copy=False)
def set_root_transforms(self, target: Model | State, values: wp.array, mask=None):
"""
Set the root transforms of the articulations.
Call :meth:`eval_fk` to apply changes to all articulation links.
Args:
target (Model | State): Where to set the root transforms (Model or State).
values (array): The root transforms to set (dtype=wp.transform).
mask (array): Mask of articulations in this ArticulationView (all by default).
"""
if self.is_floating_base:
self._set_attribute_values("joint_q", target, values, mask=mask, _slice=Slice(0, 7))
else:
self._set_attribute_values("joint_X_p", self.model, values, mask=mask, _slice=0)
def get_root_velocities(self, source: Model | State):
"""
Get the root velocities of the articulations.
Args:
source (Model | State): Where to get the root velocities (Model or State).
Returns:
array: The root velocities (dtype=wp.spatial_vector).
"""
if self.is_floating_base:
attrib = self._get_attribute_values("joint_qd", source, _slice=Slice(0, 6))
else:
# FIXME? Non-floating articulations have no root velocities.
return None
if attrib.dtype is wp.spatial_vector:
return attrib
else:
return wp.array(attrib, dtype=wp.spatial_vector, device=self.device, copy=False)
def set_root_velocities(self, target: Model | State, values: wp.array, mask=None):
"""
Set the root velocities of the articulations.
Args:
target (Model | State): Where to set the root velocities (Model or State).
values (array): The root velocities to set (dtype=wp.spatial_vector).
mask (array): Mask of articulations in this ArticulationView (all by default).
"""
if self.is_floating_base:
self._set_attribute_values("joint_qd", target, values, mask=mask, _slice=Slice(0, 6))
else:
return # no-op
def get_link_transforms(self, source: "Model | State"):
"""
Get the world-space transforms of all links in the selected articulations.
Args:
source (Model | State): The source from which to retrieve the link transforms.
Returns:
array: The link transforms (dtype=wp.transform).
"""
return self._get_attribute_values("body_q", source)
def get_link_velocities(self, source: "Model | State"):
"""
Get the world-space spatial velocities of all links in the selected articulations.
Args:
source (Model | State): The source from which to retrieve the link velocities.
Returns:
array: The link velocities (dtype=wp.spatial_vector).
"""
return self._get_attribute_values("body_qd", source)
def get_dof_positions(self, source: "Model | State"):
"""
Get the joint coordinate positions (DoF positions) for the selected articulations.
Args:
source (Model | State): The source from which to retrieve the DoF positions.
Returns:
array: The joint coordinate positions (dtype=float).
"""
return self._get_attribute_values("joint_q", source)
def set_dof_positions(self, target: "Model | State", values, mask=None):
"""
Set the joint coordinate positions (DoF positions) for the selected articulations.
Args:
target (Model | State): The target where to set the DoF positions.
values (array): The values to set (dtype=float).
mask (array, optional): Mask of articulations in this ArticulationView (all by default).
"""
self._set_attribute_values("joint_q", target, values, mask=mask)
def get_dof_velocities(self, source: "Model | State"):
"""
Get the joint coordinate velocities (DoF velocities) for the selected articulations.
Args:
source (Model | State): The source from which to retrieve the DoF velocities.
Returns:
array: The joint coordinate velocities (dtype=float).
"""
return self._get_attribute_values("joint_qd", source)
def set_dof_velocities(self, target: "Model | State", values, mask=None):
"""
Set the joint coordinate velocities (DoF velocities) for the selected articulations.
Args:
target (Model | State): The target where to set the DoF velocities.
values (array): The values to set (dtype=float).
mask (array, optional): Mask of articulations in this ArticulationView (all by default).
"""
self._set_attribute_values("joint_qd", target, values, mask=mask)
def get_dof_forces(self, source: "Control"):
"""
Get the joint forces (DoF forces) for the selected articulations.
Args:
source (Control): The source from which to retrieve the DoF forces.
Returns:
array: The joint forces (dtype=float).
"""
return self._get_attribute_values("joint_f", source)
def set_dof_forces(self, target: "Control", values, mask=None):
"""
Set the joint forces (DoF forces) for the selected articulations.
Args:
target (Control): The target where to set the DoF forces.
values (array): The values to set (dtype=float).
mask (array, optional): Mask of articulations in this ArticulationView (all by default).
"""
self._set_attribute_values("joint_f", target, values, mask=mask)
# ========================================================================================
# Utilities
def _resolve_mask(self, mask):
# accept 1D and 2D Boolean masks
if isinstance(mask, wp.array):
if mask.dtype is wp.bool and mask.ndim < 3:
return mask
else:
# try interpreting as a 1D world mask
try:
return wp.array(mask, dtype=bool, shape=self.world_count, device=self.device, copy=False)
except Exception:
pass
# try interpreting as a 2D (world, arti) mask
try:
return wp.array(
mask, dtype=bool, shape=(self.world_count, self.count_per_world), device=self.device, copy=False
)
except Exception:
pass
# no match
raise ValueError(
f"Expected Boolean mask with shape ({self.world_count}, {self.count_per_world}) or ({self.world_count},)"
)
def get_model_articulation_mask(self, mask=None):
"""
Get Model articulation mask from a mask in this ArticulationView.
Args:
mask (array): Mask of articulations in this ArticulationView (all by default).
"""
if mask is None:
return self.articulation_mask
else:
mask = self._resolve_mask(mask)
articulation_mask = wp.zeros(self.model.articulation_count, dtype=bool, device=self.device)
if mask.ndim == 1:
wp.launch(
set_model_articulation_mask_per_world_kernel,
dim=self.articulation_ids.shape,
inputs=[mask, self.articulation_ids, articulation_mask],
device=self.device,
)
else:
wp.launch(
set_model_articulation_mask_kernel,
dim=self.articulation_ids.shape,
inputs=[mask, self.articulation_ids, articulation_mask],
device=self.device,
)
return articulation_mask
def eval_fk(self, target: Model | State, mask=None):
"""
Evaluates forward kinematics given the joint coordinates and updates the body information.
Args:
target (Model | State): The target where to evaluate forward kinematics (Model or State).
mask (array): Mask of articulations in this ArticulationView (all by default).
"""
# translate view mask to Model articulation mask
articulation_mask = self.get_model_articulation_mask(mask=mask)
eval_fk(self.model, target.joint_q, target.joint_qd, target, mask=articulation_mask)
def eval_jacobian(self, state: State, J=None, joint_S_s=None, mask=None):
"""Evaluate spatial Jacobian for articulations in this view.
Computes the spatial Jacobian J that maps joint velocities to spatial
velocities of each link in world frame.
Args:
state: The state containing body transforms (body_q).
J: Optional output array for the Jacobian, shape (articulation_count, max_links*6, max_dofs).
If None, allocates internally.
joint_S_s: Optional pre-allocated temp array for motion subspaces.
mask: Optional mask of articulations in this ArticulationView (all by default).
Returns:
The Jacobian array J, or None if the model has no articulations.
"""
articulation_mask = self.get_model_articulation_mask(mask=mask)
return eval_jacobian(self.model, state, J, joint_S_s=joint_S_s, mask=articulation_mask)
def eval_mass_matrix(self, state: State, H=None, J=None, body_I_s=None, joint_S_s=None, mask=None):
"""Evaluate generalized mass matrix for articulations in this view.
Computes the generalized mass matrix H = J^T * M * J, where J is the spatial
Jacobian and M is the block-diagonal spatial mass matrix.
Args:
state: The state containing body transforms (body_q).
H: Optional output array for mass matrix, shape (articulation_count, max_dofs, max_dofs).
If None, allocates internally.
J: Optional pre-computed Jacobian. If None, computes internally.
body_I_s: Optional pre-allocated temp array for spatial inertias.
joint_S_s: Optional pre-allocated temp array for motion subspaces.
mask: Optional mask of articulations in this ArticulationView (all by default).
Returns:
The mass matrix array H, or None if the model has no articulations.
"""
articulation_mask = self.get_model_articulation_mask(mask=mask)
return eval_mass_matrix(
self.model, state, H, J=J, body_I_s=body_I_s, joint_S_s=joint_S_s, mask=articulation_mask
)
# ========================================================================================
# Actuator parameter access
@functools.cache # noqa: B019 - cache is tied to view lifetime
def _get_actuator_dof_mapping(self, actuator: "Actuator") -> wp.array:
"""
Build mapping from view DOF positions to actuator parameter indices.
Note:
For selection we assume that input_indices is 1D (one input per actuator),
not the general 2D case (multiple inputs per actuator) which is supported
by the library.
Returns array of shape (world_count * dofs_per_world,) where each element is:
- actuator parameter index if that DOF is actuated
- -1 if that DOF is not actuated by this actuator
"""
num_actuators = actuator.input_indices.shape[0]
actuators_per_world = num_actuators // self.world_count
dof_layout = self.frequency_layouts[AttributeFrequency.JOINT_DOF]
dofs_per_arti = dof_layout.selected_value_count
dofs_per_world = dofs_per_arti * self.count_per_world
if dofs_per_world == 0:
return wp.empty(0, dtype=int, device=self.device)
mapping = wp.full(self.world_count * dofs_per_world, -1, dtype=int, device=self.device)
if dof_layout.is_contiguous:
wp.launch(
build_actuator_dof_mapping_slice_kernel,
dim=actuators_per_world,
inputs=[
actuator.input_indices,
actuators_per_world,
dof_layout.offset,
dof_layout.slice.start,
dof_layout.slice.stop,
dof_layout.stride_within_worlds,
self.count_per_world,
dofs_per_arti,
dofs_per_world,
self.world_count,
],
outputs=[mapping],
device=self.device,
)
else:
wp.launch(
build_actuator_dof_mapping_indices_kernel,
dim=actuators_per_world,
inputs=[
actuator.input_indices,
dof_layout.indices,
dof_layout.offset,
dof_layout.stride_within_worlds,
self.count_per_world,
actuators_per_world,
dofs_per_arti,
dofs_per_world,
self.world_count,
],
outputs=[mapping],
device=self.device,
)
return mapping
def _get_actuator_attribute_array(self, actuator: "Actuator", name: str) -> wp.array:
"""Get actuator parameter array shaped (world_count, dofs_per_world), zeros for non-actuated DOFs."""
mapping = self._get_actuator_dof_mapping(actuator)
if len(mapping) == 0:
return wp.empty((self.world_count, 0), dtype=float, device=self.device)
src = getattr(actuator, name)
dofs_per_world = len(mapping) // self.world_count
dst = wp.zeros(len(mapping), dtype=src.dtype, device=self.device)
wp.launch(
gather_actuator_by_indices_kernel,
dim=len(mapping),
inputs=[src, mapping],
outputs=[dst],
device=self.device,
)
batched_shape = (self.world_count, dofs_per_world, *src.shape[1:])
return dst.reshape(batched_shape)
def get_actuator_parameter(self, actuator: "Actuator", name: str) -> wp.array:
"""
Get actuator parameter values for actuators corresponding to this view's DOFs.
Args:
actuator: An actuator instance with input_indices and parameter arrays.
name (str): Parameter name (e.g., 'kp', 'kd', 'max_force', 'gear', 'constant_force').
Returns:
wp.array: Parameter values shaped (world_count, dofs_per_world).
"""
return self._get_actuator_attribute_array(actuator, name)
def set_actuator_parameter(
self, actuator: "Actuator", name: str, values: wp.array, mask: wp.array | None = None
) -> None:
"""
Set actuator parameter values for actuators corresponding to this view's DOFs.
Args:
actuator: An actuator instance with input_indices and parameter arrays.
name (str): Parameter name (e.g., 'kp', 'kd', 'max_force', 'gear', 'constant_force').
values: New parameter values shaped (world_count, dofs_per_world). Non-actuated DOFs are ignored.
mask (array, optional): Per-world mask (world_count,). Only masked worlds are updated.
"""
mapping = self._get_actuator_dof_mapping(actuator)
if len(mapping) == 0:
return
dst = getattr(actuator, name)
dofs_per_world = len(mapping) // self.world_count
expected_shape = (self.world_count, dofs_per_world, *dst.shape[1:])
if not is_array(values):
values = wp.array(values, dtype=dst.dtype, shape=expected_shape, device=self.device, copy=False)
if values.shape[:2] != expected_shape[:2]:
raise ValueError(f"Expected values shape {expected_shape}, got {values.shape}")
if mask is None:
mask = self.full_mask
else:
if not isinstance(mask, wp.array):
mask = wp.array(mask, dtype=bool, shape=(self.world_count,), device=self.device, copy=False)
if mask.shape != (self.world_count,):
raise ValueError(f"Expected mask shape ({self.world_count},), got {mask.shape}")
wp.launch(
scatter_actuator_with_mask_kernel,
dim=(self.world_count, dofs_per_world),
inputs=[values, mapping, mask, dofs_per_world],
outputs=[dst],
device=self.device,
)
================================================
FILE: newton/_src/utils/texture.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import io
import os
import warnings
from urllib.parse import unquote, urlparse
from urllib.request import urlopen
import numpy as np
_texture_url_cache: dict[str, bytes] = {}
def _is_http_url(path: str) -> bool:
parsed = urlparse(path)
return parsed.scheme in ("http", "https")
def _resolve_file_url(path: str) -> str:
parsed = urlparse(path)
if parsed.scheme != "file":
return path
return unquote(parsed.path)
def _download_texture_from_file_bytes(url: str) -> bytes | None:
if url in _texture_url_cache:
return _texture_url_cache[url]
try:
with urlopen(url, timeout=10) as response:
data = response.read()
_texture_url_cache[url] = data
return data
except Exception as exc:
warnings.warn(f"Failed to download texture image: {url} ({exc})", stacklevel=2)
return None
def load_texture_from_file(texture_path: str | None) -> np.ndarray | None:
"""Load a texture image from disk or URL into a numpy array.
Args:
texture_path: Path or URL to the texture image.
Returns:
Texture image as uint8 RGBA numpy array (H, W, 4), or None if load fails.
"""
if texture_path is None:
return None
try:
from PIL import Image
if _is_http_url(texture_path):
data = _download_texture_from_file_bytes(texture_path)
if data is None:
return None
with Image.open(io.BytesIO(data)) as source_img:
img = source_img.convert("RGBA")
return np.array(img)
texture_path = _resolve_file_url(texture_path)
with Image.open(texture_path) as source_img:
img = source_img.convert("RGBA")
return np.array(img)
except Exception as exc:
warnings.warn(f"Failed to load texture image: {texture_path} ({exc})", stacklevel=2)
return None
def load_texture(texture: str | os.PathLike[str] | np.ndarray | None) -> np.ndarray | None:
"""Normalize a texture input into a contiguous image array.
Args:
texture: Path/URL to a texture image or an array (H, W, C).
Returns:
np.ndarray | None: Contiguous image array, or None if unavailable.
"""
if texture is None:
return None
if isinstance(texture, os.PathLike):
texture = os.fspath(texture)
if isinstance(texture, str):
loaded = load_texture_from_file(texture)
if loaded is None:
return None
return np.ascontiguousarray(loaded)
return np.ascontiguousarray(np.asarray(texture))
def normalize_texture(
texture_image: np.ndarray | None,
*,
flip_vertical: bool = False,
require_channels: bool = False,
scale_unit_range: bool = True,
) -> np.ndarray | None:
"""Normalize a texture array for rendering.
Args:
texture_image: Texture image array (H, W, C) or None.
flip_vertical: Whether to flip the image vertically.
require_channels: Whether to enforce 3/4-channel images and expand grayscale.
scale_unit_range: Whether to scale unit-range floats to 0-255.
Returns:
np.ndarray | None: Normalized uint8 image array or None if unavailable.
"""
if texture_image is None:
return None
image = np.asarray(texture_image)
if image.dtype != np.uint8:
image = np.clip(image, 0.0, 255.0)
if scale_unit_range and image.max() <= 1.0:
image = image * 255.0
image = image.astype(np.uint8)
if require_channels:
if image.ndim == 2:
image = np.repeat(image[:, :, None], 3, axis=2)
if image.ndim < 2 or image.shape[0] == 0 or image.shape[1] == 0:
raise ValueError("Texture image has invalid dimensions.")
if image.shape[2] not in (3, 4):
raise ValueError(f"Unsupported texture channels: {image.shape[2]}")
if flip_vertical:
image = np.flipud(image)
return np.ascontiguousarray(image)
def compute_texture_hash(texture: str | os.PathLike[str] | np.ndarray | None) -> int:
"""Compute a stable hash for a texture (path or array).
Args:
texture: Texture path/URL string, PathLike, or image array (H, W, C), or None.
Returns:
Hash of the texture path or array contents, or 0 for None.
"""
if texture is None:
return 0
# Handle string paths and PathLike - hash the path string without decoding
if isinstance(texture, os.PathLike):
return hash(os.fspath(texture))
if isinstance(texture, str):
return hash(texture)
# Array input - hash based on shape and sampled content
texture = np.ascontiguousarray(texture)
flat_size = texture.size
if flat_size == 0:
sample_bytes = b""
else:
# Only sample a small portion of the texture to avoid hashing large textures in full.
flat = texture.ravel()
max_samples = 1024
step = max(1, flat.size // max_samples)
sample = flat[::step]
sample_bytes = sample.tobytes()
return hash((texture.shape, texture.dtype.str, sample_bytes))
================================================
FILE: newton/_src/utils/topology.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
from collections import defaultdict, deque
from collections.abc import Sequence
from typing import TypeVar, cast
NodeT = TypeVar("NodeT", int, str)
"""A generic type variable for nodes in a topology, which can be either integers or strings."""
def _joint_key(item: tuple[int, NodeT]) -> int:
return item[0]
def topological_sort(
joints: Sequence[tuple[NodeT, NodeT]],
custom_indices: Sequence[int] | None = None,
use_dfs: bool = True,
ensure_single_root: bool = False,
) -> list[int]:
"""
Topological sort of a list of joints connecting rigid bodies.
Args:
joints (Sequence[tuple[int, int]] | Sequence[tuple[str, str]]): A list of body link pairs (parent, child). Bodies can be identified by their name or index.
custom_indices (Sequence[int] | None): A list of custom indices to return for the joints. If None, the joint indices will be used.
use_dfs (bool): If True, use depth-first search for topological sorting.
If False, use Kahn's algorithm. Default is True.
ensure_single_root (bool): If True, raise a ValueError if there is more than one root body. Default is False.
Returns:
list[int]: A list of joint indices in topological order.
"""
if custom_indices is not None and len(custom_indices) != len(joints):
raise ValueError(
f"Length of custom indices must match length of joints: {len(custom_indices)} != {len(joints)}"
)
incoming: dict[NodeT, set[tuple[int, NodeT]]] = defaultdict(set)
outgoing: dict[NodeT, set[tuple[int, NodeT]]] = defaultdict(set)
nodes: set[NodeT] = set()
for joint_id, (parent, child) in enumerate(joints):
if len(incoming[child]) == 1:
raise ValueError(f"Multiple joints lead to body {child}")
incoming[child].add((joint_id, parent))
outgoing[parent].add((joint_id, child))
nodes.add(parent)
nodes.add(child)
roots = nodes - set(incoming.keys())
if len(roots) == 0:
raise ValueError("No root found in the joint graph.")
if ensure_single_root and len(roots) > 1:
raise ValueError(f"Multiple roots found in the joint graph: {roots}")
joint_order: list[int] = []
visited = set()
if use_dfs:
def visit(node: NodeT) -> None:
visited.add(node)
# sort by joint ID to retain original order if topological order is not unique
outs = sorted(outgoing[node], key=_joint_key)
for joint_id, child in outs:
if child in visited:
raise ValueError(f"Joint graph contains a cycle at body {child}")
joint_order.append(joint_id)
visit(child)
roots = sorted(roots)
for root in roots:
visit(root)
else:
# Breadth-first search (Kahn's algorithm)
queue = deque(sorted(roots))
while queue:
node = queue.popleft()
visited.add(node)
outs = sorted(outgoing[node], key=_joint_key)
for joint_id, child in outs:
if child in visited:
raise ValueError(f"Joint graph contains a cycle at body {child}")
joint_order.append(joint_id)
queue.append(child)
if custom_indices is not None:
joint_order = [custom_indices[i] for i in joint_order]
return joint_order
def topological_sort_undirected(
joints: Sequence[tuple[NodeT, NodeT]],
custom_indices: Sequence[int] | None = None,
use_dfs: bool = True,
ensure_single_root: bool = False,
) -> tuple[list[int], list[int]]:
"""
Topological sort of a list of joints treating the graph as undirected.
This function first attempts to use the original (parent, child) ordering.
If that fails, it orients each joint edge during traversal to produce a valid
parent-before-child ordering, and returns the joints that were reversed
relative to the input orientation.
Args:
joints (Sequence[tuple[int, int]] | Sequence[tuple[str, str]]): A list of body link pairs.
Bodies can be identified by their name or index.
custom_indices (Sequence[int] | None): A list of custom indices to return for the joints.
If None, the joint indices will be used.
use_dfs (bool): If True, use depth-first search for topological sorting.
If False, use a breadth-first traversal. Default is True.
ensure_single_root (bool): If True, raise a ValueError if there is more than one root
component. Default is False.
Returns:
tuple[list[int], list[int]]: A tuple of (joint_order, reversed_joints).
joint_order is a list of joint indices in topological order.
reversed_joints contains joint indices that were reversed during traversal.
"""
if custom_indices is not None and len(custom_indices) != len(joints):
raise ValueError(
f"Length of custom indices must match length of joints: {len(custom_indices)} != {len(joints)}"
)
try:
joint_order = topological_sort(
joints,
custom_indices=custom_indices,
use_dfs=use_dfs,
ensure_single_root=ensure_single_root,
)
return joint_order, []
except ValueError:
pass
adjacency: dict[NodeT, list[tuple[int, NodeT]]] = defaultdict(list)
nodes: set[NodeT] = set()
for joint_id, (parent, child) in enumerate(joints):
adjacency[parent].append((joint_id, child))
adjacency[child].append((joint_id, parent))
nodes.add(parent)
nodes.add(child)
if not nodes:
return [], []
joint_order: list[int] = []
reversed_joints: list[int] = []
visited = set()
def record_edge(node: NodeT, neighbor: NodeT, joint_id: int) -> None:
original_parent, original_child = joints[joint_id]
if original_parent == node and original_child == neighbor:
reversed_edge = False
elif original_parent == neighbor and original_child == node:
reversed_edge = True
else:
raise ValueError(f"Joint {joint_id} does not connect {node} and {neighbor}")
if reversed_edge:
reversed_joints.append(joint_id)
joint_order.append(joint_id)
def sorted_roots() -> list[NodeT]:
roots = sorted(nodes)
if -1 in nodes:
roots = cast(list[NodeT], [-1] + [node for node in roots if node != -1])
return roots
if use_dfs:
def visit(node: NodeT, parent: NodeT | None = None) -> None:
visited.add(node)
outs = sorted(adjacency[node], key=_joint_key)
for joint_id, neighbor in outs:
if neighbor == parent:
continue
if neighbor in visited:
raise ValueError(f"Joint graph contains a cycle at body {neighbor}")
record_edge(node, neighbor, joint_id)
visit(neighbor, node)
for root in sorted_roots():
if root in visited:
continue
if ensure_single_root and visited:
raise ValueError("Multiple roots found in the joint graph.")
visit(root)
else:
queue = deque()
for root in sorted_roots():
if root in visited:
continue
if ensure_single_root and visited:
raise ValueError("Multiple roots found in the joint graph.")
queue.append((root, None))
visited.add(root)
while queue:
node, parent = queue.popleft()
outs = sorted(adjacency[node], key=_joint_key)
for joint_id, neighbor in outs:
if neighbor == parent:
continue
if neighbor in visited:
raise ValueError(f"Joint graph contains a cycle at body {neighbor}")
record_edge(node, neighbor, joint_id)
visited.add(neighbor)
queue.append((neighbor, node))
if custom_indices is not None:
joint_order = [custom_indices[i] for i in joint_order]
reversed_joints = [custom_indices[i] for i in reversed_joints]
return joint_order, reversed_joints
================================================
FILE: newton/_src/viewer/__init__.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Viewer interface for Newton physics simulations.
This module provides a high-level, renderer-agnostic interface for interactive
visualization of Newton models and simulation states.
Example usage:
```python
import newton
from newton.viewer import ViewerGL
# Create viewer with OpenGL backend
viewer = ViewerGL(model)
# Render simulation
while viewer.is_running():
viewer.begin_frame(time)
viewer.log_state(state)
viewer.log_points(particle_positions)
viewer.end_frame()
viewer.close()
```
"""
from .viewer import ViewerBase
from .viewer_file import ViewerFile
from .viewer_gl import ViewerGL
from .viewer_null import ViewerNull
from .viewer_rerun import ViewerRerun
from .viewer_usd import ViewerUSD
from .viewer_viser import ViewerViser
__all__ = [
"ViewerBase",
"ViewerFile",
"ViewerGL",
"ViewerNull",
"ViewerRerun",
"ViewerUSD",
"ViewerViser",
]
================================================
FILE: newton/_src/viewer/camera.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import numpy as np
class Camera:
"""Camera class that encapsulates all camera settings and logic."""
def __init__(self, fov=45.0, near=0.01, far=1000.0, width=1280, height=720, pos=None, up_axis="Z"):
"""
Initialize camera with given parameters.
Args:
fov (float): Field of view in degrees
near (float): Near clipping plane
far (float): Far clipping plane
width (int): Screen width
height (int): Screen height
pos (tuple): Initial camera position (if None, uses appropriate default for up_axis)
up_axis (str): Up axis ("X", "Y", or "Z")
"""
from pyglet.math import Vec3 as PyVec3
self.fov = fov
self.near = near
self.far = far
self.width = width
self.height = height
# Handle up axis properly first
if isinstance(up_axis, int):
self.up_axis = up_axis
else:
self.up_axis = "XYZ".index(up_axis.upper())
# Set appropriate defaults based on up_axis
if pos is None:
if self.up_axis == 0: # X up
pos = (2.0, 0.0, 10.0) # 2 units up in X, 10 units back in Z
elif self.up_axis == 2: # Z up
pos = (10.0, 0.0, 2.0) # 2 units up in Z, 10 units back in Y
else: # Y up (default)
pos = (0.0, 2.0, 10.0) # 2 units up in Y, 10 units back in Z
# Camera position
self.pos = PyVec3(*pos)
# Camera orientation - this is what users can modify
self.pitch = 0.0
self.yaw = -180.0
def get_front(self):
"""Get the camera front direction vector (read-only)."""
from pyglet.math import Vec3 as PyVec3
# Clamp pitch to avoid gimbal lock
pitch = max(min(self.pitch, 89.0), -89.0)
# Calculate front vector directly in the coordinate system based on up_axis
# This ensures yaw/pitch work correctly for each coordinate system
if self.up_axis == 0: # X up
# Yaw rotates around X (vertical), pitch is elevation
front_x = np.sin(np.deg2rad(pitch))
front_y = np.cos(np.deg2rad(self.yaw)) * np.cos(np.deg2rad(pitch))
front_z = np.sin(np.deg2rad(self.yaw)) * np.cos(np.deg2rad(pitch))
return PyVec3(front_x, front_y, front_z).normalize()
elif self.up_axis == 2: # Z up
# Yaw rotates around Z (vertical), pitch is elevation
front_x = np.cos(np.deg2rad(self.yaw)) * np.cos(np.deg2rad(pitch))
front_y = np.sin(np.deg2rad(self.yaw)) * np.cos(np.deg2rad(pitch))
front_z = np.sin(np.deg2rad(pitch))
return PyVec3(front_x, front_y, front_z).normalize()
else: # Y up (default)
# Yaw rotates around Y (vertical), pitch is elevation
front_x = np.cos(np.deg2rad(self.yaw)) * np.cos(np.deg2rad(pitch))
front_y = np.sin(np.deg2rad(pitch))
front_z = np.sin(np.deg2rad(self.yaw)) * np.cos(np.deg2rad(pitch))
return PyVec3(front_x, front_y, front_z).normalize()
def get_right(self):
"""Get the camera right direction vector (read-only)."""
from pyglet.math import Vec3 as PyVec3
return PyVec3.cross(self.get_front(), self.get_up()).normalize()
def get_up(self):
"""Get the camera up direction vector (read-only)."""
from pyglet.math import Vec3 as PyVec3
# World up vector based on up axis
if self.up_axis == 0: # X up
world_up = PyVec3(1.0, 0.0, 0.0)
elif self.up_axis == 2: # Z up
world_up = PyVec3(0.0, 0.0, 1.0)
else: # Y up (default)
world_up = PyVec3(0.0, 1.0, 0.0)
# Compute right vector and use it to get proper up vector
front = self.get_front()
right = PyVec3.cross(front, world_up).normalize()
return PyVec3.cross(right, front).normalize()
def get_view_matrix(self, scaling=1.0):
"""
Compute view matrix handling up axis properly.
Args:
scaling (float): Scene scaling factor
Returns:
np.ndarray: 4x4 view matrix
"""
from pyglet.math import Mat4, Vec3
# Get camera vectors (already transformed for up axis)
pos = Vec3(*(self.pos / scaling))
front = Vec3(*self.get_front())
up = Vec3(*self.get_up())
return np.array(Mat4.look_at(pos, pos + front, up), dtype=np.float32)
def get_projection_matrix(self):
"""
Compute projection matrix.
Returns:
np.ndarray: 4x4 projection matrix
"""
from pyglet.math import Mat4 as PyMat4
if self.height == 0:
return np.eye(4, dtype=np.float32)
aspect_ratio = self.width / self.height
return np.array(PyMat4.perspective_projection(aspect_ratio, self.near, self.far, self.fov))
def get_world_ray(self, x: float, y: float):
"""Get the world ray for a given pixel.
returns:
p: wp.vec3, ray origin
d: wp.vec3, ray direction
"""
from pyglet.math import Vec3 as PyVec3
aspect_ratio = self.width / self.height
# pre-compute factor from vertical FOV
fov_rad = np.radians(self.fov)
alpha = float(np.tan(fov_rad * 0.5)) # = tan(fov/2)
# build an orthonormal basis (front, right, up)
front = self.get_front()
right = self.get_right()
up = self.get_up()
# normalised pixel coordinates
u = 2.0 * (x / self.width) - 1.0 # [-1, 1] left → right
v = 2.0 * (y / self.height) - 1.0 # [-1, 1] bottom → top
# ray direction in world space (before normalisation)
direction = front + right * u * alpha * aspect_ratio + up * v * alpha
direction = direction / float(np.linalg.norm(direction))
return self.pos, PyVec3(*direction)
def update_screen_size(self, width, height):
"""Update screen dimensions."""
self.width = width
self.height = height
================================================
FILE: newton/_src/viewer/gl/gui.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import os
class UI:
def __init__(self, window):
self._file_dialog_result: str | None = None
self._pending_file_dialog = None
try:
from imgui_bundle import (
imgui,
imguizmo,
)
from imgui_bundle.python_backends import pyglet_backend
self.imgui = imgui
self.giz = imguizmo.im_guizmo
self.is_available = True
except ImportError:
self.is_available = False
print("Warning: imgui_bundle not found. Install with: pip install imgui-bundle")
return
self.window = window
self.imgui.create_context()
try:
# Create without callbacks so we can fix the scroll handler first
self.impl = pyglet_backend.create_renderer(self.window, attach_callbacks=False)
except Exception as e:
# Unlikely to happen since RendererGL already sets PYOPENGL_PLATFORM=glx
# on Wayland, but just in case the auto-detection missed the session type.
if "no valid context" in str(e).lower() or "no current context" in str(e).lower():
raise RuntimeError(
"Failed to initialize the OpenGL UI renderer. "
"If you are on Wayland, try setting the environment variable:\n\n"
" PYOPENGL_PLATFORM=glx uv run -m newton.examples \n"
) from e
raise
self.io = self.imgui.get_io()
# Fix inverted scroll direction in the pyglet imgui backend before
# attaching callbacks so pyglet captures the corrected handler.
# The replacement must be named "on_mouse_scroll" because pyglet
# matches handlers by __name__.
io = self.io
def on_mouse_scroll(x, y, scroll_x, scroll_y):
io.add_mouse_wheel_event(scroll_x, scroll_y)
self.impl.on_mouse_scroll = on_mouse_scroll
self.impl._attach_callbacks(self.window)
# Set up proper DPI scaling for high-DPI displays
window_width, window_height = self.window.get_size()
fb_width, fb_height = self.window.get_framebuffer_size()
if window_width > 0 and window_height > 0:
scale_x = fb_width / window_width
scale_y = fb_height / window_height
self.io.display_framebuffer_scale = (scale_x, scale_y)
self.io.display_size = (fb_width, fb_height)
self._setup_dark_style()
def _setup_grey_style(self):
if not self.is_available:
return
style = self.imgui.get_style()
# Style properties
style.alpha = 1.0
# style.disabled_alpha = 0.5
style.window_padding = (13.0, 10.0)
style.window_rounding = 0.0
style.window_border_size = 1.0
style.window_min_size = (32.0, 32.0)
style.window_title_align = (0.5, 0.5)
style.window_menu_button_position = self.imgui.Dir_.right
style.child_rounding = 3.0
style.child_border_size = 1.0
style.popup_rounding = 5.0
style.popup_border_size = 1.0
style.frame_padding = (20.0, 8.100000381469727)
style.frame_rounding = 2.0
style.frame_border_size = 0.0
style.item_spacing = (3.0, 3.0)
style.item_inner_spacing = (3.0, 8.0)
style.cell_padding = (6.0, 14.10000038146973)
style.indent_spacing = 0.0
style.columns_min_spacing = 10.0
style.scrollbar_size = 10.0
style.scrollbar_rounding = 2.0
style.grab_min_size = 12.10000038146973
style.grab_rounding = 1.0
style.tab_rounding = 2.0
style.tab_border_size = 0.0
style.color_button_position = self.imgui.Dir_.right
style.button_text_align = (0.5, 0.5)
style.selectable_text_align = (0.0, 0.0)
# fmt: off
# Colors
style.set_color_(self.imgui.Col_.text, self.imgui.ImVec4(0.9803921580314636, 0.9803921580314636, 0.9803921580314636, 1.0))
style.set_color_(self.imgui.Col_.text_disabled, self.imgui.ImVec4(0.4980392158031464, 0.4980392158031464, 0.4980392158031464, 1.0))
style.set_color_(self.imgui.Col_.window_bg, self.imgui.ImVec4(0.09411764889955521, 0.09411764889955521, 0.09411764889955521, 1.0))
style.set_color_(self.imgui.Col_.child_bg, self.imgui.ImVec4(0.1568627506494522, 0.1568627506494522, 0.1568627506494522, 1.0))
style.set_color_(self.imgui.Col_.popup_bg, self.imgui.ImVec4(0.09411764889955521, 0.09411764889955521, 0.09411764889955521, 1.0))
style.set_color_(self.imgui.Col_.border, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.09803921729326248))
style.set_color_(self.imgui.Col_.border_shadow, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.0))
style.set_color_(self.imgui.Col_.frame_bg, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.09803921729326248))
style.set_color_(self.imgui.Col_.frame_bg_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.1568627506494522))
style.set_color_(self.imgui.Col_.frame_bg_active, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.0470588244497776))
style.set_color_(self.imgui.Col_.title_bg, self.imgui.ImVec4(0.1176470592617989, 0.1176470592617989, 0.1176470592617989, 1.0))
style.set_color_(self.imgui.Col_.title_bg_active, self.imgui.ImVec4(0.1568627506494522, 0.1568627506494522, 0.1568627506494522, 1.0))
style.set_color_(self.imgui.Col_.title_bg_collapsed, self.imgui.ImVec4(0.1176470592617989, 0.1176470592617989, 0.1176470592617989, 1.0))
style.set_color_(self.imgui.Col_.menu_bar_bg, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.0))
style.set_color_(self.imgui.Col_.scrollbar_bg, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.1098039224743843))
style.set_color_(self.imgui.Col_.scrollbar_grab, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.3921568691730499))
style.set_color_(self.imgui.Col_.scrollbar_grab_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.4705882370471954))
style.set_color_(self.imgui.Col_.scrollbar_grab_active, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.09803921729326248))
style.set_color_(self.imgui.Col_.check_mark, self.imgui.ImVec4(1.0, 1.0, 1.0, 1.0))
style.set_color_(self.imgui.Col_.slider_grab, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.3921568691730499))
style.set_color_(self.imgui.Col_.slider_grab_active, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.3137255012989044))
style.set_color_(self.imgui.Col_.button, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.09803921729326248))
style.set_color_(self.imgui.Col_.button_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.1568627506494522))
style.set_color_(self.imgui.Col_.button_active, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.0470588244497776))
style.set_color_(self.imgui.Col_.header, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.09803921729326248))
style.set_color_(self.imgui.Col_.header_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.1568627506494522))
style.set_color_(self.imgui.Col_.header_active, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.0470588244497776))
style.set_color_(self.imgui.Col_.separator, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.1568627506494522))
style.set_color_(self.imgui.Col_.separator_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.2352941185235977))
style.set_color_(self.imgui.Col_.separator_active, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.2352941185235977))
style.set_color_(self.imgui.Col_.resize_grip, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.1568627506494522))
style.set_color_(self.imgui.Col_.resize_grip_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.2352941185235977))
style.set_color_(self.imgui.Col_.resize_grip_active, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.2352941185235977))
style.set_color_(self.imgui.Col_.tab, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.09803921729326248))
style.set_color_(self.imgui.Col_.tab_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.1568627506494522))
style.set_color_(self.imgui.Col_.tab_selected, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.3137255012989044))
style.set_color_(self.imgui.Col_.tab_dimmed, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.1568627506494522))
style.set_color_(self.imgui.Col_.tab_dimmed_selected, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.2352941185235977))
style.set_color_(self.imgui.Col_.plot_lines, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.3529411852359772))
style.set_color_(self.imgui.Col_.plot_lines_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 1.0))
style.set_color_(self.imgui.Col_.plot_histogram, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.3529411852359772))
style.set_color_(self.imgui.Col_.plot_histogram_hovered, self.imgui.ImVec4(1.0, 1.0, 1.0, 1.0))
style.set_color_(self.imgui.Col_.table_header_bg, self.imgui.ImVec4(0.1568627506494522, 0.1568627506494522, 0.1568627506494522, 1.0))
style.set_color_(self.imgui.Col_.table_border_strong, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.3137255012989044))
style.set_color_(self.imgui.Col_.table_border_light, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.196078434586525))
style.set_color_(self.imgui.Col_.table_row_bg, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.0))
style.set_color_(self.imgui.Col_.table_row_bg_alt, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.01960784383118153))
style.set_color_(self.imgui.Col_.text_selected_bg, self.imgui.ImVec4(0.0, 0.0, 0.0, 1.0))
style.set_color_(self.imgui.Col_.drag_drop_target, self.imgui.ImVec4(0.168627455830574, 0.2313725501298904, 0.5372549295425415, 1.0))
style.set_color_(self.imgui.Col_.nav_cursor, self.imgui.ImVec4(1.0, 1.0, 1.0, 1.0))
style.set_color_(self.imgui.Col_.nav_windowing_highlight, self.imgui.ImVec4(1.0, 1.0, 1.0, 0.699999988079071))
style.set_color_(self.imgui.Col_.nav_windowing_dim_bg, self.imgui.ImVec4(0.800000011920929, 0.800000011920929, 0.800000011920929, 0.2000000029802322))
style.set_color_(self.imgui.Col_.modal_window_dim_bg, self.imgui.ImVec4(0.0, 0.0, 0.0, 0.5647059082984924))
# fmt: on
def _setup_dark_style(self):
if not self.is_available:
return
style = self.imgui.get_style()
# Style properties
style.alpha = 1.0
# style.disabled_alpha = 1.0
style.window_padding = (12.0, 12.0)
style.window_rounding = 0.0
style.window_border_size = 0.0
style.window_min_size = (20.0, 20.0)
style.window_title_align = (0.5, 0.5)
style.window_menu_button_position = self.imgui.Dir_.none
style.child_rounding = 0.0
style.child_border_size = 1.0
style.popup_rounding = 0.0
style.popup_border_size = 1.0
style.frame_padding = (6.0, 6.0)
style.frame_rounding = 0.0
style.frame_border_size = 0.0
style.item_spacing = (12.0, 6.0)
style.item_inner_spacing = (6.0, 3.0)
style.cell_padding = (12.0, 6.0)
style.indent_spacing = 20.0
style.columns_min_spacing = 6.0
style.scrollbar_size = 12.0
style.scrollbar_rounding = 0.0
style.grab_min_size = 12.0
style.grab_rounding = 0.0
style.tab_rounding = 0.0
style.tab_border_size = 0.0
# style.tab_min_width_for_close_button = 0.0 # Not available in imgui_bundle
style.color_button_position = self.imgui.Dir_.right
style.button_text_align = (0.5, 0.5)
style.selectable_text_align = (0.0, 0.0)
# fmt: off
# Colors
style.set_color_(self.imgui.Col_.text, self.imgui.ImVec4(1.0, 1.0, 1.0, 1.0))
style.set_color_(self.imgui.Col_.text_disabled, self.imgui.ImVec4(0.2745098173618317, 0.3176470696926117, 0.4509803950786591, 1.0))
style.set_color_(self.imgui.Col_.window_bg, self.imgui.ImVec4(0.0784313753247261, 0.08627451211214066, 0.1019607856869698, 1.0))
style.set_color_(self.imgui.Col_.child_bg, self.imgui.ImVec4(0.0784313753247261, 0.08627451211214066, 0.1019607856869698, 1.0))
style.set_color_(self.imgui.Col_.popup_bg, self.imgui.ImVec4(0.0784313753247261, 0.08627451211214066, 0.1019607856869698, 1.0))
style.set_color_(self.imgui.Col_.border, self.imgui.ImVec4(0.1568627506494522, 0.168627455830574, 0.1921568661928177, 1.0))
style.set_color_(self.imgui.Col_.border_shadow, self.imgui.ImVec4(0.0784313753247261, 0.08627451211214066, 0.1019607856869698, 1.0))
style.set_color_(self.imgui.Col_.frame_bg, self.imgui.ImVec4(0.1176470592617989, 0.1333333402872086, 0.1490196138620377, 1.0))
style.set_color_(self.imgui.Col_.frame_bg_hovered, self.imgui.ImVec4(0.1568627506494522, 0.168627455830574, 0.1921568661928177, 1.0))
style.set_color_(self.imgui.Col_.frame_bg_active, self.imgui.ImVec4(0.2352941185235977, 0.2156862765550613, 0.5960784554481506, 1.0))
style.set_color_(self.imgui.Col_.title_bg, self.imgui.ImVec4(0.0470588244497776, 0.05490196123719215, 0.07058823853731155, 1.0))
style.set_color_(self.imgui.Col_.title_bg_active, self.imgui.ImVec4(0.0470588244497776, 0.05490196123719215, 0.07058823853731155, 1.0))
style.set_color_(self.imgui.Col_.title_bg_collapsed, self.imgui.ImVec4(0.0784313753247261, 0.08627451211214066, 0.1019607856869698, 1.0))
style.set_color_(self.imgui.Col_.menu_bar_bg, self.imgui.ImVec4(0.09803921729326248, 0.105882354080677, 0.1215686276555061, 1.0))
style.set_color_(self.imgui.Col_.scrollbar_bg, self.imgui.ImVec4(0.0470588244497776, 0.05490196123719215, 0.07058823853731155, 1.0))
style.set_color_(self.imgui.Col_.scrollbar_grab, self.imgui.ImVec4(0.1176470592617989, 0.1333333402872086, 0.1490196138620377, 1.0))
style.set_color_(self.imgui.Col_.scrollbar_grab_hovered, self.imgui.ImVec4(0.1568627506494522, 0.168627455830574, 0.1921568661928177, 1.0))
style.set_color_(self.imgui.Col_.scrollbar_grab_active, self.imgui.ImVec4(0.1176470592617989, 0.1333333402872086, 0.1490196138620377, 1.0))
style.set_color_(self.imgui.Col_.check_mark, self.imgui.ImVec4(0.4980392158031464, 0.5137255191802979, 1.0, 1.0))
style.set_color_(self.imgui.Col_.slider_grab, self.imgui.ImVec4(0.4980392158031464, 0.5137255191802979, 1.0, 1.0))
style.set_color_(self.imgui.Col_.slider_grab_active, self.imgui.ImVec4(0.5372549295425415, 0.5529412031173706, 1.0, 1.0))
style.set_color_(self.imgui.Col_.button, self.imgui.ImVec4(0.1176470592617989, 0.1333333402872086, 0.1490196138620377, 1.0))
style.set_color_(self.imgui.Col_.button_hovered, self.imgui.ImVec4(0.196078434586525, 0.1764705926179886, 0.5450980663299561, 1.0))
style.set_color_(self.imgui.Col_.button_active, self.imgui.ImVec4(0.2352941185235977, 0.2156862765550613, 0.5960784554481506, 1.0))
style.set_color_(self.imgui.Col_.header, self.imgui.ImVec4(0.1176470592617989, 0.1333333402872086, 0.1490196138620377, 1.0))
style.set_color_(self.imgui.Col_.header_hovered, self.imgui.ImVec4(0.196078434586525, 0.1764705926179886, 0.5450980663299561, 1.0))
style.set_color_(self.imgui.Col_.header_active, self.imgui.ImVec4(0.2352941185235977, 0.2156862765550613, 0.5960784554481506, 1.0))
style.set_color_(self.imgui.Col_.separator, self.imgui.ImVec4(0.1568627506494522, 0.1843137294054031, 0.250980406999588, 1.0))
style.set_color_(self.imgui.Col_.separator_hovered, self.imgui.ImVec4(0.1568627506494522, 0.1843137294054031, 0.250980406999588, 1.0))
style.set_color_(self.imgui.Col_.separator_active, self.imgui.ImVec4(0.1568627506494522, 0.1843137294054031, 0.250980406999588, 1.0))
style.set_color_(self.imgui.Col_.resize_grip, self.imgui.ImVec4(0.1176470592617989, 0.1333333402872086, 0.1490196138620377, 1.0))
style.set_color_(self.imgui.Col_.resize_grip_hovered, self.imgui.ImVec4(0.196078434586525, 0.1764705926179886, 0.5450980663299561, 1.0))
style.set_color_(self.imgui.Col_.resize_grip_active, self.imgui.ImVec4(0.2352941185235977, 0.2156862765550613, 0.5960784554481506, 1.0))
style.set_color_(self.imgui.Col_.tab, self.imgui.ImVec4(0.0470588244497776, 0.05490196123719215, 0.07058823853731155, 1.0))
style.set_color_(self.imgui.Col_.tab_hovered, self.imgui.ImVec4(0.1176470592617989, 0.1333333402872086, 0.1490196138620377, 1.0))
style.set_color_(self.imgui.Col_.tab_selected, self.imgui.ImVec4(0.09803921729326248, 0.105882354080677, 0.1215686276555061, 1.0))
style.set_color_(self.imgui.Col_.tab_dimmed, self.imgui.ImVec4(0.0470588244497776, 0.05490196123719215, 0.07058823853731155, 1.0))
style.set_color_(self.imgui.Col_.tab_dimmed_selected, self.imgui.ImVec4(0.0784313753247261, 0.08627451211214066, 0.1019607856869698, 1.0))
style.set_color_(self.imgui.Col_.plot_lines, self.imgui.ImVec4(0.5215686559677124, 0.6000000238418579, 0.7019608020782471, 1.0))
style.set_color_(self.imgui.Col_.plot_lines_hovered, self.imgui.ImVec4(0.03921568766236305, 0.9803921580314636, 0.9803921580314636, 1.0))
style.set_color_(self.imgui.Col_.plot_histogram, self.imgui.ImVec4(1.0, 0.2901960909366608, 0.5960784554481506, 1.0))
style.set_color_(self.imgui.Col_.plot_histogram_hovered, self.imgui.ImVec4(0.9960784316062927, 0.4745098054409027, 0.6980392336845398, 1.0))
style.set_color_(self.imgui.Col_.table_header_bg, self.imgui.ImVec4(0.0470588244497776, 0.05490196123719215, 0.07058823853731155, 1.0))
style.set_color_(self.imgui.Col_.table_border_strong, self.imgui.ImVec4(0.0470588244497776, 0.05490196123719215, 0.07058823853731155, 1.0))
style.set_color_(self.imgui.Col_.table_border_light, self.imgui.ImVec4(0.0, 0.0, 0.0, 1.0))
style.set_color_(self.imgui.Col_.table_row_bg, self.imgui.ImVec4(0.1176470592617989, 0.1333333402872086, 0.1490196138620377, 1.0))
style.set_color_(self.imgui.Col_.table_row_bg_alt, self.imgui.ImVec4(0.09803921729326248, 0.105882354080677, 0.1215686276555061, 1.0))
style.set_color_(self.imgui.Col_.text_selected_bg, self.imgui.ImVec4(0.2352941185235977, 0.2156862765550613, 0.5960784554481506, 1.0))
style.set_color_(self.imgui.Col_.drag_drop_target, self.imgui.ImVec4(0.4980392158031464, 0.5137255191802979, 1.0, 1.0))
style.set_color_(self.imgui.Col_.nav_cursor, self.imgui.ImVec4(0.4980392158031464, 0.5137255191802979, 1.0, 1.0))
style.set_color_(self.imgui.Col_.nav_windowing_highlight, self.imgui.ImVec4(0.4980392158031464, 0.5137255191802979, 1.0, 1.0))
style.set_color_(self.imgui.Col_.nav_windowing_dim_bg, self.imgui.ImVec4(0.196078434586525, 0.1764705926179886, 0.5450980663299561, 0.501960813999176))
style.set_color_(self.imgui.Col_.modal_window_dim_bg, self.imgui.ImVec4(0.196078434586525, 0.1764705926179886, 0.5450980663299561, 0.501960813999176))
# fmt: on
def begin_frame(self):
"""Renders a single frame of the UI. This should be called from the main render loop."""
if not self.is_available:
return
try:
self.impl.process_inputs()
except AttributeError:
# Older integrations may not require this
pass
self.imgui.new_frame()
self.giz.begin_frame()
def end_frame(self):
if not self.is_available:
return
self._poll_file_dialog()
self.imgui.render()
self.imgui.end_frame()
def render(self):
if not self.is_available:
return
self.impl.render(self.imgui.get_draw_data())
def is_capturing_mouse(self):
if not self.is_available:
return False
return self.io.want_capture_mouse
def is_capturing_keyboard(self):
if not self.is_available:
return False
return self.io.want_capture_keyboard
def is_capturing(self):
if not self.is_available:
return False
return self.is_capturing_mouse() or self.is_capturing_keyboard()
def resize(self, width, height):
if not self.is_available:
return
# Get framebuffer size for proper DPI scaling
fb_width, fb_height = self.window.get_framebuffer_size()
# Update display framebuffer scale
if width > 0 and height > 0:
scale_x = fb_width / width
scale_y = fb_height / height
self.io.display_framebuffer_scale = (scale_x, scale_y)
# Use framebuffer size for display size
self.io.display_size = fb_width, fb_height
def get_theme_color(self, color_id, fallback_color=(1.0, 1.0, 1.0, 1.0)):
"""Get a color from the current theme with fallback.
Args:
color_id: ImGui color constant (e.g., self.imgui.Col_.text_disabled)
fallback_color: RGBA tuple to use if color not available
Returns:
RGBA tuple of the theme color or fallback
"""
if not self.is_available:
return fallback_color
try:
style = self.imgui.get_style()
color = style.color_(color_id)
return (color.x, color.y, color.z, color.w)
except (AttributeError, KeyError, IndexError):
return fallback_color
def consume_file_dialog_result(self) -> str | None:
"""Return the latest completed file dialog path once.
File dialogs are asynchronous: `open_load_file_dialog()` and
`open_save_file_dialog()` queue a native dialog and return immediately.
Poll this method from the render loop to retrieve the selected path.
"""
if not self.is_available:
return None
result = self._file_dialog_result
self._file_dialog_result = None
return result
def _poll_file_dialog(self):
"""Check if pending file dialog has completed."""
if not self.is_available:
return
if self._pending_file_dialog is None:
return
if self._pending_file_dialog.ready():
result = self._pending_file_dialog.result()
if result:
if isinstance(result, list):
if len(result) == 1:
self._file_dialog_result = result[0]
elif len(result) > 1:
print("Warning: multiple files selected; expected a single file.")
else:
self._file_dialog_result = result
self._pending_file_dialog = None
def open_save_file_dialog(self, title: str = "Save File") -> None:
"""Start an asynchronous native OS save-file dialog.
Use `consume_file_dialog_result()` to retrieve the selected path.
"""
if not self.is_available:
return
try:
from imgui_bundle import portable_file_dialogs as pfd
self._pending_file_dialog = pfd.save_file(title, os.getcwd())
except ImportError:
print("Warning: portable_file_dialogs not available")
def open_load_file_dialog(self, title: str = "Open File") -> None:
"""Start an asynchronous native OS open-file dialog.
Use `consume_file_dialog_result()` to retrieve the selected path.
"""
if not self.is_available:
return
try:
from imgui_bundle import portable_file_dialogs as pfd
self._pending_file_dialog = pfd.open_file(title, os.getcwd())
except ImportError:
print("Warning: portable_file_dialogs not available")
def shutdown(self):
if not self.is_available:
return
self.impl.shutdown()
================================================
FILE: newton/_src/viewer/gl/icon.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
# generate_icons.py
from PIL import Image, ImageDraw, ImageFont # noqa: TID253
def create_and_save_emoji_png(character: str, size: int, filename: str):
"""
Renders a Unicode character onto a transparent PNG and saves it.
"""
# Create a blank, transparent image
image = Image.new("RGBA", (size, size), (0, 0, 0, 0))
draw = ImageDraw.Draw(image)
# Use a font that supports color emoji.
# Windows: "Segoe UI Emoji" -> seguiemj.ttf
# macOS: "Apple Color Emoji"
# Linux: "Noto Color Emoji"
font_path = "seguiemj.ttf"
font_size = int(size * 0.8) # Adjust font size relative to image size
try:
font = ImageFont.truetype(font_path, size=font_size)
except OSError:
print(f"Warning: Font '{font_path}' not found. Using default font.")
print("The icon may not render in color.")
font = ImageFont.load_default()
# Calculate position to center the character
draw.textbbox((0, 0), character, font=font, spacing=0)
x = size // 2
y = size // 2 + 2 # +2 fudge factor
# Draw the character onto the image
draw.text((x, y), anchor="mm", text=character, font=font, embedded_color=True)
# Save the image as a PNG file
image.save(filename, "PNG")
print(f"Successfully created {filename}")
if __name__ == "__main__":
emoji_char = "🍏"
sizes = [16, 32, 64]
for s in sizes:
output_filename = f"icon_{s}.png"
create_and_save_emoji_png(emoji_char, s, output_filename)
================================================
FILE: newton/_src/viewer/gl/opengl.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import ctypes
import io
import os
import sys
import numpy as np
import warp as wp
from newton import Mesh
from ...utils.mesh import compute_vertex_normals
from ...utils.texture import normalize_texture
from .shaders import (
FrameShader,
ShaderLine,
ShaderLineWireframe,
ShaderShape,
ShaderSky,
ShadowShader,
)
ENABLE_CUDA_INTEROP = False
ENABLE_GL_CHECKS = False
wp.set_module_options({"enable_backward": False})
def check_gl_error():
if not ENABLE_GL_CHECKS:
return
from pyglet import gl
error = gl.glGetError()
if error != gl.GL_NO_ERROR:
error_strings = {
gl.GL_INVALID_ENUM: "GL_INVALID_ENUM",
gl.GL_INVALID_VALUE: "GL_INVALID_VALUE",
gl.GL_INVALID_OPERATION: "GL_INVALID_OPERATION",
gl.GL_INVALID_FRAMEBUFFER_OPERATION: "GL_INVALID_FRAMEBUFFER_OPERATION",
gl.GL_OUT_OF_MEMORY: "GL_OUT_OF_MEMORY",
}
error_name = error_strings.get(error, f"Unknown error code: {error}")
import traceback # noqa: PLC0415
stack = traceback.format_stack()
print(f"OpenGL error: {error_name} ({error:#x})")
print(f"Called from: {''.join(stack[-2:-1])}")
def _upload_texture_from_file(gl, texture_image: np.ndarray) -> int:
image = normalize_texture(
texture_image,
flip_vertical=True,
require_channels=True,
scale_unit_range=True,
)
if image is None:
return 0
channels = image.shape[2]
if image.size == 0:
return 0
max_size = gl.GLint()
gl.glGetIntegerv(gl.GL_MAX_TEXTURE_SIZE, max_size)
if image.shape[0] > max_size.value or image.shape[1] > max_size.value:
return 0
texture_id = gl.GLuint()
gl.glGenTextures(1, texture_id)
gl.glBindTexture(gl.GL_TEXTURE_2D, texture_id)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_WRAP_S, gl.GL_REPEAT)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_WRAP_T, gl.GL_REPEAT)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, gl.GL_LINEAR_MIPMAP_LINEAR)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_LINEAR)
format_enum = gl.GL_RGBA if channels == 4 else gl.GL_RGB
row_stride = image.shape[1] * channels
prev_alignment = None
if row_stride % 4 != 0:
prev_alignment = gl.GLint()
gl.glGetIntegerv(gl.GL_UNPACK_ALIGNMENT, prev_alignment)
gl.glPixelStorei(gl.GL_UNPACK_ALIGNMENT, 1)
gl.glTexImage2D(
gl.GL_TEXTURE_2D,
0,
format_enum,
image.shape[1],
image.shape[0],
0,
format_enum,
gl.GL_UNSIGNED_BYTE,
image.ctypes.data_as(ctypes.POINTER(ctypes.c_ubyte)),
)
if prev_alignment is not None:
gl.glPixelStorei(gl.GL_UNPACK_ALIGNMENT, prev_alignment.value)
gl.glGenerateMipmap(gl.GL_TEXTURE_2D)
gl.glBindTexture(gl.GL_TEXTURE_2D, 0)
return texture_id
@wp.struct
class RenderVertex:
pos: wp.vec3
normal: wp.vec3
uv: wp.vec2
@wp.struct
class LineVertex:
pos: wp.vec3
color: wp.vec3
@wp.kernel
def fill_vertex_data(
points: wp.array[wp.vec3],
normals: wp.array[wp.vec3],
uvs: wp.array[wp.vec2],
vertices: wp.array[RenderVertex],
):
tid = wp.tid()
vertices[tid].pos = points[tid]
if normals:
vertices[tid].normal = normals[tid]
if uvs:
vertices[tid].uv = uvs[tid]
@wp.kernel
def fill_line_vertex_data(
starts: wp.array[wp.vec3],
ends: wp.array[wp.vec3],
colors: wp.array[wp.vec3],
vertices: wp.array[LineVertex],
):
tid = wp.tid()
# Each line has 2 vertices (begin and end)
vertex_idx = tid * 2
# First vertex (line begin)
vertices[vertex_idx].pos = starts[tid]
vertices[vertex_idx].color = colors[tid]
# Second vertex (line end)
vertices[vertex_idx + 1].pos = ends[tid]
vertices[vertex_idx + 1].color = colors[tid]
class MeshGL:
"""Encapsulates mesh data and OpenGL buffers for a shape."""
def __init__(self, num_points, num_indices, device, hidden=False, backface_culling=True):
"""Initialize mesh data with vertices and indices."""
gl = RendererGL.gl
self.num_points = num_points
self.num_indices = num_indices
# Store references to input buffers and rendering data
self.device = device
self.hidden = hidden
self.backface_culling = backface_culling
self.vertices = wp.zeros(num_points, dtype=RenderVertex, device=self.device)
self.indices = None
self.normals = None # scratch buffer used during normal recomputation
self.texture_id = None
# Set up vertex attributes in the packed format the shaders expect
self.vertex_byte_size = 12 + 12 + 8
self.index_byte_size = 4
self.vbo_size = self.vertex_byte_size * num_points
self.ebo_size = self.index_byte_size * num_indices
# Create OpenGL buffers
self.vao = gl.GLuint()
gl.glGenVertexArrays(1, self.vao)
gl.glBindVertexArray(self.vao)
self.vbo = gl.GLuint()
gl.glGenBuffers(1, self.vbo)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.vbo)
gl.glBufferData(gl.GL_ARRAY_BUFFER, self.vbo_size, None, gl.GL_STATIC_DRAW)
self.ebo = gl.GLuint()
gl.glGenBuffers(1, self.ebo)
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, self.ebo)
gl.glBufferData(gl.GL_ELEMENT_ARRAY_BUFFER, self.ebo_size, None, gl.GL_STATIC_DRAW)
# positions (location 0)
gl.glVertexAttribPointer(0, 3, gl.GL_FLOAT, gl.GL_FALSE, self.vertex_byte_size, ctypes.c_void_p(0))
gl.glEnableVertexAttribArray(0)
# normals (location 1)
gl.glVertexAttribPointer(1, 3, gl.GL_FLOAT, gl.GL_FALSE, self.vertex_byte_size, ctypes.c_void_p(3 * 4))
gl.glEnableVertexAttribArray(1)
# uv coordinates (location 2)
gl.glVertexAttribPointer(2, 2, gl.GL_FLOAT, gl.GL_FALSE, self.vertex_byte_size, ctypes.c_void_p(6 * 4))
gl.glEnableVertexAttribArray(2)
# set constant instance transform
gl.glDisableVertexAttribArray(3)
gl.glDisableVertexAttribArray(4)
gl.glDisableVertexAttribArray(5)
gl.glDisableVertexAttribArray(6)
gl.glDisableVertexAttribArray(7)
gl.glDisableVertexAttribArray(8)
gl.glDisableVertexAttribArray(9)
# column 0 (1,0,0,0)
gl.glVertexAttrib4f(3, 1.0, 0.0, 0.0, 0.0)
# column 1 (0,1,0,0)
gl.glVertexAttrib4f(4, 0.0, 1.0, 0.0, 0.0)
# column 2 (0,0,1,0)
gl.glVertexAttrib4f(5, 0.0, 0.0, 1.0, 0.0)
# column 3 (0,0,0,1)
gl.glVertexAttrib4f(6, 0.0, 0.0, 0.0, 1.0)
# albedo
gl.glVertexAttrib3f(7, 0.7, 0.5, 0.3)
# material = (roughness, metallic, checker, texture_enable)
gl.glVertexAttrib4f(8, 0.5, 0.0, 0.0, 0.0)
gl.glBindVertexArray(0)
# Create CUDA-GL interop buffer for efficient updates
if ENABLE_CUDA_INTEROP and self.device.is_cuda:
self.vertex_cuda_buffer = wp.RegisteredGLBuffer(int(self.vbo.value), self.device)
else:
self.vertex_cuda_buffer = None
self._points = None
def destroy(self):
"""Clean up OpenGL resources."""
gl = RendererGL.gl
try:
if hasattr(self, "vao"):
gl.glDeleteVertexArrays(1, self.vao)
if hasattr(self, "vbo"):
gl.glDeleteBuffers(1, self.vbo)
if hasattr(self, "ebo"):
gl.glDeleteBuffers(1, self.ebo)
if hasattr(self, "texture_id") and self.texture_id is not None:
gl.glDeleteTextures(1, self.texture_id)
except Exception:
# Ignore any errors if the GL context has already been torn down
pass
def update(self, points, indices, normals, uvs, texture=None):
"""Update vertex positions in the VBO.
Args:
points: New point positions (warp array or numpy array)
scale: Scaling factor for positions
"""
gl = RendererGL.gl
if len(points) != len(self.vertices):
raise RuntimeError("Number of points does not match")
self._points = points
# only update indices the first time (no topology changes)
if self.indices is None:
self.indices = wp.clone(indices).view(dtype=wp.uint32)
self.num_indices = int(len(self.indices))
host_indices = self.indices.numpy()
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, self.ebo)
gl.glBufferData(
gl.GL_ELEMENT_ARRAY_BUFFER, host_indices.nbytes, host_indices.ctypes.data, gl.GL_STATIC_DRAW
)
# If normals are missing, compute them before packing vertex data.
if points is not None and normals is None:
self.recompute_normals()
normals = self.normals
# update gfx vertices
wp.launch(
fill_vertex_data,
dim=len(self.vertices),
inputs=[points, normals, uvs],
outputs=[self.vertices],
device=self.device,
)
# upload vertices to GL
if ENABLE_CUDA_INTEROP and self.vertices.device.is_cuda:
# upload points via CUDA if possible
vbo_vertices = self.vertex_cuda_buffer.map(dtype=RenderVertex, shape=self.vertices.shape)
wp.copy(vbo_vertices, self.vertices)
self.vertex_cuda_buffer.unmap()
else:
host_vertices = self.vertices.numpy()
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.vbo)
gl.glBufferData(gl.GL_ARRAY_BUFFER, host_vertices.nbytes, host_vertices.ctypes.data, gl.GL_STATIC_DRAW)
self.update_texture(texture)
def recompute_normals(self):
if self._points is None or self.indices is None:
return
self.normals = compute_vertex_normals(
self._points,
self.indices,
normals=self.normals,
device=self.device,
)
def update_texture(self, texture=None):
gl = RendererGL.gl
texture_image = None
if texture is not None:
from ...utils.texture import load_texture # noqa: PLC0415
texture_image = load_texture(texture)
if texture_image is None:
if self.texture_id is not None:
try:
gl.glDeleteTextures(1, self.texture_id)
except Exception:
pass
self.texture_id = None
return
if self.texture_id is not None:
try:
gl.glDeleteTextures(1, self.texture_id)
except Exception:
pass
self.texture_id = None
texture_id = _upload_texture_from_file(gl, texture_image)
if not texture_id:
return
self.texture_id = texture_id
def render(self):
if not self.hidden:
gl = RendererGL.gl
if self.backface_culling:
gl.glEnable(gl.GL_CULL_FACE)
else:
gl.glDisable(gl.GL_CULL_FACE)
gl.glActiveTexture(gl.GL_TEXTURE1)
if self.texture_id is not None:
gl.glBindTexture(gl.GL_TEXTURE_2D, self.texture_id)
else:
gl.glBindTexture(gl.GL_TEXTURE_2D, RendererGL.get_fallback_texture())
gl.glBindVertexArray(self.vao)
gl.glDrawElements(gl.GL_TRIANGLES, self.num_indices, gl.GL_UNSIGNED_INT, None)
gl.glBindVertexArray(0)
class LinesGL:
"""Encapsulates line data and OpenGL buffers for line rendering."""
def __init__(self, max_lines, device, hidden=False):
"""Initialize line data with the specified maximum number of lines.
Args:
max_lines: Maximum number of lines that can be rendered
device: Warp device to use
hidden: Whether the lines are initially hidden
"""
gl = RendererGL.gl
self.max_lines = max_lines
self.max_vertices = max_lines * 2 # Each line has 2 vertices
self.num_lines = 0 # Current number of active lines to render
# Store references to input buffers and rendering data
self.device = device
self.hidden = hidden
self.vertices = wp.zeros(self.max_vertices, dtype=LineVertex, device=self.device)
# Set up vertex attributes for lines (position + color)
self.vertex_byte_size = 12 + 12 # 3 floats for pos + 3 floats for color
self.vbo_size = self.vertex_byte_size * self.max_vertices
# Create OpenGL buffers
self.vao = gl.GLuint()
gl.glGenVertexArrays(1, self.vao)
gl.glBindVertexArray(self.vao)
self.vbo = gl.GLuint()
gl.glGenBuffers(1, self.vbo)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.vbo)
gl.glBufferData(gl.GL_ARRAY_BUFFER, self.vbo_size, None, gl.GL_DYNAMIC_DRAW)
# positions (location 0)
gl.glVertexAttribPointer(0, 3, gl.GL_FLOAT, gl.GL_FALSE, self.vertex_byte_size, ctypes.c_void_p(0))
gl.glEnableVertexAttribArray(0)
# colors (location 1)
gl.glVertexAttribPointer(1, 3, gl.GL_FLOAT, gl.GL_FALSE, self.vertex_byte_size, ctypes.c_void_p(3 * 4))
gl.glEnableVertexAttribArray(1)
gl.glBindVertexArray(0)
# Create CUDA-GL interop buffer for efficient updates
if ENABLE_CUDA_INTEROP and self.device.is_cuda:
self.vertex_cuda_buffer = wp.RegisteredGLBuffer(int(self.vbo.value), self.device)
else:
self.vertex_cuda_buffer = None
def destroy(self):
"""Clean up OpenGL resources."""
gl = RendererGL.gl
try:
if hasattr(self, "vao"):
gl.glDeleteVertexArrays(1, self.vao)
if hasattr(self, "vbo"):
gl.glDeleteBuffers(1, self.vbo)
except Exception:
# Ignore any errors if the GL context has already been torn down
pass
def update(self, starts, ends, colors):
"""Update line data in the VBO.
Args:
starts: Array of line start positions (warp array of vec3) or None
ends: Array of line end positions (warp array of vec3) or None
colors: Array of line colors (warp array of vec3) or None
"""
gl = RendererGL.gl
# Handle None values by setting line count to zero
if starts is None or ends is None or colors is None:
self.num_lines = 0
return
# Update current line count
self.num_lines = len(starts)
if self.num_lines > self.max_lines:
raise RuntimeError(f"Number of lines ({self.num_lines}) exceeds maximum ({self.max_lines})")
if len(ends) != self.num_lines:
raise RuntimeError("Number of line ends does not match line begins")
if len(colors) != self.num_lines:
raise RuntimeError("Number of line colors does not match line begins")
# Only update vertex data if we have lines to render
if self.num_lines > 0:
# Update line vertex data using the kernel
wp.launch(
fill_line_vertex_data,
dim=self.num_lines,
inputs=[starts, ends, colors],
outputs=[self.vertices],
device=self.device,
)
# Upload vertices to GL
if ENABLE_CUDA_INTEROP and self.vertices.device.is_cuda:
# Upload points via CUDA if possible
vbo_vertices = self.vertex_cuda_buffer.map(dtype=LineVertex, shape=self.vertices.shape)
wp.copy(vbo_vertices, self.vertices)
self.vertex_cuda_buffer.unmap()
else:
host_vertices = self.vertices.numpy()
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.vbo)
gl.glBufferData(gl.GL_ARRAY_BUFFER, host_vertices.nbytes, host_vertices.ctypes.data, gl.GL_DYNAMIC_DRAW)
def render(self):
if not self.hidden and self.num_lines > 0:
gl = RendererGL.gl
gl.glDisable(gl.GL_CULL_FACE) # Lines don't need culling
gl.glBindVertexArray(self.vao)
# Only render vertices for the current number of lines
current_vertices = self.num_lines * 2
gl.glDrawArrays(gl.GL_LINES, 0, current_vertices)
gl.glBindVertexArray(0)
class WireframeShapeGL:
"""Per-shape wireframe edge data rendered via GL_LINES with a geometry shader.
Stores interleaved (position, color) vertex data in model space.
The World matrix is set per-shape by the caller before drawing.
Multiple instances can share the same VAO/VBO when created via
:meth:`create_shared`. Only the *owner* (``_owns_gl == True``)
deletes the GL resources on :meth:`destroy`.
"""
def __init__(self, vertex_data: np.ndarray):
"""Create a wireframe shape that owns its GL resources."""
gl = RendererGL.gl
self.num_vertices = len(vertex_data)
self.hidden = False
self.world_matrix = np.eye(4, dtype=np.float32)
self._owns_gl = True
vertex_byte_size = 6 * 4
self.vao = gl.GLuint()
gl.glGenVertexArrays(1, self.vao)
gl.glBindVertexArray(self.vao)
self.vbo = gl.GLuint()
gl.glGenBuffers(1, self.vbo)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.vbo)
data = vertex_data.astype(np.float32)
gl.glBufferData(gl.GL_ARRAY_BUFFER, data.nbytes, data.ctypes.data, gl.GL_STATIC_DRAW)
gl.glVertexAttribPointer(0, 3, gl.GL_FLOAT, gl.GL_FALSE, vertex_byte_size, ctypes.c_void_p(0))
gl.glEnableVertexAttribArray(0)
gl.glVertexAttribPointer(1, 3, gl.GL_FLOAT, gl.GL_FALSE, vertex_byte_size, ctypes.c_void_p(3 * 4))
gl.glEnableVertexAttribArray(1)
gl.glBindVertexArray(0)
@classmethod
def create_shared(cls, owner: "WireframeShapeGL") -> "WireframeShapeGL":
"""Create an instance that shares *owner*'s VAO/VBO."""
obj = cls.__new__(cls)
obj.vao = owner.vao
obj.vbo = owner.vbo
obj.num_vertices = owner.num_vertices
obj.hidden = False
obj.world_matrix = np.eye(4, dtype=np.float32)
obj._owns_gl = False
return obj
def destroy(self):
"""Free GL resources if this instance owns them."""
if not getattr(self, "_owns_gl", False):
return
gl = RendererGL.gl
try:
if hasattr(self, "vao"):
gl.glDeleteVertexArrays(1, self.vao)
if hasattr(self, "vbo"):
gl.glDeleteBuffers(1, self.vbo)
except Exception:
pass
def render(self):
if self.hidden or self.num_vertices == 0:
return
gl = RendererGL.gl
gl.glBindVertexArray(self.vao)
gl.glDrawArrays(gl.GL_LINES, 0, self.num_vertices)
gl.glBindVertexArray(0)
@wp.kernel
def update_vbo_transforms(
instance_transforms: wp.array[wp.transform],
instance_scalings: wp.array[wp.vec3],
vbo_transforms: wp.array[wp.mat44],
):
"""Update VBO with simple instance transformation matrices."""
tid = wp.tid()
# Get transform and scaling
transform = instance_transforms[tid]
if instance_scalings:
s = instance_scalings[tid]
else:
s = wp.vec3(1.0, 1.0, 1.0)
# Extract position and rotation
p = wp.transform_get_translation(transform)
q = wp.transform_get_rotation(transform)
# Build rotation matrix
R = wp.quat_to_matrix(q)
# Apply scaling
vbo_transforms[tid] = wp.mat44(
R[0, 0] * s[0],
R[1, 0] * s[0],
R[2, 0] * s[0],
0.0,
R[0, 1] * s[1],
R[1, 1] * s[1],
R[2, 1] * s[1],
0.0,
R[0, 2] * s[2],
R[1, 2] * s[2],
R[2, 2] * s[2],
0.0,
p[0],
p[1],
p[2],
1.0,
)
@wp.kernel
def update_vbo_transforms_from_points(
points: wp.array[wp.vec3],
widths: wp.array[wp.float32],
vbo_transforms: wp.array[wp.mat44],
):
"""Update VBO with simple instance transformation matrices."""
tid = wp.tid()
# Get transform and scaling
p = points[tid]
if widths:
s = widths[tid]
else:
s = 1.0
# Build rotation matrix
R = wp.identity(n=3, dtype=wp.float32)
# Apply scaling
vbo_transforms[tid] = wp.mat44(
R[0, 0] * s,
R[1, 0] * s,
R[2, 0] * s,
0.0,
R[0, 1] * s,
R[1, 1] * s,
R[2, 1] * s,
0.0,
R[0, 2] * s,
R[1, 2] * s,
R[2, 2] * s,
0.0,
p[0],
p[1],
p[2],
1.0,
)
class MeshInstancerGL:
"""
Handles instanced rendering for a mesh.
Note the vertices must be in the 8-dimensional format:
[3D point, 3D normal, UV texture coordinates]
"""
def __init__(self, num_instances, mesh):
self.mesh = mesh
self.device = mesh.device
self.hidden = False
self.instance_transform_buffer = None
self.instance_color_buffer = None
self.instance_material_buffer = None
self.instance_transform_cuda_buffer = None
self.allocate(num_instances)
self.active_instances = num_instances
def __del__(self):
gl = RendererGL.gl
if self.instance_transform_cuda_buffer is not None:
try:
gl.glDeleteBuffers(1, self.instance_transform_cuda_buffer)
except Exception:
# Ignore any errors (e.g., context already destroyed)
pass
if hasattr(self, "vao") and self.vao is not None:
try:
gl.glDeleteVertexArrays(1, self.vao)
gl.glDeleteBuffers(1, self.instance_transform_buffer)
gl.glDeleteBuffers(1, self.instance_color_buffer)
gl.glDeleteBuffers(1, self.instance_material_buffer)
except Exception:
# Ignore any errors during interpreter shutdown
pass
def allocate(self, num_instances):
gl = RendererGL.gl
self.world_xforms = wp.zeros(num_instances, dtype=wp.mat44, device=self.device)
self.vao = gl.GLuint()
self.instance_transform_buffer = gl.GLuint()
self.instance_color_buffer = gl.GLuint()
self.instance_material_buffer = gl.GLuint()
self.num_instances = num_instances
gl.glGenVertexArrays(1, self.vao)
gl.glBindVertexArray(self.vao)
# -------------------------
# index buffer
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, self.mesh.ebo)
# ------------------------
# mesh buffers
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.mesh.vbo)
# positions
gl.glVertexAttribPointer(0, 3, gl.GL_FLOAT, gl.GL_FALSE, self.mesh.vertex_byte_size, ctypes.c_void_p(0))
gl.glEnableVertexAttribArray(0)
# normals
gl.glVertexAttribPointer(
1,
3,
gl.GL_FLOAT,
gl.GL_FALSE,
self.mesh.vertex_byte_size,
ctypes.c_void_p(3 * 4),
)
gl.glEnableVertexAttribArray(1)
# uv coordinates
gl.glVertexAttribPointer(
2,
2,
gl.GL_FLOAT,
gl.GL_FALSE,
self.mesh.vertex_byte_size,
ctypes.c_void_p(6 * 4),
)
gl.glEnableVertexAttribArray(2)
self.transform_byte_size = 16 * 4 # sizeof(mat44)
self.color_byte_size = 3 * 4 # sizeof(vec3)
self.material_byte_size = 4 * 4 # sizeof(vec4)
self.instance_transform_buffer_size = self.transform_byte_size * self.num_instances
self.instance_color_buffer_size = self.color_byte_size * self.num_instances
self.instance_material_buffer_size = self.material_byte_size * self.num_instances
# ------------------------
# transform buffer
gl.glGenBuffers(1, self.instance_transform_buffer)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_transform_buffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, self.instance_transform_buffer_size, None, gl.GL_DYNAMIC_DRAW)
# we can only send vec4s to the shader, so we need to split the instance transforms matrix into its column vectors
for i in range(4):
gl.glVertexAttribPointer(
3 + i, 4, gl.GL_FLOAT, gl.GL_FALSE, self.transform_byte_size, ctypes.c_void_p(i * 16)
)
gl.glEnableVertexAttribArray(3 + i)
gl.glVertexAttribDivisor(3 + i, 1)
# ------------------------
# colors
gl.glGenBuffers(1, self.instance_color_buffer)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_color_buffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, self.instance_color_buffer_size, None, gl.GL_STATIC_DRAW)
gl.glVertexAttribPointer(7, 3, gl.GL_FLOAT, gl.GL_FALSE, self.color_byte_size, ctypes.c_void_p(0))
gl.glEnableVertexAttribArray(7)
gl.glVertexAttribDivisor(7, 1)
# ------------------------
# materials buffer
host_materials = np.zeros(self.num_instances * 4, dtype=np.float32)
gl.glGenBuffers(1, self.instance_material_buffer)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_material_buffer)
gl.glBufferData(
gl.GL_ARRAY_BUFFER, self.instance_material_buffer_size, host_materials.ctypes.data, gl.GL_STATIC_DRAW
)
gl.glVertexAttribPointer(8, 4, gl.GL_FLOAT, gl.GL_FALSE, self.material_byte_size, ctypes.c_void_p(0))
gl.glEnableVertexAttribArray(8)
gl.glVertexAttribDivisor(8, 1)
gl.glBindVertexArray(0)
# Create CUDA buffer for instance transforms
if ENABLE_CUDA_INTEROP and self.device.is_cuda:
self._instance_transform_cuda_buffer = wp.RegisteredGLBuffer(
int(self.instance_transform_buffer.value), self.device, flags=wp.RegisteredGLBuffer.WRITE_DISCARD
)
else:
self._instance_transform_cuda_buffer = None
def update_from_transforms(
self,
transforms: wp.array = None,
scalings: wp.array = None,
colors: wp.array = None,
materials: wp.array = None,
):
if transforms is None:
active_count = 0
else:
active_count = len(transforms)
if active_count > self.num_instances:
raise ValueError(
f"Active instance count ({active_count}) exceeds allocated capacity ({self.num_instances})."
)
if scalings is not None and len(scalings) != active_count:
raise ValueError("Number of scalings must match number of transforms")
if active_count > 0:
wp.launch(
update_vbo_transforms,
dim=active_count,
inputs=[
transforms,
scalings,
],
outputs=[
self.world_xforms,
],
device=self.device,
record_tape=False,
)
self.active_instances = active_count
# Upload the full buffer; only the first `active_instances` rows are rendered
self._update_vbo(self.world_xforms, colors, materials)
# helper to update instance transforms from points
def update_from_points(self, points, widths, colors):
if points is None:
active = 0
else:
active = len(points)
if active > self.num_instances:
raise ValueError("Active point count exceeds allocated capacity. Reallocate before updating.")
self.active_instances = active
if self.active_instances > 0 and (points is not None or widths is not None):
wp.launch(
update_vbo_transforms_from_points,
dim=self.active_instances,
inputs=[
points,
widths,
],
outputs=[
self.world_xforms,
],
device=self.device,
record_tape=False,
)
self._update_vbo(self.world_xforms, colors, None)
# upload to vbo
def _update_vbo(self, xforms, colors, materials):
gl = RendererGL.gl
if ENABLE_CUDA_INTEROP and self.device.is_cuda:
vbo_transforms = self._instance_transform_cuda_buffer.map(dtype=wp.mat44, shape=(self.num_instances,))
wp.copy(vbo_transforms, xforms)
self._instance_transform_cuda_buffer.unmap()
else:
host_transforms = xforms.numpy()
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_transform_buffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, host_transforms.nbytes, host_transforms.ctypes.data, gl.GL_DYNAMIC_DRAW)
# update other properties through CPU for now
if colors is not None:
host_colors = colors.numpy()
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_color_buffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, host_colors.nbytes, host_colors.ctypes.data, gl.GL_STATIC_DRAW)
if materials is not None:
host_materials = materials.numpy()
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_material_buffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, host_materials.nbytes, host_materials.ctypes.data, gl.GL_STATIC_DRAW)
def update_from_pinned(self, host_transforms_np, count, colors=None, materials=None):
"""Upload pre-computed mat44 transforms from pinned host memory to GL.
Args:
host_transforms_np: Numpy array slice of mat44 transforms.
count: Number of active instances.
colors: Optional wp.array of per-instance colors.
materials: Optional wp.array of per-instance materials.
"""
gl = RendererGL.gl
if count > self.num_instances:
raise ValueError(f"Active instance count ({count}) exceeds allocated capacity ({self.num_instances}).")
self.active_instances = count
if count > 0:
nbytes = count * self.transform_byte_size
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_transform_buffer)
gl.glBufferSubData(gl.GL_ARRAY_BUFFER, 0, nbytes, host_transforms_np.ctypes.data)
if colors is not None:
host_colors = colors.numpy()
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_color_buffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, host_colors.nbytes, host_colors.ctypes.data, gl.GL_STATIC_DRAW)
if materials is not None:
host_materials = materials.numpy()
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self.instance_material_buffer)
gl.glBufferData(gl.GL_ARRAY_BUFFER, host_materials.nbytes, host_materials.ctypes.data, gl.GL_STATIC_DRAW)
def render(self):
gl = RendererGL.gl
if self.hidden:
return
if self.mesh.backface_culling:
gl.glEnable(gl.GL_CULL_FACE)
else:
gl.glDisable(gl.GL_CULL_FACE)
gl.glActiveTexture(gl.GL_TEXTURE1)
if self.mesh.texture_id is not None:
gl.glBindTexture(gl.GL_TEXTURE_2D, self.mesh.texture_id)
else:
gl.glBindTexture(gl.GL_TEXTURE_2D, RendererGL.get_fallback_texture())
gl.glBindVertexArray(self.vao)
gl.glDrawElementsInstanced(
gl.GL_TRIANGLES, self.mesh.num_indices, gl.GL_UNSIGNED_INT, None, self.active_instances
)
gl.glBindVertexArray(0)
class RendererGL:
gl = None # Class-level variable to hold the imported module
_fallback_texture = None # 1x1 white texture bound when no albedo is set (suppresses macOS GL warning)
@classmethod
def initialize_gl(cls):
if cls.gl is None: # Only import if not already imported
from pyglet import gl
cls.gl = gl
@classmethod
def get_fallback_texture(cls):
"""Return a 1x1 white RGBA texture, creating it on first use."""
if cls._fallback_texture is None:
gl = cls.gl
tex = gl.GLuint()
gl.glGenTextures(1, tex)
gl.glBindTexture(gl.GL_TEXTURE_2D, tex)
pixel = (gl.GLubyte * 4)(255, 255, 255, 255)
gl.glTexImage2D(gl.GL_TEXTURE_2D, 0, gl.GL_RGBA, 1, 1, 0, gl.GL_RGBA, gl.GL_UNSIGNED_BYTE, pixel)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, gl.GL_NEAREST)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_NEAREST)
gl.glBindTexture(gl.GL_TEXTURE_2D, 0)
cls._fallback_texture = tex
return cls._fallback_texture
def __init__(self, title="Newton", screen_width=1920, screen_height=1080, vsync=True, headless=None, device=None):
self.draw_sky = True
self.draw_fps = True
self.draw_shadows = True
self.draw_wireframe = False
self.wireframe_line_width = 1.5 # pixels
self.background_color = (68.0 / 255.0, 161.0 / 255.0, 255.0 / 255.0)
self.sky_upper = self.background_color
self.sky_lower = (40.0 / 255.0, 44.0 / 255.0, 55.0 / 255.0)
# Lighting settings
self._shadow_radius = 3.0
self._diffuse_scale = 1.0
self._specular_scale = 1.0
self.spotlight_enabled = True
self._shadow_extents = 10.0
self._exposure = 1.6
# Hemispherical ambient light colors, interpolated by dot(N, up).
# Decoupled from the sky background so the visible sky can be a
# saturated blue while the ambient fill stays neutral — a stand-in
# for a proper irradiance map that we don't precompute yet.
self.ambient_sky = (0.8, 0.8, 0.85)
self.ambient_ground = (0.3, 0.3, 0.35)
# On Wayland, PyOpenGL defaults to EGL which cannot see the GLX context
# that pyglet creates via XWayland. Force GLX so both libraries agree.
# Must be set before PyOpenGL is first imported (platform is selected
# once at import time).
if "PYOPENGL_PLATFORM" not in os.environ:
# WAYLAND_DISPLAY is the primary indicator; XDG_SESSION_TYPE is
# checked as a fallback for sessions where the socket is not yet set.
is_wayland = bool(os.environ.get("WAYLAND_DISPLAY")) or os.environ.get("XDG_SESSION_TYPE") == "wayland"
if is_wayland:
os.environ["PYOPENGL_PLATFORM"] = "glx"
try:
import pyglet
# disable error checking for performance
pyglet.options["debug_gl"] = False
# try imports
from pyglet.graphics.shader import Shader, ShaderProgram # noqa: F401
from pyglet.math import Vec3 as PyVec3 # noqa: F401
RendererGL.initialize_gl()
gl = RendererGL.gl
except ImportError as e:
raise Exception("OpenGLRenderer requires pyglet (version >= 2.0) to be installed.") from e
self._title = title
try:
# try to enable MSAA
config = pyglet.gl.Config(sample_buffers=1, samples=8, double_buffer=True)
self.window = pyglet.window.Window(
width=screen_width,
height=screen_height,
caption=title,
resizable=True,
vsync=vsync,
visible=not headless,
config=config,
)
gl.glEnable(gl.GL_MULTISAMPLE)
# remember sample count for later (e.g., resolving FBO)
self.msaa_samples = 4
except pyglet.window.NoSuchConfigException:
print("Warning: Could not get MSAA config, falling back to non-AA.")
self.window = pyglet.window.Window(
width=screen_width,
height=screen_height,
caption=title,
resizable=True,
vsync=vsync,
visible=not headless,
)
self.msaa_samples = 0
self._set_icon()
# Pyglet on Windows 8+ (where _always_dwm=True) disables the GL
# swap interval to avoid double-syncing with DWM, but then also
# skips calling DwmFlush() in flip() due to a condition bug.
# We call DwmFlush() ourselves in present() to work around this.
self._dwm_flush = None
if sys.platform == "win32" and getattr(self.window, "_always_dwm", False):
try:
self._dwm_flush = ctypes.windll.dwmapi.DwmFlush
except (AttributeError, OSError):
pass
if headless is None:
self.headless = pyglet.options.get("headless", False)
else:
self.headless = headless
self.app = pyglet.app
# making window current opengl rendering context
self._make_current()
self._screen_width, self._screen_height = self.window.get_framebuffer_size()
self._camera_speed = 0.04
self._last_x, self._last_y = self._screen_width // 2, self._screen_height // 2
self._key_callbacks = []
self._key_release_callbacks = []
self._env_texture = None
self._env_intensity = 1.0
self._env_path = None
self._env_texture_obj = None
default_env = os.path.join(os.path.dirname(__file__), "newton_envmap.jpg")
if os.path.exists(default_env):
self._env_path = default_env
self._mouse_drag_callbacks = []
self._mouse_press_callbacks = []
self._mouse_release_callbacks = []
self._mouse_motion_callbacks = []
self._mouse_scroll_callbacks = []
self._resize_callbacks = []
# Initialize device and shape lookup
self._device = device if device is not None else wp.get_device()
self._shape_lookup = {}
self._shadow_fbo = None
self._shadow_texture = None
self._shadow_shader = None
self._shadow_width = 4096
self._shadow_height = 4096
self._frame_texture = None
self._frame_depth_texture = None
self._frame_fbo = None
self._frame_pbo = None
self._sun_direction = None # set on first render based on camera up_axis
self._light_color = (1.0, 1.0, 1.0)
check_gl_error()
if not headless:
# set up our own event handling so we can synchronously render frames
# by calling update() in a loop
from pyglet.window import Window
Window._enable_event_queue = False
self.window.dispatch_pending_events()
platform_event_loop = self.app.platform_event_loop
platform_event_loop.start()
# start event loop
# self.app.event_loop.dispatch_event("on_enter")
# create frame buffer for rendering to a texture
self._setup_shadow_buffer()
self._setup_frame_buffer()
self._setup_sky_mesh()
self._setup_frame_mesh()
self._shadow_shader = ShadowShader(gl)
self._shape_shader = ShaderShape(gl)
self._frame_shader = FrameShader(gl)
self._sky_shader = ShaderSky(gl)
self._line_shader = ShaderLine(gl)
self._wireframe_shader = ShaderLineWireframe(gl)
if not headless:
self._setup_window_callbacks()
@property
def shadow_radius(self) -> float:
return self._shadow_radius
@shadow_radius.setter
def shadow_radius(self, value: float):
self._shadow_radius = max(float(value), 0.0)
@property
def diffuse_scale(self) -> float:
return self._diffuse_scale
@diffuse_scale.setter
def diffuse_scale(self, value: float):
self._diffuse_scale = max(float(value), 0.0)
@property
def specular_scale(self) -> float:
return self._specular_scale
@specular_scale.setter
def specular_scale(self, value: float):
self._specular_scale = max(float(value), 0.0)
@property
def shadow_extents(self) -> float:
return self._shadow_extents
@shadow_extents.setter
def shadow_extents(self, value: float):
self._shadow_extents = max(float(value), 1e-4)
@property
def exposure(self) -> float:
return self._exposure
@exposure.setter
def exposure(self, value: float):
self._exposure = max(float(value), 0.0)
def update(self):
self._make_current()
if not self.headless:
import pyglet
pyglet.clock.tick()
self.app.platform_event_loop.step(0.001) # 1ms app polling latency
try:
self.window.dispatch_events()
except (ctypes.ArgumentError, TypeError):
# Handle known issue with pyglet xlib backend on some Linux configurations
# where window handle can have wrong type in XCheckWindowEvent
# This is a non-fatal error that can be safely ignored
pass
def render(self, camera, objects, lines=None, wireframe_shapes=None):
gl = RendererGL.gl
self._make_current()
gl.glClearColor(*self.sky_upper, 1)
gl.glEnable(gl.GL_DEPTH_TEST)
gl.glDepthMask(True)
gl.glDepthRange(0.0, 1.0)
self.camera = camera
# Lazy-init sun direction based on camera up axis
if self._sun_direction is None:
_sun_dirs = {
0: np.array((0.8, 0.2, -0.3)), # X-up
1: np.array((0.2, 0.8, -0.3)), # Y-up
2: np.array((0.2, -0.3, 0.8)), # Z-up
}
d = _sun_dirs.get(camera.up_axis, _sun_dirs[2])
self._sun_direction = d / np.linalg.norm(d)
# Store matrices for other methods
self._view_matrix = self.camera.get_view_matrix()
self._projection_matrix = self.camera.get_projection_matrix()
# Lazy-load environment map after a valid GL context is active
if self._env_path is not None and self._env_texture is None:
try:
self.set_environment_map(self._env_path)
except Exception:
pass
self._env_path = None
# 1. render depth of scene to texture (from light's perspective)
gl.glViewport(0, 0, self._shadow_width, self._shadow_height)
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, self._shadow_fbo)
gl.glClear(gl.GL_DEPTH_BUFFER_BIT)
if self.draw_shadows:
# Note: lines are skipped during shadow pass since they don't cast shadows
self._render_shadow_map(objects)
# reset viewport
gl.glViewport(0, 0, self._screen_width, self._screen_height)
# select target framebuffer (MSAA or regular) for scene rendering
target_fbo = self._frame_msaa_fbo if getattr(self, "msaa_samples", 0) > 0 else self._frame_fbo
# ---------------------------------------
# Set texture as render target for MSAA resolve
gl.glBindFramebuffer(gl.GL_DRAW_FRAMEBUFFER, target_fbo)
gl.glDrawBuffer(gl.GL_COLOR_ATTACHMENT0)
gl.glClearColor(*self.sky_upper, 1)
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glBindVertexArray(0)
self._render_scene(objects)
# Render lines after main scene but before MSAA resolve
if lines:
self._render_lines(lines)
if wireframe_shapes:
self._render_wireframe_shapes(wireframe_shapes)
# ------------------------------------------------------------------
# If MSAA is enabled, resolve the multi-sample buffer into texture FBO
# ------------------------------------------------------------------
if getattr(self, "msaa_samples", 0) > 0 and self._frame_msaa_fbo is not None:
gl.glBindFramebuffer(gl.GL_READ_FRAMEBUFFER, self._frame_msaa_fbo)
gl.glReadBuffer(gl.GL_COLOR_ATTACHMENT0)
gl.glBindFramebuffer(gl.GL_DRAW_FRAMEBUFFER, self._frame_fbo)
gl.glDrawBuffer(gl.GL_COLOR_ATTACHMENT0)
gl.glBlitFramebuffer(
0,
0,
self._screen_width,
self._screen_height,
0,
0,
self._screen_width,
self._screen_height,
gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT,
gl.GL_NEAREST,
)
# ------------------------------------------------------------------
# Draw resolved texture to the screen
# ------------------------------------------------------------------
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, 0)
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glViewport(0, 0, self._screen_width, self._screen_height)
# render frame buffer texture to screen
if self._frame_fbo is not None:
with self._frame_shader:
gl.glActiveTexture(gl.GL_TEXTURE0)
gl.glBindTexture(gl.GL_TEXTURE_2D, self._frame_texture)
self._frame_shader.update(0)
gl.glBindVertexArray(self._frame_vao)
gl.glDrawElements(gl.GL_TRIANGLES, len(self._frame_indices), gl.GL_UNSIGNED_INT, None)
gl.glBindVertexArray(0)
gl.glBindTexture(gl.GL_TEXTURE_2D, 0)
if self.draw_fps:
gl.glClear(gl.GL_DEPTH_BUFFER_BIT)
gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA)
gl.glEnable(gl.GL_BLEND)
err = gl.glGetError()
assert err == gl.GL_NO_ERROR, hex(err)
def present(self):
if not self.headless:
if self._dwm_flush is not None and self.window._interval:
self._dwm_flush()
self.window.flip()
def resize(self, width, height):
self._screen_width, self._screen_height = self.window.get_framebuffer_size()
self._setup_frame_buffer()
def set_title(self, title):
self.window.set_caption(title)
def set_vsync(self, enabled: bool):
"""Enable or disable vertical synchronization (vsync).
Args:
enabled: If True, enable vsync; if False, disable vsync.
"""
self.window.set_vsync(enabled)
def get_vsync(self) -> bool:
"""Get the current vsync state.
Returns:
True if vsync is enabled, False otherwise.
"""
return self.window.vsync
def has_exit(self):
return self.app.event_loop.has_exit
def close(self):
self._make_current()
if not self.headless:
self.app.event_loop.dispatch_event("on_exit")
self.app.platform_event_loop.stop()
RendererGL._fallback_texture = None
self.window.close()
def _setup_window_callbacks(self):
"""Set up the basic window event handlers."""
import pyglet
self.window.push_handlers(on_draw=self._on_draw)
self.window.push_handlers(on_resize=self._on_window_resize)
self.window.push_handlers(on_key_press=self._on_key_press)
self.window.push_handlers(on_key_release=self._on_key_release)
self.window.push_handlers(on_close=self._on_close)
self._key_handler = pyglet.window.key.KeyStateHandler()
self.window.push_handlers(self._key_handler)
self.window.push_handlers(on_mouse_press=self._on_mouse_press)
self.window.push_handlers(on_mouse_release=self._on_mouse_release)
self.window.on_mouse_scroll = self._on_scroll
self.window.on_mouse_drag = self._on_mouse_drag
self.window.on_mouse_motion = self._on_mouse_motion
def register_key_press(self, callback):
"""Register a callback for key press events.
Args:
callback: Function that takes (symbol, modifiers) parameters
"""
self._key_callbacks.append(callback)
def register_key_release(self, callback):
"""Register a callback for key release events.
Args:
callback: Function that takes (symbol, modifiers) parameters
"""
self._key_release_callbacks.append(callback)
def register_mouse_press(self, callback):
"""Register a callback for mouse press events.
Args:
callback: Function that takes (x, y, button, modifiers) parameters
"""
self._mouse_press_callbacks.append(callback)
def register_mouse_release(self, callback):
"""Register a callback for mouse release events.
Args:
callback: Function that takes (x, y, button, modifiers) parameters
"""
self._mouse_release_callbacks.append(callback)
def register_mouse_drag(self, callback):
"""Register a callback for mouse drag events.
Args:
callback: Function that takes (x, y, dx, dy, buttons, modifiers) parameters
"""
self._mouse_drag_callbacks.append(callback)
def register_mouse_motion(self, callback):
"""Register a callback for mouse motion events.
Args:
callback: Function that takes (x, y, dx, dy) parameters
"""
self._mouse_motion_callbacks.append(callback)
def register_mouse_scroll(self, callback):
"""Register a callback for mouse scroll events.
Args:
callback: Function that takes (x, y, scroll_x, scroll_y) parameters
"""
self._mouse_scroll_callbacks.append(callback)
def register_resize(self, callback):
"""Register a callback for window resize events.
Args:
callback: Function that takes (width, height) parameters
"""
self._resize_callbacks.append(callback)
def register_update(self, callback):
"""Register a per-frame update callback receiving dt (seconds)."""
self._update_callbacks.append(callback)
def _on_key_press(self, symbol, modifiers):
# update key state
for callback in self._key_callbacks:
callback(symbol, modifiers)
def _on_key_release(self, symbol, modifiers):
# update key state
for callback in self._key_release_callbacks:
callback(symbol, modifiers)
def _on_mouse_press(self, x, y, button, modifiers):
"""Handle mouse button press events."""
for callback in self._mouse_press_callbacks:
callback(x, y, button, modifiers)
def _on_mouse_release(self, x, y, button, modifiers):
"""Handle mouse button release events."""
for callback in self._mouse_release_callbacks:
callback(x, y, button, modifiers)
def _on_mouse_drag(self, x, y, dx, dy, buttons, modifiers):
# Then call registered callbacks
for callback in self._mouse_drag_callbacks:
callback(x, y, dx, dy, buttons, modifiers)
def _on_mouse_motion(self, x, y, dx, dy):
"""Handle mouse motion events."""
for callback in self._mouse_motion_callbacks:
callback(x, y, dx, dy)
def _on_scroll(self, x, y, scroll_x, scroll_y):
for callback in self._mouse_scroll_callbacks:
callback(x, y, scroll_x, scroll_y)
def _on_window_resize(self, width, height):
self.resize(width, height)
for callback in self._resize_callbacks:
callback(width, height)
def _on_close(self):
self.close()
def _on_draw(self):
pass
# public query for key state
def is_key_down(self, symbol: int) -> bool:
if self.headless:
return False
return bool(self._key_handler[symbol])
def _setup_sky_mesh(self):
gl = RendererGL.gl
# create VAO, VBO, and EBO
self._sky_vao = gl.GLuint()
gl.glGenVertexArrays(1, self._sky_vao)
gl.glBindVertexArray(self._sky_vao)
sky_mesh = Mesh.create_sphere(
1.0,
num_latitudes=32,
num_longitudes=32,
reverse_winding=True,
compute_inertia=False,
)
vertices = np.hstack([sky_mesh.vertices, sky_mesh.normals, sky_mesh.uvs]).astype(np.float32, copy=False)
indices = sky_mesh.indices.astype(np.uint32, copy=False)
self._sky_tri_count = len(indices)
self._sky_vbo = gl.GLuint()
gl.glGenBuffers(1, self._sky_vbo)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self._sky_vbo)
gl.glBufferData(gl.GL_ARRAY_BUFFER, vertices.nbytes, vertices.ctypes.data, gl.GL_STATIC_DRAW)
self._sky_ebo = gl.GLuint()
gl.glGenBuffers(1, self._sky_ebo)
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, self._sky_ebo)
gl.glBufferData(gl.GL_ELEMENT_ARRAY_BUFFER, indices.nbytes, indices.ctypes.data, gl.GL_STATIC_DRAW)
# set up vertex attributes
vertex_stride = vertices.shape[1] * vertices.itemsize
# positions
gl.glVertexAttribPointer(0, 3, gl.GL_FLOAT, gl.GL_FALSE, vertex_stride, ctypes.c_void_p(0))
gl.glEnableVertexAttribArray(0)
# normals
gl.glVertexAttribPointer(1, 3, gl.GL_FLOAT, gl.GL_FALSE, vertex_stride, ctypes.c_void_p(3 * vertices.itemsize))
gl.glEnableVertexAttribArray(1)
# uv coordinates
gl.glVertexAttribPointer(2, 2, gl.GL_FLOAT, gl.GL_FALSE, vertex_stride, ctypes.c_void_p(6 * vertices.itemsize))
gl.glEnableVertexAttribArray(2)
gl.glBindVertexArray(0)
# unbind the VBO and VAO
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, 0)
gl.glBindVertexArray(0)
check_gl_error()
def _setup_frame_buffer(self):
gl = RendererGL.gl
# Ensure MSAA member variables exist even on first call
if not hasattr(self, "_frame_msaa_color_rb"):
self._frame_msaa_color_rb = None
if not hasattr(self, "_frame_msaa_depth_rb"):
self._frame_msaa_depth_rb = None
if not hasattr(self, "_frame_msaa_fbo"):
self._frame_msaa_fbo = None
self._make_current()
if self._frame_texture is None:
self._frame_texture = gl.GLuint()
gl.glGenTextures(1, self._frame_texture)
if self._frame_depth_texture is None:
self._frame_depth_texture = gl.GLuint()
gl.glGenTextures(1, self._frame_depth_texture)
# set up RGB texture
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, 0)
gl.glBindBuffer(gl.GL_PIXEL_UNPACK_BUFFER, 0)
gl.glBindTexture(gl.GL_TEXTURE_2D, self._frame_texture)
gl.glTexImage2D(
gl.GL_TEXTURE_2D,
0,
gl.GL_RGB,
self._screen_width,
self._screen_height,
0,
gl.GL_RGB,
gl.GL_UNSIGNED_BYTE,
None,
)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, gl.GL_LINEAR)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_LINEAR)
# set up depth texture
gl.glBindTexture(gl.GL_TEXTURE_2D, self._frame_depth_texture)
gl.glTexImage2D(
gl.GL_TEXTURE_2D,
0,
gl.GL_DEPTH_COMPONENT32,
self._screen_width,
self._screen_height,
0,
gl.GL_DEPTH_COMPONENT,
gl.GL_FLOAT,
None,
)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, gl.GL_LINEAR)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_LINEAR)
gl.glBindTexture(gl.GL_TEXTURE_2D, 0)
# create a framebuffer object (FBO)
if self._frame_fbo is None:
self._frame_fbo = gl.GLuint()
gl.glGenFramebuffers(1, self._frame_fbo)
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, self._frame_fbo)
# attach the texture to the FBO as its color attachment
gl.glFramebufferTexture2D(
gl.GL_FRAMEBUFFER, gl.GL_COLOR_ATTACHMENT0, gl.GL_TEXTURE_2D, self._frame_texture, 0
)
# attach the depth texture to the FBO as its depth attachment
gl.glFramebufferTexture2D(
gl.GL_FRAMEBUFFER, gl.GL_DEPTH_ATTACHMENT, gl.GL_TEXTURE_2D, self._frame_depth_texture, 0
)
if gl.glCheckFramebufferStatus(gl.GL_FRAMEBUFFER) != gl.GL_FRAMEBUFFER_COMPLETE:
print("Framebuffer is not complete!", flush=True)
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, 0)
sys.exit(1)
# unbind the FBO (switch back to the default framebuffer)
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, 0)
if self._frame_pbo is None:
self._frame_pbo = gl.GLuint()
gl.glGenBuffers(1, self._frame_pbo) # generate 1 buffer reference
# binding to this buffer
gl.glBindBuffer(gl.GL_PIXEL_PACK_BUFFER, self._frame_pbo)
# allocate memory for PBO
rgb_bytes_per_pixel = 3
depth_bytes_per_pixel = 4
pixels = np.zeros(
(self._screen_height, self._screen_width, rgb_bytes_per_pixel + depth_bytes_per_pixel), dtype=np.uint8
)
gl.glBufferData(gl.GL_PIXEL_PACK_BUFFER, pixels.nbytes, pixels.ctypes.data, gl.GL_DYNAMIC_DRAW)
gl.glBindBuffer(gl.GL_PIXEL_PACK_BUFFER, 0)
# ---------------------------------------------------------------------
# Additional: create MSAA framebuffer if multi-sampling is enabled
# ---------------------------------------------------------------------
if getattr(self, "msaa_samples", 0) > 0:
# color renderbuffer
if self._frame_msaa_color_rb is None:
self._frame_msaa_color_rb = gl.GLuint()
gl.glGenRenderbuffers(1, self._frame_msaa_color_rb)
gl.glBindRenderbuffer(gl.GL_RENDERBUFFER, self._frame_msaa_color_rb)
gl.glRenderbufferStorageMultisample(
gl.GL_RENDERBUFFER, self.msaa_samples, gl.GL_RGB8, self._screen_width, self._screen_height
)
# depth renderbuffer
if self._frame_msaa_depth_rb is None:
self._frame_msaa_depth_rb = gl.GLuint()
gl.glGenRenderbuffers(1, self._frame_msaa_depth_rb)
gl.glBindRenderbuffer(gl.GL_RENDERBUFFER, self._frame_msaa_depth_rb)
gl.glRenderbufferStorageMultisample(
gl.GL_RENDERBUFFER, self.msaa_samples, gl.GL_DEPTH_COMPONENT32, self._screen_width, self._screen_height
)
# FBO
if self._frame_msaa_fbo is None:
self._frame_msaa_fbo = gl.GLuint()
gl.glGenFramebuffers(1, self._frame_msaa_fbo)
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, self._frame_msaa_fbo)
gl.glFramebufferRenderbuffer(
gl.GL_FRAMEBUFFER, gl.GL_COLOR_ATTACHMENT0, gl.GL_RENDERBUFFER, self._frame_msaa_color_rb
)
gl.glFramebufferRenderbuffer(
gl.GL_FRAMEBUFFER, gl.GL_DEPTH_ATTACHMENT, gl.GL_RENDERBUFFER, self._frame_msaa_depth_rb
)
if gl.glCheckFramebufferStatus(gl.GL_FRAMEBUFFER) != gl.GL_FRAMEBUFFER_COMPLETE:
print("Warning: MSAA framebuffer incomplete, disabling MSAA.")
self.msaa_samples = 0
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, 0)
check_gl_error()
def _setup_frame_mesh(self):
gl = RendererGL.gl
# fmt: off
# set up VBO for the quad that is rendered to the user window with the texture
self._frame_vertices = np.array([
# Positions TexCoords
-1.0, -1.0, 0.0, 0.0,
1.0, -1.0, 1.0, 0.0,
1.0, 1.0, 1.0, 1.0,
-1.0, 1.0, 0.0, 1.0
], dtype=np.float32)
# fmt: on
self._frame_indices = np.array([0, 1, 2, 2, 3, 0], dtype=np.uint32)
self._frame_vao = gl.GLuint()
gl.glGenVertexArrays(1, self._frame_vao)
gl.glBindVertexArray(self._frame_vao)
self._frame_vbo = gl.GLuint()
gl.glGenBuffers(1, self._frame_vbo)
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, self._frame_vbo)
gl.glBufferData(
gl.GL_ARRAY_BUFFER, self._frame_vertices.nbytes, self._frame_vertices.ctypes.data, gl.GL_STATIC_DRAW
)
self._frame_ebo = gl.GLuint()
gl.glGenBuffers(1, self._frame_ebo)
gl.glBindBuffer(gl.GL_ELEMENT_ARRAY_BUFFER, self._frame_ebo)
gl.glBufferData(
gl.GL_ELEMENT_ARRAY_BUFFER, self._frame_indices.nbytes, self._frame_indices.ctypes.data, gl.GL_STATIC_DRAW
)
gl.glVertexAttribPointer(0, 2, gl.GL_FLOAT, gl.GL_FALSE, 4 * self._frame_vertices.itemsize, ctypes.c_void_p(0))
gl.glEnableVertexAttribArray(0)
gl.glVertexAttribPointer(
1,
2,
gl.GL_FLOAT,
gl.GL_FALSE,
4 * self._frame_vertices.itemsize,
ctypes.c_void_p(2 * self._frame_vertices.itemsize),
)
gl.glEnableVertexAttribArray(1)
check_gl_error()
def _setup_shadow_buffer(self):
gl = RendererGL.gl
self._make_current()
# create depth texture FBO
self._shadow_fbo = gl.GLuint()
gl.glGenFramebuffers(1, self._shadow_fbo)
self._shadow_texture = gl.GLuint()
gl.glGenTextures(1, self._shadow_texture)
gl.glBindTexture(gl.GL_TEXTURE_2D, self._shadow_texture)
gl.glTexImage2D(
gl.GL_TEXTURE_2D,
0,
gl.GL_DEPTH_COMPONENT,
self._shadow_width,
self._shadow_height,
0,
gl.GL_DEPTH_COMPONENT,
gl.GL_FLOAT,
None,
)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, gl.GL_NEAREST)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_NEAREST)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_WRAP_S, gl.GL_CLAMP_TO_BORDER)
gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_WRAP_T, gl.GL_CLAMP_TO_BORDER)
border_color = [1.0, 1.0, 1.0, 1.0]
gl.glTexParameterfv(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_BORDER_COLOR, (gl.GLfloat * 4)(*border_color))
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, self._shadow_fbo)
gl.glFramebufferTexture2D(gl.GL_FRAMEBUFFER, gl.GL_DEPTH_ATTACHMENT, gl.GL_TEXTURE_2D, self._shadow_texture, 0)
gl.glDrawBuffer(gl.GL_NONE)
gl.glReadBuffer(gl.GL_NONE)
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, 0)
check_gl_error()
def _render_shadow_map(self, objects):
gl = RendererGL.gl
from pyglet.math import Mat4, Vec3
self._make_current()
extents = self.shadow_extents
light_near = 1.0
light_far = 1000.0
camera_pos = np.array(self.camera.pos, dtype=np.float32)
light_pos = camera_pos + self._sun_direction * extents
light_proj = Mat4.orthogonal_projection(-extents, extents, -extents, extents, light_near, light_far)
light_view = Mat4.look_at(Vec3(*light_pos), Vec3(*camera_pos), Vec3(*self.camera.get_up()))
self._light_space_matrix = np.array(light_proj @ light_view, dtype=np.float32)
self._shadow_shader.update(self._light_space_matrix)
# render from light's point of view (skip objects that don't cast shadows)
shadow_objects = {k: v for k, v in objects.items() if getattr(v, "cast_shadow", True)}
with self._shadow_shader:
self._draw_objects(shadow_objects)
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, 0)
check_gl_error()
def _render_scene(self, objects):
gl = RendererGL.gl
if self.draw_sky:
self._draw_sky()
if self.draw_wireframe:
gl.glPolygonMode(gl.GL_FRONT_AND_BACK, gl.GL_LINE)
self._shape_shader.update(
view_matrix=self._view_matrix,
projection_matrix=self._projection_matrix,
view_pos=self.camera.pos,
fog_color=self.sky_lower,
up_axis=self.camera.up_axis,
sun_direction=self._sun_direction,
enable_shadows=self.draw_shadows,
shadow_texture=self._shadow_texture,
light_space_matrix=self._light_space_matrix,
light_color=self._light_color,
sky_color=self.ambient_sky,
ground_color=self.ambient_ground,
env_texture=self._env_texture,
env_intensity=self._env_intensity,
shadow_radius=self.shadow_radius,
diffuse_scale=self.diffuse_scale,
specular_scale=self.specular_scale,
spotlight_enabled=self.spotlight_enabled,
shadow_extents=self.shadow_extents,
exposure=self.exposure,
)
with self._shape_shader:
self._draw_objects(objects)
gl.glPolygonMode(gl.GL_FRONT_AND_BACK, gl.GL_FILL)
check_gl_error()
def _render_lines(self, lines):
"""Render all line objects using the line shader."""
# Set up line shader once for all line objects
self._line_shader.update(self._view_matrix, self._projection_matrix)
with self._line_shader:
for line_obj in lines.values():
if hasattr(line_obj, "render"):
line_obj.render()
check_gl_error()
def _render_wireframe_shapes(self, wireframe_shapes):
"""Render wireframe shapes using the geometry-shader line expansion."""
gl = RendererGL.gl
inv_asp = float(self._screen_height) / float(max(self._screen_width, 1))
clip_width = self.wireframe_line_width * 2.0 / max(self._screen_height, 1)
gl.glEnable(gl.GL_DEPTH_TEST)
gl.glDisable(gl.GL_CULL_FACE)
gl.glEnable(gl.GL_BLEND)
gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA)
with self._wireframe_shader:
self._wireframe_shader.update_frame(
self._view_matrix, self._projection_matrix, inv_asp, line_width=clip_width
)
for shape in wireframe_shapes.values():
if not shape.hidden and shape.num_vertices > 0:
self._wireframe_shader.set_world(shape.world_matrix)
shape.render()
gl.glDisable(gl.GL_BLEND)
check_gl_error()
def _draw_objects(self, objects):
for o in objects.values():
if hasattr(o, "render"):
o.render()
check_gl_error()
def _draw_sky(self):
gl = RendererGL.gl
self._make_current()
self._sky_shader.update(
view_matrix=self._view_matrix,
projection_matrix=self._projection_matrix,
camera_pos=self.camera.pos,
camera_far=self.camera.far,
sky_upper=self.sky_upper,
sky_lower=self.sky_lower,
sun_direction=self._sun_direction,
up_axis=self.camera.up_axis,
)
gl.glBindVertexArray(self._sky_vao)
gl.glDrawElements(gl.GL_TRIANGLES, self._sky_tri_count, gl.GL_UNSIGNED_INT, None)
gl.glBindVertexArray(0)
check_gl_error()
def set_environment_map(self, path: str, intensity: float = 1.0) -> None:
gl = RendererGL.gl
from ...utils.texture import load_texture_from_file # noqa: PLC0415
image = load_texture_from_file(path)
if image is None:
return
if self._env_texture is not None:
try:
gl.glDeleteTextures(1, self._env_texture)
except Exception:
pass
self._env_texture = None
self._env_texture = _upload_texture_from_file(gl, image)
self._env_texture_obj = None
self._env_intensity = float(intensity)
def _make_current(self):
try:
self.window.switch_to()
except AttributeError:
# The window could be in the process of being closed, in which case
# its corresponding context might have been destroyed and set to `None`.
pass
def _set_icon(self):
import pyglet
def load_icon(filename):
filename = os.path.join(os.path.dirname(__file__), filename)
if not os.path.exists(filename):
raise FileNotFoundError(
f"Error: Icon file '{filename}' not found. Please run the 'generate_icons.py' script first."
)
with open(filename, "rb") as f:
icon_bytes = f.read()
icon_stream = io.BytesIO(icon_bytes)
icon = pyglet.image.load(filename=filename, file=icon_stream)
return icon
icons = [load_icon("icon_16.png"), load_icon("icon_32.png"), load_icon("icon_64.png")]
# 5. Create the window and set the icon
self.window.set_icon(*icons)
================================================
FILE: newton/_src/viewer/gl/shaders.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import ctypes
import numpy as np
shadow_vertex_shader = """
#version 330 core
layout (location = 0) in vec3 aPos;
// column vectors of the instance transform matrix
layout (location = 3) in vec4 aInstanceTransform0;
layout (location = 4) in vec4 aInstanceTransform1;
layout (location = 5) in vec4 aInstanceTransform2;
layout (location = 6) in vec4 aInstanceTransform3;
uniform mat4 light_space_matrix;
void main()
{
mat4 transform = mat4(aInstanceTransform0, aInstanceTransform1, aInstanceTransform2, aInstanceTransform3);
gl_Position = light_space_matrix * transform * vec4(aPos, 1.0);
}
"""
shadow_fragment_shader = """
#version 330 core
void main() { }
"""
line_vertex_shader = """
#version 330 core
layout (location = 0) in vec3 aPos;
layout (location = 1) in vec3 aColor;
uniform mat4 view;
uniform mat4 projection;
out vec3 vertexColor;
void main()
{
vertexColor = aColor;
gl_Position = projection * view * vec4(aPos, 1.0);
}
"""
line_fragment_shader = """
#version 330 core
in vec3 vertexColor;
out vec4 FragColor;
void main()
{
FragColor = vec4(vertexColor, 1.0);
}
"""
shape_vertex_shader = """
#version 330 core
layout (location = 0) in vec3 aPos;
layout (location = 1) in vec3 aNormal;
layout (location = 2) in vec2 aTexCoord;
// column vectors of the instance transform matrix
layout (location = 3) in vec4 aInstanceTransform0;
layout (location = 4) in vec4 aInstanceTransform1;
layout (location = 5) in vec4 aInstanceTransform2;
layout (location = 6) in vec4 aInstanceTransform3;
// colors to use for the checker_enable pattern
layout (location = 7) in vec3 aObjectColor;
// material properties
layout (location = 8) in vec4 aMaterial;
uniform mat4 view;
uniform mat4 projection;
uniform mat4 light_space_matrix;
out vec3 Normal;
out vec3 FragPos;
out vec3 LocalPos;
out vec2 TexCoord;
out vec3 ObjectColor;
out vec4 FragPosLightSpace;
out vec4 Material;
void main()
{
mat4 transform = mat4(aInstanceTransform0, aInstanceTransform1, aInstanceTransform2, aInstanceTransform3);
vec4 worldPos = transform * vec4(aPos, 1.0);
gl_Position = projection * view * worldPos;
FragPos = vec3(worldPos);
LocalPos = aPos;
mat3 rotation = mat3(transform);
Normal = mat3(transpose(inverse(rotation))) * aNormal;
TexCoord = aTexCoord;
ObjectColor = aObjectColor;
FragPosLightSpace = light_space_matrix * worldPos;
Material = aMaterial;
}
"""
shape_fragment_shader = """
#version 330 core
out vec4 FragColor;
in vec3 Normal;
in vec3 FragPos;
in vec3 LocalPos;
in vec2 TexCoord;
in vec3 ObjectColor; // used as albedo
in vec4 FragPosLightSpace;
in vec4 Material;
uniform vec3 view_pos;
uniform vec3 light_color;
uniform vec3 sky_color;
uniform vec3 ground_color;
uniform vec3 sun_direction;
uniform sampler2D shadow_map;
uniform sampler2D env_map;
uniform float env_intensity;
uniform sampler2D albedo_map;
uniform vec3 fogColor;
uniform int up_axis;
uniform mat4 light_space_matrix;
uniform float shadow_radius;
uniform float diffuse_scale;
uniform float specular_scale;
uniform bool spotlight_enabled;
uniform float shadow_extents;
uniform float exposure;
const float PI = 3.14159265359;
float rand(vec2 co){
return fract(sin(dot(co.xy ,vec2(12.9898,78.233))) * 43758.5453);
}
// Analytic filtering helpers for smooth checker_enable pattern
float filterwidth(vec2 v)
{
vec2 fw = max(abs(dFdx(v)), abs(dFdy(v)));
return max(fw.x, fw.y);
}
vec2 bump(vec2 x)
{
return (floor(x / 2.0) + 2.0 * max(x / 2.0 - floor(x / 2.0) - 0.5, 0.0));
}
float checker(vec2 uv)
{
float width = filterwidth(uv);
vec2 p0 = uv - 0.5 * width;
vec2 p1 = uv + 0.5 * width;
vec2 i = (bump(p1) - bump(p0)) / width;
return i.x * i.y + (1.0 - i.x) * (1.0 - i.y);
}
vec2 poissonDisk[16] = vec2[](
vec2( -0.94201624, -0.39906216 ),
vec2( 0.94558609, -0.76890725 ),
vec2( -0.094184101, -0.92938870 ),
vec2( 0.34495938, 0.29387760 ),
vec2( -0.91588581, 0.45771432 ),
vec2( -0.81544232, -0.87912464 ),
vec2( -0.38277543, 0.27676845 ),
vec2( 0.97484398, 0.75648379 ),
vec2( 0.44323325, -0.97511554 ),
vec2( 0.53742981, -0.47373420 ),
vec2( -0.26496911, -0.41893023 ),
vec2( 0.79197514, 0.19090188 ),
vec2( -0.24188840, 0.99706507 ),
vec2( -0.81409955, 0.91437590 ),
vec2( 0.19984126, 0.78641367 ),
vec2( 0.14383161, -0.14100790 )
);
float ShadowCalculation()
{
vec3 normal = normalize(Normal);
if (!gl_FrontFacing)
normal = -normal;
vec3 lightDir = normalize(sun_direction);
// bias in normal dir - adjust for backfacing triangles
float worldTexel = (shadow_extents * 2.0) / float(4096); // world extent / shadow map resolution
float normalBias = 2.0 * worldTexel; // tune ~1-3
// For backfacing triangles, we might need different bias handling
vec4 light_space_pos;
light_space_pos = light_space_matrix * vec4(FragPos + normal * normalBias, 1.0);
vec3 projCoords = light_space_pos.xyz/light_space_pos.w;
// map to [0,1]
projCoords = projCoords * 0.5 + 0.5;
if (projCoords.z > 1.0)
return 0.0;
float frag_depth = projCoords.z;
// Fade shadow to zero near edges of the shadow map to avoid hard rectangle
float fade = 1.0;
float margin = 0.15;
fade *= smoothstep(0.0, margin, projCoords.x);
fade *= smoothstep(0.0, margin, 1.0 - projCoords.x);
fade *= smoothstep(0.0, margin, projCoords.y);
fade *= smoothstep(0.0, margin, 1.0 - projCoords.y);
// Slope-scaled depth bias: more bias when surface is nearly parallel to light
// (where self-shadowing from float precision is worst), minimal when facing light.
float NdotL_bias = max(dot(normal, lightDir), 0.0);
float depthBias = mix(0.0003, 0.00002, NdotL_bias);
float biased_depth = frag_depth - depthBias;
float shadow = 0.0;
float radius = shadow_radius;
vec2 texelSize = 1.0 / textureSize(shadow_map, 0);
float angle = rand(gl_FragCoord.xy) * 2.0 * PI;
float s = sin(angle);
float c = cos(angle);
mat2 rotationMatrix = mat2(c, -s, s, c);
for(int i = 0; i < 16; i++)
{
vec2 offset = rotationMatrix * poissonDisk[i];
float pcf_depth = texture(shadow_map, projCoords.xy + offset * radius * texelSize).r;
if(pcf_depth < biased_depth)
shadow += 1.0;
}
shadow /= 16.0;
return shadow * fade;
}
float SpotlightAttenuation()
{
if (!spotlight_enabled)
return 1.0;
// Calculate spotlight position as 20 units from the camera in sun direction
vec3 spotlight_pos = view_pos + sun_direction * 20.0;
// Vector from fragment to spotlight
vec3 fragToLight = normalize(spotlight_pos - FragPos);
// Angle between spotlight direction (towards origin) and vector from light to fragment
float cosAngle = dot(normalize(sun_direction), fragToLight);
// Fixed cone angles (inner: 30 degrees, outer: 45 degrees)
float cosInnerAngle = cos(radians(30.0));
float cosOuterAngle = cos(radians(45.0));
// Smooth falloff between inner and outer cone
float intensity = smoothstep(cosOuterAngle, cosInnerAngle, cosAngle);
return intensity;
}
vec3 sample_env_map(vec3 dir, float lod)
{
// dir assumed normalized
// Convert to a Y-up reference frame before equirect sampling.
vec3 dir_up = dir;
if (up_axis == 0) {
dir_up = vec3(-dir.y, dir.x, dir.z); // X-up -> Y-up
} else if (up_axis == 2) {
dir_up = vec3(dir.x, dir.z, -dir.y); // Z-up -> Y-up
}
float u = atan(dir_up.z, dir_up.x) / (2.0 * PI) + 0.5;
float v = asin(clamp(dir_up.y, -1.0, 1.0)) / PI + 0.5;
return textureLod(env_map, vec2(u, v), lod).rgb;
}
void main()
{
// material properties from vertex shader
float roughness = clamp(Material.x, 0.0, 1.0);
float metallic = clamp(Material.y, 0.0, 1.0);
float checker_enable = Material.z;
float texture_enable = Material.w;
float checker_scale = 1.0;
// convert to linear space
vec3 albedo = pow(ObjectColor, vec3(2.2));
if (texture_enable > 0.5)
{
vec3 tex_color = texture(albedo_map, TexCoord).rgb;
albedo *= pow(tex_color, vec3(2.2));
}
// Optional checker pattern in object-space so it follows instance transforms
if (checker_enable > 0.0)
{
vec2 uv = LocalPos.xy * checker_scale;
float cb = checker(uv);
vec3 albedo2 = albedo*0.7;
// pick between the two colors
albedo = mix(albedo, albedo2, cb);
}
// Specular color: dielectrics ~0.04, metals use albedo.
// Computed before desaturation so F0 reflects true material reflectance.
vec3 F0 = mix(vec3(0.04), albedo, metallic);
// Metals appear paler/desaturated because their look is dominated by
// bright specular reflections. Without full IBL we approximate this by
// lifting the albedo toward a brighter, less saturated version.
float luma = dot(albedo, vec3(0.2126, 0.7152, 0.0722));
albedo = mix(albedo, vec3(luma * 1.4), metallic * 0.45);
// surface vectors
vec3 N = normalize(Normal);
vec3 V = normalize(view_pos - FragPos);
// Flip normal for backfacing triangles
if (!gl_FrontFacing) N = -N;
vec3 L = normalize(sun_direction);
vec3 H = normalize(V + L);
// Cook-Torrance PBR
float NdotL = max(dot(N, L), 0.0);
float NdotH = max(dot(N, H), 0.0);
float NdotV = max(dot(N, V), 0.001);
float HdotV = max(dot(H, V), 0.0);
// GGX/Trowbridge-Reitz normal distribution
float a = roughness * roughness;
float a2 = a * a;
float denom = NdotH * NdotH * (a2 - 1.0) + 1.0;
float D = a2 / (PI * denom * denom);
// Schlick-GGX geometry function (Smith method for both view and light)
float k = (roughness + 1.0) * (roughness + 1.0) / 8.0;
float G1_V = NdotV / (NdotV * (1.0 - k) + k);
float G1_L = NdotL / (NdotL * (1.0 - k) + k);
float G = G1_V * G1_L;
// Schlick Fresnel, dampened by roughness to reduce edge aliasing
vec3 F_max = mix(F0, vec3(1.0), 1.0 - roughness);
vec3 F = F0 + (F_max - F0) * pow(1.0 - HdotV, 5.0);
// Cook-Torrance specular BRDF
vec3 spec = (D * G * F) / (4.0 * NdotV * NdotL + 0.0001);
// Diffuse uses remaining energy not reflected
vec3 kD = (1.0 - F) * (1.0 - metallic);
vec3 diffuse = kD * albedo / PI;
// Direct lighting
vec3 Lo = (diffuse * diffuse_scale + spec * specular_scale) * light_color * NdotL * 3.0;
// Hemispherical ambient (kept subtle for depth)
vec3 up = vec3(0.0, 1.0, 0.0);
if (up_axis == 0) up = vec3(1.0, 0.0, 0.0);
if (up_axis == 2) up = vec3(0.0, 0.0, 1.0);
float sky_fac = dot(N, up) * 0.5 + 0.5;
vec3 ambient = mix(ground_color, sky_color, sky_fac) * albedo * 0.7;
// Fresnel-weighted ambient specular for metallics
vec3 F_ambient = F0 + (F_max - F0) * pow(1.0 - NdotV, 5.0);
vec3 kD_ambient = (1.0 - F_ambient) * (1.0 - metallic);
ambient = kD_ambient * ambient + F_ambient * mix(ground_color, sky_color, sky_fac) * 0.35;
// shadows
float shadow = ShadowCalculation();
float spotAttenuation = SpotlightAttenuation();
vec3 color = ambient + (1.0 - shadow) * spotAttenuation * Lo;
// Environment / image-based lighting for metals
vec3 R = reflect(-V, N);
float env_lod = roughness * 8.0;
vec3 env_color = pow(sample_env_map(R, env_lod), vec3(2.2));
vec3 env_F = F0 + (F_max - F0) * pow(1.0 - NdotV, 5.0);
vec3 env_spec = env_color * env_F * env_intensity;
color += env_spec * metallic;
// fog
float dist = length(FragPos - view_pos);
float fog_start = 20.0;
float fog_end = 200.0;
float fog_factor = clamp((dist - fog_start) / (fog_end - fog_start), 0.0, 1.0);
color = mix(color, pow(fogColor, vec3(2.2)), fog_factor);
// ACES filmic tone mapping
color = color * exposure;
vec3 x = color;
color = (x * (2.51 * x + 0.03)) / (x * (2.43 * x + 0.59) + 0.14);
color = clamp(color, 0.0, 1.0);
// gamma correction (sRGB)
color = pow(color, vec3(1.0 / 2.2));
FragColor = vec4(color, 1.0);
}
"""
sky_vertex_shader = """
#version 330 core
layout (location = 0) in vec3 aPos;
layout (location = 1) in vec3 aNormal;
layout (location = 2) in vec2 aTexCoord;
uniform mat4 view;
uniform mat4 projection;
uniform vec3 view_pos;
uniform float far_plane;
out vec3 FragPos;
out vec2 TexCoord;
void main()
{
vec4 worldPos = vec4(aPos * far_plane + view_pos, 1.0);
gl_Position = projection * view * worldPos;
FragPos = vec3(worldPos);
TexCoord = aTexCoord;
}
"""
sky_fragment_shader = """
#version 330 core
out vec4 FragColor;
in vec3 FragPos;
in vec2 TexCoord;
uniform vec3 sky_upper;
uniform vec3 sky_lower;
uniform float far_plane;
uniform vec3 sun_direction;
uniform int up_axis;
void main()
{
float h = up_axis == 0 ? FragPos.x : (up_axis == 1 ? FragPos.y : FragPos.z);
float height = max(0.0, h / far_plane);
vec3 sky = mix(sky_lower, sky_upper, height);
float diff = max(dot(sun_direction, normalize(FragPos)), 0.0);
vec3 sun = pow(diff, 32) * vec3(1.0, 0.8, 0.6) * 0.5;
FragColor = vec4(sky + sun, 1.0);
}
"""
frame_vertex_shader = """
#version 330 core
layout (location = 0) in vec3 aPos;
layout (location = 1) in vec2 aTexCoord;
out vec2 TexCoord;
void main() {
gl_Position = vec4(aPos, 1.0);
TexCoord = aTexCoord;
}
"""
frame_fragment_shader = """
#version 330 core
in vec2 TexCoord;
out vec4 FragColor;
uniform sampler2D texture_sampler;
void main() {
FragColor = texture(texture_sampler, TexCoord);
}
"""
def str_buffer(string: str):
"""Convert string to C-style char pointer for OpenGL."""
return ctypes.c_char_p(string.encode("utf-8"))
def arr_pointer(arr: np.ndarray):
"""Convert numpy array to C-style float pointer for OpenGL."""
return arr.astype(np.float32).ctypes.data_as(ctypes.POINTER(ctypes.c_float))
class ShaderGL:
"""Base class for OpenGL shader wrappers."""
def __init__(self):
self.shader_program = None
self._gl = None
def _get_uniform_location(self, name: str):
"""Get uniform location for given name."""
if self.shader_program is None:
raise RuntimeError("Shader not initialized")
return self._gl.glGetUniformLocation(self.shader_program.id, str_buffer(name))
def use(self):
"""Bind this shader for use."""
if self.shader_program is None:
raise RuntimeError("Shader not initialized")
self._gl.glUseProgram(self.shader_program.id)
def __enter__(self):
"""Context manager entry - bind shader."""
self.use()
return self
def __exit__(self, exc_type, exc_val, exc_tb):
"""Context manager exit."""
pass # OpenGL doesn't need explicit unbinding
class ShaderShape(ShaderGL):
"""Shader for rendering 3D shapes with lighting and shadows."""
def __init__(self, gl):
super().__init__()
from pyglet.graphics.shader import Shader, ShaderProgram
self._gl = gl
self.shader_program = ShaderProgram(
Shader(shape_vertex_shader, "vertex"), Shader(shape_fragment_shader, "fragment")
)
# Get all uniform locations
with self:
self.loc_view = self._get_uniform_location("view")
self.loc_projection = self._get_uniform_location("projection")
self.loc_view_pos = self._get_uniform_location("view_pos")
self.loc_light_space_matrix = self._get_uniform_location("light_space_matrix")
self.loc_shadow_map = self._get_uniform_location("shadow_map")
self.loc_albedo_map = self._get_uniform_location("albedo_map")
self.loc_env_map = self._get_uniform_location("env_map")
self.loc_env_intensity = self._get_uniform_location("env_intensity")
self.loc_fog_color = self._get_uniform_location("fogColor")
self.loc_up_axis = self._get_uniform_location("up_axis")
self.loc_sun_direction = self._get_uniform_location("sun_direction")
self.loc_light_color = self._get_uniform_location("light_color")
self.loc_ground_color = self._get_uniform_location("ground_color")
self.loc_sky_color = self._get_uniform_location("sky_color")
self.loc_shadow_radius = self._get_uniform_location("shadow_radius")
self.loc_diffuse_scale = self._get_uniform_location("diffuse_scale")
self.loc_specular_scale = self._get_uniform_location("specular_scale")
self.loc_spotlight_enabled = self._get_uniform_location("spotlight_enabled")
self.loc_shadow_extents = self._get_uniform_location("shadow_extents")
self.loc_exposure = self._get_uniform_location("exposure")
def update(
self,
view_matrix: np.ndarray,
projection_matrix: np.ndarray,
view_pos: tuple[float, float, float],
fog_color: tuple[float, float, float],
up_axis: int,
sun_direction: tuple[float, float, float],
light_color: tuple[float, float, float] = (2.0, 2.0, 2.0),
ground_color: tuple[float, float, float] = (0.3, 0.3, 0.35),
sky_color: tuple[float, float, float] = (0.8, 0.8, 0.85),
enable_shadows: bool = False,
shadow_texture: int | None = None,
light_space_matrix: np.ndarray | None = None,
env_texture: int | None = None,
env_intensity: float = 1.0,
shadow_radius: float = 3.0,
diffuse_scale: float = 1.0,
specular_scale: float = 1.0,
spotlight_enabled: bool = True,
shadow_extents: float = 10.0,
exposure: float = 1.6,
):
"""Update all shader uniforms."""
with self:
# Basic matrices
self._gl.glUniformMatrix4fv(self.loc_view, 1, self._gl.GL_FALSE, arr_pointer(view_matrix))
self._gl.glUniformMatrix4fv(self.loc_projection, 1, self._gl.GL_FALSE, arr_pointer(projection_matrix))
self._gl.glUniform3f(self.loc_view_pos, *view_pos)
# Lighting
self._gl.glUniform3f(self.loc_sun_direction, *sun_direction)
self._gl.glUniform3f(self.loc_light_color, *light_color)
self._gl.glUniform3f(self.loc_ground_color, *ground_color)
self._gl.glUniform3f(self.loc_sky_color, *sky_color)
self._gl.glUniform1f(self.loc_shadow_radius, shadow_radius)
self._gl.glUniform1f(self.loc_diffuse_scale, diffuse_scale)
self._gl.glUniform1f(self.loc_specular_scale, specular_scale)
self._gl.glUniform1i(self.loc_spotlight_enabled, int(spotlight_enabled))
self._gl.glUniform1f(self.loc_shadow_extents, shadow_extents)
self._gl.glUniform1f(self.loc_exposure, exposure)
# Fog and rendering options
self._gl.glUniform3f(self.loc_fog_color, *fog_color)
self._gl.glUniform1i(self.loc_up_axis, up_axis)
# Shadows
# if enable_shadows and shadow_texture is not None and light_space_matrix is not None:
self._gl.glActiveTexture(self._gl.GL_TEXTURE0)
self._gl.glBindTexture(self._gl.GL_TEXTURE_2D, shadow_texture)
self._gl.glUniform1i(self.loc_shadow_map, 0)
self._gl.glUniformMatrix4fv(
self.loc_light_space_matrix, 1, self._gl.GL_FALSE, arr_pointer(light_space_matrix)
)
self._gl.glUniform1i(self.loc_albedo_map, 1)
self._gl.glActiveTexture(self._gl.GL_TEXTURE2)
if env_texture is not None:
self._gl.glBindTexture(self._gl.GL_TEXTURE_2D, env_texture)
else:
from .opengl import RendererGL # noqa: PLC0415
self._gl.glBindTexture(self._gl.GL_TEXTURE_2D, RendererGL.get_fallback_texture())
self._gl.glUniform1i(self.loc_env_map, 2)
self._gl.glUniform1f(self.loc_env_intensity, float(env_intensity))
class ShaderSky(ShaderGL):
"""Shader for rendering sky background."""
def __init__(self, gl):
super().__init__()
from pyglet.graphics.shader import Shader, ShaderProgram
self._gl = gl
self.shader_program = ShaderProgram(
Shader(sky_vertex_shader, "vertex"), Shader(sky_fragment_shader, "fragment")
)
# Get all uniform locations
with self:
self.loc_view = self._get_uniform_location("view")
self.loc_projection = self._get_uniform_location("projection")
self.loc_sky_upper = self._get_uniform_location("sky_upper")
self.loc_sky_lower = self._get_uniform_location("sky_lower")
self.loc_far_plane = self._get_uniform_location("far_plane")
self.loc_view_pos = self._get_uniform_location("view_pos")
self.loc_sun_direction = self._get_uniform_location("sun_direction")
self.loc_up_axis = self._get_uniform_location("up_axis")
def update(
self,
view_matrix: np.ndarray,
projection_matrix: np.ndarray,
camera_pos: tuple[float, float, float],
camera_far: float,
sky_upper: tuple[float, float, float],
sky_lower: tuple[float, float, float],
sun_direction: tuple[float, float, float],
up_axis: int = 2,
):
"""Update all shader uniforms."""
with self:
# Matrices and view position
self._gl.glUniformMatrix4fv(self.loc_view, 1, self._gl.GL_FALSE, arr_pointer(view_matrix))
self._gl.glUniformMatrix4fv(self.loc_projection, 1, self._gl.GL_FALSE, arr_pointer(projection_matrix))
self._gl.glUniform3f(self.loc_view_pos, *camera_pos)
self._gl.glUniform1f(self.loc_far_plane, camera_far * 0.9) # moves sphere slightly inside far clip plane
# Sky colors and settings
self._gl.glUniform3f(self.loc_sky_upper, *sky_upper)
self._gl.glUniform3f(self.loc_sky_lower, *sky_lower)
self._gl.glUniform3f(self.loc_sun_direction, *sun_direction)
self._gl.glUniform1i(self.loc_up_axis, up_axis)
class ShadowShader(ShaderGL):
"""Shader for rendering shadow maps."""
def __init__(self, gl):
super().__init__()
from pyglet.graphics.shader import Shader, ShaderProgram
self._gl = gl
self.shader_program = ShaderProgram(
Shader(shadow_vertex_shader, "vertex"), Shader(shadow_fragment_shader, "fragment")
)
# Get uniform locations
with self:
self.loc_light_space_matrix = self._get_uniform_location("light_space_matrix")
def update(self, light_space_matrix: np.ndarray):
"""Update light space matrix for shadow rendering."""
with self:
self._gl.glUniformMatrix4fv(
self.loc_light_space_matrix, 1, self._gl.GL_FALSE, arr_pointer(light_space_matrix)
)
class FrameShader(ShaderGL):
"""Shader for rendering the final frame buffer to screen."""
def __init__(self, gl):
super().__init__()
from pyglet.graphics.shader import Shader, ShaderProgram
self._gl = gl
self.shader_program = ShaderProgram(
Shader(frame_vertex_shader, "vertex"), Shader(frame_fragment_shader, "fragment")
)
# Get uniform locations
with self:
self.loc_texture = self._get_uniform_location("texture_sampler")
def update(self, texture_unit: int = 0):
"""Update texture uniform."""
with self:
self._gl.glUniform1i(self.loc_texture, texture_unit)
class ShaderLine(ShaderGL):
"""Simple shader for rendering lines with per-vertex colors."""
def __init__(self, gl):
super().__init__()
from pyglet.graphics.shader import Shader, ShaderProgram
self._gl = gl
self.shader_program = ShaderProgram(
Shader(line_vertex_shader, "vertex"), Shader(line_fragment_shader, "fragment")
)
# Get uniform locations
with self:
self.loc_view = self._get_uniform_location("view")
self.loc_projection = self._get_uniform_location("projection")
def update(self, view_matrix: np.ndarray, projection_matrix: np.ndarray):
"""Update view and projection matrices for line rendering."""
with self:
self._gl.glUniformMatrix4fv(self.loc_view, 1, self._gl.GL_FALSE, arr_pointer(view_matrix))
self._gl.glUniformMatrix4fv(self.loc_projection, 1, self._gl.GL_FALSE, arr_pointer(projection_matrix))
wireframe_vertex_shader = """
#version 330 core
layout (location = 0) in vec3 aPos;
layout (location = 1) in vec3 aColor;
uniform mat4 view;
uniform mat4 projection;
uniform mat4 world;
out vec3 vertexColor;
void main()
{
vec4 worldPos = world * vec4(aPos, 1.0);
vertexColor = aColor;
gl_Position = projection * view * worldPos;
}
"""
wireframe_geometry_shader = """
#version 330 core
layout (lines) in;
layout (triangle_strip, max_vertices = 6) out;
in vec3 vertexColor[2];
out vec3 lineColor;
uniform float inv_asp_ratio;
uniform float line_width;
void main()
{
vec4 s = gl_in[0].gl_Position;
vec4 e = gl_in[1].gl_Position;
if (s.w <= 0.0 || e.w <= 0.0) return;
vec2 s_ndc = s.xy / s.w;
vec2 e_ndc = e.xy / e.w;
float s_depth = s.z / s.w;
float e_depth = e.z / e.w;
vec2 dir = e_ndc - s_ndc;
vec2 right = normalize(vec2(dir.y, -dir.x));
right.x = right.x * inv_asp_ratio;
vec3 color = 0.5 * (vertexColor[0] + vertexColor[1]);
vec2 xy = 0.5 * line_width * right;
gl_Position = vec4(s_ndc - xy, s_depth, 1); lineColor = color;
EmitVertex();
gl_Position = vec4(e_ndc + xy, e_depth, 1); lineColor = color;
EmitVertex();
gl_Position = vec4(s_ndc + xy, s_depth, 1); lineColor = color;
EmitVertex();
EndPrimitive();
gl_Position = vec4(s_ndc - xy, s_depth, 1); lineColor = color;
EmitVertex();
gl_Position = vec4(e_ndc - xy, e_depth, 1); lineColor = color;
EmitVertex();
gl_Position = vec4(e_ndc + xy, e_depth, 1); lineColor = color;
EmitVertex();
EndPrimitive();
}
"""
wireframe_fragment_shader = """
#version 330 core
in vec3 lineColor;
out vec4 FragColor;
uniform float alpha;
void main()
{
FragColor = vec4(lineColor, alpha);
}
"""
class ShaderLineWireframe(ShaderGL):
"""Geometry-shader-based line renderer that expands GL_LINES into screen-space quads."""
def __init__(self, gl):
super().__init__()
from pyglet.graphics.shader import Shader, ShaderProgram
self._gl = gl
self.shader_program = ShaderProgram(
Shader(wireframe_vertex_shader, "vertex"),
Shader(wireframe_geometry_shader, "geometry"),
Shader(wireframe_fragment_shader, "fragment"),
)
with self:
self.loc_view = self._get_uniform_location("view")
self.loc_projection = self._get_uniform_location("projection")
self.loc_world = self._get_uniform_location("world")
self.loc_inv_asp_ratio = self._get_uniform_location("inv_asp_ratio")
self.loc_line_width = self._get_uniform_location("line_width")
self.loc_alpha = self._get_uniform_location("alpha")
def update_frame(
self,
view_matrix: np.ndarray,
projection_matrix: np.ndarray,
inv_asp_ratio: float,
line_width: float = 0.003,
alpha: float = 0.7,
):
"""Set per-frame uniforms (call once before rendering all wireframe shapes)."""
self._gl.glUniformMatrix4fv(self.loc_view, 1, self._gl.GL_FALSE, arr_pointer(view_matrix))
self._gl.glUniformMatrix4fv(self.loc_projection, 1, self._gl.GL_FALSE, arr_pointer(projection_matrix))
self._gl.glUniform1f(self.loc_inv_asp_ratio, float(inv_asp_ratio))
self._gl.glUniform1f(self.loc_line_width, float(line_width))
self._gl.glUniform1f(self.loc_alpha, float(alpha))
def set_world(self, world: np.ndarray):
"""Set the per-shape world matrix uniform."""
self._gl.glUniformMatrix4fv(self.loc_world, 1, self._gl.GL_FALSE, arr_pointer(world))
================================================
FILE: newton/_src/viewer/kernels.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
"""
Warp kernels for simplified Newton viewers.
These kernels handle mesh operations and transformations.
"""
from typing import Any
import warp as wp
import newton
from newton._src.math import orthonormal_basis
@wp.struct
class PickingState:
picked_point_local: wp.vec3
picked_point_world: wp.vec3
picking_target_world: wp.vec3
pick_stiffness: float
pick_damping: float
@wp.kernel
def compute_pick_state_kernel(
body_q: wp.array[wp.transform],
body_flags: wp.array[int],
body_index: int,
hit_point_world: wp.vec3,
# output
pick_body: wp.array[int],
pick_state: wp.array[PickingState],
):
"""
Initialize the pick state when a body is first picked.
"""
if body_index < 0:
return
if body_flags[body_index] & newton.BodyFlags.KINEMATIC:
pick_body[0] = -1
return
# store body index
pick_body[0] = body_index
# Get body transform
X_wb = body_q[body_index]
X_bw = wp.transform_inverse(X_wb)
# Compute local space attachment point from the hit point
pick_pos_local = wp.transform_point(X_bw, hit_point_world)
pick_state[0].picked_point_local = pick_pos_local
# store target world (current attachment point position)
pick_state[0].picking_target_world = hit_point_world
# store current world space picked point on geometry (for visualization)
pick_state[0].picked_point_world = hit_point_world
@wp.kernel
def apply_picking_force_kernel(
body_q: wp.array[wp.transform],
body_qd: wp.array[wp.spatial_vector],
body_f: wp.array[wp.spatial_vector],
pick_body_arr: wp.array[int],
pick_state: wp.array[PickingState],
body_flags: wp.array[int],
body_com: wp.array[wp.vec3],
body_mass: wp.array[float],
):
pick_body = pick_body_arr[0]
if pick_body < 0:
return
if body_flags[pick_body] & newton.BodyFlags.KINEMATIC:
return
pick_pos_local = pick_state[0].picked_point_local
pick_target_world = pick_state[0].picking_target_world
# world space attachment point
X_wb = body_q[pick_body]
pick_pos_world = wp.transform_point(X_wb, pick_pos_local)
# update current world space picked point on geometry (for visualization)
pick_state[0].picked_point_world = pick_pos_world
# Linear velocity at COM
vel_com = wp.spatial_top(body_qd[pick_body])
# Angular velocity
angular_vel = wp.spatial_bottom(body_qd[pick_body])
# Offset from COM to pick point (in world space)
offset = pick_pos_world - wp.transform_point(X_wb, body_com[pick_body])
# Velocity at the picked point
vel_at_offset = vel_com + wp.cross(angular_vel, offset)
# Adjust force to mass for more adaptive manipulation of picked bodies.
force_multiplier = 10.0 + body_mass[pick_body]
# Compute the force to apply
force_at_offset = force_multiplier * (
pick_state[0].pick_stiffness * (pick_target_world - pick_pos_world)
- (pick_state[0].pick_damping * vel_at_offset)
)
# Compute the resulting torque given the offset from COM to the picked point.
torque_at_offset = wp.cross(offset, force_at_offset)
wp.atomic_add(body_f, pick_body, wp.spatial_vector(force_at_offset, torque_at_offset))
@wp.kernel
def update_pick_target_kernel(
p: wp.vec3,
d: wp.vec3,
world_offset: wp.vec3,
# read-write
pick_state: wp.array[PickingState],
):
# get original mouse cursor target (in physics space)
original_target = pick_state[0].picking_target_world
# Add world offset to convert to offset space for distance calculation
original_target_offset = original_target + world_offset
# compute distance from ray origin to original target (to maintain depth)
dist = wp.length(original_target_offset - p)
# Project new mouse cursor target at the same depth (in offset space)
new_mouse_target_offset = p + d * dist
# Convert back to physics space by subtracting world offset
new_mouse_target = new_mouse_target_offset - world_offset
# Update the original mouse cursor target (no smoothing here)
pick_state[0].picking_target_world = new_mouse_target
@wp.kernel
def update_shape_xforms(
shape_xforms: wp.array[wp.transform],
shape_parents: wp.array[int],
body_q: wp.array[wp.transform],
shape_worlds: wp.array[int],
world_offsets: wp.array[wp.vec3],
world_xforms: wp.array[wp.transform],
):
tid = wp.tid()
shape_xform = shape_xforms[tid]
shape_parent = shape_parents[tid]
if shape_parent >= 0:
world_xform = wp.transform_multiply(body_q[shape_parent], shape_xform)
else:
world_xform = shape_xform
if world_offsets:
shape_world = shape_worlds[tid]
if shape_world >= 0 and shape_world < world_offsets.shape[0]:
offset = world_offsets[shape_world]
world_xform = wp.transform(world_xform.p + offset, world_xform.q)
world_xforms[tid] = world_xform
@wp.kernel
def repack_shape_colors(
shape_colors: wp.array[wp.vec3],
slot_to_shape: wp.array[wp.int32],
packed_shape_colors: wp.array[wp.vec3],
):
"""Repack model-order shape colors into viewer batch order."""
tid = wp.tid()
packed_shape_colors[tid] = shape_colors[slot_to_shape[tid]]
@wp.kernel
def estimate_world_extents(
shape_transform: wp.array[wp.transform],
shape_body: wp.array[int],
shape_collision_radius: wp.array[float],
shape_world: wp.array[int],
body_q: wp.array[wp.transform],
world_count: int,
# outputs (world_count x 3 arrays for min/max xyz per world)
world_bounds_min: wp.array2d[float],
world_bounds_max: wp.array2d[float],
):
tid = wp.tid()
# Get shape's world assignment
world_idx = shape_world[tid]
# Skip global shapes (world -1) or invalid world indices
if world_idx < 0 or world_idx >= world_count:
return
# Get collision radius and skip shapes with unreasonably large radii
radius = shape_collision_radius[tid]
if radius > 1.0e5: # Skip outliers like infinite planes
return
# Get shape's world position
shape_xform = shape_transform[tid]
shape_parent = shape_body[tid]
# Compute world transform
if shape_parent >= 0:
# Shape attached to body: world_xform = body_xform * shape_xform
body_xform = body_q[shape_parent]
world_xform = wp.transform_multiply(body_xform, shape_xform)
else:
# Static shape: already in world space
world_xform = shape_xform
# Get position and radius
pos = wp.transform_get_translation(world_xform)
radius = shape_collision_radius[tid]
# Update bounds for this world using atomic operations
min_pos = pos - wp.vec3(radius, radius, radius)
max_pos = pos + wp.vec3(radius, radius, radius)
# Atomic min for each component
wp.atomic_min(world_bounds_min, world_idx, 0, min_pos[0])
wp.atomic_min(world_bounds_min, world_idx, 1, min_pos[1])
wp.atomic_min(world_bounds_min, world_idx, 2, min_pos[2])
# Atomic max for each component
wp.atomic_max(world_bounds_max, world_idx, 0, max_pos[0])
wp.atomic_max(world_bounds_max, world_idx, 1, max_pos[1])
wp.atomic_max(world_bounds_max, world_idx, 2, max_pos[2])
@wp.kernel
def compute_contact_lines(
body_q: wp.array[wp.transform],
shape_body: wp.array[int],
shape_world: wp.array[int],
world_offsets: wp.array[wp.vec3],
contact_count: wp.array[int],
contact_shape0: wp.array[int],
contact_shape1: wp.array[int],
contact_point0: wp.array[wp.vec3],
contact_offset0: wp.array[wp.vec3],
contact_normal: wp.array[wp.vec3],
line_scale: float,
# outputs
line_start: wp.array[wp.vec3],
line_end: wp.array[wp.vec3],
):
"""Create line segments along contact normals for visualization."""
tid = wp.tid()
count = contact_count[0]
if tid >= count:
line_start[tid] = wp.vec3(wp.nan, wp.nan, wp.nan)
line_end[tid] = wp.vec3(wp.nan, wp.nan, wp.nan)
return
shape_a = contact_shape0[tid]
shape_b = contact_shape1[tid]
if shape_a == shape_b:
line_start[tid] = wp.vec3(wp.nan, wp.nan, wp.nan)
line_end[tid] = wp.vec3(wp.nan, wp.nan, wp.nan)
return
# Get world transforms for both shapes
body_a = shape_body[shape_a]
X_wb_a = wp.transform_identity()
if body_a >= 0:
X_wb_a = body_q[body_a]
# Compute world space contact positions
world_pos0 = wp.transform_point(X_wb_a, contact_point0[tid] + contact_offset0[tid])
# Anchor the debug normal at shape 0's contact point.
contact_center = world_pos0
# Apply world offset
world_a, world_b = shape_world[shape_a], shape_world[shape_b]
if world_a >= 0 or world_b >= 0:
contact_center += world_offsets[world_a if world_a >= 0 else world_b]
# Create line along normal direction
# Normal points from shape0 to shape1, draw from center in normal direction
normal = contact_normal[tid]
line_vector = normal * line_scale
line_start[tid] = contact_center
line_end[tid] = contact_center + line_vector
@wp.kernel
def compute_joint_basis_lines(
joint_type: wp.array[int],
joint_parent: wp.array[int],
joint_child: wp.array[int],
joint_transform: wp.array[wp.transform],
body_q: wp.array[wp.transform],
body_world: wp.array[int],
world_offsets: wp.array[wp.vec3],
shape_collision_radius: wp.array[float],
shape_body: wp.array[int],
line_scale: float,
# outputs - unified buffers for all joint lines
line_starts: wp.array[wp.vec3],
line_ends: wp.array[wp.vec3],
line_colors: wp.array[wp.vec3],
):
"""Create line segments for joint basis vectors for visualization.
Each joint produces 3 lines (x, y, z axes).
Thread ID maps to line index: joint_id * 3 + axis_id
"""
tid = wp.tid()
# Determine which joint and which axis this thread handles
joint_id = tid // 3
axis_id = tid % 3
# Check if this is a supported joint type
if joint_id >= len(joint_type):
line_starts[tid] = wp.vec3(wp.nan, wp.nan, wp.nan)
line_ends[tid] = wp.vec3(wp.nan, wp.nan, wp.nan)
line_colors[tid] = wp.vec3(0.0, 0.0, 0.0)
return
joint_t = joint_type[joint_id]
if (
joint_t != int(newton.JointType.PRISMATIC)
and joint_t != int(newton.JointType.REVOLUTE)
and joint_t != int(newton.JointType.D6)
and joint_t != int(newton.JointType.CABLE)
and joint_t != int(newton.JointType.BALL)
):
# Set NaN for unsupported joints to hide them
line_starts[tid] = wp.vec3(wp.nan, wp.nan, wp.nan)
line_ends[tid] = wp.vec3(wp.nan, wp.nan, wp.nan)
line_colors[tid] = wp.vec3(0.0, 0.0, 0.0)
return
# Get joint transform
joint_tf = joint_transform[joint_id]
joint_pos = wp.transform_get_translation(joint_tf)
joint_rot = wp.transform_get_rotation(joint_tf)
# Get parent body transform
parent_body = joint_parent[joint_id]
if parent_body >= 0:
parent_tf = body_q[parent_body]
# Transform joint to world space
world_pos = wp.transform_point(parent_tf, joint_pos)
world_rot = wp.mul(wp.transform_get_rotation(parent_tf), joint_rot)
# Apply world offset
parent_body_world = body_world[parent_body]
if world_offsets and parent_body_world >= 0:
world_pos += world_offsets[parent_body_world]
else:
world_pos = joint_pos
world_rot = joint_rot
# Determine scale based on child body shapes
scale_factor = line_scale
# Create the appropriate basis vector based on axis_id
if axis_id == 0: # X-axis (red)
axis_vec = wp.quat_rotate(world_rot, wp.vec3(1.0, 0.0, 0.0))
color = wp.vec3(1.0, 0.0, 0.0)
elif axis_id == 1: # Y-axis (green)
axis_vec = wp.quat_rotate(world_rot, wp.vec3(0.0, 1.0, 0.0))
color = wp.vec3(0.0, 1.0, 0.0)
else: # Z-axis (blue)
axis_vec = wp.quat_rotate(world_rot, wp.vec3(0.0, 0.0, 1.0))
color = wp.vec3(0.0, 0.0, 1.0)
# Set line endpoints
line_starts[tid] = world_pos
line_ends[tid] = world_pos + axis_vec * scale_factor
line_colors[tid] = color
@wp.kernel
def compute_com_positions(
body_q: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
body_world: wp.array[int],
world_offsets: wp.array[wp.vec3],
com_positions: wp.array[wp.vec3],
):
tid = wp.tid()
body_tf = body_q[tid]
world_com = wp.transform_point(body_tf, body_com[tid])
world_idx = body_world[tid]
if world_offsets and world_idx >= 0 and world_idx < world_offsets.shape[0]:
world_com = world_com + world_offsets[world_idx]
com_positions[tid] = world_com
@wp.kernel
def compute_inertia_box_lines(
body_q: wp.array[wp.transform],
body_com: wp.array[wp.vec3],
body_inertia: wp.array[wp.mat33],
body_inv_mass: wp.array[float],
body_world: wp.array[int],
world_offsets: wp.array[wp.vec3],
max_worlds: int,
color: wp.vec3,
# outputs: 12 lines per body
line_starts: wp.array[wp.vec3],
line_ends: wp.array[wp.vec3],
line_colors: wp.array[wp.vec3],
):
"""Compute wireframe edges for inertia boxes. 12 edges per body."""
tid = wp.tid()
body_id = tid // 12
edge_id = tid % 12
nan_line = wp.vec3(wp.nan, wp.nan, wp.nan)
zero_color = wp.vec3(0.0, 0.0, 0.0)
# Skip bodies from worlds beyond max_worlds limit
world_idx = body_world[body_id]
if max_worlds >= 0 and world_idx >= 0 and world_idx >= max_worlds:
line_starts[tid] = nan_line
line_ends[tid] = nan_line
line_colors[tid] = zero_color
return
inv_m = body_inv_mass[body_id]
if inv_m == 0.0:
line_starts[tid] = nan_line
line_ends[tid] = nan_line
line_colors[tid] = zero_color
return
# Compute principal inertia axes and extents
rot, principal_inertia = wp.eig3(body_inertia[body_id])
# Skip eigenvector rotation for near-isotropic inertia (e.g., cubes, spheres).
# When eigenvalues are nearly equal, eig3 returns arbitrary eigenvectors
# causing the wireframe box to appear randomly rotated.
max_eig = wp.max(principal_inertia)
min_eig = wp.min(principal_inertia)
if min_eig > 0.0 and max_eig < 1.01 * min_eig: # within 1% -> isotropic
rot = wp.identity(3, float)
elif min_eig > 0.0:
# Stabilize for axisymmetric inertia (2 of 3 eigenvalues nearly equal, e.g. cylinders).
# The two degenerate eigenvectors are arbitrary; rebuild a deterministic frame
# from the unique eigenvector.
d01 = wp.abs(principal_inertia[0] - principal_inertia[1])
d02 = wp.abs(principal_inertia[0] - principal_inertia[2])
d12 = wp.abs(principal_inertia[1] - principal_inertia[2])
min_diff = wp.min(d01, wp.min(d02, d12))
if min_diff < 0.01 * max_eig: # within 1% -> axisymmetric
# Identify unique eigenvector (column not in degenerate pair)
if d12 <= d01 and d12 <= d02: # e1 approx eq e2, unique = col 0
u = wp.vec3(rot[0, 0], rot[1, 0], rot[2, 0])
elif d02 <= d01: # e0 approx eq e2, unique = col 1
u = wp.vec3(rot[0, 1], rot[1, 1], rot[2, 1])
else: # e0 approx eq e1, unique = col 2
u = wp.vec3(rot[0, 2], rot[1, 2], rot[2, 2])
u = wp.normalize(u)
# Deterministic orthonormal basis from unique axis
v1, v2 = orthonormal_basis(u)
# Assign columns as cyclic permutation of (u, v1, v2) to keep det=+1
c0 = v1
c1 = v2
c2 = u
if d12 <= d01 and d12 <= d02: # unique col 0
c0 = u
c1 = v1
c2 = v2
elif d02 <= d01: # unique col 1
c0 = v2
c1 = u
c2 = v1
# mat33(*v) unpacks vectors as rows; transpose to place them as columns
rot = wp.transpose(wp.mat33(*c0, *c1, *c2))
box_inertia = principal_inertia * inv_m * (12.0 / 8.0)
sx = wp.sqrt(wp.abs(box_inertia[2] + box_inertia[1] - box_inertia[0]))
sy = wp.sqrt(wp.abs(box_inertia[0] + box_inertia[2] - box_inertia[1]))
sz = wp.sqrt(wp.abs(box_inertia[1] + box_inertia[0] - box_inertia[2]))
# Box edges: pairs of corner indices
# Corners: 0=(-,-,-) 1=(+,-,-) 2=(+,+,-) 3=(-,+,-)
# 4=(-,-,+) 5=(+,-,+) 6=(+,+,+) 7=(-,+,+)
# Bottom face edges (0-3), top face edges (4-7), vertical edges (8-11)
c0x = float(0.0)
c0y = float(0.0)
c0z = float(0.0)
c1x = float(0.0)
c1y = float(0.0)
c1z = float(0.0)
if edge_id == 0: # 0-1
c0x = -sx
c0y = -sy
c0z = -sz
c1x = sx
c1y = -sy
c1z = -sz
elif edge_id == 1: # 1-2
c0x = sx
c0y = -sy
c0z = -sz
c1x = sx
c1y = sy
c1z = -sz
elif edge_id == 2: # 2-3
c0x = sx
c0y = sy
c0z = -sz
c1x = -sx
c1y = sy
c1z = -sz
elif edge_id == 3: # 3-0
c0x = -sx
c0y = sy
c0z = -sz
c1x = -sx
c1y = -sy
c1z = -sz
elif edge_id == 4: # 4-5
c0x = -sx
c0y = -sy
c0z = sz
c1x = sx
c1y = -sy
c1z = sz
elif edge_id == 5: # 5-6
c0x = sx
c0y = -sy
c0z = sz
c1x = sx
c1y = sy
c1z = sz
elif edge_id == 6: # 6-7
c0x = sx
c0y = sy
c0z = sz
c1x = -sx
c1y = sy
c1z = sz
elif edge_id == 7: # 7-4
c0x = -sx
c0y = sy
c0z = sz
c1x = -sx
c1y = -sy
c1z = sz
elif edge_id == 8: # 0-4
c0x = -sx
c0y = -sy
c0z = -sz
c1x = -sx
c1y = -sy
c1z = sz
elif edge_id == 9: # 1-5
c0x = sx
c0y = -sy
c0z = -sz
c1x = sx
c1y = -sy
c1z = sz
elif edge_id == 10: # 2-6
c0x = sx
c0y = sy
c0z = -sz
c1x = sx
c1y = sy
c1z = sz
elif edge_id == 11: # 3-7
c0x = -sx
c0y = sy
c0z = -sz
c1x = -sx
c1y = sy
c1z = sz
local0 = wp.vec3(c0x, c0y, c0z)
local1 = wp.vec3(c1x, c1y, c1z)
# Transform from inertia-principal frame to body COM frame
inertia_rot = wp.quat_from_matrix(rot)
local0 = wp.quat_rotate(inertia_rot, local0)
local1 = wp.quat_rotate(inertia_rot, local1)
# Transform from COM frame to world frame
body_tf = body_q[body_id]
body_rot = wp.transform_get_rotation(body_tf)
body_pos = wp.transform_get_translation(body_tf)
com = body_com[body_id]
# COM offset in world frame
world_com = body_pos + wp.quat_rotate(body_rot, com)
world0 = world_com + wp.quat_rotate(body_rot, local0)
world1 = world_com + wp.quat_rotate(body_rot, local1)
# Apply world offset
if world_offsets and world_idx >= 0 and world_idx < world_offsets.shape[0]:
offset = world_offsets[world_idx]
world0 = world0 + offset
world1 = world1 + offset
line_starts[tid] = world0
line_ends[tid] = world1
line_colors[tid] = color
@wp.func
def depth_to_color(depth: float, min_depth: float, max_depth: float) -> wp.vec3:
"""Convert depth value to a color using a blue-to-red colormap."""
# Normalize depth to [0, 1]
t = wp.clamp((depth - min_depth) / (max_depth - min_depth + 1e-8), 0.0, 1.0)
# Blue (0,0,1) -> Cyan (0,1,1) -> Green (0,1,0) -> Yellow (1,1,0) -> Red (1,0,0)
if t < 0.25:
s = t / 0.25
return wp.vec3(0.0, s, 1.0)
elif t < 0.5:
s = (t - 0.25) / 0.25
return wp.vec3(0.0, 1.0, 1.0 - s)
elif t < 0.75:
s = (t - 0.5) / 0.25
return wp.vec3(s, 1.0, 0.0)
else:
s = (t - 0.75) / 0.25
return wp.vec3(1.0, 1.0 - s, 0.0)
@wp.kernel(enable_backward=False)
def compute_hydro_contact_surface_lines(
triangle_vertices: wp.array[wp.vec3],
face_depths: wp.array[wp.float32],
face_shape_pairs: wp.array[wp.vec2i],
shape_world: wp.array[int],
world_offsets: wp.array[wp.vec3],
num_faces: int,
min_depth: float,
max_depth: float,
penetrating_only: bool,
line_starts: wp.array[wp.vec3],
line_ends: wp.array[wp.vec3],
line_colors: wp.array[wp.vec3],
):
"""Convert hydroelastic contact surface triangle vertices to line segments for wireframe rendering."""
tid = wp.tid()
if tid >= num_faces:
return
# Get the 3 vertices of this triangle
v0 = triangle_vertices[tid * 3 + 0]
v1 = triangle_vertices[tid * 3 + 1]
v2 = triangle_vertices[tid * 3 + 2]
# Compute color from depth (standard convention: negative = penetrating)
depth = face_depths[tid]
# Skip non-penetrating contacts if requested (only render depth < 0)
if penetrating_only and depth >= 0.0:
zero = wp.vec3(0.0, 0.0, 0.0)
line_starts[tid * 3 + 0] = zero
line_ends[tid * 3 + 0] = zero
line_colors[tid * 3 + 0] = zero
line_starts[tid * 3 + 1] = zero
line_ends[tid * 3 + 1] = zero
line_colors[tid * 3 + 1] = zero
line_starts[tid * 3 + 2] = zero
line_ends[tid * 3 + 2] = zero
line_colors[tid * 3 + 2] = zero
return
# Apply world offset if available
offset = wp.vec3(0.0, 0.0, 0.0)
if shape_world and world_offsets:
shape_pair = face_shape_pairs[tid]
world_a = shape_world[shape_pair[0]]
world_b = shape_world[shape_pair[1]]
if world_a >= 0 or world_b >= 0:
offset = world_offsets[world_a if world_a >= 0 else world_b]
v0 = v0 + offset
v1 = v1 + offset
v2 = v2 + offset
# Use penetration magnitude (negated depth) for color - deeper = more red
if depth < 0.0:
color = depth_to_color(-depth, min_depth, max_depth)
else:
color = wp.vec3(0.0, 0.0, 0.0)
# Each triangle produces 3 line segments (edges)
# Edge 0: v0 -> v1
line_starts[tid * 3 + 0] = v0
line_ends[tid * 3 + 0] = v1
line_colors[tid * 3 + 0] = color
# Edge 1: v1 -> v2
line_starts[tid * 3 + 1] = v1
line_ends[tid * 3 + 1] = v2
line_colors[tid * 3 + 1] = color
# Edge 2: v2 -> v0
line_starts[tid * 3 + 2] = v2
line_ends[tid * 3 + 2] = v0
line_colors[tid * 3 + 2] = color
PARTICLE_ACTIVE = wp.constant(wp.int32(newton.ParticleFlags.ACTIVE))
@wp.kernel
def build_active_particle_mask(
flags: wp.array[wp.int32],
mask: wp.array[wp.int32],
):
i = wp.tid()
if (flags[i] & PARTICLE_ACTIVE) != wp.int32(0):
mask[i] = wp.int32(1)
else:
mask[i] = wp.int32(0)
@wp.kernel
def compact(
src: wp.array[Any],
mask: wp.array[wp.int32],
offsets: wp.array[wp.int32],
dst: wp.array[Any],
):
i = wp.tid()
if mask[i] == wp.int32(1):
dst[offsets[i]] = src[i]
================================================
FILE: newton/_src/viewer/picking.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
import numpy as np
import warp as wp
import newton
from ..geometry import raycast
from .kernels import PickingState, apply_picking_force_kernel, compute_pick_state_kernel, update_pick_target_kernel
class Picking:
"""
Picking system.
Allows to pick a body in the viewer by right clicking on it and dragging the mouse.
This can be used to move objects around in the viewer, a typical use case is to check for solver resilience or
see how well a RL policy is coping with disturbances.
"""
def __init__(
self,
model: newton.Model,
pick_stiffness: float = 50.0,
pick_damping: float = 5.0,
world_offsets: wp.array | None = None,
) -> None:
"""
Initializes the picking system.
Args:
model: The model to pick from.
pick_stiffness: The stiffness that will be used to compute the force applied to the picked body.
pick_damping: The damping that will be used to compute the force applied to the picked body.
world_offsets: Optional warp array of world offsets (dtype=wp.vec3) for multi-world picking support.
"""
self.model = model
self.pick_stiffness = pick_stiffness
self.pick_damping = pick_damping
self.world_offsets = world_offsets
self.min_dist = None
self.min_index = None
self.min_body_index = None
self.lock = None
self._contact_points0 = None
self._contact_points1 = None
self._debug = False
# picking state
if model and model.device.is_cuda:
self.pick_body = wp.array([-1], dtype=int, pinned=True, device=model.device)
else:
self.pick_body = wp.array([-1], dtype=int, device="cpu")
pick_state_np = np.empty(1, dtype=PickingState.numpy_dtype())
pick_state_np[0]["pick_stiffness"] = pick_stiffness
pick_state_np[0]["pick_damping"] = pick_damping
self.pick_state = wp.array(pick_state_np, dtype=PickingState, device=model.device if model else "cpu", ndim=1)
self.pick_dist = 0.0
self.picking_active = False
self._default_on_mouse_drag = None
def _apply_picking_force(self, state: newton.State) -> None:
"""
Applies a force to the picked body.
Args:
state: The simulation state.
"""
if self.model is None:
return
# Launch kernel always because of graph capture
wp.launch(
kernel=apply_picking_force_kernel,
dim=1,
inputs=[
state.body_q,
state.body_qd,
state.body_f,
self.pick_body,
self.pick_state,
self.model.body_flags,
self.model.body_com,
self.model.body_mass,
],
device=self.model.device,
)
def is_picking(self) -> bool:
"""Checks if picking is active.
Returns:
bool: True if picking is active, False otherwise.
"""
return self.picking_active
def release(self) -> None:
"""Releases the picking."""
self.pick_body.fill_(-1)
self.picking_active = False
def update(self, ray_start: wp.vec3f, ray_dir: wp.vec3f) -> None:
"""
Updates the picking target.
This function is used to track the force that needs to be applied to the picked body as the mouse is dragged.
Args:
ray_start: The start point of the ray.
ray_dir: The direction of the ray.
"""
if not self.is_picking():
return
# Get the world offset for the picked body
world_offset = wp.vec3(0.0, 0.0, 0.0)
if self.world_offsets is not None and self.world_offsets.shape[0] > 0:
# Get the picked body index
picked_body_idx = self.pick_body.numpy()[0]
if picked_body_idx >= 0 and self.model.body_world is not None:
# Find which world this body belongs to
body_world_idx = self.model.body_world.numpy()[picked_body_idx]
if body_world_idx >= 0 and body_world_idx < self.world_offsets.shape[0]:
offset_np = self.world_offsets.numpy()[body_world_idx]
world_offset = wp.vec3(float(offset_np[0]), float(offset_np[1]), float(offset_np[2]))
wp.launch(
kernel=update_pick_target_kernel,
dim=1,
inputs=[
ray_start,
ray_dir,
world_offset,
self.pick_state,
],
device=self.model.device,
)
def pick(self, state: newton.State, ray_start: wp.vec3f, ray_dir: wp.vec3f) -> None:
"""
Picks the selected geometry and computes the initial state of the picking. I.e. the force that
will be applied to the picked body.
Args:
state: The simulation state.
ray_start: The start point of the ray.
ray_dir: The direction of the ray.
"""
if self.model is None:
return
p, d = ray_start, ray_dir
num_geoms = self.model.shape_count
if num_geoms == 0:
return
if self.min_dist is None:
self.min_dist = wp.array([1.0e10], dtype=float, device=self.model.device)
self.min_index = wp.array([-1], dtype=int, device=self.model.device)
self.min_body_index = wp.array([-1], dtype=int, device=self.model.device)
self.lock = wp.array([0], dtype=wp.int32, device=self.model.device)
else:
self.min_dist.fill_(1.0e10)
self.min_index.fill_(-1)
self.min_body_index.fill_(-1)
self.lock.zero_()
# Get world offsets if available
shape_world = (
self.model.shape_world
if self.model.shape_world is not None
else wp.array([], dtype=int, device=self.model.device)
)
if self.world_offsets is not None:
world_offsets = self.world_offsets
else:
world_offsets = wp.array([], dtype=wp.vec3, device=self.model.device)
wp.launch(
kernel=raycast.raycast_kernel,
dim=num_geoms,
inputs=[
state.body_q,
self.model.shape_body,
self.model.shape_transform,
self.model.shape_type,
self.model.shape_scale,
self.model.shape_source_ptr,
p,
d,
self.lock,
],
outputs=[self.min_dist, self.min_index, self.min_body_index, shape_world, world_offsets],
device=self.model.device,
)
wp.synchronize()
dist = self.min_dist.numpy()[0]
index = self.min_index.numpy()[0]
body_index = self.min_body_index.numpy()[0]
if dist < 1.0e10 and body_index >= 0:
self.pick_dist = dist
# Ensures that the ray direction and start point are vec3f objects
d = wp.vec3f(d[0], d[1], d[2])
p = wp.vec3f(p[0], p[1], p[2])
# world space hit point (in offset coordinate system from raycast)
hit_point_world = p + d * float(dist)
# Convert hit point from offset space to physics space
# The raycast was done with world offsets applied, so we need to remove them
if world_offsets.shape[0] > 0 and shape_world.shape[0] > 0 and index >= 0:
world_idx_np = shape_world.numpy()[index] if hasattr(shape_world, "numpy") else shape_world[index]
if world_idx_np >= 0 and world_idx_np < world_offsets.shape[0]:
offset_np = world_offsets.numpy()[world_idx_np]
hit_point_world = wp.vec3f(
hit_point_world[0] - offset_np[0],
hit_point_world[1] - offset_np[1],
hit_point_world[2] - offset_np[2],
)
wp.launch(
kernel=compute_pick_state_kernel,
dim=1,
inputs=[state.body_q, self.model.body_flags, body_index, hit_point_world],
outputs=[self.pick_body, self.pick_state],
device=self.model.device,
)
wp.synchronize()
self.picking_active = self.pick_body.numpy()[0] >= 0
if self._debug:
if dist < 1.0e10:
print("#" * 80)
print(f"Hit geom {index} of body {body_index} at distance {dist}")
print("#" * 80)
================================================
FILE: newton/_src/viewer/viewer.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import enum
import os
import sys
import warnings
from abc import ABC, abstractmethod
from collections.abc import Sequence
from typing import Any
import numpy as np
import warp as wp
import newton
from newton.utils import compute_world_offsets, solidify_mesh
from ..core.types import MAXVAL, Axis
from .kernels import (
build_active_particle_mask,
compact,
compute_hydro_contact_surface_lines,
estimate_world_extents,
repack_shape_colors,
)
class ViewerBase(ABC):
class SDFMarginMode(enum.IntEnum):
"""Controls which offset surface is visualized for SDF debug wireframes."""
OFF = 0
"""Do not draw SDF margin debug wireframes."""
MARGIN = 1
"""Wireframe at ``shape_margin`` only."""
MARGIN_GAP = 2
"""Wireframe at ``shape_margin`` + ``shape_gap`` (outer contact threshold), not gap alone."""
def __init__(self):
"""Initialize shared viewer state and rendering caches."""
self.time = 0.0
self.device = wp.get_device()
self.picking_enabled = True
# All model-dependent state is initialized by clear_model()
self.clear_model()
def is_running(self) -> bool:
"""Report whether the viewer backend should keep running.
Returns:
bool: True while the viewer should continue rendering.
"""
return True
def is_paused(self) -> bool:
"""Report whether the viewer is currently paused.
Returns:
bool: True when simulation stepping is paused.
"""
return False
def is_key_down(self, key: str | int) -> bool:
"""Default key query API. Concrete viewers can override.
Args:
key: Key identifier (string or backend-specific code)
Returns:
bool: Always False by default.
"""
return False
def clear_model(self) -> None:
"""Reset all model-dependent state to defaults.
Called from ``__init__`` to establish initial values and whenever the
current model needs to be discarded (e.g. before :meth:`set_model` or
when switching examples).
"""
self.model = None
self.model_changed = True
# Shape instance batches (shape hash -> ShapeInstances)
self._shape_instances = {}
# Inertia box wireframe line vertices (12 lines per body)
self._inertia_box_points0 = None
self._inertia_box_points1 = None
self._inertia_box_colors = None
# Geometry mesh cache (geometry hash -> mesh path)
self._geometry_cache: dict[int, str] = {}
# Contact line vertices
self._contact_points0 = None
self._contact_points1 = None
# Joint basis line vertices (3 lines per joint)
self._joint_points0 = None
self._joint_points1 = None
self._joint_colors = None
# Center-of-mass visualization
self._com_positions = None
self._com_colors = None
self._com_radii = None
# World offset support
self.world_offsets = None
self.max_worlds = None
# Picking
self.picking_enabled = True
# Display options
self.show_joints = False
self.show_com = False
self.show_particles = False
self.show_contacts = False
self.show_springs = False
self.show_triangles = True
self.show_gaussians = False
self.show_collision = False
self.show_visual = True
self.show_static = False
self.show_inertia_boxes = False
self.show_hydro_contact_surface = False
self.sdf_margin_mode: ViewerBase.SDFMarginMode = ViewerBase.SDFMarginMode.OFF
self.gaussians_max_points = 100_000 # Max number of points to visualize per gaussian
# Hydroelastic contact surface line cache
self._hydro_surface_line_starts: wp.array | None = None
self._hydro_surface_line_ends: wp.array | None = None
self._hydro_surface_line_colors: wp.array | None = None
# Per-shape color buffer and indexing
self.model_shape_color: wp.array[wp.vec3] = None
self._shape_to_slot: np.ndarray | None = None
self._slot_to_shape: np.ndarray | None = None
self._slot_to_shape_wp: wp.array | None = None
self._shape_to_batch: list[ViewerBase.ShapeInstances | None] | None = None
# Isomesh cache for SDF collision visualization
self._isomesh_cache: dict[int, newton.Mesh | None] = {}
# Gaussian shapes rendered as point clouds (skipped by the mesh instancing pipeline).
# Each entry is (name, gaussian, parent_body, shape_xform, world_index, flags, is_static).
self._gaussian_instances: list[tuple[str, newton.Gaussian, int, wp.transform, int, int, bool]] = []
self._sdf_isomesh_instances: dict[int, ViewerBase.ShapeInstances] = {}
self._sdf_isomesh_populated: bool = False
self._shape_sdf_index_host: np.ndarray | None = None
# SDF margin visualization (wireframe edges).
# Mesh cache: keyed by (geo_type, geo_scale, geo_src_id, offset).
# Vertex-data cache: keyed by (id(mesh), color) — avoids redundant
# edge extraction when the same mesh appears on multiple shapes.
# Edge caches: per-mode dict of
# {shape_idx: (vertex_data, body_idx, shape_xf, world_idx)}.
# Keeping separate per-mode caches lets mode toggling reuse GPU VBOs.
self._sdf_margin_mesh_cache: dict[tuple, newton.Mesh | None] = {}
self._sdf_margin_vdata_cache: dict[tuple, np.ndarray] = {}
self._sdf_margin_edge_caches: dict[
ViewerBase.SDFMarginMode, dict[int, tuple[np.ndarray, int, np.ndarray, int]]
] = {}
def set_model(self, model: newton.Model | None, max_worlds: int | None = None):
"""
Set the model to be visualized.
Args:
model: The Newton model to visualize.
max_worlds: Maximum number of worlds to render (None = all).
Useful for performance when training with many environments.
"""
if self.model is not None:
self.clear_model()
self.model = model
self.max_worlds = max_worlds
if model is not None:
self.device = model.device
self._shape_sdf_index_host = model.shape_sdf_index.numpy() if model.shape_sdf_index is not None else None
self._populate_shapes()
# Auto-compute world offsets if not already set
if self.world_offsets is None:
self._auto_compute_world_offsets()
def _should_render_world(self, world_idx: int) -> bool:
"""Check if a world should be rendered based on max_worlds limit."""
if world_idx == -1: # Global entities always rendered
return True
if self.max_worlds is None:
return True
return world_idx < self.max_worlds
def _get_render_world_count(self) -> int:
"""Get the number of worlds to render."""
if self.model is None:
return 0
if self.max_worlds is None:
return self.model.world_count
return min(self.max_worlds, self.model.world_count)
def _get_shape_isomesh(self, shape_idx: int) -> newton.Mesh | None:
"""Get the isomesh for a collision shape with a texture SDF.
Computes the marching-cubes isosurface from the texture SDF and caches it
by SDF table index.
Args:
shape_idx: Index of the shape.
Returns:
Mesh object for the isomesh, or ``None`` if shape has no texture SDF.
"""
if self.model is None:
return None
sdf_idx = int(self._shape_sdf_index_host[shape_idx]) if self._shape_sdf_index_host is not None else -1
if sdf_idx < 0 or self.model.texture_sdf_data is None:
return None
if sdf_idx in self._isomesh_cache:
return self._isomesh_cache[sdf_idx]
slots = (
self.model.texture_sdf_subgrid_start_slots[sdf_idx]
if hasattr(self.model, "texture_sdf_subgrid_start_slots") and self.model.texture_sdf_subgrid_start_slots
else None
)
if slots is None:
self._isomesh_cache[sdf_idx] = None
return None
from ..geometry.sdf_texture import compute_isomesh_from_texture_sdf # noqa: PLC0415
coarse_tex = self.model.texture_sdf_coarse_textures[sdf_idx]
coarse_dims = (coarse_tex.width - 1, coarse_tex.height - 1, coarse_tex.depth - 1)
isomesh = compute_isomesh_from_texture_sdf(
self.model.texture_sdf_data, sdf_idx, slots, coarse_dims, device=self.device
)
self._isomesh_cache[sdf_idx] = isomesh
return isomesh
def set_camera(self, pos: wp.vec3, pitch: float, yaw: float):
"""Set the camera position and orientation.
Args:
pos: The position of the camera.
pitch: The pitch of the camera.
yaw: The yaw of the camera.
"""
return
def set_world_offsets(self, spacing: tuple[float, float, float] | list[float] | wp.vec3):
"""Set world offsets for visual separation of multiple worlds.
Args:
spacing: Spacing between worlds along each axis as a tuple, list, or wp.vec3.
Example: (5.0, 5.0, 0.0) for 5 units spacing in X and Y.
Raises:
RuntimeError: If model has not been set yet
"""
if self.model is None:
raise RuntimeError("Model must be set before calling set_world_offsets()")
world_count = self._get_render_world_count()
# Get up axis from model
up_axis = self.model.up_axis
# Convert to tuple if needed
if isinstance(spacing, (list, wp.vec3)):
spacing = (float(spacing[0]), float(spacing[1]), float(spacing[2]))
# Compute offsets using the shared utility function
world_offsets = compute_world_offsets(world_count, spacing, up_axis)
# Convert to warp array
self.world_offsets = wp.array(world_offsets, dtype=wp.vec3, device=self.device)
def _get_world_extents(self) -> tuple[float, float, float] | None:
"""Get the maximum extents of all worlds in the model."""
if self.model is None:
return None
world_count = self.model.world_count
# Initialize bounds arrays for all worlds
world_bounds_min = wp.full((world_count, 3), MAXVAL, dtype=wp.float32, device=self.device)
world_bounds_max = wp.full((world_count, 3), -MAXVAL, dtype=wp.float32, device=self.device)
# Get initial state for body transforms
state = self.model.state()
# Launch kernel to compute bounds for all worlds
wp.launch(
kernel=estimate_world_extents,
dim=self.model.shape_count,
inputs=[
self.model.shape_transform,
self.model.shape_body,
self.model.shape_collision_radius,
self.model.shape_world,
state.body_q,
world_count,
],
outputs=[world_bounds_min, world_bounds_max],
device=self.device,
)
# Get bounds back to CPU
bounds_min_np = world_bounds_min.numpy()
bounds_max_np = world_bounds_max.numpy()
# Find maximum extents across all worlds
# Mask out invalid bounds (inf values)
valid_mask = ~np.isinf(bounds_min_np[:, 0])
if not valid_mask.any():
# No valid worlds found
return None
# Compute extents for valid worlds and take maximum
valid_min = bounds_min_np[valid_mask]
valid_max = bounds_max_np[valid_mask]
world_extents = valid_max - valid_min
max_extents = np.max(world_extents, axis=0)
return tuple(max_extents)
def _auto_compute_world_offsets(self):
"""Automatically compute world offsets based on model extents."""
max_extents = self._get_world_extents()
if max_extents is None:
return
# Add margin
margin = 1.5 # 50% margin between worlds
# Default to 2D square grid arrangement perpendicular to up axis
spacing = [np.ceil(max(max_extents) * margin)] * 3
spacing[self.model.up_axis] = 0.0
# Set world offsets with computed spacing
self.set_world_offsets(tuple(spacing))
def begin_frame(self, time: float):
"""Begin a new frame.
Args:
time: The current frame time.
"""
self.time = time
@abstractmethod
def end_frame(self):
"""
End the current frame.
"""
pass
def log_state(self, state: newton.State):
"""Update the viewer with the given state of the simulation.
Args:
state: The current state of the simulation.
"""
if self.model is None:
return
self._sync_shape_colors_from_model()
# compute shape transforms and render
for shapes in self._shape_instances.values():
visible = self._should_show_shape(shapes.flags, shapes.static)
if visible:
shapes.update(state, world_offsets=self.world_offsets)
colors = shapes.colors if self.model_changed or shapes.colors_changed else None
materials = shapes.materials if self.model_changed else None
# Capsules may be rendered via a specialized path by the concrete viewer/backend
# (e.g., instanced cylinder body + instanced sphere end caps for better batching).
# The base implementation of log_capsules() falls back to log_instances().
if shapes.geo_type == newton.GeoType.CAPSULE:
self.log_capsules(
shapes.name,
shapes.mesh,
shapes.world_xforms,
shapes.scales,
colors,
materials,
hidden=not visible,
)
else:
self.log_instances(
shapes.name,
shapes.mesh,
shapes.world_xforms,
shapes.scales, # Always pass scales - needed for transform matrix calculation
colors,
materials,
hidden=not visible,
)
shapes.colors_changed = False
self._log_gaussian_shapes(state)
self._log_non_shape_state(state)
self.model_changed = False
def _sync_shape_colors_from_model(self):
"""Propagate model-owned shape colors into viewer batches.
Always launches a GPU kernel to repack colors from model order into
viewer batch order. This is cheaper than a D2H transfer + host-side
comparison every frame.
"""
if (
self.model is None
or self.model.shape_color is None
or self.model_shape_color is None
or self._slot_to_shape_wp is None
):
return
wp.launch(
kernel=repack_shape_colors,
dim=len(self.model_shape_color),
inputs=[self.model.shape_color, self._slot_to_shape_wp],
outputs=[self.model_shape_color],
device=self.device,
record_tape=False,
)
for batch_ref in self._shape_instances.values():
batch_ref.colors_changed = True
def _log_gaussian_shapes(self, state: newton.State):
"""Render Gaussian shapes as point clouds with current body transforms."""
if not self._gaussian_instances:
return
body_q_np = None
offsets_np = None
for gname, gaussian, parent, shape_xform, world_idx, flags, is_static in self._gaussian_instances:
visible = self._should_show_shape(flags, is_static)
if not visible or not self.show_gaussians:
self.log_gaussian(gname, gaussian, hidden=True)
continue
if parent >= 0:
if body_q_np is None:
body_q_np = state.body_q.numpy()
body_xform = wp.transform_expand(body_q_np[parent])
world_xform = wp.transform_multiply(body_xform, shape_xform)
else:
world_xform = shape_xform
if self.world_offsets is not None and world_idx >= 0:
if offsets_np is None:
offsets_np = self.world_offsets.numpy()
offset = offsets_np[world_idx]
world_xform = wp.transformf(
wp.vec3(world_xform.p[0] + offset[0], world_xform.p[1] + offset[1], world_xform.p[2] + offset[2]),
world_xform.q,
)
self.log_gaussian(gname, gaussian, xform=world_xform, hidden=False)
def _log_non_shape_state(self, state: newton.State):
"""Log SDF isomeshes, inertia boxes, triangles, particles, joints, COM."""
sdf_isomesh_just_populated = False
if self.show_collision and not self._sdf_isomesh_populated:
self._populate_sdf_isomesh_instances()
self._sdf_isomesh_populated = True
sdf_isomesh_just_populated = True
for shapes in self._sdf_isomesh_instances.values():
visible = self.show_collision
if visible:
shapes.update(state, world_offsets=self.world_offsets)
send_appearance = self.model_changed or sdf_isomesh_just_populated
self.log_instances(
shapes.name,
shapes.mesh,
shapes.world_xforms,
shapes.scales,
shapes.colors if send_appearance else None,
shapes.materials if send_appearance else None,
hidden=not visible,
)
self._log_inertia_boxes(state)
self._log_sdf_margin_wireframes(state)
self._log_triangles(state)
self._log_particles(state)
self._log_joints(state)
self._log_com(state)
def log_contacts(self, contacts: newton.Contacts, state: newton.State):
"""
Creates line segments along contact normals for rendering.
Args:
contacts: The contacts to render.
state: The current state of the simulation.
"""
if not self.show_contacts:
# Pass None to hide joints - renderer will handle creating empty arrays
self.log_lines("/contacts", None, None, None)
return
# Get contact count, clamped to buffer size (counter may exceed max on overflow)
max_contacts = contacts.rigid_contact_max
num_contacts = min(int(contacts.rigid_contact_count.numpy()[0]), max_contacts)
# Ensure we have buffers for line endpoints
if self._contact_points0 is None or len(self._contact_points0) < max_contacts:
self._contact_points0 = wp.array(np.zeros((max_contacts, 3)), dtype=wp.vec3, device=self.device)
self._contact_points1 = wp.array(np.zeros((max_contacts, 3)), dtype=wp.vec3, device=self.device)
# Always run the kernel to ensure buffers are properly cleared/updated
if max_contacts > 0:
from .kernels import compute_contact_lines # noqa: PLC0415
wp.launch(
kernel=compute_contact_lines,
dim=max_contacts,
inputs=[
state.body_q,
self.model.shape_body,
self.model.shape_world,
self.world_offsets,
contacts.rigid_contact_count,
contacts.rigid_contact_shape0,
contacts.rigid_contact_shape1,
contacts.rigid_contact_point0,
contacts.rigid_contact_offset0,
contacts.rigid_contact_normal,
0.1, # line length scale factor
],
outputs=[
self._contact_points0, # line start points
self._contact_points1, # line end points
],
device=self.device,
)
# Always call log_lines to update the renderer (handles zero contacts gracefully)
if num_contacts > 0:
# Slice arrays to only include active contacts
starts = self._contact_points0[:num_contacts]
ends = self._contact_points1[:num_contacts]
else:
# Create empty arrays for zero contacts case
starts = wp.array([], dtype=wp.vec3, device=self.device)
ends = wp.array([], dtype=wp.vec3, device=self.device)
# Use green color for contact normals
colors = (0.0, 1.0, 0.0)
self.log_lines("/contacts", starts, ends, colors)
def log_hydro_contact_surface(
self,
contact_surface_data: newton.geometry.HydroelasticSDF.ContactSurfaceData | None,
penetrating_only: bool = True,
):
"""
Render the hydroelastic contact surface triangles as wireframe lines.
Args:
contact_surface_data: A :class:`newton.geometry.HydroelasticSDF.ContactSurfaceData`
instance containing vertex arrays for visualization, or None if hydroelastic
collision is not enabled.
penetrating_only: If True, only render penetrating contacts (depth < 0).
"""
if not self.show_hydro_contact_surface:
self.log_lines("/hydro_contact_surface", None, None, None)
return
if contact_surface_data is None:
self.log_lines("/hydro_contact_surface", None, None, None)
return
# Get the number of face contacts (triangles)
num_contacts = int(contact_surface_data.face_contact_count.numpy()[0])
if num_contacts == 0:
self.log_lines("/hydro_contact_surface", None, None, None)
return
# Each triangle has 3 edges -> 3 line segments per contact
num_lines = 3 * num_contacts
max_lines = 3 * contact_surface_data.max_num_face_contacts
# Pre-allocate line buffers (only once, to max capacity)
if self._hydro_surface_line_starts is None or len(self._hydro_surface_line_starts) < max_lines:
self._hydro_surface_line_starts = wp.zeros(max_lines, dtype=wp.vec3, device=self.device)
self._hydro_surface_line_ends = wp.zeros(max_lines, dtype=wp.vec3, device=self.device)
self._hydro_surface_line_colors = wp.zeros(max_lines, dtype=wp.vec3, device=self.device)
# Get depth range for colormap
depths = contact_surface_data.contact_surface_depth[:num_contacts]
# Convert triangles to line segments with depth-based colors
vertices = contact_surface_data.contact_surface_point
shape_pairs = contact_surface_data.contact_surface_shape_pair
wp.launch(
compute_hydro_contact_surface_lines,
dim=num_contacts,
inputs=[
vertices,
depths,
shape_pairs,
self.model.shape_world,
self.world_offsets,
num_contacts,
0.0,
0.0005,
penetrating_only,
],
outputs=[self._hydro_surface_line_starts, self._hydro_surface_line_ends, self._hydro_surface_line_colors],
device=self.device,
)
# Render as lines
self.log_lines(
"/hydro_contact_surface",
self._hydro_surface_line_starts[:num_lines],
self._hydro_surface_line_ends[:num_lines],
self._hydro_surface_line_colors[:num_lines],
)
def log_shapes(
self,
name: str,
geo_type: int,
geo_scale: float | tuple[float, ...] | list[float] | np.ndarray,
xforms: wp.array[wp.transform],
colors: wp.array[wp.vec3] | None = None,
materials: wp.array[wp.vec4] | None = None,
geo_thickness: float = 0.0,
geo_is_solid: bool = True,
geo_src: newton.Mesh | newton.Heightfield | None = None,
hidden: bool = False,
):
"""
Convenience helper to create/cache a mesh of a given geometry and
render a batch of instances with the provided transforms/colors/materials.
Args:
name: Instance path/name (e.g., "/world/spheres").
geo_type: Geometry type value from :class:`newton.GeoType`.
geo_scale: Geometry scale parameters:
- Sphere: float radius
- Capsule/Cylinder/Cone: (radius, height)
- Plane: (width, length) or float for both
- Box: (x_extent, y_extent, z_extent) or float for all
xforms: wp.array[wp.transform] of instance transforms
colors: wp.array[wp.vec3] or None (broadcasted if length 1)
materials: wp.array[wp.vec4] or None (broadcasted if length 1)
geo_thickness: Optional thickness used for hashing and solidification.
geo_is_solid: If False, use shell-thickening for mesh-based geometry.
geo_src: Source geometry to use only when ``geo_type`` is
:attr:`newton.GeoType.MESH`.
hidden: If True, the shape will not be rendered
"""
# normalize geo_scale to a list for hashing + mesh creation
def _as_float_list(value):
if isinstance(value, tuple | list | np.ndarray):
return [float(v) for v in value]
else:
return [float(value)]
geo_scale = _as_float_list(geo_scale)
# ensure mesh exists (shared with populate path)
mesh_path = self._populate_geometry(
int(geo_type),
tuple(geo_scale),
float(geo_thickness),
bool(geo_is_solid),
geo_src=geo_src,
)
# prepare instance properties
num_instances = len(xforms)
# scales default to ones
scales = wp.array([wp.vec3(1.0, 1.0, 1.0)] * num_instances, dtype=wp.vec3, device=self.device)
# broadcast helpers
def _ensure_vec3_array(arr, default):
if arr is None:
return wp.array([default] * num_instances, dtype=wp.vec3, device=self.device)
if len(arr) == 1 and num_instances > 1:
val = wp.vec3(*arr.numpy()[0])
return wp.array([val] * num_instances, dtype=wp.vec3, device=self.device)
return arr
def _ensure_vec4_array(arr, default):
if arr is None:
return wp.array([default] * num_instances, dtype=wp.vec4, device=self.device)
if len(arr) == 1 and num_instances > 1:
val = wp.vec4(*arr.numpy()[0])
return wp.array([val] * num_instances, dtype=wp.vec4, device=self.device)
return arr
# defaults
default_color = wp.vec3(0.3, 0.8, 0.9)
default_material = wp.vec4(0.5, 0.0, 0.0, 0.0)
# planes default to checkerboard and mid-gray if not overridden
if geo_type == newton.GeoType.PLANE:
default_color = wp.vec3(0.125, 0.125, 0.25)
# default_material = wp.vec4(0.5, 0.0, 1.0, 0.0)
colors = _ensure_vec3_array(colors, default_color)
materials = _ensure_vec4_array(materials, default_material)
# finally, log the instances
self.log_instances(name, mesh_path, xforms, scales, colors, materials, hidden=hidden)
def log_geo(
self,
name: str,
geo_type: int,
geo_scale: tuple[float, ...],
geo_thickness: float,
geo_is_solid: bool,
geo_src: newton.Mesh | newton.Heightfield | None = None,
hidden: bool = False,
):
"""
Create a primitive mesh and upload it via :meth:`log_mesh`.
Expects mesh generators to return interleaved vertices [x, y, z, nx, ny, nz, u, v]
and an index buffer. Slices them into separate arrays and forwards to log_mesh.
Args:
name: Unique path/name used to register the mesh.
geo_type: Geometry type value from :class:`newton.GeoType`.
geo_scale: Geometry scale tuple, interpreted per geometry type.
geo_thickness: Shell thickness for non-solid mesh generation.
geo_is_solid: Whether to render mesh geometry as a solid.
geo_src: Source :class:`newton.Mesh` or
:class:`newton.Heightfield` data when required
by ``geo_type``.
hidden: Whether the created mesh should be hidden.
"""
if geo_type == newton.GeoType.GAUSSIAN:
if geo_src is None:
raise ValueError(f"log_geo requires geo_src for GAUSSIAN (name={name})")
if not isinstance(geo_src, newton.Gaussian):
raise TypeError(f"log_geo expected newton.Gaussian for GAUSSIAN (name={name})")
if not self.show_gaussians:
hidden = True
self.log_gaussian(name, geo_src, hidden=hidden)
return
# Heightfield: convert to mesh for rendering
if geo_type == newton.GeoType.HFIELD:
if geo_src is None:
raise ValueError(f"log_geo requires geo_src for HFIELD (name={name})")
assert isinstance(geo_src, newton.Heightfield)
# Denormalize elevation data to actual Z heights.
# Transpose because create_mesh_heightfield uses ij indexing (i=X, j=Y)
# while Heightfield uses row-major (row=Y, col=X).
actual_heights = geo_src.min_z + geo_src.data * (geo_src.max_z - geo_src.min_z)
mesh = newton.Mesh.create_heightfield(
heightfield=actual_heights.T,
extent_x=geo_src.hx * 2.0,
extent_y=geo_src.hy * 2.0,
ground_z=geo_src.min_z,
compute_inertia=False,
)
points = wp.array(mesh.vertices, dtype=wp.vec3, device=self.device)
indices = wp.array(mesh.indices, dtype=wp.int32, device=self.device)
self.log_mesh(name, points, indices, hidden=hidden)
return
# GEO_MESH handled by provided source geometry
if geo_type in (newton.GeoType.MESH, newton.GeoType.CONVEX_MESH):
if geo_src is None:
raise ValueError(f"log_geo requires geo_src for MESH or CONVEX_MESH (name={name})")
assert isinstance(geo_src, newton.Mesh)
# resolve points/indices from source, solidify if requested
if not geo_is_solid:
indices, points = solidify_mesh(geo_src.indices, geo_src.vertices, geo_thickness)
else:
indices, points = geo_src.indices, geo_src.vertices
# prepare warp arrays; synthesize normals/uvs
points = wp.array(points, dtype=wp.vec3, device=self.device)
indices = wp.array(indices, dtype=wp.int32, device=self.device)
normals = None
uvs = None
texture = None
if geo_src._normals is not None:
normals = wp.array(geo_src._normals, dtype=wp.vec3, device=self.device)
if geo_src._uvs is not None:
uvs = wp.array(geo_src._uvs, dtype=wp.vec2, device=self.device)
if hasattr(geo_src, "texture"):
texture = geo_src.texture
self.log_mesh(
name,
points,
indices,
normals,
uvs,
hidden=hidden,
texture=texture,
)
return
# Generate vertices/indices for supported primitive types
if geo_type == newton.GeoType.PLANE:
# Handle "infinite" planes encoded with non-positive scales
width = geo_scale[0] if geo_scale and geo_scale[0] > 0.0 else 1000.0
length = geo_scale[1] if len(geo_scale) > 1 and geo_scale[1] > 0.0 else 1000.0
mesh = newton.Mesh.create_plane(width, length, compute_inertia=False)
elif geo_type == newton.GeoType.SPHERE:
radius = geo_scale[0]
mesh = newton.Mesh.create_sphere(radius, compute_inertia=False)
elif geo_type == newton.GeoType.CAPSULE:
radius, half_height = geo_scale[:2]
mesh = newton.Mesh.create_capsule(radius, half_height, up_axis=newton.Axis.Z, compute_inertia=False)
elif geo_type == newton.GeoType.CYLINDER:
radius, half_height = geo_scale[:2]
mesh = newton.Mesh.create_cylinder(radius, half_height, up_axis=newton.Axis.Z, compute_inertia=False)
elif geo_type == newton.GeoType.CONE:
radius, half_height = geo_scale[:2]
mesh = newton.Mesh.create_cone(radius, half_height, up_axis=newton.Axis.Z, compute_inertia=False)
elif geo_type == newton.GeoType.BOX:
if len(geo_scale) == 1:
ext = (geo_scale[0],) * 3
else:
ext = tuple(geo_scale[:3])
mesh = newton.Mesh.create_box(ext[0], ext[1], ext[2], duplicate_vertices=True, compute_inertia=False)
elif geo_type == newton.GeoType.ELLIPSOID:
# geo_scale contains (rx, ry, rz) semi-axes
rx = geo_scale[0] if len(geo_scale) > 0 else 1.0
ry = geo_scale[1] if len(geo_scale) > 1 else rx
rz = geo_scale[2] if len(geo_scale) > 2 else rx
mesh = newton.Mesh.create_ellipsoid(rx, ry, rz, compute_inertia=False)
else:
raise ValueError(f"log_geo does not support geo_type={geo_type} (name={name})")
# Convert to Warp arrays and forward to log_mesh
points = wp.array(mesh.vertices, dtype=wp.vec3, device=self.device)
normals = wp.array(mesh.normals, dtype=wp.vec3, device=self.device)
uvs = wp.array(mesh.uvs, dtype=wp.vec2, device=self.device)
indices = wp.array(mesh.indices, dtype=wp.int32, device=self.device)
self.log_mesh(name, points, indices, normals, uvs, hidden=hidden, texture=None)
def log_gizmo(
self,
name: str,
transform: wp.transform,
*,
translate: Sequence[Axis] | None = None,
rotate: Sequence[Axis] | None = None,
snap_to: wp.transform | None = None,
):
"""Log a gizmo GUI element for the given name and transform.
Args:
name: The name of the gizmo.
transform: The transform of the gizmo.
translate: Axes on which the translation handles are shown.
Defaults to all axes when ``None``. Pass an empty sequence
to hide all translation handles.
rotate: Axes on which the rotation rings are shown.
Defaults to all axes when ``None``. Pass an empty sequence
to hide all rotation rings.
snap_to: Optional world transform to snap to when this gizmo is
released by the user.
"""
return
@abstractmethod
def log_mesh(
self,
name: str,
points: wp.array[wp.vec3],
indices: wp.array[wp.int32] | wp.array[wp.uint32],
normals: wp.array[wp.vec3] | None = None,
uvs: wp.array[wp.vec2] | None = None,
texture: np.ndarray | str | None = None,
hidden: bool = False,
backface_culling: bool = True,
):
"""
Register or update a mesh prototype in the viewer backend.
Args:
name: Unique path/name for the mesh asset.
points: Vertex positions as a Warp vec3 array.
indices: Triangle index buffer as a Warp integer array.
normals: Optional vertex normals as a Warp vec3 array.
uvs: Optional texture coordinates as a Warp vec2 array.
texture: Optional texture image array or path.
hidden: Whether the mesh should be hidden.
backface_culling: Whether back-face culling should be enabled.
"""
pass
@abstractmethod
def log_instances(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""
Log a batch of mesh instances.
Args:
name: Unique path/name for the instance batch.
mesh: Path/name of a mesh previously registered via :meth:`log_mesh`.
xforms: Optional per-instance transforms as a Warp transform array.
scales: Optional per-instance scales as a Warp vec3 array.
colors: Optional per-instance colors as a Warp vec3 array.
materials: Optional per-instance material parameters as a Warp vec4 array.
hidden: Whether the instance batch should be hidden.
"""
pass
def log_capsules(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""
Log capsules as instances. This is a specialized path for rendering capsules.
If the viewer backend does not specialize this path, it will fall back to
:meth:`log_instances`.
Args:
name: Unique path/name for the capsule batch.
mesh: Path/name of a mesh previously registered via
:meth:`log_mesh`.
xforms: Optional per-capsule transforms as a Warp transform array.
scales: Optional per-capsule scales as a Warp vec3 array.
colors: Optional per-capsule colors as a Warp vec3 array.
materials: Optional per-capsule material parameters as a Warp vec4 array.
hidden: Whether the capsule batch should be hidden.
"""
self.log_instances(name, mesh, xforms, scales, colors, materials, hidden=hidden)
@abstractmethod
def log_lines(
self,
name: str,
starts: wp.array[wp.vec3] | None,
ends: wp.array[wp.vec3] | None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None),
width: float = 0.01,
hidden: bool = False,
):
"""
Log line segments for rendering.
Args:
name: Unique path/name for the line batch.
starts: Optional line start points as a Warp vec3 array.
ends: Optional line end points as a Warp vec3 array.
colors: Per-line colors as a Warp array, or a single RGB triplet.
width: Line width in rendered scene units.
hidden: Whether the line batch should be hidden.
"""
pass
def log_wireframe_shape( # noqa: B027
self,
name: str,
vertex_data: np.ndarray | None,
world_matrix: np.ndarray | None,
hidden: bool = False,
):
"""Log a wireframe shape for rendering via the geometry-shader line pipeline.
Args:
name: Unique path/name for the wireframe shape.
vertex_data: ``(N, 6)`` float32 array of interleaved ``[px,py,pz, cr,cg,cb]``
line-segment vertices (pairs). Pass ``None`` to keep existing
geometry and only update the transform.
world_matrix: 4x4 float32 model-to-world matrix, or ``None`` to
keep the current matrix.
hidden: Whether the wireframe shape should be hidden.
"""
pass
def clear_wireframe_vbo_cache(self): # noqa: B027
"""Clear the shared wireframe VBO cache (overridden by GL viewer)."""
pass
@abstractmethod
def log_points(
self,
name: str,
points: wp.array[wp.vec3] | None,
radii: wp.array[wp.float32] | float | None = None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None) = None,
hidden: bool = False,
):
"""
Log a point cloud for rendering.
Args:
name: Unique path/name for the point batch.
points: Optional point positions as a Warp vec3 array.
radii: Optional per-point radii array or a single radius value.
colors: Optional per-point colors or a single RGB triplet.
hidden: Whether the points should be hidden.
"""
pass
def log_gaussian(
self,
name: str,
gaussian: newton.Gaussian,
xform: wp.transformf | None = None,
hidden: bool = False,
):
"""
Log a :class:`newton.Gaussian` splat asset as a point cloud of spheres.
Each Gaussian is rendered as a sphere positioned at its center, with
radius equal to the largest per-axis scale and color derived from the
DC spherical-harmonics coefficients.
The default implementation is a no-op. Override in viewer backends
that support point-cloud rendering.
Args:
name: Unique path/name for the Gaussian point cloud.
gaussian: The :class:`newton.Gaussian` asset to visualize.
xform: Optional world-space transform applied to all splat centers.
hidden: Whether the point cloud should be hidden.
"""
return
@abstractmethod
def log_array(self, name: str, array: wp.array[Any] | np.ndarray):
"""
Log a numeric array for backend-specific visualization utilities.
Args:
name: Unique path/name for the array signal.
array: Array data as a Warp array or NumPy array.
"""
pass
@abstractmethod
def log_scalar(self, name: str, value: int | float | bool | np.number):
"""
Log a scalar signal for backend-specific visualization utilities.
Args:
name: Unique path/name for the scalar signal.
value: Scalar value to record.
"""
pass
@abstractmethod
def apply_forces(self, state: newton.State):
"""
Apply forces to the state from picking and wind (if available).
Args:
state: The current state of the simulation.
"""
pass
@abstractmethod
def close(self):
"""
Close the viewer.
"""
pass
# handles a batch of mesh instances attached to bodies in the Newton Model
class ShapeInstances:
"""
A batch of shape instances.
"""
def __init__(self, name: str, static: bool, flags: int, mesh: str, device: wp.Device):
"""
Initialize the ShapeInstances.
"""
self.name = name
self.static = static
self.flags = flags
self.mesh = mesh
self.device = device
# Optional geometry type for specialized rendering paths (e.g., capsules).
# -1 means "unknown / not set".
self.geo_type = -1
self.parents = []
self.xforms = []
self.scales = []
self.colors = []
"""Color (vec3f) per instance."""
self.materials = []
self.worlds = [] # World index for each shape
self.model_shapes = []
self.world_xforms = None
self.colors_changed: bool = False
"""Indicates that finalized
``ShapeInstances.colors`` changed and
should be included in
:meth:`~newton.viewer.ViewerBase.log_instances`.
"""
def add(
self,
parent: int,
xform: wp.transform,
scale: wp.vec3,
color: wp.vec3,
material: wp.vec4,
shape_index: int,
world: int = -1,
):
"""
Add an instance of the geometry to the batch.
Args:
parent: The parent body index.
xform: The transform of the instance.
scale: The scale of the instance.
color: The color of the instance.
material: The material of the instance.
shape_index: The shape index.
world: The world index.
"""
self.parents.append(parent)
self.xforms.append(xform)
self.scales.append(scale)
self.colors.append(color)
self.materials.append(material)
self.worlds.append(world)
self.model_shapes.append(shape_index)
def finalize(self, shape_colors: wp.array[wp.vec3] | None = None):
"""
Allocates the batch of shape instances as Warp arrays.
Args:
shape_colors: The colors of the shapes.
"""
self.parents = wp.array(self.parents, dtype=int, device=self.device)
self.xforms = wp.array(self.xforms, dtype=wp.transform, device=self.device)
self.scales = wp.array(self.scales, dtype=wp.vec3, device=self.device)
if shape_colors is not None:
assert len(shape_colors) == len(self.scales), "shape_colors length mismatch"
self.colors = shape_colors
else:
self.colors = wp.array(self.colors, dtype=wp.vec3, device=self.device)
self.materials = wp.array(self.materials, dtype=wp.vec4, device=self.device)
self.worlds = wp.array(self.worlds, dtype=int, device=self.device)
self.world_xforms = wp.zeros_like(self.xforms)
def update(self, state: newton.State, world_offsets: wp.array[wp.vec3]):
"""
Update the world transforms of the shape instances.
Args:
state: The current state of the simulation.
world_offsets: The world offsets.
"""
from .kernels import update_shape_xforms # noqa: PLC0415
wp.launch(
kernel=update_shape_xforms,
dim=len(self.xforms),
inputs=[
self.xforms,
self.parents,
state.body_q,
self.worlds,
world_offsets,
],
outputs=[self.world_xforms],
device=self.device,
)
# returns a unique (non-stable) identifier for a geometry configuration
def _hash_geometry(self, geo_type: int, geo_scale, thickness: float, is_solid: bool, geo_src=None) -> int:
return hash((int(geo_type), geo_src, *geo_scale, float(thickness), bool(is_solid)))
def _hash_shape(self, geo_hash, shape_static, shape_flags) -> int:
return hash((geo_hash, shape_static, shape_flags))
def _should_show_shape(self, flags: int, is_static: bool) -> bool:
"""Determine if a shape should be visible based on current settings."""
has_collide_flag = bool(flags & int(newton.ShapeFlags.COLLIDE_SHAPES))
has_visible_flag = bool(flags & int(newton.ShapeFlags.VISIBLE))
# Static shapes override (e.g., for debugging)
if is_static and self.show_static:
return True
# Shapes can be both collision AND visual (e.g., ground plane).
# Show if either relevant toggle is enabled.
if has_collide_flag and self.show_collision:
return True
if has_visible_flag and self.show_visual:
return True
# Hide if shape has no enabled flags
return False
def _populate_geometry(
self,
geo_type: int,
geo_scale,
thickness: float,
is_solid: bool,
geo_src=None,
) -> str:
"""Ensure a geometry mesh exists and return its mesh path.
Computes a stable hash from the parameters; creates and caches the mesh path if needed.
"""
# normalize
if isinstance(geo_scale, list | tuple | np.ndarray):
scale_list = [float(v) for v in geo_scale]
else:
scale_list = [float(geo_scale)]
# include geo_src in hash to match model-driven batching
geo_hash = self._hash_geometry(
int(geo_type),
tuple(scale_list),
float(thickness),
bool(is_solid),
geo_src,
)
if geo_hash in self._geometry_cache:
return self._geometry_cache[geo_hash]
base_name = {
newton.GeoType.PLANE: "plane",
newton.GeoType.SPHERE: "sphere",
newton.GeoType.CAPSULE: "capsule",
newton.GeoType.CYLINDER: "cylinder",
newton.GeoType.CONE: "cone",
newton.GeoType.BOX: "box",
newton.GeoType.ELLIPSOID: "ellipsoid",
newton.GeoType.MESH: "mesh",
newton.GeoType.CONVEX_MESH: "convex_hull",
newton.GeoType.HFIELD: "heightfield",
}.get(geo_type)
if base_name is None:
raise ValueError(f"Unsupported geo_type for ensure_geometry: {geo_type}")
mesh_path = f"/geometry/{base_name}_{len(self._geometry_cache)}"
self.log_geo(
mesh_path,
int(geo_type),
tuple(scale_list),
float(thickness),
bool(is_solid),
geo_src=geo_src
if geo_type in (newton.GeoType.MESH, newton.GeoType.CONVEX_MESH, newton.GeoType.HFIELD)
else None,
hidden=True,
)
self._geometry_cache[geo_hash] = mesh_path
return mesh_path
# creates meshes and instances for each shape in the Model
def _populate_shapes(self):
# convert to NumPy
shape_body = self.model.shape_body.numpy()
shape_geo_src = self.model.shape_source
shape_geo_type = self.model.shape_type.numpy()
shape_geo_scale = self.model.shape_scale.numpy()
shape_geo_thickness = self.model.shape_margin.numpy()
shape_geo_is_solid = self.model.shape_is_solid.numpy()
shape_transform = self.model.shape_transform.numpy()
shape_flags = self.model.shape_flags.numpy()
shape_world = self.model.shape_world.numpy()
shape_display_color = self.model.shape_color.numpy() if self.model.shape_color is not None else None
shape_sdf_index = self._shape_sdf_index_host
shape_count = len(shape_body)
# loop over shapes
for s in range(shape_count):
# skip shapes from worlds beyond max_worlds limit
if not self._should_render_world(shape_world[s]):
continue
geo_type = shape_geo_type[s]
geo_scale = [float(v) for v in shape_geo_scale[s]]
geo_thickness = float(shape_geo_thickness[s])
geo_is_solid = bool(shape_geo_is_solid[s])
geo_src = shape_geo_src[s]
# Gaussians bypass the mesh instancing pipeline; render as point clouds.
if geo_type == newton.GeoType.GAUSSIAN:
if isinstance(geo_src, newton.Gaussian):
parent = shape_body[s]
xform = wp.transform_expand(shape_transform[s])
gname = f"/model/gaussians/gaussian_{len(self._gaussian_instances)}"
self._gaussian_instances.append(
(gname, geo_src, int(parent), xform, int(shape_world[s]), int(shape_flags[s]), parent == -1)
)
continue
# check whether we can instance an already created shape with the same geometry
geo_hash = self._hash_geometry(
int(geo_type),
tuple(geo_scale),
float(geo_thickness),
bool(geo_is_solid),
geo_src,
)
# ensure geometry exists and get mesh path
if geo_hash not in self._geometry_cache:
mesh_name = self._populate_geometry(
int(geo_type),
tuple(geo_scale),
float(geo_thickness),
bool(geo_is_solid),
geo_src=geo_src
if geo_type
in (
newton.GeoType.MESH,
newton.GeoType.CONVEX_MESH,
newton.GeoType.HFIELD,
)
else None,
)
else:
mesh_name = self._geometry_cache[geo_hash]
# shape options
flags = shape_flags[s]
parent = shape_body[s]
static = parent == -1
# For collision shapes that ALSO have the VISIBLE flag AND have SDF volumes,
# treat the original mesh as visual geometry (the SDF isomesh will be rendered
# separately for collision visualization).
#
# Shapes that only have COLLIDE_SHAPES (no VISIBLE) should remain as collision
# shapes - these are typically convex hull approximations where a separate
# visual-only copy exists.
is_collision_shape = flags & int(newton.ShapeFlags.COLLIDE_SHAPES)
is_visible = flags & int(newton.ShapeFlags.VISIBLE)
# Check for texture SDF existence without computing the isomesh (lazy evaluation)
sdf_idx = int(shape_sdf_index[s]) if shape_sdf_index is not None else -1
has_sdf = sdf_idx >= 0 and self.model.texture_sdf_data is not None
if is_collision_shape and is_visible and has_sdf:
# Remove COLLIDE_SHAPES flag so this is treated as a visual shape
flags = flags & ~int(newton.ShapeFlags.COLLIDE_SHAPES)
shape_hash = self._hash_shape(geo_hash, static, flags)
# ensure batch exists
if shape_hash not in self._shape_instances:
shape_name = f"/model/shapes/shape_{len(self._shape_instances)}"
batch = ViewerBase.ShapeInstances(shape_name, static, flags, mesh_name, self.device)
batch.geo_type = geo_type
self._shape_instances[shape_hash] = batch
else:
batch = self._shape_instances[shape_hash]
xform = wp.transform_expand(shape_transform[s])
scale = np.array([1.0, 1.0, 1.0])
if shape_display_color is not None:
color = wp.vec3(shape_display_color[s])
elif (shape_flags[s] & int(newton.ShapeFlags.COLLIDE_SHAPES)) == 0:
color = wp.vec3(0.5, 0.5, 0.5)
else:
# Use shape index for color to ensure each collision shape has a different color
color = wp.vec3(self._shape_color_map(s))
material = wp.vec4(0.5, 0.0, 0.0, 0.0) # roughness, metallic, checker, texture_enable
if geo_type in (newton.GeoType.MESH, newton.GeoType.CONVEX_MESH):
scale = np.asarray(geo_scale, dtype=np.float32)
if shape_display_color is None and geo_src.color is not None:
color = wp.vec3(geo_src.color[0:3])
if getattr(geo_src, "roughness", None) is not None:
material = wp.vec4(float(geo_src.roughness), material.y, material.z, material.w)
if getattr(geo_src, "metallic", None) is not None:
material = wp.vec4(material.x, float(geo_src.metallic), material.z, material.w)
if geo_src is not None and geo_src._uvs is not None:
has_texture = getattr(geo_src, "texture", None) is not None
if has_texture:
material = wp.vec4(material.x, material.y, material.z, 1.0)
# Planes keep their checkerboard material even when model.shape_color
# is populated with resolved default colors.
if geo_type == newton.GeoType.PLANE:
if shape_display_color is None:
color = wp.vec3(0.125, 0.125, 0.15)
material = wp.vec4(0.5, 0.0, 1.0, 0.0)
# add render instance
batch.add(
parent=parent,
xform=xform,
scale=scale,
color=color,
material=material,
shape_index=s,
world=shape_world[s],
)
# each shape instance object (batch) is associated with one slice
batches = list(self._shape_instances.values())
offsets = np.cumsum(np.array([0, *[len(b.scales) for b in batches]], dtype=np.int32)).tolist()
total_instances = int(offsets[-1])
# Allocate single contiguous color buffer and copy initial per-batch colors
if total_instances:
self.model_shape_color = wp.zeros(total_instances, dtype=wp.vec3, device=self.device)
for b_idx, batch in enumerate(batches):
if total_instances:
color_array = self.model_shape_color[offsets[b_idx] : offsets[b_idx + 1]]
color_array.assign(wp.array(batch.colors, dtype=wp.vec3, device=self.device))
batch.finalize(shape_colors=color_array)
else:
batch.finalize()
shape_to_slot = np.full(shape_count, -1, dtype=np.int32)
for b_idx, batch in enumerate(batches):
start = offsets[b_idx]
for local_idx, s_idx in enumerate(batch.model_shapes):
shape_to_slot[s_idx] = start + local_idx
self._shape_to_slot = shape_to_slot
slot_to_shape = np.empty(total_instances, dtype=np.int32)
for s_idx, slot in enumerate(shape_to_slot):
if slot >= 0:
slot_to_shape[slot] = s_idx
self._slot_to_shape = slot_to_shape
self._slot_to_shape_wp = (
wp.array(slot_to_shape, dtype=wp.int32, device=self.device) if total_instances else None
)
# Build shape -> batch reference mapping for change signalling
shape_to_batch: list[ViewerBase.ShapeInstances | None] = [None] * shape_count
for batch in batches:
for s_idx in batch.model_shapes:
shape_to_batch[s_idx] = batch
self._shape_to_batch = shape_to_batch
# Note: SDF isomesh instances are populated lazily when show_collision is True
# to avoid GPU memory allocation until actually needed for visualization
def _populate_sdf_isomesh_instances(self):
"""Create shape instances for SDF isomeshes (marching cubes visualization).
These are rendered separately based on the show_collision flag to allow
independent control of visual mesh and SDF collision visualization.
"""
if self.model is None:
return
shape_body = self.model.shape_body.numpy()
shape_transform = self.model.shape_transform.numpy()
shape_flags = self.model.shape_flags.numpy()
shape_world = self.model.shape_world.numpy()
shape_geo_scale = self.model.shape_scale.numpy()
tex_sdf_np = self.model.texture_sdf_data.numpy() if self.model.texture_sdf_data is not None else None
shape_sdf_index = self._shape_sdf_index_host
shape_count = len(shape_body)
for s in range(shape_count):
# skip shapes from worlds beyond max_worlds limit
if not self._should_render_world(shape_world[s]):
continue
# Only process collision shapes with texture SDFs
is_collision_shape = shape_flags[s] & int(newton.ShapeFlags.COLLIDE_SHAPES)
if not is_collision_shape:
continue
isomesh = self._get_shape_isomesh(s)
if isomesh is None:
continue
sdf_idx = int(shape_sdf_index[s]) if shape_sdf_index is not None else -1
scale_baked = (
bool(tex_sdf_np[sdf_idx]["scale_baked"]) if (tex_sdf_np is not None and sdf_idx >= 0) else True
)
# Create isomesh geometry (always use (1,1,1) for geometry since isomesh is in SDF space)
geo_type = newton.GeoType.MESH
geo_scale = (1.0, 1.0, 1.0)
geo_thickness = 0.0
geo_is_solid = True
geo_hash = self._hash_geometry(
int(geo_type),
geo_scale,
geo_thickness,
geo_is_solid,
isomesh,
)
# Ensure geometry exists and get mesh path
if geo_hash not in self._geometry_cache:
mesh_name = self._populate_geometry(
int(geo_type),
geo_scale,
geo_thickness,
geo_is_solid,
geo_src=isomesh,
)
else:
mesh_name = self._geometry_cache[geo_hash]
# Shape options
flags = shape_flags[s]
parent = shape_body[s]
static = parent == -1
# Use the geo_hash as the batch key for SDF isomesh instances
if geo_hash not in self._sdf_isomesh_instances:
shape_name = f"/model/sdf_isomesh/isomesh_{len(self._sdf_isomesh_instances)}"
batch = ViewerBase.ShapeInstances(shape_name, static, flags, mesh_name, self.device)
batch.geo_type = geo_type
self._sdf_isomesh_instances[geo_hash] = batch
else:
batch = self._sdf_isomesh_instances[geo_hash]
xform = wp.transform_expand(shape_transform[s])
# Apply shape scale if not baked into SDF, otherwise use (1,1,1)
if scale_baked:
scale = np.array([1.0, 1.0, 1.0])
else:
scale = np.asarray(shape_geo_scale[s], dtype=np.float32)
# Use distinct collision color palette (different from visual shapes)
color = wp.vec3(self._collision_color_map(s))
material = wp.vec4(0.3, 0.0, 0.0, 0.0) # roughness, metallic, checker, texture_enable
batch.add(
parent=parent,
xform=xform,
scale=scale,
color=color,
material=material,
shape_index=s,
world=shape_world[s],
)
# Finalize all SDF isomesh batches
for batch in self._sdf_isomesh_instances.values():
batch.finalize()
def update_shape_colors(self, shape_colors: dict[int, wp.vec3 | tuple[float, float, float]]):
"""
Set colors for a set of shapes at runtime.
Args:
shape_colors: mapping from shape index -> color
"""
warnings.warn(
"Viewer.update_shape_colors() is deprecated. Write to model.shape_color instead.",
category=DeprecationWarning,
stacklevel=2,
)
if self._shape_to_slot is None or self._shape_to_batch is None:
return
for s_idx, col in shape_colors.items():
if s_idx < 0 or s_idx >= len(self._shape_to_slot):
raise ValueError(f"Shape index {s_idx} out of bounds")
if self.model is not None and self.model.shape_color is not None:
self.model.shape_color[s_idx : s_idx + 1].fill_(wp.vec3(col))
slot = int(self._shape_to_slot[s_idx])
if slot < 0:
continue
if self.model_shape_color is not None:
self.model_shape_color[slot : slot + 1].fill_(wp.vec3(col))
batch_ref = self._shape_to_batch[s_idx]
if batch_ref is not None:
batch_ref.colors_changed = True
def _log_inertia_boxes(self, state: newton.State):
"""Render inertia boxes as wireframe lines."""
if not self.show_inertia_boxes:
self.log_lines("/model/inertia_boxes", None, None, None)
return
body_count = self.model.body_count
if body_count == 0:
return
# 12 edges per body
num_lines = body_count * 12
if self._inertia_box_points0 is None or len(self._inertia_box_points0) < num_lines:
self._inertia_box_points0 = wp.zeros(num_lines, dtype=wp.vec3, device=self.device)
self._inertia_box_points1 = wp.zeros(num_lines, dtype=wp.vec3, device=self.device)
self._inertia_box_colors = wp.zeros(num_lines, dtype=wp.vec3, device=self.device)
from .kernels import compute_inertia_box_lines # noqa: PLC0415
wp.launch(
kernel=compute_inertia_box_lines,
dim=num_lines,
inputs=[
state.body_q,
self.model.body_com,
self.model.body_inertia,
self.model.body_inv_mass,
self.model.body_world,
self.world_offsets,
self.max_worlds if self.max_worlds is not None else -1,
wp.vec3(0.5, 0.5, 0.5), # color
],
outputs=[
self._inertia_box_points0,
self._inertia_box_points1,
self._inertia_box_colors,
],
device=self.device,
)
self.log_lines(
"/model/inertia_boxes", self._inertia_box_points0, self._inertia_box_points1, self._inertia_box_colors
)
def _compute_shape_offset_mesh(
self,
shape_idx: int,
mode: ViewerBase.SDFMarginMode,
margin_np: np.ndarray,
gap_np: np.ndarray,
type_np: np.ndarray,
scale_np: np.ndarray,
) -> newton.Mesh | None:
"""Compute the offset isosurface mesh for a collision shape.
Args:
shape_idx: Index of the shape in the model.
mode: Which offset to use (MARGIN or MARGIN_GAP).
margin_np: Pre-snapshotted ``shape_margin`` host array.
gap_np: Pre-snapshotted ``shape_gap`` host array.
type_np: Pre-snapshotted ``shape_type`` host array.
scale_np: Pre-snapshotted ``shape_scale`` host array.
Returns:
Mesh for the offset surface, or ``None`` if unavailable.
"""
if self.model is None or mode == self.SDFMarginMode.OFF:
return None
shape_margin_val = float(margin_np[shape_idx])
if mode == self.SDFMarginMode.MARGIN:
offset = shape_margin_val
else:
offset = shape_margin_val + float(gap_np[shape_idx])
if offset < 0.0:
return None
geo_type = int(type_np[shape_idx])
geo_scale = [float(v) for v in scale_np[shape_idx]]
geo_src = self.model.shape_source[shape_idx]
# Replicated meshes share the same SDF object via Mesh.__deepcopy__,
# so keying on id(sdf) deduplicates across worlds.
geo_identity = id(getattr(geo_src, "sdf", None) or geo_src) if geo_src is not None else 0
cache_key = (geo_type, tuple(geo_scale), geo_identity, offset)
if cache_key in self._sdf_margin_mesh_cache:
return self._sdf_margin_mesh_cache[cache_key]
from ..geometry.sdf_utils import compute_offset_mesh # noqa: PLC0415
mesh = compute_offset_mesh(
shape_type=geo_type,
shape_geo=geo_src if geo_type in (newton.GeoType.MESH, newton.GeoType.CONVEX_MESH) else None,
shape_scale=geo_scale,
offset=offset,
device=self.device,
)
self._sdf_margin_mesh_cache[cache_key] = mesh
return mesh
@staticmethod
def _extract_wireframe_edges(mesh: newton.Mesh, color: tuple[float, float, float]) -> np.ndarray:
"""Extract deduplicated edges from a mesh and return interleaved vertex data.
Args:
mesh: Source mesh.
color: RGB colour tuple applied to every vertex.
Returns:
``(E*2, 6)`` float32 array — pairs of ``[px, py, pz, cr, cg, cb]``.
"""
verts = np.asarray(mesh.vertices, dtype=np.float32).reshape(-1, 3)
indices = np.asarray(mesh.indices, dtype=np.int32).reshape(-1, 3)
edge_set: set[tuple[int, int]] = set()
for tri in indices:
i0, i1, i2 = int(tri[0]), int(tri[1]), int(tri[2])
edge_set.add((min(i0, i1), max(i0, i1)))
edge_set.add((min(i1, i2), max(i1, i2)))
edge_set.add((min(i2, i0), max(i2, i0)))
num_edges = len(edge_set)
data = np.empty((num_edges * 2, 6), dtype=np.float32)
cr, cg, cb = color
idx = 0
for a, b in edge_set:
pa = verts[a]
pb = verts[b]
data[idx] = [pa[0], pa[1], pa[2], cr, cg, cb]
data[idx + 1] = [pb[0], pb[1], pb[2], cr, cg, cb]
idx += 2
return data
def _populate_sdf_margin_edges(
self,
mode: ViewerBase.SDFMarginMode,
target: dict[int, tuple[np.ndarray, int, np.ndarray, int]],
):
"""Compute offset meshes and extract wireframe edge data for every collision shape.
Results are written into *target* (keyed by shape index).
"""
if self.model is None:
return
if mode == self.SDFMarginMode.MARGIN:
color_rgb = (1.0, 0.9, 0.0)
else:
color_rgb = (1.0, 0.5, 0.0)
shape_body = self.model.shape_body.numpy()
shape_flags = self.model.shape_flags.numpy()
shape_world = self.model.shape_world.numpy()
shape_transform = self.model.shape_transform.numpy()
margin_np = self.model.shape_margin.numpy()
gap_np = self.model.shape_gap.numpy()
type_np = self.model.shape_type.numpy()
scale_np = self.model.shape_scale.numpy()
shape_count = len(shape_body)
for s in range(shape_count):
if not self._should_render_world(shape_world[s]):
continue
if not (shape_flags[s] & int(newton.ShapeFlags.COLLIDE_SHAPES)):
continue
offset_mesh = self._compute_shape_offset_mesh(s, mode, margin_np, gap_np, type_np, scale_np)
if offset_mesh is None:
continue
vd_key = (id(offset_mesh), color_rgb)
vertex_data = self._sdf_margin_vdata_cache.get(vd_key)
if vertex_data is None:
vertex_data = self._extract_wireframe_edges(offset_mesh, color_rgb)
self._sdf_margin_vdata_cache[vd_key] = vertex_data
body_idx = int(shape_body[s])
world_idx = int(shape_world[s])
shape_xf = shape_transform[s].copy()
target[s] = (vertex_data, body_idx, shape_xf, world_idx)
@staticmethod
def _transform_to_mat44(tf: np.ndarray) -> np.ndarray:
"""Convert a 7-element Warp transform ``[tx,ty,tz, qx,qy,qz,qw]`` to a flat column-major 4x4 matrix.
Returns a shape ``(16,)`` float32 array laid out column-by-column
(OpenGL convention), matching the format used by pyglet ``Mat4``.
"""
px, py, pz = float(tf[0]), float(tf[1]), float(tf[2])
qx, qy, qz, qw = float(tf[3]), float(tf[4]), float(tf[5]), float(tf[6])
x2, y2, z2 = 2 * qx * qx, 2 * qy * qy, 2 * qz * qz
xy, xz, yz = 2 * qx * qy, 2 * qx * qz, 2 * qy * qz
wx, wy, wz = 2 * qw * qx, 2 * qw * qy, 2 * qw * qz
# fmt: off
return np.array([
1 - y2 - z2, xy + wz, xz - wy, 0, # column 0
xy - wz, 1 - x2 - z2, yz + wx, 0, # column 1
xz + wy, yz - wx, 1 - x2 - y2, 0, # column 2
px, py, pz, 1, # column 3
], dtype=np.float32)
# fmt: on
def _log_sdf_margin_wireframes(self, state: newton.State):
"""Update and render SDF margin wireframe edges."""
mode = self.sdf_margin_mode
visible = mode != self.SDFMarginMode.OFF
if self.model_changed:
self._sdf_margin_edge_caches.clear()
self._sdf_margin_mesh_cache.clear()
self._sdf_margin_vdata_cache.clear()
self.clear_wireframe_vbo_cache()
if visible:
edge_cache = self._sdf_margin_edge_caches.get(mode)
if edge_cache is None:
edge_cache = {}
self._populate_sdf_margin_edges(mode, edge_cache)
self._sdf_margin_edge_caches[mode] = edge_cache
identity = np.eye(4, dtype=np.float32).ravel(order="F")
for s, (vertex_data, _body_idx, _shape_xf, _world_idx) in edge_cache.items():
name = f"/model/sdf_margin_wf/{mode.value}/{s}"
self.log_wireframe_shape(name, vertex_data, identity, hidden=False)
# Hide inactive modes, show active mode
for cached_mode, cached_edges in self._sdf_margin_edge_caches.items():
hidden = not visible or cached_mode != mode
for s in cached_edges:
name = f"/model/sdf_margin_wf/{cached_mode.value}/{s}"
self.log_wireframe_shape(name, None, None, hidden=hidden)
if not visible:
return
# Update world transforms for the active mode
body_q = state.body_q.numpy() if state is not None else None
offsets_np = self.world_offsets.numpy() if self.world_offsets is not None else None
for s, (_vertex_data, body_idx, shape_xf, world_idx) in edge_cache.items():
name = f"/model/sdf_margin_wf/{mode.value}/{s}"
shape_mat = self._transform_to_mat44(shape_xf)
if body_idx >= 0 and body_q is not None:
body_mat = self._transform_to_mat44(body_q[body_idx])
bm = body_mat.reshape(4, 4, order="F")
sm = shape_mat.reshape(4, 4, order="F")
world_mat = (bm @ sm).ravel(order="F")
else:
world_mat = shape_mat.copy()
if offsets_np is not None and world_idx >= 0:
world_mat[12] += offsets_np[world_idx][0]
world_mat[13] += offsets_np[world_idx][1]
world_mat[14] += offsets_np[world_idx][2]
self.log_wireframe_shape(name, None, world_mat, hidden=False)
def _log_joints(self, state: newton.State):
"""
Creates line segments for joint basis vectors for rendering.
Args:
state: Current simulation state
"""
if not self.show_joints:
# Pass None to hide joints - renderer will handle creating empty arrays
self.log_lines("/model/joints", None, None, None)
return
# Get the number of joints
num_joints = len(self.model.joint_type)
if num_joints == 0:
return
# Each joint produces 3 lines (x, y, z axes)
max_lines = num_joints * 3
# Ensure we have buffers for joint line endpoints
if self._joint_points0 is None or len(self._joint_points0) < max_lines:
self._joint_points0 = wp.zeros(max_lines, dtype=wp.vec3, device=self.device)
self._joint_points1 = wp.zeros(max_lines, dtype=wp.vec3, device=self.device)
self._joint_colors = wp.zeros(max_lines, dtype=wp.vec3, device=self.device)
# Run the kernel to compute joint basis lines
# Launch with 3 * num_joints threads (3 lines per joint)
from .kernels import compute_joint_basis_lines # noqa: PLC0415
wp.launch(
kernel=compute_joint_basis_lines,
dim=max_lines,
inputs=[
self.model.joint_type,
self.model.joint_parent,
self.model.joint_child,
self.model.joint_X_p,
state.body_q,
self.model.body_world,
self.world_offsets,
self.model.shape_collision_radius,
self.model.shape_body,
0.1, # line scale factor
],
outputs=[
self._joint_points0,
self._joint_points1,
self._joint_colors,
],
device=self.device,
)
# Log all joint lines in a single call
self.log_lines("/model/joints", self._joint_points0, self._joint_points1, self._joint_colors)
def _log_com(self, state: newton.State):
num_bodies = self.model.body_count
if num_bodies == 0:
return
if self._com_positions is None or len(self._com_positions) < num_bodies:
self._com_positions = wp.zeros(num_bodies, dtype=wp.vec3, device=self.device)
self._com_colors = wp.full(num_bodies, wp.vec3(1.0, 0.8, 0.0), device=self.device)
self._com_radii = wp.full(num_bodies, 0.05, dtype=float, device=self.device)
from .kernels import compute_com_positions # noqa: PLC0415
wp.launch(
kernel=compute_com_positions,
dim=num_bodies,
inputs=[
state.body_q,
self.model.body_com,
self.model.body_world,
self.world_offsets,
],
outputs=[self._com_positions],
device=self.device,
)
self.log_points("/model/com", self._com_positions, self._com_radii, self._com_colors, hidden=not self.show_com)
def _log_triangles(self, state: newton.State):
if self.model.tri_count:
self.log_mesh(
"/model/triangles",
state.particle_q,
self.model.tri_indices.flatten(),
hidden=not self.show_triangles,
backface_culling=False,
)
def _log_particles(self, state: newton.State):
if self.model.particle_count:
points = state.particle_q
radii = self.model.particle_radius
# Filter out inactive particles so emitters/culled particles are not rendered.
# Uses Warp stream compaction to stay on device and avoid GPU→CPU→GPU roundtrips.
if self.model.particle_flags is not None:
n = self.model.particle_count
mask = wp.zeros(n, dtype=wp.int32, device=self.device)
wp.launch(
build_active_particle_mask, dim=n, inputs=[self.model.particle_flags, mask], device=self.device
)
offsets = wp.empty(n, dtype=wp.int32, device=self.device)
wp.utils.array_scan(mask, offsets, inclusive=False)
# Slice to transfer only the last element instead of the full array.
active_count = int(offsets[-1:].numpy()[0]) + int(mask[-1:].numpy()[0])
if active_count == 0:
self.log_points(name="/model/particles", points=None, hidden=True)
return
if active_count < n:
points_out = wp.empty(active_count, dtype=wp.vec3, device=self.device)
wp.launch(compact, dim=n, inputs=[points, mask, offsets, points_out], device=self.device)
points = points_out
if isinstance(radii, wp.array):
radii_out = wp.empty(active_count, dtype=wp.float32, device=self.device)
wp.launch(compact, dim=n, inputs=[radii, mask, offsets, radii_out], device=self.device)
radii = radii_out
if self.model_changed:
colors = wp.full(shape=len(points), value=wp.vec3(0.7, 0.6, 0.4), device=self.device)
else:
colors = None
self.log_points(
name="/model/particles",
points=points,
radii=radii,
colors=colors,
hidden=not self.show_particles,
)
@staticmethod
def _shape_color_map(i: int) -> list[float]:
color = newton.ModelBuilder._SHAPE_COLOR_PALETTE[i % len(newton.ModelBuilder._SHAPE_COLOR_PALETTE)]
return [c / 255.0 for c in color]
@staticmethod
def _collision_color_map(i: int) -> list[float]:
# Distinct palette for collision shapes (semi-transparent wireframe look)
# Uses cooler, more desaturated tones to contrast with bright visual colors
colors = [
[180, 120, 200], # lavender
[120, 180, 160], # sage
[200, 160, 120], # tan
[140, 160, 200], # steel blue
[200, 140, 160], # dusty rose
[160, 200, 140], # moss
[180, 180, 140], # khaki
[140, 180, 180], # slate
[200, 180, 200], # mauve
]
num_colors = len(colors)
return [c / 255.0 for c in colors[i % num_colors]]
def is_jupyter_notebook():
"""
Detect if we're running inside a Jupyter Notebook.
Returns:
True if running in a Jupyter Notebook, False otherwise.
"""
try:
# Check if get_ipython is defined (available in IPython environments)
shell = get_ipython().__class__.__name__
if shell == "ZMQInteractiveShell":
# This indicates a Jupyter Notebook or JupyterLab environment
return True
elif shell == "TerminalInteractiveShell":
# This indicates a standard IPython terminal
return False
else:
# Other IPython-like environments
return False
except NameError:
# get_ipython is not defined, so it's likely a standard Python script
return False
def is_sphinx_build() -> bool:
"""
Detect if we're running inside a Sphinx documentation build (via nbsphinx).
Returns:
True if running in Sphinx/nbsphinx, False if in regular Jupyter session.
"""
# Check for Newton's custom env var (set in docs/conf.py, inherited by nbsphinx subprocesses)
if os.environ.get("NEWTON_SPHINX_BUILD"):
return True
# nbsphinx sets SPHINXBUILD or we can check for sphinx in the call stack
if os.environ.get("SPHINXBUILD"):
return True
# Check if sphinx is in the module list (imported during doc build)
if "sphinx" in sys.modules or "nbsphinx" in sys.modules:
return True
# Check call stack for sphinx-related frames
try:
import traceback # noqa: PLC0415
for frame_info in traceback.extract_stack():
if "sphinx" in frame_info.filename.lower() or "nbsphinx" in frame_info.filename.lower():
return True
except Exception:
pass
return False
================================================
FILE: newton/_src/viewer/viewer_file.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import json
import os
from collections.abc import Iterable, Mapping
from pathlib import Path
from typing import Any, Generic, TypeVar
import numpy as np
import warp as wp
from warp._src import types as warp_types
from ..core.types import override
from ..geometry import Mesh
from ..sim import Model, State
from .viewer import ViewerBase
# Optional CBOR2 support
try:
import cbor2
HAS_CBOR2 = True
except ImportError:
HAS_CBOR2 = False
T = TypeVar("T")
class RingBuffer(Generic[T]):
"""
A ring buffer that behaves like a list but only keeps the last N items.
This class provides a list-like interface while maintaining a fixed capacity.
When the buffer is full, new items overwrite the oldest items.
"""
def __init__(self, capacity: int = 100):
"""
Initialize the ring buffer.
Args:
capacity: Maximum number of items to store. Default is 100.
"""
self.capacity = capacity
self._buffer: list[T] = []
self._start = 0 # Index of the oldest item
self._size = 0 # Current number of items
def append(self, item: T) -> None:
"""Add an item to the buffer."""
if self._size < self.capacity:
# Buffer not full yet, just append
self._buffer.append(item)
self._size += 1
else:
# Buffer is full, overwrite the oldest item
self._buffer[self._start] = item
self._start = (self._start + 1) % self.capacity
def __len__(self) -> int:
"""Return the number of items in the buffer."""
return self._size
def __getitem__(self, index: int) -> T:
"""Get an item by index (0 is the oldest item)."""
if not isinstance(index, int):
raise TypeError("Index must be an integer")
if not (0 <= index < self._size):
raise IndexError(f"Index {index} out of range [0, {self._size})")
# Convert logical index to physical buffer index
if self._size < self.capacity:
# Buffer not full, simple indexing
return self._buffer[index]
else:
# Buffer is full, need to account for wrap-around
physical_index = (self._start + index) % self.capacity
return self._buffer[physical_index]
def __setitem__(self, index: int, value: T) -> None:
"""Set an item by index."""
if not isinstance(index, int):
raise TypeError("Index must be an integer")
if not (0 <= index < self._size):
raise IndexError(f"Index {index} out of range [0, {self._size})")
# Convert logical index to physical buffer index
if self._size < self.capacity:
# Buffer not full, simple indexing
self._buffer[index] = value
else:
# Buffer is full, need to account for wrap-around
physical_index = (self._start + index) % self.capacity
self._buffer[physical_index] = value
def __iter__(self):
"""Iterate over items in order (oldest to newest)."""
for i in range(self._size):
yield self[i]
def clear(self) -> None:
"""Clear all items from the buffer."""
self._buffer.clear()
self._start = 0
self._size = 0
def to_list(self) -> list[T]:
"""Convert the ring buffer to a regular list."""
return [self[i] for i in range(self._size)]
def from_list(self, items: list[T]) -> None:
"""Replace buffer contents with items from a list."""
self.clear()
for item in items:
self.append(item)
class ArrayCache(Generic[T]):
"""
Cache that assigns a monotonically increasing index to each unique key and stores an object with it.
- Keys are uint64-compatible integers (use Python int).
- Values are stored alongside the assigned index.
- During serialization, repeated keys return their existing index; new keys return -1 and are added.
- During deserialization, lookups happen by index and return the associated object or raise if missing.
"""
def __init__(self):
self._key_to_entry: dict[int, tuple[int, T]] = {}
self._index_to_entry: dict[int, T] = {}
self._next_index: int = 1
def try_register_pointer_and_value(self, key: int, value: T) -> int:
"""
Register an object under a numeric key.
Args:
key: Unsigned 64-bit compatible integer key
value: Object to cache
Returns:
Existing index if the key already exists; otherwise 0 after inserting a new entry.
"""
existing_entry = self._key_to_entry.get(key, None)
if existing_entry is not None:
existing_index, _ = existing_entry
return existing_index
assigned_index = self._next_index
self._next_index += 1
self._key_to_entry[key] = (assigned_index, value)
self._index_to_entry[assigned_index] = value
return 0
def try_get_value(self, index: int) -> T:
"""
Resolve an object by its index.
Args:
index: Previously assigned index from try_register_pointer_and_value() or
try_register_pointer_and_value_and_index()
Returns:
The object associated with the given index.
"""
return self._index_to_entry[index]
def try_register_pointer_and_value_and_index(self, key: int, value: T, index: int) -> int:
"""
Register an object with an explicit, well-defined index (used during deserialization).
- If the key already exists, the stored index must equal the provided index.
Returns that index, or raises on mismatch.
- If the key is new, the provided index must not be used by another entry.
Adds the mapping and returns the index.
- Advances the internal next-index counter if necessary.
"""
existing_entry = self._key_to_entry.get(key, None)
if existing_entry is not None:
existing_index, existing_value = existing_entry
if existing_index != index:
raise ValueError(
f"ArrayCache: key already registered with a different index (have {existing_index}, got {index})"
)
return existing_index
existing_value = self._index_to_entry.get(index, None)
if existing_value is not None:
raise ValueError(f"ArrayCache: index {index} already in use for another entry")
self._key_to_entry[key] = (index, value)
self._index_to_entry[index] = value
if index >= self._next_index:
self._next_index = index + 1
return index
def get_index_for_key(self, key: int) -> int:
"""Return the assigned index for an existing key, else raise KeyError."""
existing_entry = self._key_to_entry.get(key, None)
if existing_entry is None:
raise KeyError(f"ArrayCache: key {key} not found")
return existing_entry[0]
def clear(self) -> None:
"""Remove all entries and reset the index counter."""
self._key_to_entry.clear()
self._index_to_entry.clear()
self._next_index = 1
def __len__(self) -> int:
return len(self._key_to_entry)
def _get_serialization_format(file_path: str) -> str:
"""
Determine serialization format based on file extension.
Args:
file_path: Path to the file
Returns:
'json' for .json files, 'cbor2' for .bin files
Raises:
ValueError: If file extension is not supported
"""
_, ext = os.path.splitext(file_path.lower())
if ext == ".json":
return "json"
elif ext == ".bin":
if not HAS_CBOR2:
raise ImportError("cbor2 library is required for .bin files. Install with: pip install cbor2")
return "cbor2"
else:
raise ValueError(f"Unsupported file extension '{ext}'. Supported extensions: .json, .bin")
def _ptr_key_from_numpy(arr: np.ndarray) -> int:
# Use the underlying buffer address as a stable key within a process
# for non-aliased arrays. For views, this still points to the base buffer;
# since user guarantees no aliasing across arrays, we can use the data address.
return int(arr.__array_interface__["data"][0])
_NP_TAG = 1 << 60
_WARP_TAG = 2 << 60
_MESH_TAG = 3 << 60
def _np_key(arr: np.ndarray) -> int:
return _NP_TAG + _ptr_key_from_numpy(arr)
def _warp_key(x) -> int:
try:
base = int(x.ptr)
except Exception:
base = int(id(x))
return _WARP_TAG + base
def _mesh_key_from_vertices(vertices: np.ndarray, fallback_obj=None) -> int:
try:
base = _ptr_key_from_numpy(vertices)
except Exception:
base = int(id(fallback_obj)) if fallback_obj is not None else int(id(vertices))
return _MESH_TAG + base
def serialize_ndarray(arr: np.ndarray, format_type: str = "json", cache: ArrayCache | None = None) -> dict:
"""
Serialize a numpy ndarray to a dictionary representation.
Args:
arr: The numpy array to serialize.
format_type: The serialization format ('json' or 'cbor2').
Returns:
A dictionary containing the array's type, dtype, shape, and data.
"""
if format_type == "json":
return {
"__type__": "numpy.ndarray",
"dtype": str(arr.dtype),
"shape": arr.shape,
"data": json.dumps(arr.tolist()),
}
elif format_type == "cbor2":
try:
arr_c = np.ascontiguousarray(arr)
# Required check to test if tobytes will work without using pickle internally
# arr.view will throw an exception if the dtype is not supported
arr.view(dtype=np.float32)
if cache is None:
return {
"__type__": "numpy.ndarray",
"dtype": arr.dtype.str,
"shape": arr.shape,
"order": "C",
"binary_data": arr_c.tobytes(order="C"),
}
# Cache-aware: assign or reuse an index
key = _np_key(arr_c)
idx = cache.try_register_pointer_and_value(key, arr_c)
if idx == 0:
# First occurrence: write full payload with index
assigned = cache.get_index_for_key(key)
return {
"__type__": "numpy.ndarray",
"dtype": arr_c.dtype.str,
"shape": arr_c.shape,
"order": "C",
"binary_data": arr_c.tobytes(order="C"),
"cache_index": int(assigned),
}
else:
# Reference only
return {
"__type__": "numpy.ndarray_ref",
"cache_index": int(idx),
"dtype": arr_c.dtype.str,
"shape": arr_c.shape,
"order": "C",
}
except (ValueError, TypeError):
# Fallback to list serialization for dtypes that can't be serialized as binary
return {
"__type__": "numpy.ndarray",
"dtype": str(arr.dtype),
"shape": arr.shape,
"data": arr.tolist(),
"is_binary": False,
}
else:
raise ValueError(f"Unsupported format_type: {format_type}")
def deserialize_ndarray(data: dict, format_type: str = "json", cache: ArrayCache | None = None) -> np.ndarray:
"""
Deserialize a dictionary representation back to a numpy ndarray.
Args:
data: Dictionary containing the serialized array data.
format_type: The serialization format ('json' or 'cbor2').
Returns:
The reconstructed numpy array.
"""
if data.get("__type__") == "numpy.ndarray_ref":
if cache is None:
raise ValueError("ArrayCache is required to resolve numpy.ndarray_ref")
ref_index = int(data["cache_index"])
# Try to resolve immediately; if not yet registered (forward ref), defer
try:
return cache.try_get_value(ref_index)
except KeyError:
return {"__cache_ref__": {"index": ref_index, "kind": "numpy"}}
if data.get("__type__") != "numpy.ndarray":
raise ValueError("Invalid data format for numpy array deserialization")
dtype = np.dtype(data["dtype"])
shape = tuple(data["shape"])
if format_type == "json":
array_data = json.loads(data["data"])
return np.array(array_data, dtype=dtype).reshape(shape)
elif format_type == "cbor2":
if "binary_data" in data:
binary = data["binary_data"]
order = data.get("order", "C")
arr = np.frombuffer(binary, dtype=dtype)
arr = arr.reshape(shape, order=order)
# Register in cache if available and index provided
if cache is not None and "cache_index" in data:
# We cannot recover a stable pointer from bytes; use id(arr.data) as key surrogate
# Since no aliasing is guaranteed, each full array is unique in the stream
key = _np_key(arr)
cache.try_register_pointer_and_value_and_index(key, arr, int(data["cache_index"]))
return arr
else:
# Fallback to list deserialization for non-binary data
array_data = data["data"]
return np.array(array_data, dtype=dtype).reshape(shape)
else:
raise ValueError(f"Unsupported format_type: {format_type}")
def serialize(obj, callback, _visited=None, _path="", format_type="json", cache: ArrayCache | None = None):
"""
Recursively serialize an object into a dict, handling primitives,
containers, and custom class instances. Calls callback(obj) for every object
and replaces obj with the callback's return value before continuing.
Args:
obj: The object to serialize.
callback: A function taking two arguments (the object and current path) and returning the (possibly transformed) object.
_visited: Internal set to avoid infinite recursion from circular references.
_path: Internal parameter tracking the current path/member name.
format_type: The serialization format ('json' or 'cbor2').
"""
if _visited is None:
_visited = set()
# Run through callback first (object may be replaced)
result = callback(obj, _path)
if result is not obj:
return result
obj_id = id(obj)
if obj_id in _visited:
return ""
# Add to visited set (stack-like behavior)
_visited.add(obj_id)
try:
# Primitive types
if isinstance(obj, str | int | float | bool | type(None)):
return {"__type__": type(obj).__name__, "value": obj}
# NumPy scalar types
if isinstance(obj, np.number):
# Normalize to "numpy." for compatibility with deserializer
return {
"__type__": f"numpy.{type(obj).__name__}",
"value": obj.item(), # Convert numpy scalar to Python scalar
}
# NumPy arrays
if isinstance(obj, np.ndarray):
return serialize_ndarray(obj, format_type, cache)
# Mappings (like dict)
if isinstance(obj, Mapping):
return {
"__type__": type(obj).__name__,
"items": {
str(k): serialize(
v, callback, _visited, f"{_path}.{k}" if _path else str(k), format_type, cache=cache
)
for k, v in obj.items()
},
}
# Iterables (like list, tuple, set)
if isinstance(obj, Iterable) and not isinstance(obj, str | bytes | bytearray):
return {
"__type__": type(obj).__name__,
"items": [
serialize(
item, callback, _visited, f"{_path}[{i}]" if _path else f"[{i}]", format_type, cache=cache
)
for i, item in enumerate(obj)
],
}
# Custom object — serialize attributes
if hasattr(obj, "__dict__"):
return {
"__type__": obj.__class__.__name__,
"__module__": obj.__class__.__module__,
"attributes": {
attr: serialize(
value, callback, _visited, f"{_path}.{attr}" if _path else attr, format_type, cache=cache
)
for attr, value in vars(obj).items()
},
}
# Fallback — non-serializable type
raise ValueError(f"Cannot serialize object of type {type(obj)}")
finally:
# Remove from visited set when done (stack-like cleanup)
_visited.discard(obj_id)
def _is_struct_dtype(dtype) -> bool:
"""Check if a warp dtype is a struct type (decorated with @wp.struct)."""
return type(dtype).__name__ == "Struct"
def _serialize_warp_dtype(dtype) -> dict:
"""
Serialize a warp dtype with full metadata for proper reconstruction.
For built-in types (vec3f, mat33f, etc.), just stores the type string.
For dynamically created types (vec_t, mat_t), also stores length/shape
and scalar type metadata to enable reconstruction.
Args:
dtype: The warp dtype to serialize.
Returns:
A dict containing dtype info that can be used to reconstruct the type.
"""
dtype_str = str(dtype)
dtype_name = dtype.__name__
result = {"__dtype__": dtype_str}
# Check if this is a dynamically created type that needs extra metadata
if dtype_name in ("vec_t", "mat_t", "quat_t"):
# Get scalar type
try:
scalar_type = warp_types.type_scalar_type(dtype)
result["__scalar_type__"] = scalar_type.__name__
except Exception:
pass
# Get length/shape
try:
length = warp_types.type_length(dtype)
result["__type_length__"] = length
except Exception:
pass
# For matrices, also get shape
if hasattr(dtype, "_shape_"):
result["__type_shape__"] = list(dtype._shape_)
return result
def pointer_as_key(obj, format_type: str = "json", cache: ArrayCache | None = None):
def callback(x, path):
if isinstance(x, wp.array):
# Skip arrays with struct dtypes - they can't be serialized
if _is_struct_dtype(x.dtype):
return None
# Get dtype info with metadata for dynamic types
dtype_info = _serialize_warp_dtype(x.dtype)
# Use device pointer as cache key
if cache is not None:
key = _warp_key(x)
idx = cache.try_register_pointer_and_value(key, x)
if idx > 0:
return {
"__type__": "warp.array_ref",
**dtype_info,
"cache_index": int(idx),
}
# First occurrence: store full payload plus cache_index
assigned = cache.get_index_for_key(key)
return {
"__type__": "warp.array",
**dtype_info,
"cache_index": int(assigned),
# Avoid nested cache for raw bytes to keep warp-level dedup authoritative
"data": serialize_ndarray(x.numpy(), format_type, cache=None),
}
# No cache: fall back to plain encoding
return {
"__type__": "warp.array",
**dtype_info,
"data": serialize_ndarray(x.numpy(), format_type, cache=None),
}
if isinstance(x, wp.HashGrid):
return {"__type__": "warp.HashGrid", "data": None}
if isinstance(x, wp.Mesh):
return {"__type__": "warp.Mesh", "data": None}
if isinstance(x, Mesh):
# Use vertices buffer address as mesh key
mesh_data = {
"vertices": serialize_ndarray(x.vertices, format_type, cache),
"indices": serialize_ndarray(x.indices, format_type, cache),
"is_solid": x.is_solid,
"has_inertia": x.has_inertia,
"maxhullvert": x.maxhullvert,
"mass": x.mass,
"com": [float(x.com[0]), float(x.com[1]), float(x.com[2])],
"inertia": serialize_ndarray(np.array(x.inertia), format_type, cache),
}
if cache is not None:
mesh_key = _mesh_key_from_vertices(x.vertices, fallback_obj=x)
idx = cache.try_register_pointer_and_value(mesh_key, x)
if idx > 0:
return {"__type__": "newton.geometry.Mesh_ref", "cache_index": int(idx)}
assigned = cache.get_index_for_key(mesh_key)
return {"__type__": "newton.geometry.Mesh", "cache_index": int(assigned), "data": mesh_data}
return {"__type__": "newton.geometry.Mesh", "data": mesh_data}
if isinstance(x, wp.Device):
return {"__type__": "wp.Device", "data": None}
if callable(x):
return {"__type__": "callable", "data": None}
return x
return serialize(obj, callback, format_type=format_type, cache=cache)
def transfer_to_model(source_dict, target_obj, post_load_init_callback=None, _path=""):
"""
Recursively transfer values from source_dict to target_obj, respecting the tree structure.
Only transfers values where both source and target have matching attributes.
Args:
source_dict: Dictionary containing the values to transfer (from deserialization).
target_obj: Target object to receive the values.
post_load_init_callback: Optional function taking (target_obj, path) called after all children are processed.
_path: Internal parameter tracking the current path.
"""
if not hasattr(target_obj, "__dict__"):
return
# Handle case where source_dict is not a dict (primitive value)
if not isinstance(source_dict, dict):
return
# Iterate through all attributes of the target object
for attr_name in dir(target_obj):
# Skip private/magic methods and properties
if attr_name.startswith("_"):
continue
# Skip if attribute doesn't exist in target or is not settable
try:
target_value = getattr(target_obj, attr_name)
except (AttributeError, TypeError, RuntimeError):
# Skip attributes that can't be accessed (including CUDA stream on CPU devices)
continue
# Skip methods and non-data attributes
if callable(target_value):
continue
# Check if source_dict has this attribute (optimization: single dict lookup)
source_value = source_dict.get(attr_name, _MISSING := object())
if source_value is _MISSING:
continue
# Handle different types of values
if hasattr(target_value, "__dict__") and isinstance(source_value, dict):
# Recursively transfer for custom objects
# Build path only when needed (optimization: lazy string formatting)
current_path = f"{_path}.{attr_name}" if _path else attr_name
transfer_to_model(source_value, target_value, post_load_init_callback, current_path)
elif isinstance(source_value, list | tuple) and hasattr(target_value, "__len__"):
# Handle sequences - try to transfer if lengths match or target is empty
try:
# Optimization: cache len() call to avoid redundant computation
target_len = len(target_value)
if target_len == 0 or target_len == len(source_value):
# For now, just assign the value directly
# In a more sophisticated implementation, you might want to handle
# element-wise transfer for lists of objects
setattr(target_obj, attr_name, source_value)
except (TypeError, AttributeError):
# If we can't handle the sequence, try direct assignment
try:
setattr(target_obj, attr_name, source_value)
except (AttributeError, TypeError):
# Skip if we can't set the attribute
pass
else:
# Direct assignment for primitive types and other values
try:
setattr(target_obj, attr_name, source_value)
except (AttributeError, TypeError):
# Skip if we can't set the attribute (e.g., read-only property)
pass
# Call post_load_init_callback after all children have been processed
if post_load_init_callback is not None:
post_load_init_callback(target_obj, _path)
def deserialize(data, callback, _path="", format_type="json", cache: ArrayCache | None = None):
"""
Recursively deserialize a dict back into objects, handling primitives,
containers, and custom class instances. Calls callback(obj, path) for every object
and replaces obj with the callback's return value before continuing.
Args:
data: The serialized data to deserialize.
callback: A function taking two arguments (the data dict and current path) and returning the (possibly transformed) object.
_path: Internal parameter tracking the current path/member name.
format_type: The serialization format ('json' or 'cbor2').
"""
# Run through callback first (object may be replaced)
result = callback(data, _path)
if result is not data:
return result
# If not a dict with __type__, return as-is
if not isinstance(data, dict) or "__type__" not in data:
return data
type_name = data["__type__"]
# Primitive types
if type_name in ("str", "int", "float", "bool", "NoneType"):
return data["value"]
# NumPy scalar types
if type_name.startswith("numpy."):
if type_name == "numpy.ndarray":
return deserialize_ndarray(data, format_type, cache)
else:
# NumPy scalar types
numpy_type = getattr(np, type_name.split(".")[-1])
return numpy_type(data["value"])
# Mappings (like dict)
if type_name == "dict":
return {
k: deserialize(v, callback, f"{_path}.{k}" if _path else k, format_type, cache)
for k, v in data["items"].items()
}
# Iterables (like list, tuple, set)
if type_name in ("list", "tuple", "set"):
items = [
deserialize(item, callback, f"{_path}[{i}]" if _path else f"[{i}]", format_type, cache)
for i, item in enumerate(data["items"])
]
if type_name == "tuple":
return tuple(items)
elif type_name == "set":
return set(items)
else:
return items
# Custom objects
if "attributes" in data:
# For now, return a simple dict representation
# In a full implementation, you might want to reconstruct the actual class
return {
attr: deserialize(value, callback, f"{_path}.{attr}" if _path else attr, format_type, cache)
for attr, value in data["attributes"].items()
}
# Unknown type - return the data as-is
return data["value"] if isinstance(data, dict) and "value" in data else data
def extract_type_path(class_str: str) -> str:
"""
Extracts the fully qualified type name from a string like:
""
"""
# The format is always "", so we strip the prefix/suffix
if class_str.startswith(""):
return class_str[len("")]
raise ValueError(f"Unexpected format: {class_str}")
def extract_last_type_name(class_str: str) -> str:
"""
Extracts the last type name from a string like:
"" -> "uint64"
"""
if class_str.startswith(""):
inner = class_str[len("")]
return inner.split(".")[-1]
raise ValueError(f"Unexpected format: {class_str}")
# Mapping of scalar type names to warp scalar types and suffixes
_SCALAR_TYPE_MAP = {
"float32": (wp.float32, "f"),
"float64": (wp.float64, "d"),
"int32": (wp.int32, "i"),
"int64": (wp.int64, "l"),
"uint32": (wp.uint32, "u"),
"uint64": (wp.uint64, "ul"),
"int16": (wp.int16, "h"),
"uint16": (wp.uint16, "uh"),
"int8": (wp.int8, "b"),
"uint8": (wp.uint8, "ub"),
}
# Mapping numpy dtypes to warp scalar suffixes
_NUMPY_DTYPE_TO_SUFFIX = {
np.float32: "f",
np.float64: "d",
np.int32: "i",
np.int64: "l",
np.uint32: "u",
np.uint64: "ul",
}
# Mapping numpy dtypes to warp scalar types (for dynamic type creation)
_NUMPY_TO_WARP_SCALAR = {
np.float32: wp.float32,
np.float64: wp.float64,
np.int32: wp.int32,
np.int64: wp.int64,
np.uint32: wp.uint32,
np.uint64: wp.uint64,
np.int16: wp.int16,
np.uint16: wp.uint16,
np.int8: wp.int8,
np.uint8: wp.uint8,
}
def _resolve_warp_dtype(dtype_str: str, serialized_data: dict | None = None, np_arr: np.ndarray | None = None):
"""
Resolve a dtype string to a warp dtype, with backwards compatibility.
Uses metadata from serialized_data when available (for new recordings),
falls back to inferring from numpy array shape (for old recordings).
Args:
dtype_str: The dtype name extracted from serialized data.
serialized_data: Optional dict containing dtype metadata (__scalar_type__, __type_length__, __type_shape__).
np_arr: Optional numpy array to infer shape for generic types (fallback for old recordings).
Returns:
The warp dtype object.
Raises:
AttributeError: If the dtype cannot be resolved.
"""
# Try direct lookup first
if hasattr(wp, dtype_str):
return getattr(wp, dtype_str)
# Try to reconstruct from metadata (new recordings)
if serialized_data is not None:
scalar_type_name = serialized_data.get("__scalar_type__")
type_length = serialized_data.get("__type_length__")
type_shape = serialized_data.get("__type_shape__")
if scalar_type_name and scalar_type_name in _SCALAR_TYPE_MAP:
scalar_type, suffix = _SCALAR_TYPE_MAP[scalar_type_name]
# Handle vector types (vec_t)
if dtype_str == "vec_t" and type_length:
inferred = f"vec{type_length}{suffix}"
if hasattr(wp, inferred):
return getattr(wp, inferred)
# For non-standard lengths, create dynamically
return wp.types.vector(type_length, scalar_type)
# Handle matrix types (mat_t)
if dtype_str == "mat_t" and type_shape and len(type_shape) == 2:
rows, cols = type_shape
inferred = f"mat{rows}{cols}{suffix}"
if hasattr(wp, inferred):
return getattr(wp, inferred)
# For non-standard shapes, create dynamically
return wp.types.matrix((rows, cols), scalar_type)
# Handle quaternion types (quat_t)
if dtype_str == "quat_t":
inferred = f"quat{suffix}"
if hasattr(wp, inferred):
return getattr(wp, inferred)
# Fallback: infer from numpy array shape (for old recordings without metadata)
if dtype_str == "vec_t" and np_arr is not None and np_arr.ndim >= 1:
vec_len = np_arr.shape[-1] if np_arr.ndim > 1 else np_arr.shape[0]
suffix = _NUMPY_DTYPE_TO_SUFFIX.get(np_arr.dtype.type, "f")
inferred = f"vec{vec_len}{suffix}"
if hasattr(wp, inferred):
print(f"[Recorder] Info: Inferred dtype '{inferred}' from data shape for generic 'vec_t'")
return getattr(wp, inferred)
# For non-standard vector lengths (e.g., vec5), create dynamically
scalar_type = _NUMPY_TO_WARP_SCALAR.get(np_arr.dtype.type, wp.float32)
print(f"[Recorder] Info: Creating dynamic vector type vec{vec_len} with {scalar_type.__name__}")
return wp.types.vector(vec_len, scalar_type)
if dtype_str == "mat_t" and np_arr is not None and np_arr.ndim >= 2:
rows = np_arr.shape[-2] if np_arr.ndim > 2 else np_arr.shape[0]
cols = np_arr.shape[-1]
suffix = _NUMPY_DTYPE_TO_SUFFIX.get(np_arr.dtype.type, "f")
inferred = f"mat{rows}{cols}{suffix}"
if hasattr(wp, inferred):
print(f"[Recorder] Info: Inferred dtype '{inferred}' from data shape for generic 'mat_t'")
return getattr(wp, inferred)
# For non-standard matrix shapes, create dynamically
scalar_type = _NUMPY_TO_WARP_SCALAR.get(np_arr.dtype.type, wp.float32)
print(f"[Recorder] Info: Creating dynamic matrix type mat{rows}x{cols} with {scalar_type.__name__}")
return wp.types.matrix((rows, cols), scalar_type)
# If dtype ends with 'f' or 'd', try without suffix (e.g., vec3f -> vec3)
if dtype_str.endswith("f") or dtype_str.endswith("d"):
base = dtype_str[:-1]
if hasattr(wp, base):
return getattr(wp, base)
raise AttributeError(f"Cannot resolve warp dtype: '{dtype_str}'")
# returns a model and a state history
def depointer_as_key(data: dict, format_type: str = "json", cache: ArrayCache | None = None):
"""
Deserialize Newton simulation data using callback approach.
Args:
data: The serialized data containing model and states.
format_type: The serialization format ('json' or 'cbor2').
Returns:
The deserialized data structure.
"""
def callback(x, path):
# Optimization: extract type once to avoid repeated isinstance and dict lookups
x_type = x.get("__type__") if isinstance(x, dict) else None
if x_type == "warp.array_ref":
if cache is None:
raise ValueError("ArrayCache required to resolve warp.array_ref")
ref_index = int(x["cache_index"])
try:
return cache.try_get_value(ref_index)
except KeyError:
return {"__cache_ref__": {"index": ref_index, "kind": "warp.array"}}
elif x_type == "warp.array":
try:
dtype_str = extract_last_type_name(x["__dtype__"])
np_arr = deserialize_ndarray(x["data"], format_type, cache)
# Pass the full serialized dict for metadata, and np_arr as fallback for old recordings
a = _resolve_warp_dtype(dtype_str, serialized_data=x, np_arr=np_arr)
result = wp.array(np_arr, dtype=a)
# Register in cache if provided index present (optimization: single dict lookup)
cache_index = x.get("cache_index")
if cache is not None and cache_index is not None:
key = _warp_key(result)
cache.try_register_pointer_and_value_and_index(key, result, int(cache_index))
return result
except Exception as e:
print(f"[Recorder] Warning: Failed to deserialize warp.array at '{path}': {e}")
return None
elif x_type == "warp.HashGrid":
# Return None or create empty HashGrid as appropriate
return None
elif x_type == "warp.Mesh":
# Return None or create empty Mesh as appropriate
return None
elif x_type == "newton.geometry.Mesh_ref":
if cache is None:
raise ValueError("ArrayCache required to resolve Mesh_ref")
ref_index = int(x["cache_index"])
try:
return cache.try_get_value(ref_index)
except KeyError:
return {"__cache_ref__": {"index": ref_index, "kind": "mesh"}}
elif x_type == "newton.geometry.Mesh":
try:
mesh_data = x["data"]
vertices = deserialize_ndarray(mesh_data["vertices"], format_type, cache)
indices = deserialize_ndarray(mesh_data["indices"], format_type, cache)
# Create the mesh without computing inertia since we'll restore the saved values
mesh = Mesh(
vertices=vertices,
indices=indices,
compute_inertia=False,
is_solid=mesh_data["is_solid"],
maxhullvert=mesh_data["maxhullvert"],
)
# Restore the saved inertia properties
mesh.has_inertia = mesh_data["has_inertia"]
mesh.mass = mesh_data["mass"]
mesh.com = wp.vec3(*mesh_data["com"])
# Accept legacy recordings that stored mesh inertia as "I".
inertia_data = mesh_data.get("inertia")
if inertia_data is None:
inertia_data = mesh_data["I"]
mesh.inertia = wp.mat33(deserialize_ndarray(inertia_data, format_type, cache))
# Optimization: single dict lookup
cache_index = x.get("cache_index")
if cache is not None and cache_index is not None:
mesh_key = _mesh_key_from_vertices(vertices, fallback_obj=mesh)
cache.try_register_pointer_and_value_and_index(mesh_key, mesh, int(cache_index))
return mesh
except Exception as e:
print(f"[Recorder] Warning: Failed to deserialize Mesh at '{path}': {e}")
return None
elif x_type == "callable":
# Return None for callables as they can't be serialized/deserialized
return None
return x
result = deserialize(data, callback, format_type=format_type, cache=cache)
def _resolve_cache_refs(obj):
if isinstance(obj, dict):
# Optimization: single dict lookup instead of checking membership then accessing
cache_ref = obj.get("__cache_ref__")
if cache_ref is not None:
idx = int(cache_ref["index"])
# Will raise KeyError with clear message if still missing
return cache.try_get_value(idx) if cache is not None else obj
# Recurse into dict
return {k: _resolve_cache_refs(v) for k, v in obj.items()}
if isinstance(obj, list):
return [_resolve_cache_refs(v) for v in obj]
if isinstance(obj, tuple):
return tuple(_resolve_cache_refs(v) for v in obj)
if isinstance(obj, set):
return {_resolve_cache_refs(v) for v in obj}
return obj
# Resolve any forward references now that all definitive objects have populated the cache
return _resolve_cache_refs(result)
class ViewerFile(ViewerBase):
"""
File-based viewer backend for Newton physics simulations.
This backend records simulation data to JSON or binary files using the same
ViewerBase API as other viewers. It captures model structure and state data
during simulation for later replay or analysis.
Format is determined by file extension:
- .json: Human-readable JSON format
- .bin: Binary CBOR2 format (more efficient)
"""
def __init__(
self,
output_path: str,
auto_save: bool = True,
save_interval: int = 100,
max_history_size: int | None = None,
):
"""
Initialize the File viewer backend for Newton physics simulations.
Args:
output_path: Path to the output file (.json or .bin)
auto_save: If True, automatically save periodically during recording
save_interval: Number of frames between auto-saves (when auto_save=True)
max_history_size: Maximum number of states to keep in memory.
If None, uses unlimited history. If set, keeps only the last N states.
"""
super().__init__()
if not output_path:
raise ValueError("output_path must be a non-empty path")
self.output_path = Path(output_path)
self.auto_save = auto_save
self.save_interval = save_interval
# Recording storage
if max_history_size is None:
self.history: list[dict] = []
else:
self.history: RingBuffer[dict] = RingBuffer(max_history_size)
self.raw_model: Model | None = None
self.deserialized_model: dict | None = None
self._frame_count = 0
self._model_recorded = False
@override
def set_model(self, model: Model | None, max_worlds: int | None = None):
"""Override set_model to record the model when it is set.
Args:
model: Model to bind to this viewer.
max_worlds: Optional cap on rendered worlds.
"""
super().set_model(model, max_worlds=max_worlds)
if model is not None and not self._model_recorded:
self.record_model(model)
self._model_recorded = True
@override
def log_state(self, state: State):
"""Record a state snapshot in addition to the base viewer processing.
Args:
state: Current simulation state to record.
"""
super().log_state(state)
# Record the state
self.record(state)
self._frame_count += 1
# Auto-save if enabled
if self.auto_save and self._frame_count % self.save_interval == 0:
self._save_recording()
def save_recording(self, file_path: str | None = None, verbose: bool = False):
"""Save the recorded data to file.
Args:
file_path: Optional override for the output path. If omitted, uses
the ``output_path`` from construction.
verbose: If True, print status output on success/failure.
"""
effective_path = file_path if file_path is not None else str(self.output_path)
self._save_recording(effective_path, verbose=verbose)
def _save_recording(self, file_path: str | None = None, verbose: bool = True):
"""Internal method to save recording."""
try:
effective_path = file_path if file_path is not None else str(self.output_path)
self._save_to_file(effective_path)
if verbose:
print(f"Recording saved to {effective_path} ({self._frame_count} frames)")
except Exception as e:
if verbose:
print(f"Error saving recording: {e}")
def record(self, state: State):
"""Record a snapshot of the provided simulation state.
Args:
state: State to snapshot into the recording history.
"""
state_data = {}
for name, value in state.__dict__.items():
if isinstance(value, wp.array):
state_data[name] = wp.clone(value)
self.history.append(state_data)
def playback(self, state: State, frame_id: int):
"""Restore a state snapshot from history into a State object.
Args:
state: Destination state object to populate.
frame_id: Frame index to load from history.
"""
if not (0 <= frame_id < len(self.history)):
print(f"Warning: frame_id {frame_id} is out of bounds. Playback skipped.")
return
state_data = self.history[frame_id]
for name, value_wp in state_data.items():
if hasattr(state, name):
setattr(state, name, value_wp)
def record_model(self, model: Model):
"""Record a reference to the simulation model for later serialization.
Args:
model: Model to keep for serialization.
"""
self.raw_model = model
def playback_model(self, model: Model):
"""Populate a Model instance from loaded recording data.
Args:
model: Destination model object to populate.
"""
if not self.deserialized_model:
print("Warning: No model data to playback.")
return
def post_load_init_callback(target_obj, path):
if isinstance(target_obj, Mesh):
target_obj.finalize()
transfer_to_model(self.deserialized_model, model, post_load_init_callback)
def _save_to_file(self, file_path: str):
"""Save recorded model and history to disk."""
try:
format_type = _get_serialization_format(file_path)
except ValueError:
if "." not in os.path.basename(file_path):
file_path = file_path + ".json"
format_type = "json"
else:
raise
states_to_save = self.history.to_list() if isinstance(self.history, RingBuffer) else self.history
data_to_save = {"model": self.raw_model, "states": states_to_save}
array_cache = ArrayCache()
serialized_data = pointer_as_key(data_to_save, format_type, cache=array_cache)
if format_type == "json":
with open(file_path, "w") as f:
json.dump(serialized_data, f, indent=2)
elif format_type == "cbor2":
cbor_data = cbor2.dumps(serialized_data)
with open(file_path, "wb") as f:
f.write(cbor_data)
def _load_from_file(self, file_path: str):
"""Load recording data from disk, replacing current model/history."""
try:
format_type = _get_serialization_format(file_path)
except ValueError:
if "." not in os.path.basename(file_path):
json_path = file_path + ".json"
if os.path.exists(json_path):
file_path = json_path
format_type = "json"
else:
raise FileNotFoundError(f"File not found: {file_path} (tried .json extension)") from None
else:
raise
if format_type == "json":
with open(file_path) as f:
serialized_data = json.load(f)
elif format_type == "cbor2":
with open(file_path, "rb") as f:
file_data = f.read()
serialized_data = cbor2.loads(file_data)
array_cache = ArrayCache()
raw = depointer_as_key(serialized_data, format_type, cache=array_cache)
self.deserialized_model = raw["model"]
loaded_states = raw["states"]
if isinstance(self.history, RingBuffer):
self.history.from_list(loaded_states)
else:
self.history = loaded_states
# Abstract method implementations (no-ops for file recording)
@override
def log_mesh(
self,
name: str,
points: wp.array[wp.vec3],
indices: wp.array[wp.int32] | wp.array[wp.uint32],
normals: wp.array[wp.vec3] | None = None,
uvs: wp.array[wp.vec2] | None = None,
texture: np.ndarray | str | None = None,
hidden: bool = False,
backface_culling: bool = True,
):
"""File viewer does not render meshes.
Args:
name: Unique path/name for the mesh.
points: Mesh vertex positions.
indices: Mesh triangle indices.
normals: Optional vertex normals.
uvs: Optional UV coordinates.
texture: Optional texture path/URL or image array.
hidden: Whether the mesh is hidden.
backface_culling: Whether back-face culling is enabled.
"""
pass
@override
def log_instances(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""File viewer does not render instances.
Args:
name: Unique path/name for the instance batch.
mesh: Name of the base mesh.
xforms: Optional per-instance transforms.
scales: Optional per-instance scales.
colors: Optional per-instance colors.
materials: Optional per-instance material parameters.
hidden: Whether the instance batch is hidden.
"""
pass
@override
def log_lines(
self,
name: str,
starts: wp.array[wp.vec3] | None,
ends: wp.array[wp.vec3] | None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None),
width: float = 0.01,
hidden: bool = False,
):
"""File viewer does not render line primitives.
Args:
name: Unique path/name for the line batch.
starts: Optional line start points.
ends: Optional line end points.
colors: Optional per-line colors or a shared RGB triplet.
width: Line width hint.
hidden: Whether the line batch is hidden.
"""
pass
@override
def log_points(
self,
name: str,
points: wp.array[wp.vec3] | None,
radii: wp.array[wp.float32] | float | None = None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None) = None,
hidden: bool = False,
):
"""File viewer does not render point primitives.
Args:
name: Unique path/name for the point batch.
points: Optional point positions.
radii: Optional per-point radii or a shared radius.
colors: Optional per-point colors or a shared RGB triplet.
hidden: Whether the point batch is hidden.
"""
pass
@override
def log_array(self, name: str, array: wp.array[Any] | np.ndarray):
"""File viewer does not visualize generic arrays.
Args:
name: Unique path/name for the array signal.
array: Array data to visualize.
"""
pass
@override
def log_scalar(self, name: str, value: int | float | bool | np.number):
"""File viewer does not visualize scalar signals.
Args:
name: Unique path/name for the scalar signal.
value: Scalar value to visualize.
"""
pass
@override
def end_frame(self):
"""No frame rendering needed for file viewer."""
pass
@override
def apply_forces(self, state: State):
"""File viewer does not apply interactive forces.
Args:
state: Current simulation state.
"""
pass
@override
def close(self):
"""Save final recording and cleanup."""
if self._frame_count > 0:
self._save_recording()
print(f"ViewerFile closed. Total frames recorded: {self._frame_count}")
def load_recording(self, file_path: str | None = None, verbose: bool = False):
"""Load a previously recorded file for playback.
After loading, use load_model() and load_state() to restore the model
and state at a given frame.
Args:
file_path: Optional override for the file path. If omitted, uses
the ``output_path`` from construction.
verbose: If True, print status output after loading.
"""
effective_path = file_path if file_path is not None else str(self.output_path)
self._load_from_file(effective_path)
self._frame_count = len(self.history)
if verbose:
print(f"Loaded recording with {self._frame_count} frames from {effective_path}")
def get_frame_count(self) -> int:
"""Return the number of frames in the loaded or recorded session.
Returns:
int: Number of frames available for playback.
"""
return self._frame_count
def has_model(self) -> bool:
"""Return whether the loaded recording contains model data.
Returns:
bool: True when model data is available for playback.
"""
return self.deserialized_model is not None
def load_model(self, model: Model):
"""Restore a Model from the loaded recording.
Must be called after load_recording(). The given model is populated
with the recorded model structure (bodies, shapes, etc.).
Args:
model: A Newton Model instance to populate.
"""
self.playback_model(model)
def load_state(self, state: State, frame_id: int):
"""Restore State to a specific frame from the loaded recording.
Must be called after load_recording(). The given state is updated
with the state snapshot at frame_id.
Args:
state: A Newton State instance to populate.
frame_id: Frame index in [0, get_frame_count()).
"""
self.playback(state, frame_id)
================================================
FILE: newton/_src/viewer/viewer_gl.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import ctypes
import re
import time
from collections.abc import Callable, Sequence
from importlib import metadata
from typing import Any, Literal
import numpy as np
import warp as wp
import newton as nt
from newton.selection import ArticulationView
from ..core.types import Axis, override
from ..utils.render import copy_rgb_frame_uint8
from .camera import Camera
from .gl.gui import UI
from .gl.opengl import LinesGL, MeshGL, MeshInstancerGL, RendererGL
from .picking import Picking
from .viewer import ViewerBase
from .wind import Wind
def _parse_version_tuple(version: str) -> tuple[int, ...]:
parts = re.findall(r"\d+", version)
return tuple(int(part) for part in parts[:3])
def _imgui_uses_imvec4_color_edit3() -> bool:
"""Return True when installed imgui_bundle expects ImVec4 in color_edit3."""
try:
version = metadata.version("imgui_bundle")
except metadata.PackageNotFoundError:
return False
return _parse_version_tuple(version) >= (1, 92, 6)
_IMGUI_BUNDLE_IMVEC4_COLOR_EDIT3 = _imgui_uses_imvec4_color_edit3()
@wp.kernel
def _capsule_duplicate_vec3(in_values: wp.array[wp.vec3], out_values: wp.array[wp.vec3]):
# Duplicate N values into 2N values (two caps per capsule).
tid = wp.tid()
out_values[tid] = in_values[tid // 2]
@wp.kernel
def _capsule_duplicate_vec4(in_values: wp.array[wp.vec4], out_values: wp.array[wp.vec4]):
# Duplicate N values into 2N values (two caps per capsule).
tid = wp.tid()
out_values[tid] = in_values[tid // 2]
@wp.kernel
def _capsule_build_body_scales(
shape_scale: wp.array[wp.vec3],
shape_indices: wp.array[wp.int32],
out_scales: wp.array[wp.vec3],
):
# model.shape_scale stores capsule params as (radius, half_height, _unused).
# ViewerGL instances scale meshes with a full (x, y, z) vector, so we expand to
# (radius, radius, half_height) for the cylinder body.
tid = wp.tid()
s = shape_indices[tid]
scale = shape_scale[s]
r = scale[0]
half_height = scale[1]
out_scales[tid] = wp.vec3(r, r, half_height)
@wp.kernel
def _capsule_build_cap_xforms_and_scales(
capsule_xforms: wp.array[wp.transform],
capsule_scales: wp.array[wp.vec3],
out_xforms: wp.array[wp.transform],
out_scales: wp.array[wp.vec3],
):
tid = wp.tid()
i = tid // 2
# Each capsule has two caps; even tid is the +Z end, odd tid is the -Z end.
is_plus_end = (tid % 2) == 0
t = capsule_xforms[i]
p = wp.transform_get_translation(t)
q = wp.transform_get_rotation(t)
r = capsule_scales[i][0]
half_height = capsule_scales[i][2]
offset_local = wp.vec3(0.0, 0.0, half_height if is_plus_end else -half_height)
p2 = p + wp.quat_rotate(q, offset_local)
out_xforms[tid] = wp.transform(p2, q)
out_scales[tid] = wp.vec3(r, r, r)
@wp.kernel
def _compute_shape_vbo_xforms(
shape_transform: wp.array[wp.transformf],
shape_body: wp.array[int],
body_q: wp.array[wp.transformf],
shape_scale: wp.array[wp.vec3],
shape_type: wp.array[int],
shape_world: wp.array[int],
world_offsets: wp.array[wp.vec3],
write_indices: wp.array[int],
out_world_xforms: wp.array[wp.transformf],
out_vbo_xforms: wp.array[wp.mat44],
):
"""Process all model shapes, write mat44 to grouped output positions."""
tid = wp.tid()
out_idx = write_indices[tid]
if out_idx < 0:
return
local_xform = shape_transform[tid]
parent = shape_body[tid]
if parent >= 0:
xform = wp.transform_multiply(body_q[parent], local_xform)
else:
xform = local_xform
if world_offsets:
wi = shape_world[tid]
if wi >= 0 and wi < world_offsets.shape[0]:
p = wp.transform_get_translation(xform)
xform = wp.transform(p + world_offsets[wi], wp.transform_get_rotation(xform))
out_world_xforms[out_idx] = xform
p = wp.transform_get_translation(xform)
q = wp.transform_get_rotation(xform)
R = wp.quat_to_matrix(q)
# Only mesh/convex_mesh shapes use model scale; other primitives have
# their dimensions baked into the geometry mesh, so scale is (1,1,1).
geo = shape_type[tid]
if geo == nt.GeoType.MESH or geo == nt.GeoType.CONVEX_MESH:
s = shape_scale[tid]
else:
s = wp.vec3(1.0, 1.0, 1.0)
out_vbo_xforms[out_idx] = wp.mat44(
R[0, 0] * s[0],
R[1, 0] * s[0],
R[2, 0] * s[0],
0.0,
R[0, 1] * s[1],
R[1, 1] * s[1],
R[2, 1] * s[1],
0.0,
R[0, 2] * s[2],
R[1, 2] * s[2],
R[2, 2] * s[2],
0.0,
p[0],
p[1],
p[2],
1.0,
)
class ViewerGL(ViewerBase):
"""
OpenGL-based interactive viewer for Newton physics models.
This class provides a graphical interface for visualizing and interacting with
Newton models using OpenGL rendering. It supports real-time simulation control,
camera navigation, object picking, wind effects, and a rich ImGui-based UI for
model introspection and visualization options.
Key Features:
- Real-time 3D rendering of Newton models and simulation states.
- Camera navigation with WASD/QE and mouse controls.
- Object picking and manipulation via mouse.
- Visualization toggles for joints, contacts, particles, springs, etc.
- Wind force controls and visualization.
- Performance statistics overlay (FPS, object counts, etc.).
- Selection panel for introspecting and filtering model attributes.
- Extensible logging of meshes, lines, points, and arrays for custom visualization.
"""
def __init__(
self,
width: int = 1920,
height: int = 1080,
vsync: bool = False,
headless: bool = False,
):
"""
Initialize the OpenGL viewer and UI.
Args:
width: Window width in pixels.
height: Window height in pixels.
vsync: Enable vertical sync.
headless: Run in headless mode (no window).
"""
# Pre-initialize callback registry; clear_model() (called from
# super().__init__()) resets the "side" slot on each model change.
self._ui_callbacks = {"side": [], "stats": [], "free": [], "panel": []}
super().__init__()
self.renderer = RendererGL(vsync=vsync, screen_width=width, screen_height=height, headless=headless)
self.renderer.set_title("Newton Viewer")
fb_w, fb_h = self.renderer.window.get_framebuffer_size()
self.camera = Camera(width=fb_w, height=fb_h, up_axis="Z")
self._paused = False
# Selection panel state
self._selection_ui_state = {
"selected_articulation_pattern": "*",
"selected_articulation_view": None,
"selected_attribute": "joint_q",
"attribute_options": ["joint_q", "joint_qd", "joint_f", "body_q", "body_qd"],
"include_joints": "",
"exclude_joints": "",
"include_links": "",
"exclude_links": "",
"show_values": False,
"selected_batch_idx": 0,
"error_message": "",
}
self.renderer.register_key_press(self.on_key_press)
self.renderer.register_key_release(self.on_key_release)
self.renderer.register_mouse_press(self.on_mouse_press)
self.renderer.register_mouse_release(self.on_mouse_release)
self.renderer.register_mouse_drag(self.on_mouse_drag)
self.renderer.register_mouse_scroll(self.on_mouse_scroll)
self.renderer.register_resize(self.on_resize)
# Camera movement settings
self._camera_speed = 0.04
self._cam_vel = np.zeros(3, dtype=np.float32)
self._cam_speed = 4.0 # m/s
self._cam_damp_tau = 0.083 # s
# initialize viewer-local timer for per-frame integration
self._last_time = time.perf_counter()
# Only create UI in non-headless mode to avoid OpenGL context dependency
if not headless:
self.ui = UI(self.renderer.window)
else:
self.ui = None
self._gizmo_log = None
self._gizmo_active = {}
self.gizmo_is_using = False
# Performance tracking
self._fps_history = []
self._last_fps_time = time.perf_counter()
self._frame_count = 0
self._current_fps = 0.0
# a low resolution sphere mesh for point rendering
self._point_mesh = None
# Very low-poly sphere mesh dedicated to Gaussian splat rendering.
self._gaussian_mesh: MeshGL | None = None
# Per-name cache of numpy arrays for Gaussian point cloud rendering.
self._gaussian_cache: dict[str, dict] = {}
# UI visibility toggle
self.show_ui = True
# Initialize PBO (Pixel Buffer Object) resources used in the `get_frame` method.
self._pbo = None
self._wp_pbo = None
def _hash_geometry(self, geo_type: int, geo_scale, thickness: float, is_solid: bool, geo_src=None) -> int:
# For capsules, ignore (radius, half_height) in the geometry hash so varying-length capsules batch together.
# Capsule dimensions are stored per-shape in model.shape_scale as (radius, half_height, _unused) and
# are remapped in set_model() to per-instance render scales (radius, radius, half_height).
if geo_type == nt.GeoType.CAPSULE:
geo_scale = (1.0, 1.0)
return super()._hash_geometry(geo_type, geo_scale, thickness, is_solid, geo_src)
def _invalidate_pbo(self):
"""Invalidate PBO resources, forcing reallocation on next get_frame() call."""
if self._wp_pbo is not None:
self._wp_pbo = None # Let Python garbage collect the RegisteredGLBuffer
if self._pbo is not None:
gl = RendererGL.gl
pbo_id = (gl.GLuint * 1)(self._pbo)
gl.glDeleteBuffers(1, pbo_id)
self._pbo = None
def register_ui_callback(
self,
callback: Callable[[Any], None],
position: Literal["side", "stats", "free", "panel"] = "side",
):
"""
Register a UI callback to be rendered during the UI phase.
Args:
callback: Function to be called during UI rendering
position: Position where the UI should be rendered. One of:
"side" - Side callback (default)
"stats" - Stats/metrics area
"free" - Free-floating UI elements
"panel" - Top-level collapsing headers in left panel
"""
if not callable(callback):
raise TypeError("callback must be callable")
if position not in self._ui_callbacks:
valid_positions = list(self._ui_callbacks.keys())
raise ValueError(f"Invalid position '{position}'. Must be one of: {valid_positions}")
self._ui_callbacks[position].append(callback)
# helper function to create a low resolution sphere mesh for point rendering
def _create_point_mesh(self):
"""
Create a low-resolution sphere mesh for point rendering.
"""
mesh = nt.Mesh.create_sphere(1.0, num_latitudes=6, num_longitudes=6, compute_inertia=False)
self._point_mesh = MeshGL(len(mesh.vertices), len(mesh.indices), self.device)
points = wp.array(mesh.vertices, dtype=wp.vec3, device=self.device)
normals = wp.array(mesh.normals, dtype=wp.vec3, device=self.device)
uvs = wp.array(mesh.uvs, dtype=wp.vec2, device=self.device)
indices = wp.array(mesh.indices, dtype=wp.int32, device=self.device)
self._point_mesh.update(points, indices, normals, uvs)
@override
def log_gizmo(
self,
name: str,
transform: wp.transform,
*,
translate: Sequence[Axis] | None = None,
rotate: Sequence[Axis] | None = None,
snap_to: wp.transform | None = None,
):
"""Log or update a transform gizmo for the current frame.
Args:
name: Unique gizmo path/name.
transform: Gizmo world transform.
translate: Axes on which the translation handles are shown.
Defaults to all axes when ``None``. Pass an empty sequence
to hide all translation handles.
rotate: Axes on which the rotation rings are shown.
Defaults to all axes when ``None``. Pass an empty sequence
to hide all rotation rings.
snap_to: Optional world transform to snap to when this gizmo is
released by the user.
"""
axis_order = (Axis.X, Axis.Y, Axis.Z)
if translate is None:
t = axis_order
else:
translate_axes = {Axis.from_any(axis) for axis in translate}
t = tuple(axis for axis in axis_order if axis in translate_axes)
if rotate is None:
r = axis_order
else:
rotate_axes = {Axis.from_any(axis) for axis in rotate}
r = tuple(axis for axis in axis_order if axis in rotate_axes)
self._gizmo_log[name] = {
"transform": transform,
"snap_to": snap_to,
"translate": t,
"rotate": r,
}
@override
def clear_model(self):
"""Reset GL-specific model-dependent state to defaults.
Called from ``__init__`` (via ``super().__init__`` → ``clear_model``)
and whenever the current model is discarded.
"""
# Render object and line caches (path -> GL object)
self.objects = {}
self.lines = {}
self._destroy_all_wireframes()
self.wireframe_shapes = {}
self._wireframe_vbo_owners: dict[int, WireframeShapeGL] = {}
# Interactive picking and wind force helpers
self.picking = None
self.wind = None
# State caching for selection panel
self._last_state = None
self._last_control = None
# Packed GPU arrays for batched shape transform computation
self._packed_groups = []
self._capsule_keys = set()
self._packed_write_indices = None
self._packed_world_xforms = None
self._packed_vbo_xforms = None
self._packed_vbo_xforms_host = None
# Clear example-specific UI callbacks; panel/stats persist
self._ui_callbacks["side"] = []
self._ui_callbacks["free"] = []
super().clear_model()
@override
def set_model(self, model: nt.Model | None, max_worlds: int | None = None):
"""
Set the Newton model to visualize.
Args:
model: The Newton model instance.
max_worlds: Maximum number of worlds to render (None = all).
"""
super().set_model(model, max_worlds=max_worlds)
if self.model is not None:
# For capsule batches, replace per-instance scales with (radius, radius, half_height)
# so the capsule instancer path has the needed parameters.
shape_scale = self.model.shape_scale
if shape_scale.device != self.device:
# Defensive: ensure inputs are on the launch device.
shape_scale = wp.clone(shape_scale, device=self.device)
def _ensure_indices_wp(model_shapes) -> wp.array:
# Return shape indices as a Warp array on the viewer device
if isinstance(model_shapes, wp.array):
if model_shapes.device == self.device:
return model_shapes
return wp.array(model_shapes.numpy().astype(np.int32), dtype=wp.int32, device=self.device)
return wp.array(model_shapes, dtype=wp.int32, device=self.device)
for batch in self._shape_instances.values():
if batch.geo_type != nt.GeoType.CAPSULE:
continue
shape_indices = _ensure_indices_wp(batch.model_shapes)
num_shapes = len(shape_indices)
out_scales = wp.empty(num_shapes, dtype=wp.vec3, device=self.device)
if num_shapes == 0:
batch.scales = out_scales
continue
wp.launch(
_capsule_build_body_scales,
dim=num_shapes,
inputs=[shape_scale, shape_indices],
outputs=[out_scales],
device=self.device,
record_tape=False,
)
batch.scales = out_scales
self.picking = Picking(model, world_offsets=self.world_offsets)
self.wind = Wind(model)
# Precompile picking/raycast kernels to avoid JIT delay on first pick
if model is not None:
try:
from ..geometry import raycast as _raycast_module # noqa: PLC0415
wp.load_module(module=_raycast_module, device=model.device)
wp.load_module(module="newton._src.viewer.kernels", device=model.device)
except Exception:
pass
# Build packed arrays for batched GPU rendering of shape instances
self._build_packed_vbo_arrays()
fb_w, fb_h = self.renderer.window.get_framebuffer_size()
self.camera = Camera(width=fb_w, height=fb_h, up_axis=model.up_axis if model else "Z")
def _build_packed_vbo_arrays(self):
"""Build write-index + output arrays for batched shape transform computation.
The kernel processes all model shapes (coalesced reads), uses a write-index
array to scatter results into contiguous groups in the output buffer.
"""
from .gl.opengl import MeshGL, MeshInstancerGL # noqa: PLC0415
if self.model is None:
self._packed_groups = []
return
shape_count = self.model.shape_count
device = self.device
groups = []
capsule_keys = set()
total = 0
for key, shapes in self._shape_instances.items():
n = shapes.xforms.shape[0] if isinstance(shapes.xforms, wp.array) else len(shapes.xforms)
if n == 0:
continue
if shapes.geo_type == nt.GeoType.CAPSULE:
capsule_keys.add(key)
groups.append((key, shapes, total, n))
total += n
self._capsule_keys = capsule_keys
self._packed_groups = groups
if total == 0:
return
# Write-index: maps model shape index → packed output position (-1 = skip)
write_np = np.full(shape_count, -1, dtype=np.int32)
# World xforms output (capsules read these for cap sphere computation)
all_world_xforms = wp.empty(total, dtype=wp.transform, device=device)
for _key, shapes, offset, n in groups:
model_shapes = np.asarray(shapes.model_shapes, dtype=np.int32)
write_np[model_shapes] = np.arange(offset, offset + n, dtype=np.int32)
if _key in capsule_keys:
shapes.world_xforms = all_world_xforms[offset : offset + n]
if _key not in capsule_keys:
if shapes.name not in self.objects:
if shapes.mesh in self.objects and isinstance(self.objects[shapes.mesh], MeshGL):
self.objects[shapes.name] = MeshInstancerGL(max(n, 1), self.objects[shapes.mesh])
self._packed_write_indices = wp.array(write_np, dtype=int, device=device)
self._packed_world_xforms = all_world_xforms
self._packed_vbo_xforms = wp.empty(total, dtype=wp.mat44, device=device)
self._packed_vbo_xforms_host = wp.empty(total, dtype=wp.mat44, device="cpu", pinned=True)
@override
def set_world_offsets(self, spacing: tuple[float, float, float] | list[float] | wp.vec3):
"""Set world offsets and update the picking system.
Args:
spacing: Spacing between worlds along each axis.
"""
super().set_world_offsets(spacing)
# Update picking system with new world offsets
if hasattr(self, "picking") and self.picking is not None:
self.picking.world_offsets = self.world_offsets
@override
def set_camera(self, pos: wp.vec3, pitch: float, yaw: float):
"""
Set the camera position, pitch, and yaw.
Args:
pos: The camera position.
pitch: The camera pitch.
yaw: The camera yaw.
"""
self.camera.pos = pos
self.camera.pitch = max(min(pitch, 89.0), -89.0)
self.camera.yaw = (yaw + 180.0) % 360.0 - 180.0
@override
def log_mesh(
self,
name: str,
points: wp.array[wp.vec3],
indices: wp.array[wp.int32] | wp.array[wp.uint32],
normals: wp.array[wp.vec3] | None = None,
uvs: wp.array[wp.vec2] | None = None,
texture: np.ndarray | str | None = None,
hidden: bool = False,
backface_culling: bool = True,
):
"""
Log a mesh for rendering.
Args:
name: Unique name for the mesh.
points: Vertex positions.
indices: Triangle indices.
normals: Vertex normals.
uvs: Vertex UVs.
texture: Texture path/URL or image array (H, W, C).
hidden: Whether the mesh is hidden.
backface_culling: Enable backface culling.
"""
assert isinstance(points, wp.array)
assert isinstance(indices, wp.array)
assert normals is None or isinstance(normals, wp.array)
assert uvs is None or isinstance(uvs, wp.array)
if name not in self.objects:
self.objects[name] = MeshGL(
len(points), len(indices), self.device, hidden=hidden, backface_culling=backface_culling
)
self.objects[name].update(points, indices, normals, uvs, texture)
self.objects[name].hidden = hidden
self.objects[name].backface_culling = backface_culling
@override
def log_instances(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""
Log a batch of mesh instances for rendering.
Args:
name: Unique name for the instancer.
mesh: Name of the base mesh.
xforms: Array of transforms.
scales: Array of scales.
colors: Array of colors.
materials: Array of materials.
hidden: Whether the instances are hidden.
"""
if mesh not in self.objects:
raise RuntimeError(f"Path {mesh} not found")
# check it is a mesh object
if not isinstance(self.objects[mesh], MeshGL):
raise RuntimeError(f"Path {mesh} is not a Mesh object")
instancer = self.objects.get(name, None)
transform_count = len(xforms) if xforms is not None else 0
resized = False
if instancer is None:
capacity = max(transform_count, 1)
instancer = MeshInstancerGL(capacity, self.objects[mesh])
self.objects[name] = instancer
resized = True
elif transform_count > instancer.num_instances:
new_capacity = max(transform_count, instancer.num_instances * 2)
instancer = MeshInstancerGL(new_capacity, self.objects[mesh])
self.objects[name] = instancer
resized = True
needs_update = resized or not hidden
if needs_update:
self.objects[name].update_from_transforms(xforms, scales, colors, materials)
self.objects[name].hidden = hidden
@override
def log_capsules(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""
Render capsules using instanced cylinder bodies + instanced sphere end caps.
This specialized path improves batching for varying-length capsules by reusing two
prototype meshes (unit cylinder + unit sphere) and applying per-instance transforms/scales.
Args:
name: Unique name for the capsule instancer group.
mesh: Capsule prototype mesh path from ViewerBase (unused in this backend).
xforms: Capsule instance transforms (wp.transform), length N.
scales: Capsule body instance scales, expected (radius, radius, half_height), length N.
colors: Capsule instance colors (wp.vec3), length N or None (no update).
materials: Capsule instance materials (wp.vec4), length N or None (no update).
hidden: Whether the instances are hidden.
"""
# Render capsules via instanced cylinder body + instanced sphere caps.
sphere_mesh = "/geometry/_capsule_instancer/sphere"
cylinder_mesh = "/geometry/_capsule_instancer/cylinder"
if sphere_mesh not in self.objects:
self.log_geo(sphere_mesh, nt.GeoType.SPHERE, (1.0,), 0.0, True, hidden=True)
if cylinder_mesh not in self.objects:
self.log_geo(cylinder_mesh, nt.GeoType.CYLINDER, (1.0, 1.0), 0.0, True, hidden=True)
# Cylinder body uses the capsule transforms and (radius, radius, half_height) scaling.
cyl_name = f"{name}/capsule_cylinder"
cap_name = f"{name}/capsule_caps"
# If hidden, just hide the instancers (skip all per-frame cap buffer work).
if hidden:
self.log_instances(cyl_name, cylinder_mesh, None, None, None, None, hidden=True)
self.log_instances(cap_name, sphere_mesh, None, None, None, None, hidden=True)
return
self.log_instances(cyl_name, cylinder_mesh, xforms, scales, colors, materials, hidden=hidden)
# Sphere caps: two spheres per capsule, offset by ±half_height along local +Z.
n = len(xforms) if xforms is not None else 0
if n == 0:
self.log_instances(cap_name, sphere_mesh, None, None, None, None, hidden=True)
return
cap_count = n * 2
cap_xforms = wp.empty(cap_count, dtype=wp.transform, device=self.device)
cap_scales = wp.empty(cap_count, dtype=wp.vec3, device=self.device)
wp.launch(
_capsule_build_cap_xforms_and_scales,
dim=cap_count,
inputs=[xforms, scales],
outputs=[cap_xforms, cap_scales],
device=self.device,
record_tape=False,
)
cap_colors = None
if colors is not None:
cap_colors = wp.empty(cap_count, dtype=wp.vec3, device=self.device)
wp.launch(
_capsule_duplicate_vec3,
dim=cap_count,
inputs=[colors],
outputs=[cap_colors],
device=self.device,
record_tape=False,
)
cap_materials = None
if materials is not None:
cap_materials = wp.empty(cap_count, dtype=wp.vec4, device=self.device)
wp.launch(
_capsule_duplicate_vec4,
dim=cap_count,
inputs=[materials],
outputs=[cap_materials],
device=self.device,
record_tape=False,
)
self.log_instances(cap_name, sphere_mesh, cap_xforms, cap_scales, cap_colors, cap_materials, hidden=hidden)
@override
def log_lines(
self,
name: str,
starts: wp.array[wp.vec3] | None,
ends: wp.array[wp.vec3] | None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None),
width: float = 0.01,
hidden: bool = False,
):
"""
Log line data for rendering.
Args:
name: Unique identifier for the line batch.
starts: Array of line start positions (shape: [N, 3]) or None for empty.
ends: Array of line end positions (shape: [N, 3]) or None for empty.
colors: Array of line colors (shape: [N, 3]) or tuple/list of RGB or None for empty.
width: The width of the lines.
hidden: Whether the lines are initially hidden.
"""
# Handle empty logs by resetting the LinesGL object
if starts is None or ends is None or colors is None:
if name in self.lines:
self.lines[name].update(None, None, None)
return
assert isinstance(starts, wp.array)
assert isinstance(ends, wp.array)
num_lines = len(starts)
assert len(ends) == num_lines, "Number of line ends must match line begins"
# Handle tuple/list colors by expanding to array (only if not already converted above)
if isinstance(colors, tuple | list):
if num_lines > 0:
color_vec = wp.vec3(*colors)
colors = wp.zeros(num_lines, dtype=wp.vec3, device=self.device)
colors.fill_(color_vec) # Efficiently fill on GPU
else:
# Handle zero lines case
colors = wp.array([], dtype=wp.vec3, device=self.device)
assert isinstance(colors, wp.array)
assert len(colors) == num_lines, "Number of line colors must match line begins"
# Create or resize LinesGL object based on current requirements
if name not in self.lines:
# Start with reasonable default size, will expand as needed
max_lines = max(num_lines, 1000) # Reasonable default
self.lines[name] = LinesGL(max_lines, self.device, hidden=hidden)
elif num_lines > self.lines[name].max_lines:
# Need to recreate with larger capacity
self.lines[name].destroy()
max_lines = max(num_lines, self.lines[name].max_lines * 2)
self.lines[name] = LinesGL(max_lines, self.device, hidden=hidden)
self.lines[name].update(starts, ends, colors)
@override
def log_wireframe_shape(
self,
name: str,
vertex_data: np.ndarray | None,
world_matrix: np.ndarray | None,
hidden: bool = False,
):
"""Log a wireframe shape for geometry-shader line rendering.
Args:
name: Unique path/name for the wireframe shape.
vertex_data: ``(N, 6)`` float32 interleaved vertex data, or ``None``
to keep existing geometry.
world_matrix: 4x4 float32 world matrix, or ``None`` to keep current.
hidden: Whether the shape is hidden.
"""
existing = self.wireframe_shapes.get(name)
if vertex_data is not None:
if existing is not None:
existing.destroy()
from .gl.opengl import WireframeShapeGL # noqa: PLC0415
vbo_key = id(vertex_data)
owner = self._wireframe_vbo_owners.get(vbo_key)
if owner is None:
owner = WireframeShapeGL(vertex_data)
self._wireframe_vbo_owners[vbo_key] = owner
obj = WireframeShapeGL.create_shared(owner)
obj.hidden = hidden
if world_matrix is not None:
obj.world_matrix = world_matrix.astype(np.float32)
self.wireframe_shapes[name] = obj
elif existing is not None:
existing.hidden = hidden
if world_matrix is not None:
existing.world_matrix = world_matrix.astype(np.float32)
def _destroy_all_wireframes(self):
"""Destroy all wireframe GL resources (visible shapes and VBO owners)."""
for obj in getattr(self, "wireframe_shapes", {}).values():
obj.destroy()
for owner in getattr(self, "_wireframe_vbo_owners", {}).values():
owner.destroy()
@override
def clear_wireframe_vbo_cache(self):
for obj in self.wireframe_shapes.values():
obj.destroy()
self.wireframe_shapes.clear()
for owner in self._wireframe_vbo_owners.values():
owner.destroy()
self._wireframe_vbo_owners.clear()
@override
def log_points(
self,
name: str,
points: wp.array[wp.vec3] | None,
radii: wp.array[wp.float32] | float | None = None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None) = None,
hidden: bool = False,
):
"""
Log a batch of points for rendering as spheres.
Args:
name: Unique name for the point batch.
points: Array of point positions.
radii: Array of point radius values.
colors: Array of point colors.
hidden: Whether the points are hidden.
"""
if points is None:
if name in self.objects:
self.objects[name].hidden = True
return
if self._point_mesh is None:
self._create_point_mesh()
num_points = len(points)
object_recreated = False
if name not in self.objects:
# Start with a reasonable default.
initial_capacity = max(num_points, 256)
self.objects[name] = MeshInstancerGL(initial_capacity, self._point_mesh)
object_recreated = True
elif num_points > self.objects[name].num_instances:
old = self.objects[name]
new_capacity = max(num_points, old.num_instances * 2)
self.objects[name] = MeshInstancerGL(new_capacity, self._point_mesh)
object_recreated = True
if radii is None:
radii = wp.full(num_points, 0.1, dtype=wp.float32, device=self.device)
# If a point object is first created/recreated and no colors are provided,
# initialize to white to avoid uninitialized instance color buffers.
if colors is None and object_recreated:
colors = wp.full(num_points, wp.vec3(1.0, 1.0, 1.0), dtype=wp.vec3, device=self.device)
self.objects[name].update_from_points(points, radii, colors)
self.objects[name].hidden = hidden
_SH_C0 = 0.28209479177387814
def _create_gaussian_mesh(self):
"""Create a very low-poly sphere mesh dedicated to Gaussian splat rendering."""
mesh = nt.Mesh.create_sphere(1.0, num_latitudes=3, num_longitudes=4, compute_inertia=False)
self._gaussian_mesh = MeshGL(len(mesh.vertices), len(mesh.indices), self.device)
points = wp.array(mesh.vertices, dtype=wp.vec3, device=self.device)
normals = wp.array(mesh.normals, dtype=wp.vec3, device=self.device)
uvs = wp.array(mesh.uvs, dtype=wp.vec2, device=self.device)
indices = wp.array(mesh.indices, dtype=wp.int32, device=self.device)
self._gaussian_mesh.update(points, indices, normals, uvs)
@override
def log_gaussian(
self,
name: str,
gaussian: nt.Gaussian,
xform: wp.transformf | None = None,
hidden: bool = False,
):
"""Log a :class:`newton.Gaussian` as a point cloud of spheres.
Args:
name: Unique path/name for the Gaussian point cloud.
gaussian: The :class:`newton.Gaussian` asset to visualize.
xform: Optional world-space transform applied to all splat centers.
hidden: Whether the point cloud should be hidden.
"""
if hidden:
if name in self.objects:
self.objects[name].hidden = True
return
if self._gaussian_mesh is None:
self._create_gaussian_mesh()
gaussian_cache_key = (id(gaussian), gaussian.count)
cache = self._gaussian_cache.get(name)
if cache is not None and cache.get("gaussian_cache_key") != gaussian_cache_key:
cache = None
if cache is None:
n = gaussian.count
# Subsample large Gaussians to keep rendering interactive.
max_pts = self.gaussians_max_points
if n > max_pts:
idx = np.linspace(0, n - 1, max_pts, dtype=np.intp)
positions = np.ascontiguousarray(gaussian.positions[idx], dtype=np.float32)
scales = gaussian.scales[idx]
sh = gaussian.sh_coeffs[idx] if gaussian.sh_coeffs is not None else None
n = max_pts
else:
idx = None
positions = np.ascontiguousarray(gaussian.positions, dtype=np.float32)
scales = gaussian.scales
sh = gaussian.sh_coeffs
radii = np.average(scales, axis=1).astype(np.float32)
# Pre-build the VBO mat44 buffer: diagonal = radii, [15] = 1.0.
vbo = np.zeros((n, 16), dtype=np.float32)
vbo[:, 0] = radii
vbo[:, 5] = radii
vbo[:, 10] = radii
vbo[:, 15] = 1.0
if sh is not None and sh.shape[1] >= 3:
colors = np.ascontiguousarray((self._SH_C0 * sh[:, :3] + 0.5).clip(0.0, 1.0).astype(np.float32))
else:
colors = np.ones((n, 3), dtype=np.float32)
cache = {
"gaussian_cache_key": gaussian_cache_key,
"local_pos": positions,
"vbo": vbo,
"colors": colors,
"colors_uploaded": False,
"world_pos_buf": np.empty((n, 3), dtype=np.float32),
"last_xform": None,
}
self._gaussian_cache[name] = cache
n = len(cache["local_pos"])
recreated = False
if name not in self.objects:
self.objects[name] = MeshInstancerGL(max(n, 256), self._gaussian_mesh)
self.objects[name].cast_shadow = False
recreated = True
elif n > self.objects[name].num_instances:
self.objects[name] = MeshInstancerGL(max(n, self.objects[name].num_instances * 2), self._gaussian_mesh)
self.objects[name].cast_shadow = False
recreated = True
instancer = self.objects[name]
instancer.active_instances = n
instancer.hidden = False
# Fast-path: skip VBO update when the transform has not changed.
xform_key: tuple | None = None
if xform is not None:
xform_key = (
float(xform.p[0]),
float(xform.p[1]),
float(xform.p[2]),
float(xform.q[0]),
float(xform.q[1]),
float(xform.q[2]),
float(xform.q[3]),
)
if not recreated and cache["last_xform"] == xform_key:
return
cache["last_xform"] = xform_key
# Transform local positions to world space (pure numpy, no GPU round-trip).
vbo = cache["vbo"]
if xform is not None:
qx, qy, qz, qw = xform_key[3], xform_key[4], xform_key[5], xform_key[6]
R = np.array(
[
[1.0 - 2.0 * (qy * qy + qz * qz), 2.0 * (qx * qy - qw * qz), 2.0 * (qx * qz + qw * qy)],
[2.0 * (qx * qy + qw * qz), 1.0 - 2.0 * (qx * qx + qz * qz), 2.0 * (qy * qz - qw * qx)],
[2.0 * (qx * qz - qw * qy), 2.0 * (qy * qz + qw * qx), 1.0 - 2.0 * (qx * qx + qy * qy)],
],
dtype=np.float32,
)
t = np.array(xform_key[:3], dtype=np.float32)
wp_buf = cache["world_pos_buf"]
np.dot(cache["local_pos"], R.T, out=wp_buf)
wp_buf += t
vbo[:, 12:15] = wp_buf
else:
vbo[:, 12:15] = cache["local_pos"]
# Upload transforms directly to GL.
gl = RendererGL.gl
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, instancer.instance_transform_buffer)
gl.glBufferSubData(gl.GL_ARRAY_BUFFER, 0, n * 64, vbo.ctypes.data)
if recreated or not cache["colors_uploaded"]:
gl.glBindBuffer(gl.GL_ARRAY_BUFFER, instancer.instance_color_buffer)
gl.glBufferSubData(gl.GL_ARRAY_BUFFER, 0, n * 12, cache["colors"].ctypes.data)
cache["colors_uploaded"] = True
@override
def log_array(self, name: str, array: wp.array[Any] | np.ndarray):
"""
Log a generic array for visualization (not implemented).
Args:
name: Unique path/name for the array signal.
array: Array data to visualize.
"""
pass
@override
def log_scalar(self, name: str, value: int | float | bool | np.number):
"""
Log a scalar value for visualization (not implemented).
Args:
name: Unique path/name for the scalar signal.
value: Scalar value to visualize.
"""
pass
@override
def log_state(self, state: nt.State):
"""
Log the current simulation state for rendering.
For shape instances on CUDA, uses a batched path: 2 kernel launches +
1 D2H copy to a shared pinned buffer, then uploads slices per instancer.
Everything else (capsules, SDF, particles, joints, …) uses the standard path.
Args:
state: Current simulation state for all rendered bodies/shapes.
"""
self._last_state = state
if self.model is None:
return
self._sync_shape_colors_from_model()
if self._packed_vbo_xforms is not None and self.device.is_cuda:
# ---- Single kernel over all model shapes, scatter-write to grouped output ----
wp.launch(
_compute_shape_vbo_xforms,
dim=self.model.shape_count,
inputs=[
self.model.shape_transform,
self.model.shape_body,
state.body_q,
self.model.shape_scale,
self.model.shape_type,
self.model.shape_world,
self.world_offsets,
self._packed_write_indices,
],
outputs=[self._packed_world_xforms, self._packed_vbo_xforms],
device=self.device,
record_tape=False,
)
wp.copy(self._packed_vbo_xforms_host, self._packed_vbo_xforms)
wp.synchronize() # copy is async (pinned destination), must sync before CPU read
# ---- Upload pinned host slices to GL per instancer ----
host_np = self._packed_vbo_xforms_host.numpy()
for key, shapes, offset, count in self._packed_groups:
visible = self._should_show_shape(shapes.flags, shapes.static)
colors = shapes.colors if self.model_changed or shapes.colors_changed else None
materials = shapes.materials if self.model_changed else None
if key in self._capsule_keys:
self.log_capsules(
shapes.name,
shapes.mesh,
shapes.world_xforms,
shapes.scales,
colors,
materials,
hidden=not visible,
)
else:
instancer = self.objects.get(shapes.name)
if instancer is not None:
instancer.hidden = not visible
instancer.update_from_pinned(
host_np[offset : offset + count],
count,
colors,
materials,
)
shapes.colors_changed = False
# ---- Gaussians and non-shape rendering use standard synchronous paths ----
self._log_gaussian_shapes(state)
self._log_non_shape_state(state)
self.model_changed = False
else:
# Fallback for CPU or when no packed data is available
super().log_state(state)
self._render_picking_line(state)
def _render_picking_line(self, state):
"""
Render a line from the mouse cursor to the actual picked point on the geometry.
Args:
state: The current simulation state.
"""
if not self.picking_enabled or self.picking is None or not self.picking.is_picking():
# Clear the picking line if not picking
self.log_lines("picking_line", None, None, None)
return
# Get the picked body index
pick_body_idx = self.picking.pick_body.numpy()[0]
if pick_body_idx < 0:
self.log_lines("picking_line", None, None, None)
return
# Get the pick target and current picked point on geometry (in physics space)
pick_state = self.picking.pick_state.numpy()
pick_target = pick_state[0]["picking_target_world"]
picked_point = pick_state[0]["picked_point_world"]
# Apply world offset to convert from physics space to visual space
if self.world_offsets is not None and self.world_offsets.shape[0] > 0:
if self.model.body_world is not None:
body_world_idx = self.model.body_world.numpy()[pick_body_idx]
if body_world_idx >= 0 and body_world_idx < self.world_offsets.shape[0]:
world_offset = self.world_offsets.numpy()[body_world_idx]
pick_target = pick_target + world_offset
picked_point = picked_point + world_offset
# Create line data
starts = wp.array(
[wp.vec3(picked_point[0], picked_point[1], picked_point[2])], dtype=wp.vec3, device=self.device
)
ends = wp.array([wp.vec3(pick_target[0], pick_target[1], pick_target[2])], dtype=wp.vec3, device=self.device)
colors = wp.array([wp.vec3(0.0, 1.0, 1.0)], dtype=wp.vec3, device=self.device)
# Render the line
self.log_lines("picking_line", starts, ends, colors, hidden=False)
@override
def begin_frame(self, time: float):
"""
Begin a new frame (calls parent implementation).
Args:
time: Current simulation time.
"""
super().begin_frame(time)
self._gizmo_log = {}
@override
def end_frame(self):
"""
Finish rendering the current frame and process window events.
This method first updates the renderer which will poll and process
window events. It is possible that the user closes the window during
this event processing step, which would invalidate the underlying
OpenGL context. Trying to issue GL calls after the context has been
destroyed results in a crash (access violation). Therefore we check
whether an exit was requested and early-out before touching GL if so.
"""
self._update()
@override
def apply_forces(self, state: nt.State):
"""
Apply viewer-driven forces (picking, wind) to the model.
Args:
state: The current simulation state.
"""
if self.picking_enabled and self.picking is not None:
self.picking._apply_picking_force(state)
if self.wind is not None:
self.wind._apply_wind_force(state)
def _update(self):
"""
Internal update: process events, update camera, wind, render scene and UI.
"""
self.renderer.update()
# Integrate camera motion with viewer-owned timing
now = time.perf_counter()
dt = max(0.0, min(0.1, now - self._last_time))
self._last_time = now
self._update_camera(dt)
if self.wind is not None:
self.wind.update(dt)
# If the window was closed during event processing, skip rendering
if self.renderer.has_exit():
return
# Render the scene and present it
self.renderer.render(self.camera, self.objects, self.lines, self.wireframe_shapes)
# Always update FPS tracking, even if UI is hidden
self._update_fps()
if self.ui and self.ui.is_available and self.show_ui:
self.ui.begin_frame()
# Render the UI
self._render_ui()
self.ui.end_frame()
self.ui.render()
self.renderer.present()
def get_frame(self, target_image: wp.array | None = None, render_ui: bool = False) -> wp.array:
"""
Retrieve the last rendered frame.
This method uses OpenGL Pixel Buffer Objects (PBO) and CUDA interoperability
to transfer pixel data entirely on the GPU, avoiding expensive CPU-GPU transfers.
Args:
target_image:
Optional pre-allocated Warp array with shape `(height, width, 3)`
and dtype `wp.uint8`. If `None`, a new array will be created.
render_ui: Whether to render the UI.
Returns:
wp.array: GPU array containing RGB image data with shape `(height, width, 3)`
and dtype `wp.uint8`. Origin is top-left (OpenGL's bottom-left is flipped).
"""
gl = RendererGL.gl
w, h = self.renderer._screen_width, self.renderer._screen_height
# Lazy initialization of PBO (Pixel Buffer Object).
if self._pbo is None:
pbo_id = (gl.GLuint * 1)()
gl.glGenBuffers(1, pbo_id)
self._pbo = pbo_id[0]
# Allocate PBO storage.
gl.glBindBuffer(gl.GL_PIXEL_PACK_BUFFER, self._pbo)
gl.glBufferData(gl.GL_PIXEL_PACK_BUFFER, gl.GLsizeiptr(w * h * 3), None, gl.GL_STREAM_READ)
gl.glBindBuffer(gl.GL_PIXEL_PACK_BUFFER, 0)
# Register with CUDA.
self._wp_pbo = wp.RegisteredGLBuffer(
gl_buffer_id=int(self._pbo),
device=self.device,
flags=wp.RegisteredGLBuffer.READ_ONLY,
)
# Set alignment once.
gl.glPixelStorei(gl.GL_PACK_ALIGNMENT, 1)
# GPU-to-GPU readback into PBO.
assert self.renderer._frame_fbo is not None
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, self.renderer._frame_fbo)
gl.glBindBuffer(gl.GL_PIXEL_PACK_BUFFER, self._pbo)
if render_ui and self.ui:
self.ui.begin_frame()
self._render_ui()
self.ui.end_frame()
self.ui.render()
gl.glReadPixels(0, 0, w, h, gl.GL_RGB, gl.GL_UNSIGNED_BYTE, ctypes.c_void_p(0))
gl.glBindBuffer(gl.GL_PIXEL_PACK_BUFFER, 0)
gl.glBindFramebuffer(gl.GL_FRAMEBUFFER, 0)
# Map PBO buffer and copy using RGB kernel.
assert self._wp_pbo is not None
buf = self._wp_pbo.map(dtype=wp.uint8, shape=(w * h * 3,))
if target_image is None:
target_image = wp.empty(
shape=(h, w, 3),
dtype=wp.uint8, # pyright: ignore[reportArgumentType]
device=self.device,
)
if target_image.shape != (h, w, 3):
raise ValueError(f"Shape of `target_image` must be ({h}, {w}, 3), got {target_image.shape}")
# Launch the RGB kernel.
wp.launch(
copy_rgb_frame_uint8,
dim=(w, h),
inputs=[buf, w, h],
outputs=[target_image],
device=self.device,
)
# Unmap the PBO buffer.
self._wp_pbo.unmap()
return target_image
@override
def is_running(self) -> bool:
"""
Check if the viewer is still running.
Returns:
bool: True if the window is open, False if closed.
"""
return not self.renderer.has_exit()
@override
def is_paused(self) -> bool:
"""
Check if the simulation is paused.
Returns:
bool: True if paused, False otherwise.
"""
return self._paused
@override
def close(self):
"""
Close the viewer and clean up resources.
"""
self.renderer.close()
@property
def vsync(self) -> bool:
"""
Get the current vsync state.
Returns:
bool: True if vsync is enabled, False otherwise.
"""
return self.renderer.get_vsync()
@vsync.setter
def vsync(self, enabled: bool):
"""
Set the vsync state.
Args:
enabled: Enable or disable vsync.
"""
self.renderer.set_vsync(enabled)
@override
def is_key_down(self, key: str | int) -> bool:
"""
Check if a key is currently pressed.
Args:
key: Either a string representing a character/key name, or an int
representing a pyglet key constant.
String examples: 'w', 'a', 's', 'd', 'space', 'escape', 'enter'
Int examples: pyglet.window.key.W, pyglet.window.key.SPACE
Returns:
bool: True if the key is currently pressed, False otherwise.
"""
try:
import pyglet
except Exception:
return False
if isinstance(key, str):
# Convert string to pyglet key constant
key = key.lower()
# Handle single characters
if len(key) == 1 and key.isalpha():
key_code = getattr(pyglet.window.key, key.upper(), None)
elif len(key) == 1 and key.isdigit():
key_code = getattr(pyglet.window.key, f"_{key}", None)
else:
# Handle special key names
special_keys = {
"space": pyglet.window.key.SPACE,
"escape": pyglet.window.key.ESCAPE,
"esc": pyglet.window.key.ESCAPE,
"enter": pyglet.window.key.ENTER,
"return": pyglet.window.key.ENTER,
"tab": pyglet.window.key.TAB,
"shift": pyglet.window.key.LSHIFT,
"ctrl": pyglet.window.key.LCTRL,
"alt": pyglet.window.key.LALT,
"up": pyglet.window.key.UP,
"down": pyglet.window.key.DOWN,
"left": pyglet.window.key.LEFT,
"right": pyglet.window.key.RIGHT,
"backspace": pyglet.window.key.BACKSPACE,
"delete": pyglet.window.key.DELETE,
}
key_code = special_keys.get(key, None)
if key_code is None:
return False
else:
# Assume it's already a pyglet key constant
key_code = key
return self.renderer.is_key_down(key_code)
# events
def on_mouse_scroll(self, x: float, y: float, scroll_x: float, scroll_y: float):
"""
Handle mouse scroll for zooming (FOV adjustment).
Args:
x: Mouse X position in window coordinates.
y: Mouse Y position in window coordinates.
scroll_x: Horizontal scroll delta.
scroll_y: Vertical scroll delta.
"""
if self._ui_is_capturing_mouse():
return
fov_delta = scroll_y * 2.0
self.camera.fov -= fov_delta
self.camera.fov = max(min(self.camera.fov, 90.0), 15.0)
def _to_framebuffer_coords(self, x: float, y: float) -> tuple[float, float]:
"""Convert window coordinates to framebuffer coordinates."""
fb_w, fb_h = self.renderer.window.get_framebuffer_size()
win_w, win_h = self.renderer.window.get_size()
if win_w <= 0 or win_h <= 0:
return float(x), float(y)
scale_x = fb_w / win_w
scale_y = fb_h / win_h
return float(x) * scale_x, float(y) * scale_y
def on_mouse_press(self, x: float, y: float, button: int, modifiers: int):
"""
Handle mouse press events (object picking).
Args:
x: Mouse X position in window coordinates.
y: Mouse Y position in window coordinates.
button: Mouse button pressed.
modifiers: Modifier keys.
"""
if self._ui_is_capturing_mouse():
return
import pyglet
# Handle right-click for picking
if button == pyglet.window.mouse.RIGHT and self.picking_enabled and self.picking is not None:
fb_x, fb_y = self._to_framebuffer_coords(x, y)
ray_start, ray_dir = self.camera.get_world_ray(fb_x, fb_y)
if self._last_state is not None:
self.picking.pick(self._last_state, ray_start, ray_dir)
def on_mouse_release(self, x: float, y: float, button: int, modifiers: int):
"""
Handle mouse release events to stop dragging.
Args:
x: Mouse X position in window coordinates.
y: Mouse Y position in window coordinates.
button: Mouse button released.
modifiers: Modifier keys.
"""
if self.picking is not None:
self.picking.release()
def on_mouse_drag(
self,
x: float,
y: float,
dx: float,
dy: float,
buttons: int,
modifiers: int,
):
"""
Handle mouse drag events for camera and picking.
Args:
x: Mouse X position in window coordinates.
y: Mouse Y position in window coordinates.
dx: Mouse delta along X since previous event.
dy: Mouse delta along Y since previous event.
buttons: Mouse buttons pressed.
modifiers: Modifier keys.
"""
if self._ui_is_capturing_mouse():
return
import pyglet
if buttons & pyglet.window.mouse.LEFT:
sensitivity = 0.1
dx *= sensitivity
dy *= sensitivity
# Map screen-space right drag to a right turn (clockwise),
# independent of world up-axis convention.
self.camera.yaw = (self.camera.yaw - dx + 180.0) % 360.0 - 180.0
self.camera.pitch = max(min(self.camera.pitch + dy, 89.0), -89.0)
if buttons & pyglet.window.mouse.RIGHT and self.picking_enabled:
fb_x, fb_y = self._to_framebuffer_coords(x, y)
ray_start, ray_dir = self.camera.get_world_ray(fb_x, fb_y)
if self.picking is not None and self.picking.is_picking():
self.picking.update(ray_start, ray_dir)
def on_mouse_motion(self, x: float, y: float, dx: float, dy: float):
"""
Handle mouse motion events (not used).
Args:
x: Mouse X position in window coordinates.
y: Mouse Y position in window coordinates.
dx: Mouse delta along X since previous event.
dy: Mouse delta along Y since previous event.
"""
pass
def _ui_is_capturing_mouse(self) -> bool:
"""Return whether the UI wants to consume mouse input this frame."""
if not self.ui:
return False
if hasattr(self.ui, "is_capturing_mouse"):
return bool(self.ui.is_capturing_mouse())
if hasattr(self.ui, "is_capturing"):
return bool(self.ui.is_capturing())
return False
def _ui_is_capturing_keyboard(self) -> bool:
"""Return whether the UI wants to consume keyboard input this frame."""
if not self.ui:
return False
if hasattr(self.ui, "is_capturing_keyboard"):
return bool(self.ui.is_capturing_keyboard())
if hasattr(self.ui, "is_capturing"):
return bool(self.ui.is_capturing())
return False
def on_key_press(self, symbol: int, modifiers: int):
"""
Handle key press events for UI and simulation control.
Args:
symbol: Key symbol.
modifiers: Modifier keys.
"""
if self._ui_is_capturing_keyboard():
return
try:
import pyglet
except Exception:
return
if symbol == pyglet.window.key.H:
self.show_ui = not self.show_ui
elif symbol == pyglet.window.key.SPACE:
# Toggle pause with space key
self._paused = not self._paused
elif symbol == pyglet.window.key.F:
# Frame camera around model bounds
self._frame_camera_on_model()
elif symbol == pyglet.window.key.ESCAPE:
# Exit with Escape key
self.renderer.close()
def on_key_release(self, symbol: int, modifiers: int):
"""
Handle key release events (not used).
Args:
symbol: Released key code.
modifiers: Active modifier bitmask for this event.
"""
pass
def _frame_camera_on_model(self):
"""
Frame the camera to show all visible objects in the scene.
"""
if self.model is None:
return
# Compute bounds from all visible objects
min_bounds = np.array([float("inf")] * 3)
max_bounds = np.array([float("-inf")] * 3)
found_objects = False
# Check body positions if available
if hasattr(self, "_last_state") and self._last_state is not None:
if hasattr(self._last_state, "body_q") and self._last_state.body_q is not None:
body_q = self._last_state.body_q.numpy()
# body_q is an array of transforms (7 values: 3 pos + 4 quat)
# Extract positions (first 3 values of each transform)
for i in range(len(body_q)):
pos = body_q[i, :3]
min_bounds = np.minimum(min_bounds, pos)
max_bounds = np.maximum(max_bounds, pos)
found_objects = True
# If no objects found, use default bounds
if not found_objects:
min_bounds = np.array([-5.0, -5.0, -5.0])
max_bounds = np.array([5.0, 5.0, 5.0])
# Calculate center and size of bounding box
center = (min_bounds + max_bounds) * 0.5
size = max_bounds - min_bounds
max_extent = np.max(size)
# Ensure minimum size to avoid camera being too close
if max_extent < 1.0:
max_extent = 1.0
# Calculate camera distance based on field of view
# Distance = extent / tan(fov/2) with some padding
fov_rad = np.radians(self.camera.fov)
padding = 1.5
distance = max_extent / (2.0 * np.tan(fov_rad / 2.0)) * padding
# Position camera at distance from current viewing direction, looking at center
from pyglet.math import Vec3 as PyVec3
front = self.camera.get_front()
new_pos = PyVec3(
center[0] - front.x * distance,
center[1] - front.y * distance,
center[2] - front.z * distance,
)
self.camera.pos = new_pos
def _update_camera(self, dt: float):
"""
Update the camera position and orientation based on user input.
Args:
dt: Time delta since last update.
"""
if self._ui_is_capturing_keyboard():
return
# camera-relative basis
forward = np.array(self.camera.get_front(), dtype=np.float32)
right = np.array(self.camera.get_right(), dtype=np.float32)
up = np.array(self.camera.get_up(), dtype=np.float32)
# keep motion in the horizontal plane
forward -= up * float(np.dot(forward, up))
right -= up * float(np.dot(right, up))
# renormalize
fn = float(np.linalg.norm(forward))
ln = float(np.linalg.norm(right))
if fn > 1.0e-6:
forward /= fn
if ln > 1.0e-6:
right /= ln
import pyglet
desired = np.zeros(3, dtype=np.float32)
if self.renderer.is_key_down(pyglet.window.key.W) or self.renderer.is_key_down(pyglet.window.key.UP):
desired += forward
if self.renderer.is_key_down(pyglet.window.key.S) or self.renderer.is_key_down(pyglet.window.key.DOWN):
desired -= forward
if self.renderer.is_key_down(pyglet.window.key.A) or self.renderer.is_key_down(pyglet.window.key.LEFT):
desired -= right # strafe left
if self.renderer.is_key_down(pyglet.window.key.D) or self.renderer.is_key_down(pyglet.window.key.RIGHT):
desired += right # strafe right
if self.renderer.is_key_down(pyglet.window.key.Q):
desired -= up # pan down
if self.renderer.is_key_down(pyglet.window.key.E):
desired += up # pan up
dn = float(np.linalg.norm(desired))
if dn > 1.0e-6:
desired = desired / dn * self._cam_speed
else:
desired[:] = 0.0
tau = max(1.0e-4, float(self._cam_damp_tau))
self._cam_vel += (desired - self._cam_vel) * (dt / tau)
# integrate position
dv = type(self.camera.pos)(*self._cam_vel)
self.camera.pos += dv * dt
def on_resize(self, width: int, height: int):
"""
Handle window resize events.
Args:
width: New window width.
height: New window height.
"""
fb_w, fb_h = self.renderer.window.get_framebuffer_size()
self.camera.update_screen_size(fb_w, fb_h)
self._invalidate_pbo()
if self.ui:
self.ui.resize(width, height)
def _update_fps(self):
"""
Update FPS calculation and statistics.
"""
current_time = time.perf_counter()
self._frame_count += 1
# Update FPS every second
if current_time - self._last_fps_time >= 1.0:
time_delta = current_time - self._last_fps_time
self._current_fps = self._frame_count / time_delta
self._fps_history.append(self._current_fps)
# Keep only last 60 FPS readings
if len(self._fps_history) > 60:
self._fps_history.pop(0)
self._last_fps_time = current_time
self._frame_count = 0
def _render_gizmos(self):
self.gizmo_is_using = False
if not self._gizmo_log:
self._gizmo_active.clear()
return
if not self.ui:
self._gizmo_active.clear()
return
giz = self.ui.giz
io = self.ui.io
# Setup ImGuizmo viewport
giz.set_orthographic(False)
giz.set_rect(0.0, 0.0, float(io.display_size[0]), float(io.display_size[1]))
giz.set_gizmo_size_clip_space(0.07)
giz.set_axis_limit(0.0)
giz.set_plane_limit(0.0)
giz.allow_axis_flip(False)
# Camera matrices
view = self.camera.get_view_matrix().reshape(4, 4).transpose()
proj = self.camera.get_projection_matrix().reshape(4, 4).transpose()
def m44_to_mat16(m):
"""Row-major 4x4 -> giz.Matrix16 (column-major, 16 floats)."""
m = np.asarray(m, dtype=np.float32).reshape(4, 4)
return giz.Matrix16(m.flatten(order="F").tolist())
def safe_bool(value) -> bool:
try:
return bool(value)
except Exception:
return False
view_ = m44_to_mat16(view)
proj_ = m44_to_mat16(proj)
axis_translate = {
Axis.X: giz.OPERATION.translate_x,
Axis.Y: giz.OPERATION.translate_y,
Axis.Z: giz.OPERATION.translate_z,
}
axis_rotate = {
Axis.X: giz.OPERATION.rotate_x,
Axis.Y: giz.OPERATION.rotate_y,
Axis.Z: giz.OPERATION.rotate_z,
}
# Draw & mutate each gizmo
logged_ids = set()
for gid, gizmo_data in self._gizmo_log.items():
logged_ids.add(gid)
transform = gizmo_data["transform"]
snap_to = gizmo_data["snap_to"]
translate = gizmo_data["translate"]
rotate = gizmo_data["rotate"]
# Use compound ops when all axes are active (includes plane handles).
if len(translate) == 3:
t_ops = (giz.OPERATION.translate,)
else:
t_ops = tuple(axis_translate[a] for a in translate)
if len(rotate) == 3:
r_ops = (giz.OPERATION.rotate,)
else:
r_ops = tuple(axis_rotate[a] for a in rotate)
ops = t_ops + r_ops
was_active = self._gizmo_active.get(gid, False)
if not ops:
if was_active and snap_to is not None:
transform[:] = snap_to
self._gizmo_active[gid] = False
continue
giz.push_id(str(gid))
M = wp.transform_to_matrix(transform)
M_ = m44_to_mat16(M)
op_modified = False
for op in ops:
op_modified = safe_bool(giz.manipulate(view_, proj_, op, giz.MODE.world, M_, None, None)) or op_modified
any_gizmo_is_using = safe_bool(giz.is_using_any())
if hasattr(giz, "is_using"):
# manipulate() only reports matrix changes this frame. Keep the
# gizmo active across stationary drag frames until release.
is_active = safe_bool(giz.is_using()) and any_gizmo_is_using
else:
is_active = op_modified or (was_active and any_gizmo_is_using)
if was_active and not is_active and snap_to is not None:
transform[:] = snap_to
else:
M[:] = M_.values.reshape(4, 4, order="F")
transform[:] = wp.transform_from_matrix(M)
self._gizmo_active[gid] = is_active
giz.pop_id()
# Drop stale interaction state for gizmos that are no longer logged.
for gid in tuple(self._gizmo_active):
if gid not in logged_ids:
del self._gizmo_active[gid]
self.gizmo_is_using = giz.is_using_any()
def _render_ui(self):
"""
Render the complete ImGui interface (left panel, stats overlay, and custom UI).
"""
if not self.ui or not self.ui.is_available:
return
# Render gizmos
self._render_gizmos()
# Render left panel
self._render_left_panel()
# Render top-right stats overlay
self._render_stats_overlay()
# allow users to create custom windows
for callback in self._ui_callbacks["free"]:
callback(self.ui.imgui)
def _render_left_panel(self):
"""
Render the left panel with model info and visualization controls.
"""
imgui = self.ui.imgui
# Use theme colors directly
nav_highlight_color = self.ui.get_theme_color(imgui.Col_.nav_cursor, (1.0, 1.0, 1.0, 1.0))
# Position the window on the left side
io = self.ui.io
imgui.set_next_window_pos(imgui.ImVec2(10, 10))
imgui.set_next_window_size(imgui.ImVec2(300, io.display_size[1] - 20))
# Main control panel window - use safe flag values
flags = imgui.WindowFlags_.no_resize.value
if imgui.begin(f"Newton Viewer v{nt.__version__}", flags=flags):
imgui.separator()
# Collapsing headers default-open handling (first frame only)
header_flags = 0
# Panel callbacks (e.g. example browser) - top-level collapsing headers
for callback in self._ui_callbacks["panel"]:
callback(self.ui.imgui)
# Model Information section
if self.model is not None:
imgui.set_next_item_open(True, imgui.Cond_.appearing)
if imgui.collapsing_header("Model Information", flags=header_flags):
imgui.separator()
axis_names = ["X", "Y", "Z"]
imgui.text(f"Up Axis: {axis_names[self.model.up_axis]}")
gravity = self.model.gravity.numpy()[0]
gravity_text = f"Gravity: ({gravity[0]:.2f}, {gravity[1]:.2f}, {gravity[2]:.2f})"
imgui.text(gravity_text)
# Pause simulation checkbox
changed, self._paused = imgui.checkbox("Pause", self._paused)
# Visualization Controls section
imgui.set_next_item_open(True, imgui.Cond_.appearing)
if imgui.collapsing_header("Visualization", flags=header_flags):
imgui.separator()
# Joint visualization
show_joints = self.show_joints
changed, self.show_joints = imgui.checkbox("Show Joints", show_joints)
# Contact visualization
show_contacts = self.show_contacts
changed, self.show_contacts = imgui.checkbox("Show Contacts", show_contacts)
# Particle visualization
show_particles = self.show_particles
changed, self.show_particles = imgui.checkbox("Show Particles", show_particles)
# Spring visualization
show_springs = self.show_springs
changed, self.show_springs = imgui.checkbox("Show Springs", show_springs)
# Center of mass visualization
show_com = self.show_com
changed, self.show_com = imgui.checkbox("Show Center of Mass", show_com)
# Triangle mesh visualization
show_triangles = self.show_triangles
changed, self.show_triangles = imgui.checkbox("Show Cloth", show_triangles)
# Collision geometry toggle
show_collision = self.show_collision
changed, self.show_collision = imgui.checkbox("Show Collision", show_collision)
# Gap + margin wireframe mode
_sdf_margin_labels = ["Off", "Margin", "Margin + Gap"]
_, new_sdf_idx = imgui.combo("Gap + Margin", int(self.sdf_margin_mode), _sdf_margin_labels)
self.sdf_margin_mode = self.SDFMarginMode(new_sdf_idx)
if self.sdf_margin_mode != self.SDFMarginMode.OFF:
_, self.renderer.wireframe_line_width = imgui.slider_float(
"Line Width (px)", self.renderer.wireframe_line_width, 0.5, 5.0
)
# Visual geometry toggle
show_visual = self.show_visual
changed, self.show_visual = imgui.checkbox("Show Visual", show_visual)
# Inertia boxes toggle
show_inertia_boxes = self.show_inertia_boxes
changed, self.show_inertia_boxes = imgui.checkbox("Show Inertia Boxes", show_inertia_boxes)
imgui.set_next_item_open(True, imgui.Cond_.appearing)
if imgui.collapsing_header("Example Options"):
# Render UI callbacks for side panel
for callback in self._ui_callbacks["side"]:
callback(self.ui.imgui)
# Rendering Options section
imgui.set_next_item_open(True, imgui.Cond_.appearing)
if imgui.collapsing_header("Rendering Options"):
imgui.separator()
# VSync
changed, vsync = imgui.checkbox("VSync", self.vsync)
if changed:
self.vsync = vsync
# Sky rendering
changed, self.renderer.draw_sky = imgui.checkbox("Sky", self.renderer.draw_sky)
# Shadow rendering
changed, self.renderer.draw_shadows = imgui.checkbox("Shadows", self.renderer.draw_shadows)
# Wireframe mode
changed, self.renderer.draw_wireframe = imgui.checkbox("Wireframe", self.renderer.draw_wireframe)
def _edit_color3(
label: str, color: tuple[float, float, float]
) -> tuple[bool, tuple[float, float, float]]:
"""Normalize color_edit3 input/output across imgui_bundle versions."""
if _IMGUI_BUNDLE_IMVEC4_COLOR_EDIT3:
changed, updated_color = imgui.color_edit3(label, imgui.ImVec4(*color, 1.0))
return changed, (updated_color.x, updated_color.y, updated_color.z)
changed, updated_color = imgui.color_edit3(label, color)
return changed, (updated_color[0], updated_color[1], updated_color[2])
# Light color
changed, self.renderer._light_color = _edit_color3("Light Color", self.renderer._light_color)
# Sky color
changed, self.renderer.sky_upper = _edit_color3("Sky Color", self.renderer.sky_upper)
# Ground color
changed, self.renderer.sky_lower = _edit_color3("Ground Color", self.renderer.sky_lower)
# Wind Effects section
if self.wind is not None:
imgui.set_next_item_open(False, imgui.Cond_.once)
if imgui.collapsing_header("Wind"):
imgui.separator()
changed, amplitude = imgui.slider_float("Wind Amplitude", self.wind.amplitude, -2.0, 2.0, "%.2f")
if changed:
self.wind.amplitude = amplitude
changed, period = imgui.slider_float("Wind Period", self.wind.period, 1.0, 30.0, "%.2f")
if changed:
self.wind.period = period
changed, frequency = imgui.slider_float("Wind Frequency", self.wind.frequency, 0.1, 5.0, "%.2f")
if changed:
self.wind.frequency = frequency
direction = [self.wind.direction[0], self.wind.direction[1], self.wind.direction[2]]
changed, direction = imgui.slider_float3("Wind Direction", direction, -1.0, 1.0, "%.2f")
if changed:
self.wind.direction = direction
# Camera Information section
imgui.set_next_item_open(True, imgui.Cond_.appearing)
if imgui.collapsing_header("Controls"):
imgui.separator()
pos = self.camera.pos
pos_text = f"Position: ({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})"
imgui.text(pos_text)
imgui.text(f"FOV: {self.camera.fov:.1f}°")
imgui.text(f"Pitch: {self.camera.pitch:.1f}°")
imgui.text(f"Yaw: {self.camera.yaw:.1f}°")
# Camera controls hint
imgui.separator()
imgui.push_style_color(imgui.Col_.text, imgui.ImVec4(*nav_highlight_color))
imgui.text("Controls:")
imgui.pop_style_color()
imgui.text("WASD - Move camera")
imgui.text("QE - Pan up/down")
imgui.text("Left Click - Look around")
imgui.text("Right Click - Pick objects")
imgui.text("Scroll - Zoom")
imgui.text("Space - Pause/Resume")
imgui.text("H - Toggle UI")
imgui.text("F - Frame camera around model")
# Selection API section
self._render_selection_panel()
imgui.end()
def _render_stats_overlay(self):
"""
Render performance stats overlay in the top-right corner.
"""
imgui = self.ui.imgui
io = self.ui.io
# Use fallback color for FPS display
fps_color = (1.0, 1.0, 1.0, 1.0) # Bright white
# Position in top-right corner
window_pos = (io.display_size[0] - 10, 10)
imgui.set_next_window_pos(imgui.ImVec2(window_pos[0], window_pos[1]), pivot=imgui.ImVec2(1.0, 0.0))
# Transparent background, auto-sized, non-resizable/movable - use safe flags
# try:
flags: imgui.WindowFlags = (
imgui.WindowFlags_.no_decoration.value
| imgui.WindowFlags_.always_auto_resize.value
| imgui.WindowFlags_.no_resize.value
| imgui.WindowFlags_.no_saved_settings.value
| imgui.WindowFlags_.no_focus_on_appearing.value
| imgui.WindowFlags_.no_nav.value
| imgui.WindowFlags_.no_move.value
)
# Set semi-transparent background for the overlay window
pushed_window_bg = False
try:
# Preferred API name in pyimgui
imgui.set_next_window_bg_alpha(0.7)
except AttributeError:
# Fallback: temporarily override window bg color alpha
try:
style = imgui.get_style()
bg = style.color_(imgui.Col_.window_bg)
r, g, b = bg.x, bg.y, bg.z
except Exception:
# Reasonable dark default
r, g, b = 0.094, 0.094, 0.094
imgui.push_style_color(imgui.Col_.window_bg, imgui.ImVec4(r, g, b, 0.7))
pushed_window_bg = True
if imgui.begin("Performance Stats", flags=flags):
# FPS display
fps_text = f"FPS: {self._current_fps:.1f}"
imgui.push_style_color(imgui.Col_.text, imgui.ImVec4(*fps_color))
imgui.text(fps_text)
imgui.pop_style_color()
# Model stats
if self.model is not None:
imgui.separator()
imgui.text(f"Worlds: {self.model.world_count}")
imgui.text(f"Bodies: {self.model.body_count}")
imgui.text(f"Shapes: {self.model.shape_count}")
imgui.text(f"Joints: {self.model.joint_count}")
imgui.text(f"Particles: {self.model.particle_count}")
imgui.text(f"Springs: {self.model.spring_count}")
imgui.text(f"Triangles: {self.model.tri_count}")
imgui.text(f"Edges: {self.model.edge_count}")
imgui.text(f"Tetrahedra: {self.model.tet_count}")
# Rendered objects count
imgui.separator()
imgui.text(f"Unique Objects: {len(self.objects)}")
# Custom stats
for callback in self._ui_callbacks["stats"]:
callback(self.ui.imgui)
imgui.end()
# Restore bg color if we pushed it
if pushed_window_bg:
imgui.pop_style_color()
def _render_selection_panel(self):
"""
Render the selection panel for Newton Model introspection.
"""
imgui = self.ui.imgui
# Selection Panel section
header_flags = 0
imgui.set_next_item_open(False, imgui.Cond_.appearing) # Default to closed
if imgui.collapsing_header("Selection API", flags=header_flags):
imgui.separator()
# Check if we have state data available
if self._last_state is None:
imgui.text("No state data available.")
imgui.text("Start simulation to enable selection.")
return
state = self._selection_ui_state
# Display error message if any
if state["error_message"]:
imgui.push_style_color(imgui.Col_.text, imgui.ImVec4(1.0, 0.3, 0.3, 1.0))
imgui.text(f"Error: {state['error_message']}")
imgui.pop_style_color()
imgui.separator()
# Articulation Pattern Input
imgui.text("Articulation Pattern:")
imgui.push_item_width(200)
_changed, state["selected_articulation_pattern"] = imgui.input_text(
"##pattern", state["selected_articulation_pattern"]
)
imgui.pop_item_width()
if imgui.is_item_hovered():
tooltip = "Pattern to match articulations (e.g., '*', 'robot*', 'cartpole')"
imgui.set_tooltip(tooltip)
# Joint filtering
imgui.spacing()
imgui.text("Joint Filters (optional):")
imgui.push_item_width(150)
imgui.text("Include:")
imgui.same_line()
_, state["include_joints"] = imgui.input_text("##inc_joints", state["include_joints"])
if imgui.is_item_hovered():
imgui.set_tooltip("Comma-separated joint names/patterns")
imgui.text("Exclude:")
imgui.same_line()
_, state["exclude_joints"] = imgui.input_text("##exc_joints", state["exclude_joints"])
if imgui.is_item_hovered():
imgui.set_tooltip("Comma-separated joint names/patterns")
imgui.pop_item_width()
# Link filtering
imgui.spacing()
imgui.text("Link Filters (optional):")
imgui.push_item_width(150)
imgui.text("Include:")
imgui.same_line()
_, state["include_links"] = imgui.input_text("##inc_links", state["include_links"])
if imgui.is_item_hovered():
imgui.set_tooltip("Comma-separated link names/patterns")
imgui.text("Exclude:")
imgui.same_line()
_, state["exclude_links"] = imgui.input_text("##exc_links", state["exclude_links"])
if imgui.is_item_hovered():
imgui.set_tooltip("Comma-separated link names/patterns")
imgui.pop_item_width()
# Create View Button
imgui.spacing()
if imgui.button("Create Articulation View"):
self._create_articulation_view()
# Show view info if created
if state["selected_articulation_view"] is not None:
view = state["selected_articulation_view"]
imgui.separator()
imgui.text(f" Count: {view.count}")
imgui.text(f" Joints: {view.joint_count}")
imgui.text(f" Links: {view.link_count}")
imgui.text(f" DOFs: {view.joint_dof_count}")
imgui.text(f" Fixed base: {view.is_fixed_base}")
imgui.text(f" Floating base: {view.is_floating_base}")
# Attribute selector
imgui.spacing()
imgui.text("Select Attribute:")
imgui.push_item_width(150)
if state["selected_attribute"] in state["attribute_options"]:
current_attr_idx = state["attribute_options"].index(state["selected_attribute"])
else:
current_attr_idx = 0
_, new_attr_idx = imgui.combo("##attribute", current_attr_idx, state["attribute_options"])
state["selected_attribute"] = state["attribute_options"][new_attr_idx]
imgui.pop_item_width()
# Toggle values display
_, state["show_values"] = imgui.checkbox("Show Values", state["show_values"])
# Display attribute values if requested
if state["show_values"]:
self._render_attribute_values(view, state["selected_attribute"])
def _create_articulation_view(self):
"""
Create an ArticulationView based on current UI state.
"""
state = self._selection_ui_state
try:
# Clear any previous error
state["error_message"] = ""
# Parse filter strings
if state["include_joints"]:
include_joints = [j.strip() for j in state["include_joints"].split(",") if j.strip()]
else:
include_joints = None
if state["exclude_joints"]:
exclude_joints = [j.strip() for j in state["exclude_joints"].split(",") if j.strip()]
else:
exclude_joints = None
if state["include_links"]:
include_links = [link.strip() for link in state["include_links"].split(",") if link.strip()]
else:
include_links = None
if state["exclude_links"]:
exclude_links = [link.strip() for link in state["exclude_links"].split(",") if link.strip()]
else:
exclude_links = None
# Create ArticulationView
state["selected_articulation_view"] = ArticulationView(
model=self.model,
pattern=state["selected_articulation_pattern"],
include_joints=include_joints,
exclude_joints=exclude_joints,
include_links=include_links,
exclude_links=exclude_links,
verbose=False, # Don't print to console in UI
)
except Exception as e:
state["error_message"] = str(e)
state["selected_articulation_view"] = None
def _render_attribute_values(self, view: ArticulationView, attribute_name: str):
"""
Render the values of the selected attribute in the selection panel.
Args:
view: The current articulation view.
attribute_name: The attribute to display.
"""
imgui = self.ui.imgui
state = self._selection_ui_state
try:
# Determine source based on attribute
if attribute_name.startswith("joint_f"):
# Forces come from control
if hasattr(self, "_last_control") and self._last_control is not None:
source = self._last_control
else:
imgui.text("No control data available for forces")
return
else:
# Other attributes come from state or model
source = self._last_state
# Get the attribute values
# get_attribute returns shape (world_count, count_per_world, value_count, *trailing)
raw_values = view.get_attribute(attribute_name, source).numpy()
imgui.separator()
imgui.text(f"Attribute: {attribute_name}")
imgui.text(f"Shape: {raw_values.shape}")
imgui.text(f"Dtype: {raw_values.dtype}")
# Reshape: (world_count, count_per_world, value_count, *trailing) →
# (world_count, count_per_world * value_count * prod(trailing))
world_count = raw_values.shape[0]
values = raw_values.reshape(world_count, -1)
# World selector
if world_count > 1:
imgui.spacing()
imgui.text("World Selection:")
imgui.push_item_width(100)
state["selected_batch_idx"] = max(0, min(state["selected_batch_idx"], world_count - 1))
_, state["selected_batch_idx"] = imgui.slider_int(
"##batch", state["selected_batch_idx"], 0, world_count - 1
)
imgui.pop_item_width()
imgui.same_line()
imgui.text(f"World {state['selected_batch_idx']} / {world_count}")
batch_idx = state["selected_batch_idx"] if world_count > 1 else 0
flat_values = values[batch_idx]
# Display values as sliders in a scrollable region
imgui.spacing()
imgui.text("Values:")
# Create a child window for scrollable content
child_flags = int(imgui.ChildFlags_.borders)
if imgui.begin_child("values_scroll", imgui.ImVec2(0, 300), child_flags):
names = self._get_attribute_names(view, attribute_name)
self._render_value_sliders(flat_values, names, attribute_name, state)
imgui.end_child()
# Show some statistics for numeric data
if flat_values.dtype.kind in "biufc": # numeric types
imgui.spacing()
if world_count > 1:
imgui.text(f"Statistics for World {batch_idx}:")
else:
imgui.text("Statistics:")
imgui.text(f" Min: {np.min(flat_values):.6f}")
imgui.text(f" Max: {np.max(flat_values):.6f}")
imgui.text(f" Mean: {np.mean(flat_values):.6f}")
if flat_values.size > 1:
imgui.text(f" Std: {np.std(flat_values):.6f}")
except Exception as e:
imgui.text(f"Error getting attribute: {e!s}")
def _get_attribute_names(self, view: ArticulationView, attribute_name: str):
"""
Get the names associated with an attribute (joint names, link names, etc.).
Args:
view: The current articulation view.
attribute_name: The attribute to get names for.
Returns:
list or None: List of names or None if not available.
"""
try:
if attribute_name.startswith("joint_q") or attribute_name.startswith("joint_f"):
# For joint positions/velocities/forces, return DOF names or coord names
if attribute_name == "joint_q":
return view.joint_coord_names
else: # joint_qd, joint_f
return view.joint_dof_names
elif attribute_name.startswith("body_"):
# For body attributes, return body/link names
return view.body_names
else:
return None
except Exception:
return None
def _render_value_sliders(self, values: np.ndarray, names: list[str], attribute_name: str, state: dict):
"""
Render values as individual sliders for each DOF.
Args:
values: Array of values to display.
names: List of names for each value.
attribute_name: The attribute being displayed.
state: UI state dictionary.
"""
imgui = self.ui.imgui
# Determine appropriate slider ranges based on attribute type
if attribute_name.startswith("joint_q"):
# Joint positions - use reasonable angle/position ranges
slider_min, slider_max = -3.14159, 3.14159 # Default to ±π
elif attribute_name.startswith("joint_qd"):
# Joint velocities - use reasonable velocity ranges
slider_min, slider_max = -10.0, 10.0
elif attribute_name.startswith("joint_f"):
# Joint forces - use reasonable force ranges
slider_min, slider_max = -100.0, 100.0
else:
# For other attributes, use data-driven ranges
if len(values) > 0 and values.dtype.kind in "biufc": # numeric
val_min, val_max = float(np.min(values)), float(np.max(values))
val_range = val_max - val_min
if val_range < 1e-6: # Nearly constant values
slider_min = val_min - 1.0
slider_max = val_max + 1.0
else:
# Add 20% padding
padding = val_range * 0.2
slider_min = val_min - padding
slider_max = val_max + padding
else:
slider_min, slider_max = -1.0, 1.0
# Initialize slider state if needed
if "slider_values" not in state:
state["slider_values"] = {}
slider_key = f"{attribute_name}_sliders"
if slider_key not in state["slider_values"]:
state["slider_values"][slider_key] = [float(v) for v in values]
# Ensure slider values array has correct length
current_sliders = state["slider_values"][slider_key]
while len(current_sliders) < len(values):
current_sliders.append(0.0)
while len(current_sliders) > len(values):
current_sliders.pop()
# Update slider values to match current data
for i, val in enumerate(values):
if i < len(current_sliders):
current_sliders[i] = float(val)
# Render sliders (read-only display)
imgui.begin_disabled()
for i, val in enumerate(values):
name = names[i] if names and i < len(names) else f"[{i}]"
if isinstance(val, int | float) or hasattr(val, "dtype"):
# shorten floating base key for ui
# todo: consider doing this in the importers
if name.startswith("floating_base"):
name = "base"
# Truncate name for display but keep full name for tooltip
display_name = name[:8] + "..." if len(name) > 8 else name
# Pad display name to ensure consistent width
display_name = f"{display_name:<11}"
# Show truncated name with tooltip
imgui.text(display_name)
if imgui.is_item_hovered() and len(name) > 8:
imgui.set_tooltip(name)
imgui.same_line()
# Use slider for numeric values with fixed width
imgui.push_item_width(150)
slider_id = f"##{attribute_name}_{i}"
_changed, _new_val = imgui.slider_float(slider_id, current_sliders[i], slider_min, slider_max, "%.6f")
imgui.pop_item_width()
# if changed:
# current_sliders[i] = new_val
else:
# For non-numeric values, just show as text
imgui.text(f"{name}: {val}")
imgui.end_disabled()
================================================
FILE: newton/_src/viewer/viewer_null.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import time as _time
from typing import Any
import numpy as np
import warp as wp
import newton
from ..core.types import override
from .viewer import ViewerBase
class ViewerNull(ViewerBase):
"""
A no-operation (no-op) viewer implementation for Newton.
This class provides a minimal, non-interactive viewer that does not perform any rendering
or visualization. It is intended for use in headless or automated worlds where
visualization is not required. The viewer runs for a fixed number of frames and provides
stub implementations for all logging and frame management methods.
"""
def __init__(
self,
num_frames: int = 1000,
benchmark: bool = False,
benchmark_timeout: float | None = None,
benchmark_start_frame: int = 3,
):
"""
Initialize a no-op Viewer that runs for a fixed number of frames.
Args:
num_frames: The number of frames to run before stopping.
benchmark: Enable benchmark timing (FPS measurement after warmup).
benchmark_timeout: If set, stop after this many seconds of
steady-state simulation (measured after warmup). Implicitly
enables *benchmark*.
benchmark_start_frame: Number of warmup frames before benchmark
timing starts.
"""
super().__init__()
self.num_frames = num_frames
self.frame_count = 0
self.benchmark = benchmark or benchmark_timeout is not None
self.benchmark_timeout = benchmark_timeout
self.benchmark_start_frame = benchmark_start_frame
self._bench_start_time: float | None = None
self._bench_frames = 0
self._bench_elapsed = 0.0
@override
def log_mesh(
self,
name: str,
points: wp.array[wp.vec3],
indices: wp.array[wp.int32] | wp.array[wp.uint32],
normals: wp.array[wp.vec3] | None = None,
uvs: wp.array[wp.vec2] | None = None,
texture: np.ndarray | str | None = None,
hidden: bool = False,
backface_culling: bool = True,
):
"""
No-op implementation for logging a mesh.
Args:
name: Name of the mesh.
points: Vertex positions.
indices: Mesh indices.
normals: Vertex normals (optional).
uvs: Texture coordinates (optional).
texture: Optional texture path/URL or image array.
hidden: Whether the mesh is hidden.
backface_culling: Whether to enable backface culling.
"""
pass
@override
def log_instances(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""
No-op implementation for logging mesh instances.
Args:
name: Name of the instance batch.
mesh: Mesh object.
xforms: Instance transforms.
scales: Instance scales.
colors: Instance colors.
materials: Instance materials.
hidden: Whether the instances are hidden.
"""
pass
@override
def begin_frame(self, time: float):
"""
No-op implementation for beginning a frame.
Args:
time: The current simulation time.
"""
pass
@override
def end_frame(self):
"""
Increment the frame count at the end of each frame.
"""
self.frame_count += 1
if self.benchmark:
if self.frame_count == self.benchmark_start_frame:
wp.synchronize()
self._bench_start_time = _time.perf_counter()
elif self._bench_start_time is not None:
wp.synchronize()
self._bench_frames = self.frame_count - self.benchmark_start_frame
self._bench_elapsed = _time.perf_counter() - self._bench_start_time
@override
def is_running(self) -> bool:
"""
Check if the viewer should continue running.
Returns:
bool: True if the frame count is less than the maximum number of frames
and the benchmark timeout (if any) has not been reached.
"""
if self.frame_count >= self.num_frames:
return False
if (
self.benchmark_timeout is not None
and self._bench_start_time is not None
and self._bench_elapsed >= self.benchmark_timeout
):
return False
return True
def benchmark_result(self) -> dict[str, float | int] | None:
"""Return benchmark results, or ``None`` if benchmarking was not enabled.
Returns:
Dictionary with ``fps``, ``frames``, and ``elapsed`` keys,
or ``None`` if benchmarking is not enabled.
"""
if not self.benchmark:
return None
if self._bench_frames == 0 or self._bench_elapsed == 0.0:
return {"fps": 0.0, "frames": 0, "elapsed": 0.0}
return {
"fps": self._bench_frames / self._bench_elapsed,
"frames": self._bench_frames,
"elapsed": self._bench_elapsed,
}
@override
def close(self):
"""
No-op implementation for closing the viewer.
"""
pass
@override
def log_lines(
self,
name: str,
starts: wp.array[wp.vec3] | None,
ends: wp.array[wp.vec3] | None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None),
width: float = 0.01,
hidden: bool = False,
):
"""
No-op implementation for logging lines.
Args:
name: Name of the line batch.
starts: Line start points.
ends: Line end points.
colors: Line colors.
width: Line width hint.
hidden: Whether the lines are hidden.
"""
pass
@override
def log_points(
self,
name: str,
points: wp.array[wp.vec3] | None,
radii: wp.array[wp.float32] | float | None = None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None) = None,
hidden: bool = False,
):
"""
No-op implementation for logging points.
Args:
name: Name of the point batch.
points: Point positions.
radii: Point radii.
colors: Point colors.
hidden: Whether the points are hidden.
"""
pass
@override
def log_array(self, name: str, array: wp.array[Any] | np.ndarray):
"""
No-op implementation for logging a generic array.
Args:
name: Name of the array.
array: The array data.
"""
pass
@override
def log_scalar(self, name: str, value: int | float | bool | np.number):
"""
No-op implementation for logging a scalar value.
Args:
name: Name of the scalar.
value: The scalar value.
"""
pass
@override
def apply_forces(self, state: newton.State):
"""Null backend does not apply interactive forces.
Args:
state: Current simulation state.
"""
pass
================================================
FILE: newton/_src/viewer/viewer_rerun.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import inspect
import subprocess
from typing import Any
import numpy as np
import warp as wp
import newton
from ..core.types import override
from ..utils.mesh import compute_vertex_normals
from ..utils.texture import load_texture, normalize_texture
from .viewer import ViewerBase, is_jupyter_notebook
try:
import rerun as rr
import rerun.blueprint as rrb
except ImportError:
rr = None
rrb = None
class ViewerRerun(ViewerBase):
"""
ViewerRerun provides a backend for visualizing Newton simulations using the rerun visualization library.
This viewer logs mesh and instance data to rerun, enabling real-time or offline visualization of simulation
geometry and transforms. By default, it spawns a viewer. Alternatively, it can connect to a specific
rerun server address. Multiple parallel simulations are supported—use unique app_id values to differentiate them.
The class manages mesh assets, instanced geometry, and frame/timeline synchronization with rerun.
"""
@staticmethod
def _to_numpy(x) -> np.ndarray | None:
"""Convert warp arrays or other array-like objects to numpy arrays."""
if x is None:
return None
if hasattr(x, "numpy"):
return x.numpy()
return np.asarray(x)
@staticmethod
def _call_rr_constructor(ctor, **kwargs):
"""Call a rerun constructor with only supported keyword args."""
try:
signature = inspect.signature(ctor)
allowed = {k: v for k, v in kwargs.items() if k in signature.parameters}
return ctor(**allowed)
except Exception:
return ctor(**kwargs)
@staticmethod
def _prepare_texture(texture: np.ndarray | str | None) -> np.ndarray | None:
"""Load and normalize texture data for rerun."""
return normalize_texture(
load_texture(texture),
flip_vertical=False,
require_channels=True,
scale_unit_range=True,
)
@staticmethod
def _flip_uvs_for_rerun(uvs: np.ndarray) -> np.ndarray:
"""Rerun uses top-left UV origin; flip V from OpenGL-style UVs."""
uvs = np.asarray(uvs, dtype=np.float32)
if uvs.size == 0:
return uvs
flipped = uvs.copy()
flipped[:, 1] = 1.0 - flipped[:, 1]
return flipped
@staticmethod
def _build_texture_components(texture_image: np.ndarray):
"""Create rerun ImageBuffer/ImageFormat components from a texture."""
if texture_image is None:
return None, None
if rr is None:
return None, None
height, width, channels = texture_image.shape
texture_image = np.ascontiguousarray(texture_image)
try:
color_model = rr.datatypes.ColorModel.RGBA if channels == 4 else rr.datatypes.ColorModel.RGB
except Exception:
color_model = "RGBA" if channels == 4 else "RGB"
try:
channel_dtype = rr.datatypes.ChannelDatatype.U8
except Exception:
channel_dtype = "U8"
texture_buffer = rr.components.ImageBuffer(texture_image.tobytes())
texture_format = rr.components.ImageFormat(
width=int(width),
height=int(height),
color_model=color_model,
channel_datatype=channel_dtype,
)
return texture_buffer, texture_format
def _mesh3d_supports(self, field_name: str) -> bool:
if not self._mesh3d_params:
return True
return field_name in self._mesh3d_params
def __init__(
self,
*,
app_id: str | None = None,
address: str | None = None,
serve_web_viewer: bool = True,
web_port: int = 9090,
grpc_port: int = 9876,
keep_historical_data: bool = False,
keep_scalar_history: bool = True,
record_to_rrd: str | None = None,
):
"""
Initialize the ViewerRerun backend for Newton using the Rerun.io visualization library.
This viewer supports both standalone and Jupyter notebook environments. When an address is provided,
it connects to that remote rerun server regardless of environment. When address is None, it spawns
a local viewer (web-based or standalone, depending on ``serve_web_viewer`` flag), only if not running in a Jupyter notebook (notebooks use show_notebook() instead).
Args:
app_id: Application ID for rerun (defaults to 'newton-viewer').
Use different IDs to differentiate between parallel viewer instances.
address: Optional server address to connect to a remote rerun server via gRPC.
You will need to start a stand-alone rerun server first, e.g. by typing ``rerun`` in your terminal.
See rerun.io documentation for supported address formats.
If provided, connects to the specified server regardless of environment.
serve_web_viewer: If True, serves a web viewer over HTTP on the given ``web_port`` and opens it in the browser.
If False, spawns a native Rerun viewer (only outside Jupyter notebooks).
Defaults to True.
web_port: Port to serve the web viewer on. Only used if ``serve_web_viewer`` is True.
grpc_port: Port to serve the gRPC server on.
keep_historical_data: If True, keep historical data in the timeline of the web viewer.
If False, the web viewer will only show the current frame to keep the memory usage constant when sending transform updates via :meth:`~newton.viewer.ViewerBase.log_state`.
This is useful for visualizing long and complex simulations that would quickly fill up the web viewer's memory if the historical data was kept.
If True, the historical simulation data is kept in the viewer to be able to scrub through the simulation timeline. Defaults to False.
keep_scalar_history: If True, historical scala data logged via :meth:`ViewerRerun.log_scalar` is kept in the viewer.
record_to_rrd: Path to record the viewer to a ``*.rrd`` recording file (e.g. "my_recording.rrd"). If None, the viewer will not record to a file.
"""
if rr is None:
raise ImportError("rerun package is required for ViewerRerun. Install with: pip install rerun-sdk")
super().__init__()
self.app_id = app_id or "newton-viewer"
self._running = True
self._viewer_process = None
self.keep_historical_data = keep_historical_data
self.keep_scalar_history = keep_scalar_history
# Store mesh data for instances
self._meshes = {}
self._instances = {}
# Store scalar data for logging
self._scalars = {}
# Initialize rerun using a blueprint that only shows the 3D view and a collapsed time panel
blueprint = self._get_blueprint()
rr.init(self.app_id, default_blueprint=blueprint)
if record_to_rrd is not None:
rr.save(record_to_rrd, default_blueprint=blueprint)
try:
self._mesh3d_params = set(inspect.signature(rr.Mesh3D).parameters)
except Exception:
self._mesh3d_params = set()
self._grpc_server_uri = None
# Launch viewer client
self.is_jupyter_notebook = is_jupyter_notebook()
if address is not None:
rr.connect_grpc(address)
elif not self.is_jupyter_notebook:
if serve_web_viewer:
self._grpc_server_uri = rr.serve_grpc(grpc_port=grpc_port, default_blueprint=blueprint)
rr.serve_web_viewer(connect_to=self._grpc_server_uri, web_port=web_port)
else:
rr.spawn(port=grpc_port)
# Make sure the timeline is set up
rr.set_time("time", timestamp=0.0)
def _get_blueprint(self):
scalar_panel = None
if len(self._scalars) > 0:
scalar_panel = rrb.TimeSeriesView()
return rrb.Blueprint(
rrb.Horizontal(
*[rrb.Spatial3DView(), scalar_panel] if scalar_panel is not None else [rrb.Spatial3DView()],
),
rrb.TimePanel(timeline="time", state="collapsed"),
collapse_panels=True,
)
@override
def log_mesh(
self,
name: str,
points: wp.array[wp.vec3],
indices: wp.array[wp.int32] | wp.array[wp.uint32],
normals: wp.array[wp.vec3] | None = None,
uvs: wp.array[wp.vec2] | None = None,
texture: np.ndarray | str | None = None,
hidden: bool = False,
backface_culling: bool = True,
):
"""
Log a mesh to rerun for visualization.
Args:
name: Entity path for the mesh.
points: Vertex positions.
indices: Triangle indices.
normals: Vertex normals.
uvs: UV coordinates.
texture: Optional texture path/URL or image array.
hidden: Whether the mesh is hidden.
backface_culling: Whether to enable backface culling (unused).
"""
if not hidden:
assert isinstance(points, wp.array)
assert isinstance(indices, wp.array)
assert normals is None or isinstance(normals, wp.array)
assert uvs is None or isinstance(uvs, wp.array)
# Convert to numpy arrays
points_np = self._to_numpy(points).astype(np.float32)
indices_np = self._to_numpy(indices).astype(np.uint32)
# Rerun expects indices as (N, 3) for triangles
if indices_np.ndim == 1:
indices_np = indices_np.reshape(-1, 3)
if normals is None:
if hidden and not (isinstance(points, wp.array) and isinstance(indices, wp.array)):
# Hidden-mode callers can use lightweight array-like objects that are
# incompatible with compute_vertex_normals.
normals_np = None
else:
normals = compute_vertex_normals(points, indices, device=self.device)
normals_np = self._to_numpy(normals)
else:
normals_np = self._to_numpy(normals)
uvs_np = self._to_numpy(uvs).astype(np.float32) if uvs is not None else None
texture_image = self._prepare_texture(texture)
if uvs_np is not None and len(uvs_np) != len(points_np):
uvs_np = None
texture_image = None
if texture_image is not None and uvs_np is None:
texture_image = None
if uvs_np is not None:
uvs_np = self._flip_uvs_for_rerun(uvs_np)
texture_buffer = None
texture_format = None
if texture_image is not None and self._mesh3d_supports("albedo_texture_buffer"):
texture_buffer, texture_format = self._build_texture_components(texture_image)
# make sure deformable mesh updates are not kept in the viewer if desired
static = name in self._meshes and not self.keep_historical_data
# Store mesh data for instancing
self._meshes[name] = {
"points": points_np,
"indices": indices_np,
"normals": normals_np,
"uvs": uvs_np,
"texture_image": texture_image,
"texture_buffer": texture_buffer,
"texture_format": texture_format,
}
if hidden:
return
mesh_kwargs = {
"vertex_positions": points_np,
"triangle_indices": indices_np,
"vertex_normals": self._meshes[name]["normals"],
}
if uvs_np is not None and self._mesh3d_supports("vertex_texcoords"):
mesh_kwargs["vertex_texcoords"] = uvs_np
if texture_buffer is not None and texture_format is not None:
mesh_kwargs["albedo_texture_buffer"] = texture_buffer
mesh_kwargs["albedo_texture_format"] = texture_format
elif texture_image is not None and self._mesh3d_supports("albedo_texture"):
mesh_kwargs["albedo_texture"] = texture_image
# Log the mesh as a static asset
mesh_3d = self._call_rr_constructor(rr.Mesh3D, **mesh_kwargs)
rr.log(name, mesh_3d, static=static)
@override
def log_instances(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""
Log instanced mesh data to rerun using InstancePoses3D.
Args:
name: Entity path for the instances.
mesh: Name of the mesh asset to instance.
xforms: Instance transforms.
scales: Instance scales.
colors: Instance colors.
materials: Instance materials.
hidden: Whether the instances are hidden.
"""
if hidden:
if name in self._instances:
rr.log(name, rr.Clear(recursive=False))
self._instances.pop(name, None)
return
# Check that mesh exists
if mesh not in self._meshes:
raise RuntimeError(f"Mesh {mesh} not found. Call log_mesh first.")
# re-run needs to generate a new mesh for each instancer
if name not in self._instances:
mesh_data = self._meshes[mesh]
has_texture = (
mesh_data.get("texture_buffer") is not None and mesh_data.get("texture_format") is not None
) or mesh_data.get("texture_image") is not None
# Handle colors - ReRun doesn't support per-instance colors
# so we just use the first instance's color for all instances
vertex_colors = None
if colors is not None and not has_texture:
colors_np = self._to_numpy(colors).astype(np.float32)
# Take the first instance's color and apply to all vertices
first_color = colors_np[0]
color_rgb = np.array(first_color * 255, dtype=np.uint8)
num_vertices = len(mesh_data["points"])
vertex_colors = np.tile(color_rgb, (num_vertices, 1))
# Log the base mesh with optional colors
mesh_kwargs = {
"vertex_positions": mesh_data["points"],
"triangle_indices": mesh_data["indices"],
"vertex_normals": mesh_data["normals"],
}
if vertex_colors is not None:
mesh_kwargs["vertex_colors"] = vertex_colors
if mesh_data.get("uvs") is not None and self._mesh3d_supports("vertex_texcoords"):
mesh_kwargs["vertex_texcoords"] = mesh_data["uvs"]
if mesh_data.get("texture_buffer") is not None and mesh_data.get("texture_format") is not None:
mesh_kwargs["albedo_texture_buffer"] = mesh_data["texture_buffer"]
mesh_kwargs["albedo_texture_format"] = mesh_data["texture_format"]
elif mesh_data.get("texture_image") is not None and self._mesh3d_supports("albedo_texture"):
mesh_kwargs["albedo_texture"] = mesh_data["texture_image"]
mesh_3d = self._call_rr_constructor(rr.Mesh3D, **mesh_kwargs)
rr.log(name, mesh_3d)
# save reference
self._instances[name] = mesh_3d
# hide the reference mesh
rr.log(mesh, rr.Clear(recursive=False))
# Convert transforms and properties to numpy
if xforms is not None:
# Convert warp arrays to numpy first
xforms_np = self._to_numpy(xforms)
# Extract positions and quaternions using vectorized operations
# Warp transform format: [x, y, z, qx, qy, qz, qw]
translations = xforms_np[:, :3].astype(np.float32)
# Warp quaternion is in (x, y, z, w) order,
# rerun expects (x, y, z, w) for Quaternion datatype
quaternions = xforms_np[:, 3:7].astype(np.float32)
scales_np = None
if scales is not None:
scales_np = self._to_numpy(scales).astype(np.float32)
# Colors are already handled in the mesh
# (first instance color applied to all)
# Create instance poses
instance_poses = rr.InstancePoses3D(
translations=translations,
quaternions=quaternions,
scales=scales_np,
)
# Log the instance poses
rr.log(name, instance_poses, static=not self.keep_historical_data)
@override
def begin_frame(self, time: float):
"""
Begin a new frame and set the timeline for rerun.
Args:
time: The current simulation time.
"""
self.time = time
# Set the timeline for this frame
rr.set_time("time", timestamp=time)
@override
def end_frame(self):
"""
End the current frame.
Note:
Rerun handles frame finishing automatically.
"""
# Rerun handles frame finishing automatically
pass
@override
def is_running(self) -> bool:
"""
Check if the viewer is still running.
Returns:
bool: True if the viewer is running, False otherwise.
"""
# Check if viewer process is still alive
if self._viewer_process is not None:
return self._viewer_process.poll() is None
return self._running
@override
def close(self):
"""
Close the viewer and clean up resources.
This will terminate any spawned viewer process and disconnect from rerun.
"""
self._running = False
# Close viewer process if we spawned one
if self._viewer_process is not None:
try:
self._viewer_process.terminate()
self._viewer_process.wait(timeout=5)
except subprocess.TimeoutExpired:
self._viewer_process.kill()
except Exception:
pass
self._viewer_process = None
# Disconnect from rerun
try:
rr.disconnect()
except Exception:
pass
@override
def apply_forces(self, state: newton.State):
"""Rerun backend does not apply interactive forces.
Args:
state: Current simulation state.
"""
pass
@override
def log_lines(
self,
name: str,
starts: wp.array[wp.vec3] | None,
ends: wp.array[wp.vec3] | None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None),
width: float = 0.01,
hidden: bool = False,
):
"""
Log lines for visualization.
Args:
name: Name of the line batch.
starts: Line start points.
ends: Line end points.
colors: Line colors.
width: Line width.
hidden: Whether the lines are hidden.
"""
if hidden:
return # Do not log hidden lines
if starts is None or ends is None:
return # Nothing to log
# Convert inputs to numpy for rerun API compatibility
# Expecting starts/ends as wp arrays or numpy arrays
starts_np = self._to_numpy(starts)
ends_np = self._to_numpy(ends)
colors_np = self._to_numpy(colors) if colors is not None else None
# Both starts and ends should be (N, 3)
if starts_np is None or ends_np is None or len(starts_np) == 0:
return
# LineStrips3D expects a list of line strips, where each strip is a sequence of points
# For disconnected line segments, each segment becomes its own strip of 2 points
line_strips = []
for start, end in zip(starts_np, ends_np, strict=False):
line_strips.append([start, end])
# Prepare line color argument
rr_kwargs = {}
if colors_np is not None:
# If single color for all lines (shape (3,))
if colors_np.ndim == 1 and colors_np.shape[0] == 3:
rr_kwargs["colors"] = colors_np
# If (N,3), per-line colors
elif colors_np.ndim == 2 and colors_np.shape[1] == 3:
rr_kwargs["colors"] = colors_np
if width is not None:
rr_kwargs["radii"] = width
# Log to rerun
rr.log(name, rr.LineStrips3D(line_strips, **rr_kwargs), static=not self.keep_historical_data)
@override
def log_array(self, name: str, array: wp.array[Any] | np.ndarray):
"""
Log a generic array for visualization.
Args:
name: Name of the array.
array: The array data (can be a wp.array or a numpy array).
"""
if array is None:
return
array_np = self._to_numpy(array)
rr.log(name, rr.Scalars(array_np), static=not self.keep_historical_data)
@override
def log_scalar(self, name: str, value: int | float | bool | np.number):
"""
Log a scalar value for visualization.
Args:
name: Name of the scalar.
value: The scalar value.
"""
# Basic scalar logging for rerun: log as a 'Scalar' component (if present)
if name is None:
return
# Only support standard Python/numpy scalars, not generic objects for now
if hasattr(value, "item"):
val = value.item()
else:
val = value
rr.log(name, rr.Scalars(val), static=not self.keep_scalar_history)
if len(self._scalars) == 0:
self._scalars[name] = val
blueprint = self._get_blueprint()
rr.send_blueprint(blueprint)
else:
self._scalars[name] = val
@override
def log_geo(
self,
name: str,
geo_type: int,
geo_scale: tuple[float, ...],
geo_thickness: float,
geo_is_solid: bool,
geo_src: newton.Mesh | newton.Heightfield | None = None,
hidden: bool = False,
):
"""Log a geometry primitive, with plane expansion for infinite planes.
Args:
name: Unique path/name for the geometry asset.
geo_type: Geometry type value from `newton.GeoType`.
geo_scale: Geometry scale tuple interpreted by `geo_type`.
geo_thickness: Shell thickness for mesh-like geometry.
geo_is_solid: Whether mesh geometry is treated as solid.
geo_src: Optional source geometry for mesh-backed types.
hidden: Whether the resulting geometry is hidden.
"""
# Generate vertices/indices for supported primitive types
if geo_type == newton.GeoType.PLANE:
# Handle "infinite" planes encoded with non-positive scales
if geo_scale[0] == 0.0 or geo_scale[1] == 0.0:
extents = self._get_world_extents()
if extents is None:
width, length = 10.0, 10.0
else:
max_extent = max(extents) * 1.5
width = max_extent
length = max_extent
else:
width = geo_scale[0]
length = geo_scale[1] if len(geo_scale) > 1 else 10.0
mesh = newton.Mesh.create_plane(width, length, compute_inertia=False)
points = wp.array(mesh.vertices, dtype=wp.vec3, device=self.device)
normals = wp.array(mesh.normals, dtype=wp.vec3, device=self.device)
uvs = wp.array(mesh.uvs, dtype=wp.vec2, device=self.device)
indices = wp.array(mesh.indices, dtype=wp.int32, device=self.device)
self.log_mesh(name, points, indices, normals, uvs, hidden=hidden)
else:
super().log_geo(name, geo_type, geo_scale, geo_thickness, geo_is_solid, geo_src, hidden)
@override
def log_points(
self,
name: str,
points: wp.array[wp.vec3] | None,
radii: wp.array[wp.float32] | float | None = None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None) = None,
hidden: bool = False,
):
"""
Log points for visualization.
Args:
name: Name of the point batch.
points: Point positions (can be a wp.array or a numpy array).
radii: Point radii (can be a wp.array or a numpy array).
colors: Point colors (can be a wp.array or a numpy array).
hidden: Whether the points are hidden.
"""
if hidden:
# Optionally, skip logging hidden points
return
if points is None:
return
pts = self._to_numpy(points)
n_points = pts.shape[0]
# Handle radii (point size)
if radii is not None:
size = self._to_numpy(radii)
if size.ndim == 0 or size.shape == ():
sizes = np.full((n_points,), float(size))
elif size.shape == (n_points,):
sizes = size
else:
sizes = np.full((n_points,), 0.1)
else:
sizes = np.full((n_points,), 0.1)
# Handle colors
if colors is not None:
cols = self._to_numpy(colors)
if cols.shape == (n_points, 3):
colors_val = cols
elif cols.shape == (3,):
colors_val = np.tile(cols, (n_points, 1))
else:
colors_val = np.full((n_points, 3), 1.0)
else:
colors_val = np.full((n_points, 3), 1.0)
# Log as points to rerun
rr.log(
name,
rr.Points3D(
positions=pts,
radii=sizes,
colors=colors_val,
),
static=not self.keep_historical_data,
)
def show_notebook(self, width: int = 1000, height: int = 400, legacy_notebook_show: bool = False):
"""
Show the viewer in a Jupyter notebook.
Args:
width: Width of the viewer in pixels.
height: Height of the viewer in pixels.
legacy_notebook_show: Whether to use ``rr.legacy_notebook_show`` instead of ``rr.notebook_show`` for displaying the viewer as static HTML with embedded recording data.
"""
if legacy_notebook_show and self.is_jupyter_notebook:
rr.legacy_notebook_show(width=width, height=height, blueprint=self._get_blueprint())
else:
rr.notebook_show(width=width, height=height, blueprint=self._get_blueprint())
def _ipython_display_(self):
"""
Display the viewer in an IPython notebook when the viewer is at the end of a cell.
"""
self.show_notebook()
================================================
FILE: newton/_src/viewer/viewer_usd.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import os
from typing import Any
import numpy as np
import warp as wp
import newton
from ..core.types import override
try:
from pxr import Gf, Sdf, Usd, UsdGeom, Vt
except ImportError:
Gf = Sdf = Usd = UsdGeom = Vt = None
from .viewer import ViewerBase
# transforms a cylinder such that it connects the two points pos0, pos1
def _compute_segment_xform(pos0, pos1):
mid = (pos0 + pos1) * 0.5
height = (pos1 - pos0).GetLength()
dir = (pos1 - pos0) / height
rot = Gf.Rotation()
rot.SetRotateInto((0.0, 0.0, 1.0), Gf.Vec3d(dir))
scale = Gf.Vec3f(1.0, 1.0, height)
return (mid, Gf.Quath(rot.GetQuat()), scale)
def _usd_add_xform(prim):
prim = UsdGeom.Xform(prim)
prim.ClearXformOpOrder()
prim.AddTranslateOp()
prim.AddOrientOp()
prim.AddScaleOp()
def _usd_set_xform(
xform,
pos: tuple | None = None,
rot: tuple | None = None,
scale: tuple | None = None,
time: float = 0.0,
):
xform = UsdGeom.Xform(xform)
xform_ops = xform.GetOrderedXformOps()
if pos is not None:
xform_ops[0].Set(Gf.Vec3d(float(pos[0]), float(pos[1]), float(pos[2])), time)
if rot is not None:
xform_ops[1].Set(Gf.Quatf(float(rot[3]), float(rot[0]), float(rot[1]), float(rot[2])), time)
if scale is not None:
xform_ops[2].Set(Gf.Vec3d(float(scale[0]), float(scale[1]), float(scale[2])), time)
class ViewerUSD(ViewerBase):
"""
USD viewer backend for Newton physics simulations.
This backend creates a USD stage and manages mesh prototypes and instanced rendering
using PointInstancers. It supports time-sampled transforms for efficient playback
and visualization of simulation data.
"""
def __init__(
self,
output_path: str,
fps: int = 60,
up_axis: str = "Z",
num_frames: int | None = 100,
scaling: float = 1.0,
):
"""
Initialize the USD viewer backend for Newton physics simulations.
Args:
output_path: Path to the output USD file.
fps: Frames per second for time sampling. Default is 60.
up_axis: USD up axis, either 'Y' or 'Z'. Default is 'Z'.
num_frames: Maximum number of frames to record. Default is 100. If None, recording is unlimited.
scaling: Uniform scaling applied to the scene root. Default is 1.0.
Raises:
ImportError: If the usd-core package is not installed.
"""
if Usd is None:
raise ImportError("usd-core package is required for ViewerUSD. Install with: pip install usd-core")
super().__init__()
self.output_path = os.path.abspath(output_path)
self.fps = fps
self.up_axis = up_axis
self.num_frames = num_frames
# Create USD stage. If this output path is already registered in the
# current process, reuse and clear the existing layer instead of
# calling CreateNew() again (which raises for duplicate identifiers).
existing_layer = Sdf.Layer.Find(self.output_path)
if existing_layer is not None:
existing_layer.Clear()
self.stage = Usd.Stage.Open(existing_layer)
else:
self.stage = Usd.Stage.CreateNew(self.output_path)
self.stage.SetTimeCodesPerSecond(fps) # number of timeCodes per second for data storage
self.stage.SetFramesPerSecond(fps) # display frame rate (timeline FPS in DCC tools)
self.stage.SetStartTimeCode(0)
axis_token = {
"X": UsdGeom.Tokens.x,
"Y": UsdGeom.Tokens.y,
"Z": UsdGeom.Tokens.z,
}.get(self.up_axis.strip().upper())
UsdGeom.SetStageUpAxis(self.stage, axis_token)
UsdGeom.SetStageMetersPerUnit(self.stage, 1.0)
self.root = UsdGeom.Xform.Define(self.stage, "/root")
# apply root scaling
self.root.ClearXformOpOrder()
s = self.root.AddScaleOp()
s.Set(Gf.Vec3d(float(scaling), float(scaling), float(scaling)), 0.0)
self.stage.SetDefaultPrim(self.root.GetPrim())
# Track meshes and instancers
self._meshes = {} # mesh_name -> prototype_path
self._instancers = {} # instancer_name -> UsdGeomPointInstancer
self._points = {} # point_name -> UsdGeomPoints
# Track current frame
self._frame_index = 0
self._frame_count = 0
self.set_model(None)
@override
def begin_frame(self, time: float):
"""
Begin a new frame at the given simulation time.
Args:
time: The simulation time for the new frame.
"""
super().begin_frame(time)
self._frame_index = int(time * self.fps)
self._frame_count += 1
# Update stage end time if needed
if self._frame_index > self.stage.GetEndTimeCode():
self.stage.SetEndTimeCode(self._frame_index)
@override
def end_frame(self):
"""
End the current frame.
This method is a placeholder for any end-of-frame logic required by the backend.
"""
pass
@override
def is_running(self):
"""
Check if the viewer is still running.
Returns:
bool: False if the frame limit is exceeded, True otherwise.
"""
if self.num_frames is not None:
return self._frame_count < self.num_frames
return True
@override
def close(self):
"""
Finalize and save the USD stage.
This should be called when all logging is complete to ensure the USD file is written.
"""
self.stage.GetRootLayer().Save()
self.stage = None
if self.output_path:
print(f"USD output saved in: {os.path.abspath(self.output_path)}")
def _get_path(self, name):
# Handle both absolute and relative paths correctly
if name.startswith("/"):
return "/root" + name
else:
return "/root/" + name
@override
def log_mesh(
self,
name: str,
points: wp.array[wp.vec3],
indices: wp.array[wp.int32] | wp.array[wp.uint32],
normals: wp.array[wp.vec3] | None = None,
uvs: wp.array[wp.vec2] | None = None,
texture: np.ndarray | str | None = None,
hidden: bool = False,
backface_culling: bool = True,
):
"""
Create a USD mesh prototype from vertex and index data.
Args:
name: Mesh name or Sdf.Path string.
points: Vertex positions as a warp array of wp.vec3.
indices: Triangle indices as a warp array of wp.uint32.
normals: Vertex normals as a warp array of wp.vec3.
uvs: UV coordinates as a warp array of wp.vec2.
texture: Optional texture path/URL or image array.
hidden: If True, mesh will be hidden.
backface_culling: If True, enable backface culling.
"""
# Convert warp arrays to numpy
points_np = points.numpy().astype(np.float32)
indices_np = indices.numpy().astype(np.uint32)
if name not in self._meshes:
self._ensure_scopes_for_path(self.stage, self._get_path(name))
mesh_prim = UsdGeom.Mesh.Define(self.stage, self._get_path(name))
# setup topology once (do not set every frame)
face_vertex_counts = [3] * (len(indices_np) // 3)
mesh_prim.GetFaceVertexCountsAttr().Set(face_vertex_counts)
mesh_prim.GetFaceVertexIndicesAttr().Set(indices_np)
# Store the prototype path
self._meshes[name] = mesh_prim
mesh_prim = self._meshes[name]
mesh_prim.GetPointsAttr().Set(points_np, self._frame_index)
# Set normals if provided
if normals is not None:
normals_np = normals.numpy().astype(np.float32)
mesh_prim.GetNormalsAttr().Set(normals_np, self._frame_index)
mesh_prim.SetNormalsInterpolation(UsdGeom.Tokens.vertex)
# Set UVs if provided (simplified for now)
if uvs is not None:
# TODO: Implement UV support for USD meshes
pass
# how to hide the prototype mesh but not the instances in USD?
mesh_prim.GetVisibilityAttr().Set("inherited" if not hidden else "invisible", self._frame_index)
# log a set of instances as individual mesh prims, slower but makes it easier
# to do post-editing of instance materials etc. default for Newton shapes
@override
def log_instances(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""
Log a batch of mesh instances for rendering.
Args:
name: Unique name for the instancer.
mesh: Name of the base mesh.
xforms: Array of transforms.
scales: Array of scales.
colors: Array of colors.
materials: Array of materials.
hidden: Whether the instances are hidden.
"""
# Get prototype path
if mesh not in self._meshes:
msg = f"Mesh prototype '{mesh}' not found for log_instances(). Call log_mesh() first."
raise RuntimeError(msg)
self._ensure_scopes_for_path(self.stage, self._get_path(name) + "/scope")
if xforms is not None:
xforms = xforms.numpy()
else:
xforms = np.empty((0, 7), dtype=np.float32)
if scales is not None:
scales = scales.numpy()
else:
scales = np.ones((len(xforms), 3), dtype=np.float32)
if colors is not None:
colors = colors.numpy()
for i in range(len(xforms)):
instance_path = self._get_path(name) + f"/instance_{i}"
instance = self.stage.GetPrimAtPath(instance_path)
if not instance:
instance = self.stage.DefinePrim(instance_path)
instance.GetReferences().AddInternalReference(self._get_path(mesh))
UsdGeom.Imageable(instance).GetVisibilityAttr().Set("inherited" if not hidden else "invisible")
_usd_add_xform(instance)
# update transform
if xforms is not None:
pos = xforms[i][:3]
rot = xforms[i][3:7]
_usd_set_xform(instance, pos, rot, scales[i], self._frame_index)
# update color
if colors is not None:
displayColor = UsdGeom.PrimvarsAPI(instance).GetPrimvar("displayColor")
displayColor.Set(colors[i], self._frame_index)
# log a set of instances as a point instancer, faster but less flexible
def log_instances_point_instancer(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | np.ndarray | None,
colors: (
wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | np.ndarray | None
),
materials: wp.array[wp.vec4] | None,
):
"""
Create or update a PointInstancer for mesh instances.
Args:
name: Instancer name or Sdf.Path string.
mesh: Mesh prototype name (must be previously logged).
xforms: Instance transforms as a warp array of wp.transform.
scales: Instance scales as a warp array of wp.vec3.
colors: Instance colors as a warp array of wp.vec3.
materials: Instance materials as a warp array of wp.vec4.
Raises:
RuntimeError: If the mesh prototype is not found.
"""
# Get prototype path
if mesh not in self._meshes:
msg = f"Mesh prototype '{mesh}' not found for log_instances(). Call log_mesh() first."
raise RuntimeError(msg)
num_instances = len(xforms)
# Create instancer if it doesn't exist
if name not in self._instancers:
self._ensure_scopes_for_path(self.stage, self._get_path(name))
instancer = UsdGeom.PointInstancer.Define(self.stage, self._get_path(name))
instancer.CreateIdsAttr().Set(list(range(num_instances)))
instancer.CreateProtoIndicesAttr().Set([0] * num_instances)
UsdGeom.PrimvarsAPI(instancer).CreatePrimvar(
"displayColor", Sdf.ValueTypeNames.Color3fArray, UsdGeom.Tokens.vertex, 1
)
# Set the prototype relationship
instancer.GetPrototypesRel().AddTarget(self._get_path(mesh))
self._instancers[name] = instancer
instancer = self._instancers[name]
# Convert transforms to USD format
if xforms is not None:
xforms_np = xforms.numpy()
# Extract positions from warp transforms using vectorized operations
# Warp transform format: [x, y, z, qx, qy, qz, qw]
positions = xforms_np[:, :3].astype(np.float32)
# Convert quaternion format: Warp (x, y, z, w) → USD (w, (x,y,z))
# USD expects quaternions as Gf.Quath(real, imag_vec3)
quat_w = xforms_np[:, 6].astype(np.float32)
quat_xyz = xforms_np[:, 3:6].astype(np.float32)
# Create orientations list with proper USD quaternion format
orientations = []
for i in range(num_instances):
quat = Gf.Quath(
float(quat_w[i]), Gf.Vec3h(float(quat_xyz[i, 0]), float(quat_xyz[i, 1]), float(quat_xyz[i, 2]))
)
orientations.append(quat)
# Handle scales with numpy operations
if scales is None:
scales = np.ones((num_instances, 3), dtype=np.float32)
elif isinstance(scales, wp.array):
scales = scales.numpy().astype(np.float32)
# Set attributes at current time
instancer.GetPositionsAttr().Set(positions, self._frame_index)
instancer.GetOrientationsAttr().Set(orientations, self._frame_index)
if scales is not None:
instancer.GetScalesAttr().Set(scales, self._frame_index)
if colors is not None:
# Promote colors to proper numpy array format
colors_np = self._promote_colors_to_array(colors, num_instances)
# Set color per-instance
displayColor = UsdGeom.PrimvarsAPI(instancer).GetPrimvar("displayColor")
displayColor.Set(colors_np, self._frame_index)
# Explicit identity indices [0, 1, 2, ...], otherwise OV won't pick them up
indices = Vt.IntArray(range(num_instances))
displayColor.SetIndices(indices, self._frame_index)
# Abstract methods that need basic implementations
@override
def log_lines(
self,
name: str,
starts: wp.array[wp.vec3] | None,
ends: wp.array[wp.vec3] | None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None),
width: float = 0.01,
hidden: bool = False,
):
"""Debug helper to add a line list as a set of capsules
Args:
name: Unique name for the line batch.
starts: The vertices of the lines (wp.array)
ends: The vertices of the lines (wp.array)
colors: The colors of the lines (wp.array)
width: The width of the lines.
hidden: Whether the lines are hidden.
"""
if name not in self._instancers:
self._ensure_scopes_for_path(self.stage, self._get_path(name))
instancer = UsdGeom.PointInstancer.Define(self.stage, self._get_path(name))
# define nested capsule prim
instancer_capsule = UsdGeom.Capsule.Define(self.stage, instancer.GetPath().AppendChild("capsule"))
instancer_capsule.GetRadiusAttr().Set(width)
instancer.CreatePrototypesRel().SetTargets([instancer_capsule.GetPath()])
UsdGeom.PrimvarsAPI(instancer).CreatePrimvar(
"displayColor", Sdf.ValueTypeNames.Color3fArray, UsdGeom.Tokens.vertex, 1
)
self._instancers[name] = instancer
instancer = self._instancers[name]
if starts is not None and ends is not None:
num_lines = int(len(starts))
if num_lines > 0:
# bring to host
starts = starts.numpy()
ends = ends.numpy()
line_positions = []
line_rotations = []
line_scales = []
for i in range(num_lines):
pos0 = starts[i]
pos1 = ends[i]
(pos, rot, scale) = _compute_segment_xform(
Gf.Vec3f(float(pos0[0]), float(pos0[1]), float(pos0[2])),
Gf.Vec3f(float(pos1[0]), float(pos1[1]), float(pos1[2])),
)
line_positions.append(pos)
line_rotations.append(rot)
line_scales.append(scale)
instancer.GetPositionsAttr().Set(line_positions, self._frame_index)
instancer.GetOrientationsAttr().Set(line_rotations, self._frame_index)
instancer.GetScalesAttr().Set(line_scales, self._frame_index)
instancer.GetProtoIndicesAttr().Set([0] * num_lines, self._frame_index)
instancer.CreateIdsAttr().Set(list(range(num_lines)))
if colors is not None:
# Promote colors to proper numpy array format
colors_np = self._promote_colors_to_array(colors, num_lines)
# Set color per-instance
displayColor = UsdGeom.PrimvarsAPI(instancer).GetPrimvar("displayColor")
displayColor.Set(colors_np, self._frame_index)
# Explicit identity indices [0, 1, 2, ...], otherwise OV won't pick them up
indices = Vt.IntArray(range(num_lines))
displayColor.SetIndices(indices, self._frame_index)
instancer.GetVisibilityAttr().Set("inherited" if not hidden else "invisible", self._frame_index)
@override
def log_points(
self,
name: str,
points: wp.array[wp.vec3] | None,
radii: wp.array[wp.float32] | float | None = None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None) = None,
hidden: bool = False,
):
"""Log points as a USD `Points` primitive.
Args:
name: Unique name for the point primitive.
points: Point positions.
radii: Point radii or a single shared radius.
colors: Optional per-point colors or a shared RGB triplet.
hidden: Whether the point primitive is hidden.
Returns:
Sdf.Path of the created/updated points primitive.
"""
if points is None:
return
num_points = len(points)
if radii is None:
radii = 0.1
if np.isscalar(radii):
radius_interp = "constant"
else:
radius_interp = "vertex"
colors, color_interp = self._normalize_point_colors(colors, num_points)
path = self._get_path(name)
instancer = UsdGeom.Points.Get(self.stage, path)
if not instancer:
self._ensure_scopes_for_path(self.stage, path)
instancer = UsdGeom.Points.Define(self.stage, path)
UsdGeom.Primvar(instancer.GetWidthsAttr()).SetInterpolation(radius_interp)
UsdGeom.Primvar(instancer.GetDisplayColorAttr()).SetInterpolation(color_interp)
instancer.GetPointsAttr().Set(points.numpy(), self._frame_index)
# convert radii to widths for USD
if np.isscalar(radii):
widths = (radii * 2.0,)
elif isinstance(radii, wp.array):
widths = radii.numpy() * 2.0
else:
widths = np.array(radii) * 2.0
instancer.GetWidthsAttr().Set(widths, self._frame_index)
if colors is not None:
instancer.GetDisplayColorAttr().Set(colors, self._frame_index)
instancer.GetVisibilityAttr().Set("inherited" if not hidden else "invisible", self._frame_index)
return instancer.GetPath()
@override
def log_array(self, name: str, array: wp.array[Any] | np.ndarray):
"""
Log array data (not implemented for USD backend).
This method is a placeholder and does not log array data in the USD backend.
Args:
name: Unique path/name for the array signal.
array: Array data to visualize.
"""
pass
@override
def log_scalar(self, name: str, value: int | float | bool | np.number):
"""
Log scalar value (not implemented for USD backend).
This method is a placeholder and does not log scalar values in the USD backend.
Args:
name: Unique path/name for the scalar signal.
value: Scalar value to visualize.
"""
pass
@override
def apply_forces(self, state: newton.State):
"""USD backend does not apply interactive forces.
Args:
state: Current simulation state.
"""
pass
def _promote_colors_to_array(self, colors, num_items):
"""
Helper method to promote colors to a numpy array format.
Parameters:
colors: Input colors in various formats (wp.array, list/tuple, np.ndarray)
num_items: Number of items that need colors
Returns:
np.ndarray: Colors as numpy array with shape (num_items, 3)
"""
if colors is None:
return None
if isinstance(colors, wp.array):
# Convert warp array to numpy
return colors.numpy()
elif isinstance(colors, list | tuple) and len(colors) == 3 and all(np.isscalar(x) for x in colors):
# Single color (list/tuple of 3 floats) - promote to array with one value per item
return np.tile(colors, (num_items, 1))
elif isinstance(colors, np.ndarray):
# Already numpy array - pass through
return colors
else:
# Fallback for other formats
return np.array(colors)
@staticmethod
def _is_single_rgb_triplet(colors) -> bool:
"""Returns True when colors represent one RGB triplet."""
if isinstance(colors, np.ndarray):
return colors.ndim == 1 and colors.shape[0] == 3
if isinstance(colors, list | tuple):
return len(colors) == 3 and all(np.isscalar(x) for x in colors)
return False
def _normalize_point_colors(self, colors, num_points):
"""Normalize point colors and return (values, interpolation token)."""
if colors is None:
return None, "constant"
if isinstance(colors, wp.array):
colors = colors.numpy()
if self._is_single_rgb_triplet(colors):
colors_arr = np.asarray(colors, dtype=np.float32)
return colors_arr.reshape(1, 3), "constant"
if isinstance(colors, np.ndarray):
return colors, "vertex"
if isinstance(colors, list | tuple):
# Keep list/tuple inputs as-is for existing valid per-point color inputs.
if len(colors) == num_points:
return colors, "vertex"
return np.asarray(colors), "vertex"
return np.asarray(colors), "vertex"
@staticmethod
def _ensure_scopes_for_path(stage: Usd.Stage, prim_path_str: str):
"""
Ensure that all parent prims in the hierarchy exist as 'Scope' prims.
If a prim does not exist at the given path, this method creates all
non-existent parent prims in its hierarchy as 'Scope' prims. This is
useful for ensuring a valid hierarchy before defining a prim.
Parameters:
stage: The USD stage to operate on.
prim_path_str: The Sdf.Path string for the target prim.
"""
# Convert the string to an Sdf.Path object for robust manipulation
prim_path = Sdf.Path(prim_path_str)
# First, check if the target prim already exists.
if stage.GetPrimAtPath(prim_path):
return
# We only want to create the parent hierarchy, not the final prim itself.
parent_path = prim_path.GetParentPath()
# GetPrefixes() provides a convenient list of all ancestor paths.
# For "/A/B/C", it returns ["/", "/A", "/A/B"].
for path in parent_path.GetPrefixes():
# The absolute root path ('/') always exists, so we can skip it.
if path == Sdf.Path.absoluteRootPath:
continue
# Check if a prim exists at the current ancestor path.
if not stage.GetPrimAtPath(path):
stage.DefinePrim(path, "Scope")
================================================
FILE: newton/_src/viewer/viewer_viser.py
================================================
# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers
# SPDX-License-Identifier: Apache-2.0
from __future__ import annotations
import inspect
import os
import warnings
from pathlib import Path
from typing import Any, ClassVar
import numpy as np
import warp as wp
import newton
from ..core.types import override
from ..utils.texture import load_texture, normalize_texture
from .viewer import ViewerBase, is_jupyter_notebook
class ViewerViser(ViewerBase):
"""
ViewerViser provides a backend for visualizing Newton simulations using the viser library.
Viser is a Python library for interactive 3D visualization in the browser. This viewer
launches a web server and renders simulation geometry via WebGL. It supports both
standalone browser viewing and Jupyter notebook integration.
Features:
- Real-time 3D visualization in any web browser
- Jupyter notebook integration with inline display
- Static HTML export for sharing visualizations
- Interactive camera controls
"""
_viser_module = None
@classmethod
def _get_viser(cls):
"""Lazily import and return the viser module."""
if cls._viser_module is None:
try:
import viser
cls._viser_module = viser
except ImportError as e:
raise ImportError("viser package is required for ViewerViser. Install with: pip install viser") from e
return cls._viser_module
@staticmethod
def _to_numpy(x) -> np.ndarray | None:
"""Convert warp arrays or other array-like objects to numpy arrays."""
if x is None:
return None
if hasattr(x, "numpy"):
return x.numpy()
return np.asarray(x)
@staticmethod
def _prepare_texture(texture: np.ndarray | str | None) -> np.ndarray | None:
"""Load and normalize texture data for viser/glTF usage."""
return normalize_texture(
load_texture(texture),
flip_vertical=False,
require_channels=True,
scale_unit_range=True,
)
@staticmethod
def _build_trimesh_mesh(points: np.ndarray, indices: np.ndarray, uvs: np.ndarray, texture: np.ndarray):
"""Create a trimesh object with texture visuals (if trimesh is available)."""
try:
import trimesh
except Exception:
return None
if len(uvs) != len(points):
return None
faces = indices.astype(np.int64)
mesh = trimesh.Trimesh(vertices=points, faces=faces, process=False)
try:
from PIL import Image
from trimesh.visual.texture import TextureVisuals
image = Image.fromarray(texture)
mesh.visual = TextureVisuals(uv=uvs, image=image)
except Exception:
visual_mod = getattr(trimesh, "visual", None)
TextureVisuals = getattr(visual_mod, "TextureVisuals", None) if visual_mod is not None else None
if TextureVisuals is not None:
mesh.visual = TextureVisuals(uv=uvs, image=texture)
return mesh
def __init__(
self,
*,
port: int = 8080,
label: str | None = None,
verbose: bool = True,
share: bool = False,
record_to_viser: str | None = None,
):
"""
Initialize the ViewerViser backend for Newton using the viser visualization library.
This viewer supports both standalone browser viewing and Jupyter notebook environments.
It launches a web server that serves an interactive 3D visualization.
Args:
port: Port number for the web server. Defaults to 8080.
label: Optional label for the viser server window title.
verbose: If True, print the server URL when starting. Defaults to True.
share: If True, create a publicly accessible URL via viser's share feature.
record_to_viser: Path to record the viewer to a ``*.viser`` recording file
(e.g. "my_recording.viser"). If None, the viewer will not record to a file.
"""
viser = self._get_viser()
super().__init__()
self._running = True
self.verbose = verbose
# Store mesh data for instances
self._meshes = {}
self._instances = {}
self._scene_handles = {} # Track viser scene node handles
# Initialize viser server
self._server = viser.ViserServer(port=port, label=label or "Newton Viewer")
self._camera_request: tuple[np.ndarray, np.ndarray, np.ndarray] | None = None
self._pending_camera_clients: set[int] = set()
self._server.on_client_connect(self._handle_client_connect)
self._server.on_client_disconnect(self._handle_client_disconnect)
if share:
self._share_url = self._server.request_share_url()
if verbose:
print(f"Viser share URL: {self._share_url}")
else:
self._share_url = None
if verbose:
print(f"Viser server running at: http://localhost:{port}")
# Store configuration
self._port = port
# Track if running in Jupyter
self.is_jupyter_notebook = is_jupyter_notebook()
# Recording state
self._frame_dt = 0.0
self._record_to_viser = record_to_viser
self._serializer = self._server.get_scene_serializer() if record_to_viser else None
# Set up default scene
self._setup_scene()
if self._serializer is not None and verbose:
print(f"Recording to: {record_to_viser}")
def _setup_scene(self):
"""Set up the default scene configuration."""
self._server.scene.add_light_ambient("ambient_light")
# remove HDR map
self._server.scene.configure_environment_map(hdri=None)
@staticmethod
def _call_scene_method(method, **kwargs):
"""Call a viser scene method with only supported keyword args."""
try:
signature = inspect.signature(method)
allowed = {k: v for k, v in kwargs.items() if k in signature.parameters}
return method(**allowed)
except Exception:
return method(**kwargs)
@property
def url(self) -> str:
"""Get the URL of the viser server.
Returns:
str: Local HTTP URL for the running viser server.
"""
return f"http://localhost:{self._port}"
@staticmethod
def _is_client_camera_ready(client: Any) -> bool:
"""Return True if the client has reported an initial camera state."""
try:
update_timestamp = float(client.camera.update_timestamp)
except Exception:
# Older viser versions may not expose update_timestamp.
try:
_ = client.camera.position
except Exception:
return False
return True
return update_timestamp > 0.0
def _handle_client_connect(self, client: Any):
"""Apply cached camera settings to newly connected clients."""
self._pending_camera_clients.discard(int(client.client_id))
self._apply_camera_to_client(client)
def _handle_client_disconnect(self, client: Any):
"""Clear pending camera state for disconnected clients."""
self._pending_camera_clients.discard(int(client.client_id))
def _get_camera_up_axis(self) -> int:
"""Resolve the model up-axis as an integer index (0, 1, 2)."""
if self.model is None:
return 2
up_axis = self.model.up_axis
if isinstance(up_axis, str):
return "XYZ".index(up_axis.upper())
return int(up_axis)
def _compute_camera_front_up(self, pitch: float, yaw: float) -> tuple[np.ndarray, np.ndarray]:
"""Compute camera front and up vectors from pitch/yaw angles."""
pitch = float(np.clip(pitch, -89.0, 89.0))
yaw = float(yaw)
pitch_rad = np.deg2rad(pitch)
yaw_rad = np.deg2rad(yaw)
up_axis = self._get_camera_up_axis()
if up_axis == 0: # X up
front = np.array(
[
np.sin(pitch_rad),
np.cos(yaw_rad) * np.cos(pitch_rad),
np.sin(yaw_rad) * np.cos(pitch_rad),
],
dtype=np.float64,
)
world_up = np.array([1.0, 0.0, 0.0], dtype=np.float64)
elif up_axis == 2: # Z up
front = np.array(
[
np.cos(yaw_rad) * np.cos(pitch_rad),
np.sin(yaw_rad) * np.cos(pitch_rad),
np.sin(pitch_rad),
],
dtype=np.float64,
)
world_up = np.array([0.0, 0.0, 1.0], dtype=np.float64)
else: # Y up
front = np.array(
[
np.cos(yaw_rad) * np.cos(pitch_rad),
np.sin(pitch_rad),
np.sin(yaw_rad) * np.cos(pitch_rad),
],
dtype=np.float64,
)
world_up = np.array([0.0, 1.0, 0.0], dtype=np.float64)
front /= np.linalg.norm(front)
right = np.cross(front, world_up)
right_norm = np.linalg.norm(right)
if right_norm < 1.0e-8:
fallback_up = np.array([0.0, 1.0, 0.0], dtype=np.float64)
if np.linalg.norm(np.cross(front, fallback_up)) < 1.0e-8:
fallback_up = np.array([0.0, 0.0, 1.0], dtype=np.float64)
right = np.cross(front, fallback_up)
right_norm = np.linalg.norm(right)
right /= right_norm
up = np.cross(right, front)
up /= np.linalg.norm(up)
return front, up
def _apply_camera_to_client(self, client: Any):
"""Apply the cached camera request to a connected client if ready."""
if self._camera_request is None:
return
client_id = int(client.client_id)
if not self._is_client_camera_ready(client):
if client_id in self._pending_camera_clients:
return
self._pending_camera_clients.add(client_id)
def _on_camera_update(_camera: Any):
if client_id not in self._pending_camera_clients:
return
self._pending_camera_clients.discard(client_id)
self._apply_camera_to_client(client)
client.camera.on_update(_on_camera_update)
return
self._pending_camera_clients.discard(client_id)
position, look_at, up_direction = self._camera_request
# Keep camera updates synchronized to avoid transient jitter.
if hasattr(client, "atomic"):
with client.atomic():
client.camera.position = tuple(position.tolist())
client.camera.look_at = tuple(look_at.tolist())
client.camera.up_direction = tuple(up_direction.tolist())
else:
client.camera.position = tuple(position.tolist())
client.camera.look_at = tuple(look_at.tolist())
client.camera.up_direction = tuple(up_direction.tolist())
@override
def set_camera(self, pos: wp.vec3, pitch: float, yaw: float):
"""Set camera position and orientation for connected Viser clients.
The requested view is also cached so that newly connected clients receive
the same camera setup as soon as they report camera state.
Args:
pos: Requested camera position.
pitch: Requested camera pitch angle.
yaw: Requested camera yaw angle.
"""
position = np.asarray((float(pos[0]), float(pos[1]), float(pos[2])), dtype=np.float64)
front, up_direction = self._compute_camera_front_up(pitch, yaw)
look_at = position + front
self._camera_request = (position, look_at, up_direction)
if hasattr(self._server, "initial_camera"):
self._server.initial_camera.position = tuple(position.tolist())
self._server.initial_camera.look_at = tuple(look_at.tolist())
if hasattr(self._server.initial_camera, "up"):
self._server.initial_camera.up = tuple(up_direction.tolist())
elif hasattr(self._server.initial_camera, "up_direction"):
self._server.initial_camera.up_direction = tuple(up_direction.tolist())
for client in self._server.get_clients().values():
self._apply_camera_to_client(client)
@staticmethod
def _camera_query_from_request(camera_request: tuple[np.ndarray, np.ndarray, np.ndarray] | None) -> str:
"""Build URL query parameters for playback initial camera overrides."""
if camera_request is None:
return ""
position, look_at, up_direction = camera_request
def _format_vec3(values: np.ndarray) -> str:
values = np.asarray(values, dtype=np.float64).reshape(3)
return ",".join(f"{float(v):.9g}" for v in values)
return (
f"&initialCameraPosition={_format_vec3(position)}"
f"&initialCameraLookAt={_format_vec3(look_at)}"
f"&initialCameraUp={_format_vec3(up_direction)}"
)
@override
def log_mesh(
self,
name: str,
points: wp.array[wp.vec3],
indices: wp.array[wp.int32] | wp.array[wp.uint32],
normals: wp.array[wp.vec3] | None = None,
uvs: wp.array[wp.vec2] | None = None,
texture: np.ndarray | str | None = None,
hidden: bool = False,
backface_culling: bool = True,
):
"""
Log a mesh to viser for visualization.
Args:
name: Entity path for the mesh.
points: Vertex positions.
indices: Triangle indices.
normals: Vertex normals, unused in viser.
uvs: UV coordinates, used for textures if supported.
texture: Texture path/URL or image array (H, W, C).
hidden: Whether the mesh is hidden.
backface_culling: Whether to enable backface culling.
"""
assert isinstance(points, wp.array)
assert isinstance(indices, wp.array)
# Convert to numpy arrays
points_np = self._to_numpy(points).astype(np.float32)
indices_np = self._to_numpy(indices).astype(np.uint32)
uvs_np = self._to_numpy(uvs).astype(np.float32) if uvs is not None else None
texture_image = self._prepare_texture(texture)
if texture_image is not None and uvs_np is None:
warnings.warn(f"Mesh {name} has a texture but no UVs; texture will be ignored.", stacklevel=2)
texture_image = None
if texture_image is not None and uvs_np is not None and len(uvs_np) != len(points_np):
warnings.warn(
f"Mesh {name} has {len(uvs_np)} UVs for {len(points_np)} vertices; texture will be ignored.",
stacklevel=2,
)
texture_image = None
# Viser expects indices as (N, 3) for triangles
if indices_np.ndim == 1:
indices_np = indices_np.reshape(-1, 3)
trimesh_mesh = None
if texture_image is not None and uvs_np is not None:
trimesh_mesh = self._build_trimesh_mesh(points_np, indices_np, uvs_np, texture_image)
if trimesh_mesh is None:
warnings.warn(
"Viser textured meshes require trimesh; falling back to untextured rendering.",
stacklevel=2,
)
# Store mesh data for instancing
self._meshes[name] = {
"points": points_np,
"indices": indices_np,
"uvs": uvs_np,
"texture": texture_image,
"trimesh": trimesh_mesh,
}
# Remove existing mesh if present
if name in self._scene_handles:
try:
self._scene_handles[name].remove()
except Exception:
pass
if hidden:
return
# Add mesh to viser scene
if trimesh_mesh is not None:
handle = self._call_scene_method(
self._server.scene.add_mesh_trimesh,
name=name,
mesh=trimesh_mesh,
)
else:
handle = self._call_scene_method(
self._server.scene.add_mesh_simple,
name=name,
vertices=points_np,
faces=indices_np,
color=(180, 180, 180), # Default gray color
wireframe=False,
side="double" if not backface_culling else "front",
)
self._scene_handles[name] = handle
@override
def log_instances(
self,
name: str,
mesh: str,
xforms: wp.array[wp.transform] | None,
scales: wp.array[wp.vec3] | None,
colors: wp.array[wp.vec3] | None,
materials: wp.array[wp.vec4] | None,
hidden: bool = False,
):
"""
Log instanced mesh data to viser using efficient batched rendering.
Uses viser's add_batched_meshes_simple for GPU-accelerated instanced rendering.
Supports in-place updates of transforms for real-time animation.
Args:
name: Entity path for the instances.
mesh: Name of the mesh asset to instance.
xforms: Instance transforms.
scales: Instance scales.
colors: Instance colors.
materials: Instance materials.
hidden: Whether the instances are hidden.
"""
# Check that mesh exists
if mesh not in self._meshes:
raise RuntimeError(f"Mesh {mesh} not found. Call log_mesh first.")
mesh_data = self._meshes[mesh]
base_points = mesh_data["points"]
base_indices = mesh_data["indices"]
base_uvs = mesh_data.get("uvs")
texture_image = self._prepare_texture(mesh_data.get("texture"))
trimesh_mesh = mesh_data.get("trimesh")
if hidden:
# Remove existing instances if present
if name in self._scene_handles:
try:
self._scene_handles[name].remove()
except Exception:
pass
del self._scene_handles[name]
if name in self._instances:
del self._instances[name]
return
# Convert transforms and properties to numpy
if xforms is None:
return
xforms_np = self._to_numpy(xforms)
scales_np = self._to_numpy(scales) if scales is not None else None
colors_np = self._to_numpy(colors) if colors is not None else None
num_instances = len(xforms_np)
# Extract positions from transforms
# Warp transform format: [x, y, z, qx, qy, qz, qw]
positions = xforms_np[:, :3].astype(np.float32)
# Convert quaternions from Warp format (x, y, z, w) to viser format (w, x, y, z)
quats_xyzw = xforms_np[:, 3:7]
quats_wxyz = np.zeros((num_instances, 4), dtype=np.float32)
quats_wxyz[:, 0] = quats_xyzw[:, 3] # w
quats_wxyz[:, 1] = quats_xyzw[:, 0] # x
quats_wxyz[:, 2] = quats_xyzw[:, 1] # y
quats_wxyz[:, 3] = quats_xyzw[:, 2] # z
# Prepare scales
if scales_np is not None:
batched_scales = scales_np.astype(np.float32)
else:
batched_scales = np.ones((num_instances, 3), dtype=np.float32)
# Prepare colors (convert from 0-1 float to 0-255 uint8)
if colors_np is not None:
batched_colors = (colors_np * 255).astype(np.uint8)
else:
batched_colors = None # Will use cached colors or default gray
# Check if we already have a batched mesh handle for this name
use_trimesh = trimesh_mesh is not None and texture_image is not None and base_uvs is not None
if name in self._instances and name in self._scene_handles:
# Update existing batched mesh in-place (much faster)
handle = self._scene_handles[name]
prev_count = self._instances[name]["count"]
prev_use_trimesh = self._instances[name].get("use_trimesh", False)
# If instance count changed, we need to recreate the mesh
if prev_count != num_instances or prev_use_trimesh != use_trimesh:
try:
handle.remove()
except Exception:
pass
del self._scene_handles[name]
del self._instances[name]
else:
# Update transforms in-place
try:
handle.batched_positions = positions
handle.batched_wxyzs = quats_wxyz
if hasattr(handle, "batched_scales"):
handle.batched_scales = batched_scales
# Only update colors if they were explicitly provided
if batched_colors is not None and hasattr(handle, "batched_colors"):
handle.batched_colors = batched_colors
# Cache the colors for future reference
self._instances[name]["colors"] = batched_colors
return
except Exception:
# If update fails, recreate the mesh
try:
handle.remove()
except Exception:
pass
del self._scene_handles[name]
del self._instances[name]
# For new instances, use provided colors or default gray
if batched_colors is None:
batched_colors = np.full((num_instances, 3), 180, dtype=np.uint8)
# Create new batched mesh
if use_trimesh:
handle = self._call_scene_method(
self._server.scene.add_batched_meshes_trimesh,
name=name,
mesh=trimesh_mesh,
batched_positions=positions,
batched_wxyzs=quats_wxyz,
batched_scales=batched_scales,
lod="auto",
)
else:
handle = self._call_scene_method(
self._server.scene.add_batched_meshes_simple,
name=name,
vertices=base_points,
faces=base_indices,
batched_positions=positions,
batched_wxyzs=quats_wxyz,
batched_scales=batched_scales,
batched_colors=batched_colors,
lod="auto",
)
self._scene_handles[name] = handle
self._instances[name] = {
"mesh": mesh,
"count": num_instances,
"colors": batched_colors, # Cache the colors
"use_trimesh": use_trimesh,
}
@override
def begin_frame(self, time: float):
"""
Begin a new frame.
Args:
time: The current simulation time.
"""
self._frame_dt = time - self.time
self.time = time
@override
def end_frame(self):
"""
End the current frame.
If recording is active, inserts a sleep command for playback timing.
"""
if self._serializer is not None:
# Insert sleep for frame timing during recording
self._serializer.insert_sleep(self._frame_dt)
@override
def is_running(self) -> bool:
"""
Check if the viewer is still running.
Returns:
bool: True if the viewer is running, False otherwise.
"""
return self._running
@override
def close(self):
"""
Close the viewer and clean up resources.
"""
self._running = False
try:
self._server.stop()
if self._serializer is not None:
self.save_recording()
except Exception:
pass
@override
def apply_forces(self, state: newton.State):
"""Viser backend does not apply interactive forces.
Args:
state: Current simulation state.
"""
pass
def save_recording(self):
"""
Save the current recording to a .viser file.
The recording can be played back in a static HTML viewer.
See build_static_viewer() for creating the HTML player.
Note:
Recording must be enabled by passing ``record_to_viser`` to the constructor.
Example:
.. code-block:: python
viewer = ViewerViser(record_to_viser="my_simulation.viser")
# ... run simulation ...
viewer.save_recording()
"""
if self._serializer is None or self._record_to_viser is None:
raise RuntimeError("No recording in progress. Pass record_to_viser to the constructor.")
from pathlib import Path # noqa: PLC0415
data = self._serializer.serialize()
Path(self._record_to_viser).write_bytes(data)
self._serializer = None
if self.verbose:
print(f"Recording saved to: {self._record_to_viser}")
@override
def log_lines(
self,
name: str,
starts: wp.array[wp.vec3] | None,
ends: wp.array[wp.vec3] | None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None),
width: float = 0.01,
hidden: bool = False,
):
"""
Log lines for visualization.
Args:
name: Name of the line batch.
starts: Line start points.
ends: Line end points.
colors: Line colors.
width: Line width.
hidden: Whether the lines are hidden.
"""
# Remove existing lines if present
if name in self._scene_handles:
try:
self._scene_handles[name].remove()
except Exception:
pass
if hidden:
return
if starts is None or ends is None:
return
starts_np = self._to_numpy(starts)
ends_np = self._to_numpy(ends)
if starts_np is None or ends_np is None or len(starts_np) == 0:
return
starts_np = np.asarray(starts_np, dtype=np.float32)
ends_np = np.asarray(ends_np, dtype=np.float32)
num_lines = len(starts_np)
# Viser requires points with shape (N, 2, 3): [start, end] per segment.
line_points = np.stack((starts_np, ends_np), axis=1)
def _rgb_to_uint8_array(rgb: np.ndarray) -> np.ndarray:
rgb = np.asarray(rgb, dtype=np.float32)
max_val = float(np.max(rgb)) if rgb.size > 0 else 0.0
if max_val <= 1.0:
rgb = rgb * 255.0
return np.clip(rgb, 0, 255).astype(np.uint8)
# Process colors
color_rgb: tuple[int, int, int] | np.ndarray = (0, 255, 0)
if colors is not None:
colors_np = self._to_numpy(colors)
if colors_np is not None:
colors_np = np.asarray(colors_np)
if colors_np.ndim == 1 and colors_np.shape[0] == 3:
# Single color for all lines.
color_rgb = tuple(_rgb_to_uint8_array(colors_np).tolist())
elif colors_np.ndim == 2 and colors_np.shape == (num_lines, 3):
# Per-line colors: repeat each line color for [start, end].
line_colors = _rgb_to_uint8_array(colors_np)
color_rgb = np.repeat(line_colors[:, None, :], 2, axis=1)
elif colors_np.ndim == 3 and colors_np.shape == (num_lines, 2, 3):
# Already per-point-per-segment colors.
color_rgb = _rgb_to_uint8_array(colors_np)
# Add line segments to viser
handle = self._server.scene.add_line_segments(
name=name,
points=line_points,
colors=color_rgb,
line_width=width * 100, # Scale for visibility
)
self._scene_handles[name] = handle
@override
def log_geo(
self,
name: str,
geo_type: int,
geo_scale: tuple[float, ...],
geo_thickness: float,
geo_is_solid: bool,
geo_src: newton.Mesh | newton.Heightfield | None = None,
hidden: bool = False,
):
"""Log a geometry primitive, with plane expansion for infinite planes.
Args:
name: Unique path/name for the geometry asset.
geo_type: Geometry type value from `newton.GeoType`.
geo_scale: Geometry scale tuple interpreted by `geo_type`.
geo_thickness: Shell thickness for mesh-like geometry.
geo_is_solid: Whether mesh geometry is treated as solid.
geo_src: Optional source geometry for mesh-backed types.
hidden: Whether the resulting geometry is hidden.
"""
if geo_type == newton.GeoType.PLANE:
# Handle "infinite" planes encoded with non-positive scales
if geo_scale[0] == 0.0 or geo_scale[1] == 0.0:
extents = self._get_world_extents()
if extents is None:
width, length = 10.0, 10.0
else:
max_extent = max(extents) * 1.5
width = max_extent
length = max_extent
else:
width = geo_scale[0]
length = geo_scale[1] if len(geo_scale) > 1 else 10.0
mesh = newton.Mesh.create_plane(width, length, compute_inertia=False)
points = wp.array(mesh.vertices, dtype=wp.vec3, device=self.device)
normals = wp.array(mesh.normals, dtype=wp.vec3, device=self.device)
uvs = wp.array(mesh.uvs, dtype=wp.vec2, device=self.device)
indices = wp.array(mesh.indices, dtype=wp.int32, device=self.device)
self.log_mesh(name, points, indices, normals, uvs, hidden=hidden)
else:
super().log_geo(name, geo_type, geo_scale, geo_thickness, geo_is_solid, geo_src, hidden)
@override
def log_points(
self,
name: str,
points: wp.array[wp.vec3] | None,
radii: wp.array[wp.float32] | float | None = None,
colors: (wp.array[wp.vec3] | wp.array[wp.float32] | tuple[float, float, float] | list[float] | None) = None,
hidden: bool = False,
):
"""
Log points for visualization.
Args:
name: Name of the point batch.
points: Point positions (can be a wp.array or a numpy array).
radii: Point radii (can be a wp.array or a numpy array).
colors: Point colors (can be a wp.array or a numpy array).
hidden: Whether the points are hidden.
"""
# Remove existing points if present
if name in self._scene_handles:
try:
self._scene_handles[name].remove()
except Exception:
pass
if hidden:
return
if points is None:
return
pts = self._to_numpy(points)
n_points = pts.shape[0]
if n_points == 0:
return
# Handle radii (point size)
if radii is not None:
size = self._to_numpy(radii)
if size.ndim == 0 or size.shape == ():
point_size = float(size)
elif len(size) == n_points:
point_size = float(np.mean(size)) # Use average for uniform size
else:
point_size = 0.1
else:
point_size = 0.1
# Handle colors
if colors is not None:
cols = self._to_numpy(colors)
if cols.shape == (n_points, 3):
# Convert from 0-1 to 0-255
colors_val = (cols * 255).astype(np.uint8)
elif cols.shape == (3,):
colors_val = np.tile((cols * 255).astype(np.uint8), (n_points, 1))
else:
colors_val = np.full((n_points, 3), 255, dtype=np.uint8)
else:
colors_val = np.full((n_points, 3), 255, dtype=np.uint8)
# Add point cloud to viser
handle = self._server.scene.add_point_cloud(
name=name,
points=pts.astype(np.float32),
colors=colors_val,
point_size=point_size,
point_shape="circle",
)
self._scene_handles[name] = handle
@override
def log_array(self, name: str, array: wp.array[Any] | np.ndarray):
"""Viser viewer does not visualize generic arrays.
Args:
name: Unique path/name for the array signal.
array: Array data to visualize.
"""
pass
@override
def log_scalar(self, name: str, value: int | float | bool | np.number):
"""Viser viewer does not visualize scalar signals.
Args:
name: Unique path/name for the scalar signal.
value: Scalar value to visualize.
"""
pass
def show_notebook(self, width: int | str = "100%", height: int | str = 400):
"""
Show the viewer in a Jupyter notebook.
If recording is active, saves the recording and displays using the static HTML
viewer with timeline controls. Otherwise, displays the live server in an IFrame.
Args:
width: Width of the embedded player in pixels.
height: Height of the embedded player in pixels.
Returns:
The display object.
Example:
.. code-block:: python
viewer = newton.viewer.ViewerViser(record_to_viser="my_sim.viser")
viewer.set_model(model)
# ... run simulation ...
viewer.show_notebook() # Saves recording and displays with timeline
"""
from IPython.display import HTML, IFrame, display
from .viewer import is_sphinx_build # noqa: PLC0415
if self._record_to_viser is None:
# No recording - display the live server via IFrame
return display(IFrame(src=self.url, width=width, height=height))
if self._serializer is not None:
# Recording is active - save it first
recording_path = Path(self._record_to_viser)
recording_path.parent.mkdir(parents=True, exist_ok=True)
self.save_recording()
# Check if recording path contains _static - indicates Sphinx docs build
recording_str = str(self._record_to_viser).replace("\\", "/")
if is_sphinx_build():
# Sphinx build - use static HTML with viser player
# The recording path needs to be relative to the viser index.html location
# which is at _static/viser/index.html
# Find the _static portion of the path
static_idx = recording_str.find("_static/")
if static_idx == -1:
raise ValueError(
f"Recordings that are supposed to appear in the Sphinx documentation must be stored in docs/_static/, but the path {recording_str} does not contain _static/"
)
else:
# Extract path from _static onwards (e.g., "_static/recordings/foo.viser")
static_relative = recording_str[static_idx:]
# The viser index.html is at _static/viser/index.html
# So from there, we need "../recordings/foo.viser"
# Remove the "_static/" prefix and prepend "../"
playback_path = "../" + static_relative[len("_static/") :]
camera_query = self._camera_query_from_request(self._camera_request)
embed_html = f"""
"""
return display(HTML(embed_html))
else:
# Regular Jupyter - use local HTTP server with viser client
player_url = self._serve_viser_recording(
self._record_to_viser,
camera_request=self._camera_request,
)
return display(IFrame(src=player_url, width=width, height=height))
def _ipython_display_(self):
"""
Display the viewer in an IPython notebook when the viewer is at the end of a cell.
"""
self.show_notebook()
@staticmethod
def _serve_viser_recording(
recording_path: str, camera_request: tuple[np.ndarray, np.ndarray, np.ndarray] | None = None
) -> str:
"""
Hosts a simple HTTP server to serve the viser recording file with the viser client
and returns the URL of the player.
Args:
recording_path: Path to the .viser recording file.
camera_request: Optional `(position, look_at, up_direction)` triple used
to append initial camera URL overrides for playback.
Returns:
URL of the player.
"""
import socket # noqa: PLC0415
import threading # noqa: PLC0415
from http.server import HTTPServer, SimpleHTTPRequestHandler # noqa: PLC0415
# Get viser client directory (bundled with package at newton/_src/viewer/static/viser)
recording_path = Path(recording_path).resolve()
if not recording_path.exists():
raise FileNotFoundError(f"Recording file not found: {recording_path}")
viser_client_dir = Path(__file__).parent / "viser" / "static"
if not viser_client_dir.exists():
raise FileNotFoundError(
f"Viser client files not found at {viser_client_dir}. "
"The notebook playback feature requires the viser client assets."
)
# Read the recording file content
recording_bytes = recording_path.read_bytes()
# Find an available port
def find_free_port():
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.bind(("", 0))
return s.getsockname()[1]
port = find_free_port()
# Create a custom HTTP handler factory that serves both viser client and the recording
def make_handler(recording_data: bytes, client_dir: str):
class RecordingHandler(SimpleHTTPRequestHandler):
# Fix MIME types for JavaScript and other files (Windows often has wrong mappings)
extensions_map: ClassVar = { # pyright: ignore[reportIncompatibleVariableOverride]
**SimpleHTTPRequestHandler.extensions_map,
".html": "text/html",
".htm": "text/html",
".css": "text/css",
".js": "application/javascript",
".json": "application/json",
".wasm": "application/wasm",
".svg": "image/svg+xml",
".png": "image/png",
".jpg": "image/jpeg",
".jpeg": "image/jpeg",
".ico": "image/x-icon",
".ttf": "font/ttf",
".hdr": "application/octet-stream",
".viser": "application/octet-stream",
"": "application/octet-stream",
}
def __init__(self, *args, **kwargs):
self.recording_data = recording_data
super().__init__(*args, directory=client_dir, **kwargs)
def do_GET(self):
# Parse path without query string
path = self.path.split("?")[0]
# Serve the recording file at /recording.viser
if path == "/recording.viser":
self.send_response(200)
self.send_header("Content-Type", "application/octet-stream")
self.send_header("Content-Length", str(len(self.recording_data)))
self.send_header("Access-Control-Allow-Origin", "*")
self.end_headers()
self.wfile.write(self.recording_data)
else:
# Serve viser client files
super().do_GET()
def log_message(self, format, *args):
pass # Suppress log messages
return RecordingHandler
handler_class = make_handler(recording_bytes, str(viser_client_dir))
# Bind to all interfaces so IFrame can access it
server = HTTPServer(("", port), handler_class)
# Start server in background thread
server_thread = threading.Thread(target=server.serve_forever, daemon=True)
server_thread.start()
# Keep playbackPath relative so notebook proxy prefixes (e.g. /lab/proxy//)
# are preserved. Each viewer instance uses a different port, so paths stay distinct.
playback_path = "recording.viser"
base_url = f"http://127.0.0.1:{port}"
player_url = f"{base_url}/?playbackPath={playback_path}"
# Route through Jupyter's proxy only when jupyter-server-proxy is installed.
# Without that package, proxy URLs may be unavailable and break playback.
jupyter_base_url = None
try:
from importlib.util import find_spec # noqa: PLC0415
has_jupyter_server_proxy = find_spec("jupyter_server_proxy") is not None
except Exception:
has_jupyter_server_proxy = False
if has_jupyter_server_proxy:
# JUPYTER_BASE_URL is not always exported (e.g. CLI --NotebookApp.base_url).
# In that case, fall back to common env vars and running server metadata.
for env_name in ("JUPYTER_BASE_URL", "JUPYTERHUB_SERVICE_PREFIX", "NB_PREFIX"):
candidate = os.environ.get(env_name)
if candidate:
jupyter_base_url = candidate
break
if not jupyter_base_url:
try:
from jupyter_server.serverapp import list_running_servers # noqa: PLC0415
for server in list_running_servers():
candidate = server.get("base_url")
if candidate:
jupyter_base_url = candidate
break
except Exception:
pass
if jupyter_base_url:
if not jupyter_base_url.startswith("/"):
jupyter_base_url = "/" + jupyter_base_url
if jupyter_base_url != "/":
jupyter_base_url = jupyter_base_url.rstrip("/")
else:
jupyter_base_url = ""
player_url = f"{jupyter_base_url}/proxy/{port}/?playbackPath={playback_path}"
return player_url + ViewerViser._camera_query_from_request(camera_request)
================================================
FILE: newton/_src/viewer/viser/static/assets/SplatSortWorker-DiSpcAPr.js
================================================
var nn=(()=>{var D=import.meta.url;return async function(F={}){var I,u=F,Y,E,w=new Promise((r,e)=>{Y=r,E=e}),pr=typeof window=="object",B=typeof importScripts=="function",hr=typeof process=="object"&&typeof process.versions=="object"&&typeof process.versions.node=="string";if(hr){const{createRequire:r}=await Promise.resolve().then(function(){return an});var gr=r(import.meta.url)}var Or=Object.assign({},u),b="";function we(r){return u.locateFile?u.locateFile(r,b):b+r}var yr,mr,rr;if(hr){var Ur=gr("fs"),Dr=gr("path");b=gr("url").fileURLToPath(new URL("./",import.meta.url)),yr=(r,e)=>(r=nr(r)?new URL(r):Dr.normalize(r),Ur.readFileSync(r,e?void 0:"utf8")),rr=r=>{var e=yr(r,!0);return e.buffer||(e=new Uint8Array(e)),e},mr=(r,e,t,n=!0)=>{r=nr(r)?new URL(r):Dr.normalize(r),Ur.readFile(r,n?void 0:"utf8",(a,s)=>{a?t(a):e(n?s.buffer:s)})},!u.thisProgram&&process.argv.length>1&&process.argv[1].replace(/\\/g,"/"),process.argv.slice(2)}else(pr||B)&&(B?b=self.location.href:typeof document<"u"&&document.currentScript&&(b=document.currentScript.src),D&&(b=D),b.startsWith("blob:")?b="":b=b.substr(0,b.replace(/[?#].*/,"").lastIndexOf("/")+1),yr=r=>{var e=new XMLHttpRequest;return e.open("GET",r,!1),e.send(null),e.responseText},B&&(rr=r=>{var e=new XMLHttpRequest;return e.open("GET",r,!1),e.responseType="arraybuffer",e.send(null),new Uint8Array(e.response)}),mr=(r,e,t)=>{if(nr(r)){var n=new XMLHttpRequest;n.open("GET",r,!0),n.responseType="arraybuffer",n.onload=()=>{if(n.status==200||n.status==0&&n.response){e(n.response);return}t()},n.onerror=t,n.send(null);return}fetch(r,{credentials:"same-origin"}).then(a=>a.ok?a.arrayBuffer():Promise.reject(new Error(a.status+" : "+a.url))).then(e,t)});u.print||console.log.bind(console);var M=u.printErr||console.error.bind(console);Object.assign(u,Or),Or=null,u.arguments&&u.arguments,u.thisProgram&&u.thisProgram,u.quit&&u.quit;var L;u.wasmBinary&&(L=u.wasmBinary);var er,Ir=!1,W,m,z,tr,q,h,jr,Vr;function Hr(){var r=er.buffer;u.HEAP8=W=new Int8Array(r),u.HEAP16=z=new Int16Array(r),u.HEAPU8=m=new Uint8Array(r),u.HEAPU16=tr=new Uint16Array(r),u.HEAP32=q=new Int32Array(r),u.HEAPU32=h=new Uint32Array(r),u.HEAPF32=jr=new Float32Array(r),u.HEAPF64=Vr=new Float64Array(r)}var Br=[],Mr=[],Lr=[];function be(){if(u.preRun)for(typeof u.preRun=="function"&&(u.preRun=[u.preRun]);u.preRun.length;)Ce(u.preRun.shift());_r(Br)}function Pe(){_r(Mr)}function Te(){if(u.postRun)for(typeof u.postRun=="function"&&(u.postRun=[u.postRun]);u.postRun.length;)zr(u.postRun.shift());_r(Lr)}function Ce(r){Br.unshift(r)}function Ae(r){Mr.unshift(r)}function zr(r){Lr.unshift(r)}var k=0,G=null;function Re(r){k++,u.monitorRunDependencies?.(k)}function Fe(r){if(k--,u.monitorRunDependencies?.(k),k==0&&G){var e=G;G=null,e()}}function qr(r){u.onAbort?.(r),r="Aborted("+r+")",M(r),Ir=!0,r+=". Build with -sASSERTIONS for more info.";var e=new WebAssembly.RuntimeError(r);throw E(e),e}var Ee="data:application/octet-stream;base64,",Gr=r=>r.startsWith(Ee),nr=r=>r.startsWith("file://");function Se(){if(u.locateFile){var r="Sorter.wasm";return Gr(r)?r:we(r)}return new URL(""+new URL("Sorter-Df0J3ZWJ.wasm",import.meta.url).href,import.meta.url).href}var ar;function Nr(r){if(r==ar&&L)return new Uint8Array(L);if(rr)return rr(r);throw"both async and sync fetching of the wasm failed"}function We(r){return L?Promise.resolve().then(()=>Nr(r)):new Promise((e,t)=>{mr(r,n=>e(new Uint8Array(n)),n=>{try{e(Nr(r))}catch(a){t(a)}})})}function Jr(r,e,t){return We(r).then(n=>WebAssembly.instantiate(n,e)).then(t,n=>{M(`failed to asynchronously prepare wasm: ${n}`),qr(n)})}function ke(r,e,t,n){return!r&&typeof WebAssembly.instantiateStreaming=="function"&&!Gr(e)&&!nr(e)&&!hr&&typeof fetch=="function"?fetch(e,{credentials:"same-origin"}).then(a=>{var s=WebAssembly.instantiateStreaming(a,t);return s.then(n,function(o){return M(`wasm streaming compile failed: ${o}`),M("falling back to ArrayBuffer instantiation"),Jr(e,t,n)})}):Jr(e,t,n)}function Oe(){return{a:en}}function Ue(){var r=Oe();function e(n,a){return A=n.exports,er=A.z,Hr(),te=A.C,Ae(A.A),Fe(),A}Re();function t(n){e(n.instance)}if(u.instantiateWasm)try{return u.instantiateWasm(r,e)}catch(n){M(`Module.instantiateWasm callback failed with error: ${n}`),E(n)}return ar||(ar=Se()),ke(L,ar,r,t).catch(E),{}}var _r=r=>{for(;r.length>0;)r.shift()(u)};u.noExitRuntime;class De{constructor(e){this.excPtr=e,this.ptr=e-24}set_type(e){h[this.ptr+4>>2]=e}get_type(){return h[this.ptr+4>>2]}set_destructor(e){h[this.ptr+8>>2]=e}get_destructor(){return h[this.ptr+8>>2]}set_caught(e){e=e?1:0,W[this.ptr+12]=e}get_caught(){return W[this.ptr+12]!=0}set_rethrown(e){e=e?1:0,W[this.ptr+13]=e}get_rethrown(){return W[this.ptr+13]!=0}init(e,t){this.set_adjusted_ptr(0),this.set_type(e),this.set_destructor(t)}set_adjusted_ptr(e){h[this.ptr+16>>2]=e}get_adjusted_ptr(){return h[this.ptr+16>>2]}get_exception_ptr(){var e=ge(this.get_type());if(e)return h[this.excPtr>>2];var t=this.get_adjusted_ptr();return t!==0?t:this.excPtr}}var Xr=0,Ie=(r,e,t)=>{var n=new De(r);throw n.init(e,t),Xr=r,Xr},je=()=>{qr("")},Ve=(r,e,t,n,a)=>{},He=()=>{for(var r=new Array(256),e=0;e<256;++e)r[e]=String.fromCharCode(e);Zr=r},Zr,_=r=>{for(var e="",t=r;m[t];)e+=Zr[m[t++]];return e},j={},O={},ir={},V,d=r=>{throw new V(r)},xr,sr=r=>{throw new xr(r)},N=(r,e,t)=>{r.forEach(function(i){ir[i]=e});function n(i){var l=t(i);l.length!==r.length&&sr("Mismatched type converter count");for(var f=0;f{O.hasOwnProperty(i)?a[l]=O[i]:(s.push(i),j.hasOwnProperty(i)||(j[i]=[]),j[i].push(()=>{a[l]=O[i],++o,o===s.length&&n(a)}))}),s.length===0&&n(a)};function Be(r,e,t={}){var n=e.name;if(r||d(`type "${n}" must have a positive integer typeid pointer`),O.hasOwnProperty(r)){if(t.ignoreDuplicateRegistrations)return;d(`Cannot register type '${n}' twice`)}if(O[r]=e,delete ir[r],j.hasOwnProperty(r)){var a=j[r];delete j[r],a.forEach(s=>s())}}function T(r,e,t={}){if(!("argPackAdvance"in e))throw new TypeError("registerType registeredInstance requires argPackAdvance");return Be(r,e,t)}var S=8,Me=(r,e,t,n)=>{e=_(e),T(r,{name:e,fromWireType:function(a){return!!a},toWireType:function(a,s){return s?t:n},argPackAdvance:S,readValueFromPointer:function(a){return this.fromWireType(m[a])},destructorFunction:null})},Le=r=>({count:r.count,deleteScheduled:r.deleteScheduled,preservePointerOnDelete:r.preservePointerOnDelete,ptr:r.ptr,ptrType:r.ptrType,smartPtr:r.smartPtr,smartPtrType:r.smartPtrType}),$r=r=>{function e(t){return t.$$.ptrType.registeredClass.name}d(e(r)+" instance already deleted")},wr=!1,Kr=r=>{},ze=r=>{r.smartPtr?r.smartPtrType.rawDestructor(r.smartPtr):r.ptrType.registeredClass.rawDestructor(r.ptr)},Qr=r=>{r.count.value-=1;var e=r.count.value===0;e&&ze(r)},Yr=(r,e,t)=>{if(e===t)return r;if(t.baseClass===void 0)return null;var n=Yr(r,e,t.baseClass);return n===null?null:t.downcast(n)},re={},qe=()=>Object.keys(Z).length,Ge=()=>{var r=[];for(var e in Z)Z.hasOwnProperty(e)&&r.push(Z[e]);return r},J=[],br=()=>{for(;J.length;){var r=J.pop();r.$$.deleteScheduled=!1,r.delete()}},X,Ne=r=>{X=r,J.length&&X&&X(br)},Je=()=>{u.getInheritedInstanceCount=qe,u.getLiveInheritedInstances=Ge,u.flushPendingDeletes=br,u.setDelayFunction=Ne},Z={},Xe=(r,e)=>{for(e===void 0&&d("ptr should not be undefined");r.baseClass;)e=r.upcast(e),r=r.baseClass;return e},Ze=(r,e)=>(e=Xe(r,e),Z[e]),or=(r,e)=>{(!e.ptrType||!e.ptr)&&sr("makeClassHandle requires ptr and ptrType");var t=!!e.smartPtrType,n=!!e.smartPtr;return t!==n&&sr("Both smartPtrType and smartPtr must be specified"),e.count={value:1},x(Object.create(r,{$$:{value:e,writable:!0}}))};function xe(r){var e=this.getPointee(r);if(!e)return this.destructor(r),null;var t=Ze(this.registeredClass,e);if(t!==void 0){if(t.$$.count.value===0)return t.$$.ptr=e,t.$$.smartPtr=r,t.clone();var n=t.clone();return this.destructor(r),n}function a(){return this.isSmartPointer?or(this.registeredClass.instancePrototype,{ptrType:this.pointeeType,ptr:e,smartPtrType:this,smartPtr:r}):or(this.registeredClass.instancePrototype,{ptrType:this,ptr:r})}var s=this.registeredClass.getActualType(e),o=re[s];if(!o)return a.call(this);var i;this.isConst?i=o.constPointerType:i=o.pointerType;var l=Yr(e,this.registeredClass,i.registeredClass);return l===null?a.call(this):this.isSmartPointer?or(i.registeredClass.instancePrototype,{ptrType:i,ptr:l,smartPtrType:this,smartPtr:r}):or(i.registeredClass.instancePrototype,{ptrType:i,ptr:l})}var x=r=>typeof FinalizationRegistry>"u"?(x=e=>e,r):(wr=new FinalizationRegistry(e=>{Qr(e.$$)}),x=e=>{var t=e.$$,n=!!t.smartPtr;if(n){var a={$$:t};wr.register(e,a,e)}return e},Kr=e=>wr.unregister(e),x(r)),Ke=()=>{Object.assign(ur.prototype,{isAliasOf(r){if(!(this instanceof ur)||!(r instanceof ur))return!1;var e=this.$$.ptrType.registeredClass,t=this.$$.ptr;r.$$=r.$$;for(var n=r.$$.ptrType.registeredClass,a=r.$$.ptr;e.baseClass;)t=e.upcast(t),e=e.baseClass;for(;n.baseClass;)a=n.upcast(a),n=n.baseClass;return e===n&&t===a},clone(){if(this.$$.ptr||$r(this),this.$$.preservePointerOnDelete)return this.$$.count.value+=1,this;var r=x(Object.create(Object.getPrototypeOf(this),{$$:{value:Le(this.$$)}}));return r.$$.count.value+=1,r.$$.deleteScheduled=!1,r},delete(){this.$$.ptr||$r(this),this.$$.deleteScheduled&&!this.$$.preservePointerOnDelete&&d("Object already scheduled for deletion"),Kr(this),Qr(this.$$),this.$$.preservePointerOnDelete||(this.$$.smartPtr=void 0,this.$$.ptr=void 0)},isDeleted(){return!this.$$.ptr},deleteLater(){return this.$$.ptr||$r(this),this.$$.deleteScheduled&&!this.$$.preservePointerOnDelete&&d("Object already scheduled for deletion"),J.push(this),J.length===1&&X&&X(br),this.$$.deleteScheduled=!0,this}})};function ur(){}var K=(r,e)=>Object.defineProperty(e,"name",{value:r}),ee=(r,e,t)=>{if(r[e].overloadTable===void 0){var n=r[e];r[e]=function(...a){return r[e].overloadTable.hasOwnProperty(a.length)||d(`Function '${t}' called with an invalid number of arguments (${a.length}) - expects one of (${r[e].overloadTable})!`),r[e].overloadTable[a.length].apply(this,a)},r[e].overloadTable=[],r[e].overloadTable[n.argCount]=n}},Qe=(r,e,t)=>{u.hasOwnProperty(r)?(d(`Cannot register public name '${r}' twice`),ee(u,r,r),u.hasOwnProperty(t)&&d(`Cannot register multiple overloads of a function with the same number of arguments (${t})!`),u[r].overloadTable[t]=e):u[r]=e},Ye=48,rt=57,et=r=>{if(r===void 0)return"_unknown";r=r.replace(/[^a-zA-Z0-9_]/g,"$");var e=r.charCodeAt(0);return e>=Ye&&e<=rt?`_${r}`:r};function tt(r,e,t,n,a,s,o,i){this.name=r,this.constructor=e,this.instancePrototype=t,this.rawDestructor=n,this.baseClass=a,this.getActualType=s,this.upcast=o,this.downcast=i,this.pureVirtualFunctions=[]}var Pr=(r,e,t)=>{for(;e!==t;)e.upcast||d(`Expected null or instance of ${t.name}, got an instance of ${e.name}`),r=e.upcast(r),e=e.baseClass;return r};function nt(r,e){if(e===null)return this.isReference&&d(`null is not a valid ${this.name}`),0;e.$$||d(`Cannot pass "${Rr(e)}" as a ${this.name}`),e.$$.ptr||d(`Cannot pass deleted object as a pointer of type ${this.name}`);var t=e.$$.ptrType.registeredClass,n=Pr(e.$$.ptr,t,this.registeredClass);return n}function at(r,e){var t;if(e===null)return this.isReference&&d(`null is not a valid ${this.name}`),this.isSmartPointer?(t=this.rawConstructor(),r!==null&&r.push(this.rawDestructor,t),t):0;(!e||!e.$$)&&d(`Cannot pass "${Rr(e)}" as a ${this.name}`),e.$$.ptr||d(`Cannot pass deleted object as a pointer of type ${this.name}`),!this.isConst&&e.$$.ptrType.isConst&&d(`Cannot convert argument of type ${e.$$.smartPtrType?e.$$.smartPtrType.name:e.$$.ptrType.name} to parameter type ${this.name}`);var n=e.$$.ptrType.registeredClass;if(t=Pr(e.$$.ptr,n,this.registeredClass),this.isSmartPointer)switch(e.$$.smartPtr===void 0&&d("Passing raw pointer to smart pointer is illegal"),this.sharingPolicy){case 0:e.$$.smartPtrType===this?t=e.$$.smartPtr:d(`Cannot convert argument of type ${e.$$.smartPtrType?e.$$.smartPtrType.name:e.$$.ptrType.name} to parameter type ${this.name}`);break;case 1:t=e.$$.smartPtr;break;case 2:if(e.$$.smartPtrType===this)t=e.$$.smartPtr;else{var a=e.clone();t=this.rawShare(t,$.toHandle(()=>a.delete())),r!==null&&r.push(this.rawDestructor,t)}break;default:d("Unsupporting sharing policy")}return t}function it(r,e){if(e===null)return this.isReference&&d(`null is not a valid ${this.name}`),0;e.$$||d(`Cannot pass "${Rr(e)}" as a ${this.name}`),e.$$.ptr||d(`Cannot pass deleted object as a pointer of type ${this.name}`),e.$$.ptrType.isConst&&d(`Cannot convert argument of type ${e.$$.ptrType.name} to parameter type ${this.name}`);var t=e.$$.ptrType.registeredClass,n=Pr(e.$$.ptr,t,this.registeredClass);return n}function lr(r){return this.fromWireType(h[r>>2])}var st=()=>{Object.assign(fr.prototype,{getPointee(r){return this.rawGetPointee&&(r=this.rawGetPointee(r)),r},destructor(r){this.rawDestructor?.(r)},argPackAdvance:S,readValueFromPointer:lr,fromWireType:xe})};function fr(r,e,t,n,a,s,o,i,l,f,c){this.name=r,this.registeredClass=e,this.isReference=t,this.isConst=n,this.isSmartPointer=a,this.pointeeType=s,this.sharingPolicy=o,this.rawGetPointee=i,this.rawConstructor=l,this.rawShare=f,this.rawDestructor=c,!a&&e.baseClass===void 0?n?(this.toWireType=nt,this.destructorFunction=null):(this.toWireType=it,this.destructorFunction=null):this.toWireType=at}var ot=(r,e,t)=>{u.hasOwnProperty(r)||sr("Replacing nonexistent public symbol"),u[r].overloadTable!==void 0&&t!==void 0||(u[r]=e,u[r].argCount=t)},ut=(r,e,t)=>{r=r.replace(/p/g,"i");var n=u["dynCall_"+r];return n(e,...t)},cr=[],te,ne=r=>{var e=cr[r];return e||(r>=cr.length&&(cr.length=r+1),cr[r]=e=te.get(r)),e},lt=(r,e,t=[])=>{if(r.includes("j"))return ut(r,e,t);var n=ne(e)(...t);return n},ft=(r,e)=>(...t)=>lt(r,e,t),H=(r,e)=>{r=_(r);function t(){return r.includes("j")?ft(r,e):ne(e)}var n=t();return typeof n!="function"&&d(`unknown function pointer with signature ${r}: ${e}`),n},ct=(r,e)=>{var t=K(e,function(n){this.name=e,this.message=n;var a=new Error(n).stack;a!==void 0&&(this.stack=this.toString()+`
`+a.replace(/^Error(:[^\n]*)?\n/,""))});return t.prototype=Object.create(r.prototype),t.prototype.constructor=t,t.prototype.toString=function(){return this.message===void 0?this.name:`${this.name}: ${this.message}`},t},ae,ie=r=>{var e=he(r),t=_(e);return R(e),t},Tr=(r,e)=>{var t=[],n={};function a(s){if(!n[s]&&!O[s]){if(ir[s]){ir[s].forEach(a);return}t.push(s),n[s]=!0}}throw e.forEach(a),new ae(`${r}: `+t.map(ie).join([", "]))},vt=(r,e,t,n,a,s,o,i,l,f,c,v,p)=>{c=_(c),s=H(a,s),i&&=H(o,i),f&&=H(l,f),p=H(v,p);var g=et(c);Qe(g,function(){Tr(`Cannot construct ${c} due to unbound types`,[n])}),N([r,e,t],n?[n]:[],y=>{y=y[0];var Q,U;n?(Q=y.registeredClass,U=Q.instancePrototype):U=ur.prototype;var dr=K(c,function(...kr){if(Object.getPrototypeOf(this)!==Wr)throw new V("Use 'new' to construct "+c);if(P.constructor_body===void 0)throw new V(c+" has no accessible constructor");var $e=P.constructor_body[kr.length];if($e===void 0)throw new V(`Tried to invoke ctor of ${c} with invalid number of parameters (${kr.length}) - expected (${Object.keys(P.constructor_body).toString()}) parameters instead!`);return $e.apply(this,kr)}),Wr=Object.create(U,{constructor:{value:dr}});dr.prototype=Wr;var P=new tt(c,dr,Wr,p,Q,s,i,f);P.baseClass&&(P.baseClass.__derivedClasses??=[],P.baseClass.__derivedClasses.push(P));var tn=new fr(c,P,!0,!1,!1),me=new fr(c+"*",P,!1,!1,!1),_e=new fr(c+" const*",P,!1,!0,!1);return re[r]={pointerType:me,constPointerType:_e},ot(g,dr),[tn,me,_e]})},se=(r,e)=>{for(var t=[],n=0;n>2]);return t},oe=r=>{for(;r.length;){var e=r.pop(),t=r.pop();t(e)}};function ue(r){for(var e=1;e0?", ":"")+i),f+=(t||n?"var rv = ":"")+"invoker(fn"+(i.length>0?", ":"")+i+`);
`,a)f+=`runDestructors(destructors);
`;else for(var l=e?1:2;l{var o=se(e,t);a=H(n,a),N([],[r],i=>{i=i[0];var l=`constructor ${i.name}`;if(i.registeredClass.constructor_body===void 0&&(i.registeredClass.constructor_body=[]),i.registeredClass.constructor_body[e-1]!==void 0)throw new V(`Cannot register multiple constructors with identical number of parameters (${e-1}) for class '${i.name}'! Overload resolution is currently only performed using the parameter count, not actual type info!`);return i.registeredClass.constructor_body[e-1]=()=>{Tr(`Cannot construct ${i.name} due to unbound types`,o)},N([],o,f=>(f.splice(1,0,null),i.registeredClass.constructor_body[e-1]=fe(l,f,null,a,s),[])),[]})},ht=r=>{r=r.trim();const e=r.indexOf("(");return e!==-1?r.substr(0,e):r},gt=(r,e,t,n,a,s,o,i,l)=>{var f=se(t,n);e=_(e),e=ht(e),s=H(a,s),N([],[r],c=>{c=c[0];var v=`${c.name}.${e}`;e.startsWith("@@")&&(e=Symbol[e.substring(2)]),i&&c.registeredClass.pureVirtualFunctions.push(e);function p(){Tr(`Cannot call ${v} due to unbound types`,f)}var g=c.registeredClass.instancePrototype,y=g[e];return y===void 0||y.overloadTable===void 0&&y.className!==c.name&&y.argCount===t-2?(p.argCount=t-2,p.className=c.name,g[e]=p):(ee(g,e,v),g[e].overloadTable[t-2]=p),N([],f,Q=>{var U=fe(v,Q,c,s,o,l);return g[e].overloadTable===void 0?(U.argCount=t-2,g[e]=U):g[e].overloadTable[t-2]=U,[]}),[]})},Cr=[],C=[],Ar=r=>{r>9&&--C[r+1]===0&&(C[r]=void 0,Cr.push(r))},yt=()=>C.length/2-5-Cr.length,mt=()=>{C.push(0,1,void 0,1,null,1,!0,1,!1,1),u.count_emval_handles=yt},$={toValue:r=>(r||d("Cannot use deleted val. handle = "+r),C[r]),toHandle:r=>{switch(r){case void 0:return 2;case null:return 4;case!0:return 6;case!1:return 8;default:{const e=Cr.pop()||C.length;return C[e]=r,C[e+1]=1,e}}}},_t={name:"emscripten::val",fromWireType:r=>{var e=$.toValue(r);return Ar(r),e},toWireType:(r,e)=>$.toHandle(e),argPackAdvance:S,readValueFromPointer:lr,destructorFunction:null},$t=r=>T(r,_t),Rr=r=>{if(r===null)return"null";var e=typeof r;return e==="object"||e==="array"||e==="function"?r.toString():""+r},wt=(r,e)=>{switch(e){case 4:return function(t){return this.fromWireType(jr[t>>2])};case 8:return function(t){return this.fromWireType(Vr[t>>3])};default:throw new TypeError(`invalid float width (${e}): ${r}`)}},bt=(r,e,t)=>{e=_(e),T(r,{name:e,fromWireType:n=>n,toWireType:(n,a)=>a,argPackAdvance:S,readValueFromPointer:wt(e,t),destructorFunction:null})},Pt=(r,e,t)=>{switch(e){case 1:return t?n=>W[n]:n=>m[n];case 2:return t?n=>z[n>>1]:n=>tr[n>>1];case 4:return t?n=>q[n>>2]:n=>h[n>>2];default:throw new TypeError(`invalid integer width (${e}): ${r}`)}},Tt=(r,e,t,n,a)=>{e=_(e);var s=c=>c;if(n===0){var o=32-8*t;s=c=>c<>>o}var i=e.includes("unsigned"),l=(c,v)=>{},f;i?f=function(c,v){return l(v,this.name),v>>>0}:f=function(c,v){return l(v,this.name),v},T(r,{name:e,fromWireType:s,toWireType:f,argPackAdvance:S,readValueFromPointer:Pt(e,t,n!==0),destructorFunction:null})},Ct=(r,e,t)=>{var n=[Int8Array,Uint8Array,Int16Array,Uint16Array,Int32Array,Uint32Array,Float32Array,Float64Array],a=n[e];function s(o){var i=h[o>>2],l=h[o+4>>2];return new a(W.buffer,l,i)}t=_(t),T(r,{name:t,fromWireType:s,argPackAdvance:S,readValueFromPointer:s},{ignoreDuplicateRegistrations:!0})},At=(r,e,t,n)=>{if(!(n>0))return 0;for(var a=t,s=t+n-1,o=0;o=55296&&i<=57343){var l=r.charCodeAt(++o);i=65536+((i&1023)<<10)|l&1023}if(i<=127){if(t>=s)break;e[t++]=i}else if(i<=2047){if(t+1>=s)break;e[t++]=192|i>>6,e[t++]=128|i&63}else if(i<=65535){if(t+2>=s)break;e[t++]=224|i>>12,e[t++]=128|i>>6&63,e[t++]=128|i&63}else{if(t+3>=s)break;e[t++]=240|i>>18,e[t++]=128|i>>12&63,e[t++]=128|i>>6&63,e[t++]=128|i&63}}return e[t]=0,t-a},Rt=(r,e,t)=>At(r,m,e,t),Ft=r=>{for(var e=0,t=0;t=55296&&n<=57343?(e+=4,++t):e+=3}return e},ce=typeof TextDecoder<"u"?new TextDecoder("utf8"):void 0,Et=(r,e,t)=>{for(var n=e+t,a=e;r[a]&&!(a>=n);)++a;if(a-e>16&&r.buffer&&ce)return ce.decode(r.subarray(e,a));for(var s="";e>10,56320|f&1023)}}return s},St=(r,e)=>r?Et(m,r,e):"",Wt=(r,e)=>{e=_(e);var t=e==="std::string";T(r,{name:e,fromWireType(n){var a=h[n>>2],s=n+4,o;if(t)for(var i=s,l=0;l<=a;++l){var f=s+l;if(l==a||m[f]==0){var c=f-i,v=St(i,c);o===void 0?o=v:(o+="\0",o+=v),i=f+1}}else{for(var p=new Array(a),l=0;l >2]=s,t&&o)Rt(a,l,s+1);else if(o)for(var f=0;f255&&(R(l),d("String has UTF-16 code units that do not fit in 8 bits")),m[l+f]=c}else for(var f=0;f{for(var t=r,n=t>>1,a=n+e/2;!(n>=a)&&tr[n];)++n;if(t=n<<1,t-r>32&&ve)return ve.decode(m.subarray(r,t));for(var s="",o=0;!(o>=e/2);++o){var i=z[r+o*2>>1];if(i==0)break;s+=String.fromCharCode(i)}return s},Ot=(r,e,t)=>{if(t??=2147483647,t<2)return 0;t-=2;for(var n=e,a=t>1]=o,e+=2}return z[e>>1]=0,e-n},Ut=r=>r.length*2,Dt=(r,e)=>{for(var t=0,n="";!(t>=e/4);){var a=q[r+t*4>>2];if(a==0)break;if(++t,a>=65536){var s=a-65536;n+=String.fromCharCode(55296|s>>10,56320|s&1023)}else n+=String.fromCharCode(a)}return n},It=(r,e,t)=>{if(t??=2147483647,t<4)return 0;for(var n=e,a=n+t-4,s=0;s=55296&&o<=57343){var i=r.charCodeAt(++s);o=65536+((o&1023)<<10)|i&1023}if(q[e>>2]=o,e+=4,e+4>a)break}return q[e>>2]=0,e-n},jt=r=>{for(var e=0,t=0;t=55296&&n<=57343&&++t,e+=4}return e},Vt=(r,e,t)=>{t=_(t);var n,a,s,o;e===2?(n=kt,a=Ot,o=Ut,s=i=>tr[i>>1]):e===4&&(n=Dt,a=It,o=jt,s=i=>h[i>>2]),T(r,{name:t,fromWireType:i=>{for(var l=h[i>>2],f,c=i+4,v=0;v<=l;++v){var p=i+4+v*e;if(v==l||s(p)==0){var g=p-c,y=n(c,g);f===void 0?f=y:(f+="\0",f+=y),c=p+e}}return R(i),f},toWireType:(i,l)=>{typeof l!="string"&&d(`Cannot pass non-string to C++ string type ${t}`);var f=o(l),c=Sr(4+f+e);return h[c>>2]=f/e,a(l,c+4,f+e),i!==null&&i.push(R,c),c},argPackAdvance:S,readValueFromPointer:lr,destructorFunction(i){R(i)}})},Ht=(r,e)=>{e=_(e),T(r,{isVoid:!0,name:e,argPackAdvance:0,fromWireType:()=>{},toWireType:(t,n)=>{}})},Bt=(r,e,t)=>m.copyWithin(r,e,e+t),Fr=(r,e)=>{var t=O[r];return t===void 0&&d(`${e} has unknown type ${ie(r)}`),t},de=(r,e,t)=>{var n=[],a=r.toWireType(n,t);return n.length&&(h[e>>2]=$.toHandle(n)),a},Mt=(r,e,t)=>(r=$.toValue(r),e=Fr(e,"emval::as"),de(e,t,r)),Lt={},pe=r=>{var e=Lt[r];return e===void 0?_(r):e},Er=[],zt=(r,e,t,n,a)=>(r=Er[r],e=$.toValue(e),t=pe(t),r(e,e[t],n,a)),qt=r=>{var e=Er.length;return Er.push(r),e},Gt=(r,e)=>{for(var t=new Array(r),n=0;n>2],"parameter "+n);return t},Nt=(r,e,t)=>{var n=Gt(r,e),a=n.shift();r--;var s=`return function (obj, func, destructorsRef, args) {
`,o=0,i=[];t===0&&i.push("obj");for(var l=["retType"],f=[a],c=0;cy.name).join(", ")}) => ${a.name}>`;return qt(K(g,p))},Jt=(r,e)=>(r=$.toValue(r),e=$.toValue(e),$.toHandle(r[e])),Xt=r=>{r>9&&(C[r+1]+=1)},Zt=r=>$.toHandle(pe(r)),xt=r=>{var e=$.toValue(r);oe(e),Ar(r)},Kt=(r,e)=>{r=Fr(r,"_emval_take_value");var t=r.readValueFromPointer(e);return $.toHandle(t)},Qt=()=>1073741824,Yt=r=>{var e=er.buffer,t=(r-e.byteLength+65535)/65536;try{return er.grow(t),Hr(),1}catch{}},rn=r=>{var e=m.length;r>>>=0;var t=Qt();if(r>t)return!1;for(var n=(l,f)=>l+(f-l%f)%f,a=1;a<=4;a*=2){var s=e*(1+.2/a);s=Math.min(s,r+100663296);var o=Math.min(t,n(Math.max(r,s),65536)),i=Yt(o);if(i)return!0}return!1};He(),V=u.BindingError=class extends Error{constructor(e){super(e),this.name="BindingError"}},xr=u.InternalError=class extends Error{constructor(e){super(e),this.name="InternalError"}},Ke(),Je(),st(),ae=u.UnboundTypeError=ct(Error,"UnboundTypeError"),mt();var en={n:Ie,t:je,s:Ve,x:Me,r:vt,q:pt,k:gt,w:$t,m:bt,c:Tt,a:Ct,l:Wt,f:Vt,y:Ht,v:Bt,h:Mt,o:zt,b:Ar,p:Nt,i:Jt,g:Xt,j:Zt,d:xt,e:Kt,u:rn},A=Ue(),he=r=>(he=A.B)(r),Sr=r=>(Sr=A.D)(r),R=r=>(R=A.E)(r),ge=r=>(ge=A.F)(r);u.addOnPostRun=zr;var vr;G=function r(){vr||ye(),vr||(G=r)};function ye(){if(k>0||(be(),k>0))return;function r(){vr||(vr=!0,u.calledRun=!0,!Ir&&(Pe(),Y(u),u.onRuntimeInitialized&&u.onRuntimeInitialized(),Te()))}u.setStatus?(u.setStatus("Running..."),setTimeout(function(){setTimeout(function(){u.setStatus("")},1),r()},1)):r()}if(u.preInit)for(typeof u.preInit=="function"&&(u.preInit=[u.preInit]);u.preInit.length>0;)u.preInit.pop()();return ye(),I=w,I}})();{let D=null,F=null,I=!1;const u=()=>{if(D===null||F===null){setTimeout(u,1);return}if(I)return;I=!0;const E=F,w=D.sort(F).slice();self.postMessage({sortedIndices:w},[w.buffer]),setTimeout(()=>{I=!1,F!==null&&(E.every((pr,B)=>pr===F[B])||u())},0)},Y=nn();self.onmessage=async E=>{const w=E.data;"setBuffer"in w?D=new(await Y).Sorter(w.setBuffer,w.setGroupIndices):"setTz_camera_groups"in w?(F=w.setTz_camera_groups,u()):"close"in w&&self.close()}}var an=Object.freeze({__proto__:null});
================================================
FILE: newton/_src/viewer/viser/static/assets/WebsocketServerWorker-C6PJJ7Dx.js
================================================
function O(n){const e=n.length;let t=0,i=0;for(;i=55296&&s<=56319&&i>6&31|192;else{if(o>=55296&&o<=56319&&r>18&7|240,e[s++]=o>>12&63|128,e[s++]=o>>6&63|128):(e[s++]=o>>12&15|224,e[s++]=o>>6&63|128)}else{e[s++]=o;continue}e[s++]=o&63|128}}const q=new TextEncoder,X=50;function Y(n,e,t){q.encodeInto(n,e.subarray(t))}function G(n,e,t){n.length>X?Y(n,e,t):J(n,e,t)}const Z=4096;function R(n,e,t){let i=e;const s=i+t,r=[];let o="";for(;i65535&&(f-=65536,r.push(f>>>10&1023|55296),f=56320|f&1023),r.push(f)}else r.push(a);r.length>=Z&&(o+=String.fromCharCode(...r),r.length=0)}return r.length>0&&(o+=String.fromCharCode(...r)),o}const Q=new TextDecoder,j=200;function ee(n,e,t){const i=n.subarray(e,e+t);return Q.decode(i)}function te(n,e,t){return t>j?ee(n,e,t):R(n,e,t)}class S{constructor(e,t){this.type=e,this.data=t}}class c extends Error{constructor(e){super(e);const t=Object.create(c.prototype);Object.setPrototypeOf(this,t),Object.defineProperty(this,"name",{configurable:!0,enumerable:!1,value:c.name})}}const x=4294967295;function se(n,e,t){const i=t/4294967296,s=t;n.setUint32(e,i),n.setUint32(e+4,s)}function H(n,e,t){const i=Math.floor(t/4294967296),s=t;n.setUint32(e,i),n.setUint32(e+4,s)}function b(n,e){const t=n.getInt32(e),i=n.getUint32(e+4);return t*4294967296+i}function ie(n,e){const t=n.getUint32(e),i=n.getUint32(e+4);return t*4294967296+i}const ne=-1,re=4294967296-1,oe=17179869184-1;function ae({sec:n,nsec:e}){if(n>=0&&e>=0&&n<=oe)if(e===0&&n<=re){const t=new Uint8Array(4);return new DataView(t.buffer).setUint32(0,n),t}else{const t=n/4294967296,i=n&4294967295,s=new Uint8Array(8),r=new DataView(s.buffer);return r.setUint32(0,e<<2|t&3),r.setUint32(4,i),s}else{const t=new Uint8Array(12),i=new DataView(t.buffer);return i.setUint32(0,e),H(i,4,n),t}}function he(n){const e=n.getTime(),t=Math.floor(e/1e3),i=(e-t*1e3)*1e6,s=Math.floor(i/1e9);return{sec:t+s,nsec:i-s*1e9}}function ce(n){if(n instanceof Date){const e=he(n);return ae(e)}else return null}function fe(n){const e=new DataView(n.buffer,n.byteOffset,n.byteLength);switch(n.byteLength){case 4:return{sec:e.getUint32(0),nsec:0};case 8:{const t=e.getUint32(0),i=e.getUint32(4),s=(t&3)*4294967296+i,r=t>>>2;return{sec:s,nsec:r}}case 12:{const t=b(e,4),i=e.getUint32(0);return{sec:t,nsec:i}}default:throw new c(`Unrecognized data size for timestamp (expected 4, 8, or 12): ${n.length}`)}}function de(n){const e=fe(n);return new Date(e.sec*1e3+e.nsec/1e6)}const le={type:ne,encode:ce,decode:de};class E{constructor(){this.builtInEncoders=[],this.builtInDecoders=[],this.encoders=[],this.decoders=[],this.register(le)}register({type:e,encode:t,decode:i}){if(e>=0)this.encoders[e]=t,this.decoders[e]=i;else{const s=-1-e;this.builtInEncoders[s]=t,this.builtInDecoders[s]=i}}tryToEncode(e,t){for(let i=0;ithis.maxDepth)throw new Error(`Too deep objects in depth ${t}`);e==null?this.encodeNil():typeof e=="boolean"?this.encodeBoolean(e):typeof e=="number"?this.forceIntegerToFloat?this.encodeNumberAsFloat(e):this.encodeNumber(e):typeof e=="string"?this.encodeString(e):this.useBigInt64&&typeof e=="bigint"?this.encodeBigInt64(e):this.encodeObject(e,t)}ensureBufferSizeToWrite(e){const t=this.pos+e;this.view.byteLength=0?e<128?this.writeU8(e):e<256?(this.writeU8(204),this.writeU8(e)):e<65536?(this.writeU8(205),this.writeU16(e)):e<4294967296?(this.writeU8(206),this.writeU32(e)):this.useBigInt64?this.encodeNumberAsFloat(e):(this.writeU8(207),this.writeU64(e)):e>=-32?this.writeU8(224|e+32):e>=-128?(this.writeU8(208),this.writeI8(e)):e>=-32768?(this.writeU8(209),this.writeI16(e)):e>=-2147483648?(this.writeU8(210),this.writeI32(e)):this.useBigInt64?this.encodeNumberAsFloat(e):(this.writeU8(211),this.writeI64(e)):this.encodeNumberAsFloat(e)}encodeNumberAsFloat(e){this.forceFloat32?(this.writeU8(202),this.writeF32(e)):(this.writeU8(203),this.writeF64(e))}encodeBigInt64(e){e>=BigInt(0)?(this.writeU8(207),this.writeBigUint64(e)):(this.writeU8(211),this.writeBigInt64(e))}writeStringHeader(e){if(e<32)this.writeU8(160+e);else if(e<256)this.writeU8(217),this.writeU8(e);else if(e<65536)this.writeU8(218),this.writeU16(e);else if(e<4294967296)this.writeU8(219),this.writeU32(e);else throw new Error(`Too long string: ${e} bytes in UTF-8`)}encodeString(e){const i=O(e);this.ensureBufferSizeToWrite(5+i),this.writeStringHeader(i),G(e,this.bytes,this.pos),this.pos+=i}encodeObject(e,t){const i=this.extensionCodec.tryToEncode(e,this.context);if(i!=null)this.encodeExtension(i);else if(Array.isArray(e))this.encodeArray(e,t);else if(ArrayBuffer.isView(e))this.encodeBinary(e);else if(typeof e=="object")this.encodeMap(e,t);else throw new Error(`Unrecognized object: ${Object.prototype.toString.apply(e)}`)}encodeBinary(e){const t=e.byteLength;if(t<256)this.writeU8(196),this.writeU8(t);else if(t<65536)this.writeU8(197),this.writeU16(t);else if(t<4294967296)this.writeU8(198),this.writeU32(t);else throw new Error(`Too large binary: ${t}`);const i=z(e);this.writeU8a(i)}encodeArray(e,t){const i=e.length;if(i<16)this.writeU8(144+i);else if(i<65536)this.writeU8(220),this.writeU16(i);else if(i<4294967296)this.writeU8(221),this.writeU32(i);else throw new Error(`Too large array: ${i}`);for(const s of e)this.doEncode(s,t+1)}countWithoutUndefined(e,t){let i=0;for(const s of t)e[s]!==void 0&&i++;return i}encodeMap(e,t){const i=Object.keys(e);this.sortKeys&&i.sort();const s=this.ignoreUndefined?this.countWithoutUndefined(e,i):i.length;if(s<16)this.writeU8(128+s);else if(s<65536)this.writeU8(222),this.writeU16(s);else if(s<4294967296)this.writeU8(223),this.writeU32(s);else throw new Error(`Too large map object: ${s}`);for(const r of i){const o=e[r];this.ignoreUndefined&&o===void 0||(this.encodeString(r),this.doEncode(o,t+1))}}encodeExtension(e){if(typeof e.data=="function"){const i=e.data(this.pos+6),s=i.length;if(s>=4294967296)throw new Error(`Too large extension object: ${s}`);this.writeU8(201),this.writeU32(s),this.writeI8(e.type),this.writeU8a(i);return}const t=e.data.length;if(t===1)this.writeU8(212);else if(t===2)this.writeU8(213);else if(t===4)this.writeU8(214);else if(t===8)this.writeU8(215);else if(t===16)this.writeU8(216);else if(t<256)this.writeU8(199),this.writeU8(t);else if(t<65536)this.writeU8(200),this.writeU16(t);else if(t<4294967296)this.writeU8(201),this.writeU32(t);else throw new Error(`Too large extension object: ${t}`);this.writeI8(e.type),this.writeU8a(e.data)}writeU8(e){this.ensureBufferSizeToWrite(1),this.view.setUint8(this.pos,e),this.pos++}writeU8a(e){const t=e.length;this.ensureBufferSizeToWrite(t),this.bytes.set(e,this.pos),this.pos+=t}writeI8(e){this.ensureBufferSizeToWrite(1),this.view.setInt8(this.pos,e),this.pos++}writeU16(e){this.ensureBufferSizeToWrite(2),this.view.setUint16(this.pos,e),this.pos+=2}writeI16(e){this.ensureBufferSizeToWrite(2),this.view.setInt16(this.pos,e),this.pos+=2}writeU32(e){this.ensureBufferSizeToWrite(4),this.view.setUint32(this.pos,e),this.pos+=4}writeI32(e){this.ensureBufferSizeToWrite(4),this.view.setInt32(this.pos,e),this.pos+=4}writeF32(e){this.ensureBufferSizeToWrite(4),this.view.setFloat32(this.pos,e),this.pos+=4}writeF64(e){this.ensureBufferSizeToWrite(8),this.view.setFloat64(this.pos,e),this.pos+=8}writeU64(e){this.ensureBufferSizeToWrite(8),se(this.view,this.pos,e),this.pos+=8}writeI64(e){this.ensureBufferSizeToWrite(8),H(this.view,this.pos,e),this.pos+=8}writeBigUint64(e){this.ensureBufferSizeToWrite(8),this.view.setBigUint64(this.pos,e),this.pos+=8}writeBigInt64(e){this.ensureBufferSizeToWrite(8),this.view.setBigInt64(this.pos,e),this.pos+=8}}function ye(n,e){return new D(e).encodeSharedRef(n)}function v(n){return`${n<0?"-":""}0x${Math.abs(n).toString(16).padStart(2,"0")}`}const pe=16,ge=16;class me{constructor(e=pe,t=ge){this.hit=0,this.miss=0,this.maxKeyLength=e,this.maxLengthPerKey=t,this.caches=[];for(let i=0;i0&&e<=this.maxKeyLength}find(e,t,i){const s=this.caches[i-1];e:for(const r of s){const o=r.bytes;for(let a=0;a=this.maxLengthPerKey?i[Math.random()*i.length|0]=s:i.push(s)}decode(e,t,i){const s=this.find(e,t,i);if(s!=null)return this.hit++,s;this.miss++;const r=R(e,t,i),o=Uint8Array.prototype.slice.call(e,t,t+i);return this.store(o,r),r}}const _="array",p="map_key",K="map_value",Ue=n=>{if(typeof n=="string"||typeof n=="number")return n;throw new c("The type of key must be string or number but "+typeof n)};class Se{constructor(){this.stack=[],this.stackHeadPosition=-1}get length(){return this.stackHeadPosition+1}top(){return this.stack[this.stackHeadPosition]}pushArrayState(e){const t=this.getUninitializedStateFromPool();t.type=_,t.position=0,t.size=e,t.array=new Array(e)}pushMapState(e){const t=this.getUninitializedStateFromPool();t.type=p,t.readCount=0,t.size=e,t.map={}}getUninitializedStateFromPool(){if(this.stackHeadPosition++,this.stackHeadPosition===this.stack.length){const e={type:void 0,size:0,array:void 0,position:0,readCount:0,map:void 0,key:null};this.stack.push(e)}return this.stack[this.stackHeadPosition]}release(e){if(this.stack[this.stackHeadPosition]!==e)throw new Error("Invalid stack state. Released state is not on top of the stack.");if(e.type===_){const i=e;i.size=0,i.array=void 0,i.position=0,i.type=void 0}if(e.type===p||e.type===K){const i=e;i.size=0,i.map=void 0,i.readCount=0,i.type=void 0}this.stackHeadPosition--}reset(){this.stack.length=0,this.stackHeadPosition=-1}}const y=-1,P=new DataView(new ArrayBuffer(0)),Ee=new Uint8Array(P.buffer);try{P.getInt8(0)}catch(n){if(!(n instanceof RangeError))throw new Error("This module is not supported in the current JavaScript engine because DataView does not throw RangeError on out-of-bounds access")}const $=new RangeError("Insufficient data"),Be=new me;class L{constructor(e){this.totalPos=0,this.pos=0,this.view=P,this.bytes=Ee,this.headByte=y,this.stack=new Se,this.entered=!1,this.extensionCodec=e?.extensionCodec??E.defaultCodec,this.context=e?.context,this.useBigInt64=e?.useBigInt64??!1,this.rawStrings=e?.rawStrings??!1,this.maxStrLength=e?.maxStrLength??x,this.maxBinLength=e?.maxBinLength??x,this.maxArrayLength=e?.maxArrayLength??x,this.maxMapLength=e?.maxMapLength??x,this.maxExtLength=e?.maxExtLength??x,this.keyDecoder=e?.keyDecoder!==void 0?e.keyDecoder:Be,this.mapKeyConverter=e?.mapKeyConverter??Ue}clone(){return new L({extensionCodec:this.extensionCodec,context:this.context,useBigInt64:this.useBigInt64,rawStrings:this.rawStrings,maxStrLength:this.maxStrLength,maxBinLength:this.maxBinLength,maxArrayLength:this.maxArrayLength,maxMapLength:this.maxMapLength,maxExtLength:this.maxExtLength,keyDecoder:this.keyDecoder})}reinitializeState(){this.totalPos=0,this.headByte=y,this.stack.reset()}setBuffer(e){const t=z(e);this.bytes=t,this.view=new DataView(t.buffer,t.byteOffset,t.byteLength),this.pos=0}appendBuffer(e){if(this.headByte===y&&!this.hasRemaining(1))this.setBuffer(e);else{const t=this.bytes.subarray(this.pos),i=z(e),s=new Uint8Array(t.length+i.length);s.set(t),s.set(i,t.length),this.setBuffer(s)}}hasRemaining(e){return this.view.byteLength-this.pos>=e}createExtraByteError(e){const{view:t,pos:i}=this;return new RangeError(`Extra ${t.byteLength-i} of ${t.byteLength} byte(s) found at buffer[${e}]`)}decode(e){if(this.entered)return this.clone().decode(e);try{this.entered=!0,this.reinitializeState(),this.setBuffer(e);const t=this.doDecodeSync();if(this.hasRemaining(1))throw this.createExtraByteError(this.pos);return t}finally{this.entered=!1}}*decodeMulti(e){if(this.entered){yield*this.clone().decodeMulti(e);return}try{for(this.entered=!0,this.reinitializeState(),this.setBuffer(e);this.hasRemaining(1);)yield this.doDecodeSync()}finally{this.entered=!1}}async decodeAsync(e){if(this.entered)return this.clone().decodeAsync(e);try{this.entered=!0;let t=!1,i;for await(const a of e){if(t)throw this.entered=!1,this.createExtraByteError(this.totalPos);this.appendBuffer(a);try{i=this.doDecodeSync(),t=!0}catch(h){if(!(h instanceof RangeError))throw h}this.totalPos+=this.pos}if(t){if(this.hasRemaining(1))throw this.createExtraByteError(this.totalPos);return i}const{headByte:s,pos:r,totalPos:o}=this;throw new RangeError(`Insufficient data in parsing ${v(s)} at ${o} (${r} in the current buffer)`)}finally{this.entered=!1}}decodeArrayStream(e){return this.decodeMultiAsync(e,!0)}decodeStream(e){return this.decodeMultiAsync(e,!1)}async*decodeMultiAsync(e,t){if(this.entered){yield*this.clone().decodeMultiAsync(e,t);return}try{this.entered=!0;let i=t,s=-1;for await(const r of e){if(t&&s===0)throw this.createExtraByteError(this.totalPos);this.appendBuffer(r),i&&(s=this.readArraySize(),i=!1,this.complete());try{for(;yield this.doDecodeSync(),--s!==0;);}catch(o){if(!(o instanceof RangeError))throw o}this.totalPos+=this.pos}}finally{this.entered=!1}}doDecodeSync(){e:for(;;){const e=this.readHeadByte();let t;if(e>=224)t=e-256;else if(e<192)if(e<128)t=e;else if(e<144){const s=e-128;if(s!==0){this.pushMapState(s),this.complete();continue e}else t={}}else if(e<160){const s=e-144;if(s!==0){this.pushArrayState(s),this.complete();continue e}else t=[]}else{const s=e-160;t=this.decodeString(s,0)}else if(e===192)t=null;else if(e===194)t=!1;else if(e===195)t=!0;else if(e===202)t=this.readF32();else if(e===203)t=this.readF64();else if(e===204)t=this.readU8();else if(e===205)t=this.readU16();else if(e===206)t=this.readU32();else if(e===207)this.useBigInt64?t=this.readU64AsBigInt():t=this.readU64();else if(e===208)t=this.readI8();else if(e===209)t=this.readI16();else if(e===210)t=this.readI32();else if(e===211)this.useBigInt64?t=this.readI64AsBigInt():t=this.readI64();else if(e===217){const s=this.lookU8();t=this.decodeString(s,1)}else if(e===218){const s=this.lookU16();t=this.decodeString(s,2)}else if(e===219){const s=this.lookU32();t=this.decodeString(s,4)}else if(e===220){const s=this.readU16();if(s!==0){this.pushArrayState(s),this.complete();continue e}else t=[]}else if(e===221){const s=this.readU32();if(s!==0){this.pushArrayState(s),this.complete();continue e}else t=[]}else if(e===222){const s=this.readU16();if(s!==0){this.pushMapState(s),this.complete();continue e}else t={}}else if(e===223){const s=this.readU32();if(s!==0){this.pushMapState(s),this.complete();continue e}else t={}}else if(e===196){const s=this.lookU8();t=this.decodeBinary(s,1)}else if(e===197){const s=this.lookU16();t=this.decodeBinary(s,2)}else if(e===198){const s=this.lookU32();t=this.decodeBinary(s,4)}else if(e===212)t=this.decodeExtension(1,0);else if(e===213)t=this.decodeExtension(2,0);else if(e===214)t=this.decodeExtension(4,0);else if(e===215)t=this.decodeExtension(8,0);else if(e===216)t=this.decodeExtension(16,0);else if(e===199){const s=this.lookU8();t=this.decodeExtension(s,1)}else if(e===200){const s=this.lookU16();t=this.decodeExtension(s,2)}else if(e===201){const s=this.lookU32();t=this.decodeExtension(s,4)}else throw new c(`Unrecognized type byte: ${v(e)}`);this.complete();const i=this.stack;for(;i.length>0;){const s=i.top();if(s.type===_)if(s.array[s.position]=t,s.position++,s.position===s.size)t=s.array,i.release(s);else continue e;else if(s.type===p){if(t==="__proto__")throw new c("The key __proto__ is not allowed");s.key=this.mapKeyConverter(t),s.type=K;continue e}else if(s.map[s.key]=t,s.readCount++,s.readCount===s.size)t=s.map,i.release(s);else{s.key=null,s.type=p;continue e}}return t}}readHeadByte(){return this.headByte===y&&(this.headByte=this.readU8()),this.headByte}complete(){this.headByte=y}readArraySize(){const e=this.readHeadByte();switch(e){case 220:return this.readU16();case 221:return this.readU32();default:{if(e<160)return e-144;throw new c(`Unrecognized array type byte: ${v(e)}`)}}}pushMapState(e){if(e>this.maxMapLength)throw new c(`Max length exceeded: map length (${e}) > maxMapLengthLength (${this.maxMapLength})`);this.stack.pushMapState(e)}pushArrayState(e){if(e>this.maxArrayLength)throw new c(`Max length exceeded: array length (${e}) > maxArrayLength (${this.maxArrayLength})`);this.stack.pushArrayState(e)}decodeString(e,t){return!this.rawStrings||this.stateIsMapKey()?this.decodeUtf8String(e,t):this.decodeBinary(e,t)}decodeUtf8String(e,t){if(e>this.maxStrLength)throw new c(`Max length exceeded: UTF-8 byte length (${e}) > maxStrLength (${this.maxStrLength})`);if(this.bytes.byteLength0?this.stack.top().type===p:!1}decodeBinary(e,t){if(e>this.maxBinLength)throw new c(`Max length exceeded: bin length (${e}) > maxBinLength (${this.maxBinLength})`);if(!this.hasRemaining(e+t))throw $;const i=this.pos+t,s=this.bytes.subarray(i,i+e);return this.pos+=t+e,s}decodeExtension(e,t){if(e>this.maxExtLength)throw new c(`Max length exceeded: ext length (${e}) > maxExtLength (${this.maxExtLength})`);const i=this.view.getInt8(this.pos+t),s=this.decodeBinary(e,t+1);return this.extensionCodec.decode(s,i,this.context)}lookU8(){return this.view.getUint8(this.pos)}lookU16(){return this.view.getUint16(this.pos)}lookU32(){return this.view.getUint32(this.pos)}readU8(){const e=this.view.getUint8(this.pos);return this.pos++,e}readI8(){const e=this.view.getInt8(this.pos);return this.pos++,e}readU16(){const e=this.view.getUint16(this.pos);return this.pos+=2,e}readI16(){const e=this.view.getInt16(this.pos);return this.pos+=2,e}readU32(){const e=this.view.getUint32(this.pos);return this.pos+=4,e}readI32(){const e=this.view.getInt32(this.pos);return this.pos+=4,e}readU64(){const e=ie(this.view,this.pos);return this.pos+=8,e}readI64(){const e=b(this.view,this.pos);return this.pos+=8,e}readU64AsBigInt(){const e=this.view.getBigUint64(this.pos);return this.pos+=8,e}readI64AsBigInt(){const e=this.view.getBigInt64(this.pos);return this.pos+=8,e}readF32(){const e=this.view.getFloat32(this.pos);return this.pos+=4,e}readF64(){const e=this.view.getFloat64(this.pos);return this.pos+=8,e}}function Te(n,e){return new L(e).decode(n)}var B=typeof globalThis<"u"?globalThis:typeof window<"u"?window:typeof global<"u"?global:typeof self<"u"?self:{},W={},d=B&&B.__classPrivateFieldGet||function(n,e,t,i){if(t==="a"&&!i)throw new TypeError("Private accessor was defined without a getter");if(typeof e=="function"?n!==e||!i:!e.has(n))throw new TypeError("Cannot read private member from an object whose class did not declare it");return t==="m"?i:t==="a"?i.call(n):i?i.value:e.get(n)},M=B&&B.__classPrivateFieldSet||function(n,e,t,i,s){if(i==="m")throw new TypeError("Private method is not writable");if(i==="a"&&!s)throw new TypeError("Private accessor was defined without a setter");if(typeof e=="function"?n!==e||!s:!e.has(n))throw new TypeError("Cannot write private member to an object whose class did not declare it");return i==="a"?s.call(n,t):s?s.value=t:e.set(n,t),t},l,w;Object.defineProperty(W,"__esModule",{value:!0});class Ae{constructor(){l.set(this,!1),w.set(this,new Set)}get acquired(){return d(this,l,"f")}acquireAsync({timeout:e}={}){if(!d(this,l,"f"))return M(this,l,!0,"f"),Promise.resolve();if(e==null)return new Promise(s=>{d(this,w,"f").add(s)});let t,i;return Promise.race([new Promise(s=>{t=()=>{clearTimeout(i),s()},d(this,w,"f").add(t)}),new Promise((s,r)=>{i=setTimeout(()=>{d(this,w,"f").delete(t),r(new Error("Timed out waiting for lock"))},e)})])}tryAcquire(){return d(this,l,"f")?!1:(M(this,l,!0,"f"),!0)}release(){if(!d(this,l,"f"))throw new Error("Cannot release an unacquired lock");if(d(this,w,"f").size>0){const[e]=d(this,w,"f");d(this,w,"f").delete(e),e()}else M(this,l,!1,"f")}}var Ie=W.default=Ae;l=new WeakMap,w=new WeakMap;const k="1.0.16";function N(n,e){if(n instanceof ArrayBuffer)e.add(n);else if(n instanceof Uint8Array)e.add(n.buffer);else if(n&&typeof n=="object")for(const t in n)Object.prototype.hasOwnProperty.call(n,t)&&N(n[t],e);return e}{let n=null,e=null;const t=new Ie,i=(r,o)=>{self.postMessage(r,o)},s=()=>{e!==null&&e.close();const r=`viser-v${k}`;console.log(`Connecting to: ${n} with protocol: ${r}`),e=new WebSocket(n,[r]);const o=setTimeout(()=>{e?.close()},5e3);e.onopen=()=>{clearTimeout(o),console.log(`Connected! ${n}`),i({type:"connected"})},e.onclose=h=>{const u=h.code===1002;i({type:"closed",versionMismatch:u,clientVersion:k,closeReason:h.reason||"Connection closed"}),console.log(`Disconnected! ${n} code=${h.code}, reason: ${h.reason}`),u&&console.warn(`Connection rejected due to version mismatch. Client version: ${k}`),clearTimeout(o),n!==null&&requestAnimationFrame(()=>{setTimeout(s,1e3)})};const a={jsTimeMinusPythonTime:1/0};e.onmessage=async h=>{const u=new Promise(F=>{h.data.arrayBuffer().then(U=>{F(Te(new Uint8Array(U)))})}),T=performance.now();await t.acquireAsync({timeout:1e4}).catch(()=>{console.log("Order lock timed out."),t.release()});const f=await u;a.jsTimeMinusPythonTime=Math.min(T-f.timestampSec*1e3,a.jsTimeMinusPythonTime);const C=f.messages,V=N(C,new Set),A=()=>{i({type:"message_batch",messages:C},Array.from(V)),t.release()},g=performance.now(),m=f.timestampSec*1e3,I=m-(a.prevPythonTimestampMs??m);if(a.prevPythonTimestampMs=m,a.lastIdealJsMs===void 0||I>100||g-a.jsTimeMinusPythonTime-m>100)A(),a.lastIdealJsMs=g;else{const U=a.lastIdealJsMs+I-g;U>3?(setTimeout(A,U*.95),a.lastIdealJsMs=a.lastIdealJsMs+I*.95):(A(),a.lastIdealJsMs=g)}}};self.onmessage=r=>{const o=r.data;o.type==="send"?e.send(ye(o.message)):o.type==="set_server"?(n=o.server,s()):o.type=="close"?(n=null,e!==null&&e.close(),self.close()):console.log(`WebSocket worker: got ${o}, not sure what to do with it!`)}}
================================================
FILE: newton/_src/viewer/viser/static/assets/__vite-browser-external-BIHI7g3E.js
================================================
const e={};export{e as default};
================================================
FILE: newton/_src/viewer/viser/static/assets/index-BVvA0mmR.css
================================================
:root{color-scheme:var(--mantine-color-scheme)}*,*:before,*:after{box-sizing:border-box}input,button,textarea,select{font:inherit}button,select{text-transform:none}body{margin:0;font-family:var(--mantine-font-family);font-size:var(--mantine-font-size-md);line-height:var(--mantine-line-height);background-color:var(--mantine-color-body);color:var(--mantine-color-text);-webkit-font-smoothing:var(--mantine-webkit-font-smoothing);-moz-osx-font-smoothing:var(--mantine-moz-font-smoothing)}@media screen and (max-device-width: 31.25em){body{-webkit-text-size-adjust:100%}}@media (prefers-reduced-motion: reduce){[data-respect-reduced-motion] [data-reduce-motion]{transition:none;animation:none}}[data-mantine-color-scheme=light] .mantine-light-hidden,[data-mantine-color-scheme=dark] .mantine-dark-hidden{display:none}.mantine-focus-auto:focus-visible{outline:2px solid var(--mantine-primary-color-filled);outline-offset:calc(.125rem * var(--mantine-scale))}.mantine-focus-always:focus{outline:2px solid var(--mantine-primary-color-filled);outline-offset:calc(.125rem * var(--mantine-scale))}.mantine-focus-never:focus{outline:none}.mantine-active:active{transform:translateY(calc(.0625rem * var(--mantine-scale)))}fieldset:disabled .mantine-active:active{transform:none}:where([dir=rtl]) .mantine-rotate-rtl{transform:rotate(180deg)}:root{--mantine-z-index-app: 100;--mantine-z-index-modal: 200;--mantine-z-index-popover: 300;--mantine-z-index-overlay: 400;--mantine-z-index-max: 9999;--mantine-scale: 1;--mantine-cursor-type: default;--mantine-webkit-font-smoothing: antialiased;--mantine-moz-font-smoothing: grayscale;--mantine-color-white: #fff;--mantine-color-black: #000;--mantine-line-height: 1.55;--mantine-font-family: -apple-system, BlinkMacSystemFont, Segoe UI, Roboto, Helvetica, Arial, sans-serif, Apple Color Emoji, Segoe UI Emoji;--mantine-font-family-monospace: ui-monospace, SFMono-Regular, Menlo, Monaco, Consolas, Liberation Mono, Courier New, monospace;--mantine-font-family-headings: -apple-system, BlinkMacSystemFont, Segoe UI, Roboto, Helvetica, Arial, sans-serif, Apple Color Emoji, Segoe UI Emoji;--mantine-heading-font-weight: 700;--mantine-heading-text-wrap: wrap;--mantine-radius-default: calc(.25rem * var(--mantine-scale));--mantine-primary-color-filled: var(--mantine-color-blue-filled);--mantine-primary-color-filled-hover: var(--mantine-color-blue-filled-hover);--mantine-primary-color-light: var(--mantine-color-blue-light);--mantine-primary-color-light-hover: var(--mantine-color-blue-light-hover);--mantine-primary-color-light-color: var(--mantine-color-blue-light-color);--mantine-breakpoint-xs: 36em;--mantine-breakpoint-sm: 48em;--mantine-breakpoint-md: 62em;--mantine-breakpoint-lg: 75em;--mantine-breakpoint-xl: 88em;--mantine-spacing-xs: calc(.625rem * var(--mantine-scale));--mantine-spacing-sm: calc(.75rem * var(--mantine-scale));--mantine-spacing-md: calc(1rem * var(--mantine-scale));--mantine-spacing-lg: calc(1.25rem * var(--mantine-scale));--mantine-spacing-xl: calc(2rem * var(--mantine-scale));--mantine-font-size-xs: calc(.75rem * var(--mantine-scale));--mantine-font-size-sm: calc(.875rem * var(--mantine-scale));--mantine-font-size-md: calc(1rem * var(--mantine-scale));--mantine-font-size-lg: calc(1.125rem * var(--mantine-scale));--mantine-font-size-xl: calc(1.25rem * var(--mantine-scale));--mantine-line-height-xs: 1.4;--mantine-line-height-sm: 1.45;--mantine-line-height-md: 1.55;--mantine-line-height-lg: 1.6;--mantine-line-height-xl: 1.65;--mantine-shadow-xs: 0 calc(.0625rem * var(--mantine-scale)) calc(.1875rem * var(--mantine-scale)) rgba(0, 0, 0, .05), 0 calc(.0625rem * var(--mantine-scale)) calc(.125rem * var(--mantine-scale)) rgba(0, 0, 0, .1);--mantine-shadow-sm: 0 calc(.0625rem * var(--mantine-scale)) calc(.1875rem * var(--mantine-scale)) rgba(0, 0, 0, .05), rgba(0, 0, 0, .05) 0 calc(.625rem * var(--mantine-scale)) calc(.9375rem * var(--mantine-scale)) calc(-.3125rem * var(--mantine-scale)), rgba(0, 0, 0, .04) 0 calc(.4375rem * var(--mantine-scale)) calc(.4375rem * var(--mantine-scale)) calc(-.3125rem * var(--mantine-scale));--mantine-shadow-md: 0 calc(.0625rem * var(--mantine-scale)) calc(.1875rem * var(--mantine-scale)) rgba(0, 0, 0, .05), rgba(0, 0, 0, .05) 0 calc(1.25rem * var(--mantine-scale)) calc(1.5625rem * var(--mantine-scale)) calc(-.3125rem * var(--mantine-scale)), rgba(0, 0, 0, .04) 0 calc(.625rem * var(--mantine-scale)) calc(.625rem * var(--mantine-scale)) calc(-.3125rem * var(--mantine-scale));--mantine-shadow-lg: 0 calc(.0625rem * var(--mantine-scale)) calc(.1875rem * var(--mantine-scale)) rgba(0, 0, 0, .05), rgba(0, 0, 0, .05) 0 calc(1.75rem * var(--mantine-scale)) calc(1.4375rem * var(--mantine-scale)) calc(-.4375rem * var(--mantine-scale)), rgba(0, 0, 0, .04) 0 calc(.75rem * var(--mantine-scale)) calc(.75rem * var(--mantine-scale)) calc(-.4375rem * var(--mantine-scale));--mantine-shadow-xl: 0 calc(.0625rem * var(--mantine-scale)) calc(.1875rem * var(--mantine-scale)) rgba(0, 0, 0, .05), rgba(0, 0, 0, .05) 0 calc(2.25rem * var(--mantine-scale)) calc(1.75rem * var(--mantine-scale)) calc(-.4375rem * var(--mantine-scale)), rgba(0, 0, 0, .04) 0 calc(1.0625rem * var(--mantine-scale)) calc(1.0625rem * var(--mantine-scale)) calc(-.4375rem * var(--mantine-scale));--mantine-radius-xs: calc(.125rem * var(--mantine-scale));--mantine-radius-sm: calc(.25rem * var(--mantine-scale));--mantine-radius-md: calc(.5rem * var(--mantine-scale));--mantine-radius-lg: calc(1rem * var(--mantine-scale));--mantine-radius-xl: calc(2rem * var(--mantine-scale));--mantine-primary-color-0: var(--mantine-color-blue-0);--mantine-primary-color-1: var(--mantine-color-blue-1);--mantine-primary-color-2: var(--mantine-color-blue-2);--mantine-primary-color-3: var(--mantine-color-blue-3);--mantine-primary-color-4: var(--mantine-color-blue-4);--mantine-primary-color-5: var(--mantine-color-blue-5);--mantine-primary-color-6: var(--mantine-color-blue-6);--mantine-primary-color-7: var(--mantine-color-blue-7);--mantine-primary-color-8: var(--mantine-color-blue-8);--mantine-primary-color-9: var(--mantine-color-blue-9);--mantine-color-dark-0: #c9c9c9;--mantine-color-dark-1: #b8b8b8;--mantine-color-dark-2: #828282;--mantine-color-dark-3: #696969;--mantine-color-dark-4: #424242;--mantine-color-dark-5: #3b3b3b;--mantine-color-dark-6: #2e2e2e;--mantine-color-dark-7: #242424;--mantine-color-dark-8: #1f1f1f;--mantine-color-dark-9: #141414;--mantine-color-gray-0: #f8f9fa;--mantine-color-gray-1: #f1f3f5;--mantine-color-gray-2: #e9ecef;--mantine-color-gray-3: #dee2e6;--mantine-color-gray-4: #ced4da;--mantine-color-gray-5: #adb5bd;--mantine-color-gray-6: #868e96;--mantine-color-gray-7: #495057;--mantine-color-gray-8: #343a40;--mantine-color-gray-9: #212529;--mantine-color-red-0: #fff5f5;--mantine-color-red-1: #ffe3e3;--mantine-color-red-2: #ffc9c9;--mantine-color-red-3: #ffa8a8;--mantine-color-red-4: #ff8787;--mantine-color-red-5: #ff6b6b;--mantine-color-red-6: #fa5252;--mantine-color-red-7: #f03e3e;--mantine-color-red-8: #e03131;--mantine-color-red-9: #c92a2a;--mantine-color-pink-0: #fff0f6;--mantine-color-pink-1: #ffdeeb;--mantine-color-pink-2: #fcc2d7;--mantine-color-pink-3: #faa2c1;--mantine-color-pink-4: #f783ac;--mantine-color-pink-5: #f06595;--mantine-color-pink-6: #e64980;--mantine-color-pink-7: #d6336c;--mantine-color-pink-8: #c2255c;--mantine-color-pink-9: #a61e4d;--mantine-color-grape-0: #f8f0fc;--mantine-color-grape-1: #f3d9fa;--mantine-color-grape-2: #eebefa;--mantine-color-grape-3: #e599f7;--mantine-color-grape-4: #da77f2;--mantine-color-grape-5: #cc5de8;--mantine-color-grape-6: #be4bdb;--mantine-color-grape-7: #ae3ec9;--mantine-color-grape-8: #9c36b5;--mantine-color-grape-9: #862e9c;--mantine-color-violet-0: #f3f0ff;--mantine-color-violet-1: #e5dbff;--mantine-color-violet-2: #d0bfff;--mantine-color-violet-3: #b197fc;--mantine-color-violet-4: #9775fa;--mantine-color-violet-5: #845ef7;--mantine-color-violet-6: #7950f2;--mantine-color-violet-7: #7048e8;--mantine-color-violet-8: #6741d9;--mantine-color-violet-9: #5f3dc4;--mantine-color-indigo-0: #edf2ff;--mantine-color-indigo-1: #dbe4ff;--mantine-color-indigo-2: #bac8ff;--mantine-color-indigo-3: #91a7ff;--mantine-color-indigo-4: #748ffc;--mantine-color-indigo-5: #5c7cfa;--mantine-color-indigo-6: #4c6ef5;--mantine-color-indigo-7: #4263eb;--mantine-color-indigo-8: #3b5bdb;--mantine-color-indigo-9: #364fc7;--mantine-color-blue-0: #e7f5ff;--mantine-color-blue-1: #d0ebff;--mantine-color-blue-2: #a5d8ff;--mantine-color-blue-3: #74c0fc;--mantine-color-blue-4: #4dabf7;--mantine-color-blue-5: #339af0;--mantine-color-blue-6: #228be6;--mantine-color-blue-7: #1c7ed6;--mantine-color-blue-8: #1971c2;--mantine-color-blue-9: #1864ab;--mantine-color-cyan-0: #e3fafc;--mantine-color-cyan-1: #c5f6fa;--mantine-color-cyan-2: #99e9f2;--mantine-color-cyan-3: #66d9e8;--mantine-color-cyan-4: #3bc9db;--mantine-color-cyan-5: #22b8cf;--mantine-color-cyan-6: #15aabf;--mantine-color-cyan-7: #1098ad;--mantine-color-cyan-8: #0c8599;--mantine-color-cyan-9: #0b7285;--mantine-color-teal-0: #e6fcf5;--mantine-color-teal-1: #c3fae8;--mantine-color-teal-2: #96f2d7;--mantine-color-teal-3: #63e6be;--mantine-color-teal-4: #38d9a9;--mantine-color-teal-5: #20c997;--mantine-color-teal-6: #12b886;--mantine-color-teal-7: #0ca678;--mantine-color-teal-8: #099268;--mantine-color-teal-9: #087f5b;--mantine-color-green-0: #ebfbee;--mantine-color-green-1: #d3f9d8;--mantine-color-green-2: #b2f2bb;--mantine-color-green-3: #8ce99a;--mantine-color-green-4: #69db7c;--mantine-color-green-5: #51cf66;--mantine-color-green-6: #40c057;--mantine-color-green-7: #37b24d;--mantine-color-green-8: #2f9e44;--mantine-color-green-9: #2b8a3e;--mantine-color-lime-0: #f4fce3;--mantine-color-lime-1: #e9fac8;--mantine-color-lime-2: #d8f5a2;--mantine-color-lime-3: #c0eb75;--mantine-color-lime-4: #a9e34b;--mantine-color-lime-5: #94d82d;--mantine-color-lime-6: #82c91e;--mantine-color-lime-7: #74b816;--mantine-color-lime-8: #66a80f;--mantine-color-lime-9: #5c940d;--mantine-color-yellow-0: #fff9db;--mantine-color-yellow-1: #fff3bf;--mantine-color-yellow-2: #ffec99;--mantine-color-yellow-3: #ffe066;--mantine-color-yellow-4: #ffd43b;--mantine-color-yellow-5: #fcc419;--mantine-color-yellow-6: #fab005;--mantine-color-yellow-7: #f59f00;--mantine-color-yellow-8: #f08c00;--mantine-color-yellow-9: #e67700;--mantine-color-orange-0: #fff4e6;--mantine-color-orange-1: #ffe8cc;--mantine-color-orange-2: #ffd8a8;--mantine-color-orange-3: #ffc078;--mantine-color-orange-4: #ffa94d;--mantine-color-orange-5: #ff922b;--mantine-color-orange-6: #fd7e14;--mantine-color-orange-7: #f76707;--mantine-color-orange-8: #e8590c;--mantine-color-orange-9: #d9480f;--mantine-h1-font-size: calc(2.125rem * var(--mantine-scale));--mantine-h1-line-height: 1.3;--mantine-h1-font-weight: 700;--mantine-h2-font-size: calc(1.625rem * var(--mantine-scale));--mantine-h2-line-height: 1.35;--mantine-h2-font-weight: 700;--mantine-h3-font-size: calc(1.375rem * var(--mantine-scale));--mantine-h3-line-height: 1.4;--mantine-h3-font-weight: 700;--mantine-h4-font-size: calc(1.125rem * var(--mantine-scale));--mantine-h4-line-height: 1.45;--mantine-h4-font-weight: 700;--mantine-h5-font-size: calc(1rem * var(--mantine-scale));--mantine-h5-line-height: 1.5;--mantine-h5-font-weight: 700;--mantine-h6-font-size: calc(.875rem * var(--mantine-scale));--mantine-h6-line-height: 1.5;--mantine-h6-font-weight: 700}:root[data-mantine-color-scheme=dark]{--mantine-color-scheme: dark;--mantine-primary-color-contrast: var(--mantine-color-white);--mantine-color-bright: var(--mantine-color-white);--mantine-color-text: var(--mantine-color-dark-0);--mantine-color-body: var(--mantine-color-dark-7);--mantine-color-error: var(--mantine-color-red-8);--mantine-color-placeholder: var(--mantine-color-dark-3);--mantine-color-anchor: var(--mantine-color-blue-4);--mantine-color-default: var(--mantine-color-dark-6);--mantine-color-default-hover: var(--mantine-color-dark-5);--mantine-color-default-color: var(--mantine-color-white);--mantine-color-default-border: var(--mantine-color-dark-4);--mantine-color-dimmed: var(--mantine-color-dark-2);--mantine-color-disabled: var(--mantine-color-dark-6);--mantine-color-disabled-color: var(--mantine-color-dark-3);--mantine-color-disabled-border: var(--mantine-color-dark-4);--mantine-color-dark-text: var(--mantine-color-dark-4);--mantine-color-dark-filled: var(--mantine-color-dark-8);--mantine-color-dark-filled-hover: var(--mantine-color-dark-9);--mantine-color-dark-light: rgba(46, 46, 46, .15);--mantine-color-dark-light-hover: rgba(46, 46, 46, .2);--mantine-color-dark-light-color: var(--mantine-color-dark-3);--mantine-color-dark-outline: var(--mantine-color-dark-4);--mantine-color-dark-outline-hover: rgba(66, 66, 66, .05);--mantine-color-gray-text: var(--mantine-color-gray-4);--mantine-color-gray-filled: var(--mantine-color-gray-8);--mantine-color-gray-filled-hover: var(--mantine-color-gray-9);--mantine-color-gray-light: rgba(134, 142, 150, .15);--mantine-color-gray-light-hover: rgba(134, 142, 150, .2);--mantine-color-gray-light-color: var(--mantine-color-gray-3);--mantine-color-gray-outline: var(--mantine-color-gray-4);--mantine-color-gray-outline-hover: rgba(206, 212, 218, .05);--mantine-color-red-text: var(--mantine-color-red-4);--mantine-color-red-filled: var(--mantine-color-red-8);--mantine-color-red-filled-hover: var(--mantine-color-red-9);--mantine-color-red-light: rgba(250, 82, 82, .15);--mantine-color-red-light-hover: rgba(250, 82, 82, .2);--mantine-color-red-light-color: var(--mantine-color-red-3);--mantine-color-red-outline: var(--mantine-color-red-4);--mantine-color-red-outline-hover: rgba(255, 135, 135, .05);--mantine-color-pink-text: var(--mantine-color-pink-4);--mantine-color-pink-filled: var(--mantine-color-pink-8);--mantine-color-pink-filled-hover: var(--mantine-color-pink-9);--mantine-color-pink-light: rgba(230, 73, 128, .15);--mantine-color-pink-light-hover: rgba(230, 73, 128, .2);--mantine-color-pink-light-color: var(--mantine-color-pink-3);--mantine-color-pink-outline: var(--mantine-color-pink-4);--mantine-color-pink-outline-hover: rgba(247, 131, 172, .05);--mantine-color-grape-text: var(--mantine-color-grape-4);--mantine-color-grape-filled: var(--mantine-color-grape-8);--mantine-color-grape-filled-hover: var(--mantine-color-grape-9);--mantine-color-grape-light: rgba(190, 75, 219, .15);--mantine-color-grape-light-hover: rgba(190, 75, 219, .2);--mantine-color-grape-light-color: var(--mantine-color-grape-3);--mantine-color-grape-outline: var(--mantine-color-grape-4);--mantine-color-grape-outline-hover: rgba(218, 119, 242, .05);--mantine-color-violet-text: var(--mantine-color-violet-4);--mantine-color-violet-filled: var(--mantine-color-violet-8);--mantine-color-violet-filled-hover: var(--mantine-color-violet-9);--mantine-color-violet-light: rgba(121, 80, 242, .15);--mantine-color-violet-light-hover: rgba(121, 80, 242, .2);--mantine-color-violet-light-color: var(--mantine-color-violet-3);--mantine-color-violet-outline: var(--mantine-color-violet-4);--mantine-color-violet-outline-hover: rgba(151, 117, 250, .05);--mantine-color-indigo-text: var(--mantine-color-indigo-4);--mantine-color-indigo-filled: var(--mantine-color-indigo-8);--mantine-color-indigo-filled-hover: var(--mantine-color-indigo-9);--mantine-color-indigo-light: rgba(76, 110, 245, .15);--mantine-color-indigo-light-hover: rgba(76, 110, 245, .2);--mantine-color-indigo-light-color: var(--mantine-color-indigo-3);--mantine-color-indigo-outline: var(--mantine-color-indigo-4);--mantine-color-indigo-outline-hover: rgba(116, 143, 252, .05);--mantine-color-blue-text: var(--mantine-color-blue-4);--mantine-color-blue-filled: var(--mantine-color-blue-8);--mantine-color-blue-filled-hover: var(--mantine-color-blue-9);--mantine-color-blue-light: rgba(34, 139, 230, .15);--mantine-color-blue-light-hover: rgba(34, 139, 230, .2);--mantine-color-blue-light-color: var(--mantine-color-blue-3);--mantine-color-blue-outline: var(--mantine-color-blue-4);--mantine-color-blue-outline-hover: rgba(77, 171, 247, .05);--mantine-color-cyan-text: var(--mantine-color-cyan-4);--mantine-color-cyan-filled: var(--mantine-color-cyan-8);--mantine-color-cyan-filled-hover: var(--mantine-color-cyan-9);--mantine-color-cyan-light: rgba(21, 170, 191, .15);--mantine-color-cyan-light-hover: rgba(21, 170, 191, .2);--mantine-color-cyan-light-color: var(--mantine-color-cyan-3);--mantine-color-cyan-outline: var(--mantine-color-cyan-4);--mantine-color-cyan-outline-hover: rgba(59, 201, 219, .05);--mantine-color-teal-text: var(--mantine-color-teal-4);--mantine-color-teal-filled: var(--mantine-color-teal-8);--mantine-color-teal-filled-hover: var(--mantine-color-teal-9);--mantine-color-teal-light: rgba(18, 184, 134, .15);--mantine-color-teal-light-hover: rgba(18, 184, 134, .2);--mantine-color-teal-light-color: var(--mantine-color-teal-3);--mantine-color-teal-outline: var(--mantine-color-teal-4);--mantine-color-teal-outline-hover: rgba(56, 217, 169, .05);--mantine-color-green-text: var(--mantine-color-green-4);--mantine-color-green-filled: var(--mantine-color-green-8);--mantine-color-green-filled-hover: var(--mantine-color-green-9);--mantine-color-green-light: rgba(64, 192, 87, .15);--mantine-color-green-light-hover: rgba(64, 192, 87, .2);--mantine-color-green-light-color: var(--mantine-color-green-3);--mantine-color-green-outline: var(--mantine-color-green-4);--mantine-color-green-outline-hover: rgba(105, 219, 124, .05);--mantine-color-lime-text: var(--mantine-color-lime-4);--mantine-color-lime-filled: var(--mantine-color-lime-8);--mantine-color-lime-filled-hover: var(--mantine-color-lime-9);--mantine-color-lime-light: rgba(130, 201, 30, .15);--mantine-color-lime-light-hover: rgba(130, 201, 30, .2);--mantine-color-lime-light-color: var(--mantine-color-lime-3);--mantine-color-lime-outline: var(--mantine-color-lime-4);--mantine-color-lime-outline-hover: rgba(169, 227, 75, .05);--mantine-color-yellow-text: var(--mantine-color-yellow-4);--mantine-color-yellow-filled: var(--mantine-color-yellow-8);--mantine-color-yellow-filled-hover: var(--mantine-color-yellow-9);--mantine-color-yellow-light: rgba(250, 176, 5, .15);--mantine-color-yellow-light-hover: rgba(250, 176, 5, .2);--mantine-color-yellow-light-color: var(--mantine-color-yellow-3);--mantine-color-yellow-outline: var(--mantine-color-yellow-4);--mantine-color-yellow-outline-hover: rgba(255, 212, 59, .05);--mantine-color-orange-text: var(--mantine-color-orange-4);--mantine-color-orange-filled: var(--mantine-color-orange-8);--mantine-color-orange-filled-hover: var(--mantine-color-orange-9);--mantine-color-orange-light: rgba(253, 126, 20, .15);--mantine-color-orange-light-hover: rgba(253, 126, 20, .2);--mantine-color-orange-light-color: var(--mantine-color-orange-3);--mantine-color-orange-outline: var(--mantine-color-orange-4);--mantine-color-orange-outline-hover: rgba(255, 169, 77, .05)}:root[data-mantine-color-scheme=light]{--mantine-color-scheme: light;--mantine-primary-color-contrast: var(--mantine-color-white);--mantine-color-bright: var(--mantine-color-black);--mantine-color-text: #000;--mantine-color-body: #fff;--mantine-color-error: var(--mantine-color-red-6);--mantine-color-placeholder: var(--mantine-color-gray-5);--mantine-color-anchor: var(--mantine-color-blue-6);--mantine-color-default: var(--mantine-color-white);--mantine-color-default-hover: var(--mantine-color-gray-0);--mantine-color-default-color: var(--mantine-color-black);--mantine-color-default-border: var(--mantine-color-gray-4);--mantine-color-dimmed: var(--mantine-color-gray-6);--mantine-color-disabled: var(--mantine-color-gray-2);--mantine-color-disabled-color: var(--mantine-color-gray-5);--mantine-color-disabled-border: var(--mantine-color-gray-3);--mantine-color-dark-text: var(--mantine-color-dark-filled);--mantine-color-dark-filled: var(--mantine-color-dark-6);--mantine-color-dark-filled-hover: var(--mantine-color-dark-7);--mantine-color-dark-light: rgba(46, 46, 46, .1);--mantine-color-dark-light-hover: rgba(46, 46, 46, .12);--mantine-color-dark-light-color: var(--mantine-color-dark-6);--mantine-color-dark-outline: var(--mantine-color-dark-6);--mantine-color-dark-outline-hover: rgba(46, 46, 46, .05);--mantine-color-gray-text: var(--mantine-color-gray-filled);--mantine-color-gray-filled: var(--mantine-color-gray-6);--mantine-color-gray-filled-hover: var(--mantine-color-gray-7);--mantine-color-gray-light: rgba(134, 142, 150, .1);--mantine-color-gray-light-hover: rgba(134, 142, 150, .12);--mantine-color-gray-light-color: var(--mantine-color-gray-6);--mantine-color-gray-outline: var(--mantine-color-gray-6);--mantine-color-gray-outline-hover: rgba(134, 142, 150, .05);--mantine-color-red-text: var(--mantine-color-red-filled);--mantine-color-red-filled: var(--mantine-color-red-6);--mantine-color-red-filled-hover: var(--mantine-color-red-7);--mantine-color-red-light: rgba(250, 82, 82, .1);--mantine-color-red-light-hover: rgba(250, 82, 82, .12);--mantine-color-red-light-color: var(--mantine-color-red-6);--mantine-color-red-outline: var(--mantine-color-red-6);--mantine-color-red-outline-hover: rgba(250, 82, 82, .05);--mantine-color-pink-text: var(--mantine-color-pink-filled);--mantine-color-pink-filled: var(--mantine-color-pink-6);--mantine-color-pink-filled-hover: var(--mantine-color-pink-7);--mantine-color-pink-light: rgba(230, 73, 128, .1);--mantine-color-pink-light-hover: rgba(230, 73, 128, .12);--mantine-color-pink-light-color: var(--mantine-color-pink-6);--mantine-color-pink-outline: var(--mantine-color-pink-6);--mantine-color-pink-outline-hover: rgba(230, 73, 128, .05);--mantine-color-grape-text: var(--mantine-color-grape-filled);--mantine-color-grape-filled: var(--mantine-color-grape-6);--mantine-color-grape-filled-hover: var(--mantine-color-grape-7);--mantine-color-grape-light: rgba(190, 75, 219, .1);--mantine-color-grape-light-hover: rgba(190, 75, 219, .12);--mantine-color-grape-light-color: var(--mantine-color-grape-6);--mantine-color-grape-outline: var(--mantine-color-grape-6);--mantine-color-grape-outline-hover: rgba(190, 75, 219, .05);--mantine-color-violet-text: var(--mantine-color-violet-filled);--mantine-color-violet-filled: var(--mantine-color-violet-6);--mantine-color-violet-filled-hover: var(--mantine-color-violet-7);--mantine-color-violet-light: rgba(121, 80, 242, .1);--mantine-color-violet-light-hover: rgba(121, 80, 242, .12);--mantine-color-violet-light-color: var(--mantine-color-violet-6);--mantine-color-violet-outline: var(--mantine-color-violet-6);--mantine-color-violet-outline-hover: rgba(121, 80, 242, .05);--mantine-color-indigo-text: var(--mantine-color-indigo-filled);--mantine-color-indigo-filled: var(--mantine-color-indigo-6);--mantine-color-indigo-filled-hover: var(--mantine-color-indigo-7);--mantine-color-indigo-light: rgba(76, 110, 245, .1);--mantine-color-indigo-light-hover: rgba(76, 110, 245, .12);--mantine-color-indigo-light-color: var(--mantine-color-indigo-6);--mantine-color-indigo-outline: var(--mantine-color-indigo-6);--mantine-color-indigo-outline-hover: rgba(76, 110, 245, .05);--mantine-color-blue-text: var(--mantine-color-blue-filled);--mantine-color-blue-filled: var(--mantine-color-blue-6);--mantine-color-blue-filled-hover: var(--mantine-color-blue-7);--mantine-color-blue-light: rgba(34, 139, 230, .1);--mantine-color-blue-light-hover: rgba(34, 139, 230, .12);--mantine-color-blue-light-color: var(--mantine-color-blue-6);--mantine-color-blue-outline: var(--mantine-color-blue-6);--mantine-color-blue-outline-hover: rgba(34, 139, 230, .05);--mantine-color-cyan-text: var(--mantine-color-cyan-filled);--mantine-color-cyan-filled: var(--mantine-color-cyan-6);--mantine-color-cyan-filled-hover: var(--mantine-color-cyan-7);--mantine-color-cyan-light: rgba(21, 170, 191, .1);--mantine-color-cyan-light-hover: rgba(21, 170, 191, .12);--mantine-color-cyan-light-color: var(--mantine-color-cyan-6);--mantine-color-cyan-outline: var(--mantine-color-cyan-6);--mantine-color-cyan-outline-hover: rgba(21, 170, 191, .05);--mantine-color-teal-text: var(--mantine-color-teal-filled);--mantine-color-teal-filled: var(--mantine-color-teal-6);--mantine-color-teal-filled-hover: var(--mantine-color-teal-7);--mantine-color-teal-light: rgba(18, 184, 134, .1);--mantine-color-teal-light-hover: rgba(18, 184, 134, .12);--mantine-color-teal-light-color: var(--mantine-color-teal-6);--mantine-color-teal-outline: var(--mantine-color-teal-6);--mantine-color-teal-outline-hover: rgba(18, 184, 134, .05);--mantine-color-green-text: var(--mantine-color-green-filled);--mantine-color-green-filled: var(--mantine-color-green-6);--mantine-color-green-filled-hover: var(--mantine-color-green-7);--mantine-color-green-light: rgba(64, 192, 87, .1);--mantine-color-green-light-hover: rgba(64, 192, 87, .12);--mantine-color-green-light-color: var(--mantine-color-green-6);--mantine-color-green-outline: var(--mantine-color-green-6);--mantine-color-green-outline-hover: rgba(64, 192, 87, .05);--mantine-color-lime-text: var(--mantine-color-lime-filled);--mantine-color-lime-filled: var(--mantine-color-lime-6);--mantine-color-lime-filled-hover: var(--mantine-color-lime-7);--mantine-color-lime-light: rgba(130, 201, 30, .1);--mantine-color-lime-light-hover: rgba(130, 201, 30, .12);--mantine-color-lime-light-color: var(--mantine-color-lime-6);--mantine-color-lime-outline: var(--mantine-color-lime-6);--mantine-color-lime-outline-hover: rgba(130, 201, 30, .05);--mantine-color-yellow-text: var(--mantine-color-yellow-filled);--mantine-color-yellow-filled: var(--mantine-color-yellow-6);--mantine-color-yellow-filled-hover: var(--mantine-color-yellow-7);--mantine-color-yellow-light: rgba(250, 176, 5, .1);--mantine-color-yellow-light-hover: rgba(250, 176, 5, .12);--mantine-color-yellow-light-color: var(--mantine-color-yellow-6);--mantine-color-yellow-outline: var(--mantine-color-yellow-6);--mantine-color-yellow-outline-hover: rgba(250, 176, 5, .05);--mantine-color-orange-text: var(--mantine-color-orange-filled);--mantine-color-orange-filled: var(--mantine-color-orange-6);--mantine-color-orange-filled-hover: var(--mantine-color-orange-7);--mantine-color-orange-light: rgba(253, 126, 20, .1);--mantine-color-orange-light-hover: rgba(253, 126, 20, .12);--mantine-color-orange-light-color: var(--mantine-color-orange-6);--mantine-color-orange-outline: var(--mantine-color-orange-6);--mantine-color-orange-outline-hover: rgba(253, 126, 20, .05)}.m_d57069b5{--scrollarea-scrollbar-size: calc(.75rem * var(--mantine-scale));position:relative;overflow:hidden}.m_d57069b5:where([data-autosize]) .m_b1336c6{min-width:min-content}.m_c0783ff9{scrollbar-width:none;overscroll-behavior:var(--scrollarea-over-scroll-behavior);-ms-overflow-style:none;-webkit-overflow-scrolling:touch;width:100%;height:100%}.m_c0783ff9::-webkit-scrollbar{display:none}.m_c0783ff9:where([data-scrollbars=xy],[data-scrollbars=y]):where([data-offset-scrollbars=xy],[data-offset-scrollbars=y],[data-offset-scrollbars=present]):where([data-vertical-hidden]){padding-inline-end:0;padding-inline-start:0}.m_c0783ff9:where([data-scrollbars=xy],[data-scrollbars=y]):where([data-offset-scrollbars=xy],[data-offset-scrollbars=y],[data-offset-scrollbars=present]):not([data-vertical-hidden]){padding-inline-end:var(--scrollarea-scrollbar-size);padding-inline-start:unset}.m_c0783ff9:where([data-scrollbars=xy],[data-scrollbars=x]):where([data-offset-scrollbars=xy],[data-offset-scrollbars=x],[data-offset-scrollbars=present]):where([data-horizontal-hidden]){padding-bottom:0}.m_c0783ff9:where([data-scrollbars=xy],[data-scrollbars=x]):where([data-offset-scrollbars=xy],[data-offset-scrollbars=x],[data-offset-scrollbars=present]):not([data-horizontal-hidden]){padding-bottom:var(--scrollarea-scrollbar-size)}.m_f8f631dd{min-width:100%;display:table}.m_c44ba933{-webkit-user-select:none;user-select:none;touch-action:none;box-sizing:border-box;transition:background-color .15s ease,opacity .15s ease;padding:calc(var(--scrollarea-scrollbar-size) / 5);display:flex;background-color:transparent;flex-direction:row}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_c44ba933:hover{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=light]) .m_c44ba933:hover>.m_d8b5e363{background-color:#00000080}:where([data-mantine-color-scheme=dark]) .m_c44ba933:hover{background-color:var(--mantine-color-dark-8)}:where([data-mantine-color-scheme=dark]) .m_c44ba933:hover>.m_d8b5e363{background-color:#ffffff80}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_c44ba933:active{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=light]) .m_c44ba933:active>.m_d8b5e363{background-color:#00000080}:where([data-mantine-color-scheme=dark]) .m_c44ba933:active{background-color:var(--mantine-color-dark-8)}:where([data-mantine-color-scheme=dark]) .m_c44ba933:active>.m_d8b5e363{background-color:#ffffff80}}.m_c44ba933:where([data-hidden],[data-state=hidden]){display:none}.m_c44ba933:where([data-orientation=vertical]){width:var(--scrollarea-scrollbar-size);top:0;bottom:var(--sa-corner-width);inset-inline-end:0}.m_c44ba933:where([data-orientation=horizontal]){height:var(--scrollarea-scrollbar-size);flex-direction:column;bottom:0;inset-inline-start:0;inset-inline-end:var(--sa-corner-width)}.m_d8b5e363{flex:1;border-radius:var(--scrollarea-scrollbar-size);position:relative;transition:background-color .15s ease;overflow:hidden;opacity:var(--thumb-opacity)}.m_d8b5e363:before{content:"";position:absolute;top:50%;left:50%;transform:translate(-50%,-50%);width:100%;height:100%;min-width:calc(2.75rem * var(--mantine-scale));min-height:calc(2.75rem * var(--mantine-scale))}:where([data-mantine-color-scheme=light]) .m_d8b5e363{background-color:#0006}:where([data-mantine-color-scheme=dark]) .m_d8b5e363{background-color:#fff6}.m_21657268{position:absolute;opacity:0;transition:opacity .15s ease;display:block;inset-inline-end:0;bottom:0}:where([data-mantine-color-scheme=light]) .m_21657268{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_21657268{background-color:var(--mantine-color-dark-8)}.m_21657268:where([data-hovered]){opacity:1}.m_21657268:where([data-hidden]){display:none}.m_b1336c6{min-width:100%}.m_87cf2631{background-color:transparent;cursor:pointer;border:0;padding:0;appearance:none;font-size:var(--mantine-font-size-md);text-align:left;text-decoration:none;color:inherit;touch-action:manipulation;-webkit-tap-highlight-color:transparent}:where([dir=rtl]) .m_87cf2631{text-align:right}.m_515a97f8{border:0;clip:rect(0 0 0 0);height:calc(.0625rem * var(--mantine-scale));width:calc(.0625rem * var(--mantine-scale));margin:calc(-.0625rem * var(--mantine-scale));overflow:hidden;padding:0;position:absolute;white-space:nowrap}.m_1b7284a3{--paper-radius: var(--mantine-radius-default);outline:0;-webkit-tap-highlight-color:transparent;display:block;touch-action:manipulation;text-decoration:none;border-radius:var(--paper-radius);box-shadow:var(--paper-shadow);background-color:var(--mantine-color-body)}[data-mantine-color-scheme=light] .m_1b7284a3{--paper-border-color: var(--mantine-color-gray-3)}[data-mantine-color-scheme=dark] .m_1b7284a3{--paper-border-color: var(--mantine-color-dark-4)}.m_1b7284a3:where([data-with-border]){border:calc(.0625rem * var(--mantine-scale)) solid var(--paper-border-color)}.m_9814e45f{inset:0;position:absolute;background:var(--overlay-bg, rgba(0, 0, 0, .6));-webkit-backdrop-filter:var(--overlay-filter);backdrop-filter:var(--overlay-filter);border-radius:var(--overlay-radius, 0);z-index:var(--overlay-z-index)}.m_9814e45f:where([data-fixed]){position:fixed}.m_9814e45f:where([data-center]){display:flex;align-items:center;justify-content:center}.m_38a85659{position:absolute;border:1px solid var(--popover-border-color);padding:var(--mantine-spacing-sm) var(--mantine-spacing-md);box-shadow:var(--popover-shadow, none);border-radius:var(--popover-radius, var(--mantine-radius-default))}.m_38a85659:where([data-fixed]){position:fixed}.m_38a85659:focus{outline:none}:where([data-mantine-color-scheme=light]) .m_38a85659{--popover-border-color: var(--mantine-color-gray-2);background-color:var(--mantine-color-white)}:where([data-mantine-color-scheme=dark]) .m_38a85659{--popover-border-color: var(--mantine-color-dark-4);background-color:var(--mantine-color-dark-6)}.m_a31dc6c1{background-color:inherit;border:1px solid var(--popover-border-color);z-index:1}.m_3d7bc908{position:fixed;inset:0}.m_5ae2e3c{--loader-size-xs: calc(1.125rem * var(--mantine-scale));--loader-size-sm: calc(1.375rem * var(--mantine-scale));--loader-size-md: calc(2.25rem * var(--mantine-scale));--loader-size-lg: calc(2.75rem * var(--mantine-scale));--loader-size-xl: calc(3.625rem * var(--mantine-scale));--loader-size: var(--loader-size-md);--loader-color: var(--mantine-primary-color-filled)}@keyframes m_5d2b3b9d{0%{transform:scale(.6);opacity:0}50%,to{transform:scale(1)}}.m_7a2bd4cd{position:relative;width:var(--loader-size);height:var(--loader-size);display:flex;gap:calc(var(--loader-size) / 5)}.m_870bb79{flex:1;background:var(--loader-color);animation:m_5d2b3b9d 1.2s cubic-bezier(0,.5,.5,1) infinite;border-radius:calc(.125rem * var(--mantine-scale))}.m_870bb79:nth-of-type(1){animation-delay:-.24s}.m_870bb79:nth-of-type(2){animation-delay:-.12s}.m_870bb79:nth-of-type(3){animation-delay:0}@keyframes m_aac34a1{0%,to{transform:scale(1);opacity:1}50%{transform:scale(.6);opacity:.5}}.m_4e3f22d7{display:flex;justify-content:center;align-items:center;gap:calc(var(--loader-size) / 10);position:relative;width:var(--loader-size);height:var(--loader-size)}.m_870c4af{width:calc(var(--loader-size) / 3 - var(--loader-size) / 15);height:calc(var(--loader-size) / 3 - var(--loader-size) / 15);border-radius:50%;background:var(--loader-color);animation:m_aac34a1 .8s infinite linear}.m_870c4af:nth-child(2){animation-delay:.4s}@keyframes m_f8e89c4b{0%{transform:rotate(0)}to{transform:rotate(360deg)}}.m_b34414df{display:inline-block;width:var(--loader-size);height:var(--loader-size)}.m_b34414df:after{content:"";display:block;width:var(--loader-size);height:var(--loader-size);border-radius:calc(625rem * var(--mantine-scale));border-width:calc(var(--loader-size) / 8);border-style:solid;border-color:var(--loader-color) var(--loader-color) var(--loader-color) transparent;animation:m_f8e89c4b 1.2s linear infinite}.m_8d3f4000{--ai-size-xs: calc(1.125rem * var(--mantine-scale));--ai-size-sm: calc(1.375rem * var(--mantine-scale));--ai-size-md: calc(1.75rem * var(--mantine-scale));--ai-size-lg: calc(2.125rem * var(--mantine-scale));--ai-size-xl: calc(2.75rem * var(--mantine-scale));--ai-size-input-xs: calc(1.875rem * var(--mantine-scale));--ai-size-input-sm: calc(2.25rem * var(--mantine-scale));--ai-size-input-md: calc(2.625rem * var(--mantine-scale));--ai-size-input-lg: calc(3.125rem * var(--mantine-scale));--ai-size-input-xl: calc(3.75rem * var(--mantine-scale));--ai-size: var(--ai-size-md);--ai-color: var(--mantine-color-white);line-height:1;display:inline-flex;align-items:center;justify-content:center;position:relative;-webkit-user-select:none;user-select:none;overflow:hidden;width:var(--ai-size);height:var(--ai-size);min-width:var(--ai-size);min-height:var(--ai-size);border-radius:var(--ai-radius, var(--mantine-radius-default));background:var(--ai-bg, var(--mantine-primary-color-filled));color:var(--ai-color, var(--mantine-color-white));border:var(--ai-bd, calc(.0625rem * var(--mantine-scale)) solid transparent);cursor:pointer}@media (hover: hover){.m_8d3f4000:hover:where(:not([data-loading],:disabled,[data-disabled])){background-color:var(--ai-hover, var(--mantine-primary-color-filled-hover));color:var(--ai-hover-color, var(--ai-color))}}@media (hover: none){.m_8d3f4000:active:where(:not([data-loading],:disabled,[data-disabled])){background-color:var(--ai-hover, var(--mantine-primary-color-filled-hover));color:var(--ai-hover-color, var(--ai-color))}}.m_8d3f4000[data-loading]{cursor:not-allowed}.m_8d3f4000[data-loading] .m_8d3afb97{opacity:0;transform:translateY(100%)}.m_8d3f4000:where(:disabled:not([data-loading]),[data-disabled]:not([data-loading])){cursor:not-allowed;border:calc(.0625rem * var(--mantine-scale)) solid transparent;color:var(--mantine-color-disabled-color);background-color:var(--mantine-color-disabled)}.m_8d3f4000:where(:disabled:not([data-loading]),[data-disabled]:not([data-loading])):active{transform:none}.m_302b9fb1{inset:calc(-.0625rem * var(--mantine-scale));position:absolute;border-radius:var(--ai-radius, var(--mantine-radius-default));display:flex;align-items:center;justify-content:center}:where([data-mantine-color-scheme=light]) .m_302b9fb1{background-color:#ffffff26}:where([data-mantine-color-scheme=dark]) .m_302b9fb1{background-color:#00000026}.m_1a0f1b21{--ai-border-width: calc(.0625rem * var(--mantine-scale));display:flex}.m_1a0f1b21 :where(*):focus{position:relative;z-index:1}.m_1a0f1b21[data-orientation=horizontal]{flex-direction:row}.m_1a0f1b21[data-orientation=horizontal] .m_8d3f4000:not(:only-child):first-child,.m_1a0f1b21[data-orientation=horizontal] .m_437b6484:not(:only-child):first-child{border-end-end-radius:0;border-start-end-radius:0;border-inline-end-width:calc(var(--ai-border-width) / 2)}.m_1a0f1b21[data-orientation=horizontal] .m_8d3f4000:not(:only-child):last-child,.m_1a0f1b21[data-orientation=horizontal] .m_437b6484:not(:only-child):last-child{border-end-start-radius:0;border-start-start-radius:0;border-inline-start-width:calc(var(--ai-border-width) / 2)}.m_1a0f1b21[data-orientation=horizontal] .m_8d3f4000:not(:only-child):not(:first-child):not(:last-child),.m_1a0f1b21[data-orientation=horizontal] .m_437b6484:not(:only-child):not(:first-child):not(:last-child){border-radius:0;border-inline-width:calc(var(--ai-border-width) / 2)}.m_1a0f1b21[data-orientation=vertical]{flex-direction:column}.m_1a0f1b21[data-orientation=vertical] .m_8d3f4000:not(:only-child):first-child,.m_1a0f1b21[data-orientation=vertical] .m_437b6484:not(:only-child):first-child{border-end-start-radius:0;border-end-end-radius:0;border-bottom-width:calc(var(--ai-border-width) / 2)}.m_1a0f1b21[data-orientation=vertical] .m_8d3f4000:not(:only-child):last-child,.m_1a0f1b21[data-orientation=vertical] .m_437b6484:not(:only-child):last-child{border-start-start-radius:0;border-start-end-radius:0;border-top-width:calc(var(--ai-border-width) / 2)}.m_1a0f1b21[data-orientation=vertical] .m_8d3f4000:not(:only-child):not(:first-child):not(:last-child),.m_1a0f1b21[data-orientation=vertical] .m_437b6484:not(:only-child):not(:first-child):not(:last-child){border-radius:0;border-bottom-width:calc(var(--ai-border-width) / 2);border-top-width:calc(var(--ai-border-width) / 2)}.m_8d3afb97{display:flex;align-items:center;justify-content:center;transition:transform .15s ease,opacity .1s ease;width:100%;height:100%}.m_437b6484{--section-height-xs: calc(1.125rem * var(--mantine-scale));--section-height-sm: calc(1.375rem * var(--mantine-scale));--section-height-md: calc(1.75rem * var(--mantine-scale));--section-height-lg: calc(2.125rem * var(--mantine-scale));--section-height-xl: calc(2.75rem * var(--mantine-scale));--section-height-input-xs: calc(1.875rem * var(--mantine-scale));--section-height-input-sm: calc(2.25rem * var(--mantine-scale));--section-height-input-md: calc(2.625rem * var(--mantine-scale));--section-height-input-lg: calc(3.125rem * var(--mantine-scale));--section-height-input-xl: calc(3.75rem * var(--mantine-scale));--section-padding-x-xs: calc(.375rem * var(--mantine-scale));--section-padding-x-sm: calc(.5rem * var(--mantine-scale));--section-padding-x-md: calc(.625rem * var(--mantine-scale));--section-padding-x-lg: calc(.75rem * var(--mantine-scale));--section-padding-x-xl: calc(1rem * var(--mantine-scale));--section-height: var(--section-height-sm);--section-padding-x: var(--section-padding-x-sm);--section-color: var(--mantine-color-white);font-weight:600;width:auto;border-radius:var(--section-radius, var(--mantine-radius-default));font-size:var(--section-fz, var(--mantine-font-size-sm));background:var(--section-bg, var(--mantine-primary-color-filled));border:var(--section-bd, calc(.0625rem * var(--mantine-scale)) solid transparent);color:var(--section-color, var(--mantine-color-white));height:var(--section-height, var(--section-height-sm));padding-inline:var(--section-padding-x, var(--section-padding-x-sm));vertical-align:middle;line-height:1;display:inline-flex;align-items:center;justify-content:center}.m_86a44da5{--cb-size-xs: calc(1.125rem * var(--mantine-scale));--cb-size-sm: calc(1.375rem * var(--mantine-scale));--cb-size-md: calc(1.75rem * var(--mantine-scale));--cb-size-lg: calc(2.125rem * var(--mantine-scale));--cb-size-xl: calc(2.75rem * var(--mantine-scale));--cb-size: var(--cb-size-md);--cb-icon-size: 70%;--cb-radius: var(--mantine-radius-default);line-height:1;display:inline-flex;align-items:center;justify-content:center;position:relative;-webkit-user-select:none;user-select:none;width:var(--cb-size);height:var(--cb-size);min-width:var(--cb-size);min-height:var(--cb-size);border-radius:var(--cb-radius)}:where([data-mantine-color-scheme=light]) .m_86a44da5{color:var(--mantine-color-gray-7)}:where([data-mantine-color-scheme=dark]) .m_86a44da5{color:var(--mantine-color-dark-1)}.m_86a44da5[data-disabled],.m_86a44da5:disabled{cursor:not-allowed;opacity:.6}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_220c80f2:where(:not([data-disabled],:disabled)):hover{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_220c80f2:where(:not([data-disabled],:disabled)):hover{background-color:var(--mantine-color-dark-6)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_220c80f2:where(:not([data-disabled],:disabled)):active{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_220c80f2:where(:not([data-disabled],:disabled)):active{background-color:var(--mantine-color-dark-6)}}.m_4081bf90{display:flex;flex-direction:row;flex-wrap:var(--group-wrap, wrap);justify-content:var(--group-justify, flex-start);align-items:var(--group-align, center);gap:var(--group-gap, var(--mantine-spacing-md))}.m_4081bf90:where([data-grow])>*{flex-grow:1;max-width:var(--group-child-width)}.m_615af6c9{line-height:1;padding:0;margin:0;font-weight:400;font-size:var(--mantine-font-size-md)}.m_b5489c3c{display:flex;justify-content:space-between;align-items:center;padding:var(--mb-padding, var(--mantine-spacing-md));padding-inline-end:calc(var(--mb-padding, var(--mantine-spacing-md)) - calc(.3125rem * var(--mantine-scale)));position:sticky;top:0;background-color:var(--mantine-color-body);z-index:1000;min-height:calc(3.75rem * var(--mantine-scale));transition:padding-inline-end .1s}.m_60c222c7{position:fixed;width:100%;top:0;bottom:0;z-index:var(--mb-z-index);pointer-events:none}.m_fd1ab0aa{pointer-events:all;box-shadow:var(--mb-shadow, var(--mantine-shadow-xl))}.m_fd1ab0aa [data-mantine-scrollbar]{z-index:1001}[data-offset-scrollbars] .m_fd1ab0aa:has([data-mantine-scrollbar]) .m_b5489c3c{padding-inline-end:calc(var(--mb-padding, var(--mantine-spacing-md)) + calc(.3125rem * var(--mantine-scale)))}.m_606cb269{margin-inline-start:auto}.m_5df29311{padding:var(--mb-padding, var(--mantine-spacing-md));padding-top:var(--mb-padding, var(--mantine-spacing-md))}.m_5df29311:where(:not(:only-child)){padding-top:0}.m_6c018570{position:relative;margin-top:var(--input-margin-top, 0rem);margin-bottom:var(--input-margin-bottom, 0rem);--input-height-xs: calc(1.875rem * var(--mantine-scale));--input-height-sm: calc(2.25rem * var(--mantine-scale));--input-height-md: calc(2.625rem * var(--mantine-scale));--input-height-lg: calc(3.125rem * var(--mantine-scale));--input-height-xl: calc(3.75rem * var(--mantine-scale));--input-padding-y-xs: calc(.3125rem * var(--mantine-scale));--input-padding-y-sm: calc(.375rem * var(--mantine-scale));--input-padding-y-md: calc(.5rem * var(--mantine-scale));--input-padding-y-lg: calc(.625rem * var(--mantine-scale));--input-padding-y-xl: calc(.8125rem * var(--mantine-scale));--input-height: var(--input-height-sm);--input-radius: var(--mantine-radius-default);--input-cursor: text;--input-text-align: left;--input-line-height: calc(var(--input-height) - calc(.125rem * var(--mantine-scale)));--input-padding: calc(var(--input-height) / 3);--input-padding-inline-start: var(--input-padding);--input-padding-inline-end: var(--input-padding);--input-placeholder-color: var(--mantine-color-placeholder);--input-color: var(--mantine-color-text);--input-disabled-bg: var(--mantine-color-disabled);--input-disabled-color: var(--mantine-color-disabled-color);--input-left-section-size: var(--input-left-section-width, calc(var(--input-height) - calc(.125rem * var(--mantine-scale))));--input-right-section-size: var( --input-right-section-width, calc(var(--input-height) - calc(.125rem * var(--mantine-scale))) );--input-size: var(--input-height);--section-y: calc(.0625rem * var(--mantine-scale));--left-section-start: calc(.0625rem * var(--mantine-scale));--left-section-border-radius: var(--input-radius) 0 0 var(--input-radius);--right-section-end: calc(.0625rem * var(--mantine-scale));--right-section-border-radius: 0 var(--input-radius) var(--input-radius) 0}.m_6c018570[data-variant=unstyled]{--input-padding: 0;--input-padding-y: 0;--input-padding-inline-start: 0;--input-padding-inline-end: 0}.m_6c018570[data-pointer]{--input-cursor: pointer}.m_6c018570[data-multiline]{--input-padding-y-xs: calc(.28125rem * var(--mantine-scale));--input-padding-y-sm: calc(.34375rem * var(--mantine-scale));--input-padding-y-md: calc(.4375rem * var(--mantine-scale));--input-padding-y-lg: calc(.59375rem * var(--mantine-scale));--input-padding-y-xl: calc(.8125rem * var(--mantine-scale));--input-size: auto;--input-line-height: var(--mantine-line-height)}.m_6c018570[data-with-left-section]{--input-padding-inline-start: var(--input-left-section-size)}.m_6c018570[data-with-right-section]{--input-padding-inline-end: var(--input-right-section-size)}.m_6c018570[data-size=xs] .m_6c018570[data-with-right-section]:has([data-combined-clear-section]){--input-padding-inline-end: calc(2.5625rem * var(--mantine-scale))}.m_6c018570[data-size=sm] .m_6c018570[data-with-right-section]:has([data-combined-clear-section]){--input-padding-inline-end: calc(3.125rem * var(--mantine-scale))}.m_6c018570[data-size=md] .m_6c018570[data-with-right-section]:has([data-combined-clear-section]){--input-padding-inline-end: calc(3.75rem * var(--mantine-scale))}.m_6c018570[data-size=lg] .m_6c018570[data-with-right-section]:has([data-combined-clear-section]){--input-padding-inline-end: calc(4.5rem * var(--mantine-scale))}.m_6c018570[data-size=xl] .m_6c018570[data-with-right-section]:has([data-combined-clear-section]){--input-padding-inline-end: calc(5.5625rem * var(--mantine-scale))}[data-mantine-color-scheme=light] .m_6c018570[data-variant=default]{--input-bd: var(--mantine-color-gray-4);--input-bg: var(--mantine-color-white);--input-bd-focus: var(--mantine-primary-color-filled)}[data-mantine-color-scheme=light] .m_6c018570[data-variant=filled]{--input-bd: transparent;--input-bg: var(--mantine-color-gray-1);--input-bd-focus: var(--mantine-primary-color-filled)}[data-mantine-color-scheme=light] .m_6c018570[data-variant=unstyled]{--input-bd: transparent;--input-bg: transparent;--input-bd-focus: transparent}[data-mantine-color-scheme=dark] .m_6c018570[data-variant=default]{--input-bd: var(--mantine-color-dark-4);--input-bg: var(--mantine-color-dark-6);--input-bd-focus: var(--mantine-primary-color-filled)}[data-mantine-color-scheme=dark] .m_6c018570[data-variant=filled]{--input-bd: transparent;--input-bg: var(--mantine-color-dark-5);--input-bd-focus: var(--mantine-primary-color-filled)}[data-mantine-color-scheme=dark] .m_6c018570[data-variant=unstyled]{--input-bd: transparent;--input-bg: transparent;--input-bd-focus: transparent}[data-mantine-color-scheme] .m_6c018570[data-error]:not([data-variant=unstyled]){--input-bd: var(--mantine-color-error)}[data-mantine-color-scheme] .m_6c018570[data-error]{--input-color: var(--mantine-color-error);--input-placeholder-color: var(--mantine-color-error);--input-section-color: var(--mantine-color-error)}:where([dir=rtl]) .m_6c018570{--input-text-align: right;--left-section-border-radius: 0 var(--input-radius) var(--input-radius) 0;--right-section-border-radius: var(--input-radius) 0 0 var(--input-radius)}.m_8fb7ebe7{-webkit-tap-highlight-color:transparent;appearance:none;resize:var(--input-resize, none);display:block;width:100%;transition:border-color .1s ease;text-align:var(--input-text-align);color:var(--input-color);border:calc(.0625rem * var(--mantine-scale)) solid var(--input-bd);background-color:var(--input-bg);font-family:var(--input-font-family, var(--mantine-font-family));height:var(--input-size);min-height:var(--input-height);line-height:var(--input-line-height);font-size:var(--_input-fz, var(--input-fz, var(--mantine-font-size-md)));border-radius:var(--input-radius);padding-inline-start:var(--input-padding-inline-start);padding-inline-end:var(--input-padding-inline-end);padding-top:var(--input-padding-y, 0rem);padding-bottom:var(--input-padding-y, 0rem);cursor:var(--input-cursor);overflow:var(--input-overflow)}.m_8fb7ebe7[data-no-overflow]{--input-overflow: hidden}.m_8fb7ebe7[data-monospace]{--input-font-family: var(--mantine-font-family-monospace);--_input-fz: calc(var(--input-fz) - calc(.125rem * var(--mantine-scale)))}.m_8fb7ebe7:focus,.m_8fb7ebe7:focus-within{outline:none;--input-bd: var(--input-bd-focus)}[data-error] .m_8fb7ebe7:focus,[data-error] .m_8fb7ebe7:focus-within{--input-bd: var(--mantine-color-error)}.m_8fb7ebe7::placeholder{color:var(--input-placeholder-color);opacity:1}.m_8fb7ebe7::-webkit-inner-spin-button,.m_8fb7ebe7::-webkit-outer-spin-button,.m_8fb7ebe7::-webkit-search-decoration,.m_8fb7ebe7::-webkit-search-cancel-button,.m_8fb7ebe7::-webkit-search-results-button,.m_8fb7ebe7::-webkit-search-results-decoration{appearance:none}.m_8fb7ebe7[type=number]{-moz-appearance:textfield}.m_8fb7ebe7:disabled,.m_8fb7ebe7[data-disabled]{cursor:not-allowed;opacity:.6;background-color:var(--input-disabled-bg);color:var(--input-disabled-color)}.m_8fb7ebe7:has(input:disabled){cursor:not-allowed;opacity:.6;background-color:var(--input-disabled-bg);color:var(--input-disabled-color)}.m_8fb7ebe7[readonly]{caret-color:transparent}.m_82577fc2{pointer-events:var(--section-pointer-events);position:absolute;z-index:1;inset-inline-start:var(--section-start);inset-inline-end:var(--section-end);bottom:var(--section-y);top:var(--section-y);display:flex;align-items:center;justify-content:center;width:var(--section-size);border-radius:var(--section-border-radius);color:var(--input-section-color, var(--mantine-color-dimmed))}.m_82577fc2[data-position=right]{--section-pointer-events: var(--input-right-section-pointer-events);--section-end: var(--right-section-end);--section-size: var(--input-right-section-size);--section-border-radius: var(--right-section-border-radius)}.m_6c018570[data-size=xs] .m_82577fc2[data-position=right]:has([data-combined-clear-section]){--section-size: calc(2.5625rem * var(--mantine-scale))}.m_6c018570[data-size=sm] .m_82577fc2[data-position=right]:has([data-combined-clear-section]){--section-size: calc(3.125rem * var(--mantine-scale))}.m_6c018570[data-size=md] .m_82577fc2[data-position=right]:has([data-combined-clear-section]){--section-size: calc(3.75rem * var(--mantine-scale))}.m_6c018570[data-size=lg] .m_82577fc2[data-position=right]:has([data-combined-clear-section]){--section-size: calc(4.5rem * var(--mantine-scale))}.m_6c018570[data-size=xl] .m_82577fc2[data-position=right]:has([data-combined-clear-section]){--section-size: calc(5.5625rem * var(--mantine-scale))}.m_82577fc2[data-position=left]{--section-pointer-events: var(--input-left-section-pointer-events);--section-start: var(--left-section-start);--section-size: var(--input-left-section-size);--section-border-radius: var(--left-section-border-radius)}.m_88bacfd0{color:var(--input-placeholder-color, var(--mantine-color-placeholder))}[data-error] .m_88bacfd0{--input-placeholder-color: var(--input-color, var(--mantine-color-placeholder))}.m_46b77525{line-height:var(--mantine-line-height)}.m_8fdc1311{display:inline-block;font-weight:500;overflow-wrap:break-word;cursor:default;-webkit-tap-highlight-color:transparent;font-size:var(--input-label-size, var(--mantine-font-size-sm))}.m_78a94662{color:var(--input-asterisk-color, var(--mantine-color-error))}.m_8f816625,.m_fe47ce59{word-wrap:break-word;line-height:1.2;display:block;margin:0;padding:0}.m_8f816625{color:var(--mantine-color-error);font-size:var(--input-error-size, calc(var(--mantine-font-size-sm) - calc(.125rem * var(--mantine-scale))))}.m_fe47ce59{color:var(--mantine-color-dimmed);font-size:var(--input-description-size, calc(var(--mantine-font-size-sm) - calc(.125rem * var(--mantine-scale))))}.m_8bffd616{display:flex}.m_96b553a6{--transition-duration: .15s;top:0;left:0;position:absolute;z-index:0;transition-property:transform,width,height;transition-timing-function:ease;transition-duration:0ms}.m_96b553a6:where([data-initialized]){transition-duration:var(--transition-duration)}.m_96b553a6:where([data-hidden]){background-color:red;display:none}.m_9bdbb667{--accordion-radius: var(--mantine-radius-default)}.m_df78851f{overflow-wrap:break-word}.m_4ba554d4{padding:var(--mantine-spacing-md);padding-top:calc(var(--mantine-spacing-xs) / 2)}.m_8fa820a0{margin:0;padding:0}.m_4ba585b8{width:100%;display:flex;align-items:center;flex-direction:row-reverse;padding-inline:var(--mantine-spacing-md);opacity:1;cursor:pointer;background-color:transparent;color:var(--mantine-color-bright)}.m_4ba585b8:where([data-chevron-position=left]){flex-direction:row;padding-inline-start:0}.m_4ba585b8:where(:disabled,[data-disabled]){opacity:.4;cursor:not-allowed}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_6939a5e9:where(:not(:disabled,[data-disabled])):hover,:where([data-mantine-color-scheme=light]) .m_4271d21b:where(:not(:disabled,[data-disabled])):hover{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_6939a5e9:where(:not(:disabled,[data-disabled])):hover,:where([data-mantine-color-scheme=dark]) .m_4271d21b:where(:not(:disabled,[data-disabled])):hover{background-color:var(--mantine-color-dark-6)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_6939a5e9:where(:not(:disabled,[data-disabled])):active,:where([data-mantine-color-scheme=light]) .m_4271d21b:where(:not(:disabled,[data-disabled])):active{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_6939a5e9:where(:not(:disabled,[data-disabled])):active,:where([data-mantine-color-scheme=dark]) .m_4271d21b:where(:not(:disabled,[data-disabled])):active{background-color:var(--mantine-color-dark-6)}}.m_df3ffa0f{color:inherit;font-weight:400;flex:1;overflow:hidden;text-overflow:ellipsis;padding-top:var(--mantine-spacing-sm);padding-bottom:var(--mantine-spacing-sm)}.m_3f35ae96{display:flex;align-items:center;justify-content:flex-start;transition:transform var(--accordion-transition-duration, .2s) ease;width:var(--accordion-chevron-size, calc(.9375rem * var(--mantine-scale)));min-width:var(--accordion-chevron-size, calc(.9375rem * var(--mantine-scale)));transform:rotate(0)}.m_3f35ae96:where([data-rotate]){transform:rotate(180deg)}.m_3f35ae96:where([data-position=left]){margin-inline-end:var(--mantine-spacing-md);margin-inline-start:var(--mantine-spacing-md)}.m_9bd771fe{display:flex;align-items:center;justify-content:center;margin-inline-end:var(--mantine-spacing-sm)}.m_9bd771fe:where([data-chevron-position=left]){margin-inline-end:0;margin-inline-start:var(--mantine-spacing-lg)}:where([data-mantine-color-scheme=light]) .m_9bd7b098{--item-border-color: var(--mantine-color-gray-3);--item-filled-color: var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_9bd7b098{--item-border-color: var(--mantine-color-dark-4);--item-filled-color: var(--mantine-color-dark-6)}.m_fe19b709{border-bottom:1px solid var(--item-border-color)}.m_1f921b3b{border:1px solid var(--item-border-color);transition:background-color .15s ease}.m_1f921b3b:where([data-active]){background-color:var(--item-filled-color)}.m_1f921b3b:first-of-type{border-start-start-radius:var(--accordion-radius);border-start-end-radius:var(--accordion-radius)}.m_1f921b3b:first-of-type>[data-accordion-control]{border-start-start-radius:var(--accordion-radius);border-start-end-radius:var(--accordion-radius)}.m_1f921b3b:last-of-type{border-end-start-radius:var(--accordion-radius);border-end-end-radius:var(--accordion-radius)}.m_1f921b3b:last-of-type>[data-accordion-control]{border-end-start-radius:var(--accordion-radius);border-end-end-radius:var(--accordion-radius)}.m_1f921b3b+.m_1f921b3b{border-top:0}.m_2cdf939a{border-radius:var(--accordion-radius)}.m_2cdf939a:where([data-active]){background-color:var(--item-filled-color)}.m_9f59b069{background-color:var(--item-filled-color);border-radius:var(--accordion-radius);border:calc(.0625rem * var(--mantine-scale)) solid transparent;transition:background-color .15s ease}.m_9f59b069[data-active]{border-color:var(--item-border-color)}:where([data-mantine-color-scheme=light]) .m_9f59b069[data-active]{background-color:var(--mantine-color-white)}:where([data-mantine-color-scheme=dark]) .m_9f59b069[data-active]{background-color:var(--mantine-color-dark-7)}.m_9f59b069+.m_9f59b069{margin-top:var(--mantine-spacing-md)}.m_7f854edf{position:fixed;z-index:var(--affix-z-index);inset-inline-start:var(--affix-left);inset-inline-end:var(--affix-right);top:var(--affix-top);bottom:var(--affix-bottom)}.m_66836ed3{--alert-radius: var(--mantine-radius-default);--alert-bg: var(--mantine-primary-color-light);--alert-bd: calc(.0625rem * var(--mantine-scale)) solid transparent;--alert-color: var(--mantine-primary-color-light-color);padding:var(--mantine-spacing-md) var(--mantine-spacing-md);border-radius:var(--alert-radius);position:relative;overflow:hidden;background-color:var(--alert-bg);border:var(--alert-bd);color:var(--alert-color)}.m_a5d60502{display:flex}.m_667c2793{flex:1;display:flex;flex-direction:column;gap:var(--mantine-spacing-xs)}.m_6a03f287{display:flex;align-items:center;justify-content:space-between;font-size:var(--mantine-font-size-sm);font-weight:700}.m_6a03f287:where([data-with-close-button]){padding-inline-end:var(--mantine-spacing-md)}.m_698f4f23{display:block;overflow:hidden;text-overflow:ellipsis}.m_667f2a6a{line-height:1;width:calc(1.25rem * var(--mantine-scale));height:calc(1.25rem * var(--mantine-scale));display:flex;align-items:center;justify-content:flex-start;margin-inline-end:var(--mantine-spacing-md);margin-top:calc(.0625rem * var(--mantine-scale))}.m_7fa78076{text-overflow:ellipsis;overflow:hidden;font-size:var(--mantine-font-size-sm)}:where([data-mantine-color-scheme=light]) .m_7fa78076{color:var(--mantine-color-black)}:where([data-mantine-color-scheme=dark]) .m_7fa78076{color:var(--mantine-color-white)}.m_7fa78076:where([data-variant=filled]){color:var(--alert-color)}.m_7fa78076:where([data-variant=white]){color:var(--mantine-color-black)}.m_87f54839{width:calc(1.25rem * var(--mantine-scale));height:calc(1.25rem * var(--mantine-scale));color:var(--alert-color)}.m_b6d8b162{-webkit-tap-highlight-color:transparent;text-decoration:none;font-size:var(--text-fz, var(--mantine-font-size-md));line-height:var(--text-lh, var(--mantine-line-height-md));font-weight:400;margin:0;padding:0;color:var(--text-color)}.m_b6d8b162:where([data-truncate]){overflow:hidden;text-overflow:ellipsis;white-space:nowrap}.m_b6d8b162:where([data-truncate=start]){direction:rtl;text-align:right}:where([dir=rtl]) .m_b6d8b162:where([data-truncate=start]){direction:ltr;text-align:left}.m_b6d8b162:where([data-variant=gradient]){background-image:var(--text-gradient);background-clip:text;-webkit-background-clip:text;-webkit-text-fill-color:transparent}.m_b6d8b162:where([data-line-clamp]){overflow:hidden;text-overflow:ellipsis;display:-webkit-box;-webkit-line-clamp:var(--text-line-clamp);-webkit-box-orient:vertical}.m_b6d8b162:where([data-inherit]){line-height:inherit;font-weight:inherit;font-size:inherit}.m_b6d8b162:where([data-inline]){line-height:1}.m_849cf0da{color:var(--mantine-color-anchor);text-decoration:none;appearance:none;border:none;display:inline;padding:0;margin:0;background-color:transparent;cursor:pointer}@media (hover: hover){.m_849cf0da:where([data-underline=hover]):hover{text-decoration:underline}}@media (hover: none){.m_849cf0da:where([data-underline=hover]):active{text-decoration:underline}}.m_849cf0da:where([data-underline=not-hover]){text-decoration:underline}@media (hover: hover){.m_849cf0da:where([data-underline=not-hover]):hover{text-decoration:none}}@media (hover: none){.m_849cf0da:where([data-underline=not-hover]):active{text-decoration:none}}.m_849cf0da:where([data-underline=always]){text-decoration:underline}.m_849cf0da:where([data-variant=gradient]),.m_849cf0da:where([data-variant=gradient]):hover{text-decoration:none}.m_849cf0da:where([data-line-clamp]){display:-webkit-box}.m_48204f9b{width:var(--slider-size);height:var(--slider-size);position:relative;border-radius:100%;display:flex;align-items:center;justify-content:center;-webkit-user-select:none;user-select:none}.m_48204f9b:focus-within{outline:2px solid var(--mantine-primary-color-filled);outline-offset:calc(.125rem * var(--mantine-scale))}.m_48204f9b{--slider-size: calc(3.75rem * var(--mantine-scale));--thumb-size: calc(var(--slider-size) / 5)}:where([data-mantine-color-scheme=light]) .m_48204f9b{background-color:var(--mantine-color-gray-1)}:where([data-mantine-color-scheme=dark]) .m_48204f9b{background-color:var(--mantine-color-dark-5)}.m_bb9cdbad{position:absolute;inset:calc(.0625rem * var(--mantine-scale));border-radius:var(--slider-size);pointer-events:none}.m_481dd586{width:calc(.125rem * var(--mantine-scale));position:absolute;top:0;bottom:0;left:calc(50% - 1px);transform:rotate(var(--angle))}.m_481dd586:before{content:"";position:absolute;top:calc(var(--thumb-size) / 3);left:calc(.03125rem * var(--mantine-scale));width:calc(.0625rem * var(--mantine-scale));height:calc(var(--thumb-size) / 1.5);transform:translate(-50%,-50%)}:where([data-mantine-color-scheme=light]) .m_481dd586:before{background-color:var(--mantine-color-gray-4)}:where([data-mantine-color-scheme=dark]) .m_481dd586:before{background-color:var(--mantine-color-dark-3)}.m_481dd586[data-label]:after{min-width:calc(1.125rem * var(--mantine-scale));text-align:center;content:attr(data-label);position:absolute;top:calc(-1.5rem * var(--mantine-scale));left:calc(-.4375rem * var(--mantine-scale));transform:rotate(calc(360deg - var(--angle)));font-size:var(--mantine-font-size-xs)}.m_bc02ba3d{position:absolute;inset-block:0;inset-inline-start:calc(50% - 1.5px);inset-inline-end:0;height:100%;width:calc(.1875rem * var(--mantine-scale));outline:none;pointer-events:none}.m_bc02ba3d:before{content:"";position:absolute;right:0;top:0;height:min(var(--thumb-size),calc(var(--slider-size) / 2));width:calc(.1875rem * var(--mantine-scale))}:where([data-mantine-color-scheme=light]) .m_bc02ba3d:before{background-color:var(--mantine-color-gray-7)}:where([data-mantine-color-scheme=dark]) .m_bc02ba3d:before{background-color:var(--mantine-color-dark-1)}.m_bb8e875b{font-size:var(--mantine-font-size-xs)}.m_89ab340[data-resizing]{--app-shell-transition-duration: 0ms !important}.m_89ab340[data-disabled]{--app-shell-header-offset: 0rem !important;--app-shell-navbar-offset: 0rem !important;--app-shell-aside-offset: 0rem !important;--app-shell-footer-offset: 0rem !important}[data-mantine-color-scheme=light] .m_89ab340{--app-shell-border-color: var(--mantine-color-gray-3)}[data-mantine-color-scheme=dark] .m_89ab340{--app-shell-border-color: var(--mantine-color-dark-4)}.m_45252eee,.m_9cdde9a,.m_3b16f56b,.m_8983817,.m_3840c879{transition-duration:var(--app-shell-transition-duration);transition-timing-function:var(--app-shell-transition-timing-function)}.m_45252eee,.m_9cdde9a{position:fixed;display:flex;flex-direction:column;top:var(--app-shell-header-offset, 0rem);height:calc(100dvh - var(--app-shell-header-offset, 0rem) - var(--app-shell-footer-offset, 0rem));background-color:var(--mantine-color-body);transition-property:transform,top,height}:where([data-layout=alt]) .m_45252eee,:where([data-layout=alt]) .m_9cdde9a{top:0rem;height:100dvh}.m_45252eee{inset-inline-start:0;width:var(--app-shell-navbar-width);transition-property:transform,top,height;transform:var(--app-shell-navbar-transform);z-index:var(--app-shell-navbar-z-index)}:where([dir=rtl]) .m_45252eee{transform:var(--app-shell-navbar-transform-rtl)}.m_45252eee:where([data-with-border]){border-inline-end:1px solid var(--app-shell-border-color)}.m_9cdde9a{inset-inline-end:0;width:var(--app-shell-aside-width);transform:var(--app-shell-aside-transform);z-index:var(--app-shell-aside-z-index)}:where([dir=rtl]) .m_9cdde9a{transform:var(--app-shell-aside-transform-rtl)}.m_9cdde9a:where([data-with-border]){border-inline-start:1px solid var(--app-shell-border-color)}.m_8983817{padding-inline-start:calc(var(--app-shell-navbar-offset, 0rem) + var(--app-shell-padding));padding-inline-end:calc(var(--app-shell-aside-offset, 0rem) + var(--app-shell-padding));padding-top:calc(var(--app-shell-header-offset, 0rem) + var(--app-shell-padding));padding-bottom:calc(var(--app-shell-footer-offset, 0rem) + var(--app-shell-padding));min-height:100dvh;transition-property:padding}.m_3b16f56b,.m_3840c879{position:fixed;inset-inline:0;transition-property:transform,left,right;background-color:var(--mantine-color-body)}:where([data-layout=alt]) .m_3b16f56b,:where([data-layout=alt]) .m_3840c879{inset-inline-start:var(--app-shell-navbar-offset, 0rem);inset-inline-end:var(--app-shell-aside-offset, 0rem)}.m_3b16f56b{top:0;height:var(--app-shell-header-height);background-color:var(--mantine-color-body);transform:var(--app-shell-header-transform);z-index:var(--app-shell-header-z-index)}.m_3b16f56b:where([data-with-border]){border-bottom:1px solid var(--app-shell-border-color)}.m_3840c879{bottom:0;height:calc(var(--app-shell-footer-height) + env(safe-area-inset-bottom));padding-bottom:env(safe-area-inset-bottom);transform:var(--app-shell-footer-transform);z-index:var(--app-shell-footer-z-index)}.m_3840c879:where([data-with-border]){border-top:1px solid var(--app-shell-border-color)}.m_6dcfc7c7{flex-grow:0}.m_6dcfc7c7:where([data-grow]){flex-grow:1}.m_71ac47fc{--ar-ratio: 1;max-width:100%}.m_71ac47fc>:where(*:not(style)){aspect-ratio:var(--ar-ratio);width:100%}.m_71ac47fc>:where(img,video){object-fit:cover}.m_88b62a41{--combobox-padding: calc(.25rem * var(--mantine-scale));padding:var(--combobox-padding)}.m_88b62a41:has([data-mantine-scrollbar]) .m_985517d8{max-width:calc(100% + var(--combobox-padding))}.m_88b62a41[data-composed]{padding-inline-end:0}.m_88b62a41[data-hidden]{display:none}.m_88b62a41,.m_b2821a6e{--combobox-option-padding-xs: calc(.25rem * var(--mantine-scale)) calc(.5rem * var(--mantine-scale));--combobox-option-padding-sm: calc(.375rem * var(--mantine-scale)) calc(.625rem * var(--mantine-scale));--combobox-option-padding-md: calc(.5rem * var(--mantine-scale)) calc(.75rem * var(--mantine-scale));--combobox-option-padding-lg: calc(.625rem * var(--mantine-scale)) calc(1rem * var(--mantine-scale));--combobox-option-padding-xl: calc(.875rem * var(--mantine-scale)) calc(1.25rem * var(--mantine-scale));--combobox-option-padding: var(--combobox-option-padding-sm)}.m_92253aa5{padding:var(--combobox-option-padding);font-size:var(--combobox-option-fz, var(--mantine-font-size-sm));border-radius:var(--mantine-radius-default);background-color:transparent;color:inherit;cursor:pointer;overflow-wrap:break-word}.m_92253aa5:where([data-combobox-selected]){background-color:var(--mantine-primary-color-filled);color:var(--mantine-color-white)}.m_92253aa5:where([data-combobox-disabled]){cursor:not-allowed;opacity:.35}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_92253aa5:hover:where(:not([data-combobox-selected],[data-combobox-disabled])){background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_92253aa5:hover:where(:not([data-combobox-selected],[data-combobox-disabled])){background-color:var(--mantine-color-dark-7)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_92253aa5:active:where(:not([data-combobox-selected],[data-combobox-disabled])){background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_92253aa5:active:where(:not([data-combobox-selected],[data-combobox-disabled])){background-color:var(--mantine-color-dark-7)}}.m_985517d8{margin-inline:calc(var(--combobox-padding) * -1);margin-top:calc(var(--combobox-padding) * -1);width:calc(100% + var(--combobox-padding) * 2);border-top-width:0;border-inline-width:0;border-end-start-radius:0;border-end-end-radius:0;margin-bottom:var(--combobox-padding);position:relative}:where([data-mantine-color-scheme=light]) .m_985517d8,:where([data-mantine-color-scheme=light]) .m_985517d8:focus{border-color:var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_985517d8,:where([data-mantine-color-scheme=dark]) .m_985517d8:focus{border-color:var(--mantine-color-dark-4)}:where([data-mantine-color-scheme=light]) .m_985517d8{background-color:var(--mantine-color-white)}:where([data-mantine-color-scheme=dark]) .m_985517d8{background-color:var(--mantine-color-dark-7)}.m_2530cd1d{font-size:var(--combobox-option-fz, var(--mantine-font-size-sm));text-align:center;padding:var(--combobox-option-padding);color:var(--mantine-color-dimmed)}.m_858f94bd,.m_82b967cb{font-size:var(--combobox-option-fz, var(--mantine-font-size-sm));border:0 solid transparent;margin-inline:calc(var(--combobox-padding) * -1);padding:var(--combobox-option-padding)}:where([data-mantine-color-scheme=light]) .m_858f94bd,:where([data-mantine-color-scheme=light]) .m_82b967cb{border-color:var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_858f94bd,:where([data-mantine-color-scheme=dark]) .m_82b967cb{border-color:var(--mantine-color-dark-4)}.m_82b967cb{border-top-width:calc(.0625rem * var(--mantine-scale));margin-top:var(--combobox-padding);margin-bottom:calc(var(--combobox-padding) * -1)}.m_858f94bd{border-bottom-width:calc(.0625rem * var(--mantine-scale));margin-bottom:var(--combobox-padding);margin-top:calc(var(--combobox-padding) * -1)}.m_254f3e4f:has(.m_2bb2e9e5:only-child){display:none}.m_2bb2e9e5{color:var(--mantine-color-dimmed);font-size:calc(var(--combobox-option-fz, var(--mantine-font-size-sm)) * .85);padding:var(--combobox-option-padding);font-weight:500;position:relative;display:flex;align-items:center}.m_2bb2e9e5:after{content:"";flex:1;inset-inline:0;height:calc(.0625rem * var(--mantine-scale));margin-inline-start:var(--mantine-spacing-xs)}:where([data-mantine-color-scheme=light]) .m_2bb2e9e5:after{background-color:var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_2bb2e9e5:after{background-color:var(--mantine-color-dark-4)}.m_2bb2e9e5:only-child{display:none}.m_2943220b{--combobox-chevron-size-xs: calc(.875rem * var(--mantine-scale));--combobox-chevron-size-sm: calc(1.125rem * var(--mantine-scale));--combobox-chevron-size-md: calc(1.25rem * var(--mantine-scale));--combobox-chevron-size-lg: calc(1.5rem * var(--mantine-scale));--combobox-chevron-size-xl: calc(1.75rem * var(--mantine-scale));--combobox-chevron-size: var(--combobox-chevron-size-sm)}:where([data-mantine-color-scheme=light]) .m_2943220b{--_combobox-chevron-color: var(--combobox-chevron-color, var(--mantine-color-gray-6))}:where([data-mantine-color-scheme=dark]) .m_2943220b{--_combobox-chevron-color: var(--combobox-chevron-color, var(--mantine-color-dark-3))}.m_2943220b{width:var(--combobox-chevron-size);height:var(--combobox-chevron-size);color:var(--_combobox-chevron-color)}.m_2943220b:where([data-error]){color:var(--combobox-chevron-color, var(--mantine-color-error))}.m_390b5f4{display:flex;align-items:center;gap:calc(.5rem * var(--mantine-scale))}.m_390b5f4:where([data-reverse]){justify-content:space-between}.m_8ee53fc2{opacity:.4;width:.8em;min-width:.8em;height:.8em}:where([data-combobox-selected]) .m_8ee53fc2{opacity:1}.m_5f75b09e{--label-lh-xs: calc(1rem * var(--mantine-scale));--label-lh-sm: calc(1.25rem * var(--mantine-scale));--label-lh-md: calc(1.5rem * var(--mantine-scale));--label-lh-lg: calc(1.875rem * var(--mantine-scale));--label-lh-xl: calc(2.25rem * var(--mantine-scale));--label-lh: var(--label-lh-sm)}.m_5f75b09e[data-label-position=left]{--label-order: 1;--label-offset-end: var(--mantine-spacing-sm);--label-offset-start: 0}.m_5f75b09e[data-label-position=right]{--label-order: 2;--label-offset-end: 0;--label-offset-start: var(--mantine-spacing-sm)}.m_5f6e695e{-webkit-tap-highlight-color:transparent;display:flex}.m_d3ea56bb{--label-cursor: var(--mantine-cursor-type);-webkit-tap-highlight-color:transparent;display:inline-flex;flex-direction:column;font-size:var(--label-fz, var(--mantine-font-size-sm));line-height:var(--label-lh);cursor:var(--label-cursor);order:var(--label-order)}fieldset:disabled .m_d3ea56bb,.m_d3ea56bb[data-disabled]{--label-cursor: not-allowed}.m_8ee546b8{cursor:var(--label-cursor);color:inherit;padding-inline-start:var(--label-offset-start);padding-inline-end:var(--label-offset-end)}fieldset:disabled .m_8ee546b8,.m_8ee546b8:where([data-disabled]){color:var(--mantine-color-disabled-color)}.m_328f68c0,.m_8e8a99cc{margin-top:calc(var(--mantine-spacing-xs) / 2);padding-inline-start:var(--label-offset-start);padding-inline-end:var(--label-offset-end)}.m_26775b0a{--card-radius: var(--mantine-radius-default);display:block;width:100%;border-radius:var(--card-radius);cursor:pointer}.m_26775b0a :where(*){cursor:inherit}.m_26775b0a:where([data-with-border]){border:calc(.0625rem * var(--mantine-scale)) solid transparent}:where([data-mantine-color-scheme=light]) .m_26775b0a:where([data-with-border]){border-color:var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_26775b0a:where([data-with-border]){border-color:var(--mantine-color-dark-4)}.m_5e5256ee{--checkbox-size-xs: calc(1rem * var(--mantine-scale));--checkbox-size-sm: calc(1.25rem * var(--mantine-scale));--checkbox-size-md: calc(1.5rem * var(--mantine-scale));--checkbox-size-lg: calc(1.875rem * var(--mantine-scale));--checkbox-size-xl: calc(2.25rem * var(--mantine-scale));--checkbox-size: var(--checkbox-size-sm);--checkbox-color: var(--mantine-primary-color-filled)}.m_5e5256ee:where([data-variant=filled]){--checkbox-icon-color: var(--mantine-color-white)}.m_5e5256ee:where([data-variant=outline]){--checkbox-icon-color: var(--checkbox-color)}.m_5e5256ee{position:relative;border:calc(.0625rem * var(--mantine-scale)) solid transparent;width:var(--checkbox-size);min-width:var(--checkbox-size);height:var(--checkbox-size);min-height:var(--checkbox-size);border-radius:var(--checkbox-radius, var(--mantine-radius-default));transition:border-color .1s ease,background-color .1s ease;cursor:var(--mantine-cursor-type);-webkit-tap-highlight-color:transparent;display:flex;align-items:center;justify-content:center}:where([data-mantine-color-scheme=light]) .m_5e5256ee{background-color:var(--mantine-color-white);border-color:var(--mantine-color-gray-4)}:where([data-mantine-color-scheme=dark]) .m_5e5256ee{background-color:var(--mantine-color-dark-6);border-color:var(--mantine-color-dark-4)}.m_5e5256ee[data-indeterminate],.m_5e5256ee[data-checked]{background-color:var(--checkbox-color);border-color:var(--checkbox-color)}.m_5e5256ee[data-indeterminate]>.m_1b1c543a,.m_5e5256ee[data-checked]>.m_1b1c543a{opacity:1;transform:none;color:var(--checkbox-icon-color)}.m_5e5256ee[data-disabled]{cursor:not-allowed;border-color:var(--mantine-color-disabled-border);background-color:var(--mantine-color-disabled)}[data-mantine-color-scheme=light] .m_5e5256ee[data-disabled][data-checked]>.m_1b1c543a{color:var(--mantine-color-gray-5)}[data-mantine-color-scheme=dark] .m_5e5256ee[data-disabled][data-checked]>.m_1b1c543a{color:var(--mantine-color-dark-3)}.m_76e20374[data-indeterminate]:not([data-disabled]),.m_76e20374[data-checked]:not([data-disabled]){background-color:transparent;border-color:var(--checkbox-color)}.m_76e20374[data-indeterminate]:not([data-disabled])>.m_1b1c543a,.m_76e20374[data-checked]:not([data-disabled])>.m_1b1c543a{color:var(--checkbox-icon-color);opacity:1;transform:none}.m_1b1c543a{display:block;width:60%;color:transparent;pointer-events:none;transform:translateY(calc(.3125rem * var(--mantine-scale))) scale(.5);opacity:1;transition:transform .1s ease,opacity .1s ease}.m_bf2d988c{--checkbox-size-xs: calc(1rem * var(--mantine-scale));--checkbox-size-sm: calc(1.25rem * var(--mantine-scale));--checkbox-size-md: calc(1.5rem * var(--mantine-scale));--checkbox-size-lg: calc(1.875rem * var(--mantine-scale));--checkbox-size-xl: calc(2.25rem * var(--mantine-scale));--checkbox-size: var(--checkbox-size-sm);--checkbox-color: var(--mantine-primary-color-filled)}.m_bf2d988c:where([data-variant=filled]){--checkbox-icon-color: var(--mantine-color-white)}.m_bf2d988c:where([data-variant=outline]){--checkbox-icon-color: var(--checkbox-color)}.m_26062bec{position:relative;width:var(--checkbox-size);height:var(--checkbox-size);order:1}.m_26062bec:where([data-label-position=left]){order:2}.m_26063560{appearance:none;border:calc(.0625rem * var(--mantine-scale)) solid transparent;width:var(--checkbox-size);height:var(--checkbox-size);border-radius:var(--checkbox-radius, var(--mantine-radius-default));padding:0;display:block;margin:0;transition:border-color .1s ease,background-color .1s ease;cursor:var(--mantine-cursor-type);-webkit-tap-highlight-color:transparent}:where([data-mantine-color-scheme=light]) .m_26063560{background-color:var(--mantine-color-white);border-color:var(--mantine-color-gray-4)}:where([data-mantine-color-scheme=dark]) .m_26063560{background-color:var(--mantine-color-dark-6);border-color:var(--mantine-color-dark-4)}.m_26063560:where([data-error]){border-color:var(--mantine-color-error)}.m_26063560[data-indeterminate],.m_26063560:checked{background-color:var(--checkbox-color);border-color:var(--checkbox-color)}.m_26063560[data-indeterminate]+.m_bf295423,.m_26063560:checked+.m_bf295423{opacity:1;transform:none}.m_26063560:disabled{cursor:not-allowed;border-color:var(--mantine-color-disabled-border);background-color:var(--mantine-color-disabled)}.m_26063560:disabled+.m_bf295423{color:var(--mantine-color-disabled-color)}.m_215c4542+.m_bf295423{color:var(--checkbox-color)}.m_215c4542[data-indeterminate]:not(:disabled),.m_215c4542:checked:not(:disabled){background-color:transparent;border-color:var(--checkbox-color)}.m_215c4542[data-indeterminate]:not(:disabled)+.m_bf295423,.m_215c4542:checked:not(:disabled)+.m_bf295423{color:var(--checkbox-icon-color);opacity:1;transform:none}.m_bf295423{position:absolute;inset:0;width:60%;margin:auto;color:var(--checkbox-icon-color);pointer-events:none;transform:translateY(calc(.3125rem * var(--mantine-scale))) scale(.5);opacity:0;transition:transform .1s ease,opacity .1s ease}.m_11def92b{--ag-spacing: var(--mantine-spacing-sm);--ag-offset: calc(var(--ag-spacing) * -1);display:flex;padding-inline-start:var(--ag-spacing)}.m_f85678b6{--avatar-size-xs: calc(1rem * var(--mantine-scale));--avatar-size-sm: calc(1.625rem * var(--mantine-scale));--avatar-size-md: calc(2.375rem * var(--mantine-scale));--avatar-size-lg: calc(3.5rem * var(--mantine-scale));--avatar-size-xl: calc(5.25rem * var(--mantine-scale));--avatar-size: var(--avatar-size-md);--avatar-radius: calc(62.5rem * var(--mantine-scale));--avatar-bg: var(--mantine-color-gray-light);--avatar-bd: calc(.0625rem * var(--mantine-scale)) solid transparent;--avatar-color: var(--mantine-color-gray-light-color);--avatar-placeholder-fz: calc(var(--avatar-size) / 2.5);-webkit-tap-highlight-color:transparent;position:relative;display:block;-webkit-user-select:none;user-select:none;overflow:hidden;border-radius:var(--avatar-radius);text-decoration:none;padding:0;width:var(--avatar-size);height:var(--avatar-size);min-width:var(--avatar-size)}.m_f85678b6:where([data-within-group]){margin-inline-start:var(--ag-offset);border:2px solid var(--mantine-color-body);background:var(--mantine-color-body)}.m_11f8ac07{object-fit:cover;width:100%;height:100%;display:block}.m_104cd71f{font-weight:700;display:flex;align-items:center;justify-content:center;width:100%;height:100%;-webkit-user-select:none;user-select:none;border-radius:var(--avatar-radius);font-size:var(--avatar-placeholder-fz);background:var(--avatar-bg);border:var(--avatar-bd);color:var(--avatar-color)}.m_104cd71f>[data-avatar-placeholder-icon]{width:70%;height:70%}.m_2ce0de02{background-size:cover;background-position:center;display:block;width:100%;border:0;text-decoration:none;border-radius:var(--bi-radius, 0)}.m_347db0ec{--badge-height-xs: calc(1rem * var(--mantine-scale));--badge-height-sm: calc(1.125rem * var(--mantine-scale));--badge-height-md: calc(1.25rem * var(--mantine-scale));--badge-height-lg: calc(1.625rem * var(--mantine-scale));--badge-height-xl: calc(2rem * var(--mantine-scale));--badge-fz-xs: calc(.5625rem * var(--mantine-scale));--badge-fz-sm: calc(.625rem * var(--mantine-scale));--badge-fz-md: calc(.6875rem * var(--mantine-scale));--badge-fz-lg: calc(.8125rem * var(--mantine-scale));--badge-fz-xl: calc(1rem * var(--mantine-scale));--badge-padding-x-xs: calc(.375rem * var(--mantine-scale));--badge-padding-x-sm: calc(.5rem * var(--mantine-scale));--badge-padding-x-md: calc(.625rem * var(--mantine-scale));--badge-padding-x-lg: calc(.75rem * var(--mantine-scale));--badge-padding-x-xl: calc(1rem * var(--mantine-scale));--badge-height: var(--badge-height-md);--badge-fz: var(--badge-fz-md);--badge-padding-x: var(--badge-padding-x-md);--badge-radius: calc(62.5rem * var(--mantine-scale));--badge-lh: calc(var(--badge-height) - calc(.125rem * var(--mantine-scale)));--badge-color: var(--mantine-color-white);--badge-bg: var(--mantine-primary-color-filled);--badge-border-width: calc(.0625rem * var(--mantine-scale));--badge-bd: var(--badge-border-width) solid transparent;-webkit-tap-highlight-color:transparent;font-size:var(--badge-fz);border-radius:var(--badge-radius);height:var(--badge-height);line-height:var(--badge-lh);text-decoration:none;padding:0 var(--badge-padding-x);display:inline-grid;align-items:center;justify-content:center;width:fit-content;text-transform:uppercase;font-weight:700;letter-spacing:calc(.015625rem * var(--mantine-scale));cursor:default;text-overflow:ellipsis;overflow:hidden;color:var(--badge-color);background:var(--badge-bg);border:var(--badge-bd)}.m_347db0ec:where([data-with-left-section],[data-variant=dot]){grid-template-columns:auto 1fr}.m_347db0ec:where([data-with-right-section]){grid-template-columns:1fr auto}.m_347db0ec:where([data-with-left-section][data-with-right-section],[data-variant=dot][data-with-right-section]){grid-template-columns:auto 1fr auto}.m_347db0ec:where([data-block]){display:flex;width:100%}.m_347db0ec:where([data-circle]){padding-inline:calc(.125rem * var(--mantine-scale));display:flex;width:var(--badge-height)}.m_fbd81e3d{--badge-dot-size: calc(var(--badge-height) / 3.4)}:where([data-mantine-color-scheme=light]) .m_fbd81e3d{background-color:var(--mantine-color-white);border-color:var(--mantine-color-gray-4);color:var(--mantine-color-black)}:where([data-mantine-color-scheme=dark]) .m_fbd81e3d{background-color:var(--mantine-color-dark-5);border-color:var(--mantine-color-dark-5);color:var(--mantine-color-white)}.m_fbd81e3d:before{content:"";display:block;width:var(--badge-dot-size);height:var(--badge-dot-size);border-radius:var(--badge-dot-size);background-color:var(--badge-dot-color);margin-inline-end:var(--badge-dot-size)}.m_5add502a{white-space:nowrap;overflow:hidden;text-overflow:ellipsis;text-align:center;cursor:inherit}.m_91fdda9b{--badge-section-margin: calc(var(--mantine-spacing-xs) / 2);display:inline-flex;justify-content:center;align-items:center;max-height:calc(var(--badge-height) - var(--badge-border-width) * 2)}.m_91fdda9b:where([data-position=left]){margin-inline-end:var(--badge-section-margin)}.m_91fdda9b:where([data-position=right]){margin-inline-start:var(--badge-section-margin)}.m_ddec01c0{--blockquote-border: 3px solid var(--bq-bd);position:relative;margin:0;border-inline-start:var(--blockquote-border);border-start-end-radius:var(--bq-radius);border-end-end-radius:var(--bq-radius);padding:var(--mantine-spacing-xl) calc(2.375rem * var(--mantine-scale))}:where([data-mantine-color-scheme=light]) .m_ddec01c0{background-color:var(--bq-bg-light)}:where([data-mantine-color-scheme=dark]) .m_ddec01c0{background-color:var(--bq-bg-dark)}.m_dde7bd57{--blockquote-icon-offset: calc(var(--bq-icon-size) / -2);position:absolute;color:var(--bq-bd);background-color:var(--mantine-color-body);display:flex;align-items:center;justify-content:center;top:var(--blockquote-icon-offset);inset-inline-start:var(--blockquote-icon-offset);width:var(--bq-icon-size);height:var(--bq-icon-size);border-radius:var(--bq-icon-size)}.m_dde51a35{display:block;margin-top:var(--mantine-spacing-md);opacity:.6;font-size:85%}.m_8b3717df{display:flex;align-items:center;flex-wrap:wrap}.m_f678d540{line-height:1;white-space:nowrap;-webkit-tap-highlight-color:transparent}.m_3b8f2208{margin-inline:var(--bc-separator-margin, var(--mantine-spacing-xs));line-height:1;display:flex;align-items:center;justify-content:center}:where([data-mantine-color-scheme=light]) .m_3b8f2208{color:var(--mantine-color-gray-7)}:where([data-mantine-color-scheme=dark]) .m_3b8f2208{color:var(--mantine-color-dark-2)}.m_fea6bf1a{--burger-size-xs: calc(.75rem * var(--mantine-scale));--burger-size-sm: calc(1.125rem * var(--mantine-scale));--burger-size-md: calc(1.5rem * var(--mantine-scale));--burger-size-lg: calc(2.125rem * var(--mantine-scale));--burger-size-xl: calc(2.625rem * var(--mantine-scale));--burger-size: var(--burger-size-md);--burger-line-size: calc(var(--burger-size) / 12);width:calc(var(--burger-size) + var(--mantine-spacing-xs));height:calc(var(--burger-size) + var(--mantine-spacing-xs));padding:calc(var(--mantine-spacing-xs) / 2);cursor:pointer}:where([data-mantine-color-scheme=light]) .m_fea6bf1a{--burger-color: var(--mantine-color-black)}:where([data-mantine-color-scheme=dark]) .m_fea6bf1a{--burger-color: var(--mantine-color-white)}.m_d4fb9cad{position:relative;-webkit-user-select:none;user-select:none}.m_d4fb9cad,.m_d4fb9cad:before,.m_d4fb9cad:after{display:block;width:var(--burger-size);height:var(--burger-line-size);background-color:var(--burger-color);outline:calc(.0625rem * var(--mantine-scale)) solid transparent;transition-property:background-color,transform;transition-duration:var(--burger-transition-duration, .3s);transition-timing-function:var(--burger-transition-timing-function, ease)}.m_d4fb9cad:before,.m_d4fb9cad:after{position:absolute;content:"";inset-inline-start:0}.m_d4fb9cad:before{top:calc(var(--burger-size) / -3)}.m_d4fb9cad:after{top:calc(var(--burger-size) / 3)}.m_d4fb9cad[data-opened]{background-color:transparent}.m_d4fb9cad[data-opened]:before{transform:translateY(calc(var(--burger-size) / 3)) rotate(45deg)}.m_d4fb9cad[data-opened]:after{transform:translateY(calc(var(--burger-size) / -3)) rotate(-45deg)}.m_77c9d27d{--button-height-xs: calc(1.875rem * var(--mantine-scale));--button-height-sm: calc(2.25rem * var(--mantine-scale));--button-height-md: calc(2.625rem * var(--mantine-scale));--button-height-lg: calc(3.125rem * var(--mantine-scale));--button-height-xl: calc(3.75rem * var(--mantine-scale));--button-height-compact-xs: calc(1.375rem * var(--mantine-scale));--button-height-compact-sm: calc(1.625rem * var(--mantine-scale));--button-height-compact-md: calc(1.875rem * var(--mantine-scale));--button-height-compact-lg: calc(2.125rem * var(--mantine-scale));--button-height-compact-xl: calc(2.5rem * var(--mantine-scale));--button-padding-x-xs: calc(.875rem * var(--mantine-scale));--button-padding-x-sm: calc(1.125rem * var(--mantine-scale));--button-padding-x-md: calc(1.375rem * var(--mantine-scale));--button-padding-x-lg: calc(1.625rem * var(--mantine-scale));--button-padding-x-xl: calc(2rem * var(--mantine-scale));--button-padding-x-compact-xs: calc(.4375rem * var(--mantine-scale));--button-padding-x-compact-sm: calc(.5rem * var(--mantine-scale));--button-padding-x-compact-md: calc(.625rem * var(--mantine-scale));--button-padding-x-compact-lg: calc(.75rem * var(--mantine-scale));--button-padding-x-compact-xl: calc(.875rem * var(--mantine-scale));--button-height: var(--button-height-sm);--button-padding-x: var(--button-padding-x-sm);--button-color: var(--mantine-color-white);-webkit-user-select:none;user-select:none;font-weight:600;position:relative;line-height:1;text-align:center;overflow:hidden;width:auto;cursor:pointer;display:inline-block;border-radius:var(--button-radius, var(--mantine-radius-default));font-size:var(--button-fz, var(--mantine-font-size-sm));background:var(--button-bg, var(--mantine-primary-color-filled));border:var(--button-bd, calc(.0625rem * var(--mantine-scale)) solid transparent);color:var(--button-color, var(--mantine-color-white));height:var(--button-height, var(--button-height-sm));padding-inline:var(--button-padding-x, var(--button-padding-x-sm));vertical-align:middle}.m_77c9d27d:where([data-block]){display:block;width:100%}.m_77c9d27d:where([data-with-left-section]){padding-inline-start:calc(var(--button-padding-x) / 1.5)}.m_77c9d27d:where([data-with-right-section]){padding-inline-end:calc(var(--button-padding-x) / 1.5)}.m_77c9d27d:where(:disabled:not([data-loading]),[data-disabled]:not([data-loading])){cursor:not-allowed;border:calc(.0625rem * var(--mantine-scale)) solid transparent;transform:none;color:var(--mantine-color-disabled-color);background:var(--mantine-color-disabled)}.m_77c9d27d:before{content:"";pointer-events:none;position:absolute;inset:calc(-.0625rem * var(--mantine-scale));border-radius:var(--button-radius, var(--mantine-radius-default));transform:translateY(-100%);opacity:0;filter:blur(12px);transition:transform .15s ease,opacity .1s ease}:where([data-mantine-color-scheme=light]) .m_77c9d27d:before{background-color:#ffffff26}:where([data-mantine-color-scheme=dark]) .m_77c9d27d:before{background-color:#00000026}.m_77c9d27d:where([data-loading]){cursor:not-allowed;transform:none}.m_77c9d27d:where([data-loading]):before{transform:translateY(0);opacity:1}.m_77c9d27d:where([data-loading]) .m_80f1301b{opacity:0;transform:translateY(100%)}@media (hover: hover){.m_77c9d27d:hover:where(:not([data-loading],:disabled,[data-disabled])){background-color:var(--button-hover, var(--mantine-primary-color-filled-hover));color:var(--button-hover-color, var(--button-color))}}@media (hover: none){.m_77c9d27d:active:where(:not([data-loading],:disabled,[data-disabled])){background-color:var(--button-hover, var(--mantine-primary-color-filled-hover));color:var(--button-hover-color, var(--button-color))}}.m_80f1301b{display:flex;align-items:center;justify-content:var(--button-justify, center);height:100%;overflow:visible;transition:transform .15s ease,opacity .1s ease}.m_811560b9{white-space:nowrap;height:100%;overflow:hidden;display:flex;align-items:center;opacity:1}.m_811560b9:where([data-loading]){opacity:.2}.m_a74036a{display:flex;align-items:center}.m_a74036a:where([data-position=left]){margin-inline-end:var(--mantine-spacing-xs)}.m_a74036a:where([data-position=right]){margin-inline-start:var(--mantine-spacing-xs)}.m_a25b86ee{position:absolute;left:50%;top:50%}.m_80d6d844{--button-border-width: calc(.0625rem * var(--mantine-scale));display:flex}.m_80d6d844 :where(.m_77c9d27d):focus{position:relative;z-index:1}.m_80d6d844[data-orientation=horizontal]{flex-direction:row}.m_80d6d844[data-orientation=horizontal] .m_77c9d27d:not(:only-child):first-child,.m_80d6d844[data-orientation=horizontal] .m_70be2a01:not(:only-child):first-child{border-end-end-radius:0;border-start-end-radius:0;border-inline-end-width:calc(var(--button-border-width) / 2)}.m_80d6d844[data-orientation=horizontal] .m_77c9d27d:not(:only-child):last-child,.m_80d6d844[data-orientation=horizontal] .m_70be2a01:not(:only-child):last-child{border-end-start-radius:0;border-start-start-radius:0;border-inline-start-width:calc(var(--button-border-width) / 2)}.m_80d6d844[data-orientation=horizontal] .m_77c9d27d:not(:only-child):not(:first-child):not(:last-child),.m_80d6d844[data-orientation=horizontal] .m_70be2a01:not(:only-child):not(:first-child):not(:last-child){border-radius:0;border-inline-width:calc(var(--button-border-width) / 2)}.m_80d6d844[data-orientation=vertical]{flex-direction:column}.m_80d6d844[data-orientation=vertical] .m_77c9d27d:not(:only-child):first-child,.m_80d6d844[data-orientation=vertical] .m_70be2a01:not(:only-child):first-child{border-end-start-radius:0;border-end-end-radius:0;border-bottom-width:calc(var(--button-border-width) / 2)}.m_80d6d844[data-orientation=vertical] .m_77c9d27d:not(:only-child):last-child,.m_80d6d844[data-orientation=vertical] .m_70be2a01:not(:only-child):last-child{border-start-start-radius:0;border-start-end-radius:0;border-top-width:calc(var(--button-border-width) / 2)}.m_80d6d844[data-orientation=vertical] .m_77c9d27d:not(:only-child):not(:first-child):not(:last-child),.m_80d6d844[data-orientation=vertical] .m_70be2a01:not(:only-child):not(:first-child):not(:last-child){border-radius:0;border-bottom-width:calc(var(--button-border-width) / 2);border-top-width:calc(var(--button-border-width) / 2)}.m_70be2a01{--section-height-xs: calc(1.875rem * var(--mantine-scale));--section-height-sm: calc(2.25rem * var(--mantine-scale));--section-height-md: calc(2.625rem * var(--mantine-scale));--section-height-lg: calc(3.125rem * var(--mantine-scale));--section-height-xl: calc(3.75rem * var(--mantine-scale));--section-height-compact-xs: calc(1.375rem * var(--mantine-scale));--section-height-compact-sm: calc(1.625rem * var(--mantine-scale));--section-height-compact-md: calc(1.875rem * var(--mantine-scale));--section-height-compact-lg: calc(2.125rem * var(--mantine-scale));--section-height-compact-xl: calc(2.5rem * var(--mantine-scale));--section-padding-x-xs: calc(.875rem * var(--mantine-scale));--section-padding-x-sm: calc(1.125rem * var(--mantine-scale));--section-padding-x-md: calc(1.375rem * var(--mantine-scale));--section-padding-x-lg: calc(1.625rem * var(--mantine-scale));--section-padding-x-xl: calc(2rem * var(--mantine-scale));--section-padding-x-compact-xs: calc(.4375rem * var(--mantine-scale));--section-padding-x-compact-sm: calc(.5rem * var(--mantine-scale));--section-padding-x-compact-md: calc(.625rem * var(--mantine-scale));--section-padding-x-compact-lg: calc(.75rem * var(--mantine-scale));--section-padding-x-compact-xl: calc(.875rem * var(--mantine-scale));--section-height: var(--section-height-sm);--section-padding-x: var(--section-padding-x-sm);--section-color: var(--mantine-color-white);font-weight:600;width:auto;border-radius:var(--section-radius, var(--mantine-radius-default));font-size:var(--section-fz, var(--mantine-font-size-sm));background:var(--section-bg, var(--mantine-primary-color-filled));border:var(--section-bd, calc(.0625rem * var(--mantine-scale)) solid transparent);color:var(--section-color, var(--mantine-color-white));height:var(--section-height, var(--section-height-sm));padding-inline:var(--section-padding-x, var(--section-padding-x-sm));vertical-align:middle;line-height:1;display:inline-flex;align-items:center;justify-content:center}.m_e615b15f{--card-padding: var(--mantine-spacing-md);position:relative;overflow:hidden;display:flex;flex-direction:column;padding:var(--card-padding);color:var(--mantine-color-text)}:where([data-mantine-color-scheme=light]) .m_e615b15f{background-color:var(--mantine-color-white)}:where([data-mantine-color-scheme=dark]) .m_e615b15f{background-color:var(--mantine-color-dark-6)}.m_599a2148{display:block;margin-inline:calc(var(--card-padding) * -1)}.m_599a2148:where(:first-child){margin-top:calc(var(--card-padding) * -1);border-top:none!important}.m_599a2148:where(:last-child){margin-bottom:calc(var(--card-padding) * -1);border-bottom:none!important}.m_599a2148:where([data-inherit-padding]){padding-inline:var(--card-padding)}.m_599a2148:where([data-with-border]){border-top:calc(.0625rem * var(--mantine-scale)) solid;border-bottom:calc(.0625rem * var(--mantine-scale)) solid}:where([data-mantine-color-scheme=light]) .m_599a2148{border-color:var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_599a2148{border-color:var(--mantine-color-dark-4)}.m_599a2148+.m_599a2148{border-top:none!important}.m_4451eb3a{display:flex;align-items:center;justify-content:center}.m_4451eb3a:where([data-inline]){display:inline-flex}.m_f59ffda3{--chip-size-xs: calc(1.4375rem * var(--mantine-scale));--chip-size-sm: calc(1.75rem * var(--mantine-scale));--chip-size-md: calc(2rem * var(--mantine-scale));--chip-size-lg: calc(2.25rem * var(--mantine-scale));--chip-size-xl: calc(2.5rem * var(--mantine-scale));--chip-icon-size-xs: calc(.625rem * var(--mantine-scale));--chip-icon-size-sm: calc(.75rem * var(--mantine-scale));--chip-icon-size-md: calc(.875rem * var(--mantine-scale));--chip-icon-size-lg: calc(1rem * var(--mantine-scale));--chip-icon-size-xl: calc(1.125rem * var(--mantine-scale));--chip-padding-xs: calc(1rem * var(--mantine-scale));--chip-padding-sm: calc(1.25rem * var(--mantine-scale));--chip-padding-md: calc(1.5rem * var(--mantine-scale));--chip-padding-lg: calc(1.75rem * var(--mantine-scale));--chip-padding-xl: calc(2rem * var(--mantine-scale));--chip-checked-padding-xs: calc(.46875rem * var(--mantine-scale));--chip-checked-padding-sm: calc(.625rem * var(--mantine-scale));--chip-checked-padding-md: calc(.73125rem * var(--mantine-scale));--chip-checked-padding-lg: calc(.84375rem * var(--mantine-scale));--chip-checked-padding-xl: calc(.98125rem * var(--mantine-scale));--chip-spacing-xs: calc(.625rem * var(--mantine-scale));--chip-spacing-sm: calc(.75rem * var(--mantine-scale));--chip-spacing-md: calc(1rem * var(--mantine-scale));--chip-spacing-lg: calc(1.25rem * var(--mantine-scale));--chip-spacing-xl: calc(1.375rem * var(--mantine-scale));--chip-size: var(--chip-size-sm);--chip-icon-size: var(--chip-icon-size-sm);--chip-padding: var(--chip-padding-sm);--chip-spacing: var(--chip-spacing-sm);--chip-checked-padding: var(--chip-checked-padding-sm);--chip-bg: var(--mantine-primary-color-filled);--chip-hover: var(--mantine-primary-color-filled-hover);--chip-color: var(--mantine-color-white);--chip-bd: calc(.0625rem * var(--mantine-scale)) solid transparent}.m_be049a53{display:inline-flex;align-items:center;-webkit-user-select:none;user-select:none;border-radius:var(--chip-radius, 1000rem);height:var(--chip-size);font-size:var(--chip-fz, var(--mantine-font-size-sm));line-height:calc(var(--chip-size) - calc(.125rem * var(--mantine-scale)));padding-inline:var(--chip-padding);cursor:pointer;white-space:nowrap;-webkit-tap-highlight-color:transparent;border:calc(.0625rem * var(--mantine-scale)) solid transparent;color:var(--mantine-color-text)}.m_be049a53:where([data-checked]){padding:var(--chip-checked-padding)}.m_be049a53:where([data-disabled]){cursor:not-allowed;background-color:var(--mantine-color-disabled);color:var(--mantine-color-disabled-color)}:where([data-mantine-color-scheme=light]) .m_3904c1af:not([data-disabled]){background-color:var(--mantine-color-white);border:1px solid var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_3904c1af:not([data-disabled]){background-color:var(--mantine-color-dark-6);border:1px solid var(--mantine-color-dark-4)}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_3904c1af:not([data-disabled]):hover{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_3904c1af:not([data-disabled]):hover{background-color:var(--mantine-color-dark-5)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_3904c1af:not([data-disabled]):active{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_3904c1af:not([data-disabled]):active{background-color:var(--mantine-color-dark-5)}}.m_3904c1af:not([data-disabled]):where([data-checked]){--chip-icon-color: var(--chip-color);border:var(--chip-bd)}@media (hover: hover){.m_3904c1af:not([data-disabled]):where([data-checked]):hover{background-color:var(--chip-hover)}}@media (hover: none){.m_3904c1af:not([data-disabled]):where([data-checked]):active{background-color:var(--chip-hover)}}.m_fa109255:not([data-disabled]),.m_f7e165c3:not([data-disabled]){border:calc(.0625rem * var(--mantine-scale)) solid transparent;color:var(--mantine-color-text)}:where([data-mantine-color-scheme=light]) .m_fa109255:not([data-disabled]),:where([data-mantine-color-scheme=light]) .m_f7e165c3:not([data-disabled]){background-color:var(--mantine-color-gray-1)}:where([data-mantine-color-scheme=dark]) .m_fa109255:not([data-disabled]),:where([data-mantine-color-scheme=dark]) .m_f7e165c3:not([data-disabled]){background-color:var(--mantine-color-dark-5)}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_fa109255:not([data-disabled]):hover,:where([data-mantine-color-scheme=light]) .m_f7e165c3:not([data-disabled]):hover{background-color:var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_fa109255:not([data-disabled]):hover,:where([data-mantine-color-scheme=dark]) .m_f7e165c3:not([data-disabled]):hover{background-color:var(--mantine-color-dark-4)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_fa109255:not([data-disabled]):active,:where([data-mantine-color-scheme=light]) .m_f7e165c3:not([data-disabled]):active{background-color:var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_fa109255:not([data-disabled]):active,:where([data-mantine-color-scheme=dark]) .m_f7e165c3:not([data-disabled]):active{background-color:var(--mantine-color-dark-4)}}.m_fa109255:not([data-disabled]):where([data-checked]),.m_f7e165c3:not([data-disabled]):where([data-checked]){--chip-icon-color: var(--chip-color);color:var(--chip-color);background-color:var(--chip-bg)}@media (hover: hover){.m_fa109255:not([data-disabled]):where([data-checked]):hover,.m_f7e165c3:not([data-disabled]):where([data-checked]):hover{background-color:var(--chip-hover)}}@media (hover: none){.m_fa109255:not([data-disabled]):where([data-checked]):active,.m_f7e165c3:not([data-disabled]):where([data-checked]):active{background-color:var(--chip-hover)}}.m_9ac86df9{width:calc(var(--chip-icon-size) + (var(--chip-spacing) / 1.5));max-width:calc(var(--chip-icon-size) + (var(--chip-spacing) / 1.5));height:var(--chip-icon-size);display:flex;align-items:center;overflow:hidden}.m_d6d72580{width:var(--chip-icon-size);height:var(--chip-icon-size);display:block;color:var(--chip-icon-color, inherit)}.m_bde07329{width:0;height:0;padding:0;opacity:0;margin:0}.m_bde07329:focus-visible+.m_be049a53{outline:2px solid var(--mantine-primary-color-filled);outline-offset:calc(.125rem * var(--mantine-scale))}.m_b183c0a2{font-family:var(--mantine-font-family-monospace);line-height:var(--mantine-line-height);padding:2px calc(var(--mantine-spacing-xs) / 2);border-radius:var(--mantine-radius-sm);font-size:var(--mantine-font-size-xs);margin:0;overflow:auto}:where([data-mantine-color-scheme=light]) .m_b183c0a2{background-color:var(--code-bg, var(--mantine-color-gray-0))}:where([data-mantine-color-scheme=dark]) .m_b183c0a2{background-color:var(--code-bg, var(--mantine-color-dark-6))}.m_b183c0a2[data-block]{padding:var(--mantine-spacing-xs)}.m_de3d2490{--cs-size: calc(1.75rem * var(--mantine-scale));--cs-radius: calc(62.5rem * var(--mantine-scale));-webkit-tap-highlight-color:transparent;border:none;appearance:none;display:block;line-height:1;position:relative;width:var(--cs-size);height:var(--cs-size);min-width:var(--cs-size);min-height:var(--cs-size);border-radius:var(--cs-radius);color:inherit;text-decoration:none}[data-mantine-color-scheme=light] .m_de3d2490{--alpha-overlay-color: var(--mantine-color-gray-3);--alpha-overlay-bg: var(--mantine-color-white)}[data-mantine-color-scheme=dark] .m_de3d2490{--alpha-overlay-color: var(--mantine-color-dark-4);--alpha-overlay-bg: var(--mantine-color-dark-7)}.m_862f3d1b{position:absolute;inset:0;border-radius:var(--cs-radius)}.m_98ae7f22{position:absolute;inset:0;border-radius:var(--cs-radius);z-index:1;box-shadow:#0000001a 0 0 0 calc(.0625rem * var(--mantine-scale)) inset,#00000026 0 0 calc(.25rem * var(--mantine-scale)) inset}.m_95709ac0{position:absolute;inset:0;border-radius:var(--cs-radius);background-size:calc(.5rem * var(--mantine-scale)) calc(.5rem * var(--mantine-scale));background-position:0 0,0 calc(.25rem * var(--mantine-scale)),calc(.25rem * var(--mantine-scale)) calc(-.25rem * var(--mantine-scale)),calc(-.25rem * var(--mantine-scale)) 0;background-image:linear-gradient(45deg,var(--alpha-overlay-color) 25%,transparent 25%),linear-gradient(-45deg,var(--alpha-overlay-color) 25%,transparent 25%),linear-gradient(45deg,transparent 75%,var(--alpha-overlay-color) 75%),linear-gradient(-45deg,var(--alpha-overlay-bg) 75%,var(--alpha-overlay-color) 75%)}.m_93e74e3{position:absolute;inset:0;border-radius:var(--cs-radius);z-index:2;display:flex;align-items:center;justify-content:center}.m_fee9c77{--cp-width-xs: calc(11.25rem * var(--mantine-scale));--cp-width-sm: calc(12.5rem * var(--mantine-scale));--cp-width-md: calc(15rem * var(--mantine-scale));--cp-width-lg: calc(17.5rem * var(--mantine-scale));--cp-width-xl: calc(20rem * var(--mantine-scale));--cp-preview-size-xs: calc(1.625rem * var(--mantine-scale));--cp-preview-size-sm: calc(2.125rem * var(--mantine-scale));--cp-preview-size-md: calc(2.625rem * var(--mantine-scale));--cp-preview-size-lg: calc(3.125rem * var(--mantine-scale));--cp-preview-size-xl: calc(3.375rem * var(--mantine-scale));--cp-thumb-size-xs: calc(.5rem * var(--mantine-scale));--cp-thumb-size-sm: calc(.75rem * var(--mantine-scale));--cp-thumb-size-md: calc(1rem * var(--mantine-scale));--cp-thumb-size-lg: calc(1.25rem * var(--mantine-scale));--cp-thumb-size-xl: calc(1.375rem * var(--mantine-scale));--cp-saturation-height-xs: calc(6.25rem * var(--mantine-scale));--cp-saturation-height-sm: calc(6.875rem * var(--mantine-scale));--cp-saturation-height-md: calc(7.5rem * var(--mantine-scale));--cp-saturation-height-lg: calc(8.75rem * var(--mantine-scale));--cp-saturation-height-xl: calc(10rem * var(--mantine-scale));--cp-preview-size: var(--cp-preview-size-sm);--cp-thumb-size: var(--cp-thumb-size-sm);--cp-saturation-height: var(--cp-saturation-height-sm);--cp-width: var(--cp-width-sm);--cp-body-spacing: var(--mantine-spacing-sm);width:var(--cp-width);padding:calc(.0625rem * var(--mantine-scale))}.m_fee9c77:where([data-full-width]){width:100%}.m_9dddfbac{width:var(--cp-preview-size);height:var(--cp-preview-size)}.m_bffecc3e{display:flex;padding-top:calc(var(--cp-body-spacing) / 2)}.m_3283bb96{flex:1}.m_3283bb96:not(:only-child){margin-inline-end:var(--mantine-spacing-xs)}.m_40d572ba{overflow:hidden;position:absolute;box-shadow:0 0 1px #0009;border:2px solid var(--mantine-color-white);width:var(--cp-thumb-size);height:var(--cp-thumb-size);border-radius:var(--cp-thumb-size);left:calc(var(--thumb-x-offset) - var(--cp-thumb-size) / 2);top:calc(var(--thumb-y-offset) - var(--cp-thumb-size) / 2)}.m_d8ee6fd8{height:unset!important;width:unset!important;min-width:0!important;min-height:0!important;margin:calc(.125rem * var(--mantine-scale));cursor:pointer;padding-bottom:calc(var(--cp-swatch-size) - calc(.25rem * var(--mantine-scale)));flex:0 0 calc(var(--cp-swatch-size) - calc(.25rem * var(--mantine-scale)))}.m_5711e686{margin-top:calc(.3125rem * var(--mantine-scale));margin-inline:calc(-.125rem * var(--mantine-scale));display:flex;flex-wrap:wrap}.m_5711e686:only-child{margin-top:0}.m_202a296e{--cp-thumb-size-xs: calc(.5rem * var(--mantine-scale));--cp-thumb-size-sm: calc(.75rem * var(--mantine-scale));--cp-thumb-size-md: calc(1rem * var(--mantine-scale));--cp-thumb-size-lg: calc(1.25rem * var(--mantine-scale));--cp-thumb-size-xl: calc(1.375rem * var(--mantine-scale));-webkit-tap-highlight-color:transparent;position:relative;height:var(--cp-saturation-height);border-radius:var(--mantine-radius-sm);margin:calc(var(--cp-thumb-size) / 2)}.m_202a296e:where([data-focus-ring=auto]):focus:focus-visible .m_40d572ba{outline:2px solid var(--mantine-color-blue-filled)}.m_202a296e:where([data-focus-ring=always]):focus .m_40d572ba{outline:2px solid var(--mantine-color-blue-filled)}.m_11b3db02{position:absolute;border-radius:var(--mantine-radius-sm);inset:calc(var(--cp-thumb-size) * -1 / 2 - calc(.0625rem * var(--mantine-scale)))}.m_d856d47d{--cp-thumb-size-xs: calc(.5rem * var(--mantine-scale));--cp-thumb-size-sm: calc(.75rem * var(--mantine-scale));--cp-thumb-size-md: calc(1rem * var(--mantine-scale));--cp-thumb-size-lg: calc(1.25rem * var(--mantine-scale));--cp-thumb-size-xl: calc(1.375rem * var(--mantine-scale));--cp-thumb-size: var(--cp-thumb-size, calc(.75rem * var(--mantine-scale)));position:relative;height:calc(var(--cp-thumb-size) + calc(.125rem * var(--mantine-scale)));margin-inline:calc(var(--cp-thumb-size) / 2);outline:none}.m_d856d47d+.m_d856d47d{margin-top:calc(.375rem * var(--mantine-scale))}.m_d856d47d:where([data-focus-ring=auto]):focus:focus-visible .m_40d572ba{outline:2px solid var(--mantine-color-blue-filled)}.m_d856d47d:where([data-focus-ring=always]):focus .m_40d572ba{outline:2px solid var(--mantine-color-blue-filled)}:where([data-mantine-color-scheme=light]) .m_d856d47d{--slider-checkers: var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_d856d47d{--slider-checkers: var(--mantine-color-dark-4)}.m_8f327113{position:absolute;top:0;bottom:0;inset-inline:calc(var(--cp-thumb-size) * -1 / 2 - calc(.0625rem * var(--mantine-scale)));border-radius:10000rem}.m_b077c2bc{--ci-eye-dropper-icon-size-xs: calc(.875rem * var(--mantine-scale));--ci-eye-dropper-icon-size-sm: calc(1rem * var(--mantine-scale));--ci-eye-dropper-icon-size-md: calc(1.125rem * var(--mantine-scale));--ci-eye-dropper-icon-size-lg: calc(1.25rem * var(--mantine-scale));--ci-eye-dropper-icon-size-xl: calc(1.375rem * var(--mantine-scale));--ci-eye-dropper-icon-size: var(--ci-eye-dropper-icon-size-sm)}.m_c5ccdcab{--ci-preview-size-xs: calc(1rem * var(--mantine-scale));--ci-preview-size-sm: calc(1.125rem * var(--mantine-scale));--ci-preview-size-md: calc(1.375rem * var(--mantine-scale));--ci-preview-size-lg: calc(1.75rem * var(--mantine-scale));--ci-preview-size-xl: calc(2.25rem * var(--mantine-scale));--ci-preview-size: var(--ci-preview-size-sm)}.m_5ece2cd7{padding:calc(.5rem * var(--mantine-scale))}.m_7485cace{--container-size-xs: calc(33.75rem * var(--mantine-scale));--container-size-sm: calc(45rem * var(--mantine-scale));--container-size-md: calc(60rem * var(--mantine-scale));--container-size-lg: calc(71.25rem * var(--mantine-scale));--container-size-xl: calc(82.5rem * var(--mantine-scale));--container-size: var(--container-size-md)}.m_7485cace:where([data-strategy=block]){max-width:var(--container-size);padding-inline:var(--mantine-spacing-md);margin-inline:auto}.m_7485cace:where([data-strategy=block]):where([data-fluid]){max-width:100%}.m_7485cace:where([data-strategy=grid]){display:grid;grid-template-columns:1fr min(100%,var(--container-size)) 1fr;margin-inline:auto}.m_7485cace:where([data-strategy=grid])>*{grid-column:2}.m_7485cace:where([data-strategy=grid])>[data-breakout]{grid-column:1 / -1}.m_7485cace:where([data-strategy=grid])>[data-breakout]>[data-container]{max-width:var(--container-size);margin-inline:auto}.m_e2125a27{--dialog-size-xs: calc(10rem * var(--mantine-scale));--dialog-size-sm: calc(12.5rem * var(--mantine-scale));--dialog-size-md: calc(21.25rem * var(--mantine-scale));--dialog-size-lg: calc(25rem * var(--mantine-scale));--dialog-size-xl: calc(31.25rem * var(--mantine-scale));--dialog-size: var(--dialog-size-md);position:relative;width:var(--dialog-size);max-width:calc(100vw - var(--mantine-spacing-xl) * 2);min-height:calc(3.125rem * var(--mantine-scale))}.m_5abab665{position:absolute;top:calc(var(--mantine-spacing-md) / 2);inset-inline-end:calc(var(--mantine-spacing-md) / 2)}.m_3eebeb36{--divider-size-xs: calc(.0625rem * var(--mantine-scale));--divider-size-sm: calc(.125rem * var(--mantine-scale));--divider-size-md: calc(.1875rem * var(--mantine-scale));--divider-size-lg: calc(.25rem * var(--mantine-scale));--divider-size-xl: calc(.3125rem * var(--mantine-scale));--divider-size: var(--divider-size-xs)}:where([data-mantine-color-scheme=light]) .m_3eebeb36{--divider-color: var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_3eebeb36{--divider-color: var(--mantine-color-dark-4)}.m_3eebeb36:where([data-orientation=horizontal]){border-top:var(--divider-size) var(--divider-border-style, solid) var(--divider-color)}.m_3eebeb36:where([data-orientation=vertical]){border-inline-start:var(--divider-size) var(--divider-border-style, solid) var(--divider-color);height:auto;align-self:stretch}.m_3eebeb36:where([data-with-label]){border:0}.m_9e365f20{display:flex;align-items:center;font-size:var(--mantine-font-size-xs);color:var(--mantine-color-dimmed);white-space:nowrap}.m_9e365f20:where([data-position=left]):before{display:none}.m_9e365f20:where([data-position=right]):after{display:none}.m_9e365f20:before{content:"";flex:1;height:calc(.0625rem * var(--mantine-scale));border-top:var(--divider-size) var(--divider-border-style, solid) var(--divider-color);margin-inline-end:var(--mantine-spacing-xs)}.m_9e365f20:after{content:"";flex:1;height:calc(.0625rem * var(--mantine-scale));border-top:var(--divider-size) var(--divider-border-style, solid) var(--divider-color);margin-inline-start:var(--mantine-spacing-xs)}.m_f11b401e{--drawer-size-xs: calc(20rem * var(--mantine-scale));--drawer-size-sm: calc(23.75rem * var(--mantine-scale));--drawer-size-md: calc(27.5rem * var(--mantine-scale));--drawer-size-lg: calc(38.75rem * var(--mantine-scale));--drawer-size-xl: calc(48.75rem * var(--mantine-scale));--drawer-size: var(--drawer-size-md);--drawer-offset: 0rem}.m_5a7c2c9{z-index:1000}.m_b8a05bbd{flex:var(--drawer-flex, 0 0 var(--drawer-size));height:var(--drawer-height, calc(100% - var(--drawer-offset) * 2));margin:var(--drawer-offset);max-width:calc(100% - var(--drawer-offset) * 2);max-height:calc(100% - var(--drawer-offset) * 2);overflow-y:auto}.m_b8a05bbd[data-hidden]{opacity:0!important;pointer-events:none}.m_31cd769a{display:flex;justify-content:var(--drawer-justify, flex-start);align-items:var(--drawer-align, flex-start)}.m_e9408a47{padding:var(--mantine-spacing-lg);padding-top:var(--mantine-spacing-xs);border-radius:var(--fieldset-radius, var(--mantine-radius-default));min-inline-size:auto}.m_84c9523a{border:calc(.0625rem * var(--mantine-scale)) solid}:where([data-mantine-color-scheme=light]) .m_84c9523a{border-color:var(--mantine-color-gray-3);background-color:var(--mantine-color-white)}:where([data-mantine-color-scheme=dark]) .m_84c9523a{border-color:var(--mantine-color-dark-4);background-color:var(--mantine-color-dark-7)}.m_ef274e49{border:calc(.0625rem * var(--mantine-scale)) solid}:where([data-mantine-color-scheme=light]) .m_ef274e49{border-color:var(--mantine-color-gray-3);background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_ef274e49{border-color:var(--mantine-color-dark-4);background-color:var(--mantine-color-dark-6)}.m_eda993d3{padding:0;border:0;border-radius:0}.m_90794832{font-size:var(--mantine-font-size-sm)}.m_74ca27fe{padding:0;margin-bottom:var(--mantine-spacing-sm)}.m_8478a6da{container:mantine-grid / inline-size}.m_410352e9{--grid-overflow: visible;--grid-margin: calc(var(--grid-gutter) / -2);--grid-col-padding: calc(var(--grid-gutter) / 2);overflow:var(--grid-overflow)}.m_dee7bd2f{width:calc(100% + var(--grid-gutter));display:flex;flex-wrap:wrap;justify-content:var(--grid-justify);align-items:var(--grid-align);margin:var(--grid-margin)}.m_96bdd299{--col-flex-grow: 0;--col-offset: 0rem;flex-shrink:0;order:var(--col-order);flex-basis:var(--col-flex-basis);width:var(--col-width);max-width:var(--col-max-width);flex-grow:var(--col-flex-grow);margin-inline-start:var(--col-offset);padding:var(--grid-col-padding)}.m_bcb3f3c2{color:var(--mantine-color-black)}:where([data-mantine-color-scheme=light]) .m_bcb3f3c2{background-color:var(--mark-bg-light)}:where([data-mantine-color-scheme=dark]) .m_bcb3f3c2{background-color:var(--mark-bg-dark)}.m_9e117634{display:block;object-fit:var(--image-object-fit, cover);width:100%;border-radius:var(--image-radius, 0)}@keyframes m_885901b1{0%{opacity:.6;transform:scale(0)}to{opacity:0;transform:scale(2.8)}}.m_e5262200{--indicator-size: calc(.625rem * var(--mantine-scale));--indicator-color: var(--mantine-primary-color-filled);position:relative;display:block}.m_e5262200:where([data-inline]){display:inline-block}.m_760d1fb1{position:absolute;top:var(--indicator-top);left:var(--indicator-left);right:var(--indicator-right);bottom:var(--indicator-bottom);transform:translate(var(--indicator-translate-x),var(--indicator-translate-y));min-width:var(--indicator-size);height:var(--indicator-size);border-radius:var(--indicator-radius, 1000rem);z-index:var(--indicator-z-index, 200);display:flex;align-items:center;justify-content:center;font-size:var(--mantine-font-size-xs);background-color:var(--indicator-color);color:var(--indicator-text-color, var(--mantine-color-white));white-space:nowrap}.m_760d1fb1:before{content:"";position:absolute;inset:0;background-color:var(--indicator-color);border-radius:var(--indicator-radius, 1000rem);z-index:-1}.m_760d1fb1:where([data-with-label]){padding-inline:calc(var(--mantine-spacing-xs) / 2)}.m_760d1fb1:where([data-with-border]){border:2px solid var(--mantine-color-body)}.m_760d1fb1[data-processing]:before{animation:m_885901b1 1s linear infinite}.m_dc6f14e2{--kbd-fz-xs: calc(.625rem * var(--mantine-scale));--kbd-fz-sm: calc(.75rem * var(--mantine-scale));--kbd-fz-md: calc(.875rem * var(--mantine-scale));--kbd-fz-lg: calc(1rem * var(--mantine-scale));--kbd-fz-xl: calc(1.25rem * var(--mantine-scale));--kbd-fz: var(--kbd-fz-sm);font-family:var(--mantine-font-family-monospace);line-height:var(--mantine-line-height);font-weight:700;font-size:var(--kbd-fz);border-radius:var(--mantine-radius-sm);border:calc(.0625rem * var(--mantine-scale)) solid;border-bottom-width:calc(.1875rem * var(--mantine-scale));unicode-bidi:embed;text-align:center;padding:.12em .45em}:where([data-mantine-color-scheme=light]) .m_dc6f14e2{border-color:var(--mantine-color-gray-3);color:var(--mantine-color-gray-7);background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_dc6f14e2{border-color:var(--mantine-color-dark-4);color:var(--mantine-color-dark-0);background-color:var(--mantine-color-dark-6)}.m_abbac491{--list-fz: var(--mantine-font-size-md);--list-lh: var(--mantine-line-height-md);--list-marker-gap: var(--mantine-spacing-lg);list-style-position:outside;font-size:var(--list-fz);line-height:var(--list-lh);margin:0;padding:0;padding-inline-start:var(--list-marker-gap)}.m_abbac491:where([data-with-padding]){padding-inline-start:calc(var(--list-marker-gap) + var(--mantine-spacing-md))}.m_abb6bec2{white-space:normal;line-height:var(--list-lh)}.m_abb6bec2:where([data-with-icon]){list-style:none}.m_abb6bec2:where([data-with-icon]) .m_75cd9f71{--li-direction: row;--li-align: center}.m_abb6bec2:where(:not(:first-of-type)){margin-top:var(--list-spacing, 0)}.m_abb6bec2:where([data-centered]){line-height:1}.m_75cd9f71{display:inline-flex;flex-direction:var(--li-direction, column);align-items:var(--li-align, flex-start);white-space:normal}.m_60f83e5b{display:inline-block;vertical-align:middle;margin-inline-end:var(--mantine-spacing-sm)}.m_6e45937b{position:absolute;inset:0;display:flex;align-items:center;justify-content:center;overflow:hidden;z-index:var(--lo-z-index)}.m_e8eb006c{position:relative;z-index:calc(var(--lo-z-index) + 1)}.m_df587f17{z-index:var(--lo-z-index)}.m_dc9b7c9f{padding:calc(.25rem * var(--mantine-scale))}.m_9bfac126{color:var(--mantine-color-dimmed);font-weight:500;font-size:var(--mantine-font-size-xs);padding:calc(var(--mantine-spacing-xs) / 2) var(--mantine-spacing-sm);cursor:default}.m_efdf90cb{margin-top:calc(.25rem * var(--mantine-scale));margin-bottom:calc(.25rem * var(--mantine-scale));border-top:calc(.0625rem * var(--mantine-scale)) solid}:where([data-mantine-color-scheme=light]) .m_efdf90cb{border-color:var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_efdf90cb{border-color:var(--mantine-color-dark-4)}.m_99ac2aa1{font-size:var(--mantine-font-size-sm);width:100%;padding:calc(var(--mantine-spacing-xs) / 1.5) var(--mantine-spacing-sm);border-radius:var(--popover-radius, var(--mantine-radius-default));color:var(--menu-item-color, var(--mantine-color-text));display:flex;align-items:center;-webkit-user-select:none;user-select:none}.m_99ac2aa1:where([data-disabled],:disabled){color:var(--mantine-color-disabled-color);opacity:.6;cursor:not-allowed}:where([data-mantine-color-scheme=light]) .m_99ac2aa1:where(:hover,:focus):where(:not(:disabled,[data-disabled])){background-color:var(--menu-item-hover, var(--mantine-color-gray-1))}:where([data-mantine-color-scheme=dark]) .m_99ac2aa1:where(:hover,:focus):where(:not(:disabled,[data-disabled])){background-color:var(--menu-item-hover, var(--mantine-color-dark-4))}.m_99ac2aa1:where([data-sub-menu-item]){padding-inline-end:calc(.3125rem * var(--mantine-scale))}.m_5476e0d3{flex:1}.m_8b75e504{display:flex;justify-content:center;align-items:center}.m_8b75e504:where([data-position=left]){margin-inline-end:var(--mantine-spacing-xs)}.m_8b75e504:where([data-position=right]){margin-inline-start:var(--mantine-spacing-xs)}.m_b85b0bed{transform:rotate(-90deg)}:where([dir=rtl]) .m_b85b0bed{transform:rotate(90deg)}.m_9df02822{--modal-size-xs: calc(20rem * var(--mantine-scale));--modal-size-sm: calc(23.75rem * var(--mantine-scale));--modal-size-md: calc(27.5rem * var(--mantine-scale));--modal-size-lg: calc(38.75rem * var(--mantine-scale));--modal-size-xl: calc(48.75rem * var(--mantine-scale));--modal-size: var(--modal-size-md);--modal-y-offset: 5dvh;--modal-x-offset: 5vw}.m_9df02822[data-full-screen]{--modal-border-radius: 0 !important}.m_9df02822[data-full-screen] .m_54c44539{--modal-content-flex: 0 0 100%;--modal-content-max-height: auto;--modal-content-height: 100dvh}.m_9df02822[data-full-screen] .m_1f958f16{--modal-inner-y-offset: 0;--modal-inner-x-offset: 0}.m_9df02822[data-centered] .m_1f958f16{--modal-inner-align: center}.m_d0e2b9cd{border-start-start-radius:var(--modal-radius, var(--mantine-radius-default));border-start-end-radius:var(--modal-radius, var(--mantine-radius-default))}.m_54c44539{flex:var(--modal-content-flex, 0 0 var(--modal-size));max-width:100%;max-height:var(--modal-content-max-height, calc(100dvh - var(--modal-y-offset) * 2));height:var(--modal-content-height, auto);overflow-y:auto}.m_54c44539[data-full-screen]{border-radius:0}.m_54c44539[data-hidden]{opacity:0!important;pointer-events:none}.m_1f958f16{display:flex;justify-content:center;align-items:var(--modal-inner-align, flex-start);padding-top:var(--modal-inner-y-offset, var(--modal-y-offset));padding-bottom:var(--modal-inner-y-offset, var(--modal-y-offset));padding-inline:var(--modal-inner-x-offset, var(--modal-x-offset))}.m_7cda1cd6{--pill-fz-xs: calc(.625rem * var(--mantine-scale));--pill-fz-sm: calc(.75rem * var(--mantine-scale));--pill-fz-md: calc(.875rem * var(--mantine-scale));--pill-fz-lg: calc(1rem * var(--mantine-scale));--pill-fz-xl: calc(1.125rem * var(--mantine-scale));--pill-height-xs: calc(1.125rem * var(--mantine-scale));--pill-height-sm: calc(1.375rem * var(--mantine-scale));--pill-height-md: calc(1.5625rem * var(--mantine-scale));--pill-height-lg: calc(1.75rem * var(--mantine-scale));--pill-height-xl: calc(2rem * var(--mantine-scale));--pill-fz: var(--pill-fz-sm);--pill-height: var(--pill-height-sm);font-size:var(--pill-fz);flex:0;height:var(--pill-height);padding-inline:.8em;display:inline-flex;align-items:center;border-radius:var(--pill-radius, 1000rem);line-height:1;white-space:nowrap;user-select:none;-webkit-user-select:none;max-width:100%}:where([data-mantine-color-scheme=dark]) .m_7cda1cd6{background-color:var(--mantine-color-dark-7);color:var(--mantine-color-dark-0)}:where([data-mantine-color-scheme=light]) .m_7cda1cd6{color:var(--mantine-color-black)}.m_7cda1cd6:where([data-with-remove]:not(:has(button:disabled))){padding-inline-end:0}.m_7cda1cd6:where([data-disabled],:has(button:disabled)){cursor:not-allowed}:where([data-mantine-color-scheme=light]) .m_44da308b{background-color:var(--mantine-color-gray-1)}:where([data-mantine-color-scheme=light]) .m_44da308b:where([data-disabled],:has(button:disabled)){background-color:var(--mantine-color-disabled)}:where([data-mantine-color-scheme=light]) .m_e3a01f8{background-color:var(--mantine-color-white)}:where([data-mantine-color-scheme=light]) .m_e3a01f8:where([data-disabled],:has(button:disabled)){background-color:var(--mantine-color-disabled)}.m_1e0e6180{cursor:inherit;overflow:hidden;height:100%;line-height:var(--pill-height);text-overflow:ellipsis}.m_ae386778{color:inherit;font-size:inherit;height:100%;min-height:unset;min-width:2em;width:unset;border-radius:0;padding-inline-start:.1em;padding-inline-end:.3em;flex:0;border-end-end-radius:var(--pill-radius, 50%);border-start-end-radius:var(--pill-radius, 50%)}.m_7cda1cd6[data-disabled]>.m_ae386778,.m_ae386778:disabled{display:none;background-color:transparent;width:.8em;min-width:.8em;padding:0;cursor:not-allowed}.m_7cda1cd6[data-disabled]>.m_ae386778>svg,.m_ae386778:disabled>svg{display:none}.m_ae386778>svg{pointer-events:none}.m_1dcfd90b{--pg-gap-xs: calc(.375rem * var(--mantine-scale));--pg-gap-sm: calc(.5rem * var(--mantine-scale));--pg-gap-md: calc(.625rem * var(--mantine-scale));--pg-gap-lg: calc(.75rem * var(--mantine-scale));--pg-gap-xl: calc(.75rem * var(--mantine-scale));--pg-gap: var(--pg-gap-sm);display:flex;align-items:center;gap:var(--pg-gap);flex-wrap:wrap}.m_45c4369d{background-color:transparent;appearance:none;min-width:calc(6.25rem * var(--mantine-scale));flex:1;border:0;font-size:inherit;height:1.6em;color:inherit;padding:0}.m_45c4369d::placeholder{color:var(--input-placeholder-color);opacity:1}.m_45c4369d:where([data-type=hidden],[data-type=auto]){height:calc(.0625rem * var(--mantine-scale));width:calc(.0625rem * var(--mantine-scale));top:0;left:0;pointer-events:none;position:absolute;opacity:0}.m_45c4369d:focus{outline:none}.m_45c4369d:where([data-type=auto]:focus){height:1.6em;visibility:visible;opacity:1;position:static}.m_45c4369d:where([data-pointer]:not([data-disabled],:disabled)){cursor:pointer}.m_45c4369d:where([data-disabled],:disabled){cursor:not-allowed}.m_f0824112{--nl-bg: var(--mantine-primary-color-light);--nl-hover: var(--mantine-primary-color-light-hover);--nl-color: var(--mantine-primary-color-light-color);display:flex;align-items:center;width:100%;padding:8px var(--mantine-spacing-sm);-webkit-user-select:none;user-select:none}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_f0824112:hover{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_f0824112:hover{background-color:var(--mantine-color-dark-6)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_f0824112:active{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_f0824112:active{background-color:var(--mantine-color-dark-6)}}.m_f0824112:where([data-disabled]){opacity:.4;pointer-events:none}.m_f0824112:where([data-active],[aria-current=page]){background-color:var(--nl-bg);color:var(--nl-color)}@media (hover: hover){.m_f0824112:where([data-active],[aria-current=page]):hover{background-color:var(--nl-hover)}}@media (hover: none){.m_f0824112:where([data-active],[aria-current=page]):active{background-color:var(--nl-hover)}}.m_f0824112:where([data-active],[aria-current=page]) .m_57492dcc{--description-opacity: .9;--description-color: var(--nl-color)}.m_690090b5{display:flex;align-items:center;justify-content:center;transition:transform .15s ease}.m_690090b5>svg{display:block}.m_690090b5:where([data-position=left]){margin-inline-end:var(--mantine-spacing-sm)}.m_690090b5:where([data-position=right]){margin-inline-start:var(--mantine-spacing-sm)}.m_690090b5:where([data-rotate]){transform:rotate(90deg)}.m_1f6ac4c4{font-size:var(--mantine-font-size-sm)}.m_f07af9d2{flex:1;overflow:hidden;text-overflow:ellipsis}.m_f07af9d2:where([data-no-wrap]){white-space:nowrap}.m_57492dcc{display:block;font-size:var(--mantine-font-size-xs);opacity:var(--description-opacity, 1);color:var(--description-color, var(--mantine-color-dimmed));overflow:hidden;text-overflow:ellipsis}:where([data-no-wrap]) .m_57492dcc{white-space:nowrap}.m_e17b862f{padding-inline-start:var(--nl-offset, var(--mantine-spacing-lg))}.m_1fd8a00b{transform:rotate(-90deg)}.m_a513464{--notification-radius: var(--mantine-radius-default);--notification-color: var(--mantine-primary-color-filled);overflow:hidden;box-sizing:border-box;position:relative;display:flex;align-items:center;padding-inline-start:calc(1.375rem * var(--mantine-scale));padding-inline-end:var(--mantine-spacing-xs);padding-top:var(--mantine-spacing-xs);padding-bottom:var(--mantine-spacing-xs);border-radius:var(--notification-radius);box-shadow:var(--mantine-shadow-lg)}.m_a513464:before{content:"";display:block;position:absolute;width:calc(.375rem * var(--mantine-scale));top:var(--notification-radius);bottom:var(--notification-radius);inset-inline-start:calc(.25rem * var(--mantine-scale));border-radius:var(--notification-radius);background-color:var(--notification-color)}:where([data-mantine-color-scheme=light]) .m_a513464{background-color:var(--mantine-color-white)}:where([data-mantine-color-scheme=dark]) .m_a513464{background-color:var(--mantine-color-dark-6)}.m_a513464:where([data-with-icon]):before{display:none}:where([data-mantine-color-scheme=light]) .m_a513464:where([data-with-border]){border:1px solid var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_a513464:where([data-with-border]){border:1px solid var(--mantine-color-dark-4)}.m_a4ceffb{box-sizing:border-box;margin-inline-end:var(--mantine-spacing-md);width:calc(1.75rem * var(--mantine-scale));height:calc(1.75rem * var(--mantine-scale));border-radius:calc(1.75rem * var(--mantine-scale));display:flex;align-items:center;justify-content:center;background-color:var(--notification-color);color:var(--mantine-color-white)}.m_b0920b15{margin-inline-end:var(--mantine-spacing-md)}.m_a49ed24{flex:1;overflow:hidden;margin-inline-end:var(--mantine-spacing-xs)}.m_3feedf16{margin-bottom:calc(.125rem * var(--mantine-scale));overflow:hidden;text-overflow:ellipsis;font-size:var(--mantine-font-size-sm);line-height:var(--mantine-line-height-sm);font-weight:500}:where([data-mantine-color-scheme=light]) .m_3feedf16{color:var(--mantine-color-gray-9)}:where([data-mantine-color-scheme=dark]) .m_3feedf16{color:var(--mantine-color-white)}.m_3d733a3a{font-size:var(--mantine-font-size-sm);line-height:var(--mantine-line-height-sm);overflow:hidden;text-overflow:ellipsis}:where([data-mantine-color-scheme=light]) .m_3d733a3a{color:var(--mantine-color-black)}:where([data-mantine-color-scheme=dark]) .m_3d733a3a{color:var(--mantine-color-dark-0)}:where([data-mantine-color-scheme=light]) .m_3d733a3a:where([data-with-title]){color:var(--mantine-color-gray-6)}:where([data-mantine-color-scheme=dark]) .m_3d733a3a:where([data-with-title]){color:var(--mantine-color-dark-2)}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_919a4d88:hover{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_919a4d88:hover{background-color:var(--mantine-color-dark-8)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_919a4d88:active{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_919a4d88:active{background-color:var(--mantine-color-dark-8)}}.m_e2f5cd4e{--ni-right-section-width-xs: calc(1.0625rem * var(--mantine-scale));--ni-right-section-width-sm: calc(1.5rem * var(--mantine-scale));--ni-right-section-width-md: calc(1.6875rem * var(--mantine-scale));--ni-right-section-width-lg: calc(1.9375rem * var(--mantine-scale));--ni-right-section-width-xl: calc(2.125rem * var(--mantine-scale))}.m_95e17d22{--ni-chevron-size-xs: calc(.625rem * var(--mantine-scale));--ni-chevron-size-sm: calc(.875rem * var(--mantine-scale));--ni-chevron-size-md: calc(1rem * var(--mantine-scale));--ni-chevron-size-lg: calc(1.125rem * var(--mantine-scale));--ni-chevron-size-xl: calc(1.25rem * var(--mantine-scale));--ni-chevron-size: var(--ni-chevron-size-sm);display:flex;flex-direction:column;width:100%;height:calc(var(--input-height) - calc(.125rem * var(--mantine-scale)));max-width:calc(var(--ni-chevron-size) * 1.7);margin-inline-start:auto}.m_80b4b171{--control-border: 1px solid var(--input-bd);--control-radius: calc(var(--input-radius) - calc(.0625rem * var(--mantine-scale)));flex:0 0 50%;width:100%;padding:0;height:calc(var(--input-height) / 2 - calc(.0625rem * var(--mantine-scale)));border-inline-start:var(--control-border);display:flex;align-items:center;justify-content:center;color:var(--mantine-color-text);background-color:transparent;cursor:pointer}.m_80b4b171:where(:disabled){background-color:transparent;cursor:not-allowed;opacity:.6;color:var(--mantine-color-disabled-color)}.m_e2f5cd4e[data-error] :where(.m_80b4b171){color:var(--mantine-color-error)}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_80b4b171:hover{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_80b4b171:hover{background-color:var(--mantine-color-dark-4)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_80b4b171:active{background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_80b4b171:active{background-color:var(--mantine-color-dark-4)}}.m_80b4b171:where(:first-of-type){border-radius:0;border-start-end-radius:var(--control-radius)}.m_80b4b171:last-of-type{border-radius:0;border-end-end-radius:var(--control-radius)}.m_4addd315{--pagination-control-size-xs: calc(1.375rem * var(--mantine-scale));--pagination-control-size-sm: calc(1.625rem * var(--mantine-scale));--pagination-control-size-md: calc(2rem * var(--mantine-scale));--pagination-control-size-lg: calc(2.375rem * var(--mantine-scale));--pagination-control-size-xl: calc(2.75rem * var(--mantine-scale));--pagination-control-size: var(--pagination-control-size-md);--pagination-control-fz: var(--mantine-font-size-md);--pagination-active-bg: var(--mantine-primary-color-filled)}.m_326d024a{display:flex;align-items:center;justify-content:center;border:calc(.0625rem * var(--mantine-scale)) solid;cursor:pointer;color:var(--mantine-color-text);height:var(--pagination-control-size);min-width:var(--pagination-control-size);font-size:var(--pagination-control-fz);line-height:1;border-radius:var(--pagination-control-radius, var(--mantine-radius-default))}.m_326d024a:where([data-with-padding]){padding:calc(var(--pagination-control-size) / 4)}.m_326d024a:where(:disabled,[data-disabled]){cursor:not-allowed;opacity:.4}:where([data-mantine-color-scheme=light]) .m_326d024a{border-color:var(--mantine-color-gray-4);background-color:var(--mantine-color-white)}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_326d024a:hover:where(:not(:disabled,[data-disabled])){background-color:var(--mantine-color-gray-0)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_326d024a:active:where(:not(:disabled,[data-disabled])){background-color:var(--mantine-color-gray-0)}}:where([data-mantine-color-scheme=dark]) .m_326d024a{border-color:var(--mantine-color-dark-4);background-color:var(--mantine-color-dark-6)}@media (hover: hover){:where([data-mantine-color-scheme=dark]) .m_326d024a:hover:where(:not(:disabled,[data-disabled])){background-color:var(--mantine-color-dark-5)}}@media (hover: none){:where([data-mantine-color-scheme=dark]) .m_326d024a:active:where(:not(:disabled,[data-disabled])){background-color:var(--mantine-color-dark-5)}}.m_326d024a:where([data-active]){background-color:var(--pagination-active-bg);border-color:var(--pagination-active-bg);color:var(--pagination-active-color, var(--mantine-color-white))}@media (hover: hover){.m_326d024a:where([data-active]):hover{background-color:var(--pagination-active-bg)}}@media (hover: none){.m_326d024a:where([data-active]):active{background-color:var(--pagination-active-bg)}}.m_4ad7767d{height:var(--pagination-control-size);min-width:var(--pagination-control-size);display:flex;align-items:center;justify-content:center;pointer-events:none}.m_f61ca620{--psi-button-size-xs: calc(1.375rem * var(--mantine-scale));--psi-button-size-sm: calc(1.625rem * var(--mantine-scale));--psi-button-size-md: calc(1.75rem * var(--mantine-scale));--psi-button-size-lg: calc(2rem * var(--mantine-scale));--psi-button-size-xl: calc(2.5rem * var(--mantine-scale));--psi-icon-size-xs: calc(.75rem * var(--mantine-scale));--psi-icon-size-sm: calc(.9375rem * var(--mantine-scale));--psi-icon-size-md: calc(1.0625rem * var(--mantine-scale));--psi-icon-size-lg: calc(1.1875rem * var(--mantine-scale));--psi-icon-size-xl: calc(1.3125rem * var(--mantine-scale));--psi-button-size: var(--psi-button-size-sm);--psi-icon-size: var(--psi-icon-size-sm)}.m_ccf8da4c{position:relative;overflow:hidden}.m_f2d85dd2{font-family:var(--mantine-font-family);background-color:transparent;border:0;padding-inline-end:var(--input-padding-inline-end);padding-inline-start:var(--input-padding-inline-start);position:absolute;inset:0;outline:0;font-size:inherit;line-height:var(--mantine-line-height);height:100%;width:100%;color:inherit}.m_ccf8da4c[data-disabled] .m_f2d85dd2,.m_f2d85dd2:disabled{cursor:not-allowed}.m_f2d85dd2::placeholder{color:var(--input-placeholder-color);opacity:1}.m_f2d85dd2::-ms-reveal{display:none}.m_b1072d44{width:var(--psi-button-size);height:var(--psi-button-size);min-width:var(--psi-button-size);min-height:var(--psi-button-size)}.m_b1072d44:disabled{display:none}.m_f1cb205a{--pin-input-size-xs: calc(1.875rem * var(--mantine-scale));--pin-input-size-sm: calc(2.25rem * var(--mantine-scale));--pin-input-size-md: calc(2.625rem * var(--mantine-scale));--pin-input-size-lg: calc(3.125rem * var(--mantine-scale));--pin-input-size-xl: calc(3.75rem * var(--mantine-scale));--pin-input-size: var(--pin-input-size-sm)}.m_cb288ead{width:var(--pin-input-size);height:var(--pin-input-size)}@keyframes m_81a374bd{0%{background-position:0 0}to{background-position:calc(2.5rem * var(--mantine-scale)) 0}}@keyframes m_e0fb7a86{0%{background-position:0 0}to{background-position:0 calc(2.5rem * var(--mantine-scale))}}.m_db6d6462{--progress-radius: var(--mantine-radius-default);--progress-size: var(--progress-size-md);--progress-size-xs: calc(.1875rem * var(--mantine-scale));--progress-size-sm: calc(.3125rem * var(--mantine-scale));--progress-size-md: calc(.5rem * var(--mantine-scale));--progress-size-lg: calc(.75rem * var(--mantine-scale));--progress-size-xl: calc(1rem * var(--mantine-scale));position:relative;height:var(--progress-size);border-radius:var(--progress-radius);overflow:hidden;display:flex}:where([data-mantine-color-scheme=light]) .m_db6d6462{background-color:var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_db6d6462{background-color:var(--mantine-color-dark-4)}.m_db6d6462:where([data-orientation=vertical]){height:auto;width:var(--progress-size);flex-direction:column-reverse}.m_2242eb65{background-color:var(--progress-section-color);height:100%;width:var(--progress-section-size);display:flex;align-items:center;justify-content:center;overflow:hidden;background-size:calc(1.25rem * var(--mantine-scale)) calc(1.25rem * var(--mantine-scale));transition:width var(--progress-transition-duration, .1s) ease}.m_2242eb65:where([data-striped]){background-image:linear-gradient(45deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent)}.m_2242eb65:where([data-animated]){animation:m_81a374bd 1s linear infinite}.m_2242eb65:where(:last-of-type){border-radius:0;border-start-end-radius:var(--progress-radius);border-end-end-radius:var(--progress-radius)}.m_2242eb65:where(:first-of-type){border-radius:0;border-start-start-radius:var(--progress-radius);border-end-start-radius:var(--progress-radius)}.m_db6d6462:where([data-orientation=vertical]) .m_2242eb65{width:100%;height:var(--progress-section-size);transition:height var(--progress-transition-duration, .1s) ease}.m_db6d6462:where([data-orientation=vertical]) .m_2242eb65:where([data-striped]){background-image:linear-gradient(135deg,rgba(255,255,255,.15) 25%,transparent 25%,transparent 50%,rgba(255,255,255,.15) 50%,rgba(255,255,255,.15) 75%,transparent 75%,transparent)}.m_db6d6462:where([data-orientation=vertical]) .m_2242eb65:where([data-animated]){animation:m_e0fb7a86 1s linear infinite}.m_db6d6462:where([data-orientation=vertical]) .m_2242eb65:where(:last-of-type){border-radius:0;border-start-start-radius:var(--progress-radius);border-start-end-radius:var(--progress-radius)}.m_db6d6462:where([data-orientation=vertical]) .m_2242eb65:where(:first-of-type){border-radius:0;border-end-start-radius:var(--progress-radius);border-end-end-radius:var(--progress-radius)}.m_91e40b74{color:var(--progress-label-color, var(--mantine-color-white));font-weight:700;-webkit-user-select:none;user-select:none;overflow:hidden;white-space:nowrap;text-overflow:ellipsis;font-size:min(calc(var(--progress-size) * .65),calc(1.125rem * var(--mantine-scale)));line-height:1;padding-inline:calc(.25rem * var(--mantine-scale))}.m_db6d6462:where([data-orientation=vertical]) .m_91e40b74{writing-mode:vertical-rl}.m_9dc8ae12{--card-radius: var(--mantine-radius-default);display:block;width:100%;border-radius:var(--card-radius);cursor:pointer}.m_9dc8ae12 :where(*){cursor:inherit}.m_9dc8ae12:where([data-with-border]){border:calc(.0625rem * var(--mantine-scale)) solid transparent}:where([data-mantine-color-scheme=light]) .m_9dc8ae12:where([data-with-border]){border-color:var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_9dc8ae12:where([data-with-border]){border-color:var(--mantine-color-dark-4)}.m_717d7ff6{--radio-size-xs: calc(1rem * var(--mantine-scale));--radio-size-sm: calc(1.25rem * var(--mantine-scale));--radio-size-md: calc(1.5rem * var(--mantine-scale));--radio-size-lg: calc(1.875rem * var(--mantine-scale));--radio-size-xl: calc(2.25rem * var(--mantine-scale));--radio-icon-size-xs: calc(.375rem * var(--mantine-scale));--radio-icon-size-sm: calc(.5rem * var(--mantine-scale));--radio-icon-size-md: calc(.625rem * var(--mantine-scale));--radio-icon-size-lg: calc(.875rem * var(--mantine-scale));--radio-icon-size-xl: calc(1rem * var(--mantine-scale));--radio-icon-size: var(--radio-icon-size-sm);--radio-size: var(--radio-size-sm);--radio-color: var(--mantine-primary-color-filled);--radio-icon-color: var(--mantine-color-white);position:relative;border:calc(.0625rem * var(--mantine-scale)) solid transparent;width:var(--radio-size);min-width:var(--radio-size);height:var(--radio-size);min-height:var(--radio-size);border-radius:var(--radio-radius, 10000px);transition:border-color .1s ease,background-color .1s ease;cursor:var(--mantine-cursor-type);-webkit-tap-highlight-color:transparent;display:flex;align-items:center;justify-content:center}:where([data-mantine-color-scheme=light]) .m_717d7ff6{background-color:var(--mantine-color-white);border-color:var(--mantine-color-gray-4)}:where([data-mantine-color-scheme=dark]) .m_717d7ff6{background-color:var(--mantine-color-dark-6);border-color:var(--mantine-color-dark-4)}.m_717d7ff6[data-indeterminate],.m_717d7ff6[data-checked]{background-color:var(--radio-color);border-color:var(--radio-color)}.m_717d7ff6[data-indeterminate]>.m_3e4da632,.m_717d7ff6[data-checked]>.m_3e4da632{opacity:1;transform:none;color:var(--radio-icon-color)}.m_717d7ff6[data-disabled]{cursor:not-allowed;background-color:var(--mantine-color-disabled);border-color:var(--mantine-color-disabled-border)}.m_717d7ff6[data-disabled][data-checked]>.m_3e4da632{color:var(--mantine-color-disabled-color)}.m_2980836c[data-indeterminate]:not([data-disabled]),.m_2980836c[data-checked]:not([data-disabled]){background-color:transparent;border-color:var(--radio-color)}.m_2980836c[data-indeterminate]:not([data-disabled])>.m_3e4da632,.m_2980836c[data-checked]:not([data-disabled])>.m_3e4da632{color:var(--radio-color);opacity:1;transform:none}.m_3e4da632{display:block;width:var(--radio-icon-size);height:var(--radio-icon-size);color:transparent;pointer-events:none;transform:translateY(calc(.3125rem * var(--mantine-scale))) scale(.5);opacity:1;transition:transform .1s ease,opacity .1s ease}.m_f3f1af94{--radio-size-xs: calc(1rem * var(--mantine-scale));--radio-size-sm: calc(1.25rem * var(--mantine-scale));--radio-size-md: calc(1.5rem * var(--mantine-scale));--radio-size-lg: calc(1.875rem * var(--mantine-scale));--radio-size-xl: calc(2.25rem * var(--mantine-scale));--radio-size: var(--radio-size-sm);--radio-icon-size-xs: calc(.375rem * var(--mantine-scale));--radio-icon-size-sm: calc(.5rem * var(--mantine-scale));--radio-icon-size-md: calc(.625rem * var(--mantine-scale));--radio-icon-size-lg: calc(.875rem * var(--mantine-scale));--radio-icon-size-xl: calc(1rem * var(--mantine-scale));--radio-icon-size: var(--radio-icon-size-sm);--radio-icon-color: var(--mantine-color-white)}.m_89c4f5e4{position:relative;width:var(--radio-size);height:var(--radio-size);order:1}.m_89c4f5e4:where([data-label-position=left]){order:2}.m_f3ed6b2b{color:var(--radio-icon-color);opacity:var(--radio-icon-opacity, 0);transform:var(--radio-icon-transform, scale(.2) translateY(calc(.625rem * var(--mantine-scale))));transition:opacity .1s ease,transform .2s ease;pointer-events:none;width:var(--radio-icon-size);height:var(--radio-icon-size);position:absolute;top:calc(50% - var(--radio-icon-size) / 2);left:calc(50% - var(--radio-icon-size) / 2)}.m_8a3dbb89{border:calc(.0625rem * var(--mantine-scale)) solid;position:relative;appearance:none;width:var(--radio-size);height:var(--radio-size);border-radius:var(--radio-radius, var(--radio-size));margin:0;display:flex;align-items:center;justify-content:center;transition-property:background-color,border-color;transition-timing-function:ease;transition-duration:.1s;cursor:var(--mantine-cursor-type);-webkit-tap-highlight-color:transparent}:where([data-mantine-color-scheme=light]) .m_8a3dbb89{background-color:var(--mantine-color-white);border-color:var(--mantine-color-gray-4)}:where([data-mantine-color-scheme=dark]) .m_8a3dbb89{background-color:var(--mantine-color-dark-6);border-color:var(--mantine-color-dark-4)}.m_8a3dbb89:checked{background-color:var(--radio-color, var(--mantine-primary-color-filled));border-color:var(--radio-color, var(--mantine-primary-color-filled))}.m_8a3dbb89:checked+.m_f3ed6b2b{--radio-icon-opacity: 1;--radio-icon-transform: scale(1)}.m_8a3dbb89:disabled{cursor:not-allowed;background-color:var(--mantine-color-disabled);border-color:var(--mantine-color-disabled-border)}.m_8a3dbb89:disabled+.m_f3ed6b2b{--radio-icon-color: var(--mantine-color-disabled-color)}.m_8a3dbb89:where([data-error]){border-color:var(--mantine-color-error)}.m_1bfe9d39+.m_f3ed6b2b{--radio-icon-color: var(--radio-color)}.m_1bfe9d39:checked:not(:disabled){background-color:transparent;border-color:var(--radio-color)}.m_1bfe9d39:checked:not(:disabled)+.m_f3ed6b2b{--radio-icon-color: var(--radio-color);--radio-icon-opacity: 1;--radio-icon-transform: none}.m_f8d312f2{--rating-size-xs: calc(.875rem * var(--mantine-scale));--rating-size-sm: calc(1.125rem * var(--mantine-scale));--rating-size-md: calc(1.25rem * var(--mantine-scale));--rating-size-lg: calc(1.75rem * var(--mantine-scale));--rating-size-xl: calc(2rem * var(--mantine-scale));display:flex;width:max-content}.m_f8d312f2:where(:has(input:disabled)){pointer-events:none}.m_61734bb7{position:relative;transition:transform .1s ease}.m_61734bb7:where([data-active]){z-index:1;transform:scale(1.1)}.m_5662a89a{width:var(--rating-size);height:var(--rating-size);display:block}:where([data-mantine-color-scheme=light]) .m_5662a89a{fill:var(--mantine-color-gray-3);stroke:var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_5662a89a{fill:var(--mantine-color-dark-3);stroke:var(--mantine-color-dark-3)}.m_5662a89a:where([data-filled]){fill:var(--rating-color);stroke:var(--rating-color)}.m_211007ba{height:0;width:0;position:absolute;overflow:hidden;white-space:nowrap;opacity:0;-webkit-tap-highlight-color:transparent}.m_211007ba:focus-visible+label{outline:2px solid var(--mantine-primary-color-filled);outline-offset:calc(.125rem * var(--mantine-scale))}.m_21342ee4{display:block;cursor:pointer;position:absolute;top:0;left:0;z-index:var(--rating-item-z-index, 0);-webkit-tap-highlight-color:transparent}.m_21342ee4:where([data-read-only]){cursor:default}.m_21342ee4:where(:last-of-type){position:relative}.m_fae05d6a{clip-path:var(--rating-symbol-clip-path)}.m_1b3c8819{--tooltip-radius: var(--mantine-radius-default);position:absolute;padding:calc(var(--mantine-spacing-xs) / 2) var(--mantine-spacing-xs);pointer-events:none;font-size:var(--mantine-font-size-sm);white-space:nowrap;border-radius:var(--tooltip-radius)}:where([data-mantine-color-scheme=light]) .m_1b3c8819{background-color:var(--tooltip-bg, var(--mantine-color-gray-9));color:var(--tooltip-color, var(--mantine-color-white))}:where([data-mantine-color-scheme=dark]) .m_1b3c8819{background-color:var(--tooltip-bg, var(--mantine-color-gray-2));color:var(--tooltip-color, var(--mantine-color-black))}.m_1b3c8819:where([data-multiline]){white-space:normal}.m_1b3c8819:where([data-fixed]){position:fixed}.m_f898399f{background-color:inherit;border:0;z-index:1}.m_b32e4812{position:relative;width:var(--rp-size);height:var(--rp-size);min-width:var(--rp-size);min-height:var(--rp-size);--rp-transition-duration: 0ms}.m_d43b5134{width:var(--rp-size);height:var(--rp-size);min-width:var(--rp-size);min-height:var(--rp-size);transform:rotate(-90deg)}.m_b1ca1fbf{stroke:var(--curve-color, var(--rp-curve-root-color));transition:stroke-dashoffset var(--rp-transition-duration) ease,stroke-dasharray var(--rp-transition-duration) ease,stroke var(--rp-transition-duration)}[data-mantine-color-scheme=light] .m_b1ca1fbf{--rp-curve-root-color: var(--mantine-color-gray-2)}[data-mantine-color-scheme=dark] .m_b1ca1fbf{--rp-curve-root-color: var(--mantine-color-dark-4)}.m_b23f9dc4{position:absolute;top:50%;transform:translateY(-50%);inset-inline:var(--rp-label-offset)}.m_cf365364{--sc-padding-xs: calc(.125rem * var(--mantine-scale)) calc(.375rem * var(--mantine-scale));--sc-padding-sm: calc(.1875rem * var(--mantine-scale)) calc(.625rem * var(--mantine-scale));--sc-padding-md: calc(.25rem * var(--mantine-scale)) calc(.875rem * var(--mantine-scale));--sc-padding-lg: calc(.4375rem * var(--mantine-scale)) calc(1rem * var(--mantine-scale));--sc-padding-xl: calc(.625rem * var(--mantine-scale)) calc(1.25rem * var(--mantine-scale));--sc-transition-duration: .2s;--sc-padding: var(--sc-padding-sm);--sc-transition-timing-function: ease;--sc-font-size: var(--mantine-font-size-sm);position:relative;display:inline-flex;flex-direction:row;width:auto;border-radius:var(--sc-radius, var(--mantine-radius-default));overflow:hidden;padding:calc(.25rem * var(--mantine-scale))}.m_cf365364:where([data-full-width]){display:flex}.m_cf365364:where([data-orientation=vertical]){display:flex;flex-direction:column;width:max-content}.m_cf365364:where([data-orientation=vertical]):where([data-full-width]){width:auto}:where([data-mantine-color-scheme=light]) .m_cf365364{background-color:var(--mantine-color-gray-1)}:where([data-mantine-color-scheme=dark]) .m_cf365364{background-color:var(--mantine-color-dark-8)}.m_9e182ccd{position:absolute;display:block;z-index:1;border-radius:var(--sc-radius, var(--mantine-radius-default))}:where([data-mantine-color-scheme=light]) .m_9e182ccd{box-shadow:var(--sc-shadow, none);background-color:var(--sc-color, var(--mantine-color-white))}:where([data-mantine-color-scheme=dark]) .m_9e182ccd{box-shadow:none;background-color:var(--sc-color, var(--mantine-color-dark-5))}.m_1738fcb2{-webkit-tap-highlight-color:transparent;font-weight:500;display:block;text-align:center;white-space:nowrap;overflow:hidden;text-overflow:ellipsis;-webkit-user-select:none;user-select:none;border-radius:var(--sc-radius, var(--mantine-radius-default));font-size:var(--sc-font-size);padding:var(--sc-padding);transition:color var(--sc-transition-duration) var(--sc-transition-timing-function);cursor:pointer;outline:var(--segmented-control-outline, none)}:where([data-mantine-color-scheme=light]) .m_1738fcb2{color:var(--mantine-color-gray-7)}:where([data-mantine-color-scheme=dark]) .m_1738fcb2{color:var(--mantine-color-dark-1)}.m_1738fcb2:where([data-read-only]){cursor:default}fieldset:disabled .m_1738fcb2,.m_1738fcb2:where([data-disabled]){cursor:not-allowed;color:var(--mantine-color-disabled-color)}:where([data-mantine-color-scheme=light]) .m_1738fcb2:where([data-active]){color:var(--sc-label-color, var(--mantine-color-black))}:where([data-mantine-color-scheme=dark]) .m_1738fcb2:where([data-active]){color:var(--sc-label-color, var(--mantine-color-white))}.m_cf365364:where([data-initialized]) .m_1738fcb2:where([data-active]):before{display:none}.m_1738fcb2:where([data-active]):before{content:"";inset:0;z-index:0;position:absolute;border-radius:var(--sc-radius, var(--mantine-radius-default))}:where([data-mantine-color-scheme=light]) .m_1738fcb2:where([data-active]):before{box-shadow:var(--sc-shadow, none);background-color:var(--sc-color, var(--mantine-color-white))}:where([data-mantine-color-scheme=dark]) .m_1738fcb2:where([data-active]):before{box-shadow:none;background-color:var(--sc-color, var(--mantine-color-dark-5))}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_1738fcb2:where(:not([data-disabled],[data-active],[data-read-only])):hover{color:var(--mantine-color-black)}:where([data-mantine-color-scheme=dark]) .m_1738fcb2:where(:not([data-disabled],[data-active],[data-read-only])):hover{color:var(--mantine-color-white)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_1738fcb2:where(:not([data-disabled],[data-active],[data-read-only])):active{color:var(--mantine-color-black)}:where([data-mantine-color-scheme=dark]) .m_1738fcb2:where(:not([data-disabled],[data-active],[data-read-only])):active{color:var(--mantine-color-white)}}@media (hover: hover){fieldset:disabled .m_1738fcb2:hover{color:var(--mantine-color-disabled-color)!important}}@media (hover: none){fieldset:disabled .m_1738fcb2:active{color:var(--mantine-color-disabled-color)!important}}.m_1714d588{height:0;width:0;position:absolute;overflow:hidden;white-space:nowrap;opacity:0}.m_1714d588[data-focus-ring=auto]:focus:focus-visible+.m_1738fcb2{--segmented-control-outline: 2px solid var(--mantine-primary-color-filled)}.m_1714d588[data-focus-ring=always]:focus+.m_1738fcb2{--segmented-control-outline: 2px solid var(--mantine-primary-color-filled)}.m_69686b9b{position:relative;flex:1;z-index:2;transition:border-color var(--sc-transition-duration) var(--sc-transition-timing-function)}.m_cf365364[data-with-items-borders] :where(.m_69686b9b):before{content:"";position:absolute;top:0;bottom:0;inset-inline-start:0;background-color:var(--separator-color);width:calc(.0625rem * var(--mantine-scale));transition:background-color var(--sc-transition-duration) var(--sc-transition-timing-function)}.m_69686b9b[data-orientation=vertical]:before{top:0;inset-inline:0;bottom:auto;height:calc(.0625rem * var(--mantine-scale));width:auto}:where([data-mantine-color-scheme=light]) .m_69686b9b{--separator-color: var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_69686b9b{--separator-color: var(--mantine-color-dark-4)}.m_69686b9b:first-of-type:before{--separator-color: transparent}[data-mantine-color-scheme] .m_69686b9b[data-active]:before,[data-mantine-color-scheme] .m_69686b9b[data-active]+.m_69686b9b:before{--separator-color: transparent}.m_78882f40{position:relative;z-index:2}.m_fa528724{--scp-filled-segment-color: var(--mantine-primary-color-filled);--scp-transition-duration: 0ms;--scp-thickness: calc(.625rem * var(--mantine-scale))}:where([data-mantine-color-scheme=light]) .m_fa528724{--scp-empty-segment-color: var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_fa528724{--scp-empty-segment-color: var(--mantine-color-dark-4)}.m_fa528724{position:relative;width:fit-content}.m_62e9e7e2{display:block;transform:var(--scp-rotation);overflow:hidden}.m_c573fb6f{transition:stroke-dashoffset var(--scp-transition-duration) ease,stroke-dasharray var(--scp-transition-duration) ease,stroke var(--scp-transition-duration)}.m_4fa340f2{position:absolute;margin:0;padding:0;inset-inline:0;text-align:center;z-index:1}.m_4fa340f2:where([data-position=bottom]){bottom:0;padding-inline:calc(var(--scp-thickness) * 2)}.m_4fa340f2:where([data-position=bottom]):where([data-orientation=down]){bottom:auto;top:0}.m_4fa340f2:where([data-position=center]){top:50%;padding-inline:calc(var(--scp-thickness) * 3)}.m_925c2d2c{container:simple-grid / inline-size}.m_2415a157{display:grid;grid-template-columns:repeat(var(--sg-cols),minmax(0,1fr));gap:var(--sg-spacing-y) var(--sg-spacing-x)}@keyframes m_299c329c{0%,to{opacity:.4}50%{opacity:1}}.m_18320242{height:var(--skeleton-height, auto);width:var(--skeleton-width, 100%);border-radius:var(--skeleton-radius, var(--mantine-radius-default));position:relative;transform:translateZ(0);-webkit-transform:translateZ(0)}.m_18320242:where([data-animate]):after{animation:m_299c329c 1.5s linear infinite}.m_18320242:where([data-visible]){overflow:hidden}.m_18320242:where([data-visible]):before{position:absolute;content:"";inset:0;z-index:10;background-color:var(--mantine-color-body)}.m_18320242:where([data-visible]):after{position:absolute;content:"";inset:0;z-index:11}:where([data-mantine-color-scheme=light]) .m_18320242:where([data-visible]):after{background-color:var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_18320242:where([data-visible]):after{background-color:var(--mantine-color-dark-4)}.m_dd36362e{--slider-size-xs: calc(.25rem * var(--mantine-scale));--slider-size-sm: calc(.375rem * var(--mantine-scale));--slider-size-md: calc(.5rem * var(--mantine-scale));--slider-size-lg: calc(.625rem * var(--mantine-scale));--slider-size-xl: calc(.75rem * var(--mantine-scale));--slider-size: var(--slider-size-md);--slider-radius: calc(62.5rem * var(--mantine-scale));--slider-color: var(--mantine-primary-color-filled);--slider-track-disabled-bg: var(--mantine-color-disabled);-webkit-tap-highlight-color:transparent;outline:none;height:calc(var(--slider-size) * 2);padding-inline:var(--slider-size);display:flex;flex-direction:column;align-items:center;touch-action:none;position:relative}[data-mantine-color-scheme=light] .m_dd36362e{--slider-track-bg: var(--mantine-color-gray-2)}[data-mantine-color-scheme=dark] .m_dd36362e{--slider-track-bg: var(--mantine-color-dark-4)}.m_c9357328{position:absolute;top:calc(-2.25rem * var(--mantine-scale));font-size:var(--mantine-font-size-xs);color:var(--mantine-color-white);padding:calc(var(--mantine-spacing-xs) / 2);border-radius:var(--mantine-radius-sm);white-space:nowrap;pointer-events:none;-webkit-user-select:none;user-select:none;touch-action:none}:where([data-mantine-color-scheme=light]) .m_c9357328{background-color:var(--mantine-color-gray-9)}:where([data-mantine-color-scheme=dark]) .m_c9357328{background-color:var(--mantine-color-dark-4)}.m_c9a9a60a{position:absolute;display:flex;height:var(--slider-thumb-size);width:var(--slider-thumb-size);border:calc(.25rem * var(--mantine-scale)) solid;transform:translate(-50%,-50%);top:50%;cursor:pointer;border-radius:var(--slider-radius);align-items:center;justify-content:center;transition:box-shadow .1s ease,transform .1s ease;z-index:3;-webkit-user-select:none;user-select:none;touch-action:none;outline-offset:calc(.125rem * var(--mantine-scale));left:var(--slider-thumb-offset)}:where([dir=rtl]) .m_c9a9a60a{left:auto;right:calc(var(--slider-thumb-offset) - var(--slider-thumb-size))}fieldset:disabled .m_c9a9a60a,.m_c9a9a60a:where([data-disabled]){display:none}.m_c9a9a60a:where([data-dragging]){transform:translate(-50%,-50%) scale(1.05);box-shadow:var(--mantine-shadow-sm)}:where([data-mantine-color-scheme=light]) .m_c9a9a60a{color:var(--slider-color);border-color:var(--slider-color);background-color:var(--mantine-color-white)}:where([data-mantine-color-scheme=dark]) .m_c9a9a60a{color:var(--mantine-color-white);border-color:var(--mantine-color-white);background-color:var(--slider-color)}.m_a8645c2{display:flex;align-items:center;width:100%;height:calc(var(--slider-size) * 2);cursor:pointer}fieldset:disabled .m_a8645c2,.m_a8645c2:where([data-disabled]){cursor:not-allowed}.m_c9ade57f{position:relative;width:100%;height:var(--slider-size)}.m_c9ade57f:where([data-inverted]:not([data-disabled])){--track-bg: var(--slider-color)}fieldset:disabled .m_c9ade57f:where([data-inverted]),.m_c9ade57f:where([data-inverted][data-disabled]){--track-bg: var(--slider-track-disabled-bg)}.m_c9ade57f:before{content:"";position:absolute;top:0;bottom:0;border-radius:var(--slider-radius);inset-inline:calc(var(--slider-size) * -1);background-color:var(--track-bg, var(--slider-track-bg));z-index:0}.m_38aeed47{position:absolute;z-index:1;top:0;bottom:0;background-color:var(--slider-color);border-radius:var(--slider-radius);width:var(--slider-bar-width);inset-inline-start:var(--slider-bar-offset)}.m_38aeed47:where([data-inverted]){background-color:var(--slider-track-bg)}fieldset:disabled .m_38aeed47:where(:not([data-inverted])),.m_38aeed47:where([data-disabled]:not([data-inverted])){background-color:var(--mantine-color-disabled-color)}.m_b7b0423a{position:absolute;inset-inline-start:calc(var(--mark-offset) - var(--slider-size) / 2);top:0;z-index:2;height:0;pointer-events:none}.m_dd33bc19{border:calc(.125rem * var(--mantine-scale)) solid;height:var(--slider-size);width:var(--slider-size);border-radius:calc(62.5rem * var(--mantine-scale));background-color:var(--mantine-color-white);pointer-events:none}:where([data-mantine-color-scheme=light]) .m_dd33bc19{border-color:var(--mantine-color-gray-2)}:where([data-mantine-color-scheme=dark]) .m_dd33bc19{border-color:var(--mantine-color-dark-4)}.m_dd33bc19:where([data-filled]){border-color:var(--slider-color)}.m_dd33bc19:where([data-filled]):where([data-disabled]){border-color:var(--mantine-color-disabled-border)}.m_68c77a5b{transform:translate(calc(-50% + var(--slider-size) / 2),calc(var(--mantine-spacing-xs) / 2));font-size:var(--mantine-font-size-sm);white-space:nowrap;cursor:pointer;-webkit-user-select:none;user-select:none}:where([data-mantine-color-scheme=light]) .m_68c77a5b{color:var(--mantine-color-gray-6)}:where([data-mantine-color-scheme=dark]) .m_68c77a5b{color:var(--mantine-color-dark-2)}.m_559cce2d{position:relative}.m_559cce2d:where([data-has-spoiler]){margin-bottom:calc(1.5rem * var(--mantine-scale))}.m_b912df4e{display:flex;flex-direction:column;overflow:hidden;transition:max-height var(--spoiler-transition-duration, .2s) ease}.m_b9131032{position:absolute;inset-inline-start:0;top:100%;height:calc(1.5rem * var(--mantine-scale))}.m_6d731127{display:flex;flex-direction:column;align-items:var(--stack-align, stretch);justify-content:var(--stack-justify, flex-start);gap:var(--stack-gap, var(--mantine-spacing-md))}.m_cbb4ea7e{--stepper-icon-size-xs: calc(2.125rem * var(--mantine-scale));--stepper-icon-size-sm: calc(2.25rem * var(--mantine-scale));--stepper-icon-size-md: calc(2.625rem * var(--mantine-scale));--stepper-icon-size-lg: calc(3rem * var(--mantine-scale));--stepper-icon-size-xl: calc(3.25rem * var(--mantine-scale));--stepper-icon-size: var(--stepper-icon-size-md);--stepper-color: var(--mantine-primary-color-filled);--stepper-content-padding: var(--mantine-spacing-md);--stepper-spacing: var(--mantine-spacing-md);--stepper-radius: calc(62.5rem * var(--mantine-scale));--stepper-fz: var(--mantine-font-size-md);--stepper-outline-thickness: calc(.125rem * var(--mantine-scale))}[data-mantine-color-scheme=light] .m_cbb4ea7e{--stepper-outline-color: var(--mantine-color-gray-2)}[data-mantine-color-scheme=dark] .m_cbb4ea7e{--stepper-outline-color: var(--mantine-color-dark-5)}.m_aaf89d0b{display:flex;flex-wrap:nowrap;align-items:center}.m_aaf89d0b:where([data-wrap]){flex-wrap:wrap;gap:var(--mantine-spacing-md) 0}.m_aaf89d0b:where([data-orientation=vertical]){flex-direction:column}.m_aaf89d0b:where([data-orientation=vertical]):where([data-icon-position=left]){align-items:flex-start}.m_aaf89d0b:where([data-orientation=vertical]):where([data-icon-position=right]){align-items:flex-end}.m_aaf89d0b:where([data-orientation=horizontal]){flex-direction:row}.m_2a371ac9{transition:background-color .15s ease;flex:1;height:var(--stepper-outline-thickness);margin-inline:var(--mantine-spacing-md);background-color:var(--stepper-outline-color)}.m_2a371ac9:where([data-active]){background-color:var(--stepper-color)}.m_78da155d{padding-top:var(--stepper-content-padding)}.m_cbb57068{--step-color: var(--stepper-color);display:flex;cursor:default}.m_cbb57068:where([data-allow-click]){cursor:pointer}.m_cbb57068:where([data-icon-position=left]){flex-direction:row}.m_cbb57068:where([data-icon-position=right]){flex-direction:row-reverse}.m_f56b1e2c{align-items:center}.m_833edb7e{--separator-spacing: calc(var(--mantine-spacing-xs) / 2);justify-content:flex-start;min-height:calc(var(--stepper-icon-size) + var(--mantine-spacing-xl) + var(--separator-spacing));margin-top:var(--separator-spacing);overflow:hidden}.m_833edb7e:where(:first-of-type){margin-top:0}.m_833edb7e:where(:last-of-type){min-height:auto}.m_833edb7e:where(:last-of-type) .m_6496b3f3{display:none}.m_818e70b{position:relative}.m_6496b3f3{top:calc(var(--stepper-icon-size) + var(--separator-spacing));inset-inline-start:calc(var(--stepper-icon-size) / 2);height:100vh;position:absolute;border-inline-start:var(--stepper-outline-thickness) solid var(--stepper-outline-color)}.m_6496b3f3:where([data-active]){border-color:var(--stepper-color)}.m_1959ad01{height:var(--stepper-icon-size);width:var(--stepper-icon-size);min-height:var(--stepper-icon-size);min-width:var(--stepper-icon-size);border-radius:var(--stepper-radius);font-size:var(--stepper-fz);display:flex;align-items:center;justify-content:center;position:relative;font-weight:700;transition:background-color .15s ease,border-color .15s ease;border:var(--stepper-outline-thickness) solid var(--stepper-outline-color);background-color:var(--stepper-outline-color)}:where([data-mantine-color-scheme=light]) .m_1959ad01{color:var(--mantine-color-gray-7)}:where([data-mantine-color-scheme=dark]) .m_1959ad01{color:var(--mantine-color-dark-1)}.m_1959ad01:where([data-progress]){border-color:var(--step-color)}.m_1959ad01:where([data-completed]){color:var(--stepper-icon-color, var(--mantine-color-white));background-color:var(--step-color);border-color:var(--step-color)}.m_a79331dc{position:absolute;inset:0;display:flex;align-items:center;justify-content:center;color:var(--stepper-icon-color, var(--mantine-color-white))}.m_1956aa2a{display:flex;flex-direction:column}.m_1956aa2a:where([data-icon-position=left]){margin-inline-start:var(--mantine-spacing-sm)}.m_1956aa2a:where([data-icon-position=right]){text-align:right;margin-inline-end:var(--mantine-spacing-sm)}:where([dir=rtl]) .m_1956aa2a:where([data-icon-position=right]){text-align:left}.m_12051f6c{font-weight:500;font-size:var(--stepper-fz);line-height:1}.m_164eea74{margin-top:calc(var(--stepper-spacing) / 3);margin-bottom:calc(var(--stepper-spacing) / 3);font-size:calc(var(--stepper-fz) - calc(.125rem * var(--mantine-scale)));line-height:1;color:var(--mantine-color-dimmed)}.m_5f93f3bb{--switch-height-xs: calc(1rem * var(--mantine-scale));--switch-height-sm: calc(1.25rem * var(--mantine-scale));--switch-height-md: calc(1.5rem * var(--mantine-scale));--switch-height-lg: calc(1.875rem * var(--mantine-scale));--switch-height-xl: calc(2.25rem * var(--mantine-scale));--switch-width-xs: calc(2rem * var(--mantine-scale));--switch-width-sm: calc(2.375rem * var(--mantine-scale));--switch-width-md: calc(2.875rem * var(--mantine-scale));--switch-width-lg: calc(3.5rem * var(--mantine-scale));--switch-width-xl: calc(4.5rem * var(--mantine-scale));--switch-thumb-size-xs: calc(.75rem * var(--mantine-scale));--switch-thumb-size-sm: calc(.875rem * var(--mantine-scale));--switch-thumb-size-md: calc(1.125rem * var(--mantine-scale));--switch-thumb-size-lg: calc(1.375rem * var(--mantine-scale));--switch-thumb-size-xl: calc(1.75rem * var(--mantine-scale));--switch-label-font-size-xs: calc(.3125rem * var(--mantine-scale));--switch-label-font-size-sm: calc(.375rem * var(--mantine-scale));--switch-label-font-size-md: calc(.4375rem * var(--mantine-scale));--switch-label-font-size-lg: calc(.5625rem * var(--mantine-scale));--switch-label-font-size-xl: calc(.6875rem * var(--mantine-scale));--switch-track-label-padding-xs: calc(.125rem * var(--mantine-scale));--switch-track-label-padding-sm: calc(.15625rem * var(--mantine-scale));--switch-track-label-padding-md: calc(.1875rem * var(--mantine-scale));--switch-track-label-padding-lg: calc(.1875rem * var(--mantine-scale));--switch-track-label-padding-xl: calc(.21875rem * var(--mantine-scale));--switch-height: var(--switch-height-sm);--switch-width: var(--switch-width-sm);--switch-thumb-size: var(--switch-thumb-size-sm);--switch-label-font-size: var(--switch-label-font-size-sm);--switch-track-label-padding: var(--switch-track-label-padding-sm);--switch-radius: calc(62.5rem * var(--mantine-scale));--switch-color: var(--mantine-primary-color-filled);--switch-disabled-color: var(--mantine-color-disabled);position:relative}.m_926b4011{height:0;width:0;opacity:0;margin:0;padding:0;position:absolute;overflow:hidden;white-space:nowrap}.m_9307d992{-webkit-tap-highlight-color:transparent;cursor:var(--switch-cursor, var(--mantine-cursor-type));overflow:hidden;position:relative;border-radius:var(--switch-radius);background-color:var(--switch-bg);height:var(--switch-height);min-width:var(--switch-width);margin:0;transition:background-color .15s ease,border-color .15s ease;appearance:none;display:flex;align-items:center;font-size:var(--switch-label-font-size);font-weight:600;order:var(--switch-order, 1);-webkit-user-select:none;user-select:none;z-index:0;line-height:0;color:var(--switch-text-color)}.m_9307d992:where([data-without-labels]){width:var(--switch-width)}.m_926b4011:focus-visible+.m_9307d992{outline:2px solid var(--mantine-primary-color-filled);outline-offset:calc(.125rem * var(--mantine-scale))}.m_926b4011:checked+.m_9307d992{--switch-bg: var(--switch-color);--switch-text-color: var(--mantine-color-white)}.m_926b4011:disabled+.m_9307d992,.m_926b4011[data-disabled]+.m_9307d992{--switch-bg: var(--switch-disabled-color);--switch-cursor: not-allowed}[data-mantine-color-scheme=light] .m_9307d992{--switch-bg: var(--mantine-color-gray-3);--switch-text-color: var(--mantine-color-gray-6)}[data-mantine-color-scheme=dark] .m_9307d992{--switch-bg: var(--mantine-color-dark-5);--switch-text-color: var(--mantine-color-dark-1)}.m_9307d992[data-label-position=left]{--switch-order: 2}.m_93039a1d{position:absolute;z-index:1;border-radius:var(--switch-radius);display:flex;background-color:var(--switch-thumb-bg, var(--mantine-color-white));height:var(--switch-thumb-size);width:var(--switch-thumb-size);inset-inline-start:var(--switch-thumb-start, var(--switch-track-label-padding));transition:inset-inline-start .15s ease}.m_93039a1d:where([data-with-thumb-indicator]):before{content:"";width:40%;height:40%;background-color:var(--switch-bg);position:absolute;border-radius:var(--switch-radius);top:50%;left:50%;transform:translate(-50%,-50%)}.m_93039a1d>*{margin:auto}.m_926b4011:checked+*>.m_93039a1d{--switch-thumb-start: calc(100% - var(--switch-thumb-size) - var(--switch-track-label-padding))}.m_926b4011:disabled+*>.m_93039a1d,.m_926b4011[data-disabled]+*>.m_93039a1d{--switch-thumb-bg: var(--switch-thumb-bg-disabled)}[data-mantine-color-scheme=light] .m_93039a1d{--switch-thumb-bg-disabled: var(--mantine-color-gray-0)}[data-mantine-color-scheme=dark] .m_93039a1d{--switch-thumb-bg-disabled: var(--mantine-color-dark-3)}.m_8277e082{height:100%;display:grid;place-content:center;min-width:calc(var(--switch-width) - var(--switch-thumb-size));padding-inline:var(--switch-track-label-padding);margin-inline-start:calc(var(--switch-thumb-size) + var(--switch-track-label-padding));transition:margin .15s ease}.m_926b4011:checked+*>.m_8277e082{margin-inline-end:calc(var(--switch-thumb-size) + var(--switch-track-label-padding));margin-inline-start:0}.m_b23fa0ef{width:100%;border-collapse:collapse;border-spacing:0;line-height:var(--mantine-line-height);font-size:var(--mantine-font-size-sm);table-layout:var(--table-layout, auto);caption-side:var(--table-caption-side, bottom);border:none}:where([data-mantine-color-scheme=light]) .m_b23fa0ef{--table-hover-color: var(--mantine-color-gray-1);--table-striped-color: var(--mantine-color-gray-0);--table-border-color: var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_b23fa0ef{--table-hover-color: var(--mantine-color-dark-5);--table-striped-color: var(--mantine-color-dark-6);--table-border-color: var(--mantine-color-dark-4)}.m_b23fa0ef:where([data-with-table-border]){border:calc(.0625rem * var(--mantine-scale)) solid var(--table-border-color)}.m_b23fa0ef:where([data-tabular-nums]){font-variant-numeric:tabular-nums}.m_b23fa0ef:where([data-variant=vertical]) :where(.m_4e7aa4f3){font-weight:500}:where([data-mantine-color-scheme=light]) .m_b23fa0ef:where([data-variant=vertical]) :where(.m_4e7aa4f3){background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_b23fa0ef:where([data-variant=vertical]) :where(.m_4e7aa4f3){background-color:var(--mantine-color-dark-6)}.m_4e7aa4f3{text-align:left}:where([dir=rtl]) .m_4e7aa4f3{text-align:right}.m_4e7aa4fd{border-bottom:none;background-color:transparent}@media (hover: hover){.m_4e7aa4fd:hover:where([data-hover]){background-color:var(--tr-hover-bg)}}@media (hover: none){.m_4e7aa4fd:active:where([data-hover]){background-color:var(--tr-hover-bg)}}.m_4e7aa4fd:where([data-with-row-border]){border-bottom:calc(.0625rem * var(--mantine-scale)) solid var(--table-border-color)}.m_4e7aa4ef,.m_4e7aa4f3{padding:var(--table-vertical-spacing) var(--table-horizontal-spacing, var(--mantine-spacing-xs))}.m_4e7aa4ef:where([data-with-column-border]:not(:first-child)),.m_4e7aa4f3:where([data-with-column-border]:not(:first-child)){border-inline-start:calc(.0625rem * var(--mantine-scale)) solid var(--table-border-color)}.m_4e7aa4ef:where([data-with-column-border]:not(:last-child)),.m_4e7aa4f3:where([data-with-column-border]:not(:last-child)){border-inline-end:calc(.0625rem * var(--mantine-scale)) solid var(--table-border-color)}.m_b2404537>:where(tr):where([data-with-row-border]:last-of-type){border-bottom:none}.m_b2404537>:where(tr):where([data-striped=odd]:nth-of-type(odd)){background-color:var(--table-striped-color)}.m_b2404537>:where(tr):where([data-striped=even]:nth-of-type(2n)){background-color:var(--table-striped-color)}.m_b2404537>:where(tr)[data-hover]{--tr-hover-bg: var(--table-highlight-on-hover-color, var(--table-hover-color))}.m_b242d975{top:var(--table-sticky-header-offset, 0);z-index:3}.m_b242d975:where([data-sticky]){position:sticky}.m_b242d975:where([data-sticky]) :where(.m_4e7aa4f3){position:sticky;top:var(--table-sticky-header-offset, 0);background-color:var(--mantine-color-body)}:where([data-with-table-border]) .m_b242d975[data-sticky]{position:sticky;top:var(--table-sticky-header-offset, 0);z-index:4;border-top:none}:where([data-with-table-border]) .m_b242d975[data-sticky]:before{content:"";display:block;position:absolute;left:0;top:calc(-.03125rem * var(--mantine-scale));width:100%;height:calc(.0625rem * var(--mantine-scale));background-color:var(--table-border-color);z-index:5}:where([data-with-table-border]) .m_b242d975[data-sticky] .m_4e7aa4f3:first-child{border-top:none}.m_9e5a3ac7{color:var(--mantine-color-dimmed)}.m_9e5a3ac7:where([data-side=top]){margin-bottom:var(--mantine-spacing-xs)}.m_9e5a3ac7:where([data-side=bottom]){margin-top:var(--mantine-spacing-xs)}.m_a100c15{overflow-x:var(--table-overflow)}.m_62259741{min-width:var(--table-min-width);max-height:var(--table-max-height)}.m_bcaa9990{display:flex;flex-direction:column;--toc-depth-offset: .8em}.m_375a65ef{display:block;padding:.3em .8em;font-size:var(--toc-size, var(--mantine-font-size-md));border-radius:var(--toc-radius, var(--mantine-radius-default));padding-left:max(calc(var(--depth-offset) * var(--toc-depth-offset)),.8em)}@media (hover: hover){:where([data-mantine-color-scheme=light]) .m_375a65ef:where(:hover):where(:not([data-variant=none])){background-color:var(--mantine-color-gray-1)}:where([data-mantine-color-scheme=dark]) .m_375a65ef:where(:hover):where(:not([data-variant=none])){background-color:var(--mantine-color-dark-5)}}@media (hover: none){:where([data-mantine-color-scheme=light]) .m_375a65ef:where(:active):where(:not([data-variant=none])){background-color:var(--mantine-color-gray-1)}:where([data-mantine-color-scheme=dark]) .m_375a65ef:where(:active):where(:not([data-variant=none])){background-color:var(--mantine-color-dark-5)}}.m_375a65ef:where([data-active]){background-color:var(--toc-bg);color:var(--toc-color)}[data-mantine-color-scheme=light] .m_89d60db1{--tab-border-color: var(--mantine-color-gray-3)}[data-mantine-color-scheme=dark] .m_89d60db1{--tab-border-color: var(--mantine-color-dark-4)}.m_89d60db1{display:var(--tabs-display);flex-direction:var(--tabs-flex-direction);--tabs-list-direction: row;--tabs-panel-grow: unset;--tabs-display: block;--tabs-flex-direction: row;--tabs-list-border-width: 0;--tabs-list-border-size: 0 0 var(--tabs-list-border-width) 0;--tabs-list-gap: unset;--tabs-list-line-bottom: 0;--tabs-list-line-top: unset;--tabs-list-line-start: 0;--tabs-list-line-end: 0;--tab-radius: var(--tabs-radius) var(--tabs-radius) 0 0;--tab-border-width: 0 0 var(--tabs-list-border-width) 0}.m_89d60db1[data-inverted]{--tabs-list-line-bottom: unset;--tabs-list-line-top: 0;--tab-radius: 0 0 var(--tabs-radius) var(--tabs-radius);--tab-border-width: var(--tabs-list-border-width) 0 0 0}.m_89d60db1[data-inverted] .m_576c9d4:before{top:0;bottom:unset}.m_89d60db1[data-orientation=vertical]{--tabs-list-line-start: unset;--tabs-list-line-end: 0;--tabs-list-line-top: 0;--tabs-list-line-bottom: 0;--tabs-list-border-size: 0 var(--tabs-list-border-width) 0 0;--tab-border-width: 0 var(--tabs-list-border-width) 0 0;--tab-radius: var(--tabs-radius) 0 0 var(--tabs-radius);--tabs-list-direction: column;--tabs-panel-grow: 1;--tabs-display: flex}[dir=rtl] .m_89d60db1[data-orientation=vertical]{--tabs-list-border-size: 0 0 0 var(--tabs-list-border-width);--tab-border-width: 0 0 0 var(--tabs-list-border-width);--tab-radius: 0 var(--tabs-radius) var(--tabs-radius) 0}.m_89d60db1[data-orientation=vertical][data-placement=right]{--tabs-flex-direction: row-reverse;--tabs-list-line-start: 0;--tabs-list-line-end: unset;--tabs-list-border-size: 0 0 0 var(--tabs-list-border-width);--tab-border-width: 0 0 0 var(--tabs-list-border-width);--tab-radius: 0 var(--tabs-radius) var(--tabs-radius) 0}[dir=rtl] .m_89d60db1[data-orientation=vertical][data-placement=right]{--tabs-list-border-size: 0 var(--tabs-list-border-width) 0 0;--tab-border-width: 0 var(--tabs-list-border-width) 0 0;--tab-radius: var(--tabs-radius) 0 0 var(--tabs-radius)}.m_89d60db1[data-variant=default]{--tabs-list-border-width: calc(.125rem * var(--mantine-scale))}[data-mantine-color-scheme=light] .m_89d60db1[data-variant=default]{--tab-hover-color: var(--mantine-color-gray-0)}[data-mantine-color-scheme=dark] .m_89d60db1[data-variant=default]{--tab-hover-color: var(--mantine-color-dark-6)}.m_89d60db1[data-variant=outline]{--tabs-list-border-width: calc(.0625rem * var(--mantine-scale))}.m_89d60db1[data-variant=pills]{--tabs-list-gap: calc(var(--mantine-spacing-sm) / 2)}[data-mantine-color-scheme=light] .m_89d60db1[data-variant=pills]{--tab-hover-color: var(--mantine-color-gray-0)}[data-mantine-color-scheme=dark] .m_89d60db1[data-variant=pills]{--tab-hover-color: var(--mantine-color-dark-6)}.m_89d33d6d{display:flex;flex-wrap:wrap;justify-content:var(--tabs-justify, flex-start);flex-direction:var(--tabs-list-direction);gap:var(--tabs-list-gap)}.m_89d33d6d:where([data-grow]) .m_4ec4dce6{flex:1}.m_b0c91715{flex-grow:var(--tabs-panel-grow)}.m_4ec4dce6{position:relative;padding:var(--mantine-spacing-xs) var(--mantine-spacing-md);font-size:var(--mantine-font-size-sm);white-space:nowrap;z-index:0;display:flex;align-items:center;line-height:1;-webkit-user-select:none;user-select:none}.m_4ec4dce6:where(:disabled,[data-disabled]){opacity:.5;cursor:not-allowed}.m_4ec4dce6:focus{z-index:1}.m_fc420b1f{display:flex;align-items:center;justify-content:center}.m_fc420b1f:where([data-position=left]:not(:only-child)){margin-inline-end:var(--mantine-spacing-xs)}.m_fc420b1f:where([data-position=right]:not(:only-child)){margin-inline-start:var(--mantine-spacing-xs)}.m_42bbd1ae{flex:1;text-align:center}.m_576c9d4{position:relative}.m_576c9d4:before{content:"";position:absolute;border:1px solid var(--tab-border-color);bottom:var(--tabs-list-line-bottom);inset-inline-start:var(--tabs-list-line-start);inset-inline-end:var(--tabs-list-line-end);top:var(--tabs-list-line-top)}.m_539e827b{border-radius:var(--tab-radius);border-width:var(--tab-border-width);border-style:solid;border-color:transparent;background-color:transparent}.m_539e827b:where([data-active]){border-color:var(--tabs-color)}@media (hover: hover){.m_539e827b:hover{background-color:var(--tab-hover-color)}.m_539e827b:hover:where(:not([data-active])){border-color:var(--tab-border-color)}}@media (hover: none){.m_539e827b:active{background-color:var(--tab-hover-color)}.m_539e827b:active:where(:not([data-active])){border-color:var(--tab-border-color)}}@media (hover: hover){.m_539e827b:disabled:hover,.m_539e827b[data-disabled]:hover{background-color:transparent}}@media (hover: none){.m_539e827b:disabled:active,.m_539e827b[data-disabled]:active{background-color:transparent}}.m_6772fbd5{position:relative}.m_6772fbd5:before{content:"";position:absolute;border-color:var(--tab-border-color);border-width:var(--tabs-list-border-size);border-style:solid;bottom:var(--tabs-list-line-bottom);inset-inline-start:var(--tabs-list-line-start);inset-inline-end:var(--tabs-list-line-end);top:var(--tabs-list-line-top)}.m_b59ab47c{border-top:calc(.0625rem * var(--mantine-scale)) solid transparent;border-bottom:calc(.0625rem * var(--mantine-scale)) solid transparent;border-right:calc(.0625rem * var(--mantine-scale)) solid transparent;border-left:calc(.0625rem * var(--mantine-scale)) solid transparent;border-top-color:var(--tab-border-top-color);border-bottom-color:var(--tab-border-bottom-color);border-radius:var(--tab-radius);position:relative;--tab-border-bottom-color: transparent;--tab-border-top-color: transparent;--tab-border-inline-end-color: transparent;--tab-border-inline-start-color: transparent}.m_b59ab47c:where([data-active]):before{content:"";position:absolute;background-color:var(--tab-border-color);bottom:var(--tab-before-bottom, calc(-.0625rem * var(--mantine-scale)));left:var(--tab-before-left, calc(-.0625rem * var(--mantine-scale)));right:var(--tab-before-right, auto);top:var(--tab-before-top, auto);width:calc(.0625rem * var(--mantine-scale));height:calc(.0625rem * var(--mantine-scale))}.m_b59ab47c:where([data-active]):after{content:"";position:absolute;background-color:var(--tab-border-color);bottom:var(--tab-after-bottom, calc(-.0625rem * var(--mantine-scale)));right:var(--tab-after-right, calc(-.0625rem * var(--mantine-scale)));left:var(--tab-after-left, auto);top:var(--tab-after-top, auto);width:calc(.0625rem * var(--mantine-scale));height:calc(.0625rem * var(--mantine-scale))}.m_b59ab47c:where([data-active]){border-top-color:var(--tab-border-top-color);border-bottom-color:var(--tab-border-bottom-color);border-inline-start-color:var(--tab-border-inline-start-color);border-inline-end-color:var(--tab-border-inline-end-color);--tab-border-top-color: var(--tab-border-color);--tab-border-inline-start-color: var(--tab-border-color);--tab-border-inline-end-color: var(--tab-border-color);--tab-border-bottom-color: var(--mantine-color-body)}.m_b59ab47c:where([data-active])[data-inverted]{--tab-border-bottom-color: var(--tab-border-color);--tab-border-top-color: var(--mantine-color-body);--tab-before-bottom: auto;--tab-before-top: calc(-.0625rem * var(--mantine-scale));--tab-after-bottom: auto;--tab-after-top: calc(-.0625rem * var(--mantine-scale))}.m_b59ab47c:where([data-active])[data-orientation=vertical][data-placement=left]{--tab-border-inline-end-color: var(--mantine-color-body);--tab-border-inline-start-color: var(--tab-border-color);--tab-border-bottom-color: var(--tab-border-color);--tab-before-right: calc(-.0625rem * var(--mantine-scale));--tab-before-left: auto;--tab-before-bottom: auto;--tab-before-top: calc(-.0625rem * var(--mantine-scale));--tab-after-left: auto;--tab-after-right: calc(-.0625rem * var(--mantine-scale))}[dir=rtl] .m_b59ab47c:where([data-active])[data-orientation=vertical][data-placement=left]{--tab-before-right: auto;--tab-before-left: calc(-.0625rem * var(--mantine-scale));--tab-after-left: calc(-.0625rem * var(--mantine-scale));--tab-after-right: auto}.m_b59ab47c:where([data-active])[data-orientation=vertical][data-placement=right]{--tab-border-inline-start-color: var(--mantine-color-body);--tab-border-inline-end-color: var(--tab-border-color);--tab-border-bottom-color: var(--tab-border-color);--tab-before-left: calc(-.0625rem * var(--mantine-scale));--tab-before-right: auto;--tab-before-bottom: auto;--tab-before-top: calc(-.0625rem * var(--mantine-scale));--tab-after-right: auto;--tab-after-left: calc(-.0625rem * var(--mantine-scale))}[dir=rtl] .m_b59ab47c:where([data-active])[data-orientation=vertical][data-placement=right]{--tab-before-left: auto;--tab-before-right: calc(-.0625rem * var(--mantine-scale));--tab-after-right: calc(-.0625rem * var(--mantine-scale));--tab-after-left: auto}.m_c3381914{border-radius:var(--tabs-radius);background-color:var(--tab-bg);color:var(--tab-color);--tab-bg: transparent;--tab-color: inherit}@media (hover: hover){.m_c3381914:not([data-disabled]):hover{--tab-bg: var(--tab-hover-color)}}@media (hover: none){.m_c3381914:not([data-disabled]):active{--tab-bg: var(--tab-hover-color)}}.m_c3381914[data-active][data-active]{--tab-bg: var(--tabs-color);--tab-color: var(--tabs-text-color, var(--mantine-color-white))}@media (hover: hover){.m_c3381914[data-active][data-active]:hover{--tab-bg: var(--tabs-color)}}@media (hover: none){.m_c3381914[data-active][data-active]:active{--tab-bg: var(--tabs-color)}}.m_7341320d{--ti-size-xs: calc(1.125rem * var(--mantine-scale));--ti-size-sm: calc(1.375rem * var(--mantine-scale));--ti-size-md: calc(1.75rem * var(--mantine-scale));--ti-size-lg: calc(2.125rem * var(--mantine-scale));--ti-size-xl: calc(2.75rem * var(--mantine-scale));--ti-size: var(--ti-size-md);line-height:1;display:inline-flex;align-items:center;justify-content:center;position:relative;-webkit-user-select:none;user-select:none;width:var(--ti-size);height:var(--ti-size);min-width:var(--ti-size);min-height:var(--ti-size);border-radius:var(--ti-radius, var(--mantine-radius-default));background:var(--ti-bg, var(--mantine-primary-color-filled));color:var(--ti-color, var(--mantine-color-white));border:var(--ti-bd, 1px solid transparent)}.m_43657ece{--offset: calc(var(--tl-bullet-size) / 2 + var(--tl-line-width) / 2);--tl-bullet-size: calc(1.25rem * var(--mantine-scale));--tl-line-width: calc(.25rem * var(--mantine-scale));--tl-radius: calc(62.5rem * var(--mantine-scale));--tl-color: var(--mantine-primary-color-filled)}.m_43657ece:where([data-align=left]){padding-inline-start:var(--offset)}.m_43657ece:where([data-align=right]){padding-inline-end:var(--offset)}.m_2ebe8099{font-weight:500;line-height:1;margin-bottom:calc(var(--mantine-spacing-xs) / 2)}.m_436178ff{--item-border: var(--tl-line-width) var(--tli-border-style, solid) var(--item-border-color);position:relative;color:var(--mantine-color-text)}.m_436178ff:before{content:"";pointer-events:none;position:absolute;top:0;left:var(--timeline-line-left, 0);right:var(--timeline-line-right, 0);bottom:calc(var(--mantine-spacing-xl) * -1);border-inline-start:var(--item-border);display:var(--timeline-line-display, none)}.m_43657ece[data-align=left] .m_436178ff:before{--timeline-line-left: calc(var(--tl-line-width) * -1);--timeline-line-right: auto}[dir=rtl] .m_43657ece[data-align=left] .m_436178ff:before{--timeline-line-left: auto;--timeline-line-right: calc(var(--tl-line-width) * -1)}.m_43657ece[data-align=right] .m_436178ff:before{--timeline-line-left: auto;--timeline-line-right: calc(var(--tl-line-width) * -1)}[dir=rtl] .m_43657ece[data-align=right] .m_436178ff:before{--timeline-line-left: calc(var(--tl-line-width) * -1);--timeline-line-right: auto}.m_43657ece:where([data-align=left]) .m_436178ff{padding-inline-start:var(--offset);text-align:left}.m_43657ece:where([data-align=right]) .m_436178ff{padding-inline-end:var(--offset);text-align:right}:where([data-mantine-color-scheme=light]) .m_436178ff{--item-border-color: var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_436178ff{--item-border-color: var(--mantine-color-dark-4)}.m_436178ff:where([data-line-active]):before{border-color:var(--tli-color, var(--tl-color))}.m_436178ff:where(:not(:last-of-type)){--timeline-line-display: block}.m_436178ff:where(:not(:first-of-type)){margin-top:var(--mantine-spacing-xl)}.m_8affcee1{width:var(--tl-bullet-size);height:var(--tl-bullet-size);border-radius:var(--tli-radius, var(--tl-radius));border:var(--tl-line-width) solid;background-color:var(--mantine-color-body);position:absolute;top:0;display:flex;align-items:center;justify-content:center;color:var(--mantine-color-text)}:where([data-mantine-color-scheme=light]) .m_8affcee1{border-color:var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_8affcee1{border-color:var(--mantine-color-dark-4)}.m_43657ece:where([data-align=left]) .m_8affcee1{left:calc((var(--tl-bullet-size) / 2 + var(--tl-line-width) / 2) * -1);right:auto}:where([dir=rtl]) .m_43657ece:where([data-align=left]) .m_8affcee1{left:auto;right:calc((var(--tl-bullet-size) / 2 + var(--tl-line-width) / 2) * -1)}.m_43657ece:where([data-align=right]) .m_8affcee1{left:auto;right:calc((var(--tl-bullet-size) / 2 + var(--tl-line-width) / 2) * -1)}:where([dir=rtl]) .m_43657ece:where([data-align=right]) .m_8affcee1{left:calc((var(--tl-bullet-size) / 2 + var(--tl-line-width) / 2) * -1);right:auto}.m_8affcee1:where([data-with-child]){border-width:var(--tl-line-width)}:where([data-mantine-color-scheme=light]) .m_8affcee1:where([data-with-child]){background-color:var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_8affcee1:where([data-with-child]){background-color:var(--mantine-color-dark-4)}.m_8affcee1:where([data-active]){border-color:var(--tli-color, var(--tl-color));background-color:var(--mantine-color-white);color:var(--tl-icon-color, var(--mantine-color-white))}.m_8affcee1:where([data-active]):where([data-with-child]){background-color:var(--tli-color, var(--tl-color));color:var(--tl-icon-color, var(--mantine-color-white))}.m_43657ece:where([data-align=left]) .m_540e8f41{padding-inline-start:var(--offset);text-align:left}:where([dir=rtl]) .m_43657ece:where([data-align=left]) .m_540e8f41{text-align:right}.m_43657ece:where([data-align=right]) .m_540e8f41{padding-inline-end:var(--offset);text-align:right}:where([dir=rtl]) .m_43657ece:where([data-align=right]) .m_540e8f41{text-align:left}.m_8a5d1357{margin:0;font-weight:var(--title-fw);font-size:var(--title-fz);line-height:var(--title-lh);font-family:var(--mantine-font-family-headings);text-wrap:var(--title-text-wrap, var(--mantine-heading-text-wrap))}.m_8a5d1357:where([data-line-clamp]){overflow:hidden;text-overflow:ellipsis;display:-webkit-box;-webkit-line-clamp:var(--title-line-clamp);-webkit-box-orient:vertical}.m_f698e191{--level-offset: var(--mantine-spacing-lg);margin:0;padding:0;-webkit-user-select:none;user-select:none}.m_75f3ecf{margin:0;padding:0}.m_f6970eb1{cursor:pointer;list-style:none;margin:0;padding:0;outline:0}.m_f6970eb1:focus-visible>.m_dc283425{outline:2px solid var(--mantine-primary-color-filled);outline-offset:calc(.125rem * var(--mantine-scale))}.m_dc283425{padding-inline-start:var(--label-offset)}:where([data-mantine-color-scheme=light]) .m_dc283425:where([data-selected]){background-color:var(--mantine-color-gray-1)}:where([data-mantine-color-scheme=dark]) .m_dc283425:where([data-selected]){background-color:var(--mantine-color-dark-5)}.m_d08caa0 :first-child{margin-top:0}.m_d08caa0 :last-child{margin-bottom:0}.m_d08caa0 :where(h1,h2,h3,h4,h5,h6){margin-bottom:var(--mantine-spacing-xs);text-wrap:var(--mantine-heading-text-wrap);font-family:var(--mantine-font-family-headings)}.m_d08caa0 :where(h1){margin-top:calc(1.5 * var(--mantine-spacing-xl));font-size:var(--mantine-h1-font-size);line-height:var(--mantine-h1-line-height);font-weight:var(--mantine-h1-font-weight)}.m_d08caa0 :where(h2){margin-top:var(--mantine-spacing-xl);font-size:var(--mantine-h2-font-size);line-height:var(--mantine-h2-line-height);font-weight:var(--mantine-h2-font-weight)}.m_d08caa0 :where(h3){margin-top:calc(.8 * var(--mantine-spacing-xl));font-size:var(--mantine-h3-font-size);line-height:var(--mantine-h3-line-height);font-weight:var(--mantine-h3-font-weight)}.m_d08caa0 :where(h4){margin-top:calc(.8 * var(--mantine-spacing-xl));font-size:var(--mantine-h4-font-size);line-height:var(--mantine-h4-line-height);font-weight:var(--mantine-h4-font-weight)}.m_d08caa0 :where(h5){margin-top:calc(.5 * var(--mantine-spacing-xl));font-size:var(--mantine-h5-font-size);line-height:var(--mantine-h5-line-height);font-weight:var(--mantine-h5-font-weight)}.m_d08caa0 :where(h6){margin-top:calc(.5 * var(--mantine-spacing-xl));font-size:var(--mantine-h6-font-size);line-height:var(--mantine-h6-line-height);font-weight:var(--mantine-h6-font-weight)}.m_d08caa0 :where(img){max-width:100%;margin-bottom:var(--mantine-spacing-xs)}.m_d08caa0 :where(p){margin-top:0;margin-bottom:var(--mantine-spacing-lg)}:where([data-mantine-color-scheme=light]) .m_d08caa0 :where(mark){background-color:var(--mantine-color-yellow-2);color:inherit}:where([data-mantine-color-scheme=dark]) .m_d08caa0 :where(mark){background-color:var(--mantine-color-yellow-5);color:var(--mantine-color-black)}.m_d08caa0 :where(a){color:var(--mantine-color-anchor);text-decoration:none}@media (hover: hover){.m_d08caa0 :where(a):hover{text-decoration:underline}}@media (hover: none){.m_d08caa0 :where(a):active{text-decoration:underline}}.m_d08caa0 :where(hr){margin-top:var(--mantine-spacing-md);margin-bottom:var(--mantine-spacing-md);border:0;border-top:calc(.0625rem * var(--mantine-scale)) solid}:where([data-mantine-color-scheme=light]) .m_d08caa0 :where(hr){border-color:var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_d08caa0 :where(hr){border-color:var(--mantine-color-dark-3)}.m_d08caa0 :where(pre){padding:var(--mantine-spacing-xs);line-height:var(--mantine-line-height);margin:0;margin-top:var(--mantine-spacing-md);margin-bottom:var(--mantine-spacing-md);overflow-x:auto;font-family:var(--mantine-font-family-monospace);font-size:var(--mantine-font-size-xs);border-radius:var(--mantine-radius-sm)}:where([data-mantine-color-scheme=light]) .m_d08caa0 :where(pre){background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_d08caa0 :where(pre){background-color:var(--mantine-color-dark-8)}.m_d08caa0 :where(pre) :where(code){background-color:transparent;padding:0;border-radius:0;color:inherit;border:0}.m_d08caa0 :where(kbd){--kbd-fz: calc(.75rem * var(--mantine-scale));--kbd-padding: calc(.1875rem * var(--mantine-scale)) calc(.3125rem * var(--mantine-scale));font-family:var(--mantine-font-family-monospace);line-height:var(--mantine-line-height);font-weight:700;padding:var(--kbd-padding);font-size:var(--kbd-fz);border-radius:var(--mantine-radius-sm);border:calc(.0625rem * var(--mantine-scale)) solid;border-bottom-width:calc(.1875rem * var(--mantine-scale))}:where([data-mantine-color-scheme=light]) .m_d08caa0 :where(kbd){border-color:var(--mantine-color-gray-3);color:var(--mantine-color-gray-7);background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_d08caa0 :where(kbd){border-color:var(--mantine-color-dark-3);color:var(--mantine-color-dark-0);background-color:var(--mantine-color-dark-5)}.m_d08caa0 :where(code){line-height:var(--mantine-line-height);padding:calc(.0625rem * var(--mantine-scale)) calc(.3125rem * var(--mantine-scale));border-radius:var(--mantine-radius-sm);font-family:var(--mantine-font-family-monospace);font-size:var(--mantine-font-size-xs)}:where([data-mantine-color-scheme=light]) .m_d08caa0 :where(code){background-color:var(--mantine-color-gray-0);color:var(--mantine-color-black)}:where([data-mantine-color-scheme=dark]) .m_d08caa0 :where(code){background-color:var(--mantine-color-dark-5);color:var(--mantine-color-white)}.m_d08caa0 :where(ul,ol):not([data-type=taskList]){margin-bottom:var(--mantine-spacing-md);padding-inline-start:var(--mantine-spacing-xl);list-style-position:outside}.m_d08caa0 :where(table){width:100%;border-collapse:collapse;caption-side:bottom;margin-bottom:var(--mantine-spacing-md)}:where([data-mantine-color-scheme=light]) .m_d08caa0 :where(table){--table-border-color: var(--mantine-color-gray-3)}:where([data-mantine-color-scheme=dark]) .m_d08caa0 :where(table){--table-border-color: var(--mantine-color-dark-4)}.m_d08caa0 :where(table) :where(caption){margin-top:var(--mantine-spacing-xs);font-size:var(--mantine-font-size-sm);color:var(--mantine-color-dimmed)}.m_d08caa0 :where(table) :where(th){text-align:left;font-weight:700;font-size:var(--mantine-font-size-sm);padding:var(--mantine-spacing-xs) var(--mantine-spacing-sm)}.m_d08caa0 :where(table) :where(thead th){border-bottom:calc(.0625rem * var(--mantine-scale)) solid;border-color:var(--table-border-color)}.m_d08caa0 :where(table) :where(tfoot th){border-top:calc(.0625rem * var(--mantine-scale)) solid;border-color:var(--table-border-color)}.m_d08caa0 :where(table) :where(td){padding:var(--mantine-spacing-xs) var(--mantine-spacing-sm);border-bottom:calc(.0625rem * var(--mantine-scale)) solid;border-color:var(--table-border-color);font-size:var(--mantine-font-size-sm)}.m_d08caa0 :where(table) :where(tr:last-of-type td){border-bottom:0}.m_d08caa0 :where(blockquote){font-size:var(--mantine-font-size-lg);line-height:var(--mantine-line-height);margin:var(--mantine-spacing-md) 0;border-radius:var(--mantine-radius-sm);padding:var(--mantine-spacing-md) var(--mantine-spacing-lg)}:where([data-mantine-color-scheme=light]) .m_d08caa0 :where(blockquote){background-color:var(--mantine-color-gray-0)}:where([data-mantine-color-scheme=dark]) .m_d08caa0 :where(blockquote){background-color:var(--mantine-color-dark-8)}.m_b37d9ac7{width:calc(100% - var(--mantine-spacing-md) * 2);position:fixed;z-index:var(--notifications-z-index);max-width:var(--notifications-container-width)}.m_b37d9ac7:where([data-position=top-center]){top:var(--mantine-spacing-md);left:50%;transform:translate(-50%)}.m_b37d9ac7:where([data-position=top-left]){top:var(--mantine-spacing-md);left:var(--mantine-spacing-md)}.m_b37d9ac7:where([data-position=top-right]){top:var(--mantine-spacing-md);right:var(--mantine-spacing-md)}.m_b37d9ac7:where([data-position=bottom-center]){bottom:var(--mantine-spacing-md);left:50%;transform:translate(-50%)}.m_b37d9ac7:where([data-position=bottom-left]){bottom:var(--mantine-spacing-md);left:var(--mantine-spacing-md)}.m_b37d9ac7:where([data-position=bottom-right]){bottom:var(--mantine-spacing-md);right:var(--mantine-spacing-md)}.m_5ed0edd0+.m_5ed0edd0{margin-top:var(--mantine-spacing-md)}.mantine-ScrollArea-scrollbar{z-index:100}html{font-size:92.5%}@media (max-width: 767px){html{font-size:83%}}@font-face{font-family:Inter;src:url(../Inter-VariableFont_slnt,wght.ttf) format("truetype");font-weight:1 100 200 300 400 500 600 700 800 900 1000;font-style:normal italic}body,html{width:100%;height:100%;margin:0;padding:0;overflow:hidden}body{font-family:Inter,-apple-system,BlinkMacSystemFont,Segoe UI,Roboto,Oxygen,Ubuntu,Cantarell,Fira Sans,Droid Sans,Helvetica Neue,sans-serif;-webkit-font-smoothing:antialiased;-moz-osx-font-smoothing:grayscale}code{font-family:source-code-pro,Menlo,Monaco,Consolas,Courier New,monospace}#root{width:100%;height:100%;overflow:hidden}.stats-panel{position:absolute!important}.gfm-color-chip{margin-left:.125rem;display:inline-block;height:.625rem;width:.625rem;border-radius:9999px;border:1px solid gray;transform:TranslateY(.07em)}._11mcxao0{height:1em;width:1em;position:relative}._11mcxao0 svg{height:auto;width:1em;position:absolute;top:50%;transform:translateY(-50%)}._11mcxao1 .mantine-Slider-markWrapper:first-of-type div:nth-of-type(2){transform:translate(-.1rem,.03rem)!important}._11mcxao1 .mantine-Slider-markWrapper:last-of-type div:nth-of-type(2){transform:translate(-85%,.03rem)!important}.mantine-Slider-mark[data-filled]:not([data-disabled]){background:var(--mantine-primary-color-filled);border-color:var(--mantine-primary-color-filled)}.mantine-Slider-mark[data-filled][data-disabled]{background:var(--mantine-color-gray-5);border-color:var(--mantine-color-gray-5)}[data-mantine-color-scheme=dark] .mantine-Slider-mark[data-filled][data-disabled]{background:var(--mantine-color-dark-3);border-color:var(--mantine-color-dark-3)}._3o7djo0{border-width:1px;position:relative;margin-left:var(--mantine-spacing-xs);margin-right:var(--mantine-spacing-xs);margin-top:var(--mantine-spacing-xs);margin-bottom:var(--mantine-spacing-xs);padding-bottom:calc(var(--mantine-spacing-xs) - .5em)}._3o7djo1{font-size:.875em;position:absolute;padding:0 .375em;top:0;left:.375em;transform:translateY(-50%);-webkit-user-select:none;user-select:none;font-weight:500}._3o7djo2{width:.9em;height:.9em;stroke-width:3;top:.1em;position:relative;margin-left:.25em;margin-right:-.1em;opacity:.5}.uplot,.uplot *,.uplot *:before,.uplot *:after{box-sizing:border-box}.uplot{font-family:system-ui,-apple-system,Segoe UI,Roboto,Helvetica Neue,Arial,Noto Sans,sans-serif,"Apple Color Emoji","Segoe UI Emoji",Segoe UI Symbol,"Noto Color Emoji";line-height:1.5;width:min-content}.u-title{text-align:center;font-size:18px;font-weight:700}.u-wrap{position:relative;-webkit-user-select:none;user-select:none}.u-over,.u-under{position:absolute}.u-under{overflow:hidden}.uplot canvas{display:block;position:relative;width:100%;height:100%}.u-axis{position:absolute}.u-legend{font-size:14px;margin:auto;text-align:center}.u-inline{display:block}.u-inline *{display:inline-block}.u-inline tr{margin-right:16px}.u-legend th{font-weight:600}.u-legend th>*{vertical-align:middle;display:inline-block}.u-legend .u-marker{width:1em;height:1em;margin-right:4px;background-clip:padding-box!important}.u-inline.u-live th:after{content:":";vertical-align:middle}.u-inline:not(.u-live) .u-value{display:none}.u-series>*{padding:4px}.u-series th{cursor:pointer}.u-legend .u-off>*{opacity:.3}.u-select{background:#00000012;position:absolute;pointer-events:none}.u-cursor-x,.u-cursor-y{position:absolute;left:0;top:0;pointer-events:none;will-change:transform}.u-hz .u-cursor-x,.u-vt .u-cursor-y{height:100%;border-right:1px dashed #607D8B}.u-hz .u-cursor-y,.u-vt .u-cursor-x{width:100%;border-bottom:1px dashed #607D8B}.u-cursor-pt{position:absolute;top:0;left:0;border-radius:50%;border:0 solid;pointer-events:none;will-change:transform;background-clip:padding-box!important}.u-axis.u-off,.u-select.u-off,.u-cursor-x.u-off,.u-cursor-y.u-off,.u-cursor-pt.u-off{display:none}.uplot .u-title{font-family:Inter,-apple-system,BlinkMacSystemFont,Segoe UI,Roboto,Oxygen,Ubuntu,Cantarell,Fira Sans,Droid Sans,Helvetica Neue,sans-serif;font-size:.875em;font-weight:500;color:var(--mantine-color-text);padding-top:.375em;padding-bottom:.25em}.uplot .u-legend{font-family:Inter,-apple-system,BlinkMacSystemFont,Segoe UI,Roboto,Oxygen,Ubuntu,Cantarell,Fira Sans,Droid Sans,Helvetica Neue,sans-serif;font-size:.7em;padding:0;margin:-1em 0 0}.uplot-container{border-color:var(--mantine-color-gray-2)!important}[data-mantine-color-scheme=dark] .uplot-container{border-color:var(--mantine-color-dark-6)!important}.uplot .u-legend .u-series{padding:0;margin-right:.375em}.uplot .u-legend .u-marker{width:.75em;height:.75em;margin-right:.25em}.uplot .u-legend .u-label{color:var(--mantine-color-text);font-weight:450}.uplot .u-legend .u-value{color:var(--mantine-color-dimmed);font-weight:400;margin-left:.0625em}.uplot .u-axis{font-family:Inter,-apple-system,BlinkMacSystemFont,Segoe UI,Roboto,Oxygen,Ubuntu,Cantarell,Fira Sans,Droid Sans,Helvetica Neue,sans-serif;font-size:.7em}.uplot .u-axis text{stroke:none;fill:var(--mantine-color-text)}.multi-slider{position:relative;width:100%;height:1rem;margin-top:-.5rem}.multi-slider-track-container{position:relative;height:.25rem;cursor:pointer;margin:1rem 0}.multi-slider-track{position:absolute;top:0;left:0;right:0;height:100%;background-color:var(--mantine-color-gray-3);border-radius:var(--mantine-radius-xl)}.multi-slider-thumb{position:absolute;top:50%;transform:translate(-50%,-50%);width:.5rem;height:.75rem;border-radius:var(--mantine-radius-xs);cursor:pointer;transition:transform .1s;z-index:2}.multi-slider-thumb:active{transform:translate(-50%,-50%) scale(1.1)}.multi-slider.disabled{opacity:.6;cursor:not-allowed}.multi-slider.disabled .multi-slider-track-container{cursor:not-allowed}.multi-slider.disabled .multi-slider-thumb{cursor:not-allowed;background-color:var(--mantine-color-gray-5)!important}.multi-slider-mark-wrapper{position:absolute;top:0;transform:translate(-50%);height:100%;display:flex;flex-direction:column;align-items:center;justify-content:center}.multi-slider-mark{width:.25rem;height:.25rem;background-color:var(--mantine-color-gray-3);border-radius:50%;transform:scale(2)}.multi-slider-mark-label{position:absolute;top:100%;margin-top:.05rem;font-size:.6rem;color:var(--mantine-color-gray-6);white-space:nowrap}[data-mantine-color-scheme=dark] .multi-slider-track{background-color:var(--mantine-color-dark-4)}[data-mantine-color-scheme=dark] .multi-slider-thumb{border-color:var(--mantine-color-dark-7)}[data-mantine-color-scheme=dark] .multi-slider.disabled .multi-slider-thumb{background-color:var(--mantine-color-dark-3)}[data-mantine-color-scheme=dark] .multi-slider-mark{background-color:var(--mantine-color-dark-4)}[data-mantine-color-scheme=dark] .multi-slider-mark-label{color:var(--mantine-color-dark-2)}.z8daqr0{border-radius:var(--mantine-radius-xs);padding:.1em 0;overflow-x:auto;display:flex;flex-direction:column;gap:0}.z8daqr1{position:relative;box-sizing:border-box;overflow-x:auto;display:flex;flex-direction:column;gap:var(--mantine-spacing-xs)}.z8daqr2{opacity:0}.z8daqr3{display:flex;align-items:center;gap:.2em;padding:0 .25em;line-height:2em;font-size:.875em}.z8daqr3:hover{[data-mantine-color-scheme=light] &{background-color:var(--mantine-color-gray-1)}[data-mantine-color-scheme=dark] &{background-color:var(--mantine-color-dark-6)}}.z8daqr4{[data-mantine-color-scheme=light] &{border-color:var(--mantine-color-gray-2)}[data-mantine-color-scheme=dark] &{border-color:var(--mantine-color-dark-5)}border-left:.3em solid;width:.2em;margin-left:.375em;height:2em}.z8daqr3:hover .z8daqr4{[data-mantine-color-scheme=light] &{border-color:var(--mantine-color-gray-3)}[data-mantine-color-scheme=dark] &{border-color:var(--mantine-color-dark-4)}}.z8daqr3:hover .z8daqr2{opacity:1}
================================================
FILE: newton/_src/viewer/viser/static/assets/index-H4DT9vxj.js
================================================
function QG(t,e){for(var n=0;ni[r]})}}}return Object.freeze(Object.defineProperty(t,Symbol.toStringTag,{value:"Module"}))}(function(){const e=document.createElement("link").relList;if(e&&e.supports&&e.supports("modulepreload"))return;for(const r of document.querySelectorAll('link[rel="modulepreload"]'))i(r);new MutationObserver(r=>{for(const a of r)if(a.type==="childList")for(const s of a.addedNodes)s.tagName==="LINK"&&s.rel==="modulepreload"&&i(s)}).observe(document,{childList:!0,subtree:!0});function n(r){const a={};return r.integrity&&(a.integrity=r.integrity),r.referrerPolicy&&(a.referrerPolicy=r.referrerPolicy),r.crossOrigin==="use-credentials"?a.credentials="include":r.crossOrigin==="anonymous"?a.credentials="omit":a.credentials="same-origin",a}function i(r){if(r.ep)return;r.ep=!0;const a=n(r);fetch(r.href,a)}})();var Fm=typeof globalThis<"u"?globalThis:typeof window<"u"?window:typeof global<"u"?global:typeof self<"u"?self:{};function Rc(t){return t&&t.__esModule&&Object.prototype.hasOwnProperty.call(t,"default")?t.default:t}function gre(t){if(t.__esModule)return t;var e=t.default;if(typeof e=="function"){var n=function i(){return this instanceof i?Reflect.construct(e,arguments,this.constructor):e.apply(this,arguments)};n.prototype=e.prototype}else n={};return Object.defineProperty(n,"__esModule",{value:!0}),Object.keys(t).forEach(function(i){var r=Object.getOwnPropertyDescriptor(t,i);Object.defineProperty(n,i,r.get?r:{enumerable:!0,get:function(){return t[i]}})}),n}var e7={exports:{}},IT={};/**
* @license React
* react-jsx-runtime.production.js
*
* Copyright (c) Meta Platforms, Inc. and affiliates.
*
* This source code is licensed under the MIT license found in the
* LICENSE file in the root directory of this source tree.
*/var bre=Symbol.for("react.transitional.element"),vre=Symbol.for("react.fragment");function t7(t,e,n){var i=null;if(n!==void 0&&(i=""+n),e.key!==void 0&&(i=""+e.key),"key"in e){n={};for(var r in e)r!=="key"&&(n[r]=e[r])}else n=e;return e=n.ref,{$$typeof:bre,type:t,key:i,ref:e!==void 0?e:null,props:n}}IT.Fragment=vre;IT.jsx=t7;IT.jsxs=t7;e7.exports=IT;var k=e7.exports;const yre=Rc(k),xre=QG({__proto__:null,default:yre},[k]);var n7={exports:{}},NT={},i7={exports:{}},r7={};/**
* @license React
* scheduler.production.js
*
* Copyright (c) Meta Platforms, Inc. and affiliates.
*
* This source code is licensed under the MIT license found in the
* LICENSE file in the root directory of this source tree.
*/(function(t){function e(F,V){var H=F.length;F.push(V);e:for(;0>>1,B=F[W];if(0>>1;Wr(G,H))der(oe,G)?(F[W]=oe,F[de]=H,W=de):(F[W]=G,F[Z]=H,W=Z);else if(der(oe,H))F[W]=oe,F[de]=H,W=de;else break e}}return V}function r(F,V){var H=F.sortIndex-V.sortIndex;return H!==0?H:F.id-V.id}if(t.unstable_now=void 0,typeof performance=="object"&&typeof performance.now=="function"){var a=performance;t.unstable_now=function(){return a.now()}}else{var s=Date,o=s.now();t.unstable_now=function(){return s.now()-o}}var l=[],c=[],u=1,f=null,h=3,d=!1,g=!1,b=!1,y=!1,m=typeof setTimeout=="function"?setTimeout:null,x=typeof clearTimeout=="function"?clearTimeout:null,_=typeof setImmediate<"u"?setImmediate:null;function w(F){for(var V=n(c);V!==null;){if(V.callback===null)i(c);else if(V.startTime<=F)i(c),V.sortIndex=V.expirationTime,e(l,V);else break;V=n(c)}}function M(F){if(b=!1,w(F),!g)if(n(l)!==null)g=!0,S||(S=!0,U());else{var V=n(c);V!==null&&q(M,V.startTime-F)}}var S=!1,A=-1,C=5,D=-1;function R(){return y?!0:!(t.unstable_now()-DF&&R());){var W=f.callback;if(typeof W=="function"){f.callback=null,h=f.priorityLevel;var B=W(f.expirationTime<=F);if(F=t.unstable_now(),typeof B=="function"){f.callback=B,w(F),V=!0;break t}f===n(l)&&i(l),w(F)}else i(l);f=n(l)}if(f!==null)V=!0;else{var J=n(c);J!==null&&q(M,J.startTime-F),V=!1}}break e}finally{f=null,h=H,d=!1}V=void 0}}finally{V?U():S=!1}}}var U;if(typeof _=="function")U=function(){_(L)};else if(typeof MessageChannel<"u"){var j=new MessageChannel,z=j.port2;j.port1.onmessage=L,U=function(){z.postMessage(null)}}else U=function(){m(L,0)};function q(F,V){A=m(function(){F(t.unstable_now())},V)}t.unstable_IdlePriority=5,t.unstable_ImmediatePriority=1,t.unstable_LowPriority=4,t.unstable_NormalPriority=3,t.unstable_Profiling=null,t.unstable_UserBlockingPriority=2,t.unstable_cancelCallback=function(F){F.callback=null},t.unstable_forceFrameRate=function(F){0>F||125W?(F.sortIndex=H,e(c,F),n(l)===null&&F===n(c)&&(b?(x(A),A=-1):b=!0,q(M,H-W))):(F.sortIndex=B,e(l,F),g||d||(g=!0,S||(S=!0,U()))),F},t.unstable_shouldYield=R,t.unstable_wrapCallback=function(F){var V=h;return function(){var H=h;h=V;try{return F.apply(this,arguments)}finally{h=H}}}})(r7);i7.exports=r7;var _re=i7.exports,a7={exports:{}},fi={};/**
* @license React
* react.production.js
*
* Copyright (c) Meta Platforms, Inc. and affiliates.
*
* This source code is licensed under the MIT license found in the
* LICENSE file in the root directory of this source tree.
*/var fN=Symbol.for("react.transitional.element"),Sre=Symbol.for("react.portal"),wre=Symbol.for("react.fragment"),Ere=Symbol.for("react.strict_mode"),Mre=Symbol.for("react.profiler"),Tre=Symbol.for("react.consumer"),Are=Symbol.for("react.context"),Cre=Symbol.for("react.forward_ref"),Rre=Symbol.for("react.suspense"),kre=Symbol.for("react.memo"),s7=Symbol.for("react.lazy"),V9=Symbol.iterator;function Dre(t){return t===null||typeof t!="object"?null:(t=V9&&t[V9]||t["@@iterator"],typeof t=="function"?t:null)}var o7={isMounted:function(){return!1},enqueueForceUpdate:function(){},enqueueReplaceState:function(){},enqueueSetState:function(){}},l7=Object.assign,c7={};function Sv(t,e,n){this.props=t,this.context=e,this.refs=c7,this.updater=n||o7}Sv.prototype.isReactComponent={};Sv.prototype.setState=function(t,e){if(typeof t!="object"&&typeof t!="function"&&t!=null)throw Error("takes an object of state variables to update or a function which returns an object of state variables.");this.updater.enqueueSetState(this,t,e,"setState")};Sv.prototype.forceUpdate=function(t){this.updater.enqueueForceUpdate(this,t,"forceUpdate")};function u7(){}u7.prototype=Sv.prototype;function hN(t,e,n){this.props=t,this.context=e,this.refs=c7,this.updater=n||o7}var dN=hN.prototype=new u7;dN.constructor=hN;l7(dN,Sv.prototype);dN.isPureReactComponent=!0;var G9=Array.isArray,$r={H:null,A:null,T:null,S:null,V:null},f7=Object.prototype.hasOwnProperty;function pN(t,e,n,i,r,a){return n=a.ref,{$$typeof:fN,type:t,key:e,ref:n!==void 0?n:null,props:a}}function Ore(t,e){return pN(t.type,e,void 0,void 0,void 0,t.props)}function mN(t){return typeof t=="object"&&t!==null&&t.$$typeof===fN}function Ire(t){var e={"=":"=0",":":"=2"};return"$"+t.replace(/[=:]/g,function(n){return e[n]})}var W9=/\/+/g;function JR(t,e){return typeof t=="object"&&t!==null&&t.key!=null?Ire(""+t.key):e.toString(36)}function q9(){}function Nre(t){switch(t.status){case"fulfilled":return t.value;case"rejected":throw t.reason;default:switch(typeof t.status=="string"?t.then(q9,q9):(t.status="pending",t.then(function(e){t.status==="pending"&&(t.status="fulfilled",t.value=e)},function(e){t.status==="pending"&&(t.status="rejected",t.reason=e)})),t.status){case"fulfilled":return t.value;case"rejected":throw t.reason}}throw t}function Pb(t,e,n,i,r){var a=typeof t;(a==="undefined"||a==="boolean")&&(t=null);var s=!1;if(t===null)s=!0;else switch(a){case"bigint":case"string":case"number":s=!0;break;case"object":switch(t.$$typeof){case fN:case Sre:s=!0;break;case s7:return s=t._init,Pb(s(t._payload),e,n,i,r)}}if(s)return r=r(t),s=i===""?"."+JR(t,0):i,G9(r)?(n="",s!=null&&(n=s.replace(W9,"$&/")+"/"),Pb(r,e,n,"",function(c){return c})):r!=null&&(mN(r)&&(r=Ore(r,n+(r.key==null||t&&t.key===r.key?"":(""+r.key).replace(W9,"$&/")+"/")+s)),e.push(r)),1;s=0;var o=i===""?".":i+":";if(G9(t))for(var l=0;l"u"||typeof __REACT_DEVTOOLS_GLOBAL_HOOK__.checkDCE!="function"))try{__REACT_DEVTOOLS_GLOBAL_HOOK__.checkDCE(m7)}catch(t){console.error(t)}}m7(),d7.exports=Co;var l_=d7.exports;const vx=Rc(l_);/**
* @license React
* react-dom-client.production.js
*
* Copyright (c) Meta Platforms, Inc. and affiliates.
*
* This source code is licensed under the MIT license found in the
* LICENSE file in the root directory of this source tree.
*/var za=_re,g7=I,zre=l_;function It(t){var e="https://react.dev/errors/"+t;if(1qb||(t.current=R8[qb],R8[qb]=null,qb--)}function Zr(t,e){qb++,R8[qb]=t.current,t.current=e}var $u=df(null),y1=df(null),Ad=df(null),AM=df(null);function CM(t,e){switch(Zr(Ad,e),Zr(y1,t),Zr($u,null),e.nodeType){case 9:case 11:t=(t=e.documentElement)&&(t=t.namespaceURI)?eB(t):0;break;default:if(t=e.tagName,e=e.namespaceURI)e=eB(e),t=Fq(e,t);else switch(t){case"svg":t=1;break;case"math":t=2;break;default:t=0}}as($u),Zr($u,t)}function N0(){as($u),as(y1),as(Ad)}function k8(t){t.memoizedState!==null&&Zr(AM,t);var e=$u.current,n=Fq(e,t.type);e!==n&&(Zr(y1,t),Zr($u,n))}function RM(t){y1.current===t&&(as($u),as(y1)),AM.current===t&&(as(AM),R1._currentValue=xm)}var D8=Object.prototype.hasOwnProperty,vN=za.unstable_scheduleCallback,$R=za.unstable_cancelCallback,qre=za.unstable_shouldYield,Xre=za.unstable_requestPaint,Zu=za.unstable_now,Kre=za.unstable_getCurrentPriorityLevel,S7=za.unstable_ImmediatePriority,w7=za.unstable_UserBlockingPriority,kM=za.unstable_NormalPriority,Yre=za.unstable_LowPriority,E7=za.unstable_IdlePriority,Jre=za.log,$re=za.unstable_setDisableYieldValue,u_=null,xl=null;function xd(t){if(typeof Jre=="function"&&$re(t),xl&&typeof xl.setStrictMode=="function")try{xl.setStrictMode(u_,t)}catch{}}var _l=Math.clz32?Math.clz32:eae,Zre=Math.log,Qre=Math.LN2;function eae(t){return t>>>=0,t===0?32:31-(Zre(t)/Qre|0)|0}var FS=256,BS=4194304;function Wp(t){var e=t&42;if(e!==0)return e;switch(t&-t){case 1:return 1;case 2:return 2;case 4:return 4;case 8:return 8;case 16:return 16;case 32:return 32;case 64:return 64;case 128:return 128;case 256:case 512:case 1024:case 2048:case 4096:case 8192:case 16384:case 32768:case 65536:case 131072:case 262144:case 524288:case 1048576:case 2097152:return t&4194048;case 4194304:case 8388608:case 16777216:case 33554432:return t&62914560;case 67108864:return 67108864;case 134217728:return 134217728;case 268435456:return 268435456;case 536870912:return 536870912;case 1073741824:return 0;default:return t}}function PT(t,e,n){var i=t.pendingLanes;if(i===0)return 0;var r=0,a=t.suspendedLanes,s=t.pingedLanes;t=t.warmLanes;var o=i&134217727;return o!==0?(i=o&~a,i!==0?r=Wp(i):(s&=o,s!==0?r=Wp(s):n||(n=o&~t,n!==0&&(r=Wp(n))))):(o=i&~a,o!==0?r=Wp(o):s!==0?r=Wp(s):n||(n=i&~t,n!==0&&(r=Wp(n)))),r===0?0:e!==0&&e!==r&&!(e&a)&&(a=r&-r,n=e&-e,a>=n||a===32&&(n&4194048)!==0)?e:r}function f_(t,e){return(t.pendingLanes&~(t.suspendedLanes&~t.pingedLanes)&e)===0}function tae(t,e){switch(t){case 1:case 2:case 4:case 8:case 64:return e+250;case 16:case 32:case 128:case 256:case 512:case 1024:case 2048:case 4096:case 8192:case 16384:case 32768:case 65536:case 131072:case 262144:case 524288:case 1048576:case 2097152:return e+5e3;case 4194304:case 8388608:case 16777216:case 33554432:return-1;case 67108864:case 134217728:case 268435456:case 536870912:case 1073741824:return-1;default:return-1}}function M7(){var t=FS;return FS<<=1,!(FS&4194048)&&(FS=256),t}function T7(){var t=BS;return BS<<=1,!(BS&62914560)&&(BS=4194304),t}function ZR(t){for(var e=[],n=0;31>n;n++)e.push(t);return e}function h_(t,e){t.pendingLanes|=e,e!==268435456&&(t.suspendedLanes=0,t.pingedLanes=0,t.warmLanes=0)}function nae(t,e,n,i,r,a){var s=t.pendingLanes;t.pendingLanes=n,t.suspendedLanes=0,t.pingedLanes=0,t.warmLanes=0,t.expiredLanes&=n,t.entangledLanes&=n,t.errorRecoveryDisabledLanes&=n,t.shellSuspendCounter=0;var o=t.entanglements,l=t.expirationTimes,c=t.hiddenUpdates;for(n=s&~n;0)":-1r||l[i]!==c[r]){var u=`
`+l[i].replace(" at new "," at ");return t.displayName&&u.includes("")&&(u=u.replace("",t.displayName)),u}while(1<=i&&0<=r);break}}}finally{e3=!1,Error.prepareStackTrace=n}return(n=t?t.displayName||t.name:"")?Ub(n):""}function lae(t){switch(t.tag){case 26:case 27:case 5:return Ub(t.type);case 16:return Ub("Lazy");case 13:return Ub("Suspense");case 19:return Ub("SuspenseList");case 0:case 15:return t3(t.type,!1);case 11:return t3(t.type.render,!1);case 1:return t3(t.type,!0);case 31:return Ub("Activity");default:return""}}function eF(t){try{var e="";do e+=lae(t),t=t.return;while(t);return e}catch(n){return`
Error generating stack: `+n.message+`
`+n.stack}}function sc(t){switch(typeof t){case"bigint":case"boolean":case"number":case"string":case"undefined":return t;case"object":return t;default:return""}}function O7(t){var e=t.type;return(t=t.nodeName)&&t.toLowerCase()==="input"&&(e==="checkbox"||e==="radio")}function cae(t){var e=O7(t)?"checked":"value",n=Object.getOwnPropertyDescriptor(t.constructor.prototype,e),i=""+t[e];if(!t.hasOwnProperty(e)&&typeof n<"u"&&typeof n.get=="function"&&typeof n.set=="function"){var r=n.get,a=n.set;return Object.defineProperty(t,e,{configurable:!0,get:function(){return r.call(this)},set:function(s){i=""+s,a.call(this,s)}}),Object.defineProperty(t,e,{enumerable:n.enumerable}),{getValue:function(){return i},setValue:function(s){i=""+s},stopTracking:function(){t._valueTracker=null,delete t[e]}}}}function DM(t){t._valueTracker||(t._valueTracker=cae(t))}function I7(t){if(!t)return!1;var e=t._valueTracker;if(!e)return!0;var n=e.getValue(),i="";return t&&(i=O7(t)?t.checked?"true":"false":t.value),t=i,t!==n?(e.setValue(t),!0):!1}function OM(t){if(t=t||(typeof document<"u"?document:void 0),typeof t>"u")return null;try{return t.activeElement||t.body}catch{return t.body}}var uae=/[\n"\\]/g;function fc(t){return t.replace(uae,function(e){return"\\"+e.charCodeAt(0).toString(16)+" "})}function I8(t,e,n,i,r,a,s,o){t.name="",s!=null&&typeof s!="function"&&typeof s!="symbol"&&typeof s!="boolean"?t.type=s:t.removeAttribute("type"),e!=null?s==="number"?(e===0&&t.value===""||t.value!=e)&&(t.value=""+sc(e)):t.value!==""+sc(e)&&(t.value=""+sc(e)):s!=="submit"&&s!=="reset"||t.removeAttribute("value"),e!=null?N8(t,s,sc(e)):n!=null?N8(t,s,sc(n)):i!=null&&t.removeAttribute("value"),r==null&&a!=null&&(t.defaultChecked=!!a),r!=null&&(t.checked=r&&typeof r!="function"&&typeof r!="symbol"),o!=null&&typeof o!="function"&&typeof o!="symbol"&&typeof o!="boolean"?t.name=""+sc(o):t.removeAttribute("name")}function N7(t,e,n,i,r,a,s,o){if(a!=null&&typeof a!="function"&&typeof a!="symbol"&&typeof a!="boolean"&&(t.type=a),e!=null||n!=null){if(!(a!=="submit"&&a!=="reset"||e!=null))return;n=n!=null?""+sc(n):"",e=e!=null?""+sc(e):n,o||e===t.value||(t.value=e),t.defaultValue=e}i=i??r,i=typeof i!="function"&&typeof i!="symbol"&&!!i,t.checked=o?t.checked:!!i,t.defaultChecked=!!i,s!=null&&typeof s!="function"&&typeof s!="symbol"&&typeof s!="boolean"&&(t.name=s)}function N8(t,e,n){e==="number"&&OM(t.ownerDocument)===t||t.defaultValue===""+n||(t.defaultValue=""+n)}function u0(t,e,n,i){if(t=t.options,e){e={};for(var r=0;r"u"||typeof window.document>"u"||typeof window.document.createElement>"u"),P8=!1;if(uh)try{var Ty={};Object.defineProperty(Ty,"passive",{get:function(){P8=!0}}),window.addEventListener("test",Ty,Ty),window.removeEventListener("test",Ty,Ty)}catch{P8=!1}var _d=null,EN=null,XE=null;function B7(){if(XE)return XE;var t,e=EN,n=e.length,i,r="value"in _d?_d.value:_d.textContent,a=r.length;for(t=0;t=Px),oF=" ",lF=!1;function j7(t,e){switch(t){case"keyup":return Fae.indexOf(e.keyCode)!==-1;case"keydown":return e.keyCode!==229;case"keypress":case"mousedown":case"focusout":return!0;default:return!1}}function H7(t){return t=t.detail,typeof t=="object"&&"data"in t?t.data:null}var Yb=!1;function zae(t,e){switch(t){case"compositionend":return H7(e);case"keypress":return e.which!==32?null:(lF=!0,oF);case"textInput":return t=e.data,t===oF&&lF?null:t;default:return null}}function jae(t,e){if(Yb)return t==="compositionend"||!TN&&j7(t,e)?(t=B7(),XE=EN=_d=null,Yb=!1,t):null;switch(t){case"paste":return null;case"keypress":if(!(e.ctrlKey||e.altKey||e.metaKey)||e.ctrlKey&&e.altKey){if(e.char&&1