python机器人库（robotics-toolbox-python）的运用

Python 机器人库的运用

文章目录

前言

找了半天发现网上关于python机器人库的资料很少，而目前我的项目要用到相关的东西，查询官网过后总结一下。

一、相关概要

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

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

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

工具箱提供：

成熟的代码，为相同算法的其他实现提供一个比较点；
例程通常以简单明了的方式编写，易于理解，但可能以牺牲计算效率为代价；
可供学习和教学阅读的源代码；
与 Robotics Toolbox for MATLAB 的向后兼容性
该工具箱利用Python 的空间数学工具箱来提供对 SO(n) 和 SE(n) 矩阵、四元数、扭曲和空间向量等数据类型的支持

二、必要的库的安装

1. 需要 Python >= 3.6
2. spatialmath-python
3. numpy
4. numpy-stl
5. vpython（这个库确实不知道怎么安装，但是安装不上也不影响后续，如果有知道到的可以，一起交流）
6. python机器人工具箱

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

1.创建机械臂模型

步骤一：在模型库里面创建一个名为’MYROBOT.py’的文件，其中MYROBOT是机器人的描述性名称，它是一个有效的文件名和Python类名。
E:\pyproject\defecate\robotics-toolbox-python\roboticstoolbox\models\DH\MYROBOT.py这是我的地址

步骤二：加载库

from math import pi
import numpy as np
from roboticstoolbox import DHRobot, RevoluteDH, PrismaticDH, RevoluteMDH, PrismaticMDH

最后一行很重要，它定义了所有可能的类的可能。不会全部使用它们，为了保持整洁，你可以删除那些你不用的。这就是他们的目的；

‘RevoluteDH’用于使用标准DH参数的转动关节

‘PrismaticDH’用于使用标准DH参数的移动关节

‘RevoluteMDH’用于使用MDH参数的转动关节

‘PrismaticMDH’用于使用MDH参数的移动关节

步骤三：描述机械臂，记录下使用的单位、DH参数以及对模型来源的引用。

class MYROBOT(DHRobot):
    """
    Create model of MYROBOT manipulator
    KR5()是一个用标准DH约定对Kuka KR5机器人建模并描述其运动学特征的类。
       .
       定义的关节构型为:
       qk1，公称工作位置1
       qk2，公称工作位置2
       qk3，公称工作位置3.

    :notes:
       .使用国际计量单位米
       .包括一个11.5厘米的工具在z方向
       .

    :references:
       .
       .
       .

    """

步骤四：在’init_’，通过创建适当的链接类的实例来定义一组链接变量

def __init__(self):

        deg = pi/180

        L0 = RevoluteDH(
            d=0,          # link length (Dennavit-Hartenberg notation)
            a=0,          # link offset (Dennavit-Hartenberg notation)
            alpha=pi/2,   # link twist (Dennavit-Hartenberg notation)
            I=[0, 0.35, 0, 0, 0, 0],  # inertia tensor of link with respect to
                                      # center of mass I = [L_xx, L_yy, L_zz,
                                      # L_xy, L_yz, L_xz]
            r=[0, 0, 0],  # distance of ith origin to center of mass [x,y,z]
                          # in link reference frame
            m=0,          # mass of link
            Jm=200e-6,    # actuator inertia
            G=-62.6111,   # gear ratio
            B=1.48e-3,    # actuator viscous friction coefficient (measured
                          # at the motor)
            Tc=[0.395, -0.435],  # actuator Coulomb friction coefficient for
                                 # direction [-,+] (measured at the motor)
            qlim=[-160*deg, 160*deg])    # minimum and maximum joint angle

        L1 = RevoluteDH(
            d=0, a=0.4318, alpha=0,
            qlim=[-45*deg, 225*deg])

            .
            .
            .   

其中包含DH四参数以及一些动力学参数，一般只考虑动力学参数的话，仅仅需要DH参数接可以了，也可以加一些关节极限qlim()。
我们提供尽可能多的参数。上述L0的定义包括运动学参数和动力学参数，而L1只有运动学参数。最小的要求是运动学参数，你甚至不需要知道关节极限，它们只需要一些工具箱函数。对于一个有N个关节的机器人，你必须定义N个关节实例。接下来，我们调用super函数来完成繁重的工作。

super().__init__(
    [L0, L1, L2, L3, L4, L5],
    name="MYROBOT",
    manufacturer="COMPANY THAT BUILDS MYROBOTs")

步骤五：创建实例属性，准备两个位姿，为后续正逆运动的验证做准备

# 零角度，L形姿势
        self._MYCONFIG = np.array([1, 2, 3, 4, 5, 6])  # 创建实例属性
        self.addconfiguration("qz", np.array([0,0,0,0,0,0 ]))  # 零角度，L形姿势
        self.addconfiguration("qr", np.array([0.610865,1.047198,0.785398,1.134464,0.628319,0 ]))

'MYCONFIG’是这个特定配置的名称。它必须是一个有N个元素的NumPy数组。然后定义一个返回该属性的属性。

@property
def MYCONFIG(self):
    return self._MYCONFIG

步骤六：底部要有一个main函数

if __name__ == '__main__':

    robot = MYROBOT()
    print(robot)
    print(robot._MYCONFIG)

步骤七：将建立的模型添加到工具箱内，编辑此文件夹中的文件’init.py’。添加一行如下，同时_all[]中也要添加模型名称

from roboticstoolbox.models.DH.MYROBOT import MYROBOT

2.正逆运动的实现

步骤一;载入库

import roboticstoolbox as rtb
import numpy as np
from math import pi

步骤二：验证机械臂参数是否能正确调用

robot = rtb.models.DH.MYROBOT()
#机械臂信息
print(robot)

输出内容

步骤三：模型验证
使用 matplotlib 图形将机器人显示为线条。matplotlib 是 Python 最普遍的图形库，可在所有平台上运行。

  import roboticstoolbox as rtb
  robot = rtb.models.DH.MYROBOT()
  qt = rtb.tools.trajectory.jtraj(robot.qz, robot.qr, 50)
  robot.plot(qt.q)

这里我是没弄出来，查了一下，网上说是可能是机器人工具箱本身存在的问题，有解决的也可以互相交流。
验证我是通过matlab进行仿真验证的

clc
clear

%        theta   d          a             alpha  sigma
L1=Link([  0     0.044      0             pi/2      0   ],'standard');
L2=Link([  0     0          0.14             0   0   ],'standard');
L3=Link([  0     0          0.0455      pi/2      0   ],'standard');
L4=Link([  0     0.1311     0      -pi/2      0   ],'standard');
L5=Link([  0     0          0.04945      pi/2  0   ],'standard');
L6=Link([  0     0.05585          0      0  0   ],'standard');
L2.offset = pi/2
L5.offset = -pi/2
%%取名
robot = SerialLink([L1 L2 L3 L4 L5 L6],'name','standard DH');
%读取角度信息
filename = '.\数据.txt';
[theta1, theta2,theta3, theta4, theta5 ,theta6] = textread(filename , '%f %f %f %f %f %f', 1);
theta = [theta1, theta2,theta3, theta4, theta5 theta6];
% theta = [0.523599 1.047189  1.047189  0.523599  1.5070796 ] 
% theta1 = theta(1);
% theta2 = theta(2);
% theta3 = theta(3);
% theta4 = theta(4);
% theta5 = theta(5);
du=pi/180;
ra=180/pi;

% -2.5946    0.0224    1.5707   -1.5930    2.5946
%定义关节范围
% L(1).qlim =[-170, 170]*du;
% L(2).qlim =[60-70, 60+70]*du;%-10,130
% L(3).qlim =[-70-70,-70+70]*du;%-140,0
% L(4).qlim =[-70,70]*du;
% L(5).qlim =[-170, 170]*du;
L1.qlim =[-180, 180]*du;
L2.qlim =[-180, 180]*du;
L3.qlim =[-180, 180]*du;
L4.qlim =[-180, 180]*du;
L5.qlim =[-180, 180]*du;
L6.qlim =[-180, 180]*du;
robot.teach()

步骤四：正运动（已知关节角度，求解末端位姿）

#正向运动
T = robot.fkine(robot.qr)

这里的qr，是建立的机械臂模型中两个姿态设定的其中一个姿态，分别是qz与qr，qz设置的是零角度姿态，qr是随意设置的一个姿态
前向运动学 (FK) 是给定关节坐标的末端执行器的位姿。可以为DHRobot或ERobot类的机器人计算
正运动学：总的来说就是末端位姿相对于基座标的位置关系，用矩阵表示，这种位置关系通过关节坐标来传递T = T01T12T23T34T45*T56，而这T矩阵就是通过DH参数表示出来的，具体操作下一个文章再说，这里简单讲一下。读者也可以自己写一下。
出来的结果如下：

这是一个末端位姿相对于极坐标的位置的矩阵，第四列代表X，Y，Z坐标，中间三行三列代表航偏角、俯仰角、滚轮角，至此正运动学算出。

步骤五：逆运动学
逆运动学 (IK) 是实现给定末端执行器姿势所需的关节坐标。功能不是唯一的，可能没有解决办法。（已知末端位姿求解关节角度）
这里给出一些python版本和matlab版本机器人工具箱的差别

我选择的是ikine_LMS()

TT = robot.ikine_LMS(T)
print(TT)

可见所得角度与之前设定的qr角度差别很小，逆运动成功，逆运动自己写也可以，我运用的是数值解，下次和正运动一起写。

DH参数

1. 1建立坐标系

   1）确定Zi轴

   作为第i+1个关节的驱动轴，其方向和关节轴线方向保持一致。例如Z0是第1个关节的驱动轴,Z1是第2个关节的驱动轴。可能看上去有点混乱，但是符合规则的，这保证了：当关节i被驱动时，连杆i以及与其相连的坐标系 OiXiYiZi，将会经历一个对应的运动。

   另外 Zi轴的方向符合右手定则，注意和关节的转动方向对应！

   2）确定基础坐标系

   由前面的讨论可知，满足DH建模约束条件的坐标变换可以用

   四个参数得到！
   基础坐标系的选择近乎是任意的，我们可以选择将基础坐标系的原点 Oo放置在Z0 轴上任何一点。然后，可以通过任意方便的方式来选择X0轴和Y0轴。但是建立0坐标系时，要尽可能使得最后的DH参数简单。

   3）确定 Xi方向

   a.轴 Zi-1和轴Zi不共面

   那么轴Zi-1和轴Zi的公垂线定义了Xi轴，并且它于轴Zi的交点即为原点Oi。

   b.轴Zi-1平行于轴Zi

   这种情况下，轴Zi-1和轴Zi之间存在无穷多个共同法线。将穿过原点Oi-1的法线选作Xi轴， Oi是该法线和Zi轴的交点。

   c.轴Zi-1和轴Zi相交

   选择Xi垂直于由Zi-1和Zi组成的平面。原点Oi一般设为Zi-1和Zi的交点。不过，轴线Zi上的任意一点都可以被选作原点。

   4)确定坐标系N

   以上的三个步骤适用于坐标系0,…N-1。最终的坐标系通常被称为末端执行器或者工具坐标系。最常见的是，将原点Oi以对称方式布置在夹具的中间, Zi轴方向沿着末端移动方向（最后一个关节伸缩的方向）。Xi轴方向沿着夹具的上下开合方向。

   因为有很多情况可以随意选择坐标系 Xi轴的方向，这就导致了不同的DH参数。为了规范DH建模方法，总结“使得DH参数简单”的一些法则。

   1、确定Xi轴方向时，如果有很多种选择，尽可能选择与Xi-1相同的方向

   2、确定坐标系0时，使得关节变量1的取值为零。如果是转动关节，关节变量就是；如果是平动关节，关节变量是d
   3、符合人的习惯

确定好坐标系过后就通过上面这张图进行DH参数的确定

标准DH中矩阵带入参数来确定相邻两关节关系。

三、总结

主要是介绍python机器人工具箱的应用，DH参数顺便记录一下