robot.regressor.K have bug and the result need to verify
This commit is contained in:
parent
059831c913
commit
af96a4b64f
|
|
@ -0,0 +1,10 @@
|
|||
file = [];
|
||||
opt.robot_def = 'direct';
|
||||
opt.KM_method = 'MDH';
|
||||
opt.LD_method = 'direct';
|
||||
|
||||
robot = get_robot(file,opt);
|
||||
robot.theta = [1,1,0];
|
||||
robot = get_Kinematics(robot, opt);
|
||||
|
||||
robot = get
|
||||
|
|
@ -1,6 +1,16 @@
|
|||
file = [];
|
||||
opt.robot_def = 'direct';
|
||||
opt.KM_method = 'MDH';
|
||||
opt.Vel_method = 'Direct';
|
||||
opt.LD_method = 'Direct';
|
||||
opt.debug = true;
|
||||
|
||||
|
||||
opt.Isreal = false;
|
||||
robot = get_robot(file,opt);
|
||||
robot.theta = [1,0,0];
|
||||
% robot.theta = [1,1,0];
|
||||
robot = get_Kinematics(robot, opt);
|
||||
|
||||
opt.Isreal = false;
|
||||
robot = get_velocity(robot, opt);
|
||||
robot = get_regressor(robot,opt);
|
||||
|
|
@ -1,29 +1,60 @@
|
|||
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;
|
||||
if(opt.Isreal)
|
||||
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
|
||||
else
|
||||
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 = sym(zeros([3,3,ndof]));
|
||||
robot.t = sym(zeros([3,1,ndof]));
|
||||
robot.T = sym(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 = sym(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
|
||||
end
|
||||
|
|
@ -0,0 +1,65 @@
|
|||
function robot = get_regressor(robot, opt)
|
||||
% Create symbolic generilized coordiates, their first and second deriatives
|
||||
ndof = robot.ndof;
|
||||
q_sym = sym('q%d',[ndof+1,1],'real');
|
||||
qd_sym = sym('qd%d',[ndof+1,1],'real');
|
||||
q2d_sym = sym('q2d%d',[ndof+1,1],'real');
|
||||
% init regressor
|
||||
robot.regressor.m = sym('m%d',[ndof,1],'real');
|
||||
robot.regressor.com = sym('com%d',[ndof,1],'real');
|
||||
robot.regressor.I = sym('I%d',[ndof,1],'real');
|
||||
robot.regressor.I_vec = inertiaMatrix2Vector(robot.regressor.I);
|
||||
robot.regressor.mc = robot.regressor.m.*robot.regressor.com;
|
||||
robot.regressor.pi = [robot.I_vec(:,i); robot.mc(:,i); robot.m(i)];
|
||||
% init matrix
|
||||
R = robot.R;
|
||||
P = robot.t;
|
||||
w = robot.vel.w ;
|
||||
dw = robot.vel.dw ;
|
||||
dv = robot.vel.dv ;
|
||||
switch opt.LD_method
|
||||
case 'Direct'
|
||||
switch opt.KM_method
|
||||
case 'MDH'
|
||||
for i = 2:ndof+1
|
||||
p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
|
||||
w_skew(:,:,i) = vec2skewSymMat(w(:,i));
|
||||
dw_skew(:,:,i) = vec2skewSymMat(dw(:,i));
|
||||
dv_skew(:,:,i) = vec2skewSymMat(dv(:,i));
|
||||
w_l(:,:,i) = vec2linearSymMat(w(:,i));
|
||||
dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
|
||||
% size of matrix A is 6*10, need to -1
|
||||
robot.regressor.A(:,:,i-1) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
|
||||
zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
|
||||
end
|
||||
% construct matrix U, size: [6*n,10*n]
|
||||
% U_ = sym(zeros([6*ndof,10*ndof]));
|
||||
U_ = [];
|
||||
for i = 1:ndof
|
||||
% tricky
|
||||
for j = i:ndof
|
||||
if(j == i)
|
||||
TT = eye(6,6);
|
||||
U_row = TT*robot.regressor.A(:,:,j);
|
||||
else
|
||||
TT = TT*Adjoint(RpToTrans(R(:,:,j),P(:,:,j)));
|
||||
U_row = [U_row,TT*robot.regressor.A(:,:,j)];
|
||||
end
|
||||
end
|
||||
U_ = [U_;zeros(6,(i-1)*10),U_row];
|
||||
end
|
||||
robot.regressor.U = U_;
|
||||
if(opt.debug)
|
||||
sprintf('size of U_=%dx%d.',size(robot.regressor.U));
|
||||
end
|
||||
robot.regressor.K = [zeros(1,3),Z0]*;
|
||||
end
|
||||
% matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
|
||||
% 'Vars',{q_sym,qd_sym,q2d_sym});
|
||||
case 'Lagrange'
|
||||
disp('TODO opt.LD_method Lagrange!')
|
||||
return;
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
function robot = get_regressor(robot, opt)
|
||||
% Create symbolic generilized coordiates, their first and second deriatives
|
||||
ndof = robot.ndof;
|
||||
q_sym = sym('q%d',[ndof+1,1],'real');
|
||||
qd_sym = sym('qd%d',[ndof+1,1],'real');
|
||||
q2d_sym = sym('q2d%d',[ndof+1,1],'real');
|
||||
% init regressor
|
||||
% robot.regressor.m = sym('m%d',[ndof,1],'real');
|
||||
% robot.regressor.com = sym('com%d',[ndof,1],'real');
|
||||
% robot.regressor.I = sym('I%d',[ndof,1],'real');
|
||||
% robot.regressor.I_vec = inertiaMatrix2Vector(robot.regressor.I);
|
||||
% robot.regressor.mc = robot.regressor.m.*robot.regressor.com;
|
||||
% robot.regressor.pi = [robot.I_vec(:,i); robot.mc(:,i); robot.m(i)];
|
||||
% init matrix
|
||||
R = robot.R;
|
||||
P = robot.t;
|
||||
w = robot.vel.w ;
|
||||
dw = robot.vel.dw ;
|
||||
dv = robot.vel.dv ;
|
||||
switch opt.LD_method
|
||||
case 'Direct'
|
||||
switch opt.KM_method
|
||||
case 'MDH'
|
||||
for i = 2:ndof+1
|
||||
p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
|
||||
w_skew(:,:,i) = vec2skewSymMat(w(:,i));
|
||||
dw_skew(:,:,i) = vec2skewSymMat(dw(:,i));
|
||||
dv_skew(:,:,i) = vec2skewSymMat(dv(:,i));
|
||||
w_l(:,:,i) = vec2linearSymMat(w(:,i));
|
||||
dw_l(:,:,i) = vec2linearSymMat(dw(:,i));
|
||||
% size of matrix A is 6*10, need to -1
|
||||
robot.regressor.A(:,:,i-1) = [dv(:,i),dw_skew(:,:,i)+w_skew(:,:,i)*w_skew(:,:,i),zeros(3,6); ...
|
||||
zeros(3,1),-dv_skew(:,:,i),dw_l(:,:,i)+w_skew(:,:,i)*w_l(:,:,i)];
|
||||
end
|
||||
% construct matrix U, size: [6*n,10*n]
|
||||
% U_ = sym(zeros([6*ndof,10*ndof]));
|
||||
U_ = [];
|
||||
for i = 1:ndof
|
||||
% tricky
|
||||
for j = i:ndof
|
||||
if(j == i)
|
||||
TT = eye(6,6);
|
||||
U_row = TT*robot.regressor.A(:,:,j);
|
||||
else
|
||||
TT = TT*Adjoint(RpToTrans(R(:,:,j),P(:,:,j)));
|
||||
U_row = [U_row,TT*robot.regressor.A(:,:,j)];
|
||||
end
|
||||
end
|
||||
U_ = [U_;zeros(6,(i-1)*10),U_row];
|
||||
end
|
||||
robot.regressor.U = U_;
|
||||
delta_ = zeros([ndof,6*ndof]);
|
||||
for i = 1:ndof
|
||||
for j = 1:ndof
|
||||
delta_(i,6*j) = 1;
|
||||
end
|
||||
end
|
||||
robot.regressor.K = delta_*robot.regressor.U;
|
||||
if(opt.debug)
|
||||
sprintf('size of U=%dx%d.',size(robot.regressor.U));
|
||||
sprintf('size of K=%dx%d.',size(robot.regressor.K));
|
||||
end
|
||||
end
|
||||
% matlabFunction(Y_f,'File','autogen/standard_regressor_Two_bar',...
|
||||
% 'Vars',{q_sym,qd_sym,q2d_sym});
|
||||
case 'Lagrange'
|
||||
disp('TODO opt.LD_method Lagrange!')
|
||||
return;
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
function robot = get_robot(file,opt)
|
||||
switch opt.robot_def
|
||||
case 'direct'
|
||||
ndof = 2;
|
||||
% Kinematics parameters
|
||||
switch opt.Isreal
|
||||
case
|
||||
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
|
||||
49
get_robot.m
49
get_robot.m
|
|
@ -2,17 +2,41 @@ function robot = get_robot(file,opt)
|
|||
switch opt.robot_def
|
||||
case 'direct'
|
||||
ndof = 2;
|
||||
robot.ndof = ndof;
|
||||
% 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;
|
||||
if(opt.Isreal)
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.theta = zeros([1,ndof]);
|
||||
robot.a = [0,1,1];
|
||||
robot.d = [0,0,0];
|
||||
robot.alpha = [0,0,0];
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
else
|
||||
% Create symbolic generilized coordiates, their first and second deriatives
|
||||
q_sym = sym('q%d',[ndof+1,1],'real');
|
||||
qd_sym = sym('qd%d',[ndof+1,1],'real');
|
||||
q2d_sym = sym('q2d%d',[ndof+1,1],'real');
|
||||
q_sym(ndof+1) = 0;
|
||||
qd_sym(ndof+1) = 0;
|
||||
q2d_sym(ndof+1) = 0;
|
||||
robot.theta = q_sym;
|
||||
robot.dtheta = qd_sym;
|
||||
robot.ddtheta = q2d_sym;
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.a = [0,1,1];
|
||||
robot.d = [0,0,0];
|
||||
robot.alpha = [0,0,0];
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
end
|
||||
% Dynamics parameters
|
||||
link_mass = [1,1];
|
||||
|
|
@ -47,4 +71,7 @@ switch opt.robot_def
|
|||
robot = [];
|
||||
disp('Bad robot define options!')
|
||||
return
|
||||
end
|
||||
end
|
||||
%Gravity
|
||||
gravity = [0;0;9.8];
|
||||
robot.gravity = gravity;
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
function robot = get_velocity(robot, opt)
|
||||
switch opt.Vel_method
|
||||
case 'Direct'
|
||||
Z = [0,0,1]';
|
||||
w0 = zeros(3,1); dw0 = zeros(3,1);dv0 = robot.gravity;
|
||||
% 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;
|
||||
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 ;
|
||||
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.dw = dw;
|
||||
robot.vdv = dv;
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
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;
|
||||
% 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;
|
||||
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 ;
|
||||
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.dw = dw;
|
||||
robot.vel.dv = dv;
|
||||
otherwise
|
||||
disp('Bad opt.Vel_method!')
|
||||
return;
|
||||
end
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
function matrix = vec2linearSymMat(w)
|
||||
matrix = [w(1),w(2),w(3),zeros(1,3);...
|
||||
0,w(1),0,w(2),w(3),0;...
|
||||
0,0,w(1),0,w(2),w(3)];
|
||||
end
|
||||
Loading…
Reference in New Issue