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

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

• 3 / 3
• 1
• 2
• 3