Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Feature/add regressor

This commit is contained in:
Rui 2024-01-23 13:42:25 +00:00
parent c734189836
commit 1830310cdc
18 changed files with 1071 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Identification_main.asv Normal file
View File

@ -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

19
Identification_main.m Normal file
View File

@ -0,0 +1,19 @@
file = [];
opt.robot_def = 'direct';
opt.KM_method = 'MDH';
opt.Vel_method = 'Direct';
opt.LD_method = 'Direct';
opt.debug = true;
opt.robotName = 'Two_bar';
opt.Isreal = false;
robot = get_robot(file,opt);
% robot.theta = [1,1,0];
robot = get_Kinematics(robot, opt);
opt.Isreal = false;
robot = get_velocity(robot, opt);
robot = get_regressor(robot,opt);
% symbol matched
verify_regressor
robot = get_baseParams(robot, opt);

24
autogen/base_regressor_Two_bar.m Normal file
View File

@ -0,0 +1,24 @@
function out1 = base_regressor_Two_bar(in1,in2,in3)
%base_regressor_Two_bar
% OUT1 = base_regressor_Two_bar(IN1,IN2,IN3)
% This function was generated by the Symbolic Math Toolbox version 9.1.
% 14-Jan-2024 21:45:43
q2 = in1(2,:);
qd1 = in2(1,:);
qd2 = in2(2,:);
qdd1 = in3(1,:);
qdd2 = in3(2,:);
t2 = cos(q2);
t3 = sin(q2);
t4 = qd1+qd2;
t5 = qdd1+qdd2;
t6 = qd1.^2;
t7 = qdd1.*t2;
t8 = qdd1.*t3;
t9 = t4.^2;
t10 = t2.*t6;
t11 = t3.*t6;
t12 = -t8;
out1 = reshape([t7+t11+t2.*t5-t3.*t9,t7+t11,t10+t12-t3.*t5-t2.*t9,t10+t12,t5,t5,qdd1,0.0],[2,4]);

26
autogen/standard_regressor_Two_bar.m Normal file
View File

@ -0,0 +1,26 @@
function out1 = standard_regressor_Two_bar(in1,in2,in3)
%standard_regressor_Two_bar
% OUT1 = standard_regressor_Two_bar(IN1,IN2,IN3)
% This function was generated by the Symbolic Math Toolbox version 9.1.
% 14-Jan-2024 21:45:42
q2 = in1(2,:);
qd1 = in2(1,:);
qd2 = in2(2,:);
qdd1 = in3(1,:);
qdd2 = in3(2,:);
t2 = cos(q2);
t3 = sin(q2);
t4 = qd1+qd2;
t5 = qdd1+qdd2;
t6 = qd1.^2;
t7 = qdd1.*t2;
t8 = qdd1.*t3;
t9 = t4.^2;
t10 = t2.*t6;
t11 = t3.*t6;
t12 = -t8;
t13 = t7+t11;
t14 = t10+t12;
out1 = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,qdd1,0.0,t2.*t13+t3.*(t8-t10),0.0,t13+t2.*t5-t3.*t9,t13,t14-t3.*t5-t2.*t9,t14,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t5,t5],[2,20]);

79
estimate_dyn.m Normal file
View File

@ -0,0 +1,79 @@
function robot = estimate_dyn(robot,opt)
% -------------------------------------------------------------------
% Get datas
% ------------------------------------------------------------------------
time = 0:0.01:2;
f=1;
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
q=[q_J;-q_J];
qd=[qd_J; -qd_J];
qdd=[qdd_J; -qdd_J];
g = [0; 0; -9.8];
tau = zeros([2,101]);
robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
robot_pi2=[2;1;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
robot_pi=[robot_pi1;robot_pi2];
for i = 1:length(q_J)
regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
tau(:,i)=regressor*robot_pi;
end
idntfcnTrjctry.t = time;
idntfcnTrjctry.q = q;
idntfcnTrjctry.qd = qd;
idntfcnTrjctry.qdd = qdd;
idntfcnTrjctry.tau = tau;
% -------------------------------------------------------------------
% Generate Regressors based on data
% ------------------------------------------------------------------------
drvGains = [];
baseQR = robot.baseQR;
[Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains);
% ---------------------------------------------------------------------
% Estimate parameters
% ---------------------------------------------------------------------
sol = struct;
method = 'OLS';
if strcmp(method, 'OLS')
% Usual least squares
[sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(Tau, Wb);
elseif strcmp(method, 'PC-OLS')
% Physically consistent OLS using SDP optimization
[sol.pi_b, sol.pi_fr, sol.pi_s] = physicallyConsistentEstimation(Tau, Wb, baseQR);
else
error("Chosen method for dynamic parameter estimation does not exist");
end
robot.sol = sol;
% Local unctions
function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains)
% The function builds observation matrix for UR10E
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
Wb = []; Tau = [];
for i = 1:1:length(idntfcnTrjctry.t)
% Yi = regressorWithMotorDynamics(idntfcnTrjctry.q(i,:)',...
% idntfcnTrjctry.qd(i,:)',...
% idntfcnTrjctry.q2d(i,:)');
% Yfrctni = frictionRegressor(idntfcnTrjctry.qd_fltrd(i,:)');
% Ybi = [Yi*E1, Yfrctni];
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i));
Wb = vertcat(Wb, Yb);
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
end
end
function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(Tau, Wb)
% Function perfroms ordinary least squares estimation of parameters
% pi_OLS = (Wb'*Wb)\(Wb'*Tau);
% pib_OLS = pi_OLS(1:40); % variables for base paramters
% pifrctn_OLS = pi_OLS(41:end);
pib_OLS=pinv(Wb)*Tau;
pifrctn_OLS = 0;
end
end

81
get_Kinematics.m Normal file
View File

@ -0,0 +1,81 @@
function robot = get_Kinematics(robot, opt)
if(opt.Isreal)
switch opt.KM_method
case 'SDH'
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))*cos(alpha(i)) sin(theta(i))*sin(alpha(i));...
sin(theta(i)) cos(theta(i))*cos(alpha(i)) -cos(theta(i))*sin(alpha(i));...
0 sin(alpha(i)) cos(alpha(i))];
robot.t(:,:,i) = [a(i)*cos(theta(i));a(i)*sin(theta(i));d(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
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

88
get_baseParams.m Normal file
View File

@ -0,0 +1,88 @@
function robot = get_baseParams(robot,opt)
% Seed the random number generator based on the current time
rng('shuffle');
ndof = robot.ndof;
includeMotorDynamics = false;
% ------------------------------------------------------------------------
% Set limits on posistion and velocities
% ------------------------------------------------------------------------
q_min = -pi*ones(ndof,1);
q_max = pi*ones(ndof,1);
qd_max = 3*pi*ones(ndof,1);
q2d_max = 6*pi*ones(ndof,1);
% -----------------------------------------------------------------------
% Find relation between independent columns and dependent columns
% -----------------------------------------------------------------------
% Get observation matrix of identifiable paramters
W = [];
for i = 1:25
q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1);
if includeMotorDynamics
% Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
else
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
end
W = vertcat(W,Y);
end
% QR decomposition with pivoting: W*E = Q*R
% R is upper triangular matrix
% Q is unitary matrix
% E is permutation matrix
[Q, R, E] = qr(W);
% matrix W has rank qr_rank which is number number of base parameters
qr_rank = rank(W);
% R = [R1 R2;
% 0 0]
% R1 is bbxbb upper triangular and reguar matrix
% R2 is bbx(c-qr_rank) matrix where c is number of standard parameters
R1 = R(1:qr_rank,1:qr_rank);
R2 = R(1:qr_rank,qr_rank+1:end);
beta = R1\R2; % the zero rows of K correspond to independent columns of WP
beta(abs(beta)<sqrt(eps)) = 0; % get rid of numerical errors
% W2 = W1*beta
% Make sure that the relation holds
W1 = W*E(:,1:qr_rank);
W2 = W*E(:,qr_rank+1:end);
assert(norm(W2 - W1*beta) < 1e-6,...
'Found realationship between W1 and W2 is not correct\n');
% -----------------------------------------------------------------------
% Find base parmaters
% -----------------------------------------------------------------------
pi_lgr_sym = robot.regressor.pi;
pi1 = E(:,1:qr_rank)'*pi_lgr_sym; % independent paramters
pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
% all of the expressions below are equivalent
pi_lgr_base = pi1 + beta*pi2;
% pi_lgr_base = [eye(qr_rank) beta]*[pi1;pi2];
% pi_lgr_base = [eye(qr_rank) beta]*E'*pi_lgr_sym;
% ---------------------------------------------------------------------
% Create structure with the result of QR decompositon a
% ---------------------------------------------------------------------
baseQR = struct;
baseQR.numberOfBaseParameters = qr_rank;
baseQR.permutationMatrix = E;
baseQR.beta = beta;
baseQR.motorDynamicsIncluded = includeMotorDynamics;
baseQR.baseParams = pi_lgr_base;
robot.baseQR = baseQR;
% ---------------------------------------------------------------------
% Gen base_regressor
% ---------------------------------------------------------------------
q_sym = sym('q%d',[ndof+1,1],'real');
qd_sym = sym('qd%d',[ndof+1,1],'real');
q2d_sym = sym('qdd%d',[ndof+1,1],'real');
robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
matlabFunction(robot.baseQR.regressor,'File',sprintf('autogen/base_regressor_%s',opt.robotName),...
'Vars',{q_sym,qd_sym,q2d_sym});

92
get_rb_dynamics.m Normal file
View File

@ -0,0 +1,92 @@
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

65
get_regressor.asv Normal file
View File

@ -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

84
get_regressor.m Normal file
View File

@ -0,0 +1,84 @@
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('qdd%d',[ndof+1,1],'real');
% init regressor
robot.regressor.m = sym('m%d',[ndof,1],'real');
robot.regressor.mc_x = sym('mc%d_x',[ndof,1],'real');
robot.regressor.mc_y = sym('mc%d_y',[ndof,1],'real');
robot.regressor.mc_z = sym('mc%d_z',[ndof,1],'real');
robot.regressor.ixx = sym('i%d_xx',[ndof,1],'real');
robot.regressor.ixy = sym('i%d_xy',[ndof,1],'real');
robot.regressor.ixz = sym('i%d_xz',[ndof,1],'real');
robot.regressor.iyy = sym('i%d_yy',[ndof,1],'real');
robot.regressor.iyz = sym('i%d_yz',[ndof,1],'real');
robot.regressor.izz = sym('i%d_zz',[ndof,1],'real');
robot.regressor.im = sym('im%d',[ndof,1],'real');
for i = 1:ndof
robot.regressor.pi(:,i) = [robot.regressor.m(i),robot.regressor.mc_x(i),robot.regressor.mc_y(i),...
robot.regressor.mc_z(i),robot.regressor.ixx(i),robot.regressor.ixy(i),...
robot.regressor.ixz(i),robot.regressor.iyy(i),robot.regressor.iyz(i),robot.regressor.izz(i)]';
end
[nLnkPrms, nLnks] = size(robot.regressor.pi);
robot.regressor.pi = reshape(robot.regressor.pi, [nLnkPrms*nLnks, 1]);
% 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 = 1:ndof
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) = [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
delta_(i,6*i) = 1;
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(robot.regressor.K,'File',sprintf('autogen/standard_regressor_%s',opt.robotName),...
'Vars',{q_sym,qd_sym,q2d_sym});
% 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

188
get_rne_mdh.m Normal file
View File

@ -0,0 +1,188 @@
function tau = get_rne_mdh(robot, a1, a2, a3, a4, a5)
z0 = [0;0;1];
grav = robot.gravity; % default gravity from the object
fext = zeros(6, 1);
% Set debug to:
% 0 no messages
% 1 display results of forward and backward recursions
% 2 display print R and p*
debug = 0;
n = robot.n;
if numcols(a1) == 3*n
Q = a1(:,1:n);
Qd = a1(:,n+1:2*n);
Qdd = a1(:,2*n+1:3*n);
np = numrows(Q);
if nargin >= 3,
grav = a2(:);
end
if nargin == 4
fext = a3;
end
else
np = numrows(a1);
Q = a1;
Qd = a2;
Qdd = a3;
if numcols(a1) ~= n || numcols(Qd) ~= n || numcols(Qdd) ~= n || ...
numrows(Qd) ~= np || numrows(Qdd) ~= np
error('bad data');
end
if nargin >= 5,
grav = a4(:);
end
if nargin == 6
fext = a5;
end
end
if robot.issym || any([isa(Q,'sym'), isa(Qd,'sym'), isa(Qdd,'sym')])
tau = zeros(np,n, 'sym');
else
tau = zeros(np,n);
end
for p=1:np
q = Q(p,:).';
qd = Qd(p,:).';
qdd = Qdd(p,:).';
Fm = [];
Nm = [];
pstarm = [];
Rm = [];
w = zeros(3,1);
wd = zeros(3,1);
vd = grav(:);
%
% init some variables, compute the link rotation matrices
%
for j=1:n
link = robot.links(j);
Tj = link.A(q(j));
switch link.type
case 'R'
D = link.d;
case 'P'
D = q(j);
end
alpha = link.alpha;
pm = [link.a; -D*sin(alpha); D*cos(alpha)]; % (i-1) P i
if j == 1
pm = t2r(robot.base) * pm;
Tj = robot.base * Tj;
end
Pm(:,j) = pm;
Rm{j} = t2r(Tj);
if debug>1
Rm{j}
Pm(:,j).'
end
end
%
% the forward recursion
%
for j=1:n
link = robot.links(j);
R = Rm{j}.'; % transpose!!
P = Pm(:,j);
Pc = link.r;
%
% trailing underscore means new value
%
switch link.type
case 'R'
% revolute axis
w_ = R*w + z0*qd(j);
wd_ = R*wd + cross(R*w,z0*qd(j)) + z0*qdd(j);
%v = cross(w,P) + R*v;
vd_ = R * (cross(wd,P) + ...
cross(w, cross(w,P)) + vd);
case 'P'
% prismatic axis
w_ = R*w;
wd_ = R*wd;
%v = R*(z0*qd(j) + v) + cross(w,P);
vd_ = R*(cross(wd,P) + ...
cross(w, cross(w,P)) + vd ...
) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j);
end
% update variables
w = w_;
wd = wd_;
vd = vd_;
vdC = cross(wd,Pc).' + ...
cross(w,cross(w,Pc)).' + vd;
F = link.m*vdC;
N = link.I*wd + cross(w,link.I*w);
Fm = [Fm F];
Nm = [Nm N];
if debug
fprintf('w: '); fprintf('%.3f ', w)
fprintf('\nwd: '); fprintf('%.3f ', wd)
fprintf('\nvd: '); fprintf('%.3f ', vd)
fprintf('\nvdbar: '); fprintf('%.3f ', vdC)
fprintf('\n');
end
end
%
% the backward recursion
%
fext = fext(:);
f = fext(1:3); % force/moments on end of arm
nn = fext(4:6);
for j=n:-1:1
%
% order of these statements is important, since both
% nn and f are functions of previous f.
%
link = robot.links(j);
if j == n
R = eye(3,3);
P = [0;0;0];
else
R = Rm{j+1};
P = Pm(:,j+1); % i/P/(i+1)
end
Pc = link.r;
f_ = R*f + Fm(:,j);
nn_ = Nm(:,j) + R*nn + cross(Pc,Fm(:,j)).' + ...
cross(P,R*f);
f = f_;
nn = nn_;
if debug
fprintf('f: '); fprintf('%.3f ', f)
fprintf('\nn: '); fprintf('%.3f ', nn)
fprintf('\n');
end
switch link.type
case 'R'
% revolute
tau(p,j) = nn.'*z0 + ...
link.G^2 * link.Jm*qdd(j) - ...
friction(link, qd(j));
case 'P'
% prismatic
tau(p,j) = f.'*z0 + ...
link.G^2 * link.Jm*qdd(j) - ...
friction(link, qd(j));
end
end
end

52
get_robot.asv Normal file
View File

@ -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

77
get_robot.m Normal file
View File

@ -0,0 +1,77 @@
function robot = get_robot(file,opt)
switch opt.robot_def
case 'direct'
ndof = 2;
robot.ndof = ndof;
% Kinematics parameters
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('qdd%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];
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
%Gravity
gravity = [0;0;9.8];
robot.gravity = gravity;

31
get_velocity.asv Normal file
View File

@ -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

43
get_velocity.m Normal file
View File

@ -0,0 +1,43 @@
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

28
test_kinematics.m Normal file
View File

@ -0,0 +1,28 @@
mdh = 1;
if mdh==1
% theta d a alpha offset
L1=Link('revolute', 'd', 0, 'a', 0, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,40]),'modified');
L2=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'modified');
L3=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'modified');
Two_bar=SerialLink([L1 L2 L3],'name','Two_bar'); %连接连杆
Two_bar.teach();
% Two_bar.plot([0 0])%机械臂图
else
% theta d a alpha offset
L1=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,40]),'standard');
L2=Link('revolute', 'd', 0, 'a', 1, 'alpha', 0, 'offset',0,'qlim',deg2rad([-20,45]),'standard');
Two_bar=SerialLink([L1 L2],'name','Two_bar'); %连接连杆
% Two_bar.plot([0 0])%机械臂图
Two_bar.teach();
end
% a = [0, -0.42500, -0.39225, 0, 0, 0];
% d = [0.0892, 0, 0, 0.10915, 0.09465, 0.0823];
% alpha = [1.570796327, 0, 0, 1.570796327, -1.570796327, 0];
% for i = 1:6
% L(i)=Link([0 d(i) a(i) alpha(i)]);
% L(i).qlim=[-2*pi,2*pi];
% end
% UR5=SerialLink(L,'name','UR5');
% UR5.teach();

5
vec2linearSymMat.m Normal file
View File

@ -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

79
verify_regressor.m Normal file
View File

@ -0,0 +1,79 @@
% function robot = verify_regressor(robot, opt)
% verify: If full regressor dynamics is the same as basic dynamics
ndof = robot.ndof;
q_sym = sym('q%d',[ndof+1,1],'real');
qd_sym = sym('qd%d',[ndof+1,1],'real');
q2d_sym = sym('qdd%d',[ndof+1,1],'real');
pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
% pi2=zeros([10,1]);
pi=[pi1;pi2];
regressor = standard_regressor_Two_bar(q_sym,qd_sym,q2d_sym);
tau=regressor*pi;
%% Two-bar
N=2;
thetalist = q_sym(1:N);
dthetalist = qd_sym(1:N);
ddthetalist = q2d_sym(1:N);
Gb= [diag([1,1,1]),zeros(3,3);
zeros(3,3),diag([1,1,1])];
Glist = cat(3, Gb, Gb);
% Glist = cat(3, Gb, zeros([6,6]));
M01 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M23 = [[1, 0, 0, 1/2]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist = cat(3, M01, M12, M23);
Slist=[[0;0;1;0;0;0],...
[0;0;1;0;-1;0]];
Adgab_mat = sym(zeros(6,6,N+1));
Fmat=sym(zeros(N,6));
F1=sym(zeros(N,6));
V1=sym(zeros(6,N+1));
G=sym(zeros(4,4,N));
T=sym(zeros(4,4,N));
Vlinear=sym(zeros(3,3));
Vd1=sym(zeros(6,N+1));
Gb= [diag([1,1,1]),zeros(3,3);
zeros(3,3),diag([1,1,1])];
J=sym(zeros(6,N));
exf=[0;0;0;0;0;0];
[V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
= InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
[0;0;0], exf, Mlist, Glist, Slist);
G = FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M12 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
M23 = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
Mlist = cat(3, M01, M12, M23);
T=FKinSpaceExpand_Sym(Mlist, Slist, thetalist);
F_Simpack = getSimpackF_Sym(G,T,Mlist,Fmat);
% Use Body Twist cal linear vel, but can't cal the end frame vel
[V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
[0;0;0], exf, Mlist, Glist, Slist);
j=1;
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
j=2;
Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
%% Numerical
clear pi;
ndof = robot.ndof;
time = 0:0.01:2;
f=1;
q_J = sin(2*pi*f*time);
qd_J = (2*pi*f)*cos(2*pi*f*time);
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
q=[q_J;-q_J];
qd=[qd_J; -qd_J];
qdd=[qdd_J; -qdd_J];
g = [0; 0; -9.8];
robot_pi1=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
robot_pi2=[1;1/2;0;0;1+1/4;0;0;1+1/4;0;1+1/4];
% pi2=zeros([10,1]);
robot_pi=[robot_pi1;robot_pi2];
tau = zeros([2,100]);
for i = 1:length(q_J)
regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
tau(:,i)=regressor*robot_pi;
end