Introduction of Barriers, Position barrier

[domrachev03]

This PR is first from the series of PRs, aimed at introduction of Control Barrier Functions, mentioned in #86 . introduces safety sets in the form of Control Barrier Function (CBF) in the presented framework.

For the theory, see our fork, particularly this Jupyter Notebook.

As for implementation, each Barrier defines constraint, and might also define penalization term, if corresponding weight is non-zero. To do this, each barrier has to be able to compute it's value, jacobian, and (optionally) recovery velocity.

Features:

1. General notion of barrier.
2. Position barrier for given frame.
3. Unit tests that cover 100% of presented functionality.

Examples:

1. Manipulator UR5 with limiting y-plane.
2. Quadruped Robot Go2 with limiting x and y planes.

Note that this code already fixes problems, mentioned in the #86. Any further suggestions would be welcome