feature/R1000-identification #2
|
|
@ -10,13 +10,13 @@ opt.reGenerate = false;
|
|||
opt.Isreal = true;
|
||||
|
||||
robot = get_robot_R1000(file,opt);
|
||||
robot = get_Kinematics(robot, opt);
|
||||
% robot = get_Kinematics(robot, opt);
|
||||
|
||||
R1000_Dynamics_num;
|
||||
% R1000_Dynamics;
|
||||
% getGravityForce;
|
||||
robot = get_velocity(robot, opt);
|
||||
robot = get_regressor(robot,opt);
|
||||
% robot = get_velocity(robot, opt);
|
||||
% robot = get_regressor(robot,opt);
|
||||
% symbol matched
|
||||
% verify_regressor_R1000;
|
||||
% robot = get_baseParams(robot, opt);
|
||||
|
|
|
|||
|
|
@ -13,17 +13,17 @@ link_mass = robot.m;
|
|||
com_pos = robot.com;
|
||||
link_inertia = robot.I;
|
||||
|
||||
thetalist = [zero_;zero_;q_J;q_J;zero_;zero_;zero_;zero_;zero_]';
|
||||
dthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
|
||||
ddthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
|
||||
thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
dthetalist = [zero_;q_J;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
|
||||
link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
|
||||
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
||||
Glist = cat(3, Glist, Gb);
|
||||
end
|
||||
% Glist=[];
|
||||
% for i = 1:N
|
||||
% link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
|
||||
% 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 = [];
|
||||
|
|
@ -89,43 +89,43 @@ end
|
|||
% 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
|
||||
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
|
||||
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:N
|
||||
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: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);
|
||||
% com_pos_R1 = robot.com_pos_R1;
|
||||
% com_pos_R2 = robot.com_pos_R2;
|
||||
% ct=[];
|
||||
% Mlist_CG_Base=[];
|
||||
% for i = 1:N
|
||||
% if i == 1
|
||||
% ct(:,i) = com_pos_R1(:,i);
|
||||
% else
|
||||
% ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||
% % else
|
||||
% % 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:N
|
||||
% 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: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
|
||||
|
|
@ -136,40 +136,42 @@ 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));
|
||||
Js(:,:,i) = JacobianSpace(Slist, thetalist(i,:)');
|
||||
VV = Js(:,:,i)*dthetalist(i,:)';
|
||||
% [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
|
||||
% 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 +1,256 @@
|
|||
function robot = get_robot_R1000(file,opt)
|
||||
switch opt.robot_def
|
||||
case 'direct'
|
||||
ndof = 9;
|
||||
robot.ndof = ndof;
|
||||
% ------------------------------------------------------------------------
|
||||
% Dynamics parameters
|
||||
% ------------------------------------------------------------------------
|
||||
link_mass = [2.1315260e+01,1.9235354e+01,1.0463871e+01,6.1538806,3.0882893,2.4450941,3.6376589,1.7005420,9.1507819e-1];
|
||||
% DVT
|
||||
% 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];
|
||||
|
||||
%DVT
|
||||
% 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;
|
||||
|
||||
com_pos_R1(:,1)=[ 1.0440117e+02 -1.1125559e+01 1.2822933e+02]'*1e-3;
|
||||
com_pos_R2(:,1)=[-8.0598830e+01 -4.2225559e+01 -1.5870672e+01]'*1e-3;
|
||||
com_pos_R1(:,2)=[ 3.5562148e+02 1.2378198e+01 1.8395830e-03]'*1e-3;
|
||||
com_pos_R2(:,2)=[-2.4437852e+02 9.4778198e+01 1.8395830e-03]'*1e-3;
|
||||
com_pos_R1(:,3)=[2.4798242e+02 -3.5267916e+01 4.8648267e-02]'*1e-3;
|
||||
com_pos_R2(:,3)=[-3.5201758e+02 -2.9007916e+01 4.8648266e-02]'*1e-3;
|
||||
com_pos_R1(:,4)=[ 4.2341069e+01 5.2603527e+01 1.2271365e+01]'*1e-3;
|
||||
com_pos_R2(:,4)=[-7.7658931e+01 -1.2956223e+01 1.9271365e+01]'*1e-3;
|
||||
com_pos_R1(:,5)=[1.0177553e+02 -3.4868152e-02 1.5686100e+00]'*1e-3;
|
||||
com_pos_R2(:,5)=[-1.0372447e+02 -3.9239008e-02 -5.4313602e+00]'*1e-3;
|
||||
com_pos_R1(:,6)=[5.9942347e+01 -4.8346177e-02 5.5784018e+00]'*1e-3;
|
||||
com_pos_R2(:,6)=[-2.3757653e+01 -4.8346177e-02 4.0028402e+01]'*1e-3;
|
||||
com_pos_R1(:,7)=[4.9494392e-02 -2.4240394e+00 -1.0332489e+02]'*1e-3;
|
||||
com_pos_R2(:,7)=[4.9494392e-02 -2.4240394e+00 2.7222511e+02]'*1e-3;
|
||||
com_pos_R1(:,8)=[-2.7158498e+01 -3.3865875e-01 -4.6588829e+01]'*1e-3;
|
||||
com_pos_R2(:,8)=[ 2.2423433e+02 -3.3865729e-01 1.9671171e+01]'*1e-3; % don't use
|
||||
com_pos_R1(:,9)=[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-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);
|
||||
else
|
||||
%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) = [[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
|
||||
com_pos = com_pos_R1;
|
||||
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
|
||||
% ------------------------------------------------------------------------
|
||||
% Kinematics parameters
|
||||
% ------------------------------------------------------------------------
|
||||
if(opt.Isreal)
|
||||
%FIXME: only consider the theta temply
|
||||
robot.dtheta = zeros([ndof,1]);
|
||||
robot.ddtheta = zeros([ndof,1]);
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
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];
|
||||
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.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([ndof,1]);
|
||||
% robot.theta(end-1) = pi/4;
|
||||
% robot.theta(3) = pi/4;
|
||||
% robot.theta(4) = pi/4;
|
||||
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]];
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
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'
|
||||
robot = [];
|
||||
disp('TODO mat robot define options!')
|
||||
otherwise
|
||||
robot = [];
|
||||
disp('Bad robot define options!')
|
||||
return
|
||||
end
|
||||
%Gravity
|
||||
gravity = [0;0;-9.806];
|
||||
robot.gravity = gravity;
|
||||
|
|
@ -6,7 +6,9 @@ switch opt.robot_def
|
|||
% ------------------------------------------------------------------------
|
||||
% Dynamics parameters
|
||||
% ------------------------------------------------------------------------
|
||||
link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2];
|
||||
link_mass = [2.1315260e+01,1.9235354e+01,1.0463871e+01,6.1538806,3.0882893,2.4450941,3.6376589,1.7005420,9.1507819e-1];
|
||||
% DVT
|
||||
% 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];
|
||||
|
|
@ -18,23 +20,42 @@ switch opt.robot_def
|
|||
axis_of_rot(:,8) = [0;-1;0];
|
||||
axis_of_rot(:,9) = [0;0;0];
|
||||
|
||||
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;
|
||||
%DVT
|
||||
% 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;
|
||||
|
||||
com_pos_R1(:,1)=[ 1.0440117e+02 -1.1125559e+01 1.2822933e+02]'*1e-3;
|
||||
com_pos_R2(:,1)=[-8.0598830e+01 -4.2225559e+01 -1.5870672e+01]'*1e-3;
|
||||
com_pos_R1(:,2)=[ 3.5562148e+02 1.2378198e+01 1.8395830e-03]'*1e-3;
|
||||
com_pos_R2(:,2)=[-2.4437852e+02 9.4778198e+01 1.8395830e-03]'*1e-3;
|
||||
com_pos_R1(:,3)=[2.4798242e+02 -3.5267916e+01 4.8648267e-02]'*1e-3;
|
||||
com_pos_R2(:,3)=[-3.5201758e+02 -2.9007916e+01 4.8648266e-02]'*1e-3;
|
||||
com_pos_R1(:,4)=[ 4.2341069e+01 5.2603527e+01 1.2271365e+01]'*1e-3;
|
||||
com_pos_R2(:,4)=[-7.7658931e+01 -1.2956223e+01 1.9271365e+01]'*1e-3;
|
||||
com_pos_R1(:,5)=[1.0177553e+02 -3.4868152e-02 1.5686100e+00]'*1e-3;
|
||||
com_pos_R2(:,5)=[-1.0372447e+02 -3.9239008e-02 -5.4313602e+00]'*1e-3;
|
||||
com_pos_R1(:,6)=[5.9942347e+01 -4.8346177e-02 5.5784018e+00]'*1e-3;
|
||||
com_pos_R2(:,6)=[-2.3757653e+01 -4.8346177e-02 4.0028402e+01]'*1e-3;
|
||||
com_pos_R1(:,7)=[4.9494392e-02 -2.4240394e+00 -1.0332489e+02]'*1e-3;
|
||||
com_pos_R2(:,7)=[4.9494392e-02 -2.4240394e+00 2.7222511e+02]'*1e-3;
|
||||
com_pos_R1(:,8)=[-2.7158498e+01 -3.3865875e-01 -4.6588829e+01]'*1e-3;
|
||||
com_pos_R2(:,8)=[ 2.2423433e+02 -3.3865729e-01 1.9671171e+01]'*1e-3; % don't use
|
||||
com_pos_R1(:,9)=[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3;
|
||||
% stack result
|
||||
robot.com_pos_R1 = com_pos_R1;
|
||||
robot.com_pos_R2 = com_pos_R2;
|
||||
|
|
@ -44,26 +65,17 @@ switch opt.robot_def
|
|||
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];
|
||||
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]);
|
||||
% 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;
|
||||
|
|
@ -118,8 +130,8 @@ switch opt.robot_def
|
|||
% ------------------------------------------------------------------------
|
||||
if(opt.Isreal)
|
||||
%FIXME: only consider the theta temply
|
||||
robot.dtheta = 1000*pi/4*ones([ndof,1]);
|
||||
robot.ddtheta = 1000*pi/4*ones([ndof,1]);
|
||||
robot.dtheta = zeros([ndof,1]);
|
||||
robot.ddtheta = zeros([ndof,1]);
|
||||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
switch opt.KM_method
|
||||
case 'SDH'
|
||||
|
|
|
|||
119
get_velocity.asv
119
get_velocity.asv
|
|
@ -1,119 +0,0 @@
|
|||
function robot = get_velocity(robot, opt)
|
||||
switch opt.KM_method
|
||||
case 'SCREW'
|
||||
Mlist_CG = robot.kine.Mlist_CG;
|
||||
% Get general mass matrix
|
||||
link_mass = robot.m;
|
||||
com_pos = robot.com;
|
||||
link_inertia = robot.I;
|
||||
Slist=robot.slist;
|
||||
Glist=[];
|
||||
for i = 1:robot.ndof
|
||||
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
||||
Glist = cat(3, Glist, Gb);
|
||||
% ?
|
||||
% mc = robot.TW(1:3,1:3,i)*robot.m(i)*robot.com(:,i);
|
||||
% robot.pi(2:4,i) = mc;
|
||||
% hack: because we only know the com in the world frame
|
||||
% the inv of rotation is beacause we want to the com in
|
||||
% our defined frame. R*com is the wrong result
|
||||
robot.pi(2:4,i) = robot.Home.R(:,:,i)'*robot.pi(2:4,i);
|
||||
com_vec2mat = vec2skewSymMat(robot.com(:,i)); %com
|
||||
robot.I(:,:,i) = robot.I(:,:,i)-link_mass(i)*com_vec2mat*com_vec2mat;
|
||||
robot.I(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
|
||||
robot.I_vec(:,i) = inertiaMatrix2Vector(robot.I(:,:,i));
|
||||
robot.pi(5:end,i) = robot.I_vec(:,i);
|
||||
end
|
||||
if opt.Isreal
|
||||
% init q
|
||||
q = robot.theta;
|
||||
qd = robot.dtheta;
|
||||
qdd = robot.ddtheta;
|
||||
[V,Vd,~,~,~] = InverseDynamics_debug(q, qd, qdd, ...
|
||||
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
|
||||
else
|
||||
% init q
|
||||
q = robot.theta;
|
||||
qd = robot.dtheta;
|
||||
qdd = robot.ddtheta;
|
||||
[V,Vd,~,~,~] = InverseDynamics_sym(q, qd, qdd, ...
|
||||
robot.gravity, [0;0;0;0;0;0],Mlist_CG, Glist, Slist);
|
||||
end
|
||||
% FIXME: twist is not equal to velocity
|
||||
% FIXME: Need to get the velocity represent at base frame
|
||||
robot.vel.w = V(1:3,2:end);
|
||||
for i = 1:robot.ndof
|
||||
robot.vel.v(:,i) = V(4:6,i+1)-vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*;
|
||||
end
|
||||
% robot.vel.v = V(4:6,2:end);
|
||||
robot.vel.dw = Vd(1:3,2:end);
|
||||
robot.vel.dv = Vd(4:6,2:end);
|
||||
% robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
|
||||
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);
|
||||
link_type = robot.link_type;
|
||||
% init q
|
||||
q = robot.theta;
|
||||
qd = robot.dtheta;
|
||||
qdd = robot.ddtheta;
|
||||
% for i = 1:robot.ndof
|
||||
% switch link_type(i)
|
||||
% case 'R'
|
||||
% %Do nothing
|
||||
% case 'P'
|
||||
% q(i) = robot.d(i);
|
||||
% qd(i) = robot.vd(i);
|
||||
% qdd(i) = robot.accd(i);
|
||||
% end
|
||||
% end
|
||||
R = robot.kine.R;
|
||||
P = robot.kine.t;
|
||||
% 1-n外推公式 参考robotics toolbox
|
||||
%第一关节
|
||||
switch link_type(1)
|
||||
case 'R' %revolute
|
||||
w(:,1) = R(:,:,1)' * w0 + qd(1) * Z;
|
||||
v(:,1) = R(:,:,1)' * v0 + cross(w0,P(:,1));
|
||||
dw(:,1) = R(:,:,1)' * dw0 + cross(R(:,:,1)' * w0, qd(1) * Z) + qdd(1) * Z;
|
||||
dv(:,1) = R(:,:,1)' * (cross(dw0,P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0);
|
||||
case 'P' %prismatic
|
||||
w(:,1) = R(:,:,1)' * w0;
|
||||
v(:,1) = R(:,:,1)' * (Z*qd(1)+v0) + cross(w0, P(:,1));
|
||||
dw(:,1) = R(:,:,1)' * dw0;
|
||||
dv(:,1) = R(:,:,1)' * (cross(dw0, P(:,1)) + cross(w0, cross(w0, P(:,1))) + dv0)+...
|
||||
2*cross(R(:,:,1)' * w0, Z * qd(1)) + Z * qdd(1);
|
||||
end
|
||||
%后面n-1关节
|
||||
for i = 1:robot.ndof-1
|
||||
% switch link_type(i)
|
||||
% case 'R' %revolute
|
||||
w(:,i+1) = R(:,:,i+1)' * w(:,i) + qd(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), qd(i+1) * Z)+ qdd(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));
|
||||
% case 'P' %prismatic
|
||||
% w(:,i+1) = R(:,:,i+1)' * w(:,i);
|
||||
% v(:,i+1) = R(:,:,i+1)' * (Z*qd(:,i)+v(:,i)) + cross(w(:,i), P(:,i+1));
|
||||
% dw(:,i+1) = R(:,:,i+1)' * dw(:,i);
|
||||
% dv(:,i+1) = R(:,:,i+1)' * (cross(dw(:,i), P(:,i+1)) + cross(w(:,i), cross(w(:,i), P(:,i+1))) + dv(:,i))+...
|
||||
% 2*cross(R(:,:,i+1)' * w(:,i), Z * qd(:,i)) + Z * qdd(:,i);
|
||||
% end
|
||||
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
|
||||
Loading…
Reference in New Issue