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

Compare commits

base: Robotics:516c6e2ba5ec89bb5a4310380d9d26d9f4d1e29e
Branches Tags
Robotics:main
Robotics:feature/calulated-col-force
Robotics:test/grab-to-unstow
Robotics:feature/Make-GC-Compilable
Robotics:feature/R1000-identification
Robotics:test/R1000-test
Robotics:feature/add-regressor
Robotics:feature/add-kinematics
...
compare: Robotics:9a41c6688d32847504dbde8e8530b7ea3a16d3ca
Branches Tags
Robotics:feature/calulated-col-force
Robotics:main
Robotics:test/grab-to-unstow
Robotics:feature/Make-GC-Compilable
Robotics:feature/R1000-identification
Robotics:test/R1000-test
Robotics:feature/add-regressor
Robotics:feature/add-kinematics

2 Commits
516c6e2ba5 ... 9a41c6688d

Author SHA1 Message Date
cosmic_power 9a41c6688d add screw kinematics 2024-09-28 21:45:14 +08:00
cosmic_power d145dd3071 add com and co 2024-09-28 19:27:12 +08:00
8 changed files with 245 additions and 81 deletions
Show Stats Expand all files Collapse all files

72
Identification_main.asv Normal file
View File

@ -0,0 +1,72 @@
close all;clc;clear
file = [];
opt.robot_def = 'direct';
opt.KM_method = 'MDH';
opt.Vel_method = 'Direct';
opt.LD_method = 'Direct';
opt.debug = true;
opt.robotName = 'R1000';
opt.Isreal = true;
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;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]];
robot.slist(4:6,:) = co;
robot.Home = [0;0;-1];
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 = get_Kinematics(robot, opt);
% R1000_Dynamics_num;
% opt.Isreal = false;
% robot = get_velocity(robot, opt);
% robot = get_regressor(robot,opt);
% symbol matched
% verify_regressor
% robot = get_baseParams(robot, opt);

50
Identification_main.m
View File

@ -1,7 +1,7 @@
close all;clc;clear
file = [];
opt.robot_def = 'direct';
opt.KM_method = 'MDH';
opt.KM_method = 'SCREW';
opt.Vel_method = 'Direct';
opt.LD_method = 'Direct';
opt.debug = true;
@ -10,7 +10,7 @@ opt.robotName = 'R1000';
opt.Isreal = true;
robot = get_robot_R1000(file,opt);
% robot.theta = [1,1,0];
robot = get_Kinematics(robot, opt);
%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;
@ -30,7 +30,51 @@ 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;
R1000_Dynamics_num;
% 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;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]];
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]];
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;
% opt.Isreal = false;
% robot = get_velocity(robot, opt);
% robot = get_regressor(robot,opt);

46
R1000_Dynamics_num.asv
View File

@ -24,37 +24,37 @@ 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);
% Get the com pos transformation in each joint reference frame
% FIXME: BUG here
% Mlist_CG=[];
% Mlist_CG = [];
% for i = 0:N-1
% if i == 0
% M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
% 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));
% 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
@ -101,6 +101,8 @@ for i = 1:length(q_J)
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

24
R1000_Dynamics_num.m
View File

@ -24,17 +24,17 @@ 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
@ -48,7 +48,7 @@ for i = 0:N-1
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));
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

19
get_Kinematics.m
View File

@ -1,6 +1,25 @@
function robot = get_Kinematics(robot, opt)
if(opt.Isreal)
switch opt.KM_method
case 'SCREW'
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(VecTose3(Slist(:, i) * thetalist(i))) * T;
end
robot.TW(:,:,j) = T;
end
for i = 1:size(thetalist)
if i == 1
robot.T(:,:,i) = robot.TW(:,:,i);
else
robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i);
end
end
robot.Fkine = robot.TW(:,:,end);
case 'SDH'
theta = robot.theta;
alpha = robot.alpha;

52
get_robot_R1000.asv
View File

@ -73,33 +73,33 @@ switch opt.robot_def
% 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(:,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]';
com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]';
com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]';
com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]';
com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]';
com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]';
com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]';
com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]';
com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]';
com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]';
com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]';
com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]';
com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]';
com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]';
com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]';
com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]';
com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]';
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

4
get_robot_R1000.m
View File

@ -13,6 +13,8 @@ switch opt.robot_def
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;
@ -51,7 +53,7 @@ switch opt.robot_def
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,0];
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];

39
untitled2.m
View File

@ -3,7 +3,7 @@ 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))
% 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);
@ -39,15 +39,40 @@ 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;
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:7
for i = 1:9
if i == 1
ct(:,i) = com_pos_R1(:,i);
elseif i< 8
ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
elseif i==8
% ct(:,)
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