Dhruv-Goyal/arm_parameters.py at master · The-Humanoid-Project/Dhruv-Goyal · GitHub

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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
import numpy as np
from roboticstoolbox import DHRobot, RevoluteMDH


class ARM(DHRobot):

    def __init__(self):

        deg = np.pi/180
        mm = 1e-3
        L = [
            RevoluteMDH(
                #value of length
                a=36,
                d=0.0,
                alpha=0.0,
                qlim=np.array([-np.pi/2,np.pi/2]),
                #mass of first portion of the arm
                m=1,
                I=[
                    7.03370e-01,
                    7.06610e-01,
                    9.11700e-03,
                    -1.39000e-04,
                    1.91690e-02,
                    6.77200e-03,
                ],
                G=1
            ),
            RevoluteMDH(
                #value of length part 2
                a=5,
                d=1,
                alpha=-np.pi/2,
                qlim=np.array([-np.pi/2,np.pi/2]),
                #mass of part 2
                m=1,
                I=[
                    7.03370e-01,
                    7.06610e-01,
                    9.11700e-03,
                    -1.39000e-04,
                    1.91690e-02,
                    6.77200e-03,
                ],
                G=1,
            ),
            RevoluteMDH(
                #value of length of part 3
                a=25,
                d=0.0,
                alpha=np.pi/2,
                qlim=np.array([-np.pi/2,np.pi/2]),
                #mass of part 3
                m=1,
                I=[
                    7.03370e-01,
                    7.06610e-01,
                    9.11700e-03,
                    -1.39000e-04,
                    1.91690e-02,
                    6.77200e-03,
                ],
                G=1,
            ),
            RevoluteMDH(
                #value of length of part 4
                a=18,
                d=0.0,
                alpha=0.0,
                qlim=np.array([-np.pi/2,np.pi/2]),
                #mass of part 4
                m=1,
                I=[
                    7.03370e-01,
                    7.06610e-01,
                    9.11700e-03,
                    -1.39000e-04,
                    1.91690e-02,
                    6.77200e-03,
                ],
                G=1,
            )
        ]

        #tool = transl(0,0,tool_offset) @ trotz(-np.pi/4)
        super().__init__(
            L,
            name="Panda",
            manufacturer="Franka Emika",
            meshdir="meshes/FRANKA-EMIKA/Panda",
            # tool=tool,
        )

        self.qr = np.array([0, -0.3, 0, -2.2])
        self.qz = np.zeros(4)

        self.addconfiguration("qr", self.qr)
        self.addconfiguration("qz", self.qz)

if __name__ == "__main__":  # pragma nocover

    arm = ARM()
    print(arm)