% 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],'real');
% for i =1:ndof
%     robot_pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1;inertiaMatrix2Tensor(link_inertia(:,:,i))];
% end
% regressor = standard_regressor_R1000(q_sym);
robot_pi_vector = reshape(robot.pi,[10*ndof,1]);
tau=robot.regressor.K*robot_pi_vector;
%% R1000
tau_mat = standard_dynamics_R1000(q_sym);
%% 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