使用机器人工具箱对rrp构型舷梯求解动力学，采用mdh，simscape进行验证

主要代码如下：

``
% 导入机器人工具箱
clear; clc;
% dh 参数
d1 = 3.106;a2 = 0.23;d30 = 3.052;d4 = 10;
% 关节运动轨迹
t = 0:0.01:(2/4)pi5;
theta1 = 0.2sin(3t);
theta1_v = 0.6cos(3t);
theta1_a = -1.8sin(3t);
theta2 = 0.1sin(3t)+pi/2 ;
theta2_v = 0.3cos(3t);
theta2_a = -0.9sin(3t);
d3 = 0.3sin(3t) + 3.052;
d3_v = 0.9cos(3t);
d3_a = -2.7sin(3t);

index = size(t);
n = index(2);
% 定义连杆（R, R, P） 使用 MDH 参数
% theta d a alpha
% 注意改进DH只能通过此方法定义
L(1) = Link([0, d1, 0, 0, 0], 'modified'); % θ1 可变
L(2) = Link([0, 0, a2, pi/2, 0], 'modified' ); % θ2 可变
L(3) = Link([0, 0, 0, pi/2, 1], 'modified'); % d3 可变 ->

% 设置关节xianwei
% L(1).qlim = [-pi, pi];
% L(2).qlim = [0, pi/4];
% L(3).qlim = [0, 1];

% 动力学建模（惯性等需补充）
% 添加动力学参数（惯量、质量等）示例：
% L(2).offset = pi/2;
% L(3).offset = d30;% 尝试添加关节初始值，但是在debug过程中发现其不会被纳入动力学内推的过程，只能将初始设置在关节曲线中。
%
L(1).m = 5;
L(1).r = [0 0 0.25];
L(1).I = [1/6 1/6 1/6 0 0 0];
L(1).Jm = 0.0001;
L(1).G = 1;

L(2).m = 3;
L(2).r = [0 0.15 0];
L(2).I = [1/8 1/8 1/8 0 0 0];
L(2).Jm = 0.0001;
L(2).G = 1;

L(3).m = 2.5;
L(3).r = [0 0 0.1];
L(3).I = [1/12 1/12 1/12 0 0 0];
L(3).Jm = 0.0001;
L(3).G = 1;

% % 创建机器人模型
robot = SerialLink(L, 'name', 'RRP-Robot', 'modified',...
'gravity', [0 0 9.81]);
robot.tool = transl(0,0,10);
% 显示机器人参数
robot.display()
% robot.teach();% 在matlab2023b中存在bug，不能运行。

for i = 1:n
%% 逆向动力学计算示例
q = [theta1(i), theta2(i), d3(i)]; % 当前位置
qd = [theta1_v(i), theta2_v(i), d3_v(i)]; % 当前速度
qdd = [theta1_a(i), theta2_a(i), d3_a(i)]; % 当前加速度
A = robot.fkine(q);
% 末端位置
possum0(i,:) = A.t;
% 末端速度
vsum0(i,:) = robot.jacob0(q)*qd';
% 末端加速度
Jdot = robot.jacob_dot(q, qd); % 雅可比矩阵的导数
a = Jdot + robot.jacob0(q) * qdd'; % 末端执行器的加速度
accsum0(i, 😃 = a'; % 存储末端执行器的加速度

% 计算各关节所需扭矩/力
tau = robot.rne(q, qd, qdd);
tausum0(i,:) = tau;

end
figure;
% 绘制正弦函数
plot(t,tausum0(:,1), 'r', 'LineWidth', 0.5); % 正弦函数
hold on; % 保持当前图形
plot(t,tausum0(:,2), 'g', 'LineWidth', 0.5); % 余弦函数
plot(t,tausum0(:,3), 'b', 'LineWidth', 0.5); % 正切函数
grid on; % 添加网格
figure;
% 绘制正弦函数
plot(t,possum0(:,1), 'r', 'LineWidth', 0.5); % 正弦函数
hold on; % 保持当前图形
plot(t,possum0(:,2), 'g', 'LineWidth', 0.5); % 余弦函数
plot(t,possum0(:,3), 'b', 'LineWidth', 0.5); % 正切函数
grid on; % 添加网格
figure;
% 绘制正弦函数
plot(t,vsum0(:,1), 'r', 'LineWidth', 0.5); % 正弦函数
hold on; % 保持当前图形
plot(t,vsum0(:,2), 'g', 'LineWidth', 0.5); % 余弦函数
plot(t,vsum0(:,3), 'b', 'LineWidth', 0.5); % 正切函数
plot(t,vsum0(:,4), '--', 'LineWidth', 0.5); % 正弦函数
hold on; % 保持当前图形
plot(t,vsum0(:,5), 'g', 'LineWidth', 0.5); % 余弦函数
plot(t,vsum0(:,6), 'b', 'LineWidth', 0.5); % 正切函数
grid on; % 添加网格
figure;
% 绘制正弦函数
plot(t,accsum0(:,1), 'r', 'LineWidth', 0.5); % 正弦函数
hold on; % 保持当前图形
plot(t,accsum0(:,2), 'g', 'LineWidth', 0.5); % 余弦函数
plot(t,accsum0(:,3), 'b', 'LineWidth', 0.5); % 正切函数
plot(t,accsum0(:,4), '--', 'LineWidth', 0.5); % 正弦函数
hold on; % 保持当前图形
plot(t,accsum0(:,5), 'x', 'LineWidth', 0.5); % 余弦函数
plot(t,accsum0(:,6), 'o', 'LineWidth', 0.5); % 正切函数
grid on; % 添加网格
``

simulink模型：