Improve / robustify swing-leg motion

[awinkler]

While a foot is swinging, it is typically represented by two/three polynomials.

Two types of constraints are active in this phase:

terrain_constraint

• enforces that the position of the nodes connecting the swing-leg polynomials is above the terrain height.

Problem:

• The 3rd order polynomial can still penetrate at all other points in time.
• The lifting of the legs is implicitly generated by this constraint and IPOPT, but not very clearly defined.

swing_constraint.h

• enforces that the foot moves in a straight line in xy from the lift-off to the touchdown position by constraining the swing-node. It also constrains this swing-node's velocity to a hardcoded value, to avoid very fast swinging motions.

Problem:

• The hardcoded velocity value as well as moving always in a straight xy line is not nice and artificially restricts the motion.

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).

[bemilio]

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 &lt;0$
  No real roots, so the minima are at the either at $t=0$ or $t=1$, so $p(t)&gt;0 \forall t$
• $t_{min} \notin [0,1]$
  the minima are at the either at $t=0$ or $t=1$, so $p(t)&gt;0 \forall t$
• $t_min \in [0,1]$
  in this case the $p(t)&gt;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]

Hey, Alex! 🙂 You mentioned:

The 3rd order polynomial can still penetrate at all other points in time.

Do you think the clipping of the stairs in the picture below is due to this issue?

[awinkler]

Yes, that's the effect. The solver doesn't take into account the stair-step height. It only sees the height of the final foothold that it wants to reach, but might connect them in a straight line (falsely going right through the stairs). A hack would be to increase the step height, but a better way would be to add a constraint that takes into account that terrain height information in between.

[ferrolho]

Thank you for the quick reply! I'll find a workaround for now and keep it in mind for future work. 👍

[awinkler]

Stay healthy! :)

[prakharg01]

Hey, Alex! slightly_smiling_face You mentioned:

The 3rd order polynomial can still penetrate at all other points in time.

Do you think the clipping of the stairs in the picture below is due to this issue?

Hi, were you able to solve this issue?

[ferrolho]

Hi, @prakharg01! :) Kinda; but not within towr. For my use case, I ended up taking the footsteps from towr and then specifying the paths for the feet to follow during swing phases myself. I used a cubic B-spline (zero velocity at start and end points) defined by three waypoints: initial and final positions, and then a midpoint. For the midpoint, I took the literal midpoint in the x and y coords; and for the z-coord, I took the max z-coord from the initial and final points and then just added something like 4 cm to that. This approach was certainly not elegant, but it worked quite well. Hope this helps!

[prakharg01]

Oh great!

I will definitely try this.

Thanks a lot!