Comments (29)
thanks, will try it out and let you know if got any result ^-^
from ilqr.
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
from ilqr.
Yes, it works, roughly. I will check the constraints and initial state for x0 and us_init thoroughly and let you know if got any progress.
Thanks for being so helpful. Regards!
Charvelau
from ilqr.
that worked perfectly!! thankyou
from ilqr.
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.
from ilqr.
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
from ilqr.
@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 NaN
s 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!
from ilqr.
@Charvelau I'm closing this for now
from ilqr.
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
from ilqr.
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.
from ilqr.
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
from ilqr.
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.
from ilqr.
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
from ilqr.
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.
from ilqr.
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.
from ilqr.
I have noticed that in the definition of PathQRCost, when calculating x_diff and u_diff, only the i_th elements is considered, will the i loop from zero to N-1 and take a sum?
from ilqr.
Yeah that's summed up here:
Lines 193 to 205 in 066814d
Does it work when the inputs are unconstrained? If so, then the problem should be your constraints.
from ilqr.
Dear @anassinator, in fact trajectory optimization with both desired x_path and u_path does not need constraints, lol. I mean, if it converge close enough to the desired trajectory, both xs and us will fall into reasonable and feasible region, naturally. What I need to do is to tuning the weighting coefficients and u_init.
Charvelau
from ilqr.
Dear @anassinator Sorry to bother you again, the controller works fine, thanks for your great work. A small issue is that the us seems to be of some high frequency oscillation (see pics below), do you have any clue how to eliminate it, through weight tuning or some other parameters maybe? Thanks a lot.
from ilqr.
Oh nice! Is this with the constraints? And are those graphs of us[0] and us[1]? If so, what are they constrained to? Or is one of those the path? And what's the blue curve?
I'm not aware of any way to deal with this off the top of my head, and iLQR doesn't ensure smooth changes in u. There might be a way to take it into account in the cost function. I'm wondering if adding an additional cost term that is proportional to (us[i] - us[i-1])
could work. But the current architecture doesn't support testing that out at the moment. Could be easily extended by adding it here:
Lines 193 to 205 in 066814d
from ilqr.
@anassinator The two pics are my us[0] and us[1] without constraints, with desired being blue and fitted be orange, I haven't uploaded the results for xs, but for xs[0], and xs[1], the fitted curve is quite perfect with relative error rate under 2%, while xs[2] also suffers the problem of oscillation. Currently, no constraints on x or u has been applied, as I have said, if an optimal path can be found, of course it will be in feasible domain, right?
Will try to extend cost with the additional term, and let you know. It may take me quite a while cause not quite familiar with python -_-
Thanks a lot.
Charvelau
from ilqr.
Dear @anassinator , I have added the following code, the idea is that I create a window with length of five, and get the standard deviation of it, then move it forward, and add them up. But it does not work, us seems to be unchanged, just as without the code below.
SmoothCost=0 for i in range(0,self.N-1) a=xs[i:i+5,1] b=us[i:i+5,1] SmoothCost=SmoothCost+np.std(a, ddof = 1)+np.std(b, ddof = 1)*1000 return sum(J) + self.cost.l(xs[-1], None, self.N, terminal=True)+SmoothCost*10
And also, I am trying to discretize my dynamics with RK4 method, this maybe can help. But the way I am defining it seems to be have some problem. Could you take a look? Thank you very much. I have uploaded it @ https://github.com/Charvelau/test/blob/master/gaitgen.ipynb
'#nonlinear dynamics.
def nonldyn(x_inputs, u_inputs):
x_inputs_dot[0]=x_inputs[3],
x_inputs_dot[1]=x_inputs[4],
x_inputs_dot[2]=x_inputs[5],
x_inputs_dot[3]=(x_inputs[0]-u_inputs[0])*u_inputs[2]/x_inputs[2],
x_inputs_dot[4]=(x_inputs[1]-u_inputs[1])*u_inputs[2]/x_inputs[2],
x_inputs_dot[5]=u_inputs[2]-g,
#return x_inputs_dot
return T.stack([
x_inputs_dot[0],
x_inputs_dot[1],
x_inputs_dot[2],
x_inputs_dot[3],
x_inputs_dot[4],
x_inputs_dot[5],]
'''
discritize with RK4
x_inputs_dot1=nonldyn(x_inputs,u_inputs)
x_inputs_dot2=nonldyn(x_inputs+.5dtx_inputs_dot1,u_inputs)
x_inputs_dot3=nonldyn(x_inputs+.5dtx_inputs_dot2,u_inputs)
x_inputs_dot4=nonldyn(x_inputs+dt*x_inputs_dot3,u_inputs)
'''
discritize with RK4
x_inputs_dot1=nonldyn(x_inputs,u_inputs)
x_inputs_dot2=nonldyn(x_inputs+.5dtnonldyn(x_inputs,u_inputs),u_inputs)
x_inputs_dot3=nonldyn(x_inputs+.5dtnonldyn(x_inputs+.5dtnonldyn(x_inputs,u_inputs),u_inputs),u_inputs)
x_inputs_dot4=nonldyn(x_inputs+dtnonldyn(x_inputs+.5dtnonldyn(x_inputs+.5dtnonldyn(x_inputs,u_inputs),u_inputs),u_inputs),u_inputs)
'''
f = x_inputs+(dt/6)(nonldyn(x_inputs,u_inputs)+2nonldyn(x_inputs+.5dtnonldyn(x_inputs,u_inputs),u_inputs)+2nonldyn(x_inputs+.5dtnonldyn(x_inputs+.5dtnonldyn(x_inputs,u_inputs),u_inputs),u_inputs)+nonldyn(x_inputs+dtnonldyn(x_inputs+.5dtnonldyn(x_inputs+.5dt*nonldyn(x_inputs,u_inputs),u_inputs),u_inputs))'
Charvelau
from ilqr.
Hey! Sorry for the delay. I'm busy with exams until Monday, so I won't be able to take a look before then. The code you added seems more complicated than I would've expected it to be.
I was thinking something more like (haven't actually tested this):
# Subtract all next values of us from their previous values
us_diff = us[1:] - us[:-1]
# Compute a scalar cost with some weight vector us_diff_weight
us_diff_cost = us_diff_weight.dot(np.sum(us_diff, axis=1))
return J + ... + us_diff_cost
But, it is very possible for this to not affect the results at all now that I think about it, because this will not be taken into account in the cost function's fist/second-order derivatives (the ones in PathQRCost
).
Maybe another way to deal with this instead if the method above fails would be by extending your state such that it is of size x + u
. And just append the current u
to x_new
as follows:
Given some dynamics model f(x, u) = x_new
Build a dynamics model
f'(x_extended, u) = x_new_extended
such that
x_new_extended[:state_size] = x_new
x_new_extended[state_size:] = u
Then you can change PathQRCost's cost function to something like:
def l(self, x, u, i, terminal=False):
"""Instantaneous cost function.
Args:
x: Current state [state_size].
u: Current control [action_size]. None if terminal.
i: Current time step.
terminal: Compute terminal cost. Default: False.
Returns:
Instantaneous cost (scalar).
"""
Q = self.Q_terminal if terminal else self.Q
R = self.R
x_diff = x - self.x_path[i]
squared_x_cost = x_diff.T.dot(Q).dot(x_diff)
if terminal:
return squared_x_cost
u_diff = u - self.u_path[i]
squared_u_cost = u_diff.T.dot(R).dot(u_diff)
# Add weighted u - u_prev
u_diff_cost = u_diff_weights.dot(u - x[state_size:])
return squared_x_cost + squared_u_cost + u_diff_cost
The first/second-derivative functions (l_x, l_u, l_xx, l_ux, l_uu
) would have to be implemented with this change as well. Or you could use AutoDiffCost
to just do it for you.
I'll have to take a look at the discretization thing after my exams :P
from ilqr.
Yeah sure, thanks for your time and good luck with the exams!
Charvelau
from ilqr.
Sorry I forgot about this. Is this still an issue?
from ilqr.
Does PathQRcost work with the MPC? I have ran into issues where the model get stuck at a point.
from ilqr.
You will need a version of PathQRCost
that adjusts the path by step_size
every receding horizon iteration since the path is indexed by i
now. Otherwise it should work.
from ilqr.
so create replace i
with step_size
and pass that into the cost function?
from ilqr.
You need to either
- Use
iteration * step_size + i
instead of justi
when indexing yourxs_path
/us_path
; or - Keep indexing with
i
, but setxs_path = xs_path[step_size:]
after each iteration (and same forus_path
if used).
In both options, you will need to either keep back-filling xs_path
/us_path
as needed after each iteration to keep them at least N-sized or make sure they're at least N * iteration_count
long.
from ilqr.
Related Issues (11)
- Non-linear dynamics -- Trajectory Following HOT 8
- alpha multiplication with k and K HOT 3
- Add constraints to control and/or state HOT 2
- Result after constraints HOT 1
- Theoretical question HOT 1
- time varying MPC HOT 7
- TypeError: Unknown parameter type: <class 'theano.tensor.var.TensorVariable'> in the CartPole example HOT 2
- Dynamics definition in a vectorized way HOT 4
- Coding Problem HOT 1
- how to deal with the dynamics with a delay? HOT 1
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from ilqr.