diff --git a/Identification_main.m b/Identification_main.m
index 1bc9382..8331d11 100644
--- a/Identification_main.m
+++ b/Identification_main.m
@@ -14,8 +14,8 @@ opt.isSixAxisFTSensor = false;
 theta = zeros(9,1);
 dtheta = zeros(9,1);
 ddtheta = zeros(9,1);
-robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);
-robot = get_Kinematics(robot, opt);
+robot = get_robot_R1000_EVT(theta,dtheta,ddtheta,file,opt);
+robot = get_Kinematics_EVT(robot, opt);
 
 % R1000_Dynamics_num;
 % R1000_Dynamics;
@@ -24,4 +24,8 @@ robot = get_regressor(robot,opt);
 % symbol matched
 % verify_regressor_R1000;
 robot = get_baseParams(robot, opt);
-robot = estimate_dyn(robot,opt);
\ No newline at end of file
+readDataFile;
+robot.posData = posData;
+robot.currentData = currentData;
+% robot = estimate_dyn(robot,opt);
+robot = estimate_dyn_form_data(robot,opt);
\ No newline at end of file
diff --git a/codegen/base_6AxisFT_regressor_R1000_DVT.m b/codegen/base_6AxisFT_regressor_R1000_DVT.m
index d25958d..7250cbb 100644
--- a/codegen/base_6AxisFT_regressor_R1000_DVT.m
+++ b/codegen/base_6AxisFT_regressor_R1000_DVT.m
@@ -11,7 +11,7 @@ opt.isJointTorqueSensor = false;
 opt.isSixAxisFTSensor = true;
 file=[];
 
-robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);
+robot = get_robot_R1000(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
 robot = get_Kinematics(robot, opt);
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot, opt);
diff --git a/codegen/base_regressor_R1000_DVT.m b/codegen/base_regressor_R1000_DVT.m
index 2962cec..340b309 100644
--- a/codegen/base_regressor_R1000_DVT.m
+++ b/codegen/base_regressor_R1000_DVT.m
@@ -11,8 +11,8 @@ opt.isJointTorqueSensor = false;
 opt.isSixAxisFTSensor = true;
 file=[];
 
-robot = get_robot_R1000(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
-robot = get_Kinematics(robot, opt);
+robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
+robot = get_Kinematics_EVT(robot, opt);
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot, opt);
 % get base params
diff --git a/codegen/regressor_R1000_DVT.m b/codegen/regressor_R1000_DVT.m
index 097421e..ac6de0a 100644
--- a/codegen/regressor_R1000_DVT.m
+++ b/codegen/regressor_R1000_DVT.m
@@ -4,15 +4,15 @@ opt.KM_method = 'SCREW';
 opt.Vel_method = 'Direct';
 opt.LD_method = 'Direct';
 opt.debug = false;
-opt.robotName = 'R1000_EVT';
+opt.robotName = 'R1000_DVT';
 opt.reGenerate = false;
 opt.Isreal = true;
 opt.isJointTorqueSensor = false;
 opt.isSixAxisFTSensor = true;
 file=[];
 
-robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);
-robot = get_Kinematics(robot, opt);
+robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
+robot = get_Kinematics_EVT(robot, opt);
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot, opt);
 regrssor = robot.regressor.U;
\ No newline at end of file
diff --git a/codegen/standard_regressor_R1000_DVT.m b/codegen/standard_regressor_R1000_DVT.m
index d33ff7e..8628eba 100644
--- a/codegen/standard_regressor_R1000_DVT.m
+++ b/codegen/standard_regressor_R1000_DVT.m
@@ -4,15 +4,15 @@ opt.KM_method = 'SCREW';
 opt.Vel_method = 'Direct';
 opt.LD_method = 'Direct';
 opt.debug = false;
-opt.robotName = 'R1000_EVT';
+opt.robotName = 'R1000_DVT';
 opt.reGenerate = false;
 opt.Isreal = true;
 opt.isJointTorqueSensor = false;
 opt.isSixAxisFTSensor = true;
 file=[];
 
-robot = get_robot_R1000(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
-robot = get_Kinematics(robot, opt);
+robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
+robot = get_Kinematics_EVT(robot, opt);
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot, opt);
 standard_regrssor = robot.regressor.K;
\ No newline at end of file
diff --git a/estimate_dyn.m b/estimate_dyn.m
index 40e5b58..512ac3c 100644
--- a/estimate_dyn.m
+++ b/estimate_dyn.m
@@ -2,7 +2,7 @@ function robot = estimate_dyn(robot,opt)
 % -------------------------------------------------------------------
 % Get datas
 % ------------------------------------------------------------------------
-time = 0:0.01:1;
+time = 0:0.01:0.5;
 f=1;
 q_J = sin(2*pi*f*time);
 qd_J = (2*pi*f)*cos(2*pi*f*time);
@@ -11,16 +11,22 @@ q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
 qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
 qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
 g = [0; 0; -9.8];
+toruqeSensorRange = [500;500;230;90;60;60;60;20;20];
 % pi -> [m;mc;I] 10 element 
 [nLnkPrms, nLnks] = size(robot.pi);
 robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
 if opt.isJointTorqueSensor
-    tau = zeros([robot.ndof,length(q_J)]);
+    %hack
+%     tau = zeros([robot.ndof,length(q_J)]);
+    tau = zeros([1,length(q_J)]);
     for i = 1:length(q_J)
         %     regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
         standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
         regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
-        tau(:,i)=regressor*robot_pi;
+        tau_Full=regressor*robot_pi;
+        joint_idex = robot.ndof-2;
+        tau(:,i) = tau_Full((joint_idex-1)+1:(joint_idex));
+%         tau(:,i) = tau(:,i) + 5*10^-3*toruqeSensorRange.*rand(size(tau(:,i)));
     end
 elseif opt.isSixAxisFTSensor
     tau = zeros([6,length(q_J)]);
@@ -75,6 +81,9 @@ robot.sol = sol;
                 base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
                 Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
                     idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
+                %hack                
+                joint_idex = robot.ndof-2;
+                Yb = Yb((joint_idex-1)+1:(joint_idex),:);
                 Wb = vertcat(Wb, Yb);
                 %             Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
                 Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
diff --git a/get_baseParams.m b/get_baseParams.m
index 1875933..c55f90a 100644
--- a/get_baseParams.m
+++ b/get_baseParams.m
@@ -25,6 +25,8 @@ for i = 1:25
     elseif isJointTorqueSensor
         standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
         Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
+        joint_idex = ndof-2;
+        Y = Y((joint_idex-1)+1:(joint_idex),:);
     elseif isSixAxisFTSensor
         regressor_func = sprintf('regressor_%s',opt.robotName);
         Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd);
diff --git a/get_regressor.m b/get_regressor.m
index c926037..5b197f8 100644
--- a/get_regressor.m
+++ b/get_regressor.m
@@ -1,28 +1,28 @@
 function robot = get_regressor(robot, opt)
 % Create symbolic generilized coordiates, their first and second deriatives
 ndof = robot.ndof;
-q_sym = sym('q%d',[ndof+1,1],'real');
-qd_sym = sym('qd%d',[ndof+1,1],'real');
-q2d_sym = sym('qdd%d',[ndof+1,1],'real');
-% init regressor
-robot.regressor.m = sym('m%d',[ndof,1],'real');
-robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real');
-robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real');
-robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real');
-robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real');
-robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real');
-robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real');
-robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real');
-robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real');
-robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
-robot.regressor.im = sym('im%d',[ndof,1],'real');
-for i = 1:ndof
-    robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),...
-        robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),...
-        robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
-end
-[nLnkPrms, nLnks] = size(robot.regressor.pi);
-robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
+% q_sym = sym('q%d',[ndof+1,1],'real');
+% qd_sym = sym('qd%d',[ndof+1,1],'real');
+% q2d_sym = sym('qdd%d',[ndof+1,1],'real');
+% % init regressor
+% robot.regressor.m = sym('m%d',[ndof,1],'real');
+% robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real');
+% robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real');
+% robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real');
+% robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real');
+% robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real');
+% robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real');
+% robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real');
+% robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real');
+% robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
+% robot.regressor.im = sym('im%d',[ndof,1],'real');
+% for i = 1:ndof
+%     robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),...
+%         robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),...
+%         robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
+% end
+% [nLnkPrms, nLnks] = size(robot.regressor.pi);
+% robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
 % init matrix
 R = robot.kine.R;
 P = robot.kine.t;
diff --git a/untitled3.m b/untitled3.m
index 93e0e9f..c36fe06 100644
--- a/untitled3.m
+++ b/untitled3.m
@@ -55,6 +55,52 @@ for i = 1:1:length(q_J)
     Wb = vertcat(Wb, Yb);
 end
 %%
+time = 0:0.01:1;
+f=1;
+q_J = sin(2*pi*f*time);
+qd_J = (2*pi*f)*cos(2*pi*f*time);
+qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
+q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
+qd=[qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J;qd_J];
+qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
+g = [0; 0; -9.8];
+tau = zeros([robot.ndof,length(q_J)]);
+% pi -> [m;mc;I] 10 element
+[nLnkPrms, nLnks] = size(robot.pi);
+robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
+Wb = []; Tau = [];Tau_estimate = [];
+for i = 1:length(q_J)
+    base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
+    Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i),robot.baseQR);
+    tau_estimate=Yb*robot.sol.pi_b;
+    tau = Yb*robot.baseQR.baseParams;
+    Tau_estimate = horzcat(Tau_estimate, tau_estimate);
+    Tau = horzcat(Tau, tau);
+end
+%% get symbol result for GC
+ndof = robot.ndof;
+q_sym = sym('q%d',[ndof+1,1],'real');
+qd_sym = sym('qd%d',[ndof+1,1],'real');
+q2d_sym = sym('qdd%d',[ndof+1,1],'real');
+% init regressor
+robot.regressor.m = sym('m%d',[ndof,1],'real');
+robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real');
+robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real');
+robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real');
+robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real');
+robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real');
+robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real');
+robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real');
+robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real');
+robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
+robot.regressor.im = sym('im%d',[ndof,1],'real');
+for i = 1:ndof
+    robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),...
+        robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),...
+        robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
+end
+[nLnkPrms, nLnks] = size(robot.regressor.pi);
+robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
 qr_rank = robot.baseQR.numberOfBaseParameters;
 E = robot.baseQR.permutationMatrix;
 pi_lgr_sym = robot.regressor.pi;
@@ -63,4 +109,36 @@ pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
 beta = robot.baseQR.beta;
 % all of the expressions below are equivalent
 pi_lgr_base = pi1 + beta*pi2;
-vpa(simplify(pi_lgr_base),2)
\ No newline at end of file
+vpa(simplify(pi_lgr_base),2)
+%% robot current and torque map
+motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185];
+gearRatio = [101,101,100,100,100,100,100,100,100];
+% readDataFile;
+% get pos
+[nRow,nCol] = size(posData);
+index_p = find(posData(:,nCol-1)==7);
+jointPos_p = posData(index_p,1:9);
+jointPos_p = jointPos_p(1:2000:end,:);
+index_n = find(posData(:,nCol-1)==-7);
+jointPos_n = posData(index_n,1:9);
+jointPos_n = jointPos_n(1:2000:end,:);
+q = (jointPos_p)';
+% dq = [zeros(robot.ndof,1),diff(q,1,2)];
+% get torque from simulator
+Wb = []; Tau = [];
+for i = 1:length(q)
+    base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
+    Yb = feval(base_regressor_func, q(:,i),zeros(size(q(:,i))),zeros(size(q(:,i))),robot.baseQR);
+    tau = Yb*robot.baseQR.baseParams;
+    Tau = horzcat(Tau, tau);
+end
+% get toruqe from current
+index_p = find(currentData(:,nCol-1)==7);
+jointCur_p = currentData(index_p,1:9);
+jointCur_p = jointCur_p(1:2000:end,:);
+index_n = find(currentData(:,nCol-1)==-7);
+jointCur_n = currentData(index_n,1:9);
+jointCur_n = jointCur_n(1:2000:end,:);
+current = (jointCur_p+jointCur_n)/2;
+torque_cur = gearRatio.*motorConst.*current;
+