add plotData_curent

complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.bat

@@ -0,0 +1,4 @@
+@echo off
+set MATLAB=D:\software\MATLAB\R2020a
+call "D:\software\MATLAB\R2020a\sys\lcc64\lcc64\mex\lcc64opts.bat"
+"D:\software\MATLAB\R2020a\toolbox\shared\coder\ninja\win64\ninja.exe" -v %*

complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.def

@@ -0,0 +1,9 @@
+LIBRARY calculateGravityModel_mex.mexw64
+EXPORTS
+mexFunction
+mexfilerequiredapiversion
+emlrtMexFcnProperties
+calculateGravityModel_initialize
+calculateGravityModel_terminate
+calculateGravityModel_atexit
+calculateGravityModel

complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.exp

@@ -0,0 +1,8 @@
+calculateGravityModel_mex.mexw64
+calculateGravityModel		calculateGravityModel
+calculateGravityModel_atexit		calculateGravityModel_atexit
+calculateGravityModel_initialize		calculateGravityModel_initialize
+calculateGravityModel_terminate		calculateGravityModel_terminate
+emlrtMexFcnProperties		emlrtMexFcnProperties
+mexFunction		mexFunction
+mexfilerequiredapiversion		mexfilerequiredapiversion

complie/R1000 DVT GravityModel V1/codegen_test.m

@@ -0,0 +1,4 @@
+thetalist = [0;0;0;0;0;pi/2;0;0;0];
+load('permutationMatrix.mat')
+load('pib_MLS.mat')
+calculateGravityModel_mex(thetalist,permutationMatrix,pib_MLS)

plotData_current.m

@@ -0,0 +1,55 @@
+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("D:\1833128421\123同步文件夹\R1000-GC-Data\lab12.mat");
+posDir = [1,1,1,1,1,1,1,-1,1];
+
+% J9 traj
+for i=6
+    fileData=eval(strcat('fileData', num2str(i)));
+    data = fileData.data;
+    dataLength = length(data);
+    figure(i)
+    d1Length = floor(length(data) / 2);
+    % Hack: change J8 dir
+    %data(:,8+1+11) = -data(:,8+1+11);
+    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;
+    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;
+    end
+    hold off;
+    title(['J' num2str(i) ' Gravity Model']);
+    xlabel('X Pos/rad');ylabel('Y Pos/rad');zlabel('Torque /Nm')
+end
+%%
+% should run identifcation program firstly
+resolution = 20;
+tau_estimate=[];
+for k=6
+    if k ==2
+        qx = idntfcnTrjctry(k).q(k,:);
+        qy = idntfcnTrjctry(k).q(6,:);
+    else
+        qx = idntfcnTrjctry(k).q(k,:);
+        qy = idntfcnTrjctry(k).q(k-1,:);
+    end
+[X, Y] = meshgrid(linspace(min(qx), max(qx), resolution), linspace(min(qy), max(qy), resolution));
+q = mean(idntfcnTrjctry(k).q,2);qd=zeros(9,1);qdd=zeros(9,1);
+for i = 1 : length(X)
+    for j = 1 : length(Y)
+        q(k) = X(i,j);q(k-1) = Y(i,j);
+        base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
+        Yb = feval(base_regressor_func,q,qd,qdd,robot.baseQR);
+        torque=Yb*robot.sol.pib_MLS;
+        current_estimate(i,j)=torque(k)/(gearRatio(k)*motorConstant(k));
+    end
+end
+end
+hold on;
+mesh(X,Y,current_estimate,current_estimate, 'FaceAlpha', 0.0)
+