stephane-caron / pink

Python inverse kinematics based on Pinocchio
Apache License 2.0
169 stars 11 forks source link

Whole Body Self Collision Barrier #94

Open domrachev03 opened 1 week ago

domrachev03 commented 1 week ago

This PR continues a series of PRs, aimed at the introduction of Control Barrier Functions, mentioned in https://github.com/stephane-caron/pink/issues/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^2i) - 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^2i)$ 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 commented 1 week ago

Pull Request Test Coverage Report for Build 9667484320

Details


Changes Missing Coverage Covered Lines Changed/Added Lines %
pink/barriers/barrier.py 11 14 78.57%
pink/configuration.py 6 13 46.15%
pink/barriers/self_collision_barrier.py 10 42 23.81%
<!-- Total: 28 70 40.0% -->
Totals Coverage Status
Change from base Build 9594397741: -2.6%
Covered Lines: 1539
Relevant Lines: 1597

πŸ’› - Coveralls
coveralls commented 1 week ago

Pull Request Test Coverage Report for Build 9698139914

Details


Changes Missing Coverage Covered Lines Changed/Added Lines %
pink/barriers/barrier.py 11 14 78.57%
pink/configuration.py 6 13 46.15%
pink/barriers/self_collision_barrier.py 10 42 23.81%
<!-- Total: 28 70 40.0% -->
Totals Coverage Status
Change from base Build 9594397741: -2.6%
Covered Lines: 1539
Relevant Lines: 1597

πŸ’› - Coveralls
coveralls commented 1 week ago

Pull Request Test Coverage Report for Build 9704434480

Details


Changes Missing Coverage Covered Lines Changed/Added Lines %
pink/configuration.py 5 7 71.43%
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%
<!-- Total: 27 69 39.13% -->
Totals Coverage Status
Change from base Build 9594397741: -2.6%
Covered Lines: 1538
Relevant Lines: 1596

πŸ’› - Coveralls
domrachev03 commented 1 week ago

Thank you once again for such detailed feedback. I'm really glad to see it since I learned something new about good practices in coding!

I believe I've addressed the majority of your comments, the only remaining questions are:

  1. Is there a better way to avoid recalculation of Jacobian and bias while computing the barrier's objective and constraints?
  2. How to include a model with two iiwas in the example? Should we add it to the robot_descriptions.py?
  3. Add unit testing (already wip).

Thank you once again for your detailed feedback, waiting for your opinion on first two questions.

simeon-ned commented 1 week ago

Thank you once again for such detailed feedback. I'm really glad to see it since I learned something new about good practices in coding!

I believe I've addressed the majority of your comments, the only remaining questions are:

  1. Is there a better way to avoid recalculation of Jacobian and bias while computing the barrier's objective and constraints?
  2. How to include a model with two iiwas in the example? Should we add it to the robot_descriptions.py?
  3. Add unit testing (already wip).

Thank you once again for your detailed feedback, waiting for your opinion on first two questions.

It seems for me that we can make the double arm model through: pin.appendModel in similar way it is done in flying_dual_arm example

domrachev03 commented 1 week ago

It seems for me that we can make the double arm model through: pin.appendModel in similar way it is done in flying_dual_arm example

Okay, now I see that it is quite simple to change frame names... I'll then rewrite the example in this way and remove model from the example. Then the only question is left is regarding jacobian and bias caching.

coveralls commented 3 hours ago

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.

Details


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%
<!-- Total: 195 238 81.93% -->
Totals Coverage Status
Change from base Build 9594397741: -2.3%
Covered Lines: 1702
Relevant Lines: 1761

πŸ’› - Coveralls
coveralls commented 1 hour ago

Pull Request Test Coverage Report for Build 9829826478

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.

Details


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%
<!-- Total: 195 238 81.93% -->
Totals Coverage Status
Change from base Build 9594397741: -2.3%
Covered Lines: 1702
Relevant Lines: 1761

πŸ’› - Coveralls