bemilio
commented
5 years ago Hi Alexander, Are you still working on this? I was thinking about how to avoid terrain penetration between nodes and this is what I came up with so far (the problem is not yet solved though).
(I write in Latex, there are plugins available to visualize it in Github)
Let's assume a polynomial $$ p(t) = a t^3 + b t^2 + c t + d $$ defined over $t \in [0,1]$ (note that if the polynomial is defined over $ t' \in [t_0; t_0 + \Delta t]$ we can get back to the nominal case with a simple transformation ).
We know $p(0), p(1), \dot{p}(0), \dot{p}(1)$ and we assume that both $p(0), p(1) >0$.
We want to find the conditions on the coefficients such that $p(t) < 0 $ for some $t$. It is easy to see that the minimum of the polynomial is given at time $$ t_{min} = \frac{-2b + \sqrt{4b^2 -12 ac } } {6a} $$
There are the following possibilities:
- $4b^2 - 12 ac <0$ No real roots, so the minima are at the either at $t=0$ or $t=1$, so $p(t)>0 \forall t$
- $t_{min} \notin [0,1]$ the minima are at the either at $t=0$ or $t=1$, so $p(t)>0 \forall t$
- $tmin \in [0,1]$ in this case the $p(t)>0 \forall t$ if and only if $p(t{min})$ is greater than zero, which can be readily checked.
So the idea would be to find the coefficients $a,b,c,d$ from the parameters of the Hermite spline and do a series of checks to assess whether the minimum is lower than 0. If this case occurs, the only solution I can think for now is to adjust the parameters found by the optimization by solving the optimization problem
$$ \min || \theta - \theta ' || $$ $$ s.t. p(t_{min}) > 0 $$ Where
$$ \theta := [ p(0), p(1), \dot{p}(0), \dot{p}(1)] $$ The equation defining the constraint is pretty nasty, though.
I will think again about the problem after better reading the code and maybe I'll open a PR
ferrolho
awinkler
prakharg01
While a foot is swinging, it is typically represented by two/three polynomials.
Two types of constraints are active in this phase:
terrain_constraint
Problem:
swing_constraint.h
Problem:
Possible solution:
In order to solve the above problems, possibly a cost term is the most general way to go. One could think about a cost that penalizes distance of the foot to the terrain. By not formulating it only on the node values, but directly on the generated polynomial queried at predefined intervalls, the solver would try to avoid penetration with the ground at all times during swing. Furthermore, the leg would automatically lift off the ground as far as the range-of-motion permits during a step.
Another interesting cost to test, instead of the swing_constraint, is penalizing accelerations of the feet. This would automatically create smooth swinging motions. Since acceleration not part of the node variables (these only include pos/vel), this cost would also have to be formulated on the spline, queried at predefined intervalls (similar to this constraint).