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 +24 -133
5 changed files with 24 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit cc9a842f0b - Show all commits

10
Identification_main.asv
View File

@ -1,10 +0,0 @@
file = [];
opt.robot_def = 'direct';
opt.KM_method = 'MDH';
opt.LD_method = 'direct';
robot = get_robot(file,opt);
robot.theta = [1,1,0];
robot = get_Kinematics(robot, opt);
robot = get

2
Identification_main.m
View File

@ -7,7 +7,7 @@ opt.debug = true;
opt.robotName = 'Two_bar';
opt.Isreal = false;
robot = get_robot(file,opt);
robot = get_robot_R1000(file,opt);
% robot.theta = [1,1,0];
robot = get_Kinematics(robot, opt);

65
get_regressor.asv
View File

@ -1,65 +0,0 @@
function robot = get_regressor(robot, opt)
% Create symbolic generilized coordiates, their first and second deriatives
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');
% 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)];
% 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 = 2:ndof+1
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-1) = [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_;
if(opt.debug)
sprintf('size of U_=%dx%d.',size(robot.regressor.U));
end
robot.regressor.K = [zeros(1,3),Z0]*;
end
% matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
% 'Vars',{q_sym,qd_sym,q2d_sym});
case 'Lagrange'
disp('TODO opt.LD_method Lagrange!')
return;
otherwise
disp('Bad opt.KM_method!')
return;
end

52
get_robot.asv
View File

@ -1,52 +0,0 @@
function robot = get_robot(file,opt)
switch opt.robot_def
case 'direct'
ndof = 2;
% Kinematics parameters
switch opt.Isreal
case
switch opt.KM_method
case 'SDH'
case 'MDH'
robot.theta = zeros([1,ndof+1]);
robot.a = [0,1,1];
robot.d = [0,0,0];
robot.alpha = [0,0,0];
otherwise
disp('Bad opt.KM_method!')
return;
end
% Dynamics parameters
link_mass = [1,1];
axis_of_rot(:,:,1) = [0;0;1];
axis_of_rot(:,:,2) = [0;0;1];
com_pos(:,:,1) = [1/2;0;0];
com_pos(:,:,2) = [1/2;0;0];
% the inertia tensor wrt the frame oriented as the body frame and with the
% origin in the COM
link_inertia(:,:,1) = diag([1,1,1]);
link_inertia(:,:,2) = diag([1,1,1]);
% manipulator regressor
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*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);
robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-...
link_mass(i)*com_vec2mat*com_vec2mat);
robot.pi(:,i) = [robot.I_vec(:,i); robot.mc(:,i); robot.m(i)];
end
case 'urdf'
robot = parse_urdf(file);
case 'mat'
robot = [];
disp('TODO mat robot define options!')
otherwise
robot = [];
disp('Bad robot define options!')
return
end

28
get_robot_R1000.m
View File

@ -1,7 +1,7 @@
function robot = get_robot(file,opt)
function robot = get_robot_R1000(file,opt)
switch opt.robot_def
case 'direct'
ndof = 2;
ndof = 8;
robot.ndof = ndof;
% Kinematics parameters
if(opt.Isreal)
@ -31,7 +31,7 @@ switch opt.robot_def
case 'SDH'
case 'MDH'
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0];
robot.d = [0,0,0,0,0,0,0,0];
robot.d = [0,0,0,0,0,0.28,0.40,0];
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2];
otherwise
disp('Bad opt.KM_method!')
@ -43,13 +43,31 @@ switch opt.robot_def
%TODO in process, seems axis_of_rot useless
axis_of_rot(:,:,1) = [0;0;1];
axis_of_rot(:,:,2) = [0;0;1];
axis_of_rot(:,:,3) = [0;0;1];
axis_of_rot(:,:,4) = [0;0;1];
axis_of_rot(:,:,5) = [0;0;1];
axis_of_rot(:,:,6) = [0;0;1];
axis_of_rot(:,:,7) = [0;0;1];
axis_of_rot(:,:,8) = [0;0;1];
% 画图
com_pos(:,:,1) = [1/2;0;0];
com_pos(:,:,2) = [1/2;0;0];
com_pos(:,:,1) = [0;0;0];
com_pos(:,:,2) = [0.2/2;0;0];
com_pos(:,:,3) = [0.5/2;0;0];
com_pos(:,:,4) = [0.45/2;0;0];
com_pos(:,:,5) = [0.12/2;0;0];
com_pos(:,:,6) = [0.28/2;0;0];
com_pos(:,:,7) = [0.4/2;0;0];
com_pos(:,:,8) = [0.2772/2;0;0];
% the inertia tensor wrt the frame oriented as the body frame and with the
% origin in the COM
link_inertia(:,:,1) = diag([1,1,1]);
link_inertia(:,:,2) = diag([1,1,1]);
link_inertia(:,:,3) = diag([1,1,1]);
link_inertia(:,:,4) = diag([1,1,1]);
link_inertia(:,:,5) = diag([1,1,1]);
link_inertia(:,:,6) = diag([1,1,1]);
link_inertia(:,:,7) = diag([1,1,1]);
link_inertia(:,:,8) = diag([1,1,1]);
% manipulator regressor
for i = 1:ndof
robot.m(i) = link_mass(i);