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

48
untitled2.m
View File

@ -3,8 +3,8 @@ 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));
% 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
@ -15,11 +15,11 @@ for i = 0:N-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));
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
@ -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< 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 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