add 6 axis force sensor identification

Identification_main.m

@@ -8,6 +8,8 @@ opt.debug = false;
 opt.robotName = 'R1000_DVT';
 opt.reGenerate = false;
 opt.Isreal = true;
+opt.isJointTorqueSensor = false;
+opt.isSixAxisFTSensor = true;
 
 theta = zeros(9,1);
 dtheta = zeros(9,1);

get_baseParams.m

@@ -14,7 +14,7 @@ q2d_max = 6*pi*ones(ndof,1);
 % Find relation between independent columns and dependent columns
 % -----------------------------------------------------------------------
 % Get observation matrix of identifiable paramters
-W = []; isSixAxisFTSensor =1; isJointTorqueSensor =0;  
+W = []; isSixAxisFTSensor =opt.isSixAxisFTSensor; isJointTorqueSensor =opt.isJointTorqueSensor;  
 for i = 1:25
     q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
     qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
@@ -26,11 +26,15 @@ for i = 1:25
         standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
         Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
     elseif isSixAxisFTSensor
+%         regressor_func = sprintf('regressor_%s',opt.robotName);
+%         Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd);
+%         joint_idex = ndof-2;
+%         Y = Y(6*(joint_idex-1)+1:6*(joint_idex),:);
+        % FIXME hack here
         standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
         Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
-        % FIXME hack here
         Zeros_ = zeros(size(Y));
-        Zeros_(ndof-3,:) = Y(ndof-3,:);
+        Zeros_(ndof-2,:) = Y(ndof-2,:);
         Y = Zeros_;
     end
     W = vertcat(W,Y);