ROS moveit 机械臂避障运动规划

机械臂moveit编程(python)

moveit默认使用的运动规划库OMPL支持臂章规划,这里选用RRT算法,使用move group中的PlanningSceneInterface()添加障碍物,观察机械臂运动效果。

程序流程:
1.初始化需要控制的规划组,初始化场景;
2.设置运动约束(可选);
3.设置终端link;
4.清理上一次运行的残留物体
5.设置障碍物size和位姿,并使用scene.attach_box(),scene.add_box() API添加
6.设置目标位姿并进行规划运动

运行程序如下:

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


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('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)
        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()
        p.header.frame_id = end_effector_link
        
        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整理

第一步初始化:在初始化中加入了一个初始化场景,PlanningSceneInterface()提供了添加和删除物体模型的功能。

 moveit_commander.roscpp_initialize(sys.argv)
 rospy.init_node('moveit_attached_object_demo')
 
 scene = PlanningSceneInterface()
 rospy.sleep(1)
 arm = MoveGroupCommander('arm')

第二步获取终端link,并且让机器人回到home位姿

 end_effector_link = arm.get_end_effector_link()

 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和tool位姿和尺寸大小

# 设置桌面的高度
        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()
        p.header.frame_id = end_effector_link
        
        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()

程序运行结果如下图
在这里插入图片描述

  • 10
    点赞
  • 125
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
ROS MoveIt是一个用于机器人操作的软件框架,其中含了一些常用的规划算法,例如RRT(快速随机树),用于机械臂的路径规划。碰撞检测是MoveIt框架的一个重要功能,用于避免机械臂在执行路径时与障碍物发生碰撞。 在MoveIt中使用RRT进行碰撞检测的过程大致如下: 1.建立机械臂运动规划场景:在ROS环境中,根据机械臂运动学模型和环境设定,使用MoveIt框架创建机械臂规划场景。这括定义机械臂的关节和末端执行器的运动限制,以及环境中的障碍物。 2.定义机械臂路径:使用RRT算法定义机械臂的路径规划,即指定机械臂的起始姿态和目标姿态,然后通过RRT算法生成一条连接两者的路径。RRT算法会在工作空间中进行随机采样,通过逐步扩展采样点来生成路径,直到到达目标姿态。 3.碰撞检测:在生成路径的过程中,对每个路径上的关节姿态进行碰撞检测。MoveIt使用机械臂的碰撞模型和环境中的障碍物模型进行碰撞检测。如果在路径上有碰撞,MoveIt会通过调整路径或重新生成路径来避免碰撞。 4.路径优化和执行:根据需求,可以对生成的路径进行优化,以提高路径的质量和执行效率。优化过程可以括减少路径的长度、姿态调整等。最后,将优化后的路径加载到机械臂控制器中执行。 在MoveIt中使用RRT进行碰撞检测可以有效避免机械臂与障碍物的碰撞,提高机械臂的路径规划和执行的安全性和可靠性。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值