()
| 313 | return np.concatenate([cube_position, cube_quat]) |
| 314 | |
| 315 | def sample_insertion_pose(): |
| 316 | # Peg |
| 317 | x_range = [0.1, 0.2] |
| 318 | y_range = [0.4, 0.6] |
| 319 | z_range = [0.05, 0.05] |
| 320 | |
| 321 | ranges = np.vstack([x_range, y_range, z_range]) |
| 322 | peg_position = np.random.uniform(ranges[:, 0], ranges[:, 1]) |
| 323 | |
| 324 | peg_quat = np.array([1, 0, 0, 0]) |
| 325 | peg_pose = np.concatenate([peg_position, peg_quat]) |
| 326 | |
| 327 | # Socket |
| 328 | x_range = [-0.2, -0.1] |
| 329 | y_range = [0.4, 0.6] |
| 330 | z_range = [0.05, 0.05] |
| 331 | |
| 332 | ranges = np.vstack([x_range, y_range, z_range]) |
| 333 | socket_position = np.random.uniform(ranges[:, 0], ranges[:, 1]) |
| 334 | |
| 335 | socket_quat = np.array([1, 0, 0, 0]) |
| 336 | socket_pose = np.concatenate([socket_position, socket_quat]) |
| 337 | |
| 338 | return peg_pose, socket_pose |
| 339 | |
| 340 | ### helper functions |
| 341 |
no outgoing calls
no test coverage detected