目录
有错误或没说清楚的地方欢迎评论指正,我会修改的,谢谢大家
apt安装必要组件
先二进制安装一堆必要的包,不安装的话后面编译会缺东西报错。
不用参考:http://t.csdnimg.cn/WvSJS
那样找文件复制进来,这样本质就是编译安装了,但其实也是可以二进制安装的,这里我准备了apt安装大礼包一步到位:
sudo apt-get install ros-noetic-ur-client-library ros-noetic-ur-msgs ros-noetic-scaled-joint-trajectory-controller ros-noetic-joint-trajectory-controller ros-noetic-speed-scaling-interface ros-noetic-speed-scaling-state-controller ros-noetic-pass-through-controllers ros-noetic-industrial-robot-status-interface ros-noetic-moveit ros-noetic-effort-controllers ros-noetic-rqt-controller-manager ros-noetic-moveit-visual-tools ros-noetic-rviz-visual-tools ros-noetic-trac-ik-kinematics-plugin ros-noetic-ompl ros-noetic-moveit-planners-ompl
下面是大礼包的组成:
ur的ros包编译通过的必要组件:
sudo apt-get install ros-noetic-ur-client-library
sudo apt-get install ros-noetic-ur-msgs
sudo apt-get install ros-noetic-scaled-joint-trajectory-controller
sudo apt-get install ros-noetic-joint-trajectory-controller
sudo apt-get install ros-noetic-speed-scaling-interface
sudo apt-get install ros-noetic-speed-scaling-state-controller
sudo apt-get install ros-noetic-pass-through-controllers
sudo apt-get install ros-noetic-industrial-robot-status-interface
sudo apt-get install ros-noetic-moveit
下面是后续rviz和moveit的必要组件:
sudo apt-get install ros-noetic-effort-controllers
sudo apt-get install ros-noetic-rqt-controller-manager
sudo apt-get install ros-noetic-moveit-visual-tools
sudo apt-get install ros-noetic-rviz-visual-tools
sudo apt-get install ros-noetic-trac-ik-kinematics-plugin
sudo apt-get install ros-noetic-ompl
sudo apt-get install ros-noetic-moveit-planners-ompl
这里注意,如果后续要在moveit的ompl中添加自己的算法,可以编译安装moveit,我这里不涉及修改,所以moveit就使用二进制安装了(后续修改可以随时sudo apt-get remove moveit卸载并重新使用下载编译进行安装)。
下载ur的ros包
到工作空间目录下,下载ur的ros包1和ros包2,全部下载好之后编译:
cd ~/catkin_RealSense_ws/src
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git
git clone -b noetic https://github.com/ros-industrial/universal_robot.git
cd ..
catkin_make
缺东西报错的情况(只是示例,moveit我们之前已经安装了):
一般这种显示ros缺什么就安装什么,
sudo apt install ros-noetic-xxx
比如这里显示缺moveit_core,那就使用如下命令进行安装(注意下划线_变为-)
sudo apt install ros-noetic-moveit-core
或者直接安装整个组件(core只是其中一部分)
sudo apt install ros-noetic-moveit
ur5通讯连接(external control已有)
这里需要先在机器人面板中添加一个控制程序external control,但是我这里已经有前人导入过了,具体的操作我也不明白所以也不好写。(我也比较懒,不想再搞一次)看教程好像也是要用个u盘导入进去即可。上面几个链接各位看一下吧。
虚拟机修改网络连接方式
如果是虚拟机的话,把网络连接修改为桥接模式。
开启ur5并用网线连接电脑,确认插入的是哪个网口
在那个网口手动设置ip地址
首先可以查看机器人的ip地址,一般为192.168.x.1,不同路由器有不同的x值,这个即为网关地址,子网掩码填255.255.255.0,最后是设置的地址192.168.x.yyy,这里的yyy可以随便填,但是最好填大一些,较小的值可能会和局域网内其他上网的设备ip冲突,我这里填192.168.0.114,记为地址1。
ur设置自身网络
设置机器人-网络-ip地址 填为 192.168.x.zzz,这里的zzz也可以随便填,我这里填192.168.0.101,记为地址2
ur设置External Control网络
为机器人编程-加载程序-打开externalcontrol.urp-安装设置-External control-设置ip地址为192.168.0.114,端口设置为50002
先不要运行,等ros节点开启后再点运行。
确保安装设置中的Ethernet/IP为禁用状态。
确认连接状况
ping 地址2
ping 192.168.0.101
如果有反应那就是连接成功
启动ros连接节点和示教器程序
roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=地址2 limited:=true
roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=192.168.0.101 limited:=true
在示教器上,安装设置左边-程序-点击下面的播放(启动),这时应该不会有报错
继续启动ros节点
启动moveit+启动rviz:
roslaunch ur5_moveit_config moveit_planning_execution.launch limited:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true
总共需要开三个终端。
Rviz设置
在rviz中,Fixed Frame改为base_link或base。点击add-Motion_Planning确认。
把Planning Group改为manipulator。
如果发现设置为manipulator没有出现小球的话,那就是少安装下面这个东西了
sudo apt-get install ros-noetic-trac-ik-kinematics-plugin
参考:http://t.csdnimg.cn/Sbm4x
至此,所有设置完毕
实际测试(注意安全,必要时按急停键)
拖动球(不建议,关节会乱)或拉动joints中的关节值之后,点击plan查看预期轨迹,点击execute执行轨迹
直接控制机械臂不经过moveit的方法(注意安全)
在之前自己创建的ros包中添加demo01.py(这边建议用vscode)
#! /usr/bin/env python
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']
Positions = [
[1.57079632679490, -1.57079632679490, 0, 0, 0, 0],
[1.57079632679490, -1.57079632679490, 1.57, 0, 0, 0],
[0.535223679329313, -1.38916449768429, 0.0707966926333014, 0.00992183797719428, 0.207852467763008, -0.524655836120612],
[0.446489423955664, -1.42714661245728, 0.294078481730749, 0.0980603444067962, -0.0256790257130683, -0.607041204841708],
[-0.0216574244814170, -1.40521223969719, 0.402378963072291, -0.0189799772752150, -0.112174110863272, -0.803850090823849],
[-0.531253055104619, -1.50586863685589, 0.398415734706829, 0.262154054243566, -0.0905583415373497, -0.714059925812534],
[-1.05374250471503, -1.53850223076636, 0.198166237055209, 0.130391643533963, -0.0450424122672318, -0.355163102768306],
[-1.00600034209831, -1.51992796220248, 0.184870711478269, 0.322985701688979, 0.242597136147687, -0.336522962390776],
[-1.38004123386078, -1.43510968807964, 0.216763030854692, 0.267613089847475, 0.121854942204861, 0.0131483190630100],
[-1.57079632679490, -1.57079632679490, 1.57, 0, 0, 0],
[-1.57079632679490, -1.57079632679490, 0, 0, 0, 0]
]
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
print("here")
#给trajectory中的第二个成员points赋值
#points中有四个变量,positions,velocities,accelerations,effort,我们给前三个中的全部或者其中一两个赋值就行了
#goal.trajectory.points=[0]*3
goal.trajectory.points=[0]*(len(Positions)+1)
goal.trajectory.points[0]=JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6,time_from_start=rospy.Duration(0.0))
for i in range(len(Positions)):
goal.trajectory.points[i+1]=JointTrajectoryPoint(positions=Positions[i], velocities=[0]*6,time_from_start=rospy.Duration(float((i+1)*3)))
#print(i)
#goal.trajectory.points[1]=JointTrajectoryPoint(positions=[0,-1.58,0,-1.58,0,0], velocities=[0]*6,time_from_start=rospy.Duration(3.0))
#goal.trajectory.points[2]=JointTrajectoryPoint(positions=[0,-0.785,-0.785,-1.58,0,0], velocities=[0]*6,time_from_start=rospy.Duration(6.0))
#goal.trajectory.points[3]=JointTrajectoryPoint(positions=[1.5,-1.58,-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对应,话题为/pos_joint_traj_controller/follow_joint_trajectory,消息类型为FollowJointTrajectoryAction
#gazebo仿真 pos_joint_traj_controller
#client = actionlib.SimpleActionClient('/pos_joint_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
#真实机械臂 scaled_pos_joint_traj_controller
client = actionlib.SimpleActionClient('/scaled_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__":
count = 0
while not rospy.is_shutdown():
count += 1
pub_test()
rospy.loginfo("发布次数:%d",count)
在cmakelists.txt中添加py文件
之后重新编译一下。
cd ~/catkin_RealSense_ws
catkin_make
记得给py文件权限
如果显示
的话,参考:http://t.csdnimg.cn/PICNehttp://t.csdnimg.cn/PICNe
运行ur5启动launch:
roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=192.168.0.101 limited:=true
示教器上启动external control
运行自己的节点:
rosrun my_bag demo01.py
之后可以看到它按照代码中的预设参数执行轨迹。
编写简单的moveit程序控制轨迹
在my_bag/scripts中新建一个demo02.py和demo03.py,具体过程略,这种demo用的节点要用的时候直接run就行,不一定要通过ros启动。
demo02.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, sys
import moveit_commander
import math
def degrees_to_radians(degrees):
return degrees * math.pi / 180.0
def degrees_list_to_radians_list(degrees_list):
return [degrees_to_radians(degrees) for degrees in degrees_list]
def plan_and_execute(arm_group, target_pose=None, joint_state=None, max_velocity_scaling_factor=0.2):
"""规划并执行目标位姿或关节状态的辅助函数。"""
arm_group.set_max_velocity_scaling_factor(max_velocity_scaling_factor)
if target_pose:
arm_group.set_pose_target(target_pose)
plan = arm_group.plan()
elif joint_state:
plan = arm_group.plan(joint_state)
else:
raise ValueError("Either target_pose or joint_state must be provided")
success = arm_group.execute(plan[1], wait=True)
arm_group.stop()
arm_group.clear_pose_targets()
while not success:
if target_pose:
arm_group.set_pose_target(target_pose)
plan = arm_group.plan()
elif joint_state:
plan = arm_group.plan(joint_state)
success = arm_group.execute(plan[1], wait=True)
arm_group.stop()
arm_group.clear_pose_targets()
return success
class MoveItFkDemo:
def __init__(self):
# 初始化move_group的API,出现roscpp是因为底层是通过C++进行实现的
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点,节点名为'moveit_fk_demo'
rospy.init_node('moveit_fk_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('manipulator')
# 设置机械臂运动的允许误差值,单位弧度
arm.set_goal_joint_tolerance(0.01)
# 设置允许的最大速度和加速度,范围0~1
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# # 控制机械臂先回到初始化位置,home是setup assistant中设置的
# arm.set_named_target('home')
# arm.go() #让机械臂先规划,再运动,阻塞指令,直到机械臂到达home后再向下执行
# rospy.sleep(1)
while True:
# 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
# joint_positions = [0, 0, -0.376217, 0, 0, 0]
joint_positions = degrees_list_to_radians_list([-21.5, -104.4, 113.84, -90, 0, 0])
# arm.set_joint_value_target(joint_positions) #设置关节值作为目标值
# 控制机械臂完成运动
plan_and_execute(arm, joint_state=joint_positions)
rospy.sleep(1) # 等待1秒
# 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
# joint_positions = [0, 0, -0.376217, 0, 0, 0]
joint_positions = degrees_list_to_radians_list([165.0, -104.4, 113.84, -90, 0, 0])
arm.set_joint_value_target(joint_positions) #设置关节值作为目标值
# 控制机械臂完成运动
plan_and_execute(arm, joint_state=joint_positions)
rospy.sleep(1) # 等待1秒
# # 控制机械臂先回到初始化位置
# arm.set_named_target('home')
# arm.go()
# rospy.sleep(1)
# 关闭并退出moveit
# moveit_commander.roscpp_shutdown()
# moveit_commander.os._exit(0)
if __name__ == "__main__":
try:
MoveItFkDemo()
except rospy.ROSInterruptException:
pass
demo03.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, sys
import moveit_commander
import math
def degrees_to_radians(degrees):
return degrees * math.pi / 180.0
def degrees_list_to_radians_list(degrees_list):
return [degrees_to_radians(degrees) for degrees in degrees_list]
class MoveItFkDemo:
def __init__(self):
# 初始化move_group的API,出现roscpp是因为底层是通过C++进行实现的
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点,节点名为'moveit_fk_demo'
rospy.init_node('moveit_fk_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('manipulator')
# 设置机械臂运动的允许误差值,单位弧度
arm.set_goal_joint_tolerance(0.01)
# 设置允许的最大速度和加速度,范围0~1
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# # 控制机械臂先回到初始化位置,home是setup assistant中设置的
# arm.set_named_target('home')
# arm.go() #让机械臂先规划,再运动,阻塞指令,直到机械臂到达home后再向下执行
# rospy.sleep(1)
while True:
# 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
# joint_positions = [0, 0, -0.376217, 0, 0, 0]
joint_positions = degrees_list_to_radians_list([-21.5, -104.4, 113.84, -90, 0, 0])
arm.set_joint_value_target(joint_positions) #设置关节值作为目标值
# 控制机械臂完成运动
success = arm.go() # 规划+执行
print(success)
arm.stop()
arm.clear_pose_targets()
rospy.sleep(1)
# 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
# joint_positions = [0, 0, -0.376217, 0, 0, 0]
joint_positions = degrees_list_to_radians_list([165.0, -104.4, 113.84, -90, 0, 0])
arm.set_joint_value_target(joint_positions) #设置关节值作为目标值
# 控制机械臂完成运动
arm.go() # 规划+执行
print(success)
arm.stop()
arm.clear_pose_targets()
rospy.sleep(1)
# # 控制机械臂先回到初始化位置
# arm.set_named_target('home')
# arm.go()
# rospy.sleep(1)
# 关闭并退出moveit
# moveit_commander.roscpp_shutdown()
# moveit_commander.os._exit(0)
if __name__ == "__main__":
try:
MoveItFkDemo()
except rospy.ROSInterruptException:
pass
Plan()+execuate()和go()的功能差不多,只是后面我们要添加传感器部分,execuate()我试下来遇到障碍物好像不会中间停下来,所以我之后还是用go()。
修改ompl默认规划算法(非必要)
在下面路径中修改默认规划算法,文件路径如图所示。
/home/lu/catkin_RealSense_ws/src/universal_robot/ur5_moveit_config/config/ompl_planning.yaml