| # TrajOpt Implementation Guide (Robot Franka Panda) |
|
|
| ## 1. Mục tiêu |
|
|
| Tài liệu này chỉ đặc tả cách cài đặt **TrajOpt** cho bài toán của hệ thống: |
|
|
| - Đầu vào: |
| - Trạng thái khớp hiện tại `q_start` |
| - Grasp pose `T_grasp` (đã được module khác xác định) |
| - Target placement pose `T_target` |
| - Robot model (URDF) |
| - Collision meshes |
| - Môi trường |
| - Đầu ra: |
| - Quỹ đạo khớp liên tục từ `q_start -> grasp -> target`. |
|
|
| TrajOpt **không** sinh grasp pose. |
| TrajOpt **không** giải IK. |
| TrajOpt chỉ tối ưu quỹ đạo. |
|
|
| --- |
|
|
| # 2. Biến tối ưu |
|
|
| Mỗi waypoint: |
|
|
| `q_t = [q1,...,q7]` |
|
|
| Toàn bộ trajectory: |
|
|
| `Q=[q1,q2,...,qT]` |
|
|
| Nếu T=30 thì số biến là 210. |
|
|
| TrajOpt tối ưu đồng thời toàn bộ waypoint. |
|
|
| --- |
|
|
| # 3. Khởi tạo quỹ đạo |
|
|
| Không sử dụng random, RRT hay PRM. |
|
|
| Khởi tạo bằng nội suy tuyến tính: |
|
|
| - q_start -> q_grasp |
| - q_grasp -> q_target |
|
|
| Quỹ đạo ban đầu được phép xuyên vật cản. |
|
|
| --- |
|
|
| # 4. Hàm mục tiêu |
|
|
| Mục tiêu chính: |
|
|
| J = Σ ||q(t+1)-q(t)||² |
|
|
| Có thể bổ sung acceleration cost nếu cần quỹ đạo mượt hơn. |
|
|
| --- |
|
|
| # 5. Hard Constraints |
|
|
| ## Joint limits |
|
|
| Mọi waypoint đều phải thỏa: |
|
|
| q_min ≤ q_t ≤ q_max |
| |
| Giới hạn lấy từ URDF. |
| |
| ## Grasp constraint |
| |
| Waypoint grasp phải thỏa: |
| |
| FK(q_k)=T_grasp |
| |
| Đây là hard equality constraint. |
| |
| ## Placement constraint |
| |
| Waypoint cuối: |
| |
| FK(q_T)=T_target |
| |
| Đây cũng là hard equality constraint. |
| |
| --- |
| |
| # 6. Collision World |
| |
| TrajOpt không đọc trực tiếp PyBullet. |
| |
| Cần xây dựng Collision World gồm: |
| |
| - Robot collision mesh |
| - Object collision mesh |
| - Table |
| - Ground |
| - Static obstacles |
| |
| Mỗi object cần: |
| |
| - Mesh |
| - World pose |
| - Collision margin |
| - AABB |
| |
| Trước mỗi lần planning phải đồng bộ pose từ PyBullet sang Collision World. |
| |
| --- |
| |
| # 7. Continuous Collision Checking |
| |
| Không chỉ kiểm tra tại waypoint. |
| |
| TrajOpt kiểm tra chuyển động giữa hai waypoint liên tiếp. |
| |
| Đối với mỗi robot link: |
| |
| - xây dựng swept volume |
| - tính signed distance |
| - lấy contact normal |
| - lấy closest points |
| |
| Nếu signed distance < d_safe thì sinh gradient đẩy quỹ đạo ra xa vật cản. |
|
|
| --- |
|
|
| # 8. Tuyến tính hóa |
|
|
| Khoảng cách va chạm được tuyến tính hóa quanh nghiệm hiện tại: |
|
|
| sd(q) ≈ sd(q0)+∇sd(q0)(q-q0) |
|
|
| Gradient được tính từ: |
|
|
| - contact normal |
| - robot Jacobian |
|
|
| Sau đó chuyển thành ràng buộc tuyến tính trong bài toán QP. |
|
|
| --- |
|
|
| # 9. SQP |
|
|
| Mỗi iteration: |
|
|
| 1. Forward Kinematics |
| 2. Collision Query |
| 3. Jacobian |
| 4. Linearization |
| 5. Xây dựng Quadratic Program |
| 6. Giải QP |
| 7. Cập nhật trajectory |
| 8. Kiểm tra hội tụ |
|
|
| Thông thường 10–30 iteration. |
|
|
| --- |
|
|
| # 10. Điều kiện hội tụ |
|
|
| Thuật toán dừng khi: |
|
|
| - Không còn vi phạm khoảng cách an toàn. |
| - Các hard equality constraints được thỏa mãn trong giới hạn chính xác số học của solver. |
| - Chi phí thay đổi giữa hai iteration nhỏ hơn ngưỡng. |
| - Trust region không còn cải thiện nghiệm. |
|
|
| Nếu không hội tụ trước số iteration tối đa thì planner trả về thất bại. |
|
|
| --- |
|
|
| # 11. Lưu ý |
|
|
| - TrajOpt không tìm grasp pose. |
| - TrajOpt không giải IK. |
| - IK phải được giải trước. |
| - Collision World phải được cập nhật trước mỗi lần lập kế hoạch. |
| - Khi attach object, collision model phải cập nhật để quỹ đạo sau grasp tính cả vật được gắp. |
|
|
