UR機械臂學習（8）：Python實現機械臂運動控制（不使用MoveIt）

以下是在gazebo仿真中使用，如果是控制真實機械臂，只需要修改訂閱的話題即可

后期肯定會自己進行軌跡算法規划，用moveit的話想要更換算法太麻煩，所以嘗試自己寫程序不通過moveit來控制機械臂。

本文參考：https://blog.csdn.net/qq_42823342/article/details/116236950?spm=1001.2014.3001.5501

首先，啟動gazebo；
其次，運行rostopic list，查看當前運行話題

其中，/pos_joint_traj_controller/follow_joint_trajectory就是控制ur運動的話題，顯然，這是 action 通信。

然后，使用rostopic type /pos_joint_traj_controller/follow_joint_trajectory/goal 命令，
找出該話題的消息類型為control_msgs/FollowJointTrajectoryActionGoal

然后，使用rosmsg show control_msgs/FollowJointTrajectoryActionGoal命令，
找出該消息具體包含的數據類型如下：
其中，points的部分是我們需要關心注意的

在使用moveIt進行機械臂控制時，其軌跡格式為：
（這個和上面那個是一樣的格式）

最后，新建一個python腳本，然后運行即可

#!/usr/bin/python
# -*- coding: utf-8 -*-

# 參考 https://blog.csdn.net/fengyu19930920/article/details/81144042

from trajectory_msgs.msg import *
from control_msgs.msg import *
import rospy
import actionlib
from sensor_msgs.msg import JointState

JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']

def move():
#goal就是我們向發送的關節運動數據，實例化為FollowJointTrajectoryGoal()類
goal = FollowJointTrajectoryGoal()

#goal當中的trajectory就是我們要操作的，其余的Header之類的不用管
goal.trajectory = JointTrajectory()
#goal.trajectory底下一共還有兩個成員，分別是joint_names和points，先給joint_names賦值
goal.trajectory.joint_names = JOINT_NAMES

#從joint_state話題上獲取當前的關節角度值，因為后續要移動關節時第一個值要為當前的角度值
joint_states = rospy.wait_for_message("joint_states",JointState)
joints_pos = joint_states.position

#給trajectory中的第二個成員points賦值
#points中有四個變量，positions,velocities,accelerations,effort，我們給前三個中的全部或者其中一兩個賦值就行了
goal.trajectory.points=[0]*4
goal.trajectory.points[0]=JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6,time_from_start=rospy.Duration(0.0))
goal.trajectory.points[1]=JointTrajectoryPoint(positions=[0.5,0,-0.5,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(1.0))
goal.trajectory.points[2]=JointTrajectoryPoint(positions=[1,0,-1,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(2.0))
goal.trajectory.points[3]=JointTrajectoryPoint(positions=[1.57,0,-1.57,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(3.0))

#發布goal，注意這里的client還沒有實例化，ros節點也沒有初始化，我們在后面的程序中進行如上操作
client.send_goal(goal)
client.wait_for_result()

def pub_test():
global client

#初始化ros節點
rospy.init_node("pub_action_test")

#實例化一個action的類，命名為client，與上述client對應，話題為arm_controller/follow_joint_trajectory，消息類型為FollowJointTrajectoryAction
client = actionlib.SimpleActionClient('/pos_joint_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
print("Waiting for server...")
#等待server
client.wait_for_server()
print("Connect to server")

#執行move函數，發布action
move()

if __name__ == "__main__":
pub_test()

#!/usr/bin/python
# -*- coding: utf-8 -*-

# 參考  https://blog.csdn.net/fengyu19930920/article/details/81144042

from trajectory_msgs.msg import *
from control_msgs.msg import *
import rospy
import actionlib
from sensor_msgs.msg import JointState
 
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
               'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
 
def move():
    #goal就是我們向發送的關節運動數據，實例化為FollowJointTrajectoryGoal()類
    goal = FollowJointTrajectoryGoal()

    #goal當中的trajectory就是我們要操作的，其余的Header之類的不用管
    goal.trajectory = JointTrajectory()
    #goal.trajectory底下一共還有兩個成員，分別是joint_names和points，先給joint_names賦值
    goal.trajectory.joint_names = JOINT_NAMES

    #從joint_state話題上獲取當前的關節角度值，因為后續要移動關節時第一個值要為當前的角度值
    joint_states = rospy.wait_for_message("joint_states",JointState)
    joints_pos = joint_states.position

    #給trajectory中的第二個成員points賦值
    #points中有四個變量，positions,velocities,accelerations,effort，我們給前三個中的全部或者其中一兩個賦值就行了
    goal.trajectory.points=[0]*4
    goal.trajectory.points[0]=JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6,time_from_start=rospy.Duration(0.0))
    goal.trajectory.points[1]=JointTrajectoryPoint(positions=[0.5,0,-0.5,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(1.0))
    goal.trajectory.points[2]=JointTrajectoryPoint(positions=[1,0,-1,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(2.0))
    goal.trajectory.points[3]=JointTrajectoryPoint(positions=[1.57,0,-1.57,0,0,0], velocities=[0]*6,time_from_start=rospy.Duration(3.0))
    
    #發布goal，注意這里的client還沒有實例化，ros節點也沒有初始化，我們在后面的程序中進行如上操作
    client.send_goal(goal)
    client.wait_for_result()
 
def pub_test():
    global client

    #初始化ros節點
    rospy.init_node("pub_action_test")

    #實例化一個action的類，命名為client，與上述client對應，話題為arm_controller/follow_joint_trajectory，消息類型為FollowJointTrajectoryAction
    client = actionlib.SimpleActionClient('/pos_joint_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    print("Waiting for server...")
    #等待server
    client.wait_for_server()
    print("Connect to server")

    #執行move函數，發布action
    move()
 
if __name__ == "__main__":
    pub_test()

 

注意：

新建的python程序需設置為可執行文件，然后通過rosrun [功能包名] [文件名] 或 python [文件名]運行從程序
控制真實機械臂，需要修改話題名，將/pos_joint_traj_controller/follow_joint_trajectory改為/scaled_pos_joint_traj_controller/follow_joint_trajectory
————————————————
版權聲明：本文為CSDN博主「咕咚咚」的原創文章，遵循CC 4.0 BY-SA版權協議，轉載請附上原文出處鏈接及本聲明。
原文鏈接：https://blog.csdn.net/gyxx1998/article/details/119351029