Whole Body Self Collision Barrier

[domrachev03]

This PR continues a series of PRs, aimed at the introduction of Control Barrier Functions, mentioned in #86. Namely, me and @simeon-ned implemented a whole-body self-collision avoidance barrier, based on Pinocchio's collision model. Namely, it ensures the distance between any collision pair is greater than constant:

$$h(q) = \begin{bmatrix} \ldots \ d(p^1_i, p^2_i) - d_{min} \ \ldots \end{bmatrix} \quad \forall i \in 0 \ldots N$$

where $N$ is number of collision pairs, $p^k_i$ is the $k-th$ body in $i$-th collision pair,
$d(p^1_i, p^2_i)$ is the distance between collision bodies in the pair,
and $d_{min}$ is minimal distance between any collision bodies.

The functionality is demonstrated on the custom model of two Kuka iiwa14 manipulators:

Screencast.from.06-25-2024.01.20.45.PM.webm

There are some notable modifications outside of the barrier class:

1. Barriers now cache the computation of jacobians and barrier values to avoid recomputation.
2. configuration now (optionally) accepts collision model and path to an srdf file to handle
   collisions. Note that back-compatibility is preserved, and collisions are not computed if the model is not provided
   We are kindly asking you to review those changes and make any comments on things you think are done incorrectly.

Note that while it is more expressive than Spherical Barrier, it is also much more computationally expensive. Therefore, we believe that those two barriers have different application scenarios, and should co-exist in this package.

Any further suggestions would be welcome, waiting for your feedback!

P.S. Unit Tests have not been added yet, we want first to make sure we agree on the details of implementation, and then we would quickly add them before merging.

[coveralls]

Pull Request Test Coverage Report for Build 9829262525

Warning: This coverage report may be inaccurate.

This pull request's base commit is no longer the HEAD commit of its target branch. This means it includes changes from outside the original pull request, including, potentially, unrelated coverage changes.

• For more information on this, see Tracking coverage changes with pull request builds.
• To avoid this issue with future PRs, see these Recommended CI Configurations.
• For a quick fix, rebase this PR at GitHub. Your next report should be accurate.

Details

• 195 of 238 (81.93%) changed or added relevant lines in 10 files are covered.
• No unchanged relevant lines lost coverage.
• Overall coverage decreased (-2.3%) to 96.65%

---

Changes Missing Coverage	Covered Lines	Changed/Added Lines	%
pink/tasks/com_task.py	41	42	97.62%
pink/configuration.py	10	12	83.33%
pink/barriers/barrier.py	10	13	76.92%
pink/utils.py	1	6	16.67%
pink/barriers/self_collision_barrier.py	10	42	23.81%

Totals
Change from base Build 9594397741:	-2.3%
Covered Lines:	1702
Relevant Lines:	1761

---

💛 - Coveralls

[coveralls]

Pull Request Test Coverage Report for Build 10041989314

Warning: This coverage report may be inaccurate.

This pull request's base commit is no longer the HEAD commit of its target branch. This means it includes changes from outside the original pull request, including, potentially, unrelated coverage changes.

• For more information on this, see Tracking coverage changes with pull request builds.
• To avoid this issue with future PRs, see these Recommended CI Configurations.
• For a quick fix, rebase this PR at GitHub. Your next report should be accurate.

Details

• 328 of 329 (99.7%) changed or added relevant lines in 14 files are covered.
• No unchanged relevant lines lost coverage.
• Overall coverage increased (+0.1%) to 99.077%

---

Changes Missing Coverage	Covered Lines	Changed/Added Lines	%
pink/tasks/com_task.py	41	42	97.62%

Totals
Change from base Build 9594397741:	0.1%
Covered Lines:	1825
Relevant Lines:	1842

---

💛 - Coveralls

[domrachev03]

Sorry for the absence, we've rewritten an example as you've suggested. Now only srdf file left, which represents which collision pairs should be removed.

Additionally, we've implemented the tests, which cover whole new functionality.

To sum up, we've implemented all discussed fixes, rewrote an example and added tests. Waiting for your feedback!

[domrachev03]

@stephane-caron I think everything is ready from our side to merge, so if you see no problems, I think we could merge those barriers. What do you think?

[stephane-caron]

My apologies for taking longer than usual on this PR (due in part to attending a conference last week). The PR is in excellent condition, let's merge.

Thank you for this excellent work 🤩