基于pybullet的机械臂导纳控制实验_python导纳控制

基于pybullet的机械臂导纳控制实验——根据知乎韩冰老师的文章

谢谢韩冰老师的原创图：

这副图大概的意思是：
对机械臂的末端执行器搭载的外部六维力矩传感器施加一个力，导纳控制公式，得到一个(x…加速度),加速度通过积分，可以使用梯形公式等数值积分方法得到(x.)速度，再通过积分得到(x)位置。上面的加速度，速度，位置都是施加力引起的位移变化，所以要在原来的位置上加上这些变化。
随着力的施加，位置会发生变化。当力释放时，这个也是计算过程，机械臂会弹回原来的位置。
所以要给在计算时，要给时间它复原。
下面是程序：

mport  numpy

class action_FT():
    def __init__(self, m, b, k, dt):
        self.M = m
        self.B = b
        self.K = k
        self.DT = dt

    def imdenpence_control(self, want_position, FT_data,ddxe,dxe,xe,ddye,dye,ye,ddze,dze,ze):
        #FT_data = numpy.array(FT_data)
        Fxe = FT_data[0,0]
        Fye = FT_data[0,1]
        Fze = FT_data[0,2]
        ###下面是计算过程,这是梯形公式
        ddxe1 = (Fxe - self.B * dxe - self.K * xe) / self.M
        dxe1 = self.DT * ((ddxe1 + ddxe) / 2) + dxe
        xe1 = self.DT * ((dxe1 + dxe) / 2 )+ xe  # 这个是最后的结果
        #####
        ddye1 = (Fye - self.B * dye - self.K * ye) / self.M
        dye1 = self.DT * ((ddye1 + ddye) / 2) + dye
        ye1 = self.DT * ((dye1 + dye) / 2 ) + ye  # 这个是最后的结果
        #####
        ddze1 = (Fze - self.B * dze - self.K * ze) / self.M
        dze1 = self.DT * ((ddze1 + ddze) / 2)  + dze
        ze1 = self.DT * ((dze1 + dze) / 2) + ze # 这个是最后的结果

        dposition = numpy.array([xe1, ye1, ze1])
        ####期望位置+力的位置变化， 随后会输入到逆向运动学控制器中， 这里的加法就是一个闭环
        position_offset = +dposition +want_position

        return position_offset,ddxe1,dxe1,xe1,ddye1,dye1,ye1,ddze1,dze1,ze1# 返回一个position

    def imdenpence_control1(self, want_position, FT_data,ddxe,dxe,xe,ddye,dye,ye,ddze,dze,ze):
        #FT_data = numpy.array(FT_data)
        Fxe = FT_data[0,0]
        Fye = FT_data[0,1]
        Fze = FT_data[0,2]

        midddle_xishu=4*0.499
        ###下面是计算过程,这个simpsong公式

        ddxe1 = (Fxe - self.B * dxe - self.K * xe) / self.M
        dxe1 = self.DT * ((ddxe1 + ddxe+midddle_xishu*(ddxe1+ddxe)) / 6) + dxe
        xe1 = self.DT * ((dxe1 + dxe+midddle_xishu*(dxe1+dxe)) / 6 )+ xe  # 这个是最后的结果
        #####
        ddye1 = (Fye - self.B * dye - self.K * ye) / self.M
        dye1 = self.DT * ((ddye1 + ddye+midddle_xishu*(ddye1+ddye)) / 6) + dye
        ye1 = self.DT * ((dye1 + dye*midddle_xishu*(dye1+dye)) / 6 ) + ye  # 这个是最后的结果
        #####
        ddze1 = (Fze - self.B * dze - self.K * ze) / self.M
        dze1 = self.DT * ((ddze1 + ddze+midddle_xishu*(ddze1+ddze)) / 6)  + dze
        ze1 = self.DT * ((dze1 + dze+midddle_xishu*(dze1+dze)) / 6) + ze # 这个是最后的结果

        dposition = numpy.array([xe1, ye1, ze1])
        ####期望位置+力的位置变化， 随后会输入到逆向运动学控制器中， 这里的加法就是一个闭环
        position_offset = dposition +want_position

        return position_offset,ddxe1,dxe1,xe1,ddye1,dye1,ye1,ddze1,dze1,ze1# 返回一个position
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

这个是导纳控制的主程序，大家可以看着控制框图来写，主要是积分公式的应用
还有是控制机械臂运动的主程序，和pybullet的环境设计。