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

Identification_main.m

@@ -20,13 +20,13 @@ 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);
 
-% R1000_Dynamics_num;
-% R1000_Dynamics;
-robot = get_velocity(robot, opt);
-robot = get_regressor(robot,opt);
-% symbol matched
-% verify_regressor_R1000;
-robot = get_baseParams(robot, opt);
-% robot = estimate_dyn(robot,opt);
-% robot = estimate_dyn_form_data(robot,opt);
-robot = estimate_dyn_MLS(robot,opt);
+R1000_Dynamics_num;
+% % R1000_Dynamics;
+% robot = get_velocity(robot, opt);
+% robot = get_regressor(robot,opt);
+% % symbol matched
+% % verify_regressor_R1000;
+% robot = get_baseParams(robot, opt);
+% % robot = estimate_dyn(robot,opt);
+% % robot = estimate_dyn_form_data(robot,opt);
+% robot = estimate_dyn_MLS(robot,opt);

R1000_Dynamics_num.m

@@ -21,9 +21,9 @@ link_inertia = robot.I;
 
 %real traj
 get_GCTraj_R1000_DVT;
-thetalist = idntfcnTrjctry(6).q';
-dthetalist = idntfcnTrjctry(6).qd';
-ddthetalist = idntfcnTrjctry(6).qdd';
+thetalist = idntfcnTrjctry(8).q';
+dthetalist = idntfcnTrjctry(8).qd';
+ddthetalist = idntfcnTrjctry(8).qdd';
 
 % Get general mass matrix
 Glist=[];
@@ -143,7 +143,7 @@ Vlinear=sym(zeros(3,3));
 J=sym(zeros(6,N));
 exf=[0;0;0;0;0;0];
 
-for i = 1:length(q_J)
+for i = 1:length(thetalist)
     [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
         = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
         [0;0;-9.806], exf, Mlist_CG, Glist, Slist);

get_GCTraj_R1000_DVT.m

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

plotData_current.m

@@ -1,7 +1,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("D:\1833128421\123同步文件夹\R1000-GC-Data\lab12.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

untitled3.m

@@ -167,3 +167,9 @@ xlabel('Time/s')
 ylabel('Torque/Nm')
 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)