python 机器人工具箱——robotics-toolbox-python_python robotics toolbox

这个工具箱为 Python 带来了机器人特定的功能，并利用 Python 的可移植性、普遍性和支持性的优势，以及线性代数（numpy、scipy）、图形（matplotlib、three.js、WebGL）的开源生态系统的能力，交互式开发（jupyter、jupyterlab、mybinder.org）和文档（sphinx）。

python -m pip install --user numpy scipy matplotlib ipython jupyter pandas sympy nose

工具箱提供了用于表示串行链接机械手的运动学和动力学的工具 - 您可以轻松地以 Denavit-Hartenberg 形式创建自己的工具，导入 URDF 文件，或使用来自 Franka-Emika 的 30 多种提供的知名当代机器人模型， Kinova、Universal Robotics、Rethink 以及 Puma 560 和 Stanford arm 等经典机器人。

该工具箱还将支持具有机器人运动模型（独轮车、自行车）、路径规划算法（bug、距离变换、D*、PRM）、运动动力学规划（晶格、RRT）、定位（EKF、粒子过滤器）等功能的移动机器人，地图构建 (EKF) 和同时定位和映射 (EKF)。

该工具箱利用Python 的空间数学工具箱来提供对 SO(n) 和 SE(n) 矩阵、四元数、扭曲和空间向量等数据类型的支持。

工具箱提供：

• 成熟的代码，为相同算法的其他实现提供一个比较点；
• 例程通常以简单明了的方式编写，易于理解，但可能以牺牲计算效率为代价；
• 可供学习和教学阅读的源代码；
• 与 Robotics Toolbox for MATLAB 的向后兼容性

工具箱如下：

pip3 install roboticstoolbox-python

git clone https://github.com/petercorke/robotics-toolbox-python.git
cd robotics-toolbox-python
pip3 install -e .

https://github.com/petercorke/robotics-toolbox-pythonhttps://github.com/petercorke/robotics-toolbox-python

roboticstoolbox-python ·皮皮 (pypi.org)

加载一个使用修改（克雷格约定）Denavit-Hartenberg 符号经典定义的 Franka-Emika Panda 机器人模型

import roboticstoolbox as rtb
robot = rtb.models.DH.Panda()
print(robot)
 
	Panda (by Franka Emika): 7 axes (RRRRRRR), modified DH parameters
	┏━━━━━━━━┳━━━━━━━━┳━━━━━┳━━━━━━━┳━━━━━━━━━┳━━━━━━━━┓
	┃ aⱼ₋₁   ┃  ⍺ⱼ₋₁  ┃ θⱼ  ┃  dⱼ   ┃   q⁻    ┃   q⁺   ┃
	┣━━━━━━━━╋━━━━━━━━╋━━━━━╋━━━━━━━╋━━━━━━━━━╋━━━━━━━━┫
	┃    0.0 ┃   0.0° ┃  q1 ┃ 0.333 ┃ -166.0° ┃ 166.0° ┃
	┃    0.0 ┃ -90.0° ┃  q2 ┃   0.0 ┃ -101.0° ┃ 101.0° ┃
	┃    0.0 ┃  90.0° ┃  q3 ┃ 0.316 ┃ -166.0° ┃ 166.0° ┃
	┃ 0.0825 ┃  90.0° ┃  q4 ┃   0.0 ┃ -176.0° ┃  -4.0° ┃
	┃-0.0825 ┃ -90.0° ┃  q5 ┃ 0.384 ┃ -166.0° ┃ 166.0° ┃
	┃    0.0 ┃  90.0° ┃  q6 ┃   0.0 ┃   -1.0° ┃ 215.0° ┃
	┃  0.088 ┃  90.0° ┃  q7 ┃ 0.107 ┃ -166.0° ┃ 166.0° ┃
	┗━━━━━━━━┻━━━━━━━━┻━━━━━┻━━━━━━━┻━━━━━━━━━┻━━━━━━━━┛
 
	┌─────┬───────────────────────────────────────┐
	│tool │ t = 0, 0, 0.1; rpy/xyz = -45°, 0°, 0° │
	└─────┴───────────────────────────────────────┘
 
	┌─────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐
	│name │ q0  │ q1     │ q2  │ q3    │ q4  │ q5    │ q6   │
	├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤
	│  qz │  0° │  0°    │  0° │  0°   │  0° │  0°   │  0°  │
	│  qr │  0° │ -17.2° │  0° │ -126° │  0° │  115° │  45° │
	└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘
 
T = robot.fkine(robot.qz)  # forward kinematics
print(T)
 
	   0.707107    0.707107    0           0.088
	   0.707107   -0.707107    0           0
	   0           0          -1           0.823
	   0           0           0           1

求解逆运动学。我们首先选择根据位置和方向定义的 SE(3) 姿势（末端执行器 z 轴向下 (A=-Z) 和平行于 y 轴的手指方向 (O=+Y)）。

from spatialmath import SE3
 
T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
sol = robot.ikine_LM(T)         # solve IK
print(sol)
	IKsolution(q=array([  0.2134,    1.867,  -0.2264,   0.4825,   0.2198,    1.396,   -2.037]), success=True, reason=None, iterations=12, residual=1.4517646473808178e-11)
 
q_pickup = sol.q
print(robot.fkine(q_pickup))    # FK shows that desired end-effector pose was achieved
 
	Out[35]:
		-1            9.43001e-14  2.43909e-12  0.7
		 9.43759e-14  1            7.2574e-13   0.2
		-2.43913e-12  7.2575e-13  -1            0.1
		 0            0            0            1

这个机器人是冗余的，所以除了末端执行器姿势之外，我们无法控制手臂配置，即。我们无法控制肘部的高度。

我们可以为从直立qz配置到此拾取配置的路径设置动画

qt = rtb.jtraj(robot.qz, q_pickup, 50)
robot.plot(qt.q, movie='panda1.gif')

现在让我们加载同一个机器人的 URDF 模型。运动学表示不再基于 Denavit-Hartenberg 参数，它现在是刚体树。

robot = rtb.models.URDF.Panda()  # load URDF version of the Panda
print(robot)    # display the model
 
	panda (by Franka Emika): 7 axes (RRRRRRR), ETS model
	┌───┬──────────────┬─────────────┬──────────────┬──────────────────────────────────────────────────────────────────────────────┐
	│id │     link     │   parent    │    joint     │                                     ETS                                      │
	├───┼──────────────┼─────────────┼──────────────┼──────────────────────────────────────────────────────────────────────────────┤
	│ 0 │  panda_link0 │         _O_ │              │ {panda_link0} = {_O_}                                                        │
	│ 1 │  panda_link1 │ panda_link0 │ panda_joint1 │ {panda_link1} = {panda_link0}  * tz(0.333) * Rz(q0)                          │
	│ 2 │  panda_link2 │ panda_link1 │ panda_joint2 │ {panda_link2} = {panda_link1}  * Rx(-90°) * Rz(q1)                           │
	│ 3 │  panda_link3 │ panda_link2 │ panda_joint3 │ {panda_link3} = {panda_link2}  * ty(-0.316) * Rx(90°) * Rz(q2)               │
	│ 4 │  panda_link4 │ panda_link3 │ panda_joint4 │ {panda_link4} = {panda_link3}  * tx(0.0825) * Rx(90°) * Rz(q3)               │
	│ 5 │  panda_link5 │ panda_link4 │ panda_joint5 │ {panda_link5} = {panda_link4}  * tx(-0.0825) * ty(0.384) * Rx(-90°) * Rz(q4) │
	│ 6 │  panda_link6 │ panda_link5 │ panda_joint6 │ {panda_link6} = {panda_link5}  * Rx(90°) * Rz(q5)                            │
	│ 7 │  panda_link7 │ panda_link6 │ panda_joint7 │ {panda_link7} = {panda_link6}  * tx(0.088) * Rx(90°) * Rz(q6)                │
	│ 8 │ @panda_link8 │ panda_link7 │ panda_joint8 │ {panda_link8} = {panda_link7}  * tz(0.107)                                   │
	└───┴──────────────┴─────────────┴──────────────┴──────────────────────────────────────────────────────────────────────────────┘
 
	┌─────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐
	│name │ q0  │ q1     │ q2  │ q3    │ q4  │ q5    │ q6   │
	├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤
	│  qz │  0° │  0°    │  0° │  0°   │  0° │  0°   │  0°  │
	│  qr │  0° │ -17.2° │  0° │ -126° │  0° │  115° │  45° │
	└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘

链接表示为末端执行器，即刚体树中的叶节点。

我们可以在基于浏览器的 3d 模拟环境中实例化我们的机器人。

from roboticstoolbox.backends.Swift import Swift  # instantiate 3D browser-based visualizer
backend = Swift()
backend.launch()            # activate it
backend.add(robot)          # add robot to the 3D scene
for qk in qt.q:             # for each joint configuration on trajectory
      robot.q = qk          # update the robot state
      backend.step()        # update visualization