2024-12-16 16:33:21 +00:00
|
|
|
function robot = get_robot_R1000_DVT(theta,dtheta,ddtheta,opt)
|
|
|
|
|
switch opt.robot_def
|
|
|
|
|
case 'direct'
|
|
|
|
|
robot = struct;
|
2024-12-17 13:44:56 +00:00
|
|
|
robot.ndof = 9;
|
2024-12-16 16:33:21 +00:00
|
|
|
% ------------------------------------------------------------------------
|
|
|
|
|
% Structure define
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
|
|
|
robot.m = zeros(1,9);
|
|
|
|
|
robot.I = zeros(3,3,9);
|
|
|
|
|
robot.com = zeros(3,9);
|
|
|
|
|
robot.axis = zeros(3,9);
|
|
|
|
|
robot.com_pos_R1 = zeros(3,9);
|
|
|
|
|
robot.com_pos_R2 = zeros(3,9);
|
|
|
|
|
robot.slist = zeros(6,9);
|
|
|
|
|
robot.theta = zeros(9,1);
|
|
|
|
|
robot.dtheta = zeros([9,1]);
|
|
|
|
|
robot.ddtheta = zeros([9,1]);
|
|
|
|
|
robot.Home.com = zeros(3,9);
|
|
|
|
|
robot.Home.R = zeros(3,3,9);
|
|
|
|
|
robot.Home.P = zeros(3,9);
|
|
|
|
|
robot.Home.M = zeros(4,4,9);
|
|
|
|
|
robot.kine.TW = zeros(4,4,9);
|
|
|
|
|
robot.kine.Fkine = zeros(4,4);
|
|
|
|
|
robot.kine.T = zeros(4,4,9);
|
|
|
|
|
robot.kine.R = zeros(3,3,9);
|
|
|
|
|
robot.kine.t = zeros(3,1,9);
|
|
|
|
|
robot.kine.Mlist_CG_Base = zeros(4,4,10);
|
|
|
|
|
robot.kine.Mlist_CG = zeros(4,4,10);
|
|
|
|
|
robot.kine.Mlist_ED = zeros(4,4,10);
|
|
|
|
|
robot.gravity = zeros(3,1);
|
|
|
|
|
robot.pi = zeros(10,9);
|
2024-12-17 13:44:56 +00:00
|
|
|
robot.vel.w=zeros(3,9);
|
|
|
|
|
robot.vel.dw=zeros(3,9);
|
|
|
|
|
robot.vel.v=zeros(3,9);
|
|
|
|
|
robot.vel.dv=zeros(3,9);
|
|
|
|
|
robot.I_vec=zeros(6,9);
|
|
|
|
|
robot.regressor.A = zeros(6,10,9);
|
|
|
|
|
robot.regressor.U = zeros(54,90);
|
|
|
|
|
robot.regressor.K = zeros(9,90);
|
2024-12-16 16:33:21 +00:00
|
|
|
% ------------------------------------------------------------------------
|
|
|
|
|
% Dynamics parameters
|
|
|
|
|
% ------------------------------------------------------------------------
|
2024-12-24 01:29:06 +00:00
|
|
|
link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
|
2024-12-16 16:33:21 +00:00
|
|
|
% DVT
|
|
|
|
|
% link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
|
|
|
|
|
axis_of_rot = zeros(3,robot.ndof);
|
|
|
|
|
axis_of_rot(:,1) = [0;0;1];
|
|
|
|
|
axis_of_rot(:,2) = [0;-1;0];
|
|
|
|
|
axis_of_rot(:,3) = [0;-1;0];
|
|
|
|
|
axis_of_rot(:,4) = [0;-1;0];
|
|
|
|
|
axis_of_rot(:,5) = [0;0;1];
|
|
|
|
|
axis_of_rot(:,6) = [1;0;0];
|
|
|
|
|
axis_of_rot(:,7) = [0;0;-1];
|
|
|
|
|
axis_of_rot(:,8) = [0;-1;0];
|
|
|
|
|
axis_of_rot(:,9) = [0;0;0];
|
|
|
|
|
|
2024-12-17 13:44:56 +00:00
|
|
|
com_pos_R1 = zeros(3,9);
|
|
|
|
|
com_pos_R2= zeros(3,9);
|
2024-12-16 16:33:21 +00:00
|
|
|
|
2024-12-24 01:29:06 +00:00
|
|
|
com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
|
|
|
|
|
com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
|
|
|
|
|
com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3;
|
|
|
|
|
com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3;
|
|
|
|
|
com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3;
|
|
|
|
|
com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3;
|
|
|
|
|
com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3;
|
|
|
|
|
com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3;
|
|
|
|
|
com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
|
|
|
|
|
com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3;
|
|
|
|
|
com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
|
|
|
|
|
com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3;
|
|
|
|
|
com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
|
|
|
|
|
com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3;
|
|
|
|
|
com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
|
|
|
|
|
com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use
|
|
|
|
|
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
2024-12-16 16:33:21 +00:00
|
|
|
% stack result
|
|
|
|
|
robot.com_pos_R1 = com_pos_R1;
|
|
|
|
|
robot.com_pos_R2 = com_pos_R2;
|
|
|
|
|
% FIXME:fix this hack
|
|
|
|
|
% Get 3D coordinate of CO
|
|
|
|
|
co=zeros(3,robot.ndof-1);
|
|
|
|
|
for i = 1:robot.ndof-1
|
|
|
|
|
if i == 1
|
|
|
|
|
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
|
2024-12-24 01:29:06 +00:00
|
|
|
elseif i<8
|
2024-12-16 16:33:21 +00:00
|
|
|
%From base to ISA Origin
|
|
|
|
|
co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
|
2024-12-24 01:29:06 +00:00
|
|
|
else
|
|
|
|
|
%From base to ISA Origin
|
|
|
|
|
co(:,i) = co(:,i-1)-[0.23;0;0.05896];
|
2024-12-16 16:33:21 +00:00
|
|
|
end
|
|
|
|
|
end
|
|
|
|
|
co = [zeros(3,1),co];
|
|
|
|
|
% the inertia tensor wrt the frame oriented as the body frame and with the
|
|
|
|
|
% origin in the COM
|
|
|
|
|
link_inertia = zeros(3,3,robot.ndof);
|
2024-12-24 01:29:06 +00:00
|
|
|
link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];...
|
2024-12-16 16:33:21 +00:00
|
|
|
[-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) = [[1.5162872e+03 -4.6245133e+01 4.4556929e+03];...
|
|
|
|
|
[-4.6245133e+01 5.9901606e+04 6.0350548e+0];...
|
|
|
|
|
[4.4556929e+03 6.0350548e+0 5.8819998e+04]]*10^-6;
|
|
|
|
|
link_inertia(:,:,9) = [[1.1730106e+03 3.3834506e+0 4.6097678e+01];...
|
|
|
|
|
[3.3834506e+0 1.7996852e+03 5.2088778e+0];...
|
|
|
|
|
[4.6097678e+01 5.2088778e+0 1.3960734e+03]]*10^-6;
|
|
|
|
|
% manipulator regressor
|
|
|
|
|
com_pos = com_pos_R1;
|
|
|
|
|
for i = 1:robot.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);
|
|
|
|
|
end
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
|
|
|
% Kinematics parameters
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
|
|
|
if(opt.Isreal)
|
|
|
|
|
robot.theta = theta; robot.dtheta = dtheta; robot.ddtheta = ddtheta;
|
|
|
|
|
% robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
|
|
|
|
switch opt.KM_method
|
|
|
|
|
case 'SCREW'
|
|
|
|
|
robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]];
|
|
|
|
|
robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]];
|
|
|
|
|
robot.Home.R(:,:,3) = [[1;0;0],[0;0;1],[0;-1;0]];
|
|
|
|
|
robot.Home.R(:,:,4) = [[1;0;0],[0;0;1],[0;-1;0]];
|
|
|
|
|
robot.Home.R(:,:,5) = [[0;-1;0],[1;0;0],[0;0;1]];
|
|
|
|
|
robot.Home.R(:,:,6) = [[0;-1;0],[0;0;-1],[1;0;0]];
|
|
|
|
|
robot.Home.R(:,:,7) = [[1;0;0],[0;-1;0],[0;0;-1]];
|
|
|
|
|
robot.Home.R(:,:,8) = [[0;0;-1],[1;0;0],[0;-1;0]];
|
|
|
|
|
robot.Home.R(:,:,9) = [[0;0;-1],[0;1;0],[1;0;0]];
|
|
|
|
|
for i=1:robot.ndof
|
|
|
|
|
robot.Home.P(:,i) = co(:,i);
|
|
|
|
|
robot.Home.M(:,:,i) = RpToTrans(robot.Home.R(:,:,i),robot.Home.P(:,i));
|
|
|
|
|
end
|
|
|
|
|
robot.slist=[[0;0;1;co(:,1)],...
|
|
|
|
|
[0;-1;0;cross(-[0;-1;0],co(:,2))]...
|
|
|
|
|
[0;-1;0;cross(-[0;-1;0],co(:,3))]...
|
|
|
|
|
[0;-1;0;cross(-[0;-1;0],co(:,4))]...
|
|
|
|
|
[0;0;1;cross(-[0;0;1],co(:,5))]...
|
|
|
|
|
[1;0;0;cross(-[1;0;0],co(:,6))]...
|
|
|
|
|
[0;0;-1;cross(-[0;0;-1],co(:,7))]...
|
|
|
|
|
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
|
|
|
|
|
[0;0;0;1;0;0]];
|
|
|
|
|
otherwise
|
|
|
|
|
disp('Bad opt.KM_method!')
|
|
|
|
|
return;
|
|
|
|
|
end
|
|
|
|
|
end
|
|
|
|
|
otherwise
|
|
|
|
|
robot = [];
|
|
|
|
|
disp('Bad robot define options!')
|
|
|
|
|
return
|
|
|
|
|
end
|
|
|
|
|
%Gravity
|
|
|
|
|
gravity = [0;0;-9.806];
|
|
|
|
|
robot.gravity = gravity;
|
