ROS机械臂控制之跟踪二维码-ros机械臂抓取

1 构建机械臂的xacro模型

创建一个6机械臂的xacro模型文件，完整gazebo模型如下所示：

首先，定义6个link的形状以及末端夹具的大小： <!– link1 properties –><xacro:property name=“link0_radius” value=“0.05”/><xacro:property name=“link0_length” value=“0.04”/><xacro:property name=“link0_mass” value=“1”/><!– link1 properties –><xacro:property name=“link1_radius” value=“0.03”/><xacro:property name=“link1_length” value=“0.10”/><xacro:property name=“link1_mass” value=“1”/><!– link2 properties –><xacro:property name=“link2_radius” value=“0.03”/><xacro:property name=“link2_length” value=“0.14”/><xacro:property name=“link2_mass” value=“0.8”/><!– link3 properties –><xacro:property name=“link3_radius” value=“0.03”/><xacro:property name=“link3_length” value=“0.15”/><xacro:property name=“link3_mass” value=“0.8”/><!– link4 properties –><xacro:property name=“link4_radius” value=“0.025”/><xacro:property name=“link4_length” value=“0.06”/><xacro:property name=“link4_mass” value=“0.7”/><!– link5 properties –><xacro:property name=“link5_radius” value=“0.03”/><xacro:property name=“link5_length” value=“0.06”/><xacro:property name=“link5_mass” value=“0.7”/><!– link6 properties –><xacro:property name=“link6_radius” value=“0.04”/><xacro:property name=“link6_length” value=“0.02”/><xacro:property name=“link6_mass” value=“0.6”/><!– gripper –><xacro:property name=“gripper_length” value=“0.03”/><xacro:property name=“gripper_width” value=“0.01”/><xacro:property name=“gripper_height” value=“0.06”/><xacro:property name=“gripper_mass” value=“0.5”/>

接着，需要设置每一个Link的详细参数以及两个Link连接处的joint的类型和旋转轴：

<!–Gripper frame –><xacro:property name=“grasp_frame_radius” value=“0.001”/><!–Macrofor inertia matrix –><xacro:macro name=“cylinder_inertial_matrix” params=“m r h”><inertial><mass value=“${m}”/><inertia ixx=“${m*(3*r*r+h*h)/12}” ixy =“0” ixz =“0” iyy=“${m*(3*r*r+h*h)/12}” iyz =“0” izz=“${m*r*r/2}”/></inertial></xacro:macro><xacro:macro name=“box_inertial_matrix” params=“m w h d”><inertial><mass value=“${m}”/><inertia ixx=“${m*(h*h+d*d)/12}” ixy =“0” ixz =“0” iyy=“${m*(w*w+d*d)/12}” iyz =“0” izz=“${m*(w*w+h*h)/12}”/></inertial></xacro:macro><!–///////////////////////////////////// ARM BASE //////////////////////////////////////////////–><xacro:macro name=“arm_base” params=“parent xyz rpy”><joint name=“${parent}_arm_joint” type=“fixed”><origin xyz=“${xyz}” rpy=“${rpy}”/><parent link=“${parent}”/><child link=“base_link”/></joint><link name=“base_link”><visual><origin xyz=“0 0 0” rpy=“0 0 0”/><geometry><box size=“0.001 0.001 0.001”/></geometry></visual></link><joint name=“base_joint” type=“fixed”><origin xyz=“0 0 ${link0_length/2}” rpy=“0 0 0”/><parent link=“base_link”/><child link=“link0”/></joint><!–///////////////////////////////////// LINK0 //////////////////////////////////////////////–><link name=“link0”><visual><origin xyz=“0 0 0” rpy=“0 0 0”/><geometry><cylinder radius=“${link0_radius}” length=“${link0_length}”/></geometry><material name=“White”/></visual><collision><origin xyz=“0 0 0” rpy=“0 0 0”/><geometry><cylinder radius=“${link0_radius}” length=“${link0_length}”/></geometry></collision><cylinder_inertial_matrix m=“${link0_mass}” r=“${link0_radius}” h=“${link0_length}”/></link><joint name=“joint1” type=“revolute”><parent link=“link0”/><child link=“link1”/><origin xyz=“0 0 ${link0_length/2}” rpy=“0 ${M_PI/2} 0”/><axis xyz=“-1 0 0”/><limit effort=“300” velocity=“1” lower=“${-M_PI}” upper=“${M_PI}”/></joint><!–///////////////////////////////////// LINK1 //////////////////////////////////////////////–><link name=“link1”><visual><origin xyz=“-${link1_length/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><cylinder radius=“${link1_radius}” length=“${link1_length}”/></geometry><material name=“Blue”/></visual><collision><origin xyz=“-${link1_length/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><cylinder radius=“${link1_radius}” length=“${link1_length}”/></geometry></collision><cylinder_inertial_matrix m=“${link1_mass}” r=“${link1_radius}” h=“${link1_length}”/></link><joint name=“joint2” type=“revolute”><parent link=“link1”/><child link=“link2”/><origin xyz=“-${link1_length} 0 0.0” rpy=“-${M_PI/2} 0 ${M_PI/2}”/><axis xyz=“1 0 0”/><limit effort=“300” velocity=“1” lower=“${-M_PI}” upper=“${M_PI}”/></joint><!–/////////////////////////////////////// LINK2 //////////////////////////////////////////////–><link name=“link2”><visual><origin xyz=“0 0 ${link2_length/2}” rpy=“0 0 0”/><geometry><cylinder radius=“${link2_radius}” length=“${link2_length}”/></geometry><material name=“White”/></visual><collision><origin xyz=“0 0 ${link2_length/2}” rpy=“0 0 0”/><geometry><cylinder radius=“${link2_radius}” length=“${link2_length}”/></geometry></collision><cylinder_inertial_matrix m=“${link2_mass}” r=“${link2_radius}” h=“${link2_length}”/></link><joint name=“joint3” type=“revolute”><parent link=“link2”/><child link=“link3”/><origin xyz=“0 0 ${link2_length}” rpy=“0 ${M_PI} 0”/><axis xyz=“-1 0 0”/><limit effort=“300” velocity=“1” lower=“${-M_PI}” upper=“${M_PI}”/></joint><!–///////////////////////////////// LINK3 /////////////////////////////////////////////////////–><link name=“link3”><visual><origin xyz=“0 0 -${link3_length/2}” rpy=“0 0 0”/><geometry><cylinder radius=“${link3_radius}” length=“${link3_length}”/></geometry><material name=“Blue”/></visual><collision><origin xyz=“0 0 -${link3_length/2}” rpy=“0 0 0”/><geometry><cylinder radius=“${link3_radius}” length=“${link3_length}”/></geometry></collision><cylinder_inertial_matrix m=“${link3_mass}” r=“${link3_radius}” h=“${link3_length}”/></link><joint name=“joint4” type=“revolute”><parent link=“link3”/><child link=“link4”/><origin xyz=“0.0 0.0 -${link3_length}” rpy=“0 ${M_PI/2} ${M_PI}”/><axis xyz=“1 0 0”/><limit effort=“300” velocity=“1” lower=“${-M_PI}” upper=“${M_PI}”/></joint><!–/////////////////////////////////// LINK4 ////////////////////////////////////////////////–><link name=“link4”><visual><origin xyz=“${link4_length/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><cylinder radius=“${link4_radius}” length=“${link4_length}”/></geometry><material name=“Black”/></visual><collision><origin xyz=“${link4_length/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><cylinder radius=“${link4_radius}” length=“${link4_length}”/></geometry></collision><cylinder_inertial_matrix m=“${link4_mass}” r=“${link4_radius}” h=“${link4_length}”/></link><joint name=“joint5” type=“revolute”><parent link=“link4”/><child link=“link5”/><origin xyz=“${link4_length} 0.0 0.0” rpy=“0 ${M_PI/2} 0”/><axis xyz=“1 0 0”/><limit effort=“300” velocity=“1” lower=“${-M_PI}” upper=“${M_PI}”/></joint><!–////////////////////////////////// LINK5 /////////////////////////////////////////////////–><link name=“link5”><visual><origin xyz=“0 0 ${link4_length/2}” rpy=“0 0 0”/><geometry><cylinder radius=“${link5_radius}” length=“${link5_length}”/></geometry><material name=“White”/></visual><collision><origin xyz=“0 0 ${link4_length/2} “ rpy=“0 0 0”/><geometry><cylinder radius=“${link5_radius}” length=“${link5_length}”/></geometry></collision><cylinder_inertial_matrix m=“${link5_mass}” r=“${link5_radius}” h=“${link5_length}”/></link><joint name=“joint6” type=“revolute”><parent link=“link5”/><child link=“link6”/><origin xyz=“0 0 ${link4_length}” rpy=“${1.5*M_PI} -${M_PI/2} 0”/><axis xyz=“1 0 0”/><limit effort=“300” velocity=“1” lower=“${-2*M_PI}” upper=“${2*M_PI}”/></joint><!–//////////////////////////////// LINK6 /////////////////////////////////////////////////–><link name=“link6”><visual><origin xyz=“${link6_length/2} 0 0 “ rpy=“0 ${M_PI/2} 0”/><geometry><cylinder radius=“${link6_radius}” length=“${link6_length}”/></geometry><material name=“Blue”/></visual><collision><origin xyz=“${link6_length/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><cylinder radius=“${link6_radius}” length=“${link6_length}”/></geometry></collision><cylinder_inertial_matrix m=“${link6_mass}” r=“${link6_radius}” h=“${link6_length}”/></link><joint name=“finger_joint1” type=“prismatic”><parent link=“link6”/><child link=“gripper_finger_link1”/><origin xyz=“${link6_length} -0.03 0” rpy=“0 0 0”/><axis xyz=“0 1 0”/><limit effort=“100” lower=“0” upper=“0.06” velocity=“0.02”/></joint><!–////////////////////////////////////// gripper //////////////////////////////////////////////–><!– LEFT GRIPPER AFT LINK –><link name=“gripper_finger_link1”><visual><origin xyz=“${gripper_height/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><box size=“${gripper_length} ${gripper_width} ${gripper_height}”/></geometry><material name=“White”/></visual><collision><origin xyz=“${gripper_height/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><box size=“${gripper_length} ${gripper_width} ${gripper_height}”/></geometry></collision><box_inertial_matrix m=“${gripper_mass}” w=“${gripper_width}” h=“${gripper_height}” d=“${gripper_length}”/></link><joint name=“finger_joint2” type=“fixed”><parent link=“link6”/><child link=“gripper_finger_link2”/><origin xyz=“${link6_length} 0.03 0” rpy=“0 0 0”/></joint><!– RIGHT GRIPPER AFT LINK –><link name=“gripper_finger_link2”><visual><origin xyz=“${gripper_height/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><box size=“${gripper_length} ${gripper_width} ${gripper_height}”/></geometry><material name=“White”/></visual><collision><origin xyz=“${gripper_height/2} 0 0” rpy=“0 ${M_PI/2} 0”/><geometry><box size=“${gripper_length} ${gripper_width} ${gripper_height}”/></geometry></collision><box_inertial_matrix m=“${gripper_mass}” w=“${gripper_width}” h=“${gripper_height}” d=“${gripper_length}”/></link><!–Grasping frame –><link name=“grasping_frame”/><joint name=“grasping_frame_joint” type=“fixed”><parent link=“link6”/><child link=“grasping_frame”/><origin xyz=“${gripper_height} 0 0” rpy=“0 0 0”/></joint> 注意：可以看到其中joint1和joint4的旋转轴和其他的不同，是因为后面在moveit中通过拖拽末端设置目标位姿时，必须至少包含2个这种旋转joint才能实现任意拖拽，否则机械臂是不能被拖动的。

最后，为每一个joint添加一个硬件仿真接口:

<!–Transmissionsfor ROS Control–><xacro:macro name=“transmission_block” params=“joint_name”><transmission name=“${joint_name}_trans”><type>transmission_interface/SimpleTransmission</type><joint name=“${joint_name}”><hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface></joint><actuator name=“${joint_name}_motor”><hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission></xacro:macro><xacro:transmission_block joint_name=“joint1”/><xacro:transmission_block joint_name=“joint2”/><xacro:transmission_block joint_name=“joint3”/><xacro:transmission_block joint_name=“joint4”/><xacro:transmission_block joint_name=“joint5”/><xacro:transmission_block joint_name=“joint6”/><xacro:transmission_block joint_name=“finger_joint1”/><!– ros_control plugin –><gazebo><plugin name=“gazebo_ros_control” filename=“libgazebo_ros_control.so”><robotNamespace>/arm</robotNamespace></plugin></gazebo></xacro:macro>

这样机械臂的模型就建立好了，还需要注意的是，机械臂底座的质量一定要大些。

2 添加一个kinect模型

虽然我们使用了一个深度相机，但是该教程中我们并没有使用深度话题，仅仅使用了RGB图像。

<xacro:property name=“deg_to_rad” value=“0.01745329251994329577”/><!– kinect –><joint name=“kinect_joint” type=“fixed”><origin xyz=“0.1 0 0.8” rpy=“0 ${75.0 * deg_to_rad} 0”/><parent link=“base_link”/><child link=“kinect_link”/></joint><xacro:kinect_camera prefix=“kinect”/>

最终效果如图所示：

3 配置二维码识别包

需要使用ros的ar_track_alvar包。

首先安装：

sudo apt–get install ros–melodic–ar–track–alvar //替换成自己的ROS版本

之后，创建一个launch文件配置ar_track_alvar节点的内部参数：

<launch><arg name=“marker_size” default=“9”/><arg name=“max_new_marker_error” default=“0.08”/><arg name=“max_track_error” default=“0.2”/><arg name=“cam_image_topic” default=“/kinect/rgb/image_raw”/><arg name=“cam_info_topic” default=“/kinect/rgb/camera_info”/><arg name=“output_frame” default=“/base_link”/><node name=“ar_track_alvar” pkg=“ar_track_alvar” type=“individualMarkersNoKinect” respawn=“false” output=“screen”><param name=“marker_size” type=“double” value=“$(arg marker_size)”/><param name=“max_new_marker_error” type=“double” value=“$(arg max_new_marker_error)”/><param name=“max_track_error” type=“double” value=“$(arg max_track_error)”/><param name=“output_frame” type=“string” value=“$(arg output_frame)”/><remap from=“camera_image” to=“$(arg cam_image_topic)”/><remap from=“camera_info” to=“$(arg cam_info_topic)”/></node></launch> 注意：marker_size是二维码的大小，单位是cm，还需要修改launch文件中的相机话题部分，其中out_frame是输出二维码pose时候参考的坐标系，这里参考的是base_link。

运行launch文件效果如下，可以看到在rviz中，已经检测到了二维码。

查看pose的话题消息： rostopic echo /ar_pose_marker

数据包含了三维坐标值和四元数。

4 在gazebo中添加二维码模型

我直接提供了一个ar_track的二维码模型下载，ID=0，尺寸是10厘米。

链接: https://pan.baidu.com/s/1n_eIhEIaDxaboZfK8IgwqQ

密码: mlht

将这个模型放到.gazebo/models目录下即可，在打开gazbeo后添加模型，选择ar_track_mark0,如图所示：

5 编写机械臂控制程序

本教程使用python编程。

注意，此时你已经将你的机械臂模型通过moveit_setup_assistant进行了配置，若不熟悉，请参考古月学院的课程：MoveIt可视化配置及仿真指南 或古月居的资料：ROS探索总结（六十三）

完整程序如下：

#!/usr/bin/env python# -*- coding: utf-8 -*-import rospy, sysimport moveit_commanderimport tfimport threadingfrom moveit_msgs.msg importRobotTrajectoryfrom trajectory_msgs.msg importJointTrajectoryPointfrom geometry_msgs.msg importPoseStamped,Posefrom ar_track_alvar_msgs.msg importAlvarMarkers,AlvarMarkerx =0y =0z =0ox =0oy =0oz =0zw =0# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化需要使用move group控制的机械臂中的arm grouparm = moveit_commander.MoveGroupCommander(arm)# 初始化需要使用move group控制的机械臂中的gripper groupgripper = moveit_commander.MoveGroupCommander(gripper)# 获取终端link的名称end_effector_link = arm.get_end_effector_link()# 设置目标位置所使用的参考坐标系reference_frame =base_linkarm.set_pose_reference_frame(reference_frame)# 当运动规划失败后，允许重新规划arm.allow_replanning(True)# 设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.set_goal_position_tolerance(0.01)arm.set_goal_orientation_tolerance(0.05)gripper.set_goal_joint_tolerance(0.001)# 控制机械臂先回到初始化位置#arm.set_named_target(home)#arm.go()# 设置机器臂当前的状态作为运动初始状态arm.set_start_state_to_current_state()target_pose =PoseStamped()a =1defListener(): rospy.init_node(listener, anonymous=True) rospy.Subscriber(“/ar_pose_marker”,AlvarMarkers,ar_pose) rospy.spin()def ar_pose(data): x = data.markers[0].pose.pose.position.x y = data.markers[0].pose.pose.position.y z = data.markers[0].pose.pose.position.z ox = data.markers[0].pose.pose.orientation.x oy = data.markers[0].pose.pose.orientation.y oz = data.markers[0].pose.pose.orientation.z ow = data.markers[0].pose.pose.orientation.w target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = x–0.08 target_pose.pose.position.y = y target_pose.pose.position.z = z+0.03 target_pose.pose.orientation.x =0.926402 target_pose.pose.orientation.y =0.000348296 target_pose.pose.orientation.z =–0.376537 target_pose.pose.orientation.w =–0.0001816print(target_pose)# 设置机械臂终端运动的目标位姿 arm.set_pose_target(target_pose, end_effector_link) arm.go()global a a+=1print(” count “,a)# 关闭并退出moveit#moveit_commander.roscpp_shutdown()#moveit_commander.os._exit(0) arm.clear_pose_targets()print(“清除”)if __name__ ==“__main__”:Listener() 注意：arm.go()函数是一个阻塞函数，若不添加下面的arm.clear_pose_targets()则控制器中的pose将不会更新，导致机械臂停留在第一次执行后的位置不变

程序里，我指定了四元数，这个数值是我期待的一个末端抓取所用的位姿，三维坐标也手动添加了一下偏差，是为了避免末端遮挡二维码导致kinect无法正常检测二维码。