New method for friction cone constraints about consim HOT 1 OPEN

Good news: I found out that we don't need the explicit time dependency for considering a constant-velocity anchor point. If we define the state as the difference between contact point and anchor point $\bar{x} = x-x_0$ then the dynamics takes the same form as usual:
\begin{align}
\dot{\bar{x}} = A \bar{x} + b + u
\end{align}
where $u$ is our QP decision variable, representing the anchor point velocities (to be more precise the first half of u must be zero, and the second half contains the anchor point velocities). This is very close to what currently written in the paper, which is what @hammoudbilal is implementing. In terms of computation time, this should be quite fast, but before spending time figuring out how to compute the needed terms efficiently, I'd rather to implement it in an inefficient way in the Python simulator, just to check whether it works. We can leave the computational optimization for the last days as long as they don't affect the results.

from consim.