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
%%
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)