function robot = get_robot_R1000(file,opt) switch opt.robot_def case 'direct' ndof = 9; robot.ndof = ndof; % ------------------------------------------------------------------------ % Dynamics parameters % ------------------------------------------------------------------------ link_mass = [2.1315260e+01,1.9235354e+01,1.0463871e+01,6.1538806,3.0882893,2.4450941,3.6376589,1.7005420,9.1507819e-1]; % DVT % link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2]; %TODO in process, seems axis_of_rot useless 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]; %DVT % 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; com_pos_R1(:,1)=[ 1.0440117e+02 -1.1125559e+01 1.2822933e+02]'*1e-3; com_pos_R2(:,1)=[-8.0598830e+01 -4.2225559e+01 -1.5870672e+01]'*1e-3; com_pos_R1(:,2)=[ 3.5562148e+02 1.2378198e+01 1.8395830e-03]'*1e-3; com_pos_R2(:,2)=[-2.4437852e+02 9.4778198e+01 1.8395830e-03]'*1e-3; com_pos_R1(:,3)=[2.4798242e+02 -3.5267916e+01 4.8648267e-02]'*1e-3; com_pos_R2(:,3)=[-3.5201758e+02 -2.9007916e+01 4.8648266e-02]'*1e-3; com_pos_R1(:,4)=[ 4.2341069e+01 5.2603527e+01 1.2271365e+01]'*1e-3; com_pos_R2(:,4)=[-7.7658931e+01 -1.2956223e+01 1.9271365e+01]'*1e-3; com_pos_R1(:,5)=[1.0177553e+02 -3.4868152e-02 1.5686100e+00]'*1e-3; com_pos_R2(:,5)=[-1.0372447e+02 -3.9239008e-02 -5.4313602e+00]'*1e-3; com_pos_R1(:,6)=[5.9942347e+01 -4.8346177e-02 5.5784018e+00]'*1e-3; com_pos_R2(:,6)=[-2.3757653e+01 -4.8346177e-02 4.0028402e+01]'*1e-3; com_pos_R1(:,7)=[4.9494392e-02 -2.4240394e+00 -1.0332489e+02]'*1e-3; com_pos_R2(:,7)=[4.9494392e-02 -2.4240394e+00 2.7222511e+02]'*1e-3; com_pos_R1(:,8)=[-2.7158498e+01 -3.3865875e-01 -4.6588829e+01]'*1e-3; com_pos_R2(:,8)=[ 2.2423433e+02 -3.3865729e-01 1.9671171e+01]'*1e-3; % don't use com_pos_R1(:,9)=[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3; % 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=[]; for i = 1:8 if i == 1 co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i); else %From base to ISA Origin co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i); % else % %From base to ISA Origin % co(:,i) = co(:,i-1)-[0;0;0.05896]; 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(:,:,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) = [[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; % 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 % manipulator regressor com_pos = com_pos_R1; 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(i)*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(:,i)); robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia(:,:,i)-... link_mass(i)*com_vec2mat*com_vec2mat); robot.pi(:,i) = [robot.m(i);robot.mc(:,i);robot.I_vec(:,i)]; end % ------------------------------------------------------------------------ % Kinematics parameters % ------------------------------------------------------------------------ if(opt.Isreal) %FIXME: only consider the theta temply robot.dtheta = zeros([ndof,1]); robot.ddtheta = zeros([ndof,1]); robot.link_type = ['R','R','R','R','R','R','R','R','P']; switch opt.KM_method case 'SDH' case 'MDH' 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.05896]; robot.d = [0,0,0,0,0,0.28,0.40,0,0]; robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2]; 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:9 % 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]]; 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]]; robot.theta = zeros([ndof,1]); % robot.theta(end-1) = pi/4; % robot.theta(3) = pi/4; % robot.theta(4) = pi/4; otherwise disp('Bad opt.KM_method!') return; end else % Create symbolic generilized coordiates, their first and second deriatives q_sym = sym('q%d',[ndof,1],'real'); qd_sym = sym('qd%d',[ndof,1],'real'); q2d_sym = sym('qdd%d',[ndof,1],'real'); robot.theta = q_sym; %FIXME: only consider the theta temply robot.dtheta = zeros([ndof,1]); robot.ddtheta = zeros([ndof,1]); % robot.dtheta = qd_sym; % robot.ddtheta = q2d_sym; % %R1000 ISA ?? % robot.theta(ndof) = 0; % robot.dtheta(ndof) = 0; % robot.ddtheta(ndof) = 0; 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:9 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]]; robot.link_type = ['R','R','R','R','R','R','R','R','P']; case 'SDH' case 'MDH' 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]; robot.link_type = ['R','R','R','R','R','R','R','R','P']; robot.d = sym(robot.d); robot.d(ndof)=q_sym(ndof); %init vd robot.vd = robot.d; robot.vd(ndof)=q_sym(ndof); %init accd robot.accd = robot.d; robot.accd(ndof)=q_sym(ndof); otherwise disp('Bad opt.KM_method!') return; end 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.806]; robot.gravity = gravity;