259 lines
7.9 KiB
Matlab
259 lines
7.9 KiB
Matlab
%% R1000
|
||
N=9;
|
||
% traj
|
||
load('pose1_isa_depth_95mm.mat');
|
||
time = (100:size(q_log,2))/2000;
|
||
q_J = q_log(2:10,100:end);
|
||
qd_J = vel_log(2:10,100:end);
|
||
qdd_J = acc_log(2:10,100:end);
|
||
|
||
% Dynamics parameters
|
||
link_mass = robot.m;
|
||
com_pos = robot.com;
|
||
link_inertia = robot.I;
|
||
|
||
thetalist = q_J';
|
||
dthetalist = qd_J';
|
||
ddthetalist = qdd_J';
|
||
|
||
%real traj
|
||
% get_GCTraj_R1000_DVT;
|
||
% thetalist = idntfcnTrjctry(6).q';
|
||
% dthetalist = idntfcnTrjctry(6).qd';
|
||
% ddthetalist = idntfcnTrjctry(6).qdd';
|
||
|
||
% Get general mass matrix
|
||
Glist=[];
|
||
for i = 1:N
|
||
link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
|
||
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
||
Glist = cat(3, Glist, Gb);
|
||
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=[];
|
||
% 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)),robot.Home.R(:,:,i)*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
|
||
% elseif i==9
|
||
% 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);
|
||
|
||
% get the CG at the world base frame
|
||
com_pos_R1 = robot.com_pos_R1;
|
||
com_pos_R2 = robot.com_pos_R2;
|
||
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.23;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
|
||
% get the CG at the last GC frame
|
||
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
|
||
M = robot.T(:,:,i);
|
||
Mlist_ED = cat(3, Mlist_ED, M);
|
||
end
|
||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||
Mlist_ED = cat(3, Mlist_ED, M);
|
||
|
||
%TODO: Get Slist form DH table method
|
||
% RRRRRRRRP
|
||
Slist=robot.slist;
|
||
exf=[0;0;0;0;1;0];
|
||
|
||
for i = 1:length(thetalist)
|
||
[V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
|
||
= InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
|
||
[0;0;-9.806], exf, Mlist_CG, Glist, Slist);
|
||
% [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
|
||
|
||
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
|
||
% above 2020b
|
||
% F_Simpack = pagetranspose(F_Simpack);
|
||
% below 2020b
|
||
|
||
% for i = 1:3
|
||
% subplot(3,1,i);
|
||
% hold on;
|
||
% %added minus, so should be the same as simpack
|
||
% plot(time,taumat(i+6,:))
|
||
% xlabel('time(s)')
|
||
% ylabel('Torque(Nm)')
|
||
% % plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
||
% end
|
||
|
||
F_Simpack = permute(F_Simpack,[2 1 3]);
|
||
F_Simpack = -F_Simpack;
|
||
|
||
% 确保输出文件夹存在
|
||
output_main = 'output';
|
||
output_sub = fullfile(output_main, 'pose1_isa_depth_95mm');
|
||
|
||
if ~exist(output_main, 'dir')
|
||
mkdir(output_main);
|
||
end
|
||
if ~exist(output_sub, 'dir')
|
||
mkdir(output_sub);
|
||
end
|
||
|
||
% 获取时间序列长度
|
||
n_time = size(F_Simpack, 3);
|
||
|
||
% 遍历所有关节（1到8）
|
||
for joint_idx = 1:8
|
||
fig = figure('Position', [100, 100, 1000, 900]); % 创建大尺寸图形窗口
|
||
|
||
% 遍历三个力矩分量
|
||
for comp_idx = 1:3
|
||
subplot(3, 1, comp_idx);
|
||
hold on;
|
||
|
||
% 提取并处理数据（取负值以匹配Simpack约定）
|
||
data = -squeeze(F_Simpack(joint_idx, comp_idx, :));
|
||
|
||
% 绘制力矩曲线
|
||
plot(time, data, 'LineWidth', 1.5);
|
||
|
||
% 计算并标注最大绝对值（峰值）
|
||
[abs_max, max_idx] = max(abs(data)); % 找到最大绝对值
|
||
peak_value = data(max_idx); % 对应的实际值（带符号）
|
||
|
||
% 标记峰值点
|
||
plot(time(max_idx), peak_value, 'ro', 'MarkerSize', 8, 'LineWidth', 2);
|
||
|
||
% 创建标注文本
|
||
text_str = sprintf('Peak: %.2f Nm (|%.2f|)', peak_value, abs_max);
|
||
|
||
% 确定文本位置（避免重叠）
|
||
y_range = ylim;
|
||
y_offset = 0.05 * (y_range(2) - y_range(1));
|
||
|
||
if peak_value >= 0
|
||
text_pos = [time(max_idx), peak_value - y_offset];
|
||
vert_align = 'top';
|
||
else
|
||
text_pos = [time(max_idx), peak_value + y_offset];
|
||
vert_align = 'bottom';
|
||
end
|
||
|
||
% 添加文本标注
|
||
text(text_pos(1), text_pos(2), text_str, ...
|
||
'VerticalAlignment', vert_align, 'HorizontalAlignment', 'center', ...
|
||
'FontSize', 10, 'BackgroundColor', 'white', 'EdgeColor', 'black');
|
||
|
||
% 添加标签和网格
|
||
switch comp_idx
|
||
case 1
|
||
title('Bending Moment (M_x)');
|
||
case 2
|
||
title('Bending Moment (M_y)');
|
||
case 3
|
||
title('Torque (M_z)');
|
||
end
|
||
xlabel('Time [s]');
|
||
ylabel('Moment [Nm]');
|
||
grid on;
|
||
box on;
|
||
end
|
||
|
||
% 添加总标题
|
||
sgtitle(sprintf('Joint %d - Moment Components (Peak Values)', joint_idx), ...
|
||
'FontSize', 14, 'FontWeight', 'bold');
|
||
|
||
% 保存图形
|
||
saveas(fig, fullfile(output_sub, sprintf('joint_%d.png', joint_idx)));
|
||
close(fig); % 关闭图形释放内存
|
||
end
|
||
|
||
disp('所有关节图表已保存至 output/pose2_isa_depth_95mm 文件夹');
|
||
|
||
% Use Body Twist cal linear vel, but can't cal the end frame vel
|
||
% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
|
||
% [0;0;0], exf, Mlist, Glist, Slist);
|
||
% j=1;
|
||
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
|
||
% j=2;
|
||
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23); |