ROS MoveIt! 反運動學 (Inverse Kinematics, IK)

 

ROS MoveIt! 反運動學 (Inverse Kinematics, IK) 及路徑規劃實作指南

📌 1️⃣ 什麼是手臂反運動學 (Inverse Kinematics, IK)？

🔹 什麼是運動學 (Kinematics)？

• 正運動學 (Forward Kinematics, FK): 給定關節角度 (Joint Angles) 計算出機械手臂的末端位置 (End Effector Position)。
• 反運動學 (Inverse Kinematics, IK): 給定末端位置，反算機械手臂各關節應該設定的角度。

🔹 為何需要 IK？

• 手臂需要移動到特定座標時 (如抓取物件)，我們不能手動設定每個關節角度，因此使用 IK 計算最佳關節角度。
• IK 可用於機械手臂運動規劃，如抓取、避障等應用。

🔹 ROS MoveIt! 提供哪些 IK 演算法？

MoveIt! 支援多種 IK 解算器 (IK Solver)，例如：

IK 解算器	特色
KDL (Kinematics and Dynamics Library)	適用於樞軸型機械手臂 (Revolute Joints)，精度高但速度較慢
IKFast (OpenRAVE)	使用代數運算符，計算速度極快，但需要專門生成
TRAC-IK	比 KDL 更快，更適用於多關節手臂
MoveIt! Default Solver	MoveIt! 內建的 IK Solver，適用於一般機械手臂

---

📌 2️⃣ 使用 MoveIt! 模擬機械手臂運動

MoveIt! 是 ROS 內建的運動規劃框架，可以： ✅ 規劃機械手臂的運動路徑 (Path Planning)
✅ 執行反運動學 (IK) 計算
✅ 避障 (Collision Avoidance)

---

🚀 3️⃣ MoveIt! + Gazebo + Rviz 遙控手臂運動

📌 安裝 MoveIt! (如果尚未安裝)

bash
sudo apt update
sudo apt install ros-noetic-moveit

📌 啟動 MoveIt! 及 Rviz

bash
roslaunch panda_moveit_config demo_gazebo.launch

✅ 這將會開啟 Rviz，並顯示機械手臂，可以透過 GUI 進行操控

---

📌 4️⃣ 使用 MoveIt! 控制機械手臂

🔹 1️⃣ 透過 Rviz 手動控制手臂

1️⃣ 開啟 Rviz

bash
roslaunch panda_moveit_config demo.launch

2️⃣ 選擇 MotionPlanning Plugin 3️⃣ 選擇 Interactive Marker，拖動機械手臂到目標位置 4️⃣ 點擊 Plan (規劃) 5️⃣ 點擊 Execute (執行)

✅ 機械手臂會根據 MoveIt! 的 IK 計算，調整關節角度來達成目標位置

---

🔹 2️⃣ 透過 Python 控制機械手臂

📌 Python 範例：讓機械手臂移動到特定位置

python
#!/usr/bin/env python

import sys
import rospy
import moveit_commander

# 初始化 MoveIt!
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_arm_control', anonymous=True)

# 初始化機械手臂
arm = moveit_commander.MoveGroupCommander("panda_arm")

# 設定目標位置 (末端點座標)
target_pose = arm.get_current_pose().pose
target_pose.position.x = 0.5
target_pose.position.y = 0.2
target_pose.position.z = 0.4

# 設定新的目標位置
arm.set_pose_target(target_pose)

# 計算路徑並執行
plan = arm.go(wait=True)
arm.stop()
arm.clear_pose_targets()

# 結束
moveit_commander.roscpp_shutdown()

📌 執行程式

bash
rosrun panda_moveit_config moveit_arm_control.py

✅ 機械手臂應該會自動移動到指定位置

---

📌 5️⃣ MoveIt! 控制器規劃 & 障礙物建置

🔹 1️⃣ 加入障礙物 (Collision Object)

在 MoveIt! 裡，我們可以建立 障礙物 (Collision Object)，讓機械手臂避開障礙物進行運動規劃。

📌 Python 程式碼 (加入障礙物)

python
from moveit_commander import PlanningSceneInterface

scene = PlanningSceneInterface()
rospy.sleep(1)

# 添加桌面障礙物
scene.add_box("table", pose_stamped, size=(1.5, 1.0, 0.5))

# 添加障礙牆
scene.add_box("wall", pose_stamped, size=(0.2, 1.0, 1.0))

📌 執行程式

bash
rosrun panda_moveit_config add_obstacles.py

✅ 在 Rviz 中應該會顯示桌子與障礙牆，機械手臂會避開這些物體

---

📌 6️⃣ MoveIt! 設定不同的 IK 解算器

如果你的機械手臂需要更快的 IK 計算，可以選擇不同的 IK Solver！

🔹 切換 IKFast 解算器

📌 修改 MoveIt! 的 kinematics.yaml

bash
nano ~/catkin_ws/src/panda_moveit_config/config/kinematics.yaml

✅ 設定 IKFast 作為解算器

yaml
panda_arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05

📌 重新啟動 MoveIt!

bash
roslaunch panda_moveit_config demo.launch

✅ 你的機械手臂現在會使用 IKFast 來加速 IK 計算！

---

🚀 7️⃣ 總結

功能	指令 / 設定
安裝 MoveIt!	sudo apt install ros-noetic-moveit
啟動 Rviz 控制機械手臂	roslaunch panda_moveit_config demo.launch
Python 控制機械手臂	rosrun panda_moveit_config moveit_arm_control.py
設定 IKFast 解算器	修改 kinematics.yaml
加入障礙物	rosrun panda_moveit_config add_obstacles.py

---

🚀 透過 MoveIt! 進行 IK 控制與運動規劃，可以大幅提升機械手臂的自主運作能力！
💡 如果有特定需求 (如手臂避障、夾爪控制)，可以進一步優化 MoveIt! 的規劃參數！ 🚀