diff --git a/Identification_main.m b/Identification_main.m index a28daea..b828a13 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -24,4 +24,4 @@ robot = get_regressor(robot,opt); % symbol matched % verify_regressor_R1000; robot = get_baseParams(robot, opt); -% robot = estimate_dyn(robot,opt); \ No newline at end of file +robot = estimate_dyn(robot,opt); \ No newline at end of file diff --git a/codegen/base_6AxisFT_regressor_R1000_DVT.m b/codegen/base_6AxisFT_regressor_R1000_DVT.m new file mode 100644 index 0000000..d25958d --- /dev/null +++ b/codegen/base_6AxisFT_regressor_R1000_DVT.m @@ -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; \ No newline at end of file diff --git a/codegen/base_regressor_R1000_DVT.m b/codegen/base_regressor_R1000_DVT.m index 8f3134b..18f4d0b 100644 --- a/codegen/base_regressor_R1000_DVT.m +++ b/codegen/base_regressor_R1000_DVT.m @@ -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); diff --git a/codegen/regressor_R1000_DVT.m b/codegen/regressor_R1000_DVT.m new file mode 100644 index 0000000..097421e --- /dev/null +++ b/codegen/regressor_R1000_DVT.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_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; \ No newline at end of file diff --git a/codegen/standard_regressor_R1000_DVT.m b/codegen/standard_regressor_R1000_DVT.m index a9e620c..521bd4f 100644 --- a/codegen/standard_regressor_R1000_DVT.m +++ b/codegen/standard_regressor_R1000_DVT.m @@ -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); diff --git a/estimate_dyn.asv b/estimate_dyn.asv new file mode 100644 index 0000000..c38b194 --- /dev/null +++ b/estimate_dyn.asv @@ -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 \ No newline at end of file diff --git a/estimate_dyn.m b/estimate_dyn.m index babb132..40e5b58 100644 --- a/estimate_dyn.m +++ b/estimate_dyn.m @@ -11,15 +11,26 @@ 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]); -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; +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; @@ -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]; - 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)); + 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 diff --git a/get_baseParams.m b/get_baseParams.m index 7790557..1875933 100644 --- a/get_baseParams.m +++ b/get_baseParams.m @@ -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