MoveIt!机械臂控制（四）

一、MoveIt!编程

在实际应用中，GUI提供的功能有限，很多实现需要使用代码完成

MoveIt!的move_group也提供了丰富的C++和Python的编程API

1.1关节空间规则

关节空间运动是机械臂常用的一种控制方法

关节空间：就是以关节角度为控制量的机器人运动，各关节之间相互独立，互不影响

机器人状态使用各轴位置来描述，在指定运动目标的机器人状态后，通过控制各州运动来达到目标位姿

<launch>
    <!-- 不使用仿真时间 -->
    <param name="/use_sim_time" value="false" />

    <!-- 启动 arbotix driver-->
    <arg name="sim" default="true" />

    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find marm_description)/urdf/arm.xacro'" />

    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find marm_description)/config/arm.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

    <node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller">
        <rosparam>
            model: singlesided
            invert: false
            center: 0.0
            pad_width: 0.004
            finger_length: 0.08
            min_opening: 0.0
            max_opening: 0.06
            joint: finger_joint1
        </rosparam>
    </node>

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />

    <include file="$(find marm_moveit_config)/launch/move_group.launch" />

    <!-- 启动rviz可视化界面 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find marm_planning)/config/pick_and_place.rviz" required="true" />

</launch>

该文件与启动ArbotiX控制器时所使用的launch文件类似，添加了启动move_group相关节点和RViz相关节点的操作

当move_group启动时会自动加载已配置好的MoveIt!关节控制器

使用如下命令实现对marm关节空间的运动测试：

$ roslaunch marm_planning arm_planning.launch
$ rosrun marm_planning moveit_fk_demo.py

moveit_fk_demo.py控制的是机械臂的夹爪，使其能够进行闭合动作

1.1.1源码

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

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

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('arm')
        
        # 初始化需要使用move group控制的机械臂中的gripper group
        gripper = moveit_commander.MoveGroupCommander('gripper')
        
        # 设置机械臂和夹爪的允许误差值
        arm.set_goal_joint_tolerance(0.001)
        gripper.set_goal_joint_tolerance(0.001)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
         
        # 设置夹爪的目标位置，并控制夹爪运动
        gripper.set_joint_value_target([0.01])
        gripper.go()
        rospy.sleep(1)
         
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

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

1.1.2代码分析

• 使用MoveIt!的API之前，需要导入Python接口模块，其中GripperCommand是用来控制夹爪的

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

• 使用MoveIt!的PythonAPI之前，需要先对API进行初始化，初始化的底层依然使用roscpp接口，只是用了Python封装

moveit_commander.roscpp_initialize(sys.argv)

• 在moveit_commander中提供一个重要的类MoveGroupCommander可以创建针对规划的控制对象

在该模拟情况中，有两个控制对象：机械臂本体和夹爪，所以都要进行初始化

# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('arm')
        
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = moveit_commander.MoveGroupCommander('gripper')

• 用代码来设置运动控制的允许误差，也就是说机器人各轴都运动到目标位置为某弧度的范围内，即认为达到目标

# 设置机械臂和夹爪的允许误差值（单位为弧度）
arm.set_goal_joint_tolerance(0.001)
gripper.set_goal_joint_tolerance(0.001)

• 配合Setup Assistant中设置的“home”位姿，在此处可以让机械臂回到初始位置

# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(2)

该段代码首先设置了运动位置，然后使用了arm.go()命令，即让机器人规划、运动到“home”，最后保持一段时间的延时确保机械臂完成运动rospy.sleep(2)

• 配置运动的目标位姿，设置关节空间下目标位姿所使用的接口set_joint_value_target()参数是目标位姿各关节的弧度

# 设置夹爪的目标位置，并控制夹爪运动
gripper.set_joint_value_target([0.01])
gripper.go()
rospy.sleep(1)
         
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
arm.set_joint_value_target(joint_positions)

• 完成关节运动后关闭接口，退出程序

# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)

1.2工作空间规划

关节空间规划是不需要考虑机器人终端的姿态的

与之对应的是工作空间规划，这种规划方式不在通过各轴位置给定，而是通过机器人终端的三维坐标与姿态给定，在进行运动规划时使用逆向运动学求解各轴位置

$ roslaunch marm_planning arm_planning.launch
$ rosrun marm_planning moveit_ik_demo.py

1.2.1源码

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

import rospy, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler

class MoveItIkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')
                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
                
        # 获取终端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)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
               
        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
        # 姿态使用四元数描述，基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.191995
        target_pose.pose.position.y = 0.213868
        target_pose.pose.position.z = 0.520436
        target_pose.pose.orientation.x = 0.911822
        target_pose.pose.orientation.y = -0.0269758
        target_pose.pose.orientation.z = 0.285694
        target_pose.pose.orientation.w = -0.293653
        
        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()
        
        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)
        
        # 规划运动路径
        traj = arm.plan()
        
        # 按照规划的运动路径控制机械臂运动
        arm.execute(traj)
        rospy.sleep(1)
         
        # 控制机械臂终端向右移动5cm
        arm.shift_pose_target(1, -0.05, end_effector_link)
        arm.go()
        rospy.sleep(1)
  
        # 控制机械臂终端反向旋转90度
        arm.shift_pose_target(3, -1.57, end_effector_link)
        arm.go()
        rospy.sleep(1)
           
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    MoveItIkDemo()

1.2.2代码分析

代码中很多部分和关节空间规划是相同的

• 获取link在模型文件中的名称

# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()

• 工作空间的位姿需要使用笛卡尔坐标值进行描述，所以必须声明该位姿所在坐标系

set_pose_reference_frame()用于设置目标位姿所在坐标系，这里设置的是机器人的基坐标base_link

# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)

• 反向运动学求解时可能会出现无解或多解的问题，通过配置允许规划失败之后的重新规划

# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)

• 使用ROS中的PoseStamped描述机器人的目标位姿

首先需要设置位姿所在的参考坐标系，然后创建时间戳，最后设置目标位姿的xyz坐标值和四元数姿态值

# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
# 姿态使用四元数描述，基于base_link坐标系
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()     
target_pose.pose.position.x = 0.191995
target_pose.pose.position.y = 0.213868
target_pose.pose.position.z = 0.520436
target_pose.pose.orientation.x = 0.911822
target_pose.pose.orientation.y = -0.0269758
target_pose.pose.orientation.z = 0.285694
target_pose.pose.orientation.w = -0.293653

• 规划起始状态，使用set_start_state_to_current_state()方法

# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
        
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)

• 运动规划进行的第一步就是规划路径，使用plan()方法完成，如果路径规划成功，则会返回一条规划好的路径，然后execute()方法控制机器人完成；如果规划失败，会根据设置项重新规划

# 规划运动路径
traj = arm.plan()
        
# 按照规划的运动路径控制机械臂运动
arm.execute(traj)
rospy.sleep(1)

• 除了使用PoseStamped数据描述目标位姿进行运动规划之外，还可以使用shift_pose_target()实现单轴方向上的目标设置和规划

# 控制机械臂终端向右移动5cm
arm.shift_pose_target(1, -0.05, end_effector_link)
arm.go()
rospy.sleep(1)
  
# 控制机械臂终端反向旋转90度
arm.shift_pose_target(3, -1.57, end_effector_link)
arm.go()
rospy.sleep(1)

1.3笛卡尔运动规划

工作空间中的运动规划没有对机器人终端轨迹有任何约束，目标位姿给定值后，可以通过运动学反解获得关节空间下的各轴弧度，接下来的运动规划和运动依然在关节空间中完成

但是在实际场景中，不仅仅是关心机械臂的起终，对运动过程中的位姿也有一定的要求，比如希望机器人终端能够走出去一条直线或者是圆弧轨迹

MoveIt!也提供了笛卡尔运动规划的接口，使用如下命令：

$ roslaunch marm_planning arm_planning_with_trail.launch
$ rosrun marm_planning moveit_cartesian_demo.py _cartesian:=True

1.3.1源码

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

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy

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

        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)
        
        # 是否需要使用笛卡尔空间的运动规划
        cartesian = rospy.get_param('~cartesian', True)
                        
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('arm')
        
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        arm.set_pose_reference_frame('base_link')
                
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.1)
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
                                        
        # 控制机械臂运动到之前设置的“forward”姿态
        arm.set_named_target('forward')
        arm.go()
        
        # 获取当前位姿数据最为机械臂运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose
                
        # 初始化路点列表
        waypoints = []
                
        # 将初始位姿加入路点列表
        if cartesian:
            waypoints.append(start_pose)
            
        # 设置第二个路点数据，并加入路点列表
        # 第二个路点需要向后运动0.2米，向右运动0.2米
        wpose = deepcopy(start_pose)
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
         
        # 设置第三个路点数据，并加入路点列表
        wpose.position.x += 0.05
        wpose.position.y += 0.15
        wpose.position.z -= 0.15
          
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
        
        # 设置第四个路点数据，回到初始位置，并加入路点列表
        if cartesian:
            waypoints.append(deepcopy(start_pose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0   #路径规划覆盖率
            maxtries = 100   #最大尝试规划次数
            attempts = 0     #已经尝试规划次数
            
            # 设置机器臂当前的状态作为运动初始状态
            arm.set_start_state_to_current_state()
     
            # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses，路点列表
                                        0.01,        # eef_step，终端步进值
                                        0.0,         # jump_threshold，跳跃阈值
                                        True)        # avoid_collisions，避障规划
                
                # 尝试次数累加
                attempts += 1
                
                # 打印运动规划进程
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
            # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
                arm.execute(plan)
                rospy.loginfo("Path execution complete.")
            # 如果路径规划失败，则打印失败信息
            else:
                rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

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

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

1.3.2代码分析

• waypoint表示的是路点，笛卡尔路径中需要经过的每一个位姿点，相邻两个路点之间使用直线轨迹运动，需要将运动中需要经过的路点都加入路点列表中

# 初始化路点列表
waypoints = []
                
# 将初始位姿加入路点列表
if cartesian:
    waypoints.append(start_pose)

• 整个历程的核心部分，使用了笛卡尔路径规划的API

            # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses，路点列表
                                        0.01,        # eef_step，终端步进值
                                        0.0,         # jump_threshold，跳跃阈值
                                        True)        # avoid_collisions，避障规划
                
                # 尝试次数累加
                attempts += 1
                
                # 打印运动规划进程
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")

其总共有四个参数：第一个参数是之前创建的路点列表；第二个参数是终端步进值；第三个参数是跳跃阀值；第四个参数是用于设置运动过程中是否考虑避障

compute_cartesian_path()执行后会返回两个值：plan是规划出来的运动轨迹；fraction用于描述规划成功的轨迹在给定路点列表中的覆盖值，从0到1

如果fraction小于1，说明给定的路点列表没有办法完整规划，这种情况下可以重新进行规划，但是需要人为设置规划次数

• 如果规划成功，fraction的值为1，此时就可以实现execute()控制机器人执行规划成功的路径轨迹

# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
    rospy.loginfo("Path computed successfully. Moving the arm.")
    arm.execute(plan)
    rospy.loginfo("Path execution complete.")
    # 如果路径规划失败，则打印失败信息
    else:
        rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

1.4避障规划

在很多应用场景下机器人工作环境中会有一些周围物体，这些物体可能在机器人工作空间内称为机器人运动规划过程中的障碍物，所以运动规划需要考虑避障问题

运行以下命令，启动避障规划

$ roslaunch marm_planning arm_planning.launch
$ rosrun marm_planning moveit_obstacles_demo.py

1.4.1源码

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

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import  PlanningScene, ObjectColor
from geometry_msgs.msg import PoseStamped, Pose

class MoveItObstaclesDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_obstacles_demo')
        
        # 初始化场景对象
        scene = PlanningSceneInterface()
        
        # 创建一个发布场景变化信息的发布者
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
        
        # 创建一个存储物体颜色的字典对象
        self.colors = dict()
        
        # 等待场景准备就绪
        rospy.sleep(1)
                        
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('arm')
        
        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()
        
        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
       
        # 当运动规划失败后，允许重新规划
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)
        
        # 设置每次运动规划的时间限制：5s
        arm.set_planning_time(5)
        
        # 设置场景物体的名称
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # 移除场景中之前运行残留的物体
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)    
        rospy.sleep(1)
        
        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)
        
        # 设置桌面的高度
        table_ground = 0.25
        
        # 设置table、box1和box2的三维尺寸
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # 将三个物体加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.26
        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_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.21
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.19
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)
        
        # 将桌子设置成红色，两个box设置成橙色
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # 将场景中的颜色设置发布
        self.sendColors()    
        
        # 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.2
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose.pose.orientation.w = 1.0
        
        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # 设置机械臂的运动目标位置，进行避障规划
        target_pose2 = PoseStamped()
        target_pose2.header.frame_id = reference_frame
        target_pose2.pose.position.x = 0.2
        target_pose2.pose.position.y = -0.25
        target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose2.pose.orientation.w = 1.0
        
        # 控制机械臂运动到目标位置
        arm.set_pose_target(target_pose2, end_effector_link)
        arm.go()
        rospy.sleep(2)
        
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
        
    # 设置场景物体的颜色
    def setColor(self, name, r, g, b, a = 0.9):
        # 初始化moveit颜色对象
        color = ObjectColor()
        
        # 设置颜色值
        color.id = name
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a
        
        # 更新颜色字典
        self.colors[name] = color

    # 将颜色设置发送并应用到moveit场景当中
    def sendColors(self):
        # 初始化规划场景对象
        p = PlanningScene()

        # 需要设置规划场景是否有差异     
        p.is_diff = True
        
        # 从颜色字典中取出颜色设置
        for color in self.colors.values():
            p.object_colors.append(color)
        
        # 发布场景物体颜色设置
        self.scene_pub.publish(p)

if __name__ == "__main__":
    try:
        MoveItObstaclesDemo()
    except KeyboardInterrupt:
        raise

1.4.2源码分析

该历程的主要流程是：

• 初始化场景，设置参数
• 在可视化环境中加入障碍物模型
• 设置机器人的起始位姿和目标位姿
• 进行避障规划

• 使用PlanningSceneInterface接口提供的添加、删除物体模型的功能，使用ObjectColor消息来设置物体的颜色，PlanningScene消息更新话题planning_scene订阅的消息类型

from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import  PlanningScene, ObjectColor

• 创建PlanningSceneInterface类示例，通过这个实例可以添加或删除物体模型

# 初始化场景对象
scene = PlanningSceneInterface()

• 创建一个planning_scene话题发布者，用来更新物体的颜色等信息

# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)

• 物体模型在环境中需要有唯一的ID，该历程中包括三个物体：

# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'

• 该历程可以在终端重复运行，但是之前加载的物体模型不会自动清除，需要使用remove_world_object()方法进行清除

# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)    
rospy.sleep(1)

• 设置物体模型尺寸

# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]

• 使用PoseStamped消息描述确定物体位置，并使用PlanningSceneInterface的add_box()接口添加到场景中

# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.26
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_id, table_pose, table_size)

• 设置颜色

# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

• setColor()通过输入的RGBA值创建一条ObjectColor类型的消息，并保存到颜色变量列表中

# 设置场景物体的颜色
def setColor(self, name, r, g, b, a = 0.9):
    # 初始化moveit颜色对象
    color = ObjectColor()
    
    # 设置颜色值
    color.id = name
    color.color.r = r
    color.color.g = g
    color.color.b = b
    color.color.a = a
    
    # 更新颜色字典
    self.colors[name] = color

• sendColors()取出颜色列表中的颜色，通过创建的PlanningScene消息，将场景的更新信息发布

# 将颜色设置发送并应用到moveit场景当中
def sendColors(self):
    # 初始化规划场景对象
    p = PlanningScene()

    # 需要设置规划场景是否有差异     
    p.is_diff = True
    
    # 从颜色字典中取出颜色设置
    for color in self.colors.values():
        p.object_colors.append(color)
        
    # 发布场景物体颜色设置
    self.scene_pub.publish(p)

• 将机械臂放置到两个物体之间

# 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame       tar
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0

• 设置目标位姿在障碍物外边

# 设置机械臂的运动目标位置，进行避障规划
target_pose2 = PoseStamped()
target_pose2.header.frame_id = reference_frame
target_pose2.pose.position.x = 0.2
target_pose2.pose.position.y = -0.25
target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose2.pose.orientation.w = 1.0

二、pick and place实例

目的：让机器人用夹爪将工作空间中的某一个物体夹起来，然后将该物体放到目标位置上

2.1应用效果

$ roslaunch marm_planning arm_planning.launch
$ rosrun marm_planning moveit_pick_and_place_demo.py

效果如下所示：

2.2抓取物体

# 设置目标物体的尺寸
target_size = [0.02, 0.01, 0.12]

# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.32
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0

2.3设置物体的放置位置

# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.32
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0

2.4生成抓取姿态

    # 创建夹爪的姿态数据JointTrajectory
    def make_gripper_posture(self, joint_positions):
        # 初始化夹爪的关节运动轨迹
        t = JointTrajectory()
        
        # 设置夹爪的关节名称
        t.joint_names = ['finger_joint1']
        
        # 初始化关节轨迹点
        tp = JointTrajectoryPoint()
        
        # 将输入的关节位置作为一个目标轨迹点
        tp.positions = joint_positions
        
        # 设置夹爪的力度
        tp.effort = [1.0]
        
        # 设置运动时间
        tp.time_from_start = rospy.Duration(1.0)
        
        # 将目标轨迹点加入到运动轨迹中
        t.points.append(tp)
        
        # 返回夹爪的关节运动轨迹
        return t
    
    # 使用给定向量创建夹爪的translation结构
    def make_gripper_translation(self, min_dist, desired, vector):
        # 初始化translation对象
        g = GripperTranslation()
        
        # 设置方向向量
        g.direction.vector.x = vector[0]
        g.direction.vector.y = vector[1]
        g.direction.vector.z = vector[2]
        
        # 设置参考坐标系
        g.direction.header.frame_id = GRIPPER_FRAME
        
        # 设置最小和期望的距离
        g.min_distance = min_dist
        g.desired_distance = desired
        
        return g

    # 创建一个允许的的抓取姿态列表
    def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
        # 初始化抓取姿态对象
        g = Grasp()
        
        # 创建夹爪张开、闭合的姿态
        g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
        g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
                
        # 设置期望的夹爪靠近、撤离目标的参数
        g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0])
        g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0])
        
        # 设置抓取姿态
        g.grasp_pose = initial_pose_stamped
    
        # 需要尝试改变姿态的数据列表
        pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
        yaw_vals = [0]

        # 抓取姿态的列表
        grasps = []

        # 改变姿态，生成抓取动作
        for y in yaw_vals:
            for p in pitch_vals:
                # 欧拉角到四元数的转换
                q = quaternion_from_euler(0, p, y)
                
                # 设置抓取的姿态
                g.grasp_pose.pose.orientation.x = q[0]
                g.grasp_pose.pose.orientation.y = q[1]
                g.grasp_pose.pose.orientation.z = q[2]
                g.grasp_pose.pose.orientation.w = q[3]
                
                # 设置抓取的唯一id号
                g.id = str(len(grasps))
                
                # 设置允许接触的物体
                g.allowed_touch_objects = allowed_touch_objects
                
                # 将本次规划的抓取放入抓取列表中
                grasps.append(deepcopy(g))
                
        # 返回抓取列表
        return grasps

2.5pick动作

# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
    n_attempts += 1
    rospy.loginfo("Pick attempt: " +  str(n_attempts))
    result = arm.pick(target_id, grasps)
    rospy.sleep(0.2)

针对不同的抓取姿态，如果无法求解运动学逆解或者规划轨迹会发生碰撞，pick动作则会出错

2.6place动作

# 如果pick成功，则进入place阶段 
if result == MoveItErrorCodes.SUCCESS:
    result = None
    n_attempts = 0
    
    # 生成放置姿态
    places = self.make_places(place_pose)
    
    # 重复尝试放置，直道成功或者超多最大尝试次数
    while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
        n_attempts += 1
        rospy.loginfo("Place attempt: " +  str(n_attempts))
        for place in places:
            result = arm.place(target_id, place)
            if result == MoveItErrorCodes.SUCCESS:
                break
        rospy.sleep(0.2)

使用make_place()方法生成一个可能的夹爪放置姿态，然后根据这些可能的放置姿态尝试规划place操作的轨迹