本文使用的机械臂模型是《MoveIt可视化配置及仿真指南》课程中的机械臂模型，并加入了小修改。
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" />
<!-- Macro for 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添加一个硬件仿真接口:
<!-- Transmissions for 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, sys
import moveit_commander
import tf
import threading
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose
from ar_track_alvar_msgs.msg import AlvarMarkers,AlvarMarker
x = 0
y = 0
z = 0
ox = 0
oy = 0
oz = 0
zw = 0
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('arm')
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = moveit_commander.MoveGroupCommander('gripper')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.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 = 1
def Listener():
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.0001816
print(target_pose)
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
global a
a+=1
print(" 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无法正常检测二维码。
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS机械臂控制之跟踪二维码！
