function robot = get_robot_R1000(file,opt) switch opt.robot_def case 'direct' ndof = 8; robot.ndof = ndof; % Kinematics parameters if(opt.Isreal) switch opt.KM_method case 'SDH' case 'MDH' robot.theta = zeros([1,ndof]); robot.a = [0,1,1]; robot.d = [0,0,0]; robot.alpha = [0,0,0]; 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,1],'real'); qd_sym = sym('qd%d',[ndof+1,1],'real'); q2d_sym = sym('qdd%d',[ndof+1,1],'real'); q_sym(ndof+1) = 0; qd_sym(ndof+1) = 0; q2d_sym(ndof+1) = 0; robot.theta = q_sym; robot.dtheta = qd_sym; robot.ddtheta = q2d_sym; switch opt.KM_method 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.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!') return; end end % Dynamics parameters link_mass = [1,1,1,1,1,1,1,1]; %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) = [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); 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 %Gravity gravity = [0;0;9.8]; robot.gravity = gravity;