Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix bugs in complier

This commit is contained in:
cosmic_power 2024-12-24 09:29:06 +08:00
parent 0bdf3e4bb3
commit b449059a0c
8 changed files with 37 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
complie/R1000 DVT GravityModel V1/base_regressor_R1000_DVT.m
View File

@ -1,21 +0,0 @@
function base_regrssor = base_regressor_R1000_DVT(theta,dtheta,ddtheta,baseQR)
opt.robot_def = 'direct';
opt.KM_method = 'SCREW';
opt.Vel_method = 'Direct';
opt.LD_method = 'Direct';
opt.debug = false;
opt.robotName = 'R1000_DVT';
opt.reGenerate = false;
opt.Isreal = true;
opt.isJointTorqueSensor = true;
opt.isSixAxisFTSensor = false;
file=[];
robot = get_robot_R1000_DVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
robot = get_Kinematics_R1000_DVT(robot, opt);
robot = get_velocity(robot, opt);
robot = get_regressor(robot, opt);
% get base params
robot.baseQR = baseQR;
robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
base_regrssor = robot.baseQR.regressor;

9
complie/R1000 DVT GravityModel V1/get_Kinematics_R1000_DVT.m
View File

@ -33,12 +33,11 @@ if(opt.Isreal)
for i = 1:length(thetalist) for i = 1:length(thetalist)
if i == 1 if i == 1
ct(:,i) = com_pos_R1(:,i); ct(:,i) = com_pos_R1(:,i);
% elseif i< 9 elseif i< 9
else
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i); ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
% else else
% % HACK % HACK
% ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i); ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.23;0;0.05896]+com_pos_R1(:,i);
end end
robot.Home.com(:,i) = ct(:,i); robot.Home.com(:,i) = ct(:,i);
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i)); M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));

46
complie/R1000 DVT GravityModel V1/get_robot_R1000_DVT.m
View File

@ -41,7 +41,7 @@ switch opt.robot_def
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
% Dynamics parameters % Dynamics parameters
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
link_mass = [2.1315260e+01,1.9235354e+01,1.0463871e+01,6.1538806,3.0882893,2.4450941,3.6376589,1.7005420,9.1507819e-1]; link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
% DVT % DVT
% link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2]; % link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
axis_of_rot = zeros(3,robot.ndof); axis_of_rot = zeros(3,robot.ndof);
@ -58,23 +58,23 @@ switch opt.robot_def
com_pos_R1 = zeros(3,9); com_pos_R1 = zeros(3,9);
com_pos_R2= zeros(3,9); com_pos_R2= zeros(3,9);
com_pos_R1(:,1)=[ 1.0440117e+02 -1.1125559e+01 1.2822933e+02]'*1e-3; com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
com_pos_R2(:,1)=[-8.0598830e+01 -4.2225559e+01 -1.5870672e+01]'*1e-3; com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
com_pos_R1(:,2)=[ 3.5562148e+02 1.2378198e+01 1.8395830e-03]'*1e-3; com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3;
com_pos_R2(:,2)=[-2.4437852e+02 9.4778198e+01 1.8395830e-03]'*1e-3; com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3;
com_pos_R1(:,3)=[2.4798242e+02 -3.5267916e+01 4.8648267e-02]'*1e-3; com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3;
com_pos_R2(:,3)=[-3.5201758e+02 -2.9007916e+01 4.8648266e-02]'*1e-3; com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3;
com_pos_R1(:,4)=[ 4.2341069e+01 5.2603527e+01 1.2271365e+01]'*1e-3; com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3;
com_pos_R2(:,4)=[-7.7658931e+01 -1.2956223e+01 1.9271365e+01]'*1e-3; com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3;
com_pos_R1(:,5)=[1.0177553e+02 -3.4868152e-02 1.5686100e+00]'*1e-3; com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
com_pos_R2(:,5)=[-1.0372447e+02 -3.9239008e-02 -5.4313602e+00]'*1e-3; com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3;
com_pos_R1(:,6)=[5.9942347e+01 -4.8346177e-02 5.5784018e+00]'*1e-3; com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
com_pos_R2(:,6)=[-2.3757653e+01 -4.8346177e-02 4.0028402e+01]'*1e-3; com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3;
com_pos_R1(:,7)=[4.9494392e-02 -2.4240394e+00 -1.0332489e+02]'*1e-3; com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
com_pos_R2(:,7)=[4.9494392e-02 -2.4240394e+00 2.7222511e+02]'*1e-3; com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3;
com_pos_R1(:,8)=[-2.7158498e+01 -3.3865875e-01 -4.6588829e+01]'*1e-3; com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
com_pos_R2(:,8)=[ 2.2423433e+02 -3.3865729e-01 1.9671171e+01]'*1e-3; % don't use com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use
com_pos_R1(:,9)=[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3; com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
% stack result % stack result
robot.com_pos_R1 = com_pos_R1; robot.com_pos_R1 = com_pos_R1;
robot.com_pos_R2 = com_pos_R2; robot.com_pos_R2 = com_pos_R2;
@ -84,19 +84,19 @@ switch opt.robot_def
for i = 1:robot.ndof-1 for i = 1:robot.ndof-1
if i == 1 if i == 1
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i); co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
else elseif i<8
%From base to ISA Origin %From base to ISA Origin
co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i); co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
% else else
% %From base to ISA Origin %From base to ISA Origin
% co(:,i) = co(:,i-1)-[0;0;0.05896]; co(:,i) = co(:,i-1)-[0.23;0;0.05896];
end end
end end
co = [zeros(3,1),co]; co = [zeros(3,1),co];
% the inertia tensor wrt the frame oriented as the body frame and with the % the inertia tensor wrt the frame oriented as the body frame and with the
% origin in the COM % origin in the COM
link_inertia = zeros(3,3,robot.ndof); link_inertia = zeros(3,3,robot.ndof);
link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];... link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];...
[-1.1694021e+04 2.3289532e+05 -3.0395414e+02];... [-1.1694021e+04 2.3289532e+05 -3.0395414e+02];...
[-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6; [-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6;
link_inertia(:,:,2) = [[3.7926328e+04 -9.0569033e+01 4.7526575e+04];... link_inertia(:,:,2) = [[3.7926328e+04 -9.0569033e+01 4.7526575e+04];...

18
complie/R1000 DVT GravityModel V1/regressor_R1000_DVT.m
View File

@ -1,18 +0,0 @@
function regrssor = regressor_R1000_DVT(theta,dtheta,ddtheta)
opt.robot_def = 'direct';
opt.KM_method = 'SCREW';
opt.Vel_method = 'Direct';
opt.LD_method = 'Direct';
opt.debug = false;
opt.robotName = 'R1000_DVT';
opt.reGenerate = false;
opt.Isreal = true;
opt.isJointTorqueSensor = false;
opt.isSixAxisFTSensor = true;
file=[];
robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
robot = get_Kinematics_EVT(robot, opt);
robot = get_velocity(robot, opt);
robot = get_regressor(robot, opt);
regrssor = robot.regressor.U;

BIN
figure2/J2 Gravity Model.jpg
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 74 KiB

After

Width:  |  Height:  |  Size: 57 KiB

3
get_GCTraj_R1000_DVT.m
View File

@ -2,7 +2,8 @@
gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))]; gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))];
motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031]; motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
sensorDir = [-1,1,-1,-1,-1,1,-1,1,1]; sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
load("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat"); % load("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
load("D:\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat")
posDir = [1,1,1,1,1,1,1,-1,1]; posDir = [1,1,1,1,1,1,1,-1,1];
isCurrentSensor = true; isCurrentSensor = true;
% J9 traj % J9 traj

12
plotData_current.m
View File

@ -5,7 +5,7 @@ sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
posDir = [1,1,1,1,1,1,1,-1,1]; posDir = [1,1,1,1,1,1,1,-1,1];
% J9 traj % J9 traj
for i=5 for i=9
fileData=eval(strcat('fileData', num2str(i))); fileData=eval(strcat('fileData', num2str(i)));
data = fileData.data; data = fileData.data;
dataLength = length(data); dataLength = length(data);
@ -16,21 +16,21 @@ for i=5
if i==2 if i==2
plot3(data(1:d1Length,i+1+11),data(1:d1Length,6+1+11),data(1:d1Length,i+1+11*2),'r'); hold on; plot3(data(1:d1Length,i+1+11),data(1:d1Length,6+1+11),data(1:d1Length,i+1+11*2),'r'); hold on;
plot3(data(d1Length:end,i+1+11),data(d1Length:end,6+1+11),data(d1Length:end,i+1+11*2),'b'); hold on; plot3(data(d1Length:end,i+1+11),data(d1Length:end,6+1+11),data(d1Length:end,i+1+11*2),'b'); hold on;
plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m'); hold on; % plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m'); hold on;
else else
plot3(data(1:d1Length,i+1+11),data(1:d1Length,i+11),data(1:d1Length,i+1+11*2),'r'); hold on; plot3(data(1:d1Length,i+1+11),data(1:d1Length,i+11),data(1:d1Length,i+1+11*2),'r'); hold on;
plot3(data(d1Length:end,i+1+11),data(d1Length:end,i+11),data(d1Length:end,i+1+11*2),'b'); hold on; plot3(data(d1Length:end,i+1+11),data(d1Length:end,i+11),data(d1Length:end,i+1+11*2),'b'); hold on;
plot3(data(:,i+1+11),data(:,i+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m'); hold on; % plot3(data(:,i+1+11),data(:,i+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m'); hold on;
end end
hold off; hold off;
title(['J' num2str(i) ' Gravity Model']); title(['J' num2str(i) ' Gravity Model']);
xlabel('X Pos/rad');ylabel('Y Pos/rad');zlabel('Torque /Nm') xlabel('X Pos/rad');ylabel('Y Pos/rad');zlabel('Current /A')
end end
%% %%
% should run identifcation program firstly % should run identifcation program firstly
resolution = 20; resolution = 20;
tau_estimate=[]; tau_estimate=[];
for k=5 for k=9
if k ==2 if k ==2
qx = idntfcnTrjctry(k).q(k,:); qx = idntfcnTrjctry(k).q(k,:);
qy = idntfcnTrjctry(k).q(6,:); qy = idntfcnTrjctry(k).q(6,:);
@ -53,5 +53,5 @@ end
hold on; hold on;
mesh(X,Y,current_estimate,current_estimate, 'FaceAlpha', 0.0) mesh(X,Y,current_estimate,current_estimate, 'FaceAlpha', 0.0)
%% %%
saveas(gcf, ['./figure/' 'J' num2str(k) ' Gravity Model' '.jpg']) % saveas(gcf, ['./figure/' 'J' num2str(k) ' Gravity Model' '.jpg'])

4
plotData_test.m
View File

@ -5,7 +5,7 @@ sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
posDir = [1,1,1,1,1,1,1,-1,1]; posDir = [1,1,1,1,1,1,1,-1,1];
% J9 traj % J9 traj
for i=2 for i=6
fileData=eval(strcat('fileData', num2str(i))); fileData=eval(strcat('fileData', num2str(i)));
data = fileData.data; data = fileData.data;
dataLength = length(data); dataLength = length(data);
@ -30,7 +30,7 @@ end
% should run identifcation program firstly % should run identifcation program firstly
resolution = 20; resolution = 20;
tau_estimate=[]; tau_estimate=[];
for k=2 for k=6
if k ==2 if k ==2
qx = idntfcnTrjctry(k).q(k,:); qx = idntfcnTrjctry(k).q(k,:);
qy = idntfcnTrjctry(k).q(6,:); qy = idntfcnTrjctry(k).q(6,:);