testing

# Conflicts:
#	get_GCTraj_R1000_DVT.m

Identification_main.m

@@ -20,8 +20,10 @@ robot = feval(get_robot_func,theta,dtheta,ddtheta,file,opt);
 get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
 robot = feval(get_Kinematics_func,robot,opt);
 
+Jaco=JacobianSpace(robot.slist(:,1:7),theta(1:7));
 R1000_Dynamics_num;
-% % R1000_Dynamics;
+% test_SixAxisFT;
+% R1000_Dynamics;
 % robot = get_velocity(robot, opt);
 % robot = get_regressor(robot,opt);
 % % symbol matched

R1000_Dynamics_num.m

@@ -15,15 +15,15 @@ link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-% thetalist = [zero_;0.4807*ones_;-0.8717*ones_;0.3466*ones_;zero_;0.6965*ones_;-0.5224*ones_;0.3473*ones_;0.05*ones_]';
-% dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-% ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+thetalist = [zero_;zero_;zero_;zero_;zero_;pi/3*ones_;zero_;zero_;zero_]';
+dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 
 %real traj
-get_GCTraj_R1000_DVT;
-thetalist = idntfcnTrjctry(8).q';
-dthetalist = idntfcnTrjctry(8).qd';
-ddthetalist = idntfcnTrjctry(8).qdd';
+% get_GCTraj_R1000_DVT;
+% thetalist = idntfcnTrjctry(6).q';
+% dthetalist = idntfcnTrjctry(6).qd';
+% ddthetalist = idntfcnTrjctry(6).qdd';
 
 % Get general mass matrix
 Glist=[];
@@ -107,7 +107,7 @@ for i = 1:N
     elseif i< 9
         ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
     else
-        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));
@@ -138,9 +138,6 @@ Mlist_ED = cat(3, Mlist_ED, M);
 %TODO: Get Slist form DH table method
 % RRRRRRRRP
 Slist=robot.slist;
-
-Vlinear=sym(zeros(3,3));
-J=sym(zeros(6,N));
 exf=[0;0;0;0;0;0];
 
 for i = 1:length(thetalist)
@@ -170,6 +167,7 @@ for i = 1:3
 end
 
 F_Simpack = permute(F_Simpack,[2 1 3]);
+F_Simpack = -F_Simpack;
 figure(2)
 for i = 1:3
     subplot(3,1,i);

get_GCTraj_R1000_DVT.m

@@ -1,8 +1,12 @@
 %temp get DVT traj
 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];
+% DVT1-2
+% motorConstant = [0.21,0.21,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:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\lab12.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")
+load("D:\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
 isCurrentSensor = true;
 % J9 traj

plotData_current.m

@@ -1,11 +1,13 @@
 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];
+% DVT1-2
+% motorConstant = [0.21,0.21,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:\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");
 posDir = [1,1,1,1,1,1,1,-1,1];
 
 % J9 traj
-for i=9
+for i=8
     fileData=eval(strcat('fileData', num2str(i)));
     data = fileData.data;
     dataLength = length(data);
@@ -30,7 +32,7 @@ end
 % should run identifcation program firstly
 resolution = 20;
 tau_estimate=[];
-for k=9
+for k=8
     if k ==2
         qx = idntfcnTrjctry(k).q(k,:);
         qy = idntfcnTrjctry(k).q(6,:);

untitled3.m

@@ -169,7 +169,7 @@ legend('Torque compute form current')
 title('Torque compute form current')
 %%
 % F_Simpack = permute(F_Simpack,[2 1 3]);
-plot(thetalist(:,8),-reshape(F_Simpack(7,3,:),[1,length(F_Simpack)]));
-test = fileData8.data(:,(6*(8+1))+11*3-3);
-test_time = fileData8.data(:,8+1+11);
-hold on;plot(test_time,-test)
+plot(thetalist(:,6),-reshape(F_Simpack(7,5,:),[1,length(F_Simpack)]));
+test = fileData6.data(:,(6*(8+1))+11*3-5);
+test_time = fileData6.data(:,6+1+11);
+hold on;plot(test_time,test)