# 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.