ROS机械臂开发：MoveIt!编程

一、编程接口

提供C++、Python、GUI、命令行等接口

move_group节点 Python和C++接口示例

MoveIt关注更多的是plan工作，后面的execute需要控制器执行，MoveIt只是监控功能。

编程步骤

• 连接控制需要的规划组
  左臂右臂
• 设置目标位姿
  关节空间、笛卡尔空间
• 设置运动约束
  可选，比如末端的水不能撒出来
• 使用MoveIt!规划一条到达目标的轨迹
• 修改轨迹
  如果对plan不满意可以修改，比如速度等参数
• 执行规划出的轨迹
  MoveIt! API参考

二、关节空间运动

每个关节独立规划，不关注末端的笛卡尔轨迹

import rospy, sys
import moveit_commander

class MoveItFkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)
 
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('manipulator')
        
        # 设置机械臂运动的允许误差值
        arm.set_goal_joint_tolerance(0.001)

        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
         
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass

go()相当与plan再execute，阻塞函数
运行脚本后的效果：

代码运行效果

三、自主避障运动

MoveIt里有一个规划场景监听器，用于检测场景中是否有障碍物，障碍物有3种方式可以告诉监听器，1、通过rviz界面添加；2、通过程序（C++，python）添加；3、通过外部传感器，如kinect添加障碍物。这里先介绍前两种，通过外部传感器的方式在视觉章节里介绍。

规划场景的模块结构

• 通过rviz界面添加
  启动demo.launch文件，点开scene objects标签，点Import File，可以选择solidworks建好的模型或者模型库中已有的模型添加进来，放到机器人可能的运动路径上，点击publish scene发布物体，告诉moveit场景中存在该物体。
  Import File

image.png

设置目标位置，在拖动过程中，可以发现与障碍物接触的连杆变为红色高亮：

image.png

拖动到障碍物下方，plan并执行，可以看出机器人进行了自主避障，走出了“夸张”的轨迹。

避障运动

• 通过代码添加

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface
from geometry_msgs.msg import PoseStamped


class MoveItWithObstacle:
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('motion_with_obstacle')

        # 初始化场景对象
        scene = PlanningSceneInterface()
        rospy.sleep(1)

        self.ur5 = MoveGroupCommander('manipulator')
        self.robot = RobotCommander()
        self.eef_link = self.ur5.get_end_effector_link()

        self.ur5.set_goal_position_tolerance(0.01)
        self.ur5.set_goal_orientation_tolerance(0.05)
        self.ur5.allow_replanning(True)
        self.ur5.set_planning_time(10)

        self.ur5.set_named_target('home')
        self.ur5.go()

        # 移除场景中之前运行残留的物体
        scene.remove_attached_object(self.eef_link, 'tool')
        scene.remove_world_object('table')
        scene.remove_world_object('tool')

        rospy.sleep(2)
        # 设置桌面的高度
        table_ground = 0.9

        # 设置table和tool的三维尺寸
        table_size = [0.1, 0.7, 0.01]
        tool_size = [0.02, 0.2, 0.02]

        # 设置tool的位姿
        tool_pose = PoseStamped()
        # 相对于机器人终端描述位姿
        tool_pose.header.frame_id = self.eef_link

        tool_pose.pose.position.x = 0
        tool_pose.pose.position.y = 0.08
        tool_pose.pose.position.z = 0
        tool_pose.pose.orientation.w = 0
        box_name = 'tool'
        scene.add_box(box_name, tool_pose, tool_size)
        # 将tool附着到机器人的终端
        touch_links = self.robot.get_link_names('manipulator')
        scene.attach_box(self.eef_link, box_name, touch_links=touch_links)
        rospy.sleep(2)
        # 将table加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = 'base_link'
        table_pose.pose.position.x = 0.25
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box('table', table_pose, table_size)

        rospy.sleep(2)

    def move(self):
        # 更新当前的位姿
        self.ur5.set_start_state_to_current_state()
        # 设置机械臂的目标位置
        target_pose = PoseStamped()
        target_pose.header.frame_id = 'base_link'
        target_pose.pose.position.x = 0.32577
        target_pose.pose.position.y = 0.1176
        target_pose.pose.position.z = 0.48681
        target_pose.pose.orientation.x = 0.7071
        target_pose.pose.orientation.y = 0
        target_pose.pose.orientation.z = 0
        target_pose.pose.orientation.x = 0.7071

        self.ur5.set_pose_target(target_pose, self.eef_link)
        # 控制机械臂完成运动
        self.ur5.go()
        rospy.sleep(1)

        # 控制机械臂回到初始化位置
        self.ur5.set_named_target('home')
        self.ur5.go()

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)


if __name__ == '__main__':
    try:
        move_with_obstacle = MoveItWithObstacle()
        move_with_obstacle.move()
    except rospy.ROSInterruptException:
        pass

实现的效果是机器人末端带着工具避开障碍物：

代码添加障碍物