120 lines
5.4 KiB
Matlab
120 lines
5.4 KiB
Matlab
function robot = get_velocity(robot, opt)
|
|
switch opt.KM_method
|
|
case 'SCREW'
|
|
Mlist_CG = robot.kine.Mlist_CG;
|
|
% Get general mass matrix
|
|
link_mass = robot.m;
|
|
com_pos = robot.com;
|
|
link_inertia = robot.I;
|
|
Slist=robot.slist;
|
|
Glist=[];
|
|
for i = 1:robot.ndof
|
|
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
|
Glist = cat(3, Glist, Gb);
|
|
% ?
|
|
% mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
|
|
% robot.pi(2:4,i) = mc;
|
|
% hack: because we only know the com in the world frame
|
|
% the inv of rotation is beacause we want to the com in
|
|
% our defined frame. R*com is the wrong result
|
|
robot.pi(2:4,i) = robot.m(i)*robot.Home.R(:,:,i)'*robot.com(:,i);
|
|
com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
|
|
% get joint inertial
|
|
robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
|
|
robot.I(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
|
|
robot.I_vec(:,i) = inertiaMatrix2Vector(robot.I(:,:,i));
|
|
robot.pi(5:end,i) = robot.I_vec(:,i);
|
|
end
|
|
if opt.Isreal
|
|
% init q
|
|
q = robot.theta;
|
|
qd = robot.dtheta;
|
|
qdd = robot.ddtheta;
|
|
[V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ...
|
|
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
|
|
else
|
|
% init q
|
|
q = robot.theta;
|
|
qd = robot.dtheta;
|
|
qdd = robot.ddtheta;
|
|
[V,Vd,~,~,~] = InverseDynamics_sym(q, qd, qdd, ...
|
|
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
|
|
end
|
|
% FIXME: twist is not equal to velocity
|
|
% FIXME: Need to get the velocity represent at base frame
|
|
robot.vel.w = V(1:3,2:end);
|
|
robot.vel.dw = Vd(1:3,2:end);
|
|
% We need the vel at COM but we want to know vel at joint origin
|
|
for i = 1:robot.ndof
|
|
robot.vel.v(:,i) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.w(:,i)+V(4:6,i+1);
|
|
robot.vel.dv(:,i) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.dw(:,i)+Vd(4:6,i+1);
|
|
end
|
|
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);
|
|
link_type = robot.link_type;
|
|
% init q
|
|
q = robot.theta;
|
|
qd = robot.dtheta;
|
|
qdd = robot.ddtheta;
|
|
% for i = 1:robot.ndof
|
|
% switch link_type(i)
|
|
% case 'R'
|
|
% %Do nothing
|
|
% case 'P'
|
|
% q(i) = robot.d(i);
|
|
% qd(i) = robot.vd(i);
|
|
% qdd(i) = robot.accd(i);
|
|
% end
|
|
% end
|
|
R = robot.kine.R;
|
|
P = robot.kine.t;
|
|
% 1-n外推公式 参考robotics toolbox
|
|
%第一关节
|
|
switch link_type(1)
|
|
case 'R' %revolute
|
|
w(:,1) = R(:,:,1)' * w0 + qd(1) * Z;
|
|
v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
|
|
dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, qd(1) * Z) + qdd(1) * Z;
|
|
dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0);
|
|
case 'P' %prismatic
|
|
w(:,1) = R(:,:,1)' * w0;
|
|
v(:,1) = R(:,:,1)' * (Z*qd(1)+v0) + cross(w0, P(:,1));
|
|
dw(:,1) = R(:,:,1)' * dw0;
|
|
dv(:,1) = R(:,:,1)' * (cross(dw0, P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0)+...
|
|
2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1);
|
|
end
|
|
%后面n-1关节
|
|
for i = 1:robot.ndof-1
|
|
% switch link_type(i)
|
|
% case 'R' %revolute
|
|
w(:,i+1) = R(:,:,i+1)' * w(:,i) + qd(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), qd(i+1) * Z)+ qdd(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));
|
|
% case 'P' %prismatic
|
|
% w(:,i+1) = R(:,:,i+1)' * w(:,i);
|
|
% v(:,i+1) = R(:,:,i+1)' * (Z*qd(:,i)+v(:,i)) + cross(w(:,i), P(:,i+1));
|
|
% dw(:,i+1) = R(:,:,i+1)' * dw(:,i);
|
|
% dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i))+...
|
|
% 2*cross(R(:,:,i+1)' * w(:,i), Z * qd(:,i)) + Z * qdd(:,i);
|
|
% end
|
|
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
|