96 lines
3.4 KiB
Mathematica
96 lines
3.4 KiB
Mathematica
|
%% R1000
|
||
|
|
N=9;
|
||
|
|
ndof = N;
|
||
|
|
% traj
|
||
|
|
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);
|
||
|
|
zero_ = zeros(1,length(q_J));
|
||
|
|
|
||
|
|
thetalist = [q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||
|
|
dthetalist = [qd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||
|
|
ddthetalist = [qdd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||
|
|
% Dynamics parameters
|
||
|
|
link_mass = robot.m;
|
||
|
|
com_pos = robot.com;
|
||
|
|
link_inertia = robot.I;
|
||
|
|
|
||
|
|
|
||
|
|
% init regressor
|
||
|
|
robot.regressor.m = link_mass;
|
||
|
|
robot.regressor.mc_x = com_pos(1,:);
|
||
|
|
robot.regressor.mc_y = com_pos(2,:);
|
||
|
|
robot.regressor.mc_z = com_pos(3,:);
|
||
|
|
robot.regressor.ixx = link_inertia(1,1,:);
|
||
|
|
robot.regressor.ixy = link_inertia(1,2,:);
|
||
|
|
robot.regressor.ixz = link_inertia(1,3,:);
|
||
|
|
robot.regressor.iyy = link_inertia(2,2,:);
|
||
|
|
robot.regressor.iyz = link_inertia(2,3,:);
|
||
|
|
robot.regressor.izz = link_inertia(3,3,:);
|
||
|
|
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;
|
||
|
|
w = robot.vel.w ;
|
||
|
|
dw = robot.vel.dw ;
|
||
|
|
dv = robot.vel.dv ;
|
||
|
|
switch opt.LD_method
|
||
|
|
case 'Direct'
|
||
|
|
switch opt.KM_method
|
||
|
|
case 'MDH'
|
||
|
|
for i = 1:ndof
|
||
|
|
p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
|
||
|
|
w_skew(:,:,i) = vec2skewSymMat(w(:,i));
|
||
|
|
dw_skew(:,:,i) = vec2skewSymMat(dw(:,i));
|
||
|
|
dv_skew(:,:,i) = vec2skewSymMat(dv(:,i));
|
||
|
|
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) = [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]
|
||
|
|
% U_ = sym(zeros([6*ndof,10*ndof]));
|
||
|
|
U_ = [];
|
||
|
|
for i = 1:ndof
|
||
|
|
% tricky
|
||
|
|
for j = i:ndof
|
||
|
|
if(j == i)
|
||
|
|
TT = eye(6,6);
|
||
|
|
U_row = TT*robot.regressor.A(:,:,j);
|
||
|
|
else
|
||
|
|
TT = TT*Adjoint(RpToTrans(R(:,:,j),P(:,:,j)));
|
||
|
|
U_row = [U_row,TT*robot.regressor.A(:,:,j)];
|
||
|
|
end
|
||
|
|
end
|
||
|
|
U_ = [U_;zeros(6,(i-1)*10),U_row];
|
||
|
|
end
|
||
|
|
robot.regressor.U = U_;
|
||
|
|
delta_ = zeros([ndof,6*ndof]);
|
||
|
|
for i = 1:ndof
|
||
|
|
delta_(i,6*i) = 1;
|
||
|
|
end
|
||
|
|
robot.regressor.K = delta_*robot.regressor.U;
|
||
|
|
if(opt.debug)
|
||
|
|
sprintf('size of U=%dx%d.',size(robot.regressor.U));
|
||
|
|
sprintf('size of K=%dx%d.',size(robot.regressor.K));
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
case 'Lagrange'
|
||
|
|
disp('TODO opt.LD_method Lagrange!')
|
||
|
|
return;
|
||
|
|
otherwise
|
||
|
|
disp('Bad opt.KM_method!')
|
||
|
|
return;
|
||
|
|
end
|