(Robot_arm)Python (rospy) 來控制機械手臂的關節 (joint)

 在 ROS (Robot Operating System) 中，使用 Python (rospy) 來控制機械手臂的關節 (joint)，

通常透過 joint_trajectory_controller 來發送 關節軌跡 (Joint Trajectory) 命令。

---

1. 使用 rospy 發送關節位置命令

這是一個基本範例，透過 trajectory_msgs/JointTrajectory 控制機械手臂的關節運動。

📌 需求

1. 機械手臂必須已經運行 joint_trajectory_controller (通常是 robot_arm_controller)。
2. 安裝 rospy (ROS 的 Python 介面)：

   bash
   sudo apt install ros-noetic-rospy
   

---

2. Python 控制關節的完整範例

python
#!/usr/bin/env python
import rospy
import time
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

def move_joint(joint_names, positions, duration=2.0):
    """
    發送 Joint Trajectory 命令，讓機械手臂移動到指定位置
    :param joint_names: 關節名稱列表 (list)
    :param positions: 關節目標角度 (list)
    :param duration: 運動持續時間 (秒)
    """
    pub = rospy.Publisher('/robot_arm_controller/command', JointTrajectory, queue_size=10)
    rospy.init_node('joint_trajectory_publisher', anonymous=True)
    
    # 等待 Publisher 準備好
    while pub.get_num_connections() == 0 and not rospy.is_shutdown():
        rospy.loginfo("等待 joint_trajectory_controller 連接...")
        time.sleep(1)
    
    # 創建 JointTrajectory 訊息
    traj_msg = JointTrajectory()
    traj_msg.joint_names = joint_names  # 設定關節名稱

    # 設定關節目標位置
    point = JointTrajectoryPoint()
    point.positions = positions  # 目標角度
    point.time_from_start = rospy.Duration(duration)  # 運行時間
    
    traj_msg.points.append(point)

    # 發送訊息
    rospy.loginfo("發送 Joint Trajectory 指令: %s", positions)
    pub.publish(traj_msg)

if __name__ == '__main__':
    try:
        # 設定機械手臂的關節名稱
        joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"]
        
        # 目標位置 (角度，單位: 弧度)
        target_positions = [0.0, -0.5, 1.0, -1.5, 0.5, 0.0] 
        
        move_joint(joint_names, target_positions, duration=3.0)

        rospy.loginfo("關節運動完成")
    except rospy.ROSInterruptException:
        rospy.logwarn("程序中斷")

---

3. 代碼解析

🔹 主要流程

1. 初始化 ROS 節點

   python
   rospy.init_node('joint_trajectory_publisher', anonymous=True)
   

   • 啟動一個 ROS 節點 (joint_trajectory_publisher)。

2. 建立 Publisher，發送 JointTrajectory 訊息

   python
   pub = rospy.Publisher('/robot_arm_controller/command', JointTrajectory, queue_size=10)
   

   • 發送關節命令到 /robot_arm_controller/command。
   • 確保控制器 (joint_trajectory_controller) 已啟動並訂閱該 Topic。

3. 設定關節名稱

   python
   traj_msg.joint_names = joint_names
   

   • 需要與機械手臂的關節名稱匹配 (joint_1, joint_2, ... joint_6)。

4. 設置關節目標位置

   python
   point.positions = positions
   point.time_from_start = rospy.Duration(duration)
   

   • 設定每個關節的目標位置 (positions，單位為 弧度 rad)。
   • 設定關節達到目標所需的時間 (duration)。

5. 發送訊息

   python
   pub.publish(traj_msg)
   

   • 發送 Joint Trajectory 訊息，機械手臂將會在 duration 秒內移動到 positions。

---

4. 運行方式

1️⃣ 啟動 ROS 與機械手臂控制器

在 ROS workspace 內運行：

bash
roslaunch robot_arm_urdf robot_arm_control.launch

⚠️ 確保 joint_trajectory_controller 已啟動！ 可以使用以下指令檢查：

bash
rostopic list

應該會看到 /robot_arm_controller/command 這個 topic 存在。

---

2️⃣ 運行 Python 控制腳本

bash
chmod +x move_joint.py  # 讓腳本變成可執行
./move_joint.py

或者：

bash
rosrun robot_arm_urdf move_joint.py

---

5. 進階應用

🔹 1. 逐步運動 (多個點)

可以讓機械手臂經過多個目標點：

python
traj_msg.points = [
    JointTrajectoryPoint(positions=[0.0, -0.5, 1.0, -1.5, 0.5, 0.0], time_from_start=rospy.Duration(2.0)),
    JointTrajectoryPoint(positions=[0.5, -0.3, 0.8, -1.2, 0.3, 0.2], time_from_start=rospy.Duration(4.0)),
    JointTrajectoryPoint(positions=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start=rospy.Duration(6.0))
]

這樣機械手臂會先移動到第一個點，然後移動到第二個點，最後回到初始位置。

---

🔹 2. 使用 velocity 控制運動速度

python
point.velocities = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5]  # 設定速度 (rad/s)

這樣可以設定關節的移動速度。

---

6. 總結

功能	說明
控制方式	joint_trajectory_controller
指令類型	JointTrajectory
控制模式	位置控制 (可擴展為速度控制)
運行方式	Python (rospy)
適用環境	Gazebo 模擬 或 真實機械手臂

這個方法適合： ✅ 控制機械手臂的運動軌跡
✅ 模擬 Gazebo 內的關節運動
✅ 用於 MoveIt! 或 ROS 控制器

如果你需要速度控制 (velocity)、力矩控制 (effort)，

或更進階的 MoveIt! 控制，可以擴展這個腳本！🚀