two bar ok

autogen/base_regressor_Two_bar.m

@@ -0,0 +1,24 @@
+function out1 = base_regressor_Two_bar(in1,in2,in3)
+%base_regressor_Two_bar
+%    OUT1 = base_regressor_Two_bar(IN1,IN2,IN3)
+
+%    This function was generated by the Symbolic Math Toolbox version 9.1.
+%    14-Jan-2024 21:45:43
+
+q2 = in1(2,:);
+qd1 = in2(1,:);
+qd2 = in2(2,:);
+qdd1 = in3(1,:);
+qdd2 = in3(2,:);
+t2 = cos(q2);
+t3 = sin(q2);
+t4 = qd1+qd2;
+t5 = qdd1+qdd2;
+t6 = qd1.^2;
+t7 = qdd1.*t2;
+t8 = qdd1.*t3;
+t9 = t4.^2;
+t10 = t2.*t6;
+t11 = t3.*t6;
+t12 = -t8;
+out1 = reshape([t7+t11+t2.*t5-t3.*t9,t7+t11,t10+t12-t3.*t5-t2.*t9,t10+t12,t5,t5,qdd1,0.0],[2,4]);

autogen/standard_regressor_Two_bar.m

@@ -3,7 +3,7 @@ function out1 = standard_regressor_Two_bar(in1,in2,in3)
 %    OUT1 = standard_regressor_Two_bar(IN1,IN2,IN3)
 
 %    This function was generated by the Symbolic Math Toolbox version 9.1.
-%    10-Jan-2024 23:01:38
+%    14-Jan-2024 21:45:42
 
 q2 = in1(2,:);
 qd1 = in2(1,:);

estimate_dyn.m

@@ -0,0 +1,79 @@
+function robot = estimate_dyn(robot,opt)
+% -------------------------------------------------------------------
+% Get datas
+% ------------------------------------------------------------------------
+time = 0:0.01:2;
+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];
+qd=[qd_J; -qd_J];
+qdd=[qdd_J; -qdd_J];
+g = [0; 0; -9.8];
+tau = zeros([2,101]);
+robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+robot_pi2=[2;1;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+robot_pi=[robot_pi1;robot_pi2];
+for i = 1:length(q_J)
+    regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
+    tau(:,i)=regressor*robot_pi;
+end
+idntfcnTrjctry.t = time;
+idntfcnTrjctry.q = q;
+idntfcnTrjctry.qd = qd;
+idntfcnTrjctry.qdd = qdd;
+idntfcnTrjctry.tau = tau;
+% -------------------------------------------------------------------
+% Generate Regressors based on data
+% ------------------------------------------------------------------------
+drvGains = [];
+baseQR = robot.baseQR;
+[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains);
+
+% ---------------------------------------------------------------------
+% 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, sol.pi_fr, sol.pi_s] = physicallyConsistentEstimation(Tau, Wb, baseQR);
+else
+    error("Chosen method for dynamic parameter estimation does not exist");
+end
+robot.sol = sol;
+% Local unctions
+    function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains)
+        % The function builds observation matrix for UR10E
+        E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
+
+        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];
+            base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
+            Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
+                idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i));
+            Wb = vertcat(Wb, Yb);
+            %             Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
+            Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
+        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
+end

get_baseParams.m

@@ -76,3 +76,13 @@ baseQR.beta = beta;
 baseQR.motorDynamicsIncluded = includeMotorDynamics;
 baseQR.baseParams = pi_lgr_base;
 robot.baseQR = baseQR;
+
+% ---------------------------------------------------------------------
+% Gen base_regressor
+% ---------------------------------------------------------------------
+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');
+robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
+matlabFunction(robot.baseQR.regressor,'File',sprintf('autogen/base_regressor_%s',opt.robotName),...
+    'Vars',{q_sym,qd_sym,q2d_sym});

verify_regressor.m

@@ -4,7 +4,7 @@ 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');
-pi1=[2;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
 pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
 % pi2=zeros([10,1]);
 pi=[pi1;pi2];
@@ -56,3 +56,24 @@ j=1;
 Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
 j=2;
 Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
+%% Numerical
+clear pi;
+ndof = robot.ndof;
+time = 0:0.01:2;
+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];
+qd=[qd_J; -qd_J];
+qdd=[qdd_J; -qdd_J];
+g = [0; 0; -9.8];
+robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+robot_pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
+% pi2=zeros([10,1]);
+robot_pi=[robot_pi1;robot_pi2];
+tau = zeros([2,100]);
+for i = 1:length(q_J)
+    regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
+    tau(:,i)=regressor*robot_pi;
+end