97 lines
3.5 KiB
Matlab
97 lines
3.5 KiB
Matlab
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; |