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