ROS机械臂控制之跟踪二维码

2028
4
2020年5月12日 13时50分

本文使用的机械臂模型是《MoveIt可视化配置及仿真指南》课程中的机械臂模型,并加入了小修改。

 

1 构建机械臂的xacro模型

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

  
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"/>

 

最终效果如图所示:

gazebo中的模型效果

 

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

 

pose数据

 

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

 

4 在gazebo中添加二维码模型

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

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

密码: mlht

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

gazbeo中添加二维码

 

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无法正常检测二维码。

发表评论

后才能评论

评论列表(4条)

  • 3lake_3721 2020年7月14日 下午10:41

    你好,完整的工程文件能分享下吗?二维码的模型已经下载不了了

  • 神秘老铁 2020年5月13日 上午11:53

    当前项目是一个demo演示,后期会继续完善,大家持续关注古月居

  • Sergio_Wong 2020年5月12日 下午9:46

    博主您好,我现在在做的毕业设计就是在仿真平台通过二维码识别物体位置并控制机械臂进行抓取实验,有一些相关的问题想向您请教,如果您愿意赐教的话,方便加一下wx吗15905659284,不胜感激