Proposal for Integration of Control Barrier Functions (CBFs) into pink

[domrachev03]

Hello Dr. Caron,

First of all, thank you for maintaining the pink repository. I found it extremely useful in practice.

My colleague @simeon-ned and I have been actively working on the concept of Control Barrier Functions (CBFs) in robotic applications. This approach integrates seamlessly into QP-like formulations and it was intriguing for us to incorporate it into pink to ensure safety limitations, specifically solving IK with inequality constraints:

$$h(q) \geq 0$$

We have added this functionality in pink. You can find our forked repository here: link. The notes on the related theory can be found in the example notebook.

So far, we have added a new Barriers abstract class, along with several practical barriers:

• PositionBarrier: PositionBarrier implementation
• ConfigurationBarrier: ConfigurationBarrier implementation (can be seen as an alternative to limits)
• Body Spherical Barrier: Body Spherical Barrier implementation (for self-collision avoidance)

Documentation for these features, based on our fork, can be found on our local copy of GitHub pages: Barriers Documentation.

To demonstrate the capabilities of these features, we have developed two examples:

1. Joint and End Effector Barriers in UR5 Robot:
   • UR5 Robot Example Code

ur5_example.mp4

2. Spherical Self Collisions in Two Arms Yumi Robot:
   • Yumi Robot Example Code

yumi_example.mp4

We are eager to contribute these enhancements to your repository, believing they will greatly benefit the community. We are prepared to develop comprehensive tests and consider the integration of more complex geometries for self-collision management using the HPP FCL library, pending your approval.

We look forward to your feedback and hope for a positive collaboration.

Best regards,

Ivan Domrachev

[luoleicn]

@domrachev03 hi, thanks to your greate work, I tried it but I found it dont work for floating base robot, because Hessian matrix from Task is based on velocity vector space, and Hessian matrix from Barrier is based on configuration vector space, they dont match for floating base robot

[domrachev03]

Hello, @luoleicn, thank you for your interest in our solution!

It can be the case, because there is no test for floating base model in our fork. Could you provide your code so that we could debug it and fix dimension issues?

[jcarpent]

A small hint for extending your code to more complex configuration space is to use pin.difference(model,q0,q1) to properly measure the difference between two configuration vectors defined on Lie groups, as the one occurring for floating-base systems.

[simeon-ned]

@jcarpent Exactly! Thanks for reminding us about the magic of tangent space – can't believe we overlooked it, will fix it soon. Also, huge thanks for Pinocchio, I'm using it every day and it's awesome!

[jcarpent]

Very glad to see that Pinocchio may become an elegant, versatile and useful toolbox for roboticists 🤗

[simeon-ned]

@jcarpent Certainly it is! Looking forward to v3 :).

I just went through our code, turns out we used the correct difference to subtract the two configurations in the configuration barrier, however it may be the case that we forgot to use it somewhere. For now, the problem is that we just used joint dim nq instead of nv in hessians and gradients as @luoleicn mentioned. I'll fix this today or tomorrow and post an example with some robot with floating joints, like Unitree Go2.

[stephane-caron]

This looks great @domrachev03 @simeon-ned 🤩 It would be a significant improvement for Pink, and I'd be happy to help review and integrate it.

If you can split it into successive functional PRs (for instance starting with the positional barrier, adding unit tests, then adding the other barriers one by one), it will probably make it smoother for us to review and merge them progressively.

Feel free to open a PR when you are ready 👍

[simeon-ned]

Thank you @stephane-caron for the positive feedback! We're really glad to contribute and are excited to see this improvement for Pink. We'll definitely split the work into successive functional PRs as you suggested, starting with the positional barrier and its unit tests. You can expect the first PR to be opened this week. Looking forward to collaborating on this! 👍

[simeon-ned]

@luoleicn we fixed the floating base issue and added an example of go2 robot performing squats with constraints $y\leq0, z\leq0.35$, thanks for pointing out this bug.

go2_example.mp4