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

feature/R1000-identification #2

Merged
cosmic_power merged 48 commits from feature/R1000-identification into main 2024-12-16 15:25:53 +00:00
Conversation 0 Commits 48 Files Changed 129 +36 -14
1 changed files with 36 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit d145dd3071 - Show all commits

32
untitled2.m
View File

@ -39,15 +39,37 @@ 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< 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 COM
co=[]
for i = 1:8
if i == 1
co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i);
elseif i<8
ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
elseif i==8
% ct(:,)
%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