trajectory tracking problem

[QingweiLau]

Hi anassinator, great work and thanks for the contribution. I am wondering if this can handle problems with with one step cost being L(X; u) = (X-X*)TQ(X-X*)+ (u-u*)TR(u-u*), where X* and u* are my target trajectory? @anassinator

[anassinator]

Hi @Charvelau,

Thanks! Yes it should. You can see #1 for how to use PathQRCost to do just that. I haven't tested that part out much yet though, so it would be interesting to see your results.

[QingweiLau]

thanks, will try it out and let you know if got any result ^-^

[QingweiLau]

Hi @anassinator, I have defined my system and tried the PathQRCost class, the cost function works all right but it's stuck on xs, us = ilqr.fit(x0, us_init, on_iteration=on_iteration)
Cause the comment system is not quite friendly so I have uploaded a copy here
https://github.com/Charvelau/test/blob/master/gaitgen.ipynb

Currently the x_path and u_path are both zero matrices.

There seems to be some thing wrong with N > T, and with N= T, there is still a named
UnboundLocalError: local variable 'k' referenced before assignment

Could you help to find where the problem is ? And thanks in advance.
Merry Christmas ! Cheers

[anassinator]

@Charvelau

I think I just fixed the UnboundLocalError you're getting so it won't crash in 322658e. It still won't converge as is with what you have right now though.

The issue is that you're dividing by zero when z = 0 and that yields NaNs everywhere. (This was consequently not setting the k variables and crashing). So unless you change how you write out the system's dynamics to avoid a division by zero or start at z != 0. I tested starting it at z = 1 and it converged for me in 11 steps (not sure if it converged to what you want though :P).

Hope this helps!

[QingweiLau]

Hi @anassinator
yeah, sorry about forgetting z should not be anyplace near 0. thanks a lot. I will add practical constraints and desired path.
thanks again and it helps a lot!

cheers

[anassinator]

@Charvelau I'm closing this for now

[QingweiLau]

Dear @anassinator,
Sorry for holding so long for implementing your helpful algorithm, been doing another project.
The thing is, I have deployed my real targeting trajectory for both x_input and u_input, and it can converge. But I just cannot figured out how to implement the constraint for u with tensor_constrain, I have noticed that you created a separate class for your dynamic model, do I have to do the same thing or can I just add several lines in the main code. Say, my constraint for u_1 is [0,100], u_2 [0,20] and u_3 [0 9.8]. Sorry for the inconvenience, I am not quite familiar with python actually, using matlab most of the time. :P).
I have updated my file in https://github.com/Charvelau/test/blob/master/gaitgen.ipynb.
Thanks very much, and really appreciate.
Cheers.
All the best.

Qingwei

[anassinator]

Hi @Charvelau,

You don't need to create a separate class, that's just to make things clearer. I believe all you need to do is something as follows:

# Constrain the inputs.
min_bounds = np.array([0.0, 0.0, 0.0])
max_bounds = np.array([100.0, 20.0, 9.8])
u_constrained_inputs = tensor_constrain(u_inputs, min_bounds, max_bounds)

# Build your dynamics model f using u_constrained_inputs instead of u_inputs.
# You still need to give u_inputs and not u_constrained_inputs to AutoDiffDynamics.
...

# Constrain the solution.
xs, us_unconstrained = ilqr.fit(x0, us_init)
us = constrain(us_unconstrained, min_bounds, max_bounds)

Hope this helps.

[QingweiLau]

Dear @anassinator , it does help. I have updated the constraint, and it is working. And it also converge, but just not the way I am looking forward to, xs is quite far from x_input (just optimized in the sense of the first element of x_input) and us seems to be stuck on the bounds. I will work on parameter tuning. Hope will get a satisfactory result. Thanks very much.
Cheers. All the best.

Charvelau

[anassinator]

I haven't been able to take a look at your system, so I'm not sure if I understood what you mean exactly -- especially the "first element" part.

[QingweiLau]

Dear @anassinator, I mean, the first row of x_input, which is [10,5,8,0,0,0] in my case. While actually I expect xs to rack the entire trajectory of x_input.
Thanks a lot.
Charvelau

[anassinator]

Oh! You mean only x0 is correct (which is always the case). Maybe the desired path is infeasible with the constraints set. Otherwise I'd try generating the random path differently -- a random walk for example might converge more easily.

[QingweiLau]

So maybe I should change my constraints or something. I have change the constraints for u and the fitted xs is the same. Actually I think it may try to get close to the desired x_input, but always only the x0 is tracked
-_- Kind of stuck right now.