R = robot.kine.R;
P = robot.kine.t;
F1 = Adjoint(RpToTrans(RotX(pi/4),[1;2;3]))*robot.regressor.A(:,:,end)*robot.pi(:,end)
F2 = robot.regressor.A(:,:,end-1)*robot.pi(:,end-1)+F1

FF1 = Adjoint(TransInv(RpToTrans(RotX(pi/4),[1;2;3])))'*F_Simpack(end,:,1)'
FF2 = F_Simpack(end-1,:,1)'+FF1
%%
F1 = robot.regressor.A(:,:,end)*robot.pi(:,end);
F3 = robot.regressor.A(:,:,end-2)*robot.pi(:,end-2);
%%
F_Simpack(end,:,1)
F_Simpack(end-2,:,1)
%%
robot_pi_vecoter = reshape(robot.pi,[90,1]);
F = robot.regressor.U*robot_pi_vecoter;
FF = reshape(F,[6,9])
%%
F_Simpack(:,:,1)
%%
robot_pi_vecoter = reshape(robot.pi,[90,1]);
tau_standard  = robot.regressor.K*robot_pi_vecoter;
tau_standard = reshape(tau_standard,[1,9])

tau_base  = robot.baseQR.regressor*robot.baseQR.baseParams;
tau_base = reshape(tau_base,[1,9])
%%
tau_estimate  = robot.baseQR.regressor*robot.sol.pi_b;
tau_estimate = reshape(tau_estimate,[1,9])
%%
time = 0:0.1:1;
f=1;
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
g = [0; 0; -9.8];
tau = zeros([robot.ndof,length(q_J)]);
% pi -> [m;mc;I] 10 element
[nLnkPrms, nLnks] = size(robot.pi);
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
Wb = []; Tau = [];
for i = 1:length(q_J)
    %     regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
    standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
    regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
    tau=regressor*robot_pi;
    Tau = vertcat(Tau, tau);
end
for i = 1:1:length(q_J)
    base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
    Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i));
    Wb = vertcat(Wb, Yb);
end
%%
time = 0:0.01:1;
f=1;
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
g = [0; 0; -9.8];
tau = zeros([robot.ndof,length(q_J)]);
% pi -> [m;mc;I] 10 element
[nLnkPrms, nLnks] = size(robot.pi);
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
Wb = []; Tau = [];Tau_estimate = [];
for i = 1:length(q_J)
    base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
    Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i),robot.baseQR);
    tau_estimate=Yb*robot.sol.pi_b;
    tau = Yb*robot.baseQR.baseParams;
    Tau_estimate = horzcat(Tau_estimate, tau_estimate);
    Tau = horzcat(Tau, tau);
end
%% get symbol result for GC
ndof = robot.ndof;
q_sym = sym('q%d',[ndof+1,1],'real');
qd_sym = sym('qd%d',[ndof+1,1],'real');
q2d_sym = sym('qdd%d',[ndof+1,1],'real');
% init regressor
robot.regressor.m = sym('m%d',[ndof,1],'real');
robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real');
robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real');
robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real');
robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real');
robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real');
robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real');
robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real');
robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real');
robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
robot.regressor.im = sym('im%d',[ndof,1],'real');
for i = 1:ndof
    robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),...
        robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),...
        robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
end
[nLnkPrms, nLnks] = size(robot.regressor.pi);
robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
qr_rank = robot.baseQR.numberOfBaseParameters;
E = robot.baseQR.permutationMatrix;
pi_lgr_sym = robot.regressor.pi;
pi1 = E(:,1:qr_rank)'*pi_lgr_sym; % independent paramters
pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
beta = robot.baseQR.beta;
% all of the expressions below are equivalent
pi_lgr_base = pi1 + beta*pi2;
vpa(simplify(pi_lgr_base),2)
%% robot current and torque map
motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185];
gearRatio = [101,101,100,100,100,100,100,100,100];
% readDataFile;
% get pos
[nRow,nCol] = size(posData);
index_p = find(posData(:,nCol-1)==7);
jointPos_p = posData(index_p,1:9);
jointPos_p = jointPos_p(1:500:end,:);
index_n = find(posData(:,nCol-1)==-7);
jointPos_n = posData(index_n,1:9);
jointPos_n = jointPos_n(1:500:end,:);
q = (jointPos_p)';
% dq = [zeros(robot.ndof,1),diff(q,1,2)];
% get torque from simulator
Wb = []; Tau = [];
for i = 1:length(q)
    base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
    Yb = feval(base_regressor_func, q(:,i),zeros(size(q(:,i))),zeros(size(q(:,i))),robot.baseQR);
    tau = Yb*robot.baseQR.baseParams;
    Tau = horzcat(Tau, tau);
end
% get toruqe from current
index_p = find(currentData(:,nCol-1)==7);
jointCur_p = currentData(index_p,1:9);
jointCur_p = jointCur_p(1:500:end,:);
index_n = find(currentData(:,nCol-1)==-7);
jointCur_n = currentData(index_n,1:9);
jointCur_n = jointCur_n(1:500:end,:);
current = (jointCur_p+jointCur_n)/2;
torque_cur = gearRatio.*motorConst.*current;
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),Tau(7,:))
xlabel('Time/s')
ylabel('Torque/Nm')
legend('Torque compute form current','Torque generate form simulator using CAD data')
title('Torque compute form current vs Torque generate form simulator using CAD data')
%%
Wb = []; Tau = [];
for i = 1:length(q)
    base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
    Yb = feval(base_regressor_func, q(:,i),zeros(size(q(:,i))),zeros(size(q(:,i))),robot.baseQR);
    tau = Yb*robot.sol.pi_b;
    Tau = horzcat(Tau, tau);
end
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),Tau(7,:))
xlabel('Time/s')
ylabel('Torque/Nm')
legend('Torque compute form current','Torque form simulator using identificated data')
title('Torque compute form current vs Torque form simulator using identificated data')
%%
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
xlabel('Time/s')
ylabel('Torque/Nm')
legend('Torque compute form current')
title('Torque compute form current')