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

EVT gc identification form datas

This commit is contained in:
cosmic_power 2024-11-07 21:19:53 +08:00
parent ecce2eaa10
commit 312d2bc8db
10 changed files with 133 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Identification_main.m
View File

@ -8,14 +8,14 @@ opt.debug = false;
opt.robotName = 'R1000_DVT'; opt.robotName = 'R1000_DVT';
opt.reGenerate = false; opt.reGenerate = false;
opt.Isreal = true; opt.Isreal = true;
opt.isJointTorqueSensor = false; opt.isJointTorqueSensor = true;
opt.isSixAxisFTSensor = true; opt.isSixAxisFTSensor = false;
theta = zeros(9,1); theta = zeros(9,1);
dtheta = zeros(9,1); dtheta = zeros(9,1);
ddtheta = zeros(9,1); ddtheta = zeros(9,1);
robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt); robot = get_robot_R1000_EVT(theta,dtheta,ddtheta,file,opt);
robot = get_Kinematics(robot, opt); robot = get_Kinematics_EVT(robot, opt);
% R1000_Dynamics_num; % R1000_Dynamics_num;
% R1000_Dynamics; % R1000_Dynamics;
@ -24,4 +24,8 @@ robot = get_regressor(robot,opt);
% symbol matched % symbol matched
% verify_regressor_R1000; % verify_regressor_R1000;
robot = get_baseParams(robot, opt); robot = get_baseParams(robot, opt);
robot = estimate_dyn(robot,opt); readDataFile;
robot.posData = posData;
robot.currentData = currentData;
% robot = estimate_dyn(robot,opt);
robot = estimate_dyn_form_data(robot,opt);

2
codegen/base_6AxisFT_regressor_R1000_DVT.m
View File

@ -11,7 +11,7 @@ opt.isJointTorqueSensor = false;
opt.isSixAxisFTSensor = true; opt.isSixAxisFTSensor = true;
file=[]; 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_Kinematics(robot, opt);
robot = get_velocity(robot, opt); robot = get_velocity(robot, opt);
robot = get_regressor(robot, opt); robot = get_regressor(robot, opt);

4
codegen/base_regressor_R1000_DVT.m
View File

@ -11,8 +11,8 @@ opt.isJointTorqueSensor = false;
opt.isSixAxisFTSensor = true; opt.isSixAxisFTSensor = true;
file=[]; file=[];
robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt); robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
robot = get_Kinematics(robot, opt); robot = get_Kinematics_EVT(robot, opt);
robot = get_velocity(robot, opt); robot = get_velocity(robot, opt);
robot = get_regressor(robot, opt); robot = get_regressor(robot, opt);
% get base params % get base params

6
codegen/regressor_R1000_DVT.m
View File

@ -4,15 +4,15 @@ opt.KM_method = 'SCREW';
opt.Vel_method = 'Direct'; opt.Vel_method = 'Direct';
opt.LD_method = 'Direct'; opt.LD_method = 'Direct';
opt.debug = false; opt.debug = false;
opt.robotName = 'R1000_EVT'; opt.robotName = 'R1000_DVT';
opt.reGenerate = false; opt.reGenerate = false;
opt.Isreal = true; opt.Isreal = true;
opt.isJointTorqueSensor = false; opt.isJointTorqueSensor = false;
opt.isSixAxisFTSensor = true; opt.isSixAxisFTSensor = true;
file=[]; file=[];
robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt); robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
robot = get_Kinematics(robot, opt); robot = get_Kinematics_EVT(robot, opt);
robot = get_velocity(robot, opt); robot = get_velocity(robot, opt);
robot = get_regressor(robot, opt); robot = get_regressor(robot, opt);
regrssor = robot.regressor.U; regrssor = robot.regressor.U;

6
codegen/standard_regressor_R1000_DVT.m
View File

@ -4,15 +4,15 @@ opt.KM_method = 'SCREW';
opt.Vel_method = 'Direct'; opt.Vel_method = 'Direct';
opt.LD_method = 'Direct'; opt.LD_method = 'Direct';
opt.debug = false; opt.debug = false;
opt.robotName = 'R1000_EVT'; opt.robotName = 'R1000_DVT';
opt.reGenerate = false; opt.reGenerate = false;
opt.Isreal = true; opt.Isreal = true;
opt.isJointTorqueSensor = false; opt.isJointTorqueSensor = false;
opt.isSixAxisFTSensor = true; opt.isSixAxisFTSensor = true;
file=[]; file=[];
robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt); robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
robot = get_Kinematics(robot, opt); robot = get_Kinematics_EVT(robot, opt);
robot = get_velocity(robot, opt); robot = get_velocity(robot, opt);
robot = get_regressor(robot, opt); robot = get_regressor(robot, opt);
standard_regrssor = robot.regressor.K; standard_regrssor = robot.regressor.K;

109
estimate_dyn.asv
View File

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

15
estimate_dyn.m
View File

@ -2,7 +2,7 @@ function robot = estimate_dyn(robot,opt)
% ------------------------------------------------------------------- % -------------------------------------------------------------------
% Get datas % Get datas
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
time = 0:0.01:1; time = 0:0.01:0.5;
f=1; f=1;
q_J = sin(2*pi*f*time); q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(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]; 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]; qdd=[qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J;qdd_J];
g = [0; 0; -9.8]; g = [0; 0; -9.8];
toruqeSensorRange = [500;500;230;90;60;60;60;20;20];
% pi -> [m;mc;I] 10 element % pi -> [m;mc;I] 10 element
[nLnkPrms, nLnks] = size(robot.pi); [nLnkPrms, nLnks] = size(robot.pi);
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]); robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
if opt.isJointTorqueSensor 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) for i = 1:length(q_J)
% regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i)); % regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i)); 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 end
elseif opt.isSixAxisFTSensor elseif opt.isSixAxisFTSensor
tau = zeros([6,length(q_J)]); tau = zeros([6,length(q_J)]);
@ -75,6 +81,9 @@ robot.sol = sol;
base_regressor_func = sprintf('base_regressor_%s',opt.robotName); base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ... Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR); 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); Wb = vertcat(Wb, Yb);
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)'); % Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i)); Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));

2
get_baseParams.m
View File

@ -25,6 +25,8 @@ for i = 1:25
elseif isJointTorqueSensor elseif isJointTorqueSensor
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName); standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd); 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 elseif isSixAxisFTSensor
regressor_func = sprintf('regressor_%s',opt.robotName); regressor_func = sprintf('regressor_%s',opt.robotName);
Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd); Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd);

44
get_regressor.m
View File

@ -1,28 +1,28 @@
function robot = get_regressor(robot, opt) function robot = get_regressor(robot, opt)
% Create symbolic generilized coordiates, their first and second deriatives % Create symbolic generilized coordiates, their first and second deriatives
ndof = robot.ndof; ndof = robot.ndof;
q_sym = sym('q%d',[ndof+1,1],'real'); % q_sym = sym('q%d',[ndof+1,1],'real');
qd_sym = sym('qd%d',[ndof+1,1],'real'); % qd_sym = sym('qd%d',[ndof+1,1],'real');
q2d_sym = sym('qdd%d',[ndof+1,1],'real'); % q2d_sym = sym('qdd%d',[ndof+1,1],'real');
% init regressor % % init regressor
robot.regressor.m = sym('m%d',[ndof,1],'real'); % robot.regressor.m = sym('m%d',[ndof,1],'real');
robot.regressor.mc_x = sym('mc%d_x',[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_y = sym('mc%d_y',[ndof,1],'real');
robot.regressor.mc_z = sym('mc%d_z',[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.ixx = sym('i%d_xx',[ndof,1],'real');
robot.regressor.ixy = sym('i%d_xy',[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.ixz = sym('i%d_xz',[ndof,1],'real');
robot.regressor.iyy = sym('i%d_yy',[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.iyz = sym('i%d_yz',[ndof,1],'real');
robot.regressor.izz = sym('i%d_zz',[ndof,1],'real'); % robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
robot.regressor.im = sym('im%d',[ndof,1],'real'); % robot.regressor.im = sym('im%d',[ndof,1],'real');
for i = 1:ndof % for i = 1:ndof
robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),... % 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.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)]'; % robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
end % end
[nLnkPrms, nLnks] = size(robot.regressor.pi); % [nLnkPrms, nLnks] = size(robot.regressor.pi);
robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]); % robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
% init matrix % init matrix
R = robot.kine.R; R = robot.kine.R;
P = robot.kine.t; P = robot.kine.t;

80
untitled3.m
View File

@ -55,6 +55,52 @@ for i = 1:1:length(q_J)
Wb = vertcat(Wb, Yb); Wb = vertcat(Wb, Yb);
end 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; qr_rank = robot.baseQR.numberOfBaseParameters;
E = robot.baseQR.permutationMatrix; E = robot.baseQR.permutationMatrix;
pi_lgr_sym = robot.regressor.pi; 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; beta = robot.baseQR.beta;
% all of the expressions below are equivalent % all of the expressions below are equivalent
pi_lgr_base = pi1 + beta*pi2; pi_lgr_base = pi1 + beta*pi2;
vpa(simplify(pi_lgr_base),2) 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;