当前位置：服务支持 > 软件文章 > 升级版Matlab机器人工具箱Pinocchio Mucojo验证7轴机械臂正运动学雅克比矩阵

升级版Matlab机器人工具箱Pinocchio Mucojo验证7轴机械臂正运动学雅克比矩阵

阅读数 3

1、目的

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

2、配置

2.1、电脑系统

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

别的平台也大差不差

2.2、pinocchio

就是一个机器人动力学的库：https://github.com/stack-of-tasks/pinocchio，参考这个链接https://stack-of-tasks.github.io/pinocchio/download.html，选择适合自己的平台安装。我选择使用python版本，利用这个命令安装conda install pinocchio -c conda-forge。版本如下：

2.3、matlab机器人工具箱

之前做过一个10.4版本的教程

Matlab Robotics Toolbox（10.4）下载、安装、使用教程

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

2.4、Mujoco

mujoco是一个仿真环境（目前也在学习，后续也打算出教程哦～），利用python版本，使用

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]时的正运动学，微分运动学，代码片段：

代码块

clike

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)

      复制成功

运行结果：

图1

利用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

      复制成功

运行结果：

图2

可视化：

利用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)

      复制成功

运行结果：