Example in Posa et. a. 2013: A direct method.....

[Peter230655]

I am playing around with the example of two link fingers as shown as figure 3 on page 12 of the a.m. paper.
I simply cannot get it to converge, and I use the 'smooth function' approach, not the slack variable one.
I model the impact as a strong linear spring, I guess o.k. if the bodies are made from e.g. steel.

Central to the problem is what they call $\phi(q)$ the closest distance between the finger ellipse and the (green) target ellipse.
$\phi(q)$ is some complicated nonlinear equation with at least two solutions. (if c1, c2 are the points of the ellipses where the distance is minimal, then a necessary condition is that the tangents at c1, c2 be parallel. There are therefore at least tow possible solutions).

Call this equation distance. So I add distance to the equations of motion : eom = eom.col_join(sm.Matrix(distance)

My question:
Could it be that the non-uniqueness of the solution of distance could cause the problem with convergence?

(it is so simple mechanically that I cannot come up with another reason)

NB: to make sure, my eoms are correct I simply integrated them, The motion looks as per expectation.

[tvdbogert]

That should be a nice test problem.

Isn't there only one set of c1 and c2 where the distance is minimal? I am curious how you found the function $\phi(q)$.

If you derived it only from the condition of parallel tangents, you could also be getting the maximal distance. Perhaps it is easy to see which one is the minimum.

[Peter230655]

Actually, I found it by discussing with chatGPT;
chatGPT calculated the distance between arbitrary points $c_1 \in$ circumference of ellipse1, $c_2 \in$ circumference od ellipse2 and then one should numerically minimize this distance.
I do not know how to implement $min_{q_1, q_2, q_3} distance(q_1, q_2, q_3, params)$ in opty.
So I told chatGPT that a necessary condition for ||$c_1 - c_2$|| = min ist that the tangents at $c_1, c_2$ must be parallel. Then it returned a nonlinear equation, which I added to the equations of motion.
I do not think, there is an explicit equation to give the minimum.

I made a plot to see if it did give the minimum distance, and found that it does, depending on the initial guess for scypy.root. With a bad initial guess it would give some other distance.

The minimum distance depends continuously on the locations of the ellipses, so I thought this would work.

I do not know, whether this is the problem, I just do not know what else it might be.

NB:
In that paper, they write that their finger easily found the green ellipse. opty also finds the green ellipse easily, and the animation looks reasonable, but it says it does not converge.

[Peter230655]

I finally managed to get it to converge for certain initial conditions.
I put it here: https://pydy.org/pst-notebooks/examples/plot_ellipses_Posa_2013.html#sphx-glr-examples-plot-ellipses-posa-2013-py
I still suspect the distance function to be the culprit of the difficult convergence.
Maybe I will try something similar with discs, where the distance function is trivial.

[moorepants]

nice work!

it would also be interesting to see if you can do it with slack variables

[Peter230655]

Thanks! :-)

I doubt it for these reasons:

• Already the way I did it it was very difficult to get it to converge. For example when I changed k_spring form 5,000 to 10,000 it would not converge. As per my little experience, convergence of opty is easier with smooth functions.
• What would be the force when the tow bodies get in contact? With Coulomb friction it was clear, but here I would not know. I guess it would be like a Newtonian impact, but no idea how to do this if two moving bodies are involved, not a body and a wall.

NB:
I do not think, we have such an example in opty/examples, yet: Here are two 'unrelated' bodies. opty has to do somethnig with body A to make body B do something else. opty easily found that it must use body A to hit body B - not so easy to achieve as per Posa's paper.
(when I say convergence was difficult, body A hitting body B was never any issue, this always worked)

I am thinking about a similar taks for opy, but using discs instead of ellipses, then the distance function is trivial.
Should it work out, I will push it to opty/examples.

[moorepants]

Why do you doubt the slack variable solution would work? Isn't that what Posa et al claims?

[Peter230655]

Yes, they say so, but they give no details.

My doubt comes from this:
When I played around with your plot_friction_slack.py simulation, I compared it to a simulation identical to yours, except I used a smooth function (I used -tanh(steep * v(t)) to approximate the direction of Fb) .
The ''slack_variable version'' did not converge for random initial guesses, the ''tanh version'' converged for just about any initial guesses.
(and the results were 'good enough for Government work' as Ton said :-) )

Here even the ''tanh version'' did not converge for most initial guesses I tried.

I think in my simulation the problem is what they call $\phi(q)$, the distance between the potential contact points. Maybe they found a better function for this than me.

They do not say, how to get the force when the two ellipse collide.