175 lines
6.7 KiB
Matlab
175 lines
6.7 KiB
Matlab
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])
|
|
%%
|
|
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
|
|
%%
|
|
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);
|
|
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 = [];Tau_estimate = [];
|
|
for i = 1:length(q_J)
|
|
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
|
|
Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i),robot.baseQR);
|
|
tau_estimate=Yb*robot.sol.pi_b;
|
|
tau = Yb*robot.baseQR.baseParams;
|
|
Tau_estimate = horzcat(Tau_estimate, tau_estimate);
|
|
Tau = horzcat(Tau, tau);
|
|
end
|
|
%% get symbol result for GC
|
|
ndof = robot.ndof;
|
|
q_sym = sym('q%d',[ndof+1,1],'real');
|
|
qd_sym = sym('qd%d',[ndof+1,1],'real');
|
|
q2d_sym = sym('qdd%d',[ndof+1,1],'real');
|
|
% init regressor
|
|
robot.regressor.m = sym('m%d',[ndof,1],'real');
|
|
robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real');
|
|
robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real');
|
|
robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real');
|
|
robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real');
|
|
robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real');
|
|
robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real');
|
|
robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real');
|
|
robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real');
|
|
robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
|
|
robot.regressor.im = sym('im%d',[ndof,1],'real');
|
|
for i = 1:ndof
|
|
robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),...
|
|
robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),...
|
|
robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
|
|
end
|
|
[nLnkPrms, nLnks] = size(robot.regressor.pi);
|
|
robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
|
|
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
|
|
pi_lgr_base = pi1 + beta*pi2;
|
|
vpa(simplify(pi_lgr_base),2)
|
|
%% robot current and torque map
|
|
motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185];
|
|
gearRatio = [101,101,100,100,100,100,100,100,100];
|
|
% readDataFile;
|
|
% get pos
|
|
[nRow,nCol] = size(posData);
|
|
index_p = find(posData(:,nCol-1)==7);
|
|
jointPos_p = posData(index_p,1:9);
|
|
jointPos_p = jointPos_p(1:500:end,:);
|
|
index_n = find(posData(:,nCol-1)==-7);
|
|
jointPos_n = posData(index_n,1:9);
|
|
jointPos_n = jointPos_n(1:500:end,:);
|
|
q = (jointPos_p)';
|
|
% dq = [zeros(robot.ndof,1),diff(q,1,2)];
|
|
% get torque from simulator
|
|
Wb = []; Tau = [];
|
|
for i = 1:length(q)
|
|
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
|
|
Yb = feval(base_regressor_func, q(:,i),zeros(size(q(:,i))),zeros(size(q(:,i))),robot.baseQR);
|
|
tau = Yb*robot.baseQR.baseParams;
|
|
Tau = horzcat(Tau, tau);
|
|
end
|
|
% get toruqe from current
|
|
index_p = find(currentData(:,nCol-1)==7);
|
|
jointCur_p = currentData(index_p,1:9);
|
|
jointCur_p = jointCur_p(1:500:end,:);
|
|
index_n = find(currentData(:,nCol-1)==-7);
|
|
jointCur_n = currentData(index_n,1:9);
|
|
jointCur_n = jointCur_n(1:500:end,:);
|
|
current = (jointCur_p+jointCur_n)/2;
|
|
torque_cur = gearRatio.*motorConst.*current;
|
|
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
|
|
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),Tau(7,:))
|
|
xlabel('Time/s')
|
|
ylabel('Torque/Nm')
|
|
legend('Torque compute form current','Torque generate form simulator using CAD data')
|
|
title('Torque compute form current vs Torque generate form simulator using CAD data')
|
|
%%
|
|
Wb = []; Tau = [];
|
|
for i = 1:length(q)
|
|
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
|
|
Yb = feval(base_regressor_func, q(:,i),zeros(size(q(:,i))),zeros(size(q(:,i))),robot.baseQR);
|
|
tau = Yb*robot.sol.pi_b;
|
|
Tau = horzcat(Tau, tau);
|
|
end
|
|
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
|
|
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),Tau(7,:))
|
|
xlabel('Time/s')
|
|
ylabel('Torque/Nm')
|
|
legend('Torque compute form current','Torque form simulator using identificated data')
|
|
title('Torque compute form current vs Torque form simulator using identificated data')
|
|
%%
|
|
plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
|
|
xlabel('Time/s')
|
|
ylabel('Torque/Nm')
|
|
legend('Torque compute form current')
|
|
title('Torque compute form current')
|
|
%%
|
|
% F_Simpack = permute(F_Simpack,[2 1 3]);
|
|
plot(thetalist(:,8),-reshape(F_Simpack(7,3,:),[1,length(F_Simpack)]));
|
|
test = fileData8.data(:,(6*(8+1))+11*3-3);
|
|
test_time = fileData8.data(:,8+1+11);
|
|
hold on;plot(test_time,-test) |