1.POE#

（3）刚体运动#

3.1旋转与角速度#

1.旋转矩阵#

$$R=Rot(\hat{\omega},\theta)$$

满足以下条件

• 正则条件
• 正交条件
• 坐标系三个坐标轴遵循右手定则

3*3旋转矩阵组成的集合被称为特殊正交群SO(3)

特性

• 封闭性
• 结合律
• 幺元性
• 可逆性

用途

1. 表示姿态
2. 进行坐标系转换
3. 对向量和坐标系进行旋转变换

左乘旋转矩阵会得到绕固定坐标系中的转轴转动，右乘会得到绕物体坐标系中转轴的转动。

2.角速度#

$$\omega=\hat{\omega}\dot{\theta}$$

反对称矩阵

在空间坐标系中的角速度

$$[\omega_s]=\dot{R}R^{-1}$$

物体坐标系下的角速度

$$[\omega_b]=R^{-1}\dot{R}$$

$\omega \in R^3$是角速度的向量表达形式，$[\omega]\in so(3)$是3*3矩阵表示形式。

3.转动的指数坐标表示#

转动的三参数指数坐标

向量$\dot\omega \theta \in R^3$就是转动的三参数指数坐标形式，单独写就是转动的轴-角表示法

刚体转动的指数坐标

刚体转动的矩阵对数

若$\dot\omega \theta \in R^3$表示旋转矩阵R的指数坐标，那么反对称矩阵$[\hat{\omega }\theta]=[\hat{\omega }]\theta \in so(3)$就是矩阵R的矩阵对数。矩阵对数是矩阵指数的可逆形式。正像矩阵指数是矩阵形式的角速度$[\hat{\omega }]\theta \in so(3)$在单位时间（ 1秒）内的定积分，使刚体运动到给定姿态$R\in SO(3)$—样，矩阵对数则是对R进行微分，以找到一个常值角速度$[\hat{\omega }]\theta \in so(3)$的矩阵表示形式。

3.2刚体运动与运动旋量#

齐次变换矩阵T与旋转矩阵R相对应，螺旋轴S与旋转轴$\hat\omega$相对应，运动旋量V(可表示成$S\dot\theta$）与角速度$[\hat{\omega }\theta]=[\hat{\omega }]\theta \in so(3)$相对应，而用于描述刚体位移的指数坐标$S\theta \in R^6$与刚体转动的指数坐标$\hat \omega \theta \in R^3$相对应。

**特殊欧氏群SE(3)**亦称刚体运动群或齐次变换矩阵群，是所有4×4实矩阵T的集合 。

运动旋量#

将$\omega_b,v_b$合在一起组成一个六维向量形式。为此定义为物体坐标系中的速度，简称为物体运动旋量。

如前所述，角速度可以很方便的写成反对称矩阵形式，同样，运动旋量也能写成矩阵形式。

同样，基于空间坐标系，写出空间运动旋量

物体运动旋量和空间运动旋量的关系

运动旋量的螺旋释义#

运动旋量$V$也可以写成螺旋轴$S$与绕该轴转动的速度$\dot\theta$组合形式

$S=(\omega,v)$的矩阵表达式为

3.3刚体运动的指数坐标表达#

Chasles_Mozzi定理<任何刚体运动都可通过绕空间某>—固定螺旋轴S的运动来实现 。

类似转动情况，定义矩阵指数和矩阵对数

将矩阵指数进行级数展开

矩阵对数

3.4力旋量#

将力矩与力合成为—个六维的空间力（spatial lOrce）,称为力旋量（wrench）。

回顾力与速度的点积即为功率，而功率为坐标系无关量，因此有

3.5函数#

1
1.计算旋转矩阵的逆
2
invR = RotInv(R)
3

4
2.将三维向量转换为3*3反对称矩阵
5
so3mat = VecToso3(omg)
6

7
3.将3*3反对称矩阵转换为三维向量
8
omg = so3ToVec(so3mat)
9

10
4.从旋转的三维指数坐标中提取出轴线和角度
11
[omghat,theta] = AxisAng3(expc3)
12

13
5.计算矩阵指数对应的旋转矩阵
14
R = MatrixExp3(so3mat)
15

16
6.计算旋转矩阵对应的矩阵对数
17
so3mat = MatrixLog3(R)
18

19
7.M到SO（3）旋转矩阵的空间距离的度量
20
d = DistanceToSO3(mat)
21

22
8.判断M是否为旋转矩阵
23
judge = TestIfSO3(mat)
24

25
9.返回与M最近的旋转矩阵
26
R = ProjectToSO3(mat)
27

28
10.将旋转矩阵和位置向量转换wei
29
T = RpToTrans(R,p)
30

31
11.从变换矩阵中分离出旋转矩阵和位置向量
32
[R,p] = TransToRp(T)
33

34
12.变换矩阵求逆
35
invT = TransInv(T)
36

37
13.构造与六维向量形式的运动旋量V对应的
38
se3mat = VecTose3(V)
39

40
14.构造与se(3)矩阵对应的六维向量形式的运动旋量V
41
V = se3ToVec(se3mat)
42

43
15.计算变换矩阵T的6*6的伴随矩阵
44
AdT = Adjoint(T)
45

46
16.返回正则化的螺旋轴表达式
47
S = ScrewToAxis(q,s,h)
48

49
17.从六位向量形式的指数坐标中提取出螺旋轴和沿轴线移动的距离
50
[S,theta] = AxisAng6(expc6)
51

52
18.计算矩阵指数对应的变换矩阵
53
T = MatrixExp6(se3mat)
54

55
19.计算变换矩阵对应的矩阵对数
56
se3mat = MatrixLog6(T)
57

58
20.M到变换矩阵的空间距离
59
d = DistanceToSE3(mat)
60

61
21.判断是否为SE3变换矩阵
62
judge = TestIfSE3(mat)
63

64
22.离M最近的变换矩阵
65
T = ProjectToSE3(mat)

展开收起

（4）正向运动学#

4.1指数积公式#

现在假定关节$n$对应的关节变量为$\theta$末端坐标系$M$的位移可写成

式中，$T \in SO(3)$为末端的新位形’，$S=(\omega_n,v_n)$为表示在基坐标系中的关节$n$的旋量坐标。若关节$n$是转动副（对应的是零节距的螺旋运动），则叫$\omega_n \in R^3$是沿关节轴正向的单位向量，$v_n=-\omega_n\times q_n$，$q_n$为关节轴上任一点，坐标值在基坐标系中进行度量。若关节$n$ 是移动副，则叫$\omega_n=0 ,v_n\in R^3$是沿关节轴正向的单位向量，$\theta_n$表示移动的距离。

上述方程就是指数积公式的空间坐标表达形式

同样也可以导出物体坐标系的指数积公式

4.2函数#

1
末端坐标系下。给定初始形位，关节旋量以及关节值，计算末端坐标系
2
T = FKinBody(M,Blist,thetalist)
3
空间坐标系下。给定初始形位，关节旋量以及关节值，计算末端坐标系
4
T = FKinSpace(M,Slist,thetalist)

4.3库卡KR60-3运动学建模#

1
import modern_robotics as mr
2
import numpy as np
3
from math import pi
4

5

6
omega_1 = np.array([0,0,-1])
7
omega_2=np.array([0,1,0])
8
omega_3=np.array([0,1,0])
9
omega_4=np.array([-1,0,0])
10
omega_5=np.array([0,1,0])
11
omega_6=np.array([-1,0,0])
12

13
q1=np.array([0,0,0])
14
q2=np.array([350,0,815])
15
q3=np.array([1200,0,815])
16
q4=np.array([2020,0,960])
17
q5=np.array([2020,0,960])
18
q6=np.array([2020,0,960])
19

20
M=np.array([[ 0,    0,    1,  2190],
21
            [ 0,    1,    0,     0],
22
            [-1,    0,    0,   960],
23
            [ 0,    0,    0,     1]])
24

25
# thetalist = np.array([0,-pi/2,pi/2,0,0,0])
26
thetalist = np.array([-2.410000 / 180 * pi, -57.230000 / 180 * pi, 112.080000 / 180 * pi, -0.780000 / 180 * pi, -54.890000 / 180 * pi,
27
     1.440000 / 180 * pi])
28

29

30
def vcaculate(omega,q):
31
    v=np.cross(-omega,q)
32
    return v
33
v1=vcaculate(omega_1,q1)
34
v2=vcaculate(omega_2,q2)
35
v3=vcaculate(omega_3,q3)
36
v4=vcaculate(omega_4,q4)
37
v5=vcaculate(omega_5,q5)
38
v6=vcaculate(omega_6,q6)
39

40
s1=np.append(omega_1,v1)
41
s2=np.append(omega_2,v2)
42
s3=np.append(omega_3,v3)
43
s4=np.append(omega_4,v4)
44
s5=np.append(omega_5,v5)
45
s6=np.append(omega_6,v6)
46

47
Slist=np.array([s1,s2,s3,s4,s5,s6]).T
48
# print(M)
49
# print(v)
50
# print(thetalist)
51

52

53
T = mr.FKinSpace(M,Slist,thetalist)
54
print(T)

展开收起

（5）一阶运动学和静力学#

机器人正运动学可以写成

$$x(t)=f(\theta(t))$$

对上式关于时间t求导

$$\dot{x}=\frac {\partial f(\theta) } {\partial \theta }\frac {d(\theta(t)) } {dt }=\frac{\partial f(\theta)} {\partial \theta} = J(\theta)\dot{\theta}$$

式中，$J(\theta)$被称为雅可比，表示末端执行器速度相对关节速度的线性敏感度，时关于关节变量$\theta$的函数。在某些形位上，雅可比矩阵会变成奇异阵，相应的形位被称为奇异形位，特征主要表现在机器人在末端的某些方向上的速度不能实现。

雅可比矩阵可以用来将关节转速的边界映射到$v_{tip}$的边界中，如图5.2所示。不是如图那样将关节速度通过雅可比矩阵映射成多边形，而是在$\dot{\theta}_1-\dot{\theta}_2$平面内将关节速度映射成—个单位圆。这个圆表示关节速度空间的等廓线，其中驱动器的共同作用可考虑为关节速度的平方和。通过映射，单位圆映射成末端速度的—个椭球，这个椭球称为可操作度椭球。当操作手的位形接近奇异位形时，椭球将退化成一个线段，末端沿某方向运动的能力将会丧失。

假设用于机器人运动的功忽略不计，机器人末端施加的功应与关节处产生的功相等，末端力向量记作$f_{tip}$关节力矩向量记作$\tau$ 根据能量守恒定律可得

由于$v_{tip}=J(\theta)\dot{\theta}$，因此等式改写为

用于生成末端力$f_{tip}$的关节力矩$\tau$可以通过上式计算得到。如果机器人处于非奇异形位，雅可比矩阵及其转置矩阵可逆，可以写成

同样，类似于可操作度椭球，可以通过雅可比矩阵的逆转置，将$\tau_{1}-\tau_{2}$平面内的一个等廓单位圆映射成$f_{1}-f_{2}$末端力平面的一个椭球，这个椭球被称为力椭球。表示生成不同方向力的难易程度。

由可操作度椭球和力椭球明显可以看出，若在某—方向上比较容易地产生末端速度，该方向产生力就变得比较困难，反之亦然，具体如图5.6所示。事实上，对于给定的机器人位形，可操作度椭球与力椭球的主轴方问完全重合，但力椭球的主轴长度与可操作度椭球的主轴长度正好相反

5.1机器人雅可比#

5.1.1空间雅可比#

考虑一个n杆的开链机器人，其正向运动学的指数积公式为

空间速度$V_S$可以写成$[V_S]=\dot{T}T^{-1}$，其中

可计算得

可通过伴随映射写成向量形式

矩阵$J_S(\theta)$即为空间固定坐标形式下得雅可比矩阵，简称为空间雅可比。

$J_{si}(\theta)$得第i列为

第$i$列的结构形式为$J_{si}(\theta)=Ad_{T_{i-1}}(S_{i})$其中$T_{i-1}=e^{[S_1\theta_1]}\cdots e^{[S_{i-1}\theta_{i-1}]}$。回顾$S_i$为机器人处于零位时第i个关节相对固定坐标系的旋量坐标，因此$Ad_{T_{i-1}}(S_{i})$为该机器人经历刚体位移$T_{i-1}$之后的第i个关节相对固定坐标系的旋量坐标。物理上，这与前面的i-l个关节从零位到当前值theta1…theta_i-1时的旋量坐标是等效的。因此,$J_{s}(\theta)$的第i列$J_{si}(\theta)$只是描述第i个关节轴相对固定坐标系的旋量，同时也是关节变量theta1…theta_i-1的函数。不过是取任意的$\theta$值而不是$\theta$＝0。

5.1.2物体雅可比#

物体坐标系下，$[V_b]=T^{-1}\dot{T}$，正运动学公式为

由此计算可得

以矩阵形式表示

矩阵$J_{b}(\theta)$即为物体坐标系下得雅可比行列式，简称物体雅可比

5.1.3几何解释#

5.1.4空间雅可比和物体雅可比的关系#

5.1.5几何雅可比和解析雅可比的关系#

5.2开链机器人静力学#

5.3奇异性分析#

​ 如在某个或者多个方向同时失去运动能力的位姿，我们称之为运动学奇异（kinematicsingularjty），或者简称为奇 异（singularity）。数学上,奇异位姿意味着雅可比矩阵$J(\theta)$不再满秩 。

​ 当$J(\theta)$的阶数降到最大值以下，奇异位姿对应此时的$\theta$。奇异位姿下末端坐标系丧失掉在某—维或多维方向的瞬时空间速度，这种奇异下损失的自由度同时伴随产生了与之对应方向上的约束力旋量 。

• 2个旋转轴共轴
• 3各平面转动副轴线平行
• 4各转动副轴线共点
• 4个转动副轴线共面
• 6个转动副都与一条线相交

5.4 可操作度#

对于一个通用的″关节开链机器人，任务空间的坐标为q，可操作度椭球对应的是当关节速率满足$|\dot{\theta}|=1$时末端执行器的速度。在n维关节速度空间内的一个单元球。假设$J$可逆,单位关节速度条件数可以写成

由线性代数可知，上式定义m维空间椭球。椭球主轴方向为$v_i$，主轴半径长为$\sqrt{\lambda_i}$。椭球的体积V与主轴半径长的乘积成正比。

角速度与线速度的量纲不同，于是将雅可比矩阵分开。由此可以得到两个三维可操作度椭球<一个表示角速度>,另—个表示线速度。这两个可操作度椭球都有与A的特征向量相对应的主轴，长度为其特征值的平方根。其中，对于角速度可操作度椭球$J=J_\omega J_\omega^T$，对于线速度可操作度椭球,$J=J_v J_v^T$

当在计算线速度可操作度椭球时’使用物体雅可比$J_b$更合适些,因为我们通常对末端坐标系原点的速度更感兴趣而非固定坐标系原点的速度。

除了可操作度椭球的几何特征之外,还有一种方法是定义可操作度椭球的最长轴半径与最短轴半径的比值

当$\mu_1(A)$接近1时,可操作度椭球接近球形或者各向同性。意味着该机器人沿任何方向都同样容易，这是理想情况。相反,当机器人接近奇异时$\mu_1(A)$趋于无穷大 。

类似的，可以定义$\mu_2(A)$为$\mu_1(A)$的平方，又称为矩阵A的条件数 ，该值越小越理想。矩阵的条件数普遍用于衡量矩阵与向量相乘后的敏感度误差减少的程度 。

最后一种度量方法是简单利用可操作度椭球的体积，这种情况是越大越好。

与可操作度椭球类似，可以定义力椭球。 由单位关节力矩满足$||{\tau} ||=1$，得到与上述类似的结果

因此，力椭球的主轴长度为可操作椭球长度的倒数。

5.5软件函数#

给定物体坐标系下描述的各关节旋量$B_i$及关节角，计算物体雅可比$J_B(\theta)$

1
Jb=JacobianBody(Blist,thetalist)

给定物体坐标系下描述的各关节旋量$S_i$及关节角，计算物体雅可比$J_S(\theta)$

1
J=JacobianBody(Slist,thetalist)

2.改进D-H建模#

（1）D-H参数#

机器人工具箱中中定义改进DH连杆时一定要注意DH[theta d a alpha]中前两个参数下表为i，即当前关节的DH值，后两个参数下表为i-1，及前一个关节的DH值参数值！

此时对比代码中的DH参数和图中画的坐标系，得到DH定义为

theta(i)：绕Zi轴，从Xi-1旋转到Xi的角度

d(i)：沿Zi轴，从Xi-1移动到Xi的距离

a(i)：沿Xi轴，从Zi移动到Zi+1的距离

alpha(i)：绕Xi轴，从Zi旋转到Zi+1的角度

杆件	theta	d	a	alpha
0	0	0	0	0
1	0	815	350	-90
2	-90	0	850	0
3	0	0	145	-90
4	0	820	0	90
5	0	0	0	-90
6	180	180	0	0

所以在程序中定义DH参数：

杆件	theta	d	a	alpha
1	0	815	0	0
2	-90	0	350	-90
3	0	0	850	0
4	0	820	145	-90
5	0	0	0	90
6	180	180	0	-90

上图所示为机器人处于位姿原点位置，在D-H模型中关节转角theta为[0,-90,0,0,0,180]，而实际的机器人处于原点位姿的关节转角theta’为[0,-90,90,0,0,0]，这是由于机器人关节零位和旋转方向定义与D-H模型不同造成的，实际机器人与D-H模型第一，四，六轴的旋转方向相反，第三、六轴分别差-90°和180°，需要对理论模型和实际模型的关节转角转换关系进行明确的定义：

1
theta1=-theta1';
2

3
theta2=-theta2';
4

5
theta3=theta3'-90;
6

7
theta4=-theta4';
8

9
theta5=-theta5'
10

11
theta6=180-theta6'

1
import roboticstoolbox as rtb
2
from spatialmath import SE3
3
from roboticstoolbox import DHRobot, RevoluteDH, RevoluteMDH
4
from math import pi
5
import numpy
6
from numpy import *
7
import numpy as np
8

9
class KR60(DHRobot):
10
    def __init__(self):
11
        deg = pi / 180
12

13
        # Updated values form ARTE git. Old values left as comments
14

15
        L1 = RevoluteMDH(a=0,
16
                         d=0.815,
17
                         alpha=0,  # alpha=pi / 2,
18
                         # theta=0,
19
                         qlim=[-185 * deg, 185 * deg]
20
                        )
21
        L2 = RevoluteMDH(a=0.35,
22
                         d=0,  # d=0.135,
23
                         alpha=-pi/2,  # alpha=pi,
24
                         # theta=0,
25
                         qlim=[-135 * deg, 35 * deg]
26
                        )
27
        L3 = RevoluteMDH(a=0.85,
28
                        d=0,  # d=0.135,
29
                        alpha=0,  # alpha=-pi / 2,
30
                        # theta=-pi/2,
31
                        qlim=[-120 * deg, 158 * deg]
32
                        )
33
        L4 = RevoluteMDH(a=0.145,
34
                        d=0.82,  # d=0.62,
35
                        alpha=-pi/2,  # alpha=pi / 2,
36
                        # theta=0,
37
                        qlim=[-350 * deg, 350 * deg]
38
                        )
39
        L5 = RevoluteMDH(a=0.0,
40
                        d=0.0,
41
                        alpha=pi/2,  # alpha=-pi / 2,
42
                        # theta=0,
43
                        qlim=[-119 * deg, 119 * deg]
44
                        )
45
        L6 = RevoluteMDH(a=0,
46
                        d=0.17,
47
                        alpha=-pi/2,
48
                        # theta=-pi,
49
                        qlim=[-350 * deg, 350 * deg]
50
                        )
51

52
        L = [L1, L2, L3, L4, L5, L6]
53

54
        # Create SerialLink object
55
        super().__init__(
56
            L,
57
            # meshdir="KUKA/KR5_arc",
58
            name='KR60',
59
            manufacturer='KUKA')
60
            # meshdir="meshes/KUKA/KR60_arc")
61

62
        self.addconfiguration("qz", [0, -pi/2, 0, 0, 0, pi])
63
        self.addconfiguration(
64
          "qk1", [0, -pi/2, 0, 0, 0, 0])
65
        self.addconfiguration(
66
          "qk2", [pi / 4, pi / 3, pi / 6, pi / 3, pi / 4, pi / 6])
67
        self.addconfiguration(
68
          "qk3", [pi / 6, pi / 3, pi / 6, pi / 3, pi / 6, pi / 3])
69

70

71
if __name__ == '__main__':   # pragma nocover
72
    robot = KR60()
73
    print(robot)

1
import roboticstoolbox as rtb
2
from spatialmath import SE3
3
from spatialmath.base import *
4
import KR60
5
from math import pi
6
import math
7
import numpy
8
from numpy import *
9
import numpy as np
10

11
def thetaTrans(q):
12
    q[0] = -q[0]
13
    q[1] = q[1]
14
    q[2] = q[2] - pi / 2
15
    q[3] = -q[3]
16
    q[4] = q[4]
17
    q[5] = pi - q[5]
18
    return q
19

20
q = [-2.410000 / 180 * pi, -57.230000 / 180 * pi, 112.080000 / 180 * pi, -0.780000 / 180 * pi, -54.890000 / 180 * pi,
21
     1.440000 / 180 * pi]
22
q = thetaTrans(q)
23
print(q)
24

25
robot = KR60.KR60()
26
T = robot.fkine(q)
27
print(T)
28
T.printline()

1
[0.04206243497306334, -0.9988519309163547, 0.38536869884034797, 0.01361356816555577, -0.9580112264196875, 3.1164599123610746]
2
   0.001189 -0.03091   0.9995    1.569
3
  -0.01727   0.9994    0.03092   0.06416
4
  -0.9999   -0.01729   0.0006545  0.9428
5
   0         0         0         1
6

7
t = 1.57, 0.0642, 0.943; rpy/zyx = -87.8°, 89°, -86.1°

1
>>> from spatialmath.base import *
2
>>> rotx(30, 'deg')
3
array([[ 1.   ,  0.   ,  0.   ],
4
       [ 0.   ,  0.866, -0.5  ],
5
       [ 0.   ,  0.5  ,  0.866]])

1
>>> from spatialmath import *
2
>>> T = SE3.Rx(30, 'deg')
3
>>> type(T)
4
<class 'spatialmath.pose3d.SE3'>
5
>>> print(T)
6
   1         0         0         0
7
   0         0.866    -0.5       0
8
   0         0.5       0.866     0
9
   0         0         0         1

1
>> R.eul()

1
>>> R = SO3()   # the identity
2
>>> R.append(R1)
3
>>> R.append(R2)
4
>>> len(R)
5
 3
6
>>> R[1]
7
   1         0         0
8
   0         0.955336 -0.29552
9
   0         0.29552   0.955336

1
>>> R = SO3( [ SO3(), R1, R2 ] )
2
>>> len(R)
3
 3

1
>>> R = SO3.Rx( np.arange(0, 2*np.pi, 0.2))
2
>>> len(R)
3
 32

1
>>> A = R * SO3.Ry(0.5)
2
>>> len(R)
3
 32

1
>>> A = SO3.Ry(0.5) * R
2
>>> len(R)
3
 32

1
>>> A = R * R
2
>>> len(R)
3
 32

1
>>> SO3.Rx(0.3) * 2
2
array([[ 2.        ,  0.        ,  0.        ],
3
       [ 0.        ,  1.91067298, -0.59104041],
4
       [ 0.        ,  0.59104041,  1.91067298]])
5

6
>>> SO3.Rx(0.3) - 1
7
array([[ 0.        , -1.        , -1.        ],
8
       [-1.        , -0.04466351, -1.29552021],
9
       [-1.        , -0.70447979, -0.04466351]])

1
>>> T = SE3(1,2,3) * SE3.Rx(30, 'deg')
2
>>> T.print()
3
   1         0         0         1
4
   0         0.866025 -0.5       2
5
   0         0.5       0.866025  3
6
   0         0         0         1
7

8
>>> T.printline()
9
t =        1,        2,        3; rpy/zyx =       30,        0,        0 deg
10

11
>>> T.plot()

1.建立URDF模型#

准备工作：下载solidworks导出urdf文件的插件：http://wiki.ros.org/sw_urdf_exporter ，下载直接安装即可。

1. 去库卡官网下载KR60-3的STL模型

2. 按照面重合-轴线重合的方式，装配好机器人模型。

3. 在装配体中编辑零件，提取关节基准轴，以第六轴为例，命名为axis_6

4. 对各关节建立起相应的轴线之后，退出零件编辑，返回装配体设置，对基座、关节以及刀具建立坐标系，注意所建立的坐标系不能在零件的内部，必须在装配体的树形目录下，坐标系的Z轴指向关节旋转方向。

5. 点击工具-tool-export as URDF,轴和坐标系之前建立的关节轴线和坐标轴，零件选择相应的杆件，轴的类型都选择旋转副。

6. 

   导出URDF文件，在最后的配置文件中可以调整杆件的颜色

2.导入VREP#

1. 在Vrep中，Plugins-URDF export导入模型文件，注意导入路径不能有中文，否则导入会失败。

2. 将基座设置为固定，调整相应的杆件关节极限,运动模式修改为Torque/force mode。进一步修改杆件的动力学信息，使能电动机，将电动机的最大力矩调整为合适的大小，打开control position enable，修改关节速度极限，这样就可以在脚本中修改关节数据，使机器人按我们设定的轨迹运动。

3. 添加运动脚本，ADD-Associated child scripts-no thread scripts。

   API解释见：regular API reference (coppeliarobotics.com)

   Lua脚本分为4部分，包含由系统适当调用的回调函数集合。除了初始化回调之外，所有其他回调都是可选的。默认主脚本通常分为4个回调函数：

   初始化函数：sysCall_init（）。此回调函数不是可选的。它将在模拟开始时执行一次。代码负责准备模拟等。通过sim.getObjectHandle函数初始化各关节的句柄

   驱动函数：sysCall_actuation()。此回调函数将在每次模拟过程中执行。代码负责处理模拟的所有驱动功能。有一个函数特别有趣：sim.HandleChildScript，它调用子脚本的系统调用回调函数。如果没有这些命令，子脚本将无法执行或无法执行其驱动功能，特定的模型功能或行为也无法按预期运行。

   传感函数：sysCall_sensing()。此回调函数将在每次模拟过程中执行。代码负责以通用方式处理模拟的所有传感功能（接近传感器等）。同样也有sim.HandleChildScript，它调用子脚本的系统调用回调函数。

   恢复函数：sysCall_cleanup()。此函数将在模拟结束前执行一次。该代码负责恢复对象的初始配置、清除传感器状态等。

   1
   function sysCall_init()
   2
       -- do some initialization here
   3
       axis_1_handle = sim.getObjectHandle('axis_1')
   4
       axis_2_handle = sim.getObjectHandle('axis_2')
   5
       axis_3_handle = sim.getObjectHandle('axis_3')
   6
       axis_4_handle = sim.getObjectHandle('axis_4')
   7
       axis_5_handle = sim.getObjectHandle('axis_5')
   8
       axis_6_handle = sim.getObjectHandle('axis_6')
   9
       t = 0
   10
   
   11
       --graph
   12
       graph=sim.getObjectHandle('Graph')
   13
       JointPos=sim.getObjectHandle('JointPos')
   14
   
   15
       objectPosX=sim.addGraphStream(graph,'object pos x','m',1)
   16
       objectPosY=sim.addGraphStream(graph,'object pos y','m',1)
   17
       objectPosZ=sim.addGraphStream(graph,'object pos Z','m',1)
   18
   
   19
       objectJoint1=sim.addGraphStream(JointPos,'Joint1','deg/s',0,{1,0,0})
   20
       objectJoint2=sim.addGraphStream(JointPos,'Joint2','deg/s',0,{1,0.5,0})
   21
       objectJoint3=sim.addGraphStream(JointPos,'Joint3','deg/s',0,{1,1,0})
   22
       objectJoint4=sim.addGraphStream(JointPos,'Joint4','deg/s',0,{1,0,0.5})
   23
       objectJoint5=sim.addGraphStream(JointPos,'Joint5','deg/s',0,{1,0,1})
   24
       objectJoint6=sim.addGraphStream(JointPos,'Joint6','deg/s',0,{1,1,1})
   25
   
   26
       objectTime=sim.addGraphStream(JointPos,'Time','m',1)
   27
   
   28
       sim.addGraphCurve(graph,'object pos x/y/z',3,{objectPosX,objectPosY,objectPosZ},{0,0,0},'m by m')
   29
       sim.addGraphCurve(JointPos,'Joint1',2,{objectJoint1,objectTime},{0,0,0},'m by m')
   30
   
   31
       --base frame
   32
       base_frame = sim.getObjectHandle('baseframe')
   33
   end
   34
   
   35
   function sysCall_actuation()
   36
       -- put your actuation code here
   37
   
   38
       t = t + 0.01
   39
       --print(t)
   40
   
   41
       axis_1_angle = sim.getJointPosition(axis_1_handle)
   42
       axis_2_angle = sim.getJointPosition(axis_2_handle)
   43
       axis_3_angle = sim.getJointPosition(axis_3_handle)
   44
       axis_4_angle = sim.getJointPosition(axis_4_handle)
   45
       axis_5_angle = sim.getJointPosition(axis_5_handle)
   46
       axis_6_angle = sim.getJointPosition(axis_6_handle)
   47
   
   48
       axis_1_targetAngle = math.pi / 3 * math.sin(t)
   49
       axis_2_targetAngle = -math.pi/2 * math.sin(t)
   50
       axis_3_targetAngle = math.pi/2 * math.sin(t)
   51
       axis_4_targetAngle = math.pi/4 * math.sin(t)
   52
       axis_5_targetAngle = math.pi/6 * math.sin(t)
   53
       axis_6_targetAngle = math.pi/5 * math.sin(t)
   54
   
   55
       --print(180*sim.getJointPosition(axis_6_handle)/math.pi)
   56
   
   57
       --print(180*sim.getJointPosition(axis_1_handle)/math.pi)
   58
       --print(axis_1_targetAngle)
   59
       sim.setJointTargetPosition(axis_1_handle,axis_1_targetAngle)
   60
       sim.setJointTargetPosition(axis_2_handle,axis_2_targetAngle)
   61
       sim.setJointTargetPosition(axis_3_handle,axis_3_targetAngle)
   62
       sim.setJointTargetPosition(axis_4_handle,axis_4_targetAngle)
   63
       sim.setJointTargetPosition(axis_5_handle,axis_5_targetAngle)
   64
       sim.setJointTargetPosition(axis_6_handle,axis_6_targetAngle)
   65
       --print(axis_1_angle)
   66
       --print(axis_2_angle)
   67
       --print(axis_2_targetAngle)
   68
   
   69
   
   70
   end
   71
   
   72
   function sysCall_sensing()
   73
       -- put your sensing code here
   74
       local pos=sim.getObjectPosition(graph,-1)
   75
       --print(pos)
   76
   
   77
       sim.setGraphStreamValue(graph,objectPosX,pos[1])
   78
       sim.setGraphStreamValue(graph,objectPosY,pos[2])
   79
       sim.setGraphStreamValue(graph,objectPosZ,pos[3])
   80
   
   81
       sim.setGraphStreamValue(JointPos,objectJoint1,180*sim.getJointPosition(axis_1_handle)/math.pi)
   82
       sim.setGraphStreamValue(JointPos,objectJoint2,180*sim.getJointPosition(axis_2_handle)/math.pi)
   83
       sim.setGraphStreamValue(JointPos,objectJoint3,180*sim.getJointPosition(axis_3_handle)/math.pi)
   84
       sim.setGraphStreamValue(JointPos,objectJoint4,180*sim.getJointPosition(axis_4_handle)/math.pi)
   85
       sim.setGraphStreamValue(JointPos,objectJoint5,180*sim.getJointPosition(axis_5_handle)/math.pi)
   86
       sim.setGraphStreamValue(JointPos,objectJoint6,180*sim.getJointPosition(axis_6_handle)/math.pi)
   87
   
   88
       --Joint1={}
   89
       --table.insert(Joint1,axis_1_angle)
   90
       --sim.setGraphStreamValue(JointPos,objectJoint1,Joint1)
   91
   end
   92
   
   93
   function sysCall_cleanup()
   94
       -- do some clean-up here
   95
   end
   96
   
   97
   -- See the user manual or the available code snippets for additional callback functions and details

   展开收起
   4. 点击仿真进行按钮，机器人就按着脚本中关节设置的轨迹进行移动，同时在空间中绘制出末端的运行轨迹，在弹出的框中也可以看到末端在XY平面的轨迹。

3.联合仿真#

Vrep提供了三种远程api，远程API是应用程序与CoppeliaSim连接的几种方式之一。它允许CoppeliaSim和外部应用程序（即在不同进程或不同机器上运行的应用程序）之间的通信，是跨平台的，支持服务调用（即阻塞调用），还支持双向数据流。它有三种不同的版本/框架：

The ZeroMQ-based remote API：它轻量级且易于使用，提供了通过CoppeliaSim脚本也可用的所有API函数，目前支持Python客户端。

The legacy remote API (or simply remote API)：它相对轻量级，并且比基于B0的远程API具有更少的依赖性。它提供了API函数的子集，并支持以下客户端：C/C++、Java、Python、Matlab、Octave和Lua。

The B0-based remote API：它基于BlueZero中间件及其与CoppeliaSim的接口插件。它提供了API函数的子集，可以很容易地扩展和支持以下客户端：C++、java、python、Matlab和Lua。

1
V-REP端：
2
simExtRemoteApiStart(19999)

1
MATLAB端：
2
vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
3
vrep.simxFinish(-1); % just in case, close all opened connections
4
clientID=vrep.simxStart('blog.leihub.cn',19999,true,true,5000,5);

1
import b0RemoteApi
2
import time
3

4
with b0RemoteApi.RemoteApiClient('b0RemoteApi_pythonClient','b0RemoteApi') as client:
5
    def getJointPos(num):
6
        jointname = "axis_" + str(num)
7
        [bool, jointhandle] = client.simxGetObjectHandle(jointname, client.simxServiceCall())
8
        [bool, joint] = client.simxGetJointPosition(jointhandle, client.simxServiceCall())
9
        print(joint)
10
        return joint
11
    client.simxStartSimulation(client.simxServiceCall()) #开始仿真
12
    startTime = time.time()
13
    getJointPos(1)
14
    while time.time() < startTime + 5:
15
        continue
16
    client.simxStopSimulation(client.simxServiceCall()) #停止仿真

1
import b0RemoteApi
2
import time
3

4
with b0RemoteApi.RemoteApiClient('b0RemoteApi_pythonClient','b0RemoteApi') as client:
5
    doNextStep=True
6

7
    def simulationStepStarted(msg):
8
        simTime=msg[1][b'simulationTime'];
9
        print('Simulation step started. Simulation time: ',simTime)
10

11
    def simulationStepDone(msg):
12
        simTime=msg[1][b'simulationTime'];
13
        print('Simulation step done. Simulation time: ',simTime);
14
        global doNextStep
15
        doNextStep=True
16

17
    client.simxSynchronous(True)
18
    client.simxGetSimulationStepStarted(client.simxDefaultSubscriber(simulationStepStarted));
19
    client.simxGetSimulationStepDone(client.simxDefaultSubscriber(simulationStepDone));
20
    client.simxStartSimulation(client.simxDefaultPublisher())
21

22
    startTime=time.time()
23
    while time.time()<startTime+5:
24
        if doNextStep:
25
            doNextStep=False
26
            client.simxSynchronousTrigger()
27
        client.simxSpinOnce()
28
    client.simxStopSimulation(client.simxDefaultPublisher())

1.创建anaconda虚拟环境#

1
1、用conda创建Python虚拟环境（在conda prompt环境下完成）
2
conda create -n Vrep python=3.7
3

4
2、激活虚拟环境（在conda prompt环境下完成）
5
activate Vrep
6

7
3、给虚拟环境安装外部包
8
conda install -n Vrep [package]
9
例如: conda install -n tensorflow pandas
10

11
4、查看已有的环境(当前已激活的环境会显示一个星号)
12
conda info -e
13

14
5、删除一个已有的虚拟环境
15
conda remove --name your_env_name --all
16

17
6、查看pip的安装目录
18
pip list
19

20
7、删除已经安装的模块
21
pip uninstall **
22
(例如：pip uninstall numpy)

创建anaconda的虚拟环境后，会生成E:\Anaconda\envs\Vrep的文件夹，在Pycharm中选择创建的虚拟环境。

2.pip换镜像源#

在 python 里经常要安装各种这样的包，安装各种包时最常用的就是 pip，pip 默认从官网下载文件，官网位于国外，下载速度时快时慢，还经常断线，国内的体验并不太好。

解决办法是把 pip 源换成国内的，最常用的并且可信赖的源包括清华大学源、豆瓣源、阿里源：

pypi 清华大学源：https://pypi.tuna.tsinghua.edu.cn/simple pypi 豆瓣源 ：http://pypi.douban.com/simple/ pypi 腾讯源：http://mirrors.cloud.tencent.com/pypi/simple pypi 阿里源：https://mirrors.aliyun.com/pypi

pip 源具体修改方式是，我们以安装 python 的robotics-toolbox-python模块为例，通常的方式是直接在命令行运行

1
pip install robotics-toolbox-python

这样会从国外官网下载robotics-toolbox-python模块并安装。

若要把 pip 源换成国内的，只需要把上面的代码改成下图这样（下图以清华大学源为例）：

1
pip install robotics-toolbox-python -i https://pypi.tuna.tsinghua.edu.cn/simple

这样我们就从清华大学源成功安装了机器人工具箱，速度会比过pip默认的国外源快很多。

上述做法是临时改成国内源，如果不想每次用 pip 都加上 -i https://pypi.tuna.tsinghua.edu.cn/simple，那么可以把国内源设为默认，做法是：

1
# 清华源
2
pip config set global.index-url https://pypi.tuna.tsinghua.edu.cn/simple
3
# 阿里源
4
pip config set global.index-url https://mirrors.aliyun.com/pypi/simple/
5
# 腾讯源
6
pip config set global.index-url http://mirrors.cloud.tencent.com/pypi/simple
7
# 豆瓣源
8
pip config set global.index-url http://pypi.douban.com/simple/