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.

[QingweiLau]

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?

[anassinator]

Yeah that's summed up here:

ilqr/ilqr/controller.py

Lines 193 to 205 in 066814d

	def _trajectory_cost(self, xs, us):
	"""Computes the given trajectory's cost.
	Args:
	xs: State path [N+1, state_size].
	us: Control path [N, action_size].
	Returns:
	Trajectory's total cost.
	"""
	J = map(lambda args: self.cost.l(*args), zip(xs[:-1], us,
	range(self.N)))
	return sum(J) + self.cost.l(xs[-1], None, self.N, terminal=True)

Does it work when the inputs are unconstrained? If so, then the problem should be your constraints.

[QingweiLau]

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

[QingweiLau]

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

[QingweiLau]

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.

Charvelau

[anassinator]

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:

ilqr/ilqr/controller.py

Lines 193 to 205 in 066814d

	def _trajectory_cost(self, xs, us):
	"""Computes the given trajectory's cost.
	Args:
	xs: State path [N+1, state_size].
	us: Control path [N, action_size].
	Returns:
	Trajectory's total cost.
	"""
	J = map(lambda args: self.cost.l(*args), zip(xs[:-1], us,
	range(self.N)))
	return sum(J) + self.cost.l(xs[-1], None, self.N, terminal=True)

[QingweiLau]

@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

[QingweiLau]

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

[anassinator]

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

[QingweiLau]

Yeah sure, thanks for your time and good luck with the exams!

Charvelau

[anassinator]

Sorry I forgot about this. Is this still an issue?

[nag92]

Does PathQRcost work with the MPC? I have ran into issues where the model get stuck at a point.

[anassinator]

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.

[nag92]

so create replace i with step_size and pass that into the cost function?

[anassinator]

You need to either

1. Use iteration * step_size + i instead of just i when indexing your xs_path/us_path; or
2. Keep indexing with i, but set xs_path = xs_path[step_size:] after each iteration (and same for us_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.

[nag92]

that worked perfectly!! thankyou