feature/R1000-identification #2
|
|
@ -31,7 +31,7 @@ com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't us
|
|||
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
||||
|
||||
% Get 3D coordinate of CO
|
||||
co=[]
|
||||
co=[];
|
||||
for i = 1:8
|
||||
if i == 1
|
||||
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
|
||||
|
|
@ -46,17 +46,16 @@ end
|
|||
co = [zeros(3,1),co];
|
||||
|
||||
% temp slist
|
||||
robot.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])]...
|
||||
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(4:6,:) = co;
|
||||
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]];
|
||||
|
|
@ -74,7 +73,7 @@ robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
|||
|
||||
robot = get_Kinematics(robot, opt);
|
||||
|
||||
% R1000_Dynamics_num;
|
||||
R1000_Dynamics_num;
|
||||
% opt.Isreal = false;
|
||||
% robot = get_velocity(robot, opt);
|
||||
% robot = get_regressor(robot,opt);
|
||||
|
|
|
|||
|
|
@ -24,51 +24,94 @@ for i = 1:N
|
|||
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);
|
||||
% 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 = 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+1)*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);
|
||||
|
||||
% ct=[];
|
||||
% Mlist_CG=[];
|
||||
% for i = 1:N
|
||||
% if i == 1
|
||||
% ct(:,i) = com_pos_R1(:,i);
|
||||
% elseif i< 9
|
||||
% ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||
% else
|
||||
% ct(:,i) = -com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
||||
% end
|
||||
% robot.Home.com(:,i) = ct(:,i);
|
||||
% M = RpToTrans(robot.T(1:3,1:3,i),robot.Home.R(:,:,i)*robot.Home.com(:,i));
|
||||
% 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)));
|
||||
% M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,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)));
|
||||
% M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,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);
|
||||
|
||||
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
|
||||
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
|
||||
|
|
@ -80,15 +123,7 @@ 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]];
|
||||
Slist=robot.slist;
|
||||
|
||||
Vlinear=sym(zeros(3,3));
|
||||
J=sym(zeros(6,N));
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@ if(opt.Isreal)
|
|||
end
|
||||
robot.TW(:,:,j) = T;
|
||||
end
|
||||
for i = 1:size(thetalist)
|
||||
for i = 1:length(thetalist)
|
||||
if i == 1
|
||||
robot.T(:,:,i) = robot.TW(:,:,i);
|
||||
else
|
||||
|
|
|
|||
|
|
@ -15,6 +15,8 @@ switch opt.robot_def
|
|||
robot.link_type = ['R','R','R','R','R','R','R','R','P'];
|
||||
case 'SCREW'
|
||||
robot.theta = zeros(1,9);
|
||||
robot.theta(5) = pi/3;
|
||||
robot.theta(7) = pi/3;
|
||||
otherwise
|
||||
disp('Bad opt.KM_method!')
|
||||
return;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,81 @@
|
|||
Mlist_CG=[]
|
||||
for i = 0:N-1
|
||||
if i == 0
|
||||
M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
else
|
||||
% rotation_i = TransToRp(robot.T(:,:,i+1))
|
||||
M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
end
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
end
|
||||
%%
|
||||
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,com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,com_pos(:,i+1));
|
||||
end
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
end
|
||||
%%
|
||||
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 this
|
||||
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
||||
% plot 3D: Get 3D coordinate of COM
|
||||
ct=[];
|
||||
for i = 1:9
|
||||
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
|
||||
end
|
||||
plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r')
|
||||
axis equal
|
||||
grid on
|
||||
% plot 3D: 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];
|
||||
hold on
|
||||
plot3(co(1,:),co(2,:),co(3,:),'o','Color','b')
|
||||
axis equal
|
||||
grid on
|
||||
%%
|
||||
for i = 1:9
|
||||
co(:,i+1)-co(:,i);
|
||||
end
|
||||
%%
|
||||
robot.
|
||||
16
untitled2.m
16
untitled2.m
|
|
@ -42,7 +42,7 @@ 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 this
|
||||
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3;
|
||||
% plot 3D: Get 3D coordinate of COM
|
||||
ct=[]
|
||||
ct=[];
|
||||
for i = 1:9
|
||||
if i == 1
|
||||
ct(:,i) = com_pos_R1(:,i);
|
||||
|
|
@ -56,7 +56,7 @@ plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r')
|
|||
axis equal
|
||||
grid on
|
||||
% plot 3D: Get 3D coordinate of CO
|
||||
co=[]
|
||||
co=[];
|
||||
for i = 1:8
|
||||
if i == 1
|
||||
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
|
||||
|
|
@ -73,6 +73,16 @@ hold on
|
|||
plot3(co(1,:),co(2,:),co(3,:),'o','Color','b')
|
||||
axis equal
|
||||
grid on
|
||||
%%
|
||||
for i = 1:9
|
||||
co(:,i+1)-co(:,i)
|
||||
co(:,i+1)-co(:,i);
|
||||
end
|
||||
%%
|
||||
for i = 1:9
|
||||
tt(:,i)=robot.TW(1:3,1:3,i)*com_pos_R1(:,i);
|
||||
end
|
||||
%%
|
||||
yy=eye(4,4);
|
||||
for i =1:9
|
||||
yy = yy*Mlist_CG(:,:,i)
|
||||
end
|
||||
Loading…
Reference in New Issue