FirstBot/kinematic.py at master · MarcDcls/FirstBot · 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
105
106
107
108
109
110
import numpy as np

d = 0.14
D = 0.052


def direct_kinematics(w_l, w_r):
    """
    Takes as parameters wheel speeds (rad/s) and returns linear (m/s) and angular (rad/s) speeds of the robot

    :param w_l: left wheel speed
    :param w_r: right wheel speed
    :return: (v, w) linear and angular speeds of the robot

    >>> abs(direct_kinematics(0, 2 * np.pi)[1] - np.pi * D / d) < 0.0001
    True
    >>> abs(direct_kinematics(2 * np.pi, 2 * np.pi)[0] - np.pi * D) < 0.0001
    True
    >>> abs(direct_kinematics(-1, 1)[0]) < 0.0001
    True
    >>> abs(direct_kinematics(-1, 1)[1] - D / d) < 0.0001
    True
    >>> abs(direct_kinematics(1, 1)[1]) < 0.0001
    True
    >>> abs(direct_kinematics(0, 1)[0] - 0.0013) < 0.0001
    True
    >>> abs(direct_kinematics(0, 1)[1] - 0.1857142) < 0.0001
    True
    >>> abs(direct_kinematics(1, 0)[0] - 0.0013) < 0.0001
    True
    >>> abs(direct_kinematics(1, 0)[1] + 0.1857142) < 0.0001
    True
    >>> abs(direct_kinematics(0, -1)[0] + 0.0013) < 0.0001
    True
    >>> abs(direct_kinematics(0, -1)[1] + 0.1857142) < 0.0001
    True
    >>> abs(direct_kinematics(-1, 0)[0] + 0.0013) < 0.0001
    True
    >>> abs(direct_kinematics(-1, 0)[1] - 0.1857142) < 0.0001
    True
    """
    # Case of a rotation around the middle of the 2 wheels
    if w_l == - w_r:
        return 0, D * w_r / d

    # Case of a straight line
    if w_l == w_r:
        return D * w_l / 2, 0

    # General Case :
    v_r = D * w_r / 2
    v_l = D * w_l / 2
    v = (v_r + v_l) / 2
    w = (v_r - v_l) / d

    return v, w


def inverse_kinematics(v, w):
    """
    Takes as parameters the linear and angular target speeds, and computes the speed for left and right wheels

    :param v: linear velocity of the robot
    :param w: angular velocity of the robot
    :return: left wheel speed, right wheel speed

    >>> abs(inverse_kinematics(*direct_kinematics(1, 1))[0] - 1) < 0.0001
    True
    >>> abs(inverse_kinematics(*direct_kinematics(1, 1))[1] - 1) < 0.0001
    True
    >>> abs(inverse_kinematics(*direct_kinematics(0, 0))[0]) < 0.0001
    True
    >>> abs(inverse_kinematics(*direct_kinematics(0, 0))[1]) < 0.0001
    True
    >>> abs(inverse_kinematics(*direct_kinematics(-1, 1))[0] + 1) < 0.0001
    True
    >>> abs(inverse_kinematics(*direct_kinematics(-1, 1))[1] - 1) < 0.0001
    True
    >>> abs(inverse_kinematics(*direct_kinematics(0, 1))[0]) < 0.0001
    True
    >>> abs(inverse_kinematics(*direct_kinematics(0, 1))[1] - 1) < 0.0001
    True
    """
    # Exceptions
    if w == 0:
        if v == 0:
            return 0, 0  # No motion
        return 2 * v / D, 2 * v / D  # Case of a straight line
    if v == 0:
        return - w * d / D, w * d / D  # Case of a rotation around the middle of the 2 wheels

    # General case
    r = v / w
    if w > 0:
        r_l = r - d / 2
        r_r = r + d / 2
    else:
        r_l = r + d / 2
        r_r = r - d / 2
    w_l = 2 * v * r_l / (D * r)
    w_r = 2 * v * r_r / (D * r)
    return w_l, w_r


################################### TESTS ###################################

if __name__ == "__main__":
    import doctest

    doctest.testmod()