封面

前言：之前写过一篇类似的内容，但是现在发现有点问题，所以在重新分析一下（有详细代码）

1、目的

学习pinocchio库，利用它计算以及验证雅可比矩阵，和matlab机器人工具箱、mujoco等进行了对比分析

2、配置

2.1、电脑系统

芯片（Apple M1 Pro），MacOS（Sequoia 15.1.1）

别的平台也大差不差

2.2、pinocchio

就是一个机器人动力学的库，参考这个下载安装，选择适合自己的平台安装。我选择使用python版本，利用这个命令安装conda install pinocchio -c conda-forge。版本如下：

2.3、matlab机器人工具箱

之前做过一个10.4版本的教程；也做过9.10版本系列的教程。

本次验证使用的是10.4版本，如下：

2.4、Mujoco

mujoco是一个仿真环境（目前也在学习，后续也打算出教程哦～），利用python版本，使用pip install mujoco进行安装，版本如下：

3、计算7自由度机器人（Franka）的FK、Jacobian

首先，本教程有一个配套的工程，工程文件目录如下

然后，都设置关节角度为q = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]，利用不同的库或者工具，分别计算Franka机器人的正运动学和雅可比矩阵。

3.1、pinocchio

利用pinocchio库，计算关节角度q = [0.1,0.2,0.3,0.4,0.5,0.6,0.7]时的正运动学，微分运动学，代码片段：

pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)

# 计算Flange的fk
Flange_pose = data.oMf[Flange_Id]
print("Flange_pose \n", Flange_pose)

# 计算jacobian，相对于local frame
Flange_Jaco_wrt_local = pin.computeFrameJacobian(model, data, q, Flange_Id, pin.LOCAL)
print("Flange_Jaco_wrt_local \n", Flange_Jaco_wrt_local)

# 计算jacobian，相对于世界坐标系
Flange_Jaco_wrt_world = pin.computeFrameJacobian(model, data, q, Flange_Id, pin.WORLD)
print("Flange_Jaco_wrt_world \n", Flange_Jaco_wrt_world)

# 计算jacobian，相对于世界坐标系
Flange_Jaco_wrt_local_world = pin.computeFrameJacobian(model, data, q, Flange_Id, pin.LOCAL_WORLD_ALIGNED)
print("Flange_Jaco_wrt_local_world \n", Flange_Jaco_wrt_local_world)

运行结果：

利用pinocchio.visualize import MeshcatVisualizer，可视化

3.2、Robotics Toolbox for Matlab

利用matlab机器人工具箱中的自带的franka panda模型mdl_panda，计算关节角度q = [0.1,0.2,0.3,0.4,0.5,0.6,0.7]时的正运动学，微分运动学。代码片段：

% 直接调用matlab中的robotics toolbox的自带的panda，参数跟fr3是一样的
mdl_panda
% 一开始加载的panda机器人，有一个末端工具的位置，类似加上hand后的那个tcp
% 为了统一，都是按照flange的位置，所以tool的位置改成0.
panda.tool.t = [0;0;0];
% 设置关节角度
q = [0.1,0.2,0.3,0.4,0.5,0.6,0.7];
panda.plot(q);
% 计算fk
disp("panda.fkine(q)")
panda.fkine(q)
% 计算jacobian， with respect to the world frame
disp("panda.jacob0(q)")
panda.jacob0(q)
% 计算jacobian，with respect to the end effector frame
disp("panda.jacobe(q)")
panda.jacobe(q) % 在9版本是jacobn，在10版本中是jacobe

运行结果：

可视化：

利用matlab的机器人工具箱的Link、SerialLink函数，结合DH参数。在计算计算关节角度q = [0.1,0.2,0.3,0.4,0.5,0.6,0.7]时的正运动学，微分运动学；结果跟上述保持一致。

手算各个关节连杆之间的齐次旋转矩阵T_01、xxx、T_67，然后相乘T_we = T_01*T_12*T_23*T_34*T_45*T_56*T_67;，计算正运动学，依旧跟上述保持一致

3.3、Mujoco

利用mujoco，计算关节角度q = [0.1,0.2,0.3,0.4,0.5,0.6,0.7]时的正运动学，微分运动学。代码片段：

# 获取末端法兰Flange的id，计算正运动学
Flange_site_id = m.site('attachment_site').id
Flange_site_pos = d.site_xpos[Flange_site_id]
Flange_site_rot = d.site_xmat[Flange_site_id].reshape(3, 3)
print("Flange_site 的全局位置:", Flange_site_pos)
print("Flange_site 的旋转矩阵:\n", Flange_site_rot)

# 计算末端法兰Flange的 Jacobian
Flange_site_jacp = np.zeros((3, m.nv))
Flange_site_jacr = np.zeros((3, m.nv))
mujoco.mj_jacSite(m, d, Flange_site_jacp, Flange_site_jacr, Flange_site_id)
Flange_site_jacobian = np.vstack((Flange_site_jacp, Flange_site_jacr))
print("franka fr3 的 Flange_site处的雅可比矩阵（相对于world）: \n", Flange_site_jacobian)

运行结果：

利用mujoco.viewer，可视化：

注：目前Mujoco中好像只可以计算相对于world的雅可比矩阵（mj_jacSite）。局部雅可比好像可以用mj_jac函数进行转换，后面在研究吧。

4、结果

总结一下三种方式得到的结果

发现当关节角度为[0.1,0.2,0.3,0.4,0.5,0.6,0.7]时

1、他们的FK结果都是一样。

2、pinocchio中的LOCAL参考坐标系的结果和matlab机器人工具箱中的jacobe（相对于当前局部坐标系）的结果是一致的。

3、pinocchio中的LOCAL_WORLD_ALIGNED参考坐标系结果和我理解的相对于世界坐标系（matlab机器人工具箱中的jacob0，mujoco中的mj_jacSite）的结束是一致的。

5、疑问

计算相对于世界坐标系的时候，感觉应该是pinocchio中的WORLD参考坐标系和matlab的jacob0、mujoco的mj_jacSite保持一样？但是计算WORLD，结果又很奇怪？

在pinocchio的官网中，有三个参考系的描述：

总感觉pinocchio中LOCAL和LOCAL_WORLD_ALIGNED表反了？？？

（欢迎大家讨论）

解决

参考这个。好像又理解了一点。2024.11.11

2024.11.13

有了一点心得感悟，感觉就是world好像是随着关节的变化，所进行移动，好像是这种感觉，但是在pinocchio中进行可视化，想直接显示这三个world、local、local_eorld_aligned。没事成功

后续尝试一下，Flange_Id在这三个位姿下的不同效果。就行比较。