csu-hmc
[WIP] Added Ackermann & van den Bogert 2010 example.
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*durationwhich 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
I updated gait2d to work with modern dependencies here: https://github.com/csu-hmc/gait2d/pull/3
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.
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.
When the discrete variables are replaced in, they likely don't retain the assumptions placed on the continuous variables.
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.
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 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!
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! :-)
First solution that converged!
https://github.com/user-attachments/assets/f3079089-3f8f-44c5-9347-026dae35f364
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!😊😊
I changed the problem to simply find a solution from state 1: standing still upright to state 2: standing still upright 2 meters in front of the state 1 and it actually "walked". I had been trying to find a cyclic stride solution (mid-walking) and still have that speed constraint issue. But this shows that opty can solve the model and that I'm probably close to a more realistic solution.
https://github.com/user-attachments/assets/02ae41b8-5720-47da-ab58-2fa2466014f2
I changed the problem to simply find a solution from state 1: standing still upright to state 2: standing still upright 2 meters in front of the state 1 and it actually "walked". I had been trying to find a cyclic stride solution (mid-walking) and still have that speed constraint issue. But this shows that opty can solve the model and that I'm probably close to a more realistic solution.
first-2m-walk.mp4
Neat video!
https://github.com/csu-hmc/opty/pull/29/commits/9a8f08fcd2fda577e33b8160d509eb4fb14bb96a gives:
https://github.com/user-attachments/assets/d76e7cc6-3bd9-4efa-a5c2-f1cbb401e11b
https://epubs.siam.org/doi/10.1137/16M1062569
Is example 7 in this paper related to your example? (I stumbled across this by accident)
I think I finally got the actual solution (changed back to the periodic solution!):
I think I finally got the actual solution (changed back to the periodic solution!):
Neat video! Can you let it take a few more steps - maybe even nicer.
Can you let it take a few more steps - maybe even nicer.
Actually no, as it is just the periodic solution. You can loop the animation.
@Peter230655 your suggestion for the average speed constraint with a.diff(t) -1 = 0 was key to getting this! Thank you.
That's definitely it!
This looks really neat, very natural in my opinion! Earlier today, he looked like he had a bit too much to drink.
@Peter230655 your suggestion for the average speed constraint with
a.diff(t) -1 = 0was key to getting this! Thank you.
Pleasure I could help a little bit. :-) Thanks!
@Peter230655 would you like to review this for me? I have it 99% there, maybe I'll tweak a couple things. But it is ready for a review if you are interested.
@Peter230655 would you like to review this for me? I have it 99% there, maybe I'll tweak a couple things. But it is ready for a review if you are interested.
I have followed it closely over the last few days, looks fine to me!
I do not know this utils.f_minus_ma, and could not find it in the docs. Where can I find it pygait2d has to be added to my environment, you say this somewhere in the program.
Oh that function is in opty.utils.
I have no doubt, but if I go there
I only find create_objective_function and parse_free
I see many functions in there: https://github.com/csu-hmc/opty/blob/master/opty/utils.py