diff --git a/complie/R1000 DVT GravityModel V1/calculateGravityModel_mex.mexw64 b/complie/R1000 DVT GravityModel V1/calculateGravityModel_mex.mexw64 new file mode 100644 index 0000000..e2a3c07 Binary files /dev/null and b/complie/R1000 DVT GravityModel V1/calculateGravityModel_mex.mexw64 differ diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/SetEnv.bat b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/SetEnv.bat new file mode 100644 index 0000000..e69de29 diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/buildInfo.mat b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/buildInfo.mat new file mode 100644 index 0000000..4ff410b Binary files /dev/null and b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/buildInfo.mat differ diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.bat b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.bat new file mode 100644 index 0000000..1b13afa --- /dev/null +++ b/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 %* diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.def b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.def new file mode 100644 index 0000000..96257bd --- /dev/null +++ b/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 diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.exp b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.exp new file mode 100644 index 0000000..863c02f --- /dev/null +++ b/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 diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.lib b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.lib new file mode 100644 index 0000000..e0ab0a6 Binary files /dev/null and b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.lib differ diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.mexw64 b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.mexw64 new file mode 100644 index 0000000..e2a3c07 Binary files /dev/null and b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/calculateGravityModel_mex.mexw64 differ diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/cfiGuiReport.mat b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/cfiGuiReport.mat new file mode 100644 index 0000000..3af6984 Binary files /dev/null and b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/cfiGuiReport.mat differ diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/gcGuiReport.mat b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/gcGuiReport.mat new file mode 100644 index 0000000..ed08962 Binary files /dev/null and b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/gcGuiReport.mat differ diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/html/report.mldatx b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/html/report.mldatx new file mode 100644 index 0000000..3e16b5c Binary files /dev/null and b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/html/report.mldatx differ diff --git a/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/mexFunction.def b/complie/R1000 DVT GravityModel V1/codegen/mex/calculateGravityModel/mexFunction.def new file mode 100644 index 0000000..e69de29 diff --git a/complie/R1000 DVT GravityModel V1/codegen_test.m b/complie/R1000 DVT GravityModel V1/codegen_test.m new file mode 100644 index 0000000..a0b38a9 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/plotData_current.m b/plotData_current.m new file mode 100644 index 0000000..2c34ff0 --- /dev/null +++ b/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) +