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 = [];
|
file = [];
|
||||||
opt.robot_def = 'direct';
|
opt.robot_def = 'direct';
|
||||||
opt.KM_method = 'MDH';
|
opt.KM_method = 'MDH';
|
||||||
|
opt.Vel_method = 'Direct';
|
||||||
|
opt.LD_method = 'Direct';
|
||||||
|
opt.debug = true;
|
||||||
|
|
||||||
|
|
||||||
|
opt.Isreal = false;
|
||||||
robot = get_robot(file,opt);
|
robot = get_robot(file,opt);
|
||||||
robot.theta = [1,0,0];
|
% robot.theta = [1,1,0];
|
||||||
robot = get_Kinematics(robot, opt);
|
robot = get_Kinematics(robot, opt);
|
||||||
|
|
||||||
|
opt.Isreal = false;
|
||||||
|
robot = get_velocity(robot, opt);
|
||||||
|
robot = get_regressor(robot,opt);
|
||||||
|
|
@ -1,4 +1,5 @@
|
||||||
function robot = get_Kinematics(robot, opt)
|
function robot = get_Kinematics(robot, opt)
|
||||||
|
if(opt.Isreal)
|
||||||
switch opt.KM_method
|
switch opt.KM_method
|
||||||
case 'SDH'
|
case 'SDH'
|
||||||
case 'MDH'
|
case 'MDH'
|
||||||
|
|
@ -27,3 +28,33 @@ switch opt.KM_method
|
||||||
disp('Bad opt.KM_method!')
|
disp('Bad opt.KM_method!')
|
||||||
return;
|
return;
|
||||||
end
|
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
|
||||||
29
get_robot.m
29
get_robot.m
|
|
@ -2,11 +2,13 @@ function robot = get_robot(file,opt)
|
||||||
switch opt.robot_def
|
switch opt.robot_def
|
||||||
case 'direct'
|
case 'direct'
|
||||||
ndof = 2;
|
ndof = 2;
|
||||||
|
robot.ndof = ndof;
|
||||||
% Kinematics parameters
|
% Kinematics parameters
|
||||||
|
if(opt.Isreal)
|
||||||
switch opt.KM_method
|
switch opt.KM_method
|
||||||
case 'SDH'
|
case 'SDH'
|
||||||
case 'MDH'
|
case 'MDH'
|
||||||
robot.theta = zeros([1,ndof+1]);
|
robot.theta = zeros([1,ndof]);
|
||||||
robot.a = [0,1,1];
|
robot.a = [0,1,1];
|
||||||
robot.d = [0,0,0];
|
robot.d = [0,0,0];
|
||||||
robot.alpha = [0,0,0];
|
robot.alpha = [0,0,0];
|
||||||
|
|
@ -14,6 +16,28 @@ switch opt.robot_def
|
||||||
disp('Bad opt.KM_method!')
|
disp('Bad opt.KM_method!')
|
||||||
return;
|
return;
|
||||||
end
|
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
|
% Dynamics parameters
|
||||||
link_mass = [1,1];
|
link_mass = [1,1];
|
||||||
axis_of_rot(:,:,1) = [0;0;1];
|
axis_of_rot(:,:,1) = [0;0;1];
|
||||||
|
|
@ -48,3 +72,6 @@ switch opt.robot_def
|
||||||
disp('Bad robot define options!')
|
disp('Bad robot define options!')
|
||||||
return
|
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