Notes
- 功能
- 在 PyBullet 仿真环境里加载一个 Franka Emika Panda 机械臂；
- 利用一个 动态模型（Pinocchio 封装） 来做前向运动学和控制；
- 给机械臂的末端执行器一个 正弦参考轨迹；
- 用 反馈线性化控制器 计算力矩指令并驱动机器人；
- 记录数据，并在最后绘制 关节位置和速度的跟踪曲线。
- CartesianDiffKin，高级控制器
- 输入
dyn_model：机器人动力学模型（能算 FK、Jacobian 等）。controlled_frame_name：被控制的连杆（比如 panda_link8）。cur_q：当前关节角度。p_des, pd_des：期望末端位置和速度。ori_des, ori_vel_des：期望末端姿态和角速度。delta_t：时间步长。ori_pos_both：控制模式（位置 / 姿态 / 两者）。kp_pos, kp_ori：笛卡尔空间 P 控制增益。joint_vel_saturation：关节速度限制。
- 输出
q_des：下一步期望关节角度。qd_des_clip：饱和后的期望关节速度。
- 解析法
- 分析过程
- FeedbackLin，低级控制器
feedback_lin_ctrl 计算关节力矩指令 tau_FL，使得机器人关节能按照期望位置 q_d 和期望速度 qd_d 跟踪，补偿掉了机器人动力学的非线性部分（惯性、科里奥利力、重力）。- 过程
- 设置 PD 增益矩阵
P 和 D。
- 用动力学模型
dyn_model 计算惯性矩阵、科里奥利、重力项。
- 计算位置和速度误差。
- 生成虚拟控制输入
u（PD 控制律）。
- 通过反馈线性化，把
u 转成实际关节力矩 tau_FL。
- 输入
dyn_model: 封装了机器人动力学（质量矩阵 M，科里奥利项 c，重力项 g 等）的对象，基于 Pinocchio。q_: 实际测量的关节位置。qd_: 实际测量的关节速度。q_d: 期望的关节位置。qd_d: 期望的关节速度。kp, kd: 控制器增益，可以是标量（所有关节一样）或向量（每个关节不同）。
- 输出
- 分析过程