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

Merge branch 'feature/Make-GC-Compilable' of http://8.138.4.170:8980/Robotics/IRDYn into feature/Make-GC-Compilable 

# Conflicts:
#	get_GCTraj_R1000_DVT.m
This commit is contained in:
cosmic_power 2024-12-26 00:22:19 +08:00
parent 3a4b7adfb4 b449059a0c
commit c55501f898
7 changed files with 35 additions and 75 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)
if i == 1
ct(:,i) = com_pos_R1(:,i);
% elseif i< 9
else
elseif i< 9
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
% else
% % HACK
% ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
else
% HACK
ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.23;0;0.05896]+com_pos_R1(:,i);
end
robot.Home.com(:,i) = ct(:,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
% ------------------------------------------------------------------------
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
% 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);
@ -58,23 +58,23 @@ switch opt.robot_def
com_pos_R1 = 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_R2(:,1)=[-8.0598830e+01 -4.2225559e+01 -1.5870672e+01]'*1e-3;
com_pos_R1(:,2)=[ 3.5562148e+02 1.2378198e+01 1.8395830e-03]'*1e-3;
com_pos_R2(:,2)=[-2.4437852e+02 9.4778198e+01 1.8395830e-03]'*1e-3;
com_pos_R1(:,3)=[2.4798242e+02 -3.5267916e+01 4.8648267e-02]'*1e-3;
com_pos_R2(:,3)=[-3.5201758e+02 -2.9007916e+01 4.8648266e-02]'*1e-3;
com_pos_R1(:,4)=[ 4.2341069e+01 5.2603527e+01 1.2271365e+01]'*1e-3;
com_pos_R2(:,4)=[-7.7658931e+01 -1.2956223e+01 1.9271365e+01]'*1e-3;
com_pos_R1(:,5)=[1.0177553e+02 -3.4868152e-02 1.5686100e+00]'*1e-3;
com_pos_R2(:,5)=[-1.0372447e+02 -3.9239008e-02 -5.4313602e+00]'*1e-3;
com_pos_R1(:,6)=[5.9942347e+01 -4.8346177e-02 5.5784018e+00]'*1e-3;
com_pos_R2(:,6)=[-2.3757653e+01 -4.8346177e-02 4.0028402e+01]'*1e-3;
com_pos_R1(:,7)=[4.9494392e-02 -2.4240394e+00 -1.0332489e+02]'*1e-3;
com_pos_R2(:,7)=[4.9494392e-02 -2.4240394e+00 2.7222511e+02]'*1e-3;
com_pos_R1(:,8)=[-2.7158498e+01 -3.3865875e-01 -4.6588829e+01]'*1e-3;
com_pos_R2(:,8)=[ 2.2423433e+02 -3.3865729e-01 1.9671171e+01]'*1e-3; % don't use
com_pos_R1(:,9)=[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3;
com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3;
com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3;
com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3;
com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3;
com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3;
com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3;
com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3;
com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3;
com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3;
com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
% stack result
robot.com_pos_R1 = com_pos_R1;
robot.com_pos_R2 = com_pos_R2;
@ -84,19 +84,19 @@ switch opt.robot_def
for i = 1:robot.ndof-1
if i == 1
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
else
elseif i<8
%From base to ISA Origin
co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
% else
% %From base to ISA Origin
% co(:,i) = co(:,i-1)-[0;0;0.05896];
else
%From base to ISA Origin
co(:,i) = co(:,i-1)-[0.23;0;0.05896];
end
end
co = [zeros(3,1),co];
% the inertia tensor wrt the frame oriented as the body frame and with the
% origin in the COM
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];...
[-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6;
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

12
plotData_current.m
View File

@ -5,7 +5,7 @@ load("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\
posDir = [1,1,1,1,1,1,1,-1,1];
% J9 traj
for i=5
for i=9
fileData=eval(strcat('fileData', num2str(i)));
data = fileData.data;
dataLength = length(data);
@ -16,21 +16,21 @@ for i=5
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(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
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(:,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
hold off;
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
%%
% should run identifcation program firstly
resolution = 20;
tau_estimate=[];
for k=5
for k=9
if k ==2
qx = idntfcnTrjctry(k).q(k,:);
qy = idntfcnTrjctry(k).q(6,:);
@ -53,5 +53,5 @@ end
hold on;
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];
% J9 traj
for i=2
for i=6
fileData=eval(strcat('fileData', num2str(i)));
data = fileData.data;
dataLength = length(data);
@ -30,7 +30,7 @@ end
% should run identifcation program firstly
resolution = 20;
tau_estimate=[];
for k=2
for k=6
if k ==2
qx = idntfcnTrjctry(k).q(k,:);
qy = idntfcnTrjctry(k).q(6,:);