RigidBodyCollisions.jl

⚠️ AI Development Disclaimer This is an experimental project where the code has been developed solely by AI assistants, specifically Claude (Anthropic) and Codex (OpenAI). While the project demonstrates the potential of AI-driven software development, users should exercise appropriate caution and thoroughly test any code before use in production environments.

Overview

RigidBodyCollisions.jl extends RigidBodyDynamics.jl with differentiable collision detection capabilities using DifferentiableCollisions.jl. This enables collision-free inverse kinematics and motion planning using gradient-based optimization methods.

Features

🤖 Collision Detection

• Self-collision detection between robot links
• Environment collision detection with static obstacles
• Differentiable collision checking for optimization-based methods
• Efficient gradient computation with analytical Jacobians

🎯 Optimization Integration

• Collision-free inverse kinematics using Optimization.jl
• Gradient-based collision avoidance with ForwardDiff support
• Configurable collision margins and penalty weights
• Multi-stage optimization workflows

📊 Visualization & Analysis

• Interactive 3D visualization with MeshCat.jl
• Real-time collision highlighting with color-coded feedback
• Dynamic collision scene visualization during robot motion
• Collision geometry rendering with transparency and material options

🔧 Mesh Processing

• Automatic mesh-to-primitive fitting (spheres, capsules, boxes)
• Optimized collision geometry generation from URDF visual meshes
• Collision pair exclusion rules for adjacent links
• Multiple primitive types per robot link

Installation

using Pkg
Pkg.add(url="https://github.com/your-username/RigidBodyCollisions.jl")

Dependencies

This package requires:

• Julia ≥ 1.9
• RigidBodyDynamics.jl ≥ 2.5
• DifferentiableCollisions.jl ≥ 0.1.5
• Optimization.jl ≥ 4.6
• MeshCat.jl ≥ 0.16 (for visualization)

Quick Start

using RigidBodyCollisions
using RigidBodyDynamics

# Load robot from URDF
urdf_path = "path/to/your/robot.urdf"
mechanism, collision_model = create_kinematic_collision_model(urdf_path)

# Create collision state
collision_state = RobotCollisionState(mechanism, collision_model)

# Check self-collisions
q = zeros(num_positions(mechanism))  # Joint configuration
update_collision_poses!(collision_state, q)
alphas = check_self_collisions(collision_state)

println("Minimum collision alpha: ", minimum(alphas))
println(minimum(alphas) <= 1.0 ? "Collision detected!" : "Collision-free")

Examples

The examples/ directory contains comprehensive demonstrations:

🎯 Core Functionality

• example_usage.jl - Basic collision detection usage
• collision_pairs_example.jl - Working with collision pairs
• update_collision_poses_example.jl - Dynamic pose updates

🤖 Self-Collision Detection

• self_collision_kuka_example.jl - KUKA IIWA self-collision demo
• self_collision_gradient_performance.jl - Performance comparison: analytical vs finite difference gradients

🎯 Inverse Kinematics & Optimization

• tcp_pose_ik_optimization.jl - Main demo: Two-stage TCP pose optimization with collision avoidance
• mesh_fitting_example.jl - Automatic mesh-to-primitive fitting

📊 Visualization & Analysis

• collision_scene_visualization.jl - Complete collision scene rendering
• robot_collision_state_visualization.jl - Robot-specific collision visualization
• mesh_capsule_visualization.jl - Mesh and fitted capsule comparison
• dynamic_color_example.jl - Dynamic color schemes and highlighting

Main Demo: TCP Pose IK Optimization

The flagship example demonstrates two-stage collision-free inverse kinematics:

# Run without visualization
julia --project=. examples/tcp_pose_ik_optimization.jl

# Run with interactive 3D visualization
RBC_SHOW_MESHCAT=true julia --project=. examples/tcp_pose_ik_optimization.jl

Features:

• Stage 1: Solve IK without collision constraints
• Stage 2: Add environment obstacle and re-solve with collision avoidance
• Real-time collision highlighting during optimization
• Null-space exploitation for redundant manipulators
• Interactive MeshCat visualization with collision feedback

API Reference

Core Types

# Main collision state for a robot
RobotCollisionState(mechanism, collision_model)

# Complete collision scene (robot + environment)
CollisionScene(robot_collision_state)

Collision Detection

# Update collision geometries to current joint configuration
update_collision_poses!(collision_state, q)

# Check self-collisions (returns α values, ≤1 indicates collision)
alphas = check_self_collisions(collision_state)

# Check robot-environment collisions
alphas = check_robot_env_collisions(scene)

# Get actively colliding pairs
pairs = get_self_colliding_pairs(collision_state)
pairs = get_robot_env_colliding_pairs(scene)

Optimization Integration

# Self-collision cost function (differentiable)
cost = self_collision_cost(collision_state, q; margin=1.05, weight=10.0)

# Environment collision cost function
cost = environment_collision_cost(scene, q; margin=1.05, weight=10.0)

# With analytical gradients
cost, grad = self_collision_cost_with_gradient(collision_state, q; ...)
cost, grad = environment_collision_cost_with_gradient(scene, q; ...)

Visualization

# Visualize robot collision geometries
visualize_robot_collision_state!(vis, collision_state)

# Visualize complete collision scene
visualize_collision_scene!(vis, scene)

# Update visualization during motion
update_collision_scene_visualization!(vis, scene)

Collision Detection Details

Self-Collision Detection

• Uses fitted primitive geometries (spheres, capsules, boxes) for efficiency
• Excludes adjacent links and user-specified link pairs
• Returns continuous distance measures (α values) for smooth optimization
• Supports analytical gradient computation via ChainRules.jl

Environment Collision Detection

• Handles arbitrary static obstacles (spheres, boxes, cylinders, polytopes)
• Integrates with robot collision checking in unified scene representation
• Supports dynamic obstacle addition/removal
• Compatible with all DifferentiableCollisions.jl primitive types

Distance Semantics

• α > 1: Objects separated (no collision)
• α = 1: Objects touching (contact)
• α < 1: Objects interpenetrating (collision)

Performance Notes

• Analytical gradients are ~10-100× faster than finite differences
• Capsule fitting provides good collision approximation with fewer primitives
• Collision pair exclusions reduce computational overhead
• Batch collision checking scales efficiently with robot complexity

Project Structure

RigidBodyCollisions.jl/
├── src/
│   ├── RigidBodyCollisions.jl     # Main module
│   ├── collision_state.jl         # Robot collision state management
│   ├── collision_scene.jl         # Complete scene management
│   ├── forward_kinematics.jl      # Kinematic chain utilities
│   ├── mesh_fitting.jl            # Mesh-to-primitive fitting
│   ├── urdf_parsing.jl           # URDF loading and processing
│   └── visualization.jl           # MeshCat visualization tools
├── examples/                      # Comprehensive demonstrations
├── test/                         # Test suite
└── assets/                       # Robot models and meshes

Contributing

While this project was developed by AI assistants, contributions from the community are welcome:

1. Bug reports and feature requests via GitHub Issues
2. Pull requests for fixes and enhancements
3. Example contributions showcasing new use cases
4. Documentation improvements and tutorials

Please thoroughly test any modifications before submitting.

License

This project is licensed under the MIT License - see the LICENSE file for details.

Related Projects

• RigidBodyDynamics.jl - Rigid body dynamics in Julia
• DifferentiableCollisions.jl - Differentiable collision detection primitives
• MeshCat.jl - 3D visualization in Julia
• Optimization.jl - Mathematical optimization in Julia

Citation

If you use this project in research, please cite:

@software{rigidbodycollisions_jl,
  title = {RigidBodyCollisions.jl: Differentiable Collision Detection for Robot Motion Planning},
  author = {AI Development (Claude \& Codex)},
  year = {2024},
  url = {https://github.com/your-username/RigidBodyCollisions.jl},
  note = {Experimental AI-developed software}
}

---

⚠️ Reminder: This codebase was developed entirely by AI assistants. While functional, it should be considered experimental and thoroughly validated before use in critical applications.