44 lines
1.7 KiB
Mathematica
44 lines
1.7 KiB
Mathematica
|
function robot = get_velocity(robot, opt)
|
||
|
|
switch opt.KM_method
|
||
|
|
case 'SDH'
|
||
|
|
case 'MDH'
|
||
|
|
switch opt.Vel_method
|
||
|
|
case 'Direct'
|
||
|
|
Z = [0,0,1]';
|
||
|
|
w0 = zeros(3,1); dw0 = zeros(3,1);
|
||
|
|
% dv0 = robot.gravity;
|
||
|
|
v0 = zeros(3,1);dv0 = zeros(3,1);
|
||
|
|
% w = zeros(3,number);
|
||
|
|
% dw = zeros(3,number);
|
||
|
|
% dv = zeros(3,number);
|
||
|
|
theta = robot.theta;
|
||
|
|
dtheta = robot.dtheta;
|
||
|
|
ddtheta = robot.ddtheta;
|
||
|
|
R = robot.R;
|
||
|
|
P = robot.t;
|
||
|
|
% 1-n外推公式
|
||
|
|
%第一关节
|
||
|
|
w(:,1) = R(:,:,1)' * w0 + dtheta(1) * Z;
|
||
|
|
v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
|
||
|
|
dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, dtheta(1) * Z) + ddtheta(1) * Z;
|
||
|
|
dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0,cross(w0, P(:,1))) + dv0);
|
||
|
|
%后面n-1关节
|
||
|
|
for i = 1:robot.ndof
|
||
|
|
w(:,i+1) = R(:,:,i+1)' * w(:,i) + dtheta(i+1) * Z ;
|
||
|
|
v(:,i+1) = R(:,:,i+1)' * v(:,i) + cross(w(:,i), P(:,i+1));
|
||
|
|
dw(:,i+1) = R(:,:,i+1)' * dw(:,i) + cross(R(:,:,i+1)' * w(:,i), dtheta(i+1) * Z)+ ddtheta(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));
|
||
|
|
end
|
||
|
|
robot.vel.w = w;
|
||
|
|
robot.vel.v = v;
|
||
|
|
robot.vel.dw = dw;
|
||
|
|
robot.vel.dv = dv;
|
||
|
|
otherwise
|
||
|
|
disp('Bad opt.Vel_method!')
|
||
|
|
return;
|
||
|
|
end
|
||
|
|
otherwise
|
||
|
|
disp('Bad opt.KM_method!')
|
||
|
|
return;
|
||
|
|
end
|