405 lines
23 KiB
Mathematica
405 lines
23 KiB
Mathematica
|
function [M,C,G,Y] = screw_dynamics22(q,q_d,q_2d,ur10)
|
||
|
|
|
||
|
|
gamma0 = [0, 0, 9.81, 0, 0, 0]'; %gravity acceleration vector
|
||
|
|
|
||
|
|
T_pk = zeros(4,4,6); %homogenous transformation from p to k
|
||
|
|
invAd_pk = zeros(6,6,6); %inverse of adjoint matrix from p to k
|
||
|
|
invAd_0k = zeros(6,6,7); invAd_0k(:,:,1) = eye(6,6);
|
||
|
|
Jk = zeros(6,6,7); %jacobian up to body k
|
||
|
|
|
||
|
|
adj_pk = zeros(6,6,6);
|
||
|
|
adj_0k = zeros(6,6,6);
|
||
|
|
xi_k = zeros(6,6);
|
||
|
|
v_k = zeros(6,7);
|
||
|
|
Jk_d = zeros(6,6,7);
|
||
|
|
FI_k = zeros(6,6,6);
|
||
|
|
|
||
|
|
M = zeros(6,6);
|
||
|
|
C = zeros(6,6);
|
||
|
|
G = zeros(6,1);
|
||
|
|
g2 = zeros(6,1);
|
||
|
|
M1 = zeros(6,1);
|
||
|
|
|
||
|
|
R1 = [];
|
||
|
|
R2 = [];
|
||
|
|
Lmda = [];
|
||
|
|
|
||
|
|
for i = 1:1:6
|
||
|
|
aor_k = str2num(ur10.robot.joint{i}.axis.Attributes.xyz)';
|
||
|
|
R_pk = RPY(str2num(ur10.robot.joint{i}.origin.Attributes.rpy))*Rot(q(i),aor_k);
|
||
|
|
p_pk = str2num(ur10.robot.joint{i}.origin.Attributes.xyz)';
|
||
|
|
T_pk(:,:,i) = [R_pk, p_pk; zeros(1,3), 1];
|
||
|
|
|
||
|
|
invAd_pk(:,:,i) = inv_Ad_transf(T_pk(:,:,i));
|
||
|
|
invAd_0k(:,:,i+1) = invAd_pk(:,:,i)*invAd_0k(:,:,i);
|
||
|
|
Jk(:,:,i+1) = invAd_pk(:,:,i)*Jk(:,:,i) + ur10.XI(:,:,i);
|
||
|
|
|
||
|
|
xi_k(:,i) = ur10.XI(:,:,i)*q_d;
|
||
|
|
v_k(:,i+1) = invAd_pk(:,:,i)*v_k(:,i) + xi_k(:,i);
|
||
|
|
adj_pk(:,:,i) = adj_transf(xi_k(:,i));
|
||
|
|
adj_0k(:,:,i) = adj_transf(v_k(:,i+1));
|
||
|
|
FI_k(:,:,i) = ur10.Lmbd_k(:,:,i)*adj_0k(:,:,i) - adj_0k(:,:,i)'*ur10.Lmbd_k(:,:,i);
|
||
|
|
Jk_d(:,:,i+1) = invAd_pk(:,:,i)*Jk_d(:,:,i) - adj_pk(:,:,i)*invAd_pk(:,:,i)*Jk(:,:,i);
|
||
|
|
|
||
|
|
M = M + Jk(:,:,i+1)'*ur10.Lmbd_k(:,:,i)*Jk(:,:,i+1);
|
||
|
|
|
||
|
|
C = C + Jk(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + ur10.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) );
|
||
|
|
|
||
|
|
gamma_k = invAd_0k(:,:,i+1)*gamma0; %body gravitational acceleration
|
||
|
|
G = G + Jk(:,:,i+1)'*ur10.Lmbd_k(:,:,i)*gamma_k;
|
||
|
|
% --------------------------------------------------------------------
|
||
|
|
% t1 = gamma_k;
|
||
|
|
% t2 = str2num(ur10.robot.link{i+1}.inertial.origin.Attributes.xyz)';
|
||
|
|
% A = [t1(1:3), vec2skewSymMat(t1(4:6)), zeros(3,6);
|
||
|
|
% zeros(3,1), -vec2skewSymMat(t1(1:3)) + vec2skewSymMat(t2)*vec2skewSymMat(t1(4:6)),...
|
||
|
|
% vec2mat_ssmat(t1(4:6))];
|
||
|
|
%
|
||
|
|
% dpi_k(:,:,i) = Jk(:,:,i+1)'*A;
|
||
|
|
% g2 = g2 + dpi_k(:,:,i)*ur10.pi_reg(:,i);
|
||
|
|
%
|
||
|
|
% t3 = Jk(:,1,i+1);
|
||
|
|
% A = [t3(1:3), vec2skewSymMat(t3(4:6)), zeros(3,6);
|
||
|
|
% zeros(3,1), -vec2skewSymMat(t3(1:3)) + vec2skewSymMat(t2)*vec2skewSymMat(t3(4:6)),...
|
||
|
|
% vec2mat_ssmat(t3(4:6))];
|
||
|
|
% M1 = M1 + Jk(:,:,i+1)'*A*ur10.pi_reg(:,i);
|
||
|
|
|
||
|
|
|
||
|
|
% -------------------------------------------------------------------
|
||
|
|
% Estimateing Regressor
|
||
|
|
% -------------------------------------------------------------------
|
||
|
|
r_k = str2num(ur10.robot.link{i+1}.inertial.origin.Attributes.xyz)';
|
||
|
|
alpha_k = Jk(:,:,i+1)*q_2d + adj_0k(:,:,i)*Jk(:,:,i+1)*q_d + Jk_d(:,:,i+1)*q_d + gamma_k;
|
||
|
|
|
||
|
|
A1 = [alpha_k(1:3), vec2skewSymMat(alpha_k(4:6)), zeros(3,6);
|
||
|
|
zeros(3,1), -vec2skewSymMat(alpha_k(1:3)) + vec2skewSymMat(r_k)*vec2skewSymMat(alpha_k(4:6)),...
|
||
|
|
vec2mat_ssmat(alpha_k(4:6))];
|
||
|
|
|
||
|
|
t4 = Jk(:,:,i+1)*q_d;
|
||
|
|
|
||
|
|
A2 = [t4(1:3), vec2skewSymMat(t4(4:6)), zeros(3,6);
|
||
|
|
zeros(3,1), -vec2skewSymMat(t4(1:3)) + vec2skewSymMat(r_k)*vec2skewSymMat(t4(4:6)),...
|
||
|
|
vec2mat_ssmat(t4(4:6))];
|
||
|
|
|
||
|
|
t4 = A1 - adj_0k(:,:,i)'*A2;
|
||
|
|
|
||
|
|
R1 = vertcat(R1, Jk(:,:,i+1));
|
||
|
|
R2 = blkdiag(R2, t4);
|
||
|
|
% -------------------------------------------------------------------
|
||
|
|
end
|
||
|
|
Y = R1'*R2;
|
||
|
|
|
||
|
|
% g3 = reshape(dpi_k,[6,60])*reshape(ur10.pi_reg,[60,1]);
|
||
|
|
return
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
%PRELOCATING VARIABLES
|
||
|
|
if ordr >= 1
|
||
|
|
Tpk = zeros(4,4,6); %homogenous transformation from p to k
|
||
|
|
invAd_pk = zeros(6,6,6); %inverse of adjoint matrix from p to k
|
||
|
|
invAd_0k = zeros(6,6,7); invAd_0k(:,:,1) = eye(6,6);
|
||
|
|
Jk = zeros(6,6,7); %jacobian up to body k
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'M'))~=0 || sum(strcmp(lst,'sys_jcbn'))~=0; scrw.M = zeros(6,6); end
|
||
|
|
if sum(strcmp(lst,'g'))~=0 || sum(strcmp(lst,'sys_jcbn'))~=0 || sum(strcmp(lst,'momentum'))~=0; scrw.g = zeros(6,1); end
|
||
|
|
|
||
|
|
%-------------------------------------------------------------------------------
|
||
|
|
if ordr >= 2
|
||
|
|
xi_k = zeros(6,6);
|
||
|
|
v_k = zeros(6,7); %body velocity of k-th body
|
||
|
|
Jk_d = zeros(6,6,7);
|
||
|
|
adj_pk = zeros(6,6,6);
|
||
|
|
adj_0k = zeros(6,6,6);
|
||
|
|
FI_k = zeros(6,6,6);
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'C'))~=0 || sum(strcmp(lst,'sys_jcbn'))~=0 || sum(strcmp(lst,'momentum'))~=0; scrw.C = zeros(6,6); end
|
||
|
|
if sum(strcmp(lst,'M_d'))~=0 ; scrw.M_d = zeros(6,6); end
|
||
|
|
if sum(strcmp(lst,'g_d'))~=0; scrw.g_d = zeros(6,1); end
|
||
|
|
if sum(strcmp(lst,'momentum'))~=0; scrw.momentum = zeros(6,1); end
|
||
|
|
if sum(strcmp(lst,'sys_jcbn'))~=0
|
||
|
|
scrw.sys_jcbn = zeros(6,12);
|
||
|
|
|
||
|
|
Jk_q1 = zeros(6,6,7);
|
||
|
|
Jk_q2 = zeros(6,6,7);
|
||
|
|
Jk_q3 = zeros(6,6,7);
|
||
|
|
Jk_q4 = zeros(6,6,7);
|
||
|
|
Jk_q5 = zeros(6,6,7);
|
||
|
|
Jk_q6 = zeros(6,6,7);
|
||
|
|
|
||
|
|
Jk_d_q1 = zeros(6,6,7);
|
||
|
|
Jk_d_q2 = zeros(6,6,7);
|
||
|
|
Jk_d_q3 = zeros(6,6,7);
|
||
|
|
Jk_d_q4 = zeros(6,6,7);
|
||
|
|
Jk_d_q5 = zeros(6,6,7);
|
||
|
|
Jk_d_q6 = zeros(6,6,7);
|
||
|
|
|
||
|
|
M_q1 = zeros(6,6);
|
||
|
|
M_q2 = zeros(6,6);
|
||
|
|
M_q3 = zeros(6,6);
|
||
|
|
M_q4 = zeros(6,6);
|
||
|
|
M_q5 = zeros(6,6);
|
||
|
|
M_q6 = zeros(6,6);
|
||
|
|
|
||
|
|
g_q1 = zeros(6,1);
|
||
|
|
g_q2 = zeros(6,1);
|
||
|
|
g_q3 = zeros(6,1);
|
||
|
|
g_q4 = zeros(6,1);
|
||
|
|
g_q5 = zeros(6,1);
|
||
|
|
g_q6 = zeros(6,1);
|
||
|
|
|
||
|
|
C_q1 = zeros(6,6);
|
||
|
|
C_q2 = zeros(6,6);
|
||
|
|
C_q3 = zeros(6,6);
|
||
|
|
C_q4 = zeros(6,6);
|
||
|
|
C_q5 = zeros(6,6);
|
||
|
|
C_q6 = zeros(6,6);
|
||
|
|
|
||
|
|
|
||
|
|
C_q1_d = zeros(6,6);
|
||
|
|
C_q2_d = zeros(6,6);
|
||
|
|
C_q3_d = zeros(6,6);
|
||
|
|
C_q4_d = zeros(6,6);
|
||
|
|
C_q5_d = zeros(6,6);
|
||
|
|
C_q6_d = zeros(6,6);
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
%-------------------------------------------------------------------------------
|
||
|
|
if ordr >= 3
|
||
|
|
xi_k_d = zeros(6,6);
|
||
|
|
adj_pk_d = zeros(6,6,6);
|
||
|
|
adj_0k_d = zeros(6,6,6);
|
||
|
|
v_k_d = zeros(6,7);
|
||
|
|
FI_k_d = zeros(6,6,6);
|
||
|
|
Jk_2d = zeros(6,6,7); %second derivative of a jacobian
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'C_d'))~=0 ; scrw.C_d = zeros(6,6); end
|
||
|
|
if sum(strcmp(lst,'M_2d'))~=0 ; scrw.M_2d = zeros(6,6); end
|
||
|
|
if sum(strcmp(lst,'g_2d'))~=0 ; scrw.g_2d = zeros(6,6); end
|
||
|
|
|
||
|
|
%--------------------------------------------------------------------------------
|
||
|
|
if ordr >=4
|
||
|
|
xi_k_2d = zeros(6,6);
|
||
|
|
adj_pk_2d = zeros(6,6,6);
|
||
|
|
adj_0k_2d = zeros(6,6,6);
|
||
|
|
v_k_2d = zeros(6,7);
|
||
|
|
Jk_3d = zeros(6,6,7); %second derivative of a jacobia
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'C_2d'))~=0 ; scrw.C_2d = zeros(6,6); end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
for i = 1:1:6
|
||
|
|
if ordr >= 1
|
||
|
|
R = RPY(par.joint_origin(i, 4:6))*Rotz_s(Q(1,i));
|
||
|
|
p = par.joint_origin(i, 1:3)';
|
||
|
|
Tpk(:,:,i) = [R, p; zeros(1,3), 1];
|
||
|
|
|
||
|
|
invAd_pk(:,:,i) = inv_Ad_transf(Tpk(:,:,i));
|
||
|
|
invAd_0k(:,:,i+1) = invAd_pk(:,:,i)*invAd_0k(:,:,i);
|
||
|
|
|
||
|
|
Jk(:,:,i+1) = invAd_pk(:,:,i)*Jk(:,:,i) + par.XI(:,:,i);
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'M'))~=0 || sum(strcmp(lst,'sys_jcbn'))~=0
|
||
|
|
scrw.M = scrw.M + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1);
|
||
|
|
end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'g'))~=0 || sum(strcmp(lst,'sys_jcbn'))~=0 || sum(strcmp(lst,'momentum'))~=0
|
||
|
|
gamma_k = invAd_0k(:,:,i+1)*gamma0; %body gravitational acceleration
|
||
|
|
scrw.g = scrw.g + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*gamma_k;
|
||
|
|
end
|
||
|
|
|
||
|
|
%----------------------------------------------------------------------------------
|
||
|
|
if ordr >= 2
|
||
|
|
xi_k(:,i) = par.XI(:,:,i)*Q(2,:)';
|
||
|
|
v_k(:,i+1) = invAd_pk(:,:,i)*v_k(:,i) + xi_k(:,i);
|
||
|
|
adj_pk(:,:,i) = adj_transf(xi_k(:,i));
|
||
|
|
adj_0k(:,:,i) = adj_transf(v_k(:,i+1));
|
||
|
|
FI_k(:,:,i) = par.Lmbd_k(:,:,i)*adj_0k(:,:,i) - adj_0k(:,:,i)'*par.Lmbd_k(:,:,i);
|
||
|
|
Jk_d(:,:,i+1) = invAd_pk(:,:,i)*Jk_d(:,:,i) - adj_pk(:,:,i)*invAd_pk(:,:,i)*Jk(:,:,i);
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'C'))~=0 || sum(strcmp(lst,'sys_jcbn'))~=0 || sum(strcmp(lst,'momentum'))~=0
|
||
|
|
scrw.C = scrw.C + Jk(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) );
|
||
|
|
end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'M_d'))~=0
|
||
|
|
scrw.M_d = scrw.M_d + Jk_d(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1);
|
||
|
|
end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'g_d'))~=0
|
||
|
|
if sum(strcmp(lst,'g')) == 0
|
||
|
|
gamma_k = invAd_0k(:,:,i+1)*gamma0; %body gravitational acceleration
|
||
|
|
end
|
||
|
|
scrw.g_d = scrw.g_d + (Jk_d(:,:,i+1)'*par.Lmbd_k(:,:,i) - Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_0k(:,:,i))*gamma_k;
|
||
|
|
end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'sys_jcbn'))~=0
|
||
|
|
Jk_q1(:,:,i+1) = invAd_pk(:,:,i)*Jk_q1(:,:,i) - adj_transf(par.XI(:,1,i))*invAd_pk(:,:,i)*Jk(:,:,i);
|
||
|
|
Jk_q2(:,:,i+1) = invAd_pk(:,:,i)*Jk_q2(:,:,i) - adj_transf(par.XI(:,2,i))*invAd_pk(:,:,i)*Jk(:,:,i);
|
||
|
|
Jk_q3(:,:,i+1) = invAd_pk(:,:,i)*Jk_q3(:,:,i) - adj_transf(par.XI(:,3,i))*invAd_pk(:,:,i)*Jk(:,:,i);
|
||
|
|
Jk_q4(:,:,i+1) = invAd_pk(:,:,i)*Jk_q4(:,:,i) - adj_transf(par.XI(:,4,i))*invAd_pk(:,:,i)*Jk(:,:,i);
|
||
|
|
Jk_q5(:,:,i+1) = invAd_pk(:,:,i)*Jk_q5(:,:,i) - adj_transf(par.XI(:,5,i))*invAd_pk(:,:,i)*Jk(:,:,i);
|
||
|
|
Jk_q6(:,:,i+1) = invAd_pk(:,:,i)*Jk_q6(:,:,i) - adj_transf(par.XI(:,6,i))*invAd_pk(:,:,i)*Jk(:,:,i);
|
||
|
|
|
||
|
|
Jk_d_q1(:,:,i+1) = invAd_pk(:,:,i)*Jk_d_q1(:,:,i) - adj_transf(par.XI(:,1,i))*invAd_pk(:,:,i)*Jk_d(:,:,i) - adj_pk(:,:,i)*Jk_q1(:,:,i+1);
|
||
|
|
Jk_d_q2(:,:,i+1) = invAd_pk(:,:,i)*Jk_d_q2(:,:,i) - adj_transf(par.XI(:,2,i))*invAd_pk(:,:,i)*Jk_d(:,:,i) - adj_pk(:,:,i)*Jk_q2(:,:,i+1);
|
||
|
|
Jk_d_q3(:,:,i+1) = invAd_pk(:,:,i)*Jk_d_q3(:,:,i) - adj_transf(par.XI(:,3,i))*invAd_pk(:,:,i)*Jk_d(:,:,i) - adj_pk(:,:,i)*Jk_q3(:,:,i+1);
|
||
|
|
Jk_d_q4(:,:,i+1) = invAd_pk(:,:,i)*Jk_d_q4(:,:,i) - adj_transf(par.XI(:,4,i))*invAd_pk(:,:,i)*Jk_d(:,:,i) - adj_pk(:,:,i)*Jk_q4(:,:,i+1);
|
||
|
|
Jk_d_q5(:,:,i+1) = invAd_pk(:,:,i)*Jk_d_q5(:,:,i) - adj_transf(par.XI(:,5,i))*invAd_pk(:,:,i)*Jk_d(:,:,i) - adj_pk(:,:,i)*Jk_q5(:,:,i+1);
|
||
|
|
Jk_d_q6(:,:,i+1) = invAd_pk(:,:,i)*Jk_d_q6(:,:,i) - adj_transf(par.XI(:,6,i))*invAd_pk(:,:,i)*Jk_d(:,:,i) - adj_pk(:,:,i)*Jk_q6(:,:,i+1);
|
||
|
|
|
||
|
|
M_q1 = M_q1 + (Jk_q1(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1) + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q1(:,:,i+1));
|
||
|
|
M_q2 = M_q2 + (Jk_q2(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1) + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q2(:,:,i+1));
|
||
|
|
M_q3 = M_q3 + (Jk_q3(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1) + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q3(:,:,i+1));
|
||
|
|
M_q4 = M_q4 + (Jk_q4(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1) + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q4(:,:,i+1));
|
||
|
|
M_q5 = M_q5 + (Jk_q5(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1) + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q5(:,:,i+1));
|
||
|
|
M_q6 = M_q6 + (Jk_q6(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1) + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q6(:,:,i+1));
|
||
|
|
|
||
|
|
g_q1 = g_q1 + (Jk_q1(:,:,i+1)'*par.Lmbd_k(:,:,i) - Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_transf(Jk(:,1,i+1)))*gamma_k;
|
||
|
|
g_q2 = g_q2 + (Jk_q2(:,:,i+1)'*par.Lmbd_k(:,:,i) - Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_transf(Jk(:,2,i+1)))*gamma_k;
|
||
|
|
g_q3 = g_q3 + (Jk_q3(:,:,i+1)'*par.Lmbd_k(:,:,i) - Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_transf(Jk(:,3,i+1)))*gamma_k;
|
||
|
|
g_q4 = g_q4 + (Jk_q4(:,:,i+1)'*par.Lmbd_k(:,:,i) - Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_transf(Jk(:,4,i+1)))*gamma_k;
|
||
|
|
g_q5 = g_q5 + (Jk_q5(:,:,i+1)'*par.Lmbd_k(:,:,i) - Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_transf(Jk(:,5,i+1)))*gamma_k;
|
||
|
|
g_q6 = g_q6 + (Jk_q6(:,:,i+1)'*par.Lmbd_k(:,:,i) - Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_transf(Jk(:,6,i+1)))*gamma_k;
|
||
|
|
|
||
|
|
C_q1 = C_q1 + (Jk_q1(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) ) + ...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*Jk_d_q1(:,:,i+1) + FI_k(:,:,i)*Jk_q1(:,:,i+1)) +...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*adj_transf(Jk_q1(:,:,i+1)*Q(2,:)') - ...
|
||
|
|
adj_transf(Jk_q1(:,:,i+1)*Q(2,:)')'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1));
|
||
|
|
C_q2 = C_q2 + (Jk_q2(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) ) + ...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*Jk_d_q2(:,:,i+1) + FI_k(:,:,i)*Jk_q2(:,:,i+1)) +...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*adj_transf(Jk_q2(:,:,i+1)*Q(2,:)') - ...
|
||
|
|
adj_transf(Jk_q2(:,:,i+1)*Q(2,:)')'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1));
|
||
|
|
C_q3 = C_q3 + (Jk_q3(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) ) + ...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*Jk_d_q3(:,:,i+1) + FI_k(:,:,i)*Jk_q3(:,:,i+1)) +...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*adj_transf(Jk_q3(:,:,i+1)*Q(2,:)') - ...
|
||
|
|
adj_transf(Jk_q3(:,:,i+1)*Q(2,:)')'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1));
|
||
|
|
C_q4 = C_q4 + (Jk_q4(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) ) + ...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*Jk_d_q4(:,:,i+1) + FI_k(:,:,i)*Jk_q4(:,:,i+1)) +...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*adj_transf(Jk_q4(:,:,i+1)*Q(2,:)') - ...
|
||
|
|
adj_transf(Jk_q4(:,:,i+1)*Q(2,:)')'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1));
|
||
|
|
C_q5 = C_q5 + (Jk_q5(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) ) + ...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*Jk_d_q5(:,:,i+1) + FI_k(:,:,i)*Jk_q5(:,:,i+1)) +...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*adj_transf(Jk_q5(:,:,i+1)*Q(2,:)') - ...
|
||
|
|
adj_transf(Jk_q5(:,:,i+1)*Q(2,:)')'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1));
|
||
|
|
C_q6 = C_q6 + (Jk_q6(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) ) + ...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*Jk_d_q6(:,:,i+1) + FI_k(:,:,i)*Jk_q6(:,:,i+1)) +...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*adj_transf(Jk_q6(:,:,i+1)*Q(2,:)') - ...
|
||
|
|
adj_transf(Jk_q6(:,:,i+1)*Q(2,:)')'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1));
|
||
|
|
|
||
|
|
|
||
|
|
C_q1_d = C_q1_d + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q1(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*( par.Lmbd_k(:,:,i)*adj_transf(Jk(:,1,i+1)) - adj_transf(Jk(:,1,i+1))'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1);
|
||
|
|
C_q2_d = C_q2_d + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q2(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*( par.Lmbd_k(:,:,i)*adj_transf(Jk(:,2,i+1)) - adj_transf(Jk(:,2,i+1))'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1);
|
||
|
|
C_q3_d = C_q3_d + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q3(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*( par.Lmbd_k(:,:,i)*adj_transf(Jk(:,3,i+1)) - adj_transf(Jk(:,3,i+1))'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1);
|
||
|
|
C_q4_d = C_q4_d + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q4(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*( par.Lmbd_k(:,:,i)*adj_transf(Jk(:,4,i+1)) - adj_transf(Jk(:,4,i+1))'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1);
|
||
|
|
C_q5_d = C_q5_d + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q5(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*( par.Lmbd_k(:,:,i)*adj_transf(Jk(:,5,i+1)) - adj_transf(Jk(:,5,i+1))'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1);
|
||
|
|
C_q6_d = C_q6_d + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_q6(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*( par.Lmbd_k(:,:,i)*adj_transf(Jk(:,6,i+1)) - adj_transf(Jk(:,6,i+1))'*par.Lmbd_k(:,:,i) )*Jk(:,:,i+1);
|
||
|
|
|
||
|
|
end
|
||
|
|
|
||
|
|
%---------------------------------------------------------------------------------
|
||
|
|
if ordr >= 3
|
||
|
|
xi_k_d(:,i) = par.XI(:,:,i)*Q(3,:)';
|
||
|
|
adj_pk_d(:,:,i) = adj_transf(xi_k_d(:,i));
|
||
|
|
v_k_d(:,i+1) = invAd_pk(:,:,i)*v_k_d(:,i) - adj_pk(:,:,i)*invAd_pk(:,:,i)*v_k(:,i) + xi_k_d(:,i);
|
||
|
|
adj_0k_d(:,:,i) = adj_transf(v_k_d(:,i+1));
|
||
|
|
FI_k_d(:,:,i) = par.Lmbd_k(:,:,i)*adj_0k_d(:,:,i) - adj_0k_d(:,:,i)'*par.Lmbd_k(:,:,i);
|
||
|
|
Jk_2d(:,:,i+1) = invAd_pk(:,:,i)*Jk_2d(:,:,i) - adj_pk(:,:,i)*invAd_pk(:,:,i)*Jk_d(:,:,i) - ...
|
||
|
|
adj_pk_d(:,:,i)*invAd_pk(:,:,i)*Jk(:,:,i) - adj_pk(:,:,i)*Jk_d(:,:,i+1);
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'M_2d'))~=0
|
||
|
|
scrw.M_2d = scrw.M_2d + Jk_2d(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk(:,:,i+1) + ...
|
||
|
|
2*Jk_d(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_2d(:,:,i+1);
|
||
|
|
end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'C_d'))~=0
|
||
|
|
scrw.C_d = scrw.C_d + Jk_d(:,:,i+1)'*( FI_k(:,:,i)*Jk(:,:,i+1) + par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) ) + ...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*Jk_2d(:,:,i+1) + FI_k(:,:,i)*Jk_d(:,:,i+1)) +...
|
||
|
|
Jk(:,:,i+1)'*FI_k_d(:,:,i)*Jk(:,:,i+1);
|
||
|
|
end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'g_2d'))~=0
|
||
|
|
if (sum(strcmp(lst,'g')) == 0) && (sum(strcmp(lst,'g_d')) == 0)
|
||
|
|
gamma_k = invAd_0k(:,:,i+1)*gamma0; %body gravitational acceleration
|
||
|
|
end
|
||
|
|
scrw.g_2d = scrw.g_2d + (Jk_2d(:,:,i+1)'*par.Lmbd_k(:,:,i) - 2*Jk_d(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_0k(:,:,i) - ...
|
||
|
|
Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_0k_d(:,:,i) + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*adj_0k(:,:,i)*adj_0k(:,:,i))*gamma_k;
|
||
|
|
end
|
||
|
|
|
||
|
|
%-----------------------------------------------------------------------------------
|
||
|
|
if ordr >=4
|
||
|
|
xi_k_2d(:,i) = par.XI(:,:,i)*Q(4,:)';
|
||
|
|
v_k_2d(:,i+1) = invAd_pk(:,:,i)*v_k_2d(:,i) - adj_pk(:,:,i)*invAd_pk(:,:,i)*v_k_d(:,i) - ...
|
||
|
|
adj_pk(:,:,i)*(invAd_pk(:,:,i)*v_k_d(:,i) - adj_pk(:,:,i)*invAd_pk(:,:,i)*v_k(:,i)) - ...
|
||
|
|
adj_pk_d(:,:,i)*invAd_pk(:,:,i)*v_k(:,i) + xi_k_2d(:,i);
|
||
|
|
adj_pk_2d(:,:,i) = adj_transf(xi_k_2d(:,i));
|
||
|
|
adj_0k_2d(:,:,i) = adj_transf(v_k_2d(:,i+1));
|
||
|
|
Jk_3d(:,:,i+1) = invAd_pk(:,:,i)*Jk_3d(:,:,i) - adj_pk(:,:,i)*invAd_pk(:,:,i)*Jk_2d(:,:,i) - ...
|
||
|
|
adj_pk_d(:,:,i)*invAd_pk(:,:,i)*Jk_d(:,:,i) - ...
|
||
|
|
adj_pk(:,:,i)*(invAd_pk(:,:,i)*Jk_2d(:,:,i) - adj_pk(:,:,i)*invAd_pk(:,:,i)*Jk_d(:,:,i)) - ...
|
||
|
|
adj_pk_2d(:,:,i)*invAd_pk(:,:,i)*Jk(:,:,i) - 2*adj_pk_d(:,:,i)*Jk_d(:,:,i+1) - ...
|
||
|
|
adj_pk(:,:,i)*Jk_2d(:,:,i+1);
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'C_2d'))~=0
|
||
|
|
scrw.C_2d = scrw.C_2d + Jk_2d(:,:,i+1)'*FI_k(:,:,i)*Jk(:,:,i+1) + 2*Jk_d(:,:,i+1)'*FI_k(:,:,i)*Jk_d(:,:,i+1) + ...
|
||
|
|
2*Jk_d(:,:,i+1)'*FI_k_d(:,:,i)*Jk(:,:,i+1) + ...
|
||
|
|
Jk_2d(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_d(:,:,i+1) + Jk(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_3d(:,:,i+1) + ...
|
||
|
|
2*Jk_d(:,:,i+1)'*par.Lmbd_k(:,:,i)*Jk_2d(:,:,i+1) + Jk(:,:,i+1)'*FI_k(:,:,i)*Jk_2d(:,:,i+1) + ...
|
||
|
|
2*Jk(:,:,i+1)'*FI_k_d(:,:,i)*Jk_d(:,:,i+1) + ...
|
||
|
|
Jk(:,:,i+1)'*(par.Lmbd_k(:,:,i)*adj_0k_2d(:,:,i) - adj_0k_2d(:,:,i)'*par.Lmbd_k(:,:,i))*Jk(:,:,i+1);
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'J'))~=0; scrw.J = Jk(:,:,6+1); end
|
||
|
|
if sum(strcmp(lst,'J_d'))~=0; scrw.J_d = Jk_d(:,:,6+1); end
|
||
|
|
if sum(strcmp(lst,'J_2d'))~=0; scrw.J_2d = Jk_2d(:,:,6+1); end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'sys_jcbn'))~=0
|
||
|
|
I6 = eye(6);
|
||
|
|
invM = scrw.M\I6;
|
||
|
|
temp_tau = scrw.C*Q(2,:)' + scrw.g;
|
||
|
|
scrw.sys_jcbn(:,1) = invM*M_q1*invM*temp_tau - invM*(C_q1*Q(2,:)' + g_q1);
|
||
|
|
scrw.sys_jcbn(:,2) = invM*M_q2*invM*temp_tau - invM*(C_q2*Q(2,:)' + g_q2);
|
||
|
|
scrw.sys_jcbn(:,3) = invM*M_q3*invM*temp_tau - invM*(C_q3*Q(2,:)' + g_q3);
|
||
|
|
scrw.sys_jcbn(:,4) = invM*M_q4*invM*temp_tau - invM*(C_q4*Q(2,:)' + g_q4);
|
||
|
|
scrw.sys_jcbn(:,5) = invM*M_q5*invM*temp_tau - invM*(C_q5*Q(2,:)' + g_q5);
|
||
|
|
scrw.sys_jcbn(:,6) = invM*M_q6*invM*temp_tau - invM*(C_q6*Q(2,:)' + g_q6);
|
||
|
|
scrw.sys_jcbn(:,7) = -invM*(C_q1_d*Q(2,:)' + scrw.C*I6(:,1));
|
||
|
|
scrw.sys_jcbn(:,8) = -invM*(C_q2_d*Q(2,:)' + scrw.C*I6(:,2));
|
||
|
|
scrw.sys_jcbn(:,9) = -invM*(C_q3_d*Q(2,:)' + scrw.C*I6(:,3));
|
||
|
|
scrw.sys_jcbn(:,10) = -invM*(C_q4_d*Q(2,:)' + scrw.C*I6(:,4));
|
||
|
|
scrw.sys_jcbn(:,11) = -invM*(C_q5_d*Q(2,:)' + scrw.C*I6(:,5));
|
||
|
|
scrw.sys_jcbn(:,12) = -invM*(C_q6_d*Q(2,:)' + scrw.C*I6(:,6));
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'M'))==0; scrw = rmfield(scrw,'M'); end
|
||
|
|
if sum(strcmp(lst,'C'))==0; scrw = rmfield(scrw,'C'); end
|
||
|
|
if sum(strcmp(lst,'g'))==0; scrw = rmfield(scrw,'g'); end
|
||
|
|
end
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'momentum'))~=0
|
||
|
|
scrw.momentum = -scrw.g + scrw.C'*Q(2,:)';
|
||
|
|
|
||
|
|
if sum(strcmp(lst,'C'))==0; scrw = rmfield(scrw,'C'); end
|
||
|
|
if sum(strcmp(lst,'g'))==0; scrw = rmfield(scrw,'g'); end
|
||
|
|
end
|
||
|
|
|
||
|
|
end
|