function [Vd,AdXinvXdVd,V,Xerr,control] = FeedbackControl(X,Xd,Xdnext,Kp,Ki,dt,thetalist)
%UNTITLED4 反馈控制算法：配备机械臂的麦克纳姆轮智能车运动学任务空间的前馈+PI反馈控制算法：
%   当前时刻，实际的末端执行器的位型𝑋
%   当前时刻，参考轨迹上期望的末端执行器的位型𝑋𝑑
%   下一时刻（∆𝑡时间后），参考轨迹上的末端执行器的位型𝑋𝑑,𝑛𝑒𝑥𝑡
%   PI 控制器的增益矩阵𝐾𝑝和𝐾𝑖,均为对角阵(例如𝐾𝑝=𝐾𝑖=𝑒𝑦𝑒(6),即6×6的单位矩阵)； 
%   两个相邻参考轨迹位形（𝑋𝑑和𝑋𝑑,𝑛𝑒𝑥𝑡）间的时间间隔，即步长∆𝑡（例如0.01𝑠𝑒𝑐）。 

Vd = se3ToVec(1/dt*MatrixLog6(Xd\Xdnext));

AdXinvXdVd = Adjoint(X\Xd)*Vd;


Xerr = se3ToVec(MatrixLog6(X\Xd));

V = AdXinvXdVd + Kp*Xerr + Ki*(Xerr*dt);

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]]; 


%thetalist = [0;  0;  0.2; -1.6;  0];%可修改
Jarm = JacobianBody(Blist, thetalist);

Tb0 = [ 1  0  0  0.1662; 
     0  1  0  0; 
          0  0  1  0.0026; 
     0  0  0  1]; 
T0b = inv(Tb0); 

M0e = [ 1  0  0  0.033; 
    0  1  0  0; 
          0  0  1  0.6546; 
     0  0  0  1]; 

T0e = FKinBody(M0e, Blist, thetalist); 
Te0 = inv(T0e); 
r = 0.0475; % radius of whell 
l = 0.47/2; % L的小写l，与下面的1作区分 
w = 0.3/2;  % w 
 
F6 = [ 0        0        0        0      ; 
        0        0        0        0      ; 
     -1/(l+w)  1/(l+w)  1/(l+w) -1/(l+w); 
        1        1        1        1      ; 
       -1        1       -1        1      ; 
              0        0        0        0      ;]*r/4; 
%得到智能车的雅可比矩阵 
Jbase = Adjoint(Te0*T0b)*F6; 
%组合机械臂和智能车的雅克比矩阵 
Je = [Jbase Jarm] ;
control = pinv(Je)*V ;

end