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

6 axis force sensor can be estimated

This commit is contained in:
cosmic_power 2024-11-05 23:20:47 +08:00
parent 22d94771f9
commit ecce2eaa10
8 changed files with 198 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Identification_main.m
View File

@ -24,4 +24,4 @@ robot = get_regressor(robot,opt);
% symbol matched
% verify_regressor_R1000;
robot = get_baseParams(robot, opt);
% robot = estimate_dyn(robot,opt);
robot = estimate_dyn(robot,opt);

21
codegen/base_6AxisFT_regressor_R1000_DVT.m Normal file
View File

@ -0,0 +1,21 @@
function base_regrssor = base_6AxisFT_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(theta,dtheta,ddtheta,file,opt);
robot = get_Kinematics(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;

2
codegen/base_regressor_R1000_DVT.m
View File

@ -7,6 +7,8 @@ opt.debug = false;
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);

18
codegen/regressor_R1000_DVT.m Normal file
View File

@ -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_EVT';
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_velocity(robot, opt);
robot = get_regressor(robot, opt);
regrssor = robot.regressor.U;

2
codegen/standard_regressor_R1000_DVT.m
View File

@ -7,6 +7,8 @@ opt.debug = false;
opt.robotName = 'R1000_EVT';
opt.reGenerate = false;
opt.Isreal = true;
opt.isJointTorqueSensor = false;
opt.isSixAxisFTSensor = true;
file=[];
robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);

109
estimate_dyn.asv Normal file
View File

@ -0,0 +1,109 @@
function robot = estimate_dyn(robot,opt)
% -------------------------------------------------------------------
% Get datas
% ------------------------------------------------------------------------
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];
% 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)]);
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;
end
elseif opt.isSixAxisFTSensor
tau = zeros([6,length(q_J)]);
for i = 1:length(q_J)
regressor_func = sprintf('regressor_%s',opt.robotName);
regressor = feval(regressor_func,q(:,i),qd(:,i),qdd(:,i));
FT = regressor*robot_pi;
joint_idex = robot.ndof-2;
tau(:,i) = FT(6*(joint_idex-1)+1:6*(joint_idex));
end
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, 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, 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,opt)
% 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];
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);
Wb = vertcat(Wb, Yb);
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
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;
tau(:,i) = FT(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
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
end

28
estimate_dyn.m
View File

@ -11,16 +11,27 @@ 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]);
if opt.isJointTorqueSensor
tau = zeros([robot.ndof,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;
end
elseif opt.isSixAxisFTSensor
tau = zeros([6,length(q_J)]);
for i = 1:length(q_J)
regressor_func = sprintf('regressor_%s',opt.robotName);
regressor = feval(regressor_func,q(:,i),qd(:,i),qdd(:,i));
FT = regressor*robot_pi;
joint_idex = robot.ndof-2;
tau(:,i) = FT(6*(joint_idex-1)+1:6*(joint_idex));
end
end
idntfcnTrjctry.t = time;
idntfcnTrjctry.q = q;
idntfcnTrjctry.qd = qd;
@ -31,7 +42,7 @@ idntfcnTrjctry.tau = tau;
% ------------------------------------------------------------------------
drvGains = [];
baseQR = robot.baseQR;
[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains);
[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains, opt);
% ---------------------------------------------------------------------
% Estimate parameters
@ -49,7 +60,7 @@ else
end
robot.sol = sol;
% Local unctions
function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains)
function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt)
% The function builds observation matrix for UR10E
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
@ -60,12 +71,23 @@ robot.sol = sol;
% 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);
Wb = vertcat(Wb, Yb);
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
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
end

18
get_baseParams.m
View File

@ -26,16 +26,16 @@ 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),:);
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);
Zeros_ = zeros(size(Y));
Zeros_(ndof-2,:) = Y(ndof-2,:);
Y = Zeros_;
% standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
% Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
% Zeros_ = zeros(size(Y));
% Zeros_(ndof-2,:) = Y(ndof-2,:);
% Y = Zeros_;
end
W = vertcat(W,Y);
end