由于疫情原因不能进入实验室,遂学习在仿真环境gazebo下利用UR5机械臂搭建模拟平台,此模拟平台可以用于UR5机械臂通用视觉抓取平台。以下是个人总结一些观点,仅供参考。 1. 环境搭建
- 世界环境 参考网址:http://gazebosim.org/tutorials?cat=build_world 作为视觉抓取环境,我选了一个桌子作为抓取平台和三个带颜色的方形物块作为抓取对象,同时我给UR5机械臂加了一个基座。其实我们可以根据根据自己需求搭建自己的环境,但是这些模型又是怎么产生的呢,可以参照上面参考网址进行了解。当然本身官网已经有了大量模型,我们直接拿来用。 gazebo模型数据集网址:https://github.com/osrf/gazebo_models 世界环境的搭建,主要是在ur_icam_gazebo的worlds文件下进行,它本身语言的含义可以通过简单字面意思去知道模型位置,大小,摩擦力特性等,这里说一下这些模型的默认搜索位置应该是在/home/×××/.gazebo/models下,默认是一个隐藏文件,需要在home目录下,同时按ctrl+h来显示隐藏文件.gazebo。 由于程序太长,这里我不会贴出来,后面会有相关的链接。
- 模型环境 首先我们需要准备好的UR5本身的官方包。相机这里我选用了kinect_V2作为深度相机用于视觉采集,然后手抓方面我选择了robotiq_85_gripper二指手抓,这里的深度相机和手抓是可以去更换的,当然需要重新编写URDF或xacro文件来进行配置。 这里我可能会疑问我怎么把UR5机械臂设置成如下默认位置,以及相机位置的选择,怎么把手抓和相机,机械臂三者结合到一块。 1,首先设置UR5机械臂的位置,我是在gazebo启动的ur5.launch文件下,首先设置了暂停模式
<arg name="paused" default="true" doc="Starts gazebo in paused mode" />
然后利用gazebo_ros的spawn_gazebo_model节点获取了关节位置,这样可以修改UR5的其实位置和姿态
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model robot -z 0.594
-J shoulder_lift_joint -2.0
-J elbow_joint 1.0"
output="screen" />
最后启动暂停的gazebo文件,便可以获得如下的初始位姿。
2,我是先在ur5_single_arm.urdf.xacro文件下,确定了世界坐标与UR5基座标的关系后,然后在相机的urdf文件中,又确立了相机坐标与世界坐标的关系,引入到模型中。 ur5_single_arm.urdf.xacro
<joint name="world_joint" type="fixed">
<parent link="world" />
<child link = "base_link" />
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</joint>
ur5_single_arm.urdf.xacro中
<xacro:include filename="$(find kinect_v2)/urdf/kinect_v2.urdf.xacro" />
<xacro:kinect_v2 parent="world">
<origin xyz="0.4 0 1.57"
rpy="0 1.57 0" />
</xacro:kinect_v2>
3,第三个问题主要是在robotiq_85_gripper.urdf.xacro文件下 我将三个urdf文件进行了声明,然后我找的包本身已经把手抓和机械臂在这个文件中结合到一块,相机的话我第二个问题有提到
<!-- robotiq 85 -->
<xacro:include filename="$(find robotiq_85_description)/urdf/robotiq_85_gripper.urdf.xacro" />
<!-- gripper -->
<xacro:robotiq_85_gripper prefix="" parent="ee_link" >
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:robotiq_85_gripper>
根据上面所说的我搭建了自己的模拟环境如下:
2,gazebo ros控制 ros-control包含两个控制,一个对UR5机械臂的控制,另一个是对二指手抓的控制。 分别是在加载gazebo的同时启动相应的控制。 在ur5.launch下,利用下面的语句查找了控制器
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
其中对ur5的控制器是查找了ur5官方提供包的arm_controller_ur5.yaml 利用了position_controllers/JointTrajectoryController,关节轨迹控制器 对手抓的控制是查找了gripper_controller_robotiq.yaml
<rosparam file="$(find robotiq_85_gazebo)/controller/gripper_controller_robotiq.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller gripper" respawn="false" output="screen"/>
控制器内容:
gripper:
type: position_controllers/JointTrajectoryController
joints:
- gripper_finger1_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
gripper_finger1_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 25
action_monitor_rate: 10
利用了 position_controllers/JointTrajectoryController,手抓action控制
上述两者的消息类型,可以在这里查找到 http://docs.ros.org/api/control_msgs/html/index-msg.html 只有这里对ros-control进行声明,才可以对手臂和手抓进行驱动,类似于驱动的协议。 3,moveit MoveIt Setup Assistant moveit配置: 大致配置思路如下: Step 1: Start
roslaunch moveit_setup_assistant setup_assistant.launch
Click on the Create New MoveIt Configuration Package button to bring up the following screen: 选择/ur_icam_description/urdf/ur5_robot.urdf.xacro,进行加载
Step 2: Generate Self-Collision Matrix 点击产生碰撞矩阵
Step 3: Add Virtual Joints
Step 4: Add Planning Groups
添加手臂运动规划组arm
添加手抓运动规划组gripper:
Step 5: Add Robot Poses home
up
open
grasp
Step 6: Label End Effectors
最后加载输出,新建一个ur_icam_moveit_config文件夹,点击产生包
上述过程可以在ur5.srdf找到对应的配置结果
<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="ur5">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="manipulator">
<chain base_link="base_link" tip_link="ee_link" />
</group>
<group name="gripper">
<joint name="gripper_base_joint" />
<joint name="gripper_finger1_inner_knuckle_joint" />
<joint name="gripper_finger1_finger_tip_joint" />
<joint name="gripper_finger1_joint" />
<joint name="gripper_finger1_finger_joint" />
<joint name="gripper_finger2_inner_knuckle_joint" />
<joint name="gripper_finger2_finger_tip_joint" />
<joint name="gripper_finger2_joint" />
<joint name="gripper_finger2_finger_joint" />
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="home" group="manipulator">
<joint name="elbow_joint" value="0" />
<joint name="shoulder_lift_joint" value="0" />
<joint name="shoulder_pan_joint" value="0" />
<joint name="wrist_1_joint" value="0" />
<joint name="wrist_2_joint" value="0" />
<joint name="wrist_3_joint" value="0" />
</group_state>
<group_state name="home_j" group="manipulator">
<joint name="elbow_joint" value="2" />
<joint name="shoulder_lift_joint" value="-1.9198" />
<joint name="shoulder_pan_joint" value="0" />
<joint name="wrist_1_joint" value="-1.6755" />
<joint name="wrist_2_joint" value="-1.56" />
<joint name="wrist_3_joint" value="0" />
</group_state>
<group_state name="up" group="manipulator">
<joint name="elbow_joint" value="0" />
<joint name="shoulder_lift_joint" value="-1.5" />
<joint name="shoulder_pan_joint" value="0" />
<joint name="wrist_1_joint" value="0" />
<joint name="wrist_2_joint" value="0" />
<joint name="wrist_3_joint" value="0" />
</group_state>
<group_state name="pickup" group="manipulator">
<joint name="elbow_joint" value="1.5" />
<joint name="shoulder_lift_joint" value="-1.5" />
<joint name="shoulder_pan_joint" value="0" />
<joint name="wrist_1_joint" value="-1.5" />
<joint name="wrist_2_joint" value="-1.5" />
<joint name="wrist_3_joint" value="0" />
</group_state>
<group_state name="open" group="gripper">
<joint name="gripper_finger1_joint" value="0" />
</group_state>
<group_state name="close" group="gripper">
<joint name="gripper_finger1_joint" value="0.4" />
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="gripper" parent_link="ee_link" group="gripper" />
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="base_link" link2="robot_pilar" reason="Adjacent" />
<disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
<disable_collisions link1="ee_link" link2="gripper_base_link" reason="Adjacent" />
<disable_collisions link1="ee_link" link2="gripper_finger1_finger_link" reason="Never" />
<disable_collisions link1="ee_link" link2="gripper_finger1_finger_tip_link" reason="Never" />
<disable_collisions link1="ee_link" link2="gripper_finger1_inner_knuckle_link" reason="Never" />
<disable_collisions link1="ee_link" link2="gripper_finger1_knuckle_link" reason="Never" />
<disable_collisions link1="ee_link" link2="gripper_finger2_finger_link" reason="Never" />
<disable_collisions link1="ee_link" link2="gripper_finger2_finger_tip_link" reason="Never" />
<disable_collisions link1="ee_link" link2="gripper_finger2_inner_knuckle_link" reason="Never" />
<disable_collisions link1="ee_link" link2="gripper_finger2_knuckle_link" reason="Never" />
<disable_collisions link1="ee_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="ee_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="ee_link" link2="wrist_3_link" reason="Adjacent" />
<disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
<disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
<disable_collisions link1="gripper_base_link" link2="gripper_finger1_finger_link" reason="Never" />
<disable_collisions link1="gripper_base_link" link2="gripper_finger1_finger_tip_link" reason="Never" />
<disable_collisions link1="gripper_base_link" link2="gripper_finger1_inner_knuckle_link" reason="Adjacent" />
<disable_collisions link1="gripper_base_link" link2="gripper_finger1_knuckle_link" reason="Adjacent" />
<disable_collisions link1="gripper_base_link" link2="gripper_finger2_finger_link" reason="Never" />
<disable_collisions link1="gripper_base_link" link2="gripper_finger2_finger_tip_link" reason="Never" />
<disable_collisions link1="gripper_base_link" link2="gripper_finger2_inner_knuckle_link" reason="Adjacent" />
<disable_collisions link1="gripper_base_link" link2="gripper_finger2_knuckle_link" reason="Adjacent" />
<disable_collisions link1="gripper_base_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_base_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_base_link" link2="wrist_3_link" reason="Default" />
<disable_collisions link1="gripper_finger1_finger_link" link2="gripper_finger1_finger_tip_link" reason="Default" />
<disable_collisions link1="gripper_finger1_finger_link" link2="gripper_finger1_inner_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_link" link2="gripper_finger1_knuckle_link" reason="Adjacent" />
<disable_collisions link1="gripper_finger1_finger_link" link2="gripper_finger2_finger_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_link" link2="gripper_finger2_finger_tip_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_link" link2="gripper_finger2_inner_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_link" link2="gripper_finger2_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="gripper_finger1_inner_knuckle_link" reason="Adjacent" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="gripper_finger1_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="gripper_finger2_finger_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="gripper_finger2_finger_tip_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="gripper_finger2_inner_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="gripper_finger2_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_finger1_finger_tip_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="gripper_finger1_inner_knuckle_link" link2="gripper_finger1_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_inner_knuckle_link" link2="gripper_finger2_finger_link" reason="Never" />
<disable_collisions link1="gripper_finger1_inner_knuckle_link" link2="gripper_finger2_finger_tip_link" reason="Never" />
<disable_collisions link1="gripper_finger1_inner_knuckle_link" link2="gripper_finger2_inner_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_inner_knuckle_link" link2="gripper_finger2_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_inner_knuckle_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_finger1_inner_knuckle_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_finger1_inner_knuckle_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="gripper_finger1_knuckle_link" link2="gripper_finger2_finger_link" reason="Never" />
<disable_collisions link1="gripper_finger1_knuckle_link" link2="gripper_finger2_finger_tip_link" reason="Never" />
<disable_collisions link1="gripper_finger1_knuckle_link" link2="gripper_finger2_inner_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_knuckle_link" link2="gripper_finger2_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger1_knuckle_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_finger1_knuckle_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_finger1_knuckle_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="gripper_finger2_finger_link" link2="gripper_finger2_finger_tip_link" reason="Default" />
<disable_collisions link1="gripper_finger2_finger_link" link2="gripper_finger2_inner_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger2_finger_link" link2="gripper_finger2_knuckle_link" reason="Adjacent" />
<disable_collisions link1="gripper_finger2_finger_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_finger2_finger_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_finger2_finger_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="gripper_finger2_finger_tip_link" link2="gripper_finger2_inner_knuckle_link" reason="Adjacent" />
<disable_collisions link1="gripper_finger2_finger_tip_link" link2="gripper_finger2_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger2_finger_tip_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_finger2_finger_tip_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_finger2_finger_tip_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="gripper_finger2_inner_knuckle_link" link2="gripper_finger2_knuckle_link" reason="Never" />
<disable_collisions link1="gripper_finger2_inner_knuckle_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_finger2_inner_knuckle_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_finger2_inner_knuckle_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="gripper_finger2_knuckle_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="gripper_finger2_knuckle_link" link2="wrist_2_link" reason="Never" />
<disable_collisions link1="gripper_finger2_knuckle_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="robot_pilar" link2="shoulder_link" reason="Never" />
<disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
<disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
<disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
</robot>
上述完成后,在~/src/ur5-gazebo-grasping/ur5_single_arm_moveit_config/config下新建一个controllers.yaml 内容:
controller_list:
- name: "arm_controller"
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
- name: "gripper"
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- gripper_finger1_joint
新建一个在ur5_moveit_planning_execution.launch文件(在ur5_single_arm_moveit_config)/launch下) 到这里我们将moveit的启动加入到launch文件中。 运行
roslaunch ur5_single_arm_tufts ur5_single_arm_gazebo.launch
其中手抓张合可以直接在这里修改
配置完成后,启动moveit和rviz,并打开点云,改变订阅话题
到这里我们可以利用movegroup进行机械臂和手抓的控制。 4,move-group 控制 move_group move_group python接口:
这里我用了moveit_commander(python)接口写了控制路径规划。机器人控制器之前说了用到了JointTrajectory控制。 这里演示了抓取红色物块的过程。 具体控制逻辑: 路点1:运行到预抓取位置 路点2:手抓张开 路点3:运行到抓取物体上方 路点4:运行到抓取点 路点5:手抓闭合 路点6:运行到抓取物体上方 路点7:运行到放置点 路点8:手抓张开 控制程序grasping_demo.py
#! /usr/bin/env python
import sys
import rospy
import moveit_commander
import geometry_msgs
import tf
moveit_commander.roscpp_initializer.roscpp_initialize(sys.argv)
rospy.init_node('move_group_grasp', anonymous=True)
robot = moveit_commander.robot.RobotCommander()
arm_group = moveit_commander.move_group.MoveGroupCommander("manipulator")
hand_group = moveit_commander.move_group.MoveGroupCommander("gripper")
#hand_group.set_named_target("close")
#plan = hand_group.go()
arm_group.set_named_target("home_j")
plan = arm_group.go()
print("Point 1")
# Open
#hand_group.set_joint_value_target([9.800441184282249e-05, -9.800441184282249e-05, 9.800441184282249e-05, 9.800441184282249e-05, -9.800441184282249e-05, 9.800441184282249e-05])
#hand_group.go(wait=True)
#print("Point 2")
hand_group.set_named_target("open")
plan = hand_group.go()
print("Point 2")
pose_target = arm_group.get_current_pose().pose
# Block point
pose_target.position.x = 0.4
pose_target.position.y = 0.0
pose_target.position.z = pose_target.position.z+0.06
arm_group.set_pose_target(pose_target)
arm_group.go(wait=True)
print("Point 3")
# Block point
pose_target.position.x = 0.4
pose_target.position.y = 0.0
pose_target.position.z = pose_target.position.z-0.07
arm_group.set_pose_target(pose_target)
arm_group.go(wait=True)
print("Point 4")
hand_group.set_named_target("close")
plan = hand_group.go()
print("Point 5")
pose_target.position.z = pose_target.position.z+0.05
arm_group.set_pose_target(pose_target)
plan = arm_group.go()
print("Point 6")
pose_target.position.z = pose_target.position.z+0.05
pose_target.position.x = 0.5
arm_group.set_pose_target(pose_target)
plan = arm_group.go()
print("Point 7")
hand_group.set_named_target("open")
plan = hand_group.go()
print("Point 8")
rospy.sleep(5)
moveit_commander.roscpp_initializer.roscpp_shutdown()
运行语句:
roslaunch ur5_single_arm_tufts ur5_single_arm_gazebo.launch
rosrun ur5_single_arm_manipulation grasping_demo.py
问题: 由于手抓和物快摩擦存在问题,导致张开手抓时物体会崩开。 手抓控制: 也可以直接发送关节角度来控制: #hand_group.set_joint_value_target([9.800441184282249e-05, -9.800441184282249e-05, 9.800441184282249e-05, 9.800441184282249e-05, -9.800441184282249e-05, 9.800441184282249e-05]) 演示效果:
节点图:
rqt_graph
5,深度相机kinect V2
rqt_image_view
可以采集RGB图像
可以采集深度图像
结合视觉算法,可以完成视觉抓取操作。 有精力再补充。 程序下载地址: https://github.com/harrycomeon/ur5-gazebo-grasping 程序包中还缺少UR5机械臂的驱动包,通过git下载
git clone https://github.com/ros-industrial/universal_robot.git
一些参考网址: 1,http://gazebosim.org/tutorials?tut=ros_roslaunch&cat=connect_ros There are two ways to launch your URDF-based robot into Gazebo using roslaunch: ROS Service Call Spawn Method The first method keeps your robot’s ROS packages more portable between computers and repository check outs. It allows you to keep your robot’s location relative to a ROS package path, but also requires you to make a ROS service call using a small (python) script. Model Database Method The second method allows you to include your robot within the .world file, which seems cleaner and more convenient but requires you to add your robot to the Gazebo model database by setting an environment variable. We will go over both methods. Overall our recommended method is using the ‘’‘ROS Service Call Spawn Method’’’ 2, 相机插件 http://gazebosim.org/tutorials?tut=ros_gzplugins&cat=connect_ros http://gazebosim.org/tutorials?tut=ros_depth_camera&cat=connect_ros 3,Rigidly Fixing A Model to the World(添加世界坐标) http://gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros 问题1:
解决方案: 加权限
整个程序的代码: 链接:https://pan.baidu.com/s/1lSxzbBk-AX9r1iQuNic_rA 提取码:03k1 复制这段内容后打开百度网盘手机App,操作更方便哦