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

add com and co

This commit is contained in:
cosmic_power 2024-09-28 19:27:12 +08:00
parent 516c6e2ba5
commit d145dd3071
1 changed files with 36 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
untitled2.m
View File

@ -3,8 +3,8 @@ for i = 0:N-1
if i == 0 if i == 0
M=robot.T(:,:,i+1)*transl(com_pos(:,i+1)); M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
else 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)); M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
end end
Mlist_CG = cat(3, Mlist_CG, M); Mlist_CG = cat(3, Mlist_CG, M);
end end
@ -15,11 +15,11 @@ for i = 0:N-1
M=robot.T(:,:,i+1)*transl(com_pos(:,i+1)); M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
else else
rotation_i = diag([1,1,1]); rotation_i = diag([1,1,1]);
for j = 1:i for j = 1:i
rotation_i = rotation_i*TransToRp(robot.T(:,:,i)); rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1)); rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
end end
M = TransInv(RpToTrans(rotation_i,com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,com_pos(:,i+1)); M = TransInv(RpToTrans(rotation_i,com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,com_pos(:,i+1));
end end
Mlist_CG = cat(3, Mlist_CG, M); Mlist_CG = cat(3, Mlist_CG, M);
end end
@ -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_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_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_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; 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:7 for i = 1:9
if i == 1 if i == 1
ct(:,i) = com_pos_R1(:,i); ct(:,i) = com_pos_R1(:,i);
elseif i< 8 elseif i< 9
ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i); ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
elseif i==8 else
% ct(:,) ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
end end
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
%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