Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feature/R1000-identification #2

Merged
cosmic_power merged 48 commits from feature/R1000-identification into main 2024-12-16 15:25:53 +00:00
Conversation 0 Commits 48 Files Changed 129 +113
2 changed files with 113 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit a50a0698d6 - Show all commits

96
R1000_Dynamics_num_regressor.m Normal file
View File

@ -0,0 +1,96 @@
%% 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

17
Untitled.m Normal file
View File

@ -0,0 +1,17 @@
com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]';
com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]';
com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]';
com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]';
com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]';
com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]';
com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]';
com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]';
com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]';
com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]';
com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]';
com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]';
com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]';
com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]';
com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]';
com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]';
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]';