当前位置：物联沃-IOTWORD物联网 > 技术教程 > 使用Pybullet实现UR5机器人RRT/RRT*避障仿真的Python实践
代码收藏家代码收藏家 技术教程

使用Pybullet实现UR5机器人RRT/RRT*避障仿真的Python实践

主要是为了记录一下Pybullet的使用过程，顺便实现一下RRT/RRT*的避障仿真。

本文希望能够将机器人的规划模块尽可能独立，即尽可能与环境和机器人结构解耦。因此将任务主要分为机器人类、环境类、规划类、可视类四个部分来编写。

1. 机器人类

机器人类中包含规划和可视化中可能用到的内容，如机器人的基座位姿、URDF文件路径、DOF、末端执行器关节序号、关节限位等。

def __init__(self):
        self.type = "manipulator"
        self.startPos = [0, 0, 0]
        self.startOrientation = p.getQuaternionFromEuler([0, 0, 0])
        self.robotID = p.loadURDF(
            "ur5/ur5.urdf", self.startPos, self.startOrientation
        )
        self.DOF = 6
        self.end_effector_index = 8
        # limit可以用getJointInfo()里的信息
        self.limit_lower = [-2, -2, -2, -2, -2, -2]
        self.limit_upper = [2, 2, 2, 2, 2, 2]

为了方便后续使用，还实现了正运动学的功能。首先resetJointState根据索引和关节值改变机器人的姿态，利用getLinkState获取了机器人末端执行器的笛卡尔坐标。

def kinematics(self, configuration):
        for i in range(self.DOF):
            p.resetJointState(self.robotID, i, configuration[i])
        return p.getLinkState(self.robotID, self.end_effector_index)[0]

在这里还通过setJointMotorControlArray函数实现了电机位置控制的功能，本意是希望机器人规划完路径后，以更符合真实物理规律的形式（电机控制）运行一遍。但对于不同的机器人，电机控制的PID以及其他的设置都不太一样，都需要额外设置。例如UR5中就设计P为1，如下所示。

def run_path(self, configurations):
        for i in range(len(configurations)):
        # 获取当前时间步的关节配置
            target_positions = configurations[i]

            # 设置关节电机控制 Pandas的有些问题
            p.setJointMotorControlArray(
                positionGains=[1] * self.DOF,
                bodyUniqueId=self.robotID,
                jointIndices=list(range(self.DOF)),
                controlMode=p.POSITION_CONTROL,
                targetPositions=target_positions,
            )

            # 进行一步模拟
            p.stepSimulation()
            time.sleep(0.5)

2. 环境类

在环境类中主要是定义了环境中的障碍物。例如本文使用了createCollisionShape构造了一个立方体碰撞形状，并使用createMultiBody创建了这个碰撞形状的刚体。

# 创建一个立方体的碰撞形状
collision_shape_id = p.createCollisionShape(
    shapeType=p.GEOM_BOX, halfExtents=[0.05, 0.05, 0.1]
)

# 创建一个具有该碰撞形状的刚体
base_position = [0.2, 0.2, 0.8]
base_orientation = p.getQuaternionFromEuler([0, 0, 0])
body_id = p.createMultiBody(
    baseMass=1,
    baseCollisionShapeIndex=collision_shape_id,
    basePosition=base_position,
    baseOrientation=base_orientation,
)
self.obstacleID = [collision_shape_id]

3. 规划类

RRT按照下列的思路进行：

  1. 初始化机器人型号、初始配置、目标配置、关节限位

  2.  随机采样

  3. 树内检索最近点

  4. 最近点限位

  5. 取间隔点，逐点避障检测

  6. 添加点至RRT

  7. 找到路径回溯

为方便后续进行向量化操作，将rrt中每个节点的值、父节点索引分别存储在两个rrt树中，这样在后续寻找最近点时，可以对存有值的rrt树整体进行操作，而不用手动遍历。RRT规划的主函数如下：

def planning(self, plot=True):
    for i in range(self.iter_max):
        node_rand = self.generate_random_node()
        node_near, index_near = self.nearest_node(node_rand)
        index_near = index_near
        node_new = self.new_node(node_near, node_rand)

        if not self.is_collision(node_near, node_new):
            self.rrt_append(node_new,index_near)
            dist = np.linalg.norm(self.goal - node_new, 2)

            # self.plot_points_and_lines()
            if dist <= self.step_delta and not self.is_collision(
                node_new, self.goal
            ):
                if plot:
                    self.plot_points_and_lines()
                # len(self.rrt_value) - 2为node_new的索引
                self.rrt_append(self.goal,len(self.rrt_value) - 2)
                # len(self.rrt_value) - 1为goal的索引
                return self.extract_path(len(self.rrt_value) - 1)

RRT*加入了重布线的操作，按照下列的思路进行：

  1. 初始化机器人型号、初始配置、目标配置、关节限位

  2. 随机采样

  3. 找潜在父节点

  4. 计算代价

  5. 碰撞检测

  6. 重新布线

  7. 取间隔点，逐点避障检测

  8. 添加点至RRT

  9. 找到路径回溯

RRT*规划的主函数如下：

def planning(self, plot=True):
    for i in range(self.iter_max):
        node_rand = self.generate_random_node()
        node_near, index_near = self.nearest_node(node_rand)
        index_near = index_near
        node_new = self.new_node(node_near, node_rand)

        if not self.is_collision(node_near, node_new):   
            neighbor_index = self.find_neighbor(node_new) 

            self.rrt_append(node_new, index_near)

            if neighbor_index is not None:
                parent_index = self.choose_parent(node_new, neighbor_index)
                if parent_index:
                    self.rewire(node_new, parent_index)

            dist = np.linalg.norm(self.goal - node_new, 2)               
            if dist <= self.step_delta and not self.is_collision(
                node_new, self.goal
            ):
                if plot:
                    self.plot_points_and_lines()
                # len(self.rrt_value) - 2为node_new的索引
                self.rrt_append(self.goal,len(self.rrt_value) - 2)
                # len(self.rrt_value) - 1为goal的索引
                return self.extract_path(len(self.rrt_value) - 1)

4. 可视化类

可视化类中的函数使用staticmethod定义，利用addUserDebugPoints和addUserDebugLine在Pybullet中绘制点和线，具体如下：

import numpy as np
import pybullet as p

class Visualize:
    @staticmethod
    def plot_points(position, color, size):
        if np.array(position).ndim == 1:
            position = [position]
        if np.array(color).ndim == 1:
            color = [color]
        if len(color) == 1:
            num_points = len(position)
            color = color * num_points

        p.addUserDebugPoints(
            pointPositions=position,
            pointColorsRGB=color,
            pointSize=size,
        )

    @staticmethod
    def plot_lines(position1, position2, color, size):
        if np.array(position1).ndim == 1:
            position1 = [position1]
        if np.array(position2).ndim == 1:
            position2 = [position2]

        assert len(position1) == len(position2)

        # 循环绘制连线
        for start, end in zip(position1, position2):
            p.addUserDebugLine(
                lineFromXYZ=start, lineToXYZ=end, lineColorRGB=color, lineWidth=size
            )

    @staticmethod
    def plot_path(path, point_color, line_color, point_size, line_size):
        Visualize.plot_points(path, point_color, point_size)
        if np.array(path).ndim == 1 or len(path) == 1:
            return
        position1 = path[:-1]
        position2 = path[1:]
        Visualize.plot_lines(position1, position2, line_color, line_size)

5. 主函数

主函数调用rrt/rrt*进行仿真，代码如下：

import pybullet as p
import numpy as np

from env import Env
from rrtManipulator import RRT
from rrtstarManipulator import RRTstar
from visualize import Visualize

np.random.seed(123)

env = Env()
robot = env.robot
ur5_start = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
ur5_end = [0, 0, 0, 0, 0, 0]

rrt = RRT(
    start=ur5_start,
    goal=ur5_end,
    step_delta=0.05,
    iter_max=10000,
    sample_rate=0.7,
    robot=robot,
)
rrtstar = RRTstar(
    start=ur5_start,
    goal=ur5_end,
    step_delta=0.05,
    iter_max=10000,
    sample_rate=0.7,
    search_radius=0.5,
    robot=robot,
)

configuration, path = rrtstar.planning(plot=False)

Visualize.plot_path(path, [0.1, 0.1, 0.1], [0.8, 0, 0.8], 4, 2)

robot.run_path(configuration)

while p.isConnected():
    p.stepSimulation()

使用RRT规划结果如下：

使用RRT*规划结果如下：

可以看到，RRT*因为重布线轨迹比RRT更短更平滑些。

将planning函数中的plot设为True，可以将rrt中存储的所有的点都可视化出来，如下图所示，红点为所有的点，绿线为点之间的连接边，黑点为最终路径的点，紫线为最终路径的连接边。点和线的颜色和大小都能够更改。
 

如果需要使用其他的机械臂进行仿真，可以参考上传代码的PandasRobot()写法进行修改，但可能需要调整positionGains等参数以实现电机控制显示最终路径的功能。

作者：世界尽头的青豆

物联沃分享整理
物联沃-IOTWORD物联网 » 使用Pybullet实现UR5机器人RRT/RRT*避障仿真的Python实践

代码收藏家 普通

分享到：

相关推荐

发表回复