2024-09-21 07:43:05 +00:00
|
|
|
function robot = get_robot_R1000(file,opt)
|
2024-09-19 16:29:22 +00:00
|
|
|
switch opt.robot_def
|
|
|
|
|
case 'direct'
|
2024-09-22 18:09:08 +00:00
|
|
|
ndof = 9;
|
2024-09-19 16:29:22 +00:00
|
|
|
robot.ndof = ndof;
|
|
|
|
|
% Kinematics parameters
|
|
|
|
|
if(opt.Isreal)
|
|
|
|
|
switch opt.KM_method
|
|
|
|
|
case 'SDH'
|
|
|
|
|
case 'MDH'
|
2024-09-22 18:09:08 +00:00
|
|
|
robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0];
|
|
|
|
|
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];
|
|
|
|
|
robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157];
|
|
|
|
|
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
|
2024-09-23 15:28:41 +00:00
|
|
|
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
2024-09-19 16:29:22 +00:00
|
|
|
otherwise
|
|
|
|
|
disp('Bad opt.KM_method!')
|
|
|
|
|
return;
|
|
|
|
|
end
|
|
|
|
|
else
|
|
|
|
|
% Create symbolic generilized coordiates, their first and second deriatives
|
2024-09-23 15:28:41 +00:00
|
|
|
q_sym = sym('q%d',[ndof,1],'real');
|
|
|
|
|
qd_sym = sym('qd%d',[ndof,1],'real');
|
|
|
|
|
q2d_sym = sym('qdd%d',[ndof,1],'real');
|
|
|
|
|
|
2024-09-19 16:29:22 +00:00
|
|
|
robot.theta = q_sym;
|
|
|
|
|
robot.dtheta = qd_sym;
|
|
|
|
|
robot.ddtheta = q2d_sym;
|
2024-09-22 18:09:08 +00:00
|
|
|
%R1000 ISA
|
|
|
|
|
robot.theta(ndof) = 0;
|
|
|
|
|
robot.dtheta(ndof) = 0;
|
|
|
|
|
robot.ddtheta(ndof) = 0;
|
2024-09-19 16:29:22 +00:00
|
|
|
switch opt.KM_method
|
|
|
|
|
case 'SDH'
|
|
|
|
|
case 'MDH'
|
2024-09-22 18:09:08 +00:00
|
|
|
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];
|
2024-09-23 15:28:41 +00:00
|
|
|
robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157];
|
2024-09-22 18:09:08 +00:00
|
|
|
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
|
|
|
|
|
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
2024-09-19 16:29:22 +00:00
|
|
|
otherwise
|
|
|
|
|
disp('Bad opt.KM_method!')
|
|
|
|
|
return;
|
|
|
|
|
end
|
2024-09-23 15:28:41 +00:00
|
|
|
robot.d = sym(robot.d);
|
|
|
|
|
robot.d(ndof)=q_sym(ndof);
|
2024-09-22 18:09:08 +00:00
|
|
|
%init vd
|
|
|
|
|
robot.vd = robot.d;
|
2024-09-23 15:28:41 +00:00
|
|
|
robot.vd(ndof)=q_sym(ndof);
|
2024-09-22 18:09:08 +00:00
|
|
|
%init accd
|
|
|
|
|
robot.accd = robot.d;
|
2024-09-23 15:28:41 +00:00
|
|
|
robot.accd(ndof)=q_sym(ndof);
|
2024-09-19 16:29:22 +00:00
|
|
|
end
|
|
|
|
|
% Dynamics parameters
|
2024-09-22 18:09:08 +00:00
|
|
|
link_mass = [17.42,7.7,2.42,5.16,2.22,1.78,2.32,2.92 0.1];
|
2024-09-19 16:29:22 +00:00
|
|
|
%TODO in process, seems axis_of_rot useless
|
2024-09-22 18:09:08 +00:00
|
|
|
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];
|
|
|
|
|
axis_of_rot(:,9) = [0;0;1];
|
2024-09-19 16:29:22 +00:00
|
|
|
% 画图
|
2024-09-23 15:28:41 +00:00
|
|
|
% com_pos(:,1) = [0;0;0.122];
|
|
|
|
|
% com_pos(:,2) = [0.373;0;0];
|
|
|
|
|
% com_pos(:,3) = [0.188;0;0];
|
|
|
|
|
% com_pos(:,4) = [0.05;0;-0.05];
|
|
|
|
|
% com_pos(:,5) = [0;0.13;0];
|
|
|
|
|
% com_pos(:,6) = [0;0.028;0.049];
|
|
|
|
|
% com_pos(:,7) = [0;0;0.102];
|
|
|
|
|
% com_pos(:,8) = [0;0.06;0];
|
|
|
|
|
% com_pos(:,9) = [0;0;0];
|
|
|
|
|
|
|
|
|
|
com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
|
|
|
|
|
com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3;
|
|
|
|
|
com_pos(:,3) = [1.8811711e+02 4.9225523e-04 -7.9651429e+00]'*10^-3;
|
|
|
|
|
com_pos(:,4) = [5.3924501e+01 -5.1146307e+00 -5.8247754e+01]'*10^-3;
|
|
|
|
|
com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
|
|
|
|
|
com_pos(:,6) = [-6.0339586e-02 2.8867234e+01 4.9027126e+01]'*10^-3;
|
|
|
|
|
com_pos(:,7) = [ -2.3140597e-02 -9.5969566e+00 1.0229381e+02]'*10^-3;
|
|
|
|
|
com_pos(:,8) = [1.4847544e+01 6.8637348e+01 -1.5713395e-01]'*10^-3;
|
2024-09-22 18:09:08 +00:00
|
|
|
com_pos(:,9) = [0;0;0];
|
2024-09-19 16:29:22 +00:00
|
|
|
% the inertia tensor wrt the frame oriented as the body frame and with the
|
|
|
|
|
% origin in the COM
|
2024-09-23 15:28:41 +00:00
|
|
|
% 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]);
|
|
|
|
|
% link_inertia(:,:,9) = diag([1,1,1]);
|
|
|
|
|
link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];...
|
|
|
|
|
[-1.1694021e+04 2.3289532e+05 -3.0395414e+02];...
|
|
|
|
|
[-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6;
|
|
|
|
|
link_inertia(:,:,2) = [[3.7926328e+04 -9.0569033e+01 4.7526575e+04];...
|
|
|
|
|
[-9.0569033e+01 2.9714754e+05 6.8396715e+00];...
|
|
|
|
|
[4.7526575e+04 6.8396715e+00 2.8138392e+05]]*10^-6;
|
|
|
|
|
link_inertia(:,:,3) = [[4.4513887e+03 1.9981964e-01 -3.0303891e+02];...
|
|
|
|
|
[1.9981964e-01 6.7952039e+04 -8.8585864e-02];...
|
|
|
|
|
[-3.0303891e+02 -8.8585864e-02 6.9958344e+04]]*10^-6;
|
|
|
|
|
link_inertia(:,:,4) = [[1.1642351e+04 2.2997175e+03 2.9159431e+03];...
|
|
|
|
|
[2.2997175e+03 2.6031269e+04 -1.3518384e+02];...
|
|
|
|
|
[2.9159431e+03 -1.3518384e+02 2.4694742e+04]]*10^-6;
|
|
|
|
|
link_inertia(:,:,5) = [[3.0930544e+03 8.3558814e-01 -2.8169092e+03];...
|
|
|
|
|
[8.3558814e-01 1.2796446e+04 -3.3666469e+00];...
|
|
|
|
|
[-2.8169092e+03 -3.3666469e+00 1.2128856e+04]]*10^-6;
|
|
|
|
|
link_inertia(:,:,6) = [[3.6635776e+03 -7.0081461e+00 2.2392870e+00];...
|
|
|
|
|
[-7.0081461e+00 1.8152305e+03 -2.4828765e+02];...
|
|
|
|
|
[2.2392870e+00 -2.4828765e+02 3.4602935e+03]]*10^-6;
|
|
|
|
|
link_inertia(:,:,7) = [[1.3662652e+04 -3.6340953e+00 4.4011670e-01];...
|
|
|
|
|
[-3.6340953e+00 1.3222824e+04 -4.3625500e+02];...
|
|
|
|
|
[ 4.4011670e-01 -4.3625500e+02 2.2500397e+03]]*10^-6;
|
|
|
|
|
link_inertia(:,:,8) = [[4.6491328e+03 3.0225715e+03 2.8800116e+01];...
|
|
|
|
|
[3.0225715e+03 8.8414058e+04 -3.0084286e+01];...
|
|
|
|
|
[2.8800116e+01 -3.0084286e+01 9.1858852e+04]]*10^-6;
|
2024-09-22 18:09:08 +00:00
|
|
|
link_inertia(:,:,9) = diag([1,1,1]);
|
2024-09-23 15:28:41 +00:00
|
|
|
% verify if link_inertia is issymmetric
|
|
|
|
|
for i = 1:ndof
|
|
|
|
|
if(issymmetric(link_inertia(:,:,i))==false)
|
|
|
|
|
fprintf('Bad definition of inertia matrix %d\n',i)
|
|
|
|
|
return;
|
|
|
|
|
end
|
|
|
|
|
end
|
2024-09-19 16:29:22 +00:00
|
|
|
% manipulator regressor
|
|
|
|
|
for i = 1:ndof
|
|
|
|
|
robot.m(i) = link_mass(i);
|
|
|
|
|
robot.axis(:,i) = axis_of_rot(i);
|
2024-09-22 18:09:08 +00:00
|
|
|
robot.com(:,i) = com_pos(:,i);
|
|
|
|
|
robot.I(:,:,i) = link_inertia(:,:,i);
|
|
|
|
|
robot.mc(:,i) = link_mass(i)*com_pos(:,i);
|
2024-09-19 16:29:22 +00:00
|
|
|
% the inertia tensor wrt the frame oriented as the body frame and with the
|
|
|
|
|
% origin in the Joint i
|
2024-09-22 18:09:08 +00:00
|
|
|
com_vec2mat = vec2skewSymMat(com_pos(:,i));
|
|
|
|
|
robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia(:,:,i)-...
|
2024-09-19 16:29:22 +00:00
|
|
|
link_mass(i)*com_vec2mat*com_vec2mat);
|
2024-09-22 18:09:08 +00:00
|
|
|
robot.pi(:,i) = [robot.m(i);robot.mc(:,i);robot.I_vec(:,i)];
|
2024-09-19 16:29:22 +00:00
|
|
|
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
|
|
|
|
|
%Gravity
|
|
|
|
|
gravity = [0;0;9.8];
|
|
|
|
|
robot.gravity = gravity;
|
