128 lines
4.1 KiB
Matlab
128 lines
4.1 KiB
Matlab
function [base,regressor] = khalil_gautier(lRot,lPos,lZ,q_sym)
|
|
% Find base parameters and regressor of a serial manipulator
|
|
% lRot - array of rotations from i-1 to i (3x3xN)
|
|
% lPos - array of translation from i-1 to i (3x1xN)
|
|
% lZ - array of axis orientaions (3x1xN) in form [0;0;1]
|
|
% q_sym - vector of symbolic joints (Nx1)
|
|
% Return:
|
|
% base - vector of base parameters
|
|
% regressor - regressor matrix
|
|
|
|
% number of joints
|
|
n = size(lRot,3);
|
|
|
|
% get parameters
|
|
[PI,qd_sym,qdd_sym] = params(n);
|
|
|
|
% uncomment to apply parallel axis theorem
|
|
%PI = steiner(PI);
|
|
|
|
% find energetical parameters
|
|
f_sym = sym(zeros(10,n)); % f term
|
|
lam_sym = sym(zeros(10,10,n)); % lambdas
|
|
mu_sym = sym(zeros(10,10,n)); % mus
|
|
PI_reg = sym(zeros(10,n)); % regrouped parameters
|
|
|
|
vtmp = sym(zeros(3,1)); % linear velocity
|
|
wtmp = sym(zeros(3,1)); % rotational velocity
|
|
detmp = sym(zeros(10,1)); % current DE value
|
|
dutmp = sym([0;0;0;0;0;0;0;0;9.81;0]); % current DU value
|
|
for i = 1:n
|
|
% current transformation
|
|
R = sym(lRot(:,:,i)); % from i-1 to i, transpose
|
|
p = sym(lPos(:,:,i)); % from i-1 to i
|
|
z = sym(lZ(:,:,i)); % around i axis
|
|
% velocities
|
|
vtmp = R'*(vtmp + cross(wtmp,p));
|
|
wtmp = R'*wtmp + z*qd_sym(i);
|
|
% f
|
|
f_sym(:,i) = getF(vtmp,wtmp,z,qd_sym(i));
|
|
% DE
|
|
lambda = getLambda(R,p);
|
|
detmp = lambda*detmp + qd_sym(i)*f_sym(:,i);
|
|
lam_sym(:,:,i) = lambda;
|
|
% DU
|
|
mu = getMu(R,p);
|
|
dutmp = mu * dutmp;
|
|
mu_sym(:,:,i) = mu;
|
|
|
|
% remove those who no effect
|
|
ind = no_effect(detmp,dutmp,[q_sym;qd_sym]);
|
|
PI_reg(:,i) = PI(:,i);
|
|
PI_reg(ind,i) = 0;
|
|
end
|
|
|
|
% regroup
|
|
for i = n:-1:2
|
|
[pprev,pcur] = group(PI_reg(:,i-1),PI_reg(:,i),lam_sym(:,:,i),lZ(:,:,i));
|
|
PI_reg(:,i-1) = pprev;
|
|
PI_reg(:,i) = pcur;
|
|
end
|
|
|
|
% find base parameters and regressor
|
|
base = sym([]);
|
|
Lagr = sym([]);
|
|
detmp = sym(zeros(10,1)); % current DE value
|
|
dutmp = sym([0;0;0;0;0;0;0;0;9.81;0]); % current DU value
|
|
for i = 1:n
|
|
detmp = lam_sym(:,:,i)*detmp + qd_sym(i)*f_sym(:,i);
|
|
dutmp = mu_sym(:,:,i)*dutmp;
|
|
ind = find(PI_reg(:,i) ~= 0);
|
|
base = [base;PI_reg(ind,i)];
|
|
Lagr = [Lagr;(detmp(ind)-dutmp(ind))];
|
|
end
|
|
|
|
% find regressor
|
|
regressor = sym(0);
|
|
dL_dqd = jacobian(Lagr,qd_sym)';
|
|
dL_qd = jacobian(Lagr,q_sym)';
|
|
|
|
for i = 1:n
|
|
regressor = regressor + diff(dL_dqd,qd_sym(i))*qdd_sym(i) + ...
|
|
diff(dL_dqd,q_sym(i))*qd_sym(i);
|
|
end
|
|
regressor = regressor - dL_qd;
|
|
end
|
|
|
|
function [par,qd,qdd] = params(n)
|
|
% Generate symbolic parameters with size n
|
|
% joints
|
|
qd = sym('qd', [n 1],'real');
|
|
qdd = sym('qdd',[n 1],'real');
|
|
% inertial
|
|
Ixx = sym('Ixx',[1 n],'real');
|
|
Ixy = sym('Ixy',[1 n],'real');
|
|
Ixz = sym('Ixz',[1 n],'real');
|
|
Iyy = sym('Iyy',[1 n],'real');
|
|
Iyz = sym('Iyz',[1 n],'real');
|
|
Izz = sym('Izz',[1 n],'real');
|
|
rX = sym('r_x', [1 n],'real');
|
|
rY = sym('r_y', [1 n],'real');
|
|
rZ = sym('r_z', [1 n],'real');
|
|
m = sym('m', [1 n],'real');
|
|
par = [Ixx; Ixy; Ixz; Iyy; Iyz; Izz; m .* rX; m .* rY; m .* rZ; m];
|
|
end
|
|
|
|
function v = no_effect(de,du,vars)
|
|
v = [];
|
|
for i = 1:10
|
|
if de(i) == 0 && (du(i) == 0 || ~has(du(i),vars))
|
|
v = [v;i];
|
|
end
|
|
end
|
|
end
|
|
|
|
function par = steiner(pi)
|
|
% Apply parallel axis theorem
|
|
par = pi;
|
|
for i = 1:size(pi,2)
|
|
col = pi(:,i);
|
|
prod = vec2skewSymMat(col(7:9))'*vec2skewSymMat(col(7:9)) / col(10);
|
|
par(1,i) = pi(1,i) + prod(1,1);
|
|
par(2,i) = pi(2,i) + prod(1,2);
|
|
par(3,i) = pi(3,i) + prod(1,2);
|
|
par(4,i) = pi(4,1) + prod(2,2);
|
|
par(5,i) = pi(5,i) + prod(2,3);
|
|
par(6,i) = pi(6,i) + prod(3,3);
|
|
end
|
|
end |