92 lines
3.4 KiB
Mathematica
92 lines
3.4 KiB
Mathematica
|
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
|