Feedforward + PI Control Algorithm Yields Different Results
1 visualizzazione (ultimi 30 giorni)
Mostra commenti meno recenti
Hello there, hope you are all doing well.
I wrote a couple of days ago in order to let you know about a program of mine not working as expected. I just found a chance to try what you all said and I am suspecting about a bug in my FeedForward + PI Control Algorithm. Before giving out the code I want to tell you exactly what my code is expected to accomplish.
This the structure of the function:
[Vd, V, Je, u_theta_dot, Xerr, Xerr_integral] = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, thetaList, Xerr_integral)
Inputs;
- X -> Actual End-Effector Configuration Tse,
- Xd -> Desired/Reference End-Effector Configuration Tse_d,
- Xd_next -> Desired/Reference End-Effector Configuration at the next
- time step, Tse_d1,
- Kp and Ki-> Gains
- delta_t -> Timestep between configurations,
- thetaList -> Current actual joint angles,
- Xerr_integral -> Used in order to update the integral of Xerr.
Outputs;
- Vd -> Resulting Feed-forward twist
- V -> Commanded End-Effector twist
- Je -> Jbase(theta) and Jarm(theta) concatanated
- u_theta_dot -> controls vector (u and theta_dot)
- Xerr -> Configuration Error
- Xerr_integral -> Used to pass as a parameter in the next_step
Here are the required Math Equations:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379591/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379596/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379601/image.png)
[Xerr] = log(inverse(X) * Xd) -> This needs to be converted to a vector.
Quick Note: My function uses some functions from Modern Robotics Library, which can be found (on Github) here.
With all of these, here is my function implementation:
function [Vd, V, Je, u_theta_dot, Xerr, Xerr_integral] = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, thetaList, Xerr_integral)
%% Arm properties.
Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
l = 0.47/2;
w = 0.30/2;
r = 0.0475;
F = (r/4) * [-1/(l + w), 1/(l + w), 1/(l + w), -1/(l + w); 1 1 1 1; -1 1 -1 1];
sizee = size(F);
m = sizee(2);
zeross = zeros(1, m);
F6 = [zeross; zeross; F; zeross];
Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M0e = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];
T0e = FKinBody(M0e, Blist, thetaList);
Tbe = Tb0 * T0e;
Teb = TransInv(Tbe);
Jbase = Adjoint(Teb) * F6;
Jarm = JacobianBody(Blist, thetaList);
Je = [Jbase, Jarm];
psInv = pinv(Je);
Xerr_bracket = MatrixLog6(TransInv(X) * Xd);
Xerr = se3ToVec(Xerr_bracket);
Xerr_integral = Xerr_integral + (delta_t * Xerr);
Xerr_integral = round(Xerr_integral, 3);
Vd_bracket = (1/delta_t) * MatrixLog6(TransInv(Xd) * Xd_next);
Vd = se3ToVec(Vd_bracket);
V = (Adjoint(TransInv(X) * Xd) * Vd) + (Kp * Xerr) + Ki * Xerr_integral;
u_theta_dot = psInv * V;
u_theta_dot = round(u_theta_dot, 3);
Xerr = round(Xerr, 3);
V = round(V, 3);
Vd = round(Vd, 3);
Je = round(Je, 3);
end
Last couple of lines are used to round the results to 3 decimals places.
When I test my function with pre-calculated parameters, these are the expected results:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379606/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379611/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379616/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379621/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379626/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/379631/image.png)
Here is my test script:
clc
clear
Blist = [0 0 1 0 0.033 0; 0 -1 0 -0.5076 0 0; 0 -1 0 -0.3526 0 0; 0 -1 0 -0.2176 0 0; 0 0 1 0 0 0]';
Tb0 = [1 0 0 0.1662; 0 1 0 0; 0 0 1 0.0026; 0 0 0 1];
M = [1 0 0 0.033; 0 1 0 0; 0 0 1 0.6546; 0 0 0 1];
thetaList = [0 0 0.2 -1.6 0]';
Xd = [0 0 1 0.5; 0 1 0 0; -1 0 0 0.5; 0 0 0 1];
Xd_next = [0 0 1 0.6; 0 1 0 0; -1 0 0 0.3; 0 0 0 1];
X = [0.170 0 0.985 0.387; 0 1 0 0; -0.985 0 0.170 0.570; 0 0 0 1];
Xerr_bracket = MatrixLog6(TransInv(X) * Xd);
Xerr = se3ToVec(Xerr_bracket);
Kp = 0 * eye(6);
Ki = 0 * eye(6);
delta_t = 0.01;
[Vd, V, Je, u_theta_dot, Xerr, Xerr_integral] = FeedbackControl(X, Xd, Xd_next, Kp, Ki, delta_t, thetaList, Xerr)
My Je, and Vd outputs are exactly right. However, the other outputs have some decimal errors. The differences do not have anything to do with the fact that I rounded the numbers up. In fact, the expected results were rounded up and that was the reason I did too, in the first place. I do not think that the expected results are incorrect, so that is why I am suspicious of my function implementation. Can you help me with that?
0 Commenti
Risposte (0)
Vedere anche
Categorie
Scopri di più su Publishers and Subscribers in Help Center e File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!