kevin-tracy / differentiablecollisions.jl

Differentiable collision detection for polytopes, capsules, cylinders, cones, spheres, and polygons.

A library for differential collision detection, as implemented from Differentiable Collision Detection for a Set of Convex Primitives. The core algorithmn, DCOL, computes collision information between the following convex primitives:

• polytopes
• capsules
• cylinders
• cones
• spheres
• ellipsoids
• padded polygons

A limited version of DCOL is available in Python/JAX as dpax.

Interface

DCOL works by creating a struct for each shape, and calling a function to query a proximity value between them.

Primitives

Each primitive is implemented as a struct in DCOL. The defining dimensions for each primitive is described in the paper, and the primitives can be constructed as the following:

import DifferentiableCollisions as dc

polytope = dc.Polytope(A, b)   # polytope is described by Ax <= b
capsule  = dc.Capsule(R, L)    # radius R, length L
cylinder = dc.Cylinder(R, L)   # radius R, length L
cone     = dc.Cone(H, β)       # height H, half angle β
sphere   = dc.Sphere(R)        # radius R
ellips   = dc.Ellipsoid(P)     # x'*P*x ≦ 1
polygon  = dc.Polygon(A, b, R) # polygon is described by Ay <= b, cushion radius R

where all of these structs are ::AbstractPrimitive, and use a quaternion for attitude. The position and attitude of a primitive P1::AbstractPrimitive are updated in the following way:

using StaticArrays
P1 =  dc.Polytope(A,  b)::AbstractPrimitive
P1.r = SA[1, 2, 3.0]     # position in world frame W
P1.q = SA[1.0, 0, 0, 0]  # quaternion ᵂqᴮ

MRP Support

In cases where a three-parameter attitude parameterization is more convenient, a Modified Rodrigues Parameter (MRP) can be used in the following way:

P1 = dc.PolytopeMRP(A, b)::AbstractPrimitiveMRP
P1.r = SA[1, 2, 3.0]    # position in world frame W
P1.p = SA[0.0, 0, 0]    # MRP ᵂpᴮ

Proximity Functions

DCOL exposes a function proximity for collision detection, as well as proximity_jacobian for collision detection and derivatives. Two optional arguments are included that pertain to the optimization solver under the hood, verbose turns on logging for this solver, and pdip_tol is the termination criteria.

# return min scaling α and intersection x
α, x = dc.proximity(P1, P2; verbose = false, pdip_tol = 1e-6)

# return min scaling α and gradient of α wrt configurations 
α, dα_dstate = dc.proximity_gradient(P1, P2; verbose = false, pdip_tol = 1e-6)

# return min scaling α, intersection x, and jacobian J (*)
α, x, J = dc.proximity_jacobian(P1, P2; verbose = false, pdip_tol = 1e-6)

These functions output $\alpha$ as the minimum scaling, with the following significance:

• $\alpha \leq 1$ means there is a collision between the two primitives
• $\alpha >1$ means there is not a collision between the two primitives

Also, returned is x which is the intersection point between the scaled shapes (see algorithm for significance), and a Jacobian J which is the following:

$$ \begin{align*} J &= \frac{\partial (x,\alpha) }{\partial (r_1,q_1,r_2,q_2)} \end{align*} $$

In the case where AbstractPrimitiveMRP's are used, proximity_jacobian will automatically return the following Jacobian:

$$ \begin{align*} J &= \frac{\partial (x,\alpha) }{\partial (r_1,p_1,r_2,p_2)} \end{align*} $$

Visualizer

All of the primitives (both quaternion and MRP) can be visualized in MeshCat. Below is an example of visualization for a cone:

import DifferentiableCollisions as dc
import MeshCat as mc
using StaticArrays
using LinearAlgebra

vis = mc.Visualizer()
mc.open(vis)

cone = dc.Cone(3.0, deg2rad(22))
cone.r = @SVector randn(3)
cone.q = normalize((@SVector randn(4)))

# build primitive scaled by α = 1.0
dc.build_primitive!(vis, cone, :cone; α = 1.0, color = mc.RGBA(1,0,0,1.0))

# update position and attitude
dc.update_pose!(vis[:cone], cone)

Algorithm

DCOL calculates the collision information between two primitives by solving for the minimum scaling applied to both primitives that result in an intersection. This is done by forming an optimization problem with the following primal variables:

• $\alpha \in \mathbb{R}$, the scaling applied to each primitive
• $x \in \mathbb{R}^3$, an intersection point in the world frame

The following optimization problem solves for the minimum scaling α such that a point x exists in the scaled versions of two primitives P1 and P2.

$$ \begin{align*} \underset{x,\alpha}{\text{minimize}} & \quad \alpha \\ \text{subject to} & \quad \alpha \geq 0, \\ & \quad x \in P_1(\alpha),\\ & \quad x \in P_2(\alpha) \end{align*} $$

This problem is a convex optimization problem with conic constraints, and is solved with a custom primal-dual interior-point method inspired by cvxopt. If the minimum scaling α > 1, then there is no collision because each primitive had to be scaled up in order to find an intersection. Alternatively, this means that if α ≤ 1, the two primitives are in contact. The solution to this optimization problem can be differentiated with respect to the position and orientation of each primitive using the implicit function theorem. By using a primal-dual interior-point method and returning a solution at a pdip_tol of [1e-4,1e-8], the log barrier will effectively smooth out the corners of the primitives to return useful and smooth derivatives.

Paper Examples

Examples from our paper are available here:

• examples/trajectory_optimization/piano_mover.jl
• examples/trajectory_optimization/cluttered_hallway_quadrotor.jl
• examples/trajectory_optimization/cone_through_wall.jl
• examples/contact_physics/ncp_contact_simulation.jl

differentiablecollisions.jl's Issues

Can I also get the Hessian?

I see that the function used to compute the gradient w.r.t. the problem parameters is

@inline function obj_val_grad(capsule::P1,
           cone::P2,
           x::SVector{nx,T1},
           s::SVector{nz,T7},
           z::SVector{nz,T2},
           idx_ort::SVector{n_ort,Ti},
           idx_soc1::SVector{n_soc1,Ti},
           idx_soc2::SVector{n_soc2,Ti}) where {nx,nz,n_ort,n_soc1,n_soc2,Ti,T1,T2,T7,P1 <: AbstractPrimitive, P2 <: AbstractPrimitive}


   idx_x = SVector{nx}(1:nx)
   idx_z = SVector{nz}((nx + 1):(nx + nz))
   idx_r1 = SVector{3}(1:3)
   idx_q1 = SVector{4}(4:7)
   idx_r2 = SVector{3}(8:10)
   idx_q2 = SVector{4}(11:14)

   ForwardDiff.gradient(_θ -> lag_con_part(capsule,cone,x,s,z,_θ[idx_r1],_θ[idx_q1],_θ[idx_r2],_θ[idx_q2],idx_ort,idx_soc1,idx_soc2), [capsule.r;capsule.q;cone.r;cone.q])
end

If I also want to write a function that gives me the Hessian, i.e.,

$$H = \frac{\partial^2\alpha^\star}{\partial(r, q)^2}$$

would simply replacing

ForwardDiff.gradient(_θ -> lag_con_part(capsule,cone,x,s,z,_θ[idx_r1],_θ[idx_q1],_θ[idx_r2],_θ[idx_q2],idx_ort,idx_soc1,idx_soc2), [capsule.r;capsule.q;cone.r;cone.q])

with

ForwardDiff.hessian(_θ -> lag_con_part(capsule,cone,x,s,z,_θ[idx_r1],_θ[idx_q1],_θ[idx_r2],_θ[idx_q2],idx_ort,idx_soc1,idx_soc2), [capsule.r;capsule.q;cone.r;cone.q])

work?

Thanks in advance!

TagBot trigger issue

This issue is used to trigger TagBot; feel free to unsubscribe.

If you haven't already, you should update your TagBot.yml to include issue comment triggers.
Please see this post on Discourse for instructions and more details.

If you'd like for me to do this for you, comment TagBot fix on this issue.
I'll open a PR within a few hours, please be patient!

How can I install DCOL

Hi,

I was trying out the repo and I am new to Julia. One issue I got was not being able to install DCOL. It shows that DCOL is not in the Julia registry. Would it be possible to add some instructions on how to install it?

Should I simply replace DCOL with DifferentiableCollisions?

Thanks in advance!

Add support for differentiable polytope geometries

Add the ability to describe a parametrized polytope with θ, such that A(θ)y ≤ b(θ), and differentiate the proximity value α wrt this parameter.

Currently you can achieve this with the following:

using LinearAlgebra
import DifferentiableCollisions as dc 
using StaticArrays
import ForwardDiff as FD 
import FiniteDiff
using BenchmarkTools

"""
This function is where you express how to go from your parameters 
θ to your polytope A(θ)y ≤ b(θ) where y is in the body frame.

In this example, I showed a rectangular prism parametrized by
length width and height. 
"""
function build_primative_Ab(θ::SVector{nθ, T}) where {nθ, T}
    
    len, wid, hei = θ

    A = SA[
         1  0  0.0;
         0  1  0.0;
         0  0  1.0;
        -1  0  0.0;
         0 -1  0.0;
         0  0 -1.0
    ]
    
    b = SA[
        len / 2,
        wid / 2,
        hei / 2,
        len / 2,
        wid / 2,
        hei / 2
    ]
   
    return A, b 
end


"""
build the true Gx≤h constraint in the solver. This is accomplished 
by using the A and b from `build_primative_Ab`, scaling the b by 
α, and expressing the intersection in the world frame. Details are
all in the paper.
"""
function build_lp_cons(θ, r, q)
    n_Q_b = dc.dcm_from_q(q)
    A, b = build_primative_Ab(θ) 
    AQt = A * n_Q_b'
    G = [AQt  -b]
    h = AQt * r
    return G, h 
end


""" stack up problem matrices """
function problem_matrices(θ1, r1, q1, θ2, r2, q2)
    
    G1, h1 = build_lp_cons(θ1, r1, q1)
    G2, h2 = build_lp_cons(θ2, r2, q2)
    
    G = [G1; G2]
    h = [h1; h2]
    
    return G, h 
end


"""
Calculate the proximity α between two polytopes, where the polytope
geometries are parametrized with θ such that A(θ) y ≤ b(θ) when y 
is in the body frame. The pose of each polytope is expressed with a
position r and quaternion q. Optional argument of diff returns the 
derivatives of α wrt all the inputs, in that order. 
"""
function proximity(θ1, r1, q1, θ2, r2, q2; diff = false)
    
    q = SA[0, 0, 0, 1.0]
    
    
    G, h = problem_matrices(θ1, r1, q1, θ2, r2, q2)

    n_ort = length(h)
    idx_ort = SVector{n_ort}(1 : n_ort)
    idx_soc = SVector{0,Int}(1:0)
    x, s, z = dc.solve_socp(q, G, h, idx_ort, idx_soc, idx_soc; verbose = false)
    
    if diff 
        # first body 
        dα_dθ1 = FD.gradient(_θ1 -> lagrangian(_θ1, r1, q1, θ2, r2, q2, x, z), θ1)
        dα_dr1 = FD.gradient(_r1 -> lagrangian(θ1, _r1, q1, θ2, r2, q2, x, z), r1)
        dα_dq1 = FD.gradient(_q1 -> lagrangian(θ1, r1, _q1, θ2, r2, q2, x, z), q1)
        # second body 
        dα_dθ2 = FD.gradient(_θ2 -> lagrangian(θ1, r1, q1, _θ2, r2, q2, x, z), θ2)
        dα_dr2 = FD.gradient(_r2 -> lagrangian(θ1, r1, q1, θ2, _r2, q2, x, z), r2)
        dα_dq2 = FD.gradient(_q2 -> lagrangian(θ1, r1, q1, θ2, r2, _q2, x, z), q2)
        
        return x[4], dα_dθ1, dα_dr1, dα_dq1, dα_dθ2, dα_dr2, dα_dq2
    else
    
        return x[4]
    end
end


""" 
constraint part of the lagrangian, evaluated at a primal-dual solution x, z 
allows for the gradients of the proximity α (which is also the objective value)
to be computed wrt all of the inputs. See the paper for more details. 
"""
function lagrangian(θ1::SVector{nθ1, T1},
                    r1::SVector{3, T2},
                    q1::SVector{4, T3},
                    θ2::SVector{nθ2, T4},
                    r2::SVector{3, T5},
                    q2::SVector{4, T6},
                    x::SVector{4, T7},
                    z::SVector{nz, T8}) where {T1, T2, T3, T4, T5, T6, T7, T8, nθ1, nθ2, nz}
    
    G, h = problem_matrices(θ1, r1, q1, θ2, r2, q2)
    
    z' * (G * x - h)
end
    
    
    

let
    
    # create the dcol objects with length, width, height 
    # attitude is quat here, but we can use mrp as well with attitude = :MRP
    θ1 = SA[4,3,2.0] # len, wid, hei 
    P1,m1,J1 = dc.create_rect_prism(θ1...; attitude = :quat)
    θ2 = SA[2,4,1.0] # len, wid, hei 
    P2,m2,J2 = dc.create_rect_prism(θ2...; attitude = :quat)

    # set position and attitude of each primitive 
    r1 = 1.5*(@SVector randn(3))
    P1.r = r1
    q1 = normalize((@SVector randn(4)))
    P1.q = q1
    r2 = 1.5*(@SVector randn(3))
    P2.r = r2
    q2 = normalize((@SVector randn(4)))
    P2.q = q2
    
    # calculate proximity with dc 
    α,x = dc.proximity(P1, P2)
    α2 = proximity(θ1,r1,q1,θ2,r2,q2)
    @show abs(α - α2)

    # calculate derivatives 
    α2, dα_dθ1, dα_dr1, dα_dq1, dα_dθ2, dα_dr2, dα_dq2 = proximity(θ1,r1,q1,θ2,r2,q2; diff = true)
    
    dα_dθ1_v2 = FiniteDiff.finite_difference_jacobian(_θ1 -> [proximity(_θ1,r1,q1,θ2,r2,q2)], θ1)
    dα_dr1_v2 = FiniteDiff.finite_difference_jacobian(_r1 -> [proximity(θ1,_r1,q1,θ2,r2,q2)], r1)
    dα_dq1_v2 = FiniteDiff.finite_difference_jacobian(_q1 -> [proximity(θ1,r1,_q1,θ2,r2,q2)], q1)
    dα_dθ2_v2 = FiniteDiff.finite_difference_jacobian(_θ2 -> [proximity(θ1,r1,q1,_θ2,r2,q2)], θ2)
    dα_dr2_v2 = FiniteDiff.finite_difference_jacobian(_r2 -> [proximity(θ1,r1,q1,θ2,_r2,q2)], r2)
    dα_dq2_v2 = FiniteDiff.finite_difference_jacobian(_q2 -> [proximity(θ1,r1,q1,θ2,r2,_q2)], q2)
    
    # compare to finite difference 
    @show norm(dα_dθ1_v2), norm(dα_dθ1 - vec(dα_dθ1_v2))
    @show norm(dα_dr1_v2), norm(dα_dr1 - vec(dα_dr1_v2))
    @show norm(dα_dq1_v2), norm(dα_dq1 - vec(dα_dq1_v2))
    @show norm(dα_dθ2_v2), norm(dα_dθ2 - vec(dα_dθ2_v2))
    @show norm(dα_dr2_v2), norm(dα_dr2 - vec(dα_dr2_v2))
    @show norm(dα_dq2_v2), norm(dα_dq2 - vec(dα_dq2_v2))
    
    # benchmark 
    @btime proximity($θ1,$r1,$q1,$θ2,$r2,$q2)
    @btime proximity($θ1,$r1,$q1,$θ2,$r2,$q2; diff = true)
    
end

with output:

abs(α - α2) = 1.1940712352664917e-5
(norm(dα_dθ1_v2), norm(dα_dθ1 - vec(dα_dθ1_v2))) = (0.3628352598311084, 5.909399170654912e-6)
(norm(dα_dr1_v2), norm(dα_dr1 - vec(dα_dr1_v2))) = (0.41931204594961624, 4.6833014632811656e-5)
(norm(dα_dq1_v2), norm(dα_dq1 - vec(dα_dq1_v2))) = (2.677445106023322, 0.00020442880520120344)
(norm(dα_dθ2_v2), norm(dα_dθ2 - vec(dα_dθ2_v2))) = (0.3628053586223467, 7.136829217209465e-5)
(norm(dα_dr2_v2), norm(dα_dr2 - vec(dα_dr2_v2))) = (0.4193120548908776, 4.684328490570112e-5)
(norm(dα_dq2_v2), norm(dα_dq2 - vec(dα_dq2_v2))) = (0.888174774939717, 0.0005394122785190625)
  2.662 μs (0 allocations: 0 bytes)
  3.312 μs (0 allocations: 0 bytes)