add base params decompose
This commit is contained in:
parent
af96a4b64f
commit
49baf628f1
|
|
@ -4,7 +4,7 @@ opt.KM_method = 'MDH';
|
|||
opt.Vel_method = 'Direct';
|
||||
opt.LD_method = 'Direct';
|
||||
opt.debug = true;
|
||||
|
||||
opt.robotName = 'Two_bar';
|
||||
|
||||
opt.Isreal = false;
|
||||
robot = get_robot(file,opt);
|
||||
|
|
@ -14,3 +14,6 @@ robot = get_Kinematics(robot, opt);
|
|||
opt.Isreal = false;
|
||||
robot = get_velocity(robot, opt);
|
||||
robot = get_regressor(robot,opt);
|
||||
% symbol matched
|
||||
verify_regressor
|
||||
robot = get_baseParams(robot, opt);
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
function out1 = standard_regressor_Two_bar(in1,in2,in3)
|
||||
%standard_regressor_Two_bar
|
||||
% OUT1 = standard_regressor_Two_bar(IN1,IN2,IN3)
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.1.
|
||||
% 10-Jan-2024 23:01:38
|
||||
|
||||
q2 = in1(2,:);
|
||||
qd1 = in2(1,:);
|
||||
qd2 = in2(2,:);
|
||||
qdd1 = in3(1,:);
|
||||
qdd2 = in3(2,:);
|
||||
t2 = cos(q2);
|
||||
t3 = sin(q2);
|
||||
t4 = qd1+qd2;
|
||||
t5 = qdd1+qdd2;
|
||||
t6 = qd1.^2;
|
||||
t7 = qdd1.*t2;
|
||||
t8 = qdd1.*t3;
|
||||
t9 = t4.^2;
|
||||
t10 = t2.*t6;
|
||||
t11 = t3.*t6;
|
||||
t12 = -t8;
|
||||
t13 = t7+t11;
|
||||
t14 = t10+t12;
|
||||
out1 = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,qdd1,0.0,t2.*t13+t3.*(t8-t10),0.0,t13+t2.*t5-t3.*t9,t13,t14-t3.*t5-t2.*t9,t14,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t5,t5],[2,20]);
|
||||
|
|
@ -2,6 +2,27 @@ function robot = get_Kinematics(robot, opt)
|
|||
if(opt.Isreal)
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
theta = robot.theta;
|
||||
alpha = robot.alpha;
|
||||
a = robot.a;
|
||||
d = robot.d;
|
||||
robot.Fkine = eye(4,4);
|
||||
ndof = length(theta); % special for MDH
|
||||
% init transform matrix
|
||||
robot.R = zeros([3,3,ndof]);
|
||||
robot.t = zeros([3,1,ndof]);
|
||||
robot.T = zeros([4,4,ndof]);
|
||||
for i = 1:ndof
|
||||
robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i))*cos(alpha(i)) sin(theta(i))*sin(alpha(i));...
|
||||
sin(theta(i)) cos(theta(i))*cos(alpha(i)) -cos(theta(i))*sin(alpha(i));...
|
||||
0 sin(alpha(i)) cos(alpha(i))];
|
||||
robot.t(:,:,i) = [a(i)*cos(theta(i));a(i)*sin(theta(i));d(i)];
|
||||
Transform = eye(4,4);
|
||||
Transform(1:3,1:3) = robot.R(:,:,i);
|
||||
Transform(1:3,4) = robot.t(:,:,i);
|
||||
robot.T(:,:,i) = Transform;
|
||||
robot.Fkine = robot.Fkine*robot.T(:,:,i);
|
||||
end
|
||||
case 'MDH'
|
||||
theta = robot.theta;
|
||||
alpha = robot.alpha;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,78 @@
|
|||
function robot = get_baseParams(robot,opt)
|
||||
% Seed the random number generator based on the current time
|
||||
rng('shuffle');
|
||||
ndof = robot.ndof;
|
||||
includeMotorDynamics = false;
|
||||
% ------------------------------------------------------------------------
|
||||
% 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
|
||||
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);
|
||||
|
||||
if includeMotorDynamics
|
||||
% Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
|
||||
else
|
||||
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
||||
Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
|
||||
end
|
||||
W = vertcat(W,Y);
|
||||
end
|
||||
|
||||
% QR decomposition with pivoting: W*E = Q*R
|
||||
% R is upper triangular matrix
|
||||
% Q is unitary matrix
|
||||
% E is permutation matrix
|
||||
[Q, R, E] = qr(W);
|
||||
|
||||
% matrix W has rank qr_rank which is number number of base parameters
|
||||
qr_rank = rank(W);
|
||||
|
||||
% R = [R1 R2;
|
||||
% 0 0]
|
||||
% R1 is bbxbb upper triangular and reguar matrix
|
||||
% R2 is bbx(c-qr_rank) matrix where c is number of standard parameters
|
||||
R1 = R(1:qr_rank,1:qr_rank);
|
||||
R2 = R(1:qr_rank,qr_rank+1:end);
|
||||
beta = R1\R2; % the zero rows of K correspond to independent columns of WP
|
||||
beta(abs(beta)<sqrt(eps)) = 0; % get rid of numerical errors
|
||||
% W2 = W1*beta
|
||||
|
||||
% Make sure that the relation holds
|
||||
W1 = W*E(:,1:qr_rank);
|
||||
W2 = W*E(:,qr_rank+1:end);
|
||||
assert(norm(W2 - W1*beta) < 1e-6,...
|
||||
'Found realationship between W1 and W2 is not correct\n');
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Find base parmaters
|
||||
% -----------------------------------------------------------------------
|
||||
pi_lgr_sym = robot.regressor.pi;
|
||||
pi1 = E(:,1:qr_rank)'*pi_lgr_sym; % independent paramters
|
||||
pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
|
||||
|
||||
% all of the expressions below are equivalent
|
||||
pi_lgr_base = pi1 + beta*pi2;
|
||||
% pi_lgr_base = [eye(qr_rank) beta]*[pi1;pi2];
|
||||
% pi_lgr_base = [eye(qr_rank) beta]*E'*pi_lgr_sym;
|
||||
|
||||
% ---------------------------------------------------------------------
|
||||
% Create structure with the result of QR decompositon a
|
||||
% ---------------------------------------------------------------------
|
||||
baseQR = struct;
|
||||
baseQR.numberOfBaseParameters = qr_rank;
|
||||
baseQR.permutationMatrix = E;
|
||||
baseQR.beta = beta;
|
||||
baseQR.motorDynamicsIncluded = includeMotorDynamics;
|
||||
baseQR.baseParams = pi_lgr_base;
|
||||
robot.baseQR = baseQR;
|
||||
|
|
@ -3,14 +3,26 @@ function robot = get_regressor(robot, opt)
|
|||
ndof = robot.ndof;
|
||||
q_sym = sym('q%d',[ndof+1,1],'real');
|
||||
qd_sym = sym('qd%d',[ndof+1,1],'real');
|
||||
q2d_sym = sym('q2d%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.com = sym('com%d',[ndof,1],'real');
|
||||
% robot.regressor.I = sym('I%d',[ndof,1],'real');
|
||||
% robot.regressor.I_vec = inertiaMatrix2Vector(robot.regressor.I);
|
||||
% robot.regressor.mc = robot.regressor.m.*robot.regressor.com;
|
||||
% robot.regressor.pi = [robot.I_vec(:,i); robot.mc(:,i); robot.m(i)];
|
||||
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.R;
|
||||
P = robot.t;
|
||||
|
|
@ -21,7 +33,7 @@ switch opt.LD_method
|
|||
case 'Direct'
|
||||
switch opt.KM_method
|
||||
case 'MDH'
|
||||
for i = 2:ndof+1
|
||||
for i = 1:ndof
|
||||
p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
|
||||
w_skew(:,:,i) = vec2skewSymMat(w(:,i));
|
||||
dw_skew(:,:,i) = vec2skewSymMat(dw(:,i));
|
||||
|
|
@ -29,7 +41,7 @@ switch opt.LD_method
|
|||
w_l(:,:,i) = vec2linearSymMat(w(:,i));
|
||||
dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
|
||||
% size of matrix A is 6*10, need to -1
|
||||
robot.regressor.A(:,:,i-1) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
|
||||
robot.regressor.A(:,:,i) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
|
||||
zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
|
||||
end
|
||||
% construct matrix U, size: [6*n,10*n]
|
||||
|
|
@ -51,9 +63,7 @@ switch opt.LD_method
|
|||
robot.regressor.U = U_;
|
||||
delta_ = zeros([ndof,6*ndof]);
|
||||
for i = 1:ndof
|
||||
for j = 1:ndof
|
||||
delta_(i,6*j) = 1;
|
||||
end
|
||||
delta_(i,6*i) = 1;
|
||||
end
|
||||
robot.regressor.K = delta_*robot.regressor.U;
|
||||
if(opt.debug)
|
||||
|
|
@ -61,6 +71,8 @@ switch opt.LD_method
|
|||
sprintf('size of K=%dx%d.',size(robot.regressor.K));
|
||||
end
|
||||
end
|
||||
matlabFunction(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
|
||||
'Vars',{q_sym,qd_sym,q2d_sym});
|
||||
% matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
|
||||
% 'Vars',{q_sym,qd_sym,q2d_sym});
|
||||
case 'Lagrange'
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ switch opt.robot_def
|
|||
% Create symbolic generilized coordiates, their first and second deriatives
|
||||
q_sym = sym('q%d',[ndof+1,1],'real');
|
||||
qd_sym = sym('qd%d',[ndof+1,1],'real');
|
||||
q2d_sym = sym('q2d%d',[ndof+1,1],'real');
|
||||
q2d_sym = sym('qdd%d',[ndof+1,1],'real');
|
||||
q_sym(ndof+1) = 0;
|
||||
qd_sym(ndof+1) = 0;
|
||||
q2d_sym(ndof+1) = 0;
|
||||
|
|
|
|||
|
|
@ -5,7 +5,9 @@ switch opt.KM_method
|
|||
switch opt.Vel_method
|
||||
case 'Direct'
|
||||
Z = [0,0,1]';
|
||||
w0 = zeros(3,1); dw0 = zeros(3,1);dv0 = robot.gravity;
|
||||
w0 = zeros(3,1); dw0 = zeros(3,1);
|
||||
% dv0 = robot.gravity;
|
||||
v0 = zeros(3,1);dv0 = zeros(3,1);
|
||||
% w = zeros(3,number);
|
||||
% dw = zeros(3,number);
|
||||
% dv = zeros(3,number);
|
||||
|
|
@ -17,15 +19,18 @@ switch opt.KM_method
|
|||
% 1-n外推公式
|
||||
%第一关节
|
||||
w(:,1) = R(:,:,1)' * w0 + dtheta(1) * Z;
|
||||
v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
|
||||
dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dtheta(1) * Z) + ddtheta(1) * Z;
|
||||
dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
|
||||
%后面n-1关节
|
||||
for i = 1:robot.ndof
|
||||
w(:,i+1) = R(:,:,i+1)' * w(:,i) + dtheta(i+1) * Z ;
|
||||
v(:,i+1) = R(:,:,i+1)' * v(:,i) + cross(w(:,i), P(:,i+1));
|
||||
dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dtheta(i+1) * Z)+ ddtheta(i+1) * Z;
|
||||
dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
|
||||
end
|
||||
robot.vel.w = w;
|
||||
robot.vel.v = v;
|
||||
robot.vel.dw = dw;
|
||||
robot.vel.dv = dv;
|
||||
otherwise
|
||||
|
|
|
|||
|
|
@ -0,0 +1,58 @@
|
|||
% function robot = verify_regressor(robot, opt)
|
||||
% verify: If full regressor dynamics is the same as basic dynamics
|
||||
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');
|
||||
pi1=[2;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
|
||||
pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
|
||||
% pi2=zeros([10,1]);
|
||||
pi=[pi1;pi2];
|
||||
regressor = standard_regressor_Two_bar(q_sym,qd_sym,q2d_sym);
|
||||
tau=regressor*pi;
|
||||
%% Two-bar
|
||||
N=2;
|
||||
thetalist = q_sym(1:N);
|
||||
dthetalist = qd_sym(1:N);
|
||||
ddthetalist = q2d_sym(1:N);
|
||||
|
||||
Gb= [diag([1,1,1]),zeros(3,3);
|
||||
zeros(3,3),diag([1,1,1])];
|
||||
Glist = cat(3, Gb, Gb);
|
||||
% Glist = cat(3, Gb, zeros([6,6]));
|
||||
M01 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
M23 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist = cat(3, M01, M12, M23);
|
||||
Slist=[[0;0;1;0;0;0],...
|
||||
[0;0;1;0;-1;0]];
|
||||
Adgab_mat = sym(zeros(6,6,N+1));
|
||||
Fmat=sym(zeros(N,6));
|
||||
F1=sym(zeros(N,6));
|
||||
V1=sym(zeros(6,N+1));
|
||||
G=sym(zeros(4,4,N));
|
||||
T=sym(zeros(4,4,N));
|
||||
Vlinear=sym(zeros(3,3));
|
||||
Vd1=sym(zeros(6,N+1));
|
||||
Gb= [diag([1,1,1]),zeros(3,3);
|
||||
zeros(3,3),diag([1,1,1])];
|
||||
J=sym(zeros(6,N));
|
||||
exf=[0;0;0;0;0;0];
|
||||
|
||||
[V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
|
||||
= InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
|
||||
[0;0;0], exf, Mlist, Glist, Slist);
|
||||
G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
|
||||
M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
M23 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist = cat(3, M01, M12, M23);
|
||||
T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
|
||||
F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
|
||||
% Use Body Twist cal linear vel, but can't cal the end frame vel
|
||||
[V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
|
||||
[0;0;0], exf, Mlist, Glist, Slist);
|
||||
j=1;
|
||||
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
|
||||
j=2;
|
||||
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
|
||||
Loading…
Reference in New Issue