2024-10-07 15:10:42 +00:00
|
|
|
% function robot = verify_regressor(robot, opt)
|
|
|
|
|
% verify: If full regressor dynamics is the same as basic dynamics
|
|
|
|
|
ndof = robot.ndof;
|
2024-10-15 23:36:50 +00:00
|
|
|
% Dynamics parameters
|
|
|
|
|
link_mass = robot.m;
|
|
|
|
|
com_pos = robot.com;
|
|
|
|
|
link_inertia = robot.I;
|
2024-10-07 15:10:42 +00:00
|
|
|
|
2024-10-15 23:36:50 +00:00
|
|
|
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);
|
2024-10-07 15:10:42 +00:00
|
|
|
%% Check if screw method is equal to regressor
|
|
|
|
|
isequal(simplify(tau),simplify(tau_mat))
|
2024-10-15 23:36:50 +00:00
|
|
|
% %% 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
|
