From 312d2bc8dbfbe60d77cbd5f16bb548b968ed3d57 Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Thu, 7 Nov 2024 21:19:53 +0800 Subject: [PATCH] EVT gc identification form datas --- Identification_main.m | 14 ++- codegen/base_6AxisFT_regressor_R1000_DVT.m | 2 +- codegen/base_regressor_R1000_DVT.m | 4 +- codegen/regressor_R1000_DVT.m | 6 +- codegen/standard_regressor_R1000_DVT.m | 6 +- estimate_dyn.asv | 109 --------------------- estimate_dyn.m | 15 ++- get_baseParams.m | 2 + get_regressor.m | 44 ++++----- untitled3.m | 80 ++++++++++++++- 10 files changed, 133 insertions(+), 149 deletions(-) delete mode 100644 estimate_dyn.asv diff --git a/Identification_main.m b/Identification_main.m index b828a13..8331d11 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -8,14 +8,14 @@ opt.debug = false; opt.robotName = 'R1000_DVT'; opt.reGenerate = false; opt.Isreal = true; -opt.isJointTorqueSensor = false; -opt.isSixAxisFTSensor = true; +opt.isJointTorqueSensor = true; +opt.isSixAxisFTSensor = false; theta = zeros(9,1); dtheta = zeros(9,1); ddtheta = zeros(9,1); -robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt); -robot = get_Kinematics(robot, opt); +robot = get_robot_R1000_EVT(theta,dtheta,ddtheta,file,opt); +robot = get_Kinematics_EVT(robot, opt); % R1000_Dynamics_num; % R1000_Dynamics; @@ -24,4 +24,8 @@ 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 +readDataFile; +robot.posData = posData; +robot.currentData = currentData; +% robot = estimate_dyn(robot,opt); +robot = estimate_dyn_form_data(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 index d25958d..7250cbb 100644 --- a/codegen/base_6AxisFT_regressor_R1000_DVT.m +++ b/codegen/base_6AxisFT_regressor_R1000_DVT.m @@ -11,7 +11,7 @@ opt.isJointTorqueSensor = false; opt.isSixAxisFTSensor = true; file=[]; -robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt); +robot = get_robot_R1000(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt); robot = get_Kinematics(robot, opt); robot = get_velocity(robot, opt); robot = get_regressor(robot, opt); diff --git a/codegen/base_regressor_R1000_DVT.m b/codegen/base_regressor_R1000_DVT.m index 18f4d0b..340b309 100644 --- a/codegen/base_regressor_R1000_DVT.m +++ b/codegen/base_regressor_R1000_DVT.m @@ -11,8 +11,8 @@ opt.isJointTorqueSensor = false; opt.isSixAxisFTSensor = true; file=[]; -robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt); -robot = get_Kinematics(robot, opt); +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 diff --git a/codegen/regressor_R1000_DVT.m b/codegen/regressor_R1000_DVT.m index 097421e..ac6de0a 100644 --- a/codegen/regressor_R1000_DVT.m +++ b/codegen/regressor_R1000_DVT.m @@ -4,15 +4,15 @@ opt.KM_method = 'SCREW'; opt.Vel_method = 'Direct'; opt.LD_method = 'Direct'; opt.debug = false; -opt.robotName = 'R1000_EVT'; +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_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; \ No newline at end of file diff --git a/codegen/standard_regressor_R1000_DVT.m b/codegen/standard_regressor_R1000_DVT.m index 521bd4f..8628eba 100644 --- a/codegen/standard_regressor_R1000_DVT.m +++ b/codegen/standard_regressor_R1000_DVT.m @@ -4,15 +4,15 @@ opt.KM_method = 'SCREW'; opt.Vel_method = 'Direct'; opt.LD_method = 'Direct'; opt.debug = false; -opt.robotName = 'R1000_EVT'; +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_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); standard_regrssor = robot.regressor.K; \ No newline at end of file diff --git a/estimate_dyn.asv b/estimate_dyn.asv deleted file mode 100644 index c38b194..0000000 --- a/estimate_dyn.asv +++ /dev/null @@ -1,109 +0,0 @@ -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 40e5b58..512ac3c 100644 --- a/estimate_dyn.m +++ b/estimate_dyn.m @@ -2,7 +2,7 @@ function robot = estimate_dyn(robot,opt) % ------------------------------------------------------------------- % Get datas % ------------------------------------------------------------------------ -time = 0:0.01:1; +time = 0:0.01:0.5; f=1; q_J = sin(2*pi*f*time); qd_J = (2*pi*f)*cos(2*pi*f*time); @@ -11,16 +11,22 @@ 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]; +toruqeSensorRange = [500;500;230;90;60;60;60;20;20]; % 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)]); + %hack +% tau = zeros([robot.ndof,length(q_J)]); + tau = zeros([1,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; + tau_Full=regressor*robot_pi; + joint_idex = robot.ndof-2; + tau(:,i) = tau_Full((joint_idex-1)+1:(joint_idex)); +% tau(:,i) = tau(:,i) + 5*10^-3*toruqeSensorRange.*rand(size(tau(:,i))); end elseif opt.isSixAxisFTSensor tau = zeros([6,length(q_J)]); @@ -75,6 +81,9 @@ robot.sol = sol; 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 = robot.ndof-2; + Yb = Yb((joint_idex-1)+1:(joint_idex),:); Wb = vertcat(Wb, Yb); % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); diff --git a/get_baseParams.m b/get_baseParams.m index 1875933..c55f90a 100644 --- a/get_baseParams.m +++ b/get_baseParams.m @@ -25,6 +25,8 @@ for i = 1:25 elseif isJointTorqueSensor 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),:); elseif isSixAxisFTSensor regressor_func = sprintf('regressor_%s',opt.robotName); Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd); diff --git a/get_regressor.m b/get_regressor.m index c926037..5b197f8 100644 --- a/get_regressor.m +++ b/get_regressor.m @@ -1,28 +1,28 @@ function robot = get_regressor(robot, opt) % Create symbolic generilized coordiates, their first and second deriatives 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'); -% init regressor -robot.regressor.m = sym('m%d',[ndof,1],'real'); -robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real'); -robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real'); -robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real'); -robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real'); -robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real'); -robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real'); -robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real'); -robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real'); -robot.regressor.izz = sym('i%d_zz',[ndof,1],'real'); -robot.regressor.im = sym('im%d',[ndof,1],'real'); -for i = 1:ndof - robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),... - robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),... - robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]'; -end -[nLnkPrms, nLnks] = size(robot.regressor.pi); -robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]); +% 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'); +% % init regressor +% robot.regressor.m = sym('m%d',[ndof,1],'real'); +% robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real'); +% robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real'); +% robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real'); +% robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real'); +% robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real'); +% robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real'); +% robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real'); +% robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real'); +% robot.regressor.izz = sym('i%d_zz',[ndof,1],'real'); +% robot.regressor.im = sym('im%d',[ndof,1],'real'); +% for i = 1:ndof +% robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),... +% robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),... +% robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]'; +% end +% [nLnkPrms, nLnks] = size(robot.regressor.pi); +% robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]); % init matrix R = robot.kine.R; P = robot.kine.t; diff --git a/untitled3.m b/untitled3.m index 93e0e9f..c36fe06 100644 --- a/untitled3.m +++ b/untitled3.m @@ -55,6 +55,52 @@ for i = 1:1:length(q_J) Wb = vertcat(Wb, Yb); end %% +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]; +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]); +Wb = []; Tau = [];Tau_estimate = []; +for i = 1:length(q_J) + base_regressor_func = sprintf('base_regressor_%s',opt.robotName); + Yb = feval(base_regressor_func, q(:,i),qd(:,i),qdd(:,i),robot.baseQR); + tau_estimate=Yb*robot.sol.pi_b; + tau = Yb*robot.baseQR.baseParams; + Tau_estimate = horzcat(Tau_estimate, tau_estimate); + Tau = horzcat(Tau, tau); +end +%% get symbol result for GC +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'); +% init regressor +robot.regressor.m = sym('m%d',[ndof,1],'real'); +robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real'); +robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real'); +robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real'); +robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real'); +robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real'); +robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real'); +robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real'); +robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real'); +robot.regressor.izz = sym('i%d_zz',[ndof,1],'real'); +robot.regressor.im = sym('im%d',[ndof,1],'real'); +for i = 1:ndof + robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),... + robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),... + robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]'; +end +[nLnkPrms, nLnks] = size(robot.regressor.pi); +robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]); qr_rank = robot.baseQR.numberOfBaseParameters; E = robot.baseQR.permutationMatrix; pi_lgr_sym = robot.regressor.pi; @@ -63,4 +109,36 @@ pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres beta = robot.baseQR.beta; % all of the expressions below are equivalent pi_lgr_base = pi1 + beta*pi2; -vpa(simplify(pi_lgr_base),2) \ No newline at end of file +vpa(simplify(pi_lgr_base),2) +%% robot current and torque map +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]; +% readDataFile; +% get pos +[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,:); +index_n = find(posData(:,nCol-1)==-7); +jointPos_n = posData(index_n,1:9); +jointPos_n = jointPos_n(1:2000:end,:); +q = (jointPos_p)'; +% dq = [zeros(robot.ndof,1),diff(q,1,2)]; +% get torque from simulator +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.baseQR.baseParams; + Tau = horzcat(Tau, tau); +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,:); +index_n = find(currentData(:,nCol-1)==-7); +jointCur_n = currentData(index_n,1:9); +jointCur_n = jointCur_n(1:2000:end,:); +current = (jointCur_p+jointCur_n)/2; +torque_cur = gearRatio.*motorConst.*current; +