2024-10-19 17:39:18 +00:00
|
|
|
R = robot.kine.R;
|
|
|
|
|
P = robot.kine.t;
|
|
|
|
|
F1 = Adjoint(RpToTrans(RotX(pi/4),[1;2;3]))*robot.regressor.A(:,:,end)*robot.pi(:,end)
|
|
|
|
|
F2 = robot.regressor.A(:,:,end-1)*robot.pi(:,end-1)+F1
|
|
|
|
|
|
|
|
|
|
FF1 = Adjoint(TransInv(RpToTrans(RotX(pi/4),[1;2;3])))'*F_Simpack(end,:,1)'
|
|
|
|
|
FF2 = F_Simpack(end-1,:,1)'+FF1
|
|
|
|
|
%%
|
|
|
|
|
F1 = robot.regressor.A(:,:,end)*robot.pi(:,end);
|
|
|
|
|
F3 = robot.regressor.A(:,:,end-2)*robot.pi(:,end-2);
|
|
|
|
|
%%
|
|
|
|
|
F_Simpack(end,:,1)
|
|
|
|
|
F_Simpack(end-2,:,1)
|
|
|
|
|
%%
|
|
|
|
|
robot_pi_vecoter = reshape(robot.pi,[90,1]);
|
|
|
|
|
F = robot.regressor.U*robot_pi_vecoter;
|
|
|
|
|
FF = reshape(F,[6,9])
|
2024-10-20 06:49:54 +00:00
|
|
|
%%
|
2024-10-25 14:34:21 +00:00
|
|
|
F_Simpack(:,:,1)
|
|
|
|
|
%%
|
|
|
|
|
robot_pi_vecoter = reshape(robot.pi,[90,1]);
|
|
|
|
|
tau_standard = robot.regressor.K*robot_pi_vecoter;
|
|
|
|
|
tau_standard = reshape(tau_standard,[1,9])
|
|
|
|
|
|
|
|
|
|
tau_base = robot.baseQR.regressor*robot.baseQR.baseParams;
|
|
|
|
|
tau_base = reshape(tau_base,[1,9])
|
|
|
|
|
%%
|
|
|
|
|
tau_estimate = robot.baseQR.regressor*robot.sol.pi_b;
|
|
|
|
|
tau_estimate = reshape(tau_estimate,[1,9])
|
|
|
|
|
%%
|
|
|
|
|
time = 0:0.1: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);
|
|
|
|
|
q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
|
|
|
|
|
qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
|
|
|
|
|
qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
|
|
|
|
|
g = [0; 0; -9.8];
|
|
|
|
|
tau = zeros([robot.ndof,length(q_J)]);
|
|
|
|
|
% pi -> [m;mc;I] 10 element
|
|
|
|
|
[nLnkPrms, nLnks] = size(robot.pi);
|
|
|
|
|
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
|
|
|
|
|
Wb = []; Tau = [];
|
|
|
|
|
for i = 1:length(q_J)
|
|
|
|
|
% regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
|
|
|
|
|
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
|
|
|
|
regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
|
|
|
|
|
tau=regressor*robot_pi;
|
|
|
|
|
Tau = vertcat(Tau, tau);
|
|
|
|
|
end
|
|
|
|
|
for i = 1:1:length(q_J)
|
|
|
|
|
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
|
|
|
|
|
Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i));
|
|
|
|
|
Wb = vertcat(Wb, Yb);
|
|
|
|
|
end
|
|
|
|
|
%%
|
|
|
|
|
qr_rank = robot.baseQR.numberOfBaseParameters;
|
|
|
|
|
E = robot.baseQR.permutationMatrix;
|
|
|
|
|
pi_lgr_sym = robot.regressor.pi;
|
|
|
|
|
pi1 = E(:,1:qr_rank)'*pi_lgr_sym; % independent paramters
|
|
|
|
|
pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
|
|
|
|
|
beta = robot.baseQR.beta;
|
|
|
|
|
% all of the expressions below are equivalent
|
2024-10-25 15:41:23 +00:00
|
|
|
pi_lgr_base = pi1 + beta*pi2;
|
|
|
|
|
vpa(simplify(pi_lgr_base),2)
|
