| 19 | |
| 20 | |
| 21 | def LSL(alpha, beta, d): |
| 22 | sa = math.sin(alpha) |
| 23 | sb = math.sin(beta) |
| 24 | ca = math.cos(alpha) |
| 25 | cb = math.cos(beta) |
| 26 | c_ab = math.cos(alpha - beta) |
| 27 | |
| 28 | tmp0 = d + sa - sb |
| 29 | |
| 30 | mode = ["L", "S", "L"] |
| 31 | p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb)) |
| 32 | if p_squared < 0: |
| 33 | return None, None, None, mode |
| 34 | tmp1 = math.atan2((cb - ca), tmp0) |
| 35 | t = mod2pi(-alpha + tmp1) |
| 36 | p = math.sqrt(p_squared) |
| 37 | q = mod2pi(beta - tmp1) |
| 38 | # print(np.rad2deg(t), p, np.rad2deg(q)) |
| 39 | |
| 40 | return t, p, q, mode |
| 41 | |
| 42 | |
| 43 | def RSR(alpha, beta, d): |