feature/R1000-identification #2
|
|
@ -0,0 +1,111 @@
|
||||||
|
function robot = estimate_dyn_form_data(robot,opt)
|
||||||
|
% -------------------------------------------------------------------
|
||||||
|
% Get datas
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
posData = robot.posData;
|
||||||
|
currentData = robot.currentData;
|
||||||
|
[nRow,nCol] = size(posData);
|
||||||
|
index_p = find(posData(:,nCol-1)==7);
|
||||||
|
jointPos_p = posData(index_p,1:9);
|
||||||
|
jointPos_p = jointPos_p(1:100:end,:);
|
||||||
|
index_n = find(posData(:,nCol-1)==-7);
|
||||||
|
jointPos_n = posData(index_n,1:9);
|
||||||
|
jointPos_n = jointPos_n(1:100:end,:);
|
||||||
|
q = [jointPos_p]';
|
||||||
|
qd = q; qdd = q;
|
||||||
|
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];
|
||||||
|
% pi -> [m;mc;I] 10 element
|
||||||
|
index_p = find(currentData(:,nCol-1)==7);
|
||||||
|
jointCur_p = currentData(index_p,1:9);
|
||||||
|
jointCur_p = jointCur_p(1:100:end,:);
|
||||||
|
index_n = find(currentData(:,nCol-1)==-7);
|
||||||
|
jointCur_n = currentData(index_n,1:9);
|
||||||
|
jointCur_n = jointCur_n(1:100:end,:);
|
||||||
|
current = (jointCur_p+jointCur_n)/2;
|
||||||
|
torque_cur = gearRatio.*motorConst.*current;
|
||||||
|
tau = torque_cur(:,7)';
|
||||||
|
|
||||||
|
idntfcnTrjctry.t = 1:length(q);
|
||||||
|
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 = 'PC-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] = physicallyConsistentEstimation(Tau, Wb);
|
||||||
|
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);
|
||||||
|
%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));
|
||||||
|
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
|
||||||
|
|
||||||
|
|
||||||
|
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
|
||||||
|
function pib = physicallyConsistentEstimation(Tau, Wb)
|
||||||
|
% 求解线性规划问题
|
||||||
|
options = optimoptions('lsqlin','Algorithm','trust-region-reflective','Display','iter');
|
||||||
|
pib = lsqlin(Wb,Tau,[],[],[],[],-2*ones(5,1),2*ones(5,1),[0.9151,0.0092,0.0002,-0.0644,0.1067]',options)
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
@ -0,0 +1,73 @@
|
||||||
|
function robot = get_Kinematics(robot, opt)
|
||||||
|
if(opt.Isreal)
|
||||||
|
switch opt.KM_method
|
||||||
|
case 'SCREW'
|
||||||
|
% Get transformation for each joint
|
||||||
|
thetalist = robot.theta;
|
||||||
|
Slist = robot.slist;
|
||||||
|
% TODO:move to lib
|
||||||
|
for j = 1:length(thetalist)
|
||||||
|
T=robot.Home.M(:,:,j);
|
||||||
|
for i = j: -1: 1
|
||||||
|
T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T;
|
||||||
|
end
|
||||||
|
robot.kine.TW(:,:,j) = T;
|
||||||
|
end
|
||||||
|
for i = 1:length(thetalist)
|
||||||
|
if i == 1
|
||||||
|
robot.kine.T(:,:,i) = robot.kine.TW(:,:,i);
|
||||||
|
else
|
||||||
|
robot.kine.T(:,:,i) = TransInv(robot.kine.TW(:,:,i-1))*robot.kine.TW(:,:,i);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
%stack result into kine structure
|
||||||
|
robot.kine.R = robot.kine.T(1:3,1:3,:);
|
||||||
|
robot.kine.t = robot.kine.T(1:3,4,:);
|
||||||
|
robot.kine.Fkine = robot.kine.TW(:,:,end);
|
||||||
|
% get the CG at the world base frame
|
||||||
|
% FIXME: Fix this hack
|
||||||
|
ct=zeros(3,robot.ndof);
|
||||||
|
Mlist_CG_Base=zeros(4,4,10);
|
||||||
|
com_pos_R1 = robot.com_pos_R1;
|
||||||
|
com_pos_R2 = robot.com_pos_R2;
|
||||||
|
for i = 1:length(thetalist)
|
||||||
|
if i == 1
|
||||||
|
ct(:,i) = com_pos_R1(:,i);
|
||||||
|
% elseif i< 9
|
||||||
|
else
|
||||||
|
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||||
|
% else
|
||||||
|
% % HACK
|
||||||
|
% ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
||||||
|
end
|
||||||
|
robot.Home.com(:,i) = ct(:,i);
|
||||||
|
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
||||||
|
Mlist_CG_Base(:,:,i) = M_CG_Base;
|
||||||
|
end
|
||||||
|
robot.kine.Mlist_CG_Base = Mlist_CG_Base;
|
||||||
|
% get the CG at the last GC frame
|
||||||
|
Mlist_CG = zeros(4,4,9);
|
||||||
|
for i = 1:length(thetalist)
|
||||||
|
if i == 1
|
||||||
|
Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
|
||||||
|
else
|
||||||
|
Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||||
|
Mlist_CG = cat(3, Mlist_CG, M);
|
||||||
|
robot.kine.Mlist_CG = Mlist_CG;
|
||||||
|
|
||||||
|
% Get the end efforce transformation in each joint reference frame
|
||||||
|
Mlist_ED = zeros(4,4,9);
|
||||||
|
for i = 1:length(thetalist)
|
||||||
|
Mlist_ED(:,:,i) = robot.kine.T(:,:,i);
|
||||||
|
end
|
||||||
|
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||||
|
Mlist_ED = cat(3, Mlist_ED, M);
|
||||||
|
robot.kine.Mlist_ED = Mlist_ED;
|
||||||
|
otherwise
|
||||||
|
disp('Bad opt.KM_method!')
|
||||||
|
return;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
@ -0,0 +1,178 @@
|
||||||
|
function robot = get_robot_R1000_EVT(theta,dtheta,ddtheta,file,opt)
|
||||||
|
switch opt.robot_def
|
||||||
|
case 'direct'
|
||||||
|
ndof = 9;
|
||||||
|
robot = struct;
|
||||||
|
robot.ndof = ndof;
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Structure define
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
robot.m = zeros(1,9);
|
||||||
|
robot.I = zeros(3,3,9);
|
||||||
|
robot.com = zeros(3,9);
|
||||||
|
robot.axis = zeros(3,9);
|
||||||
|
robot.com_pos_R1 = zeros(3,9);
|
||||||
|
robot.com_pos_R2 = zeros(3,9);
|
||||||
|
robot.slist = zeros(6,9);
|
||||||
|
robot.theta = zeros(9,1);
|
||||||
|
robot.dtheta = zeros([9,1]);
|
||||||
|
robot.ddtheta = zeros([9,1]);
|
||||||
|
robot.Home.com = zeros(3,9);
|
||||||
|
robot.Home.R = zeros(3,3,9);
|
||||||
|
robot.Home.P = zeros(3,9);
|
||||||
|
robot.Home.M = zeros(4,4,9);
|
||||||
|
robot.kine.TW = zeros(4,4,9);
|
||||||
|
robot.kine.Fkine = zeros(4,4);
|
||||||
|
robot.kine.T = zeros(4,4,9);
|
||||||
|
robot.kine.R = zeros(3,3,9);
|
||||||
|
robot.kine.t = zeros(3,1,9);
|
||||||
|
robot.kine.Mlist_CG_Base = zeros(4,4,10);
|
||||||
|
robot.kine.Mlist_CG = zeros(4,4,10);
|
||||||
|
robot.kine.Mlist_ED = zeros(4,4,10);
|
||||||
|
robot.gravity = zeros(3,1);
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Dynamics parameters
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
link_mass = [2.1315260e+01,1.9235354e+01,1.0463871e+01,6.1538806,3.0882893,2.4450941,3.6376589,1.7005420,9.1507819e-1];
|
||||||
|
% DVT
|
||||||
|
% link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
|
||||||
|
axis_of_rot = zeros(3,robot.ndof);
|
||||||
|
axis_of_rot(:,1) = [0;0;1];
|
||||||
|
axis_of_rot(:,2) = [0;-1;0];
|
||||||
|
axis_of_rot(:,3) = [0;-1;0];
|
||||||
|
axis_of_rot(:,4) = [0;-1;0];
|
||||||
|
axis_of_rot(:,5) = [0;0;1];
|
||||||
|
axis_of_rot(:,6) = [1;0;0];
|
||||||
|
axis_of_rot(:,7) = [0;0;-1];
|
||||||
|
axis_of_rot(:,8) = [0;-1;0];
|
||||||
|
axis_of_rot(:,9) = [0;0;0];
|
||||||
|
|
||||||
|
com_pos_R1 = zeros(3,robot.ndof);
|
||||||
|
com_pos_R2= zeros(3,robot.ndof);
|
||||||
|
|
||||||
|
com_pos_R1(:,1)=[ 1.0440117e+02 -1.1125559e+01 1.2822933e+02]'*1e-3;
|
||||||
|
com_pos_R2(:,1)=[-8.0598830e+01 -4.2225559e+01 -1.5870672e+01]'*1e-3;
|
||||||
|
com_pos_R1(:,2)=[ 3.5562148e+02 1.2378198e+01 1.8395830e-03]'*1e-3;
|
||||||
|
com_pos_R2(:,2)=[-2.4437852e+02 9.4778198e+01 1.8395830e-03]'*1e-3;
|
||||||
|
com_pos_R1(:,3)=[2.4798242e+02 -3.5267916e+01 4.8648267e-02]'*1e-3;
|
||||||
|
com_pos_R2(:,3)=[-3.5201758e+02 -2.9007916e+01 4.8648266e-02]'*1e-3;
|
||||||
|
com_pos_R1(:,4)=[ 4.2341069e+01 5.2603527e+01 1.2271365e+01]'*1e-3;
|
||||||
|
com_pos_R2(:,4)=[-7.7658931e+01 -1.2956223e+01 1.9271365e+01]'*1e-3;
|
||||||
|
com_pos_R1(:,5)=[1.0177553e+02 -3.4868152e-02 1.5686100e+00]'*1e-3;
|
||||||
|
com_pos_R2(:,5)=[-1.0372447e+02 -3.9239008e-02 -5.4313602e+00]'*1e-3;
|
||||||
|
com_pos_R1(:,6)=[5.9942347e+01 -4.8346177e-02 5.5784018e+00]'*1e-3;
|
||||||
|
com_pos_R2(:,6)=[-2.3757653e+01 -4.8346177e-02 4.0028402e+01]'*1e-3;
|
||||||
|
com_pos_R1(:,7)=[4.9494392e-02 -2.4240394e+00 -1.0332489e+02]'*1e-3;
|
||||||
|
com_pos_R2(:,7)=[4.9494392e-02 -2.4240394e+00 2.7222511e+02]'*1e-3;
|
||||||
|
com_pos_R1(:,8)=[-2.7158498e+01 -3.3865875e-01 -4.6588829e+01]'*1e-3;
|
||||||
|
com_pos_R2(:,8)=[ 2.2423433e+02 -3.3865729e-01 1.9671171e+01]'*1e-3; % don't use
|
||||||
|
com_pos_R1(:,9)=[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3;
|
||||||
|
% stack result
|
||||||
|
robot.com_pos_R1 = com_pos_R1;
|
||||||
|
robot.com_pos_R2 = com_pos_R2;
|
||||||
|
% FIXME:fix this hack
|
||||||
|
% Get 3D coordinate of CO
|
||||||
|
co=zeros(3,robot.ndof-1);
|
||||||
|
for i = 1:robot.ndof-1
|
||||||
|
if i == 1
|
||||||
|
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
|
||||||
|
else
|
||||||
|
%From base to ISA Origin
|
||||||
|
co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
|
||||||
|
% else
|
||||||
|
% %From base to ISA Origin
|
||||||
|
% co(:,i) = co(:,i-1)-[0;0;0.05896];
|
||||||
|
end
|
||||||
|
end
|
||||||
|
co = [zeros(3,1),co];
|
||||||
|
% the inertia tensor wrt the frame oriented as the body frame and with the
|
||||||
|
% origin in the COM
|
||||||
|
link_inertia = zeros(3,3,robot.ndof);
|
||||||
|
link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];...
|
||||||
|
[-1.1694021e+04 2.3289532e+05 -3.0395414e+02];...
|
||||||
|
[-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6;
|
||||||
|
link_inertia(:,:,2) = [[3.7926328e+04 -9.0569033e+01 4.7526575e+04];...
|
||||||
|
[-9.0569033e+01 2.9714754e+05 6.8396715e+00];...
|
||||||
|
[4.7526575e+04 6.8396715e+00 2.8138392e+05]]*10^-6;
|
||||||
|
link_inertia(:,:,3) = [[4.4513887e+03 1.9981964e-01 -3.0303891e+02];...
|
||||||
|
[1.9981964e-01 6.7952039e+04 -8.8585864e-02];...
|
||||||
|
[-3.0303891e+02 -8.8585864e-02 6.9958344e+04]]*10^-6;
|
||||||
|
link_inertia(:,:,4) = [[1.1642351e+04 2.2997175e+03 2.9159431e+03];...
|
||||||
|
[2.2997175e+03 2.6031269e+04 -1.3518384e+02];...
|
||||||
|
[2.9159431e+03 -1.3518384e+02 2.4694742e+04]]*10^-6;
|
||||||
|
link_inertia(:,:,5) = [[3.0930544e+03 8.3558814e-01 -2.8169092e+03];...
|
||||||
|
[8.3558814e-01 1.2796446e+04 -3.3666469e+00];...
|
||||||
|
[-2.8169092e+03 -3.3666469e+00 1.2128856e+04]]*10^-6;
|
||||||
|
link_inertia(:,:,6) = [[3.6635776e+03 -7.0081461e+00 2.2392870e+00];...
|
||||||
|
[-7.0081461e+00 1.8152305e+03 -2.4828765e+02];...
|
||||||
|
[2.2392870e+00 -2.4828765e+02 3.4602935e+03]]*10^-6;
|
||||||
|
link_inertia(:,:,7) = [[1.3662652e+04 -3.6340953e+00 4.4011670e-01];...
|
||||||
|
[-3.6340953e+00 1.3222824e+04 -4.3625500e+02];...
|
||||||
|
[ 4.4011670e-01 -4.3625500e+02 2.2500397e+03]]*10^-6;
|
||||||
|
link_inertia(:,:,8) = [[1.5162872e+03 -4.6245133e+01 4.4556929e+03];...
|
||||||
|
[-4.6245133e+01 5.9901606e+04 6.0350548e+0];...
|
||||||
|
[4.4556929e+03 6.0350548e+0 5.8819998e+04]]*10^-6;
|
||||||
|
link_inertia(:,:,9) = [[1.1730106e+03 3.3834506e+0 4.6097678e+01];...
|
||||||
|
[3.3834506e+0 1.7996852e+03 5.2088778e+0];...
|
||||||
|
[4.6097678e+01 5.2088778e+0 1.3960734e+03]]*10^-6;
|
||||||
|
% manipulator regressor
|
||||||
|
com_pos = com_pos_R1;
|
||||||
|
for i = 1:ndof
|
||||||
|
robot.m(i) = link_mass(i);
|
||||||
|
robot.axis(:,i) = axis_of_rot(i);
|
||||||
|
robot.com(:,i) = com_pos(:,i);
|
||||||
|
robot.I(:,:,i) = link_inertia(:,:,i);
|
||||||
|
robot.mc(:,i) = link_mass(i)*com_pos(:,i);
|
||||||
|
% the inertia tensor wrt the frame oriented as the body frame and with the
|
||||||
|
% origin in the Joint i
|
||||||
|
com_vec2mat = vec2skewSymMat(com_pos(:,i));
|
||||||
|
robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia(:,:,i)-...
|
||||||
|
link_mass(i)*com_vec2mat*com_vec2mat);
|
||||||
|
robot.pi(:,i) = [robot.m(i);robot.mc(:,i);robot.I_vec(:,i)];
|
||||||
|
end
|
||||||
|
robot.motorConst = [0.405, 0.405, 0.167, 0.126,0.073,0.151,0.083,0.073,0.03185];
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
% Kinematics parameters
|
||||||
|
% ------------------------------------------------------------------------
|
||||||
|
if(opt.Isreal)
|
||||||
|
robot.theta = theta;
|
||||||
|
robot.dtheta = dtheta;
|
||||||
|
robot.ddtheta = ddtheta;
|
||||||
|
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||||
|
switch opt.KM_method
|
||||||
|
case 'SCREW'
|
||||||
|
robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]];
|
||||||
|
robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||||
|
robot.Home.R(:,:,3) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||||
|
robot.Home.R(:,:,4) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||||
|
robot.Home.R(:,:,5) = [[0;-1;0],[1;0;0],[0;0;1]];
|
||||||
|
robot.Home.R(:,:,6) = [[0;-1;0],[0;0;-1],[1;0;0]];
|
||||||
|
robot.Home.R(:,:,7) = [[1;0;0],[0;-1;0],[0;0;-1]];
|
||||||
|
robot.Home.R(:,:,8) = [[0;0;-1],[1;0;0],[0;-1;0]];
|
||||||
|
robot.Home.R(:,:,9) = [[0;0;-1],[0;1;0],[1;0;0]];
|
||||||
|
for i=1:9
|
||||||
|
robot.Home.P(:,i) = co(:,i);
|
||||||
|
robot.Home.M(:,:,i) = RpToTrans(robot.Home.R(:,:,i),robot.Home.P(:,i));
|
||||||
|
end
|
||||||
|
robot.slist=[[0;0;1;co(:,1)],...
|
||||||
|
[0;-1;0;cross(-[0;-1;0],co(:,2))]...
|
||||||
|
[0;-1;0;cross(-[0;-1;0],co(:,3))]...
|
||||||
|
[0;-1;0;cross(-[0;-1;0],co(:,4))]...
|
||||||
|
[0;0;1;cross(-[0;0;1],co(:,5))]...
|
||||||
|
[1;0;0;cross(-[1;0;0],co(:,6))]...
|
||||||
|
[0;0;-1;cross(-[0;0;-1],co(:,7))]...
|
||||||
|
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
|
||||||
|
[0;0;0;1;0;0]];
|
||||||
|
otherwise
|
||||||
|
disp('Bad opt.KM_method!')
|
||||||
|
return;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
otherwise
|
||||||
|
robot = [];
|
||||||
|
disp('Bad robot define options!')
|
||||||
|
return
|
||||||
|
end
|
||||||
|
%Gravity
|
||||||
|
gravity = [0;0;-9.806];
|
||||||
|
robot.gravity = gravity;
|
||||||
Binary file not shown.
|
|
@ -0,0 +1,6 @@
|
||||||
|
% function = readDataFile()
|
||||||
|
folder = 'C:\GitLab\Robotics\Identification\IRDYN\simpack_data\data\';
|
||||||
|
posInfoFile = 'GC_cali.csv';
|
||||||
|
currentInfoFile = 'GC_cur.csv';
|
||||||
|
posData = readmatrix([folder posInfoFile]);
|
||||||
|
currentData = readmatrix([folder currentInfoFile]);
|
||||||
Loading…
Reference in New Issue