add figure and real mpk pos

get_GCTraj_R1000_DVT.m

@@ -2,7 +2,7 @@
 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];
 sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
-load("C:\GitLab\gtools\development_files\SLX_Development\R1000\SA System\large_range_lab1.mat");
+load("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
 isCurrentSensor = true;
 % J9 traj

get_Kinematics_R1000_DVT.m

@@ -41,7 +41,7 @@ if(opt.Isreal)
                     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.3157;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
                 robot.Home.com(:,i) = ct(:,i);
                 M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
@@ -160,7 +160,7 @@ else
                     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.3157;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
                 robot.Home.com(:,i) = ct(:,i);
                 M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));

get_robot_R1000_DVT.m

@@ -49,7 +49,7 @@ switch opt.robot_def
                 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.3157;0;0.05896];
+                co(:,i) = co(:,i-1)-[0.23;0;0.05896];
             end
         end
         co = [zeros(3,1),co];
@@ -200,7 +200,7 @@ switch opt.robot_def
                 case 'SDH'
                 case 'MDH'
                     robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];
-                    robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157];
+                    robot.d = [0,0,0,0,0,0.28,0.40,0,-0.23];
                     robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
                     robot.link_type = ['R','R','R','R','R','R','R','R','P'];
                     robot.d = sym(robot.d);

plotData_current.m

@@ -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=6
+for i=5
     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=6
+for k=5
     if k ==2
         qx = idntfcnTrjctry(k).q(k,:);
         qy = idntfcnTrjctry(k).q(6,:);
@@ -52,4 +52,6 @@ end
 end
 hold on;
 mesh(X,Y,current_estimate,current_estimate, 'FaceAlpha', 0.0)
+%%
+saveas(gcf, ['./figure/' 'J' num2str(k) ' Gravity Model' '.jpg'])
 

plotData_test.m

@@ -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=6
+for i=2
     fileData=eval(strcat('fileData', num2str(i)));
     data = fileData.data;
     dataLength = length(data);
@@ -16,11 +16,11 @@ for i=6
     if i==2
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,6+1+11),data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,6+1+11),data(d1Length:end,i+1+11*2)*gearRatio(i)*motorConstant(i),'b');    hold on;
-        plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
+%         plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
     else
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,i+11),data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,i+11),data(d1Length:end,i+1+11*2)*gearRatio(i)*motorConstant(i),'b');    hold on;
-        plot3(data(:,i+1+11),data(:,i+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
+%         plot3(data(:,i+1+11),data(:,i+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
     end
     hold off;
     title(['J' num2str(i) ' Gravity Model']);
@@ -30,7 +30,7 @@ end
 % should run identifcation program firstly
 resolution = 20;
 tau_estimate=[];
-for k=6
+for k=2
     if k ==2
         qx = idntfcnTrjctry(k).q(k,:);
         qy = idntfcnTrjctry(k).q(6,:);
@@ -52,4 +52,6 @@ end
 end
 hold on;
 mesh(X,Y,tau_estimate,tau_estimate, 'FaceAlpha', 0.0)
+%%
+saveas(gcf, ['./figure2/' 'J' num2str(k) ' Gravity Model' '.jpg'])