UR机械臂学习(8):Python实现机械臂运动控制(不使用MoveIt)

时间:2024-03-12 16:51:37

以下是在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