refactor

Identification_main.m

@@ -5,7 +5,7 @@ opt.KM_method = 'SCREW';
 opt.Vel_method = 'Direct';
 opt.LD_method = 'Direct';
 opt.debug = false;
-opt.robotName = 'R1000_DVT';
+opt.robotName = 'R1000_EVT';
 opt.reGenerate = false;
 opt.Isreal = true;
 opt.isJointTorqueSensor = true;
@@ -14,8 +14,11 @@ opt.isSixAxisFTSensor = false;
 theta = zeros(9,1);
 dtheta = zeros(9,1);
 ddtheta = zeros(9,1);
-robot = get_robot_R1000_EVT(theta,dtheta,ddtheta,file,opt);
+
-robot = get_Kinematics_EVT(robot, opt);
+get_robot_func = sprintf('get_robot_%s',opt.robotName);
+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;
@@ -24,8 +27,8 @@ robot = get_regressor(robot,opt);
 % symbol matched
 % verify_regressor_R1000;
 robot = get_baseParams(robot, opt);
-readDataFile;
+% readDataFile;
-robot.posData = posData;
+% robot.posData = posData;
-robot.currentData = currentData;
+% robot.currentData = currentData;
-% robot = estimate_dyn(robot,opt);
+% % % robot = estimate_dyn(robot,opt);
-robot = estimate_dyn_form_data(robot,opt);
+% robot = estimate_dyn_form_data(robot,opt);

codegen/base_6AxisFT_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_robot_R1000_DVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
-robot = get_Kinematics(robot, opt);
+robot = get_Kinematics_R1000_DVT(robot, opt);
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot, opt);
 % get base params

codegen/base_6AxisFT_regressor_R1000_EVT.m

@@ -0,0 +1,21 @@
+function base_regrssor  = base_6AxisFT_regressor_R1000_EVT(theta,dtheta,ddtheta,baseQR)
+opt.robot_def = 'direct';
+opt.KM_method = 'SCREW';
+opt.Vel_method = 'Direct';
+opt.LD_method = 'Direct';
+opt.debug = false;
+opt.robotName = 'R1000_DVT';
+opt.reGenerate = false;
+opt.Isreal = true;
+opt.isJointTorqueSensor = false;
+opt.isSixAxisFTSensor = true;
+file=[];
+
+robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
+robot = get_Kinematics_R1000_EVT(robot, opt);
+robot = get_velocity(robot, opt);
+robot = get_regressor(robot, opt);
+% get base params
+robot.baseQR = baseQR;
+robot.baseQR.regressor = robot.regressor.U*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
+base_regrssor = robot.baseQR.regressor;

codegen/base_regressor_R1000_DVT.m

@@ -4,11 +4,11 @@ opt.KM_method = 'SCREW';
 opt.Vel_method = 'Direct';
 opt.LD_method = 'Direct';
 opt.debug = false;
-opt.robotName = 'R1000_DVT';
+opt.robotName = 'R1000_EVT';
 opt.reGenerate = false;
 opt.Isreal = true;
-opt.isJointTorqueSensor = false;
+opt.isJointTorqueSensor = true;
-opt.isSixAxisFTSensor = true;
+opt.isSixAxisFTSensor = false;
 file=[];
 
 robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);

codegen/base_regressor_R1000_EVT.m

@@ -0,0 +1,21 @@
+function base_regrssor  = base_regressor_R1000_DVT(theta,dtheta,ddtheta,baseQR)
+opt.robot_def = 'direct';
+opt.KM_method = 'SCREW';
+opt.Vel_method = 'Direct';
+opt.LD_method = 'Direct';
+opt.debug = false;
+opt.robotName = 'R1000_DVT';
+opt.reGenerate = false;
+opt.Isreal = true;
+opt.isJointTorqueSensor = false;
+opt.isSixAxisFTSensor = true;
+file=[];
+
+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
+robot.baseQR = baseQR;
+robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
+base_regrssor = robot.baseQR.regressor;

codegen/regressor_R1000_EVT.m

@@ -0,0 +1,18 @@
+function regrssor  = regressor_R1000_DVT(theta,dtheta,ddtheta)
+opt.robot_def = 'direct';
+opt.KM_method = 'SCREW';
+opt.Vel_method = 'Direct';
+opt.LD_method = 'Direct';
+opt.debug = false;
+opt.robotName = 'R1000_DVT';
+opt.reGenerate = false;
+opt.Isreal = true;
+opt.isJointTorqueSensor = false;
+opt.isSixAxisFTSensor = true;
+file=[];
+
+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;

codegen/standard_regressor_R1000_EVT.m

@@ -0,0 +1,18 @@
+function standard_regrssor  = standard_regressor_R1000_EVT(theta,dtheta,ddtheta)
+opt.robot_def = 'direct';
+opt.KM_method = 'SCREW';
+opt.Vel_method = 'Direct';
+opt.LD_method = 'Direct';
+opt.debug = false;
+opt.robotName = 'R1000_EVT';
+opt.reGenerate = false;
+opt.Isreal = true;
+opt.isJointTorqueSensor = false;
+opt.isSixAxisFTSensor = true;
+file=[];
+
+robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
+robot = get_Kinematics_R1000_EVT(robot, opt);
+robot = get_velocity(robot, opt);
+robot = get_regressor(robot, opt);
+standard_regrssor = robot.regressor.K;

estimate_dyn_form_data_MLS.m

@@ -0,0 +1,117 @@
+function robot = estimate_dyn_form_data(robot,opt)
+% -------------------------------------------------------------------
+% Get datas
+% ------------------------------------------------------------------------
+
+% -------------------------------------------------------------------
+% Generate Regressors based on data
+% ------------------------------------------------------------------------
+drvGains = [];
+baseQR = robot.baseQR;
+pi_b = buildObservationMatrices_MLS(baseQR, drvGains, opt);
+
+% ---------------------------------------------------------------------
+% Estimate parameters
+% ---------------------------------------------------------------------
+sol = struct;
+% method = 'OLS';
+% if strcmp(method, 'OLS')
+%     % Usual least squares
+%     [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(Tau, Wb);
+% elseif strcmp(method, 'PC-OLS')
+%     % Physically consistent OLS using SDP optimization
+%     [sol.pi_b] = physicallyConsistentEstimation(Tau, Wb);
+% else
+%     error("Chosen method for dynamic parameter estimation does not exist");
+% end
+sol.pi_b = pi_b;
+robot.sol = sol;
+% Local unctions
+    function pi_b = buildObservationMatrices_MLS(baseQR, drvGains,opt)
+        % The function builds observation matrix for UR10E
+%         E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
+        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];
+        posData = robot.posData;
+        currentData = robot.currentData;
+        [nRow,nCol] = size(posData); pi_b =[];
+        %get data for MLS
+        for j = 9:-1:7
+            index_p = find(posData(:,nCol-1)==j);
+            jointPos_p = posData(index_p,1:9);
+            jointPos_p = jointPos_p(1:500:end,:);
+            index_n = find(posData(:,nCol-1)==-j);
+            jointPos_n = posData(index_n,1:9);
+            jointPos_n = jointPos_n(1:500:end,:);
+            q = [jointPos_p]';
+            qd = q; qdd = q;
+            
+            % pi -> [m;mc;I] 10 element
+            index_p = find(currentData(:,nCol-1)==j);
+            jointCur_p = currentData(index_p,1:9);
+            jointCur_p = jointCur_p(1:500:end,:);
+            index_n = find(currentData(:,nCol-1)==-j);
+            jointCur_n = currentData(index_n,1:9);
+            jointCur_n = jointCur_n(1:500:end,:);
+            current = (jointCur_p+jointCur_n)/2;
+            torque_cur = gearRatio.*motorConst.*current;
+            tau = torque_cur(:,j)';
+            
+            idntfcnTrjctry.t = 1:length(q);
+            idntfcnTrjctry.q = q;
+            idntfcnTrjctry.qd = qd;
+            idntfcnTrjctry.qdd = qdd;
+            idntfcnTrjctry.tau = tau;
+            Wb = []; Tau = [];
+            for i = 1:1:length(idntfcnTrjctry.t)
+                %             Yi = regressorWithMotorDynamics(idntfcnTrjctry.q(i,:)',...
+                %                 idntfcnTrjctry.qd(i,:)',...
+                %                 idntfcnTrjctry.q2d(i,:)');
+                %             Yfrctni = frictionRegressor(idntfcnTrjctry.qd_fltrd(i,:)');
+                %             Ybi = [Yi*E1, Yfrctni];
+                if opt.isJointTorqueSensor
+                    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 = j;
+                    Yb = Yb(j,:);
+                    Wb = vertcat(Wb, Yb);
+                    Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
+                elseif opt.isSixAxisFTSensor
+                    base_6AxisFT_regressor_func = sprintf('base_6AxisFT_regressor_%s',opt.robotName);
+                    Yb = feval(base_6AxisFT_regressor_func, idntfcnTrjctry.q(:,i), ...
+                        idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
+                    joint_idex = robot.ndof-2;
+                    Yb = Yb(6*(joint_idex-1)+1:6*(joint_idex),:);
+                    Wb = vertcat(Wb, Yb);
+                    %             Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
+                    Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
+                end
+            end
+            pi_b_joint = ordinaryLeastSquareEstimation(Tau, Wb);
+            pi_b = vertcat(pi_b,pi_b_joint);
+        end
+    end
+
+
+    function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(Tau, Wb)
+        % Function perfroms ordinary least squares estimation of parameters
+%         pi_OLS = (Wb'*Wb)\(Wb'*Tau);
+%         pib_OLS = pi_OLS(1:40); % variables for base paramters
+%         pifrctn_OLS = pi_OLS(41:end);
+          pib_OLS=pinv(Wb)*Tau;
+          pifrctn_OLS = 0;
+    end
+    function [pib_OLS, pifrctn_OLS] = MultiLeastSquareEstimation(idntfcnTrjctry, Tau, Wb)
+        % Function perfroms Multi step ordinary least squares estimation of parameters
+          
+        pib_OLS=pinv(Wb)*Tau;
+        pifrctn_OLS = 0;
+    end
+    function pib = physicallyConsistentEstimation(Tau, Wb)
+        % 求解线性规划问题
+        options = optimoptions('lsqlin','Algorithm','trust-region-reflective','Display','iter');
+        pib = lsqlin(Wb,Tau,[],[],[],[],-2*ones(5,1),2*ones(5,1),[0.9151,0.0092,0.0002,-0.0644,0.1067]',options)
+    end
+end

get_baseParams.m

@@ -26,7 +26,7 @@ for i = 1:25
         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),:);
+        Y = Y(joint_idex:end,:);
     elseif isSixAxisFTSensor
         regressor_func = sprintf('regressor_%s',opt.robotName);
         Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd);

untitled3.m

@@ -118,10 +118,10 @@ gearRatio = [101,101,100,100,100,100,100,100,100];
 [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,:);
+jointPos_p = jointPos_p(1:500:end,:);
 index_n = find(posData(:,nCol-1)==-7);
 jointPos_n = posData(index_n,1:9);
-jointPos_n = jointPos_n(1:2000:end,:);
+jointPos_n = jointPos_n(1:500:end,:);
 q = (jointPos_p)';
 % dq = [zeros(robot.ndof,1),diff(q,1,2)];
 % get torque from simulator
@@ -135,10 +135,35 @@ 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,:);
+jointCur_p = jointCur_p(1:500:end,:);
 index_n = find(currentData(:,nCol-1)==-7);
 jointCur_n = currentData(index_n,1:9);
-jointCur_n = jointCur_n(1:2000:end,:);
+jointCur_n = jointCur_n(1:500:end,:);
 current = (jointCur_p+jointCur_n)/2;
 torque_cur = gearRatio.*motorConst.*current;
-
+plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
+plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),Tau(7,:))
+xlabel('Time/s')
+ylabel('Torque/Nm')
+legend('Torque compute form current','Torque generate form simulator using CAD data')
+title('Torque compute form current vs Torque generate form simulator using CAD data')
+%%
+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.sol.pi_b;
+    Tau = horzcat(Tau, tau);
+end
+plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
+plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),Tau(7,:))
+xlabel('Time/s')
+ylabel('Torque/Nm')
+legend('Torque compute form current','Torque form simulator using identificated data')
+title('Torque compute form current vs Torque form simulator using identificated data')
+%%
+plot(posData(index_p(1:500:end),end)-posData(index_p(1),end),torque_cur(:,7));hold on;
+xlabel('Time/s')
+ylabel('Torque/Nm')
+legend('Torque compute form current')
+title('Torque compute form current')