[WIP] Added Ackermann & van den Bogert 2010 example.

[moorepants]

TODO:

• investigate whether the periodicity constraints are actually working, current solutions do not show equivalent state at beginning and end of the simulation
• figure out how to add an instance constraint such as qax(duration) - speed*duration which would impose a displacement, I don't think instance constraints support h in the expression
• there should be a constraint that prevents the feet from being deeply displaced in the ground (maybe a max on the ground reaction force), otherwise the solver can exploit the system by starting a foot under ground to get a spring start

[moorepants]

I updated gait2d to work with modern dependencies here: csu-hmc/gait2d#3

[moorepants]

I'm currently getting a Derivative term that cause failure:

PrintMethodNotImplementedError: Unsupported by <class 'opty.utils.OptyC99CodePrinter'>: <class 'sympy.core.function.Derivative'>
Set the printer option 'strict' to False in order to generate partially printed code.
> /home/moorepants/miniconda/envs/gait2d-dev/lib/python3.12/site-packages/sympy/printing/codeprinter.py(582)_print_not_supported()
    580     def _print_not_supported(self, expr):
    581         if self._settings.get('strict', False):
--> 582             raise PrintMethodNotImplementedError("Unsupported by %s: %s" % (str(type(self)), str(type(expr))) + \
    583                              "\nSet the printer option 'strict' to False in order to generate partially printed code.")
    584         try:

ipdb> expr
Derivative(qayi, qayi)
ipdb> expr.args
(qayi, (qayi, 1))
ipdb> expr.doit()
1

If I call doit() it reduces to 1.

If one of the equations is qay.diff(t) - uay then it would discretize to (qayp - qayi)/h - uayi or something similar. Then the jacobian would take the derivative of that expression wrt qayi, for example. So a Derivative(qayi, qayi) would show up, but it should give a 1.

[moorepants]

Here are some of the sub expressions showing up in the Jacobian:

(z383 + Abs(qayi)/2)**2*sign(qayi)*Derivative(qayi, qayi)/2 - 3*kc*(z378 + 1)

Derivative(re(z106), z106) + im(z106)*Derivative(im(z106), z106))*(z383 + z450 + z451 + z452)**2*sign(z106)/(2*z106) - 1))

re(z126)*Derivative(re(z126), z126) + im(z126)*Derivative(im(z126), z126))*sign(z126)/(2*z126) - 1), (z1439, 3*kc*z1434*z488**2 + z1420*(3*kc*z488**2*(re(z126)*Derivative(re(z126), z126) + im(z126)*Derivative(im(z126), z126))*sign(z126)/(2*z126) + 1)), (z1440, 3*kc*z1435*z488**2 + z1421*(3*kc*z488**2*(re(z126)*Derivative(re(z126), z126) + im(z126)*Derivative(im(z126), z126))*sign(z126)/(2*z126) + 1)

These are from taking the derivative of the contact force and it seems that assumptions may not be working as I expected. There shouldn't' be real and imaginary parts. This could have changed with a new sympy and the derivative 10 years ago gave a different result.

[moorepants]

When the discrete variables are replaced in, they likely don't retain the assumptions placed on the continuous variables.

[Peter230655]

Here are some of the sub expressions showing up in the Jacobian:

(z383 + Abs(qayi)/2)**2*sign(qayi)*Derivative(qayi, qayi)/2 - 3*kc*(z378 + 1)

Derivative(re(z106), z106) + im(z106)*Derivative(im(z106), z106))*(z383 + z450 + z451 + z452)**2*sign(z106)/(2*z106) - 1))

re(z126)*Derivative(re(z126), z126) + im(z126)*Derivative(im(z126), z126))*sign(z126)/(2*z126) - 1), (z1439, 3*kc*z1434*z488**2 + z1420*(3*kc*z488**2*(re(z126)*Derivative(re(z126), z126) + im(z126)*Derivative(im(z126), z126))*sign(z126)/(2*z126) + 1)), (z1440, 3*kc*z1435*z488**2 + z1421*(3*kc*z488**2*(re(z126)*Derivative(re(z126), z126) + im(z126)*Derivative(im(z126), z126))*sign(z126)/(2*z126) + 1)

These are from taking the derivative of the contact force and it seems that assumptions may not be working as I expected. There shouldn't' be real and imaginary parts. This could have changed with a new sympy and the derivative 10 years ago gave a different result.

In my (primitive, compared to this one) examples abs(..), sign(..), heaviside, peicewise(..) mostly caused trouble, the error often was that "jacobian must not contain infs or NaN" or similar. Probably unrelated to this issue.

[moorepants]

This could also be an effect of the new Jacobian function that does cse before taking the Jacobian. Each of the z variables will not retain the correct assumption that is tied to what it is replacing, thus the im() and re() may appear.

[moorepants]

This could also be an effect of the new Jacobian function that does cse before taking the Jacobian. Each of the z variables will not retain the correct assumption that is tied to what it is replacing, thus the im() and re() may appear.

This is the issue!

[moorepants]

Fix here: #216

[Peter230655]

This could also be an effect of the new Jacobian function that does cse before taking the Jacobian. Each of the z variables will not retain the correct assumption that is tied to what it is replacing, thus the im() and re() may appear.

This is the issue!

This could also be an effect of the new Jacobian function that does cse before taking the Jacobian. Each of the z variables will not retain the correct assumption that is tied to what it is replacing, thus the im() and re() may appear.

This is the issue!

This could also be an effect of the new Jacobian function that does cse before taking the Jacobian. Each of the z variables will not retain the correct assumption that is tied to what it is replacing, thus the im() and re() may appear.

This is the issue!

Glad I did not tackle this! I would have taken me 100 years to find out! :-)

[moorepants]

First solution that converged!

first-walk.mp4

[Peter230655]

So you were right when you said you did not have the time to fix it in the last decade. When you did have the time you fixed it in two hours!😊😊