45 lines
1.5 KiB
Matlab
45 lines
1.5 KiB
Matlab
% % 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));
|
|
thetalist = [zero_;zero_;zero_;zero_;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
|