Compare commits
1 Commits
main
...
feature/ad
| Author | SHA1 | Date |
|---|---|---|
 cosmic_power
|
059831c913 |
|
|
@ -0,0 +1,6 @@
|
|||
file = [];
|
||||
opt.robot_def = 'direct';
|
||||
opt.KM_method = 'MDH';
|
||||
robot = get_robot(file,opt);
|
||||
robot.theta = [1,0,0];
|
||||
robot = get_Kinematics(robot, opt);
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
function robot = get_Kinematics(robot, opt)
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
theta = robot.theta;
|
||||
alpha = robot.alpha;
|
||||
a = robot.a;
|
||||
d = robot.d;
|
||||
robot.Fkine = eye(4,4);
|
||||
ndof = length(theta); % special for MDH
|
||||
% init transform matrix
|
||||
robot.R = zeros([3,3,ndof]);
|
||||
robot.t = zeros([3,1,ndof]);
|
||||
robot.T = zeros([4,4,ndof]);
|
||||
for i = 1:ndof
|
||||
robot.R(:,:,i) = [cos(theta(i)) -sin(theta(i)) 0;...
|
||||
sin(theta(i))*cos(alpha(i)) cos(theta(i))*cos(alpha(i)) -sin(alpha(i));...
|
||||
sin(theta(i))*sin(alpha(i)) cos(theta(i))*sin(alpha(i)) cos(alpha(i))];
|
||||
robot.t(:,:,i) = [a(i);-d(i)*sin(alpha(i));d(i)*cos(alpha(i))];
|
||||
Transform = eye(4,4);
|
||||
Transform(1:3,1:3) = robot.R(:,:,i);
|
||||
Transform(1:3,4) = robot.t(:,:,i);
|
||||
robot.T(:,:,i) = Transform;
|
||||
robot.Fkine = robot.Fkine*robot.T(:,:,i);
|
||||
end
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
function [taulist]= get_rb_dynamics(thetalist, dthetalist, ddthetalist, g,...
|
||||
dh_list, mass_list, mass_center_list, inertia_tensor_list, f_tip)
|
||||
%---------变量定义-----------------
|
||||
% thetalist:6x1,关节变量，dthetalist:6x1,关节变量一阶导数, ddthetalist:6x1,关节变量二阶导数
|
||||
% g: 1x1,重力加速度
|
||||
% dh_list:6x4,modified_DH参数
|
||||
% mass_list:6x1,连杆质量，mass_center_list:6x3,连杆质心相对于坐标系{i}坐标，
|
||||
% inertia_tensor_list: 3x3x6,连杆相对于质心坐标系的惯量张量
|
||||
% f_tip: 2x3,机械臂末端施加外力和力矩
|
||||
|
||||
%taulist:6x1,各关节所需力矩
|
||||
|
||||
|
||||
%R：3x3x6,旋转矩阵，P：3x6,后一连杆坐标系在前一连杆坐标系中的位置
|
||||
%w：3x6,连杆角速度，dw：3x6,连杆角加速度，dv：3x6,连杆线加速度，dvc：3x6,连杆质心线加速度
|
||||
%Ic:3x3x6,等同于inertia_tensor_list
|
||||
%Pc:3x6, mass_center_list的转置
|
||||
%F:3x6,各轴所受合力，N:3x6,各轴所受合力矩
|
||||
%f:3x6,前一轴给后一轴的力，n:3x6,前一轴给后一轴的力矩
|
||||
|
||||
dof_num = size(dthetalist,1);
|
||||
|
||||
alpha = dh_list(:,1);
|
||||
a = dh_list(:,2);
|
||||
d = dh_list(:,3);
|
||||
theta = dh_list(:,4);
|
||||
|
||||
m = mass_list;
|
||||
Pc = mass_center_list';
|
||||
Ic = inertia_tensor_list;
|
||||
|
||||
Z=[0;0;1];
|
||||
%转换矩阵建立
|
||||
|
||||
theta = theta + thetalist;
|
||||
T=zeros(4,4,dof_num);R=zeros(3,3,dof_num);P=zeros(3,dof_num);
|
||||
for i=1:dof_num
|
||||
T(:,:,i)=[cos(theta(i)) -sin(theta(i)) 0 a(i)
|
||||
sin(theta(i))*cos(alpha(i)) cos(theta(i))*cos(alpha(i)) -sin(alpha(i)) -d(i)*sin(alpha(i))
|
||||
sin(theta(i))*sin(alpha(i)) cos(theta(i))*sin(alpha(i)) cos(alpha(i)) d(i)*cos(alpha(i))
|
||||
0 0 0 1];
|
||||
R(:,:,i)=T(1:3,1:3,i);
|
||||
P(:,i)=T(1:3,4,i);
|
||||
end
|
||||
|
||||
TT = eye(4,4);
|
||||
for i = 1:dof_num
|
||||
TT = TT*T(:,:,i);
|
||||
end
|
||||
|
||||
%运动学正向递推
|
||||
w0 = zeros(3,1); dw0 = zeros(3,1);
|
||||
dv0 = [0;0;g];
|
||||
w = zeros(3,dof_num); dw = zeros(3,dof_num);
|
||||
dv = zeros(3,dof_num); dvc = zeros(3,dof_num);
|
||||
F = zeros(3,dof_num); N = zeros(3,dof_num);
|
||||
|
||||
%i = 0
|
||||
w(:,1) = R(:,:,1)' * w0 + dthetalist(1) * Z;
|
||||
dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dthetalist(1) * Z) + ddthetalist(1) * Z;
|
||||
dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
|
||||
dvc(:,1) = cross(dw(:,1), Pc(:,1))+cross(w(:,1), cross(w(:,1), Pc(:,1))) + dv(:,1);
|
||||
for i = 1:dof_num-1
|
||||
w(:,i+1) = R(:,:,i+1)' * w(:,i) + dthetalist(i+1) * Z ;
|
||||
dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dthetalist(i+1) * Z)+ ddthetalist(i+1) * Z;
|
||||
dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i));
|
||||
dvc(:,i+1) = cross(dw(:,i+1), Pc(:,i+1)) + cross(w(:,i+1), cross(w(:,i+1), Pc(:,i+1))) + dv(:,i+1);
|
||||
end
|
||||
|
||||
for i = 1:dof_num
|
||||
F(:,i)=m(i)*dvc(:,i) ;
|
||||
N(:,i)=Ic(:,:,i) * dw(:,i) + cross(w(:,i), Ic(:,:,i) * w(:,i));
|
||||
end
|
||||
|
||||
%动力学逆向递推
|
||||
%先计算杆6的力和力矩
|
||||
taulist = zeros(dof_num,1);
|
||||
f=zeros(3,dof_num); n=zeros(3,dof_num);
|
||||
|
||||
f(:,dof_num) = F(:,dof_num) + f_tip(1,:)';
|
||||
n(:,dof_num) = N(:,dof_num) + f_tip(2,:)' + cross(Pc(:,dof_num), F(:,dof_num));
|
||||
taulist(dof_num) = n(:,dof_num)' * Z;
|
||||
%再计算杆5到1的力和力矩
|
||||
for i=dof_num-1:-1:1
|
||||
f(:,i) = R(:,:,i+1) * f(:,i+1) + F(:,i);
|
||||
n(:,i) = N(:,i) + R(:,:,i+1) * n(:,i+1) + cross(Pc(:,i), F(:,i))...
|
||||
+ cross(P(:,i+1), R(:,:,i+1) * f(:,i+1));
|
||||
taulist(i) = n(:,i)' * Z;
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
function tau = get_rne_mdh(robot, a1, a2, a3, a4, a5)
|
||||
|
||||
z0 = [0;0;1];
|
||||
grav = robot.gravity; % default gravity from the object
|
||||
fext = zeros(6, 1);
|
||||
|
||||
% Set debug to:
|
||||
% 0 no messages
|
||||
% 1 display results of forward and backward recursions
|
||||
% 2 display print R and p*
|
||||
debug = 0;
|
||||
|
||||
n = robot.n;
|
||||
if numcols(a1) == 3*n
|
||||
Q = a1(:,1:n);
|
||||
Qd = a1(:,n+1:2*n);
|
||||
Qdd = a1(:,2*n+1:3*n);
|
||||
np = numrows(Q);
|
||||
if nargin >= 3,
|
||||
grav = a2(:);
|
||||
end
|
||||
if nargin == 4
|
||||
fext = a3;
|
||||
end
|
||||
else
|
||||
np = numrows(a1);
|
||||
Q = a1;
|
||||
Qd = a2;
|
||||
Qdd = a3;
|
||||
if numcols(a1) ~= n || numcols(Qd) ~= n || numcols(Qdd) ~= n || ...
|
||||
numrows(Qd) ~= np || numrows(Qdd) ~= np
|
||||
error('bad data');
|
||||
end
|
||||
if nargin >= 5,
|
||||
grav = a4(:);
|
||||
end
|
||||
if nargin == 6
|
||||
fext = a5;
|
||||
end
|
||||
end
|
||||
|
||||
if robot.issym || any([isa(Q,'sym'), isa(Qd,'sym'), isa(Qdd,'sym')])
|
||||
tau = zeros(np,n, 'sym');
|
||||
else
|
||||
tau = zeros(np,n);
|
||||
end
|
||||
|
||||
for p=1:np
|
||||
q = Q(p,:).';
|
||||
qd = Qd(p,:).';
|
||||
qdd = Qdd(p,:).';
|
||||
|
||||
Fm = [];
|
||||
Nm = [];
|
||||
pstarm = [];
|
||||
Rm = [];
|
||||
w = zeros(3,1);
|
||||
wd = zeros(3,1);
|
||||
vd = grav(:);
|
||||
|
||||
%
|
||||
% init some variables, compute the link rotation matrices
|
||||
%
|
||||
for j=1:n
|
||||
link = robot.links(j);
|
||||
Tj = link.A(q(j));
|
||||
switch link.type
|
||||
case 'R'
|
||||
D = link.d;
|
||||
case 'P'
|
||||
D = q(j);
|
||||
end
|
||||
alpha = link.alpha;
|
||||
pm = [link.a; -D*sin(alpha); D*cos(alpha)]; % (i-1) P i
|
||||
if j == 1
|
||||
pm = t2r(robot.base) * pm;
|
||||
Tj = robot.base * Tj;
|
||||
end
|
||||
Pm(:,j) = pm;
|
||||
Rm{j} = t2r(Tj);
|
||||
if debug>1
|
||||
Rm{j}
|
||||
Pm(:,j).'
|
||||
end
|
||||
end
|
||||
|
||||
%
|
||||
% the forward recursion
|
||||
%
|
||||
for j=1:n
|
||||
link = robot.links(j);
|
||||
|
||||
R = Rm{j}.'; % transpose!!
|
||||
P = Pm(:,j);
|
||||
Pc = link.r;
|
||||
|
||||
%
|
||||
% trailing underscore means new value
|
||||
%
|
||||
switch link.type
|
||||
case 'R'
|
||||
% revolute axis
|
||||
w_ = R*w + z0*qd(j);
|
||||
wd_ = R*wd + cross(R*w,z0*qd(j)) + z0*qdd(j);
|
||||
%v = cross(w,P) + R*v;
|
||||
vd_ = R * (cross(wd,P) + ...
|
||||
cross(w, cross(w,P)) + vd);
|
||||
|
||||
case 'P'
|
||||
% prismatic axis
|
||||
w_ = R*w;
|
||||
wd_ = R*wd;
|
||||
%v = R*(z0*qd(j) + v) + cross(w,P);
|
||||
vd_ = R*(cross(wd,P) + ...
|
||||
cross(w, cross(w,P)) + vd ...
|
||||
) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j);
|
||||
end
|
||||
% update variables
|
||||
w = w_;
|
||||
wd = wd_;
|
||||
vd = vd_;
|
||||
|
||||
vdC = cross(wd,Pc).' + ...
|
||||
cross(w,cross(w,Pc)).' + vd;
|
||||
F = link.m*vdC;
|
||||
N = link.I*wd + cross(w,link.I*w);
|
||||
Fm = [Fm F];
|
||||
Nm = [Nm N];
|
||||
if debug
|
||||
fprintf('w: '); fprintf('%.3f ', w)
|
||||
fprintf('\nwd: '); fprintf('%.3f ', wd)
|
||||
fprintf('\nvd: '); fprintf('%.3f ', vd)
|
||||
fprintf('\nvdbar: '); fprintf('%.3f ', vdC)
|
||||
fprintf('\n');
|
||||
end
|
||||
end
|
||||
|
||||
%
|
||||
% the backward recursion
|
||||
%
|
||||
|
||||
fext = fext(:);
|
||||
f = fext(1:3); % force/moments on end of arm
|
||||
nn = fext(4:6);
|
||||
|
||||
for j=n:-1:1
|
||||
|
||||
%
|
||||
% order of these statements is important, since both
|
||||
% nn and f are functions of previous f.
|
||||
%
|
||||
link = robot.links(j);
|
||||
|
||||
if j == n
|
||||
R = eye(3,3);
|
||||
P = [0;0;0];
|
||||
else
|
||||
R = Rm{j+1};
|
||||
P = Pm(:,j+1); % i/P/(i+1)
|
||||
end
|
||||
Pc = link.r;
|
||||
|
||||
f_ = R*f + Fm(:,j);
|
||||
nn_ = Nm(:,j) + R*nn + cross(Pc,Fm(:,j)).' + ...
|
||||
cross(P,R*f);
|
||||
|
||||
f = f_;
|
||||
nn = nn_;
|
||||
|
||||
if debug
|
||||
fprintf('f: '); fprintf('%.3f ', f)
|
||||
fprintf('\nn: '); fprintf('%.3f ', nn)
|
||||
fprintf('\n');
|
||||
end
|
||||
switch link.type
|
||||
case 'R'
|
||||
% revolute
|
||||
tau(p,j) = nn.'*z0 + ...
|
||||
link.G^2 * link.Jm*qdd(j) - ...
|
||||
friction(link, qd(j));
|
||||
case 'P'
|
||||
% prismatic
|
||||
tau(p,j) = f.'*z0 + ...
|
||||
link.G^2 * link.Jm*qdd(j) - ...
|
||||
friction(link, qd(j));
|
||||
end
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
function robot = get_robot(file,opt)
|
||||
switch opt.robot_def
|
||||
case 'direct'
|
||||
ndof = 2;
|
||||
% Kinematics parameters
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.theta = zeros([1,ndof+1]);
|
||||
robot.a = [0,1,1];
|
||||
robot.d = [0,0,0];
|
||||
robot.alpha = [0,0,0];
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
% Dynamics parameters
|
||||
link_mass = [1,1];
|
||||
axis_of_rot(:,:,1) = [0;0;1];
|
||||
axis_of_rot(:,:,2) = [0;0;1];
|
||||
com_pos(:,:,1) = [1/2;0;0];
|
||||
com_pos(:,:,2) = [1/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]);
|
||||
% 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
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
mdh = 1;
|
||||
if mdh==1
|
||||
% theta d a alpha offset
|
||||
L1=Link('revolute', 'd', 0, 'a', 0, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,40]),'modified');
|
||||
L2=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'modified');
|
||||
L3=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'modified');
|
||||
Two_bar=SerialLink([L1 L2 L3],'name','Two_bar'); %连接连杆
|
||||
Two_bar.teach();
|
||||
% Two_bar.plot([0 0])%机械臂图
|
||||
else
|
||||
% theta d a alpha offset
|
||||
L1=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,40]),'standard');
|
||||
L2=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'standard');
|
||||
Two_bar=SerialLink([L1 L2],'name','Two_bar'); %连接连杆
|
||||
% Two_bar.plot([0 0])%机械臂图
|
||||
Two_bar.teach();
|
||||
end
|
||||
|
||||
|
||||
% a = [0, -0.42500, -0.39225, 0, 0, 0];
|
||||
% d = [0.0892, 0, 0, 0.10915, 0.09465, 0.0823];
|
||||
% alpha = [1.570796327, 0, 0, 1.570796327, -1.570796327, 0];
|
||||
% for i = 1:6
|
||||
% L(i)=Link([0 d(i) a(i) alpha(i)]);
|
||||
% L(i).qlim=[-2*pi,2*pi];
|
||||
% end
|
||||
% UR5=SerialLink(L,'name','UR5');
|
||||
% UR5.teach();
|
||||
Loading…
Reference in New Issue