search

marco-faroni / TrajectoryScaling

4 stars 2 forks source link

About the cost function #2

Closed yakunsjtu closed 1 year ago

yakunsjtu commented 3 years ago

Hey @marco-faroni , I went through the Matlab code and try to compare the implemented algrothom with the following paper: Predictive joint trajectory scaling for manipulators with kinodynamic constraints.

In the file controllers/thor/ctrl_law.m (line 156), I was confused by these intermediate matrixes:

Heq = [ctrl.F(:,:,2)+[ctrl.Kclik*ctrl.F(1:df,:,1);0*ctrl.F(df+1:end,:,1)], -diag(qd_des)*ctrl.PD; zeros(Ns,Np*df), eye(Ns)]

H = Heq'*ctrl.Wsc*Heq + ctrl.lambdaU*ctrl.Hu;

f = -[ -(ctrl.L(:,:,2) + [ ctrl.Kclik*ctrl.L(1:df,:,1);0*ctrl.L(df+1:end,:,1)] )*q0 + [ctrl.Kclik*qdes; zeros(df*(Np-1),1)] ; s_ref.*ones(Ns,1) ]'*ctrl.Wsc*ctrl.Hp*Heq;

And finally the Hessian matrix is computed: HH = (H+H')/2;

But I could not figure out the underlying theory and the paper did not mention it, I only found out the cost function in the paper like this:

image

From the figure, the weighting matrixes are Q, R and P. But how these 3 matrixes correspond to the matrixes in the code then (how to transform the cost function to the code format)? And what are the matlab variables corresponding to these 4 variables in the equation?

marco-faroni commented 3 years ago

@yakunsjtu this implementation is slightly different from the equation of the paper, but they are almost equivalent. Matrices ctrl.F and ctrl.L are the forced and free response of a double integrator system (see paper [2] in the references for the math). In practice, ctrl.L(:,:,2)q0 + ctrl.F(1:df,:,2)u is equal to qdot(k+i) for i=1...p, where q0 is the current state (q(k),qdot(k)) and u is the optimization variable (qddot). In the code, the third term of the cost function is not present, as I put the position error into the first term. So, Q is equivalent to ctrl.Wsc and R is equivalent to ctrl.Hu. P is not present in the code, Kclik has a similar role; more or less it should be P=(Kclik^2)*Wsc(1:df,1:df)

yakunsjtu commented 3 years ago

Thanks @marco-faroni , after reading the paper you mentioned, I'm much clear about the prediction method. As for the construction of the QP problem, do you mean the following procedure:

Firstly, merge the third iterm into the first one, and let , the cost function is , and the we expand the item to get those intermediate matrices? But I feel not easy to do the math to separate out the optimization variable. If this is the right way, I will try again to do it.

marco-faroni commented 2 years ago

From the paper, the objective function in Eq. 16 is composed of three terms, the first one accounts for path following (at velocity level), the second one accounts for effort, the third one for position error. These three terms can be seen as squared norms of vectors. Now consider such three vectors and sum (instead of concatenating) the first one and the third one and then derive the cost function.