Additional functions for checking robot collisions. #7

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open

WeihangGuo wants to merge 23 commits into CoMMALab:main from WeihangGuo:robot-coll

.github/workflows/docs.yml

This file was deleted.

0 .github/workflows/formatting.yaml → .github/workflows/formatting.yml

File renamed without changes.

.gitignore

-Original file line number
+Diff line change
@@ Expand Up / @@ -15,3 +15,4 @@ htmlcov @@
     .envrc
     .vite
     build
+    tmp/

README.md

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -1,32 +1,13 @@
  
    # `PyRoNot`: A Better Python Robot Kinematics Library

    # `PyRoNot`: A Python Library for Robot KinematicsUsing Spherical Approximations

    ## Citation

    [![Format Check](https://github.com/CoMMALab/pyronot/actions/workflows/formatting.yml/badge.svg)](https://github.com/CoMMALab/pyronot/actions/workflows/formatting.yml)

    [![Pyright](https://github.com/CoMMALab/pyronot/actions/workflows/pyright.yml/badge.svg)](https://github.com/CoMMALab/pyronot/actions/workflows/pyright.yml)

    [![Pytest](https://github.com/CoMMALab/pyronot/actions/workflows/pytest.yml/badge.svg)](https://github.com/CoMMALab/pyronot/actions/workflows/pytest.yml)

    [![PyPI - Version](https://img.shields.io/pypi/v/pyronot)](https://pypi.org/project/pyronot/)

    This repository is based on pyroki.

    <table><tr><td>

        Chung Min Kim*, Brent Yi*, Hongsuk Choi, Yi Ma, Ken Goldberg, Angjoo Kanazawa.

        <strong>PyRoki: A Modular Toolkit for Robot Kinematic Optimization</strong>

        arXiV, 2025.

    </td></tr>

    </table>

    <sup>\*</sup><em>Equal Contribution</em>, <em>UC Berkeley</em>.

    Please cite PyRoki if you find this work useful for your research:

    ```

    @inproceedings{kim2025pyroki,

      title={PyRoki: A Modular Toolkit for Robot Kinematic Optimization},

      author={Kim*, Chung Min and Yi*, Brent and Choi, Hongsuk and Ma, Yi and Goldberg, Ken and Kanazawa, Angjoo},

      booktitle={2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},

      year={2025},

      url={https://arxiv.org/abs/2505.03728},

    }

    ```

    Thanks!

    This repository is based on [pyroki](https://github.com/chungmin99/pyroki).

    ## Installation

    ```

benchmark/robot_coll_benchmark.py

-Original file line number
+Diff line change
@@ -0,0 +1,195 @@
+    import jax
+    jax.config.update('jax_platform_name', 'cpu')
+    print(f"================================================")
+    print(f"Jax platform: {jax.lib.xla_bridge.get_backend().platform}")
+    print(f"================================================")
+    import numpy as np
+    import pyronot as prn
+    import pyroki as pk
+    from pyroki.collision import Sphere, RobotCollision
+    from pyronot.collision import Sphere as SphereNot, RobotCollisionSpherized as RobotCollisionSpherizedNot
+    import yourdfpy
+    import pinocchio as pin
+    import hppfcl
+    import time
+    np.random.seed(41)
+    NUM_SAMPLES = 1000
+    SPHERE_CENTERS = [
+        [0.55, 0, 0.25],
+        [0.35, 0.35, 0.25],
+        [0, 0.55, 0.25],
+        [-0.55, 0, 0.25],
+        [-0.35, -0.35, 0.25],
+        [0, -0.55, 0.25],
+        [0.35, -0.35, 0.25],
+        [0.35, 0.35, 0.8],
+        [0, 0.55, 0.8],
+        [-0.35, 0.35, 0.8],
+        [-0.55, 0, 0.8],
+        [-0.35, -0.35, 0.8],
+        [0, -0.55, 0.8],
+        [0.35, -0.35, 0.8],
+        ]
+    SPHERE_R = [0.2] * len(SPHERE_CENTERS)
+    urdf_path = "resources/ur5/ur5_spherized.urdf"
+    mesh_dir = "resources/ur5/meshes"
+    # ============ Pinocchio Ground Truth Setup ============
+    def setup_pinocchio_collision(urdf_path, mesh_dir, sphere_centers, sphere_radii):
+        """Setup Pinocchio collision model with environment spheres."""
+        pin_model = pin.buildModelFromUrdf(urdf_path)
+        pin_geom_model = pin.buildGeomFromUrdf(pin_model, urdf_path, pin.COLLISION, mesh_dir)
+        num_robot_geoms = len(pin_geom_model.geometryObjects)
+        # Add obstacle spheres to the geometry model
+        for i, (center, radius) in enumerate(zip(sphere_centers, sphere_radii)):
+            sphere_shape = hppfcl.Sphere(radius)
+            placement = pin.SE3(np.eye(3), np.array(center))
+            geom_obj = pin.GeometryObject(
+                f"obstacle_sphere_{i}",
+,  # parent frame (universe)
+,  # parent joint (universe)
+                sphere_shape,
+                placement,
+            )
+            pin_geom_model.addGeometryObject(geom_obj)
+        # Add collision pairs: robot geometries vs obstacle spheres
+        num_obstacles = len(sphere_centers)
+        for robot_geom_id in range(num_robot_geoms):
+            for obs_idx in range(num_obstacles):
+                obs_geom_id = num_robot_geoms + obs_idx
+                pin_geom_model.addCollisionPair(
+                    pin.CollisionPair(robot_geom_id, obs_geom_id)
+                )
+        pin_data = pin_model.createData()
+        pin_geom_data = pin.GeometryData(pin_geom_model)
+        return pin_model, pin_data, pin_geom_model, pin_geom_data
+    def check_collision_pinocchio(pin_model, pin_data, pin_geom_model, pin_geom_data, q):
+        """Check if configuration q is in collision using Pinocchio (ground truth)."""
+        pin.updateGeometryPlacements(pin_model, pin_data, pin_geom_model, pin_geom_data, q)
+        return pin.computeCollisions(pin_geom_model, pin_geom_data, True)
+    # Setup Pinocchio collision checker
+    pin_model, pin_data, pin_geom_model, pin_geom_data = setup_pinocchio_collision(
+        urdf_path, mesh_dir, SPHERE_CENTERS, SPHERE_R
+    )
+    world_coll_not = SphereNot.from_center_and_radius(SPHERE_CENTERS, SPHERE_R)
+    world_coll = Sphere.from_center_and_radius(SPHERE_CENTERS, SPHERE_R)
+    urdf = yourdfpy.URDF.load(urdf_path, mesh_dir=mesh_dir)
+    robot_not = prn.Robot.from_urdf(urdf)
+    robot = pk.Robot.from_urdf(urdf)
+    robot_coll = RobotCollision.from_urdf(urdf)
+    robot_coll_not = RobotCollisionSpherizedNot.from_urdf(urdf)
+    def generate_dataset(num_samples):
+        q_batch = []
+        robot_lower_limits = robot.joints.lower_limits
+        robot_upper_limits = robot.joints.upper_limits
+        for _ in range(num_samples):
+            q = np.random.uniform(robot_lower_limits, robot_upper_limits)
+            q_batch.append(q)
+        return np.array(q_batch)
+    q_batch = generate_dataset(NUM_SAMPLES)
+    print(f"Generated {q_batch.shape[0]} samples")
+    # ============ Generate Ground Truth with Pinocchio ============
+    print("\n=== Generating Pinocchio Ground Truth ===")
+    start_time = time.time()
+    ground_truth = []
+    for q in q_batch:
+        collision = check_collision_pinocchio(pin_model, pin_data, pin_geom_model, pin_geom_data, q)
+        ground_truth.append(collision)
+    ground_truth = np.array(ground_truth)
+    end_time = time.time()
+    time_taken_ms = (end_time - start_time) * 1000
+    print(f"Time taken for Pinocchio ground truth (ms): {time_taken_ms:.2f}")
+    print(f"Time per collision check (ms): {time_taken_ms/NUM_SAMPLES:.4f}")
+    print(f"Collision rate: {ground_truth.sum()}/{NUM_SAMPLES} ({100*ground_truth.mean():.1f}%)")
+    # ============ Benchmark pyronot collision methods ============
+    print("\n=== Benchmarking pyronot Collision Methods ===")
+    # Warmup for JIT
+    q = q_batch[0]
+    robot_coll.at_config(robot, q)
+    jax.block_until_ready(robot_coll.compute_world_collision_distance(robot, q, world_coll))
+    print(f"Type of robot_coll.compute_world_collision_distance: {type(robot_coll.compute_world_collision_distance)}")
+    try:
+        print(f"Capsule cache: {robot_coll.compute_world_collision_distance._cache_size()}")
+    except AttributeError:
+        print("Capsule method is NOT JIT compiled")
+    robot_coll_not.at_config(robot_not, q)
+    jax.block_until_ready(robot_coll_not.compute_world_collision_distance(robot_not, q, world_coll_not))
+    print(f"Type of robot_coll_not.compute_world_collision_distance: {type(robot_coll_not.compute_world_collision_distance)}")
+    try:
+        print(f"Sphere cache: {robot_coll_not.compute_world_collision_distance._cache_size()}")
+    except AttributeError:
+        print("Sphere method is NOT JIT compiled")
+    # End of warmup
+    with jax.profiler.trace("./tmp/sphere_trace"):
+        q = q_batch[0]
+        result = robot_coll_not.compute_world_collision_distance(robot_not, q, world_coll_not)
+        jax.block_until_ready(result)
+    print(f"=== Result ===")
+    print(f"Time taken for pinocchio for a signle collision check (ms): \t{time_taken_ms/NUM_SAMPLES:.4f}")
+    start_time = time.time()
+    for q in q_batch:
+        # robot_coll_not.at_config(robot_not, q)
+        robot_coll_not.compute_world_collision_distance(robot_not, q, world_coll_not)
+    end_time = time.time()
+    time_taken_ms = (end_time - start_time) * 1000
+    print(f"Time taken for sphere for single collision check (ms): \t\t{time_taken_ms/NUM_SAMPLES:.4f}")
+    start_time = time.time()
+    for q in q_batch:
+        # robot_coll.at_config(robot, q)
+        robot_coll.compute_world_collision_distance(robot, q, world_coll)
+    end_time = time.time()
+    time_taken_ms = (end_time - start_time) * 1000
+    print(f"Time taken for capsule for single collision check (ms): \t{time_taken_ms/NUM_SAMPLES:.4f}")
+    # ============ Compare with Ground Truth ============
+    print("\n=== Accuracy Comparison with Ground Truth ===")
+    # Check sphere model accuracy
+    sphere_predictions = []
+    for q in q_batch:
+        dist = robot_coll_not.compute_world_collision_distance(robot_not, q, world_coll_not)
+        # Collision if any distance is negative
+        in_collision = (np.array(dist) < 0).any()
+        sphere_predictions.append(in_collision)
+    sphere_predictions = np.array(sphere_predictions)
+    sphere_accuracy = (sphere_predictions == ground_truth).mean()
+    print(f"Sphere model accuracy: {100*sphere_accuracy:.2f}%")
+    # Check capsule model accuracy
+    capsule_predictions = []
+    for q in q_batch:
+        dist = robot_coll.compute_world_collision_distance(robot, q, world_coll)
+        # Collision if any distance is negative
+        in_collision = (np.array(dist) < 0).any()
+        capsule_predictions.append(in_collision)
+    capsule_predictions = np.array(capsule_predictions)
+    capsule_accuracy = (capsule_predictions == ground_truth).mean()
+    print(f"Capsule model accuracy: {100*capsule_accuracy:.2f}%")

docs/Makefile

This file was deleted.

docs/README.md

This file was deleted.

docs/requirements.txt

This file was deleted.

docs/source/_static/basic_ik.mov

Binary file not shown.

docs/source/_static/css/custom.css

This file was deleted.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Additional functions for checking robot collisions. #7

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Additional functions for checking robot collisions. #7

Are you sure you want to change the base?

Uh oh!

Additional functions for checking robot collisions. #7

Uh oh!

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!