Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

two bar ok

This commit is contained in:
cosmic_power 2024-01-23 21:39:40 +08:00
parent 49baf628f1
commit b9faa56921
5 changed files with 138 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
autogen/base_regressor_Two_bar.m Normal file
View File

@ -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]);

2
autogen/standard_regressor_Two_bar.m
View File

@ -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,:);

79
estimate_dyn.m Normal file
View File

@ -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

12
get_baseParams.m
View File

@ -75,4 +75,14 @@ baseQR.permutationMatrix = E;
baseQR.beta = beta;
baseQR.motorDynamicsIncluded = includeMotorDynamics;
baseQR.baseParams = pi_lgr_base;
robot.baseQR = baseQR;
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});

25
verify_regressor.m
View File

@ -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];
@ -55,4 +55,25 @@ F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
j=1;
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
j=2;
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
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