| 462 | (point2[2] - point1[2])**2) |
| 463 | |
| 464 | def getPoseAfterMakeAction(new_pose, action): |
| 465 | x, y, z, yaw = new_pose |
| 466 | |
| 467 | # Define step size |
| 468 | step_size = 3.0 # Translation step size (units can be adjusted as needed) |
| 469 | |
| 470 | # Update new_pose based on action value |
| 471 | if action == 0: |
| 472 | pass |
| 473 | elif action == 1: |
| 474 | x += step_size * math.cos(yaw) |
| 475 | y += step_size * math.sin(yaw) |
| 476 | elif action == 2: |
| 477 | yaw += math.radians(30) |
| 478 | elif action == 3: |
| 479 | yaw -= math.radians(30) |
| 480 | elif action == 4: |
| 481 | z += step_size |
| 482 | elif action == 5: |
| 483 | z -= step_size |
| 484 | elif action == 6: |
| 485 | x -= step_size * math.sin(yaw) |
| 486 | y += step_size * math.cos(yaw) |
| 487 | elif action == 7: |
| 488 | x += step_size * math.sin(yaw) |
| 489 | y -= step_size * math.cos(yaw) |
| 490 | elif action == 8: |
| 491 | x += step_size * math.cos(yaw) *2 |
| 492 | y += step_size * math.sin(yaw) *2 |
| 493 | elif action == 9: |
| 494 | x += step_size * math.cos(yaw) *3 |
| 495 | y += step_size * math.sin(yaw) *3 |
| 496 | |
| 497 | yaw = (yaw + math.pi) % (2 * math.pi) - math.pi |
| 498 | |
| 499 | return [x, y, z, yaw] |
| 500 | |
| 501 | def main(): |
| 502 | eval_info = "configs/eval_test.json" |