| 99 | |
| 100 | |
| 101 | def RLR(alpha, beta, d): |
| 102 | sa = math.sin(alpha) |
| 103 | sb = math.sin(beta) |
| 104 | ca = math.cos(alpha) |
| 105 | cb = math.cos(beta) |
| 106 | c_ab = math.cos(alpha - beta) |
| 107 | |
| 108 | mode = ["R", "L", "R"] |
| 109 | tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0 |
| 110 | if abs(tmp_rlr) > 1.0: |
| 111 | return None, None, None, mode |
| 112 | |
| 113 | p = mod2pi(2 * math.pi - math.acos(tmp_rlr)) |
| 114 | t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0)) |
| 115 | q = mod2pi(alpha - beta - t + mod2pi(p)) |
| 116 | return t, p, q, mode |
| 117 | |
| 118 | |
| 119 | def LRL(alpha, beta, d): |