TrajOpt_Implementation_Guide.md

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.