% function robot = verify_regressor(robot, opt)
% verify: If full regressor dynamics is the same as basic dynamics
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');
pi1=[2;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
% 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);