feature/R1000-identification #2
|
|
@ -7,76 +7,18 @@ opt.LD_method = 'Direct';
|
|||
opt.debug = true;
|
||||
opt.robotName = 'R1000';
|
||||
|
||||
opt.Isreal = true;
|
||||
opt.Isreal = false;
|
||||
robot = get_robot_R1000(file,opt);
|
||||
% robot.theta = [1,1,0];
|
||||
|
||||
%TODO verify kinematics via robotics toolbox or other software result
|
||||
com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
|
||||
com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
|
||||
com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3;
|
||||
com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3;
|
||||
com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3;
|
||||
com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3;
|
||||
com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3;
|
||||
com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3;
|
||||
com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
|
||||
com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3;
|
||||
com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
|
||||
com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3;
|
||||
com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
|
||||
com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3;
|
||||
com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
|
||||
com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use
|
||||
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
||||
|
||||
% Get 3D coordinate of CO
|
||||
co=[];
|
||||
for i = 1:8
|
||||
if i == 1
|
||||
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
|
||||
elseif i<8
|
||||
%From base to ISA Origin
|
||||
co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
|
||||
else
|
||||
%From base to ISA Origin
|
||||
co(:,i) = co(:,i-1)-[0;0;0.05896];
|
||||
end
|
||||
end
|
||||
co = [zeros(3,1),co];
|
||||
|
||||
% temp slist
|
||||
robot.slist=[[0;0;1;co(:,1)],...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,2))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,3))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,4))]...
|
||||
[0;0;1;cross(-[0;0;1],co(:,5))]...
|
||||
[1;0;0;cross(-[1;0;0],co(:,6))]...
|
||||
[0;0;-1;cross(-[0;0;-1],co(:,7))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
|
||||
[0;0;0;1;0;0]];
|
||||
|
||||
robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]];
|
||||
robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,3) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,4) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,5) = [[0;-1;0],[1;0;0],[0;0;1]];
|
||||
robot.Home.R(:,:,6) = [[0;-1;0],[0;0;-1],[1;0;0]];
|
||||
robot.Home.R(:,:,7) = [[1;0;0],[0;-1;0],[0;0;-1]];
|
||||
robot.Home.R(:,:,8) = [[0;0;-1],[1;0;0],[0;-1;0]];
|
||||
robot.Home.R(:,:,9) = [[0;0;-1],[0;1;0],[1;0;0]];
|
||||
for i=1:9
|
||||
robot.Home.P(:,i) = co(:,i);
|
||||
robot.Home.M(:,:,i) = RpToTrans(robot.Home.R(:,:,i),robot.Home.P(:,i));
|
||||
end
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
|
||||
robot = get_Kinematics(robot, opt);
|
||||
|
||||
R1000_Dynamics_num;
|
||||
% R1000_Dynamics_num;
|
||||
% R1000_Dynamics;
|
||||
% opt.Isreal = false;
|
||||
% robot = get_velocity(robot, opt);
|
||||
% robot = get_regressor(robot,opt);
|
||||
robot = get_velocity(robot, opt);
|
||||
robot = get_regressor(robot,opt);
|
||||
% symbol matched
|
||||
% verify_regressor
|
||||
% robot = get_baseParams(robot, opt);
|
||||
% robot = get_baseParams(robot, opt);
|
||||
|
|
|
|||
|
|
@ -1,21 +1,14 @@
|
|||
%% R1000
|
||||
N=9;
|
||||
% traj
|
||||
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);
|
||||
zero_ = zeros(1,length(q_J));
|
||||
N=length(robot.theta);
|
||||
% Dynamics parameters
|
||||
link_mass = robot.m;
|
||||
com_pos = robot.com;
|
||||
link_inertia = robot.I;
|
||||
|
||||
q_sym = sym('q%d',[N,1],'real');
|
||||
qd_sym = sym('qd%d',[N,1],'real');
|
||||
q2d_sym = sym('qdd%d',[N,1],'real');
|
||||
thetalist = qd_sym(1:N);
|
||||
qd_sym = zeros(N,1);%sym('qd%d',[N,1],'real');
|
||||
q2d_sym = zeros(N,1);%sym('qdd%d',[N,1],'real');
|
||||
thetalist = q_sym(1:N);
|
||||
dthetalist = qd_sym(1:N);
|
||||
ddthetalist = q2d_sym(1:N);
|
||||
|
||||
|
|
@ -26,39 +19,13 @@ for i = 1:N
|
|||
Glist = cat(3, Glist, Gb);
|
||||
end
|
||||
|
||||
% Get the com pos transformation in each joint reference frame
|
||||
Mlist_CG = [];
|
||||
for i = 0:N-1
|
||||
if i == 0
|
||||
M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
else
|
||||
M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
end
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% Get the end efforce transformation in each joint reference frame
|
||||
Mlist_ED = [];
|
||||
for i = 1:N
|
||||
M = robot.T(:,:,i);
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
Mlist_CG = robot.kine.Mlist_CG;
|
||||
Mlist_ED = robot.kine.Mlist_ED;
|
||||
|
||||
%TODO: Get Slist form DH table method
|
||||
% RRRRRRRRP
|
||||
Slist=[[0;0;1;0;0;0],...
|
||||
[0;-1;0;cross(-[0;-1;0],[0.2;0;0])]...
|
||||
[0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]...
|
||||
[0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45;0;0])]...
|
||||
[0;0;1;cross(-[0;0;1],[0.2+0.5+0.45+0.12;0;0])]...
|
||||
[1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]...
|
||||
[0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
|
||||
[0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
|
||||
[0;0;0;1;0;0]];
|
||||
Slist=robot.slist;
|
||||
|
||||
Vlinear=sym(zeros(3,3));
|
||||
J=sym(zeros(6,N));
|
||||
|
|
@ -66,14 +33,14 @@ exf=[0;0;0;0;0;0];
|
|||
|
||||
[V1,Vd1,Adgab_mat,Fmat,tau_mat] ...
|
||||
= InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
|
||||
[0;0;-9.8], exf, Mlist_CG, Glist, Slist);
|
||||
[0;0;-9.806], exf, Mlist_CG, Glist, Slist);
|
||||
% G = FKinSpaceExpand_Sym(Mlist_CG, Slist, thetalist);
|
||||
% T=FKinSpaceExpand_Sym(Mlist_ED, Slist, thetalist);
|
||||
% F_Simpack = getSimpackF_Sym(G,T,Mlist_ED,Fmat);
|
||||
|
||||
%Gen Files
|
||||
matlabFunction(Fmat,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),...
|
||||
'Vars',{q_sym,qd_sym,q2d_sym});
|
||||
matlabFunction(tau_mat,'File',sprintf('autogen/standard_dynamics_%s',opt.robotName),...
|
||||
'Vars',{q_sym});
|
||||
standard_dynamics_func = sprintf('standard_dynamics_%s',opt.robotName);
|
||||
% traj
|
||||
% time = 0:1:2;
|
||||
|
|
|
|||
|
|
@ -1,139 +0,0 @@
|
|||
%% R1000
|
||||
N=9;
|
||||
% traj
|
||||
time = 0:0.01:1;
|
||||
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);
|
||||
zero_ = zeros(1,length(q_J));
|
||||
% Dynamics parameters
|
||||
link_mass = robot.m;
|
||||
com_pos = robot.com;
|
||||
link_inertia = robot.I;
|
||||
|
||||
thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
|
||||
% Get general mass matrix
|
||||
Glist=[];
|
||||
for i = 1:N
|
||||
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
||||
Glist = cat(3, Glist, Gb);
|
||||
end
|
||||
|
||||
% Get the com pos transformation in each joint reference frame
|
||||
% Mlist_CG = [];
|
||||
% for i = 0:N-1
|
||||
% if i == 0
|
||||
% M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
% else
|
||||
% M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
% end
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% Get the com pos transformation in each joint reference frame
|
||||
% FIXME: BUG here
|
||||
Mlist_CG=[];
|
||||
for i = 0:N-1
|
||||
if i == 0
|
||||
M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
else
|
||||
rotation_i = diag([1,1,1]);
|
||||
for j = 1:i
|
||||
rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
|
||||
rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
|
||||
end
|
||||
M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1));
|
||||
end
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% Mlist_CG=[];
|
||||
% for i = 1:N
|
||||
% if i == 1
|
||||
% M = [diag([1,1,1]),com_pos_R1(:,i);zeros(1,3),1];
|
||||
% elseif i<=8
|
||||
% M = RpToTrans(TransToRp(robot.T(:,:,i)),TransToRp(robot.T(:,:,i))*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
|
||||
% elseif i==9
|
||||
% M = RpToTrans(TransToRp(robot.T(:,:,i)),TransToRp(robot.T(:,:,i))*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i)));
|
||||
% end
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% Get the end efforce transformation in each joint reference frame
|
||||
Mlist_ED = [];
|
||||
for i = 1:N
|
||||
M = robot.T(:,:,i);
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
|
||||
%TODO: Get Slist form DH table method
|
||||
% RRRRRRRRP
|
||||
Slist=[[0;0;1;0;0;0],...
|
||||
[0;-1;0;cross(-[0;-1;0],[0.2;0;0])]...
|
||||
[0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]...
|
||||
[0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45;0;0])]...
|
||||
[0;0;1;cross(-[0;0;1],[0.2+0.5+0.45+0.12;0;0])]...
|
||||
[1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]...
|
||||
[0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
|
||||
[0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]...
|
||||
[0;0;0;1;0;0]];
|
||||
|
||||
Vlinear=sym(zeros(3,3));
|
||||
J=sym(zeros(6,N));
|
||||
exf=[0;0;0;0;0;0];
|
||||
|
||||
for i = 1:length(q_J)
|
||||
[V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
|
||||
= InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
|
||||
[0;0;-9.806], exf, Mlist_CG, Glist, Slist);
|
||||
G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
|
||||
T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
|
||||
%Want to get the result from TC_delta, which means F at CG represent under frame at the last origin
|
||||
%why we need Mlist_ED
|
||||
%please explain this more
|
||||
F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
|
||||
end
|
||||
% plot Torque
|
||||
% above 2020b
|
||||
% F_Simpack = pagetranspose(F_Simpack);
|
||||
% below 2020b
|
||||
|
||||
for i = 1:3
|
||||
subplot(3,1,i);
|
||||
hold on;
|
||||
%added minus, so should be the same as simpack
|
||||
plot(time,taumat(i+6,:))
|
||||
xlabel('time(s)')
|
||||
ylabel('Torque(Nm)')
|
||||
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
||||
end
|
||||
|
||||
F_Simpack = permute(F_Simpack,[2 1 3]);
|
||||
figure(2)
|
||||
for i = 1:3
|
||||
subplot(3,1,i);
|
||||
hold on;
|
||||
%added minus, so should be the same as simpack
|
||||
plot(time,-reshape(F_Simpack(8,i,:),[1,length(F_Simpack)]))
|
||||
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
||||
end
|
||||
|
||||
% 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);
|
||||
|
|
@ -12,9 +12,9 @@ link_mass = robot.m;
|
|||
com_pos = robot.com;
|
||||
link_inertia = robot.I;
|
||||
|
||||
thetalist = [q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
dthetalist = [qd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
ddthetalist = [qdd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
thetalist = [zero_;q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
|
||||
% Get general mass matrix
|
||||
Glist=[];
|
||||
|
|
@ -86,6 +86,9 @@ end
|
|||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% get the CG at the world base frame
|
||||
com_pos_R1 = robot.com_pos_R1;
|
||||
com_pos_R2 = robot.com_pos_R2;
|
||||
ct=[];
|
||||
Mlist_CG_Base=[];
|
||||
for i = 1:N
|
||||
|
|
@ -100,6 +103,7 @@ for i = 1:N
|
|||
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
||||
Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
|
||||
end
|
||||
% get the CG at the last GC frame
|
||||
Mlist_CG=[];
|
||||
for i = 1:N
|
||||
if i == 1
|
||||
|
|
|
|||
|
|
@ -0,0 +1,341 @@
|
|||
function tau_mat = standard_dynamics_R1000(in1)
|
||||
%standard_dynamics_R1000
|
||||
% TAU_MAT = standard_dynamics_R1000(IN1)
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.1.
|
||||
% 07-Oct-2024 20:05:17
|
||||
|
||||
q2 = in1(2,:);
|
||||
q3 = in1(3,:);
|
||||
q4 = in1(4,:);
|
||||
q5 = in1(5,:);
|
||||
q6 = in1(6,:);
|
||||
q7 = in1(7,:);
|
||||
q8 = in1(8,:);
|
||||
q9 = in1(9,:);
|
||||
t2 = cos(q2);
|
||||
t3 = cos(q3);
|
||||
t4 = cos(q4);
|
||||
t5 = cos(q5);
|
||||
t6 = cos(q6);
|
||||
t7 = cos(q7);
|
||||
t8 = cos(q8);
|
||||
t9 = sin(q2);
|
||||
t10 = sin(q3);
|
||||
t11 = sin(q4);
|
||||
t12 = sin(q5);
|
||||
t13 = sin(q6);
|
||||
t14 = sin(q7);
|
||||
t15 = sin(q8);
|
||||
t38 = q9+3.647515815399993e-2;
|
||||
t16 = t2.*t3.*9.806;
|
||||
t17 = t2.*t10.*9.806;
|
||||
t18 = t3.*t9.*9.806;
|
||||
t19 = t9.*t10.*9.806;
|
||||
t21 = t2.*t3.*2.373052e+1;
|
||||
t22 = t2.*t10.*2.373052e+1;
|
||||
t23 = t3.*t9.*2.373052e+1;
|
||||
t24 = t9.*t10.*2.373052e+1;
|
||||
t32 = t4.*4.922552299946314e-7;
|
||||
t33 = t11.*4.922552299946314e-7;
|
||||
t36 = t3.*4.409419000001025e-5;
|
||||
t37 = t10.*4.409419000001025e-5;
|
||||
t39 = t8.*2.32103180000387e-5;
|
||||
t40 = t15.*2.32103180000387e-5;
|
||||
t43 = t7.*4.740360800000037e-3;
|
||||
t44 = t14.*4.740360800000037e-3;
|
||||
t45 = t6.*1.230157900000001e-2;
|
||||
t46 = t13.*1.230157900000001e-2;
|
||||
t48 = t8.*2.0257876e-1;
|
||||
t49 = t15.*2.0257876e-1;
|
||||
t51 = t3.*1.2654643e-1;
|
||||
t52 = t10.*1.2654643e-1;
|
||||
t55 = t5.*6.595131499999993e-2;
|
||||
t56 = t12.*6.595131499999993e-2;
|
||||
t57 = t4.*2.618828899999998e-1;
|
||||
t58 = t11.*2.618828899999998e-1;
|
||||
t62 = t6.*4.895353899999624e-5;
|
||||
t63 = t13.*4.895353899999624e-5;
|
||||
t68 = t7.*5.992854699999624e-5;
|
||||
t69 = t14.*5.992854699999624e-5;
|
||||
t70 = t5.*9.036099100000004e-3;
|
||||
t71 = t12.*9.036099100000004e-3;
|
||||
t20 = -t19;
|
||||
t25 = -t24;
|
||||
t26 = t17+t18;
|
||||
t35 = -t32;
|
||||
t41 = -t36;
|
||||
t42 = -t40;
|
||||
t50 = -t44;
|
||||
t61 = -t56;
|
||||
t67 = -t62;
|
||||
t99 = t33+t57+5.404868499999996e-2;
|
||||
t100 = t37+t51+1.8811711e-1;
|
||||
t101 = t39+t49+3.657193499999997e-2;
|
||||
t107 = t43+t69+2.32103180000387e-5;
|
||||
t113 = t45+t63-2.8858572e-2;
|
||||
t119 = t55+t71+1.302852800000001e-1;
|
||||
t27 = t16+t20;
|
||||
t28 = t4.*t26;
|
||||
t29 = t11.*t26;
|
||||
t97 = t41+t52+4.922552299946314e-7;
|
||||
t98 = t35+t58+5.091563100000002e-3;
|
||||
t102 = t42+t48+4.712426700000003e-2;
|
||||
t105 = t4.*t99;
|
||||
t106 = t11.*t99;
|
||||
t108 = t8.*t101;
|
||||
t109 = t15.*t101;
|
||||
t114 = t7.*t107;
|
||||
t115 = t14.*t107;
|
||||
t118 = t46+t67+5.992854699999624e-5;
|
||||
t120 = t6.*t113;
|
||||
t121 = t13.*t113;
|
||||
t123 = t12.*t119;
|
||||
t124 = t61+t70+4.895353899999624e-5;
|
||||
t125 = t50+t68+9.503114400000004e-3;
|
||||
t128 = t5.*t119;
|
||||
t30 = t4.*t27;
|
||||
t31 = t11.*t27;
|
||||
t34 = -t29;
|
||||
t47 = t29.*(1.11e+2./5.0e+1);
|
||||
t59 = t28.*(5.19e+2./1.0e+2);
|
||||
t60 = t29.*(7.41e+2./1.0e+2);
|
||||
t103 = t4.*t98;
|
||||
t104 = t11.*t98;
|
||||
t110 = -t106;
|
||||
t111 = t8.*t102;
|
||||
t112 = t15.*t102;
|
||||
t117 = -t114;
|
||||
t122 = -t121;
|
||||
t126 = t6.*t118;
|
||||
t127 = t13.*t118;
|
||||
t129 = -t128;
|
||||
t130 = t5.*t124;
|
||||
t131 = t7.*t125;
|
||||
t132 = t12.*t124;
|
||||
t133 = t14.*t125;
|
||||
t53 = t30.*(1.11e+2./5.0e+1);
|
||||
t54 = -t47;
|
||||
t64 = t30.*(7.41e+2./1.0e+2);
|
||||
t65 = t31.*(5.19e+2./1.0e+2);
|
||||
t66 = -t60;
|
||||
t72 = t28+t31;
|
||||
t73 = t30+t34;
|
||||
t76 = -t6.*(t29-t30);
|
||||
t77 = -t13.*(t29-t30);
|
||||
t85 = t13.*(t29-t30).*(-9.0./5.0);
|
||||
t92 = t6.*(t29-t30).*(-2.31e+2./1.0e+2);
|
||||
t93 = t6.*(t29-t30).*(-4.11e+2./1.0e+2);
|
||||
t116 = -t112;
|
||||
t154 = t104+t105;
|
||||
t155 = t103+t110;
|
||||
t156 = t109+t111;
|
||||
t164 = t115+t131;
|
||||
t165 = t120+t127;
|
||||
t166 = t117+t133;
|
||||
t167 = t122+t126;
|
||||
t168 = t123+t130;
|
||||
t169 = t129+t132;
|
||||
t74 = t6.*t12.*t72;
|
||||
t75 = t12.*t13.*t72;
|
||||
t78 = t5.*t7.*t72;
|
||||
t80 = t5.*t72.*(9.0./5.0);
|
||||
t82 = t5.*t14.*t72.*(6.0./5.0);
|
||||
t84 = t5.*t72.*(2.01e+2./5.0e+1);
|
||||
t86 = t12.*t72.*(1.11e+2./5.0e+1);
|
||||
t88 = t5.*t14.*t72.*(1.31e+2./2.5e+1);
|
||||
t89 = t5.*t14.*t72.*(2.93e+2./1.0e+2);
|
||||
t96 = t5.*t14.*t72.*5.529408000000002e-2;
|
||||
t160 = t108+t116;
|
||||
t79 = -t74;
|
||||
t81 = -t78;
|
||||
t83 = t74.*(9.0./5.0);
|
||||
t90 = t75.*(2.31e+2./1.0e+2);
|
||||
t91 = t75.*(4.11e+2./1.0e+2);
|
||||
t94 = t78.*(2.31e+2./1.0e+2);
|
||||
t134 = t75+t76;
|
||||
t136 = -t14.*(t74+t13.*(t29-t30));
|
||||
t138 = t7.*(t74+t13.*(t29-t30)).*(-6.0./5.0);
|
||||
t141 = t7.*(t74+t13.*(t29-t30)).*(-1.31e+2./2.5e+1);
|
||||
t143 = t7.*(t74+t13.*(t29-t30)).*(-2.93e+2./1.0e+2);
|
||||
t144 = t14.*(t74+t13.*(t29-t30)).*(-2.31e+2./1.0e+2);
|
||||
t147 = t7.*(t74+t13.*(t29-t30)).*(-5.529408000000002e-2);
|
||||
t153 = t15.*(t78+t14.*(t74+t13.*(t29-t30))).*(-6.0./5.0);
|
||||
t158 = t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(-2.93e+2./1.0e+2);
|
||||
t159 = t15.*(t78+t14.*(t74+t13.*(t29-t30))).*(-2.93e+2./1.0e+2);
|
||||
t170 = t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(-5.529408000000002e-2);
|
||||
t171 = t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(-6.05542308e-4);
|
||||
t172 = t15.*(t78+t14.*(t74+t13.*(t29-t30))).*(-6.05542308e-4);
|
||||
t87 = -t83;
|
||||
t95 = -t94;
|
||||
t135 = t77+t79;
|
||||
t137 = t8.*t134.*(6.0./5.0);
|
||||
t139 = t8.*t134.*(2.93e+2./1.0e+2);
|
||||
t140 = t15.*t134.*(2.93e+2./1.0e+2);
|
||||
t145 = t15.*t134.*5.529408000000002e-2;
|
||||
t148 = t15.*t134.*6.05542308e-4;
|
||||
t149 = t8.*t134.*6.05542308e-4;
|
||||
t151 = t81+t136;
|
||||
t152 = t82+t138;
|
||||
t157 = t88+t141;
|
||||
t161 = t89+t143;
|
||||
t142 = -t140;
|
||||
t146 = -t145;
|
||||
t150 = -t148;
|
||||
t162 = t7.*t157;
|
||||
t163 = t14.*t157;
|
||||
t173 = t38.*t152;
|
||||
t177 = t107.*t157;
|
||||
t178 = t137+t153;
|
||||
t179 = t139+t159;
|
||||
t183 = -t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2));
|
||||
t184 = -t15.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2));
|
||||
t185 = t15.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2));
|
||||
t190 = t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)).*(-9.140287820000001e-3);
|
||||
t193 = t156.*t161;
|
||||
t194 = t160.*t161;
|
||||
t197 = -t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2));
|
||||
t202 = t96+t147+t149+t172;
|
||||
t174 = t162.*1.65162668e-1;
|
||||
t175 = t163.*1.65162668e-1;
|
||||
t180 = t142+t158;
|
||||
t181 = t8.*t179;
|
||||
t182 = t15.*t179;
|
||||
t186 = t38.*t178;
|
||||
t191 = t185.*(-9.140287820000001e-3);
|
||||
t192 = t185.*9.140287820000001e-3;
|
||||
t195 = -t194;
|
||||
t196 = t101.*t179;
|
||||
t198 = t150+t171+t173;
|
||||
t199 = -t8.*(t148-t173+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*6.05542308e-4);
|
||||
t200 = -t15.*(t148-t173+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*6.05542308e-4);
|
||||
t201 = t15.*(t148-t173+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*6.05542308e-4);
|
||||
t203 = t8.*t202;
|
||||
t204 = t15.*t202;
|
||||
t176 = -t175;
|
||||
t187 = -t186;
|
||||
t188 = t181.*9.140287820000001e-3;
|
||||
t189 = t182.*9.140287820000001e-3;
|
||||
t205 = t90+t92+t181+t185;
|
||||
t206 = t91+t93+t181+t185;
|
||||
t211 = t95+t144+t182+t183;
|
||||
t212 = -t7.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)));
|
||||
t213 = -t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)));
|
||||
t214 = t7.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)));
|
||||
t216 = t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))).*(-1.65162668e-1);
|
||||
t218 = -t125.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)));
|
||||
t219 = t125.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)));
|
||||
t231 = -t6.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0));
|
||||
t232 = -t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0));
|
||||
t233 = t6.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0));
|
||||
t235 = t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)).*(-1.448743539999999e-1);
|
||||
t238 = -t113.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0));
|
||||
t239 = t113.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0));
|
||||
t244 = -t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2);
|
||||
t245 = -t14.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2);
|
||||
t207 = t13.*t206;
|
||||
t208 = t6.*t206;
|
||||
t215 = t214.*(-1.65162668e-1);
|
||||
t217 = t118.*t206;
|
||||
t220 = t164.*t205;
|
||||
t221 = -t205.*(t114-t133);
|
||||
t223 = t80+t163+t214;
|
||||
t224 = t84+t163+t214;
|
||||
t230 = t85+t87+t162+t213;
|
||||
t234 = t233.*(-1.448743539999999e-1);
|
||||
t236 = t233.*1.448743539999999e-1;
|
||||
t243 = t146+t170+t187+t196+t197;
|
||||
t265 = t188+t192+t193+t201+t203;
|
||||
t279 = t177+t189+t190+t195+t199+t204+t219;
|
||||
t209 = t208.*1.448743539999999e-1;
|
||||
t210 = t207.*1.448743539999999e-1;
|
||||
t222 = -t220;
|
||||
t225 = t5.*t224;
|
||||
t226 = t12.*t224;
|
||||
t237 = t124.*t224;
|
||||
t240 = t165.*t223;
|
||||
t241 = -t223.*(t121-t126);
|
||||
t246 = t86+t207+t233;
|
||||
t247 = t53+t54+t208+t232;
|
||||
t250 = t64+t66+t208+t232;
|
||||
t251 = -t4.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)));
|
||||
t252 = -t11.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)));
|
||||
t253 = t11.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)));
|
||||
t257 = t4.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))).*(-5.562904389999999e-2);
|
||||
t260 = -t99.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)));
|
||||
t262 = -t168.*(t47-t53-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)));
|
||||
t263 = (t128-t132).*(t47-t53-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)));
|
||||
t266 = t7.*t265;
|
||||
t267 = t14.*t265;
|
||||
t280 = t6.*t279;
|
||||
t281 = t13.*t279;
|
||||
t227 = -t226;
|
||||
t228 = t225.*6.710016000000013e-3;
|
||||
t229 = t226.*6.710016000000013e-3;
|
||||
t242 = -t240;
|
||||
t248 = t5.*t246;
|
||||
t249 = t12.*t246;
|
||||
t258 = t253.*(-5.562904389999999e-2);
|
||||
t259 = t253.*5.562904389999999e-2;
|
||||
t261 = t119.*t246;
|
||||
t264 = -t263;
|
||||
t268 = -t267;
|
||||
t282 = -t280;
|
||||
t286 = -t13.*(t175+t214.*1.65162668e-1+t267+t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2)+t205.*(t114-t133));
|
||||
t287 = -t6.*(t175+t214.*1.65162668e-1+t267+t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2)+t205.*(t114-t133));
|
||||
t288 = t13.*(t175+t214.*1.65162668e-1+t267+t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2)+t205.*(t114-t133));
|
||||
t289 = t6.*(t175+t214.*1.65162668e-1+t267+t7.*(t145+t186-t196+t102.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*5.529408000000002e-2)+t205.*(t114-t133));
|
||||
t290 = t174+t216+t217+t222+t239+t245+t266;
|
||||
t254 = t248.*6.710016000000013e-3;
|
||||
t255 = t249.*6.710016000000013e-3;
|
||||
t269 = t227+t248;
|
||||
t270 = t59+t65+t225+t249;
|
||||
t276 = -t154.*(t226-t248);
|
||||
t277 = -t155.*(t226-t248);
|
||||
t278 = t155.*(t226-t248);
|
||||
t285 = t176+t215+t221+t244+t268;
|
||||
t291 = t5.*t290;
|
||||
t292 = t12.*t290;
|
||||
t294 = t209+t235+t242+t281+t289;
|
||||
t297 = t210+t236+t237+t241+t261+t282+t288;
|
||||
t256 = -t254;
|
||||
t271 = t4.*t270;
|
||||
t272 = t11.*t270;
|
||||
t275 = t98.*t270;
|
||||
t293 = -t292;
|
||||
t295 = t5.*t294;
|
||||
t296 = t12.*t294;
|
||||
t298 = t4.*t297;
|
||||
t299 = t11.*t297;
|
||||
t273 = t271.*5.562904389999999e-2;
|
||||
t274 = t272.*5.562904389999999e-2;
|
||||
t283 = t21+t25+t251+t272;
|
||||
t284 = t22+t23+t253+t271;
|
||||
t300 = t229+t256+t262+t291+t296;
|
||||
t301 = t4.*t300;
|
||||
t302 = t11.*t300;
|
||||
t303 = -t302;
|
||||
t305 = t257+t274+t278+t299+t301;
|
||||
t304 = t259+t273+t276+t298+t303;
|
||||
et1 = t2.*(t2.*1.0040117e-1+t9.*3.452226e-2+3.734539500000001e-1);
|
||||
et2 = t9.*(t2.*(-3.452226e-2)+t9.*1.0040117e-1+4.431314100000749e-5);
|
||||
et3 = t226.*9.743525e-2-t248.*9.743525e-2+t2.*(t3.*t284.*1.043363139e-1-t10.*t283.*1.043363139e-1-t3.*t304+t10.*t305+(t226-t248).*(t3.*t100+t10.*t97));
|
||||
et4 = t9.*(t3.*t283.*(-1.043363139e-1)-t10.*t284.*1.043363139e-1+t3.*t305+t10.*t304+(t226-t248).*(t3.*t97-t10.*t100))+(et1+et2).*(t226-t248)+t9.*(t2.*7.55062e+1+t3.*t283+t10.*t284).*3.967117054999999e-2;
|
||||
et5 = t2.*(t9.*7.55062e+1+t3.*t284-t10.*t283).*(-3.967117054999999e-2);
|
||||
et6 = t2.*2.819808863949e+1+t9.*3.345916886974766e-3+t228+t255+t260+t264+t275+t293+t295+t3.*t283.*3.734539500000001e-1+t3.*t284.*4.431314100000749e-5;
|
||||
et7 = t10.*t283.*(-4.431314100000749e-5)+t10.*t284.*3.734539500000001e-1+t97.*t284+t100.*t283;
|
||||
et8 = t228+t255+t260+t264+t272.*1.8811711e-1+t275+t293+t295+t2.*t3.*4.4641168411972+t2.*t10.*1.16814725804922e-5+t3.*t9.*1.16814725804922e-5;
|
||||
et9 = t9.*t10.*(-4.4641168411972)+t32.*t270-t4.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0))).*1.8811711e-1;
|
||||
et10 = t33.*(t60-t64-t208+t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)));
|
||||
et11 = t28.*2.642521248900001e-2-t29.*4.005007558499997e-1+t30.*4.005007558499997e-1+t31.*2.642521248900001e-2+t208.*5.404868499999996e-2;
|
||||
et12 = t225.*1.180157910000001e-2+t249.*1.180157910000001e-2+t264+t293+t295-t13.*(t83-t162+t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)))+t13.*(t29-t30).*(9.0./5.0)).*5.404868499999996e-2;
|
||||
et13 = t163.*(-4.895353899999624e-5)-t207.*2.75159634e-1-t214.*4.895353899999624e-5-t233.*2.75159634e-1+t280+t286-t5.*t72.*1.967932267799849e-4;
|
||||
et14 = t12.*t72.*(-2.892333216000002e-1)+t223.*(t121-t126);
|
||||
et15 = t74.*(-5.19454296e-2)+t75.*2.463063281699846e-4+t162.*1.9402124e-1+t181.*5.992854699999624e-5+t185.*5.992854699999624e-5+t222+t245+t266;
|
||||
et16 = t14.*(t94-t182+t14.*(t74+t13.*(t29-t30)).*(2.31e+2./1.0e+2)+t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2))).*(-1.9402124e-1)-t6.*(t29-t30).*2.463063281699846e-4-t13.*(t29-t30).*5.19454296e-2;
|
||||
et17 = t78.*(-2.195219426400001e-2)+t182.*3.628265800000032e-4+t194-t204+t7.*(t74+t13.*(t29-t30)).*1.216220663202028e-4-t14.*(t74+t13.*(t29-t30)).*2.195219426400001e-2;
|
||||
et18 = t8.*(t148-t173+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*6.05542308e-4)-t8.*(t140+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(2.93e+2./1.0e+2)).*3.628265800000032e-4-t5.*t14.*t72.*1.216220663202028e-4;
|
||||
mt1 = [et3+et4+et5;et6+et7;et8+et9+et10;et11+et12;et13+et14;et15+et16;et17+et18];
|
||||
mt2 = [t186-t8.*t134.*1.071557695499999e-1+t15.*t134.*1.933681823100001e-1+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*1.933681823100001e-1+t15.*(t78+t14.*(t74+t13.*(t29-t30))).*1.071557695499999e-1];
|
||||
mt3 = [t15.*t134.*(6.0./5.0)+t8.*(t78+t14.*(t74+t13.*(t29-t30))).*(6.0./5.0)];
|
||||
tau_mat = [mt1;mt2;mt3];
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
function out1 = standard_regressor_R1000(in1,in2,in3)
|
||||
%standard_regressor_R1000
|
||||
% OUT1 = standard_regressor_R1000(IN1,IN2,IN3)
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 9.1.
|
||||
% 07-Oct-2024 20:37:53
|
||||
|
||||
mt1 = [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,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,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,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,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,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,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,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,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,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,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,0.0,0.0];
|
||||
mt2 = [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,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,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,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,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,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,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,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,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,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,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,0.0,0.0];
|
||||
mt3 = [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,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,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,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,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,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,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,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,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,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,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,0.0,0.0];
|
||||
mt4 = [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,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,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,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,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,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,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,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,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,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,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,0.0,0.0];
|
||||
mt5 = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];
|
||||
out1 = reshape([mt1,mt2,mt3,mt4,mt5],9,90);
|
||||
107
get_Kinematics.m
107
get_Kinematics.m
|
|
@ -2,6 +2,7 @@ function robot = get_Kinematics(robot, opt)
|
|||
if(opt.Isreal)
|
||||
switch opt.KM_method
|
||||
case 'SCREW'
|
||||
% Get transformation for each joint
|
||||
thetalist = robot.theta;
|
||||
Slist = robot.slist;
|
||||
% TODO:move to lib
|
||||
|
|
@ -20,6 +21,43 @@ if(opt.Isreal)
|
|||
end
|
||||
end
|
||||
robot.Fkine = robot.TW(:,:,end);
|
||||
|
||||
%stack result into kine structure
|
||||
robot.kine.TW = robot.TW;
|
||||
robot.kine.T = robot.T;
|
||||
robot.kine.Fkine = robot.Fkine;
|
||||
% get the CG at the world base frame
|
||||
% FIXME: Fix this hack
|
||||
ct=[];
|
||||
Mlist_CG_Base=[];
|
||||
com_pos_R1 = robot.com_pos_R1;
|
||||
com_pos_R2 = robot.com_pos_R2;
|
||||
for i = 1:length(thetalist)
|
||||
if i == 1
|
||||
ct(:,i) = com_pos_R1(:,i);
|
||||
elseif i< 9
|
||||
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||
else
|
||||
% HACK
|
||||
ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
||||
end
|
||||
robot.Home.com(:,i) = ct(:,i);
|
||||
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
||||
Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
|
||||
end
|
||||
robot.kine.Mlist_CG_Base = Mlist_CG_Base;
|
||||
% get the CG at the last GC frame
|
||||
Mlist_CG=[];
|
||||
for i = 1:length(thetalist)
|
||||
if i == 1
|
||||
Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
|
||||
else
|
||||
Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
|
||||
end
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
robot.kine.Mlist_CG = Mlist_CG;
|
||||
case 'SDH'
|
||||
theta = robot.theta;
|
||||
alpha = robot.alpha;
|
||||
|
|
@ -70,6 +108,75 @@ if(opt.Isreal)
|
|||
end
|
||||
else
|
||||
switch opt.KM_method
|
||||
case 'SCREW'
|
||||
% Get transformation for each joint
|
||||
thetalist = robot.theta;
|
||||
Slist = robot.slist;
|
||||
% TODO:move to lib
|
||||
for j = 1:length(thetalist)
|
||||
T=robot.Home.M(:,:,j);
|
||||
for i = j: -1: 1
|
||||
T = MatrixExp6_Sym(VecTose3(Slist(:, i) * thetalist(i))) * T;
|
||||
end
|
||||
robot.TW(:,:,j) = T;
|
||||
end
|
||||
for i = 1:length(thetalist)
|
||||
if i == 1
|
||||
robot.T(:,:,i) = robot.TW(:,:,i);
|
||||
else
|
||||
robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i);
|
||||
end
|
||||
robot.R(:,:,i) = robot.T(1:3,1:3,i);
|
||||
robot.t(:,:,i) = robot.T(1:3,4,i);
|
||||
end
|
||||
robot.Fkine = robot.TW(:,:,end);
|
||||
|
||||
%stack result into kine structure
|
||||
robot.kine.TW = robot.TW;
|
||||
robot.kine.T = robot.T;
|
||||
robot.kine.Fkine = robot.Fkine;
|
||||
% get the CG at the world base frame
|
||||
% FIXME: Fix this hack
|
||||
ct=[];
|
||||
Mlist_CG_Base=[];
|
||||
com_pos_R1 = robot.com_pos_R1;
|
||||
com_pos_R2 = robot.com_pos_R2;
|
||||
for i = 1:length(thetalist)
|
||||
if i == 1
|
||||
ct(:,i) = com_pos_R1(:,i);
|
||||
elseif i< 9
|
||||
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||
else
|
||||
% HACK
|
||||
ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
||||
end
|
||||
robot.Home.com(:,i) = ct(:,i);
|
||||
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
||||
Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
|
||||
end
|
||||
% get the CG at the last GC frame
|
||||
Mlist_CG=[];
|
||||
for i = 1:length(thetalist)
|
||||
if i == 1
|
||||
Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
|
||||
else
|
||||
Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
|
||||
end
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% Get the end efforce transformation in each joint reference frame
|
||||
Mlist_ED = [];
|
||||
for i = 1:length(thetalist)
|
||||
M = robot.T(:,:,i);
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
% stack result
|
||||
robot.kine.Mlist_CG_Base = Mlist_CG_Base;
|
||||
robot.kine.Mlist_CG = Mlist_CG;
|
||||
robot.kine.Mlist_ED = Mlist_ED;
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
theta = robot.theta;
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ dv = robot.vel.dv ;
|
|||
switch opt.LD_method
|
||||
case 'Direct'
|
||||
switch opt.KM_method
|
||||
case 'MDH'
|
||||
case {'MDH' , 'SCREW'}
|
||||
for i = 1:ndof
|
||||
p_skew(:,:,i) = vec2skewSymMat(P(:,:,i));
|
||||
w_skew(:,:,i) = vec2skewSymMat(w(:,i));
|
||||
|
|
|
|||
|
|
@ -1,175 +0,0 @@
|
|||
function robot = get_robot_R1000(file,opt)
|
||||
switch opt.robot_def
|
||||
case 'direct'
|
||||
ndof = 9;
|
||||
robot.ndof = ndof;
|
||||
% Kinematics parameters
|
||||
if(opt.Isreal)
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0];
|
||||
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896];
|
||||
robot.d = [0,0,0,0,0,0.28,0.40,0,0];
|
||||
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
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],'real');
|
||||
qd_sym = sym('qd%d',[ndof,1],'real');
|
||||
q2d_sym = sym('qdd%d',[ndof,1],'real');
|
||||
|
||||
robot.theta = q_sym;
|
||||
robot.dtheta = qd_sym;
|
||||
robot.ddtheta = q2d_sym;
|
||||
%R1000 ISA
|
||||
robot.theta(ndof) = 0;
|
||||
robot.dtheta(ndof) = 0;
|
||||
robot.ddtheta(ndof) = 0;
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];
|
||||
robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157];
|
||||
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
robot.d = sym(robot.d);
|
||||
robot.d(ndof)=q_sym(ndof);
|
||||
%init vd
|
||||
robot.vd = robot.d;
|
||||
robot.vd(ndof)=q_sym(ndof);
|
||||
%init accd
|
||||
robot.accd = robot.d;
|
||||
robot.accd(ndof)=q_sym(ndof);
|
||||
end
|
||||
% Dynamics parameters
|
||||
link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
|
||||
%TODO in process, seems axis_of_rot useless
|
||||
axis_of_rot(:,1) = [0;0;1];
|
||||
axis_of_rot(:,2) = [0;-1;0];
|
||||
axis_of_rot(:,3) = [0;-1;0];
|
||||
axis_of_rot(:,4) = [0;-1;0];
|
||||
axis_of_rot(:,5) = [0;0;1];
|
||||
axis_of_rot(:,6) = [1;0;0];
|
||||
axis_of_rot(:,7) = [0;0;-1];
|
||||
axis_of_rot(:,8) = [0;-1;0];
|
||||
axis_of_rot(:,9) = [0;0;0];
|
||||
% 画图
|
||||
% com_pos(:,1) = [0;0;0.122];
|
||||
% com_pos(:,2) = [0.373;0;0];
|
||||
% com_pos(:,3) = [0.188;0;0];
|
||||
% com_pos(:,4) = [0.05;0;-0.05];
|
||||
% com_pos(:,5) = [0;0.13;0];
|
||||
% com_pos(:,6) = [0;0.028;0.049];
|
||||
% com_pos(:,7) = [0;0;0.102];
|
||||
% com_pos(:,8) = [0;0.06;0];
|
||||
% com_pos(:,9) = [0;0;0];
|
||||
|
||||
com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
|
||||
com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3;
|
||||
com_pos(:,3) = [1.8811711e+02 4.9225523e-04 -7.9651429e+00]'*10^-3;
|
||||
com_pos(:,4) = [5.4048685e+01 5.8463901e+01 -5.0915631e+00]'*10^-3;
|
||||
com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
|
||||
com_pos(:,6) = [4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
|
||||
com_pos(:,7) = [2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
|
||||
com_pos(:,8) = [-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
|
||||
com_pos(:,9) = [-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
||||
|
||||
com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
|
||||
com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
|
||||
com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3;
|
||||
com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3;
|
||||
com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3;
|
||||
com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3;
|
||||
com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3;
|
||||
com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3;
|
||||
com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
|
||||
com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3;
|
||||
com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
|
||||
com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3;
|
||||
com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
|
||||
com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3;
|
||||
com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
|
||||
com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use
|
||||
com_pos_R1(:,9)=[0 0 0]'*10^-3;
|
||||
|
||||
% 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]);
|
||||
% link_inertia(:,:,3) = diag([1,1,1]);
|
||||
% link_inertia(:,:,4) = diag([1,1,1]);
|
||||
% link_inertia(:,:,5) = diag([1,1,1]);
|
||||
% link_inertia(:,:,6) = diag([1,1,1]);
|
||||
% link_inertia(:,:,7) = diag([1,1,1]);
|
||||
% link_inertia(:,:,8) = diag([1,1,1]);
|
||||
% link_inertia(:,:,9) = diag([1,1,1]);
|
||||
link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];...
|
||||
[-1.1694021e+04 2.3289532e+05 -3.0395414e+02];...
|
||||
[-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6;
|
||||
link_inertia(:,:,2) = [[3.7926328e+04 -9.0569033e+01 4.7526575e+04];...
|
||||
[-9.0569033e+01 2.9714754e+05 6.8396715e+00];...
|
||||
[4.7526575e+04 6.8396715e+00 2.8138392e+05]]*10^-6;
|
||||
link_inertia(:,:,3) = [[4.4513887e+03 1.9981964e-01 -3.0303891e+02];...
|
||||
[1.9981964e-01 6.7952039e+04 -8.8585864e-02];...
|
||||
[-3.0303891e+02 -8.8585864e-02 6.9958344e+04]]*10^-6;
|
||||
link_inertia(:,:,4) = [[1.1642351e+04 2.2997175e+03 2.9159431e+03];...
|
||||
[2.2997175e+03 2.6031269e+04 -1.3518384e+02];...
|
||||
[2.9159431e+03 -1.3518384e+02 2.4694742e+04]]*10^-6;
|
||||
link_inertia(:,:,5) = [[3.0930544e+03 8.3558814e-01 -2.8169092e+03];...
|
||||
[8.3558814e-01 1.2796446e+04 -3.3666469e+00];...
|
||||
[-2.8169092e+03 -3.3666469e+00 1.2128856e+04]]*10^-6;
|
||||
link_inertia(:,:,6) = [[3.6635776e+03 -7.0081461e+00 2.2392870e+00];...
|
||||
[-7.0081461e+00 1.8152305e+03 -2.4828765e+02];...
|
||||
[2.2392870e+00 -2.4828765e+02 3.4602935e+03]]*10^-6;
|
||||
link_inertia(:,:,7) = [[1.3662652e+04 -3.6340953e+00 4.4011670e-01];...
|
||||
[-3.6340953e+00 1.3222824e+04 -4.3625500e+02];...
|
||||
[ 4.4011670e-01 -4.3625500e+02 2.2500397e+03]]*10^-6;
|
||||
link_inertia(:,:,8) = [[1.5162872e+03 -4.6245133e+01 4.4556929e+03];...
|
||||
[-4.6245133e+01 5.9901606e+04 6.0350548e+0];...
|
||||
[4.4556929e+03 6.0350548e+0 5.8819998e+04]]*10^-6;
|
||||
link_inertia(:,:,9) = [[1.1730106e+03 3.3834506e+0 4.6097678e+01];...
|
||||
[3.3834506e+0 1.7996852e+03 5.2088778e+0];...
|
||||
[4.6097678e+01 5.2088778e+0 1.3960734e+03]]*10^-6;
|
||||
% verify if link_inertia is issymmetric
|
||||
for i = 1:ndof
|
||||
if(issymmetric(link_inertia(:,:,i))==false)
|
||||
fprintf('Bad definition of inertia matrix %d\n',i)
|
||||
return;
|
||||
end
|
||||
end
|
||||
% 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(i)*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(:,i));
|
||||
robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia(:,:,i)-...
|
||||
link_mass(i)*com_vec2mat*com_vec2mat);
|
||||
robot.pi(:,i) = [robot.m(i);robot.mc(:,i);robot.I_vec(:,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;
|
||||
|
|
@ -3,56 +3,9 @@ switch opt.robot_def
|
|||
case 'direct'
|
||||
ndof = 9;
|
||||
robot.ndof = ndof;
|
||||
% Kinematics parameters
|
||||
if(opt.Isreal)
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0];
|
||||
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896];
|
||||
robot.d = [0,0,0,0,0,0.28,0.40,0,0];
|
||||
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
case 'SCREW'
|
||||
robot.theta = zeros(1,9);
|
||||
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],'real');
|
||||
qd_sym = sym('qd%d',[ndof,1],'real');
|
||||
q2d_sym = sym('qdd%d',[ndof,1],'real');
|
||||
|
||||
robot.theta = q_sym;
|
||||
robot.dtheta = qd_sym;
|
||||
robot.ddtheta = q2d_sym;
|
||||
%R1000 ISA
|
||||
robot.theta(ndof) = 0;
|
||||
robot.dtheta(ndof) = 0;
|
||||
robot.ddtheta(ndof) = 0;
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];
|
||||
robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157];
|
||||
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
robot.d = sym(robot.d);
|
||||
robot.d(ndof)=q_sym(ndof);
|
||||
%init vd
|
||||
robot.vd = robot.d;
|
||||
robot.vd(ndof)=q_sym(ndof);
|
||||
%init accd
|
||||
robot.accd = robot.d;
|
||||
robot.accd(ndof)=q_sym(ndof);
|
||||
end
|
||||
% ------------------------------------------------------------------------
|
||||
% Dynamics parameters
|
||||
% ------------------------------------------------------------------------
|
||||
link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
|
||||
%TODO in process, seems axis_of_rot useless
|
||||
axis_of_rot(:,1) = [0;0;1];
|
||||
|
|
@ -64,26 +17,6 @@ switch opt.robot_def
|
|||
axis_of_rot(:,7) = [0;0;-1];
|
||||
axis_of_rot(:,8) = [0;-1;0];
|
||||
axis_of_rot(:,9) = [0;0;0];
|
||||
% 画图
|
||||
% com_pos(:,1) = [0;0;0.122];
|
||||
% com_pos(:,2) = [0.373;0;0];
|
||||
% com_pos(:,3) = [0.188;0;0];
|
||||
% com_pos(:,4) = [0.05;0;-0.05];
|
||||
% com_pos(:,5) = [0;0.13;0];
|
||||
% com_pos(:,6) = [0;0.028;0.049];
|
||||
% com_pos(:,7) = [0;0;0.102];
|
||||
% com_pos(:,8) = [0;0.06;0];
|
||||
% com_pos(:,9) = [0;0;0];
|
||||
|
||||
com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
|
||||
com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3;
|
||||
com_pos(:,3) = [1.8811711e+02 4.9225523e-04 -7.9651429e+00]'*10^-3;
|
||||
com_pos(:,4) = [5.4048685e+01 5.8463901e+01 -5.0915631e+00]'*10^-3;
|
||||
com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3;
|
||||
com_pos(:,6) = [4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3;
|
||||
com_pos(:,7) = [2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3;
|
||||
com_pos(:,8) = [-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
|
||||
com_pos(:,9) = [-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
||||
|
||||
com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3;
|
||||
com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3;
|
||||
|
|
@ -101,8 +34,25 @@ switch opt.robot_def
|
|||
com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3;
|
||||
com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3;
|
||||
com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use
|
||||
com_pos_R1(:,9)=[0 0 0]'*10^-3;
|
||||
|
||||
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
||||
% stack result
|
||||
robot.com_pos_R1 = com_pos_R1;
|
||||
robot.com_pos_R2 = com_pos_R2;
|
||||
% FIXME:fix this hack
|
||||
% Get 3D coordinate of CO
|
||||
co=[];
|
||||
for i = 1:8
|
||||
if i == 1
|
||||
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
|
||||
elseif i<8
|
||||
%From base to ISA Origin
|
||||
co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
|
||||
else
|
||||
%From base to ISA Origin
|
||||
co(:,i) = co(:,i-1)-[0;0;0.05896];
|
||||
end
|
||||
end
|
||||
co = [zeros(3,1),co];
|
||||
% 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]);
|
||||
|
|
@ -149,6 +99,7 @@ switch opt.robot_def
|
|||
end
|
||||
end
|
||||
% manipulator regressor
|
||||
com_pos = com_pos_R1;
|
||||
for i = 1:ndof
|
||||
robot.m(i) = link_mass(i);
|
||||
robot.axis(:,i) = axis_of_rot(i);
|
||||
|
|
@ -162,6 +113,106 @@ switch opt.robot_def
|
|||
link_mass(i)*com_vec2mat*com_vec2mat);
|
||||
robot.pi(:,i) = [robot.m(i);robot.mc(:,i);robot.I_vec(:,i)];
|
||||
end
|
||||
% ------------------------------------------------------------------------
|
||||
% Kinematics parameters
|
||||
% ------------------------------------------------------------------------
|
||||
if(opt.Isreal)
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0];
|
||||
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896];
|
||||
robot.d = [0,0,0,0,0,0.28,0.40,0,0];
|
||||
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
case 'SCREW'
|
||||
robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]];
|
||||
robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,3) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,4) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,5) = [[0;-1;0],[1;0;0],[0;0;1]];
|
||||
robot.Home.R(:,:,6) = [[0;-1;0],[0;0;-1],[1;0;0]];
|
||||
robot.Home.R(:,:,7) = [[1;0;0],[0;-1;0],[0;0;-1]];
|
||||
robot.Home.R(:,:,8) = [[0;0;-1],[1;0;0],[0;-1;0]];
|
||||
robot.Home.R(:,:,9) = [[0;0;-1],[0;1;0],[1;0;0]];
|
||||
for i=1:9
|
||||
robot.Home.P(:,i) = co(:,i);
|
||||
robot.Home.M(:,:,i) = RpToTrans(robot.Home.R(:,:,i),robot.Home.P(:,i));
|
||||
end
|
||||
robot.slist=[[0;0;1;co(:,1)],...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,2))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,3))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,4))]...
|
||||
[0;0;1;cross(-[0;0;1],co(:,5))]...
|
||||
[1;0;0;cross(-[1;0;0],co(:,6))]...
|
||||
[0;0;-1;cross(-[0;0;-1],co(:,7))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
|
||||
[0;0;0;1;0;0]];
|
||||
robot.theta = zeros(1,9);
|
||||
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],'real');
|
||||
qd_sym = sym('qd%d',[ndof,1],'real');
|
||||
q2d_sym = sym('qdd%d',[ndof,1],'real');
|
||||
|
||||
robot.theta = q_sym;
|
||||
%FIXME: only consider the theta temply
|
||||
robot.dtheta = zeros([ndof,1]);
|
||||
robot.ddtheta = zeros([ndof,1]);
|
||||
% robot.dtheta = qd_sym;
|
||||
% robot.ddtheta = q2d_sym;
|
||||
% %R1000 ISA ??
|
||||
% robot.theta(ndof) = 0;
|
||||
% robot.dtheta(ndof) = 0;
|
||||
% robot.ddtheta(ndof) = 0;
|
||||
switch opt.KM_method
|
||||
case 'SCREW'
|
||||
robot.Home.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]];
|
||||
robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,3) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,4) = [[1;0;0],[0;0;1],[0;-1;0]];
|
||||
robot.Home.R(:,:,5) = [[0;-1;0],[1;0;0],[0;0;1]];
|
||||
robot.Home.R(:,:,6) = [[0;-1;0],[0;0;-1],[1;0;0]];
|
||||
robot.Home.R(:,:,7) = [[1;0;0],[0;-1;0],[0;0;-1]];
|
||||
robot.Home.R(:,:,8) = [[0;0;-1],[1;0;0],[0;-1;0]];
|
||||
robot.Home.R(:,:,9) = [[0;0;-1],[0;1;0],[1;0;0]];
|
||||
for i=1:9
|
||||
robot.Home.P(:,i) = co(:,i);
|
||||
robot.Home.M(:,:,i) = RpToTrans(robot.Home.R(:,:,i),robot.Home.P(:,i));
|
||||
end
|
||||
robot.slist=[[0;0;1;co(:,1)],...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,2))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,3))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,4))]...
|
||||
[0;0;1;cross(-[0;0;1],co(:,5))]...
|
||||
[1;0;0;cross(-[1;0;0],co(:,6))]...
|
||||
[0;0;-1;cross(-[0;0;-1],co(:,7))]...
|
||||
[0;-1;0;cross(-[0;-1;0],co(:,8))]...
|
||||
[0;0;0;1;0;0]];
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493];
|
||||
robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157];
|
||||
robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2];
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
robot.d = sym(robot.d);
|
||||
robot.d(ndof)=q_sym(ndof);
|
||||
%init vd
|
||||
robot.vd = robot.d;
|
||||
robot.vd(ndof)=q_sym(ndof);
|
||||
%init accd
|
||||
robot.accd = robot.d;
|
||||
robot.accd(ndof)=q_sym(ndof);
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
end
|
||||
end
|
||||
|
||||
case 'urdf'
|
||||
robot = parse_urdf(file);
|
||||
case 'mat'
|
||||
|
|
|
|||
|
|
@ -1,5 +1,20 @@
|
|||
function robot = get_velocity(robot, opt)
|
||||
switch opt.KM_method
|
||||
case 'SCREW'
|
||||
% init q
|
||||
% q = robot.theta;
|
||||
% qd = robot.dtheta;
|
||||
% qdd = robot.ddtheta;
|
||||
% [V,Vd,~,~,~] = InverseDynamics_debug(thetalist, dthetalist, ddthetalist, ...
|
||||
% g, Ftip,Mlist, Glist, Slist);
|
||||
% robot.vel.w = w;
|
||||
% robot.vel.v = v;
|
||||
% robot.vel.dw = dw;
|
||||
% robot.vel.dv = dv;
|
||||
robot.vel.w = zeros(3,robot.ndof);
|
||||
robot.vel.v = zeros(3,robot.ndof);
|
||||
robot.vel.dw = zeros(3,robot.ndof);
|
||||
robot.vel.dv = zeros(3,robot.ndof);
|
||||
case 'SDH'
|
||||
case 'MDH'
|
||||
switch opt.Vel_method
|
||||
|
|
|
|||
|
|
@ -0,0 +1,81 @@
|
|||
% 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);
|
||||
%% Check if screw method is equal to regressor
|
||||
isequal(simplify(tau),simplify(tau_mat))
|
||||
%% 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
|
||||
Loading…
Reference in New Issue