kevin-tracy / DifferentiableCollisions.jl

Differentiable collision detection for polytopes, capsules, cylinders, cones, spheres, and polygons.
MIT License
164 stars 11 forks source link

Can I also get the Hessian? #8

Open BolunDai0216 opened 1 year ago

BolunDai0216 commented 1 year ago

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!

kevin-tracy commented 1 year ago

I tried this and wasn't able to get it to match up with the finite difference derivatives. Part of this may be due to the challenges associated with finite differencing a second derivative, but I'm not sure.

Have you made any progress on this?

BolunDai0216 commented 1 year ago

Nothing a the moment, I haven't had the capacity to spend much time on the project that uses the Hessian.