MoveIt编程实现机械臂自主避障运动

105
0
2020年11月17日 09时15分

Moveit在规划路径的时候考虑如何躲避障碍物的问题,Moveit可以实时的检测空间中的障碍物,并规划处轨迹绕过障碍物。

在场景中加入障碍物方式

 

1

 

在Moveit中 具有一个规划场景监听器的模块结构,可以用来检测机器人场景中是否存在障碍物。障碍物有几种方式可以告诉监听器:

  1. 通过Rviz界面的形式添加。
  2. 通过程序(C++/Python)编程进行添加。
  3. 通过机器人的外部传感器,例如通过Kinect深度相机实时检测到的信息加入到场景中。

在这篇文章中主要介绍前两种方法,后续还会专门介绍第三种方法。

通过Rviz界面

运行以下命令在Rviz中打开机械臂模型:

 

roslaunch probot_anno_moveit_config demo.launch

 

启动之后,在如图所示位置有Scene Objects选项卡,用来添加场景物体,可以添加已有模型或者Solidwors构建的模型:

 

2

 

点击左下角的Import Flie可以添加一些模型进来,例如我们直接用.gazebo中的离线模型:

 

3

 

我们选择一个碗的模型添加进来,通过箭头调整碗的位置:

 

4

 

点击Publish Scene将障碍物的位置发布出去:

 

5

 

再次回到Plan选项卡中,拖拽机械臂,使机械臂碰撞障碍物,就会发现机械臂发生碰撞的部分变成了红色。

 

7

 

再次进行归经规划的时候,机械臂就会自动避开障碍物:

 

8

 

编程形式添加障碍物

当机械臂抓取到某个物体之后没,它的轨迹进行规划的时候,需要将抓取得到的物体当作机械臂本身的一部分,必涨的时候要考虑抓取到的物体也不能发生碰撞。

打开模型:

 

roslaunch probot_anno_moveit_config demo.launch

 

附着物体避障:

 

rosrun probot_demo moveit_attached_object_demo.py

 

运行结果:

 

9

 

编程形式添加障碍物

当机械臂抓取到某个物体之后没,它的轨迹进行规划的时候,需要将抓取得到的物体当作机械臂本身的一部分,必涨的时候要考虑抓取到的物体也不能发生碰撞。

打开模型:

 

roslaunch probot_anno_moveit_config demo.launch

 

附着物体避障:

 

rosrun probot_demo moveit_attached_object_demo.py

 

运行结果:

 

10

 

运行过程中,抓取到的物体也会进行避障,不会碰到上面绿色的桌面。

Python源码解析:

 

import rospy, sys
import thread, copy
import moveit_commander
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, Pose
from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, PlanningScene
from math import radians
from copy import deepcopy
 
class MoveAttachedObjectDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_attached_object_demo')
        
        # 初始化场景对象,用来监听外部环境的变化
        scene = PlanningSceneInterface()
        rospy.sleep(1)
                                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('manipulator')
        
        # 获取终端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)
        arm.set_planning_time(10)
 
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
        
        # 移除场景中之前运行残留的物体
        scene.remove_attached_object(end_effector_link, 'tool')
        scene.remove_world_object('table') 
        scene.remove_world_object('target')
 
        # 设置桌面的高度
        table_ground = 0.6
        
        # 设置table和tool的三维尺寸
        table_size = [0.1, 0.7, 0.01]  //设置长宽高
        tool_size = [0.2, 0.02, 0.02]
        
        # 设置tool的位姿
        p = PoseStamped() 
        #tool放置位置的参考坐标系
        p.header.frame_id = end_effector_link
        
        #tool的具体位置姿态
        p.pose.position.x = tool_size[0] / 2.0 - 0.025
        p.pose.position.y = -0.015
        p.pose.position.z = 0.0
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1
        
        # 将tool附着到机器人的终端,该函数为附着某物体到机器人上
        scene.attach_box(end_effector_link, 'tool', p, tool_size)
 
        # 将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)  
 
        # 更新当前的位姿
        arm.set_start_state_to_current_state()
 
        # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)
        joint_positions = [0.827228546495185, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858]
        arm.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)
        
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()
 
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
 
if __name__ == "__main__":
    MoveAttachedObjectDemo()

 

关键API:scene.attach_box(end_effector_link, ‘tool’, p, tool_size)

  • end_effector_link为机械臂终端的坐标系
  • ‘tool’是要附着的对象
  • p为附着物的位置
  • tool_size为附着物体的尺寸

scene.add_box(‘table’, table_pose, table_size)

  • ‘table’是要添加的障碍物对象
  • table_pose为障碍物的姿态信息
  • table_size为障碍物的大小

发表评论

后才能评论