Question about exporting collision detection to CasADi expressions

[Zkjoker]

Dear Pinocchio Team,
I'm currently using Pinocchio with CasADi for robot motion planning and truly appreciate the interface for kinematics symbolic conversion.

I was wondering if I might kindly seek your expertise on collision detection integration:
Core question:
Is there currently a way to export collision detection results (e.g., minimal distance between links/obstacles) as CasADi symbolic expressions?

Use case:
I aim to incorporate collision avoidance either:
As hard constraints (e.g., distance(q) ≥ d_safe)
Or as soft constraints in the objective function (e.g., penalty terms)
If possible, could you please advise:

Whether such functionality exists? (Any minimal example would be immensely helpful!)
If not yet supported, are there any planned implementations or recommended workarounds?
I completely understand this might require significant effort to clarify, so thank you sincerely for your time and insights. Your guidance would greatly advance my project!

Best regards,
Kai

[jorisv]

Hello @Zkjoker,

First, to clarify, were you seeking the derivative of the distance function ?

The tricky thing is that hpp-fcl (the collision library used by pinocchio) doesn't play well with Casadi.
hpp-fcl use an iterative algorithm to compute minimal distance that can't be modeled with Casadi scalar type.

However, I found a helpful tutorial that uses ellipsoid representations for robot bodies and calculates the minimal distance analytically using Casadi: tutorial . I believe this could be a valuable resource for your work.

I hope this can help.

[jcarpent]

To complement the answer provided by @jorisv, the distance function is always differentiable and can be easily obtained by following the seminal work of Faverjon and Tournassou: https://inria.hal.science/inria-00075608/document. Although this work is old, it remains very up to date.

[Nicosoh]

Hi @jorisv,

Can I confirm that it is not possible to cast the minimum distance between 2 meshes to a CasADi object? I am trying to perform something similar to OP with a gradient based solver(ACADOS).

Currently I am setting the constraints by using cpin.forwardKinematics to derive the end effector's location then manually defining the distance function inside my solver. But it would be much cleaner to define it in pin and then export it.

I also did read #2700 but I am not looking for collision detection but distance computation between meshes.

[jorisv]

It's not possible right now.

Currently I am setting the constraints by using cpin.forwardKinematics to derive the end effector's location then manually defining the
distance function inside my solver. But it would be much cleaner to define it in pin and then export it.

This is the simplest solution in my point of view if you want to use casadi only. You can also compute the distance with pinocchio and the derivative separately through the Faverjon and Tournassou work (see how they compute $\dot{d}$ through the jacobians). But then, you will not have a full casadi computation.