need to check why Wb is not good
This commit is contained in:
parent
a30a3d51c1
commit
4fc5a23b70
|
|
@ -30,5 +30,6 @@ robot = get_baseParams(robot, opt);
|
||||||
% readDataFile;
|
% readDataFile;
|
||||||
% robot.posData = posData;
|
% robot.posData = posData;
|
||||||
% robot.currentData = currentData;
|
% robot.currentData = currentData;
|
||||||
% % % robot = estimate_dyn(robot,opt);
|
% robot = estimate_dyn(robot,opt);
|
||||||
% robot = estimate_dyn_form_data(robot,opt);
|
% robot = estimate_dyn_form_data(robot,opt);
|
||||||
|
robot = estimate_dyn_MLS(robot,opt);
|
||||||
|
|
@ -1,4 +1,4 @@
|
||||||
function base_regrssor = base_regressor_R1000_DVT(theta,dtheta,ddtheta,baseQR)
|
function base_regrssor = base_regressor_R1000_EVT(theta,dtheta,ddtheta,baseQR)
|
||||||
opt.robot_def = 'direct';
|
opt.robot_def = 'direct';
|
||||||
opt.KM_method = 'SCREW';
|
opt.KM_method = 'SCREW';
|
||||||
opt.Vel_method = 'Direct';
|
opt.Vel_method = 'Direct';
|
||||||
|
|
@ -12,7 +12,7 @@ opt.isSixAxisFTSensor = true;
|
||||||
file=[];
|
file=[];
|
||||||
|
|
||||||
robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
|
robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
|
||||||
robot = get_Kinematics_EVT(robot, opt);
|
robot = get_Kinematics_R1000_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
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,110 @@
|
||||||
|
function robot = estimate_dyn_MLS(robot,opt)
|
||||||
|
% -------------------------------------------------------------------
|
||||||
|
% Get datas
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
get_GCTraj_R1000_EVT;
|
||||||
|
% -------------------------------------------------------------------
|
||||||
|
% Generate Regressors based on data
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
drvGains = [];
|
||||||
|
baseQR = robot.baseQR;
|
||||||
|
|
||||||
|
for i = 1:1:robot.ndof
|
||||||
|
q = idntfcnTrjctry(i).q;
|
||||||
|
qd = idntfcnTrjctry(i).qd;
|
||||||
|
qdd = idntfcnTrjctry(i).qdd;
|
||||||
|
[nRow,nCol] = size(idntfcnTrjctry(i).qd);
|
||||||
|
Wb = []; Tau = [];
|
||||||
|
for j = 1:nRow/robot.ndof
|
||||||
|
for k = 1:nCol
|
||||||
|
if opt.isJointTorqueSensor
|
||||||
|
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
|
||||||
|
Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
|
||||||
|
qd(robot.ndof*(j-1)+1:robot.ndof*j,k),...
|
||||||
|
qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR);
|
||||||
|
Yb = Yb(i,:);
|
||||||
|
Wb = vertcat(Wb, Yb);
|
||||||
|
Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k));
|
||||||
|
end
|
||||||
|
end
|
||||||
|
end
|
||||||
|
observationMatrix(i).Wb = Wb;
|
||||||
|
observationMatrix(i).Tau = Tau;
|
||||||
|
observationMatrix(i).rank = robot.baseQR.rank(i);
|
||||||
|
end
|
||||||
|
|
||||||
|
% ---------------------------------------------------------------------
|
||||||
|
% Estimate parameters
|
||||||
|
% ---------------------------------------------------------------------
|
||||||
|
sol = struct;
|
||||||
|
for i = 9:-1:1
|
||||||
|
Wb = observationMatrix(i).Wb;
|
||||||
|
Tau = observationMatrix(i).Tau;
|
||||||
|
% [nRow,nCol] = size(Wb);
|
||||||
|
if i == 9
|
||||||
|
pib_OLS=pinv(Wb(:,1:observationMatrix(i).rank))*Tau;
|
||||||
|
pib_MLS = [];
|
||||||
|
elseif i > 1
|
||||||
|
pib_OLS=pinv(Wb(:,observationMatrix(i+1).rank+1:observationMatrix(i).rank))*...
|
||||||
|
(-Wb(:,1:observationMatrix(i+1).rank)*pib_MLS+Tau);
|
||||||
|
else
|
||||||
|
break;
|
||||||
|
end
|
||||||
|
pifrctn_OLS = 0;
|
||||||
|
pib_MLS = [pib_MLS;pib_OLS];
|
||||||
|
end
|
||||||
|
a=1
|
||||||
|
% method = 'OLS';
|
||||||
|
% if strcmp(method, 'OLS')
|
||||||
|
% % Usual least squares
|
||||||
|
% [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(observationMatrix);
|
||||||
|
% 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 observationMatrix = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt)
|
||||||
|
% for i = 1:1:robot.ndof
|
||||||
|
% [nRow,nCol] = size(idntfcnTrjctry(i).qd);
|
||||||
|
% Wb = []; Tau = [];
|
||||||
|
% for j = 1:nRow/robot.ndof
|
||||||
|
% for k = 1:nCol
|
||||||
|
% if opt.isJointTorqueSensor
|
||||||
|
% base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
|
||||||
|
% Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
|
||||||
|
% qd(robot.ndof*(j-1)+1:robot.ndof*j,k),...
|
||||||
|
% qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR);
|
||||||
|
% Yb = Yb(i,:);
|
||||||
|
% Wb = vertcat(Wb, Yb);
|
||||||
|
% Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k));
|
||||||
|
% end
|
||||||
|
% end
|
||||||
|
% end
|
||||||
|
% observationMatrix(i).Wb = Wb;
|
||||||
|
% observationMatrix(i).Tau = Tau;
|
||||||
|
% end
|
||||||
|
% end
|
||||||
|
%
|
||||||
|
%
|
||||||
|
% function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(observationMatrix)
|
||||||
|
% % 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);
|
||||||
|
% for i = 9:-1:1
|
||||||
|
% Wb = observationMatrix(i).Wb;
|
||||||
|
% Tau = observationMatrix(i).Tau;
|
||||||
|
% pib_OLS(i)=pinv(Wb)*Tau;
|
||||||
|
% pifrctn_OLS = 0;
|
||||||
|
% end
|
||||||
|
% 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
|
||||||
|
|
@ -0,0 +1,219 @@
|
||||||
|
% function robot = get_GCTraj_R1000_EVT(robot,opt)
|
||||||
|
% J9 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(9,:)=q_J;
|
||||||
|
q(8,:)=pi/3*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(9).t = time;
|
||||||
|
idntfcnTrjctry(9).q = q;
|
||||||
|
idntfcnTrjctry(9).qd = qd;
|
||||||
|
idntfcnTrjctry(9).qdd = qdd;
|
||||||
|
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(9,:)=q_J;
|
||||||
|
q(8,:)=-pi/3*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
|
||||||
|
idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
|
||||||
|
idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
|
||||||
|
idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);
|
||||||
|
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(9,:)=q_J;
|
||||||
|
q(6,:)=pi/2*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
|
||||||
|
idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
|
||||||
|
idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
|
||||||
|
idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);
|
||||||
|
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(9,:)=q_J;
|
||||||
|
q(6,:)=-pi/2*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
|
||||||
|
idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
|
||||||
|
idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
|
||||||
|
idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);
|
||||||
|
|
||||||
|
% J8 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(8,:)=pi/3*q_J;
|
||||||
|
q(6,:)=pi/2*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(8).t = time;
|
||||||
|
idntfcnTrjctry(8).q = q;
|
||||||
|
idntfcnTrjctry(8).qd = qd;
|
||||||
|
idntfcnTrjctry(8).qdd = qdd;
|
||||||
|
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(8,:)=pi/3*q_J;
|
||||||
|
q(6,:)=-pi/2*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(8).t = vertcat(idntfcnTrjctry(8).t,time);
|
||||||
|
idntfcnTrjctry(8).q = vertcat(idntfcnTrjctry(8).q,q);
|
||||||
|
idntfcnTrjctry(8).qd = vertcat(idntfcnTrjctry(8).qd,qd);
|
||||||
|
idntfcnTrjctry(8).qdd = vertcat(idntfcnTrjctry(8).qdd,qdd);
|
||||||
|
|
||||||
|
% J7 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
% q(7,:)=q_J;
|
||||||
|
% q(6,:)=q_J;q(5,:)=q_J;q(8,:)=q_J;
|
||||||
|
% q(4,:)=q_J;q(9,:)=q_J;
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
|
||||||
|
idntfcnTrjctry(7).t = time;
|
||||||
|
idntfcnTrjctry(7).q = q;
|
||||||
|
idntfcnTrjctry(7).qd = qd;
|
||||||
|
idntfcnTrjctry(7).qdd = qdd;
|
||||||
|
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
% q(7,:)=q_J;
|
||||||
|
% q(6,:)=q_J;q(5,:)=q_J;q(8,:)=q_J;
|
||||||
|
% q(4,:)=q_J;q(9,:)=q_J;
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
|
||||||
|
idntfcnTrjctry(7).t = vertcat(idntfcnTrjctry(7).t,time);
|
||||||
|
idntfcnTrjctry(7).q = vertcat(idntfcnTrjctry(7).q,q);
|
||||||
|
idntfcnTrjctry(7).qd = vertcat(idntfcnTrjctry(7).qd,qd);
|
||||||
|
idntfcnTrjctry(7).qdd = vertcat(idntfcnTrjctry(7).qdd,qdd);
|
||||||
|
|
||||||
|
% J6 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(6,:)=pi/2*q_J;
|
||||||
|
q(4,:)=pi/4*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(6).t= time;
|
||||||
|
idntfcnTrjctry(6).q= q;
|
||||||
|
idntfcnTrjctry(6).qd= qd;
|
||||||
|
idntfcnTrjctry(6).qdd= qdd;
|
||||||
|
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(6,:)=pi/2*q_J;
|
||||||
|
q(4,:)=-pi/4*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(6).t= vertcat(idntfcnTrjctry(6).t,time);
|
||||||
|
idntfcnTrjctry(6).q= vertcat(idntfcnTrjctry(6).q,q);
|
||||||
|
idntfcnTrjctry(6).qd= vertcat(idntfcnTrjctry(6).qd,qd);
|
||||||
|
idntfcnTrjctry(6).qdd= vertcat(idntfcnTrjctry(6).qdd,qdd);
|
||||||
|
|
||||||
|
% J5 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(5,:)=pi/2*q_J;
|
||||||
|
q(4,:)=pi/4*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(5).t = time;
|
||||||
|
idntfcnTrjctry(5).q = q;
|
||||||
|
idntfcnTrjctry(5).qd = qd;
|
||||||
|
idntfcnTrjctry(5).qdd = qdd;
|
||||||
|
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(5,:)=pi/2*q_J;
|
||||||
|
q(4,:)=-pi/4*ones(size(q_J));
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(5).t = vertcat(idntfcnTrjctry(5).t,time);
|
||||||
|
idntfcnTrjctry(5).q = vertcat(idntfcnTrjctry(5).q,q);
|
||||||
|
idntfcnTrjctry(5).qd = vertcat(idntfcnTrjctry(5).qd,qd);
|
||||||
|
idntfcnTrjctry(5).qdd = vertcat(idntfcnTrjctry(5).qdd,qdd);
|
||||||
|
|
||||||
|
% J4 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(4,:)=pi/4*q_J;
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(4).t = time;
|
||||||
|
idntfcnTrjctry(4).q = q;
|
||||||
|
idntfcnTrjctry(4).qd = qd;
|
||||||
|
idntfcnTrjctry(4).qdd = qdd;
|
||||||
|
|
||||||
|
% J3 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(3,:)=pi/4*q_J;
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(3).t = time;
|
||||||
|
idntfcnTrjctry(3).q = q;
|
||||||
|
idntfcnTrjctry(3).qd = qd;
|
||||||
|
idntfcnTrjctry(3).qdd = qdd;
|
||||||
|
|
||||||
|
% J2 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(2,:)=pi/4*q_J;
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(2).t = time;
|
||||||
|
idntfcnTrjctry(2).q = q;
|
||||||
|
idntfcnTrjctry(2).qd = qd;
|
||||||
|
idntfcnTrjctry(2).qdd = qdd;
|
||||||
|
|
||||||
|
% J1 traj
|
||||||
|
time = 0:0.1:1;
|
||||||
|
f=1;
|
||||||
|
q_J = sin(2*pi*f*time);
|
||||||
|
q=zeros(robot.ndof,length(q_J));
|
||||||
|
q(1,:)=pi/4*q_J;
|
||||||
|
qd=zeros(robot.ndof,length(q_J));qdd=qd;
|
||||||
|
idntfcnTrjctry(1).t = time;
|
||||||
|
idntfcnTrjctry(1).q = q;
|
||||||
|
idntfcnTrjctry(1).qd = qd;
|
||||||
|
idntfcnTrjctry(1).qdd = qdd;
|
||||||
|
|
||||||
|
% pi -> [m;mc;I] 10 element
|
||||||
|
[nLnkPrms, nLnks] = size(robot.pi);
|
||||||
|
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
|
||||||
|
for i=1:robot.ndof
|
||||||
|
q = idntfcnTrjctry(i).q;
|
||||||
|
qd = idntfcnTrjctry(i).qd;
|
||||||
|
qdd = idntfcnTrjctry(i).qdd;
|
||||||
|
[nRow,nCol] = size(idntfcnTrjctry(i).qd);
|
||||||
|
for j = 1:nRow/robot.ndof
|
||||||
|
for k = 1:nCol
|
||||||
|
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
||||||
|
regressor = feval(standard_regressor_func,q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
|
||||||
|
qd(robot.ndof*(j-1)+1:robot.ndof*j,k),qdd(robot.ndof*(j-1)+1:robot.ndof*j,k));
|
||||||
|
tau_Full=regressor*robot_pi;
|
||||||
|
tau(j,k) = tau_Full(i);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
idntfcnTrjctry(i).tau = tau;
|
||||||
|
end
|
||||||
|
|
@ -1,4 +1,4 @@
|
||||||
function robot = get_Kinematics(robot, opt)
|
function robot = get_Kinematics_R1000_DVT(robot, opt)
|
||||||
if(opt.Isreal)
|
if(opt.Isreal)
|
||||||
switch opt.KM_method
|
switch opt.KM_method
|
||||||
case 'SCREW'
|
case 'SCREW'
|
||||||
|
|
|
||||||
|
|
@ -1,4 +1,4 @@
|
||||||
function robot = get_Kinematics(robot, opt)
|
function robot = get_Kinematics_R1000_EVT(robot, opt)
|
||||||
if(opt.Isreal)
|
if(opt.Isreal)
|
||||||
switch opt.KM_method
|
switch opt.KM_method
|
||||||
case 'SCREW'
|
case 'SCREW'
|
||||||
|
|
|
||||||
|
|
@ -14,7 +14,7 @@ q2d_max = 6*pi*ones(ndof,1);
|
||||||
% Find relation between independent columns and dependent columns
|
% Find relation between independent columns and dependent columns
|
||||||
% -----------------------------------------------------------------------
|
% -----------------------------------------------------------------------
|
||||||
% Get observation matrix of identifiable paramters
|
% Get observation matrix of identifiable paramters
|
||||||
W = []; isSixAxisFTSensor =opt.isSixAxisFTSensor; isJointTorqueSensor =opt.isJointTorqueSensor;
|
W = [];
|
||||||
for i = 1:25
|
for i = 1:25
|
||||||
q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
|
q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
|
||||||
qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
|
qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
|
||||||
|
|
@ -22,12 +22,12 @@ for i = 1:25
|
||||||
|
|
||||||
if includeMotorDynamics
|
if includeMotorDynamics
|
||||||
% Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
|
% Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
|
||||||
elseif isJointTorqueSensor
|
elseif opt.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;
|
% joint_idex = ndof-2;
|
||||||
Y = Y(joint_idex:end,:);
|
% Y = Y(joint_idex:end,:);
|
||||||
elseif isSixAxisFTSensor
|
elseif opt.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);
|
||||||
joint_idex = ndof-2;
|
joint_idex = ndof-2;
|
||||||
|
|
@ -99,6 +99,21 @@ baseQR.baseParams = pi_lgr_base;
|
||||||
robot.baseQR = baseQR;
|
robot.baseQR = baseQR;
|
||||||
robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
|
robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
|
||||||
|
|
||||||
|
% Get observation matrix of identifiable paramters
|
||||||
|
for j = 1:robot.ndof
|
||||||
|
W = [];
|
||||||
|
for i = 1:25
|
||||||
|
q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
|
||||||
|
qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
|
||||||
|
q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1);
|
||||||
|
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
||||||
|
Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
|
||||||
|
W = vertcat(W,Y(j,:));
|
||||||
|
end
|
||||||
|
% matrix W has rank qr_rank which is number number of base parameters
|
||||||
|
robot.baseQR.rank(j) = rank(W);
|
||||||
|
end
|
||||||
|
|
||||||
if(opt.reGenerate)
|
if(opt.reGenerate)
|
||||||
tic
|
tic
|
||||||
disp('compiling robot.baseQR.regressor');
|
disp('compiling robot.baseQR.regressor');
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,24 @@
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Set limits on posistion and velocities
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
q_min = -pi*ones(ndof,1);
|
||||||
|
q_max = pi*ones(ndof,1);
|
||||||
|
qd_max = 3*pi*ones(ndof,1);
|
||||||
|
q2d_max = 6*pi*ones(ndof,1);
|
||||||
|
% -----------------------------------------------------------------------
|
||||||
|
% Find relation between independent columns and dependent columns
|
||||||
|
% -----------------------------------------------------------------------
|
||||||
|
% Get observation matrix of identifiable paramters
|
||||||
|
for j = 1:robot.ndof
|
||||||
|
W = [];
|
||||||
|
for i = 1:25
|
||||||
|
q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
|
||||||
|
qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
|
||||||
|
q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1);
|
||||||
|
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
||||||
|
Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
|
||||||
|
W = vertcat(W,Y(j,:));
|
||||||
|
end
|
||||||
|
% matrix W has rank qr_rank which is number number of base parameters
|
||||||
|
robot.baseQR.rank(i) = rank(W);
|
||||||
|
end
|
||||||
|
|
@ -1,4 +1,4 @@
|
||||||
function robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt)
|
function robot = get_robot_R1000_DVT(theta,dtheta,ddtheta,file,opt)
|
||||||
switch opt.robot_def
|
switch opt.robot_def
|
||||||
case 'direct'
|
case 'direct'
|
||||||
ndof = 9;
|
ndof = 9;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue