% function robot = verify_regressor(robot, opt)
% verify: If full regressor dynamics is the same as basic dynamics
ndof = robot.ndof;
% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;

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');
for i =1:ndof
pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1;];
end
% pi2=zeros([10,1]);
pi=[pi1;pi2];
regressor = standard_regressor_Two_bar(q_sym,qd_sym,q2d_sym);
tau=regressor*pi;
%% Two-bar
N=2;
thetalist = q_sym(1:N);
dthetalist = qd_sym(1:N);
ddthetalist = q2d_sym(1:N);

Gb= [diag([1,1,1]),zeros(3,3);
    zeros(3,3),diag([1,1,1])];
Glist = cat(3, Gb, Gb);
% Glist = cat(3, Gb, zeros([6,6]));
M01 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M23 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist = cat(3, M01, M12, M23);
Slist=[[0;0;1;0;0;0],...
    [0;0;1;0;-1;0]];
Adgab_mat = sym(zeros(6,6,N+1));
Fmat=sym(zeros(N,6));
F1=sym(zeros(N,6));
V1=sym(zeros(6,N+1));
G=sym(zeros(4,4,N));
T=sym(zeros(4,4,N));
Vlinear=sym(zeros(3,3));
Vd1=sym(zeros(6,N+1));
Gb= [diag([1,1,1]),zeros(3,3);
    zeros(3,3),diag([1,1,1])];
J=sym(zeros(6,N));
exf=[0;0;0;0;0;0];

[V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
    = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
    [0;0;0], exf, Mlist, Glist, Slist);
G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M23 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist = cat(3, M01, M12, M23);
T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
% Use Body Twist cal linear vel, but can't cal the end frame vel
[V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
    [0;0;0], exf, Mlist, Glist, Slist);
j=1;
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
j=2;
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
%% Check if screw method is equal to regressor
isequal(simplify(tau),simplify(tau_mat))
%% Numerical
clear pi;
ndof = robot.ndof;
time = 0:0.01:2;
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];
qd=[qd_J; -qd_J];
qdd=[qdd_J; -qdd_J];
g = [0; 0; -9.8];
robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
robot_pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
% pi2=zeros([10,1]);
robot_pi=[robot_pi1;robot_pi2];
tau = zeros([2,100]);
for i = 1:length(q_J)
    regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
    tau(:,i)=regressor*robot_pi;
end