milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 1 | import abc |
| 2 | import numpy as np |
| 3 | import sys |
| 4 | import traceback |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 5 | |
| 6 | # joint_center in x-y space. |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 7 | IN_TO_M = 0.0254 |
| 8 | joint_center = (-0.203, 0.787) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 9 | |
| 10 | # Joint distances (l1 = "proximal", l2 = "distal") |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 11 | l1 = 20.0 * IN_TO_M |
| 12 | l2 = 31.5 * IN_TO_M |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 13 | |
| 14 | max_dist = 0.01 |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 15 | max_dist_theta = np.pi / 64 |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 16 | xy_end_circle_size = 0.01 |
| 17 | theta_end_circle_size = 0.07 |
| 18 | |
| 19 | |
milind-u | 060e4cf | 2023-02-22 00:08:52 -0800 | [diff] [blame] | 20 | # Shift the angle between the convention used for input/output and the convention we use for some computations here |
| 21 | def shift_angle(theta): |
| 22 | return np.pi / 2 - theta |
| 23 | |
| 24 | |
| 25 | def shift_angles(thetas): |
| 26 | return [shift_angle(theta) for theta in thetas] |
| 27 | |
| 28 | |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 29 | # Convert from x-y coordinates to theta coordinates. |
| 30 | # orientation is a bool. This orientation is circular_index mod 2. |
| 31 | # where circular_index is the circular index, or the position in the |
| 32 | # "hyperextension" zones. "cross_point" allows shifting the place where |
| 33 | # it rounds the result so that it draws nicer (no other functional differences). |
milind-u | 600738b | 2023-02-22 14:42:19 -0800 | [diff] [blame] | 34 | def to_theta(pt, circular_index, cross_point=-np.pi, die=True): |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 35 | orient = (circular_index % 2) == 0 |
| 36 | x = pt[0] |
| 37 | y = pt[1] |
| 38 | x -= joint_center[0] |
| 39 | y -= joint_center[1] |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 40 | l3 = np.hypot(x, y) |
| 41 | t3 = np.arctan2(y, x) |
| 42 | theta1 = np.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3)) |
| 43 | if np.isnan(theta1): |
milind-u | 600738b | 2023-02-22 14:42:19 -0800 | [diff] [blame] | 44 | print(("Couldn't fit triangle to %f, %f, %f" % (l1, l2, l3))) |
| 45 | if die: |
| 46 | traceback.print_stack() |
| 47 | sys.exit(1) |
| 48 | return None |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 49 | |
| 50 | if orient: |
| 51 | theta1 = -theta1 |
| 52 | theta1 += t3 |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 53 | theta1 = (theta1 - cross_point) % (2 * np.pi) + cross_point |
| 54 | theta2 = np.arctan2(y - l1 * np.sin(theta1), x - l1 * np.cos(theta1)) |
| 55 | return np.array((theta1, theta2)) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 56 | |
| 57 | |
| 58 | # Simple trig to go back from theta1, theta2 to x-y |
| 59 | def to_xy(theta1, theta2): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 60 | x = np.cos(theta1) * l1 + np.cos(theta2) * l2 + joint_center[0] |
| 61 | y = np.sin(theta1) * l1 + np.sin(theta2) * l2 + joint_center[1] |
| 62 | orient = ((theta2 - theta1) % (2.0 * np.pi)) < np.pi |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 63 | return (x, y, orient) |
| 64 | |
| 65 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 66 | END_EFFECTOR_X_LEN = (-1.0 * IN_TO_M, 10.425 * IN_TO_M) |
milind-u | 2a28c59 | 2023-02-24 23:13:04 -0800 | [diff] [blame] | 67 | END_EFFECTOR_Y_LEN = (-3.5 * IN_TO_M, 7.325 * IN_TO_M) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 68 | END_EFFECTOR_Z_LEN = (-11.0 * IN_TO_M, 11.0 * IN_TO_M) |
| 69 | |
| 70 | |
| 71 | def abs_sum(l): |
| 72 | result = 0 |
| 73 | for e in l: |
| 74 | result += abs(e) |
| 75 | return result |
| 76 | |
| 77 | |
| 78 | def affine_3d(R, T): |
| 79 | H = np.eye(4) |
| 80 | H[:3, 3] = T |
| 81 | H[:3, :3] = R |
| 82 | return H |
| 83 | |
| 84 | |
| 85 | # Simple trig to go back from theta1, theta2, and theta3 to |
| 86 | # the 8 corners on the roll joint x-y-z |
| 87 | def to_end_effector_points(theta1, theta2, theta3): |
| 88 | x, y, _ = to_xy(theta1, theta2) |
| 89 | # Homogeneous end effector points relative to the end_effector |
| 90 | # ee = end effector |
| 91 | endpoints_ee = [] |
| 92 | for i in range(2): |
| 93 | for j in range(2): |
| 94 | for k in range(2): |
| 95 | endpoints_ee.append( |
| 96 | np.array((END_EFFECTOR_X_LEN[i], END_EFFECTOR_Y_LEN[j], |
| 97 | END_EFFECTOR_Z_LEN[k], 1.0))) |
| 98 | |
| 99 | # Only roll. |
| 100 | # rj = roll joint |
| 101 | roll = theta3 |
| 102 | T_rj_ee = np.zeros(3) |
| 103 | R_rj_ee = np.array([[1.0, 0.0, 0.0], [0.0, |
| 104 | np.cos(roll), -np.sin(roll)], |
| 105 | [0.0, np.sin(roll), np.cos(roll)]]) |
| 106 | H_rj_ee = affine_3d(R_rj_ee, T_rj_ee) |
| 107 | |
| 108 | # Roll joint pose relative to the origin |
| 109 | # o = origin |
| 110 | T_o_rj = np.array((x, y, 0)) |
| 111 | # Only yaw |
| 112 | yaw = theta1 + theta2 |
| 113 | R_o_rj = [[np.cos(yaw), -np.sin(yaw), 0.0], |
| 114 | [np.sin(yaw), np.cos(yaw), 0.0], [0.0, 0.0, 1.0]] |
| 115 | H_o_rj = affine_3d(R_o_rj, T_o_rj) |
| 116 | |
| 117 | # Now compute the pose of the end effector relative to the origin |
| 118 | H_o_ee = H_o_rj @ H_rj_ee |
| 119 | |
| 120 | # Get the translation from these transforms |
| 121 | endpoints_o = [(H_o_ee @ endpoint_ee)[:3] for endpoint_ee in endpoints_ee] |
| 122 | |
| 123 | diagonal_distance = np.linalg.norm( |
| 124 | np.array(endpoints_o[0]) - np.array(endpoints_o[-1])) |
| 125 | actual_diagonal_distance = np.linalg.norm( |
| 126 | np.array((abs_sum(END_EFFECTOR_X_LEN), abs_sum(END_EFFECTOR_Y_LEN), |
| 127 | abs_sum(END_EFFECTOR_Z_LEN)))) |
| 128 | assert abs(diagonal_distance - actual_diagonal_distance) < 1e-5 |
| 129 | |
| 130 | return np.array(endpoints_o) |
| 131 | |
| 132 | |
| 133 | # Returns all permutations of rectangle points given two opposite corners. |
| 134 | # x is the two x values, y is the two y values, z is the two z values |
| 135 | def rect_points(x, y, z): |
| 136 | points = [] |
| 137 | for i in range(2): |
| 138 | for j in range(2): |
| 139 | for k in range(2): |
| 140 | points.append((x[i], y[j], z[k])) |
| 141 | return np.array(points) |
| 142 | |
| 143 | |
| 144 | DRIVER_CAM_Z_OFFSET = 3.225 * IN_TO_M |
| 145 | DRIVER_CAM_POINTS = rect_points( |
| 146 | (-5.126 * IN_TO_M + joint_center[0], 0.393 * IN_TO_M + joint_center[0]), |
| 147 | (5.125 * IN_TO_M + joint_center[1], 17.375 * IN_TO_M + joint_center[1]), |
| 148 | (-8.475 * IN_TO_M - DRIVER_CAM_Z_OFFSET, |
| 149 | -4.350 * IN_TO_M - DRIVER_CAM_Z_OFFSET)) |
| 150 | |
| 151 | |
milind-u | 2a28c59 | 2023-02-24 23:13:04 -0800 | [diff] [blame] | 152 | def rect_collision_1d(min_1, max_1, min_2, max_2): |
| 153 | return (min_1 <= min_2 <= max_1) or (min_1 <= max_2 <= max_1) or ( |
| 154 | min_2 < min_1 and max_2 > max_1) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 155 | |
| 156 | |
| 157 | def roll_joint_collision(theta1, theta2, theta3): |
milind-u | 060e4cf | 2023-02-22 00:08:52 -0800 | [diff] [blame] | 158 | theta1 = shift_angle(theta1) |
| 159 | theta2 = shift_angle(theta2) |
| 160 | theta3 = shift_angle(theta3) |
| 161 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 162 | end_effector_points = to_end_effector_points(theta1, theta2, theta3) |
| 163 | |
| 164 | assert len(end_effector_points) == 8 and len(end_effector_points[0]) == 3 |
| 165 | assert len(DRIVER_CAM_POINTS) == 8 and len(DRIVER_CAM_POINTS[0]) == 3 |
milind-u | 2a28c59 | 2023-02-24 23:13:04 -0800 | [diff] [blame] | 166 | collided = True |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 167 | |
milind-u | 2a28c59 | 2023-02-24 23:13:04 -0800 | [diff] [blame] | 168 | for i in range(len(end_effector_points[0])): |
| 169 | min_ee = min(end_effector_points[:, i]) |
| 170 | max_ee = max(end_effector_points[:, i]) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 171 | |
milind-u | 2a28c59 | 2023-02-24 23:13:04 -0800 | [diff] [blame] | 172 | min_dc = min(DRIVER_CAM_POINTS[:, i]) |
| 173 | max_dc = max(DRIVER_CAM_POINTS[:, i]) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 174 | |
milind-u | 2a28c59 | 2023-02-24 23:13:04 -0800 | [diff] [blame] | 175 | collided &= rect_collision_1d(min_ee, max_ee, min_dc, max_dc) |
| 176 | return collided |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 177 | |
| 178 | |
milind-u | eeb08c5 | 2023-02-21 22:30:16 -0800 | [diff] [blame] | 179 | # Delta limit means theta2 - theta1. |
| 180 | # The limit for the proximal and distal is relative, |
| 181 | # so define constraints for this delta. |
| 182 | UPPER_DELTA_LIMIT = 0.0 |
| 183 | LOWER_DELTA_LIMIT = -1.9 * np.pi |
| 184 | |
| 185 | # TODO(milind): put actual proximal limits |
| 186 | UPPER_PROXIMAL_LIMIT = np.pi * 1.5 |
| 187 | LOWER_PROXIMAL_LIMIT = -np.pi |
| 188 | |
Austin Schuh | 8edaf3e | 2023-02-22 21:20:52 -0800 | [diff] [blame] | 189 | UPPER_DISTAL_LIMIT = 0.75 * np.pi |
| 190 | LOWER_DISTAL_LIMIT = -0.75 * np.pi |
| 191 | |
milind-u | eeb08c5 | 2023-02-21 22:30:16 -0800 | [diff] [blame] | 192 | UPPER_ROLL_JOINT_LIMIT = 0.75 * np.pi |
| 193 | LOWER_ROLL_JOINT_LIMIT = -0.75 * np.pi |
| 194 | |
| 195 | |
| 196 | def arm_past_limit(theta1, theta2, theta3): |
| 197 | delta = theta2 - theta1 |
Austin Schuh | 8edaf3e | 2023-02-22 21:20:52 -0800 | [diff] [blame] | 198 | return delta > UPPER_DELTA_LIMIT or delta < LOWER_DELTA_LIMIT or \ |
| 199 | theta1 > UPPER_PROXIMAL_LIMIT or theta1 < LOWER_PROXIMAL_LIMIT or \ |
| 200 | theta2 > UPPER_DISTAL_LIMIT or theta2 < LOWER_DISTAL_LIMIT or \ |
| 201 | theta3 > UPPER_ROLL_JOINT_LIMIT or theta3 < LOWER_ROLL_JOINT_LIMIT |
milind-u | eeb08c5 | 2023-02-21 22:30:16 -0800 | [diff] [blame] | 202 | |
| 203 | |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 204 | def get_circular_index(theta): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 205 | return int(np.floor((theta[1] - theta[0]) / np.pi)) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 206 | |
| 207 | |
| 208 | def get_xy(theta): |
milind-u | 060e4cf | 2023-02-22 00:08:52 -0800 | [diff] [blame] | 209 | theta1 = shift_angle(theta[0]) |
| 210 | theta2 = shift_angle(theta[1]) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 211 | x = np.cos(theta1) * l1 + np.cos(theta2) * l2 + joint_center[0] |
| 212 | y = np.sin(theta1) * l1 + np.sin(theta2) * l2 + joint_center[1] |
| 213 | return np.array((x, y)) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 214 | |
| 215 | |
| 216 | # Subdivide in theta space. |
| 217 | def subdivide_theta(lines): |
| 218 | out = [] |
| 219 | last_pt = lines[0] |
| 220 | out.append(last_pt) |
| 221 | for n_pt in lines[1:]: |
| 222 | for pt in subdivide(last_pt, n_pt, max_dist_theta): |
| 223 | out.append(pt) |
| 224 | last_pt = n_pt |
| 225 | |
| 226 | return out |
| 227 | |
| 228 | |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 229 | def to_theta_with_ci(pt, circular_index): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 230 | return (to_theta_with_circular_index(pt[0], pt[1], circular_index)) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 231 | |
| 232 | |
| 233 | # to_theta, but distinguishes between |
| 234 | def to_theta_with_circular_index(x, y, circular_index): |
| 235 | theta1, theta2 = to_theta((x, y), circular_index) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 236 | n_circular_index = int(np.floor((theta2 - theta1) / np.pi)) |
| 237 | theta2 = theta2 + ((circular_index - n_circular_index)) * np.pi |
milind-u | 060e4cf | 2023-02-22 00:08:52 -0800 | [diff] [blame] | 238 | return np.array((shift_angle(theta1), shift_angle(theta2))) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 239 | |
| 240 | |
| 241 | # to_theta, but distinguishes between |
| 242 | def to_theta_with_circular_index_and_roll(x, y, roll, circular_index): |
| 243 | theta12 = to_theta_with_circular_index(x, y, circular_index) |
| 244 | theta3 = roll |
| 245 | return np.array((theta12[0], theta12[1], theta3)) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 246 | |
| 247 | |
| 248 | # alpha is in [0, 1] and is the weight to merge a and b. |
| 249 | def alpha_blend(a, b, alpha): |
| 250 | """Blends a and b. |
| 251 | |
| 252 | Args: |
| 253 | alpha: double, Ratio. Needs to be in [0, 1] and is the weight to blend a |
| 254 | and b. |
| 255 | """ |
| 256 | return b * alpha + (1.0 - alpha) * a |
| 257 | |
| 258 | |
| 259 | def normalize(v): |
| 260 | """Normalize a vector while handling 0 length vectors.""" |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 261 | norm = np.linalg.norm(v) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 262 | if norm == 0: |
| 263 | return v |
| 264 | return v / norm |
| 265 | |
| 266 | |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 267 | # Generic subdivision algorithm. |
| 268 | def subdivide(p1, p2, max_dist): |
| 269 | dx = p2[0] - p1[0] |
| 270 | dy = p2[1] - p1[1] |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 271 | dist = np.sqrt(dx**2 + dy**2) |
| 272 | n = int(np.ceil(dist / max_dist)) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 273 | return [(alpha_blend(p1[0], p2[0], |
| 274 | float(i) / n), alpha_blend(p1[1], p2[1], |
| 275 | float(i) / n)) |
| 276 | for i in range(1, n + 1)] |
| 277 | |
| 278 | |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 279 | def spline_eval(start, control1, control2, end, alpha): |
| 280 | a = alpha_blend(start, control1, alpha) |
| 281 | b = alpha_blend(control1, control2, alpha) |
| 282 | c = alpha_blend(control2, end, alpha) |
| 283 | return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha), |
| 284 | alpha) |
| 285 | |
| 286 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 287 | SPLINE_SUBDIVISIONS = 100 |
| 288 | |
| 289 | |
| 290 | def subdivide_multistep(): |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 291 | # TODO: pick N based on spline parameters? or otherwise change it to be more evenly spaced? |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 292 | for i in range(0, SPLINE_SUBDIVISIONS + 1): |
| 293 | yield i / float(SPLINE_SUBDIVISIONS) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 294 | |
| 295 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 296 | def get_proximal_distal_derivs(t_prev, t, t_next): |
| 297 | d_prev = normalize(t - t_prev) |
| 298 | d_next = normalize(t_next - t) |
| 299 | accel = (d_next - d_prev) / np.linalg.norm(t - t_next) |
| 300 | return (ThetaPoint(t[0], d_next[0], |
| 301 | accel[0]), ThetaPoint(t[1], d_next[1], accel[1])) |
| 302 | |
| 303 | |
| 304 | def get_roll_joint_theta(theta_i, theta_f, t): |
| 305 | # Fit a theta(t) = (1 - cos(pi*t)) / 2, |
| 306 | # so that theta(0) = theta_i, and theta(1) = theta_f |
| 307 | offset = theta_i |
| 308 | scalar = (theta_f - theta_i) / 2.0 |
| 309 | freq = np.pi |
| 310 | theta_curve = lambda t: scalar * (1 - np.cos(freq * t)) + offset |
| 311 | |
| 312 | return theta_curve(t) |
| 313 | |
| 314 | |
| 315 | def get_roll_joint_theta_multistep(alpha_rolls, alpha): |
| 316 | # Figure out which segment in the motion we're in |
| 317 | theta_i = None |
| 318 | theta_f = None |
| 319 | t = None |
| 320 | |
| 321 | for i in range(len(alpha_rolls) - 1): |
| 322 | # Find the alpha segment we're in |
| 323 | if alpha_rolls[i][0] <= alpha <= alpha_rolls[i + 1][0]: |
| 324 | theta_i = alpha_rolls[i][1] |
| 325 | theta_f = alpha_rolls[i + 1][1] |
| 326 | |
| 327 | total_dalpha = alpha_rolls[-1][0] - alpha_rolls[0][0] |
| 328 | assert total_dalpha == 1.0 |
| 329 | dalpha = alpha_rolls[i + 1][0] - alpha_rolls[i][0] |
| 330 | t = (alpha - alpha_rolls[i][0]) * (total_dalpha / dalpha) |
| 331 | break |
| 332 | assert theta_i is not None |
| 333 | assert theta_f is not None |
| 334 | assert t is not None |
| 335 | |
| 336 | return get_roll_joint_theta(theta_i, theta_f, t) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 337 | |
| 338 | |
Maxwell Henderson | 83cf6d6 | 2023-02-10 20:29:26 -0800 | [diff] [blame] | 339 | # Draw a list of lines to a cairo context. |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 340 | def draw_lines(cr, lines): |
| 341 | cr.move_to(lines[0][0], lines[0][1]) |
| 342 | for pt in lines[1:]: |
| 343 | cr.line_to(pt[0], pt[1]) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 344 | |
| 345 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 346 | class Path(abc.ABC): |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 347 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 348 | def __init__(self, name): |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 349 | self.name = name |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 350 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 351 | @abc.abstractmethod |
| 352 | def DoToThetaPoints(self): |
| 353 | pass |
| 354 | |
| 355 | @abc.abstractmethod |
| 356 | def DoDrawTo(self): |
| 357 | pass |
| 358 | |
| 359 | @abc.abstractmethod |
milind-u | add8fa3 | 2023-02-24 23:37:36 -0800 | [diff] [blame] | 360 | def joint_thetas(self): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 361 | pass |
| 362 | |
| 363 | @abc.abstractmethod |
| 364 | def intersection(self, event): |
| 365 | pass |
| 366 | |
| 367 | def roll_joint_collision(self, points, verbose=False): |
| 368 | for point in points: |
| 369 | if roll_joint_collision(*point): |
| 370 | if verbose: |
| 371 | print("Roll joint collision for path %s in point %s" % |
| 372 | (self.name, point)) |
| 373 | return True |
| 374 | return False |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 375 | |
milind-u | eeb08c5 | 2023-02-21 22:30:16 -0800 | [diff] [blame] | 376 | def arm_past_limit(self, points, verbose=True): |
| 377 | for point in points: |
| 378 | if arm_past_limit(*point): |
| 379 | if verbose: |
| 380 | print("Arm past limit for path %s in point %s" % |
| 381 | (self.name, point)) |
| 382 | return True |
| 383 | return False |
| 384 | |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 385 | def DrawTo(self, cr, theta_version): |
milind-u | eeb08c5 | 2023-02-21 22:30:16 -0800 | [diff] [blame] | 386 | points = self.DoToThetaPoints() |
| 387 | if self.roll_joint_collision(points): |
| 388 | # Draw the spline red |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 389 | cr.set_source_rgb(1.0, 0.0, 0.0) |
milind-u | eeb08c5 | 2023-02-21 22:30:16 -0800 | [diff] [blame] | 390 | elif self.arm_past_limit(points): |
| 391 | # Draw the spline orange |
| 392 | cr.set_source_rgb(1.0, 0.5, 0.0) |
| 393 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 394 | self.DoDrawTo(cr, theta_version) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 395 | |
milind-u | 4037bc7 | 2023-02-22 21:39:40 -0800 | [diff] [blame] | 396 | def VerifyPoints(self): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 397 | points = self.DoToThetaPoints() |
milind-u | eeb08c5 | 2023-02-21 22:30:16 -0800 | [diff] [blame] | 398 | if self.roll_joint_collision(points, verbose=True) or \ |
| 399 | self.arm_past_limit(points, verbose=True): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 400 | sys.exit(1) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 401 | |
| 402 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 403 | class SplineSegmentBase(Path): |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 404 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 405 | def __init__(self, name): |
| 406 | super().__init__(name) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 407 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 408 | @abc.abstractmethod |
| 409 | # Returns (start, control1, control2, end), each in the form |
| 410 | # (theta1, theta2, theta3) |
| 411 | def get_controls_theta(self): |
| 412 | pass |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 413 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 414 | def intersection(self, event): |
| 415 | start, control1, control2, end = self.get_controls_theta() |
| 416 | for alpha in subdivide_multistep(): |
| 417 | x, y = get_xy(spline_eval(start, control1, control2, end, alpha)) |
| 418 | spline_point = np.array([x, y]) |
| 419 | hovered_point = np.array([event.x, event.y]) |
| 420 | if np.linalg.norm(hovered_point - spline_point) < 0.03: |
| 421 | return alpha |
| 422 | return None |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 423 | |
| 424 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 425 | class ThetaSplineSegment(SplineSegmentBase): |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 426 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 427 | # start and end are [theta1, theta2, theta3]. |
| 428 | # controls are just [theta1, theta2]. |
| 429 | # control_alpha_rolls are a list of [alpha, roll] |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 430 | def __init__(self, |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 431 | name, |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 432 | start, |
| 433 | control1, |
| 434 | control2, |
| 435 | end, |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 436 | control_alpha_rolls=[], |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 437 | alpha_unitizer=None, |
| 438 | vmax=None): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 439 | super().__init__(name) |
| 440 | self.start = start[:2] |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 441 | self.control1 = control1 |
| 442 | self.control2 = control2 |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 443 | self.end = end[:2] |
| 444 | # There will always be roll at alpha = 0 and 1 |
| 445 | self.alpha_rolls = [[0.0, start[2]] |
| 446 | ] + control_alpha_rolls + [[1.0, end[2]]] |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 447 | self.alpha_unitizer = alpha_unitizer |
| 448 | self.vmax = vmax |
| 449 | |
| 450 | def __repr__(self): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 451 | return "ThetaSplineSegment(%s, %s, %s, %s)" % (repr( |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 452 | self.start), repr(self.control1), repr( |
| 453 | self.control2), repr(self.end)) |
| 454 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 455 | def DoDrawTo(self, cr, theta_version): |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 456 | if (theta_version): |
| 457 | draw_lines(cr, [ |
milind-u | 060e4cf | 2023-02-22 00:08:52 -0800 | [diff] [blame] | 458 | shift_angles( |
| 459 | spline_eval(self.start, self.control1, self.control2, |
| 460 | self.end, alpha)) |
| 461 | for alpha in subdivide_multistep() |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 462 | ]) |
| 463 | else: |
| 464 | start = get_xy(self.start) |
| 465 | end = get_xy(self.end) |
| 466 | |
| 467 | draw_lines(cr, [ |
| 468 | get_xy( |
| 469 | spline_eval(self.start, self.control1, self.control2, |
| 470 | self.end, alpha)) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 471 | for alpha in subdivide_multistep() |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 472 | ]) |
| 473 | |
| 474 | cr.move_to(start[0] + xy_end_circle_size, start[1]) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 475 | cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * np.pi) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 476 | cr.move_to(end[0] + xy_end_circle_size, end[1]) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 477 | cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * np.pi) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 478 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 479 | def DoToThetaPoints(self): |
| 480 | points = [] |
| 481 | for alpha in subdivide_multistep(): |
| 482 | proximal, distal = spline_eval(self.start, self.control1, |
| 483 | self.control2, self.end, alpha) |
| 484 | roll_joint = get_roll_joint_theta_multistep( |
| 485 | self.alpha_rolls, alpha) |
| 486 | points.append((proximal, distal, roll_joint)) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 487 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 488 | return points |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 489 | |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 490 | def get_controls_theta(self): |
| 491 | return (self.start, self.control1, self.control2, self.end) |
Maxwell Henderson | f5123fe | 2023-02-04 13:44:41 -0800 | [diff] [blame] | 492 | |
milind-u | add8fa3 | 2023-02-24 23:37:36 -0800 | [diff] [blame] | 493 | def joint_thetas(self): |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 494 | ts = [] |
milind-u | add8fa3 | 2023-02-24 23:37:36 -0800 | [diff] [blame] | 495 | thetas = [[], [], []] |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 496 | for alpha in subdivide_multistep(): |
milind-u | add8fa3 | 2023-02-24 23:37:36 -0800 | [diff] [blame] | 497 | proximal, distal = spline_eval(self.start, self.control1, |
| 498 | self.control2, self.end, alpha) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 499 | roll_joint = get_roll_joint_theta_multistep( |
| 500 | self.alpha_rolls, alpha) |
milind-u | add8fa3 | 2023-02-24 23:37:36 -0800 | [diff] [blame] | 501 | thetas[0].append(proximal) |
| 502 | thetas[1].append(distal) |
| 503 | thetas[2].append(roll_joint) |
milind-u | 18a901d | 2023-02-17 21:51:55 -0800 | [diff] [blame] | 504 | ts.append(alpha) |
| 505 | return ts, thetas |