% % 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=regressor*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
ndof = robot.ndof;
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);
zero_ = zeros(1,length(q_J));
q_J = ones(1,length(q_J));
thetalist = [zero_;zero_;zero_;q_J;zero_;zero_;zero_;zero_;zero_]';
dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
tau = zeros([ndof,length(q_J)]);
% Dynamics parameters
link_mass = robot.m;
com_pos = robot.com;
link_inertia = robot.I;
% for i =1:ndof
%     robot_pi(:,i)=[link_mass(i);link_mass(i)*robot.com_pos_R1(:,i);inertiaMatrix2Tensor(link_inertia(:,:,i))];
% end
robot_pi_vector = reshape(robot.pi,[10*ndof,1]);
for i = 1:length(q_J)
%     regressor = standard_regressor_R1000(thetalist(i,:)');
    tau(:,i)=robot.regressor.K*robot_pi_vector;
    tau_mat(:,i) = standard_dynamics_R1000(thetalist(i,:)');
end