commit 08eb7a6b78eefb0445fa0b1b0896327f6c2e4348 Author: duongtd Date: Tue Jul 28 23:35:42 2026 +0700 first commit diff --git a/README.md b/README.md new file mode 100644 index 0000000..b079200 --- /dev/null +++ b/README.md @@ -0,0 +1,84 @@ +# mprim_gen — Sinh file `.mprim` cho SBPL lattice planner (Python) + +Port lại từ các script MATLAB gốc (`genmprim*.m` của Maxim Likhachev), viết lại +theo kiến trúc module hóa để dễ thêm robot mới mà **không phải sửa code logic**. + +## Cấu trúc file + +``` +mprim_gen/ +├── mprim_config.py # RobotConfig — định nghĩa thông số + validate (KHÔNG có logic sinh) +├── kinematics.py # 2 interpolator: unicycle (diff + ackermann) / holonomic (omni) +├── mprim_core.py # engine dùng chung: build primitive set, xoay theo góc, tự kiểm, ghi file +├── generate_mprim.py # CLI chạy +├── test_mprim_gen.py # test tự động: ràng buộc SBPL + chất lượng hình học +├── visualize_mprim.py # vẽ primitive tại 1 góc, kèm vạch heading từng pose +└── configs/ # MỖI ROBOT = 1 file config nhỏ ở đây + ├── differential.py + ├── omnidirectional.py + └── ackermann.py +``` + +**Nguyên tắc:** khi thêm robot mới, bạn KHÔNG đụng vào `mprim_core.py` hay +`kinematics.py` — chỉ copy 1 file trong `configs/`, đổi số liệu, chạy lại. + +## Cách dùng + +```bash +python3 generate_mprim.py configs/differential.py -o diff.mprim +python3 generate_mprim.py configs/omnidirectional.py -o omni.mprim +python3 generate_mprim.py configs/ackermann.py -o ackermann.mprim + +python3 test_mprim_gen.py # chạy toàn bộ test trước khi tích hợp +python3 visualize_mprim.py diff.mprim --angle 1 # kiểm tra bằng mắt ở góc lẻ +``` + +## Những gì generator ĐẢM BẢO trên file output + +Đây là các ràng buộc SBPL enforce lúc nạp file (`ReadinMotionPrimitive` trong +`environment_navxythetalat.cpp`) — engine tự kiểm sau khi sinh, vi phạm là raise +ngay lúc generate thay vì fail lúc khởi động planner trên robot: + +1. `additionalactioncostmult` là **số nguyên** (SBPL đọc bằng `fscanf %d`, + giá trị 1.5/2.5 làm hỏng cả file). +2. Pose cuối của quỹ đạo rời rạc hóa ra **đúng** `endpose_c`. +3. **Heading cuối khớp chính xác** góc rời rạc đích (không còn kiểu lệch 10 độ + do ép `l = 0` mà không tính lại `rv` như bản MATLAB gốc). +4. Khoảng cách giữa 2 intermediate pose **<= 1 cell và ~ đều nhau** trên mọi + primitive (`pose_spacing_m`) → path SBPL trả ra có pose cách đều, footprint + sweep không bỏ sót cell. +5. Không có primitive suy biến, cảnh báo khi trùng endpose, cảnh báo khi cung + sau quantize vi phạm `min_turning_radius_m` (ackermann). + +## Thông số cần đo/lấy từ robot thật để điền vào config + +| Thông số | Cách lấy | +|---|---| +| `resolution_m` | PHẢI khớp `resolution` của costmap (global/local costmap yaml) | +| `forward_long_m` | quãng đường robot đi trong ~1 chu kỳ điều khiển ở vận tốc hành trình | +| `turn_radius_m` | >= `max_vel/max_yawrate` của robot, và >= `resolution/(1-cos(bước góc))` để quantize không làm méo cung (~0.66m với lưới 5cm, 16 góc) | +| `min_turning_radius_m` (Ackermann) | đo trực tiếp trên robot (bán kính hẹp nhất không trượt bánh) | +| `sidestep_m`, `diagonal_m` (omni) | quãng đường đi ngang/chéo robot làm an toàn được | +| `max_vel_mps`, `max_yawrate_radps` | từ config local planner (vd `robot_max_v_ac`/`robot_max_w_ac`) — chỉ để sanity-check | +| cost mult | **số nguyên**; điều chỉnh planner ưu tiên hành động nào. Bộ tham chiếu an toàn: backward=40, turninplace=20, forwardandturn=2 | + +## Khác biệt so với bản MATLAB gốc (lưu ý quan trọng) + +Bản gốc định nghĩa **tay 3 bộ template số nguyên riêng** cho góc 0°/45°/22.5°. +Bản Python định nghĩa primitive **một lần theo mét** (bán kính cung, chiều dài) +rồi tự xoay + làm tròn về cell cho từng góc. Ưu điểm: dễ tham số hóa theo robot +thật, primitive trái/phải đối xứng tuyệt đối (template tay của bản gốc bị lệch +trái/phải tới 8°). Đánh đổi: sai số lượng tử hóa nhỏ ở các góc lẻ (< nửa cell, +được dàn đều dọc quỹ đạo, heading không bị ảnh hưởng). + +Khác biệt hành vi so với bản gốc: khi nghiệm "đoạn thẳng + cung" đòi đảo chiều +(do quantize), bản gốc ép `l = 0` nhưng giữ nguyên `rv` làm heading cuối lệch +tới ~10° (chỉ in warning trong MATLAB console); bản này tính lại `rv` để heading +cuối luôn chính xác — đây là điều kiện SBPL dùng để chấp nhận file. + +## Tích hợp vào planner + +File output dùng trực tiếp với `sbpl_lattice_planner` qua key `primitive_filename` +trong `sbpl_global_params.yaml`. Sau khi đổi file mprim cần kiểm tra: +`resolution_m` khớp costmap, và log khởi động planner không có +`ERROR: incorrect primitive`. diff --git a/__pycache__/generate_mprim.cpython-38.pyc b/__pycache__/generate_mprim.cpython-38.pyc new file mode 100644 index 0000000..99beeaa Binary files /dev/null and b/__pycache__/generate_mprim.cpython-38.pyc differ diff --git a/__pycache__/kinematics.cpython-38.pyc b/__pycache__/kinematics.cpython-38.pyc new file mode 100644 index 0000000..0560387 Binary files /dev/null and b/__pycache__/kinematics.cpython-38.pyc differ diff --git a/__pycache__/mprim_config.cpython-38.pyc b/__pycache__/mprim_config.cpython-38.pyc new file mode 100644 index 0000000..d408756 Binary files /dev/null and b/__pycache__/mprim_config.cpython-38.pyc differ diff --git a/__pycache__/mprim_core.cpython-38.pyc b/__pycache__/mprim_core.cpython-38.pyc new file mode 100644 index 0000000..83ea93c Binary files /dev/null and b/__pycache__/mprim_core.cpython-38.pyc differ diff --git a/configs/__pycache__/ackermann.cpython-38.pyc b/configs/__pycache__/ackermann.cpython-38.pyc new file mode 100644 index 0000000..51f8d71 Binary files /dev/null and b/configs/__pycache__/ackermann.cpython-38.pyc differ diff --git a/configs/__pycache__/differential.cpython-38.pyc b/configs/__pycache__/differential.cpython-38.pyc new file mode 100644 index 0000000..64cc8a3 Binary files /dev/null and b/configs/__pycache__/differential.cpython-38.pyc differ diff --git a/configs/__pycache__/omnidirectional.cpython-38.pyc b/configs/__pycache__/omnidirectional.cpython-38.pyc new file mode 100644 index 0000000..7d5b46a Binary files /dev/null and b/configs/__pycache__/omnidirectional.cpython-38.pyc differ diff --git a/configs/ackermann.py b/configs/ackermann.py new file mode 100644 index 0000000..b5497aa --- /dev/null +++ b/configs/ackermann.py @@ -0,0 +1,35 @@ +"""Config Ackermann (xe có bánh lái). Chỉnh số liệu theo robot thực tế.""" + +from mprim_config import RobotConfig, KinematicType + +config = RobotConfig( + name="ackermann", # tên file output: ackermann.mprim + kinematic_type=KinematicType.ACKERMANN, # xe bánh lái -> nội suy cung tròn unicycle, + # có kiểm tra min_turning_radius + + resolution_m=0.05, # [m] PHẢI khớp costmap resolution + numberofangles=16, # 16 góc heading -> 1 bước góc = 22.5 độ + + forward_short_m=0.20, # [m] bước tiến ngắn (xe dài nên bước ngắn cũng dài hơn AMR) + forward_long_m=0.60, # [m] bước tiến dài + backward_m=0.20, # [m] bước lùi thẳng + + min_turning_radius_m=0.45, # [m] bán kính quay tối thiểu VẬT LÝ, đo từ robot thật + turn_steps=(1,), # chỉ rẽ 22.5 độ mỗi primitive + turn_radius_m=0.70, # [m] cao hơn min_turning_radius làm dự phòng quantize; với + # lưới 5cm cần >= ~0.66 để cung không bị méo (validate + # sẽ cảnh báo nếu thiếu) + + has_backward_prims=True, # cho phép lùi thẳng + has_backward_turn_prims=True, # xe bánh lái cần lùi + đánh lái để quay đầu + has_turn_in_place_prims=False, # xe Ackermann KHÔNG quay tại chỗ được + + # SỐ NGUYÊN (SBPL đọc %d). Ackermann lùi tốn hơn nhiều (khó điều khiển, hạn chế tầm nhìn). + forwardcostmult=1, # tiến thẳng — mốc chuẩn + backwardcostmult=6, # lùi thẳng + forwardandturncostmult=3, # tiến + đánh lái + backwardandturncostmult=8, # lùi + đánh lái — đắt nhất, chỉ dành cho quay đầu + turninplacecostmult=5, # không dùng khi has_turn_in_place_prims=False + + pose_spacing_m=0.05, # [m] khoảng cách pose trên path +) diff --git a/configs/differential.py b/configs/differential.py new file mode 100644 index 0000000..ec638ee --- /dev/null +++ b/configs/differential.py @@ -0,0 +1,46 @@ +"""Config differential drive (2 bánh vi sai). Chỉnh số liệu theo robot thực tế. + +Cost mult lấy theo bộ tuned unicycle_highcost_5cm.mprim (backward=40, turninplace=20, +forwardandturn=2): lùi và quay tại chỗ đắt tới mức chỉ được chọn khi không còn đường +khác — an toàn cho AMR có cảm biến chính hướng về phía trước. Muốn robot thoải mái +lùi/xoay hơn thì hạ dần (vd 5/5/3) nhưng đó là quyết định vận hành có chủ đích. +""" + +from mprim_config import RobotConfig, KinematicType + +config = RobotConfig( + name="differential", # tên file output: differential.mprim + kinematic_type=KinematicType.DIFFERENTIAL, # 2 bánh vi sai -> nội suy unicycle + + resolution_m=0.05, # [m] PHẢI khớp costmap resolution (costmap_global_params.yaml) + numberofangles=16, # 16 góc heading -> 1 bước góc = 22.5 độ + + forward_short_m=0.10, # [m] bước tiến ngắn ~2 cell, để tinh chỉnh gần goal + forward_long_m=0.40, # [m] bước tiến dài ~8 cell — quãng đường 1 chu kỳ điều khiển + # ở tốc độ hành trình + backward_m=0.10, # [m] bước lùi thẳng + + # Rẽ = cung tròn: (1,) nghĩa là chỉ có primitive rẽ 22.5 độ (giống bộ tham chiếu). + # Thêm 2 vào tuple nếu cần rẽ 45 độ — đổi lại branching factor tăng, planner chậm hơn. + turn_steps=(1,), + turn_radius_m=0.70, # [m] >= max_vel/max_yawrate = 0.4/0.6 ~ 0.67 để local planner + # bám cung không phải giảm tốc; >= 0.66 để quantize 5cm + # không làm méo cung + + has_backward_prims=True, # cho phép lùi thẳng (cost 40 nên chỉ dùng khi kẹt) + has_backward_turn_prims=False, # lùi + rẽ tắt: hành động mù cảm biến, bật lại nếu thật cần + has_turn_in_place_prims=True, # cho phép quay tại chỗ (cost 20 nên hiếm khi được chọn) + + # SỐ NGUYÊN (SBPL đọc %d) + forwardcostmult=1, # tiến thẳng — mốc chuẩn + backwardcostmult=40, # lùi 0.1m "đắt" bằng tiến 4m -> chỉ lùi khi không còn cách + forwardandturncostmult=2, # rẽ đắt gấp đôi đi thẳng -> ưu tiên đường ít cua + backwardandturncostmult=40, # (không dùng khi has_backward_turn_prims=False) + turninplacecostmult=20, # xoay 22.5 độ "đắt" bằng tiến ~4m -> tránh xoay giữa đường + + pose_spacing_m=0.05, # [m] = resolution -> path SBPL trả ra có pose cách đều ~5cm + + # giới hạn động học thật (hybrid_local_planner_params.yaml: robot_max_v_ac / robot_max_w_ac) + max_vel_mps=0.4, # [m/s] vận tốc dài tối đa + max_yawrate_radps=0.6, # [rad/s] vận tốc xoay tối đa +) diff --git a/configs/omnidirectional.py b/configs/omnidirectional.py new file mode 100644 index 0000000..0f37c49 --- /dev/null +++ b/configs/omnidirectional.py @@ -0,0 +1,38 @@ +"""Config omnidirectional (mecanum/omni/swerve). Chỉnh số liệu theo robot thực tế.""" + +from mprim_config import RobotConfig, KinematicType + +config = RobotConfig( + name="omnidirectional", # tên file output: omnidirectional.mprim + kinematic_type=KinematicType.OMNIDIRECTIONAL, # holonomic -> nội suy tuyến tính x/y/theta + + resolution_m=0.05, # [m] PHẢI khớp costmap resolution + numberofangles=16, # 16 góc heading -> 1 bước góc = 22.5 độ + + forward_short_m=0.10, # [m] bước tiến ngắn ~2 cell + forward_long_m=0.40, # [m] bước tiến dài ~8 cell + backward_m=0.10, # [m] bước lùi thẳng + + turn_steps=(1,), # chỉ có primitive rẽ 22.5 độ; thêm 2 nếu cần rẽ 45 độ + turn_radius_m=0.70, # [m] bán kính cung rẽ (omni vẫn dùng cung cho mượt) + + sidestep_m=0.15, # [m] đặc thù omni: đi ngang giữ nguyên heading + diagonal_m=0.15, # [m] đặc thù omni: đi chéo 45 độ giữ nguyên heading + + has_backward_prims=True, # cho phép lùi thẳng + has_backward_turn_prims=True, # omni lùi không mù như diff-drive, giữ bật + has_turn_in_place_prims=True, # cho phép quay tại chỗ + + # SỐ NGUYÊN (SBPL đọc %d). Omni lùi/xoay "rẻ" hơn diff-drive vì không cần quay đầu. + forwardcostmult=1, # tiến thẳng — mốc chuẩn + backwardcostmult=3, # lùi rẻ hơn diff-drive (robot đối xứng, không mù hẳn) + forwardandturncostmult=3, # vừa đi vừa rẽ + backwardandturncostmult=4, # lùi + rẽ + turninplacecostmult=4, # quay tại chỗ + sidestepcostmult=2, # đi ngang — hơi đắt hơn tiến để không "cua" bằng sidestep + diagonalcostmult=2, # đi chéo 45 độ + + pose_spacing_m=0.05, # [m] khoảng cách pose trên path + + # chưa khai báo max_vel/max_yawrate: bỏ qua sanity-check động học +) diff --git a/diff.mprim b/diff.mprim new file mode 100644 index 0000000..31bc3b1 --- /dev/null +++ b/diff.mprim @@ -0,0 +1,1223 @@ +resolution_m: 0.050000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 2 0 0 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 0.0000 +0.0500 0.0000 0.0000 +0.1000 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 8 0 0 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 0.0000 +0.0500 0.0000 0.0000 +0.1000 0.0000 0.0000 +0.1500 0.0000 0.0000 +0.2000 0.0000 0.0000 +0.2500 0.0000 0.0000 +0.3000 0.0000 0.0000 +0.3500 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -2 0 0 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 0.0000 +-0.0500 0.0000 0.0000 +-0.1000 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 5 1 1 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 0.0000 +0.0427 0.0014 0.0654 +0.0853 0.0056 0.1309 +0.1275 0.0126 0.1963 +0.1691 0.0224 0.2618 +0.2100 0.0349 0.3272 +0.2500 0.0500 0.3927 +primID: 4 +startangle_c: 0 +endpose_c: 5 -1 15 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 0.0000 +0.0427 -0.0014 -0.0654 +0.0853 -0.0056 -0.1309 +0.1275 -0.0126 -0.1963 +0.1691 -0.0224 -0.2618 +0.2100 -0.0349 -0.3272 +0.2500 -0.0500 -0.3927 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0982 +0.0000 0.0000 0.1963 +0.0000 0.0000 0.2945 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 15 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0982 +0.0000 0.0000 -0.1963 +0.0000 0.0000 -0.2945 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 0.3927 +0.0333 0.0167 0.3927 +0.0667 0.0333 0.3927 +0.1000 0.0500 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 7 3 1 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 0.3927 +0.0438 0.0188 0.3927 +0.0875 0.0375 0.3927 +0.1313 0.0563 0.3927 +0.1750 0.0750 0.3927 +0.2188 0.0938 0.3927 +0.2625 0.1125 0.3927 +0.3063 0.1313 0.3927 +0.3500 0.1500 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 0.3927 +-0.0333 -0.0167 0.3927 +-0.0667 -0.0333 0.3927 +-0.1000 -0.0500 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 5 3 2 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 0.3927 +0.0453 0.0188 0.3927 +0.0903 0.0380 0.4377 +0.1337 0.0607 0.5246 +0.1750 0.0871 0.6115 +0.2139 0.1169 0.6985 +0.2500 0.1500 0.7854 +primID: 4 +startangle_c: 1 +endpose_c: 5 1 0 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 0.3927 +0.0400 0.0151 0.3290 +0.0809 0.0276 0.2632 +0.1225 0.0374 0.1974 +0.1647 0.0444 0.1316 +0.2073 0.0486 0.0658 +0.2500 0.0500 0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4909 +0.0000 0.0000 0.5890 +0.0000 0.0000 0.6872 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.2945 +0.0000 0.0000 0.1963 +0.0000 0.0000 0.0982 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 0.7854 +0.0250 0.0250 0.7854 +0.0500 0.0500 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 6 6 2 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 0.7854 +-0.0250 -0.0250 0.7854 +-0.0500 -0.0500 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 3 5 3 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 0.7854 +0.0250 0.0266 0.8345 +0.0484 0.0547 0.8836 +0.0700 0.0842 0.9327 +0.0898 0.1151 0.9817 +0.1078 0.1472 1.0308 +0.1239 0.1804 1.0799 +0.1379 0.2147 1.1290 +0.1500 0.2500 1.1781 +primID: 4 +startangle_c: 2 +endpose_c: 5 3 1 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 0.7854 +0.0266 0.0250 0.7363 +0.0547 0.0484 0.6872 +0.0842 0.0700 0.6381 +0.1151 0.0898 0.5890 +0.1472 0.1078 0.5400 +0.1804 0.1239 0.4909 +0.2147 0.1379 0.4418 +0.2500 0.1500 0.3927 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8836 +0.0000 0.0000 0.9817 +0.0000 0.0000 1.0799 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.6872 +0.0000 0.0000 0.5890 +0.0000 0.0000 0.4909 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 1.1781 +0.0167 0.0333 1.1781 +0.0333 0.0667 1.1781 +0.0500 0.1000 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 3 7 3 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 1.1781 +0.0188 0.0438 1.1781 +0.0375 0.0875 1.1781 +0.0563 0.1313 1.1781 +0.0750 0.1750 1.1781 +0.0938 0.2188 1.1781 +0.1125 0.2625 1.1781 +0.1313 0.3063 1.1781 +0.1500 0.3500 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 1.1781 +-0.0167 -0.0333 1.1781 +-0.0333 -0.0667 1.1781 +-0.0500 -0.1000 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 1 5 4 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.1781 +0.0151 0.0400 1.2418 +0.0276 0.0809 1.3076 +0.0374 0.1225 1.3734 +0.0444 0.1647 1.4392 +0.0486 0.2073 1.5050 +0.0500 0.2500 1.5708 +primID: 4 +startangle_c: 3 +endpose_c: 3 5 2 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.1781 +0.0188 0.0453 1.1781 +0.0380 0.0903 1.1331 +0.0607 0.1337 1.0462 +0.0871 0.1750 0.9593 +0.1169 0.2139 0.8723 +0.1500 0.2500 0.7854 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2763 +0.0000 0.0000 1.3744 +0.0000 0.0000 1.4726 +0.0000 0.0000 1.5708 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.0799 +0.0000 0.0000 0.9817 +0.0000 0.0000 0.8836 +0.0000 0.0000 0.7854 +primID: 0 +startangle_c: 4 +endpose_c: 0 2 4 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 1.5708 +0.0000 0.0500 1.5708 +0.0000 0.1000 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 8 4 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 1.5708 +0.0000 0.0500 1.5708 +0.0000 0.1000 1.5708 +0.0000 0.1500 1.5708 +0.0000 0.2000 1.5708 +0.0000 0.2500 1.5708 +0.0000 0.3000 1.5708 +0.0000 0.3500 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -2 4 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 1.5708 +0.0000 -0.0500 1.5708 +0.0000 -0.1000 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -1 5 5 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.5708 +-0.0014 0.0427 1.6362 +-0.0056 0.0853 1.7017 +-0.0126 0.1275 1.7671 +-0.0224 0.1691 1.8326 +-0.0349 0.2100 1.8980 +-0.0500 0.2500 1.9635 +primID: 4 +startangle_c: 4 +endpose_c: 1 5 3 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.5708 +0.0014 0.0427 1.5053 +0.0056 0.0853 1.4399 +0.0126 0.1275 1.3744 +0.0224 0.1691 1.3090 +0.0349 0.2100 1.2435 +0.0500 0.2500 1.1781 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6690 +0.0000 0.0000 1.7671 +0.0000 0.0000 1.8653 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.4726 +0.0000 0.0000 1.3744 +0.0000 0.0000 1.2763 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 1.9635 +-0.0167 0.0333 1.9635 +-0.0333 0.0667 1.9635 +-0.0500 0.1000 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -3 7 5 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 1.9635 +-0.0188 0.0438 1.9635 +-0.0375 0.0875 1.9635 +-0.0563 0.1313 1.9635 +-0.0750 0.1750 1.9635 +-0.0938 0.2188 1.9635 +-0.1125 0.2625 1.9635 +-0.1313 0.3063 1.9635 +-0.1500 0.3500 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 1.9635 +0.0167 -0.0333 1.9635 +0.0333 -0.0667 1.9635 +0.0500 -0.1000 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -3 5 6 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.9635 +-0.0188 0.0453 1.9635 +-0.0380 0.0903 2.0085 +-0.0607 0.1337 2.0954 +-0.0871 0.1750 2.1823 +-0.1169 0.2139 2.2693 +-0.1500 0.2500 2.3562 +primID: 4 +startangle_c: 5 +endpose_c: -1 5 4 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.9635 +-0.0151 0.0400 1.8998 +-0.0276 0.0809 1.8340 +-0.0374 0.1225 1.7682 +-0.0444 0.1647 1.7024 +-0.0486 0.2073 1.6366 +-0.0500 0.2500 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0617 +0.0000 0.0000 2.1598 +0.0000 0.0000 2.2580 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.8653 +0.0000 0.0000 1.7671 +0.0000 0.0000 1.6690 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 2.3562 +-0.0250 0.0250 2.3562 +-0.0500 0.0500 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -6 6 6 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 2.3562 +0.0250 -0.0250 2.3562 +0.0500 -0.0500 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -5 3 7 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 2.3562 +-0.0266 0.0250 2.4053 +-0.0547 0.0484 2.4544 +-0.0842 0.0700 2.5035 +-0.1151 0.0898 2.5525 +-0.1472 0.1078 2.6016 +-0.1804 0.1239 2.6507 +-0.2147 0.1379 2.6998 +-0.2500 0.1500 2.7489 +primID: 4 +startangle_c: 6 +endpose_c: -3 5 5 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 2.3562 +-0.0250 0.0266 2.3071 +-0.0484 0.0547 2.2580 +-0.0700 0.0842 2.2089 +-0.0898 0.1151 2.1598 +-0.1078 0.1472 2.1108 +-0.1239 0.1804 2.0617 +-0.1379 0.2147 2.0126 +-0.1500 0.2500 1.9635 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.4544 +0.0000 0.0000 2.5525 +0.0000 0.0000 2.6507 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.2580 +0.0000 0.0000 2.1598 +0.0000 0.0000 2.0617 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 2.7489 +-0.0333 0.0167 2.7489 +-0.0667 0.0333 2.7489 +-0.1000 0.0500 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -7 3 7 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 2.7489 +-0.0438 0.0188 2.7489 +-0.0875 0.0375 2.7489 +-0.1313 0.0563 2.7489 +-0.1750 0.0750 2.7489 +-0.2188 0.0938 2.7489 +-0.2625 0.1125 2.7489 +-0.3063 0.1313 2.7489 +-0.3500 0.1500 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 2.7489 +0.0333 -0.0167 2.7489 +0.0667 -0.0333 2.7489 +0.1000 -0.0500 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -5 1 8 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 2.7489 +-0.0400 0.0151 2.8126 +-0.0809 0.0276 2.8784 +-0.1225 0.0374 2.9442 +-0.1647 0.0444 3.0100 +-0.2073 0.0486 3.0758 +-0.2500 0.0500 3.1416 +primID: 4 +startangle_c: 7 +endpose_c: -5 3 6 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 2.7489 +-0.0453 0.0188 2.7489 +-0.0903 0.0380 2.7039 +-0.1337 0.0607 2.6170 +-0.1750 0.0871 2.5300 +-0.2139 0.1169 2.4431 +-0.2500 0.1500 2.3562 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.8471 +0.0000 0.0000 2.9452 +0.0000 0.0000 3.0434 +0.0000 0.0000 3.1416 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.6507 +0.0000 0.0000 2.5525 +0.0000 0.0000 2.4544 +0.0000 0.0000 2.3562 +primID: 0 +startangle_c: 8 +endpose_c: -2 0 8 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 3.1416 +-0.0500 0.0000 3.1416 +-0.1000 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -8 0 8 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 3.1416 +-0.0500 0.0000 3.1416 +-0.1000 0.0000 3.1416 +-0.1500 0.0000 3.1416 +-0.2000 0.0000 3.1416 +-0.2500 0.0000 3.1416 +-0.3000 0.0000 3.1416 +-0.3500 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 2 0 8 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 3.1416 +0.0500 0.0000 3.1416 +0.1000 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -5 -1 9 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 3.1416 +-0.0427 -0.0014 3.2070 +-0.0853 -0.0056 3.2725 +-0.1275 -0.0126 3.3379 +-0.1691 -0.0224 3.4034 +-0.2100 -0.0349 3.4688 +-0.2500 -0.0500 3.5343 +primID: 4 +startangle_c: 8 +endpose_c: -5 1 7 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 3.1416 +-0.0427 0.0014 3.0761 +-0.0853 0.0056 3.0107 +-0.1275 0.0126 2.9452 +-0.1691 0.0224 2.8798 +-0.2100 0.0349 2.8143 +-0.2500 0.0500 2.7489 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.2398 +0.0000 0.0000 3.3379 +0.0000 0.0000 3.4361 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0434 +0.0000 0.0000 2.9452 +0.0000 0.0000 2.8471 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 3.5343 +-0.0333 -0.0167 3.5343 +-0.0667 -0.0333 3.5343 +-0.1000 -0.0500 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -7 -3 9 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 3.5343 +-0.0438 -0.0188 3.5343 +-0.0875 -0.0375 3.5343 +-0.1313 -0.0563 3.5343 +-0.1750 -0.0750 3.5343 +-0.2188 -0.0938 3.5343 +-0.2625 -0.1125 3.5343 +-0.3063 -0.1313 3.5343 +-0.3500 -0.1500 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 3.5343 +0.0333 0.0167 3.5343 +0.0667 0.0333 3.5343 +0.1000 0.0500 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -5 -3 10 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 3.5343 +-0.0453 -0.0188 3.5343 +-0.0903 -0.0380 3.5793 +-0.1337 -0.0607 3.6662 +-0.1750 -0.0871 3.7531 +-0.2139 -0.1169 3.8401 +-0.2500 -0.1500 3.9270 +primID: 4 +startangle_c: 9 +endpose_c: -5 -1 8 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 3.5343 +-0.0400 -0.0151 3.4706 +-0.0809 -0.0276 3.4048 +-0.1225 -0.0374 3.3390 +-0.1647 -0.0444 3.2732 +-0.2073 -0.0486 3.2074 +-0.2500 -0.0500 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.6325 +0.0000 0.0000 3.7306 +0.0000 0.0000 3.8288 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4361 +0.0000 0.0000 3.3379 +0.0000 0.0000 3.2398 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 3.9270 +-0.0250 -0.0250 3.9270 +-0.0500 -0.0500 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -6 -6 10 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 3.9270 +0.0250 0.0250 3.9270 +0.0500 0.0500 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -3 -5 11 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 3.9270 +-0.0250 -0.0266 3.9761 +-0.0484 -0.0547 4.0252 +-0.0700 -0.0842 4.0743 +-0.0898 -0.1151 4.1233 +-0.1078 -0.1472 4.1724 +-0.1239 -0.1804 4.2215 +-0.1379 -0.2147 4.2706 +-0.1500 -0.2500 4.3197 +primID: 4 +startangle_c: 10 +endpose_c: -5 -3 9 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 3.9270 +-0.0266 -0.0250 3.8779 +-0.0547 -0.0484 3.8288 +-0.0842 -0.0700 3.7797 +-0.1151 -0.0898 3.7306 +-0.1472 -0.1078 3.6816 +-0.1804 -0.1239 3.6325 +-0.2147 -0.1379 3.5834 +-0.2500 -0.1500 3.5343 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.9270 +0.0000 0.0000 4.0252 +0.0000 0.0000 4.1233 +0.0000 0.0000 4.2215 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8288 +0.0000 0.0000 3.7306 +0.0000 0.0000 3.6325 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 4.3197 +-0.0167 -0.0333 4.3197 +-0.0333 -0.0667 4.3197 +-0.0500 -0.1000 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -3 -7 11 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 4.3197 +-0.0188 -0.0438 4.3197 +-0.0375 -0.0875 4.3197 +-0.0563 -0.1313 4.3197 +-0.0750 -0.1750 4.3197 +-0.0938 -0.2188 4.3197 +-0.1125 -0.2625 4.3197 +-0.1313 -0.3063 4.3197 +-0.1500 -0.3500 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 4.3197 +0.0167 0.0333 4.3197 +0.0333 0.0667 4.3197 +0.0500 0.1000 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -1 -5 12 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 4.3197 +-0.0151 -0.0400 4.3834 +-0.0276 -0.0809 4.4492 +-0.0374 -0.1225 4.5150 +-0.0444 -0.1647 4.5808 +-0.0486 -0.2073 4.6466 +-0.0500 -0.2500 4.7124 +primID: 4 +startangle_c: 11 +endpose_c: -3 -5 10 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 4.3197 +-0.0188 -0.0453 4.3197 +-0.0380 -0.0903 4.2747 +-0.0607 -0.1337 4.1878 +-0.0871 -0.1750 4.1008 +-0.1169 -0.2139 4.0139 +-0.1500 -0.2500 3.9270 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.4179 +0.0000 0.0000 4.5160 +0.0000 0.0000 4.6142 +0.0000 0.0000 4.7124 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2215 +0.0000 0.0000 4.1233 +0.0000 0.0000 4.0252 +0.0000 0.0000 3.9270 +primID: 0 +startangle_c: 12 +endpose_c: 0 -2 12 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 4.7124 +0.0000 -0.0500 4.7124 +0.0000 -0.1000 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -8 12 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 4.7124 +0.0000 -0.0500 4.7124 +0.0000 -0.1000 4.7124 +0.0000 -0.1500 4.7124 +0.0000 -0.2000 4.7124 +0.0000 -0.2500 4.7124 +0.0000 -0.3000 4.7124 +0.0000 -0.3500 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 2 12 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 4.7124 +0.0000 0.0500 4.7124 +0.0000 0.1000 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 1 -5 13 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 4.7124 +0.0014 -0.0427 4.7778 +0.0056 -0.0853 4.8433 +0.0126 -0.1275 4.9087 +0.0224 -0.1691 4.9742 +0.0349 -0.2100 5.0396 +0.0500 -0.2500 5.1051 +primID: 4 +startangle_c: 12 +endpose_c: -1 -5 11 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 4.7124 +-0.0014 -0.0427 4.6469 +-0.0056 -0.0853 4.5815 +-0.0126 -0.1275 4.5160 +-0.0224 -0.1691 4.4506 +-0.0349 -0.2100 4.3851 +-0.0500 -0.2500 4.3197 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.8106 +0.0000 0.0000 4.9087 +0.0000 0.0000 5.0069 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6142 +0.0000 0.0000 4.5160 +0.0000 0.0000 4.4179 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 5.1051 +0.0167 -0.0333 5.1051 +0.0333 -0.0667 5.1051 +0.0500 -0.1000 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 3 -7 13 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 5.1051 +0.0188 -0.0438 5.1051 +0.0375 -0.0875 5.1051 +0.0563 -0.1313 5.1051 +0.0750 -0.1750 5.1051 +0.0938 -0.2188 5.1051 +0.1125 -0.2625 5.1051 +0.1313 -0.3063 5.1051 +0.1500 -0.3500 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 5.1051 +-0.0167 0.0333 5.1051 +-0.0333 0.0667 5.1051 +-0.0500 0.1000 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 3 -5 14 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 5.1051 +0.0188 -0.0453 5.1051 +0.0380 -0.0903 5.1501 +0.0607 -0.1337 5.2370 +0.0871 -0.1750 5.3239 +0.1169 -0.2139 5.4109 +0.1500 -0.2500 5.4978 +primID: 4 +startangle_c: 13 +endpose_c: 1 -5 12 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 5.1051 +0.0151 -0.0400 5.0414 +0.0276 -0.0809 4.9756 +0.0374 -0.1225 4.9098 +0.0444 -0.1647 4.8440 +0.0486 -0.2073 4.7782 +0.0500 -0.2500 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.2033 +0.0000 0.0000 5.3014 +0.0000 0.0000 5.3996 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0069 +0.0000 0.0000 4.9087 +0.0000 0.0000 4.8106 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 5.4978 +0.0250 -0.0250 5.4978 +0.0500 -0.0500 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 6 -6 14 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 5.4978 +-0.0250 0.0250 5.4978 +-0.0500 0.0500 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 5 -3 15 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 5.4978 +0.0266 -0.0250 5.5469 +0.0547 -0.0484 5.5960 +0.0842 -0.0700 5.6450 +0.1151 -0.0898 5.6941 +0.1472 -0.1078 5.7432 +0.1804 -0.1239 5.7923 +0.2147 -0.1379 5.8414 +0.2500 -0.1500 5.8905 +primID: 4 +startangle_c: 14 +endpose_c: 3 -5 13 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 5.4978 +0.0250 -0.0266 5.4487 +0.0484 -0.0547 5.3996 +0.0700 -0.0842 5.3505 +0.0898 -0.1151 5.3014 +0.1078 -0.1472 5.2524 +0.1239 -0.1804 5.2033 +0.1379 -0.2147 5.1542 +0.1500 -0.2500 5.1051 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5960 +0.0000 0.0000 5.6941 +0.0000 0.0000 5.7923 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.3996 +0.0000 0.0000 5.3014 +0.0000 0.0000 5.2033 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 5.8905 +0.0333 -0.0167 5.8905 +0.0667 -0.0333 5.8905 +0.1000 -0.0500 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 7 -3 15 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 5.8905 +0.0438 -0.0188 5.8905 +0.0875 -0.0375 5.8905 +0.1313 -0.0563 5.8905 +0.1750 -0.0750 5.8905 +0.2188 -0.0938 5.8905 +0.2625 -0.1125 5.8905 +0.3063 -0.1313 5.8905 +0.3500 -0.1500 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 5.8905 +-0.0333 0.0167 5.8905 +-0.0667 0.0333 5.8905 +-0.1000 0.0500 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 5 -1 0 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 5.8905 +0.0400 -0.0151 5.9542 +0.0809 -0.0276 6.0200 +0.1225 -0.0374 6.0858 +0.1647 -0.0444 6.1516 +0.2073 -0.0486 6.2174 +0.2500 -0.0500 6.2832 +primID: 4 +startangle_c: 15 +endpose_c: 5 -3 14 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 5.8905 +0.0453 -0.0188 5.8905 +0.0903 -0.0380 5.8455 +0.1337 -0.0607 5.7586 +0.1750 -0.0871 5.6716 +0.2139 -0.1169 5.5847 +0.2500 -0.1500 5.4978 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.9887 +0.0000 0.0000 6.0868 +0.0000 0.0000 6.1850 +0.0000 0.0000 6.2832 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.7923 +0.0000 0.0000 5.6941 +0.0000 0.0000 5.5960 +0.0000 0.0000 5.4978 diff --git a/diff_5cm.mprim b/diff_5cm.mprim new file mode 100644 index 0000000..31bc3b1 --- /dev/null +++ b/diff_5cm.mprim @@ -0,0 +1,1223 @@ +resolution_m: 0.050000 +numberofangles: 16 +totalnumberofprimitives: 112 +primID: 0 +startangle_c: 0 +endpose_c: 2 0 0 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 0.0000 +0.0500 0.0000 0.0000 +0.1000 0.0000 0.0000 +primID: 1 +startangle_c: 0 +endpose_c: 8 0 0 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 0.0000 +0.0500 0.0000 0.0000 +0.1000 0.0000 0.0000 +0.1500 0.0000 0.0000 +0.2000 0.0000 0.0000 +0.2500 0.0000 0.0000 +0.3000 0.0000 0.0000 +0.3500 0.0000 0.0000 +0.4000 0.0000 0.0000 +primID: 2 +startangle_c: 0 +endpose_c: -2 0 0 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 0.0000 +-0.0500 0.0000 0.0000 +-0.1000 0.0000 0.0000 +primID: 3 +startangle_c: 0 +endpose_c: 5 1 1 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 0.0000 +0.0427 0.0014 0.0654 +0.0853 0.0056 0.1309 +0.1275 0.0126 0.1963 +0.1691 0.0224 0.2618 +0.2100 0.0349 0.3272 +0.2500 0.0500 0.3927 +primID: 4 +startangle_c: 0 +endpose_c: 5 -1 15 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 0.0000 +0.0427 -0.0014 -0.0654 +0.0853 -0.0056 -0.1309 +0.1275 -0.0126 -0.1963 +0.1691 -0.0224 -0.2618 +0.2100 -0.0349 -0.3272 +0.2500 -0.0500 -0.3927 +primID: 5 +startangle_c: 0 +endpose_c: 0 0 1 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.0000 +0.0000 0.0000 0.0982 +0.0000 0.0000 0.1963 +0.0000 0.0000 0.2945 +0.0000 0.0000 0.3927 +primID: 6 +startangle_c: 0 +endpose_c: 0 0 15 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.0000 +0.0000 0.0000 -0.0982 +0.0000 0.0000 -0.1963 +0.0000 0.0000 -0.2945 +0.0000 0.0000 -0.3927 +primID: 0 +startangle_c: 1 +endpose_c: 2 1 1 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 0.3927 +0.0333 0.0167 0.3927 +0.0667 0.0333 0.3927 +0.1000 0.0500 0.3927 +primID: 1 +startangle_c: 1 +endpose_c: 7 3 1 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 0.3927 +0.0438 0.0188 0.3927 +0.0875 0.0375 0.3927 +0.1313 0.0563 0.3927 +0.1750 0.0750 0.3927 +0.2188 0.0938 0.3927 +0.2625 0.1125 0.3927 +0.3063 0.1313 0.3927 +0.3500 0.1500 0.3927 +primID: 2 +startangle_c: 1 +endpose_c: -2 -1 1 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 0.3927 +-0.0333 -0.0167 0.3927 +-0.0667 -0.0333 0.3927 +-0.1000 -0.0500 0.3927 +primID: 3 +startangle_c: 1 +endpose_c: 5 3 2 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 0.3927 +0.0453 0.0188 0.3927 +0.0903 0.0380 0.4377 +0.1337 0.0607 0.5246 +0.1750 0.0871 0.6115 +0.2139 0.1169 0.6985 +0.2500 0.1500 0.7854 +primID: 4 +startangle_c: 1 +endpose_c: 5 1 0 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 0.3927 +0.0400 0.0151 0.3290 +0.0809 0.0276 0.2632 +0.1225 0.0374 0.1974 +0.1647 0.0444 0.1316 +0.2073 0.0486 0.0658 +0.2500 0.0500 0.0000 +primID: 5 +startangle_c: 1 +endpose_c: 0 0 2 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.4909 +0.0000 0.0000 0.5890 +0.0000 0.0000 0.6872 +0.0000 0.0000 0.7854 +primID: 6 +startangle_c: 1 +endpose_c: 0 0 0 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.3927 +0.0000 0.0000 0.2945 +0.0000 0.0000 0.1963 +0.0000 0.0000 0.0982 +0.0000 0.0000 0.0000 +primID: 0 +startangle_c: 2 +endpose_c: 1 1 2 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 0.7854 +0.0250 0.0250 0.7854 +0.0500 0.0500 0.7854 +primID: 1 +startangle_c: 2 +endpose_c: 6 6 2 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 0.7854 +0.0333 0.0333 0.7854 +0.0667 0.0667 0.7854 +0.1000 0.1000 0.7854 +0.1333 0.1333 0.7854 +0.1667 0.1667 0.7854 +0.2000 0.2000 0.7854 +0.2333 0.2333 0.7854 +0.2667 0.2667 0.7854 +0.3000 0.3000 0.7854 +primID: 2 +startangle_c: 2 +endpose_c: -1 -1 2 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 0.7854 +-0.0250 -0.0250 0.7854 +-0.0500 -0.0500 0.7854 +primID: 3 +startangle_c: 2 +endpose_c: 3 5 3 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 0.7854 +0.0250 0.0266 0.8345 +0.0484 0.0547 0.8836 +0.0700 0.0842 0.9327 +0.0898 0.1151 0.9817 +0.1078 0.1472 1.0308 +0.1239 0.1804 1.0799 +0.1379 0.2147 1.1290 +0.1500 0.2500 1.1781 +primID: 4 +startangle_c: 2 +endpose_c: 5 3 1 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 0.7854 +0.0266 0.0250 0.7363 +0.0547 0.0484 0.6872 +0.0842 0.0700 0.6381 +0.1151 0.0898 0.5890 +0.1472 0.1078 0.5400 +0.1804 0.1239 0.4909 +0.2147 0.1379 0.4418 +0.2500 0.1500 0.3927 +primID: 5 +startangle_c: 2 +endpose_c: 0 0 3 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.8836 +0.0000 0.0000 0.9817 +0.0000 0.0000 1.0799 +0.0000 0.0000 1.1781 +primID: 6 +startangle_c: 2 +endpose_c: 0 0 1 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 0.7854 +0.0000 0.0000 0.6872 +0.0000 0.0000 0.5890 +0.0000 0.0000 0.4909 +0.0000 0.0000 0.3927 +primID: 0 +startangle_c: 3 +endpose_c: 1 2 3 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 1.1781 +0.0167 0.0333 1.1781 +0.0333 0.0667 1.1781 +0.0500 0.1000 1.1781 +primID: 1 +startangle_c: 3 +endpose_c: 3 7 3 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 1.1781 +0.0188 0.0438 1.1781 +0.0375 0.0875 1.1781 +0.0563 0.1313 1.1781 +0.0750 0.1750 1.1781 +0.0938 0.2188 1.1781 +0.1125 0.2625 1.1781 +0.1313 0.3063 1.1781 +0.1500 0.3500 1.1781 +primID: 2 +startangle_c: 3 +endpose_c: -1 -2 3 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 1.1781 +-0.0167 -0.0333 1.1781 +-0.0333 -0.0667 1.1781 +-0.0500 -0.1000 1.1781 +primID: 3 +startangle_c: 3 +endpose_c: 1 5 4 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.1781 +0.0151 0.0400 1.2418 +0.0276 0.0809 1.3076 +0.0374 0.1225 1.3734 +0.0444 0.1647 1.4392 +0.0486 0.2073 1.5050 +0.0500 0.2500 1.5708 +primID: 4 +startangle_c: 3 +endpose_c: 3 5 2 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.1781 +0.0188 0.0453 1.1781 +0.0380 0.0903 1.1331 +0.0607 0.1337 1.0462 +0.0871 0.1750 0.9593 +0.1169 0.2139 0.8723 +0.1500 0.2500 0.7854 +primID: 5 +startangle_c: 3 +endpose_c: 0 0 4 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.2763 +0.0000 0.0000 1.3744 +0.0000 0.0000 1.4726 +0.0000 0.0000 1.5708 +primID: 6 +startangle_c: 3 +endpose_c: 0 0 2 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.1781 +0.0000 0.0000 1.0799 +0.0000 0.0000 0.9817 +0.0000 0.0000 0.8836 +0.0000 0.0000 0.7854 +primID: 0 +startangle_c: 4 +endpose_c: 0 2 4 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 1.5708 +0.0000 0.0500 1.5708 +0.0000 0.1000 1.5708 +primID: 1 +startangle_c: 4 +endpose_c: 0 8 4 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 1.5708 +0.0000 0.0500 1.5708 +0.0000 0.1000 1.5708 +0.0000 0.1500 1.5708 +0.0000 0.2000 1.5708 +0.0000 0.2500 1.5708 +0.0000 0.3000 1.5708 +0.0000 0.3500 1.5708 +0.0000 0.4000 1.5708 +primID: 2 +startangle_c: 4 +endpose_c: 0 -2 4 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 1.5708 +0.0000 -0.0500 1.5708 +0.0000 -0.1000 1.5708 +primID: 3 +startangle_c: 4 +endpose_c: -1 5 5 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.5708 +-0.0014 0.0427 1.6362 +-0.0056 0.0853 1.7017 +-0.0126 0.1275 1.7671 +-0.0224 0.1691 1.8326 +-0.0349 0.2100 1.8980 +-0.0500 0.2500 1.9635 +primID: 4 +startangle_c: 4 +endpose_c: 1 5 3 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.5708 +0.0014 0.0427 1.5053 +0.0056 0.0853 1.4399 +0.0126 0.1275 1.3744 +0.0224 0.1691 1.3090 +0.0349 0.2100 1.2435 +0.0500 0.2500 1.1781 +primID: 5 +startangle_c: 4 +endpose_c: 0 0 5 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.6690 +0.0000 0.0000 1.7671 +0.0000 0.0000 1.8653 +0.0000 0.0000 1.9635 +primID: 6 +startangle_c: 4 +endpose_c: 0 0 3 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.5708 +0.0000 0.0000 1.4726 +0.0000 0.0000 1.3744 +0.0000 0.0000 1.2763 +0.0000 0.0000 1.1781 +primID: 0 +startangle_c: 5 +endpose_c: -1 2 5 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 1.9635 +-0.0167 0.0333 1.9635 +-0.0333 0.0667 1.9635 +-0.0500 0.1000 1.9635 +primID: 1 +startangle_c: 5 +endpose_c: -3 7 5 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 1.9635 +-0.0188 0.0438 1.9635 +-0.0375 0.0875 1.9635 +-0.0563 0.1313 1.9635 +-0.0750 0.1750 1.9635 +-0.0938 0.2188 1.9635 +-0.1125 0.2625 1.9635 +-0.1313 0.3063 1.9635 +-0.1500 0.3500 1.9635 +primID: 2 +startangle_c: 5 +endpose_c: 1 -2 5 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 1.9635 +0.0167 -0.0333 1.9635 +0.0333 -0.0667 1.9635 +0.0500 -0.1000 1.9635 +primID: 3 +startangle_c: 5 +endpose_c: -3 5 6 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.9635 +-0.0188 0.0453 1.9635 +-0.0380 0.0903 2.0085 +-0.0607 0.1337 2.0954 +-0.0871 0.1750 2.1823 +-0.1169 0.2139 2.2693 +-0.1500 0.2500 2.3562 +primID: 4 +startangle_c: 5 +endpose_c: -1 5 4 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 1.9635 +-0.0151 0.0400 1.8998 +-0.0276 0.0809 1.8340 +-0.0374 0.1225 1.7682 +-0.0444 0.1647 1.7024 +-0.0486 0.2073 1.6366 +-0.0500 0.2500 1.5708 +primID: 5 +startangle_c: 5 +endpose_c: 0 0 6 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.9635 +0.0000 0.0000 2.0617 +0.0000 0.0000 2.1598 +0.0000 0.0000 2.2580 +0.0000 0.0000 2.3562 +primID: 6 +startangle_c: 5 +endpose_c: 0 0 4 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 1.9635 +0.0000 0.0000 1.8653 +0.0000 0.0000 1.7671 +0.0000 0.0000 1.6690 +0.0000 0.0000 1.5708 +primID: 0 +startangle_c: 6 +endpose_c: -1 1 6 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 2.3562 +-0.0250 0.0250 2.3562 +-0.0500 0.0500 2.3562 +primID: 1 +startangle_c: 6 +endpose_c: -6 6 6 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 2.3562 +-0.0333 0.0333 2.3562 +-0.0667 0.0667 2.3562 +-0.1000 0.1000 2.3562 +-0.1333 0.1333 2.3562 +-0.1667 0.1667 2.3562 +-0.2000 0.2000 2.3562 +-0.2333 0.2333 2.3562 +-0.2667 0.2667 2.3562 +-0.3000 0.3000 2.3562 +primID: 2 +startangle_c: 6 +endpose_c: 1 -1 6 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 2.3562 +0.0250 -0.0250 2.3562 +0.0500 -0.0500 2.3562 +primID: 3 +startangle_c: 6 +endpose_c: -5 3 7 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 2.3562 +-0.0266 0.0250 2.4053 +-0.0547 0.0484 2.4544 +-0.0842 0.0700 2.5035 +-0.1151 0.0898 2.5525 +-0.1472 0.1078 2.6016 +-0.1804 0.1239 2.6507 +-0.2147 0.1379 2.6998 +-0.2500 0.1500 2.7489 +primID: 4 +startangle_c: 6 +endpose_c: -3 5 5 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 2.3562 +-0.0250 0.0266 2.3071 +-0.0484 0.0547 2.2580 +-0.0700 0.0842 2.2089 +-0.0898 0.1151 2.1598 +-0.1078 0.1472 2.1108 +-0.1239 0.1804 2.0617 +-0.1379 0.2147 2.0126 +-0.1500 0.2500 1.9635 +primID: 5 +startangle_c: 6 +endpose_c: 0 0 7 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.4544 +0.0000 0.0000 2.5525 +0.0000 0.0000 2.6507 +0.0000 0.0000 2.7489 +primID: 6 +startangle_c: 6 +endpose_c: 0 0 5 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 2.3562 +0.0000 0.0000 2.2580 +0.0000 0.0000 2.1598 +0.0000 0.0000 2.0617 +0.0000 0.0000 1.9635 +primID: 0 +startangle_c: 7 +endpose_c: -2 1 7 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 2.7489 +-0.0333 0.0167 2.7489 +-0.0667 0.0333 2.7489 +-0.1000 0.0500 2.7489 +primID: 1 +startangle_c: 7 +endpose_c: -7 3 7 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 2.7489 +-0.0438 0.0188 2.7489 +-0.0875 0.0375 2.7489 +-0.1313 0.0563 2.7489 +-0.1750 0.0750 2.7489 +-0.2188 0.0938 2.7489 +-0.2625 0.1125 2.7489 +-0.3063 0.1313 2.7489 +-0.3500 0.1500 2.7489 +primID: 2 +startangle_c: 7 +endpose_c: 2 -1 7 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 2.7489 +0.0333 -0.0167 2.7489 +0.0667 -0.0333 2.7489 +0.1000 -0.0500 2.7489 +primID: 3 +startangle_c: 7 +endpose_c: -5 1 8 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 2.7489 +-0.0400 0.0151 2.8126 +-0.0809 0.0276 2.8784 +-0.1225 0.0374 2.9442 +-0.1647 0.0444 3.0100 +-0.2073 0.0486 3.0758 +-0.2500 0.0500 3.1416 +primID: 4 +startangle_c: 7 +endpose_c: -5 3 6 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 2.7489 +-0.0453 0.0188 2.7489 +-0.0903 0.0380 2.7039 +-0.1337 0.0607 2.6170 +-0.1750 0.0871 2.5300 +-0.2139 0.1169 2.4431 +-0.2500 0.1500 2.3562 +primID: 5 +startangle_c: 7 +endpose_c: 0 0 8 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.8471 +0.0000 0.0000 2.9452 +0.0000 0.0000 3.0434 +0.0000 0.0000 3.1416 +primID: 6 +startangle_c: 7 +endpose_c: 0 0 6 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 2.7489 +0.0000 0.0000 2.6507 +0.0000 0.0000 2.5525 +0.0000 0.0000 2.4544 +0.0000 0.0000 2.3562 +primID: 0 +startangle_c: 8 +endpose_c: -2 0 8 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 3.1416 +-0.0500 0.0000 3.1416 +-0.1000 0.0000 3.1416 +primID: 1 +startangle_c: 8 +endpose_c: -8 0 8 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 3.1416 +-0.0500 0.0000 3.1416 +-0.1000 0.0000 3.1416 +-0.1500 0.0000 3.1416 +-0.2000 0.0000 3.1416 +-0.2500 0.0000 3.1416 +-0.3000 0.0000 3.1416 +-0.3500 0.0000 3.1416 +-0.4000 0.0000 3.1416 +primID: 2 +startangle_c: 8 +endpose_c: 2 0 8 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 3.1416 +0.0500 0.0000 3.1416 +0.1000 0.0000 3.1416 +primID: 3 +startangle_c: 8 +endpose_c: -5 -1 9 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 3.1416 +-0.0427 -0.0014 3.2070 +-0.0853 -0.0056 3.2725 +-0.1275 -0.0126 3.3379 +-0.1691 -0.0224 3.4034 +-0.2100 -0.0349 3.4688 +-0.2500 -0.0500 3.5343 +primID: 4 +startangle_c: 8 +endpose_c: -5 1 7 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 3.1416 +-0.0427 0.0014 3.0761 +-0.0853 0.0056 3.0107 +-0.1275 0.0126 2.9452 +-0.1691 0.0224 2.8798 +-0.2100 0.0349 2.8143 +-0.2500 0.0500 2.7489 +primID: 5 +startangle_c: 8 +endpose_c: 0 0 9 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.2398 +0.0000 0.0000 3.3379 +0.0000 0.0000 3.4361 +0.0000 0.0000 3.5343 +primID: 6 +startangle_c: 8 +endpose_c: 0 0 7 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.1416 +0.0000 0.0000 3.0434 +0.0000 0.0000 2.9452 +0.0000 0.0000 2.8471 +0.0000 0.0000 2.7489 +primID: 0 +startangle_c: 9 +endpose_c: -2 -1 9 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 3.5343 +-0.0333 -0.0167 3.5343 +-0.0667 -0.0333 3.5343 +-0.1000 -0.0500 3.5343 +primID: 1 +startangle_c: 9 +endpose_c: -7 -3 9 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 3.5343 +-0.0438 -0.0188 3.5343 +-0.0875 -0.0375 3.5343 +-0.1313 -0.0563 3.5343 +-0.1750 -0.0750 3.5343 +-0.2188 -0.0938 3.5343 +-0.2625 -0.1125 3.5343 +-0.3063 -0.1313 3.5343 +-0.3500 -0.1500 3.5343 +primID: 2 +startangle_c: 9 +endpose_c: 2 1 9 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 3.5343 +0.0333 0.0167 3.5343 +0.0667 0.0333 3.5343 +0.1000 0.0500 3.5343 +primID: 3 +startangle_c: 9 +endpose_c: -5 -3 10 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 3.5343 +-0.0453 -0.0188 3.5343 +-0.0903 -0.0380 3.5793 +-0.1337 -0.0607 3.6662 +-0.1750 -0.0871 3.7531 +-0.2139 -0.1169 3.8401 +-0.2500 -0.1500 3.9270 +primID: 4 +startangle_c: 9 +endpose_c: -5 -1 8 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 3.5343 +-0.0400 -0.0151 3.4706 +-0.0809 -0.0276 3.4048 +-0.1225 -0.0374 3.3390 +-0.1647 -0.0444 3.2732 +-0.2073 -0.0486 3.2074 +-0.2500 -0.0500 3.1416 +primID: 5 +startangle_c: 9 +endpose_c: 0 0 10 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.6325 +0.0000 0.0000 3.7306 +0.0000 0.0000 3.8288 +0.0000 0.0000 3.9270 +primID: 6 +startangle_c: 9 +endpose_c: 0 0 8 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.5343 +0.0000 0.0000 3.4361 +0.0000 0.0000 3.3379 +0.0000 0.0000 3.2398 +0.0000 0.0000 3.1416 +primID: 0 +startangle_c: 10 +endpose_c: -1 -1 10 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 3.9270 +-0.0250 -0.0250 3.9270 +-0.0500 -0.0500 3.9270 +primID: 1 +startangle_c: 10 +endpose_c: -6 -6 10 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 3.9270 +-0.0333 -0.0333 3.9270 +-0.0667 -0.0667 3.9270 +-0.1000 -0.1000 3.9270 +-0.1333 -0.1333 3.9270 +-0.1667 -0.1667 3.9270 +-0.2000 -0.2000 3.9270 +-0.2333 -0.2333 3.9270 +-0.2667 -0.2667 3.9270 +-0.3000 -0.3000 3.9270 +primID: 2 +startangle_c: 10 +endpose_c: 1 1 10 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 3.9270 +0.0250 0.0250 3.9270 +0.0500 0.0500 3.9270 +primID: 3 +startangle_c: 10 +endpose_c: -3 -5 11 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 3.9270 +-0.0250 -0.0266 3.9761 +-0.0484 -0.0547 4.0252 +-0.0700 -0.0842 4.0743 +-0.0898 -0.1151 4.1233 +-0.1078 -0.1472 4.1724 +-0.1239 -0.1804 4.2215 +-0.1379 -0.2147 4.2706 +-0.1500 -0.2500 4.3197 +primID: 4 +startangle_c: 10 +endpose_c: -5 -3 9 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 3.9270 +-0.0266 -0.0250 3.8779 +-0.0547 -0.0484 3.8288 +-0.0842 -0.0700 3.7797 +-0.1151 -0.0898 3.7306 +-0.1472 -0.1078 3.6816 +-0.1804 -0.1239 3.6325 +-0.2147 -0.1379 3.5834 +-0.2500 -0.1500 3.5343 +primID: 5 +startangle_c: 10 +endpose_c: 0 0 11 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.9270 +0.0000 0.0000 4.0252 +0.0000 0.0000 4.1233 +0.0000 0.0000 4.2215 +0.0000 0.0000 4.3197 +primID: 6 +startangle_c: 10 +endpose_c: 0 0 9 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 3.9270 +0.0000 0.0000 3.8288 +0.0000 0.0000 3.7306 +0.0000 0.0000 3.6325 +0.0000 0.0000 3.5343 +primID: 0 +startangle_c: 11 +endpose_c: -1 -2 11 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 4.3197 +-0.0167 -0.0333 4.3197 +-0.0333 -0.0667 4.3197 +-0.0500 -0.1000 4.3197 +primID: 1 +startangle_c: 11 +endpose_c: -3 -7 11 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 4.3197 +-0.0188 -0.0438 4.3197 +-0.0375 -0.0875 4.3197 +-0.0563 -0.1313 4.3197 +-0.0750 -0.1750 4.3197 +-0.0938 -0.2188 4.3197 +-0.1125 -0.2625 4.3197 +-0.1313 -0.3063 4.3197 +-0.1500 -0.3500 4.3197 +primID: 2 +startangle_c: 11 +endpose_c: 1 2 11 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 4.3197 +0.0167 0.0333 4.3197 +0.0333 0.0667 4.3197 +0.0500 0.1000 4.3197 +primID: 3 +startangle_c: 11 +endpose_c: -1 -5 12 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 4.3197 +-0.0151 -0.0400 4.3834 +-0.0276 -0.0809 4.4492 +-0.0374 -0.1225 4.5150 +-0.0444 -0.1647 4.5808 +-0.0486 -0.2073 4.6466 +-0.0500 -0.2500 4.7124 +primID: 4 +startangle_c: 11 +endpose_c: -3 -5 10 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 4.3197 +-0.0188 -0.0453 4.3197 +-0.0380 -0.0903 4.2747 +-0.0607 -0.1337 4.1878 +-0.0871 -0.1750 4.1008 +-0.1169 -0.2139 4.0139 +-0.1500 -0.2500 3.9270 +primID: 5 +startangle_c: 11 +endpose_c: 0 0 12 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.4179 +0.0000 0.0000 4.5160 +0.0000 0.0000 4.6142 +0.0000 0.0000 4.7124 +primID: 6 +startangle_c: 11 +endpose_c: 0 0 10 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 4.3197 +0.0000 0.0000 4.2215 +0.0000 0.0000 4.1233 +0.0000 0.0000 4.0252 +0.0000 0.0000 3.9270 +primID: 0 +startangle_c: 12 +endpose_c: 0 -2 12 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 4.7124 +0.0000 -0.0500 4.7124 +0.0000 -0.1000 4.7124 +primID: 1 +startangle_c: 12 +endpose_c: 0 -8 12 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 4.7124 +0.0000 -0.0500 4.7124 +0.0000 -0.1000 4.7124 +0.0000 -0.1500 4.7124 +0.0000 -0.2000 4.7124 +0.0000 -0.2500 4.7124 +0.0000 -0.3000 4.7124 +0.0000 -0.3500 4.7124 +0.0000 -0.4000 4.7124 +primID: 2 +startangle_c: 12 +endpose_c: 0 2 12 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 4.7124 +0.0000 0.0500 4.7124 +0.0000 0.1000 4.7124 +primID: 3 +startangle_c: 12 +endpose_c: 1 -5 13 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 4.7124 +0.0014 -0.0427 4.7778 +0.0056 -0.0853 4.8433 +0.0126 -0.1275 4.9087 +0.0224 -0.1691 4.9742 +0.0349 -0.2100 5.0396 +0.0500 -0.2500 5.1051 +primID: 4 +startangle_c: 12 +endpose_c: -1 -5 11 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 4.7124 +-0.0014 -0.0427 4.6469 +-0.0056 -0.0853 4.5815 +-0.0126 -0.1275 4.5160 +-0.0224 -0.1691 4.4506 +-0.0349 -0.2100 4.3851 +-0.0500 -0.2500 4.3197 +primID: 5 +startangle_c: 12 +endpose_c: 0 0 13 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.8106 +0.0000 0.0000 4.9087 +0.0000 0.0000 5.0069 +0.0000 0.0000 5.1051 +primID: 6 +startangle_c: 12 +endpose_c: 0 0 11 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 4.7124 +0.0000 0.0000 4.6142 +0.0000 0.0000 4.5160 +0.0000 0.0000 4.4179 +0.0000 0.0000 4.3197 +primID: 0 +startangle_c: 13 +endpose_c: 1 -2 13 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 5.1051 +0.0167 -0.0333 5.1051 +0.0333 -0.0667 5.1051 +0.0500 -0.1000 5.1051 +primID: 1 +startangle_c: 13 +endpose_c: 3 -7 13 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 5.1051 +0.0188 -0.0438 5.1051 +0.0375 -0.0875 5.1051 +0.0563 -0.1313 5.1051 +0.0750 -0.1750 5.1051 +0.0938 -0.2188 5.1051 +0.1125 -0.2625 5.1051 +0.1313 -0.3063 5.1051 +0.1500 -0.3500 5.1051 +primID: 2 +startangle_c: 13 +endpose_c: -1 2 13 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 5.1051 +-0.0167 0.0333 5.1051 +-0.0333 0.0667 5.1051 +-0.0500 0.1000 5.1051 +primID: 3 +startangle_c: 13 +endpose_c: 3 -5 14 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 5.1051 +0.0188 -0.0453 5.1051 +0.0380 -0.0903 5.1501 +0.0607 -0.1337 5.2370 +0.0871 -0.1750 5.3239 +0.1169 -0.2139 5.4109 +0.1500 -0.2500 5.4978 +primID: 4 +startangle_c: 13 +endpose_c: 1 -5 12 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 5.1051 +0.0151 -0.0400 5.0414 +0.0276 -0.0809 4.9756 +0.0374 -0.1225 4.9098 +0.0444 -0.1647 4.8440 +0.0486 -0.2073 4.7782 +0.0500 -0.2500 4.7124 +primID: 5 +startangle_c: 13 +endpose_c: 0 0 14 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.2033 +0.0000 0.0000 5.3014 +0.0000 0.0000 5.3996 +0.0000 0.0000 5.4978 +primID: 6 +startangle_c: 13 +endpose_c: 0 0 12 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.1051 +0.0000 0.0000 5.0069 +0.0000 0.0000 4.9087 +0.0000 0.0000 4.8106 +0.0000 0.0000 4.7124 +primID: 0 +startangle_c: 14 +endpose_c: 1 -1 14 +additionalactioncostmult: 1 +intermediateposes: 3 +0.0000 0.0000 5.4978 +0.0250 -0.0250 5.4978 +0.0500 -0.0500 5.4978 +primID: 1 +startangle_c: 14 +endpose_c: 6 -6 14 +additionalactioncostmult: 1 +intermediateposes: 10 +0.0000 0.0000 5.4978 +0.0333 -0.0333 5.4978 +0.0667 -0.0667 5.4978 +0.1000 -0.1000 5.4978 +0.1333 -0.1333 5.4978 +0.1667 -0.1667 5.4978 +0.2000 -0.2000 5.4978 +0.2333 -0.2333 5.4978 +0.2667 -0.2667 5.4978 +0.3000 -0.3000 5.4978 +primID: 2 +startangle_c: 14 +endpose_c: -1 1 14 +additionalactioncostmult: 40 +intermediateposes: 3 +0.0000 0.0000 5.4978 +-0.0250 0.0250 5.4978 +-0.0500 0.0500 5.4978 +primID: 3 +startangle_c: 14 +endpose_c: 5 -3 15 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 5.4978 +0.0266 -0.0250 5.5469 +0.0547 -0.0484 5.5960 +0.0842 -0.0700 5.6450 +0.1151 -0.0898 5.6941 +0.1472 -0.1078 5.7432 +0.1804 -0.1239 5.7923 +0.2147 -0.1379 5.8414 +0.2500 -0.1500 5.8905 +primID: 4 +startangle_c: 14 +endpose_c: 3 -5 13 +additionalactioncostmult: 2 +intermediateposes: 9 +0.0000 0.0000 5.4978 +0.0250 -0.0266 5.4487 +0.0484 -0.0547 5.3996 +0.0700 -0.0842 5.3505 +0.0898 -0.1151 5.3014 +0.1078 -0.1472 5.2524 +0.1239 -0.1804 5.2033 +0.1379 -0.2147 5.1542 +0.1500 -0.2500 5.1051 +primID: 5 +startangle_c: 14 +endpose_c: 0 0 15 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.5960 +0.0000 0.0000 5.6941 +0.0000 0.0000 5.7923 +0.0000 0.0000 5.8905 +primID: 6 +startangle_c: 14 +endpose_c: 0 0 13 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.4978 +0.0000 0.0000 5.3996 +0.0000 0.0000 5.3014 +0.0000 0.0000 5.2033 +0.0000 0.0000 5.1051 +primID: 0 +startangle_c: 15 +endpose_c: 2 -1 15 +additionalactioncostmult: 1 +intermediateposes: 4 +0.0000 0.0000 5.8905 +0.0333 -0.0167 5.8905 +0.0667 -0.0333 5.8905 +0.1000 -0.0500 5.8905 +primID: 1 +startangle_c: 15 +endpose_c: 7 -3 15 +additionalactioncostmult: 1 +intermediateposes: 9 +0.0000 0.0000 5.8905 +0.0438 -0.0188 5.8905 +0.0875 -0.0375 5.8905 +0.1313 -0.0563 5.8905 +0.1750 -0.0750 5.8905 +0.2188 -0.0938 5.8905 +0.2625 -0.1125 5.8905 +0.3063 -0.1313 5.8905 +0.3500 -0.1500 5.8905 +primID: 2 +startangle_c: 15 +endpose_c: -2 1 15 +additionalactioncostmult: 40 +intermediateposes: 4 +0.0000 0.0000 5.8905 +-0.0333 0.0167 5.8905 +-0.0667 0.0333 5.8905 +-0.1000 0.0500 5.8905 +primID: 3 +startangle_c: 15 +endpose_c: 5 -1 0 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 5.8905 +0.0400 -0.0151 5.9542 +0.0809 -0.0276 6.0200 +0.1225 -0.0374 6.0858 +0.1647 -0.0444 6.1516 +0.2073 -0.0486 6.2174 +0.2500 -0.0500 6.2832 +primID: 4 +startangle_c: 15 +endpose_c: 5 -3 14 +additionalactioncostmult: 2 +intermediateposes: 7 +0.0000 0.0000 5.8905 +0.0453 -0.0188 5.8905 +0.0903 -0.0380 5.8455 +0.1337 -0.0607 5.7586 +0.1750 -0.0871 5.6716 +0.2139 -0.1169 5.5847 +0.2500 -0.1500 5.4978 +primID: 5 +startangle_c: 15 +endpose_c: 0 0 0 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.9887 +0.0000 0.0000 6.0868 +0.0000 0.0000 6.1850 +0.0000 0.0000 6.2832 +primID: 6 +startangle_c: 15 +endpose_c: 0 0 14 +additionalactioncostmult: 20 +intermediateposes: 5 +0.0000 0.0000 5.8905 +0.0000 0.0000 5.7923 +0.0000 0.0000 5.6941 +0.0000 0.0000 5.5960 +0.0000 0.0000 5.4978 diff --git a/diff_5cm_angle0.png b/diff_5cm_angle0.png new file mode 100644 index 0000000..9fb9547 Binary files /dev/null and b/diff_5cm_angle0.png differ diff --git a/diff_angle0.png b/diff_angle0.png new file mode 100644 index 0000000..eda98ce Binary files /dev/null and b/diff_angle0.png differ diff --git a/diff_angle1.png b/diff_angle1.png new file mode 100644 index 0000000..257b037 Binary files /dev/null and b/diff_angle1.png differ diff --git a/generate_mprim.py b/generate_mprim.py new file mode 100644 index 0000000..e3d9241 --- /dev/null +++ b/generate_mprim.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +""" +generate_mprim.py +------------------ +CLI để sinh file .mprim từ 1 file config trong configs/. + +Cách dùng: + python3 generate_mprim.py configs/differential.py + python3 generate_mprim.py configs/omnidirectional.py -o my_output.mprim + python3 generate_mprim.py configs/ackermann.py + +Muốn thêm robot mới: copy 1 file trong configs/, đổi số liệu, chạy lại. +KHÔNG cần sửa code trong mprim_core.py / kinematics.py. +""" + +import argparse +import importlib.util +import os +import sys + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from mprim_config import KinematicType +from mprim_core import generate_mprim_file +from kinematics import unicycle_poses, holonomic_poses + +# differential và ackermann dùng chung interpolator: đường đi của xe bánh lái theo +# cung tròn trùng với nghiệm unicycle (đoạn thẳng + cung); khác nhau chỉ ở tập +# primitive và ràng buộc min_turning_radius (mprim_core xử lý). +KINEMATICS_MAP = { + KinematicType.DIFFERENTIAL: unicycle_poses, + KinematicType.OMNIDIRECTIONAL: holonomic_poses, + KinematicType.ACKERMANN: unicycle_poses, +} + + +def load_config(config_path: str): + spec = importlib.util.spec_from_file_location("robot_config_module", config_path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + if not hasattr(module, "config"): + raise ValueError(f"{config_path} phải định nghĩa biến `config = RobotConfig(...)`") + return module.config + + +def main(): + parser = argparse.ArgumentParser(description="Sinh file .mprim từ config robot") + parser.add_argument("config", help="Đường dẫn tới file config Python (vd: configs/differential.py)") + parser.add_argument("-o", "--output", default=None, help="Đường dẫn file .mprim output") + args = parser.parse_args() + + cfg = load_config(args.config) + kinematics_fn = KINEMATICS_MAP[cfg.kinematic_type] + + output_path = args.output or f"{cfg.name}.mprim" + n, warnings = generate_mprim_file(cfg, kinematics_fn, output_path) + + print(f"[OK] {cfg.kinematic_type.value}: đã sinh {n} primitives -> {output_path}") + if warnings: + print(f"[WARN] {len(warnings)} cảnh báo:") + for w in warnings: + print(f" - {w}") + + +if __name__ == "__main__": + main() diff --git a/kinematics.py b/kinematics.py new file mode 100644 index 0000000..a859c76 --- /dev/null +++ b/kinematics.py @@ -0,0 +1,140 @@ +""" +kinematics.py +------------- +Mỗi loại drive có một cách nội suy quỹ đạo (intermediate poses) khác nhau giữa +điểm đầu và điểm cuối của 1 motion primitive. Đây là phần logic THỰC SỰ khác +nhau giữa các robot — phần còn lại (symmetry theo góc, ghi file...) dùng chung. + +Mọi interpolator đều nhận: + startpt = (x, y, theta) # luôn là (0, 0, currentangle) + endpt = (x, y, theta) # điểm kết thúc ĐÃ quantize về lưới (mét, rad) + dtheta_c = số bước góc rời rạc thay đổi (int, có dấu, +: CCW) + numberofangles = tổng số góc rời rạc + spacing_m = khoảng cách mong muốn giữa 2 pose liên tiếp [m] + spacing_rad = bước góc tối đa giữa 2 pose liên tiếp [rad] + +Trả về (poses, info): + poses: list (x, y, theta); pose đầu = startpt, pose cuối = ĐÚNG endpt, + heading cuối = ĐÚNG startangle + dtheta_c bước góc (SBPL yêu cầu để nạp file). + info: dict chẩn đoán {"radius_m": bán kính cung (None nếu thẳng), + "line_lead_m": đoạn thẳng dẫn vào của nghiệm line+arc} + — engine dùng để cảnh báo vi phạm min_turning_radius. + +Số sample tính THEO CHIỀU DÀI (n = ceil(L/spacing)+1) thay vì cố định — nhờ vậy +khoảng cách pose ~ đều nhau trên mọi primitive dài ngắn khác nhau, và path SBPL +ghép từ nhiều primitive cũng đều theo. +""" + +import numpy as np + + +def _num_samples(path_len_m, sweep_rad, spacing_m, spacing_rad): + """Số pose sao cho bước dài <= spacing_m VÀ bước góc <= spacing_rad.""" + n_lin = int(np.ceil(abs(path_len_m) / spacing_m)) if spacing_m > 0 else 0 + n_ang = int(np.ceil(abs(sweep_rad) / spacing_rad)) if spacing_rad > 0 else 0 + return max(2, n_lin + 1, n_ang + 1) + + +def _distribute_endpoint_error(poses, ex, ey): + """Dàn sai số điểm cuối tuyến tính dọc quỹ đạo để pose cuối khớp ĐÚNG lưới cell. + + Chỉ dịch (x, y), KHÔNG đụng theta — heading đã chính xác theo cấu trúc nghiệm, + dịch vị trí nhỏ (<= nửa cell) chỉ gây lệch tangent vài độ, chấp nhận được. + """ + errx = ex - poses[-1][0] + erry = ey - poses[-1][1] + n = len(poses) + return [(x + errx * i / (n - 1), y + erry * i / (n - 1), th) + for i, (x, y, th) in enumerate(poses)] + + +def unicycle_poses(startpt, endpt, dtheta_c, numberofangles, spacing_m, spacing_rad): + """Nội suy cho robot nonholonomic (differential VÀ ackermann — cùng hình học đường đi). + + Nghiệm "đoạn thẳng l + cung tròn bán kính k" giải từ hệ tuyến tính, y hệt + genmprim_unicycle*.m gốc. Khác bản gốc ở 2 điểm: + 1. Khi dấu l mâu thuẫn với chiều đi (nghiệm đòi lùi rồi mới tiến), bản gốc chỉ ép + l = 0 mà giữ nguyên rv -> heading cuối lệch tới ~10 độ so với endpose (bug F2). + Ở đây ép l = 0 RỒI TÍNH LẠI rv = rotation_angle -> heading cuối luôn chính xác, + sai số dồn hết vào vị trí và được _distribute_endpoint_error dàn đều. + 2. Sample theo chiều dài thay vì 10 điểm cố định. + """ + sx, sy, sth = startpt + ex, ey, _ = endpt + rotation_angle = dtheta_c * (2 * np.pi / numberofangles) + eth_cont = sth + rotation_angle # heading cuối liên tục (không wrap — sin/cos như nhau) + dist = np.hypot(ex - sx, ey - sy) + + # --- đi thẳng / lùi không đổi hướng --- + if dtheta_c == 0: + n = _num_samples(dist, 0.0, spacing_m, spacing_rad) + # theta giữ = heading; hướng vector đi có thể lệch heading vài độ do endpose đã + # quantize về cell — sai số lưới, không tránh được, giống bản MATLAB gốc. + poses = [(sx + (ex - sx) * t, sy + (ey - sy) * t, sth) + for t in np.linspace(0.0, 1.0, n)] + return poses, {"radius_m": None, "line_lead_m": None} + + # --- quay tại chỗ --- + if dist < 1e-9: + n = _num_samples(0.0, rotation_angle, spacing_m, spacing_rad) + poses = [(sx, sy, sth + rotation_angle * t) for t in np.linspace(0.0, 1.0, n)] + return poses, {"radius_m": None, "line_lead_m": None} + + # --- đoạn thẳng + cung tròn (unicycle, tv & rv hằng) --- + # [dx, dy]^T = [cos(sth), sin(eth)-sin(sth); sin(sth), -(cos(eth)-cos(sth))] @ [l, k]^T + # với l = chiều dài đoạn thẳng dẫn vào (có dấu), k = tv/rv = bán kính cung (có dấu). + R = np.array([ + [np.cos(sth), np.sin(eth_cont) - np.sin(sth)], + [np.sin(sth), -(np.cos(eth_cont) - np.cos(sth))], + ]) + b = np.array([ex - sx, ey - sy]) + S = np.linalg.pinv(R) @ b + l = float(S[0]) + k = float(S[1]) + if abs(k) < 1e-9: + # endpose thẳng hàng với heading nhưng vẫn phải đổi góc: cung suy biến thành + # "đi thẳng rồi xoay gắt ở cuối". Xảy ra khi turn_radius quá nhỏ so với cell + # (validate đã cảnh báo) — vẫn sinh ra pose hợp lệ, không chia cho 0. + k = 1e-9 if k >= 0 else -1e-9 + + rv = rotation_angle + l / k + tv = k * rv + if (l < 0 < tv) or (l > 0 > tv): + # Nghiệm đòi đảo chiều trong 1 primitive (do quantize đẩy endpose ra sau cung). + # Ép l = 0 và TÍNH LẠI rv để heading cuối = rotation_angle chính xác. + l = 0.0 + rv = rotation_angle + tv = k * rv + + arc_len = abs(k * rotation_angle) + n = _num_samples(abs(l) + arc_len, rotation_angle, spacing_m, spacing_rad) + poses = [] + for t in np.linspace(0.0, 1.0, n): + if tv != 0.0 and abs(t * tv) < abs(l): + # pha đi thẳng dọc heading ban đầu + poses.append((sx + t * tv * np.cos(sth), + sy + t * tv * np.sin(sth), + sth)) + else: + denom = tv if tv != 0.0 else 1e-9 + th = rv * (t - l / denom) + sth + poses.append((sx + l * np.cos(sth) + k * (np.sin(th) - np.sin(sth)), + sy + l * np.sin(sth) - k * (np.cos(th) - np.cos(sth)), + th)) + # chốt heading cuối đúng tuyệt đối (tránh sai số tích lũy float) + poses[-1] = (poses[-1][0], poses[-1][1], eth_cont) + poses = _distribute_endpoint_error(poses, ex, ey) + return poses, {"radius_m": abs(k), "line_lead_m": l} + + +def holonomic_poses(startpt, endpt, dtheta_c, numberofangles, spacing_m, spacing_rad): + """Holonomic (omni/mecanum/swerve): (x, y) độc lập với heading nên nội suy tuyến tính + cả vị trí lẫn theta — mô hình LINESEGMENT_MPRIMS của genmprim.m gốc.""" + sx, sy, sth = startpt + ex, ey, _ = endpt + rotation_angle = dtheta_c * (2 * np.pi / numberofangles) + dist = np.hypot(ex - sx, ey - sy) + n = _num_samples(dist, rotation_angle, spacing_m, spacing_rad) + poses = [(sx + (ex - sx) * t, sy + (ey - sy) * t, sth + rotation_angle * t) + for t in np.linspace(0.0, 1.0, n)] + return poses, {"radius_m": None, "line_lead_m": None} diff --git a/mprim_config.py b/mprim_config.py new file mode 100644 index 0000000..2234bf9 --- /dev/null +++ b/mprim_config.py @@ -0,0 +1,198 @@ +""" +mprim_config.py +---------------- +Cấu hình robot dùng chung cho mọi loại kinematic (differential / omnidirectional / ackermann). + +Chỉnh các thông số trong file configs/*.py theo robot thực tế của bạn. +File này KHÔNG chứa logic sinh primitive — chỉ định nghĩa dữ liệu + validate. + +Ràng buộc quan trọng của format SBPL (lý do cho các validate bên dưới): +- `additionalactioncostmult` được SBPL đọc bằng fscanf("%d") -> BẮT BUỘC là số nguyên, + giá trị lẻ (1.5, 2.5...) làm hỏng cả file ngay từ primitive đầu tiên chứa nó. +- Pose cuối của quỹ đạo phải khớp endpose sau khi rời rạc hóa, nếu lệch quá nửa bước góc + (= 180/numberofangles độ) SBPL từ chối nạp file. +- Khoảng cách giữa 2 intermediate pose liên tiếp không được vượt quá 1 cell, nếu không + bước quét footprint (get_2d_motion_cells) sẽ bỏ sót cell trên đường đi. +""" + +import math +from dataclasses import dataclass, field +from enum import Enum +from typing import Optional, Tuple + + +class KinematicType(str, Enum): + DIFFERENTIAL = "differential" # 2 bánh vi sai (unicycle model) + OMNIDIRECTIONAL = "omnidirectional" # mecanum / omni / swerve (holonomic) + ACKERMANN = "ackermann" # xe có bánh lái, bán kính quay tối thiểu + + +@dataclass +class RobotConfig: + # --- Thông tin chung --- + name: str # tên robot; dùng đặt tên file output .mprim + kinematic_type: KinematicType # differential / omnidirectional / ackermann — quyết + # định tập primitive và cách nội suy quỹ đạo + + # --- Lưới rời rạc hóa (phải khớp với costmap / planner config) --- + resolution_m: float = 0.05 # [m] kích thước 1 cell — PHẢI khớp costmap resolution, + # SBPL so sánh và từ chối file nếu lệch + numberofangles: int = 16 # số góc heading rời rạc; bội số của 8, khuyến nghị 16 + # (16 góc -> 1 bước góc = 22.5 độ) + + # --- Primitive đi thẳng --- + forward_short_m: float = 0.10 # [m] bước tiến ngắn, dùng để tinh chỉnh vị trí gần + # goal; >= 0.75*resolution để không quantize về (0,0) + forward_long_m: float = 0.40 # [m] bước tiến dài — quãng đường ~1 chu kỳ điều khiển + # ở tốc độ hành trình; quyết định tốc độ expand của A* + backward_m: float = 0.10 # [m] bước lùi thẳng (chỉ dùng khi has_backward_prims) + + # --- Primitive vừa đi vừa rẽ --- + # Rẽ được mô tả bằng CUNG TRÒN: turn_steps liệt kê số bước góc rời rạc mỗi primitive rẽ + # (1 = 22.5 độ với 16 góc), turn_radius_m là bán kính cung [m]. Điểm cuối được tự tính: + # dx = R*sin(dtheta), dy = R*(1 - cos(dtheta)) + # -> heading cuối LUÔN khớp góc rời rạc đích, không phải đoán độ lệch ngang bằng tay. + turn_steps: Tuple[int, ...] = (1,) # các mức rẽ; (1,) = chỉ rẽ 22.5 độ, (1, 2) thêm rẽ 45 + # độ — mỗi mức thêm 2 primitive/góc (CCW + CW), + # branching factor tăng -> planner chậm hơn + turn_radius_m: float = 0.70 # [m] bán kính cung rẽ. Nên >= max_vel/max_yawrate để + # local planner bám được không phải giảm tốc; và >= + # resolution/(1-cos(bước góc)) (~0.66m với lưới 5cm/16 + # góc) để quantize không làm méo cung. Ackermann: bắt + # buộc >= min_turning_radius_m. + + # --- Chỉ dùng cho OMNIDIRECTIONAL --- + sidestep_m: float = 0.10 # [m] bước đi ngang (trái/phải) giữ nguyên heading + diagonal_m: float = 0.10 # [m] chiều dài bước đi chéo 45 độ giữ nguyên heading + + # --- Chỉ dùng cho ACKERMANN --- + min_turning_radius_m: float = 0.35 # [m] bán kính quay tối thiểu VẬT LÝ (đo từ robot + # thật, không trượt bánh) — generator cảnh báo nếu + # cung sau quantize vi phạm giá trị này + + # --- Bật/tắt từng họ primitive --- + has_backward_prims: bool = True # cho phép lùi thẳng (tắt cho robot kéo hàng + # không được lùi kiểu noreverse_trolley) + has_backward_turn_prims: bool = False # cho phép lùi + rẽ; mặc định tắt vì AMR cảm biến + # chính hướng trước thì lùi-rẽ là hành động mù + has_turn_in_place_prims: bool = True # quay tại chỗ (ackermann thật thường phải tắt) + + # --- Hệ số chi phí, theo convention SBPL: cost thật = cost_thời_gian * costmult --- + # BẮT BUỘC số nguyên >= 1 (SBPL đọc bằng %d). Tham khảo bộ tuned unicycle_highcost: + # backward=40, turninplace=20, forwardandturn=2 — lùi/xoay tại chỗ đắt tới mức chỉ được + # chọn khi không còn đường nào khác. + forwardcostmult: int = 1 # tiến thẳng — mốc chuẩn, luôn để 1 + backwardcostmult: int = 40 # lùi thẳng; 40 nghĩa là lùi 0.1m "đắt" bằng tiến 4m + forwardandturncostmult: int = 2 # vừa tiến vừa rẽ; >1 để planner ưu tiên đường thẳng + backwardandturncostmult: int = 40 # lùi + rẽ (chỉ dùng khi has_backward_turn_prims) + turninplacecostmult: int = 20 # quay tại chỗ; giảm xuống nếu muốn robot xoay thay vì + # đi vòng, nhưng path sẽ xuất hiện cụm pose trùng điểm + sidestepcostmult: int = 10 # đi ngang (chỉ omni) + diagonalcostmult: int = 2 # đi chéo 45 độ (chỉ omni) + + # --- Mật độ intermediate poses (quyết định độ đều của path SBPL trả ra) --- + # Số sample được tính THEO CHIỀU DÀI: n = ceil(L / pose_spacing_m) + 1, nên mọi primitive + # dài ngắn khác nhau đều có khoảng cách pose ~ pose_spacing_m -> path ra đều nhau. + pose_spacing_m: float = 0.05 # [m] khoảng cách giữa 2 pose liên tiếp; phải <= + # resolution_m để footprint sweep không bỏ sót cell + pose_angular_spacing_deg: float = 5.625 # [deg] bước góc tối đa giữa 2 pose (quay tại chỗ + # và phần cung tròn; footprint sweep cần đủ mịn) + + # --- Giới hạn động học để sanity-check (tùy chọn, None = bỏ qua) --- + max_vel_mps: Optional[float] = None # [m/s] vận tốc dài tối đa của robot thật + max_yawrate_radps: Optional[float] = None # [rad/s] vận tốc xoay tối đa; cùng max_vel + # dùng để cảnh báo turn_radius quá gắt + + def validate(self): + """Raise ValueError nếu config sai; trả về list cảnh báo (không chặn generate).""" + errors = [] + warnings = [] + + if self.numberofangles % 8 != 0: + errors.append("numberofangles phải là bội số của 8 (khuyến nghị 16 hoặc 32).") + if self.resolution_m <= 0: + errors.append("resolution_m phải > 0.") + + # costmult: SBPL đọc bằng fscanf("%d") — số lẻ làm hỏng cả file + for f in ("forwardcostmult", "backwardcostmult", "forwardandturncostmult", + "backwardandturncostmult", "turninplacecostmult", + "sidestepcostmult", "diagonalcostmult"): + v = getattr(self, f) + if not isinstance(v, int) or isinstance(v, bool) or v < 1: + errors.append(f"{f} = {v!r}: costmult phải là SỐ NGUYÊN >= 1 " + "(SBPL đọc bằng fscanf %d, giá trị lẻ làm hỏng file).") + + # độ dài primitive thẳng: quá ngắn sẽ quantize về (0,0) ở góc 45 độ -> self-loop + min_len = self.resolution_m * 0.75 + for f in ("forward_short_m", "forward_long_m"): + if getattr(self, f) < min_len: + errors.append(f"{f} = {getattr(self, f)}: phải >= 0.75*resolution_m " + f"({min_len:.3f}m), nếu không endpose quantize về (0,0) ở góc chéo.") + elif getattr(self, f) < self.resolution_m: + warnings.append(f"{f} < resolution_m: primitive chỉ đi được ~1 cell, " + "cân nhắc tăng lên.") + if self.has_backward_prims and self.backward_m < min_len: + errors.append(f"backward_m = {self.backward_m}: phải >= {min_len:.3f}m " + "(hoặc tắt has_backward_prims).") + + # turn prims + if not self.turn_steps: + errors.append("turn_steps rỗng: cần ít nhất 1 bước góc (vd (1,)).") + for s in self.turn_steps: + if not isinstance(s, int) or s < 1 or s > self.numberofangles // 4: + errors.append(f"turn_steps chứa {s!r}: phải là int trong " + f"[1, {self.numberofangles // 4}] (tối đa 90 độ mỗi primitive).") + if len(set(self.turn_steps)) != len(self.turn_steps): + errors.append("turn_steps có phần tử trùng nhau.") + if self.turn_radius_m <= 0: + errors.append("turn_radius_m phải > 0.") + else: + step_rad = 2 * math.pi / self.numberofangles + min_step = min(self.turn_steps or [1]) + dy_min = self.turn_radius_m * (1 - math.cos(min_step * step_rad)) + if dy_min < self.resolution_m: + # dy < 1 cell nghĩa là sai số quantize cùng cỡ với chính hình dạng cung -> + # cung bị méo nặng, bán kính thực tế sau quantize có thể nhỏ hơn nhiều + r_need = self.resolution_m / (1 - math.cos(min_step * step_rad)) + warnings.append( + f"turn_radius_m = {self.turn_radius_m}: độ lệch ngang của cung rẽ nhỏ nhất " + f"({dy_min:.3f}m) < 1 cell -> quantize làm méo cung đáng kể. " + f"Khuyến nghị turn_radius_m >= {r_need:.2f}m với lưới hiện tại.") + + # sampling + if self.pose_spacing_m <= 0: + errors.append("pose_spacing_m phải > 0.") + elif self.pose_spacing_m > self.resolution_m: + errors.append(f"pose_spacing_m = {self.pose_spacing_m} > resolution_m: " + "footprint sweep của SBPL sẽ bỏ sót cell giữa 2 pose liên tiếp.") + if self.pose_angular_spacing_deg <= 0: + errors.append("pose_angular_spacing_deg phải > 0.") + + # giới hạn động học (tùy chọn) + if self.max_vel_mps is not None and self.max_yawrate_radps is not None: + r_min_kin = self.max_vel_mps / self.max_yawrate_radps + if self.turn_radius_m < r_min_kin: + warnings.append( + f"turn_radius_m = {self.turn_radius_m} < max_vel/max_yawrate = " + f"{r_min_kin:.3f}m: local planner phải giảm tốc mới bám được cung rẽ này.") + + # ackermann + if self.kinematic_type == KinematicType.ACKERMANN: + if self.min_turning_radius_m <= 0: + errors.append("Ackermann cần min_turning_radius_m > 0.") + elif self.turn_radius_m < self.min_turning_radius_m: + errors.append(f"turn_radius_m = {self.turn_radius_m} < min_turning_radius_m = " + f"{self.min_turning_radius_m}: primitive vi phạm giới hạn lái vật lý.") + if self.has_turn_in_place_prims: + warnings.append("Ackermann bật has_turn_in_place_prims: xe bánh lái thường " + "KHÔNG quay tại chỗ được — chỉ giữ nếu robot có bánh xoay đặc biệt.") + + # omni + if self.kinematic_type == KinematicType.OMNIDIRECTIONAL: + for f in ("sidestep_m", "diagonal_m"): + if getattr(self, f) < min_len: + errors.append(f"{f} = {getattr(self, f)}: phải >= {min_len:.3f}m.") + + if errors: + raise ValueError("Config không hợp lệ:\n - " + "\n - ".join(errors)) + return warnings diff --git a/mprim_core.py b/mprim_core.py new file mode 100644 index 0000000..ac19506 --- /dev/null +++ b/mprim_core.py @@ -0,0 +1,216 @@ +""" +mprim_core.py +------------- +Logic dùng CHUNG cho mọi robot: định nghĩa tập primitive theo hệ tọa độ gắn với +thân robot (body frame, heading = 0), tự động xoay sang từng góc rời rạc +(0..numberofangles-1), quantize về lưới cell, nội suy quỹ đạo và ghi file .mprim. + +Khác các file .m gốc (định nghĩa tay 3 bộ template cell riêng cho 0/45/22.5 độ), +ở đây primitive được định nghĩa 1 lần THEO NGHĨA VẬT LÝ (mét, bán kính cung) rồi +xoay + làm tròn cho từng góc. Đánh đổi là sai số lượng tử hóa nhỏ ở các góc lẻ +(< nửa cell, đã được dàn đều dọc quỹ đạo) — chấp nhận được với AMR thực tế. + +Sau khi sinh, engine TỰ KIỂM những ràng buộc mà SBPL sẽ enforce lúc nạp file +(xem ReadinMotionPrimitive trong environment_navxythetalat.cpp) để lỗi lộ ra +ngay lúc generate thay vì lúc khởi động planner trên robot. +""" + +import math +from dataclasses import dataclass +from typing import Callable, Dict, List, Tuple + +import numpy as np + +from mprim_config import RobotConfig, KinematicType + + +@dataclass +class Primitive: + dx_body_m: float # [m] dịch chuyển dọc trục robot (body frame) + dy_body_m: float # [m] dịch chuyển ngang (body frame, +: trái) + dtheta_c: int # số bước góc rời rạc thay đổi (+: CCW) + costmult: int # hệ số chi phí SBPL (số nguyên, đã validate) + label: str = "" + + +def _turn_endpoint(radius_m: float, steps: int, numberofangles: int) -> Tuple[float, float]: + """Điểm cuối body-frame của cung tròn bán kính radius_m quay steps bước góc. + + dx = R*sin(|dtheta|), dy = sign(steps)*R*(1-cos(|dtheta|)) — heading cuối trùng + khít góc rời rạc đích theo cấu trúc, không cần người dùng đoán độ lệch ngang. + """ + dtheta = abs(steps) * 2.0 * math.pi / numberofangles + return radius_m * math.sin(dtheta), math.copysign(radius_m * (1.0 - math.cos(dtheta)), steps) + + +def _nonholonomic_prims(cfg: RobotConfig, turn_radius_m: float) -> List[Primitive]: + """Tập primitive chung cho differential và ackermann (chỉ khác toggle/costmult).""" + prims = [ + Primitive(cfg.forward_short_m, 0.0, 0, cfg.forwardcostmult, "fwd_short"), + Primitive(cfg.forward_long_m, 0.0, 0, cfg.forwardcostmult, "fwd_long"), + ] + if cfg.has_backward_prims: + prims.append(Primitive(-cfg.backward_m, 0.0, 0, cfg.backwardcostmult, "bwd")) + for s in cfg.turn_steps: + dx, dy = _turn_endpoint(turn_radius_m, s, cfg.numberofangles) + prims += [ + Primitive(dx, dy, s, cfg.forwardandturncostmult, f"fwd_turn_ccw{s}"), + Primitive(dx, -dy, -s, cfg.forwardandturncostmult, f"fwd_turn_cw{s}"), + ] + if cfg.has_turn_in_place_prims: + prims += [ + Primitive(0.0, 0.0, 1, cfg.turninplacecostmult, "turn_in_place_ccw"), + Primitive(0.0, 0.0, -1, cfg.turninplacecostmult, "turn_in_place_cw"), + ] + if cfg.has_backward_turn_prims: + # lùi theo cung: heading quay CCW (+s) thì điểm cuối lệch về (-x, -y) — suy từ + # tích phân unicycle với tv < 0, rv > 0. + for s in cfg.turn_steps: + dx, dy = _turn_endpoint(turn_radius_m, s, cfg.numberofangles) + prims += [ + Primitive(-dx, -dy, s, cfg.backwardandturncostmult, f"bwd_turn_ccw{s}"), + Primitive(-dx, dy, -s, cfg.backwardandturncostmult, f"bwd_turn_cw{s}"), + ] + return prims + + +def build_primitive_set(cfg: RobotConfig) -> List[Primitive]: + """Sinh tập primitive body-frame theo loại kinematic.""" + if cfg.kinematic_type == KinematicType.DIFFERENTIAL: + return _nonholonomic_prims(cfg, cfg.turn_radius_m) + + if cfg.kinematic_type == KinematicType.ACKERMANN: + # validate() đã đảm bảo turn_radius_m >= min_turning_radius_m + return _nonholonomic_prims(cfg, cfg.turn_radius_m) + + if cfg.kinematic_type == KinematicType.OMNIDIRECTIONAL: + # kế thừa toàn bộ primitive nonholonomic (vẫn hợp lệ với omni)... + prims = _nonholonomic_prims(cfg, cfg.turn_radius_m) + # ...cộng khả năng đặc thù omni: đi ngang & đi chéo 45 độ giữ nguyên heading + diag = cfg.diagonal_m / math.sqrt(2.0) + prims += [ + Primitive(0.0, cfg.sidestep_m, 0, cfg.sidestepcostmult, "sidestep_left"), + Primitive(0.0, -cfg.sidestep_m, 0, cfg.sidestepcostmult, "sidestep_right"), + Primitive(diag, diag, 0, cfg.diagonalcostmult, "diag_fwd_left"), + Primitive(diag, -diag, 0, cfg.diagonalcostmult, "diag_fwd_right"), + ] + return prims + + raise ValueError(f"Chưa hỗ trợ kinematic_type={cfg.kinematic_type}") + + +# --- Tái hiện đúng phép rời rạc hóa của SBPL (environment_navxythetalat.cpp) --- + +def _contxy2disc(x: float, cellsize: float) -> int: + return int(math.floor(x / cellsize)) + + +def _conttheta2disc(theta: float, numberofangles: int) -> int: + step = 2.0 * math.pi / numberofangles + return int(round(((theta % (2.0 * math.pi)) / step))) % numberofangles + + +def _sbpl_endpose_check(poses, endcell, resolution: float, numberofangles: int): + """Đúng phép kiểm tra SBPL chạy khi nạp file: pose cuối (tính từ TÂM cell (0,0)) + phải rời rạc hóa ra đúng endcell — sai là SBPL từ chối cả file.""" + half = resolution / 2.0 + lx, ly, lth = poses[-1] + got = (_contxy2disc(half + lx, resolution), + _contxy2disc(half + ly, resolution), + _conttheta2disc(lth, numberofangles)) + return got == tuple(endcell), got + + +def generate_mprim_file(cfg: RobotConfig, kinematics_fn: Callable, output_path: str): + """Sinh file .mprim. Trả về (số primitive, list cảnh báo). + + kinematics_fn(startpt, endpt, dtheta_c, numberofangles, spacing_m, spacing_rad) + -> (List[(x, y, theta)], info_dict) + """ + warnings = list(cfg.validate()) + prims = build_primitive_set(cfg) + numberofangles = cfg.numberofangles + resolution = cfg.resolution_m + spacing_rad = math.radians(cfg.pose_angular_spacing_deg) + ang_step = 2.0 * math.pi / numberofangles + + lines = [ + f"resolution_m: {resolution:.6f}", + f"numberofangles: {numberofangles}", + f"totalnumberofprimitives: {len(prims) * numberofangles}", + ] + + worst_radius: Dict[str, float] = {} + for angleind in range(numberofangles): + currentangle = angleind * ang_step + endposes_seen: Dict[Tuple[int, int, int], str] = {} + for primid, p in enumerate(prims): + # xoay primitive (body frame) sang world frame tại góc hiện tại + dx_world = p.dx_body_m * math.cos(currentangle) - p.dy_body_m * math.sin(currentangle) + dy_world = p.dx_body_m * math.sin(currentangle) + p.dy_body_m * math.cos(currentangle) + + endx_c = int(round(dx_world / resolution)) + endy_c = int(round(dy_world / resolution)) + endtheta_c = (angleind + p.dtheta_c) % numberofangles + + # primitive suy biến: không đi, không xoay -> self-loop trong đồ thị SBPL + if endx_c == 0 and endy_c == 0 and p.dtheta_c == 0: + raise ValueError( + f"Primitive '{p.label}' quantize về (0,0,không đổi góc) tại " + f"startangle={angleind}: bước đi quá ngắn so với resolution.") + + key = (endx_c, endy_c, endtheta_c) + if key in endposes_seen: + warnings.append( + f"Trùng endpose {key} tại startangle={angleind}: '{p.label}' và " + f"'{endposes_seen[key]}' — successor thừa, tốn expansion vô ích.") + endposes_seen[key] = p.label + + startpt = (0.0, 0.0, currentangle) + endpt = (endx_c * resolution, endy_c * resolution, endtheta_c * ang_step) + poses, info = kinematics_fn(startpt, endpt, p.dtheta_c, numberofangles, + cfg.pose_spacing_m, spacing_rad) + + # --- tự kiểm các ràng buộc SBPL enforce lúc nạp file --- + heading_err = abs((poses[-1][2] - endtheta_c * ang_step + math.pi) + % (2.0 * math.pi) - math.pi) + if heading_err > 1e-6: + raise RuntimeError( + f"BUG nội bộ: heading cuối lệch {math.degrees(heading_err):.3f} độ tại " + f"'{p.label}' startangle={angleind} — interpolator phải trả heading chính xác.") + ok, got = _sbpl_endpose_check(poses, key, resolution, numberofangles) + if not ok: + raise RuntimeError( + f"BUG nội bộ: pose cuối rời rạc hóa ra {got} != endpose {key} tại " + f"'{p.label}' startangle={angleind} — SBPL sẽ từ chối file này.") + max_gap = max((math.hypot(x1 - x0, y1 - y0) + for (x0, y0, _), (x1, y1, _) in zip(poses, poses[1:])), + default=0.0) + if max_gap > resolution * 1.05: + warnings.append( + f"Bước giữa 2 pose = {max_gap:.3f}m > 1 cell tại '{p.label}' " + f"startangle={angleind}: footprint sweep có thể bỏ sót cell.") + if (cfg.kinematic_type == KinematicType.ACKERMANN + and info.get("radius_m") is not None + and info["radius_m"] < cfg.min_turning_radius_m * 0.999): + worst_radius[p.label] = min(worst_radius.get(p.label, math.inf), + info["radius_m"]) + + lines.append(f"primID: {primid}") + lines.append(f"startangle_c: {angleind}") + lines.append(f"endpose_c: {endx_c} {endy_c} {endtheta_c}") + lines.append(f"additionalactioncostmult: {p.costmult:d}") + lines.append(f"intermediateposes: {len(poses)}") + for (x, y, th) in poses: + lines.append(f"{x:.4f} {y:.4f} {th:.4f}") + + for label, r in sorted(worst_radius.items()): + warnings.append( + f"'{label}': bán kính cung sau quantize nhỏ nhất = {r:.3f}m < " + f"min_turning_radius_m = {cfg.min_turning_radius_m}m — xe thật không bám được, " + "tăng turn_radius_m để có dự phòng quantize.") + + with open(output_path, "w") as f: + f.write("\n".join(lines) + "\n") + + return len(prims) * numberofangles, warnings diff --git a/test_mprim_gen.py b/test_mprim_gen.py new file mode 100644 index 0000000..09594a4 --- /dev/null +++ b/test_mprim_gen.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 +""" +test_mprim_gen.py +------------------ +Test tự động cho generator: sinh cả 3 config mẫu ra thư mục tạm rồi kiểm tra +output theo đúng các ràng buộc SBPL enforce lúc nạp file + chất lượng hình học. + +Chạy: + python3 test_mprim_gen.py + +Các kiểm tra: + 1. Header khớp số primitive thực tế; parse trọn vẹn không dư token. + 2. additionalactioncostmult là SỐ NGUYÊN (SBPL đọc fscanf %d). + 3. Pose cuối rời rạc hóa ra đúng endpose_c (đúng phép check của + ReadinMotionPrimitive — sai là SBPL từ chối cả file). + 4. Heading cuối khớp CHÍNH XÁC góc rời rạc đích (bug l=0 cũ gây lệch 10 độ). + 5. Khoảng cách 2 pose liên tiếp <= 1.05 cell (footprint sweep không sót cell) + và ĐỀU trong từng primitive (max/min <= 1.5). + 6. Vi phạm nonholonomic (góc lệch vector di chuyển vs heading) < 9 độ với + differential/ackermann — bắt lỗi kiểu cung lùi sai hình học (28 độ cũ). + 7. Không có primitive suy biến, không trùng endpose trong cùng start angle. + 8. Config sai (costmult lẻ, spacing > resolution, ackermann turn_radius < + min_turning_radius) phải bị validate() chặn. +""" + +import math +import os +import sys +import tempfile + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from mprim_config import RobotConfig, KinematicType +from mprim_core import generate_mprim_file +from generate_mprim import KINEMATICS_MAP, load_config + +CONFIG_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "configs") +CHECKS = [] + + +def check(name, cond, detail=""): + CHECKS.append((name, cond, detail)) + status = "PASS" if cond else "FAIL" + print(f" [{status}] {name}" + (f" — {detail}" if detail and not cond else "")) + + +def parse(path): + toks = open(path).read().split() + it = iter(toks) + def nxt(): + return next(it) + assert nxt() == "resolution_m:" + res = float(nxt()) + assert nxt() == "numberofangles:" + nang = int(nxt()) + assert nxt() == "totalnumberofprimitives:" + ntot = int(nxt()) + prims = [] + while True: + try: + tag = nxt() + except StopIteration: + break + assert tag == "primID:" + pid = int(nxt()) + assert nxt() == "startangle_c:" + sa = int(nxt()) + assert nxt() == "endpose_c:" + end = (int(nxt()), int(nxt()), int(nxt())) + assert nxt() == "additionalactioncostmult:" + cm = nxt() + assert nxt() == "intermediateposes:" + n = int(nxt()) + poses = [(float(nxt()), float(nxt()), float(nxt())) for _ in range(n)] + prims.append(dict(pid=pid, sa=sa, end=end, cm=cm, poses=poses)) + return res, nang, ntot, prims + + +def ang_diff(a, b): + return (a - b + math.pi) % (2 * math.pi) - math.pi + + +def contxy2disc(x, cell): + return int(math.floor(x / cell)) + + +def verify_file(path, kinematic, min_turning_radius=None): + res, nang, ntot, prims = parse(path) + step = 2 * math.pi / nang + + check("header khớp số primitive parse được", ntot == len(prims), + f"header {ntot} != parsed {len(prims)}") + + bad_cm = [p["cm"] for p in prims if not p["cm"].lstrip("-").isdigit()] + check("costmult toàn số nguyên (fscanf %d)", not bad_cm, f"lẻ: {sorted(set(bad_cm))}") + + # phép check của SBPL ReadinMotionPrimitive + bad_end = 0 + for p in prims: + lx, ly, lth = p["poses"][-1] + got = (contxy2disc(res / 2 + lx, res), contxy2disc(res / 2 + ly, res), + int(round((lth % (2 * math.pi)) / step)) % nang) + if got != p["end"]: + bad_end += 1 + check("pose cuối rời rạc hóa đúng endpose_c (điều kiện SBPL nạp file)", bad_end == 0, + f"{bad_end} primitive sai") + + worst_heading = max(abs(ang_diff(p["poses"][-1][2], p["end"][2] * step)) for p in prims) + check("heading cuối khớp chính xác góc đích", worst_heading < 1e-4, + f"lệch tối đa {math.degrees(worst_heading):.2f} độ") + + worst_gap, worst_ratio = 0.0, 1.0 + for p in prims: + gaps = [math.hypot(x1 - x0, y1 - y0) + for (x0, y0, _), (x1, y1, _) in zip(p["poses"], p["poses"][1:])] + moving = [g for g in gaps if g > 1e-9] + if moving: + worst_gap = max(worst_gap, max(moving)) + worst_ratio = max(worst_ratio, max(moving) / min(moving)) + check("bước giữa 2 pose <= 1.05 cell (footprint sweep không sót)", + worst_gap <= res * 1.05, f"max {worst_gap:.4f}m") + check("spacing đều trong từng primitive (max/min <= 1.5)", worst_ratio <= 1.5, + f"tỉ lệ {worst_ratio:.2f}") + + if kinematic in (KinematicType.DIFFERENTIAL, KinematicType.ACKERMANN): + worst_nh = 0.0 + for p in prims: + if p["end"][0] == 0 and p["end"][1] == 0: + continue + for (x0, y0, t0), (x1, y1, t1) in zip(p["poses"], p["poses"][1:]): + dx, dy = x1 - x0, y1 - y0 + if math.hypot(dx, dy) < 1e-9: + continue + hd = math.atan2(dy, dx) + th = 0.5 * (t0 + t1) + worst_nh = max(worst_nh, min(abs(ang_diff(hd, th)), + abs(ang_diff(hd, th + math.pi)))) + check("vi phạm nonholonomic < 9 độ", math.degrees(worst_nh) < 9.0, + f"max {math.degrees(worst_nh):.2f} độ") + + degenerate = [p for p in prims + if p["end"] == (0, 0, p["sa"])] + check("không có primitive suy biến (0,0,không đổi góc)", not degenerate, + f"{len(degenerate)} cái") + + dup = 0 + for sa in range(nang): + seen = set() + for p in prims: + if p["sa"] != sa: + continue + if p["end"] in seen: + dup += 1 + seen.add(p["end"]) + check("không trùng endpose trong cùng start angle", dup == 0, f"{dup} cặp trùng") + + +def test_configs_generate(): + with tempfile.TemporaryDirectory() as tmp: + for fname in ("differential.py", "omnidirectional.py", "ackermann.py"): + cfg = load_config(os.path.join(CONFIG_DIR, fname)) + out = os.path.join(tmp, cfg.name + ".mprim") + n, warnings = generate_mprim_file(cfg, KINEMATICS_MAP[cfg.kinematic_type], out) + print(f"\n== {fname}: {n} primitives, {len(warnings)} cảnh báo ==") + for w in warnings: + print(f" (warn) {w}") + verify_file(out, cfg.kinematic_type, + min_turning_radius=getattr(cfg, "min_turning_radius_m", None)) + + +def test_validation_rejects_bad_configs(): + print("\n== validate() phải chặn config sai ==") + + def must_fail(name, **overrides): + base = dict(name="t", kinematic_type=KinematicType.DIFFERENTIAL) + base.update(overrides) + try: + RobotConfig(**base).validate() + check(name, False, "validate() đã KHÔNG chặn") + except ValueError: + check(name, True) + + must_fail("chặn costmult không nguyên", forwardandturncostmult=2.5) + must_fail("chặn costmult < 1", backwardcostmult=0) + must_fail("chặn pose_spacing_m > resolution", pose_spacing_m=0.08) + must_fail("chặn forward quá ngắn (quantize về 0,0)", forward_short_m=0.02) + must_fail("chặn turn_steps rỗng", turn_steps=()) + must_fail("chặn ackermann turn_radius < min_turning_radius", + kinematic_type=KinematicType.ACKERMANN, + min_turning_radius_m=0.45, turn_radius_m=0.30) + + +if __name__ == "__main__": + test_configs_generate() + test_validation_rejects_bad_configs() + failed = [c for c in CHECKS if not c[1]] + print(f"\n{'='*60}\nTổng: {len(CHECKS)} check, {len(failed)} FAIL") + sys.exit(1 if failed else 0) diff --git a/visualize_mprim.py b/visualize_mprim.py new file mode 100644 index 0000000..f9cf395 --- /dev/null +++ b/visualize_mprim.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 +""" +visualize_mprim.py +------------------- +Vẽ nhanh tất cả primitive tại 1 góc bắt đầu (startangle) để kiểm tra bằng mắt +trước khi tích hợp vào planner thật. + +Cách dùng: + python3 visualize_mprim.py diff.mprim --angle 0 + python3 visualize_mprim.py omni.mprim --angle 4 +""" + +import argparse +import matplotlib +matplotlib.use("Agg") +import matplotlib.pyplot as plt + + +def parse_mprim(path): + with open(path) as f: + lines = [l.strip() for l in f.readlines()] + + i = 0 + resolution = float(lines[i].split(":")[1]); i += 1 + numberofangles = int(lines[i].split(":")[1]); i += 1 + total = int(lines[i].split(":")[1]); i += 1 + + prims = [] + while i < len(lines) and lines[i]: + primid = int(lines[i].split(":")[1]); i += 1 + startangle = int(lines[i].split(":")[1]); i += 1 + endpose = lines[i].split(":")[1].split(); i += 1 + costmult = float(lines[i].split(":")[1]); i += 1 + n = int(lines[i].split(":")[1]); i += 1 + poses = [] + for _ in range(n): + x, y, th = map(float, lines[i].split()) + poses.append((x, y, th)) + i += 1 + prims.append({ + "primid": primid, "startangle": startangle, + "endpose_c": [int(v) for v in endpose], + "costmult": costmult, "poses": poses + }) + return resolution, numberofangles, prims + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("mprim_file") + ap.add_argument("--angle", type=int, default=0, help="startangle_c cần vẽ") + ap.add_argument("-o", "--output", default=None, help="file ảnh output (mặc định: _angleN.png)") + args = ap.parse_args() + + resolution, numberofangles, prims = parse_mprim(args.mprim_file) + subset = [p for p in prims if p["startangle"] == args.angle] + + if not subset: + print(f"Không tìm thấy primitive nào với startangle_c={args.angle}") + return + + import math + + fig, ax = plt.subplots(figsize=(6, 6)) + tick = resolution * 0.4 # độ dài vạch heading tại mỗi pose + for p in subset: + xs = [pt[0] for pt in p["poses"]] + ys = [pt[1] for pt in p["poses"]] + line, = ax.plot(xs, ys, marker="o", markersize=2, + label=f'id{p["primid"]} (cost {p["costmult"]:g})') + # vạch heading: kiểm tra bằng mắt heading có bám tangent quỹ đạo không + # (bug kiểu "heading cuối lệch 10 độ" hiện rõ ở đây) + for (x, y, th) in p["poses"]: + ax.plot([x, x + tick * math.cos(th)], [y, y + tick * math.sin(th)], + color=line.get_color(), linewidth=0.6, alpha=0.6) + ax.annotate(str(p["primid"]), (xs[-1], ys[-1]), fontsize=8) + + ax.set_aspect("equal") + ax.grid(True) + ax.set_title(f"Primitives tại startangle_c={args.angle} / {numberofangles} (resolution={resolution}m)") + ax.set_xlabel("x (m)") + ax.set_ylabel("y (m)") + ax.legend(fontsize=7, loc="upper left", bbox_to_anchor=(1.02, 1)) + plt.tight_layout() + + out = args.output or args.mprim_file.replace(".mprim", f"_angle{args.angle}.png") + plt.savefig(out, dpi=130) + print(f"[OK] Đã lưu hình: {out}") + + +if __name__ == "__main__": + main() \ No newline at end of file